#include <Arduino.h>

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

namespace Bafang {
namespace DisplaySlave {

enum eDisplayState {
    STATE_IDLE = 0,
    STATE_REQUEST = 1,
    STATE_RESPONSE = 2
};

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

void OnData(uint8_t c)
{
    static const sPacketDesc *packet = NULL;

    if(g_State == STATE_REQUEST && millis() - g_LastRx > 2 * BYTE_TIME)
        g_State = STATE_IDLE;

    if(g_State == STATE_RESPONSE)
        g_State = STATE_IDLE;

    if(g_State == STATE_IDLE)
    {
        packet = NULL;
        g_RxBufferLen = 0;
    }

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

    g_RxBuffer[g_RxBufferLen++] = c;

    switch(g_State)
    {
        case STATE_IDLE:
        {
            if(c == MODE_READ || c == MODE_WRITE)
                g_State = STATE_REQUEST;
        } break;

        case STATE_REQUEST:
        {
            if(g_RxBufferLen == 2)
            {
                packet = NULL;
                for(int i = 0; i < sizeof(PacketDesc)/sizeof(*PacketDesc); i++)
                {
                    if(PacketDesc[i].cmd == c)
                    {
                        packet = &PacketDesc[i];
                        break;
                    }
                }
            }

            if(!packet)
            {
                Serial.printf("Display: !!! UNKNOWN CMD %X\n", c);
                g_State = STATE_IDLE;
                break;
            }
            const sPacketDesc::sInfo &info = packet->req;

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

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

            if(g_RxBufferLen < readLen)
                break;

            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;
                    break;
                }
            }

            if(packet->res.len > 0)
                g_State = STATE_RESPONSE;
            else
                g_State = STATE_IDLE;

            OnDisplayRequest(packet, g_RxBuffer, g_RxBufferLen);
        } break;

        default: break;
    }

    g_LastRx = millis();
}

bool ReplyRawPacket(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen)
{
    if(g_State != STATE_RESPONSE)
        return false;

    DisplaySerial.write(data, dataLen);
    g_State = STATE_IDLE;
    return true;
}

void tick()
{

}

}
}