#include <Arduino.h>

#include "utils.h"
#include "main.h"
#include "bafang.h"

namespace Bafang {
namespace MotorMaster {

typedef void (*CommandCallback)(void *pUser, uint8_t *pData, int dataLen, bool crcError);

struct MotorCommand {
    uint8_t *pSend;
    uint8_t len;
    bool crc;
    CommandCallback pfnCallback;
    void *pUser;
};

enum eMotorState {
    STATE_IDLE = 0,
    STATE_AWAITING = 1,
    STATE_SUPERFLUOUS = 2
};

eMotorState g_State;
uint8_t g_RxBuffer[255];
uint8_t g_RxBufferLen = 0;
unsigned long g_LastRx = 0;

const sPacketDesc *g_pPacketDesc = NULL;

void OnData(uint8_t c)
{
    if(!g_RxBufferLen)
        g_LastRx = millis();

    if(millis() - g_LastRx > 2 * BYTE_TIME)
    {
        g_State = STATE_IDLE;
        g_RxBufferLen = 0;
        g_pPacketDesc = NULL;
    }

    if(g_State == STATE_IDLE)
        g_State = STATE_SUPERFLUOUS;

    if(g_RxBufferLen >= sizeof(g_RxBuffer))
        return;

    g_RxBuffer[g_RxBufferLen++] = c;

    if(g_pPacketDesc)
    {
        const sPacketDesc::sInfo &info = g_pPacketDesc->res;

        if(info.len < 0)
        {
            g_State = STATE_IDLE;
            return;
        }

        int readLen = info.len;
        if(info.crc)
            readLen++;

        if(g_RxBufferLen < readLen)
            return;

        if(info.crc)
        {
            uint8_t crc = 0;
            for(int i = 0; i < g_RxBufferLen - 1; i++)
                crc += g_RxBuffer[i];

            if(c != crc)
            {
                char bla[16];
                bytes2hex(g_RxBuffer, g_RxBufferLen, bla, sizeof(bla));
                Serial.printf("!!! CRC ERROR %X != %X -> %s\n", c, crc, bla);
                g_State = STATE_IDLE;
                return;
            }
        }

        OnMotorResponse(g_pPacketDesc, g_RxBuffer, g_RxBufferLen);
        g_State = STATE_IDLE;
        g_RxBufferLen = 0;
        g_pPacketDesc = NULL;
    }

    g_LastRx = millis();
}

void tick()
{
    if((g_State == STATE_SUPERFLUOUS || g_State == STATE_AWAITING) && g_RxBufferLen && millis() - g_LastRx > 2 * BYTE_TIME)
    {
        OnMotorResponse(g_pPacketDesc, g_RxBuffer, g_RxBufferLen);
        g_State = STATE_IDLE;
        g_RxBufferLen = 0;
        g_pPacketDesc = NULL;
    }
}

void SendRawPacket(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen)
{
    g_State = STATE_AWAITING;
    g_RxBufferLen = 0;
    g_pPacketDesc = packet;
    MotorSerial.write(data, dataLen);
}

}
}