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

# Creates a web-page interface for DiddyBorg 2
# Import library functions we need
import ThunderBorg
import time
import sys
import threading
import SocketServer
#import picamera
#import picamera.array
#import cv2
import datetime

# Settings for the web-page
webPort = 2000                            # Port number for the web-page, 80 is what web-pages normally use
#imageWidth = 240                        # Width of the captured image in pixels
#imageHeight = 192                       # Height of the captured image in pixels
#frameRate = 30                          # Number of images to capture per second
#displayRate = 10                        # Number of images to request per second
#photoDirectory = '/home/pi'             # Directory to save photos to
#flippedCamera = True                    # Swap between True and False if the camera image is rotated by 180
#jpegQuality = 80                        # JPEG quality level, smaller is faster, higher looks better (0 to 100)

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

TB = ThunderBorg.ThunderBorg()
#TB.i2cAddress = 0x15                  # Uncomment and change the value if you have changed the board address
TB.Init()
if not TB.foundChip:
    boards = ThunderBorg.ScanForThunderBorg()
    if len(boards) == 0:
        print 'No ThunderBorg found, check you are attached :)'
    else:
        print 'No ThunderBorg at address %02X, but we did find boards:' % (TB.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 'TB.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
#TB.SetCommsFailsafe(False)
TB.SetLedShowBattery(False)
TB.SetLeds(0,0,1)

# Power settings
voltageIn = 1.2 * 10                    # Total battery voltage to the ThunderBorg
voltageOut = 12.0 * 0.95                # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power

# 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(1):
                    # Connection
                    print 'Reconnected...'
                    TB.SetLedShowBattery(True)
                    timedOut = False
                    self.event.clear()
            else:
                if self.event.wait(1):
                    self.event.clear()
                else:
                    # Timed out
                    print 'Timed out...'
                    TB.SetLedShowBattery(False)
                    TB.SetLeds(0,0,1)
                    timedOut = True
                    TB.MotorsOff()


# Class used to implement the web server
class WebServer(SocketServer.BaseRequestHandler):
    def handle(self):
        global TB
        #global lastFrame
        global watchdog
        # Get the HTTP request data
        reqData = self.request.recv(1024).strip()
        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('/cam.jpg'):
            # Camera snapshot
            #lockFrame.acquire()
            #sendFrame = lastFrame
            #lockFrame.release()
            #if sendFrame != None:
                #self.send(sendFrame.tostring())
            # Turn the drives off
            elif getPath.startswith('/off'):
                httpText = '<html><body><center>'
                httpText += 'Speeds: 0 %, 0 %'
                httpText += '</center></body></html>'
                self.send(httpText)
                TB.MotorsOff()
            elif getPath.startswith('/set/'):
                parts = getPath.split('/')
                if len(parts) >= 4:
                    try:
                        driveLeft = float(parts[2])
                        driveRight = float(parts[3])
                    except:
                        driveRight = 0.0
                        driveLeft = 0.0
                else:
                    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)
                driveLeft *= maxPower
                driveRight *= maxPower
                TB.SetMotor1(driveLeft)
                TB.SetMotor2(driveRight)
            elif getPath == '/':
                httpText = '<html>'
                httpText += '<head>'
                httpText += '<script language="JavaScript"><!--'
                httpText += 'function Drive(left, right) {'
                httpText += ' var iframe = document.getElementById("setDrive");'
                httpText += ' var slider = document.getElementById("speed");'
                httpText += ' left *= speed.value / 100.0;'
                httpText += ' right *= speed.value / 100.0;'
                httpText += ' iframe.src = "/set/" + left + "/" + right;'
                httpText += '}'
                httpText += 'function Off() {'
                httpText += ' var iframe = document.getElementById("setDrive");'
                httpText += ' iframe.src = "/off";'
                httpText += '}'
#                httpText += 'function Photo() {'
#                httpText += ' var iframe = document.getElementById("setDrive");'
#                httpText += ' iframe.src = "/photo";'
#                httpText += '}'
                httpText += '//--></script>'
                httpText += '</head>'
                httpText += '<body>'
#                httpText += '<iframe src="/stream" width="100%" height="500" frameborder="0"></iframe>'
                httpText += '<iframe id="setDrive" src="/off" width="100%" height="50" frameborder="0"></iframe>'
                httpText += '<center>'
                httpText += '<button onclick="Drive(-1,1)" style="width:200px;height:100px;"><b>Spin Left</b></button>'
                httpText += '<button onclick="Drive(1,1)" style="width:200px;height:100px;"><b>Forward</b></button>'
                httpText += '<button onclick="Drive(1,-1)" style="width:200px;height:100px;"><b>Spin Right</b></button>'
                httpText += '<br /><br />'
                httpText += '<button onclick="Drive(0,1)" style="width:200px;height:100px;"><b>Turn Left</b></button>'
                httpText += '<button onclick="Drive(-1,-1)" style="width:200px;height:100px;"><b>Reverse</b></button>'
                httpText += '<button onclick="Drive(1,0)" style="width:200px;height:100px;"><b>Turn Right</b></button>'
                httpText += '<br /><br />'
                httpText += '<button onclick="Off()" style="width:200px;height:100px;"><b>Stop</b></button>'
                httpText += '<br /><br />'
#                httpText += '<button onclick="Photo()" style="width:200px;height:100px;"><b>Save Photo</b></button>'
#                httpText += '<br /><br />'
                httpText += '<input id="speed" type="range" min="0" max="100" value="100" style="width:600px" />'
                httpText += '</center>'
                httpText += '</body>'
                httpText += '</html>'
                self.send(httpText)
            elif getPath == '/hold':
                httpText = '<html>'
                httpText += '<head>'
                httpText += '<script language="JavaScript"><!--'
                httpText += 'function Drive(left, right) {'
                httpText += ' var iframe = document.getElementById("setDrive");'
                httpText += ' var slider = document.getElementById("speed");'
                httpText += ' left *= speed.value / 100.0;'
                httpText += ' right *= speed.value / 100.0;'
                httpText += ' iframe.src = "/set/" + left + "/" + right;'
                httpText += '}'
                httpText += 'function Off() {'
                httpText += ' var iframe = document.getElementById("setDrive");'
                httpText += ' iframe.src = "/off";'
                httpText += '}'
#                httpText += 'function Photo() {'
#                httpText += ' var iframe = document.getElementById("setDrive");'
#                httpText += ' iframe.src = "/photo";'
#                httpText += '}'
                httpText += '//--></script>'
                httpText += '</head>'
                httpText += '<body>'
#                httpText += '<iframe src="/stream" width="100%" height="500" frameborder="0"></iframe>'
                httpText += '<iframe id="setDrive" src="/off" width="100%" height="50" frameborder="0"></iframe>'
                httpText += '<center>'
                httpText += '<button onmousedown="Drive(-1,1)" onmouseup="Off()" style="width:200px;height:100px;"><b>Spin Left</b></button>'
                httpText += '<button onmousedown="Drive(1,1)" onmouseup="Off()" style="width:200px;height:100px;"><b>Forward</b></button>'
                httpText += '<button onmousedown="Drive(1,-1)" onmouseup="Off()" style="width:200px;height:100px;"><b>Spin Right</b></button>'
                httpText += '<br /><br />'
                httpText += '<button onmousedown="Drive(0,1)" onmouseup="Off()" style="width:200px;height:100px;"><b>Turn Left</b></button>'
                httpText += '<button onmousedown="Drive(-1,-1)" onmouseup="Off()" style="width:200px;height:100px;"><b>Reverse</b></button>'
                httpText += '<button onmousedown="Drive(1,0)" onmouseup="Off()" style="width:200px;height:100px;"><b>Turn Right</b></button>'
                httpText += '<br /><br />'
                httpText += '<button onclick="Photo()" style="width:200px;height:100px;"><b>Save Photo</b></button>'
                httpText += '<br /><br />'
                httpText += '<input id="speed" type="range" min="0" max="100" value="100" style="width:600px" />'
                httpText += '</center>'
                httpText += '</body>'
                httpText += '</html>'
                self.send(httpText)
            else:
                self.send('Path : "%s"' % (getPath))

    def send(self, content):
        self.request.sendall('HTTP/1.0 200 OK %s' % (content))


# Create the image buffer frame


print 'Setup the watchdog'
watchdog = Watchdog()

# Run the web server until we are told to close
try:
    httpServer = None
    httpServer = SocketServer.TCPServer(("0.0.0.0", webPort), WebServer)
except:
    # Failed to open the port, report common issues
    print
    print 'Failed to open port %d' % (webPort)
    print 'Make sure you are running the script with sudo permissions'
    print 'Other problems include running another script with the same port'
    print 'If the script was just working recently try waiting a minute first'
    print 
    # Flag the script to exit
    running = False
try:
    print 'Press CTRL+C to terminate the web-server'
    while running:
        httpServer.handle_request()
except KeyboardInterrupt:
    # CTRL+C exit
    print 'User shutdown'
finally:
    # Turn the motors off under all scenarios
    TB.MotorsOff()
    print 'Motors off'
# Tell each thread to stop, and wait for them to end
if httpServer != None:
    httpServer.server_close()
running = False
#captureThread.join()
#processor.terminated = True
watchdog.terminated = True
#processor.join()
watchdog.join()
#del camera
TB.SetLedShowBattery(False)
TB.SetLeds(0,0,0)
TB.MotorsOff()
print 'Web-server terminated.'