from os.path import exists
import numpy as np
import igraph as ig
import json
from geoplotlib.utils import haversine


def euler_distance(x1, y1, x2, y2):
    """
    Computes the Euler distance between two points (x1, y1) and (x2, y2).
    """
    return np.sqrt((x1 - x2) ** 2 + (y2 - y1) ** 2)


def BD72_to_WGS84(E, N):
    """
    Converts a coordinate from Belgian Lambert 72 (BD72) to GPS (WGS 84).
    This basically applies a transformation to the right coordinates, around Belgium.

    :note: BD72 is said to be not 100% accurate. While it is incredibly close, better use the
    WGS84 datasets!

    :seealso: https://epsg.io/31370, https://epsg.io/3395
    :seealso: https://gis.stackexchange.com/questions/259121/transformation-functions-for-epsg3395-projection-vs-epsg3857
    :seealso: https://pubs.usgs.gov/pp/1395/report.pdf, pp. 44-45, 63-64

    This one was used for the computations!
    :seealso: https://www.linz.govt.nz/guidance/geodetic-system/understanding-coordinate-conversions/projection-conversions/lambert-conformal-conic-geographic-transformation-formulae
    """
    a = 6378388
    f = 1./297
    lat1 = np.deg2rad(51.1666672333333)
    lat2 = np.deg2rad(49.8333339)
    lat0 = np.deg2rad(90)
    lon0 = np.deg2rad(4.36748666666667)
    N0 = 5400088.438
    E0 = 150000.013
    eps = 1e-10

    # Constants
    e = np.sqrt(2*f - f**2)
    mx = lambda latx: np.cos(latx) / np.sqrt(1 - e**2 * np.sin(latx)**2)
    m1 = mx(lat1)
    m2 = mx(lat2)
    tx = lambda latx: np.tan(np.pi/4 - latx/2) / ((1 - e * np.sin(latx))/(1 + e * np.sin(latx)))**(e/2)
    t0 = tx(lat0)
    t1 = tx(lat1)
    t2 = tx(lat2)
    n = (np.log(m1) - np.log(m2)) / (np.log(t1) - np.log(t2))
    F = m1 / (n * t1**n)
    rho0 = a * F * t0**n

    # Computation
    N -= N0
    E -= E0
    rho = np.sqrt(E**2 + (rho0 - N)**2)
    assert n * rho > 0  # check that rho has the same sign as n
    t = np.power(rho / (a * F), 1./n)
    gamma = np.arctan2(E, rho0 - N)

    lat = np.pi/2 - 2 * np.arctan(t)
    while True:
        lat2 = np.pi/2 - 2 * np.arctan(t * ((1 - e * np.sin(lat))/(1 + e * np.sin(lat)))**(e/2))
        if np.abs(lat2 - lat) < eps:
            lat = lat2
            break
        lat = lat2

    lon = gamma / n + lon0

    return np.rad2deg(lat), np.rad2deg(lon)


def get_graph():
    fname = "paths/cache.pickle.gz"
    if exists(fname):
        graph = ig.Graph.Read_Picklez(fname)
        return graph
    graph = ig.Graph()
    with open("paths/assenWGS84.json", 'r') as file:
        features = json.load(file)["features"]
    for feature in features:
        for path in feature["geometry"]["paths"]:
            traj = []
            for coord in path:
                vc, d = get_closest_vertex(graph, coord[0], coord[1])
                if d > 3.:
                    v = graph.add_vertex()
                    v["x"] = coord[0]
                    v["y"] = coord[1]
                    traj.append(v)
                else:
                    traj.append(vc)
            if len(traj) == 1:
                print("boo!", feature)
                continue
            for i in range(len(traj) - 1):
                if traj[i].index == traj[i+1].index: continue
                e = graph.add_edge(traj[i], traj[i+1])
                e["distance"] = haversine(traj[i]["x"], traj[i]["y"], traj[i+1]["x"], traj[i+1]["y"])
    return graph


def get_closest_vertex(graph, x, y):
    distance = float('inf')
    out = None
    for vertex in graph.vs:
        d = haversine(vertex["x"], vertex["y"], x, y)
        if d < distance:
            distance = d
            out = vertex
    return out, distance


def get_exact_vertex(graph, x, y):
    for vertex in graph.vs:
        if vertex["x"] == x and vertex["y"] == y:
            return vertex
    return None


def find_percentage_point_in_path(path, distances, percentage):
    tot_dist = sum(distances)
    acpos = tot_dist * percentage
    perc = 0.
    s = 0.
    p1 = path[0]
    p2 = path[-1]
    for dx, d in enumerate(distances):
        s += d
        if s > acpos:
            p1 = path[dx]
            p2 = path[dx + 1]
            p = sum(distances[dx + 1:]) / tot_dist
            if 1 - perc - p == 0:
                percentage = 1
            else:
                percentage = (percentage - perc) / (1 - perc - p)
            break
        perc = s / tot_dist
    return p1, p2, percentage


def pathfinder(graph, source, target):
    src, _ = get_closest_vertex(graph, source[0], source[1])
    tgt, _ = get_closest_vertex(graph, target[0], target[1])
    path = [source]
    dists = []
    if src.index != tgt.index:
        p = graph.get_k_shortest_paths(src.index, tgt.index, 1, weights="distance", mode="all")
        if len(p) > 0:
            for vix in p[0]:
                vertex = graph.vs[vix]
                path.append((vertex["x"], vertex["y"]))
                dists.append(haversine(path[-2][0], path[-2][1], path[-1][0], path[-1][1]))
    path.append(target)
    dists.append(haversine(path[-2][0], path[-2][1], path[-1][0], path[-1][1]))
    return path, dists



if __name__ == '__main__':
    graph = get_graph()
    # graph.write_picklez("paths/cache.pickle.gz")

    mapper = {}
    with open("paths/endpointsWGS84.json", 'r') as file:
        features = json.load(file)["features"]
    for feature in features:
        mapper[feature["attributes"]["NUMMER"]] = (feature["geometry"]["x"], feature["geometry"]["y"])

    import pandas as pd

    pf = pathfinder(graph, mapper["355"], mapper["602"])

    print(pf, sum(pf[1]))

    # berths = pd.read_csv("berths.csv", index_col=0)
    # # matrix = pd.DataFrame(columns=berths["lpl_id"].tolist(), index=berths["lpl_id"].tolist())
    #
    # bths = list(set(berths["lpl_id"]))
    # vtx = [get_closest_vertex(graph, *mapper[x])[0] for x in bths if x in mapper]
    # # vtx = list(set(vtx))
    #
    # distance = graph.distances(vtx, vtx, mode="all")
    #
    # cols = [x for x in bths if x in mapper]
    # matrix = pd.DataFrame(data=distance, columns=cols, index=cols)
    # matrix.to_csv("matrix.csv")
