#include <S50140_Driver.h>

namespace drivers
{

    S50140::S50140(drivers::MODBUS &bus, const uint8_t address) : m_bus(bus), m_address(address), m_lastRequest(millis())
    {
    }
    S50140::~S50140()
    {
    }

    const S50140::powerinfo_t S50140::getAll()
    {
        powerinfo_t info{MAXFLOAT};
        info.v = getV();
        info.a = getA();
        info.pAct = getPact();
        info.pApp = getPapp();
        info.pRea = getPrea();
        info.f = getF();
        info.whTot = getWhTot();
        info.whPar = getWhPar();
        return info;
    }

    const float_t S50140::getV()
    {
        return readFloatReg(REG_V);
    }
    const float_t S50140::getA()
    {
        return readFloatReg(REG_A);
    }
    const float_t S50140::getPact()
    {
        return readFloatReg(REG_Pact);
    }
    const float_t S50140::getPapp()
    {
        return readFloatReg(REG_Papp);
    }
    const float_t S50140::getPrea()
    {
        return readFloatReg(REG_Prea);
    }
    const float_t S50140::getF()
    {
        return readFloatReg(REG_Freq);
    }
    const float_t S50140::getWhTot()
    {
        return readFloatReg(REG_WhTot);
    }
    const float_t S50140::getWhPar()
    {
        return readFloatReg(REG_WhPart);
    }

    void S50140::delayRequest()
    {
        auto now = millis();
        if ((now - m_lastRequest) < minDelay)
        { // minimum 500ms between requests
            delay(now - m_lastRequest);
        }
        m_lastRequest = now;
    }

    const uint8_t S50140::getRegset()
    {
        std::vector<uint16_t> value;
        delayRequest();
        m_bus.readHoldingRegisters(m_address, REG_Regset, 2, value);
        if (value.empty())
            return UINT8_MAX;
        return value.front() + value.back();
    }

    const uint16_t S50140::getCounterStatus()
    {
        std::vector<uint16_t> value;
        delayRequest();
        m_bus.readHoldingRegisters(m_address, REG_PartCount, 2, value);
        if (value.empty())
            return UINT16_MAX;
        return value.front() + value.back();
    }

    void S50140::resetPartialCounters()
    {
        uint8_t retries(0);
        const uint16_t resetAll = 0x0A03;
        const uint16_t stopAll = 0x0A02;
        const uint16_t startAll = 0x0A01;
        while (retries++ < maxRetries)
        {
            bool ok(true);
            delayRequest();
            LOG_WARN("Powermeter Counter STOP");
            ok &= m_bus.writeRegisters(m_address, REG_PartCount, {0x0000, stopAll});
            delayRequest();
            LOG_WARN("Powermeter Counter RESET");
            ok &= m_bus.writeRegisters(m_address, REG_PartCount, {0x0000, resetAll});
            delayRequest();
            LOG_WARN("Powermeter Counter START");
            ok &= m_bus.writeRegisters(m_address, REG_PartCount, {0x0000, startAll});
            if (ok)
                return;
            LOG_ERROR("Unable to Reset Powermeter Partial Counters, device", m_address);
        }
        return;
    }

    float_t S50140::readFloatReg(const uint16_t reg)
    {
        uint8_t retries(0);
        std::vector<uint16_t> values;

        while (retries++ < maxRetries)
        {
            delayRequest();
            if (m_bus.readHoldingRegisters(m_address, reg, dataWords, values) && values.size() == dataWords)
            {
                floatval_t fv;           // potrebbe essere il contrario, vedremo
                fv.words.lo = values[0]; // magari va invertita ancora l'endianness
                fv.words.hi = values[1];
                return fv.f;
            }
            LOG_ERROR("Unable to Read Powermeter values, device", m_address);
        }
        return MAXFLOAT;
    }

}