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 
from geometry_msgs.msg import Quaternion
import utm

class MyPathFinder(Node):
    
    def __init__(self):
        super().__init__("path_finder")
        self.declare_parameter('source_geo', [48.7438, 9.0861])
        self.declare_parameter('dest_geo', [48.7479, 9.1029])
        self.declare_parameter('filename', '/home/andrea/campus.osm')
        self.declare_parameter('alpha', 0.2)
        self.declare_parameter('accuracy', 7)
        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
        alpha = self.get_parameter('alpha').get_parameter_value().double_value
        accuracy = self.get_parameter('accuracy').get_parameter_value().integer_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, alpha, accuracy))
    

    def dir_traj(self, trajectory, pt, conv=False):
    
        try:
            ind = trajectory.index(pt)
            if ind == 0:
                return 0

            prev_point = trajectory[ind - 1]
            next_point = trajectory[ind + 1] if ind + 1 < len(trajectory) else trajectory[ind]

            delta_x1, delta_y1 = trajectory[ind][0] - prev_point[0], trajectory[ind][1] - prev_point[1]
            prev_angle = math.atan2(delta_y1, delta_x1)

            delta_x2, delta_y2 = next_point[0] - trajectory[ind][0], next_point[1] - trajectory[ind][1]
            next_angle = math.atan2(delta_y2, delta_x2)

            dir_angle = prev_angle - next_angle

            if conv:
                return math.degrees(dir_angle)

            return dir_angle

        except ValueError:
            return 0

        except IndexError:
            return 0

    def router_callback(self, source_geo, dest_geo, filename, alpha, accuracy): 

        # 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

        # Generates smoothed path where necessary        
        smoothed_path = [] # Initialization of the smoothed list
  
        for i in range(len(routeLatLons)-1): # Calculate the control points for the Bezier curve (only if the angular deviation is higher then pi/6)    
            
            smoothed_path.append(routeLatLons[i]) # Anyway append every point of the original list into the smoothed one
            
            if self.dir_traj(routeLatLons, routeLatLons[i]) > np.pi/6: # Check whether smoothing is necessary
                smoothed_path.remove(routeLatLons[i])
                x0, y0 = routeLatLons[i-1]
                x1, y1 = routeLatLons[i]
                x2, y2 = routeLatLons[i+1]

                cx1 = x1 + alpha * (x2 - x1)
                cy1 = y1 + alpha * (y2 - y1)
                cx0 = x1 - alpha * (x1 - x0)
                cy0 = y1 - alpha * (y1 - y0)

                nodes = np.array([(cx0, cy0), (x1,y1),(cx1, cy1)]).T # Build the Bezier curve and evaluate its coordinates
                curve = bezier.Curve.from_nodes(nodes)
                x_vals = np.linspace(0, 1, accuracy)
                y_vals = curve.evaluate_multi(x_vals)
            
                for k in range(y_vals.shape[1]): # Append the trajectory of the smoothed path
                    smoothed_path.append((y_vals[0][k],y_vals[1][k]))
            
        smoothed_path = list(dict.fromkeys(smoothed_path)) # Remove duplicates by converting the list into a dict (which by definition cannot have duplicates) and then back to a list 
        routeLatLons = smoothed_path

        # 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]

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

        routeLatLons = list(zip(smoothed_x, smoothed_y))
        routeLatLons = self.to_utm(routeLatLons)

        poses = []
        for coord in routeLatLons:
            pose = Quaternion()
            pose.x = coord[0]
            pose.y = coord[1]
            pose.w = self.yaw(routeLatLons, coord)
            poses.append(pose)
             
        msg.poses = poses
        
        # msg.mypath = [coord for coord_pair in routeLatLons for coord in coord_pair]
        self.route_publisher_.publish(msg)
        
    def to_utm(self, route):
        route = [(utm.from_latlon(route[i][0], route[i][1])[0], utm.from_latlon(route[i][0], route[i][1])[1]) for i in range(len(route))]

        return route


    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()