// =============================================================================
//  AR-Autopilot v1 -- main.cpp
// =============================================================================
//
//  Sprint 1 firmware entry point. Boots in STANDBY mode, spawns the FreeRTOS
//  tasks declared in system/task_config.h, and returns control to the Arduino
//  scheduler. The loop() function intentionally does nothing -- every long-
//  running activity lives in a pinned task.
//
//  Task layout summary (full table in system/task_config.h):
//
//      Core 1 (real-time):  PID inner, PID outer, rudder sensor, safety
//      Core 0 (comms):      NMEA 2000 RX, Modbus slave, heartbeat, health
//
//  Sprint 1 boots only:
//      - heartbeat (Core 0)            -- LED + uptime log
//
//  Later sprints will register the rest from this same setup() function. The
//  task_config.h header already reserves their priorities/stacks/cores.
// =============================================================================

#include <Arduino.h>
#include <esp_system.h>

#include "hal/di_do.h"
#include "hal/knob_encoder.h"
#include "hal/nvs_config.h"
#include "hal/pinout.h"
#include "hal/rudder_actuator.h"
#include "hal/rudder_sensor.h"
#include "modes/standby.h"
#include "pid/pid_inner_task.h"
#include "pid/pid_outer_task.h"
#include "protocols/modbus_slave.h"
#include "protocols/nmea2000_consumer.h"
#include "protocols/nmea2000_publisher.h"
#include "safety/safety_monitor.h"
#include "safety/watchdog.h"
#include "system/ar_log.h"
#include "system/hwid.h"
#include "system/task_config.h"

// Forward declarations of task-spawning helpers (defined in their own .cpp
// files). Keeping main.cpp dependency-free of internals lets each subsystem
// own its own task setup code.
extern void ar_start_heartbeat_task();

namespace {
constexpr const char* TAG = "AR/MAIN";
}  // namespace

void setup() {
    // Wait for the USB CDC serial port to be ready before we start logging.
    Serial.begin(115200);
    delay(200);

    AR_LOGI(TAG, "================================================");
    AR_LOGI(TAG, "  %s -- firmware boot", AR_AUTOPILOT_FW_NAME);
    AR_LOGI(TAG, "  version : %s", AR_FW_VERSION);
    AR_LOGI(TAG, "  variant : %s", AR_BUILD_VARIANT);
    AR_LOGI(TAG, "  build   : %s", AR_FW_BUILD_TIMESTAMP);
    AR_LOGI(TAG, "  board   : %s", AR_AUTOPILOT_HW_BOARD);
    AR_LOGI(TAG, "  mcu     : %s @ %d MHz", AR_AUTOPILOT_MCU,
            (int)ESP.getCpuFreqMHz());
    AR_LOGI(TAG, "  cores   : %d  free heap: %u bytes",
            (int)ESP.getChipCores(), ESP.getFreeHeap());
    AR_LOGI(TAG, "================================================");

    // Sprint 8: log hardware ID (eFuse MAC) at boot for activation traceability.
    {
        uint8_t mac[6] = {};
        char mac_str[18] = {};
        if (arautopilot::system::hwid_get_mac(mac)) {
            arautopilot::system::hwid_format(mac, mac_str);
            AR_LOGI(TAG, "  HWID    : %s  (hash 0x%08X)",
                    mac_str, arautopilot::system::hwid_hash());
        } else {
            AR_LOGW(TAG, "  HWID    : eFuse read failed");
        }
    }

    // Initialise the pilot mode state machine (boots in STANDBY).
    arautopilot::modes::mode_init();

    // NVS (non-volatile config + calibration).  Must be first HAL call.
    arautopilot::hal::nvs_config_init();
    const auto cal = arautopilot::hal::nvs_config_load();
    if (cal.commissioned) {
        AR_LOGI(TAG, "commissioned unit: adc %d..%d  outer Kp=%.3f",
                cal.adc_port, cal.adc_stbd, cal.outer_kp);
    } else {
        AR_LOGW(TAG, "not commissioned -- using factory defaults");
    }

    // Initialise hardware abstraction layer.
    arautopilot::hal::di_init();
    arautopilot::hal::do_init();
    arautopilot::hal::rudder_sensor_init();
    if (cal.commissioned) {
        arautopilot::hal::rudder_sensor_set_calibration(
            cal.adc_port, cal.adc_stbd,
            cal.rudder_port_deg, cal.rudder_stbd_deg);
    }
    arautopilot::hal::rudder_actuator_init();
    arautopilot::hal::knob_encoder_init();

    // Safety: watchdog + DI override task. Initialised first so the
    // watchdog is armed before slower subsystems come up.
    arautopilot::safety::watchdog_init();

    // Sprint 6: if the last reset was a task-watchdog trip, set the sticky
    // alarm bit so the display knows the MCU was force-reset by the WDT.
    if (esp_reset_reason() == ESP_RST_TASK_WDT) {
        arautopilot::safety::safety_set_watchdog_tripped();
    }

    AR_LOGI(TAG, "spawning Sprint 1 tasks ...");

    arautopilot::safety::safety_monitor_start_task();
    arautopilot::hal::rudder_sensor_start_task();
    arautopilot::hal::knob_encoder_start_task();

    // PID inner loop (Sprint 2). Active only when not in STANDBY and
    // rudder sensor reading is valid; refuses internally otherwise.
    arautopilot::pid::pid_inner_task_init();
    arautopilot::pid::pid_inner_task_start();

    // PID outer loop (Sprint 3) -- Heading Hold.
    arautopilot::pid::pid_outer_task_init();
    if (cal.commissioned && cal.outer_kp > 0.0f) {
        arautopilot::pid::pid_outer_update_gains(
            cal.outer_kp, cal.outer_ki, cal.outer_kd);
        AR_LOGI(TAG, "applied commissioned outer gains Kp=%.3f Ki=%.3f Kd=%.3f",
                cal.outer_kp, cal.outer_ki, cal.outer_kd);
    }
    arautopilot::pid::pid_outer_task_start();

    ar_start_heartbeat_task();

    // Modbus slave (server) -- exposes telemetry + commands to the display.
    arautopilot::protocols::modbus::modbus_slave_init();
    arautopilot::protocols::modbus::modbus_slave_start();

    // NMEA 2000 consumer (PGN 127250 + 127251 in Sprint 1; 129026/129284 in Sprint 5).
    arautopilot::protocols::nmea2000::nmea2000_consumer_init();
    arautopilot::protocols::nmea2000::nmea2000_consumer_start_task();

    // NMEA 2000 publisher (Sprint 6): PGN 127245 rudder + PGN 127237 HTC.
    // Must be started AFTER nmea2000_consumer_init() (shared CAN stack).
    arautopilot::protocols::nmea2000::nmea2000_publisher_start_task();

    AR_LOGI(TAG, "setup() complete; control loop is FreeRTOS-driven.");
    AR_LOGI(TAG, "current mode: STANDBY (helm is manual)");
}

void loop() {
    // Intentionally empty. All real work happens in FreeRTOS tasks.
    // The Arduino loopTask itself runs at priority 1 on Core 1 by default;
    // we yield generously to keep it out of the way of the real-time tasks.
    vTaskDelay(pdMS_TO_TICKS(1000));
}
