#include "protocol.hpp"
#include <stdint.h>

#include "pico/stdlib.h"

#include "crc16.hpp"

void assembleGobotRPCHeader(char * buffer, GobotRPCNumber number, GobotRPCTypes type, size_t len) {
    buffer[0] = ((number & 0b1111) << 4) | ((type & 0b11) << 2);
    buffer[1] = (len + 4) & 0xff;
}

GobotRPCHeaderInfo extractGobotRPCHeader(char * buffer) {
    GobotRPCHeaderInfo info;
    info.number = (GobotRPCNumber)((buffer[0] >> 4) & 0b1111);
    info.type = (GobotRPCTypes)((buffer[0] >> 2) & 0b11);
    info.len = buffer[1];
    return info;
}

void assembleCRC(char * buffer, size_t data_len) {
    unsigned short crc = crc16(buffer, data_len + GobotRPC_Package_DATA_OFFSET);
    buffer[GobotRPC_Package_DATA_OFFSET + data_len + 1] = crc & 0xff;
    buffer[GobotRPC_Package_DATA_OFFSET + data_len + 0] = (crc >> 8) & 0xff;
}