#include "protocolExpose.h"
#include "string.h"

#include "crc8.h"

#if PROTOCOLEX_USING_DEVICE == PROTOCOLEX_SUPPORT_DEVICE_A12F4_3AXIS_HOLDER

#include "CSL.h"

#endif

// Protocol process status machine
typedef enum
{
    PROTOCOLEX_SM_T1 = 1, // 检测帧头
    PROTOCOLEX_SM_T2,     // 接收数据
    PROTOCOLEX_SM_T3,     // 检测帧尾
    PROTOCOLEX_SM_T4,     // CRC校验
    PROTOCOLEX_SM_T5,     // 数据解析
    PROTOCOLEX_SM_T6
} PROTOCOLEX_SM;

#if PROTOCOLEX_PARSE_ENABLE == PROTOCOLEX_SETTING_TRUE

//____________ProtocolEX Parse___________

static uint32_t sframeReadTotal = 0;            // 单帧读取长度
static uint32_t processSM_P = PROTOCOLEX_SM_T1; // status machine position
static uint8_t processBuff[PROTOCOLEX_LEN + 2];
uint32_t frameCnt = 0;

//____________ProtocolEX Parse C1___________
#if PROTOCOLEX_C1_ENABLE == PROTOCOLEX_SETTING_TRUE
static int32_t sframeReadTotal_C1 = 0;             // 单帧读取长度
static uint32_t processSM_P_C1 = PROTOCOLEX_SM_T1; // status machine position
static uint8_t processBuff_C1[PROTOCOLEX_LEN + 2];
uint32_t frameCnt_C1 = 0;
#endif
//_________________________________Ring Buff Setting___________________________

//____________ProtocolEX Parse___________

RingBuf readBuff;
RingBuf_Shot readBuff_shot;
static uint8_t _read_buff[PROTOCOLEX_PROCESS_BUFF];

//____________ProtocolEX Parse C1___________
#if PROTOCOLEX_C1_ENABLE == PROTOCOLEX_SETTING_TRUE
RingBuf readBuff_C1;
RingBuf_Shot readBuff_shot_C1;
static uint8_t _read_buff_C1[PROTOCOLEX_PROCESS_BUFF];
#endif

static inline uint8_t RingBuff_RD(RingBuf *Ringbuf)
{
    uint8_t temp;
    temp = Ringbuf->pbuf[Ringbuf->Rd_Indx & Ringbuf->Mask];
    Ringbuf->Rd_Indx++;
    return temp;
}

static inline uint8_t RingBuff_PreRDJump(RingBuf *Ringbuf, uint32_t jumpP)
{
    uint8_t temp;
    temp = Ringbuf->pbuf[(Ringbuf->Rd_Indx + jumpP) & Ringbuf->Mask];
    return temp;
}

static inline uint8_t RingBuff_PreRD(RingBuf *Ringbuf)
{
    uint8_t temp;
    temp = Ringbuf->pbuf[Ringbuf->Rd_Indx & Ringbuf->Mask];
    return temp;
}

static inline void RingBuff_FakeRD(RingBuf *Ringbuf)
{
    Ringbuf->Rd_Indx++;
}

static inline void RingBuff_WD(RingBuf *Ringbuf, uint8_t DataIn)
{
    Ringbuf->pbuf[Ringbuf->Wd_Indx & Ringbuf->Mask] = DataIn;
    Ringbuf->Wd_Indx++;
}

static inline uint16_t RingBuff_Cnt(RingBuf *Ringbuf)
{
    return (Ringbuf->Wd_Indx - Ringbuf->Rd_Indx) & Ringbuf->Mask;
}

static inline void RingBuffClear(RingBuf *Ringbuf)
{
    Ringbuf->Rd_Indx = Ringbuf->Wd_Indx;
}

/**
 * @brief RingBuff 保存快照 ，保存内部索引位置
 * @param RingBuf
 */
static inline RingBuf_Shot RingBuff_GetShot(RingBuf *RingBuf)
{
    RingBuf_Shot shot;
    shot.Rd_Indx = RingBuf->Rd_Indx;
    shot.Wd_Indx = RingBuf->Wd_Indx;
    return shot;
}

/**
 * @brief RingBuff 恢复快照 ，恢复内部索引位置
 * @param RingBuf
 */
static inline RingBuf_Shot RingBuff_RecShot(RingBuf *RingBuf, RingBuf_Shot shot)
{
    RingBuf->Rd_Indx = shot.Rd_Indx;
    RingBuf->Wd_Indx = shot.Wd_Indx;
    return shot;
}

static inline int RingBuff_RD_MultBytes(RingBuf *Ringbuf, uint8_t *readbuff, uint32_t bufflen, uint32_t readlen)
{
    uint32_t vauleLen, i;
    vauleLen = RingBuff_Cnt(Ringbuf); // 多余�???????
    if (RingBuff_Cnt(Ringbuf) < readlen)
        readlen = vauleLen;
    if (bufflen < readlen)
        readlen = bufflen;
    for (i = 0; i < readlen; i++)
    {
        readbuff[i] = RingBuff_RD(Ringbuf);
    }
    return readlen;
}

static inline int RingBuff_WD_MultBytes(RingBuf *Ringbuf, uint8_t *data, uint32_t datalen)
{
    uint32_t vauleLen, i;
    if ((PROTOCOLEX_PROCESS_BUFF - RingBuff_Cnt(Ringbuf)) < datalen)
        return -1; // Buff Full
    for (i = 0; i < datalen; i++)
    {
        RingBuff_WD(Ringbuf, data[i]);
    }
    return datalen;
}

//_________________________________ProtocolEX API Define___________________________

void protocolEX_frame_parse_INIT(void)
{
    readBuff.Wd_Indx = 0;
    readBuff.Rd_Indx = 0;
    readBuff.Mask = PROTOCOLEX_PROCESS_BUFF - 1;
    readBuff.pbuf = &_read_buff[0];

    readBuff_C1.Wd_Indx = 0;
    readBuff_C1.Rd_Indx = 0;
    readBuff_C1.Mask = PROTOCOLEX_PROCESS_BUFF - 1;
    readBuff_C1.pbuf = &_read_buff_C1[0];
}

/**
 * @brief 协议帧处理函数(只处理一帧)，将缓存中的数据进行解析存放并输出结果
 * @param buff 需要处理的数据头指针
 * @param buff_len 需要处理的数据长度
 * @return ( >0 )处理成功：(PROTOCOLEX_F_TYPE)返回处理的数据帧类型；
 *         ( <0 )处理失败：PROTOCOLEX_PARSE_STATUS
 */
#if PROTOCOLEX_USING_DEVICE == PROTOCOLEX_SUPPORT_DEVICE_A12F4_3AXIS_HOLDER
void protocolEX_frame_parse(uint8_t *buff, const uint32_t buff_len)
#else
ProEX_Parse_Result protocolEX_frame_parse(uint8_t *buff, const uint32_t buff_len)
#endif
#pragma region
{

    sizeof(PayDataEX_T2);

    uint8_t crcdata = 0;
    uint32_t readP = 0;
    uint32_t i = 0;
    uint32_t keepProcess = true; // 继续处理标志
    ProEX_Data *payloadP;

#if PROTOCOLEX_USING_DEVICE == PROTOCOLEX_SUPPORT_DEVICE_A12F4_3AXIS_HOLDER
    CSL_GenMCB_s mciData = {0};
#endif

    while ((readP < buff_len) && (keepProcess)) // 读取完毕
    {
    FP_DIRECT_NEXT_LOOP:     // 不检查重启循环
        switch (processSM_P) // 状态检查
        {
        case PROTOCOLEX_SM_T1:
            while (RingBuff_Cnt(&readBuff))
            {                                                               // 环形缓冲区不为空
                if (RingBuff_PreRD(&readBuff) == PROTOCOLEX_CONTENT_HEAD_0) // 预读缓冲区数据
                {                                                           // 环形缓冲区内检测到帧头
                    processSM_P = PROTOCOLEX_SM_T2;                         // 提升状态,重启循环
                    sframeReadTotal = RingBuff_Cnt(&readBuff);
                    readBuff_shot = RingBuff_GetShot(&readBuff); // 获取RingBuff读取起始位置,用于帧尾检测失败后的再遍历
                    goto FP_DIRECT_NEXT_LOOP;
                }
                RingBuff_FakeRD(&readBuff); // 未检测到帧头,修改缓冲区读取索引
            }
            // 环形缓冲区未检测到帧头,从剩余的buff中检索帧头
            for (; readP < buff_len; readP++)
            {
                if (buff[readP] == PROTOCOLEX_CONTENT_HEAD_0) // 检测到帧头
                {
                    processSM_P = PROTOCOLEX_SM_T2;              // 提升状态重启循环
                    readBuff_shot = RingBuff_GetShot(&readBuff); // 获取RingBuff读取起始位置,用于帧尾检测失败后的再遍历
                    goto FP_DIRECT_NEXT_LOOP;
                }
            }
            break;
        case PROTOCOLEX_SM_T2:
            if ((buff_len - readP) >= (PROTOCOLEX_LEN - sframeReadTotal)) // 传入数据够长，可以直接进行数据解析
            {
                readP += RingBuff_WD_MultBytes(&readBuff, &(buff[readP]), PROTOCOLEX_LEN - sframeReadTotal);
                processSM_P = PROTOCOLEX_SM_T3;
                sframeReadTotal = 0;
                goto FP_DIRECT_NEXT_LOOP;
            }
            else
            {                                                                                          // 传入数据不够一帧,先存下来
                sframeReadTotal += RingBuff_WD_MultBytes(&readBuff, &(buff[readP]), buff_len - readP); // read The data To the Buff
                keepProcess = false;                                                                   // data read complete exit!
            }
            break;
        case PROTOCOLEX_SM_T3:
            // 将环形缓冲区的数据传送到处理缓冲区
            // RingBuff_RD_MultBytes(&readBuff, &processBuff, PROTOCOLEX_LEN + 2, PROTOCOLEX_LEN); // Read Data
            // if (  processBuff[PROTOCOLEX_P_END] == PROTOCOLEX_CONTENT_END)    //帧尾检测
            if (RingBuff_PreRDJump(&readBuff, PROTOCOLEX_LEN - 1) == PROTOCOLEX_CONTENT_END) // 帧尾检测
            {
                RingBuff_RD_MultBytes(&readBuff, &processBuff[0], PROTOCOLEX_LEN + 2, PROTOCOLEX_LEN); // 将环形缓冲区的数据传送到处理缓冲区
                processSM_P = PROTOCOLEX_SM_T4;
                goto FP_DIRECT_NEXT_LOOP;
            }
            else
            { // 寄,收到了孤立的帧头
                processSM_P = PROTOCOLEX_SM_T1;
                readBuff.Rd_Indx = readBuff_shot.Rd_Indx + 1;
                sframeReadTotal = 0;
            }
            break;
        case PROTOCOLEX_SM_T4:
            // 开始校验数据
            crcdata = processBuff[PROTOCOLEX_P_VERIFY];
            processBuff[PROTOCOLEX_P_VERIFY] = 0x00;
            if (crcdata == crc_8(&processBuff[0], PROTOCOLEX_LEN))
            {                                   // 校验通过
                processSM_P = PROTOCOLEX_SM_T5; // CRC Complete
                goto FP_DIRECT_NEXT_LOOP;
            }
            else
            {                                   // 校验失败,直接丢弃数据
                processSM_P = PROTOCOLEX_SM_T1; // CRC Fail
            }
            break;
        case PROTOCOLEX_SM_T5:
            /* abababaaba */
            processSM_P = PROTOCOLEX_SM_T1; // CRC Success
            frameCnt++;
#if PROTOCOLEX_USING_DEVICE == PROTOCOLEX_SUPPORT_DEVICE_A12F4_3AXIS_HOLDER
            switch (processBuff[PROTOCOLEX_P_ROUTE])
            {
            case PROTOCOLEX_F_TYPE_DATA_0X11:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0X11;
                break;
            case PROTOCOLEX_F_TYPE_DATA_0X12:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0X12;
                break;
            case PROTOCOLEX_F_TYPE_DATA_0X13:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0X13;
                break;
            case PROTOCOLEX_F_TYPE_DATA_0X51:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0X51;
                break;
            case PROTOCOLEX_F_TYPE_DATA_0X52:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0X52;
                break;
            case PROTOCOLEX_F_TYPE_DATA_0XAA:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0XAA;
                break;
            default:
                goto RC_FRAME_GIVE_UP; // 无对应RC控制指令，丢弃该结果
            }
            mciData.mciType = CSL_MCITYPE_RC_FRMAEGET;
            mciData.mciData.RC_data.rcInfo.error = PROTOCOLEX_P_STATUS_OK;
            // payloadP = processBuff[PROTOCOLEX_P_DATA];
            mciData.mciData.RC_data.payloadData = *(ProEX_Data *)&processBuff[PROTOCOLEX_P_DATA];
            if (csl_sign_RTOSForISRInitDown == true)
            {
                xQueueSendFromISR(mgs_1ToOutQueue, &mciData, pdTRUE); // 该解析函数是运行在中断函数中的，所以需要使用对应的中断版本
            }
        RC_FRAME_GIVE_UP:
#endif
            break;
        }
    }
}
#pragma endregion

#if PROTOCOLEX_C1_ENABLE == PROTOCOLEX_SETTING_TRUE
/**
 * @brief 协议帧处理函数(只处理一帧)，将缓存中的数据进行解析存放并输出结果
 * @param buff 需要处理的数据头指针
 * @param buff_len 需要处理的数据长度
 * @return ( >0 )处理成功：(PROTOCOLEX_F_TYPE)返回处理的数据帧类型；
 *         ( <0 )处理失败：PROTOCOLEX_PARSE_STATUS
 */
#if PROTOCOLEX_USING_DEVICE == PROTOCOLEX_SUPPORT_DEVICE_A12F4_3AXIS_HOLDER
void protocolEX_frame_parse_C1(uint8_t *buff, const uint32_t buff_len)
#else
ProEX_Parse_Result protocolEX_frame_parse_C1(uint8_t *buff, const uint32_t buff_len)
#endif
#pragma region
{

    sizeof(PayDataEX_T2);

    uint8_t crcdata = 0;
    uint32_t readP = 0;
    uint32_t i = 0;
    uint32_t keepProcess = true; // 继续处理标志
    ProEX_Data *payloadP;

#if PROTOCOLEX_USING_DEVICE == PROTOCOLEX_SUPPORT_DEVICE_A12F4_3AXIS_HOLDER
    CSL_GenMCB_s mciData = {0};
#endif

    while ((readP < buff_len) && (keepProcess)) // 读取完毕
    {
    FP_DIRECT_NEXT_LOOP_C1:     // 不检查重启循环
        switch (processSM_P_C1) // 状态检查
        {
        case PROTOCOLEX_SM_T1:
            while (RingBuff_Cnt(&readBuff_C1))
            {                                                                  // 环形缓冲区不为空
                if (RingBuff_PreRD(&readBuff_C1) == PROTOCOLEX_CONTENT_HEAD_0) // 预读缓冲区数据
                {                                                              // 环形缓冲区内检测到帧头
                    processSM_P_C1 = PROTOCOLEX_SM_T2;                         // 提升状态,重启循环
                    sframeReadTotal_C1 = RingBuff_Cnt(&readBuff_C1);
                    readBuff_shot_C1 = RingBuff_GetShot(&readBuff_C1); // 获取RingBuff读取起始位置,用于帧尾检测失败后的再遍历
                    goto FP_DIRECT_NEXT_LOOP_C1;
                }
                RingBuff_FakeRD(&readBuff_C1); // 未检测到帧头,修改缓冲区读取索引
            }
            // 环形缓冲区未检测到帧头,从剩余的buff中检索帧头
            for (; readP < buff_len; readP++)
            {
                if (buff[readP] == PROTOCOLEX_CONTENT_HEAD_0) // 检测到帧头
                {
                    processSM_P_C1 = PROTOCOLEX_SM_T2;                 // 提升状态重启循环
                    readBuff_shot_C1 = RingBuff_GetShot(&readBuff_C1); // 获取RingBuff读取起始位置,用于帧尾检测失败后的再遍历
                    goto FP_DIRECT_NEXT_LOOP_C1;
                }
            }
            break;
        case PROTOCOLEX_SM_T2:
            if ((int32_t)(buff_len - readP) >= (int32_t)(PROTOCOLEX_LEN - sframeReadTotal_C1)) // 传入数据够长，可以直接进行数据解析
            {
                readP += RingBuff_WD_MultBytes(&readBuff_C1, &(buff[readP]), PROTOCOLEX_LEN - sframeReadTotal_C1);
                processSM_P_C1 = PROTOCOLEX_SM_T3;
                sframeReadTotal_C1 = 0;
                goto FP_DIRECT_NEXT_LOOP_C1;
            }
            else
            {                                                                                                // 传入数据不够一帧,先存下来
                sframeReadTotal_C1 += RingBuff_WD_MultBytes(&readBuff_C1, &(buff[readP]), buff_len - readP); // read The data To the Buff
                keepProcess = false;                                                                         // data read complete exit!
            }
            break;
        case PROTOCOLEX_SM_T3:
            // 将环形缓冲区的数据传送到处理缓冲区
            // RingBuff_RD_MultBytes(&readBuff_C1, &processBuff_C1, PROTOCOLEX_LEN + 2, PROTOCOLEX_LEN); // Read Data
            // if (  processBuff_C1[PROTOCOLEX_P_END] == PROTOCOLEX_CONTENT_END)    //帧尾检测
            if (RingBuff_PreRDJump(&readBuff_C1, PROTOCOLEX_LEN - 1) == PROTOCOLEX_CONTENT_END) // 帧尾检测
            {
                RingBuff_RD_MultBytes(&readBuff_C1, &processBuff_C1[0], PROTOCOLEX_LEN + 2, PROTOCOLEX_LEN); // 将环形缓冲区的数据传送到处理缓冲区
                processSM_P_C1 = PROTOCOLEX_SM_T4;
                goto FP_DIRECT_NEXT_LOOP_C1;
            }
            else
            { // 寄,收到了孤立的帧头
                processSM_P_C1 = PROTOCOLEX_SM_T1;
                readBuff_C1.Rd_Indx = readBuff_shot_C1.Rd_Indx + 1;
                sframeReadTotal_C1 = 0;
            }
            break;
        case PROTOCOLEX_SM_T4:
            // 开始校验数据
            crcdata = processBuff_C1[PROTOCOLEX_P_VERIFY];
            processBuff_C1[PROTOCOLEX_P_VERIFY] = 0x00;
            if (crcdata == crc_8(&processBuff_C1[0], PROTOCOLEX_LEN))
            {                                      // 校验通过
                processSM_P_C1 = PROTOCOLEX_SM_T5; // CRC Complete
                goto FP_DIRECT_NEXT_LOOP_C1;
            }
            else
            {                                      // 校验失败,直接丢弃数据
                processSM_P_C1 = PROTOCOLEX_SM_T1; // CRC Fail
            }
            break;
        case PROTOCOLEX_SM_T5:
            /* abababaaba */
            processSM_P_C1 = PROTOCOLEX_SM_T1; // CRC Success
            frameCnt_C1++;
#if PROTOCOLEX_USING_DEVICE == PROTOCOLEX_SUPPORT_DEVICE_A12F4_3AXIS_HOLDER
            switch (processBuff_C1[PROTOCOLEX_P_ROUTE])
            {
            case PROTOCOLEX_F_TYPE_DATA_0X11:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0X11;
                break;
            case PROTOCOLEX_F_TYPE_DATA_0X12:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0X12;
                break;
            case PROTOCOLEX_F_TYPE_DATA_0X13:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0X13;
                break;
            case PROTOCOLEX_F_TYPE_DATA_0X51:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0X51;
                break;
            case PROTOCOLEX_F_TYPE_DATA_0X52:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0X52;
                break;
            case PROTOCOLEX_F_TYPE_DATA_0XAA:
                mciData.mciData.RC_data.rcInfo.fType = PROTOCOLEX_F_TYPE_0XAA;
                break;
            default:
                goto RC_FRAME_GIVE_UP_C1; // 无对应RC控制指令，丢弃该结果
            }
            mciData.mciType = CSL_MCITYPE_RC_FRMAEGET;
            mciData.mciData.RC_data.rcInfo.error = PROTOCOLEX_P_STATUS_OK;
            // payloadP = processBuff_C1[PROTOCOLEX_P_DATA];
            mciData.mciData.RC_data.payloadData = *(ProEX_Data *)&processBuff_C1[PROTOCOLEX_P_DATA];
            if (csl_sign_RTOSForISRInitDown == true)
            {
                xQueueSendFromISR(mgs_1ToOutQueue, &mciData, pdTRUE); // 该解析函数是运行在中断函数中的，所以需要使用对应的中断版本
            }

        RC_FRAME_GIVE_UP_C1:
#endif
            break;
        }
    }
}
#endif
#pragma endregion

#endif

int protocolEX_frame_make(PROTOCOLEX_F_TYPE frameType, uint8_t *buff, uint16_t buff_len, void const *data)
{
    uint8_t crcResult, i;
    const ProEX_Data *data_t;
    uint32_t *p;

    if (buff_len <= PROTOCOLEX_LEN) // 缓冲区长度不够,退
        return -1;

    buff[PROTOCOLEX_P_HEAD_0] = PROTOCOLEX_CONTENT_HEAD_0; // 写入帧头

    switch (frameType)
    { // 写入帧类型
    case PROTOCOLEX_F_TYPE_0X11:
        buff[PROTOCOLEX_P_ROUTE] = PROTOCOLEX_F_TYPE_DATA_0X11;
        break;
    case PROTOCOLEX_F_TYPE_0X12:
        buff[PROTOCOLEX_P_ROUTE] = PROTOCOLEX_F_TYPE_DATA_0X12;
        break;
    case PROTOCOLEX_F_TYPE_0X13:
        buff[PROTOCOLEX_P_ROUTE] = PROTOCOLEX_F_TYPE_DATA_0X13;
        break;
    case PROTOCOLEX_F_TYPE_0XAA:
        buff[PROTOCOLEX_P_ROUTE] = PROTOCOLEX_F_TYPE_DATA_0XAA;
        break;
    }

    buff[PROTOCOLEX_P_SROUTE] = PROTOCOLEX_CONTENT_RESERVE;
    buff[PROTOCOLEX_P_VERIFY_SW] = PROTOCOLEX_CONTENT_RESERVE;

    data_t = data;
    p = (uint32_t *)&buff[PROTOCOLEX_P_DATA]; // 获取PayLoad指针
    // 写入帧数据
    p[0] = data_t->rawdata_32t[0];
    p[1] = data_t->rawdata_32t[1];

    // buff[19] = 0x00;
    // buff[19] = 0x01;

    // 帧校验预写入
    buff[PROTOCOLEX_P_VERIFY] = 0x00;
    buff[PROTOCOLEX_P_END] = PROTOCOLEX_CONTENT_END;

    crcResult = crc_8(buff, PROTOCOLEX_LEN);
    buff[PROTOCOLEX_P_VERIFY] = crcResult;

    return 0;
}

//=============================Export Util==========================

static inline float constrain(float input, float minValue, float maxValue)
{
    if (input < minValue)
        return minValue;
    else if (input > maxValue)
        return maxValue;
    else
        return input;
}

float proEX_Util_angleInvMapping(int16_t angleU16)
{
    return constrain(angleU16 * 180.0f / 32767, -180.0f, 180.0f);
}

int16_t proEX_Util_angleMapping(float anglef)
{
    anglef = constrain(anglef, -180.0f, 180.0f);
    return (int16_t)((32767.0f / 180.0f) * anglef);
}