import xml.etree.ElementTree as ET
from track_2_generator import create_track
from shapely.geometry import Polygon, Point, LineString
import numpy as np

from matplotlib.pyplot import plot, legend
import json

"""loading track"""
track_points = create_track(0.5,0,0, (0,0))[:-1]+[(1,0.5)]
track = Polygon(track_points)
#outside_l = create_double_circles(0.59375, (0.5,0.5),(1.5,0.5))

"""
x_shift, y_shift = 1433.0000000000002, -406.0866103896103*2#Tom: 1433.0000000000002, -406.0866103896103*2 / Damien: -540, 960
R_simu = 150#Tom: 150 / Damien: 350
R_reel = 0.5
x_shift, y_shift = x_shift/R_simu-4.77, y_shift/R_simu+2.7 #Tom:-4.77,2.7 / Damien: 1.88, -2.5
"""

def get_error(name, R_simu, center_x, center_y, start=-1, end=-1):
    
    """loading trajectory"""
    with open(name) as json_file:
        data = json.load(json_file)
    
    x_shift, y_shift = center_x/R_simu, center_y/R_simu
    
    time = data["time"]
    x_l = [(i/R_simu-x_shift)/2+1 for i in data["x"]]
    y_l = [(i/R_simu-y_shift)/2+0.5 for i in data["y"]]
    e_l = []
    
    if start>=0:
        x_l = x_l[start:]
        y_l = y_l[start:]
        time = time[start:]
    if end>=0:
        x_l = x_l[0:end]
        y_l = y_l[0:end]
        time = time[0:end]

    for x,y in zip(x_l,y_l):
        front_center = Point(np.array([x,y]))
        e = track.distance(front_center)
        e_l.append(e)
        
    return x_l, y_l, time, e_l

#plot([x for x,y in track_points], [y for x,y in track_points])
#plot([i[0] for i in outside_l], [i[1] for i in outside_l])

prefix = 'paths/'
suffix = '_normal.json'
Tom = get_error(prefix+'errorT'+suffix, 150, 1733, -666, 70, 450)
Damien = get_error(prefix+'errorD'+suffix, 350, 930, 510, 25)
plot(Damien[2],Damien[3])
plot(*Tom[2:])