import sumolib
from math import dist, ceil
from random import randint
from PySide6.QtGui import QPainter
from PySide6.QtCore import QPointF, Signal, QObject

class updateSignals(QObject):
    updateDisp = Signal(tuple)
    addGraphPt = Signal(tuple)

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]

        if self.infoWidg is None:
            return
        self.signals.updateDisp.emit(("Index",f"{self.index}/{len(self.route)}"))
        self.signals.updateDisp.emit(("Edge",self.route[self.index]))

        #print(f"{self.id} : {startEdge.getID()} -> {nextEdge.getID()} via {laneId}")
    
    def __init__(self,carID,route,startTime,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.startTime=float(startTime)
        
        self.pos=[0,0]
        self.v=0
        self.a=10
        self.b=20

        self.vmax=0

        self.S = 5
        self.T = 0.3

        self.vroom = 0

        self.rawRoute = route
        
        if infoWidg is None:
            return
        self.signals=updateSignals()
        self.signals.updateDisp.connect(self.infoWidg.setVal)
        self.signals.addGraphPt.connect(self.infoWidg.addSpeedPoint)
        
        self.signals.updateDisp.emit(("Position",self.pos))
        self.signals.updateDisp.emit(("Vitesse",self.v))
        self.signals.updateDisp.emit(("Index",f"{self.index}/{len(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 l == 0:
                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 as e:
                if l == 0:
                    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:
                if l == 0:
                    l+=dist(self.pos, closest.pos)
                else:
                    l+=dist(laneShape[shapeInd], closest.pos)
                if l <= maxDist:
                    self.leaderDist = l
                    return closest
                else:
                    return
        return

    def draw(self,painter):
        pt = QPointF(*self.pos)
        painter.drawEllipse(pt,3,3)

        if self.vroom != 0:
            painter.save()
            d=(60-self.vroom)*0.2
            painter.translate(pt + QPointF(0,d))
            painter.scale(1,-1)
            font = painter.font();
            font.setPixelSize(ceil(self.vroom*0.2));
            painter.setFont(font)
            painter.drawText(QPointF(0,0),"vroom")
            self.vroom -= 1
            painter.restore()
        #painter.drawEllipse(pt,100,100)
   
    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=self.S
        T=self.T
        vsec=vleader+(S-vmax*T)/(vbar/bbar+T)
        vd=min(self.v+self.a*dt,vmax) #,vsec)
        self.v=max(0,vd)

        if self.infoWidg is None:
            return
        self.signals.addGraphPt.emit((2,self.controller.t,self.v))
        self.signals.addGraphPt.emit((0,self.controller.t,vmax))
        self.signals.addGraphPt.emit((1,self.controller.t,vsec))

    def update(self,dt):
        if self.controller.t < self.startTime:
            return

        self.leader=self.getLeader(100)

        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
 
        if randint(0,100) == 0:
            self.vroom = 60

        if self.infoWidg is None:
            return
        self.signals.updateDisp.emit(("Position", self.pos))
        self.signals.updateDisp.emit(("Vitesse", self.v))
        self.signals.updateDisp.emit(("Leader", self.leader if self.leader is None else f"{self.leader.id} @ {self.leaderDist:.2f}m"))

    def __copy__(self):
        copy = Car(self.id, self.rawRoute, self.startTime, self.map, self.controller, self.infoWidg)
        copy.route = self.route
        copy.pos = self.pos.copy()
        copy.laneShape = self.laneShape
        copy.laneId = self.laneId
        copy.vmax = self.vmax
        return copy