import rclpy
from rclpy.node import Node
from custom_messages.msg import Mypath2 
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(Point, '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()

    def try_compute_and_publish(self):
        if self.latest_gps_msg is not None and self.latest_route_msg is not None:
            out = self.closest_point(self.latest_route_msg, self.latest_gps_msg)
            self.clpt_publisher.publish(out)
            self.latest_gps_msg = None
            self.latest_route_msg = None
        
    def closest_point(self, msg1: Mypath2, msg2: Point):
       
        out = Point()
        
        closest_point = None
        min_distance = float('inf')

        route = msg1.poses
        route_new = [(route[i].x, route[i].y) for i in range(len(route))]
        point = (msg2.x, msg2.y)
        buffer_radius = 1000

        for coord in route_new:
            distance = math.dist(coord,point)

            if distance <= buffer_radius and distance < min_distance:
                min_distance = distance
                closest_point = coord

        out.x = closest_point[0]
        out.y = closest_point[1]
        
        return out

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