#include <DigitalOutputCard.hpp>

DigitalOutputCard::DigitalOutputCard(uint8_t address) : change_callbacks(){
    this->address = address;
    // load default pin map
    for (int i = 0; i < 16; i++) {
        this->pinMap[i] = i;
        this->virtualPinMap[i] = i;
    }
    this->framBinded = false;
    this->callbacks_handler_index = 0;
}
// Instantiate the card with the specified position on the dip switch
DigitalOutputCard::DigitalOutputCard(bool bit0, bool bit1, bool bit2, bool bit3, bool bit4) :
    DigitalOutputCard(0x20+bit0+bit1*2+bit2*4+bit3*8+bit4*16)
{

    
}
// Initialize the card
bool DigitalOutputCard::begin() {
    this->pwm = Adafruit_PWMServoDriver(this->address);
    this->pwm.begin();
    pwm.setOutputMode(true);
    // Output card don't send ack, we can't check if it's connected
    // so we just return true
    return true;
}
// Set the output to the specified state
void DigitalOutputCard::digitalWrite(uint8_t pin, bool state) {
    this->pwm.setPin(virtualPinMap[pin], state ? 4095 : 0);
    this->state_buffer[pin] = state;
    this->value_buffer[pin] = state ? 4095 : 0;
    if (this->framAutoSave) {
        this->saveStateToFRAM();
        this->savePinValueToFRAM(pin);
    }
    for (const auto& callback : change_callbacks)
    {
        callback.second(pin, state, state ? 4095 : 0);
    }
}
// Set the output to the specified pwm value
void DigitalOutputCard::analogWrite(uint8_t pin, uint16_t value) {
    // If value is greater than 4095, set it to 4095
    if (value > 4095) value = 4095;
    // Set the pwm value
    this->pwm.setPin(virtualPinMap[pin], value);
    if (this->framAutoSave) {
        this->saveStateToFRAM();
        this->savePinValueToFRAM(pin);
    }
    this->state_buffer[pin] = value > 0;
    this->value_buffer[pin] = value;
    for (const auto& callback : change_callbacks)
    {
        callback.second(pin, value > 0, value);
    }
}

// Dummy loop function
void DigitalOutputCard::loop() {
}

// Get the state of the specified pin
bool DigitalOutputCard::getState(uint8_t pin) {
    return this->state_buffer[pin];
}

// Get the pwm value of the specified pin
uint16_t DigitalOutputCard::getValue(uint8_t pin) {
    return this->value_buffer[pin];
}

// Get type of card
uint8_t DigitalOutputCard::getType() {
    return CARD_TYPE_DIGITAL_OUTPUT;
}

void DigitalOutputCard::setState(uint8_t pin, bool state) {
    this-> state_buffer[pin] = state;
    this->pwm.setPin(pin, state*value_buffer[pin]);
    if(this->framAutoSave) {
        this->saveStateToFRAM();
    }
    for(const auto& callback : change_callbacks) {
        callback.second(pin, state, value_buffer[pin]);
    }
}

void DigitalOutputCard::setValue(uint8_t pin, uint16_t value) {
    // If value is greater than 4095, set it to 4095
    if (value > 4095) value = 4095;
    this-> value_buffer[pin] = value;
    this->pwm.setPin(pin, state_buffer[pin]*value);
    if (this->framAutoSave) {
        this->savePinValueToFRAM(pin);
    }
    for (const auto& callback : change_callbacks)
    {
        callback.second(pin, state_buffer[pin], value);
    }
}

uint8_t DigitalOutputCard::registerChangeCallback(std::function<void(uint8_t, bool, uint16_t)> callback) {
    this->change_callbacks[this->callbacks_handler_index] = callback;
    return this->callbacks_handler_index++;
}

void DigitalOutputCard::unregisterChangeCallback(uint8_t handler) {
    this->change_callbacks.erase(handler);
}

void DigitalOutputCard::loadPinMap(uint8_t pinMap[16]) {
    for(int i = 0; i < 16; i++) {
        this->pinMap[i] = pinMap[i];
        this->virtualPinMap[pinMap[i]] = i;
    }
}

void DigitalOutputCard::bindFRAM(FRAM *fram, uint16_t address) {
    this->fram = fram;
    this->framBinded = true;
    this->framAddress = address;
}

uint16_t DigitalOutputCard::packStates() {
    uint16_t packed = 0;
    for(int i = 0; i < 16; i++) {
        packed |= (state_buffer[i] << i);
    }
    return packed;
}

void DigitalOutputCard::unpackStates(uint16_t states) {
    for(int i = 0; i < 16; i++) {
        this->setState(i, (states >> i) & 1);
    }
}

void DigitalOutputCard::saveToFRAM() {
    if(!framBinded) return;
    // Save the state
    uint16_t packed = packStates();
    this->fram->write16(framAddress, packed);
    // Save the value
    this->fram->write(framAddress+2, (uint8_t*)value_buffer, 32);
}

void DigitalOutputCard::loadFromFRAM() {
    if(!framBinded) return;
    // Load the state
    uint16_t packed = this->fram->read16(framAddress);
    unpackStates(packed);
    // Load the value
    uint16_t value[16];
    this->fram->read(framAddress+2, (uint8_t*)value, 32);
    for(int i = 0; i < 16; i++) {
        this->setValue(i, value[i]);
    }
}

void DigitalOutputCard::setAutoSaveToFRAM(bool autoSave) {
    this->framAutoSave = autoSave;
}

void DigitalOutputCard::savePinValueToFRAM(uint8_t pin) {
    if(!framBinded) return;
    this->fram->write(framAddress+2+pin*2, (uint8_t*)&value_buffer[pin], 2);
}

void DigitalOutputCard::saveStateToFRAM() {
    if(!framBinded) return;
    uint16_t packed = packStates();
    this->fram->write16(framAddress, packed);
}