#include <digitalIO.h>
#include <utils.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(a, bus);
    }
    LOG_INFO("Initialized digitalIO -> inputs", getInNum(), "outputs", getOutNum());
}

digitalIO::~digitalIO()
{
}

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

    if (ch < drivers::TCA9554PWR::DO_MAX) // write to i2c device for local outputs
    {
        writeLocal(ch, value);
    }
    else
    {
        writeRemote(ch - drivers::TCA9554PWR::DO_MAX, value);
    }
}

void digitalIO::digitalOutWritePort(const std::vector<bool> &values)
{
    if (values.size() != getOutNum())
    {
        LOG_ERROR("Invalid digitalOutWrite channel number", values.size());
        return;
    }
    const std::vector<bool> locals(values.begin(), values.begin() + drivers::TCA9554PWR::DO_MAX);
    const std::vector<bool> remotes(values.begin() + drivers::TCA9554PWR::DO_MAX, values.end());
    writeLocalPort(locals);
    writeRemotePort(remotes);
}

const bool digitalIO::digitalOutRead(const uint8_t ch)
{
    if (ch < 0 || ch > getOutNum())
    {
        LOG_ERROR("Invalid digitalOutRead channel number", ch);
        return false;
    }

    if (ch < drivers::TCA9554PWR::DO_MAX) // write to i2c device for local outputs
    {
        return readLocalIn(ch);
    }
    else
    {
        return readRemoteIn(ch - drivers::TCA9554PWR::DO_MAX);
    }
}

const std::vector<bool> digitalIO::digitalOutReadPort()
{
    const std::vector<bool> locals(readLocalOutPort());
    const std::vector<bool> remotes(readRemoteOutPort());
    std::vector<bool> rv;
    rv.reserve(getOutNum());
    rv.insert(rv.begin(), locals.begin(), locals.end());
    rv.insert(rv.end(), remotes.begin(), remotes.end());
    return rv;
}

const bool digitalIO::digitalInRead(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 readLocalIn(ch);
    }
    else
    {
        return readRemoteIn(ch - (DI_MAX - DI1));
    }
}

const std::vector<bool> digitalIO::digitalInReadPort()
{
    const std::vector<bool> locals(readLocalInPort());
    const std::vector<bool> remotes(readRemoteInPort());
    std::vector<bool> rv;
    rv.reserve(getInNum());
    rv.insert(rv.begin(), locals.begin(), locals.end());
    rv.insert(rv.end(), remotes.begin(), remotes.end());
    return rv;
}

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::getLocalInNum()
{
    return (DI_MAX - DI1);
}
const uint8_t digitalIO::getLocalOutNum()
{
    return drivers::TCA9554PWR::DO_MAX;
}
const uint8_t digitalIO::getRemoteInNum()
{
    return m_remotes.size() * remoteIO::CH_MAX;
}
const uint8_t digitalIO::getRemoteOutNum()
{

    return m_remotes.size() * remoteIO::CH_MAX;
}

const uint8_t digitalIO::getOutNum()
{
    return getLocalOutNum() + getRemoteOutNum();
}

const uint8_t digitalIO::getInNum()
{
    return getLocalInNum() + getRemoteInNum();
}

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

void digitalIO::writeLocalPort(const std::vector<bool> &values)
{
    uint8_t retries(0);
    uint8_t decValue(0);
    for (uint8_t i(0); i < 8; i++) // convert from bits to byte value
    {
        if (values[i])
            decValue |= High << i;
    }
    while (retries++ < c_maxRetries)
    {
        if (m_localOuts.setPort(decValue))
        {
            LOG_DEBUG("writeLocalPort value", printBoolVec(values).c_str());
            return;
        }
        LOG_ERROR("Failed writeLocalPort value", printBoolVec(values).c_str());
    }
}

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

void digitalIO::writeRemotePort(const std::vector<bool> &values)
{
    uint8_t retries(0);
    while (retries++ < c_maxRetries)
    {
        bool ok(true);
        for (uint8_t i(0); i < values.size(); i += remoteIO::CH_MAX)
        {
            const uint8_t selectedRemote(floor(i / (float)remoteIO::CH_MAX));
            const std::vector<bool> currValues(values.begin() + i, values.begin() + i + remoteIO::CH_MAX);
            ok &= m_remotes[selectedRemote].setOutPort(currValues);
            if (ok)
            {
                LOG_DEBUG("writeRemotePort remote", selectedRemote, "values", printBoolVec(values).c_str());
                continue;
            }
            LOG_ERROR("Failed writeRemotePort remote", selectedRemote, "values", printBoolVec(values).c_str());
            break;
        }
        if (ok)
            break;
    }
}

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

const bool digitalIO::readLocalOut(const uint8_t ch)
{
    bool value(false);
    uint8_t retries(0);
    while (retries++ < c_maxRetries)
    {
        if (m_localOuts.readOut(ch, value))
        {
            LOG_DEBUG("readLocalOut pin", (ch), " status", value ? "True" : "False");
            return value;
        }
        LOG_ERROR("Failed readLocalOut channel", ch);
    }
    return false;
}

const std::vector<bool> digitalIO::readLocalInPort()
{
    std::vector<bool> values(getLocalInNum());
    for (uint8_t i(0); i < values.size(); i++)
    {
        values[i] = readLocalIn(i);
    }
    LOG_DEBUG("readLocalInPort values", printBoolVec(values).c_str());
    return values;
}

const std::vector<bool> digitalIO::readLocalOutPort()
{
    uint8_t retries(0);
    uint8_t state(0);
    std::vector<bool> values(getLocalOutNum());
    while (retries++ < c_maxRetries)
    {
        if (m_localOuts.readPort(state))
        {
            for (uint8_t i(0); i < values.size(); i++)
            {
                values[i] = (state >> i) & High;
            }
            LOG_DEBUG("readLocalOutPort values", printBoolVec(values).c_str());
            return values;
        }
        LOG_ERROR("Failed readLocalOutPort");
    }
    values.clear();
    return values;
}

const bool digitalIO::readRemoteIn(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++ < c_maxRetries)
    {
        if (m_remotes[selectedRemote].getIn((remoteIO::channel_t)selectedChannel, value))
        {
            LOG_DEBUG("readRemoteIn remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
            return value;
        }
        LOG_ERROR("Failed readRemoteIn remote", selectedRemote, " channel ", selectedChannel, " status", value ? "True" : "False");
    }
    return false;
}

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

const std::vector<bool> digitalIO::readRemoteInPort()
{
    uint8_t retries(0);
    std::vector<bool> values;
    values.reserve(getRemoteInNum());
    while (retries++ < c_maxRetries)
    {
        bool ok(true);
        for (uint8_t i(0); i < getRemoteInNum(); i += remoteIO::CH_MAX)
        {
            const uint8_t selectedRemote(floor(i / (float)remoteIO::CH_MAX));
            std::vector<bool> remVals(remoteIO::CH_MAX);
            ok &= m_remotes[selectedRemote].getInPort(remVals);
            if (ok)
            {
                values.insert(values.begin() + values.size(), remVals.begin(), remVals.end());
                LOG_DEBUG("readRemoteInPort remote", selectedRemote, "values", printBoolVec(remVals).c_str());
                continue;
            }
            LOG_ERROR("Failed readRemoteInPort remote", selectedRemote);
            break;
        }
        if (ok)
            return values;
    }
    values.clear();
    return values;
}

const std::vector<bool> digitalIO::readRemoteOutPort()
{
    uint8_t retries(0);
    std::vector<bool> values;
    values.reserve(getRemoteOutNum());
    while (retries++ < c_maxRetries)
    {
        bool ok(true);
        for (uint8_t i(0); i < getRemoteOutNum(); i += remoteIO::CH_MAX)
        {
            const uint8_t selectedRemote(floor(i / (float)remoteIO::CH_MAX));
            std::vector<bool> remVals(remoteIO::CH_MAX);
            ok &= m_remotes[selectedRemote].getOutPort(remVals);
            if (ok)
            {
                values.insert(values.begin() + values.size(), remVals.begin(), remVals.end());
                LOG_DEBUG("readRemoteOutPort remote", selectedRemote, "values", printBoolVec(remVals).c_str());
                continue;
            }
            LOG_ERROR("Failed readRemoteOutPort remote", selectedRemote);
            break;
        }
        if (ok)
            return values;
    }
    values.clear();
    return values;
}