import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
from car import Car

### is thrown if a Car is too close to its previous platoon member
class UnderCIPDerror(Exception):
  pass

def checkInbound(argument, target, delta):    #Ueberall ist ein delta von 0.01 hinterlegt
    if argument > target + delta:
        return False
    elif argument < target - delta:
        return False
    else:
        return True

def broadcastPlattonSettings(carList, newIPD, newPS):
    for item in carList:
        item.PS = newPS
        item.IPD = newIPD

def update(carList):
    for item in carList:
        item.updatePos()
    for item in reversed(carList):
        item.updateSpeed()

if __name__ == "__main__":
    maxT   = [20, 40, 80, 110, 130,150,170]     # times for events
    setIPD = [40, 40, 40,  60,  45, 30, 15]     # event of IPD (activ until time event)
    setPS  = [20, 30, 10,  10,  10, 10, 10]     # event of PS (activ until time event)

    v   = [52.0, 50.0, 10.0]                    # startvektor (Startgeschwindikeit)
    pos = [90, 30, 0]                           # startvektor (Startposition)
    tps = 60
    
    cars = []
    cars.append(Car(None,    v[0], pos[0], setIPD[0], setPS[0],tps))
    cars.append(Car(cars[0], v[1], pos[1], setIPD[0], setPS[0],tps))
    cars.append(Car(cars[1], v[2], pos[2], setIPD[0], setPS[0],tps))

    t = []
    IPD= []
    PS = []
   
    y_pos = [[],[],[]]
    y_speed = [[],[],[]]
    y_cSpeed = [[],[],[]]
    y_dist  = [[],[],[]]
    y_cDist = [[],[],[]]
    y_cIPD = [[],[],[]]
    y_v_v = [[],[],[]]
    
    i = 0.0; end = False
    for j in range(0, len(maxT)):
        newIPD = setIPD[j]
        newPS  = setPS[j]
        broadcastPlattonSettings(cars, newIPD, newPS)
        
        while i < maxT[j]:
            t.append(i)
            IPD.append(newIPD)
            PS.append(newPS)

            for k in range(0, len(cars)):
                y_pos[k].append(cars[k].cPos)
                y_speed[k].append(cars[k].getSpeed())
                y_cSpeed[k].append(cars[k].cSpeed)
                y_v_v[k].append(cars[k].v_v)

            for k in range(1, len(cars)):
                y_dist[k].append(cars[k].getDistance())
                y_cDist[k].append(cars[k-1].cPos - cars[k].cPos)
                y_cIPD[k].append(cars[k].getCIPD())

            i = i+1/tps
            try:
                update(cars)
            except UnderCIPDerror:
                print("catch")
                end = True  
                break
        if end: break


    plt.subplot(221)
    plt.plot(t, y_pos[0], 'r', label='pos LV')
    plt.plot(t, y_pos[1], 'b', label='pos FV1')
    plt.plot(t, y_pos[2], 'g', label='pos FV2')
    plt.title("Position")
    plt.legend()

    plt.subplot(222)
    plt.plot(t, IPD, 'r--', label='IPD')
    plt.plot(t, y_dist[1], 'b-', label='assumed dist FV1')
    plt.plot(t, y_cDist[1], 'b--', label='real dist FV1')
    plt.plot(t, y_cIPD[1], 'b-.', label='cIPD')
    plt.plot(t, y_dist[2], 'g-', label='assumed dist FV1')
    plt.plot(t, y_cDist[2], 'g--', label='real dist FV1')
    plt.plot(t, y_cIPD[2], 'g-.', label='cIPD')
    plt.title("Distance")
    plt.legend()
    
    plt.subplot(223)
    plt.plot(t, PS, 'm--', label='PS')
    # plt.plot(t, y_speed[0], 'r-', label='assumed speed LV')
    plt.plot(t, y_cSpeed[0], 'r--', label='real speed LV')
    plt.plot(t, y_v_v[1], 'b:', label='speed estiamtion FV1 for LV')
    # plt.plot(t, y_speed[1], 'b-', label='assumed speed FV1')
    # plt.plot(t, y_cSpeed[1], 'b--', label='real speed FV1')
    plt.title("Speed")
    plt.legend()
    
    plt.subplot(224)
    plt.plot(t, PS, 'm--', label='PS')
    # plt.plot(t, y_speed[1], 'b-', label='assumed speed FV1')
    plt.plot(t, y_cSpeed[1], 'b--', label='real speed FV1')
    plt.plot(t, y_v_v[2], 'g:', label='speed estiamtion FV2 for FV1')
    # plt.plot(t, y_speed[2], 'g-', label='assumed speed FV1')
    # plt.plot(t, y_cSpeed[2], 'g--', label='real speed FV1')
    plt.legend()

    plt.show()