import sumolib
import pygame as pg
import math

class Car:
    def initPath(self):
        startEdge=self.route[self.index]
        nextEdge=self.route[self.index+1]

        inverted=not (startEdge.getToNode()==nextEdge.getFromNode() or startEdge.getToNode()==nextEdge.getToNode())
        
        laneId=0 if not inverted else 1
        self.laneShape=startEdge.getLane(laneId).getShape()
        if(inverted):
            self.laneShape.reverse()

        print(f"{self.id} : {startEdge.getID()} -> {nextEdge.getID()} via {laneId}")
    
    def __init__(self,carID,route,parentMap,surface):
        self.id=carID
        self.map=parentMap
        self.index=0
        self.laneInd=0
        self.route=[]

        route=list(map(self.map.getEdge,route))

        for r,rn in zip(route,route[1:]):
            self.route.append(r)
            conn=r.getConnections(rn)[0] # Si il trouve pas de connection c'est la merde
            edge=self.map.getLane(conn.getViaLaneID()).getEdge()
            self.route.append(edge)
                        

        self.initPath()

        self.pos=list(self.laneShape[0])
        self.v=0
        self.a=0
        self.b=20

        self.surf=surface

    def draw(self,col):
        pg.draw.circle(self.surf,col,self.map.convertPos(self.pos),5)
   
    def conduite(self):
        vmax=50 # vitesse max sur la route actuelle
        vleader=vmax # vitesse de la voiture leader
        bleader=15
        vbar=(self.v+vleader)/2
        bbar=(bleader+self.b)/2 # decelleration de la vleader et de self
        S=5
        T=0.3
        vsec=vleader+(S-vmax*T)/(vbar/bbar+T)
        vd=min(vmax,vsec)
        self.v=max(0,vd)

    def update(self,dt):
        self.conduite()
        lgt=self.v*dt

        while(lgt>0):
            endPos=self.laneShape[self.laneInd+1]
            l=math.dist(self.pos,self.laneShape[self.laneInd+1])
            if lgt>=l:
                lgt-=l
                pos=list(self.laneShape[-1])
                self.laneInd+=1
                if(self.laneInd>=len(self.laneShape)-1):
                    self.laneInd=0
                    self.index+=1
                    if(self.index>=len(self.route)-1):
                        self.index=0
                    
                    self.initPath()
                    self.pos=list(self.laneShape[self.laneInd])
                    continue
            adv=lgt/l
            self.pos[0]+=(endPos[0]-self.pos[0])*adv
            self.pos[1]+=(endPos[1]-self.pos[1])*adv
            lgt=0