#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <serial/serial.h>
#include <thread>
#include <iostream>
#include <cmath>

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/transform_broadcaster.h>

#include "mavlink2ros/ServoCtrl.h"
#include "FishCom/mavlink.h"
#include <mavlink2ros/Optlimit.h>//new
#include "mavlink2ros/FunctionCtrl.h"//new

#define SERVO_DUTY_CYCLE_MIN  499
#define SERVO_DUTY_CYCLE_MAX  2499
#define SERVO_DUTY_CYCLE_INIT  1500

class RobotController {
public:
    RobotController(const std::string& port, int baud_rate) : 
        serial_device(port, baud_rate, serial::Timeout::simpleTimeout(1000)),
        chassis_odom_pub_(nh.advertise<nav_msgs::Odometry>("odom", 50)),
        optlimit_pub(nh.advertise<mavlink2ros::Optlimit>("optlimit_info", 50)) 
    {
        if (!serial_device.isOpen()) {
            ROS_ERROR_STREAM("Failed to open serial port.");
        }

        cmd_ctrl_sub_ = nh.subscribe("cmd_vel", 100, &RobotController::cmdVelCallback, this);
        servo_ctrl_sub_ = nh.subscribe("servo_ctrl", 100, &RobotController::servoCtrlCallback, this);

        // 
        receive_thread = std::thread(&RobotController::receiveData, this);


        manage_timer = nh.createTimer(ros::Duration(1.0), &RobotController::sendManageInfoRegularly, this);
        control_timer = nh.createTimer(ros::Duration(0.01), &RobotController::sendCtrlInfoRegularly, this);
        servos_timer = nh.createTimer(ros::Duration(0.01), &RobotController::sendServosInfoRegularly, this);

        chassis_odom_pub_=nh.advertise<nav_msgs::Odometry>("odom",1);
        odom_.header.frame_id = "odom";
        odom_.child_frame_id = "base_link";
        odom_tf_.header.frame_id = "odom";
        odom_tf_.child_frame_id = "base_link";

        optlimit_pub = nh.advertise<mavlink2ros::Optlimit>("optlimit_info",1);//new
        optlimit_sub = nh.subscribe("optlimit_info", 100, &RobotController::optlimitCallback, this);//new
        // optlimit_msg.pattern = gpio_read_level;//initialize the message


        function_sub = nh.subscribe("function_id", 100, &RobotController::functionCtrlCallback, this);//new functionid
    }

    ~RobotController() {
        if (receive_thread.joinable()) {
            receive_thread.join();
        }
    }

    void runTask(){
        int functionid = function_id.functionid;
        if(functionid == 1)
        {
            upper_to_lower();
            clp_close();
            lower_to_upper();
            clp_open();
            printf("run\n");
        }
        if(functionid == 2)
        {
            servo_ctrl_r(4, 1.7);
            servo_ctrl_r(4,0.3);
        }
        if(functionid == 3)
        {
            upper_to_lower();
            servo_ctrl_r(2, 0.65);
            lower_to_upper();
        }
        if(functionid == 4)
        {
            upper_to_lower();
            servo_ctrl_r(2, 0.0);
            lower_to_upper();
        }
        if(functionid == 5)
        {
           servos_positions[5] = 1.65 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN; 
        }
        if(functionid == 6)
        {
            servos_positions[5] = 1.45 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;

        }
        if(functionid == 7)
        {
            servos_positions[0] = 3.14 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        }//check id and give the diploma
        if(functionid == 8)

        {
            servos_positions[0] = 0 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        }//check id and give the diploma
    }

    void sendManageInfoRegularly(const ros::TimerEvent&) {
        sendManageInfo(1, 1, 1);
    }

    void sendCtrlInfoRegularly(const ros::TimerEvent&) {
        std::lock_guard<std::mutex> lock(twist_mutex); // 确保线程安全
        sendCtrlInfo(current_twist.linear.x, current_twist.linear.y, current_twist.angular.z);
    }

    void sendServosInfoRegularly(const ros::TimerEvent&){
        std::lock_guard<std::mutex> lock(servos_mutex); 
        sendServosInfo(servos_positions[0],servos_positions[1],servos_positions[2],servos_positions[3],
            servos_positions[4],servos_positions[5],servos_positions[6]);
    }

    void sendManageInfo(bool enable_chassis,bool enable_servos,bool reset_quaternion){
        mavlink_message_t *msg = (mavlink_message_t *)malloc(sizeof(mavlink_message_t));
        memset(msg, 0, sizeof(mavlink_message_t));

        uint16_t Txlen = 0;
        uint8_t *txbuf = (uint8_t *)malloc(MAVLINK_MSG_ID_CHS_MANAGE_INFO_LEN + 12);

        mavlink_msg_chs_manage_info_pack(
            CHS_SYSTEM_ID::CHS_ID_ORANGE,
            CHS_SYSTEM_ID::CHS_ID_ORANGE, 
            msg, enable_chassis, enable_servos, reset_quaternion);
        
        Txlen = mavlink_msg_to_send_buffer(txbuf, msg);

        serial_device.write(txbuf, Txlen);
        
        free(msg);
        free(txbuf);
    }

    void sendCtrlInfo(float vx, float vy, float vz){
        mavlink_message_t *msg = (mavlink_message_t *)malloc(sizeof(mavlink_message_t));
        memset(msg, 0, sizeof(mavlink_message_t));

        uint16_t Txlen = 0;
        uint8_t *txbuf = (uint8_t *)malloc(MAVLINK_MSG_ID_CHS_CTRL_INFO_LEN + 12);

        mavlink_msg_chs_ctrl_info_pack(
            CHS_SYSTEM_ID::CHS_ID_ORANGE,
            CHS_SYSTEM_ID::CHS_ID_ORANGE, 
            msg, 
            -vy, vx, vz);
        
        Txlen = mavlink_msg_to_send_buffer(txbuf, msg);

        serial_device.write(txbuf, Txlen);

        free(msg);
        free(txbuf);
    }

    void sendServosInfo(uint16_t servo1, uint16_t servo2, uint16_t servo3, 
                        uint16_t servo4, uint16_t servo5, uint16_t servo6, uint16_t servo7){
        mavlink_message_t *msg = (mavlink_message_t *)malloc(sizeof(mavlink_message_t));
        memset(msg, 0, sizeof(mavlink_message_t));

        uint16_t Txlen = 0;
        uint8_t *txbuf = (uint8_t *)malloc(MAVLINK_MSG_ID_CHS_SERVOS_INFO_LEN + 12);

        uint16_t servos[7] = {servo1, servo2, servo3, servo4, servo5, servo6, servo7};

        mavlink_msg_chs_servos_info_pack(
            CHS_SYSTEM_ID::CHS_ID_ORANGE,
            CHS_SYSTEM_ID::CHS_ID_ORANGE, 
            msg, 
            servos);
        
        Txlen = mavlink_msg_to_send_buffer(txbuf, msg);

        serial_device.write(txbuf, Txlen);

        free(msg);
        free(txbuf);
    }

    void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg) {
        current_twist = *msg;
    }

    void servoCtrlCallback(const mavlink2ros::ServoCtrl::ConstPtr& msg) {
        std::lock_guard<std::mutex> lock(servos_mutex);//这里使用了互斥锁（mutex），目的是确保在多线程环境中对servos_positions的访问是安全的。锁定在函数的生命周期内，以防止其他线程同时修改servos_positions。
        int servo_id = msg->servo_id;
        if (servo_id >= 0 && servo_id < 7) {
            double servo_angle = msg->servo_angle;
            servos_positions[servo_id] = static_cast<int>(servo_angle * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI) + SERVO_DUTY_CYCLE_MIN;//将上一步计算的结果强制转换为整数。这可能导致小数部分被截断，因为舵机位置通常以整数值表示。
            ROS_INFO("updated! Servo ID: %d, New Position: %d", servo_id, servos_positions[servo_id]);
        }

        std::stringstream ss;
        ss << "Current Servo Positions: ";
        for(int i = 0; i < 7; i++) {
         
            ss << servos_positions[i] << " ";
        }
        ROS_INFO_STREAM(ss.str());
    }

    void optlimitCallback(const mavlink2ros::Optlimit::ConstPtr& msg){
        int optlimit_position = msg->pattern;
        if(optlimit_position == 1)
        {
            // sendServosInfo(servos_positions[0], servos_positions[1], servos_positions[2], servos_positions[3], servos_positions[4], 1400, servos_positions[6]);
            servos_positions[5] = 1499;//need to check
        } 
    }

    void upper_to_lower()
    {
        servos_positions[1] = 1.3 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        ros::Duration(1).sleep();         
        servos_positions[0] = 2.8 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        ros::Duration(1).sleep();
        servos_positions[1] = 2.75 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        ros::Duration(1).sleep();
        servos_positions[0] = 2.5 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        ros::Duration(1.5).sleep();
    }
    void lower_to_upper()
    {
        servos_positions[0] = 2.8 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        ros::Duration(1).sleep();         
        servos_positions[1] = 1.3 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        ros::Duration(1).sleep();
        servos_positions[0] = 3.1 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        ros::Duration(1).sleep();
        servos_positions[1] = 0.55 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        ros::Duration(1).sleep();
    }
    void clp_open()
    {
        servos_positions[2] = 0.0 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        ros::Duration(0.8).sleep(); 
    }
    void clp_close()
    {
        servos_positions[2] = 0.65 * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        ros::Duration(0.8).sleep(); 
    }
    void servo_ctrl_r(int servo_Id, double servo_Angle)
    {
        servos_positions[servo_Id] = servo_Angle * (SERVO_DUTY_CYCLE_MAX - SERVO_DUTY_CYCLE_MIN) / M_PI + SERVO_DUTY_CYCLE_MIN;
        printf("%d",servos_positions[servo_Id]);
        ros::Duration(0.8).sleep();  
    }
    void functionCtrlCallback(const mavlink2ros::FunctionCtrl::ConstPtr& msg)
    {
        function_id = *msg;
        std::thread taskThread(&RobotController::runTask, this);
        taskThread.detach();
    }//new   


    void receiveData() {
        while (ros::ok()) {
            if (serial_device.available()) {
                mavlink_message_t *msg = (mavlink_message_t *)malloc(sizeof(mavlink_message_t));
                memset(msg, 0, sizeof(mavlink_message_t));

                mavlink_status_t status;
                uint8_t rxbuf[512];
                size_t RxLen = serial_device.read(rxbuf, sizeof(rxbuf));

                mavlink_chs_odom_info_t odom_info;
                mavlink_chs_ctrl_info_t ctrl_info;
                mavlink_chs_motor_info_t motor_info;
                mavlink_chs_servos_info_t servos_info;
                mavlink_chs_manage_info_t manage_info;
                mavlink_chs_remoter_info_t remoter_info;
                mavlink_chs_gpio_read_t gpio_read;


                for (size_t i = 0; i < RxLen; ++i) {
                    ROS_DEBUG_STREAM("Byte " << i << ": 0x" << std::hex << static_cast<int>(rxbuf[i]));
                    if (mavlink_parse_char(MAVLINK_COMM_0, rxbuf[i], msg, &status)) {
                        ROS_INFO_STREAM("第 " << i << " 个包，解包成功：" << "Message ID: " << msg->msgid);
                        switch (msg->msgid) {
                            case MAVLINK_MSG_ID_CHS_ODOM_INFO: {
                                mavlink_msg_chs_odom_info_decode(msg, &odom_info);
                                ROS_DEBUG_STREAM("received odom info! vx: " << odom_info.vx << ", vy: " << odom_info.vy << ", vz: " << odom_info.vw);
                                std::stringstream ss;
                                ss << "quaternion: ";
                                for (int i = 0; i < 4; i++) ss << odom_info.quaternion[i] << " ";
                                ROS_DEBUG_STREAM(ss.str());

                                std::swap(odom_info.vx,odom_info.vy);
                                odom_info.vy = -odom_info.vy;
                                

                                current_time = ros::Time::now();
                                double dt = (current_time - last_time).toSec();

                                double delta_x = (odom_info.vx * cos(th) - odom_info.vy * sin(th)) * dt;
                                double delta_y = (odom_info.vx * sin(th) + odom_info.vy * cos(th)) * dt;
                                double delta_th = odom_info.vw * dt;

                                x += delta_x;
                                y += delta_y;
                                th += delta_th;

                                odom_.header.stamp = current_time;
                                odom_.twist.twist.linear.x = odom_info.vx;
                                odom_.twist.twist.linear.y = odom_info.vy;
                                odom_.twist.twist.angular.z = odom_info.vw;

                                geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
                                odom_.pose.pose.position.x = x;
                                odom_.pose.pose.position.y = y;
                                odom_.pose.pose.position.z = 0.0;
                                odom_.pose.pose.orientation = odom_quat;

                                chassis_odom_pub_.publish(odom_);
                                

                                odom_tf_.header.stamp = current_time;
                                odom_tf_.transform.translation.x = x;
                                odom_tf_.transform.translation.y = y;
                                odom_tf_.transform.translation.z = 0.0;
                                odom_tf_.transform.rotation = odom_quat;
                                tf_broadcaster_.sendTransform(odom_tf_);

                                last_time = current_time;
                                break;
                            }
                            case MAVLINK_MSG_ID_CHS_REMOTER_INFO: {
                                mavlink_msg_chs_remoter_info_decode(msg, &remoter_info);

                                //printf("%d,",remoter_info.channel_0);

                                ROS_INFO_STREAM("received remoter_info: switch_message: " << remoter_info.switch_messgae <<
                                    ", channel_0: " << remoter_info.channel_0 << ", channel_1: " << remoter_info.channel_1 <<
                                    ", channel_2: " << remoter_info.channel_2 << ", channel_3: " << remoter_info.channel_3 <<
                                    ", wheel: " << remoter_info.wheel);

                                geometry_msgs::Twist cmd_vel;
                                cmd_vel.angular.z = static_cast<float>(remoter_info.channel_2) / -660.0;
                                cmd_vel.linear.x = static_cast<float>(remoter_info.channel_0) / 660.0;
                                cmd_vel.linear.y = static_cast<float>(remoter_info.channel_2) / -660.0;
                                cmd_vel_pub_.publish(cmd_vel);

                                int switch_messgae_1 = remoter_info.switch_messgae;
                                if(switch_messgae_1%2 != 0)
                                {
                                    uint8_t switch_messgae_2 = remoter_info.switch_messgae;  // 二进制表示为 10001110

                                        // 读取第0位
                                    //uint8_t zerothBit =  & 0x01;

                                        // 读取第1和第2位
                                    uint8_t firstTwoBits = ( switch_messgae_2>> 1) & 0x03;

                                        // 读取第3和第4位
                                    uint8_t nextTwoBits = (switch_messgae_2 >> 3) & 0x03;
                                    int channel_0_p = remoter_info.channel_0;
                                    int channel_1_p = remoter_info.channel_1;
                                    int channel_2_p = remoter_info.channel_2;
                                    int channel_3_p = remoter_info.channel_3;
                                    //start to check
                                    //判断GPS，ATTI，ATTI的状态并作出反应
                                    switch (nextTwoBits) {
                                        case 0b00:
                                            clp_open();
                                            printf("%s\n", "clp_open");
                                            break;
                                        case 0b01:
                                            printf("%s\n", "nothing");
                                            break;
                                        case 0x02:
                                            clp_close();
                                            printf("%s\n", "clp_close");
                                            break;
                                        default:
                                            sprintf("%s\n", "fuck");
                                    }
                                    if(channel_0_p >= 100)
                                    {
                                        sendCtrlInfo(0, 1.0, 0); 
                                    }
                                    if(channel_0_p <= -100)
                                    {
                                        sendCtrlInfo(0, -1.0, 0);
                                    }
                                    if(channel_1_p >= 100)//需要实验是否是这个方向
                                    {
                                        sendCtrlInfo(1.0, 0, 0); 
                                    }
                                    if(channel_1_p <= -100)
                                    {
                                        sendCtrlInfo(-1.0, 0, 0);
                                    }
                                    if(channel_3_p >= 100)//需要实验是否是这个方向
                                    {
                                        sendCtrlInfo(0, 0, 1/M_PI); 
                                    }
                                    if(channel_3_p <= -100)
                                    {
                                        sendCtrlInfo(0, 0, -1/M_PI);
                                    }
                                    if(channel_2_p >= 100)//需要实验是否是这个方向
                                    {
                                        upper_to_lower(); 
                                        printf("%s\n", "down");
                                    }
                                    if(channel_2_p <= -100)
                                    {
                                        lower_to_upper();
                                        printf("%s\n", "up");
                                    }

                                }
                                


                    
                                break;
                            }
                            case MAVLINK_MSG_ID_CHS_GPIO_READ:{
                                mavlink_msg_chs_gpio_read_decode(msg, &gpio_read);
                                optlimit_msg.pattern = gpio_read.level;
                                // printf("%d\n",optlimit_msg.pattern);
                                optlimit_pub.publish(optlimit_msg);//new 
                                break;
                            }
            
                        }
                    }
                }

                free(msg);
            }
            usleep(1000);
        }
    }

private:
    ros::NodeHandle nh;
    serial::Serial serial_device;
    std::thread receive_thread;
    geometry_msgs::Twist current_twist; 

    ros::Timer manage_timer;
    ros::Timer control_timer;
    ros::Timer servos_timer;

    std::mutex twist_mutex;
    std::mutex servos_mutex;
    ros::Subscriber cmd_ctrl_sub_;
    ros::Subscriber servo_ctrl_sub_;
    ros::Subscriber optlimit_sub;//new

    ros::Publisher cmd_vel_pub_;

    tf::TransformBroadcaster tf_broadcaster_;//! ros chassis odometry tf broadcaster
    ros::Publisher chassis_odom_pub_;//! ros odometry message publisher
    
    //self defined message
    geometry_msgs::TransformStamped odom_tf_;//! ros chassis odometry tf
    nav_msgs::Odometry odom_;//! ros odometry message

    mavlink2ros::Optlimit optlimit_msg;//define the message of optical limit
    ros::Publisher optlimit_pub;//new

    mavlink2ros::FunctionCtrl function_id;//newnew message
    ros::Subscriber function_sub;//newnew subscriber

    // mavlink2ros::FunctionCtrl function_ctrl;

    // int gpio_read_level;//new
    double x = 0.0;
    double y = 0.0;
    double th = 0.0;
    ros::Time current_time = ros::Time::now(), last_time = ros::Time::now();

    int servos_positions[7] = {2474, 849, 499, 
        SERVO_DUTY_CYCLE_INIT, 499, SERVO_DUTY_CYCLE_INIT, SERVO_DUTY_CYCLE_INIT};
}; 

int main(int argc, char** argv) {
    ros::init(argc, argv, "robot_controller");
    RobotController controller("/dev/robomaster", 115200);
    ros::spin();
    return 0;
}