import machine
from machine import I2C, Pin

import d1motor

import time
import usocket

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 
        print("setting up socket communication")
        self.socket = usocket.socket()
        self.addr = usocket.getaddrinfo('192.168.4.1', 1234)[0][-1]
        self.socket.bind(self.addr)
        print("socket setup complete")

    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 [10000, 10000]
             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(l[0])
                 print("u1 = {}".format(u1))
                 u2 = int(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
                 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)

wall_e = Robot()
wall_e.remote_control()