// =============================================================================
//  modes/standby.cpp -- pilot mode state (Sprint 1)
// =============================================================================

#include "standby.h"

#include <Arduino.h>

#include "../hal/rudder_sensor.h"
#include "../pid/pid_outer_task.h"
#include "../protocols/nmea2000_consumer.h"
#include "../system/ar_log.h"

namespace arautopilot::modes {

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

portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
Mode g_mode = Mode::STANDBY;

const char* mode_name(Mode m) {
    switch (m) {
        case Mode::STANDBY:       return "STANDBY";
        case Mode::HEADING_HOLD:  return "HEADING_HOLD";
        case Mode::TRUE_COURSE:   return "TRUE_COURSE";
        case Mode::TRACK_KEEPING: return "TRACK_KEEPING";
        case Mode::DODGE:         return "DODGE";
    }
    return "UNKNOWN";
}
}  // namespace

void mode_init() {
    portENTER_CRITICAL(&g_mux);
    g_mode = Mode::STANDBY;
    portEXIT_CRITICAL(&g_mux);
    AR_LOGI(TAG, "mode_init: %s", mode_name(g_mode));
}

Mode current_mode() {
    portENTER_CRITICAL(&g_mux);
    Mode m = g_mode;
    portEXIT_CRITICAL(&g_mux);
    return m;
}

bool is_standby() { return current_mode() == Mode::STANDBY; }

bool request_mode(Mode m) {
    if (m == Mode::STANDBY) {
        portENTER_CRITICAL(&g_mux);
        Mode prev = g_mode;
        g_mode = Mode::STANDBY;
        portEXIT_CRITICAL(&g_mux);
        if (prev != Mode::STANDBY) {
            AR_LOGI(TAG, "mode change: %s -> STANDBY", mode_name(prev));
        }
        return true;
    }

    // ----- HEADING_HOLD activation (Sprint 3) -----
    if (m == Mode::HEADING_HOLD) {
        const auto n2k = protocols::nmea2000::nmea2000_latest();
        const auto rd = hal::rudder_sensor_latest();
        if (!n2k.heading_valid) {
            AR_LOGW(TAG, "request_mode(HEADING_HOLD) rejected: heading sensor "
                          "not valid (no fresh PGN 127250)");
            return false;
        }
        if (!rd.valid) {
            AR_LOGW(TAG, "request_mode(HEADING_HOLD) rejected: rudder sensor "
                          "not valid yet (median filter still filling)");
            return false;
        }
        // Capture the current heading as the initial setpoint so the
        // operator doesn't get a sudden swing toward whatever was in
        // the holding register beforehand.
        pid::pid_outer_set_heading_setpoint_deg(n2k.heading_deg);

        portENTER_CRITICAL(&g_mux);
        Mode prev = g_mode;
        g_mode = Mode::HEADING_HOLD;
        portEXIT_CRITICAL(&g_mux);
        AR_LOGI(TAG, "mode change: %s -> HEADING_HOLD (setpoint captured at %.1f deg)",
                mode_name(prev), n2k.heading_deg);
        return true;
    }

    // TRUE_COURSE / TRACK_KEEPING / DODGE: Sprint 5.
    AR_LOGW(TAG,
            "request_mode(%s) rejected: mode not yet implemented "
            "(Sprint 5+); current mode remains %s",
            mode_name(m), mode_name(current_mode()));
    return false;
}

void force_standby(const char* reason) {
    portENTER_CRITICAL(&g_mux);
    Mode prev = g_mode;
    g_mode = Mode::STANDBY;
    portEXIT_CRITICAL(&g_mux);
    if (prev != Mode::STANDBY) {
        AR_LOGE(TAG, "force_standby: %s -> STANDBY (reason: %s)",
                mode_name(prev), reason != nullptr ? reason : "unspecified");
    }
}

}  // namespace arautopilot::modes
