#include "rc_sbus.h"

#define SBUS_HEAD 0x0F
#define SBUS_TAIL0 0x00
#define SBUS_TAIL1 0x04
#define SBUS_TAIL2 0x14
#define SBUS_TAIL3 0x24
#define SBUS_TAIL4 0x34

/** sbus 协议接收解析阶段 */
enum
{
    SBUS_PARSE_HEAD = 0,
    SBUS_PARSE_DATA,
    SBUS_PARSE_TAIL
};

void RcSbus_StructInit(RC_SBUS_Data *prc_sbus)
{
    prc_sbus->rc.signal_health = RC_SIGNAL_HEALTH;
    prc_sbus->rc.link_status = COMP_NOEXIST;
    for (uint8_t ch = 0; ch < RC_INPUT_CH_NUM; ch++)
    {
        prc_sbus->rc.ch_value[ch] = 1500;
        prc_sbus->rc.raw_ch_value[ch] = 1500;

        prc_sbus->rc.calib_offset[ch] = 1500;
        prc_sbus->rc.calib_factor_up[ch] = 1000;
        prc_sbus->rc.calib_factor_down[ch] = 1000;
    }

    prc_sbus->_bad_count_s = 0;
    prc_sbus->_fail_count_s = 0;
    prc_sbus->_health_count_s = 0;
    prc_sbus->_link_lost_time_us = 0;

    prc_sbus->_sbus_signal_status = SBUS_PARSE_HEAD;
}

/**
 * @brief sbus 协议接收
 *
 * @param rc_sbus
 * @param data
 * @return true
 * @return false
 */
bool rc_sbus_protocal_parse_char(RC_SBUS_Data *rc_sbus, uint8_t data)
{
    bool ret_val = false;

    switch (rc_sbus->_raw_sbus_parse_stage)
    {
    /* sbus 帧头 */
    case SBUS_PARSE_HEAD:
        if (SBUS_HEAD == data)
        {
            rc_sbus->_raw_sbus_head = data;
            rc_sbus->_raw_sbus_parse_stage = SBUS_PARSE_DATA;
            rc_sbus->_raw_sbus_data_rx_index = 0;
        }
        break;
    /* sbus 数据 */
    case SBUS_PARSE_DATA:
        rc_sbus->_raw_sbus_data[rc_sbus->_raw_sbus_data_rx_index++] = data;
        if (rc_sbus->_raw_sbus_data_rx_index >= _SBUS_DATA_NUM)
        {
            rc_sbus->_raw_sbus_data_rx_index = 0;
            rc_sbus->_raw_sbus_parse_stage = SBUS_PARSE_TAIL;
        }
        break;
    /* sbus 帧尾 */
    case SBUS_PARSE_TAIL:
        switch (data)
        {
        case SBUS_TAIL0:
        case SBUS_TAIL1:
        case SBUS_TAIL2:
        case SBUS_TAIL3:
        case SBUS_TAIL4:
            ret_val = true;
            break;

        default:
            break;
        }
        rc_sbus->_raw_sbus_tail = data;
        rc_sbus->_raw_sbus_parse_stage = SBUS_PARSE_HEAD;
        break;

    default:
        rc_sbus->_raw_sbus_parse_stage = SBUS_PARSE_HEAD;
        break;
    }

    return ret_val;
}

/**
 * @brief sbus 信号转 pwm
 */
static void sbus2pwm(uint16_t *pwm, uint16_t *sbus_ch)
{
    uint8_t i = 0;

    for (i = 0; i < 14; i++)
    {
        pwm[i] = (sbus_ch[i] - 1024) * 420 / 672 + 1500;
    }
}

/**
 * @brief sbus 协议解析
 *
 * @param rc_sbus
 */
void rc_sbus_protocal_decode(RC_SBUS_Data *rc_sbus)
{
    uint16_t channels[18] = {1023, 1023, 1023, 1023, 1023, 1023,
                             1023, 1023, 1023, 1023, 1023, 1023,
                             1023, 1023, 1023, 1023, 0,    0};

    const uint8_t *sbus_data = rc_sbus->_raw_sbus_data;

    channels[0] = ((sbus_data[0] | sbus_data[1] << 8) & 0x07FF);
    channels[1] = ((sbus_data[1] >> 3 | sbus_data[2] << 5) & 0x07FF);

    channels[2] =
        ((sbus_data[2] >> 6 | sbus_data[3] << 2 | sbus_data[4] << 10) & 0x07FF);
    channels[3] = ((sbus_data[4] >> 1 | sbus_data[5] << 7) & 0x07FF);
    channels[4] = ((sbus_data[5] >> 4 | sbus_data[6] << 4) & 0x07FF);
    channels[5] =
        ((sbus_data[6] >> 7 | sbus_data[7] << 1 | sbus_data[8] << 9) & 0x07FF);
    channels[6] = ((sbus_data[8] >> 2 | sbus_data[9] << 6) & 0x07FF);
    channels[7] = ((sbus_data[9] >> 5 | sbus_data[10] << 3) & 0x07FF);

    // & the other 8 + 2 channels if you need them
    channels[8] = ((sbus_data[11] | sbus_data[12] << 8) & 0x07FF);
    channels[9] = ((sbus_data[12] >> 3 | sbus_data[13] << 5) & 0x07FF);
    channels[10] =
        ((sbus_data[13] >> 6 | sbus_data[14] << 2 | sbus_data[15] << 10) &
         0x07FF);
    channels[11] = ((sbus_data[15] >> 1 | sbus_data[16] << 7) & 0x07FF);
    channels[12] = ((sbus_data[16] >> 4 | sbus_data[17] << 4) & 0x07FF);
    channels[13] =
        ((sbus_data[17] >> 7 | sbus_data[18] << 1 | sbus_data[19] << 9) &
         0x07FF);
    channels[14] = ((sbus_data[19] >> 2 | sbus_data[20] << 6) & 0x07FF);
    channels[15] = ((sbus_data[20] >> 5 | sbus_data[21] << 3) & 0x07FF);

    // DigiChannel 1
    if (sbus_data[22] & (1 << 0))
    {
        channels[16] = 1;
    }
    else
    {
        channels[16] = 0;
    }
    // DigiChannel 2
    if (sbus_data[22] & (1 << 1))
    {
        channels[17] = 1;
    }
    else
    {
        channels[17] = 0;
    }

    /* 信号失控异常标志位判断 */
    rc_sbus->_sbus_signal_status = SBUS_SIGNAL_OK;
    /* 信号丢失 */
    if (sbus_data[22] & (1 << 2))
    {
        rc_sbus->_sbus_signal_status = SBUS_SIGNAL_LOST;
    }
    /* 失控保护激活 */
    if (sbus_data[22] & (1 << 3))
    {
        rc_sbus->_sbus_signal_status = SBUS_SIGNAL_FAILSAFE;
    }

    sbus2pwm(rc_sbus->rc.raw_ch_value, channels);
}

/**
 * @brief 计算遥控器校准后的值
 *
 * @param rc_sbus
 */
void rc_sbus_calib_value(RC_SBUS_Data *rc_sbus)
{
    if (COMP_NORMAL == rc_sbus->rc.link_status)
    {
        /* 校准前 4 个通道到 1000~2000 范围 */
        for (uint8_t ch = 0; ch < _SBUS_CALIB_CH_NUM; ch++)
        {
            float cal_factor = 1.0f;
            if (rc_sbus->rc.raw_ch_value[ch] >= rc_sbus->rc.calib_offset[ch])
            {
                cal_factor = rc_sbus->rc.calib_factor_up[ch] / 1000.0f;
            }
            else
            {
                cal_factor = rc_sbus->rc.calib_factor_down[ch] / 1000.0f;
            }

            rc_sbus->rc.ch_value[ch] = 1500 + (rc_sbus->rc.raw_ch_value[ch] -
                                               rc_sbus->rc.calib_offset[ch]) *
                                                  cal_factor;
        }

        /* 后面的通道则用原始值 */
        for (uint8_t ch = _SBUS_CALIB_CH_NUM; ch < 14; ch++)
        {
            rc_sbus->rc.ch_value[ch] = rc_sbus->rc.raw_ch_value[ch];
        }
    }
}

/**
 * @brief 检查 rc_sbus 信号质量
 *
 * @param rc_sbus
 * @param dt_s
 */
void rc_sbus_signal_health_check(RC_SBUS_Data *rc_sbus, float dt_s)
{
    /* 检查信号质量 */
    bool rc_value_range_is_legal = true;
    /* 规定前 7 个通道必须在 920 ~ 2080 的范围内, 否则视为超范围 */
    for (uint8_t ch = 0; ch < 7; ch++)
    {
        if (rc_sbus->rc.raw_ch_value[ch] <= 920 &&
            rc_sbus->rc.raw_ch_value[ch] >= 2080)
        {
            rc_value_range_is_legal = false;
            break;
        }
    }

    if (rc_value_range_is_legal)
    {
        switch (rc_sbus->_sbus_signal_status)
        {
        case SBUS_SIGNAL_OK:
            rc_sbus->_health_count_s += dt_s;
            if (rc_sbus->_health_count_s > 0.5f)
            {
                rc_sbus->_health_count_s = 0.5f;
                rc_sbus->rc.signal_health = RC_SIGNAL_HEALTH;
            }
            rc_sbus->_fail_count_s = 0.0f;
            rc_sbus->_bad_count_s = 0.0f;
            break;

        case SBUS_SIGNAL_FAILSAFE:
        case SBUS_SIGNAL_LOST:
            rc_sbus->_fail_count_s += dt_s;
            if (rc_sbus->_fail_count_s > 0.5f)
            {
                rc_sbus->_fail_count_s = 0.5f;
                rc_sbus->rc.signal_health = RC_SIGNAL_FAIL;
            }
            rc_sbus->_health_count_s = 0.0f;
            rc_sbus->_bad_count_s = 0.0f;
            break;

        default:
            break;
        }
    }
    else
    {
        rc_sbus->_bad_count_s += dt_s;
        if (rc_sbus->_bad_count_s > 0.5f)
        {
            rc_sbus->_bad_count_s = 0.5f;
            rc_sbus->rc.signal_health = RC_SIGNAL_BAD;
        }
        rc_sbus->_health_count_s = 0;
        rc_sbus->_fail_count_s = 0;
    }
}