#!/usr/bin/env python3
"""
dronecan_gps_node.py — DroneCAN GPS driver for CubePilot Here4 RTK
Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725

Subscribes to:
  uavcan.equipment.gnss.Fix2            (msg ID 1063) — position + fix status
  uavcan.equipment.ahrs.MagneticFieldStrength          — compass (optional)

Publishes:
  /gps/fix        sensor_msgs/NavSatFix
  /gps/vel        geometry_msgs/TwistStamped
  /gps/rtk_status std_msgs/String

DroneCAN fix_type → sensor_msgs status mapping:
  0 = NO_FIX     → STATUS_NO_FIX  (-1)
  1 = TIME_ONLY  → STATUS_NO_FIX  (-1)
  2 = 2D_FIX    → STATUS_FIX      (0)
  3 = 3D_FIX    → STATUS_FIX      (0)
  4 = DGPS       → STATUS_SBAS_FIX (1)
  5 = RTK_FLOAT  → STATUS_GBAS_FIX (2)
  6 = RTK_FIXED  → STATUS_GBAS_FIX (2)
"""

import math
import threading

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy

from sensor_msgs.msg import NavSatFix, NavSatStatus
from geometry_msgs.msg import TwistStamped
from std_msgs.msg import String

try:
    import dronecan
except ImportError:
    dronecan = None

_SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=5,
)

# DroneCAN fix_type → (NavSatStatus.status, rtk_label)
_FIX_MAP = {
    0: (NavSatStatus.STATUS_NO_FIX,   'NO_FIX'),
    1: (NavSatStatus.STATUS_NO_FIX,   'TIME_ONLY'),
    2: (NavSatStatus.STATUS_FIX,      '2D_FIX'),
    3: (NavSatStatus.STATUS_FIX,      '3D_FIX'),
    4: (NavSatStatus.STATUS_SBAS_FIX, 'DGPS'),
    5: (NavSatStatus.STATUS_GBAS_FIX, 'RTK_FLOAT'),
    6: (NavSatStatus.STATUS_GBAS_FIX, 'RTK_FIXED'),
}


class DroneCanGpsNode(Node):
    def __init__(self) -> None:
        super().__init__('dronecan_gps')

        self.declare_parameter('can_interface', 'can0')
        self.declare_parameter('can_bitrate', 1000000)
        self.declare_parameter('node_id', 127)          # DroneCAN local node ID
        self.declare_parameter('publish_compass', True)

        self._iface = self.get_parameter('can_interface').value
        self._bitrate = self.get_parameter('can_bitrate').value
        self._node_id = self.get_parameter('node_id').value
        self._publish_compass = self.get_parameter('publish_compass').value

        self._fix_pub = self.create_publisher(NavSatFix, '/gps/fix', _SENSOR_QOS)
        self._vel_pub = self.create_publisher(TwistStamped, '/gps/vel', _SENSOR_QOS)
        self._rtk_pub = self.create_publisher(String, '/gps/rtk_status', 10)

        if dronecan is None:
            self.get_logger().error(
                'python-dronecan not installed. '
                'Run: pip install python-dronecan'
            )
            return

        self._lock = threading.Lock()
        self._dc_node = None
        self._spin_thread = threading.Thread(
            target=self._dronecan_spin, daemon=True
        )
        self._spin_thread.start()
        self.get_logger().info(
            f'DroneCanGpsNode started — interface={self._iface} '
            f'bitrate={self._bitrate}'
        )

    # ── DroneCAN spin (runs in background thread) ─────────────────────────────

    def _dronecan_spin(self) -> None:
        try:
            self._dc_node = dronecan.make_node(
                self._iface,
                node_id=self._node_id,
                bitrate=self._bitrate,
            )
            self._dc_node.add_handler(
                dronecan.uavcan.equipment.gnss.Fix2,
                self._on_fix2,
            )
            if self._publish_compass:
                self._dc_node.add_handler(
                    dronecan.uavcan.equipment.ahrs.MagneticFieldStrength,
                    self._on_mag,
                )
            self.get_logger().info(
                f'DroneCAN node online on {self._iface}'
            )
            while rclpy.ok():
                self._dc_node.spin(timeout=0.1)
        except Exception as exc:
            self.get_logger().error(f'DroneCAN spin error: {exc}')

    # ── Message handlers ──────────────────────────────────────────────────────

    def _on_fix2(self, event) -> None:
        msg = event.message
        now = self.get_clock().now().to_msg()

        fix_type = int(msg.fix_type)
        nav_status, rtk_label = _FIX_MAP.get(
            fix_type, (NavSatStatus.STATUS_NO_FIX, 'UNKNOWN')
        )

        # NavSatFix
        fix = NavSatFix()
        fix.header.stamp = now
        fix.header.frame_id = 'gps'
        fix.status.status = nav_status
        fix.status.service = NavSatStatus.SERVICE_GPS

        fix.latitude = math.degrees(msg.latitude_deg_1e8 * 1e-8)
        fix.longitude = math.degrees(msg.longitude_deg_1e8 * 1e-8)
        fix.altitude = msg.height_msl_mm * 1e-3  # mm → m

        # Covariance from position_covariance if available, else diagonal guess
        if hasattr(msg, 'position_covariance') and len(msg.position_covariance) >= 9:
            fix.position_covariance = list(msg.position_covariance)
            fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_FULL
        else:
            h_var = (msg.horizontal_pos_accuracy_m_1e2 * 1e-2) ** 2 \
                if hasattr(msg, 'horizontal_pos_accuracy_m_1e2') else 4.0
            v_var = (msg.vertical_pos_accuracy_m_1e2 * 1e-2) ** 2 \
                if hasattr(msg, 'vertical_pos_accuracy_m_1e2') else 4.0
            fix.position_covariance = [
                h_var, 0.0,   0.0,
                0.0,   h_var, 0.0,
                0.0,   0.0,   v_var,
            ]
            fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN

        self._fix_pub.publish(fix)

        # TwistStamped velocity
        if hasattr(msg, 'ned_velocity'):
            vel = TwistStamped()
            vel.header.stamp = now
            vel.header.frame_id = 'gps'
            vel.twist.linear.x = float(msg.ned_velocity[0])  # North m/s
            vel.twist.linear.y = float(msg.ned_velocity[1])  # East  m/s
            vel.twist.linear.z = float(msg.ned_velocity[2])  # Down  m/s (ROS: up+)
            self._vel_pub.publish(vel)

        # RTK status string
        rtk_msg = String()
        rtk_msg.data = rtk_label
        self._rtk_pub.publish(rtk_msg)

        self.get_logger().debug(
            f'Fix2: {fix.latitude:.6f},{fix.longitude:.6f} '
            f'alt={fix.altitude:.1f}m status={rtk_label}'
        )

    def _on_mag(self, event) -> None:
        # Compass data logged; extend to publish /imu/mag if needed
        msg = event.message
        self.get_logger().debug(
            f'Mag: {msg.magnetic_field_ga[0]:.3f} '
            f'{msg.magnetic_field_ga[1]:.3f} '
            f'{msg.magnetic_field_ga[2]:.3f} Ga'
        )


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


if __name__ == '__main__':
    main()
