#include <stdlib.h>
#include <string.h>
#include <arpa/inet.h>

#include "robomaster.h"

struct Client* session_new(void* buffer, size_t size) {
    struct Client* session = malloc(sizeof(struct Client*));
    if(!buffer)
        session->buffer = malloc(sizeof(struct Header));
    else
        session->buffer = buffer;
    session->size = 0;
    session->seq = 0;
    session->max_size = size;
    // TODO: Make this configurable
    session->hostbyte = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_CLIENT_INDEX);
    return session;
}

// private
struct Message*
message_new(struct Client* session, uint8_t cmdset, uint8_t cmdid, size_t length, const void* body) {

    struct Message* message = session->buffer + session->size;
    memcpy((void*)message->body, body, length);
    length += sizeof(struct Header) + sizeof(struct Footer);
    session->size += length;
    int16_t seq = session->seq++;

    message->header.preamble = 0x55;
    message->header.length_l = length & 0xFF;
    message->header.length_h = (length >> 8) & 0x3 | 4;
    message->header.crc = crc8(message, 3);
    message->header.seq_id = seq;
    message->header.sender = session->hostbyte;
    message->header.receiver = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX);
    message->header.ack_needed = true;
    message->header.cmdset = cmdset;
    message->header.cmdid = cmdid;

    struct Footer* footer = (void*)message + length - sizeof(struct Footer);
    uint16_t crc = crc16(message, length - sizeof(struct Footer));
    footer->crc = crc;

    return message;

}

// public
const Message
set_sdk_connection_req_new (
		Client session,
		struct SetSdkConnectionReq* msg ) {

    msg->host = session->hostbyte;
    struct Message* message = message_new(session, 0x3F, SET_SDK_CONNECTION_CMDID, sizeof(struct SetSdkConnectionReq), msg);
    return message;
}

const Message
set_system_led_req_new (
		Client session,
		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 ) {
    const struct SetSystemLedReq msg = {
        comp, led_mask,
	{ effect, 7 },
	red, green, blue,
	0, t1, t2
    };
    return message_new(session, 0x3F, SET_SYSTEM_LED_CMDID, sizeof(msg), &msg);
}