#!/usr/bin/env python3
"""Measure odometry path length and yaw without commanding the robot."""

import math
import threading

import rospy
from nav_msgs.msg import Odometry
from std_msgs.msg import Float32, String
from std_srvs.srv import Trigger, TriggerResponse

from traffic_line_task.srv import MarkDistance, MarkDistanceResponse


class RouteDistanceCalibrator:
    LABELS = (
        "first_left_turn_distance_m",
        "first_right_turn_distance_m",
        "first_straight_entry_distance_m",
        "direct_finish_distance_m",
        "straight_to_second_distance_m",
        "second_right_turn_distance_m",
        "after_second_finish_distance_m",
    )

    def __init__(self):
        rospy.init_node("route_distance_calibrator")
        self.lock = threading.RLock()
        self.odom_topic = rospy.get_param("~odom_topic", "/odom")
        self.odom_timeout = max(0.1, float(rospy.get_param("~odom_timeout", 0.5)))
        self.odom_jump_limit_m = max(
            0.01, float(rospy.get_param("~odom_jump_limit_m", 0.25))
        )
        self.publish_rate = max(1.0, float(rospy.get_param("~publish_rate", 10.0)))

        self.position = None
        self.yaw = None
        self.last_odom_time = None
        self.total_distance = 0.0
        self.total_yaw = 0.0
        self.zero_distance = 0.0
        self.zero_yaw = 0.0
        self.odom_fault = ""

        self.distance_pub = rospy.Publisher("~distance", Float32, queue_size=1)
        self.yaw_pub = rospy.Publisher("~yaw_change", Float32, queue_size=1)
        self.last_mark_pub = rospy.Publisher(
            "~last_mark", String, queue_size=1, latch=True
        )
        self.odom_sub = rospy.Subscriber(
            self.odom_topic, Odometry, self.odom_callback, queue_size=20
        )
        self.reset_service = rospy.Service("~reset", Trigger, self.reset_callback)
        self.mark_service = rospy.Service("~mark", MarkDistance, self.mark_callback)
        self.timer = rospy.Timer(
            rospy.Duration(1.0 / self.publish_rate), self.publish_callback
        )
        rospy.loginfo(
            "Route-distance calibrator is measurement only; odom=%s. "
            "It never publishes /cmd_vel.",
            self.odom_topic,
        )

    @staticmethod
    def wrap_angle(angle):
        return math.atan2(math.sin(angle), math.cos(angle))

    @staticmethod
    def yaw_from_odometry(message):
        q = message.pose.pose.orientation
        siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
        cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
        return math.atan2(siny_cosp, cosy_cosp)

    def odom_callback(self, message):
        with self.lock:
            point = message.pose.pose.position
            current_position = (float(point.x), float(point.y))
            current_yaw = self.yaw_from_odometry(message)
            if self.position is not None:
                step = math.hypot(
                    current_position[0] - self.position[0],
                    current_position[1] - self.position[1],
                )
                if step > self.odom_jump_limit_m:
                    self.odom_fault = "ODOM_JUMP: %.3f m" % step
                else:
                    self.total_distance += step
            if self.yaw is not None:
                self.total_yaw += self.wrap_angle(current_yaw - self.yaw)
            self.position = current_position
            self.yaw = current_yaw
            self.last_odom_time = rospy.get_time()

    def measurement(self):
        return (
            max(0.0, self.total_distance - self.zero_distance),
            self.total_yaw - self.zero_yaw,
        )

    def odom_error(self):
        if self.odom_fault:
            return self.odom_fault
        if self.last_odom_time is None:
            return "ODOM_UNAVAILABLE"
        age = rospy.get_time() - self.last_odom_time
        if age > self.odom_timeout:
            return "ODOM_TIMEOUT: %.3f s" % age
        return ""

    def reset_callback(self, _request):
        with self.lock:
            error = self.odom_error()
            if error and error != self.odom_fault:
                return TriggerResponse(success=False, message=error)
            self.odom_fault = ""
            self.zero_distance = self.total_distance
            self.zero_yaw = self.total_yaw
            self.last_mark_pub.publish(String(data=""))
            rospy.loginfo("Route-distance calibration measurement reset to zero.")
            return TriggerResponse(success=True, message="measurement reset")

    def mark_callback(self, request):
        with self.lock:
            label = str(request.label).strip()
            if label not in self.LABELS:
                return MarkDistanceResponse(
                    success=False,
                    distance_m=0.0,
                    yaw_rad=0.0,
                    message="unknown label; choose one of: %s" % ", ".join(self.LABELS),
                )
            error = self.odom_error()
            if error:
                return MarkDistanceResponse(
                    success=False,
                    distance_m=0.0,
                    yaw_rad=0.0,
                    message=error,
                )
            distance, yaw = self.measurement()
            result = "%s: %.4f  # yaw_change_rad=%.4f" % (label, distance, yaw)
            self.last_mark_pub.publish(String(data=result))
            rospy.loginfo("CALIBRATION MARK: %s", result)
            return MarkDistanceResponse(
                success=True,
                distance_m=distance,
                yaw_rad=yaw,
                message=result,
            )

    def publish_callback(self, _event):
        with self.lock:
            distance, yaw = self.measurement()
            self.distance_pub.publish(Float32(data=distance))
            self.yaw_pub.publish(Float32(data=yaw))


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