"""
waypoint_teacher_node.py -- Standalone waypoint teacher ROS2 node.

Listens to /social/speech/transcript for "teach route <name>" and "stop teaching".
Records robot pose at configurable intervals. Saves/loads routes via WaypointRoute.
"""

import math

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from nav_msgs.msg import Odometry
from std_msgs.msg import String

from .waypoint_teacher import WaypointRoute


class WaypointTeacherNode(Node):
    """Standalone waypoint teaching node."""

    def __init__(self):
        super().__init__('waypoint_teacher')

        self.declare_parameter('routes_dir', '/mnt/nvme/saltybot/routes')
        self.declare_parameter('recording_interval', 0.5)  # meters

        self._routes_dir = self.get_parameter('routes_dir').value
        self._interval = self.get_parameter('recording_interval').value

        self._teaching = False
        self._route = None
        self._last_x = None
        self._last_y = None
        self._robot_x = 0.0
        self._robot_y = 0.0
        self._robot_yaw = 0.0

        best_effort_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST, depth=1)

        self.create_subscription(
            Odometry, '/odom', self._on_odom, best_effort_qos)
        self.create_subscription(
            String, '/social/speech/transcript',
            self._on_transcript, 10)

        self._status_pub = self.create_publisher(
            String, '/social/waypoint/status', 10)

        # Record timer at 10Hz (check distance)
        self._timer = self.create_timer(0.1, self._record_tick)

        self.get_logger().info(
            f'Waypoint teacher ready (interval={self._interval}m, '
            f'dir={self._routes_dir})')

    def _on_odom(self, msg: Odometry):
        self._robot_x = msg.pose.pose.position.x
        self._robot_y = msg.pose.pose.position.y
        q = msg.pose.pose.orientation
        self._robot_yaw = math.atan2(
            2.0 * (q.w * q.z + q.x * q.y),
            1.0 - 2.0 * (q.y * q.y + q.z * q.z))

    def _on_transcript(self, msg: String):
        text = msg.data.strip().lower()

        import re
        m = re.match(r'teach\s+route\s+(\w+)', text)
        if m:
            name = m.group(1)
            self._route = WaypointRoute(name)
            self._teaching = True
            self._last_x = self._robot_x
            self._last_y = self._robot_y
            self._pub_status(f'RECORDING: {name}')
            self.get_logger().info(f'Recording route: {name}')
            return

        if 'stop teaching' in text:
            if self._teaching and self._route:
                self._route.save(self._routes_dir)
                n = len(self._route.waypoints)
                self._pub_status(
                    f'SAVED: {self._route.name} ({n} waypoints)')
                self.get_logger().info(
                    f'Route saved: {self._route.name} ({n} pts)')
            self._teaching = False
            self._route = None
            return

        if 'list routes' in text:
            routes = WaypointRoute.list_routes(self._routes_dir)
            self._pub_status(f'ROUTES: {", ".join(routes) or "(none)"}')

    def _record_tick(self):
        if not self._teaching or self._route is None:
            return

        if self._last_x is None:
            self._last_x = self._robot_x
            self._last_y = self._robot_y

        dist = math.hypot(
            self._robot_x - self._last_x,
            self._robot_y - self._last_y)

        if dist >= self._interval:
            yaw_deg = math.degrees(self._robot_yaw)
            self._route.add(self._robot_x, self._robot_y, 0.0, yaw_deg)
            self._last_x = self._robot_x
            self._last_y = self._robot_y

    def _pub_status(self, text: str):
        msg = String()
        msg.data = text
        self._status_pub.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    node = WaypointTeacherNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
