#include "esp32_ethernet.hpp"
#include "mpod.hpp"
#include "pins.hpp"
#include "rest.hpp"

unsigned long start;
extern bool eth_connected;
extern SNMP::Manager snmp;

TaskHandle_t restServerTask;
TaskHandle_t powerProcedureTask;

void setup() {
    Serial.begin(115200);

    initializeMCP();
    initializeNetwork();
    initializeRoutes();
    initializeSNMP();

    // Poll port expander to initialize the PINS struct.
    poll_port_expander();
    PINS.last_triggered = millis();
    PINS.stage = "";
    PINS.ramping = false;
    PINS.canDoPowerFromREST = false;

    start = millis();

    // Run the web interface on core 0
    xTaskCreatePinnedToCore(webInterfaceLoop, "Web Interface", 10000, NULL, 1, &restServerTask, 0);
    // Run the power procedure and interrupt handling on core 1, the default core
    xTaskCreatePinnedToCore(mainLoop, "PowerProcedure Task", 10000, NULL, 1, &powerProcedureTask,
                            1);
}

enum {
    NONE,
    ON,
    OFF,
};

void serialLoop() {
    if (millis() - start >= 1000) {
        start = millis();
        Serial.print(eth_connected ? "." : "-");
    }
}

void waitEnvOk() {
    // Poll port expander on initialiation to wait until environment all ok
    // If the ICBM is booted before the detector is up, as it should be,
    // then the environment will always be incorrect, creating a power down
    PINS.stage = "Waiting for environment to be okay first time";

    uint32_t lastIteration = millis();
    while (true) {
        serialLoop();
        if (millis() - lastIteration >= 200) {  // ms, faster polling makes for erroneous readings.
            poll_port_expander();
            if (PINS.sib && PINS.plc && PINS.ups) {
                break;
            }
            lastIteration = millis();
        }
    }

    PINS.stage = "";
    PINS.canDoPowerFromREST = true;
}

void webInterfaceLoop(void* pvParameters) {
    Serial.print("Web loop running on core ");
    Serial.println(xPortGetCoreID());
    while (true) {
        restServer.handleClient();
    }
}

void mainLoop(void* pvParameters) {
    Serial.print("Main loop running on core ");
    Serial.println(xPortGetCoreID());

    waitEnvOk();

    while (true) {
        isr_check_loop();
        snmp.loop();
        toggle_status_led();
        serialLoop();
        if (eth_connected) {  // We may be triggered, but disconnected from the network.
            powerOffCheckLoop();
        }
    }
}

void loop() {}