# import statements
import rclpy
from rclpy.node import Node
from pyroutelib3 import Router
import numpy as np
import bezier
import math
from custom_messages.msg import Mypath2, MyPose 
import utm
from scipy.interpolate import splprep, splev

class MyPathFinder(Node):
    
    # Node initialization with the default locations and map
    def __init__(self):
        super().__init__("path_finder")
        self.declare_parameter('source_geo', [48.75115,9.10454])
        self.declare_parameter('dest_geo', [48.74422,9.09843])
        self.declare_parameter('filename', '/home/andrea/campus.osm') # change directory according to the position of the .osm file in your pc 
        start = self.get_parameter('source_geo').get_parameter_value().double_array_value
        end = self.get_parameter('dest_geo').get_parameter_value().double_array_value
        filename = self.get_parameter('filename').get_parameter_value().string_value
        self.route_publisher_ = self.create_publisher(Mypath2, '/route_coord',10)
        timer_period = 1  # seconds
        self.timer = self.create_timer(timer_period, lambda: self.router_callback(start, end, filename))
    
    # Function generating the reference trajectory
    def router_callback(self, source_geo, dest_geo, filename): 

        # Message initialization
        msg = Mypath2()

        # Initialize the router with the desired mean of transport and the map file 
        router = Router('car', filename)
        start = router.findNode(source_geo[0],source_geo[1]) # Find start and end nodes
        end = router.findNode(dest_geo[0],dest_geo[1])

        status, route = router.doRoute(start, end) # Find the route - a list of OSM nodess

        if status == 'success':
            routeLatLons = list(map(router.nodeLatLon, route)) # Get actual route coordinates

        # Increase the density of points
        distances = [math.dist((x0, y0), (x1, y1)) for (x0, y0), (x1, y1) in zip(routeLatLons, routeLatLons[1:])]  # Calculate distances
        minimum = min(distances)

        # Precompute range values
        ranges = [range(math.floor(dist / minimum)) for dist in distances]

        augm_x, augm_y = [], []
        for (x0, y0), (x1, y1), r in zip(routeLatLons, routeLatLons[1:], ranges):
            augm_x.extend(np.linspace(x0, x1, len(r)))
            augm_y.extend(np.linspace(y0, y1, len(r)))

        routeLatLons = list(zip(augm_x, augm_y))
        routeLatLons = self.to_utm(routeLatLons)

        # Create the spline 
        no_rep = list(dict.fromkeys(routeLatLons))
        coords = np.array(no_rep)
        tck, u = splprep(coords.T, s=25)
        new_points = splev(u, tck)
        routeLatLons = list(zip(new_points[0], new_points[1]))

        # Define the message 
        poses = []
        for coord in routeLatLons:
            pose = MyPose()
            pose.x = coord[0]
            pose.y = coord[1]
            pose.theta = self.yaw(routeLatLons, coord)
            poses.append(pose)
             
        msg.poses = poses
        msg.frame_id.stamp = self.get_clock().now().to_msg()
        msg.frame_id.frame_id = 'map'

        self.route_publisher_.publish(msg)
        
    # Cooridnates conversion to UTM values for better handling    
    def to_utm(self, route):
        x, y = zip(*route)
        out = utm.from_latlon(np.array(x), np.array(y))
        route_conv = list(zip(out[0], out[1]))

        return route_conv

    # Function determining the angle between two consecutive points
    def yaw(self, traj, pt):
        ind = traj.index(pt)
        if ind + 1 < len(traj):
            next_point = traj[ind + 1]
            delta_x, delta_y = next_point[0]-pt[0], next_point[1]-pt[1]
            yaw = math.degrees(math.atan2(delta_y, delta_x))
        else:
            yaw = 0.0

        return yaw


def main(args=None):
    rclpy.init(args=args)
    node = MyPathFinder()
    # node.use_parameters()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()
