import sys

import numpy as np
import rospy
import tf2_ros
from dynamic_reconfigure.server import Server
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64
from geometry_msgs.msg import PointStamped
from fs_msgs.msg import FSDS_ControlCommand
from tf.transformations import euler_from_quaternion

from controller.cfg import ControllerConfig
from controller.path.utils import calculate_path_deviations, moving_average
from controller.ros_interfaces.longitudinal_controller import LongitudinalControllerInterface
from controller.ros_interfaces.mpc import MPCInterface
from controller.ros_interfaces.pure_pursuit import PurePursuitInterface

from fs_msgs.msg import PlannedPath, CarCommand

from ros2can_msgs.msg import dv_control_target


class ROSInterface(object):
    def __init__(self, node_name='controller', use_dynamic_reconfigure=True):
        self._initialize_node(node_name)

        self._wheelbase = 1.5
        self._rate = rospy.Rate(rospy.get_param('~publish_frequency'))

        # Inputs
        #input_names = ['path_array', 'speed_profile', 'steering', 'vehicle_pose', 'vehicle_speed']


        input_names = ['path_array', 'speed_profile', 'vehicle_pose', 'vehicle_speed']
        self.inputs = {name: None for name in input_names}




        # lateral controller evaluation variables
        self.sum_path_deviation = 0
        self.path_deviation_points_evaluated = 0


        

        # Components
        self.components = {
            #'longitudinal_controller': LongitudinalControllerInterface(),
            #'mpc': MPCInterface(self.wheelbase),
            'pure_pursuit': PurePursuitInterface(rospy.get_param('~track_name'))

        }

        # Parameters
        self.parameters = {}
        for component in self.components.values():
            component.add_custom_parameters_to_server()

        self._load_parameters()

        # Subscribers real car
        rospy.Subscriber('~path', PlannedPath, self._cb_path)
        rospy.Subscriber('~odometry', Odometry, self._cb_odometry)
        rospy.Subscriber('~steering', Float64, self._cb_steering)


        #subscribers simulation

        rospy.Subscriber('/fsds/testing_only/odom', Odometry, self._cb_odometry)






        # Publishers
        self.publishers = {
            'commands': rospy.Publisher('~commands', dv_control_target, queue_size=1),
            #'commands': rospy.Publisher('/ros2can/send/dv_control_target', dv_control_target, queue_size=1),
            'simulation_control_commands' : rospy.Publisher('/fsds/control_command',FSDS_ControlCommand,queue_size=1),
            'steering_target_publisher': rospy.Publisher('~steering_target', Float64, queue_size=1)
        }
        self.debug_publishers = {
            'lateral_deviation': rospy.Publisher('~metrics/lateral_deviation', Float64, queue_size=1),
            'angular_deviation': rospy.Publisher('~metrics/angular_deviation', Float64, queue_size=1),
            'vehicle_position': rospy.Publisher('debug_vehicle_position', PointStamped, queue_size=1),
        }

        # Dynamic reconfigure
        self.dynamic_reconfigure_parameters = {}
        self._initialize_dynamic_reconfigure(use_dynamic_reconfigure)

        # Expose components (e.g. ros_interface.pure_pursuit -> PurePursuitInterface())
        for component_name, component_value in self.components.items():
            setattr(self, component_name, component_value)

    @property
    def parameter_names(self):
        # Initialize with parameter names that are not defined in any ROSInterfaceComponent
        parameter_names = [
            # 'path_frame', (Exception: This parameter comes from _cb_path)
            'points_averaged_over_pn',
            'points_averaged_over_pp',
            'publish_frequency',
            'steering_angle_max',
            'steering_angle_min',
            'use_moving_average_pn',
            'use_moving_average_pp',
            'enable_adaptive_pure_pursuit'
        ]

        for component in self.components.values():
            parameter_names += component.parameter_names

        # Remove any duplicate parameter names
        parameter_names = list(set(parameter_names))

        return parameter_names

    @property
    def wheelbase(self):
        if self._wheelbase is not None:
            return self._wheelbase

        tf_buffer = tf2_ros.Buffer()
        tf2_ros.TransformListener(tf_buffer)

        try:
            transform = tf_buffer.lookup_transform(
                rospy.get_param('~rear_axle_frame'),
                rospy.get_param('~front_axle_frame'),
                rospy.Time(),
                rospy.Duration(2.0)
            )
        except tf2_ros.TransformException as e:
            rospy.logerr("[controller] {}".format(e))
            return None

        self._wheelbase = transform.transform.translation.x
        rospy.set_param('~wheelbase', self._wheelbase)

        return self._wheelbase

    def _initialize_node(self, node_name):
        myargv = rospy.myargv(argv=sys.argv)
        debug = myargv[1].lower() == 'true' if len(myargv) == 2 else False

        rospy.init_node(node_name, log_level=rospy.DEBUG if debug else rospy.ERROR)

    def _load_parameters(self):
        """ Loads parameters for main_interface and for each component """
        parameters = {
            parameter_name: rospy.get_param('~{}'.format(parameter_name))
            for parameter_name in self.parameter_names
        }

        for component in self.components.values():
            component.load_parameters(parameters)

        self.parameters = parameters


    def _initialize_dynamic_reconfigure(self, use_dynamic_reconfigure):
        if not use_dynamic_reconfigure:
            return

        dynamic_reconfigure_parameter_names = []
        for component in self.components.values():
            dynamic_reconfigure_parameter_names += component.dynamic_reconfigure_parameter_names

        # Remove any duplicate parameter names
        self._dynamic_reconfigure_parameter_names = list(set(dynamic_reconfigure_parameter_names))

        Server(ControllerConfig, self._cb_dynamic_reconfigure)

    # Callbacks

    def _cb_dynamic_reconfigure(self, config, levels):
        for parameter_name in self._dynamic_reconfigure_parameter_names:
            self.dynamic_reconfigure_parameters[parameter_name] = config[parameter_name]
        return config

    def _cb_odometry(self, odometry):


      
        x = odometry.pose.pose.position.x
        y = odometry.pose.pose.position.y

        
        orientation_q = odometry.pose.pose.orientation
        orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
        yaw = euler_from_quaternion(orientation_list)[2]

        self.inputs['vehicle_pose'] = np.array([x, y, yaw])


        v_pose = self.inputs['vehicle_pose'][0:2]
        goal_point_msg = PointStamped()
        goal_point_msg.header.stamp = rospy.Time.now()
        goal_point_msg.header.frame_id = self.parameters["path_frame"]
        goal_point_msg.point.x = v_pose[0]
        goal_point_msg.point.y = v_pose[1]
        goal_point_msg.point.z = 0
        

        self.debug_publishers["vehicle_position"].publish(goal_point_msg)


        x_speed = odometry.twist.twist.linear.x
        y_speed = odometry.twist.twist.linear.y

        self.inputs['vehicle_speed'] = np.sqrt(x_speed*x_speed + y_speed*y_speed)


        # if self.is_ready():

        #     dev,psi = calculate_path_deviations(self.inputs['vehicle_pose'],self.inputs["path_array"])
        #     self.sum_path_deviation += dev*dev 
        #     self.path_deviation_points_evaluated += 1

        



    def _cb_path(self, path):


        if self.parameters.get('use_moving_average_pp', False):
            path.curvatures = moving_average(
                path.curvatures, self.parameters.get('points_averaged_over_pp', 0)
            )

        self.parameters['path_frame'] = path.header.frame_id
        self.inputs['path_array'] = np.transpose(
            np.array([path.x, path.y, path.curvatures, path.speed_profile])
        )
        self.inputs['speed_profile'] = path.speed_profile

    def _cb_steering(self, steering):
        self.inputs['steering'] = steering.data

    # General publishers


    

    def publish_commands(self, steering_angle, torque, brake):

        # print("pub dv target")

        steering_angle = steering_angle or 0
        torque = torque
     
        steering_angle_max = 25

        steering_angle = (-1)*180 * steering_angle / np.pi

        torque_max = 1000
        

        if steering_angle >= steering_angle_max:
            steering_angle = steering_angle_max
        elif steering_angle <= (-1)*steering_angle_max:
            steering_angle = (-1)*steering_angle_max


        if torque >= torque_max:
            torque = torque_max
        elif torque <= (-1)*torque_max:
            torque = (-1)*torque_max

  
        commands_msg = dv_control_target()
        #print(commands_msg)
        commands_msg.dv_mission_status = 3
        commands_msg.dv_speed_target = 0
        commands_msg.dv_motor_moment_target = int(round(torque))
        commands_msg.dv_brake_hydralic_target = int(round(brake))
        commands_msg.dv_steering_angle_target = steering_angle

        # print(commands_msg)


        self.publishers['commands'].publish(commands_msg)





    def publish_commands_simulation_pp(self, steering_angle, torque, brake,pid):
        OSQP_NUMERICAL_ERROR = 0.01

        steering_angle = steering_angle or 0
     

        steering_angle_max = 25
        steering_angle_min = -25

        steering_angle = (-1)*180 * steering_angle / np.pi
        


        steering_angle = steering_angle/steering_angle_max

        steering_angle = steering_angle
        
    
        if steering_angle >= steering_angle_max + OSQP_NUMERICAL_ERROR:
            rospy.logwarn(
                '[controller] Limiting the steering angle, from {} to {}'
                .format(np.rad2deg(steering_angle), np.rad2deg(steering_angle_max))
            )
        elif steering_angle <= steering_angle_min - OSQP_NUMERICAL_ERROR:
            rospy.logwarn(
                '[controller] Limiting the steering angle, from {} to {}'
                .format(np.rad2deg(steering_angle), np.rad2deg(steering_angle_min))
            )

        if steering_angle >= 1:
            #steering_angle = steering_angle_max
            steering_angle = 1
        elif steering_angle <= -1:
            #steering_angle = steering_angle_min
            steering_angle = -1

        commands_msg = FSDS_ControlCommand()


        commands_msg.throttle = torque
        commands_msg.brake = brake
        commands_msg.steering = steering_angle
        


        self.publishers['simulation_control_commands'].publish(commands_msg)

        #self.publishers['commands'].publish(commands_msg)
        

    def publish_lateral_controller_metrics(self):
        e_y, e_psi = calculate_path_deviations(
            self.inputs['vehicle_pose'],
            self.inputs['path_array']
        )

        if None in (e_y, e_psi) or self.path_deviation_points_evaluated == 0:
            print("no pub")
            return

        self.debug_publishers['lateral_deviation'].publish(self.sum_path_deviation/self.path_deviation_points_evaluated)
        #self.debug_publishers['lateral_deviation'].publish(self.sum_path_deviation)
        self.debug_publishers['angular_deviation'].publish(e_psi)

    # General methods

    def is_ready(self):
        inputs_received = all(v is not None for v in self.inputs.values())
  
        parameters_loaded = all(
            parameter_name in self.parameters
            for parameter_name in self.parameter_names
        )
        #print("inputs: %s, params: %s" % (inputs_received , parameters_loaded))
        #print(self.i)
        return inputs_received and parameters_loaded

    def sleep(self):
        try:
            self._rate.sleep()
        except rospy.exceptions.ROSInterruptException as e:
            rospy.loginfo(
                '[controller] Program terminated. Exception: {}'
                .format(e)
            )

    @staticmethod
    def is_shutdown():
        return rospy.is_shutdown()