#pragma once

#include <pico/stdio.h>
#include "crc.h"
#include "stdio.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,
};

static inline void write_raw(void* data, size_t n) {
    const uint8_t* a = data;
    for(size_t i = 0; i < n; i++){
        putchar_raw(a[i]);
    }
}

static inline void send_tRespState(tRespState *resp) {
    resp->crc = crcFooter(*resp);
    write_raw(resp, sizeof(tRespState));
    fflush(stdout);
}

static inline void send_tRespEndCalib(tRespEndCalib *state) {
    state->crc = crcFooter(*state);
    write_raw(state, sizeof(tRespEndCalib));
    fflush(stdout);
}

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;
}