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

class Car:
    def getShape(self, edgeInd):
        startEdge = self.route[edgeInd]
        nextEdge = self.route[edgeInd+1]

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

        return (laneShape, vmax, laneId)

    def initPath(self):
        newLane = self.getShape(self.index)
        
        self.laneShape = newLane[0]
        self.vmax = newLane[1]
        self.laneId = newLane[2]

        self.infoWidg.setVal("Index",f"{self.index}/{len(self.route)}")
        self.infoWidg.setVal("Edge",self.route[self.index])

        #print(f"{self.id} : {startEdge.getID()} -> {nextEdge.getID()} via {laneId}")
    
    def __init__(self,carID,route,parentMap,parentController,infoWidg):
        self.id=carID
        self.map=parentMap
        self.controller=parentController
        self.infoWidg=infoWidg
        self.index=0
        self.laneInd=0
        self.route=[]
        self.laneShape=None
        self.laneId=0
        self.leader=None
        
        self.pos=[0,0]
        self.v=0
        self.a=10
        self.b=20

        self.vmax=0

        self.infoWidg.setVal("Position",self.pos)
        self.infoWidg.setVal("Vitesse",self.v)
        self.infoWidg.setVal("Index",f"{self.index}/{len(route)}")

        self.rawRoute = route

    def prepareRoute(self):
        route = list(map(self.map.getEdge,self.rawRoute))
        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])
    
    def getLeader(self, maxDist):
        shapeInd = self.laneInd
        edgeInd = self.index
        laneShape = self.laneShape
        laneId = self.laneId
        l = 0
        carsHere = self.controller.getCarsOnLane(self.route[edgeInd].getID(), laneId)
        while(l<maxDist):
            endPos = laneShape[shapeInd+1]
            carsHere = filter(lambda c: c.laneInd == shapeInd, carsHere)
            if(shapeInd == self.laneInd and edgeInd == self.index):
                carsHere = filter(lambda c: dist(c.pos,endPos) < dist(self.pos,endPos),carsHere)
            closest = None
            try:
                closest = min(carsHere, key=lambda c: dist(c.pos,laneShape[shapeInd]))
            except ValueError:
                if(shapeInd == self.laneInd and edgeInd == self.index):
                    l+=dist(self.pos,endPos)
                else:
                    l+=dist(laneShape[shapeInd],endPos)
                shapeInd+=1
                if(shapeInd>=len(laneShape)-1):
                    shapeInd = 0
                    edgeInd+=1
                    carsHere = self.controller.getCarsOnLane(self.route[edgeInd].getID(), laneId)
                    if(edgeInd>=len(self.route)-1):
                        return
                    newLane = self.getShape(edgeInd)
                    laneShape = newLane[0]
                    laneId = newLane[2]
            else:
                return closest
        return

    def draw(self,painter):
        pt = QPointF(*self.pos)
        painter.drawEllipse(pt,3,3)
   
    def conduite(self,vmax,leader,dt):
        if(leader is None):
            vleader=self.v
            bleader=self.b
        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)
        self.infoWidg.addSpeedPoint(2,self.controller.t,self.v)
        self.infoWidg.addSpeedPoint(0,self.controller.t,vmax)
        self.infoWidg.addSpeedPoint(1,self.controller.t,vsec)

    def update(self,dt):
        self.leader=self.getLeader(50)

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

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

        self.infoWidg.setVal("Position", self.pos)
        self.infoWidg.setVal("Vitesse", self.v)
        self.infoWidg.setVal("Leader", self.leader if self.leader is None else self.leader.id)