import sumolib
import pygame as pg
import math

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

        inverted=not (startEdge.getToNode()==nextEdge.getFromNode() or startEdge.getToNode()==nextEdge.getToNode)
        
        laneId=0 if inverted else 1
        self.laneShape=startEdge.getLane(laneId).getShape()
        if(inverted):
            self.laneShape.reverse()
    
    def __init__(self,carID,route,parentMap,surface):
        self.id=carID
        self.route=route
        self.map=parentMap
        self.index=0
        self.laneInd=0
        
        startEdge=self.map.getEdge(route[0])
        nextEdge=self.map.getEdge(route[1])
        
        self.initPath()

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

        self.surf=surface

    def draw(self,col):
        pg.draw.circle(self.surf,col,self.map.convertPos(self.pos),5)
    
    def update(self,dt):
        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)-2):
                    self.laneInd=0
                    self.index+=1
                    if(self.index>=len(self.route)-2):
                        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