#!/usr/bin/env python3
"""Sequential waypoint navigation for car_nav_lite.

Publishes /goal_pose waypoints one by one. Waits for the car to reach
each waypoint before publishing the next.

Usage:
  python3 waypoint_nav.py                           # default waypoints
  python3 waypoint_nav.py --waypoints waypoints.json  # from JSON file
  python3 waypoint_nav.py --loop                      # loop back to start

JSON format:
  [{"x": 1.0, "y": -1.5, "yaw": 0.0}, ...]
"""

import argparse
import json
import math
import time
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
from std_msgs.msg import String


# Default waypoints for 5m×5m arena (car spawns near -2.0, -2.3)
DEFAULT_WAYPOINTS = [
    {"x": -0.5, "y": -1.5, "yaw": 0.0, "desc": "mid-left"},
    {"x":  1.0, "y": -1.5, "yaw": 0.0, "desc": "right"},
    {"x":  1.5, "y":  0.0, "yaw": 0.0, "desc": "top-right"},
    {"x":  0.0, "y":  1.5, "yaw": 0.0, "desc": "top-center"},
    {"x": -1.5, "y":  1.0, "yaw": 0.0, "desc": "top-left"},
    {"x": -1.5, "y": -0.5, "yaw": 0.0, "desc": "left"},
]


class WaypointNav(Node):
    def __init__(self, waypoints, loop=False, threshold=0.25):
        super().__init__("waypoint_nav")
        self.waypoints = waypoints
        self.loop = loop
        self.threshold = threshold
        self.idx = 0
        self.reached = False
        self.fail_gen = 0
        self.skip_gen = -1
        self.skip_streak = 0     # consecutive skips
        self.pose_x = self.pose_y = self.pose_yaw = 0.0

        from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
        qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE,
                         durability=DurabilityPolicy.TRANSIENT_LOCAL)
        self.pub_goal = self.create_publisher(PoseStamped, "/goal_pose", qos)
        self.sub_odom = self.create_subscription(
            Odometry, "/odom", self.odom_cb, 10)
        self.sub_status = self.create_subscription(
            String, "/nav_status", self.status_cb, 10)
        self.timer = self.create_timer(0.2, self.check)  # 5Hz

    def odom_cb(self, msg):
        self.pose_x = msg.pose.pose.position.x
        self.pose_y = msg.pose.pose.position.y
        q = msg.pose.pose.orientation
        self.pose_yaw = math.atan2(2*(q.w*q.z + q.x*q.y), 1 - 2*(q.y*q.y + q.z*q.z))

    def status_cb(self, msg):
        if msg.data == "fail" and self.reached and self.fail_gen != self.skip_gen:
            self.skip_streak += 1
            if self.skip_streak > 3:
                self.get_logger().error("⏭ 3+ consecutive skips — stopping")
                return
            self.get_logger().warn(f"⏭ WP {self.idx+1} plan failed — skip #{self.skip_streak}")
            self.skip_gen = self.fail_gen
            self.idx += 1
            self.reached = False

    def send_goal(self, wp):
        msg = PoseStamped()
        msg.header.frame_id = "map"
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.pose.position.x = float(wp["x"])
        msg.pose.position.y = float(wp["y"])
        yaw_deg = float(wp.get("yaw", 0.0))
        yaw = math.radians(yaw_deg)  # JSON stores degrees
        msg.pose.orientation.z = math.sin(yaw / 2.0)
        msg.pose.orientation.w = math.cos(yaw / 2.0)
        self.pub_goal.publish(msg)
        desc = wp.get("desc", "")
        self.get_logger().info(
            f"→ WP {self.idx+1}/{len(self.waypoints)}: "
            f"({wp['x']:.2f},{wp['y']:.2f}) {desc}"
        )

    def check(self):
        if self.idx >= len(self.waypoints):
            if self.loop:
                self.idx = 0
                self.reached = False
            else:
                return

        wp = self.waypoints[self.idx]
        if not self.reached:
            self.fail_gen += 1  # new generation for this wp
            self.send_goal(wp)
            self.reached = True

        dist = math.hypot(self.pose_x - wp["x"], self.pose_y - wp["y"])
        if dist < self.threshold:
            self.get_logger().info(
                f"✓ WP {self.idx+1} reached (dist={dist:.2f}m)")
            self.idx += 1
            self.reached = False
            self.skip_streak = 0


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("--waypoints", type=str, default=None,
                        help="JSON file with waypoints")
    parser.add_argument("--loop", action="store_true",
                        help="Loop back to first waypoint after last")
    parser.add_argument("--threshold", type=float, default=0.3,
                        help="Arrival distance threshold (m)")
    args = parser.parse_args()

    if args.waypoints:
        with open(args.waypoints) as f:
            waypoints = json.load(f)
        print(f"Loaded {len(waypoints)} waypoints from {args.waypoints}")
    else:
        waypoints = DEFAULT_WAYPOINTS
        print(f"Using {len(waypoints)} default waypoints")

    rclpy.init()
    node = WaypointNav(waypoints, args.loop, args.threshold)
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()