#!/usr/bin/env python
import os
import tempfile

import lanelet2
import lanelet2.geometry
import lanelet2.io
import lanelet2.traffic_rules
import lanelet2.routing
from lanelet2.core import (
    AllWayStop,
    AttributeMap,
    BasicPoint2d,
    BoundingBox2d,
    Lanelet,
    LaneletMap,
    LaneletWithStopLine,
    LineString3d,
    Point2d,
    Point3d,
    RightOfWay,
    TrafficLight,
    getId,
)
from lanelet2.projection import UtmProjector, MercatorProjector, LocalCartesianProjector

example_file = os.path.join(
    os.path.dirname(os.path.abspath(__file__)),
    "../../lanelet2_maps/res/mapping_example.osm",
)
if not os.path.exists(example_file):
    # location after installing
    example_file = os.path.join(
        os.path.dirname(os.path.abspath(__file__)),
        "../../share/lanelet2_maps/res/mapping_example.osm",
    )


def tutorial():
    # We do our best to keep the python interface in sync with the c++ implementation. As a rule of thumb: Everything
    # you can do in c++ works pretty similar in python, we just "pythonized" some things. Therefore this tutorial only
    # shows you just the most important things and the things that are different from C++. For the rest have a look
    # at the c++ tutorial.
    part1primitives()
    part2regulatory_elements()
    part3lanelet_map()
    part4reading_and_writing()
    part5traffic_rules()
    part6routing()


def part1primitives():
    # Primitives work very similar to c++, except that the data can be accessed as properties instead of functions
    p = Point3d(getId(), 0, 0, 0)
    assert p.x == 0
    p.id = getId()
    p.attributes["key"] = "value"
    assert "key" in p.attributes
    assert p.attributes["key"] == "value"

    # the 2d/3d mechanics work too
    p2d = lanelet2.geometry.to2D(p)

    # all (common) geometry calculations are available as well:
    p2 = Point3d(getId(), 1, 0, 0)
    assert lanelet2.geometry.distance(p, p2) == 1
    assert lanelet2.geometry.distance(p2d, Point2d(getId(), 1, 0, 1)) == 1

    # LineStrings work conceptually similar to a list (but they only accept points, of course)
    ls = LineString3d(getId(), [p, p2])
    assert ls[0] == p
    assert ls[-1] == p2
    assert p in ls
    for pt in ls:
        assert pt.y == 0

    ls_inv = ls.invert()
    assert ls_inv[0] == p2
    ls.append(Point3d(getId(), 2, 0, 0))
    del ls[2]


def part2regulatory_elements():
    # regulatory elements profit from pythons type system

    # TrafficLight
    lanelet = get_a_lanelet()
    light = get_linestring_at_y(3)
    traffic_light_regelem = TrafficLight(getId(), AttributeMap(), [light])
    lanelet.addRegulatoryElement(traffic_light_regelem)
    assert traffic_light_regelem in lanelet.regulatoryElements
    lights = [regelem for regelem in lanelet.regulatoryElements if isinstance(regelem, TrafficLight)]
    assert traffic_light_regelem in lights
    assert light in lights[0].trafficLights

    # RightOfWay
    stop_linestring = get_linestring_at_y(0)
    right_of_way_lanelets = [get_a_lanelet(), get_a_lanelet(1)]
    yielding_lanelets = [get_a_lanelet(2)]
    right_of_way_regelem = RightOfWay(
        getId(),
        AttributeMap(),
        right_of_way_lanelets,
        yielding_lanelets,
        stop_linestring,
    )
    map = LaneletMap()
    map.add(yielding_lanelets[0])
    map.add(right_of_way_lanelets[0])
    map.add(right_of_way_lanelets[1])
    # must add to the map explicitly
    map.add(right_of_way_regelem)
    assert right_of_way_regelem in map.regulatoryElementLayer
    right_of_ways = [regelem for regelem in map.regulatoryElementLayer if isinstance(regelem, RightOfWay)]
    assert right_of_way_regelem in right_of_ways
    # must have the circular reference from the yielding lanelet
    # otherwise, the last assertion will fail
    # this should have been automatically inferred by the regulatoryElements
    # getter function
    yielding_lanelets[0].addRegulatoryElement(right_of_way_regelem)
    # This regulatory element should affect the yielding lanelet
    assert right_of_way_regelem in yielding_lanelets[0].regulatoryElements

    # AllWayStop
    lanelets_with_stop_lines = [
        LaneletWithStopLine(get_a_lanelet(), get_linestring_at_y(0)),
        LaneletWithStopLine(get_a_lanelet(1), get_linestring_at_y(1)),
        LaneletWithStopLine(get_a_lanelet(2), get_linestring_at_y(2)),
        LaneletWithStopLine(get_a_lanelet(3), get_linestring_at_y(3)),
    ]
    map = LaneletMap()
    # add the lanelets to the map, access stop line
    for lanelet_with_stop_line in lanelets_with_stop_lines:
        map.add(lanelet_with_stop_line.lanelet)

    all_way_stop_regelem = AllWayStop(getId(), AttributeMap(), lanelets_with_stop_lines)
    # must add to the map explicitly
    map.add(all_way_stop_regelem)
    assert all_way_stop_regelem in map.regulatoryElementLayer
    all_way_stops = [regelem for regelem in map.regulatoryElementLayer if isinstance(regelem, AllWayStop)]
    assert all_way_stop_regelem in all_way_stops
    # must have the circular reference from each yielding lanelet
    # otherwise, the last assertion will fail
    # this should have been automatically inferred by the regulatoryElements
    # getter function
    for lanelet_with_stop_line in lanelets_with_stop_lines:
        lanelet_with_stop_line.lanelet.addRegulatoryElement(all_way_stop_regelem)
    # This regulatory element should affect each yielding lanelet
    for lanelet_with_stop_line in lanelets_with_stop_lines:
        assert all_way_stop_regelem in lanelet_with_stop_line.lanelet.regulatoryElements


def part3lanelet_map():
    # lanelets map work just as you would expect:
    map = LaneletMap()
    lanelet = get_a_lanelet()
    map.add(lanelet)
    assert lanelet in map.laneletLayer
    assert map.pointLayer
    assert not map.areaLayer
    assert len(map.pointLayer.nearest(BasicPoint2d(0, 0), 1)) == 1
    search_box = BoundingBox2d(BasicPoint2d(0, 0), BasicPoint2d(2, 2))
    assert len(map.pointLayer.search(search_box)) > 1

    # you can also create a map from a list of primitives (replace Lanelets by the other types)
    map_bulk = lanelet2.core.createMapFromLanelets([get_a_lanelet()])
    assert len(map_bulk.laneletLayer) == 1


def part4reading_and_writing():
    # there are two ways of loading/writing a lanelet map: a robust one and an normal one. The robust one returns found
    # issues as extra return parameter
    map = LaneletMap()
    lanelet = get_a_lanelet()
    map.add(lanelet)
    path = os.path.join(tempfile.mkdtemp(), "mapfile.osm")
    # Select a suitable projector depending on the data source
    #  UtmProjector: (0,0,0) is at the provided lat/lon on the WGS84 ellipsoid
    projector = UtmProjector(lanelet2.io.Origin(49, 8.4))
    #  MercatorProjector: (0,0,0) is at the provided lat/lon on the mercator cylinder
    projector = MercatorProjector(lanelet2.io.Origin(49, 8.4))
    #  LocalCartesianProjector: (0,0,0) is at the provided origin (including elevation)
    projector = LocalCartesianProjector(lanelet2.io.Origin(49, 8.4, 123))

    # Writing the map to a file
    #  1. Write with the given projector and use default parameters
    lanelet2.io.write(path, map, projector)

    #  2. Write and get the possible errors
    write_errors = lanelet2.io.writeRobust(path, map, projector)
    assert not write_errors

    #  3. Write using the default spherical mercator projector at the giver origin
    #  This was the default projection in Lanelet1
    lanelet2.io.write(path, map, lanelet2.io.Origin(49, 8.4))

    #  4. Write using the given projector and override the default values of the optional parameters for JOSM
    params = {
        "josm_upload": "true",  # value for the attribute "upload", default is "false"
        "josm_format_elevation": "true",  # whether to limit up to 2 decimals, default is the same as for lat/lon
    }
    lanelet2.io.write(path, map, projector, params)

    # Loading the map from a file
    loaded_map, load_errors = lanelet2.io.loadRobust(path, projector)
    assert not load_errors
    assert loaded_map.laneletLayer.exists(lanelet.id)


def part5traffic_rules():
    # this is just as you would expect
    traffic_rules = lanelet2.traffic_rules.create(
        lanelet2.traffic_rules.Locations.Germany,
        lanelet2.traffic_rules.Participants.Vehicle,
    )
    lanelet = get_a_lanelet()
    lanelet.attributes["vehicle"] = "yes"
    assert traffic_rules.canPass(lanelet)
    assert traffic_rules.speedLimit(lanelet).speedLimit > 1


def part6routing():
    # and this as well
    projector = UtmProjector(lanelet2.io.Origin(49, 8.4))
    map = lanelet2.io.load(example_file, projector)
    traffic_rules = lanelet2.traffic_rules.create(
        lanelet2.traffic_rules.Locations.Germany,
        lanelet2.traffic_rules.Participants.Vehicle,
    )
    graph = lanelet2.routing.RoutingGraph(map, traffic_rules)
    lanelet = map.laneletLayer[4984315]
    to_lanelet = map.laneletLayer[2925017]
    assert graph.following(lanelet)
    assert len(graph.reachableSet(lanelet, 100, 0)) > 10
    assert len(graph.possiblePaths(lanelet, 100, 0, False)) == 1

    # here we query a route through the lanelets and get all the vehicle lanelets that conflict with the shortest path
    # in that route
    route = graph.getRoute(lanelet, to_lanelet)
    path = route.shortestPath()
    conf_llts = [llt for llt in route.allConflictingInMap() if llt not in path]
    assert len(conf_llts) > 0

    # for more complex queries, you can use the forEachSuccessor function and pass it a function object
    assert has_path_from_to(graph, lanelet, to_lanelet)


def has_path_from_to(graph, start, target):
    class TargetFound(BaseException):
        pass

    def raise_if_destination(visit_information):
        # this function is called for every successor of lanelet with a LaneletVisitInformation object.
        # if the function returns true, the search continues with the successors of this lanelet.
        # Otherwise, the followers will not be visited through this lanelet, but could still be visited through
        # other lanelets.
        if visit_information.lanelet == target:
            raise TargetFound()
        else:
            return True

    try:
        graph.forEachSuccessor(start, raise_if_destination)
        return False
    except TargetFound:
        return True


def get_linestring_at_x(x):
    return LineString3d(getId(), [Point3d(getId(), x, i, 0) for i in range(0, 3)])


def get_linestring_at_y(y):
    return LineString3d(getId(), [Point3d(getId(), i, y, 0) for i in range(0, 3)])


def get_a_lanelet(index=0):
    return Lanelet(getId(), get_linestring_at_y(2 + index), get_linestring_at_y(0 + index))


if __name__ == "__main__":
    tutorial()
