# import statements
import rclpy
from rclpy.node import Node
from custom_messages.msg import Mypath2
from geometry_msgs.msg import Point
from std_msgs.msg import Float32
import math


class Correction(Node):
    
    # Node initialization with the subscription to the desired topics
    def __init__(self):
        super().__init__("correction")
        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.imu_subscriber = self.create_subscription(Float32, '/imu', self.imu_callback, 10)
        self.correction_publisher = self.create_publisher(Float32, '/correction',10)
        
        # Initialization of the GPS, path and IMU messages  
        self.latest_gps_msg = None
        self.latest_route_msg = None
        self.latest_imu_msg = None
        self.timer = self.create_timer(1, self.try_compute_and_publish)
        
    # Verify the correct reception of the GPS message     
    def gps_callback(self, msg: Point):
        self.latest_gps_msg = msg
        self.try_compute_and_publish()
        # self.get_logger().info("I heard: " + str(self.latest_gps_msg))

    # Verify the correct reception of the route message     
    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])

    # Verify the correct reception of the IMU message     
    def imu_callback(self, msg: Float32):
        self.latest_imu_msg = msg
        self.try_compute_and_publish()
        # self.get_logger().info("I heard: " + str(self.latest_imu_msg))

    # Once correctly received all the messages, compute the correction to be applied to vehicle's heading
    def try_compute_and_publish(self):
        if self.latest_gps_msg is not None and self.latest_route_msg is not None and self.latest_imu_msg is not None:
            result = self.correction(self.latest_route_msg, self.latest_gps_msg, self.latest_imu_msg)
            self.correction_publisher.publish(result)
            self.latest_gps_msg = None
            self.latest_route_msg = None
            self.latest_imu_msg = None
        
    def correction(self, msg1: Mypath2, msg2: Point, msg3: Float32):
        
        # Message initialization
        out = Float32()
        
        # Research of the closest point on trajectory, compared to the GPS singal
        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]
        
        # Retrieval of the correct direction value
        theta_correct = closest_point.theta
        
        # Retrieval of the emulated IMU sinal
        theta_actual = msg3.data
        
        # Application of the correction
        out.data = theta_actual-theta_correct

        return out

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