#!/usr/bin/env python3
# coding: Latin-1

# Creates a web-page interface for DiddyBorg

# Import library functions we need
import PicoBorgRev3 as PicoBorgRev
import time
import sys
import threading
import socketserver
#import picamera
#import picamera.array
#import cv2
import datetime
import os

# Settings for the web-page
webPort = 8080                           # Port number for the web-page, 80 is what web-pages normally use
#imageWidth = 240                        # Width of the captured image in pixels
#imageHeight = 180                       # Height of the captured image in pixels
#frameRate = 10                          # Number of images to capture per second
#displayRate = 2                         # Number of images to request per second
#photoDirectory = '/home/pi'             # Directory to save photos to

# Global values
global PBR
#global lastFrame
#global lockFrame
#global camera
#global processor
global running
global watchdog
running = True

# Setup the PicoBorg Reverse
PBR1 = PicoBorgRev.PicoBorgRev()
PBR1.i2cAddress = 0x12                  # Uncomment and change the value if you have changed the board address
PBR2 = PicoBorgRev.PicoBorgRev()
PBR2.i2cAddress = 0x11                  # Uncomment and change the value if you have changed the board address
PBR1.Init()
PBR2.Init()
if not PBR1.foundChip:
    boards = PicoBorgRev.ScanForPicoBorgReverse()
    if len(boards) == 0:
        print ('No PicoBorg Reverse found, check you are attached :)')
    else:
        print ('No PicoBorg Reverse at address %02X, but we did find boards:' % (PBR.i2cAddress))
        for board in boards:
            print ('    %02X (%d)' % (board, board))
        print ('If you need to change the I²C address change the setup line so it is correct, e.g.')
        print ('PBR.i2cAddress = 0x%02X' % (boards[0]))
    sys.exit()
#PBR.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
PBR1.SetCommsFailsafe(False)             # Disable the communications failsafe
PBR2.SetCommsFailsafe(False)             # Disable the communications failsafe
PBR1.ResetEpo()
PBR2.ResetEpo()

# Power settings
voltageIn = 1.2 * 10                    # Total battery voltage to the PicoBorg Reverse
voltageOut = 6.0                        # Maximum motor voltage

# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)

# Timeout thread
class Watchdog(threading.Thread):
    def __init__(self):
        super(Watchdog, self).__init__()
        self.event = threading.Event()
        self.terminated = False
        self.start()
        self.timestamp = time.time()

    def run(self):
        timedOut = True
        # This method runs in a separate thread
        while not self.terminated:
            # Wait for a network event to be flagged for up to one second
            if timedOut:
                if self.event.wait(10):
                    # Connection
                    print ('Reconnected...')
                    timedOut = False
                    self.event.clear()
            else:
                if self.event.wait(10):
                    self.event.clear()
                else:
                    # Timed out
                    print ('Timed out...')
                    timedOut = True
                    PBR1.MotorsOff()
                    PBR2.MotorsOff()



# Class used to implement the web server
class WebServer(socketserver.BaseRequestHandler):
    def handle(self):
        global PBR
        #global lastFrame
        global watchdog
        # Get the HTTP request data
        reqData = self.request.recv(1024).strip()
        reqData = reqData.decode('utf-8')
        reqData = reqData.split('\n')
        # Get the URL requested
        getPath = ''
        for line in reqData:
            if line.startswith('GET'):
                parts = line.split(' ')
                getPath = parts[1]
                break
        watchdog.event.set()

        

        if getPath.startswith('/shutdown'):
            # Shut the Raspberry Pi down
            httpText = '<html><body><center>'
            httpText += 'Shutting down...'
            httpText += '</center></body></html>'
            self.send(httpText)
            os.system('sudo shutdown -h now')
           
        elif getPath.startswith('/off'):
            # Turn the drives off
            httpText = '<html><body><center>'
            httpText += 'Speeds: 0 %, 0 %'
            httpText += '</center></body></html>'
            self.send(httpText)
            PBR1.MotorsOff()
            PBR2.MotorsOff()

        elif getPath.startswith('/set/'):
            # Motor power setting: /set/driveLeft/driveRight
            parts = getPath.split('/')
            # Get the power levels
            if len(parts) >= 4:
                try:
                    driveLeft = float(parts[2])
                    driveRight = float(parts[3])
                except:
                    # Bad values
                    driveRight = 0.0
                    driveLeft = 0.0
            else:
                # Bad request
                driveRight = 0.0
                driveLeft = 0.0
            # Ensure settings are within limits
            if driveRight < -1:
                driveRight = -1
            elif driveRight > 1:
                driveRight = 1
            if driveLeft < -1:
                driveLeft = -1
            elif driveLeft > 1:
                driveLeft = 1
            # Report the current settings
            percentLeft = driveLeft * 100.0;
            percentRight = driveRight * 100.0;
            httpText = '<html><body><center>'
            httpText += 'Speeds: %.0f %%, %.0f %%' % (percentLeft, percentRight)
            httpText += '</center></body></html>'
            self.send(httpText)
            # Set the outputs
            driveLeft *= maxPower
            driveRight *= maxPower
            PBR1.SetMotor1(driveRight)
            PBR1.SetMotor2(-driveLeft)
            
        elif getPath.startswith('/set2/'):
            # Motor power setting: /set/driveLeft/driveRight
            parts = getPath.split('/')
            # Get the power levels
            if len(parts) >= 4:
                try:
                    driveLeft = float(parts[2])
                    driveRight = float(parts[3])
                except:
                    # Bad values
                    driveRight = 0.0
                    driveLeft = 0.0
            else:
                # Bad request
                driveRight = 0.0
                driveLeft = 0.0
            # Ensure settings are within limits
            if driveRight < -1:
                driveRight = -1
            elif driveRight > 1:
                driveRight = 1
            if driveLeft < -1:
                driveLeft = -1
            elif driveLeft > 1:
                driveLeft = 1
            # Report the current settings
            percentLeft = driveLeft * 100.0;
            percentRight = driveRight * 100.0;
            httpText = '<html><body><center>'
            httpText += 'Speeds: %.0f %%, %.0f %%' % (percentLeft, percentRight)
            httpText += '</center></body></html>'
            self.send(httpText)
            # Set the outputs
            driveLeft *= maxPower
            driveRight *= maxPower
            PBR2.SetMotor1(driveRight)
            PBR2.SetMotor2(-driveLeft)

        elif getPath == '/':
            # Main page, click buttons to move and to stop
            httpText = '<html>\n'
            httpText += '<head>\n'
            httpText += '<script language="JavaScript"><!--\n'
            httpText += 'function Drive(left, right) {\n'
            httpText += ' var iframe = document.getElementById("setDrive");\n'
            httpText += ' var slider = document.getElementById("speed");\n'
            httpText += ' left *= speed.value / 100.0;'
            httpText += ' right *= speed.value / 100.0;'
            httpText += ' iframe.src = "/set/" + left + "/" + right;\n'
            httpText += '}\n'
            httpText += 'function Drive2(left, right) {\n'
            httpText += ' var iframe = document.getElementById("setDrive");\n'
            httpText += ' var slider = document.getElementById("speed");\n'
            httpText += ' left *= speed2.value / 100.0;'
            httpText += ' right *= speed2.value / 100.0;'
            httpText += ' iframe.src = "/set2/" + left + "/" + right;\n'
            httpText += '}\n'
            httpText += 'function Off() {\n'
            httpText += ' var iframe = document.getElementById("setDrive");\n'
            httpText += ' iframe.src = "/off";\n'
            httpText += '}\n'
            httpText += 'function Off() {\n'
            httpText += ' var iframe = document.getElementById("setDrive");\n'
            httpText += ' iframe.src = "/off";\n'
            httpText += '}\n'
            httpText += 'function Shutdown() {\n'
            httpText += ' var iframe = document.getElementById("setDrive");\n'
            httpText += ' iframe.src = "/shutdown";\n'
            httpText += '}\n'
            

            httpText += '//--></script>\n'
            httpText += '</head>\n'
            httpText += '<body>\n'
            #httpText += '<iframe src="http://192.168.1.166:8088" width="100%" height="500" frameborder="0"></iframe>\n'
            httpText += '<iframe id="setDrive" src="/off" width="100%" height="50" frameborder="0"></iframe>\n'
            httpText += '<center>\n'
            httpText += '<button onclick="Drive(-1,1)" style="width:100px;height:100px;"><b>Spin Left</b></button>\n'
            httpText += '<button onclick="Drive2(1,1)" style="width:100px;height:100px;"><b>Forward</b></button>\n'
            httpText += '<button onclick="Drive(1,-1)" style="width:100px;height:100px;"><b>Spin Right</b></button>\n'
            httpText += '<br /><br />\n'
            httpText += '<button onclick="Drive(0,1)" style="width:100px;height:100px;"><b>Turn Left</b></button>\n'
            httpText += '<button onclick="Drive(-1,-1)" style="width:100px;height:100px;"><b>Reverse</b></button>\n'
            httpText += '<button onclick="Drive(1,0)" style="width:100px;height:100px;"><b>Turn Right</b></button>\n'
            httpText += '<button onclick="Drive2(1,1)" style="width:100px;height:100px;"><b>up</b></button>\n'
            httpText += '<button onclick="Drive2(-1,-1)" style="width:100px;height:100px;"><b>Down</b></button>\n'
            httpText += '<br /><br />\n'
            httpText += '<button onclick="Off()" style="width:100px;height:50px;"><b>Stop</b></button>\n'
            httpText += '<br /><br />\n'
            httpText += '<button onclick="Shutdown()" style="width:100px;height:50px;"><b>Shutdown</b></button>\n'
            httpText += '<input id="speed" type="range" min="0" max="100" value="100" style="width:600px" />\n'
            httpText += '<input id="speed2" type="range" min="0" max="100" value="100" style="width:600px" />\n'
            httpText += '</center>\n'
            httpText += '</body>\n'
            httpText += '</html>\n'
            self.send(httpText)
        	
        
            
        elif getPath == '/joystick':
		    # Alternate page, hold buttons to move (does not work with all devices)
            httpText = '<html>\n'
            httpText += '<head>\n'
            httpText += '<script language="JavaScript"><!--\n'
            httpText += 'function Drive(left, right) {\n'
            httpText += ' var iframe = document.getElementById("setDrive");\n'
            httpText += ' var slider = document.getElementById("speed");\n'
            httpText += ' left *= speed.value / 100.0;'
            httpText += ' right *= speed.value / 100.0;'
            httpText += ' iframe.src = "/set/" + left + "/" + right;\n'
            httpText += '}\n'
            httpText += 'function Drive2(left, right) {\n'
            httpText += ' var iframe = document.getElementById("setDrive");\n'
            httpText += ' var slider = document.getElementById("speed");\n'
            httpText += ' left *= speed2.value / 100.0;'
            httpText += ' right *= speed2.value / 100.0;'
            httpText += ' iframe.src = "/set2/" + left + "/" + right;\n'
            httpText += '}\n'
            httpText += 'function Off() {\n'
            httpText += ' var iframe = document.getElementById("setDrive");\n'
            httpText += ' iframe.src = "/off";\n'
            httpText += '}\n'
            httpText += 'function Off() {\n'
            httpText += ' var iframe = document.getElementById("setDrive");\n'
            httpText += ' iframe.src = "/off";\n'
            httpText += '}\n'
            httpText += 'function Shutdown() {\n'
            httpText += ' var iframe = document.getElementById("setDrive");\n'
            httpText += ' iframe.src = "/shutdown";\n'
            httpText += '}\n'
            # key management ------------------------------------------------
            # 38=UP 40=DOWN 37=LEFT 39=RIGHT 85=PAGE UP 68=PAGE DOWN
            httpText += 'var valLeft = 0;\n'
            httpText += 'var valRight = 0;\n'
            httpText += 'var valRise = 0;\n'
            httpText += 'function checkKey() {\n'
            httpText += 'valLeft = 0;\n'
            httpText += 'valRight = 0;\n'
            httpText += 'valRise = 0;\n'
            #PAGE UP
            httpText += '  if (map[85]) {\n'
            httpText += '      valRise = 1;\n'
            httpText += '  }\n'
            #PAGE DOWN
            httpText += '  if (map[68]) {\n'
            httpText += '      valRise = -1;\n'
            httpText += '  }\n'
            #UP
            httpText += '  if (map[38]) {\n'
            httpText += '      valLeft = 1;\n'
            httpText += '      valRight = 1;\n'
            httpText += '  }\n'
            #DOWN
            httpText += '  if (map[40]) {\n'
            httpText += '      valLeft = -1;\n'
            httpText += '      valRight = -1;\n'
            httpText += '  }\n'
            #LEFT
            httpText += '  if (map[37]) {\n'
            httpText += '      valLeft = -1;\n'
            httpText += '      valRight = 1;\n'
            httpText += '  }\n'
            #RIGHT
            httpText += '  if (map[39]) {\n'
            httpText += '      valLeft = 1;\n'
            httpText += '      valRight = -1;\n'
            httpText += '  }\n'
            #UP LEFT
            httpText += '  if (map[38] && map[37] ) {\n'
            httpText += '      valLeft = 0.1;\n'
            httpText += '      valRight = 1;\n'
            httpText += '  }\n'
            #UP RIGHT
            httpText += '  if (map[38] && map[39] ) {\n'
            httpText += '      valLeft = 1;\n'
            httpText += '      valRight = 0.1;\n'
            httpText += '  }\n'
            #UP DOWN
            httpText += '  if (map[38] && map[40] ) {\n'
            httpText += '      valLeft = 0;\n'
            httpText += '      valRight = 0;\n'
            httpText += '  }\n'
            #DOWN LEFT
            httpText += '  if (map[40] && map[37] ) {\n'
            httpText += '      valLeft = -0.1;\n'
            httpText += '      valRight = -1;\n'
            httpText += '  }\n'
            #DOWN RIGHT
            httpText += '  if (map[40] && map[39] ) {\n'
            httpText += '      valLeft = -1;\n'
            httpText += '      valRight = -0.1;\n'
            httpText += '  }\n'
            #ACTION
            httpText += '  if (valLeft == 0 && valRight == 0 && valRise == 0) {\n'
            httpText += '      Off();\n'
            httpText += '  }\n'
            httpText += '  else {\n'
            httpText += '      Drive(valLeft,valRight);\n'
            httpText += '      Drive2(valRise,0);\n'
            httpText += '  }\n'
            httpText += '}\n'
            httpText += 'var map = {38: false, 40: false, 37: false, 39: false, 90: false, 88: false};\n'
            httpText += 'onkeydown = (function(e) {\n'
            httpText += ' if (e.keyCode in map) {\n'
            httpText += '  map[e.keyCode] = true;\n'
            httpText += ' checkKey()\n'
            httpText += ' }\n'
            httpText += '})\n'
            httpText += 'onkeyup = (function(e) {\n'
            httpText += ' if (e.keyCode in map) {\n'
            httpText += '  map[e.keyCode] = false;\n'
            httpText += ' checkKey()\n'
            httpText += ' }\n'
            httpText += '});\n'
            # ---------------------------------------------------------------
            httpText += '//--></script>\n'
            httpText += '</head>\n'
            #httpText += '<body bgcolor="#6B7FB7">\n'
            httpText += '<body bgcolor="blue">\n'
            httpText += '<center>\n'
            #httpText += '<iframe src="http://192.168.1.166:8088" width="100%" height="500" frameborder="0"></iframe>\n'
            httpText += '<iframe id="setDrive" src="/off" width="100%" height="50" frameborder="0"></iframe>\n'
            httpText += '<center>\n'
            httpText += '<button onmousedown="Drive(0,1)" onmouseup="Off()" style="width:50px;height:50px;"><b>&#8592</b></button>\n'
            httpText += '<button onmousedown="Drive(1,1)" onmouseup="Off()" style="width:50px;height:50px;"><b>&#8593</b></button>\n'
            httpText += '<button onmousedown="Drive(1,0)" onmouseup="Off()" style="width:50px;height:50px;"><b>&#8594</b></button>\n'
            httpText += '<br /><br />\n'
            httpText += '<button onmousedown="Drive(-1,1)" onmouseup="Off()" style="width:50px;height:50px;"><b>&#x21ba</b></button>\n'
            httpText += '<button onmousedown="Drive(-1,-1)" onmouseup="Off()" style="width:50px;height:50px;"><b>&#8595</b></button>\n'
            httpText += '<button onmousedown="Drive(1,-1)" onmouseup="Off()" style="width:50px;height:50px;"><b>&#x21bb</b></button>\n'
            httpText += '<button onmousedown="Drive(1,1)" onmouseup="Off()" style="width:50px;height:50px;"><b>up</b></button>\n'
            httpText += '<button onmousedown="Drive(-1,-1)" onmouseup="Off()" style="width:50px;height:50px;"><b>down</b></button>\n'
            httpText += '<br /><br />\n'
            #httpText += '<button onclick="Photo()" style="width:200px;height:100px;"><b>Save Photo</b></button>\n'
            httpText += '<button onclick="Shutdown()" style="width:100px;height:50px;"><b>Shutdown</b></button>\n'
            httpText += '<br /><br />\n'
            httpText += '<input id="speed" type="range" min="0" max="100" value="100" style="width:600px" />\n'
            httpText += '</center>\n'
            httpText += '</body>\n'
            httpText += '</html>\n'
            self.send(httpText)

        else:
            # Unexpected page
            self.send('Path : "%s"' % (getPath))

    def send(self, content):
        reply = 'HTTP/1.0 200 OK\n\n%s' % (content)
        reply = reply.encode('utf-8')
        self.request.sendall(reply)




print ('Setup the watchdog')
watchdog = Watchdog()

# Run the web server until we are told to close
httpServer = socketserver.TCPServer(("0.0.0.0", webPort), WebServer)
try:
    print ('Press CTRL+C to terminate the web-server')
    while running:
        httpServer.handle_request()
except KeyboardInterrupt:
    # CTRL+C exit
    print ('\nUser shutdown')
finally:
    # Turn the motors off under all scenarios
    PBR1.MotorsOff()
    PBR2.MotorsOff()
    print ('Motors off')
# Tell each thread to stop, and wait for them to end
running = False
#captureThread.join()
#processor.terminated = True
watchdog.terminated = True
#processor.join()
watchdog.join()
#del camera
PBR1.SetLed(True)
PBR2.SetLed(True)
print ('Web-server terminated.')