#include <xmath.h>

#include "OmronPID.h"
#include "ModbusBridge.h"
#include "./OmronE5.h"
#include "./enums.h"
#include "./config.h"
#include "./utils.h"

bool printModbus = false;
bool printPIDUpdates = false;
bool _debugQuery = false;
bool _debugQueryState = false;
bool debugResponse = false;
bool debugRawResponse = false;
bool debugNextUpdates = false;
bool printMBErrors = true;

#define DEBUG_OPID_ERRORS
#ifdef DEBUG_OPID_ERRORS
    #define _DEBUG_OPID_ERRORS(format, ...) Log.verboseln(format, ##__VA_ARGS__)
#else
    #define _DEBUG_OPID_ERRORS(format, ...)
#endif

OmronState *OmronPID::nextToUpdate2(){
    return pidBySlave(queue.val());
}
OmronState *OmronPID::nextToUpdate()
{
    OmronState *oldest = NULL;
    for (short i = 0; i < NB_OMRON_PIDS; i++)
    {
        OmronState *c = &states[i];
        if (c->flags == OmronState::FLAGS::UPDATING &&
            millis() - c->lastUpdated > (1000 * 2))
        {
            c->flags == OmronState::FLAGS::DIRTY;
        }
        if (c->flags == OmronState::FLAGS::UPDATING)
        {
            continue;
        }
        if (!oldest)
        {
            oldest = c;
        }
        if (c != oldest && c->lastUpdated < oldest->lastUpdated)
        {
            oldest = c;
        }
    }
    return oldest;
}
void OmronPID::updateState()
{
    OmronState *next = nextToUpdate2();
    if (next != NULL)
    {
        modbus->nextWaitingTime = MODBUS_CMD_WAIT;
        next->flags = OmronState::FLAGS::UPDATING;
        next->lastUpdated = millis();
        if(_debugQuery){
            Log.verboseln("OmronPID::updateState : Slave=%d", next->slaveID);
        }
        read10_16(next->slaveID, 0, MB_QUERY_TYPE_STATUS_POLL);
    }
}
short OmronPID::modbusLoop()
{
    if (modbus->qstate() != IDLE)
        return E_SKIP;

    if (millis() - last < OMRON_PID_UPDATE_INTERVAL)
        return E_SKIP;

    last = now;
    fromTCP();
    updateState();
    Query *nextCommand = modbus->nextQueryByState2(QUERY_STATE::QUEUED, id);
    if (!nextCommand)
        return E_SKIP;

    if (printModbus)
        modbus->print();

    nextCommand->state = QUERY_STATE::PROCESSING;
    nextCommand->ts = millis();
    modbus->nextWaitingTime = MODBUS_CMD_WAIT;    
    if (_debugQueryState)
    {
        if (millis() - d0TS > 1000)
        {
            Log.verboseln("OmronPID::loop : Query Modbus : Slave=%d | FN=%d | Addr=%d | Nb=%d",
                          nextCommand->slave,
                          nextCommand->fn,
                          nextCommand->addr,
                          nextCommand->value);

            nextCommand->print();
            Serial.println();
            d0TS = now;
        }
    }
    

    modbus->query(
        nextCommand->slave,
        nextCommand->fn,
        nextCommand->addr,
        nextCommand->value,
        id);

    return E_OK;
}
short OmronPID::read10_16(int slaveAddress, int addr, int prio)
{
    if (modbus->skipRead(slaveAddress, ku8MBReadHoldingRegisters, addr, OMRON_PID_READ_STATUS_REGISTERS, prio))
    {
        return E_SKIP;
    }
    Query *next = modbus->nextQueryByState(DONE);
    if (next != NULL)
    {
        next->reset();
        next->fn = ku8MBReadHoldingRegisters;
        next->slave = slaveAddress;
        next->value = OMRON_PID_READ_STATUS_REGISTERS;
        next->addr = addr;
        next->state = QUERY_STATE::QUEUED;
        next->prio = prio;
        next->owner = id;
        next->ts = millis();
        return E_QUEUED;
    }
    return E_SKIP;
}
short OmronPID::onRawResponse(short size, uint8_t rxBuffer[])
{
    Query *current = modbus->nextQueryByState(PROCESSING, id);
    if (debugRawResponse)
    {
        Log.verboseln("OmronPID::rawResponse : size=%d | %X", size, rxBuffer);
    }
    if (current)
    {
        switch (current->fn)
        {
        case ku8MBWriteSingleRegister:
        {

            if (size == 5 && rxBuffer[1] == OR_E5_RESPONSE_CODE::OR_COMMAND_ERROR)
            {
                Serial.print("------ \n Command Error: ");
                Serial.print(rxBuffer[2]);
                Serial.print(" : ");
                switch (rxBuffer[2])
                {
                case OR_E5_ERROR::VARIABLE_ADDRESS_ERROR:
                {
                    Serial.println(OR_E_MSG_INVALID_ADDRESS);
                    break;
                }
                case OR_E5_ERROR::VARIABLE_RANGE_ERROR:
                {
                    Serial.println(OR_E_MSG_INVALID_RANGE);
                    break;
                }
                case OR_E5_ERROR::VARIABLE_OPERATION_ERROR:
                {
                    Serial.println(OR_E_MSG_OPERATION_ERROR);
                    break;
                }
                }
                Serial.println("\n------");
                return rxBuffer[2];
            }

            if (size == 8 && (rxBuffer[0] != current->slave || rxBuffer[2] != current->addr))
            {
                return OR_COMMAND_ERROR;
            }
            break;
        }
        }
    }
    return ERROR_OK;
}
short OmronPID::onResponse(short error)
{
    Query *last = modbus->nextQueryByState2(QUERY_STATE::PROCESSING, id);
    if (!last || last->owner != id)
    {
        return E_SKIP;
    }
    
    OmronState *state = pidBySlave(last->slave);
    if (!state)
    {
        Log.warningln("Omron-PID :: invalid slave : %d", last->slave);
        modbus->print();
        return E_NO_SUCH_PID;
    }

    if (last->fn == ku8MBWriteSingleRegister)
    {
        last->reset();
        state->flags = OmronState::FLAGS::UPDATED;
        state->lastWritten = now;
        lastError = E_OK;
        readInterval = OMRON_PID_UPDATE_INTERVAL;
        return E_OK;
    }

    if (last->fn == ku8MBReadHoldingRegisters)
    {
        state->lastDT = now - state->lastUpdated;
        state->lastUpdated = millis();
        state->statusHigh = modbus->ModbusSlaveRegisters[2];
        state->statusLow = modbus->ModbusSlaveRegisters[3];
        state->pv = modbus->ModbusSlaveRegisters[1];
        state->sp = modbus->ModbusSlaveRegisters[5];
        state->flags = OmronState::FLAGS::UPDATED;
        state->state = state->isHeating() ? 1 : 0;
        lastError = E_OK;
        readInterval = OMRON_PID_UPDATE_INTERVAL;
        if (printPIDUpdates)
        {
            Log.verboseln("Omron-PID :: Updated Slave : %d : ", state->slaveID);
            state->print();
        }
        queue.incr();
    }
    last->reset();
    updateTCP();
    return E_OK;
}
short OmronPID::onError(short error)
{
    if (printMBErrors)
    {
        Log.verboseln("Omron PID :: onError %d : %d", error, readInterval);
    }
    Query *last = modbus->nextQueryByState(QUERY_STATE::PROCESSING, id);
    if (last)
    {
        last->reset();
    }
    else
    {
        Log.errorln("Omron PID :: onError - can't find last query! ");
    }
    resetStates();

    // Modbus Errors
    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_PID_TIMEOUT);
    }

    return E_OK;
}
short OmronPID::fromTCP()
{
    millis_t t = now;
    short ret = E_SKIP;
    for (short i = 0; i < NB_OMRON_PIDS; i++)
    {
        switch (i)
        {
        case 0:
        {
            if (modbus->mb->R[MB_W_PID_1_SP] > 0)
            {
                short temp = clamp<short>(modbus->mb->R[MB_W_PID_1_SP], 0, OMRON_PID_MAX_TEMPERATURE);
                singlePID(states[i].slaveID, ku8MBWriteSingleRegister, OR_E5_SWR::OR_E5_SWR_SP, temp);
                modbus->mb->R[MB_W_PID_1_SP] = 0;
                ret = E_QUEUED;
            }
            break;
        }
        case 1:
        {

            if (modbus->mb->R[MB_W_PID_2_SP] > 0)
            {
                short temp = clamp<short>(modbus->mb->R[MB_W_PID_2_SP], 0, OMRON_PID_MAX_TEMPERATURE);
                singlePID(states[i].slaveID, ku8MBWriteSingleRegister, OR_E5_SWR::OR_E5_SWR_SP, temp);
                modbus->mb->R[MB_W_PID_2_SP] = 0;
                ret = E_QUEUED;
            }
            break;
        }
        case 2:
        {
            if (modbus->mb->R[MB_W_PID_3_SP])
            {
                short temp = clamp<short>(modbus->mb->R[MB_W_PID_3_SP], 0, OMRON_PID_MAX_TEMPERATURE);
                singlePID(states[i].slaveID, ku8MBWriteSingleRegister, OR_E5_SWR::OR_E5_SWR_SP, temp);
                modbus->mb->R[MB_W_PID_3_SP] = 0;
                ret = E_QUEUED;
            }
            break;
        }
        }
    }
    return ret;
}
void OmronPID::updateTCP()
{
    for (int i = 0; i < NB_OMRON_PIDS; i++)
    {
        modbus->mb->R[MB_REGISTER_OFFSET_TC + 0 + (i * MB_REGISTER_OFFSET_TC_RANGE)] = states[i].pv;
        modbus->mb->R[MB_REGISTER_OFFSET_TC + 1 + (i * MB_REGISTER_OFFSET_TC_RANGE)] = states[i].sp;
        modbus->mb->R[MB_REGISTER_OFFSET_TC + 2 + (i * MB_REGISTER_OFFSET_TC_RANGE)] = states[i].state;
    }
}
short OmronPID::queryResponse(short error)
{
    Query *last = modbus->nextQueryByState2(QUERY_STATE::PROCESSING, id);
    if (last)
    {
        last->state = QUERY_STATE::DONE;
    }
    return E_OK;
}
OmronState *OmronPID::pidBySlave(int slave)
{
    for (short i = 0; i < NB_OMRON_PIDS; i++)
    {
        if (states[i].slaveID == slave)
        {
            return &states[i];
        }
    }
    return NULL;
}