import machine
import time
import usocket

class Motor:
    def __init__(self, enable, dir1, dir2):
        self.enable_pin = machine.Pin(enable, machine.Pin.OUT)
        self.enable_pwm = machine.PWM(self.enable_pin)
        self.enable_pwm.freq(500)

        self.dir1_pin = machine.Pin(dir1, machine.Pin.OUT)
        self.dir2_pin = machine.Pin(dir2, machine.Pin.OUT)

        self.direction = 1 # default direction: dir1_pin = HIGH, dir2_pin = LOW
        self.reverse()

    def reverse(self):
        self.direction = not self.direction
        self.dir1_pin.value(self.direction)
        self.dir2_pin.value(not self.direction)

    def control(self, value):
        if value > 0.0: # forward
            if not self.direction: # switch direction if necessary
                self.reverse()
        else: # backward
            if self.direction: # switch direction if necessary
                self.reverse()
        # apply value as pwm signal
        self.enable_pwm.duty(int(abs(value)*1023))


class Robot:
    def __init__(self):
        print("setting up motors")
        self.m1 = Motor(4, 16, 5)
        self.m2 = Motor(14, 0, 2)
        
        # wait for connections on port 1234
        self.socket = usocket.socket()
        self.addr = usocket.getaddrinfo('192.168.4.1', 1234)[0][-1]
        self.socket.bind(self.addr)
        print("setup complete")

    def forward(self, seconds):
        print("Onward, to glory!")
        self.m1.control(1.0)
        self.m2.control(1.0)
        time.sleep(seconds)
        self.m1.control(0.0)
        self.m2.control(0.0)
    
    def backward(self, seconds):
        print("Better retreat...")
        self.m1.control(-1.0)
        self.m2.control(-1.0)
        time.sleep(seconds)
        self.m1.control(0.0)
        self.m2.control(0.0)
    
    def spin_right(self, seconds):
        print("You spin my head right round, right round..")
        self.m1.control(1.0)
        self.m2.control(-1.0)
        time.sleep(seconds)
        self.m1.control(0.0)
        self.m2.control(0.0)
    
    def spin_left(self, seconds):
        print("spinning left")
        self.m1.control(-1.0)
        self.m2.control(1.0)
        time.sleep(seconds)
        self.m1.control(0.0)
        self.m2.control(0.0)

    def sequence(self):
        self.spin_left(0.3)
        self.spin_right(0.3)
        self.forward(0.5)
        self.backward(0.5)

    def remote_control(self):
        print("waiting for connections on {}".format(self.addr))
        self.socket.listen(1)
        res = self.socket.accept() # this blocks until someone connects to the socket
        comm_socket = res[0]
        print("connected!")
        listening = True
        while listening:
             # expected data: '(u1, u2)'\n"
             # where ui = control for motor i
             # ui \in [-1.0, 1.0]
             data = comm_socket.readline()
             data_str = data.decode()
             print("Data received: {}".format(data_str))
             try:
                 print("processing data = {}".format(data_str))
                 l = data_str.strip('()\n').split(',')
                 print("l = {}".format(l))
                 u1 = float(l[0])
                 print("u1 = {}".format(u1))
                 u2 = float(l[1])
                 print("u2 = {}".format(u2))
             except ValueError:
                 print("ValueError: Data has wrong format.")
                 print("Data received: {}".format(data_str))
                 print("Shutting down")
                 u1 = u2 = 0.0
                 listening = False
             except IndexError:
                 print("IndexError: Data has wrong format.")
                 print("Data received: {}".format(data_str))
                 print("Shutting down")
                 u1 = u2 = 0.0
                 listening = False
             finally:    
                 self.m1.control(u1)
                 self.m2.control(u2)

wall_e = Robot()
wall_e.remote_control()