#include <R4DCB08_Driver.h>

namespace drivers
{
    R4DCB08::R4DCB08(drivers::MODBUS &bus, const uint8_t address) : m_address(address), m_bus(bus), m_sensors(0)
    {
        m_sensors = getNum();
        m_lastRequest = millis();
    }

    R4DCB08::~R4DCB08()
    {
    }

    void R4DCB08::delayRequest()
    {
        auto now = millis();
        if ((now - m_lastRequest) < c_minDelay)
        { // minimum m_lastRequest between requests
            LOG_DEBUG("R4DCB08 delay request", (now-m_lastRequest));
            delay(now - m_lastRequest);
        }
        m_lastRequest = millis();
    }

    const float R4DCB08::getTemp(const uint8_t ch)
    {
        uint8_t retries(0);
        std::vector<uint16_t> rawT;
        if (ch < 0 || ch > getNum())
        {
            LOG_ERROR("Invalid Temperature Channel number", ch);
            return MAXFLOAT;
        }
        std::lock_guard<std::mutex> lock(m_bus.getMutex());
        while (retries++ < c_maxRetries)
        {
            delayRequest();
            if (m_bus.readHoldingRegisters(m_address, REG_TEMP + ch, 1, rawT) && !rawT.empty())
            {
                return rawT.front() / 10.0f;
            }
            LOG_ERROR("Failed to Read Temperature, device", m_address, "channel", ch);
            rawT.clear();
        }
        return MAXFLOAT;
    }

    const std::vector<float> R4DCB08::getTempAll()
    {
        uint8_t retries(0);
        std::vector<uint16_t> rawT;
        std::vector<float> out;
        std::lock_guard<std::mutex> lock(m_bus.getMutex());
        while (retries++ < c_maxRetries)
        {
            delayRequest();
            if (m_bus.readHoldingRegisters(m_address, REG_TEMP, getNum(), rawT) && !rawT.empty())
            {
                out.reserve(rawT.size());
                for (auto v : rawT)
                {
                    out.push_back(v / 10.0f);
                }
                return out;
            }
            LOG_ERROR("Failed to Read All Temperature, device", m_address);
            rawT.clear();
        }
        out.clear();
        return out;
    }

    void R4DCB08::setCorrection(std::vector<float> corr)
    {
        uint8_t retries(0);
        uint8_t channel(0);
        corr.resize(getNum()); // max number of temperature correction values is equal to number of sensors
        std::lock_guard<std::mutex> lock(m_bus.getMutex());
        for (auto v : corr)
        {
            while (retries++ < c_maxRetries)
            {
                delayRequest();
                if (m_bus.writeRegister(m_address, REG_TEMPCORR + channel, v * 10)) // convert to decimal degreees to register value
                {
                    channel++;
                    break;
                }
                LOG_ERROR("Failed to Set Temperature Correction, device", m_address);
            }
        }
    }

    std::vector<float> R4DCB08::getCorrection()
    {
        uint8_t retries(0);
        std::vector<uint16_t> rawV;
        std::vector<float> out;
        rawV.reserve(getNum());

        std::lock_guard<std::mutex> lock(m_bus.getMutex());
        while (retries++ < c_maxRetries)
        {
            delayRequest();
            if (m_bus.readHoldingRegisters(m_address, REG_TEMPCORR, getNum(), rawV))
            {
                out.reserve(rawV.size());
                for (auto v : rawV)
                {
                    out.push_back(v / 10.0f);
                }
                return out;
            }
            LOG_ERROR("Failed to Get Temperature Correction, device", m_address);
            rawV.clear();
        }
        out.clear();
        return out;
    }

    const uint8_t R4DCB08::getNum()
    {
        if (m_sensors)
            return m_sensors;
        uint8_t retries(0);
        uint8_t sensors(0);
        std::vector<uint16_t> rawT;
        std::lock_guard<std::mutex> lock(m_bus.getMutex());
        while (retries++ < c_maxRetries)
        {
            delayRequest();
            if (m_bus.readHoldingRegisters(m_address, REG_TEMP, T_MAX, rawT))
            {
                for (auto v : rawT)
                {
                    if (v <= INT16_MAX)
                        sensors++; // 32768 is returned if sensor is disconnected
                }
                m_sensors = sensors;
                return m_sensors;
            }
            LOG_ERROR("Failed to Get Sensor Number, device", m_address);
        }
        LOG_ERROR("No Temperature Sensors Detected, device", m_address);
        return 0;
    }
}