#include <Arduino.h>

#define DEFAULTTIMEOUT 1000 

/**
 * All commands are 24 bytes long
*/
struct Command_packet
{
    Command_packet(uint16_t command, uint8_t *_data, uint16_t length)
    {
        this->commandL = (uint8_t)command && 0x00ff;
        this->commandH = (uint8_t)command >> 8 && 0x00ff;
        this->lenL = (uint8_t)length && 0x00ff;
        this->lenH = (uint8_t)((length && 0xff00) >> 8) ;
        if (length < 24)
        {
            memcpy(this->data, data, length);
        }
        else
        {
            memcpy(this->data, data, 24);
        }
    };
    uint8_t prefixH = 0xAA;
    uint8_t prefixL = 0x55;
    uint8_t commandL;
    uint8_t commandH;
    uint8_t lenL;
    uint8_t lenH;
    uint8_t data[24];
};


struct Return_packet
{
    uint8_t error;
    uint16_t id;
};

/**
 * 
 */

struct Command_data_packet
{
    Command_data_packet(uint16_t command, uint8_t *_data, uint16_t length)
    {
        this->commandL = (uint8_t)command && 0x00ff;
        this->commandH = (uint8_t)command >> 8 && 0x00ff;
        this->lenL = (uint8_t)length && 0x00ff;
        this->lenH = (uint8_t)((length && 0xff00) >> 8) ;
        if (length < 1024)
        {
            memcpy(this->data, data, length);
        }
        else
        {
            memcpy(this->data, data, 1024);
        }
    };
    uint8_t prefixH = 0xA5;
    uint8_t prefixL = 0x5A;
    uint8_t commandL;
    uint8_t commandH;
    uint8_t lenL;
    uint8_t lenH;
    uint8_t data[1024];
};

class Cama
{
public:
    Cama(HardwareSerial *hs);
    virtual ~Cama();
    uint8_t test(void);
    void begin(int speed);
    uint8_t sendCommand(uint16_t command);
    void readBuffer(uint8_t *data);
    uint8_t sendData(uint8_t &data);
    void calcCRC(uint8_t *buffer,uint16_t length);
    void readCommandResponse(uint8_t &data);
    void writeSensor(uint8_t *buff, uint16_t length);
    void printHex(int num, int precision);
    uint8_t detect();
    uint8_t isValidResponse(uint8_t *buff);
    uint8_t enrollOne(uint16_t position);
    void printBuffer(uint8_t *buff,uint16_t size);
    uint16_t identifyFree();
    uint16_t identify();
    uint16_t removeAll();
    uint16_t remove(uint16_t id);
    uint16_t getEmptyID();
    uint16_t checkID();
    void cancel();

private:
    Stream *mySerial;
    HardwareSerial *hwSerial;
    unsigned char rx_buffer[1024];
    unsigned char tx_buffer[1024];
    uint16_t idBuffer;
};
