import sumolib
from math import dist, ceil, sqrt, log, cos, sin, atan2, pi
from random import randint, uniform
from PySide6.QtGui import QPainter
from PySide6.QtCore import QPointF, Signal, QObject, Qt
from itertools import islice

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

class Car():
    CLIGNO_NONE = 0
    CLIGNO_LEFT = 1
    CLIGNO_RIGHT = 2
    def getShape(self, edgeInd):
        startEdge = self.route[edgeInd]

        laneId = 0
        lane = startEdge.getLane(laneId)
        vmax = lane.getSpeed()
        laneShape = lane.getShape()
        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.cligno=[]

        self.pos=[0,0]
        self.dir=0
        self.v=0
        self.a=10
        self.b=20
        self.minSpace=20
        self.leaderBefore=False
        self.distToInter=0
        self.timeStopped=0
        self.speedPercentage=0
        self.ticksLived=0
        self.timeAtInter = 0
        self.forceThrough = False

        self.vmax=0

        self.alpha = 0.1
        self.beta = 0.1
        self.nu = 0
        self.gamma = 10
        self.delta = 0
        self.T = uniform(0.9,1.6)
        self.size = 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))
        if None in route:
            print("error planning route, mismatched net/demand?")
            return
        for r,rn in zip(route,route[1:]):
            self.route.append(r)
            conn=r.getConnections(rn)
            if(len(conn)==0):
                continue

            if conn[0].getDirection() == "l":
                self.cligno.append(self.CLIGNO_LEFT)
            elif conn[0].getDirection() == "r":
                self.cligno.append(self.CLIGNO_RIGHT)
            else:
                self.cligno.append(self.CLIGNO_NONE)
            
            edge=self.map.getLane(conn[0].getViaLaneID()).getEdge()
            self.route.append(edge)
            self.cligno.append(self.CLIGNO_NONE)

            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)
                self.cligno.append(self.CLIGNO_NONE)
                secEdge=edge.getConnections(rn)[0].getViaLaneID()

        self.initPath()

        self.pos=list(self.laneShape[0])

    def getLeader(self, maxDist):
        shapeInd = self.laneInd
        edgeInd = self.index
        prevInd = edgeInd
        laneShape = self.laneShape
        laneId = self.laneId
        l = 0
        carsHere = self.controller.getCarsOnLane(self.route[edgeInd].getID(), laneId)
        carsHere = list(filter(lambda c: c.id != self.id, carsHere))
        self.leaderBefore = False
        while(l<maxDist):
            endPos = laneShape[shapeInd+1]
            
            carsReallyHere = filter(lambda c: c.laneInd == shapeInd, carsHere)

            if l == 0:
                carsReallyHere = filter(lambda c: dist(c.pos,endPos) < dist(self.pos,endPos),carsReallyHere)
            closest = None

            carsReallyHere = list(carsReallyHere)

            try:
                closest = min(carsReallyHere, 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
                    carComing = self.getLeaderAtIntersection(prevInd,edgeInd)
                    if(carComing is not None):
                        self.distToInter = l # y as un bug qqu part dans le calcul de la distance, ça saute quand on change d'edge
                        if self.route[edgeInd].isSpecial():
                            self.distToInter += self.route[edgeInd + 1].getLength()
                        self.leaderDist = carComing[0]
                        self.leaderBefore = True
                        return carComing[1]
                    if(not self.route[edgeInd].isSpecial()):
                        prevInd = edgeInd
                    carsHere = self.controller.getCarsOnLane(self.route[edgeInd].getID(), laneId)
                    carsHere = list(filter(lambda c: c.id != self.id, carsHere)) # me demande pas pourquoi mais si on le convertit pas en liste ici le filter original est modifié
                    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 getCurrentEdge(self):
        return self.route[self.index]

    def getCarDist(self, car, edge, laneInd, startFromEnd = True):
        lanes = edge.getLane(laneInd).getShape().copy()
        if startFromEnd:
            lanes.reverse()
        cDist = 0
        for i,l in enumerate(lanes[:-1]):
            if car.laneInd != (i if not startFromEnd else len(lanes)-i-2):
                cDist += dist(l, lanes[i+1])
            else:
                cDist += dist(l, car.pos)
                return cDist
        return cDist

    def getLeaderAtIntersection(self, prevInd, edgeInd):
        while(self.route[edgeInd].isSpecial()):
            return
            edgeInd = edgeInd + 1
            if edgeInd >= len(self.route):
                print(self.id,"fu")
                return None
        while self.route[prevInd].isSpecial():
            prevInd -= 1
            if prevInd < 0:
                print(self.id, "fu2")
                return None
        inter = self.route[edgeInd-1].getFromNode()
        connections = self.route[prevInd].getConnections(self.route[edgeInd])
        if(len(connections)==0):
            print("aaaaaaaaa")
            return None
        connection = connections[0]
        linkInd = inter.getLinkIndex(connection)
        if(linkInd == -1): # Ca devrait pas arriver, mais de toute évidence ça arrive
            print(self.id, "fu3")
            return;
        resp = inter._prohibits[linkInd] # Si je me souvient bien les variables précédées d'un _ doivent pas être touchées?
        connRaw = inter.getConnections()
        conn = [0] * len(resp)
        for c in connRaw:
            ind = inter.getLinkIndex(c)
            if(ind == -1):
                continue
            conn[ind] = c
        cars = []
        for i,f in enumerate(reversed(resp)):
            if(f == '0'):
                continue
            edge = conn[i].getFrom()
            laneInd = conn[i].getFromLane().getIndex() 
            intLane = self.map.getLane(conn[i].getViaLaneID())
            intLaneLgt = intLane.getLength()
            carsInEdge = self.controller.getCarsOnLane(edge.getID(), laneInd)
            carsInEdge = list(filter(lambda c: c.nextNonSpecialEdge() == conn[i].getTo(), carsInEdge))
            #carsInEdge = list(carsInEdge)
            #carsInEdge = list(filter(lambda c: c.nextNonSpecialEdge() == self.nextNonSpecialEdge(), carsInEdge))
            if len(carsInEdge) != 0:
                carsInEdge = zip([self.getCarDist(c, edge, laneInd)+intLaneLgt for c in carsInEdge], carsInEdge)
                closest = min(carsInEdge, key=lambda c: c[0])
                cars.append(closest)
            intEdge = intLane.getEdge()
            carsInEdge = list(self.controller.getCarsOnLane(intEdge.getID(), intLane.getIndex()))
            if len(carsInEdge) != 0:
                carsInEdge = zip([self.getCarDist(c, intEdge, intLane.getIndex()) for c in carsInEdge], carsInEdge)
                closest = min(carsInEdge, key=lambda c: c[0])
                cars.append(closest)

        if(len(cars) == 0):
            return None
        cDist,closest = min(cars, key=lambda c: c[0])
        return (cDist,closest)

    def turnNext(self):
        return self.cligno[self.index] != self.CLIGNO_NONE

    def nextNonSpecialEdge(self):
        return next(filter(lambda e: not e.isSpecial(), islice(self.route, self.index + 1, None)))

    def draw(self,painter):
        pt = QPointF(*self.pos)
        if self.forceThrough:
            painter.setPen(Qt.blue)
        elif self.leader is None:
            painter.setPen(Qt.gray)
        painter.drawEllipse(pt,self.size,self.size)
        painter.drawLine(self.pos[0], self.pos[1], self.pos[0]+5*cos(self.dir), self.pos[1]+5*sin(self.dir))
        
        if len(self.cligno) != 0 and self.cligno[self.index] != self.CLIGNO_NONE:
            painter.save()
            painter.setPen(Qt.yellow)
            d = self.dir + pi/2
            if self.cligno[self.index] == self.CLIGNO_RIGHT:
                d += pi
            painter.drawEllipse(pt + self.size * QPointF(cos(d),sin(d)), 3, 3)
            painter.restore()

        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()
        #elSize = self.v * self.T
        #painter.drawEllipse(pt,elSize, elSize)

    def conduite(self,vmax,leader,dt):
        """if self.id == "f_00" and self.controller.t%10>5:
        
            self.v = 0
            return
        """

        if(leader is None):
            self.v = self.vmax
            self.updateGraph(self.v, vmax, 0)
            return
            vleader=50#self.v
            bleader=self.b
        else:
            vleader=leader.v # vitesse de la voiture leader
            bleader=leader.b

        if(self.leaderBefore):
            if(vleader == 0):
                return
            if(self.distToInter > (self.T * self.vmax) or ((self.distToInter / self.vmax) < (self.leaderDist / vleader) - 2 * self.T)):
                self.v=self.vmax
            else:
                self.v = 0
            self.updateGraph(self.v, vmax, 0)
            return

        vbar=(self.v+vleader)/2
        bbar=(bleader+self.b)/2




       # S = vleader * 3.6 * 0.6
        Si=self.leaderDist-vleader*self.T
        #S=vf**2 / self.b + vleader**2 / bleader + self.gamma * vf + self.delta
        T=self.T
        vsec=vleader+(Si-vmax*T)/(vbar/bbar+T)
        vd=min(self.v+self.a*dt,vmax,vsec)
        #vf=min(va,vb2)

        #va=self.v+2.5*self.a*self.T*(1-(self.v/vd))*sqrt(0.025+(self.v/vd))
        #vb2=self.b*self.T+sqrt((self.b)**2*(self.T)**2-self.b*(2*(S-Si)-self.v*self.T-((vleader)**2/bbar)))



        self.v=max(0,vd)

        self.updateGraph(self.v, vmax, vsec)

    def conduiteGipps(self, dt): #Nope
        Va = self.v + 2.5 * self.a * dt * (1 - self.v/self.vmax) * sqrt(0.025 + self.v/self.vmax)
        #Vb = self.b * dt + sqrt(self.b**2 * dt**2 - self.b * )

    def calcTti(self, dist, v0, vmax, a):
        ttms = (vmax - v0)/a # "time to max speed", temps pris par la voiture pour atteindre la vmax à partir de sa vitesse actuelle
        delta = v0**2 + 2*a*dist # delta du trinome qui donne le temps mis pour traverser la distance dist
        tti = (-v0 + sqrt(delta))/a # "time to intersection", temps mis pour arriver à l'intersection(dist), si il n'y a pas de vmax
        sai = v0 + tti*a # "speed at intersection", vitesse qu'aura la voiture quand elle arrivera
        if tti > ttms: # si on atteint vmax avant l'intersection alors ça foire le calcul
            dbvm = a/2 * ttms**2 + v0 * ttms # "distance before vmax", distance parcouru avant d'atteindre vmax
            tti = ttms + (dist - dbvm) / vmax # la temps necessaire est donc : temps pour atteindre vmax + temps pour traverser le reste à la vitesse vmax
            sai = vmax
        return (tti, sai)

    def conduiteKrauss(self, vmax, leader, dt):
        """if self.id == "f_00" and self.controller.t%10>5: 
            self.v = 0
            return
        if self.id == "v_0" and self.controller.t > 5:
            self.v = 0
            self.updateGraph(self.leaderDist, self.distToInter, 0)
            return
        """
        if False and ("f_0" in self.id or "f_1" in self.id or "f_2" in self.id):
            self.v = 0
            return
        
        if not self.leaderBefore:
            self.forceThrough = False

        if leader is None:
            vd = min(self.v + self.a * dt, vmax)
            self.v = max(0, vd-self.nu)
            return

        vleader = leader.v
        bleader = leader.b
        
        # si on est à une intersection
        if(self.leaderBefore):
            if self.v == 0:
                self.timeAtInter += dt
            else:
                self.timeAtInter = 0
            
            # on calcule le temps qu'on va mettre à arriver à l'intersection
            # et le temps que le leader va mettre

            nextInternalIndex = self.index # Pour la voiture actuelle, dans l'ideal on calculerait la durée selon la vitesse sur chaque troncon
            while not self.route[nextInternalIndex].isSpecial(): # Mais pour l'instant on prend juste la vitesse sur le troncon interne (le plus lent en general)
                nextInternalIndex += 1
            tti, sai = self.calcTti(self.distToInter, self.v, self.route[nextInternalIndex].getLane(0).getSpeed(), self.a) # TODO : laneID
            ltti, lsai = self.calcTti(self.leaderDist, vleader, leader.vmax, leader.a)

            lta = (leader.vmax-vleader) / leader.a # temps ou le leader accelere (i.e on ne gagne pas de vitesse relative) (on considere que leader.a==self.a)
            marg = lta + (lsai-sai) / self.a # marge à prendre pour accelerer après l'intersection sans que le leader nous rattrape
            
            tts = self.v/self.b # time to stop, temps pour s'arreter si on freine mnt
            dts = self.v*tts - (self.b*tts**2)/2 # distance to stop, distance parcouru en tts si on freine

            #print(self.distToInter, self.minSpace, dts)

            # Si on est bloqué dans une dépendance circulaire
            if self.timeAtInter >= 0 and self.circularLeaderDep():
                self.forceThrough = True
                
            if self.forceThrough:
                self.leader = None # On supprime le leader (pour que seulement la premiere voiture detecte la dependance circulaire)
                self.v = min(vmax, self.v + self.a*dt)
                return

            
            # si on est suffisement loin de l'intersection (i.e on s'en fout du leader)
            # ou si on as le temps d'arriver à l'intersection avant le leader (plus un marge pour garder un distance de sécu)
            # alors on accelere pour s'inserer
            #print(tti, leader.T, marg, ltti)
            if self.distToInter > self.minSpace + dts or (tti + leader.T + marg) < ltti:
                self.v = min(vmax, self.v + self.a*dt)
                #print(self.id, "ca passe")
            else:# sinon on freine
                self.v = max(0, self.v - self.b*dt)
            self.updateGraph(self.v, vmax, 0)
            return

        vb = (vleader + self.v) / 2
        bb = (bleader + self.b) / 2
        vsec = vleader + (self.leaderDist - vleader * self.T - self.minSpace)/((vb/bb) + self.T)
        vd = min(self.v + self.a * dt, vsec, vmax)
        self.v = max(0, vd-self.nu)
        self.updateGraph(self.v, vmax, vsec)
    
    # fonction pour verifier si on as pas une dependence circulaire de leader
    def circularLeaderDep(self):
        l = self.leader
        timeout = 5
        ls = []
        while l is not None and timeout > 0:
            ls.append(l.id)
            if l.id == self.id:
                print(ls)
                return True
            timeout -= 1
            l = l.leader
        return False

    def updateGraph(self, v, vmax, vsec):
        if self.infoWidg is None:
            return
        self.signals.addGraphPt.emit((2,self.controller.t,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
        """
        if self.v < 1 and self.getCurrentEdge().isSpecial():
            print(f"{self.id} stalled where he souldn't have")
        """
        self.leader=self.getLeader(100)

        self.conduiteKrauss(self.vmax,self.leader,dt)

        if self.v == 0:
            self.timeStopped += dt

        self.speedPercentage += self.v/self.vmax
        self.ticksLived += 1

        lgt=self.v*dt

        self.dir = atan2(self.laneShape[self.laneInd+1][1]-self.pos[1],self.laneShape[self.laneInd+1][0]-self.pos[0])

        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.controller.destroyCar(self)

                    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 self.controller.vroomEnable and 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 (inter : {self.leaderBefore})"))

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