from math import cos, sin, pi, atan2, tan

class Robot:
    # position
    pos=(1,2)
    # orientation
    rot=0
    # velocite
    v=0
    # velocite angulaire
    angv=0
    # velocite des roues (non angulaire)
    wv=[0,0]
    # velocite max
    wmv=0.01
    # espace entre les deux roues
    l=0.1
    # time
    t = 0
    # pins level
    pins = [0]*16
    # acceleration maximal avant de perdre l'adhérance
    maxRotAccel = 10
    # pins reliés au moteurs (bw_l, fw_l, bw_r, fw_r)
    motors_pins = ((9,10),(12,11))
    # position du spot lumineux
    light_pos = (5, 1.5)
    # ecart entre les deux photores
    photores_l = 0.1
    # longeur du séparateur entre les photorésistances
    photores_sep_l = 0.1
    # constantes de résistance des photoresistances
    PHOTORES_LIGHT = 100
    PHOTORES_SHADOW = 20000
    PHOTORES_BRIDGE = 1000
    pr_l_pin = 0 # A0 voir adapter_ard.h
    pr_r_pin = 1 # A1

    def __init__(self, dt):
        self.dt = dt
        self.pr_max_ang = tan((self.photores_l/2)/self.photores_sep_l)
        
    def update(self):
        self.compute_light()
        self.t += self.dt
        # https://robotics.stackexchange.com/questions/106/what-is-a-suitable-model-for-two-wheeled-robots
        self.v = 1/2*(self.wv[0]+self.wv[1])
        self.angv = 1/self.l*(self.wv[1]-self.wv[0])
        self.rot += self.angv*self.dt
        self.pos = (self.pos[0]+self.v*sin(self.rot)*self.dt,self.pos[1]+self.v*cos(self.rot)*self.dt)
        
    def compute_light(self):
        a2s = atan2(self.light_pos[1]-self.pos[1],self.light_pos[0]-self.pos[0])
        self.angle2spot = pi/2-a2s-self.rot%(2*pi)
        if abs(self.angle2spot) > pi/2:
            self.pr_l = self.PHOTORES_SHADOW
            self.pr_r = self.PHOTORES_SHADOW
        elif abs(self.angle2spot) < self.pr_max_ang:
            self.pr_l = self.PHOTORES_LIGHT
            self.pr_r = self.PHOTORES_LIGHT
        else:
            if self.angle2spot > 0:
                self.pr_l = self.PHOTORES_LIGHT
                self.pr_r = self.PHOTORES_SHADOW
            else:
                self.pr_l = self.PHOTORES_SHADOW
                self.pr_r = self.PHOTORES_LIGHT
        
        self.pins[self.pr_l_pin] = 1024 * self.PHOTORES_BRIDGE/(self.pr_l+self.PHOTORES_BRIDGE)
        self.pins[self.pr_r_pin] = 1024 * self.PHOTORES_BRIDGE/(self.pr_r+self.PHOTORES_BRIDGE)
        
            
    def analogWrite(self, pin, dc):
        print(f">> set pin {pin} to {dc}")
        speedDelta = (self.pins[pin] - dc) / self.dt
        #assert(speedDelta < self.maxRotAccel)
        self.pins[pin] = dc
        for ind, mp in enumerate(self.motors_pins):
            assert(bool(mp[0] > 0) ^ bool(mp[1] > 0), "deux pins moteur allumés en même temps")
            self.wv[ind] = (-self.pins[mp[0]]+self.pins[mp[1]])/255*self.wmv
        
    def digitalWrite(self, pin, level):
        self.analogWrite(pin, level*255)