#!/usr/bin/env python3
#
# Copyright (C) 2014 - 2021 Johannes Schauer Marin Rodrigues <josch@mister-muffin.de>
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.

import os
import math
from math import sqrt
import numpy
from scipy import interpolate
from itertools import tee
from PIL import Image, ImageDraw
import urllib.request
import matplotlib.path
import matplotlib.transforms
import xml.etree.ElementTree as ET
import mapnik
import cairo

TILESIZE = 256
EARTHRADIUS = 6378137


def haversine(lon1, lat1, lon2, lat2):
    lon1 = math.radians(lon1)
    lat1 = math.radians(lat1)
    lon2 = math.radians(lon2)
    lat2 = math.radians(lat2)
    dlon = lon2 - lon1
    dlat = lat2 - lat1
    a = (
        math.sin(dlat / 2) ** 2
        + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2
    )
    return EARTHRADIUS * 2 * math.asin(math.sqrt(a))


def lat2y(lat, zoom):
    return math.log(math.tan(math.pi / 4 + math.radians(lat) / 2)) * EARTHRADIUS


def lon2x(lon, zoom):
    return math.radians(lon) * EARTHRADIUS


def pairwise(iterable):
    "s -> (s0,s1), (s1,s2), (s2,s3), ..."
    a, b = tee(iterable, 2)
    next(b, None)
    return zip(a, b)


def triplewise(iterable):
    "s -> (s0,s1,s2), (s1,s2,s3), (s2,s3,s4), ..."
    a, b, c = tee(iterable, 3)
    next(b, None)
    next(c, None)
    next(c, None)
    return zip(a, b, c)


def intersects(p0, p1, p2, p3):
    s10_x = p1[0] - p0[0]
    s10_y = p1[1] - p0[1]
    s32_x = p3[0] - p2[0]
    s32_y = p3[1] - p2[1]

    denom = s10_x * s32_y - s32_x * s10_y

    if denom == 0:
        return False  # collinear

    denom_is_positive = denom > 0

    s02_x = p0[0] - p2[0]
    s02_y = p0[1] - p2[1]

    s_numer = s10_x * s02_y - s10_y * s02_x

    if (s_numer < 0) == denom_is_positive:
        return False  # no collision

    t_numer = s32_x * s02_y - s32_y * s02_x

    if (t_numer < 0) == denom_is_positive:
        return False  # no collision

    if (s_numer > denom) == denom_is_positive or (t_numer > denom) == denom_is_positive:
        return False  # no collision

    return True


def main():
    import sys

    if len(sys.argv) != 4:
        print("usage: %s data.gpx mapwidth paperwidth" % sys.argv[0])
        exit(1)

    zoom = 10
    latmin = math.inf
    latmax = -1

    path = []
    with open(sys.argv[1]) as f:
        root = ET.parse(f)
        for trkpt in root.findall(
            "./gpx:trk/gpx:trkseg/gpx:trkpt",
            {"gpx": "http://www.topografix.com/GPX/1/1"},
        ):
            lat = float(trkpt.attrib["lat"])
            lon = float(trkpt.attrib["lon"])
            if lat < latmin:
                latmin = lat
            if lat > latmax:
                latmax = lat
            # apply mercator projection
            path.append((lon, lat))

    merc = mapnik.Projection('+proj=merc +a=6378137 +b=6378137 +lat_ts=0.0 +lon_0=0.0 +x_0=0.0 +y_0=0 +k=1.0 +units=m +nadgrids=@null +no_defs +over')
    longlat = mapnik.Projection('+proj=longlat +ellps=WGS84 +datum=WGS84 +no_defs')
    #m = mapnik.Map(800, 600)
    #mapnik.load_map_from_string(m, open("/tmp/openstreetmap-carto/mapnik.xml").read(), False, "/tmp/openstreetmap-carto/")
    #gpxstyle = mapnik.Style()
    #gpxrule = mapnik.Rule()
    #gpxsym = mapnik.LineSymbolizer()
    #gpxsym.stroke = mapnik.Color('red')
    #gpxsym.stroke_width = 5
    #gpxsym.stroke_opacity = 0.5
    #gpxrule.symbols.append(gpxsym)
    #gpxstyle.rules.append(gpxrule)
    #m.append_style('GPXStyle', gpxstyle)
    #gpxlayer = mapnik.Layer('GPXLayer')
    ##gpxlayer.srs =
    #gpxlayer.datasource = mapnik.Ogr(file = sys.argv[1], layer = 'tracks')
    #gpxlayer.styles.append('GPXStyle')
    #m.layers.append(gpxlayer)
    #m.aspect_fix_mode = mapnik.aspect_fix_mode.GROW_BBOX
    #m.srs = merc.params()
    #m.zoom_to_box(mapnik.ProjTransform(longlat, merc).forward(gpxlayer.envelope()))
    #mapnik.render_to_file(m, "out.png", "png", 1.0)

    length = 0
    for (lon1, lat1), (lon2, lat2) in pairwise(path):
        length += haversine(lon1, lat1, lon2, lat2)

    dpi = 96  # because we use bitmap tiles instead of vectors
    mapwidthm = float(sys.argv[2])  # map width in m
    paperwidthm = float(sys.argv[3])  # paper width in m
    earth = 6378137  # earth equator radius in m
    width = dpi / 0.0254 * paperwidthm # width in px
    zoom = math.ceil(
        math.log2(
            2
            * math.pi
            * earth
            * math.cos(math.radians((latmax + latmin) / 2))
            * width
            / (mapwidthm * TILESIZE)
        )
    )
    subdiv = math.ceil(4*length/mapwidthm)
    subdiv = 40
    width = 15000
    print("zoom:", zoom)
    print("length:", length)
    print("subdiv:", subdiv)

    path = [(lon2x(lon, zoom), lat2y(lat, zoom)) for lon, lat in path]

    halfwidth = width / 2.0
    found_smoothing = False
    #for smoothing in [2 ** i for i in range(30)]:
    for smoothing in [2 ** 35]:
        tck, u = interpolate.splprep(list(zip(*path)), s=smoothing)
        unew = numpy.linspace(0, 1.0, subdiv + 1)
        out = interpolate.splev(unew, tck)
        # prepend and append a segment
        out = (
            [2 * out[0][0] - out[0][1]] + list(out[0]) + [2 * out[0][-1] - out[0][-2]],
            [2 * out[1][0] - out[1][1]] + list(out[1]) + [2 * out[1][-1] - out[1][-2]],
        )
        heights = []
        offs = []
        height = 0.0
        for (ax, ay), (bx, by) in pairwise(zip(*out)):
            s = ax - bx
            t = ay - by
            l = sqrt(s * s + t * t)
            offs.append(height)
            height += l
            heights.append(l)
        # the border of the first segment is just perpendicular to the path
        cx = -out[1][1] + out[1][0]
        cy = out[0][1] - out[0][0]
        cl = sqrt(cx * cx + cy * cy) / halfwidth
        dx = out[1][1] - out[1][0]
        dy = -out[0][1] + out[0][0]
        dl = sqrt(dx * dx + dy * dy) / halfwidth
        px = [out[0][0] + cx / cl]
        py = [out[1][0] + cy / cl]
        qx = [out[0][0] + dx / dl]
        qy = [out[1][0] + dy / dl]
        for (ubx, uby), (ux, uy), (uax, uay) in triplewise(zip(*out)):
            # get adjacent line segment vectors
            ax = ux - ubx
            ay = uy - uby
            bx = uax - ux
            by = uay - uy
            # normalize length
            al = sqrt(ax * ax + ay * ay)
            bl = sqrt(bx * bx + by * by)
            ax = ax / al
            ay = ay / al
            bx = bx / bl
            by = by / bl
            # get vector perpendicular to sum
            cx = -ay - by
            cy = ax + bx
            cl = sqrt(cx * cx + cy * cy) / halfwidth
            px.append(ux + cx / cl)
            py.append(uy + cy / cl)
            # and in the other direction
            dx = ay + by
            dy = -ax - bx
            dl = sqrt(dx * dx + dy * dy) / halfwidth
            qx.append(ux + dx / dl)
            qy.append(uy + dy / dl)
        # the border of the last segment is just perpendicular to the path
        cx = -out[1][-1] + out[1][-2]
        cy = out[0][-1] - out[0][-2]
        cl = sqrt(cx * cx + cy * cy) / halfwidth
        dx = out[1][-1] - out[1][-2]
        dy = -out[0][-1] + out[0][-2]
        dl = sqrt(dx * dx + dy * dy) / halfwidth
        px.append(out[0][-1] + cx / cl)
        py.append(out[1][-1] + cy / cl)
        qx.append(out[0][-1] + dx / dl)
        qy.append(out[1][-1] + dy / dl)
        quads = []
        for (p2x, p2y, p1x, p1y), (p3x, p3y, p0x, p0y) in pairwise(zip(px, py, qx, qy)):
            quads.append(((p0x, p0y), (p1x, p1y), (p2x, p2y), (p3x, p3y)))
        # check for convex quads (sides intersect)
        have_convex = False
        for (p0, p1, p2, p3) in quads:
            if intersects(p0, p3, p1, p2):
                have_convex = True
                break
        if have_convex:
            print("have convex")
            continue
        ## check for quads that look too much like a triangle
        # have_triangle = False
        # for ((p00, p01), (p10, p11), (p20, p21), (p30, p31)) in quads:
        #    len1 = sqrt((p10-p00)**2+(p11-p01)**2)
        #    len2 = sqrt((p30-p20)**2+(p31-p21)**2)
        #    if len1/len2 > 100:
        #        have_triangle = True
        #        break
        #    if len2/len1 < 1/100:
        #        have_triangle = True
        #        break
        # if have_triangle:
        #    continue
        # draw
        polygon = []
        for ((p00, p01), (p10, p11), (p20, p21), (p30, p31)) in quads:
            polygon.append((p10, p11))
            polygon.append((p00, p01))
        for ((p00, p01), (p10, p11), (p20, p21), (p30, p31)) in reversed(quads):
            polygon.append((p30, p31))
            polygon.append((p20, p21))
        polygon.append(polygon[0])

        # check if path is inside polygon
        if matplotlib.path.Path(polygon).contains_path(
            matplotlib.path.Path(path)
        ):
            found_smoothing = True
            break
        print("doesn't contain path")
    if not found_smoothing:
        print("cannot find smoothing")
        exit(1)
    assert (
        len(out[0])
        == len(out[1])
        == len(px)
        == len(py)
        == len(qx)
        == len(qy)
        == len(quads) + 1
        == len(heights) + 1
        == len(offs) + 1
    )

    minx = math.inf
    maxx = -1
    miny = math.inf
    maxy = -1
    for (xi, yi) in polygon:
        if xi < minx:
            minx = xi
        if xi > maxx:
            maxx = xi
        if yi < miny:
            miny = yi
        if yi > maxy:
            maxy = yi
    print(minx, maxx, miny, maxy)

    m = mapnik.Map(800, 600)
    mapnik.load_map_from_string(m, open("/tmp/openstreetmap-carto/mapnik.xml").read(), False, "/tmp/openstreetmap-carto/")
    gpxstyle = mapnik.Style()
    gpxrule = mapnik.Rule()
    gpxsym = mapnik.LineSymbolizer()
    gpxsym.stroke = mapnik.Color('blue')
    gpxsym.stroke_width = 5
    gpxsym.stroke_opacity = 0.5
    gpxrule.symbols.append(gpxsym)
    gpxstyle.rules.append(gpxrule)
    m.append_style('GPXStyle', gpxstyle)
    gpxlayer = mapnik.Layer('GPXLayer')
    #gpxlayer.srs =
    gpxlayer.datasource = mapnik.Ogr(file = sys.argv[1], layer = 'tracks')
    gpxlayer.styles.append('GPXStyle')
    m.layers.append(gpxlayer)
    m.aspect_fix_mode = mapnik.aspect_fix_mode.GROW_BBOX
    m.zoom_to_box(mapnik.Box2d(mapnik.Coord(minx, miny), mapnik.Coord(maxx, maxy)))
    m.srs = merc.params()
    surface = cairo.SVGSurface("out.svg", 800, 600)
    ctx = cairo.Context(surface)
    mapnik.render(m, ctx)
    for pos in zip(*out):
        pos = m.view_transform().forward(mapnik.Coord(*pos))
        ctx.line_to(pos.x, pos.y)
    ctx.set_source_rgb(0,0,0)
    ctx.stroke()
    for i, (p0, p1, p2, p3) in enumerate(quads):
        p0 = m.view_transform().forward(mapnik.Coord(*p0))
        p1 = m.view_transform().forward(mapnik.Coord(*p1))
        p2 = m.view_transform().forward(mapnik.Coord(*p2))
        p3 = m.view_transform().forward(mapnik.Coord(*p3))
        if i == 1:
            ctx.arc(p0.x, p0.y, 10, 0, 2*math.pi)
            ctx.set_source_rgb(1,0,0)
            ctx.fill()
            ctx.arc(p1.x, p1.y, 10, 0, 2*math.pi)
            ctx.set_source_rgb(0,1,0)
            ctx.fill()
            ctx.arc(p2.x, p2.y, 10, 0, 2*math.pi)
            ctx.set_source_rgb(0,0,1)
            ctx.fill()
            ctx.arc(p3.x, p3.y, 10, 0, 2*math.pi)
            ctx.set_source_rgb(1,1,0)
            ctx.fill()
        ctx.move_to(p0.x, p0.y)
        ctx.line_to(p1.x, p1.y)
        ctx.line_to(p2.x, p2.y)
        ctx.line_to(p3.x, p3.y)
        ctx.close_path()
        ctx.set_source_rgb(1,0,0)
        ctx.stroke()
    surface.finish()

    vertices = []
    triangles = []
    for i, (off, h, (p0, p1, p2, p3)) in enumerate(zip(offs, heights, quads)):
        v1 = (p0[0], p0[1], width, off+h) # top right
        v2 = (p1[0], p1[1], width, off)   # bottom right
        v3 = (p2[0], p2[1], 0, off)       # bottom left
        v4 = (p3[0], p3[1], 0, off+h)     # top left
        vertices.extend([v3, v2, v1])
        triangles.append((len(vertices)-3, len(vertices)-2, len(vertices)-1))
        vertices.extend([v3, v1, v4])
        triangles.append((len(vertices)-3, len(vertices)-2, len(vertices)-1))

    with open("/tmp/tinshift.json", "w") as f:
        print("""
{
  "file_type": "triangulation_file",
  "format_version": "1.0",
  "transformed_components": [ "horizontal" ],
  "fallback_strategy": "nearest",
  "vertices_columns": [ "source_x", "source_y", "target_x", "target_y" ],
  "triangles_columns": [ "idx_vertex1", "idx_vertex2", "idx_vertex3" ],
  "vertices": [ %s ],
  "triangles": [ %s ]
}
""" % (','.join(["[ %f, %f, %f, %f ]"%v for v in vertices]), ','.join(["[%d, %d, %d]"%t for t in triangles])), file=f)

    tinshift = mapnik.Projection("+proj=pipeline +step +proj=webmerc +step +proj=tinshift +file=/tmp/tinshift.json")
    m = mapnik.Map(1600, int(1600*height/width))
    mapnik.load_map_from_string(m, open("/tmp/openstreetmap-carto/mapnik.xml").read(), False, "/tmp/openstreetmap-carto/")
    gpxstyle = mapnik.Style()
    gpxrule = mapnik.Rule()
    gpxsym = mapnik.LineSymbolizer()
    gpxsym.stroke = mapnik.Color('blue')
    gpxsym.stroke_width = 5
    gpxsym.stroke_opacity = 0.5
    gpxrule.symbols.append(gpxsym)
    gpxstyle.rules.append(gpxrule)
    m.append_style('GPXStyle', gpxstyle)
    gpxlayer = mapnik.Layer('GPXLayer')
    gpxlayer.datasource = mapnik.Ogr(file = sys.argv[1], layer = 'tracks')
    gpxlayer.styles.append('GPXStyle')
    m.layers.append(gpxlayer)
    m.aspect_fix_mode = mapnik.aspect_fix_mode.GROW_BBOX
    m.zoom_to_box(mapnik.Box2d(mapnik.Coord(0, 0), mapnik.Coord(width, height)))
    m.srs = tinshift.params()
    surface = cairo.PDFSurface("out.pdf", 1600, int(1600*height/width))
    mapnik.render(m, surface)
    surface.finish()


    #minx = math.inf
    #maxx = -1
    #miny = math.inf
    #maxy = -1
    #for (xi, yi) in polygon:
    #    if xi < minx:
    #        minx = xi
    #    if xi > maxx:
    #        maxx = xi
    #    if yi < miny:
    #        miny = yi
    #    if yi > maxy:
    #        maxy = yi
    #im1 = Image.new("RGB", (int(maxx - minx), int(maxy - miny)))
    #im2 = Image.new("RGB", (int(maxx - minx), int(maxy - miny)))
    #opener = urllib.request.build_opener()
    #opener.addheaders = [("User-agent", "mapbender")]
    #urllib.request.install_opener(opener)
    #todl = []
    #for i in range(int(minx / TILESIZE) - 1, int(maxx / TILESIZE) + 2):
    #    for j in range(int(miny / TILESIZE) - 1, int(maxy / TILESIZE) + 2):
    #        os.makedirs("%d/%d" % (zoom, i), exist_ok=True)
    #        fname = "%d/%d/%d.png" % (zoom, i, j)
    #        if not matplotlib.path.Path(numpy.array(polygon)).intersects_bbox(
    #            matplotlib.transforms.Bbox(
    #                [
    #                    (i * TILESIZE, j * TILESIZE),
    #                    (
    #                        (i + 1) * TILESIZE,
    #                        (j + 1) * TILESIZE,
    #                    ),
    #                ]
    #            )
    #        ):
    #            continue
    #        if not os.path.exists(fname):
    #            todl.append((i, j))
    #for n, (i, j) in enumerate(todl):
    #    print("%d/%d" % (n, len(todl)))
    #    fname = "%d/%d/%d.png" % (zoom, i, j)
    #    urllib.request.urlretrieve(
    #        #"https://tile.openstreetmap.org/%d/%d/%d.png" % (zoom, i, j),
    #        #https://a.tile.thunderforest.com/cycle/17/68690/44518.png?apikey=6170aad10dfd42a38d4d8c709a53
    #        "https://tile.thunderforest.com/cycle/%d/%d/%d.png?apikey=d8f470ce7a8e4dd0acf39cc8fd3cf979" % (zoom, i, j),
    #        #"https://tile.thunderforest.com/outdoors/%d/%d/%d.png?apikey=d8f470ce7a8e4dd0acf39cc8fd3cf979" % (zoom, i, j),
    #        #"https://tile.thunderforest.com/landscape/%d/%d/%d.png?apikey=d8f470ce7a8e4dd0acf39cc8fd3cf979" % (zoom, i, j),
    #        #"https://tile.thunderforest.com/atlas/%d/%d/%d.png?apikey=d8f470ce7a8e4dd0acf39cc8fd3cf979" % (zoom, i, j),
    #        filename=fname,
    #    )
    #for i in range(int(minx / TILESIZE) - 1, int(maxx / TILESIZE) + 2):
    #    for j in range(int(miny / TILESIZE) - 1, int(maxy / TILESIZE) + 2):
    #        if not matplotlib.path.Path(numpy.array(polygon)).intersects_bbox(
    #            matplotlib.transforms.Bbox(
    #                [
    #                    (i * TILESIZE, j * TILESIZE),
    #                    (
    #                        (i + 1) * TILESIZE,
    #                        (j + 1) * TILESIZE,
    #                    ),
    #                ]
    #            )
    #        ):
    #            continue
    #        fname = "%d/%d/%d.png" % (zoom, i, j)
    #        with Image.open(fname) as tile:
    #            im1.paste(tile, (int(i * TILESIZE - minx), int(j * TILESIZE - miny)))
    #            im2.paste(tile, (int(i * TILESIZE - minx), int(j * TILESIZE - miny)))
    #draw2 = ImageDraw.Draw(im2)
    #draw2.line([(xi - minx, yi - miny) for xi, yi in path], fill=(255, 0, 0), width=4)
    #draw1 = ImageDraw.Draw(im1)
    #draw1.line([(xi - minx, yi - miny) for xi, yi in path], fill=(255, 0, 0), width=4)
    #draw1.line([(xi - minx, yi - miny) for xi, yi in zip(*out)], fill=(0, 255, 0))
    #for ((p00, p01), (p10, p11), (p20, p21), (p30, p31)) in quads:
    #    draw1.polygon(
    #        [
    #            (p00 - minx, p01 - miny),
    #            (p10 - minx, p11 - miny),
    #            (p20 - minx, p21 - miny),
    #            (p30 - minx, p31 - miny),
    #        ]
    #    )
    #draw1.polygon([(xi - minx, yi - miny) for xi, yi in polygon], outline=(0, 0, 255))
    #im1.save("out2.png")

    #data = []
    #for i, (off, h, (p0, p1, p2, p3)) in enumerate(zip(offs, heights, quads)):
    #    data.append(
    #        (
    #            (
    #                0,
    #                int(height - offs[i] - heights[i]),
    #                int(width),
    #                int(height - offs[i]),
    #            ),
    #            (
    #                p0[0] - minx,
    #                p0[1] - miny,
    #                p1[0] - minx,
    #                p1[1] - miny,
    #                p2[0] - minx,
    #                p2[1] - miny,
    #                p3[0] - minx,
    #                p3[1] - miny,
    #            ),
    #        )
    #    )
    #im_out = im2.transform(
    #    (int(width), int(height)),
    #    Image.MESH,
    #    data,
    #    Image.BICUBIC,
    #)
    #im_out.save("out.png")
    #im_out.save("out.jpg", quality=95)

    #return True


if __name__ == "__main__":
    main()