#include "pins.hpp"

#include "powerproc.hpp"

MCP23S08 MCP(SS, MISO, MOSI, SCK);

// Semaphores for handling interrupts and power down events.
volatile byte MCP_ISR_FLAG = 0;
volatile byte PERFORM_PROCEDURE_FROM_INTERRUPT = 0;

struct pins PINS;

void IRAM_ATTR isr() { MCP_ISR_FLAG = 1; }

void initializeMCP() {
    pinMode(ISR_PIN, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(ISR_PIN), isr, CHANGE);

    SPI.begin();
    MCP.begin();

    // Enable inputs and interrupts on pins 0-2
    MCP.pinMode8(0b00000111);
    MCP.enableInterrupt(0, CHANGE);
    MCP.enableInterrupt(1, CHANGE);
    MCP.enableInterrupt(2, CHANGE);

    // Inform SIB we're ready
    MCP.write1(ALERT_SIB_PIN, HIGH);

    Serial.println("MCP23S08 Started");
}

void isr_check_loop() {
    if (MCP_ISR_FLAG == 0) {
        return;
    }
    // False means triggered (not ok).
    PINS.sib = false;
    PINS.plc = false;
    PINS.ups = false;
    PINS.last_triggered = millis();

    // Get the pins status at the time of the interrupt, and check their statuses.
    Serial.print("Interrupt: ");
    uint8_t regval = MCP.getInterruptCaptureRegister();  // INTCAP
    Serial.print(regval, BIN);

    uint8_t mask = 1 << 0;
    if (!(regval & mask)) {
        Serial.print(" UPS");
        PINS.ups = true;
    }

    mask = 1 << 1;
    if (!(regval & mask)) {
        Serial.print(" PLC");
        PINS.plc = true;
    }

    mask = 1 << 2;
    if (!(regval & mask)) {
        Serial.print(" SIB");
        PINS.sib = true;
    }

    Serial.println();

    // This interrupt was handled
    MCP_ISR_FLAG = 0;

    // If not all ok, set the power down flag, later handled.
    if (!(PINS.sib && PINS.plc && PINS.ups)) {
        PERFORM_PROCEDURE_FROM_INTERRUPT = 1;
        PINS.canDoPowerFromREST = true;
    }
}

void poll_port_expander() {
    PINS.sib = false;
    PINS.plc = false;
    PINS.ups = false;
    int regval = MCP.read8();

    uint8_t mask = 1 << 0;
    if (!(regval & mask)) {
        Serial.print(" UPS");
        PINS.ups = true;
    }

    mask = 1 << 1;
    if (!(regval & mask)) {
        Serial.print(" PLC");
        PINS.plc = true;
    }

    mask = 1 << 2;
    if (!(regval & mask)) {
        Serial.print(" SIB");
        PINS.sib = true;
    }

    Serial.println();
}

bool on = true;
unsigned long toggle_start = 0;
void toggle_status_led() {
    if (millis() - toggle_start >= 1000) {
        toggle_start = millis();
        on = !on;
        MCP.write1(STATUS_LED_PIN, on);
    }
}

void powerOffCheckLoop() {
    if (!PERFORM_PROCEDURE_FROM_INTERRUPT) {
        return;
    }
    // Disable interrupts while performing power procedure.
    // Once we've been triggered, we perform the power down procedure, uninterrupted,
    // regardless of changes in the environment.
    detachInterrupt(digitalPinToInterrupt(ISR_PIN));

    // Update status on serial interface and status webpage.
    PINS.ramping = true;
    Serial.println("POWERING DOWN DUE TO TRIGGER.");

    // Inform SIB that we're about to perform a procedure.
    MCP.write1(ALERT_SIB_PIN, LOW);
    Serial.println("asked sib down");

    // Wait for SIB to acknowledge by checking its input (should go to triggered).
    /* This require hardware changes, not implemented yet.
    // TODO: apply a timeout and go ahead anyway?
    do {
        delay(200);
        poll_port_expander();
    } while(!(PINS.sib));
    Serial.println("SIB ACK");
     */

    // Iterate through each group and power down.
    String group;
    String groups;
    for (int8_t groupIdx = pproc.stagesCount - 1; groupIdx >= 0; groupIdx--) {
        group = pproc.stages[groupIdx].name;
        groups += group;
        groups += ",";
        PINS.stage = group;
        pproc.powerOff(group);
        delay(1000);
    }

    // Clear statuses on webpage and serial interfaces.
    PINS.ramping = false;
    Serial.print("Finished powering down from interrupt: ");
    Serial.println(groups);
    PINS.stage = "Ramped down";

    // This event has been handled.
    PERFORM_PROCEDURE_FROM_INTERRUPT = 0;
    // Inform SIB that we're done and it can take data again.
    MCP.write1(ALERT_SIB_PIN, HIGH);
    // Re-enable interrupts before exiting.
    attachInterrupt(digitalPinToInterrupt(ISR_PIN), isr, CHANGE);
}