#!/usr/bin/env python3
"""Pure-pursuit controller for the metric IPM lane centreline."""

import math
import threading

import rospy
from geometry_msgs.msg import PointStamped, Twist
from std_msgs.msg import Bool, String
from std_srvs.srv import SetBool, SetBoolResponse


class LineFollowControl:
    """Publish /cmd_vel only while the explicit enabled safety gate is true."""

    def __init__(self):
        rospy.init_node("line_follow_control")
        self.lock = threading.RLock()

        self.lane_valid_topic = rospy.get_param(
            "~lane_valid_topic", "/line_follow_debug/lane_valid"
        )
        self.lookahead_target_topic = rospy.get_param(
            "~lookahead_target_topic", "/line_follow_debug/lookahead_target"
        )
        self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "/cmd_vel")
        self.control_rate = max(1.0, float(rospy.get_param("~control_rate", 20.0)))
        self.linear_speed = max(0.0, float(rospy.get_param("~linear_speed", 0.05)))
        self.normal_linear_speed = max(
            0.0,
            float(rospy.get_param("~normal_linear_speed", self.linear_speed)),
        )
        self.after_second_linear_speed = max(
            0.0,
            float(
                rospy.get_param(
                    "~after_second_linear_speed", self.normal_linear_speed
                )
            ),
        )
        self.task_state_topic = str(
            rospy.get_param("~task_state_topic", "/traffic_line_task/state")
        )
        self.linear_speed = self.normal_linear_speed
        self.curvature_gain = max(
            0.0, float(rospy.get_param("~curvature_gain", 1.0))
        )
        self.steering_sign = float(rospy.get_param("~steering_sign", 1.0))
        self.max_angular_speed = max(
            0.0, float(rospy.get_param("~max_angular_speed", 0.35))
        )
        self.target_alpha = min(
            1.0, max(0.01, float(rospy.get_param("~target_alpha", 0.45)))
        )
        self.lateral_deadband_m = max(
            0.0, float(rospy.get_param("~lateral_deadband_m", 0.01))
        )
        self.min_target_forward_m = max(
            0.01, float(rospy.get_param("~min_target_forward_m", 0.10))
        )
        self.max_target_distance_m = max(
            self.min_target_forward_m,
            float(rospy.get_param("~max_target_distance_m", 1.50)),
        )
        self.target_timeout = max(
            0.05, float(rospy.get_param("~target_timeout", 0.50))
        )
        self.enabled = self.parameter_is_true(rospy.get_param("~enabled", False))

        self.lane_valid = False
        self.target_forward = None
        self.target_left = None
        self.last_target_time = None
        self.was_active = False

        self.cmd_pub = rospy.Publisher(self.cmd_vel_topic, Twist, queue_size=1)
        self.valid_sub = rospy.Subscriber(
            self.lane_valid_topic, Bool, self.valid_callback, queue_size=1
        )
        self.target_sub = rospy.Subscriber(
            self.lookahead_target_topic,
            PointStamped,
            self.target_callback,
            queue_size=1,
        )
        self.task_state_sub = rospy.Subscriber(
            self.task_state_topic, String, self.task_state_callback, queue_size=1
        )
        self.enable_service = rospy.Service(
            "~set_enabled", SetBool, self.set_enabled_callback
        )
        self.timer = rospy.Timer(
            rospy.Duration(1.0 / self.control_rate), self.control_callback
        )
        rospy.on_shutdown(self.shutdown)

        rospy.loginfo(
            "Pure-pursuit line controller ready; enabled=%s target=%s.",
            self.enabled,
            self.lookahead_target_topic,
        )

    @staticmethod
    def parameter_is_true(value):
        """Avoid treating the string 'false' as truthy."""
        if isinstance(value, str):
            return value.strip().lower() in ("1", "true", "yes", "on")
        return bool(value)

    def valid_callback(self, message):
        with self.lock:
            self.lane_valid = bool(message.data)

    def task_state_callback(self, message):
        requested = (
            self.after_second_linear_speed
            if message.data == "FOLLOW_AFTER_SECOND"
            else self.normal_linear_speed
        )
        with self.lock:
            previous = self.linear_speed
            self.linear_speed = requested
        if abs(previous - requested) > 1e-6:
            rospy.loginfo(
                "Line-follow speed changed to %.2f m/s for task state %s.",
                requested,
                message.data,
            )

    def set_enabled_callback(self, request):
        with self.lock:
            self.enabled = bool(request.data)
            rospy.set_param("~enabled", self.enabled)
            if not self.enabled:
                self.publish_stop()
                self.was_active = False
                self.reset_target()
                rospy.loginfo("Line controller disabled through service.")
            else:
                rospy.loginfo("Line controller enabled through service.")
            return SetBoolResponse(
                success=True,
                message="line controller %s"
                % ("enabled" if self.enabled else "disabled"),
            )

    def target_callback(self, message):
        with self.lock:
            forward = float(message.point.x)
            left = float(message.point.y)
            distance = math.hypot(forward, left)
            if (
                not math.isfinite(forward)
                or not math.isfinite(left)
                or forward < self.min_target_forward_m
                or distance > self.max_target_distance_m
            ):
                rospy.logwarn_throttle(
                    1.0,
                    "Rejected lookahead target: forward=%.3f m left=%+.3f m.",
                    forward,
                    left,
                )
                return

            if self.target_forward is None:
                self.target_forward = forward
                self.target_left = left
            else:
                alpha = self.target_alpha
                self.target_forward = (
                    alpha * forward + (1.0 - alpha) * self.target_forward
                )
                self.target_left = alpha * left + (1.0 - alpha) * self.target_left
            self.last_target_time = rospy.get_time()

    def reset_target(self):
        self.target_forward = None
        self.target_left = None
        self.last_target_time = None

    def publish_stop(self):
        self.cmd_pub.publish(Twist())

    @staticmethod
    def pure_pursuit_curvature(forward, left):
        """Return signed path curvature for a target in base_link."""
        distance_squared = forward * forward + left * left
        if distance_squared <= 1e-6:
            return None
        return 2.0 * left / distance_squared

    def shutdown(self):
        with self.lock:
            self.publish_stop()

    def control_callback(self, _event):
        with self.lock:
            self._control_locked()

    def _control_locked(self):
        # Keep the old rosparam workflow working for manual tests while the
        # task state machine uses the SetBool service.
        parameter_enabled = self.parameter_is_true(
            rospy.get_param("~enabled", self.enabled)
        )
        if parameter_enabled != self.enabled:
            self.enabled = parameter_enabled
        if not self.enabled:
            if self.was_active:
                self.publish_stop()
                rospy.loginfo(
                    "Line controller disabled; published a zero-velocity command."
                )
            self.was_active = False
            self.reset_target()
            return

        now = rospy.get_time()
        target_fresh = (
            self.last_target_time is not None
            and (now - self.last_target_time) <= self.target_timeout
        )
        if not self.lane_valid or not target_fresh:
            self.publish_stop()
            rospy.logwarn_throttle(
                1.0,
                "Pure-pursuit safety stop: lane_valid=%s target_fresh=%s.",
                self.lane_valid,
                target_fresh,
            )
            self.was_active = False
            return

        forward = self.target_forward
        left = self.target_left
        if abs(left) <= self.lateral_deadband_m:
            left = 0.0
        curvature = self.pure_pursuit_curvature(forward, left)
        if curvature is None:
            self.publish_stop()
            rospy.logerr("Pure-pursuit safety stop: target distance is zero.")
            self.was_active = False
            return

        # In base_link, +x is forward and +y is left. Pure pursuit for a
        # unicycle gives curvature=2*y/L^2 and angular velocity=v*curvature.
        angular = (
            self.steering_sign
            * self.curvature_gain
            * self.linear_speed
            * curvature
        )
        angular = max(
            -self.max_angular_speed, min(self.max_angular_speed, angular)
        )
        if not math.isfinite(angular):
            self.publish_stop()
            rospy.logerr("Pure-pursuit safety stop: non-finite angular velocity.")
            self.was_active = False
            return

        command = Twist()
        command.linear.x = self.linear_speed
        command.angular.z = angular
        self.cmd_pub.publish(command)
        self.was_active = True
        rospy.loginfo_throttle(
            1.0,
            "Pure pursuit: target=(%.3f m,%+.3f m left) curvature=%+.3f 1/m angular=%+.3f rad/s linear=%.2f m/s.",
            forward,
            left,
            curvature,
            angular,
            self.linear_speed,
        )


if __name__ == "__main__":
    try:
        LineFollowControl()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass