#pragma once

#include "crc.h"

typedef enum tCmdId  {
    TMSG_REQ_STATE = 0x7F,
    TMSG_SET_WIDTH,
    TMSG_SET_RATIO,
    TMSG_END_CALIB,
    TMSG_SET_SPEED,
    TMSG_ZERO,
    TMSG_DELIM,
} tCmdId;

typedef struct tMsgRatio {
    double ratio;
    unsigned char crc;
}__attribute__((packed)) tMsgRatio;

typedef struct tMsgWidth {
    double width;
    unsigned char crc;
}__attribute__((packed)) tMsgWidth;

typedef struct tMsgSpeed {
    double vl;
    double vr;
    unsigned char crc;
}__attribute__((packed)) tMsgSpeed;

typedef struct tRespState {
    double x;
    double y;
    double theta;
    double vl;
    double vr;
    unsigned char crc;
}__attribute__((packed)) tRespState;

typedef struct tRespEndCalib {
    double left_gain;
    double right_gain;
    unsigned char crc;
}__attribute__((packed)) tRespEndCalib;

typedef union tMsgBuf {
    tMsgRatio msg_ratio;
    tMsgWidth msg_width;
    tMsgSpeed msg_speed;
}__attribute__((packed)) tMsgBuf;

#define TCMD_ID(id) ((uint16_t)(id << 8 | TMSG_DELIM))
#define TGET_CMD(cmd) ((cmd >> 8) & 0xFF)

enum {
    TSTATE_IDLE,
    TSTATE_BUILD_MSG,
};

inline unsigned push_tMsgBuf(tMsgBuf *buf, unsigned cnt, unsigned char byte, tCmdId id) {
    unsigned size = 0;
    switch (id) {
        case TMSG_SET_RATIO:
            size = sizeof(tMsgRatio);
            break;
        case TMSG_SET_WIDTH:
            size = sizeof(tMsgRatio);
            break;
        case TMSG_SET_SPEED:
            size = sizeof(tMsgRatio);
            break;
        default:
            return 0;
    }
    if(cnt < size) {
        ((unsigned char*)buf)[cnt] = byte;
    }
    return cnt == size-1;
}