import machine
from machine import I2C, Pin

import d1motor

import time
import usocket
import esp

class Robot:
    def __init__(self):
        print("setting up I2C")
        d1 = Pin(5)
        d2 = Pin(4)
        i2c = I2C(scl=d1, sda=d2)
        addr = i2c.scan()[0]
        print("i2c scan = {}".format(addr))
        print("setting up motors")
        self.m1 = d1motor.Motor(0, i2c, address=addr)
        self.m2 = d1motor.Motor(1, i2c, address=addr)
        print("motor setup complete")
        
        # setup socket for remote control 
        self.addr = usocket.getaddrinfo('192.168.4.1', 1234)[0][-1]

    def remote_control(self):
        while True:
            # dirty hack: free memory such that reuse of socket is possible
            print(esp.freemem())
            if esp.freemem() < 9 * 1024:
                s = time.ticks_ms()
                while esp.freemem() < 9 * 1024:
                    pass
                print("waited {} ms".format((time.ticks_ms() - s)))
            print("setting up socket communication")
            socket = usocket.socket()
            socket.bind(self.addr)
            print("socket setup complete")

            print("waiting for connections on {}".format(self.addr))
            socket.listen(1)
            res = 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 = int(float(l[0])*10000)
                     print("u1 = {}".format(u1))
                     u2 = int(float(l[1])*10000)
                     print("u2 = {}".format(u2))
                 except ValueError:
                     print("ValueError: Data has wrong format.")
                     print("Data received: {}".format(data_str))
                     print("Shutting down")
                     u1 = u2 = 0
                     listening = False
                 except IndexError:
                     print("IndexError: Data has wrong format.")
                     print("Data received: {}".format(data_str))
                     print("Shutting down")
                     u1 = u2 = 0
                     listening = False
                 finally:    
                     self.m1.speed(u1)
                     self.m2.speed(u2)
            comm_socket.close()
            socket.close()
            del comm_socket
            del socket
            print("disconnected")

wall_e = Robot()
wall_e.remote_control()