#include "OmronPID.h"
#include "ModbusBridge.h"

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

#include <xmath.h>

// info <<200;2;64;testPIDs:1:0>>
// debug <<200;2;64;debug:1:0>>

void OmronPID::testPIDs()
{
    // setAllSP(15);
    // runAll();
    //  stopAll();
    //  singlePIDW(2, OR_E5_SWR::OR_E5_SWR_SP, 300);
    //  singlePIDW(1, 5000, 20);
    //  singlePID(1, ku8MBWriteSingleRegister, 0, OR_E5_CMD::OR_E5_AT_EXCECUTE);
}
short OmronPID::onRegisterMethods(Bridge *bridge)
{

    bridge->registerMemberFunction(id, this, C_STR("testPIDs"), (ComponentFnPtr)&OmronPID::testPIDs);
    bridge->registerMemberFunction(id, this, C_STR("debug"), (ComponentFnPtr)&OmronPID::debug);
    /*
    bridge->registerMemberFunction(id, this, C_STR("info"), (ComponentFnPtr)&OmronPID::info);
    bridge->registerMemberFunction(id, this, C_STR("debug"), (ComponentFnPtr)&OmronPID::debug);
    */
    return E_OK;
}
short OmronPID::loop()
{
    modbusLoop();
    return E_OK;
}
short OmronPID::debug()
{
    Log.verboseln("Omron PID  :: debug %d", id);
    modbus->print();
    printStates();
    return false;
}
short OmronPID::info()
{
    return false;
}
void OmronPID::initPIDS()
{
    for (short i = 0; i < NB_OMRON_PIDS; i++)
    {
        states[i].slaveID = slaveStart + i;
        states[i].idx = i;
        states[i].lastUpdated = millis();
        states[i].lastWritten = millis();
        states[i].flags = OmronState::FLAGS::DIRTY;
    }
}
void OmronPID::print()
{
    printStates();
}
int OmronPID::singlePIDW(int slave, int addr, int value)
{
    singlePID(slave, ku8MBWriteSingleRegister, addr, value);
    return E_OK;
}
int OmronPID::singlePID(int slave, short fn, int addr, int value)
{
    // Query *same = modbus->nextSame(QUEUED, slave, addr, fn, value);
    if (modbus->numByState(DONE) < 2 && fn != ku8MBWriteSingleRegister)
    {
        Log.verboseln("OmronPID::singlePID : failed : no buffer");
        return false;
    }
    if (modbus->numSame(QUEUED, slave, addr, fn, value) > 2)
    {
        Log.verboseln("OmronPID::singlePID : failed : num same");
        return false;
    }

    OmronState *pid = pidBySlave(slave);
    if (!pid)
    {
        Log.errorln("Omron-PID::singlePID : invalid PID : %d", slave);
        return E_NO_SUCH_PID;
    }

    Query *next = modbus->nextQueryByState(DONE);
    if (!next)
    {
        Log.verboseln("OmronPID::singlePID : failed : no free query");
        return E_OK;
    }
    next->fn = fn;
    next->slave = pid->slaveID;
    next->value = value;
    next->addr = addr;
    next->state = QUERY_STATE::QUEUED;
    next->owner = id;
    next->ts = millis();
    pid->flags = OmronState::FLAGS::UPDATING;
    if (fn == ku8MBWriteSingleRegister)
    {
        next->prio = MB_QUERY_TYPE_CMD;
    }
    else
    {
        next->prio = MB_QUERY_TYPE_STATUS_POLL;
    }
    return E_OK;
}
int OmronPID::eachPIDW(int addr, int value)
{
    return eachPID(ku8MBWriteSingleRegister, addr, value);
}
int OmronPID::eachPID(short fn, int addr, int value)
{
    for (short i = 0; i < NB_OMRON_PIDS; i++)
    {
        Query *next = modbus->nextQueryByState(DONE);
        if (next)
        {
            next->fn = fn;
            next->slave = states[i].slaveID;
            next->value = value;
            next->addr = addr;
            next->state = QUERY_STATE::QUEUED;
            next->owner = id;
        }
        else
        {
            Log.errorln("Omron-PID::eachPID : no buffer free");
        }
    }
    return E_OK;
}
void OmronPID::stopAll()
{
    eachPID(ku8MBWriteSingleRegister, 0, OR_E5_CMD::OR_E5_STOP);
}
void OmronPID::runAll()
{
    eachPID(ku8MBWriteSingleRegister, 0, OR_E5_CMD::OR_E5_RUN);
}
void OmronPID::setAllSP(int sp)
{
    eachPID(ku8MBWriteSingleRegister, OR_E5_SWR::OR_E5_SWR_SP, sp);
}
short OmronPID::setup()
{
    initPIDS();
    return E_OK;
}

void OmronPID::resetStates()
{
    for (short i = 0; i < NB_OMRON_PIDS; i++)
    {
        states[i].flags = OmronState::FLAGS::UPDATED;
    }
}
bool OmronPID::isHeatingUp()
{
    bool ret = false;
    for (short i = 0; i < NB_OMRON_PIDS; i++)
    {
        if (states[i].isHeating())
        {
            return true;
        }
    }
    return ret;
}
bool OmronPID::isRunning()
{
    bool ret = false;
    for (short i = 0; i < NB_OMRON_PIDS; i++)
    {
        if (states[i].isRunning())
        {
            return true;
        }
    }
    return ret;
}
void OmronPID::printStates()
{
    for (short i = 0; i < NB_OMRON_PIDS; i++)
    {
        states[i].print();
    }
}