#ifndef COMMAND_MESSAGE_H
#define COMMAND_MESSAGE_H


#include <stdint.h>


#define COMMAND_PREFIX 0x69
#define COMMAND_ID 0x00
#define COMMAND_DATA_SIZE 160
#define COMMAND_HEADER_SIZE 13

typedef enum {
	COMMAND_ACK,
	COMMAND_NACK,
	LED_TOGGLE,
	LED_SET,
	SERVO_SET,
	SERVO_SET_ALL,
	ADC_READ_RAW,
	ADC_READ_RAW_ALL,
	ADC_READ,
	ADC_READ_ALL,
	ADC_SET_READ,
	DIGITAL_IN,
	DIGITAL_LEG_OUT,
	POWER_MONITOR_SET_READ,
	POWER_MONITOR_READ,
	SETTINGS_TEST_SAVE,
	SETTINGS_TEST_READ,

	MOVE_COMMAND_START,
	// All the commands going to robot thread go here
	TEST_COMMAND, // 16

	MOVE_COMMAND_END

} commands_e;

struct command_message_t {
	uint8_t prefix;
	uint8_t length;
	uint8_t id;
	uint8_t command;
	int64_t tick;
	uint8_t crc;
	uint8_t data[COMMAND_DATA_SIZE];
} __attribute__((packed));

/**
 * @brief Initialize command message to default state
 * 
 * @param msg Message to initialize
 */
void command_message_init(struct command_message_t *msg);

/**
 * @brief Create command message with data and CRC
 * 
 * @param msg Message to populate
 * @param length Data length in bytes
 * @param command Command type
 * @param data Data payload
 */
void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[]);

/**
 * @brief Calculate CRC for command message
 * 
 * @param msg Message to calculate CRC for
 * @return CRC value
 */
uint8_t command_calculate_crc(struct command_message_t *msg);

/**
 * @brief Create ACK command message
 * 
 * @param msg Message to populate
 */
void command_create_ack(struct command_message_t *msg);

/**
 * @brief Create NACK command message
 * 
 * @param msg Message to populate
 */
void command_create_nack(struct command_message_t *msg);

/**
 * @brief Print the command with LOG
 * 
 * @param msg Message to LOG
 */
void command_log(struct command_message_t *msg);


#endif // COMMAND_MESSAGE_H