#!/usr/bin/env python3
"""导航程序 (navigate_to_point)：自动驱动麦轮底盘到示教记录的点位。

- 目标点：读取 teach_points 生成的 taught_points.yaml（map 系 x,y,yaw）。
- 反馈：TF map->base_footprint（= AMCL 实时定位），需 localization.launch.py 在运行。
- 控制：holonomic P 控制器，把 map 系位姿误差投影到机体系，生成 (前进, 侧移, 转向)
        速度，按 udp_teleop 的 XYW 约定经 UDP→ESP32 下发，到达容差内停车。

速度符号默认按 udp_teleop/keyboard_control 推导：
  前进 W→chassis_y 取负, 左移 A→chassis_x 取负, 左转(CCW) Q→chassis_w 取负
  => sign_x = sign_y = sign_w = -1。若实车方向相反，翻转对应 sign 参数即可。

用法：
  # 交互选点
  ros2 run craic_localization navigate_to_point --ros-args \
      -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml
  # 一次性去某点
  ros2 run craic_localization navigate_to_point --ros-args -p points_file:=... -p goal:=B_1
  # 运行中用话题指定（供上层任务程序调用）
  ros2 topic pub -1 /goto std_msgs/String "{data: 'C_1'}"
  # 干跑（只打印命令不发，安全验证逻辑/符号）
  ros2 run craic_localization navigate_to_point --ros-args -p points_file:=... -p dry_run:=true

安全：首次务必先 dry_run，再低速单点测试，手放 Ctrl+C（退出会自动发停车）。
"""

import math
import socket
import threading

import rclpy
import yaml
from rclpy.duration import Duration
from rclpy.node import Node
from rclpy.time import Time
from std_msgs.msg import String
from tf2_ros import (
    Buffer,
    ConnectivityException,
    ExtrapolationException,
    LookupException,
    TransformListener,
)


def quat_to_yaw(qx, qy, qz, qw):
    return math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))


def norm_angle(a):
    return math.atan2(math.sin(a), math.cos(a))


def clamp(v, lo, hi):
    return max(lo, min(hi, v))


class NavigateToPoint(Node):
    def __init__(self):
        super().__init__('navigate_to_point')
        p = self.declare_parameter
        self.points_file = p('points_file', 'taught_points.yaml').value
        self.udp_ip = p('udp_ip', '192.168.4.1').value
        self.udp_port = int(p('udp_port', 8888).value)
        self.map_frame = p('map_frame', 'map').value
        self.base_frame = p('base_frame', 'base_footprint').value
        self.control_rate = float(p('control_rate', 20.0).value)
        self.max_linear = float(p('max_linear', 60.0).value)      # 麦轮线速度命令上限(同 keyboard ~100)
        self.max_angular = float(p('max_angular', 30.0).value)    # 角速度命令上限(同 keyboard ~45)
        self.kp_linear = float(p('kp_linear', 150.0).value)       # m -> 命令单位
        self.kp_angular = float(p('kp_angular', 40.0).value)      # rad -> 命令单位
        self.pos_tol = float(p('pos_tolerance', 0.05).value)      # 到位容差(米)
        self.yaw_tol = float(p('yaw_tolerance', 0.05).value)      # 到位容差(弧度)
        self.sign_x = float(p('sign_x', -1.0).value)              # 侧移符号(机体左为正)
        self.sign_y = float(p('sign_y', -1.0).value)              # 前进符号(机体前为正)
        self.sign_w = float(p('sign_w', -1.0).value)              # 转向符号(CCW 为正)
        self.goal_timeout = float(p('goal_timeout', 30.0).value)
        self.dry_run = bool(p('dry_run', False).value)
        goal_arg = str(p('goal', '').value)

        self.points = self._load_points()
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

        self._lock = threading.Lock()
        self.goal_name = None
        self.goal = None          # (x, y, yaw)
        self.goal_deadline = None
        self.one_shot = bool(goal_arg)
        self.done = False         # one_shot 完成标志

        self.create_subscription(String, 'goto', self._goto_cb, 10)
        self.timer = self.create_timer(1.0 / self.control_rate, self._control_tick)

        self.get_logger().info(
            f'navigate_to_point 就绪 (UDP {self.udp_ip}:{self.udp_port}, dry_run={self.dry_run})')
        if goal_arg:
            self.set_goal(goal_arg)

    # -------- 点位 / 目标 -------- #
    def _load_points(self):
        try:
            with open(self.points_file, 'r', encoding='utf-8') as f:
                data = yaml.safe_load(f) or {}
            pts = dict(data.get('points', {}))
            self.get_logger().info(f'载入 {len(pts)} 个点: {", ".join(pts) or "(空)"}')
            return pts
        except Exception as exc:  # noqa: BLE001
            self.get_logger().error(f'读取点位文件失败 {self.points_file}: {exc}')
            return {}

    def set_goal(self, name):
        pt = self.points.get(name)
        if pt is None:
            self.get_logger().warn(f'未知点位: {name}（可用: {", ".join(self.points)}）')
            return False
        with self._lock:
            self.goal_name = name
            self.goal = (float(pt['x']), float(pt['y']), float(pt.get('yaw', 0.0)))
            self.goal_deadline = self.get_clock().now() + Duration(seconds=self.goal_timeout)
        self.get_logger().info(
            f'前往 {name}: x={self.goal[0]:.3f} y={self.goal[1]:.3f} yaw={self.goal[2]:.3f}')
        return True

    def _goto_cb(self, msg):
        self.set_goal(msg.data.strip())

    # -------- 反馈 / 下发 -------- #
    def _lookup(self):
        try:
            t = self.tf_buffer.lookup_transform(self.map_frame, self.base_frame, Time())
        except (LookupException, ConnectivityException, ExtrapolationException):
            return None
        tr = t.transform.translation
        q = t.transform.rotation
        return (tr.x, tr.y, quat_to_yaw(q.x, q.y, q.z, q.w))

    def _send(self, cx, cy, cw):
        cmd = f'XYW:{int(round(cx))}:{int(round(cy))}:{int(round(cw))}:XZHY\n'.encode()
        if self.dry_run:
            self.get_logger().info(f'[dry] {cmd.decode().strip()}')
        else:
            self.sock.sendto(cmd, (self.udp_ip, self.udp_port))

    def _stop(self):
        self._send(0, 0, 0)

    # -------- 控制环 -------- #
    def _control_tick(self):
        with self._lock:
            goal, deadline, name = self.goal, self.goal_deadline, self.goal_name
        if goal is None:
            return
        if self.get_clock().now() > deadline:
            self.get_logger().warn(f'{name} 超时未到达，停车')
            self._finish()
            return

        pose = self._lookup()
        if pose is None:
            self._stop()
            self.get_logger().warn('无定位 (map->base_footprint)，停车等待…',
                                   throttle_duration_sec=2.0)
            return

        px, py, pyaw = pose
        tx, ty, tyaw = goal
        dx, dy = tx - px, ty - py
        dist = math.hypot(dx, dy)
        dyaw = norm_angle(tyaw - pyaw)

        if dist < self.pos_tol and abs(dyaw) < self.yaw_tol:
            self.get_logger().info(
                f'到达 {name}（位置误差 {dist * 100:.1f}cm，朝向误差 {math.degrees(dyaw):.1f}°）')
            self._finish()
            return

        # map 系误差投影到机体系（x 前, y 左, REP-103）
        e_fwd = dx * math.cos(pyaw) + dy * math.sin(pyaw)
        e_left = -dx * math.sin(pyaw) + dy * math.cos(pyaw)

        v_fwd = self.kp_linear * e_fwd
        v_left = self.kp_linear * e_left
        # 限制平移合速度（防止 x+y 叠加超限）
        speed = math.hypot(v_fwd, v_left)
        if speed > self.max_linear:
            s = self.max_linear / speed
            v_fwd *= s
            v_left *= s
        # 已到位置容差内则只修朝向
        if dist < self.pos_tol:
            v_fwd = v_left = 0.0
        w = clamp(self.kp_angular * dyaw, -self.max_angular, self.max_angular)

        self.get_logger().info(
            f'{name}: 距离 {dist * 100:.1f}cm, 朝向差 {math.degrees(dyaw):.1f}°',
            throttle_duration_sec=0.5)

        # 机体速度 -> XYW（侧移=chassis_x, 前进=chassis_y, 转向=chassis_w）
        self._send(self.sign_x * v_left, self.sign_y * v_fwd, self.sign_w * w)

    def _finish(self):
        self._stop()
        with self._lock:
            self.goal = self.goal_name = self.goal_deadline = None
        if self.one_shot:
            self.done = True

    def destroy_node(self):
        try:
            self._stop()
        except Exception:  # noqa: BLE001
            pass
        return super().destroy_node()


def interactive(node: NavigateToPoint, stop_flag: threading.Event):
    while not stop_flag.is_set() and rclpy.ok():
        try:
            line = input(f'去哪个点? ({", ".join(node.points) or "无"} | q 退出) > ').strip()
        except EOFError:
            break
        low = line.lower()
        if low in ('q', 'quit', 'exit'):
            stop_flag.set()
            break
        elif low in ('s', 'stop'):
            node._finish()
        elif line:
            node.set_goal(line)


def main(args=None):
    rclpy.init(args=args)
    node = NavigateToPoint()
    stop_flag = threading.Event()
    if not node.one_shot:
        threading.Thread(target=interactive, args=(node, stop_flag), daemon=True).start()
    try:
        while rclpy.ok() and not stop_flag.is_set() and not (node.one_shot and node.done):
            rclpy.spin_once(node, timeout_sec=0.1)
    except KeyboardInterrupt:
        pass
    finally:
        node._stop()
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()
