#include <DigitalInputCard.hpp>

// Instantiate the card with the specified address
DigitalInputCard::DigitalInputCard(uint8_t address_a, uint8_t address_b) : callbacks()
{
    this->address_a = address_a;
    this->address_b = address_b;
    this->callbacks_handler_index = 0;
}
// Instantiate the card with the specified position on the dip switch
// Bit 0,1,2 are for bank A
// Bit 3,4,5 are for bank B
DigitalInputCard::DigitalInputCard(bool bit0, bool bit1, bool bit2, bool bit3, bool bit4, bool bit5)
{
    this->address_a = 0x20;
    this->address_b = 0x20;
    this->inputBufferA = 0;
    this->inputBufferB = 0;
    if (bit0)
        this->address_a += 1;
    if (bit1)
        this->address_a += 2;
    if (bit2)
        this->address_a += 4;
    if (bit3)
        this->address_b += 1;
    if (bit4)
        this->address_b += 2;
    if (bit5)
        this->address_b += 4;
}
// Initialize the card
bool DigitalInputCard::begin()
{
    this->inputBankA = PCF8574(this->address_a);
    this->inputBankB = PCF8574(this->address_b);
    if (!this->inputBankA.begin()) {
        ESP_LOGE("DigitalInputCard", "Input Card ERROR: Failed to install input bank A");
        return false;
    }
    if (!this->inputBankB.begin()) {
        ESP_LOGE("DigitalInputCard", "Input Card ERROR: Failed to install input bank B");
        return false;
    }
    // Set the debounce time for all pins to 50ms
    for (int i = 0; i < 16; i++)
    {
        this->debounceTime[i] = 50;
        this->lastDebounceTime[i] = 0;
    }
    // Initialize the pin map to the default values
    for (int i = 0; i < 16; i++)
    {
        this->pinMap[i] = i;
        this->virtualPinMap[i] = i;
    }
    return true;
}
// Refresh and Read the input from the specified pin, always refresh the input buffers
bool DigitalInputCard::digitalRead(uint8_t pin)
{
    return this->digitalRead(pin, true);
}
// Read the input from the specified pin, also refresh the input buffers if refresh is true
bool DigitalInputCard::digitalRead(uint8_t pin, bool refresh)
{
    pin = pinMap[pin];
    // First check if the pin is in bank A or B
    if (pin >= 0 && pin <= 7)
    {
        // Refresh the input buffers if refresh is true
        if (refresh)
            refreshInputBankA();
        // Extract the bit from the buffer
        return ((inputBufferA >> (7 - pin)) & 1);
    }
    else if (pin >= 8 && pin <= 15)
    {
        // Refresh the input buffers if refresh is true
        if (refresh)
            refreshInputBankB();
        // Extract the bit from the buffer
        return ((inputBufferB >> (15 - pin)) & 1);
    }
    return 255;
}

void DigitalInputCard::handlePinChange(int pin, uint8_t &currentBuffer, uint8_t &previousBuffer)
{
    // Get the index of the pin in the pin map
    uint8_t virtualPin = virtualPinMap[pin];
    // Handle Bank A
    if (((previousBuffer >> (7 - pin)) & 1) != ((currentBuffer >> (7 - pin)) & 1))
    {
        if (millis() - lastDebounceTime[pin] > debounceTime[pin])
        {
            lastDebounceTime[pin] = millis();
            previousBuffer ^= (-((currentBuffer >> (7 - pin)) & 1) ^ previousBuffer) & (1UL << (7 - pin));
            for(const auto& callback : callbacks)
                callback.second(virtualPin, ((currentBuffer >> (7 - pin)) & 1));
        }
    }
    // Handle Bank B
    if (((previousBuffer >> (15 - pin)) & 1) != ((currentBuffer >> (15 - pin)) & 1))
    {
        if (millis() - lastDebounceTime[pin] > debounceTime[pin])
        {
            lastDebounceTime[pin] = millis();
            previousBuffer ^= (-((currentBuffer >> (15 - pin)) & 1) ^ previousBuffer) & (1UL << (15 - pin));
            for (const auto& callback : callbacks)
                callback.second(virtualPin, ((currentBuffer >> (15 - pin)) & 1));
        }
    }
}


// Preform a loop to refresh the input buffers
void DigitalInputCard::loop()
{
    // Refresh the input buffers
    refreshInputBankA();
    refreshInputBankB();
    // Iterate over all pins and check if they changed
    for (int i = 0; i < 16; i++)
    {
        // Check which bank the pin is in
        if (i < 8)
        {
            handlePinChange(i, inputBufferA, previousInputBufferA);
        }
        else if (i >= 8 && i <= 15)
        {
            handlePinChange(i, inputBufferB, previousInputBufferB);
        }
    }
}
// Get the input buffer for bank A
uint8_t DigitalInputCard::getInputBufferA()
{
    // Rearrange the bits to match the pin map
    uint8_t inputBufferA_rearranged = 0;
    for (int i = 0; i < 8; i++)
    {
        inputBufferA_rearranged |= ((inputBufferA >> i) & 1) << (7 - i);
    }
    return inputBufferA_rearranged;
}
// Get the input buffer for bank B
uint8_t DigitalInputCard::getInputBufferB()
{
    // Rearrange the bits to match the pin map
    uint8_t inputBufferB_rearranged = 0;
    for (int i = 0; i < 8; i++)
    {
        inputBufferB_rearranged |= ((inputBufferB >> i) & 1) << (7 - i);
    }
    return inputBufferB_rearranged;
}
// Register a callback function to be called when a pin changes
void DigitalInputCard::registerCallback(std::function<void(uint8_t, bool)> callback)
{
    callbacks[this->callbacks_handler_index++] = callback;
}

// Refresh the input buffer for bank A
void DigitalInputCard::refreshInputBankA()
{
    inputBufferA = inputBankA.read8();
}
// Refresh the input buffer for bank B
void DigitalInputCard::refreshInputBankB()
{
    inputBufferB = inputBankB.read8();
}

void DigitalInputCard::setDebounceTime(uint8_t pin, uint32_t debounceTime)
{
    pin = pinMap[pin];
    this->debounceTime[pin] = debounceTime;
}

void DigitalInputCard::unregisterCallback(uint8_t handler)
{
    callbacks.erase(handler);
}

void DigitalInputCard::loadPinMap(uint8_t pinMap[16])
{
    for (int i = 0; i < 16; i++)
    {
        // Load the pin map (physical pin to virtual pin)
        this->pinMap[i] = pinMap[i];
        // Load the virtual pin map (virtual pin to physical pin)
        this->virtualPinMap[pinMap[i]] = i;
    }
}

uint8_t DigitalInputCard::getType()
{
    return CARD_TYPE_DIGITAL_INPUT;
}