#!/usr/bin/env python3
"""
Origincar ROS2 桥接中转节点

将 Origincar 仿真 HTTP API 全面桥接到 ROS2 话题，双向中转。

上行（HTTP → ROS2）：
  GET /image → sensor_msgs/Image      (独立定时器，可配置频率)
  GET /imu   → sensor_msgs/Imu        (统一同步定时器)
  GET /scan  → sensor_msgs/LaserScan  (统一同步定时器)
  GET /odom  → nav_msgs/Odometry      (统一同步定时器)
  GET /power → std_msgs/Float32       (统一同步定时器)

下行（ROS2 → HTTP）：
  geometry_msgs/Twist → POST /cmd     (差速模式: vx/wz)

可配置参数：
  http_host, http_port, timeout
  image_topic, imu_topic, scan_topic, odom_topic, power_topic, cmd_vel_topic
  sync_rate  — 统一同步频率 Hz（scan/odom/imu/power，默认 30）
  image_rate — 图像拉取频率 Hz（默认 10）

使用:
  ros2 run origincar_bridge bridge_node --ros-args -p http_host:=192.168.11.159
  ros2 launch origincar_bridge bridge.launch.py http_host:=192.168.11.159
"""

import traceback

import cv2
import numpy as np
import requests
import rclpy
from rclpy.node import Node

# ROS2 消息
from sensor_msgs.msg import Image as RosImage
from sensor_msgs.msg import Imu
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist, Quaternion
from std_msgs.msg import Float32

from cv_bridge import CvBridge


class OrigincarBridge(Node):
    """Origincar 仿真 HTTP API ⇄ ROS2 话题 全功能桥接节点。"""

    def __init__(self):
        super().__init__("origincar_bridge")

        # ---------- 声明参数 ----------
        self.declare_parameter("http_host", "192.168.11.159")
        self.declare_parameter("http_port", 8765)
        self.declare_parameter("timeout", 1.0)

        self.declare_parameter("image_topic", "/image")
        self.declare_parameter("imu_topic", "/imu/data_raw")
        self.declare_parameter("scan_topic", "/scan")
        self.declare_parameter("odom_topic", "/odom")
        self.declare_parameter("power_topic", "/PowerVoltage")
        self.declare_parameter("cmd_vel_topic", "/cmd_vel")

        self.declare_parameter("sync_rate", 30.0)   # scan/odom/imu/power 频率
        self.declare_parameter("image_rate", 10.0)   # 图像独立频率

        self._read_params()

        # ---------- cv_bridge ----------
        self.bridge = CvBridge()

        # ---------- 发布者 ----------
        self.image_pub = self.create_publisher(RosImage, self.image_topic, 10)
        self.imu_pub   = self.create_publisher(Imu, self.imu_topic, 10)
        self.scan_pub  = self.create_publisher(LaserScan, self.scan_topic, 10)
        self.odom_pub  = self.create_publisher(Odometry, self.odom_topic, 10)
        self.power_pub = self.create_publisher(Float32, self.power_topic, 10)

        # ---------- 订阅者 ----------
        self.cmd_sub = self.create_subscription(
            Twist, self.cmd_vel_topic, self._cmd_vel_callback, 10
        )

        # ---------- 定时器 ----------
        # image 独立定时器（JPEG 解码较重，用较低频率）
        self.image_timer = self.create_timer(
            1.0 / self.image_rate, self._fetch_and_publish_image
        )
        # 统一同步定时器（scan / odom / imu / power 在同一拍内完成）
        self.sync_timer = self.create_timer(
            1.0 / self.sync_rate, self._sync_fast_data
        )

        # ---------- 参数变更 ----------
        self.add_on_set_parameters_callback(self._on_param_change)

        # ---------- 统计 & Session ----------
        self._cmd_count = 0
        self._session = requests.Session()

        self.get_logger().info(
            f"桥接启动 → http://{self.http_host}:{self.http_port}\n"
            f"  上行: 图像 [{self.image_topic}] @ {self.image_rate} Hz\n"
            f"        IMU  [{self.imu_topic}]    }} sync @ {self.sync_rate} Hz\n"
            f"        Scan [{self.scan_topic}]   }}\n"
            f"        Odom [{self.odom_topic}]   }}\n"
            f"        Power[{self.power_topic}]  }}\n"
            f"  下行: 控制 [{self.cmd_vel_topic}]  → POST /cmd (vx/wz)"
        )

    # ------------------------------------------------------------------
    #  参数
    # ------------------------------------------------------------------

    def _read_params(self):
        self.http_host     = self.get_parameter("http_host").value
        self.http_port     = self.get_parameter("http_port").value
        self.timeout       = self.get_parameter("timeout").value
        self.image_topic   = self.get_parameter("image_topic").value
        self.imu_topic     = self.get_parameter("imu_topic").value
        self.scan_topic    = self.get_parameter("scan_topic").value
        self.odom_topic    = self.get_parameter("odom_topic").value
        self.power_topic   = self.get_parameter("power_topic").value
        self.cmd_vel_topic = self.get_parameter("cmd_vel_topic").value
        self.sync_rate     = self.get_parameter("sync_rate").value
        self.image_rate    = self.get_parameter("image_rate").value

    def _on_param_change(self, params):
        for p in params:
            name = p.name
            if name == "http_host":     self.http_host = p.value
            elif name == "http_port":   self.http_port = p.value
            elif name == "timeout":     self.timeout = p.value
            elif name == "sync_rate":
                self.sync_rate = p.value
                self.sync_timer.timer_period_ns = int(1e9 / self.sync_rate)
            elif name == "image_rate":
                self.image_rate = p.value
                self.image_timer.timer_period_ns = int(1e9 / self.image_rate)
            elif name == "image_topic":   self.image_topic = p.value
            elif name == "imu_topic":     self.imu_topic = p.value
            elif name == "scan_topic":    self.scan_topic = p.value
            elif name == "odom_topic":    self.odom_topic = p.value
            elif name == "power_topic":   self.power_topic = p.value
            elif name == "cmd_vel_topic": self.cmd_vel_topic = p.value
        return rclpy.parameter.SetParametersResult(successful=True)

    # ------------------------------------------------------------------
    #  HTTP 基础
    # ------------------------------------------------------------------

    @property
    def _base_url(self) -> str:
        return f"http://{self.http_host}:{self.http_port}"

    def _get(self, endpoint: str):
        try:
            r = self._session.get(
                f"{self._base_url}{endpoint}", timeout=self.timeout
            )
            r.raise_for_status()
            return r
        except requests.ConnectionError:
            self.get_logger().warn(
                f"连接失败 {self._base_url}，仿真是否启动？",
                throttle_duration_sec=5.0,
            )
        except requests.Timeout:
            self.get_logger().warn(
                f"请求 {endpoint} 超时 ({self.timeout}s)",
                throttle_duration_sec=5.0,
            )
        except requests.RequestException as e:
            self.get_logger().warn(
                f"请求 {endpoint} 异常: {e}", throttle_duration_sec=5.0
            )
        return None

    def _get_json(self, endpoint: str) -> dict | None:
        r = self._get(endpoint)
        if r is not None:
            try:
                return r.json()
            except Exception:
                self.get_logger().warn(
                    f"解析 {endpoint} JSON 失败", throttle_duration_sec=5.0
                )
        return None

    def _post(self, endpoint: str, json_data: dict):
        try:
            self._session.post(
                f"{self._base_url}{endpoint}",
                json=json_data,
                timeout=self.timeout,
            )
        except requests.RequestException as e:
            self.get_logger().warn(
                f"POST {endpoint} 异常: {e}", throttle_duration_sec=5.0
            )

    # ------------------------------------------------------------------
    #  图像 — GET /image → sensor_msgs/Image (独立定时器)
    # ------------------------------------------------------------------

    def _fetch_and_publish_image(self):
        r = self._get("/image")
        if r is None or r.status_code != 200:
            return

        try:
            arr = np.frombuffer(r.content, np.uint8)
            cv_img = cv2.imdecode(arr, cv2.IMREAD_COLOR)
            if cv_img is None:
                self.get_logger().warn("图像解码失败", throttle_duration_sec=5.0)
                return

            msg = self.bridge.cv2_to_imgmsg(cv_img, encoding="bgr8")
            msg.header.stamp = self.get_clock().now().to_msg()
            msg.header.frame_id = "camera_front"
            self.image_pub.publish(msg)

        except Exception:
            self.get_logger().warn(
                f"图像处理异常:\n{traceback.format_exc()}",
                throttle_duration_sec=5.0,
            )

    # ------------------------------------------------------------------
    #  同步快速数据 — scan / odom / imu / power (统一定时器)
    # ------------------------------------------------------------------

    def _sync_fast_data(self):
        """在同一拍内拉取并发布 scan / odom / imu / power。"""
        now = self.get_clock().now().to_msg()

        # ---- LaserScan ----
        scan_data = self._get_json("/scan")
        if scan_data:
            try:
                self._publish_scan(scan_data, now)
            except Exception:
                self.get_logger().warn(
                    f"LaserScan 异常:\n{traceback.format_exc()}",
                    throttle_duration_sec=5.0,
                )

        # ---- Odometry ----
        odom_data = self._get_json("/odom")
        if odom_data:
            try:
                self._publish_odom(odom_data, now)
            except Exception:
                self.get_logger().warn(
                    f"Odometry 异常:\n{traceback.format_exc()}",
                    throttle_duration_sec=5.0,
                )

        # ---- IMU ----
        imu_data = self._get_json("/imu")
        if imu_data:
            try:
                self._publish_imu(imu_data, now)
            except Exception:
                self.get_logger().warn(
                    f"IMU 异常:\n{traceback.format_exc()}",
                    throttle_duration_sec=5.0,
                )

        # ---- Power ----
        power_data = self._get_json("/power")
        if power_data:
            try:
                self._publish_power(power_data)
            except Exception:
                self.get_logger().warn(
                    f"Power 异常:\n{traceback.format_exc()}",
                    throttle_duration_sec=5.0,
                )

    # ---- 各数据发布方法 ----

    def _publish_scan(self, data: dict, now):
        """GET /scan → sensor_msgs/LaserScan

        HTTP 返回格式:
          angle_min, angle_max, angle_increment, range_min, range_max, ranges[]
        """
        msg = LaserScan()
        msg.header.stamp = now
        msg.header.frame_id = "laser"
        msg.angle_min       = float(data["angle_min"])
        msg.angle_max       = float(data["angle_max"])
        msg.angle_increment = float(data["angle_increment"])
        msg.range_min       = float(data.get("range_min", 0.15))
        msg.range_max       = float(data.get("range_max", 3.0))
        msg.ranges          = [float(r) for r in data["ranges"]]
        # intensities 置空
        msg.intensities = []
        self.scan_pub.publish(msg)

    def _publish_odom(self, data: dict, now):
        """GET /odom → nav_msgs/Odometry

        HTTP 返回格式:
          x, y, yaw, vx, vy, wz, orientation{w,x,y,z}
        """
        msg = Odometry()
        msg.header.stamp = now
        msg.header.frame_id = "odom"
        msg.child_frame_id = "base_link"

        msg.pose.pose.position.x = float(data["x"])
        msg.pose.pose.position.y = float(data["y"])
        msg.pose.pose.position.z = 0.0

        ori = data.get("orientation", {})
        msg.pose.pose.orientation = Quaternion(
            w=float(ori.get("w", 1.0)),
            x=float(ori.get("x", 0.0)),
            y=float(ori.get("y", 0.0)),
            z=float(ori.get("z", 0.0)),
        )

        msg.twist.twist.linear.x  = float(data.get("vx", 0.0))
        msg.twist.twist.linear.y  = float(data.get("vy", 0.0))
        msg.twist.twist.angular.z = float(data.get("wz", 0.0))

        # 协方差置 -1（无估计）
        msg.pose.covariance[0] = -1.0
        msg.twist.covariance[0] = -1.0

        self.odom_pub.publish(msg)

    def _publish_imu(self, data: dict, now):
        """GET /imu → sensor_msgs/Imu

        HTTP 返回格式:
          orientation{w,x,y,z}, angular_velocity{x,y,z}, linear_acceleration{x,y,z}
        """
        msg = Imu()
        msg.header.stamp = now
        msg.header.frame_id = "gyro_link"

        ori = data.get("orientation", {})
        msg.orientation = Quaternion(
            w=float(ori.get("w", 1.0)),
            x=float(ori.get("x", 0.0)),
            y=float(ori.get("y", 0.0)),
            z=float(ori.get("z", 0.0)),
        )

        av = data.get("angular_velocity", {})
        msg.angular_velocity.x = float(av.get("x", 0.0))
        msg.angular_velocity.y = float(av.get("y", 0.0))
        msg.angular_velocity.z = float(av.get("z", 0.0))

        la = data.get("linear_acceleration", {})
        msg.linear_acceleration.x = float(la.get("x", 0.0))
        msg.linear_acceleration.y = float(la.get("y", 0.0))
        msg.linear_acceleration.z = float(la.get("z", 0.0))

        # 协方差置 -1
        msg.orientation_covariance[0] = -1.0
        msg.angular_velocity_covariance[0] = -1.0
        msg.linear_acceleration_covariance[0] = -1.0

        self.imu_pub.publish(msg)

    def _publish_power(self, data: dict):
        """GET /power → std_msgs/Float32

        HTTP 返回格式:
          {"voltage": 12.0}
        """
        msg = Float32()
        msg.data = float(data.get("voltage", 0.0))
        self.power_pub.publish(msg)

    # ------------------------------------------------------------------
    #  控制 — ROS2 /cmd_vel (Twist) → POST /cmd (差速模式 vx/wz)
    # ------------------------------------------------------------------

    def _cmd_vel_callback(self, msg: Twist):
        """将 Twist 转为差速控制指令。

        Twist.linear.x  → vx (线速度 m/s)
        Twist.angular.z → wz (角速度 rad/s)

        HTTP /cmd 只接受差速模式 {"vx": vx, "wz": wz}，
        服务端内部通过 bicycle model 将 wz 转成阿克曼转向角。
        """
        vx = float(msg.linear.x)
        wz = float(msg.angular.z)

        self._post("/cmd", {"vx": vx, "wz": wz})

        self._cmd_count += 1
        self.get_logger().debug(
            f"cmd_vel → vx={vx:.2f}, wz={wz:.2f}  (#{self._cmd_count})"
        )

    # ------------------------------------------------------------------
    #  清理
    # ------------------------------------------------------------------

    def destroy_node(self):
        self._session.close()
        super().destroy_node()


# ------------------------------------------------------------------
#  entry point
# ------------------------------------------------------------------

def main():
    rclpy.init()
    node = OrigincarBridge()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("键盘中断，停止桥接")
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()