"""
gps_waypoint_follower_node — GPS-to-Nav2 waypoint bridge for outdoor navigation

Subscribes to the OSM router's waypoint output (/outdoor/waypoints) and the
robot's GPS fix (/gps/fix), converts each waypoint to map-frame via
robot_localization's navsat_transform_node, then sends the full list to Nav2's
navigate_through_poses action server.

GPS quality handling:
  SIM7600X cellular GPS: ±2.5 m CEP — uses wider goal tolerances (2 m)
  RTK upgrade (ZED-F9P): ±0.02 m CEP — tolerances tighten automatically

GPS status thresholds (sensor_msgs/NavSatStatus):
  STATUS_NO_FIX    (-1) → reject all goals, log error
  STATUS_FIX       (0)  → standard GPS, use but warn
  STATUS_SBAS_FIX  (1)  → SBAS-corrected, OK
  STATUS_GBAS_FIX  (2)  → RTK fixed, best quality, tighten tolerance

Topics subscribed:
  /gps/fix              sensor_msgs/NavSatFix
  /outdoor/waypoints    geometry_msgs/PoseArray (from osm_router_node, map frame)
  /outdoor/abort        std_msgs/Bool           (external stop signal)

Topics published:
  /outdoor/gps_status   std_msgs/String   — current GPS quality description
  /outdoor/nav_status   std_msgs/String   — navigation state machine status

Actions called:
  /navigate_through_poses  nav2_msgs/action/NavigateThroughPoses

Services:
  /outdoor/start_navigation  std_srvs/Trigger — begin executing /outdoor/waypoints
  /outdoor/stop_navigation   std_srvs/Trigger — cancel in-progress navigation
"""

import math
from typing import Optional

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor

from sensor_msgs.msg import NavSatFix, NavSatStatus
from geometry_msgs.msg import PoseArray, PoseStamped
from std_msgs.msg import String, Bool
from std_srvs.srv import Trigger

from nav2_msgs.action import NavigateThroughPoses


# GPS fix quality thresholds
_MIN_FIX_STATUS = NavSatStatus.STATUS_FIX   # reject STATUS_NO_FIX (-1)
_RTK_FIX_STATUS = NavSatStatus.STATUS_GBAS_FIX

# Goal tolerance (metres) by GPS quality
_TOLERANCE_STANDARD_GPS = 2.0    # SIM7600X ±2.5 m CEP
_TOLERANCE_RTK          = 0.30   # ZED-F9P ±2 cm RTK


class GpsWaypointFollowerNode(Node):

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

        self.declare_parameter('map_frame',           'map')
        self.declare_parameter('min_fix_status',      _MIN_FIX_STATUS)
        self.declare_parameter('goal_tolerance_xy',   _TOLERANCE_STANDARD_GPS)
        self.declare_parameter('goal_tolerance_yaw',  3.14)    # don't care about heading
        self.declare_parameter('nav_timeout_s',       300.0)   # 5 min per waypoint batch
        self.declare_parameter('waypoint_spacing_m',  5.0)     # skip closer waypoints

        self._fix:       Optional[NavSatFix]  = None
        self._waypoints: Optional[PoseArray]  = None
        self._nav_active = False
        self._cb_group   = ReentrantCallbackGroup()

        # ── Subscriptions ────────────────────────────────────────────────────
        self.create_subscription(NavSatFix,  '/gps/fix',           self._fix_cb,       10)
        self.create_subscription(PoseArray,  '/outdoor/waypoints', self._waypoints_cb, 10)
        self.create_subscription(Bool,       '/outdoor/abort',     self._abort_cb,     10)

        # ── Publishers ───────────────────────────────────────────────────────
        self._gps_status_pub = self.create_publisher(String, '/outdoor/gps_status', 10)
        self._nav_status_pub = self.create_publisher(String, '/outdoor/nav_status',  10)

        # ── Services ─────────────────────────────────────────────────────────
        self.create_service(Trigger, '/outdoor/start_navigation', self._start_cb,
                            callback_group=self._cb_group)
        self.create_service(Trigger, '/outdoor/stop_navigation',  self._stop_cb,
                            callback_group=self._cb_group)

        # ── Nav2 action client ───────────────────────────────────────────────
        self._nav_client = ActionClient(
            self, NavigateThroughPoses, '/navigate_through_poses',
            callback_group=self._cb_group,
        )

        # ── Status timer ─────────────────────────────────────────────────────
        self.create_timer(2.0, self._publish_gps_status)

        self._nav_goal_handle = None
        self._publish_nav_status('idle')
        self.get_logger().info('gps_waypoint_follower: ready. Call /outdoor/start_navigation.')

    # ── Callbacks ─────────────────────────────────────────────────────────────

    def _fix_cb(self, msg: NavSatFix) -> None:
        self._fix = msg

    def _waypoints_cb(self, msg: PoseArray) -> None:
        self._waypoints = msg
        self.get_logger().info(f'Received {len(msg.poses)} outdoor waypoints')

    def _abort_cb(self, msg: Bool) -> None:
        if msg.data:
            self._cancel_navigation('abort signal received')

    def _start_cb(self, _req, response):
        if self._nav_active:
            response.success = False
            response.message = 'Navigation already active'
            return response

        if self._fix is None:
            response.success = False
            response.message = 'No GPS fix'
            return response

        fix_status = self._fix.status.status
        min_status = self.get_parameter('min_fix_status').value
        if fix_status < min_status:
            response.success = False
            response.message = f'GPS quality too low: status={fix_status}'
            return response

        if self._waypoints is None or len(self._waypoints.poses) == 0:
            response.success = False
            response.message = 'No waypoints loaded — call /outdoor/plan_route first'
            return response

        # Send waypoints to Nav2 asynchronously
        self.create_timer(0.1, self._send_nav_goal, callback_group=self._cb_group)
        response.success = True
        response.message = f'Starting navigation with {len(self._waypoints.poses)} waypoints'
        return response

    def _stop_cb(self, _req, response):
        self._cancel_navigation('stop requested')
        response.success = True
        response.message = 'Navigation cancelled'
        return response

    # ── Navigation ────────────────────────────────────────────────────────────

    def _send_nav_goal(self) -> None:
        """One-shot timer callback: send waypoints to Nav2."""
        # Cancel the timer that called us
        self.destroy_timer(list(self.timers)[-1])

        if not self._nav_client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error('Nav2 navigate_through_poses action not available')
            self._publish_nav_status('error:nav2_unavailable')
            return

        # Adjust goal tolerance by GPS quality
        fix_status = self._fix.status.status if self._fix else _MIN_FIX_STATUS
        if fix_status >= _RTK_FIX_STATUS:
            tol = _TOLERANCE_RTK
            self.get_logger().info('RTK fix detected — using tight tolerance (0.30 m)')
        else:
            tol = self.get_parameter('goal_tolerance_xy').value
            self.get_logger().info(f'Standard GPS — tolerance = {tol:.1f} m')

        # Build goal: PoseStamped list from PoseArray
        goal = NavigateThroughPoses.Goal()
        frame = self.get_parameter('map_frame').value
        now   = self.get_clock().now().to_msg()

        poses = self._filter_waypoints(self._waypoints.poses)
        for pose in poses:
            ps = PoseStamped()
            ps.header.stamp    = now
            ps.header.frame_id = frame
            ps.pose = pose
            goal.poses.append(ps)

        self.get_logger().info(f'Sending {len(goal.poses)} waypoints to Nav2')
        self._publish_nav_status(f'navigating:{len(goal.poses)}_waypoints')
        self._nav_active = True

        send_future = self._nav_client.send_goal_async(
            goal,
            feedback_callback=self._nav_feedback_cb,
        )
        send_future.add_done_callback(self._nav_goal_response_cb)

    def _nav_goal_response_cb(self, future) -> None:
        self._nav_goal_handle = future.result()
        if not self._nav_goal_handle.accepted:
            self.get_logger().error('Nav2 rejected navigation goal')
            self._nav_active = False
            self._publish_nav_status('error:goal_rejected')
            return
        result_future = self._nav_goal_handle.get_result_async()
        result_future.add_done_callback(self._nav_result_cb)

    def _nav_feedback_cb(self, feedback) -> None:
        fb = feedback.feedback
        remaining = getattr(fb, 'number_of_poses_remaining', '?')
        self.get_logger().info(
            f'Nav2 feedback: {remaining} waypoints remaining',
            throttle_duration_sec=10.0,
        )

    def _nav_result_cb(self, future) -> None:
        self._nav_active = False
        result = future.result()
        if result.status == 4:   # SUCCEEDED
            self.get_logger().info('Outdoor navigation completed successfully')
            self._publish_nav_status('completed')
        else:
            self.get_logger().warn(f'Navigation ended with status {result.status}')
            self._publish_nav_status(f'failed:status={result.status}')

    def _cancel_navigation(self, reason: str) -> None:
        if self._nav_goal_handle is not None and self._nav_active:
            self.get_logger().info(f'Cancelling navigation: {reason}')
            self._nav_goal_handle.cancel_goal_async()
        self._nav_active = False
        self._publish_nav_status(f'cancelled:{reason}')

    # ── Helpers ───────────────────────────────────────────────────────────────

    def _filter_waypoints(self, poses: list) -> list:
        """Remove waypoints closer than waypoint_spacing_m to previous."""
        spacing = self.get_parameter('waypoint_spacing_m').value
        out = [poses[0]]
        for p in poses[1:]:
            prev = out[-1]
            d = math.hypot(
                p.position.x - prev.position.x,
                p.position.y - prev.position.y,
            )
            if d >= spacing:
                out.append(p)
        return out

    def _publish_gps_status(self) -> None:
        if self._fix is None:
            status = 'no_fix'
        elif self._fix.status.status < 0:
            status = 'no_fix'
        elif self._fix.status.status >= _RTK_FIX_STATUS:
            status = f'rtk_fixed lat={self._fix.latitude:.6f} lon={self._fix.longitude:.6f}'
        elif self._fix.status.status >= NavSatStatus.STATUS_FIX:
            cov = self._fix.position_covariance[0] ** 0.5 if self._fix.position_covariance[0] > 0 else 2.5
            status = f'fix_ok cep~{cov:.1f}m lat={self._fix.latitude:.6f} lon={self._fix.longitude:.6f}'
        else:
            status = f'poor_fix status={self._fix.status.status}'
        msg = String()
        msg.data = status
        self._gps_status_pub.publish(msg)

    def _publish_nav_status(self, s: str) -> None:
        msg = String()
        msg.data = s
        self._nav_status_pub.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    node = GpsWaypointFollowerNode()
    executor = MultiThreadedExecutor()
    executor.add_node(node)
    try:
        executor.spin()
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == '__main__':
    main()
