/**
 * @file invicta_interface.hpp
 * @brief ROS1 数据面：订阅关节状态与 IMU，发布关节指令。关节表、话题来自 invicta_deploy.toml
 *
 * 本层只做**模型帧**的透传：/joint_states 与 /joint_commands 上都是模型量（rad / rad/s / Nm）。
 * 硬件的方向 `dir` 与零点 `offset` 由 invicta_hardware 在 raw↔model 的唯一边界处施加
 * （见 invicta_hardware/README.md「标定边界」），deploy 不再参与标定。
 * 上层状态机与策略只看到模型约定下的量。
 *
 * 相比之前：删掉了 auto_calibrate / ±360° 回绕标定（那是 M20 三关节腿的绝对编码器问题，
 * 本构型摆臂行程只有 ±80°，无回绕），也删掉了本地 dir/offset 换算（已上移到 hardware）。
 */
#pragma once

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>

#include <limits>
#include <string>
#include <unordered_map>
#include <vector>

#include "basic_function.hpp"
#include "common_types.h"
#include "config/deploy_config.h"
#include "invicta_msgs/JointCommandArray.h"
#include "invicta_msgs/control_word.h"
#include "robot_interface.h"

namespace interface {

class InvictaInterface : public RobotInterface {
public:
    InvictaInterface(const std::string& robot_name, int dof_num)
        : RobotInterface(robot_name, dof_num), cfg_(invicta::Config()) {
        if (dof_num_ != cfg_.dof) {
            ROS_WARN("[InvictaInterface] passed dof=%d does not match configured %d, using config value", dof_num_, cfg_.dof);
        }
        joint_pos_ = VecXf::Zero(dof_num_);
        joint_vel_ = VecXf::Zero(dof_num_);
        joint_tau_ = VecXf::Zero(dof_num_);
        joint_data_id_.resize(size_t(dof_num_), 0);
        driver_status_.assign(size_t(dof_num_), 1);

        for (int i = 0; i < dof_num_; ++i) name2slot_[cfg_.joint_names[size_t(i)]] = i;

        cmd_pub_ = nh_.advertise<invicta_msgs::JointCommandArray>(cfg_.topic_joint_commands,
                                                                  cfg_.queue_joint_commands);
        joint_state_sub_ = nh_.subscribe(cfg_.topic_joint_states, cfg_.queue_state,
                                        &InvictaInterface::JointStateHandler, this);
        imu_sub_ = nh_.subscribe(cfg_.topic_imu, cfg_.queue_state, &InvictaInterface::ImuHandler, this);
    }

    virtual ~InvictaInterface() {}

    // ------------------------------------------------------------------ 接口实现
    virtual void Start() {
        SendStartupSequence();
        std::cout << "[InvictaInterface] topics: " << cfg_.topic_joint_states << " / " << cfg_.topic_imu << " -> "
                  << cfg_.topic_joint_commands << std::endl;
        std::cout << "[InvictaInterface] model frame passthrough: dir/offset applied by invicta_hardware" << std::endl;
    }

    virtual void Stop() {}

    virtual double GetInterfaceTimeStamp() { return ri_ts_; }
    virtual VecXf GetJointPosition() { return joint_pos_; }
    virtual VecXf GetJointVelocity() { return joint_vel_; }
    virtual VecXf GetJointTorque() { return joint_tau_; }
    virtual Vec3f GetImuRpy() { return rpy_; }
    virtual Vec3f GetImuAcc() { return acc_; }
    virtual Vec3f GetImuOmega() { return omega_body_; }
    virtual VecXf GetContactForce() { return VecXf::Zero(4); }
    virtual double GetImuTimestamp() { return imu_ts_; }
    /// 距最近一帧**可用** IMU 的时长 [s]；从未收到过返回极大值（安全线程用，纯读）
    virtual double GetImuAgeSec() {
        if (!has_imu_valid_) return std::numeric_limits<double>::max();
        return (ros::Time::now() - imu_valid_arrival_).toSec();
    }
    /// IMU 是否健康（控制线程用；状态由 RefreshRobotData() 每周期更新）
    virtual bool IsImuHealthy() { return imu_healthy_; }
    virtual std::vector<uint16_t> GetJointDataID() { return joint_data_id_; }
    virtual std::vector<uint16_t> GetDriverStatusWord() { return driver_status_; }

    virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) {
        joint_cmd_ = input;
        invicta_msgs::JointCommandArray msg;
        msg.header.stamp = ros::Time::now();
        msg.name = cfg_.joint_names;
        msg.position.resize(size_t(dof_num_));
        msg.velocity.resize(size_t(dof_num_));
        msg.effort.resize(size_t(dof_num_));
        msg.kp.resize(size_t(dof_num_));
        msg.kd.resize(size_t(dof_num_));
        for (int i = 0; i < dof_num_; ++i) {
            const auto& j = cfg_.Joint(i);
            // 模型帧透传：不做 dir/offset 换算（帽子在 invicta_hardware）
            msg.position[i] = input(i, 1);
            msg.velocity[i] = input(i, 3);
            msg.effort[i] = input(i, 4);
            msg.kp[i] = (cfg_.wheel_kp_zero && j.is_wheel) ? 0.f : input(i, 0);
            msg.kd[i] = input(i, 2);
        }
        msg.control_word = invicta_msgs::kMotorControl;
        cmd_pub_.publish(msg);
    }

    /// 每个控制周期由状态机调一次：刷新 IMU 连续性判定（断流判坏 + 恢复去抖，见 RefreshImuHealth）
    virtual void RefreshRobotData() { RefreshImuHealth(); }

protected:
    // ------------------------------------------------------------------ 启动序列
    void PublishRawCommand(uint16_t control_word) {
        invicta_msgs::JointCommandArray msg;
        msg.header.stamp = ros::Time::now();
        msg.name = cfg_.joint_names;
        msg.position.assign(size_t(dof_num_), 0.);
        msg.velocity.assign(size_t(dof_num_), 0.);
        msg.effort.assign(size_t(dof_num_), 0.);
        msg.kp.assign(size_t(dof_num_), 0.);
        msg.kd.assign(size_t(dof_num_), 0.);
        msg.control_word = control_word;
        cmd_pub_.publish(msg);
    }

    /// 使能时序：disable → error reset → enable；此后运行期恒为 motor control
    /// ⚠ ROS1 的发布没有 latch：订阅端连接建立前发出去的消息会被直接丢掉。
    ///   若 hardware 节点后启动（或本节点重启），一次性时序帧会整段丢失，
    ///   所以这里先等订阅者出现再发。
    void SendStartupSequence() {
        for (int i = 0; i < 200 && cmd_pub_.getNumSubscribers() == 0 && ros::ok(); ++i) {
            ros::Duration(0.01).sleep();
        }
        if (cmd_pub_.getNumSubscribers() == 0) {
            std::cout << "[InvictaInterface] warning: joint_commands has no subscribers yet, startup sequence may be lost" << std::endl;
        }
        const uint16_t seq[] = {invicta_msgs::kDisable, invicta_msgs::kErrorReset, invicta_msgs::kEnable};
        for (uint16_t word : seq) {
            PublishRawCommand(word);
            ros::Duration(0.01).sleep();
        }
        std::cout << "[InvictaInterface] startup control sequence complete (disable -> error_reset -> enable)" << std::endl;
    }

    // ------------------------------------------------------------------ 回调
    void JointStateHandler(const sensor_msgs::JointState::ConstPtr& msg) {
        const size_t n = size_t(dof_num_);
        if (msg->position.size() != n || msg->velocity.size() != n || msg->effort.size() != n) {
            ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s length should be %d, got pos=%zu vel=%zu eff=%zu, dropping frame",
                              cfg_.topic_joint_states.c_str(), dof_num_, msg->position.size(), msg->velocity.size(),
                              msg->effort.size());
            return;
        }

        const bool by_name = msg->name.size() == n;
        int unknown = 0;
        for (size_t k = 0; k < n; ++k) {
            const int i = by_name ? LookupSlot(msg->name[k]) : int(k);
            if (i < 0) {
                ++unknown;
                continue;
            }
            // 模型帧透传：不做 dir/offset 换算（帽子在 invicta_hardware）
            joint_pos_(i) = float(msg->position[k]);
            joint_vel_(i) = float(msg->velocity[k]);
            joint_tau_(i) = float(msg->effort[k]);
        }
        if (!by_name && !warned_name_mismatch_) {
            warned_name_mismatch_ = true;
            ROS_WARN("[InvictaInterface] %s did not provide the agreed %d names, resolving by array index",
                     cfg_.topic_joint_states.c_str(), dof_num_);
        }
        if (unknown > 0) {
            ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s has %d unknown joint names, ignored",
                              cfg_.topic_joint_states.c_str(), unknown);
        }

        for (int i = 0; i < dof_num_; ++i) joint_data_id_[i] = joint_data_frame_;
        ++joint_data_frame_;
        ri_ts_ = msg->header.stamp.toSec();
    }

    int LookupSlot(const std::string& name) const {
        auto it = name2slot_.find(name);
        return it == name2slot_.end() ? -1 : it->second;
    }

    void ImuHandler(const sensor_msgs::Imu::ConstPtr& msg) {
        imu_arrival_ = ros::Time::now();
        has_imu_ = true;
        // orientation_covariance[0] == -1 表示「不提供姿态」
        if (msg->orientation_covariance[0] == -1.0) {
            // 帧在来但没有可用姿态：对 RL 而言等同断流（base_rpy 会停在旧值，不能当有效观测）
            has_imu_valid_ = false;
            if (!warned_imu_no_orientation_) {
                warned_imu_no_orientation_ = true;
                ROS_WARN("[InvictaInterface] %s did not provide orientation (covariance=-1), attitude observation invalid",
                         cfg_.topic_imu.c_str());
            }
            return;
        }
        Eigen::Quaternionf q(float(msg->orientation.w), float(msg->orientation.x), float(msg->orientation.y),
                             float(msg->orientation.z));
        if (q.norm() < 1e-6f) {
            has_imu_valid_ = false;
            ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s quaternion norm near zero, ignoring this frame", cfg_.topic_imu.c_str());
            return;
        }
        q.normalize();
        rpy_ = q.toRotationMatrix().eulerAngles(0, 1, 2).cast<float>();  // ZYX，与 RpyToRm 一致
        omega_body_ << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z;
        acc_ << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z;
        imu_ts_ = msg->header.stamp.toSec();
        imu_valid_arrival_ = imu_arrival_;
        has_imu_valid_ = true;
    }

    /**
     * IMU 连续性判定（只在控制线程里由 RefreshRobotData() 调一次）：
     *   判坏：最近一帧可用 IMU 的龄期 > rl_imu_timeout_s（或从未收到过、或帧不提供姿态）
     *   恢复：断流之后需要连续 rl_imu_recover_s 的新鲜可用帧才重新判为健康（去抖）
     * 之所以要恢复去抖：否则 IMU 抖动一帧就会立刻放行重新进 RL。
     */
    void RefreshImuHealth() {
        const ros::Time now = ros::Time::now();
        const bool fresh = has_imu_valid_ && (now - imu_valid_arrival_).toSec() <= cfg_.rl_imu_timeout_s;
        if (!fresh) {
            imu_good_since_valid_ = false;
            imu_healthy_ = false;
            return;
        }
        if (!imu_good_since_valid_) {
            imu_good_since_ = now;
            imu_good_since_valid_ = true;
        }
        if ((now - imu_good_since_).toSec() >= cfg_.rl_imu_recover_s) imu_healthy_ = true;
    }

protected:
    const invicta::DeployConfig& cfg_;
    ros::NodeHandle nh_;
    std::unordered_map<std::string, int> name2slot_;

    double ri_ts_ = 0., imu_ts_ = 0.;

    // ---- IMU 连续性（见 RefreshImuHealth）----
    ros::Time imu_arrival_, imu_valid_arrival_, imu_good_since_;
    bool has_imu_ = false;         ///< 是否收到过任何 IMU 帧（哪怕不提供姿态）
    bool has_imu_valid_ = false;   ///< 最近一帧是否提供了可用姿态
    bool imu_good_since_valid_ = false, imu_healthy_ = false;
    Vec3f omega_body_, rpy_, acc_;
    VecXf joint_pos_, joint_vel_, joint_tau_;
    uint16_t joint_data_frame_ = 0;
    std::vector<uint16_t> joint_data_id_;
    std::vector<uint16_t> driver_status_;
    bool warned_name_mismatch_ = false;
    bool warned_imu_no_orientation_ = false;

    ros::Publisher cmd_pub_;
    ros::Subscriber joint_state_sub_, imu_sub_;
};

};  // namespace interface