#include <Arduino.h>

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

namespace Bafang {

HardwareSerial &DisplaySerial = Serial1;
HardwareSerial &MotorSerial = Serial2;

bool g_Proxy = true;

static unsigned long s_LastDisplayRx = 0;
static unsigned long s_LastMotorRx = 0;

static bool s_BlockDisplay = false;
static bool s_BlockMotor = false;

static uint8_t s_MotorBuffer[255];
static uint8_t s_MotorBufferLen = 0;

enum eJobState {
    STATE_NEW = 0,
    STATE_SEND = 1,
    STATE_RECV = 2
};

struct sJob {
    uint8_t *pData;
    uint8_t dataLen;
    uint16_t waitMs;
    FCallback pfnCallback;
    void *pUser;
};

static const int s_QueueSize = 8;
static sJob s_aQueue[s_QueueSize];
static int s_QueueRdIdx = 0;
static int s_QueueWrIdx = 0;
static int s_QueueLen = 0;

static eJobState s_JobState;
static unsigned long s_JobFinishTime = 0;
static void s_JobDone();


void OnDisplayRx(uint8_t c)
{
    s_LastDisplayRx = millis();

    if(s_BlockDisplay)
        return;

    MotorSerial.write(c);
}

void OnMotorRx(uint8_t c)
{
    s_LastMotorRx = millis();

    if(s_BlockMotor)
    {
        if(s_MotorBufferLen <= sizeof(s_MotorBuffer))
            s_MotorBuffer[s_MotorBufferLen++] = c;
        return;
    }

    DisplaySerial.write(c);
}

void recv()
{
    static int s_lastSerial = -1;

    while(Bafang::DisplaySerial.available())
    {
        uint8_t c = Bafang::DisplaySerial.read();

        if(Main::g_Debug[Main::g_CurrentChannel] > 10)
        {
            if(s_lastSerial != 1)
                Main::channel()->write("\nD: ");
            char sBuffer[4] = "XX ";
            bytes2hex((uint8_t *)&c, 1, sBuffer, sizeof(sBuffer));
            Main::channel()->write(sBuffer);
            s_lastSerial = 1;
        }

        Bafang::OnDisplayRx(c);
    }

    while(Bafang::MotorSerial.available())
    {
        uint8_t c = Bafang::MotorSerial.read();

        if(Main::g_Debug[Main::g_CurrentChannel] > 10)
        {
            if(s_lastSerial != 2)
                Main::channel()->write("\nM: ");
            char sBuffer[4] = "XX ";
            bytes2hex((uint8_t *)&c, 1, sBuffer, sizeof(sBuffer));
            Main::channel()->write(sBuffer);
            s_lastSerial = 2;
        }

        Bafang::OnMotorRx(c);
    }
}

void tick()
{
    recv();

    bool bProxy = g_Proxy;

    sJob *pJob = NULL;
    if(s_QueueLen)
        pJob = &s_aQueue[s_QueueRdIdx];

    if(pJob)
    {
        bProxy = false;

        if(s_BlockDisplay && s_BlockMotor)
        {
            switch(s_JobState)
            {
                case STATE_NEW:
                {
                    s_MotorBufferLen = 0;
                    s_JobFinishTime = millis() + (pJob->dataLen + 2) * BYTE_TIME + pJob->waitMs;
                    s_JobState = STATE_SEND;
                    MotorSerial.write(pJob->pData, pJob->dataLen);
                } break;
                case STATE_SEND:
                {
                    if(s_MotorBufferLen)
                    {
                        s_JobState = STATE_RECV;
                        break;
                    }

                    if(millis() >= s_JobFinishTime)
                        s_JobDone();
                } break;
                case STATE_RECV:
                {
                    if(millis() - s_LastMotorRx > 2 * BYTE_TIME)
                        s_JobDone();
                } break;
            }
        }
    }
    else if(s_MotorBufferLen && millis() - s_LastMotorRx > 2 * BYTE_TIME)
    {
        PrintCallback(Main::channel(), s_MotorBuffer, s_MotorBufferLen);
        s_MotorBufferLen = 0;
    }

    if(!bProxy)
    {
        if(millis() - s_LastDisplayRx > 2 * BYTE_TIME)
            s_BlockDisplay = true;

        if(s_BlockDisplay && millis() - s_LastMotorRx > 2 * BYTE_TIME)
            s_BlockMotor = true;
    }
    else
    {
        if(s_BlockDisplay && millis() - s_LastDisplayRx > 2 * BYTE_TIME)
        {
            s_BlockDisplay = false;
            s_BlockMotor = false;
        }
    }
}

static void s_JobDone()
{
    if(!s_QueueLen)
        return;

    sJob &Job = s_aQueue[s_QueueRdIdx];

    if(Job.pfnCallback)
        Job.pfnCallback(Job.pUser, s_MotorBuffer, s_MotorBufferLen);

    delete Job.pData;
    Job.pData = NULL;
    Job.dataLen = 0;
    Job.waitMs = 0;
    Job.pfnCallback = NULL;
    Job.pUser = NULL;

    s_QueueLen--;
    s_QueueRdIdx++;
    if(s_QueueRdIdx >= s_QueueSize)
        s_QueueRdIdx = 0;

    s_JobState = STATE_NEW;
}

bool QueueMotor(const uint8_t *pData, uint8_t dataLen, FCallback pfnCallback, void *pUser, uint16_t waitMs)
{
    if(!dataLen)
        return false;

    if(s_QueueLen >= s_QueueSize)
        return false;

    sJob &Job = s_aQueue[s_QueueWrIdx];
    Job.pData = new uint8_t[dataLen];
    memcpy(Job.pData, pData, dataLen);
    Job.dataLen = dataLen;
    Job.waitMs = waitMs;
    Job.pfnCallback = pfnCallback;
    Job.pUser = pUser;

    s_QueueLen++;
    s_QueueWrIdx++;
    if(s_QueueWrIdx >= s_QueueSize)
        s_QueueWrIdx = 0;

    s_JobState = STATE_NEW;
    return true;
}

void PrintCallback(void *pUser, const uint8_t *pData, uint8_t dataLen)
{
    int channel = (int)pUser;
    if(channel == Main::CHANNEL_NONE)
        return;

    for(int i = 0; i < dataLen; i++)
        Main::channel(channel)->printf("%02X", pData[i]);
    Main::channel(channel)->write('\n');
}

void OnDisplayRequest(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen)
{
    char aBuf[512];
    bytes2hex(data, dataLen, aBuf, sizeof(aBuf));
    Serial.printf("Display: %s\n", aBuf);
}

void OnMotorResponse(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen)
{
    char aBuf[512];
    bytes2hex(data, dataLen, aBuf, sizeof(aBuf));
    Serial.printf("Motor: %s\n", aBuf);
}

int TransformPASCode(uint8_t PasCode)
{
    static const uint8_t PasLevelArray[] = {
        PAS_LEVEL_0, PAS_LEVEL_1, PAS_LEVEL_2, PAS_LEVEL_3,
        PAS_LEVEL_4, PAS_LEVEL_5, PAS_LEVEL_6, PAS_LEVEL_7,
        PAS_LEVEL_8, PAS_LEVEL_9, PAS_LEVEL_PUSH
    };

    for(int i = 0; i < sizeof(PasLevelArray)/sizeof(*PasLevelArray); i++)
    {
        if(PasCode == PasLevelArray[i])
            return i;
    }

    return -1;
}

// rpm = 60 * v / (2 * pi * r)
// Example: 29inch tire, 30km/h
// v: 30 km/h / 3.6 = 8.33 m/s
// r: d = 29in * 0.7366m = -> r = 0.7366m / 2 = 0.3683m
// rps = 8.33 / (2 * pi * 0.3683) = 3.60
// rpm = 60 * 3.60 = 216

}