import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Quaternion
from custom_messages.msg import Mypath2 
from std_msgs.msg import Float32
import math


class NextDir(Node):
    def __init__(self):
        super().__init__("next_dir")
        self.closest_pt_subscriber = self.create_subscription(Quaternion, '/closest_point', self.clpt_callback, 10)
        self.pose_subscriber = self.create_subscription(Mypath2, '/route_coord', self.route_callback, 10)
        self.dir_publisher = self.create_publisher(Float32, 'incoming_dir',10)
        self.latest_clpt_msg = None
        self.latest_route_msg = None
        self.timer = self.create_timer(1, self.try_compute_and_publish)


    def route_callback(self, msg: Mypath2):
        self.latest_route_msg = msg
        self.try_compute_and_publish()

    def clpt_callback(self, msg: Quaternion):
        self.latest_clpt_msg = msg
        self.try_compute_and_publish()

    def try_compute_and_publish(self):
        if self.latest_clpt_msg is not None and self.latest_route_msg is not None:
            out = self.dir_traj(self.latest_route_msg, self.latest_clpt_msg)
            self.dir_publisher.publish(out)
            self.latest_clpt_msg = None
            self.latest_route_msg = None

    def dir_traj(self, msg1: Mypath2, msg2: Quaternion):
    
        out = Float32()
        route = msg1.poses
        pt = (msg2.x, msg2.y)

        try:
            ind = route.index(pt)
            if ind == 0:
                return 0

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

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

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

            dir_angle = math.degrees(prev_angle - next_angle)

            out.data = dir_angle
            
            return out

        except ValueError:
            out.data = 1.0
            return out

        except IndexError:
            out.data = 2.0
            return out
        
def main(args=None):
    rclpy.init(args=args)
    node = NextDir()
    rclpy.spin(node)
    rclpy.shutdown()
