import sumolib
import math
from PySide6.QtGui import QPainter
from PySide6.QtCore import QPointF

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
        lane=startEdge.getLane(laneId)
        self.vmax=lane.getSpeed()
        self.laneShape=lane.getShape()
        if(inverted):
            self.laneShape.reverse()

        print(f"{self.id} : {startEdge.getID()} -> {nextEdge.getID()} via {laneId}")
    
    def __init__(self,carID,route,parentMap,parentController):
        self.id=carID
        self.map=parentMap
        self.controller=parentController
        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)
            if(len(conn)==0):
                continue
            edge=self.map.getLane(conn[0].getViaLaneID()).getEdge()
            self.route.append(edge)
            
            secEdge=edge.getConnections(rn)[0].getViaLaneID() # Parfois je sais pas pourquoi il coupe les edges internes, mais il marque quand même la connection, ducoup pour contourner
            while secEdge!="":
                edge=self.map.getLane(secEdge).getEdge()
                self.route.append(edge)
                secEdge=edge.getConnections(rn)[0].getViaLaneID()

        self.initPath()

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

    def draw(self,painter):
        pt = QPointF(*self.pos)
        painter.drawEllipse(pt,3,3)
   
    def conduite(self,vmax,leader,dt):
        if(leader==None):
            self.v=vmax
            return
        else:
            vleader=leader.v # vitesse de la voiture leader
            bleader=leader.b

        vbar=(self.v+vleader)/2
        bbar=(bleader+self.b)/2
        S=5
        T=0.3
        vsec=vleader+(S-vmax*T)/(vbar/bbar+T)
        vd=min(self.v+self.a*dt,vmax,vsec)
        self.v=max(0,vd)

    def update(self,dt):
        leaders=self.controller.getCarsOnEdge(self.route[self.index].getID())
        leader=None
        minDist=self.route[self.index].getLength()
        for l in leaders:
            db=math.dist(self.pos,l.pos)
            de=math.dist(l.pos,self.laneShape[-1])
            df=math.dist(self.pos,self.laneShape[-1])

            if(de<df and db<minDist):
                minDist=db
                leader=l

        self.conduite(self.vmax,leader,dt)
        
        lgt=self.v*dt

        while(lgt>0):
            endPos=self.laneShape[self.laneInd+1]
            l=math.dist(self.pos,endPos)
            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