import rclpy
from rclpy.node import Node
from custom_messages.msg import Mypath2, MyPose 
from geometry_msgs.msg import Point
import math


class ClosestPoint(Node):
    
    def __init__(self):
        super().__init__("closest_point")
        self.pose_subscriber = self.create_subscription(Mypath2, '/route_coord', self.route_callback, 10)
        self.gps_subscriber = self.create_subscription(Point, '/gps', self.gps_callback, 10)
        self.clpt_publisher = self.create_publisher(MyPose, '/closest_point',10)
        self.latest_gps_msg = None
        self.latest_route_msg = None
        self.timer = self.create_timer(1, self.try_compute_and_publish)
        
    def gps_callback(self, msg: Point):
        self.latest_gps_msg = msg
        self.try_compute_and_publish()
        # self.get_logger().info("The closest point is: " + str(self.latest_gps_msg))

    def route_callback(self, msg: Mypath2):
        self.latest_route_msg = msg
        self.try_compute_and_publish()
        # self.get_logger().info('I heard: "%s"' % msg.poses[1])

    def try_compute_and_publish(self):
        if self.latest_gps_msg is not None and self.latest_route_msg is not None:
            result = self.closest_point(self.latest_route_msg, self.latest_gps_msg)
            self.clpt_publisher.publish(result)
            self.latest_gps_msg = None
            self.latest_route_msg = None
        
    def closest_point(self, msg1: Mypath2, msg2: Point):
        out = MyPose()
        closest_point = None
        min_distance = float('inf')
        buffer_radius = 20
        for i in range(len(msg1.poses)):
            distance = math.sqrt((msg1.poses[i].x - msg2.x)**2 + (msg1.poses[i].y - msg2.y)**2)

            if distance <= buffer_radius and distance < min_distance:
                min_distance = distance
                closest_point = msg1.poses[i]
        out = closest_point

        return out

def main(args=None):
    rclpy.init(args=args)
    node = ClosestPoint()
    rclpy.spin(node)
    rclpy.shutdown()