#pragma once

#include <stdbool.h>
#include <stdint.h>

#define PACKED __attribute__((__packed__))

struct PACKED Header {

    // The preamble marks the start of a message and is always 0x55
    uint8_t preamble;

    // The length of the message includes the preamble and CRC16 at the end
    uint16_t length;

    // This is a CRC8 checksum for the preamble and length together
    uint8_t crc;

    // hostbyte of the message sender
    uint8_t sender;

    // hostbyte of the message receiver
    uint8_t receiver;

    // Each message has a sequence ID
    // The Robomaster will respond with the same ID for each request
    // The value of the sequence ID doesn't matter to the Robomaster
    // Repeating sequence IDs are acceptable
    int16_t seq_id;

    // The message attribute flags designate if a response is needed or if the
    // message is a response.
    union {
        uint8_t attribute;
        struct {
            uint8_t reserved : 6;
            bool ack_needed : 1;
            bool is_ack : 1;
        };
    };

    // Each command has a cmdset and cmdid that together make a cmd key
    union {
        uint16_t cmd;
        struct {
            uint8_t cmdset;
            uint8_t cmdid;
        };
    };

};

struct PACKED Footer {
    uint16_t crc;
};

struct PACKED SetSystemLedReq {

    struct Header header;

    // Which LEDs on which component to control
    uint32_t comp_mask;
    uint16_t led_mask;

    struct {
        // off, on, flashing, etc.
        uint8_t effect_mode : 4;
	// Always 7
        uint8_t control_mode : 4;
    };

    // RGB values for the LED color
    uint8_t red;
    uint8_t green;
    uint8_t blue;

    // Always 0
    uint8_t loop;

    // These time intervals have different meaning depending on effect
    uint16_t t1;
    uint16_t t2;

    struct Footer footer;

};

struct PACKED SetSystemLedResp {
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
};

struct PACKED SubscribeAddNodeReq
{
    struct Header header;
    uint8_t hostbyte;
    uint32_t sub_vision; // always 0x03000000
    struct Footer footer;
};

struct PACKED SubscribeAddNodeResp
{
    struct Header header;
    uint8_t retcode; // 0x00 or 0x50 are acceptable here
    uint8_t hostbyte;
    struct Footer footer;
};

struct PACKED SubNodeResetReq
{
    struct Header header;
    uint8_t hostbyte;
    struct Footer footer;
};

struct PACKED SubNodeResetResp
{
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
};

struct PACKED SetRobotModeReq
{
    struct Header header;
    uint8_t mode;
    struct Footer footer;
};

struct PACKED SetRobotModeResp
{
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
};

struct PACKED ChassisSpeedModeReq
{
    struct Header header;
    float speed[3];
    struct Footer footer;
};

struct PACKED ChassisSpeedModeResp
{
    struct Header header;
    int8_t retcode;
    struct Footer footer;
};

struct PACKED SetChassisWheelSpeedReq
{
    struct Header header;
    int8_t wheel_speed[4];
    struct Footer footer;
};

struct PACKED SetChassisWheelSpeedResp
{
    struct Header header;
    int8_t retcode;
    struct Footer footer;
};

struct PACKED SetWheelSpeedReq
{
    struct Header header;
    int16_t wheel_speed[4];
    struct Footer footer;
};

struct PACKED SetWheelSpeedResp
{
    struct Header header;
    int8_t retcode;
    struct Footer footer;
};

struct PACKED SdkHeartbeatReq {
    struct Header header;
    struct Footer footer;
};

struct PACKED SdkHeartbeatResp {
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
};

struct PACKED SetSdkModeReq {
    struct Header header;
    uint8_t enable;
    struct Footer footer;
};

struct PACKED SetSdkModeResp {
    struct Header header;
    uint8_t retcode;
    struct Footer footer;
};

struct PACKED SetSdkConnectionReq {
    struct Header header;
    uint8_t control;
    uint8_t host;
    uint8_t connection;
    uint8_t protocol;
    uint32_t ip_address;
    uint16_t port;
    struct Footer footer;
};

struct PACKED SetSdkConnectionResp {
    struct Header header;
    uint8_t retcode;
    uint8_t state;
    uint32_t config_ip;
    struct Footer footer;
};

union Request {
    struct Header header;
    struct SetSdkConnectionReq sdkconn;
    struct SetSdkModeReq sdkmode;
    struct SdkHeartbeatReq heartbeat;
    struct SetSystemLedReq led;
    struct SetWheelSpeedReq wheel;
    struct SetChassisWheelSpeedReq chswheel;
    struct ChassisSpeedModeReq chsspeed;
    struct SetRobotModeReq mvmode;
    struct SubNodeResetReq subnodereset;
    struct SubscribeAddNodeReq subnodeadd;
};
union Response {
    struct Header header;
    struct SetSdkConnectionResp sdkconn;
    struct SetSdkModeResp sdkmode;
    struct SdkHeartbeatResp heartbeat;
    struct SetSystemLedResp led;
    struct SetWheelSpeedResp wheel;
    struct SetChassisWheelSpeedResp chswheel;
    struct ChassisSpeedModeResp chsspeed;
    struct SetRobotModeResp mvmode;
    struct SubNodeResetResp subnodereset;
    struct SubscribeAddNodeResp subnodeadd;
};
union Message {
    struct Header header;
    union Request req;
    union Response resp;
};

enum MESSAGEERR {
    MESSAGEERR_NONE,
    MESSAGEERR_HEADERCRC,
    MESSAGEERR_FOOTERCRC
};

enum MESSAGEERR
message_validate(const union Message* message);

static
inline
size_t
message_length(uint16_t cmd) {
    switch(cmd) {
        case SET_SDK_CONNECTION_CMD:
            return sizeof(struct SetSdkConnectionReq);
        case SDK_HEARTBEAT_CMD:
            return sizeof(struct SdkHeartbeatReq);
        case SET_SDK_MODE_CMD:
            return sizeof(struct SetSdkModeReq);
        case SET_SYSTEM_LED_CMD:
            return sizeof(struct SetSystemLedReq);
        case SET_ROBOT_MODE_CMD:
            return sizeof(struct SetRobotModeReq);
        case SUBNODE_RESET_CMD:
            return sizeof(struct SubNodeResetReq);
        case SUBSCRIBE_ADD_NODE_CMD:
            return sizeof(struct SubscribeAddNodeReq);
        default:
            return 0;
    }
}