#ifdef MB_MONITORING_STATUS_VFD_MAX_LOAD
#include <xstatistics.h>
#endif

#include <SRegister.h>
#include "./OmronVFD.h"
#include "./ModbusBridge.h"
#include "./OmronMX2.h"

#include "PHApp.h"
#include "enums.h"
#include "utils.h"

bool didTest = true;
bool debugReceive = false;
bool debugSend = false;
bool debugFilter = false;
bool debugMultiRegs = false;
bool debugModQueries = false;
bool printModbusErrors = true;
bool debugStateUpdate = false;
bool debugSkipping = true;

uint16_t OmronVFD::updateState()
{
    if (now - mbTS > mbInterval)
    {
        OmronVFDState *state = getVFDState();
        short req = state->queue.val();
        bool queued = false;
        switch (req)
        {
        case E_VFD_MB_QUEUE_STATUS:
        {
            queued = read_16(1, OMRON_STATUS_POLL_REGISTERS, MB_QUERY_TYPE_STATUS_POLL) == E_QUEUED;
            break;
        }
        case E_VFD_MB_QUEUE_DIR:
        {
            queued = read_coil_single(MX2_C_DIR - 1) == E_QUEUED;
            break;
        }
        case E_VFD_MB_QUEUE_AMPS:
        {
            queued = read_16(MX2_R_TORQUE, 1, MB_QUERY_TYPE_STATUS_POLL) == E_QUEUED;
            break;
        }
        }
        mbTS = now;
        return queued ? E_QUEUED : E_OK;
    }
    return E_SKIP;
}
short OmronVFD::modbusLoop()
{

    if (millis() - last < OMRON_MX2_LOOP_INTERVAL)
    {
        return E_SKIP;
    }
    if (modbus->qstate() != IDLE)
    {
        return E_SKIP;
    }

    last = now;
    updateState();
    fromTCP();
    Query *nextCommand = modbus->nextQueryByState2(QUERY_STATE::QUEUED, id);
    if (!nextCommand)
    {
        return E_SKIP;
    }
    nextCommand->state = QUERY_STATE::PROCESSING;
    nextCommand->ts = now;
    modbus->nextWaitingTime = MODBUS_CMD_WAIT;
    if (debugSend)
    {
        if (now - debugTS > OMRON_MX2_DEBUG_INTERVAL)
        {
            debugTS = now;
            nextCommand->print();
            Serial.println();
        }
    }

    modbus->query(
        nextCommand->slave,
        nextCommand->fn,
        nextCommand->addr,
        nextCommand->value,
        id);
}
short OmronVFD::onResponse(short error)
{
    Query *last = modbus->nextQueryByState2(QUERY_STATE::PROCESSING, id);
    if (!last || last->slave != slaveAddress || last->owner != id)
    {
        return E_SKIP;
    }
    OmronVFDState *state = getVFDState();
    short req = state->queue.val();

    switch (req)
    {
    case E_VFD_MB_QUEUE_STATUS:
    {
        states[0].FC = modbus->ModbusSlaveRegisters[0] / 100;
        states[0].status = modbus->ModbusSlaveRegisters[2];
        states[0].state = modbus->ModbusSlaveRegisters[3];
        readInterval = OMRON_MX2_MB_INTERVAL;
        lastError = E_OK;
        state->queue.incr();
        break;
    }/*
    case E_VFD_MB_QUEUE_DIR:
    {
        states[0].direction = modbus->ModbusSlaveRegisters[0];
        readInterval = OMRON_MX2_MB_INTERVAL;
        lastError = E_OK;
        state->queue.incr();
        break;
    }*/
    case E_VFD_MB_QUEUE_AMPS:
    {
        states[0].current = modbus->ModbusSlaveRegisters[0];
        readInterval = OMRON_MX2_MB_INTERVAL;
        lastError = E_OK;
        state->queue.incr();
        break;
    }
    }

    if (debugReceive && last->fn == ku8MBWriteSingleRegister)
    {
        last->print();
        modbus->print();
        readInterval = OMRON_MX2_MB_INTERVAL;
        lastError = E_OK;
    }
    last->reset();
    updateTCP();
    return E_OK;
}
short OmronVFD::onError(short error)
{
    // 01 : 86 : 21 : 82 : 78
    if (printModbusErrors)
    {
        Log.verboseln("Omron VFD onError : %d : %d", error, readInterval);
    }
    if (error == ERR_MODBUS_TIMEOUT && lastError == ERR_MODBUS_TIMEOUT)
    {
        readInterval += MB_POLL_RETRY_STEP;
    }

    lastError = error;
    readInterval = clamp<millis_t>(readInterval, 0, MB_MAX_POLL_INTERVAL);
    if (readInterval == MB_MAX_POLL_INTERVAL)
    {
        owner->onError(id, E_VFD_TIMEOUT);
    }
    // App Level Errors
    if (error > E_VFD_CUSTOM)
    {
        switch (error)
        {
        case E_VFD_OVERLOAD:
            lastError = error;
            Log.verboseln("Omron VFD :: Overload Error ! %d", error);
            owner->onError(id, error);
            break;

        default:
            break;
        }
        return error;
    }

    Query *last = modbus->nextQueryByState(QUERY_STATE::PROCESSING, id);
    if (last)
    {
        last->reset();
    }
    return E_OK;
}
short OmronVFD::onRawResponse(short size, uint8_t rxBuffer[])
{
    if (!size)
    {
        Log.verboseln("OmronVFD::rawResponse - Invalid Size : %d", size);
        return E_OK;
    }
    if (debugReceive)
    {
        // Log.verboseln("OmronVFD::rawResponse - Size : %d", size);
        // printHex(rxBuffer, size);
    }
    if (size == 5)
    {
        Log.verboseln("Error - Modbus, invalid response size : %d", size);
    }
    return ERROR_OK;
}
short OmronVFD::ping()
{
    Query *next = modbus->nextQueryByState(DONE);
    if (next)
    {
        next->fn = ku8MBLinkTestOmronMX2Only;
        next->slave = slaveAddress;
        next->value = 1234;
        next->addr = 0;
        next->state = QUERY_STATE::QUEUED;
        next->ts = millis();
        return E_OK;
    }
    return E_QUERY_BUFFER_END;
}
void OmronVFD::updateTCP()
{
    modbus->mb->R[MB_R_VFD_STATUS] = states[0].status;
    modbus->mb->R[MB_R_VFD_STATE] = states[0].state;
    // modbus->mb->R[MB_R_VFD_DIRECTION] = states[0].direction;
    modbus->mb->R[MB_R_FREQ_TARGET] = states[0].FC;
#ifdef MB_MONITORING_STATUS_VFD_MAX_LOAD
    modbus->mb->R[MB_MONITORING_STATUS_VFD_MAX_LOAD] = states[0].max_current;
    modbus->mb->R[MB_MONITORING_STATUS_VFD_RUN_MODE] = runMode;
    modbus->mb->R[MB_MONITORING_STATUS_VFD_CURRENT] = states[0].current;
    // modbus->mb->R[75] = states[0].status2;
    // modbus->mb->R[76] = states[0].status3;
#endif
    // fromTCP();
}
short OmronVFD::fromTCP()
{
    if (modbus->mb->R[9] == 1)
    {
        modbus->mb->R[9] = 0;
        modbus->print();
    }

    short ret = E_SKIP;
    if (!directDirection())
    {
        if (modbus->mb->R[MB_W_VFD_RUN] == E_VFD_RUN_MODE::E_VFD_RUN_MODE_RUN)
        {
            onStart();
            write_Bit(MX2_START, 1);
            modbus->mb->R[MB_W_VFD_RUN] = 0;
            runMode = E_VFD_RUN_MODE::E_VFD_RUN_MODE_RUN;
            ret = E_QUEUED;
        }

        if (modbus->mb->R[MB_W_VFD_RUN] == E_VFD_RUN_MODE::E_VFD_RUN_MODE_STOP)
        {
            onStop();
            write_Bit(MX2_START, 0);
            owner->onStop(0);
            modbus->mb->R[MB_W_VFD_RUN] = 0;
            runMode = E_VFD_RUN_MODE::E_VFD_RUN_MODE_STOP;
            ret = E_QUEUED;
        }

        if (modbus->mb->R[MB_W_VFD_RUN] == E_VFD_RUN_MODE::E_VFD_RUN_MODE_STOP_RETRACT)
        {
            onStop();
            write_Bit(MX2_START, 0);
            owner->onStop(0);
            modbus->mb->R[MB_W_VFD_RUN] = 0;
            runMode = E_VFD_RUN_MODE::E_VFD_RUN_MODE_STOP_RETRACT;
            retractState = E_VFD_RETRACT_STATE::E_VFD_RETRACT_STATE_BRAKING;
            ret = E_QUEUED;
        }

        short dir = modbus->mb->R[MB_W_DIRECTION];
        if (dir)
        {
            switch (dir)
            {
            case OmronVFD::E_VFD_DIR::E_VFD_DIR_FORWARD:
                forward();
                ret = E_QUEUED;
                break;
            case OmronVFD::E_VFD_DIR::E_VFD_DIR_REVERSE:
                reverse();
                ret = E_QUEUED;
                break;
            default:
                stop();
                ret = E_QUEUED;
                break;
            }
            modbus->mb->R[MB_W_DIRECTION] = 0;
        }
    }
    if (modbus->mb->R[MB_W_FREQ_TARGET] > 0)
    {
        setTargetFreq(modbus->mb->R[MB_W_FREQ_TARGET]);
        modbus->mb->R[MB_W_FREQ_TARGET] = 0;
        ret = E_QUEUED;
    }
    return ret;
}
uint16_t OmronVFD::write_Single(uint16_t addr, unsigned int data)
{
    Query *next = modbus->nextQueryByState(DONE);
    if (next)
    {
        next->fn = ku8MBWriteSingleRegister;
        next->slave = slaveAddress;
        next->value = data;
        next->addr = addr;
        next->state = QUERY_STATE::QUEUED;
        next->ts = millis();
        next->owner = id;
        next->prio = MB_QUERY_TYPE_CMD;
        lastWriteAddress = addr;
        lastWriteValue = data;
        return E_QUEUED;
    }
    return E_SKIP;
}
uint16_t OmronVFD::read_coil_single(uint16_t addr)
{
    Query *next = modbus->nextQueryByState(DONE);
    if (next)
    {
        next->fn = ku8MBReadCoils;
        next->slave = slaveAddress;
        next->value = 1;
        next->addr = addr;
        next->state = QUERY_STATE::QUEUED;
        next->ts = millis();
        next->owner = id;
        next->prio = MB_QUERY_TYPE_STATUS_POLL_2;
        return E_QUEUED;
    }
    return E_SKIP;
}
uint16_t OmronVFD::write_Bit(uint16_t addr, int on)
{
    Query *same = modbus->nextSame(QUEUED, slaveAddress, addr, ku8MBWriteSingleCoil, on);
    if (same && millis() - same->ts < 300)
    {
    }

    Query *next = modbus->nextQueryByState(DONE);
    if (next)
    {
        next->fn = ku8MBWriteSingleCoil;
        next->slave = slaveAddress;
        next->addr = addr;
        next->value = on;
        next->state = QUERY_STATE::QUEUED;
        next->ts = millis();
        next->owner = id;
        next->prio = MB_QUERY_TYPE_CMD;
        lastWriteAddress = addr;
        lastWriteValue = on;
        return E_OK;
    }
    return E_QUERY_BUFFER_END;
}
short OmronVFD::readSingle_16(int addr, int prio)
{
    if (skipRead(slaveAddress, ku8MBReadHoldingRegisters, addr, 1, prio))
    {
        return E_SKIP;
    }
    Query *next = modbus->nextQueryByState(DONE);
    if (next)
    {
        next->fn = ku8MBReadHoldingRegisters;
        next->slave = slaveAddress;
        next->value = 1;
        next->addr = addr;
        next->state = QUERY_STATE::QUEUED;
        next->ts = millis();
        next->prio = prio;
        next->owner = id;
        return E_QUEUED;
    }
    return E_SKIP;
}
short OmronVFD::read_16(int addr, int num, int prio)
{
    if (skipRead(slaveAddress, ku8MBReadHoldingRegisters, addr, num, prio))
    {
        return E_SKIP;
    }

    Query *next = modbus->nextQueryByState(DONE);
    if (next)
    {
        next->fn = ku8MBReadHoldingRegisters;
        next->slave = slaveAddress;
        next->value = num;
        next->addr = addr;
        next->state = QUERY_STATE::QUEUED;
        next->ts = millis();
        next->prio = prio;
        next->owner = id;
        return E_QUEUED;
    }
    return E_SKIP;
}
bool OmronVFD::skipRead(int slave, int fn, int addr, int num, int prio)
{
    Query *same = modbus->nextSame(QUEUED, slave, addr, fn, num);
    if (same && millis() - same->ts < OMRON_MX2_SAME_REQUEST_INTERVAL)
    {
        return true;
    }

    if (modbus->numByState(DONE) < MODBUS_QUEUE_MIN_FREE)
    {
        return true;
    }

    if (modbus->numSame(QUEUED, slave, addr, fn, num) >= 1)
    {
        return true;
    }

    if (modbus->numSame(PROCESSING, slave, addr, fn, num) >= 1)
    {
        return true;
    }
    return false;
}