#include <digitalIO.h>

digitalIO::digitalIO(drivers::I2C &i2c, drivers::MODBUS &bus, std::vector<uint8_t> remotes) : m_localOuts(drivers::TCA9554PWR(i2c, TCA9554_ADDRESS)), m_remoteAddrs(remotes)
{
    for (uint8_t i(DI1); i < DI_MAX; i++)
    {
        pinMode(i, INPUT_PULLUP); // set all local pins as digitalInput
    }

    for (auto a : remotes)
    {
        m_remotes.emplace_back(remoteIO(a, bus));
    }
}

digitalIO::~digitalIO()
{
}

void digitalIO::digitalIOWrite(const uint8_t ch, const bool value)
{
    if (ch < 0 || ch > getOutNum())
    {
        LOG_ERROR("Invalid digitalIOWrite channel number", ch);
    }

    if (ch < drivers::TCA9554PWR::DO_MAX) // write to i2c device for local outputs
    {
        digitalWriteLocal(ch, value);
    }
    else
    {
        digitalWriteRemote(ch - drivers::TCA9554PWR::DO_MAX, value);
    }
}
const bool digitalIO::digitalIORead(const uint8_t ch)
{
    if (ch < 0 || ch > getInNum())
    {
        LOG_ERROR("Invalid digitalIORead channel number", ch);
    }

    if (ch < (DI_MAX - DI1)) // read from local inputs not as gpio numbers
    {
        return digitalReadLocal(ch);
    }
    else
    {
        return digitalReadRemote(ch - (DI_MAX - DI1));
    }
}

void digitalIO::reset()
{
    // set all local and remote outputs to 0
    m_localOuts.setPort(0x00);
    for (auto r: m_remotes)
        r.resetAll(false);
}

const uint8_t digitalIO::getOutNum()
{
    return drivers::TCA9554PWR::DO_MAX + m_remotes.size() * remoteIO::CH_MAX;
}

const uint8_t digitalIO::getInNum()
{
    return DI_MAX + m_remotes.size() * remoteIO::CH_MAX;
}

void digitalIO::digitalWriteLocal(const uint8_t ch, const bool value)
{
    uint8_t retries(0);
    while (retries++ < maxRetries)
    {
        if (m_localOuts.setOut(ch, value))
        {
            LOG_DEBUG("digitalWriteLocal channel", ch, " status", value ? "True" : "False");
            break;
        }
        LOG_ERROR("Failed digitalWriteLocal channel ", ch, " status", value ? "True" : "False");
    }
}

void digitalIO::digitalWriteRemote(const uint8_t ch, const bool value)
{
    uint8_t retries(0);
    const uint8_t selectedRemote(floor(ch / 8.0f));
    const uint8_t selectedChannel(ch % remoteIO::CH_MAX);
    while (retries++ < maxRetries)
    {
        if (m_remotes[selectedRemote].setOut((remoteIO::channel_t)selectedChannel, value))
        {
            LOG_DEBUG("digitalWriteRemote remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
            break;
        }
        LOG_ERROR("Failed digitalWriteRemote remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
    }
}

const bool digitalIO::digitalReadLocal(const uint8_t ch)
{
    bool value = !digitalRead(ch + DI1); // base pin number in enum, inverted input
    LOG_DEBUG("digitalReadLocal pin", (ch + DI1), " status", value ? "True" : "False");
    return value;
}

const bool digitalIO::digitalReadRemote(const uint8_t ch)
{
    uint8_t retries(0);
    const uint8_t selectedRemote(floor(ch / 8.0f));
    const uint8_t selectedChannel(ch % remoteIO::CH_MAX);
    bool value;
    while (retries++ < maxRetries)
    {
        if (m_remotes[selectedRemote].getIn((remoteIO::channel_t)selectedChannel, value))
        {
            LOG_DEBUG("digitalReadRemote remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
            return value;
        }
        LOG_ERROR("Failed digitalReadRemote remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
    }
    return false;
}