#include <extio.h>

// Static interrupt callback
static void onExpanderInterrupt(void *arg)
{
    auto cls = (ExternalIO *)(arg);
    if (!cls) // invalid args
        return;
    cls->extReadInterrupt();
}

ExternalIO::ExternalIO(TwoWire &i2c, std::mutex &i2c_mutex, const uint8_t int_pin) : m_i2cMutex(i2c_mutex), m_i2c(i2c), m_intPin(int_pin)
{
    std::lock_guard<std::mutex> lock(m_i2cMutex);
    // Attach OUT expanders on BUS
    m_outMap[EXPANDER_A_OUT_ADDR] = std::make_unique<PCA9555>();
    m_outMap[EXPANDER_A_OUT_ADDR]->attach(m_i2c, EXPANDER_A_OUT_ADDR);
    m_outMap[EXPANDER_B_OUT_ADDR] = std::make_unique<PCA9555>();
    m_outMap[EXPANDER_B_OUT_ADDR]->attach(m_i2c, EXPANDER_B_OUT_ADDR);

    for (auto &[a, e] : m_outMap)
    {
        e->direction(PCA95x5::Direction::OUT_ALL);
        e->polarity(PCA95x5::Polarity::ORIGINAL_ALL);
    };

    // Attach IN Expanders on Bus
    m_inMap[EXPANDER_A_IN_ADDR] = std::make_unique<PCA9555>();
    m_inMap[EXPANDER_A_IN_ADDR]->attach(m_i2c, EXPANDER_A_IN_ADDR);
    m_inMap[EXPANDER_B_IN_ADDR] = std::make_unique<PCA9555>();
    m_inMap[EXPANDER_B_IN_ADDR]->attach(m_i2c, EXPANDER_B_IN_ADDR);

    for (auto &[a, e] : m_inMap)
    {
        e->direction(PCA95x5::Direction::IN_ALL);
        e->polarity(PCA95x5::Polarity::ORIGINAL_ALL);
        m_lastInputState[a] = e->read(); /// initialize input state to collect interrupts
    };
}
ExternalIO::~ExternalIO() {

}

void ExternalIO::extDigitalWrite(const uint32_t mappedPin, const bool val)
{
    std::lock_guard<std::mutex> lock(m_i2cMutex);
    const io_t pa = map2pin(mappedPin);
    if (!m_outMap.contains(pa.addr))
    {
        LOG_ERROR("Undefined IO Expander addr: [", pa.addr, "]");
        return;
    }
    auto &io = m_outMap.at(pa.addr);
    if (!io->write(static_cast<PCA95x5::Port::Port>(pa.pin), val ? PCA95x5::Level::H : PCA95x5::Level::L))
    {
        LOG_ERROR("IO Expander [", pa.addr, "] Unable to WRITE Port [", pa.pin, "] to [", val ? "HIGH" : "LOW");
        LOG_ERROR("IO Expander Error [", io->i2c_error(), "]");
    }
}

const bool ExternalIO::extDigitalRead(const uint32_t mappedPin)
{
    std::lock_guard<std::mutex> lock(m_i2cMutex);
    const io_t pa = map2pin(mappedPin);
    if (!m_inMap.contains(pa.addr))
    {
        LOG_ERROR("Undefined IO Expander addr: [", pa.addr, "]");
        return false;
    }
    auto &io = m_inMap.at(pa.addr);
    const bool rv = io->read(static_cast<PCA95x5::Port::Port>(pa.pin)) == PCA95x5::Level::H ? true : false; // read value
    const uint8_t err = io->i2c_error();
    if (err)
    {
        LOG_ERROR("IO Expander [", pa.addr, "] Unable to READ Port [", pa.pin, "]");
        LOG_ERROR("IO Expander Error [", err, "]");
    }
    return rv;
}

void ExternalIO::extAttachInterrupt(ExtInterruptCb cb)
{
    attachInterruptArg(EXPANDER_ALL_INTERRUPT, onExpanderInterrupt, (void *)(this), FALLING);
    m_extInterruptCb = cb;
}

void ExternalIO::extDetachInterrupt()
{
    detachInterrupt(EXPANDER_ALL_INTERRUPT);
}

void ExternalIO::extReadInterrupt()
{
    std::lock_guard<std::mutex> lock(m_i2cMutex);
    disableInterrupt(EXPANDER_ALL_INTERRUPT);
    // read all registers and collect
    IOstate interruptState;
    for (auto &[a, e] : m_inMap)
    {
        interruptState[a] = e->read();
    }
    m_lastInputState = interruptState; // restore to current values
    // compare to last state to see the difference
    if (m_extInterruptCb)
    {
        for (auto &[a, v] : interruptState)
        {
            if (v)
                m_extInterruptCb(stat2map(a, v));
        }
    }

    enableInterrupt(EXPANDER_ALL_INTERRUPT);
}

const ExternalIO::io_t ExternalIO::map2pin(const uint32_t mappedIO)
{
    return io_t{
        .addr = (uint8_t)((mappedIO >> 16) & (uint8_t)0xFF),
        .pin = (uint8_t)(mappedIO && (uint32_t)0xFF),
    };
}

const uint32_t ExternalIO::stat2map(const uint8_t addr, const uint16_t stat)
{
    if (!stat)
        return 0;
    return (uint32_t)(addr << 16) | (1UL << __builtin_ctz(stat));
}