#include "board.h"
#include "hpm_can_drv.h"
#include "control_rate.h"
#include "hpm_can_drv.h"
#include "flight_mode.h"
#include "helpler_funtions.h"
#include "mode_gcs_tax_launch_run.h"
#include "params.h"
#include "soft_can.h"
#include "soft_imu.h"
#include "soft_time.h"
#include "string.h"
#include <math.h>
#include "soft_can_yy.h"
#include <stdint.h>


static uint32_t last_tx_time = 0;
static uint8_t tx_loop_on = 0;
uint8_t status_broadcast = 0; // 广播当前指令状态位 默认b0-0未收到弹射准备

static inline void memcpy_bigend(void *dst, const void *src, uint32_t length)
{
    uint8_t *p_src = (uint8_t *)src;
    uint8_t *p_dst = (uint8_t *)dst;

    for (uint32_t i = 0; i < length; ++i)
    {
        p_dst[i] = p_src[length - i - 1];
    }
}

static inline void memcpy_smallend(void *dst, const void *src, uint32_t length)
{
    uint8_t *p_src = (uint8_t *)src;
    uint8_t *p_dst = (uint8_t *)dst;

    for (uint32_t i = 0; i < length; ++i)
    {
        p_dst[i] = p_src[i];
    }
}

static struct yy_can_rx_msg_data __yy_can_data = {.txgl_yaw_sp = NAN, .txgl_alt_sp = NAN};

const struct yy_can_rx_msg_data *yy_can_rx_data_get(void)
{
    return &__yy_can_data;
}

static inline uint32_t __yy_id_comb(uint32_t tx_node, uint32_t rx_node, uint32_t msg_id)
{
    uint32_t extid = 0;
    extid += (tx_node & 0xff) << 24;
    extid += (rx_node & 0xff) << 16;
    extid += (msg_id & 0xff) << 8;
    return extid;
}

static inline uint32_t __yy_id_get_tx_node(uint32_t extid)
{
    return (extid >> 24) & 0xff;
}

static inline uint32_t __yy_id_get_rx_node(uint32_t extid)
{
    return (extid >> 16) & 0xff;
}

static inline uint32_t __yy_id_get_msg_id(uint32_t extid)
{
    return (extid >> 8) & 0xff;
}

void updata_cmd_status(enum yy_CAN_cmd_execute_bit bit, bool status)
{
    if (status)
    {
        SET_STATUS_BIT(status_broadcast, bit);
    }
    else
    {
        CLEAR_STATUS_BIT(status_broadcast, bit);
    }
}
static void send_drone_selfcheck_msg(void)
{
    struct drone_selfcheck_msg
    {
        uint8_t selfcheck_status;
        uint8_t error_code;
        uint8_t factory_info;
        uint8_t soft_stage;
        uint8_t soft_version;
        uint8_t identification_year;
        uint8_t identification_batch;
        uint8_t identification_order;
    };
    can_transmit_buf_t msg = {0};
     // msg.StdId = 0;
    msg.extend_id = 1;// 扩展ID模式
    msg.remote_frame = 0;    // 数据帧
    msg.id = __yy_id_comb(NODE_WRJ, NODE_TXGL, MSG_DRONE_SELF_CHECK); // CAN ID
    msg.dlc = 8; // 数据长度

    struct drone_selfcheck_msg p = {.selfcheck_status = 0x01,      // 自检结果 01正常
                                    .error_code = 0x00,            // 故障信息 00默认 01IMU无连接
                                    .factory_info = 0x01,          // 厂家信息 01云翼
                                    .soft_stage = 0x02,            // 软件阶段 02初样03正样04批产
                                    .soft_version = 0x01,          // 初始 01
                                    .identification_year = 24,     // 2024年取24
                                    .identification_batch = 0x01,  // 批次号
                                    .identification_order = 0x01}; // 产品编号
    p.identification_order = (uint8_t)verinf._serial_id;

    if (imu_link_status == COMP_NOEXIST)
    {
        p.selfcheck_status = 0x00;
        p.error_code = 0x01;
    }
    else if (imu_link_status == COMP_NORMAL)
    {
        p.selfcheck_status = 0x01;
        p.error_code = 0x00;
    }
    else
    {
        p.selfcheck_status = 0x11;
        p.error_code = 0x01;
    }

    memcpy(&msg.data, &p, msg.dlc);

    canSendMsg(&msg);
}

uint32_t cw_cmd_count = 0, ccw_cmd_count = 0;
uint16_t hdw_can_delay = 0;
bool debug_mode = false;
uint32_t recv_alt_time = 0;
bool alt_reached;
bool yaw_reached;
int YY_can_rx_decode(const can_receive_buf_t *rxmsg)
{
    int ret = 0;
    static bool recv_tof = false;

    if (rxmsg->extend_id == 1)
    {
        /*调试模式*/
        if(0x011c0110 == rxmsg->id)
        {
            if(0 ==(rxmsg->data[0] & 0x01))
            {
                debug_mode = false;
            }
            else if(1 == (rxmsg->data[0] & 0x01))
            {
                debug_mode = true;
            }
        }
        
        uint32_t rx_node = __yy_id_get_rx_node(rxmsg->id);
        uint32_t msg_id = __yy_id_get_msg_id(rxmsg->id);
        if ((rx_node & NODE_WRJ) == NODE_WRJ)
        {
            switch (msg_id)
            {
            case MSG_IMU_SOLV:
            {
                struct imu_solv_msg
                {
                    uint16_t yaw;
                    int16_t pitch;
                    int16_t roll;
                    uint8_t solv_status;  /* b0：0正欧拉 1-反欧拉 b1b2：01在线修正 10-纯惯导*/
                    uint8_t align_status; /* 00-默认对准中 01-1级调平正常 03-2级调平正常 05-3级调平正常
                                             02-1级调平异常 04-2级调平异常 06-3级调平异常 */
                };
                struct imu_solv_msg p = {0};
                int index = 0;

                memcpy_bigend(&p.yaw, &rxmsg->data[index], sizeof(p.yaw));
                index += sizeof(p.yaw);

                memcpy_bigend(&p.pitch, &rxmsg->data[index], sizeof(p.pitch));
                index += sizeof(p.pitch);

                memcpy_bigend(&p.roll, &rxmsg->data[index], sizeof(p.roll));
                index += sizeof(p.roll);

                p.solv_status = rxmsg->data[index];
                index += sizeof(p.solv_status);

                p.align_status = rxmsg->data[index];
                index += sizeof(p.align_status);

                // #error confirm yaw
                __yy_can_data.imu_yaw = p.yaw * 0.01f;
                __yy_can_data.imu_roll = p.roll * 0.01f;
                __yy_can_data.imu_pitch = p.pitch * 0.01f;

                __yy_can_data.imu_status = p.solv_status;
            }
            break;
            case MSG_IMU_A:
            {
                struct imu_a_msg
                {
                    int32_t x_rate;
                    int32_t y_rate;
                };

                struct imu_a_msg p = {0};

                int index = 0;
                memcpy_bigend(&p.x_rate, rxmsg->data + index, sizeof(p.x_rate));
                index += sizeof(p.x_rate);

                memcpy_bigend(&p.y_rate, rxmsg->data + index, sizeof(p.y_rate));

                __yy_can_data.imu_xyz_rate[0] = p.x_rate * 1e-6f;
                __yy_can_data.imu_xyz_rate[1] = p.y_rate * 1e-6f;
            }
            break;
            case MSG_IMU_B:
            {
                struct imu_b_msg
                {
                    int32_t z_rate;
                    int32_t x_acc;
                    int8_t temp;
                };

                struct imu_b_msg p = {0};
                int index = 0;

                memcpy_bigend(&p.z_rate, &rxmsg->data[index], sizeof(p.z_rate));
                index += sizeof(p.z_rate);

                memcpy_bigend(&p.x_acc, &rxmsg->data[index], 3);
                index += 3;

                p.temp = rxmsg->data[index];

                __yy_can_data.imu_xyz_rate[2] = p.z_rate * 1e-6f;
                __yy_can_data.imu_xyz_acc[0] = (p.x_acc & 0xffffff) * 1e-4f;
            }
            break;
            case MSG_IMU_C:
            {
                int tmp_int = 0;

                memcpy_bigend(&tmp_int, rxmsg->data, 3);
                __yy_can_data.imu_xyz_acc[1] = tmp_int * 1e-4f;

                tmp_int = 0;
                memcpy_bigend(&tmp_int, rxmsg->data + 3, 3);
                __yy_can_data.imu_xyz_acc[2] = tmp_int * 1e-4f;

                __yy_can_data.mems_status = rxmsg->data[6] + (rxmsg->data[7] << 8);
            }
            break;
            case MSG_XLTB:
            {
                __yy_can_data.txgl_wk_stage = rxmsg->data[0];
                __yy_can_data.txgl_drone_cmd = rxmsg->data[1];
                __yy_can_data.txgl_radar_cmd = rxmsg->data[2];
                __yy_can_data.txgl_selfcheck = rxmsg->data[3];

                /*收到弹射准备指令*/
                if ((__yy_can_data.txgl_drone_cmd & CTL_RTO_CMD_MASK) == CTL_RTO_ACTION)
                {
                    updata_cmd_status(TAX_READY, true);

                    /*准备弹射指令每次上电只响应一次*/
                    if(recv_tof == false)
                    {
                        recv_tof = true;
                        control_mode = CONTROL_GCS;
                        flight_mode_flag = GCS_VEHICLE_LAUNCH;
                    }
                }

                /*收到弹射指令*/
                if ((__yy_can_data.txgl_drone_cmd & CTL_TO_CMD_MASK) == CTL_TO_ACTION)
                {
                    updata_cmd_status(TAX_EXECUTE, true);
                    __mode_state = __STATE_SELF_STABILITY;
                    __time_stamp = micros();
                }

                /*收到顺时针旋转指令*/
                if ((__yy_can_data.txgl_drone_cmd & CTL_ROTATE_CMD_MASK) == CTL_CW_ROTATION)
                {
                    if (pid_m_yaw.enable_cw_rotate == ROTATE_DEFAULT || pid_m_yaw.enable_cw_rotate == ROTATE_FINISH)
                    {
                        if (cw_cmd_count++ >= 50)
                        {
                            cw_cmd_count = 0;
                            pid_m_yaw.enable_cw_rotate = ROTATE_START;
                            pid_m_yaw.rotate_angle = 0;
                        }
                    }
                }
                if (pid_m_yaw.enable_cw_rotate == ROTATE_DEFAULT)
                {
                    updata_cmd_status(CW_EXECUTE_COMPLETE, false);
                }
                else if (pid_m_yaw.enable_cw_rotate == ROTATE_START)
                {
                    updata_cmd_status(CW_EXECUTE_COMPLETE, false);
                    pid_m_yaw.enable_ccw_rotate = ROTATE_DEFAULT;
                }
                else if (pid_m_yaw.enable_cw_rotate == ROTATE_FINISH)
                {
                    updata_cmd_status(CW_EXECUTE_COMPLETE, true);
                }

                /*收到逆时针旋转指令*/
                if ((__yy_can_data.txgl_drone_cmd & CTL_ROTATE_CMD_MASK) == CTL_CCW_ROTATION)
                {
                    if (pid_m_yaw.enable_ccw_rotate == ROTATE_DEFAULT || pid_m_yaw.enable_ccw_rotate == ROTATE_FINISH)
                    {
                        if (ccw_cmd_count++ >= 50)
                        {
                            ccw_cmd_count = 0;
                            pid_m_yaw.enable_ccw_rotate = ROTATE_START;
                            pid_m_yaw.rotate_angle = 0;
                        }
                    }
                }
                if (pid_m_yaw.enable_ccw_rotate == ROTATE_DEFAULT)
                {
                    updata_cmd_status(CCW_EXECUTE_COMPLETE, false);
                }
                else if (pid_m_yaw.enable_ccw_rotate == ROTATE_START)
                {
                    pid_m_yaw.enable_cw_rotate = ROTATE_DEFAULT;
                    updata_cmd_status(CCW_EXECUTE_COMPLETE, false);
                }
                else if (pid_m_yaw.enable_ccw_rotate == ROTATE_FINISH)
                {
                    updata_cmd_status(CCW_EXECUTE_COMPLETE, true);
                }

                /*收到变高指令*/
                uint16_t alt_cmd = 0;
                static uint16_t alt_cmd_last = 0;

                if ((__yy_can_data.txgl_drone_cmd & CTL_ALT_CMD_MASK) == CTL_ALT_ACTION)
                {
                    memcpy_bigend(&alt_cmd, &rxmsg->data[4], 2);

                    if(alt_cmd != alt_cmd_last)
                    {
                        __yy_can_data.txgl_alt_sp = alt_cmd * 0.01f;
                        updata_cmd_status(ALT_SETTING, true);
                        updata_cmd_status(ALT_EXECUTE, false);

                        recv_alt_time = micros();
                        alt_reached = false;;
                    }
                    alt_cmd_last = alt_cmd;
                }

                /*收到变航向指令*/
                uint16_t yaw_cmd = 0;
                static uint16_t yaw_cmd_last = 0;                

                if ((__yy_can_data.txgl_drone_cmd & CTL_YAW_CMD_MASK) == CTL_YAW_ACTION)
                {
                    memcpy_bigend(&yaw_cmd, &rxmsg->data[6], 2);
                    if(yaw_cmd != yaw_cmd_last)
                    {
                        __yy_can_data.txgl_yaw_sp = wrap_360(yaw_cmd * 0.01f);
                        pid_m_yaw.enable_tyaw = true;

                        updata_cmd_status(YAW_SETTING, true);
                        updata_cmd_status(YAW_EXECUTE, false);
                        yaw_reached = false;
                    }
                    yaw_cmd_last = yaw_cmd;
                }

                tx_loop_on = 1;
                last_tx_time = micros();
            }
            break;

            case MSG_GLOBAL_POS:
            {
                struct global_pos_msg
                {
                    int lon; // 东经为正
                    int lat; // 南纬为正
                };

                static struct global_pos_msg p = {0};
                memcpy(&p.lon, rxmsg->data, 4);
                memcpy(&p.lat, rxmsg->data + 4, 4);
                p.lon *= 10; // 转1e-7
                p.lat *= 10;
                __yy_can_data.txgl_lon = p.lon;
                __yy_can_data.txgl_lat = fabsf(p.lat); // 取反成北纬为正
                if ((__yy_can_data.temp_count & 0x02) != 0x02)
                {
                    __yy_can_data.temp_count |= 0x02;
                }
                if (p.lon == 0)
                {
                    __yy_can_data.temp_count |= 0x08;
                }
                else
                {
                    __yy_can_data.temp_count &= ~0x08;
                }
                if (p.lat == 0)
                {
                    __yy_can_data.temp_count |= 0x10;
                }
                else
                {
                    __yy_can_data.temp_count &= ~0x10;
                }
                // 发送给IMU经纬度
                // send_imu_hdw_data(1, &p);
            }

            break;

            case MSG_GLOBAL_VEL:
            {
                struct global_vel_msg
                {
                    int gps_alt_m;      /* 海拔高度m */
                    int16_t gps_vel_dm; /* 单位km/h，需要转换成dm/s */
                    int16_t gps_yaw;    /* GPS航向0~360*/
                    uint8_t fix_status; /* b0b1:00-无定位 01-搜星中 10-定位成功 11-定位失败 */
                    uint8_t gps_num;    /* GPS星数 */
                };

                int16_t temp = 0;

// #define KN2M 1852 / 3600  //海里/时 转 米/秒
#define KM2M 1000 / 3600 // 千米/时 转 米/秒

                static struct global_vel_msg p = {0};
                memcpy_bigend(&p.gps_vel_dm, rxmsg->data, 2); // 卫导速度数据帧为大端在前
                memcpy_bigend(&p.gps_yaw, rxmsg->data + 2, 2);
                memcpy_bigend(&temp, rxmsg->data + 4, 2);
                memcpy(&p.fix_status, rxmsg->data + 6, 1);
                memcpy(&p.gps_num, rxmsg->data + 7, 1);
                p.gps_alt_m = temp;

                __yy_can_data.txgl_enu_vel[0] = p.gps_vel_dm * KM2M * 10; // 记录cm/s
                __yy_can_data.txgl_enu_vel[1] = wrap_180(p.gps_yaw);
                __yy_can_data.txgl_enu_vel[2] = p.gps_alt_m;

                p.gps_vel_dm = __yy_can_data.txgl_enu_vel[0] * 10;  // 转mm/s发给IMU
                p.gps_yaw = __yy_can_data.txgl_enu_vel[1] * 10;     // 转0.1度发给IMU
                p.gps_alt_m = __yy_can_data.txgl_enu_vel[2] * 1000; // 转mm发给IMU

                __yy_can_data.txgl_pos_fix_status = p.fix_status;
                __yy_can_data.txgl_gps_num = p.gps_num;
                if ((__yy_can_data.temp_count & 0x04) != 0x04)
                {
                    __yy_can_data.temp_count |= 0x04;
                }
                // 发送给IMU卫星速度信息
                // send_imu_hdw_data(2, &p);
            }

            break;

            case MSG_POS_BIAS:
            {
                struct pos_bias_msg
                {
                    int16_t e_pos_bias; // 现该成员为互定位延时时间，其他成员无意义
                    int16_t n_pos_bias;
                    int16_t u_pos_bias;
                    uint16_t msl_alt;
                };

                struct pos_bias_msg p = {0};

                memcpy_bigend(&p.e_pos_bias, rxmsg->data, 2);
                memcpy_bigend(&p.n_pos_bias, rxmsg->data + 2, 2);
                memcpy_bigend(&p.u_pos_bias, rxmsg->data + 4, 2);
                memcpy_bigend(&p.msl_alt, rxmsg->data + 6, 2);

                hdw_can_delay = p.e_pos_bias;

                // 更新互定位延时时间并发送给IMU
                // send_imu_hdw_data(3, &p.e_pos_bias);
            }
            break;

            case MSG_HDW:
            {
                struct hdw_msg
                {
                    int16_t e_rpos;
                    int16_t n_rpos;
                    uint16_t u_rpos;
                    uint16_t status;
                };
                static struct hdw_msg p = {0};
                static struct hdw_msg p_last = {0};
                static uint32_t hdw_can_count = 0;

                memcpy_bigend(&p.e_rpos, rxmsg->data, 2);
                memcpy_bigend(&p.n_rpos, rxmsg->data + 2, 2);
                memcpy_bigend(&p.u_rpos, rxmsg->data + 4, 2);
                memcpy_bigend(&p.status, rxmsg->data + 6, 2);

                __yy_can_data.hdw_enu_rpos[0] = p.e_rpos * 0.1f;
                __yy_can_data.hdw_enu_rpos[1] = p.n_rpos * 0.1f;
                __yy_can_data.hdw_enu_rpos[2] = p.u_rpos * 0.1f;
                __yy_can_data.hdw_status = p.status;
                // 相机状态 0-正常 1-异常
                if ((__yy_can_data.temp_count & 0x01) != 0x01)
                {
                    __yy_can_data.temp_count |= 0x01;
                }
                // 发送给IMU互定位偏差，互定位是100HZ，按照10HZ发送
                // if (hdw_can_count++ >= 10)
                {
                    hdw_can_count = 0;
                    send_imu_hdw_data(0, &p);
                }

                p_last = p;
            }
            break;

            case MSG_HDW_DRIFT:
            {
                struct hdw_drift_msg
                {
                    int16_t e_rvel;
                    int16_t n_rvel;
                    int16_t u_rvel;
                    uint16_t reserve;
                };
                struct hdw_drift_msg p = {0};

                memcpy_bigend(&p.e_rvel, rxmsg->data, 2);
                memcpy_bigend(&p.n_rvel, rxmsg->data + 2, 2);
                memcpy_bigend(&p.u_rvel, rxmsg->data + 4, 2);
                memcpy_bigend(&p.reserve, rxmsg->data + 6, 2);

                __yy_can_data.hdw_drift_vel[0] = p.n_rvel * 0.1f;
                __yy_can_data.hdw_drift_vel[1] = p.e_rvel * 0.1f;
                __yy_can_data.hdw_drift_vel[2] = p.u_rvel * 0.1f;
            }
            break;

            default:
                break;
            }
        }
    }

    return ret;
}

static void send_drone_status_msg(void)
{
    #pragma pack(1)
    struct drone_status_msg
    {
        int16_t evel; // 单位0.01kn
        // uint16_t nvel;
        int16_t nvel;
        int16_t uvel;
        uint8_t salt; // 单位0.1m
        uint8_t status;
    };
    #pragma pack()
#define M2KN 3600.0f / 1852.0f

    can_transmit_buf_t msg = {0};

    msg.extend_id = 1;// 扩展ID模式
    msg.remote_frame = 0;    // 数据帧
    msg.id = __yy_id_comb(NODE_WRJ, NODE_TXGL, MSG_DRONE_STATUS);// CAN ID
    msg.dlc = 8; // 数据长度

    struct drone_status_msg p = {0};
    if (ins.insgps_ok == INSGPS)
    {
        // 放大成0.01kn
        p.evel = (pid_m_posx.vel_c) * 0.01f * M2KN * 100;
        p.nvel = (pid_m_posy.vel_c) * 0.01f * M2KN * 100;
    }
    else
    {
        p.evel = 0;
        p.nvel = 0;
    }

    // p.evel = (int16_t)(__yy_can_data.txgl_alt_sp * 100);
    // p.nvel = (uint16_t)(__yy_can_data.txgl_yaw_sp * 100);
    p.evel = (int16_t)(pid_m_pitch.angle_c * 100.0f);
    p.nvel = (int16_t)(pid_m_roll.angle_c * 100.0f);

    
    // 放大成0.01kn  raw_sensor
    //p.uvel = (pid_m_alt.vel_c) * 0.01f * M2KN * 100;  //气压计值 原始值  -80000 pa
    p.uvel = (int16_t)(raw_gps._auxiliary_gps_lon - 80000) ;  //气压计值 原始值  -80000 pa
    if (pid_m_alt.loc_c > 0)
    {
        p.salt = fabsf(pid_m_alt.loc_c * 0.1f);
    }
    else
    {
        p.salt = 0;
    }

    p.status = status_broadcast;

    memcpy_smallend(msg.data, &p.evel, 2);
    memcpy_smallend(msg.data + 2, &p.nvel, 2);
    memcpy_smallend(msg.data + 4, &p.uvel, 2);
    msg.data[6] = p.salt;
    msg.data[7] = p.status;

    canSendMsg(&msg);
}

static void send_drone_euler_msg(void)
{
    #pragma pack(1)
    struct drone_euler_msg
    {
        uint16_t yaw;     // 单位0.01deg
        int16_t pitch;   // 单位0.01deg
        int16_t roll;    // 单位0.01deg
        int16_t reserved; 
    };
    #pragma pack()

    can_transmit_buf_t msg = {0};
    // msg.StdId = 0;
    msg.extend_id = 1;// 扩展ID模式
    msg.remote_frame = 0;    // 数据帧
    msg.id =  __yy_id_comb(NODE_WRJ, NODE_TXGL, MSG_DRONE_STATUS) + 0x10; // CAN ID
    msg.dlc = 8; // 数据长度

    struct drone_euler_msg p = {0};
    p.yaw = (uint16_t)(pid_m_yaw.angle_c * 100.0f);
    p.pitch = (int)(pid_m_pitch.angle_c * 100.0f);
    p.roll = (int)(pid_m_roll.angle_c * 100.0f);

    memcpy_bigend(msg.data, &p.yaw, 2);
    memcpy_bigend(msg.data + 2, &p.pitch, 2);
    memcpy_bigend(msg.data + 4, &p.roll, 2);
    memcpy_bigend(msg.data + 6, &p.reserved, 2);

    canSendMsg(&msg);
}

bool can_connect = false;
uint32_t send_euler_t = 0;
void YY_tx_loop(void)
{
    if (tx_loop_on)
    {
        if (micros() - last_tx_time >= 2 * 1000)
        {
            tx_loop_on = 0;

            if ((__yy_can_data.txgl_selfcheck & 0x01) == 0x01)
            {
                send_drone_selfcheck_msg();
            }
            else
            {
                send_drone_status_msg();
            }
        }

        can_connect = true;
    }
    else if(can_connect && micros() - send_euler_t >= 50 * 1000)
    {
        send_euler_t = micros();
        send_drone_euler_msg();
    }
}