/**
 * 收到数据后，按一个字节一个字节的方式进入函数imu_rx()，通过累加和校验校验，就进入到data_extraction()函数，
 * 在memcpy之后，我们就可以从614E-P数据帧结构体中获取到数据。
 */
#define IMU_PARSE_STATE_SYNC1_ID 0xAA
#define IMU_PARSE_STATE_SYNC2_ID 0x55
#include "stdint.h"
#include "string.h"
#include "stdio.h"

typedef struct
{
    volatile unsigned char state;
    volatile unsigned int count;
    volatile unsigned int id;
    volatile unsigned int length;
    volatile uint32_t check;
    volatile unsigned char id_temp;
    volatile unsigned char length_temp;
    volatile unsigned char check_temp1;
    volatile unsigned char check_temp2;
    volatile unsigned char check_temp3;
    volatile unsigned char check_temp4;
} ParseStruct;

typedef enum
{
    IMU_PARSE_STATE_WAIT_SYNC1 = 0,
    IMU_PARSE_STATE_WAIT_SYNC2,
    IMU_PARSE_STATE_WAIT_ID1,
    IMU_PARSE_STATE_WAIT_ID,
    IMU_PARSE_STATE_WAIT_LENGTH1,
    IMU_PARSE_STATE_WAIT_LENGTH2,
    IMU_PARSE_STATE_PAYLOAD,
    IMU_PARSE_STATE_CHECK1,
    IMU_PARSE_STATE_CHECK2,
    IMU_PARSE_STATE_CHECK3,
    IMU_PARSE_STATE_CHECK4
} imu_parse_state_t;

/**
 * IMU614E-P倾斜测绘输出帧
 */
struct SURVEY_USER
{
    // uint8_t header1; //0xAA 帧头
    // uint8_t header2; //0x55 帧头
    // uint16_t ID //帧ID
    // uint16_t length //帧长
    double lat_cor;      // 杆底纬度-度
    double lon_cor;      // 杆底经度-度
    double alt_cor;      // 杆底高程-米
    float px_cor;        // 杆底相对控制点的东向距离-米
    float py_cor;        // 杆底相对控制点的北向距离-米
    float pz_cor;        // 杆底相对控制点的天向距离-米
    float titlt;         // 倾角-度
    float accuracy;      // 精度因子
    float imu[7];        // 陀螺 x,y,z 轴 deg/s加速度计 x,y,z 轴 g 温度
    uint32_t status;     // 系统状态
    uint16_t len;        // 当前杆长-毫米
    float resdl;         // 标定进度
    uint8_t fixType;     // RTK 固定解状态
    uint8_t svnum;       // RTK 星数
    uint8_t diff_age;    // RTK 差分延时
    int16_t inst_att[2]; // 预留
    float rev[4];        // 预留
    // uint32 crcAccum;//累加校验和
} __attribute__((packed));

unsigned char payload[3000];
SURVEY_USER survey_user;
ParseStruct _parse;

void imu_rx(unsigned char data);

/**
 * 解析614E-P数据帧，并且打印加速度计x轴，加速度计y轴，加速度计z轴，温度
 */
int main()
{
    unsigned char data[] = {
        0xAA, 0x55, 0x66, 0x01, 0x69, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xEF, 0x90, 0x79, 0x42, 0xE1, 0xFA,
        0xC7, 0x42, 0x40, 0x1E, 0x51, 0xBE, 0x06, 0x15, 0x34, 0x3D, 0x0A, 0xF5,
        0xB8, 0x3C, 0x50, 0x78, 0x9E, 0xBE, 0xF4, 0x1E, 0x52, 0x3F, 0x85, 0x41,
        0xF8, 0xBE, 0xA7, 0x37, 0xA5, 0x41, 0x00, 0x00, 0x00, 0x00, 0x44, 0x07,
        0x00, 0x00, 0x00, 0x00, 0x01, 0x26, 0x00, 0xF7, 0xFA, 0xFA, 0xFE, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
        0xB5, 0x60, 0x48, 0x18, 0x19, 0x00, 0x00}; // 614E-P数据帧

    uint32_t nbytes = sizeof(data);
    for (uint32_t i = 0; i < nbytes; i++)
    {
        imu_rx(data[i]);
    }
    printf("加速度计x轴=%lf\n", survey_user.imu[3]);
    printf("加速度计y轴=%lf\n", survey_user.imu[4]);
    printf("加速度计z轴=%lf\n", survey_user.imu[5]);
    printf("温度=%lf\n", survey_user.imu[6]);
}

void data_extraction(void)
{
    if (_parse.id == 0x0166)
    {
        memcpy((uint8_t *)&survey_user, (uint8_t *)payload, sizeof(SURVEY_USER));
    }
}

/**
 * 解码状态机
 */
void imu_rx(unsigned char data)
{
    static uint32_t crcAccum = 1;
    switch (_parse.state)
    {
    case IMU_PARSE_STATE_WAIT_SYNC1:
        if (data == IMU_PARSE_STATE_SYNC1_ID)
        {
            _parse.state = IMU_PARSE_STATE_WAIT_SYNC2;
            crcAccum = 0;
            crcAccum += data;
        }
        break;
    case IMU_PARSE_STATE_WAIT_SYNC2:
        if (data == IMU_PARSE_STATE_SYNC2_ID)
        {
            crcAccum += data;
            _parse.state = IMU_PARSE_STATE_WAIT_ID1;
        }
        else
        {
            _parse.state = IMU_PARSE_STATE_WAIT_SYNC1;
        }
        break;
    case IMU_PARSE_STATE_WAIT_ID1:
        _parse.id_temp = data;
        crcAccum += data;
        _parse.state = IMU_PARSE_STATE_WAIT_ID;
        break;
    case IMU_PARSE_STATE_WAIT_ID:
        _parse.id = data << 8 | _parse.id_temp;
        crcAccum += data;

        _parse.state = IMU_PARSE_STATE_WAIT_LENGTH1;
        break;
    case IMU_PARSE_STATE_WAIT_LENGTH1:
        _parse.length_temp = data;

        crcAccum += data;
        _parse.state = IMU_PARSE_STATE_WAIT_LENGTH2;
        break;
    case IMU_PARSE_STATE_WAIT_LENGTH2:
        _parse.length = data << 8 | _parse.length_temp;

        crcAccum += data;
        if (_parse.length > 0 && _parse.length < 10000)
        {
            _parse.count = 0;
            _parse.state = IMU_PARSE_STATE_PAYLOAD;
        }
        else
        {
            _parse.state = IMU_PARSE_STATE_WAIT_SYNC1;
        }
        break;
    case IMU_PARSE_STATE_PAYLOAD:
        *((char *)(payload) + _parse.count) = data;
        crcAccum += data;
        if (++_parse.count == _parse.length)
            _parse.state = IMU_PARSE_STATE_CHECK1;
        break;
    case IMU_PARSE_STATE_CHECK1:

        _parse.check_temp1 = data;
        _parse.state = IMU_PARSE_STATE_CHECK2;
        break;
    case IMU_PARSE_STATE_CHECK2:
        _parse.check_temp2 = data;
        _parse.state = IMU_PARSE_STATE_CHECK3;
        break;
    case IMU_PARSE_STATE_CHECK3:
        _parse.check_temp3 = data;
        _parse.state = IMU_PARSE_STATE_CHECK4;
        break;
    case IMU_PARSE_STATE_CHECK4:

        _parse.check = (data << 24) | (_parse.check_temp3 << 16) | (_parse.check_temp2 << 8) | (_parse.check_temp1);

        if (_parse.check == crcAccum)
        {
            data_extraction();
        }

        _parse.state = IMU_PARSE_STATE_WAIT_SYNC1;
        break;
    default:
        break;
    }
}