#include "rclcpp/rclcpp.hpp"
#include "interfaces/msg/imu_msg.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include <fstream>
using namespace std;

void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
{
    double norm = sqrt(qw*qw + qx*qx + qy*qy + qz*qz);
    if (norm < 1e-12) {  // 避免除以零
        roll = 0;
        pitch = 0;
        yaw = 0;
        return;
    }

    qw /= norm;
    qx /= norm;
    qy /= norm;
    qz /= norm;
    
    // 计算Roll角（X轴）
    roll = atan2(2 * (qw*qx + qy*qz), 1 - 2 * (qx*qx + qy*qy));
    
    // 计算Pitch角（Y轴），限制输入范围避免数值误差（asin输入需在[-1,1]）
    double sin_pitch = 2 * (qw*qy - qz*qx);
    sin_pitch = std::max(std::min(sin_pitch, 1.0), -1.0); 
    pitch = asin(sin_pitch);
    
    // 计算Yaw角（Z轴）
    yaw = atan2(2 * (qw*qz + qx*qy), 1 - 2 * (qy*qy + qz*qz));
}

// std::ofstream rpy_file("rpy.txt");
void QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
{
    QuaternionToRPYRad(qw, qx, qy, qz, roll, pitch, yaw);

    roll = roll * 180.0 / 3.1415926;
    pitch = pitch * 180.0 / 3.1415926;
    yaw = yaw * 180.0 / 3.1415926;
    // rpy_file<<roll<<","<<pitch<<","<<yaw<<std::endl;
    std::cout << "Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;
}

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("imu_dev_node");
  auto imu_pub = node->create_publisher<interfaces::msg::ImuMsg>("/imu_msg", 10);
  auto imu_sub=node->create_subscription<sensor_msgs::msg::Imu>("/imu/data", 10, [=](const sensor_msgs::msg::Imu::SharedPtr msg){
      //cout<<msg->linear_acceleration.x<<","<<msg->linear_acceleration.y<<","<<msg->linear_acceleration.z<<endl;
      //cout<<msg->angular_velocity.x<<","<<msg->angular_velocity.y<<","<<msg->angular_velocity.z<<endl;
      //cout<<msg->orientation.x<<","<<msg->orientation.y<<","<<msg->orientation.z<<","<<msg->orientation.w<<endl;
      interfaces::msg::ImuMsg imu_msg;
      imu_msg.source="waveshare";
      imu_msg.position="front";
      imu_msg.type="None";
      imu_msg.imu=*msg;

      double gyroValues_[3];

      QuaternionToRPYDeg(
        imu_msg.imu.orientation.w, 
        imu_msg.imu.orientation.x, 
        imu_msg.imu.orientation.y, 
        imu_msg.imu.orientation.z, 
        gyroValues_[0], 
        gyroValues_[1], 
        gyroValues_[2]
      );

      imu_pub->publish(imu_msg);
  });
///livox/imu
  auto imu_sub_1=node->create_subscription<sensor_msgs::msg::Imu>("/livox/imu",10,[=](const sensor_msgs::msg::Imu::SharedPtr msg){
		  
		  interfaces::msg::ImuMsg imu_msg;
      imu_msg.source="livox";
      imu_msg.position="front";
      imu_msg.type="None";
      imu_msg.imu=*msg;
      imu_pub->publish(imu_msg);

		  
		  
		  });


  rclcpp::spin(node);
  rclcpp::shutdown();
  rpy_file.close();
}
