#include "payload.h"
#include "control_rate.h"
#include "dcm.h"
#include "helpler_funtions.h"
#include "quaternion.h"
#include "soft_imu.h"

/**
 * @brief 载荷控制指令
 *
 * @param ctl_ops_id 控制 id
 * @param arg 指令参数
 * @param arg_len 指令参数字节数
 * @param payload 载荷对象
 */
void payload_ctl_received(uint8_t ctl_ops_id, const void *arg, uint32_t arg_len,
                          struct payload_diaocang *payload)
{
    switch (ctl_ops_id)
    {
    case PAYLOAD_DIAOCANG_OPS_BACK_MID:
        if (payload->_ops._return_to_mid)
        {
            payload->_ops._return_to_mid(payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_SET_PITCH:
        if (payload->_ops._set_pitch)
        {
            const int16_t *param = (const int16_t *)arg;
            payload->_ops._set_pitch(param[0] * 0.1f, payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_SET_YAW:
        if (payload->_ops._set_yaw)
        {
            const int16_t *param = (const int16_t *)arg;
            payload->_ops._set_yaw(param[0] * 0.1f, payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_LASER_MEASURE:
        if (payload->_ops._laser_dist)
        {
            payload->_ops._laser_dist(payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_TAKE_PHOTO:
        if (payload->_ops._take_photo)
        {
            payload->_ops._take_photo(payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_TAKE_VEDIO:
        if (payload->_ops._take_vedio)
        {
            const uint16_t *param = (const uint16_t *)arg;
            payload->_ops._take_vedio(param[0], payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_MOVE:
        if (payload->_ops._move)
        {
            const uint16_t *param = (const uint16_t *)arg;
            payload->_ops._move(param[0], param[1] * 0.001f,
                                payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_TOUCH_MOVE:
        if (payload->_ops._touch_move)
        {
            const uint16_t *param = (const uint16_t *)arg;
            payload->_ops._touch_move(param[0] * 0.001f, param[1] * 0.001f,
                                      payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_ZOOM:
        if (payload->_ops._zoom)
        {
            const uint16_t *param = (const uint16_t *)arg;

            payload->_ops._zoom(param[0], param[1] * 0.001f,
                                payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_TAR_TRACE:
        if (payload->_ops._trace)
        {
            const uint16_t *param = (const uint16_t *)arg;

            payload->_ops._trace(param[5], param[0] * 0.001f, param[1] * 0.001f,
                                 param[2] * 0.001f, param[3] * 0.001f,
                                 payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_FOCUS:
        if (payload->_ops._focus)
        {
            const uint32_t *param = (const uint32_t *)arg;
            payload->_ops._focus(param[0], param[1], param[2],
                                 payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_OSD_SET:
        break;

    case PAYLOAD_DIAOCANG_OPS_SENSOR_SWITCH:
        if (payload->_ops._sensor_switch)
        {
            const uint32_t *param = (const uint32_t *)arg;
            payload->_ops._sensor_switch(payload->_ops._send_data, param[0]);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_FOLLOW:
        if (payload->_ops._follow_mode)
        {
            payload->_ops._follow_mode(payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_LOOK_DOWN:
        if (payload->_ops._look_downward)
        {
            payload->_ops._look_downward(payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_LOCK:
        if (payload->_ops._lock_mode)
        {
            payload->_ops._lock_mode(payload->_ops._send_data);
        }
        break;

    case PAYLOAD_DIAOCANG_OPS_TTRANS:
        if (payload->_ops._send_data)
        {
            payload->_ops._send_data(arg, arg_len);
        }
        break;

    default:
        break;
    }
}

/**
 * @brief 载荷发送轮询
 *
 * @param tiem_us 当前本地时间 us
 * @param payload 载荷对象
 */
void payload_tx_poll(uint32_t tiem_us, struct payload_diaocang *payload)
{
    if (payload->_ops._tx_poll)
    {
        payload->_ops._tx_poll(tiem_us, &payload->_last_tx_time,
                               payload->_ops._send_data);
    }
}

/**
 * @brief 载荷接收回调
 *
 * @param ch 接收到的字节数据
 * @param payload 载荷对象
 * @return int 暂无意义
 */
int payload_rx_cbk(uint8_t ch, struct payload_diaocang *payload)
{
    int ret = 0;
    if (payload->_ops._rx_cbk)
    {
        ret = payload->_ops._rx_cbk(ch, payload);
    }

    if (ret > 0)
    {
        payload->_link_lost_count = 0;
        payload->_link_status = COMP_NORMAL;
    }

    return ret;
}

/**
 * @brief 载荷链接状态轮询
 *
 * @param time_now_us
 * @param payload
 */
void payload_check_linkstatu_poll(uint32_t time_now_us,
                                  struct payload_diaocang *payload)
{
    const float dt = 0.2f;
    const float link_lost_time_period = 3.0f;

    if (time_now_us - payload->_link_check_time > dt * 1000000)
    {
        if (payload->_link_status == COMP_NORMAL)
        {
            payload->_link_check_time = time_now_us;

            payload->_link_lost_count++;
            if (payload->_link_lost_count * dt > link_lost_time_period)
            {
                payload->_link_lost_count = 0;
                payload->_link_status = COMP_LOST;
            }
        }
    }
}

/**
 * @brief 光电吊舱类载荷目标定位
 *
 * @param payload 载荷对象
 * @param gimbal_euler_frame 载荷角度类型 0-相对载具平台 1-相对导航水平平台
 */
static int _gimbal_pod_payload_target_locate(struct payload_diaocang *payload,
                                             uint8_t gimbal_euler_frame)
{
    if (payload->_link_status == COMP_NORMAL && payload->_status.trace_flag &&
        ins.insgps_ok == INSGPS)
    {
        // camera->ENU
        float qCN[4] = {1, 0, 0, 0};
        if (gimbal_euler_frame == 0)
        {
            // body->ENU
            float qbN[4];
            for (int i = 0; i < 4; ++i)
            {
                qbN[i] = ins.Q[i];
            }

            // camera->body
            float qCb[4] = {1, 0, 0, 0};
            float qtmp1[4], qtmp2[4], qtmp3[4], qtmp4[4];
            // gimbal frame sequence is yaw->roll->pitch, which is different from
            // body euler angle sequence
            Quaternion_ByEuler(deg_to_rad(-payload->_status.yaw), 0, 0, qtmp1);
            Quaternion_ByEuler(0, 0, deg_to_rad(payload->_status.roll), qtmp2);
            Quaternion_ByEuler(0, deg_to_rad(payload->_status.pitch), 0, qtmp3);
            Quaternion_Multiplication(qtmp1, qtmp2, qtmp4);
            Quaternion_Normalize(qtmp4);
            Quaternion_Multiplication(qtmp4, qtmp3, qCb);
            Quaternion_Normalize(qCb);

            // camera->ENU
            Quaternion_Multiplication(qbN, qCb, qCN);
            Quaternion_Normalize(qCN);
        }
        else if (gimbal_euler_frame == 1)
        {
            Quaternion_ByEuler(
                deg_to_rad(-payload->_status.yaw - pid_m_yaw.angle_c),
                deg_to_rad(payload->_status.pitch),
                deg_to_rad(payload->_status.roll), qCN);
        }

        // calculate camera y axis coordinate in NEU frame
        float v[3] = {0, 1, 0};
        Quaternion_Conj(qCN, v, payload->_status.y_axis);

        // 根据载荷光轴的向量和载荷的相对高度，计算载荷的光轴和地面交点向量在地面的投影
        float k1 = 0.0f;
        float payloadAlt = pid_m_alt.loc_c;
        if (fabsf(payload->_status.y_axis[2]) < 0.001f)
        {
            k1 = payloadAlt / 0.001f;
        }
        else
        {
            k1 = payloadAlt / fabsf(payload->_status.y_axis[2]);
        }

        payload->_diste_filter_buffer[payload->_filter_in_index] =
            payload->_status.y_axis[0] * k1;
        payload->_distn_filter_buffer[payload->_filter_in_index] =
            payload->_status.y_axis[1] * k1;
        payload->_filter_in_index++;
        if (payload->_filter_in_index >= PAYLOAD_SZHT_FILTER_BUFF_LEN)
            payload->_filter_in_index = 0;

        float dist_e = 0;
        float dist_n = 0;
        for (int i = 0; i < PAYLOAD_SZHT_FILTER_BUFF_LEN; ++i)
        {
            dist_e +=
                payload->_diste_filter_buffer[i] / PAYLOAD_SZHT_FILTER_BUFF_LEN;
            dist_n +=
                payload->_distn_filter_buffer[i] / PAYLOAD_SZHT_FILTER_BUFF_LEN;
        }

        payload->_status.tar_lon =
            pid_m_posx.loc_c + dist_e / ins.delta_lon_to_cm_ratio;
        payload->_status.tar_lat =
            pid_m_posy.loc_c + dist_n / ins.delta_lat_to_cm_ratio;
        payload->_status.tar_alt = 0;

        payload->_status.tar_lon_loc =
            pid_m_posx.loc_c +
            payload->_status.y_axis[0] * payload->_status.laser_dist_m * 100 / ins.delta_lon_to_cm_ratio;
        payload->_status.tar_lat_loc =
            pid_m_posy.loc_c +
            payload->_status.y_axis[1] * payload->_status.laser_dist_m * 100 / ins.delta_lat_to_cm_ratio;
        payload->_status.tar_alt_loc = 0;

        return 0;
    }

    return -1;
}

/**
 * @brief 载荷目标定位
 *
 * @param payload
 */
void payload_targte_locate(struct payload_diaocang *payload, uint8_t gimbal_euler_frame)
{
    if (payload->_type == PAYLOAD_TYPE_GIMBAL_POD)
    {
        _gimbal_pod_payload_target_locate(payload, gimbal_euler_frame);
    }
}