#pragma once

#include "message.h"

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

static const uint8_t SDK_HOST = 9;
static const uint8_t SDK_INDEX = 0;

#define SET_SDK_CONNECTION_CMD 0xD43F

enum CONNECTION {
    CONNECTION_WIFI_AP = 0,
    CONNECTION_WIFI_STA = 1,
    CONNECTION_USB_RNDIS = 2
};

void
set_sdk_connection(
		union Request* req,
        uint16_t seq,
        bool ack,
		enum CONNECTION connection_type,
		uint32_t ip_address,
		uint16_t port );

#define SDK_HEARTBEAT_CMD 0xD53F

void
sdk_heartbeat(
		union Request* req,
        uint16_t seq,
        bool ack );

#define SET_SDK_MODE_CMD 0xD13F

void
set_sdk_mode(
		union Request* req,
        uint16_t seq,
        bool ack,
		bool enable );

#define SET_SYSTEM_LED_CMD 0x333F

enum LEDCOMP {
    LEDCOMP_BOTTOM_BACK = 0x1,
    LEDCOMP_BOTTOM_FRONT = 0x2,
    LEDCOMP_BOTTOM_LEFT = 0x4,
    LEDCOMP_BOTTOM_RIGHT = 0x8,
    LEDCOMP_BOTTOM_ALL = 0xf,
    LEDCOMP_TOP_LEFT = 0x10,
    LEDCOMP_TOP_RIGHT = 0x20,
    LEDCOMP_TOP_ALL = 0x30,
    LEDCOMP_ALL = 0x3f
};

enum LEDEFFECT {
    LEDEFFECT_OFF = 0,
    LEDEFFECT_ON = 1,
    LEDEFFECT_BREATH = 2,
    LEDEFFECT_FLASH = 3,
    LEDEFFECT_SCROLLING = 4
};

void set_system_led (
		union Request* req,
        uint16_t seq,
        bool ack,
		uint8_t red,
		uint8_t green,
		uint8_t blue,
		enum LEDCOMP comp,
		uint16_t led_mask,
		enum LEDEFFECT effect,
		uint16_t t1,
		uint16_t t2 );

#define SET_ROBOT_MODE_CMD 0x463F

enum MOVEMENTMODE {
    MOVEMENTMODE_FREE,
    MOVEMENTMODE_GIMBAL_LEAD,
    MOVEMENTMODE_CHASSIS_LEAD
};

void
set_robot_mode (
		union Request* req,
        uint16_t seq,
        bool ack,
		enum MOVEMENTMODE mode );

#define SUBNODE_RESET_CMD 0x0248

void
subnode_reset (
		union Request* req,
        uint16_t seq,
        bool ack );

#define SUBSCRIBE_ADD_NODE_CMD 0x0148

void
subscribe_add_node (
		union Request* req,
        uint16_t seq,
        bool ack );

#define ADD_SUB_MSG_CMD 0x0348

#define DDS_BATTERY 0x000200096862229f
#define DDS_GIMBAL_BASE 0x00020009f5882874
#define DDS_VELOCITY 0x0002000949a4009c
#define DDS_ESC 0x00020009c14cb7c5
#define DDS_ATTITUDE 0x000200096b986306
#define DDS_IMU 0x00020009a7985b8d
#define DDS_POSITION 0x00020009eeb7cece
#define DDS_SA_STATUS 0x000200094a2c6d55
#define DDS_CHASSIS_MODE 0x000200094fcb1146
#define DDS_SBUS 0x0002000988223568
#define DDS_SERVO 0x000200095f0059e7
#define DDS_ARM 0x0002000926abd64d
#define DDS_GRIPPER 0x00020009124d156a
#define DDS_GIMBAL_POS 0x00020009f79b3c97
#define DDS_STICK 0x0002000955e9a0fa
#define DDS_MOVE_MODE 0x00020009784c7bfd
#define DDS_TOF 0x0002000986e4c05a
#define DDS_PINBOARD 0x00020009eebb9ffc

void
add_sub_msg (
        union Request* req,
        uint16_t seq,
        bool ack,
        uint8_t msg_id,
        uint8_t freq,
        uint64_t subject_uuid );