import sys
import serial
import sys
import time

from twisted.internet import reactor
from twisted.python import log
from twisted.web.server import Site
from twisted.web.static import File

from autobahn.twisted.websocket import WebSocketServerFactory, \
    WebSocketServerProtocol

from autobahn.twisted.resource import WebSocketResource

i = 0
turn_sleeptime = 0.8
straight_sleeptime = 0.5

def setMotor(command):    

    if command == 'D':
        vacuum_serial.write(str.encode(
            "SetMotor LWheelDist 155 RWheelDist 155 Speed 150 \n"))
        time.sleep(straight_sleeptime)
    if command == 'L':
        vacuum_serial.write(str.encode(
            "SetMotor LWheelDist -190 RWheelDist 190 Speed 200 \n"))
        time.sleep(turn_sleeptime)
    if command == 'R':
        vacuum_serial.write(str.encode(
            "SetMotor LWheelDist 190 RWheelDist -190 Speed 200 \n"))
        time.sleep(turn_sleeptime)
    if command == 'F':
        vacuum_serial.write(str.encode(
            "SetMotor LWheelDist -155 RWheelDist -155 Speed 150 \n"))
        time.sleep(straight_sleeptime)

def setServo(servoChannel, position):
    servoStr = str(servoChannel) + "=" + str(position) + "\n"
    with open("/dev/servoblaster", "wb") as f:
        f.write(bytes(servoStr, 'utf-8'))


class EchoServerProtocol(WebSocketServerProtocol):

    def onConnect(self, request):
        print("WebSocket connection request: {}".format(request))

    def onMessage(self, payload, isBinary):
        global i
        i = i + 1
        # if isBinary:
        #    print("Binary message received: {0} bytes".format(len(payload)))
        # else:
        #    print("Text message received: {0}".format(payload.decode('utf8')))
        string = " " + str(i)
        self.sendMessage(payload + bytes(string, 'utf-8'), isBinary)
        string = payload.decode('ASCII')
        print("Str ->{0}".format(string))
        if string[0] != 'R' and string[0] != 'L' and string[0] != 'F' and string[0] != 'D':
            setServo(2, int(string.split("/")[0]))
            setServo(1, int(string.split("/")[1]))
        else:
            setMotor(string[0])


if __name__ == '__main__':

    try:

        vacuum_serial = serial.Serial('/dev/ttyACM0', 115200)
        vacuum_serial.write(str.encode("testmode on \n"))
        time.sleep(1)
    
        log.startLogging(sys.stdout)
    
        factory = WebSocketServerFactory(u"ws://10.253.35.143:45684")
        factory.protocol = EchoServerProtocol
    
        resource = WebSocketResource(factory)
    
        # we server static files under "/" ..
        root = File(".")
    
        # and our WebSocket server under "/ws" (note that Twisted uses
        # bytes for URIs)
        root.putChild(b"ws", resource)
    
        # both under one Twisted Web Site
        site = Site(root)
        reactor.listenTCP(45684, site)
    
        reactor.run()
    
        vacuum_serial.write(str.encode("testmode off \n"))
        vacuum_serial.close()

    except KeyboardInterrupt:
        print("Fuuuuuuuuuuuu")
        vacuum_serial.write(str.encode("testmode off \n"))
        vacuum_serial.close()