import pathlib
d = pathlib.Path("/".join(__file__.split("/")[:-1]))

from scipy.spatial.transform import Rotation
import numpy as np
from numpy.linalg import norm




def visualize(filename):
    def car_place(i, sgn=1):
        coord = [x*sign[i]*sgn for x in cholesky[inp[i]]]
        return place(coord)
    def place(coord):
        coord = np.array(coord[:3])
        coord = coord / norm(coord)
        return rot.apply(coord)
    def format_place(coord):
        return f"({coord[0]}, {coord[1]})"

    def f_car_place(*args, **kwargs):
        return format_place(car_place(*args, **kwargs))

    def is_front_car(i):
        return car_place(i)[2] > 0


    def car_color(i):
        return "blue" if sum(a*b*sign[i] for a,b in zip(cholesky[inp[i]], cut)) > 0 else "red"

    with open(d/filename) as f:
        data = f.read().split("\n")
        inp = list(map(int, data[0].split()))
        n = len(inp)//2
        cholesky = [list(map(float, i.split())) for i in data[1:n+1]]
        cut = list(map(float, data[n+2].split())) 
    rot = Rotation.align_vectors([(1,0,0)], [cut])[0]
    print(rot)
    print(cut)
    print(rot.apply(cut))

    for i in cholesky:
        assert sum(x**2 for x in i) > 0.99, f"Dimension higher then input ({sum(x**2 for x in i)})"

    out = []
    type_sum = [0 for _ in range(n)]
    for i in range(2*n):
        type_sum[inp[i]] += i
    sign = [-1 if type_sum[inp[i]] > 2*i else 1 for i in range(2*n)]
    out.append("unitsize(80);")
    out.append("""
    path edge_path(pair a, pair b)
{
    path pa = a--b;
	real x = intersections(pa,circle(a, 0.017))[0][0];
	real y = intersections(pa,circle(b, 0.017)).pop()[0];
	return subpath(pa,x,y);
}
   """)

    for i in range(2*n):
        if not is_front_car(i):
            out.append(f"filldraw(circle({f_car_place(i)},0.015), light{car_color(i)});")
    for i in range(2*n-1):
        out.append(f"draw(edge_path({f_car_place(i, -1)}, {f_car_place(i+1, -1)}), gray(0.8));")
    for i in range(2*n-1):
        out.append(f"draw(edge_path({f_car_place(i)}, {f_car_place(i+1)}));")
    out.append(f"draw(circle((0,0),1));")
    out.append(f"draw((0,-1.1)-- (0, 1.1));")
    for i in range(2*n):
        if is_front_car(i):
            out.append(f"filldraw(circle({f_car_place(i)},0.015), {car_color(i)});")
    # out.append(f"filldraw(circle({format_place(place(cut))},0.01));")

    return "\n".join(out)