import geopandas as gpd
import matplotlib
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
from math import *
from shapely.geometry import Polygon, LineString


def make_bbox(long0, lat0, long1, lat1):
    return Polygon(
        [[l, lat0] for l in range(long0, long1)] + [[long1, p] for p in range(lat0, lat1)] +
        [[l, lat1] for l in range(long1, long0, -1)] + [[long0, p] for p in range(lat1, lat0, -1)])

# matplotlib.use('pgf')
# matplotlib.rcParams.update({
#     'pgf.texsystem': 'pdflatex',
#     'font.family': 'serif',
#     'text.usetex': True,
#     'pgf.rcfonts': False,
#     'figure.figsize':[7.2,7.2]
# })

bbox = make_bbox(-35, 30, 55, 75)
bbox_gdf = gpd.GeoDataFrame(index=[0], crs='epsg:4326', geometry=[bbox])
europe = gpd.read_file('https://naciscdn.org/naturalearth/50m/cultural/ne_50m_admin_0_countries.zip')
europe = europe.overlay(bbox_gdf, how="intersection")
fig, ax = plt.subplots()

bbox_gdf.to_crs('epsg:3035').plot(linewidth=1.2, ax=ax, edgecolor='#0000bb', facecolor='#eeeeff', legend=True)
europe.to_crs('epsg:3035').plot(linewidth=0.8, ax=ax, edgecolor='#ff8888', facecolor='#bbffbb', legend=True)
ax.set_axis_off()
ax.set_frame_on(False)
grat = gpd.read_file('https://naciscdn.org/naturalearth/50m/physical/ne_50m_graticules_5.zip')
grat = grat.overlay(bbox_gdf, how="intersection")
grat.to_crs('epsg:3035').plot(linewidth=1.2, ax=ax, edgecolor='#0000bb', legend=True)
labels = True


def lines(**kwargs):
    global labels
    x = np.arange(-35, 55.5, 0.5)
    y = np.arange(30, 75.75, 0.75)
    X, Y = np.meshgrid(x, y)
    Z = np.array([[degrees(tissot(*map(radians, (P, L)))) for L in x] for P in y])
    cs = plt.contour(X, Y, Z, [0.25,0.5,1.0,1.5,2.0,3.0,4.0,5.0,6.0])
    isolines = gpd.GeoDataFrame(geometry=gpd.GeoSeries())
    isolines['deg'] = None
    j = 0
    for i in range(len(cs.allsegs)):
        for line in cs.allsegs[i]:
            if len(line) > 0:
                poly = LineString(line)
                isolines.at[j, 'geometry'] = poly
                isolines.at[j, 'deg'] = cs.levels[i]
                j += 1
    isolines = isolines.set_crs('epsg:4326').to_crs('epsg:3035')
    isolines.plot(ax=ax, alpha=0.75, **kwargs)
    if labels:
        labels = False
        for idx, row in isolines.iterrows():
            plt.annotate(text=str(row['deg']) + '°', xy=row['geometry'].representative_point().coords[0],
                         horizontalalignment='center', verticalalignment='center', weight='bold', size=8)


def rad2dms(dd):
    dd = degrees(dd)
    mult = -1 if dd < 0 else 1
    mnt, sec = divmod(abs(dd) * 3600, 60)
    deg, mnt = divmod(mnt, 60)
    return '%d°%d\'%f"' % (mult * deg, mnt, sec)


def N(Phi):
    return a / sqrt(1 - e2 * sin(Phi) ** 2)


def M(Phi):
    return a * (1 - e2) / (1 - e2 * sin(Phi) ** 2) ** (3 / 2)


def auth(Phi):
    sP = sin(Phi)
    if e == 0:
        return sP
    else:
        return sP / 2 / (1 - e2 * sP ** 2) + 1 / 4 / e * log((1 + e * sP) / (1 - e * sP))


Phi0 = radians(52)
Lam0 = radians(10)


def ell2sph(Phi, Lam):
    phi = asin(A * auth(Phi) + kap)
    lam = n * (Lam - Lam0)
    return phi, lam


def sph2ell(phi, lam):
    q = (sin(phi) - kap) / A
    Phi = phi
    while True:
        print(rad2dms(Phi))
        Phi_ = Phi
        Phi = (Phi + (1 - e2 * sin(Phi) ** 2) / cos(Phi) * (q - auth(Phi)))
        if abs(Phi - Phi_) < 1e-12:
            break
        if abs(Phi) > pi / 2:
            Phi = Phi_ - copysign(1e-3, Phi_)
    Lam = lam / n + Lam0
    return Phi, Lam


def sph2azi(phi, lam):
    sp = sin(phi)
    cp = cos(phi)
    sl = sin(lam)
    cl = cos(lam)
    d = sqrt(1 + sp * sp0 + cp * cp0 * cl)
    x = R * sqrt2 * sl * cp / d
    y = R * sqrt2 * (sp * cp0 - cp * sp0 * cl) / d
    return x, y


def azi2sph(x, y):
    t = (x ** 2 + y ** 2) / 2 / R ** 2
    sq2mt = sqrt(2 - t)
    phi = asin(y * sq2mt * cp0 / sqrt2 / R + sp0 * (1 - t))
    lam = atan2(x * sq2mt, sqrt2 * R * (1 - t) * cp0 - y * sq2mt * sp0)
    return phi, lam


def ell2azi(Phi, Lam):
    x, y = sph2azi(*ell2sph(Phi, Lam))
    return x / m, y * m


def dell2azi(Phi, Lam):
    phi, lam = ell2sph(Phi, Lam)
    sp = sin(phi)
    cp = cos(phi)
    sl = sin(lam)
    cl = cos(lam)
    dpdP = M(Phi) * N(Phi) * cos(Phi) / R ** 2 / cp / n
    dldL = n
    sp_ = sp * sp0 + cp * cp0 * cl
    if sp_ >= 1:
        dxdP = 0
        dydP = R * m * dpdP
        dxdL = R * cp / m * dldL
        dydL = 0
    else:
        dsp_ = 1 / sqrt(1 - sp_ ** 2)
        dp_dp = (cp * sp0 - sp * cp0 * cl) * dsp_
        dp_dl = -cp * cp0 * sl * dsp_
        sl_cp_ = sl * cp
        cl_cp_ = cp * sp0 * cl - sp * cp0
        dtl_ = 1 / (sl_cp_ ** 2 + cl_cp_ ** 2)
        dl_dp = (-sl * sp * cl_cp_ - (-sp * sp0 * cl - cp * cp0) * sl_cp_) * dtl_
        dl_dl = (cl * cp * cl_cp_ + cp * sp0 * sl * sl_cp_) * dtl_
        dxdl_ = R * sqrt2 * cl_cp_ / sqrt(1 + sp_)
        dydl_ = R * sqrt2 * sl_cp_ / sqrt(1 + sp_)
        dxdp_ = -R / sqrt2 * sl_cp_ / sqrt(1 - sp_)
        dydp_ = R / sqrt2 * cl_cp_ / sqrt(1 - sp_)
        dxdP = (dxdp_ * dp_dp + dxdl_ * dl_dp) / m * dpdP
        dydP = (dydp_ * dp_dp + dydl_ * dl_dp) * m * dpdP
        dxdL = (dxdp_ * dp_dl + dxdl_ * dl_dl) / m * dldL
        dydL = (dydp_ * dp_dl + dydl_ * dl_dl) * m * dldL
    return (dxdP, dxdL), (dydP, dydL)


def tissot(Phi, Lam):
    j = dell2azi(Phi, Lam)
    mp = M(Phi)
    np = N(Phi) * cos(Phi)
    h = sqrt(j[0][0] ** 2 + j[1][0] ** 2) / mp
    k = sqrt(j[0][1] ** 2 + j[1][1] ** 2) / np
    p = (j[1][0] * j[0][1] - j[0][0] * j[1][1]) / mp / np
    if h ** 2 + k ** 2 < 2 * p:
        apb = 2 * sqrt(p)
        amb = 0
    else:
        apb = sqrt(h ** 2 + k ** 2 + 2 * p)
        amb = sqrt(h ** 2 + k ** 2 - 2 * p)
    w = 2 * asin(amb / apb)
    return w


Phig = range(30, 78, 12)
Lamg = range(10, 55, 18)
sqrt2 = sqrt(2)

R = 1
a = 1
e = e2 = 0
phi0 = Phi0
cp0 = cos(phi0)
sp0 = sin(phi0)
kap = 0
n = 1
m = 1
A = 1
wg = tuple(round(degrees(tissot(*map(radians, (Phi, Lam)))), 4) for Lam in Lamg for Phi in Phig)
print(rad2dms(phi0))
lines(linestyle='--', linewidth=1.5, edgecolor='#000077', label='Spherical estimate')

a = 6378137
f = 1 / 298.257223563
e2 = 2 * f - f ** 2
e = sqrt(e2)
e_2 = e2 / (1 - e2)
n = 1
R = sqrt(a ** 2 / 2 * (1 + (1 - e2) / 2 / e * log((1 + e) / (1 - e))))
A = a ** 2 * (1 - e2) / R ** 2 / n
kap = 0
phi0 = asin(A * auth(Phi0))
cp0 = cos(phi0)
sp0 = sin(phi0)
m = R * cp0 / N(Phi0) / cos(Phi0)
ws = tuple(round(degrees(tissot(*map(radians, (Phi, Lam)))), 4) for Lam in Lamg for Phi in Phig)
print(rad2dms(phi0))
lines(linestyle='-.', linewidth=1.25, edgecolor='#007700', label='Snyder\'s formulae')
print(*(lambda x, y: (x + 4321000, y + 3210000))(*ell2azi(*map(radians, (30, 45)))))

phi0 = atan(tan(Phi0) / sqrt(1 + e_2 * cos(Phi0) ** 2))
cp0 = cos(phi0)
sp0 = sin(phi0)
if sp0 == 0:
    n = sqrt(1 + e_2 * cos(Phi0) ** 2)
else:
    n = sin(Phi0) / sp0
R = sqrt(M(Phi0) * N(Phi0))
A = a ** 2 * (1 - e2) / R ** 2 / n
kap = sp0 - A * auth(Phi0)
m = 1
wk = tuple(round(degrees(tissot(*map(radians, (Phi, Lam)))), 4) for Lam in Lamg for Phi in Phig)

print(rad2dms(phi0))
lines(linewidth=0.75, edgecolor='#ff0000', label='Low-distortion sphere')
print(*(lambda x, y: (x + 4321000, y + 3210000))(*ell2azi(*map(radians, (30, 45)))))

for i in zip((Phi for Lam in Lamg for Phi in Phig), (Lam for Lam in Lamg for Phi in Phig), wk, ws, wg):
    print(*i)
ax.legend(fontsize=8)
plt.show()
#plt.savefig('laea.pgf',bbox_inches=0,transparent=True)

print(tuple(map(rad2dms, sph2ell(*map(radians, (90, 0))))))
print(kap,n)