import Robot
import asyncio;
import json
import websockets;
import sys;
import Container
from PWMDriver import PWMDriver
robot = Robot.Robot()
#robot = Robot.Robot()
#t = robot.Translation
#r = robot.Rotation
#sd = t.sd
#v = t.droit
#d = t.trv
class commandHandler:
    def ping(self):
        return "Pong"
    """def set_vector(self,vector_type:str,vector_args):
        print(f"{vector_type=}, {vector_args=}")
        for i in range(len(vector_args)):
            vector_args[i]= float(vector_args[i])
        if vector_type =='trans':
            if abs(vector_args[0])>=0.1 and abs(vector_args[1])<=0.1:
                v((vector_args[0]*10))
            if abs(vector_args[1])>=0.1 and abs(vector_args[0])<=0.1:
                sd((vector_args[1]*10))
            if abs(vector_args[1])>=0.1 and abs(vector_args[0])>=0.1:
                if vector_args[0]>0:
                    d((vector_args[0]+vector_args[1]//2)*10,True)
                elif vector_args[0]<0:
                    d((vector_args[0]+vector_args[1]//2)*10,False)
        if vector_type == 'rot':
            r(vector_args[1]*10)
        return f"{vector_type=},{vector_args=}"
        """
    def set_direction(self,arg):
        try:
            return robot.translation(arg)
        except Exception as e:
            return 'error'

cmd_handler = commandHandler()
async def handler(websocket, path):
    print('new handler created with '+websocket+' at path'+path)
    while True:
        try :
            data = await websocket.recv()
        except websockets.exceptions.ConnectionClosedOK:
            break
        print(data)
        if not data:
            data = {
                'cmd':'ping',
                'args':{}
            }
        else:
            data = json.loads(data)
        if data['cmd'] =='STOP':
            sys.exit()
        try:
            callable = getattr(cmd_handler, data['cmd'])
        except AttributeError:
            await websocket.send(json.dumps({"type":"err","message":"Invalid Command"}))
            continue
        
        try:
            res = callable(**data['args'])
        except TypeError:
            await websocket.send(json.dumps({"type":"err","message":"Invalid Args"}))

            continue
        await websocket.send(json.dumps({"type":"success","message":res}))
print('server running at 0.0.0.0 with port 1567')
start_server = websockets.serve(handler, "0.0.0.0", 1567)


asyncio.get_event_loop().run_until_complete(start_server)

asyncio.get_event_loop().run_forever()