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

class Robot:
    # position
    pos=(0.1,1)
    # orientation
    rot=pi/2
    # velocite
    v=0
    # velocite angulaire
    angv=0
    # velocite des roues (non angulaire)
    wv=[0,0]
    # velocite max
    wmv=0.3
    # espace entre les deux roues
    l=0.1
    # time
    t = 0
    # pins level
    pins = [0]*32
    # 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 = (9, 1.5)
    # ecart entre les deux photores
    photores_l = 0.07
    # longeur du séparateur entre les photorésistances
    photores_sep_l = 0.6
    # constantes de résistance des photoresistances
    PHOTORES_LIGHT = 100
    PHOTORES_SHADOW = 20000
    PHOTORES_BRIDGE = 1000
    pr_l_pin = 20 # A0 voir adapter_ard.h
    pr_r_pin = 21 # A1
    # rayon de mesure des capteurs ultrason
    ultr_sense_max = 4
    ultr_sense_rays = (pi/2, -pi/2)
    ultr_sense_scatter = radians(6) # TODO: check
    dists = [None] * len(ultr_sense_rays)
    tr = [0]*len(ultr_sense_rays)
    ci = [0]*len(tr)
    
    def __init__(self, laby):
        self.laby = laby
        self.pr_max_ang = tan((self.photores_l/2)/self.photores_sep_l)
        self.t = 0
        self.dt = -1
        self.angle2spot = 0
        
    def update(self):
        if(self.dt == -1):
            return
        self.update_ultr_sense()
        self.compute_light()
        print("***********", 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] = int(1024 * self.pr_l/(self.pr_l+self.PHOTORES_BRIDGE))
        self.pins[self.pr_r_pin] = int(1024 * self.pr_r/(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)
        
    def setTime(self, t):
        self.dt = (t/1000000 - self.t)
        self.t = t/1000000
        assert(self.dt < 0.1)
        
    def update_ultr_sense(self):
        for ind,r in enumerate(self.ultr_sense_rays):
            #tr = (self.pos[0]+self.ultr_sense_max*sin(r+self.rot), self.pos[1]+self.ultr_sense_max*cos(r+self.rot))
            #self.tr[ind] = tr
            self.ci[ind] = self.laby.get_closest_wall_dist(self.pos, r+self.rot, self.ultr_sense_max, self.ultr_sense_scatter)
            #self.ci[ind] = self.laby.get_closest_wall_dist((self.pos, tr))
            if self.ci[ind]:
                self.dists[ind] = dist(self.pos, self.ci[ind])
            else:
                self.dists[ind] = None