// =============================================================================
//  nmea2000_consumer.cpp -- NMEA 2000 backbone consumer
// =============================================================================

#include "nmea2000_consumer.h"

#include <Arduino.h>

// Override the default ESP32 CAN pins (16/4) to match our AR-NMEA-IO board.
#include "../hal/pinout.h"
#define ESP32_CAN_TX_PIN ((gpio_num_t)PIN_CAN_TX)
#define ESP32_CAN_RX_PIN ((gpio_num_t)PIN_CAN_RX)

#include <NMEA2000_CAN.h>
#include <N2kMessages.h>

#include "../system/ar_log.h"
#include "../system/task_config.h"

namespace arautopilot::protocols::nmea2000 {

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

constexpr uint32_t STALE_THRESHOLD_MS = 5000;

portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;

HeadingSnapshot g_snap{
    .heading_deg = 0.0f,
    .is_true = false,
    .rate_of_turn_dps = 0.0f,
    .heading_age_ms = 0,
    .rot_age_ms = 0,
    .heading_valid = false,
    .rot_valid = false,
};

CogSogSnapshot g_cog_sog{
    .cog_deg = NAN,
    .sog_kn  = NAN,
    .age_ms  = 0,
    .valid   = false,
};

NavDataSnapshot g_nav_data{
    .xte_m    = NAN,
    .dtw_m    = NAN,
    .wp_name  = {},
    .age_ms   = 0,
    .valid    = false,
};

HeadingControlSnapshot g_htc{
    .commanded_heading_deg = NAN,
    .mode        = HtcMode::UNKNOWN,
    .source_addr = 0xFF,
    .age_ms      = 0,
    .valid       = false,
};

// Our own NMEA2000 source address — filter out self-echoes.
constexpr uint8_t OWN_SOURCE_ADDR = 25;

float rad_to_deg_pos(float rad) {
    float d = rad * (180.0f / (float)M_PI);
    // Normalise to 0..360.
    while (d < 0.0f) d += 360.0f;
    while (d >= 360.0f) d -= 360.0f;
    return d;
}

float rad_to_deg_signed(float rad) {
    return rad * (180.0f / (float)M_PI);
}

void HandleHeading(const tN2kMsg& msg) {
    unsigned char sid;
    double heading = 0.0, deviation = 0.0, variation = 0.0;
    tN2kHeadingReference ref = N2khr_Unavailable;
    if (!ParseN2kHeading(msg, sid, heading, deviation, variation, ref)) {
        return;
    }
    if (heading > 1e8) {
        // N2k "unavailable" markers come through as huge doubles.
        return;
    }
    const float heading_deg = rad_to_deg_pos((float)heading);
    const bool is_true = (ref == N2khr_true);
    const uint32_t now = millis();
    portENTER_CRITICAL(&g_mux);
    g_snap.heading_deg = heading_deg;
    g_snap.is_true = is_true;
    g_snap.heading_age_ms = now;
    g_snap.heading_valid = true;
    portEXIT_CRITICAL(&g_mux);
    AR_LOGV(TAG, "PGN 127250 heading=%.2f deg (%s)", heading_deg,
            is_true ? "true" : "magnetic");
}

void HandleROT(const tN2kMsg& msg) {
    unsigned char sid;
    double rot = 0.0;
    if (!ParseN2kRateOfTurn(msg, sid, rot)) {
        return;
    }
    if (rot > 1e8 || rot < -1e8) {
        return;
    }
    const float rot_dps = rad_to_deg_signed((float)rot);
    const uint32_t now = millis();
    portENTER_CRITICAL(&g_mux);
    g_snap.rate_of_turn_dps = rot_dps;
    g_snap.rot_age_ms = now;
    g_snap.rot_valid = true;
    portEXIT_CRITICAL(&g_mux);
    AR_LOGV(TAG, "PGN 127251 ROT=%.3f deg/s", rot_dps);
}

void HandleCogSog(const tN2kMsg& msg) {
    unsigned char sid;
    tN2kHeadingReference ref;
    double cog = 0.0, sog = 0.0;
    if (!ParseN2kCOGSOGRapid(msg, sid, ref, cog, sog)) return;
    if (cog > 1e8 || sog > 1e8) return;
    const float cog_deg = rad_to_deg_pos((float)cog);
    const float sog_kn  = (float)(sog * 1.94384);  // m/s → knots
    const uint32_t now  = millis();
    portENTER_CRITICAL(&g_mux);
    g_cog_sog.cog_deg = cog_deg;
    g_cog_sog.sog_kn  = sog_kn;
    g_cog_sog.age_ms  = now;
    g_cog_sog.valid   = true;
    portEXIT_CRITICAL(&g_mux);
    AR_LOGV(TAG, "PGN 129026 COG=%.2f deg SOG=%.2f kn", cog_deg, sog_kn);
}

void HandleNavData(const tN2kMsg& msg) {
    unsigned char sid;
    double dist = 0.0, xte = 0.0;
    double origLat = 0.0, origLon = 0.0, destLat = 0.0, destLon = 0.0;
    double closingVel = 0.0;
    tN2kHeadingReference bearingRef = N2khr_Unavailable;
    tN2kDistanceCalculationType calcType = N2kdct_GreatCircle;
    bool perpCrossed = false, arrivalAlarm = false;
    int16_t origWpNum = 0;
    uint32_t destWpNum = 0, eta = 0;

    if (!ParseN2kNavigationInfo(
            msg, sid, dist, bearingRef,
            perpCrossed, arrivalAlarm, calcType, xte,
            origWpNum, origLat, origLon,
            destWpNum, eta, destLat, destLon, closingVel)) {
        return;
    }
    if (xte > 1e8 || xte < -1e8) return;
    const float xte_m = (float)xte;
    const float dtw_m = (dist < 1e8) ? (float)dist : NAN;
    const uint32_t now = millis();
    portENTER_CRITICAL(&g_mux);
    g_nav_data.xte_m  = xte_m;
    g_nav_data.dtw_m  = dtw_m;
    g_nav_data.age_ms = now;
    g_nav_data.valid  = true;
    portEXIT_CRITICAL(&g_mux);
    AR_LOGV(TAG, "PGN 129284 XTE=%.2f m DTW=%.0f m", xte_m, dtw_m);
}

void HandleHeadingControl(const tN2kMsg& msg) {
    // Ignore our own broadcasts — concentrador uses a different source address.
    if (msg.Source == OWN_SOURCE_ADDR) return;

    tN2kOnOff rud_lim, off_hdg, off_trk, override_st;
    tN2kSteeringMode steering_mode;
    tN2kTurnMode turn_mode;
    tN2kHeadingReference hdg_ref;
    tN2kRudderDirectionOrder cmd_rud_dir;
    double cmd_rud_angle = N2kDoubleNA;
    double hdg_to_steer  = N2kDoubleNA;
    double track         = N2kDoubleNA;
    double rud_limit     = N2kDoubleNA;
    double off_hdg_limit = N2kDoubleNA;
    double radius        = N2kDoubleNA;
    double rot_order     = N2kDoubleNA;
    double xte_lim       = N2kDoubleNA;
    double vessel_hdg    = N2kDoubleNA;

    if (!ParseN2kHeadingTrackControl(msg,
            rud_lim, off_hdg, off_trk, override_st,
            steering_mode, turn_mode, hdg_ref,
            cmd_rud_dir, cmd_rud_angle,
            hdg_to_steer, track,
            rud_limit, off_hdg_limit, radius, rot_order,
            xte_lim, vessel_hdg)) {
        return;
    }

    // Map NMEA2000 steering mode to our internal enum.
    HtcMode our_mode;
    switch (steering_mode) {
        case N2kSM_HeadingControl: our_mode = HtcMode::HEADING_HOLD; break;
        case N2kSM_MainSteering:   our_mode = HtcMode::STANDBY;      break;
        default:                   return;  // ignore unsupported modes
    }

    const float hdg_deg = (hdg_to_steer < 1e8)
        ? rad_to_deg_pos((float)hdg_to_steer)
        : NAN;

    const uint32_t now = millis();
    portENTER_CRITICAL(&g_mux);
    g_htc.commanded_heading_deg = hdg_deg;
    g_htc.mode        = our_mode;
    g_htc.source_addr = msg.Source;
    g_htc.age_ms      = now;
    g_htc.valid       = true;
    portEXIT_CRITICAL(&g_mux);

    AR_LOGI(TAG, "PGN 127237 src=%d mode=%d hdg=%.2f deg",
            msg.Source, (int)our_mode, hdg_deg);
}

void MessageHandler(const tN2kMsg& msg) {
    switch (msg.PGN) {
        case 127237L: HandleHeadingControl(msg); break;
        case 127250L: HandleHeading(msg);        break;
        case 127251L: HandleROT(msg);            break;
        case 129026L: HandleCogSog(msg);         break;
        case 129284L: HandleNavData(msg);        break;
        default: break;
    }
}

void RxTask(void* /*pv*/) {
    AR_LOGI(TAG, "nmea2000_consumer task started on core %d", xPortGetCoreID());
    for (;;) {
        NMEA2000.ParseMessages();
        const uint32_t now = millis();
        portENTER_CRITICAL(&g_mux);
        if (g_snap.heading_valid && (now - g_snap.heading_age_ms) > STALE_THRESHOLD_MS) {
            g_snap.heading_valid = false;
        }
        if (g_snap.rot_valid && (now - g_snap.rot_age_ms) > STALE_THRESHOLD_MS) {
            g_snap.rot_valid = false;
        }
        portEXIT_CRITICAL(&g_mux);
        // Update COG/SOG and nav-data stale flags.
        portENTER_CRITICAL(&g_mux);
        if (g_cog_sog.valid && (now - g_cog_sog.age_ms) > STALE_THRESHOLD_MS) {
            g_cog_sog.valid = false;
        }
        if (g_nav_data.valid && (now - g_nav_data.age_ms) > STALE_THRESHOLD_MS) {
            g_nav_data.valid = false;
        }
        // HTC commands have a shorter stale window (3 s) — if the remote stops
        // sending, we don't want to keep acting on an old command.
        if (g_htc.valid && (now - g_htc.age_ms) > 3000U) {
            g_htc.valid = false;
        }
        portEXIT_CRITICAL(&g_mux);
        // 100 Hz polling is plenty -- the CAN driver buffers incoming frames.
        vTaskDelay(pdMS_TO_TICKS(10));
    }
}

}  // namespace

void nmea2000_consumer_init() {
    NMEA2000.SetProductInformation(
        "AR-AP-1",                                 // Manufacturer's Model serial code
        100,                                        // Manufacturer's product code
        "AR-Autopilot Controller v1",               // Model ID
        AR_FW_VERSION,                              // SW version
        "AR-NMEA-IO v1.0"                            // Model version
    );
    NMEA2000.SetDeviceInformation(
        1,                          // Unique number
        150,                        // Device function: Autopilot
        40,                         // Device class: Steering and Control surfaces
        2046                        // Manufacturer code: reserved/test (real one needs NMEA registration)
    );
    NMEA2000.SetMode(tNMEA2000::N2km_ListenAndNode, 25);
    NMEA2000.EnableForward(false);
    NMEA2000.SetMsgHandler(&MessageHandler);

    if (NMEA2000.Open()) {
        AR_LOGI(TAG,
                "nmea2000_consumer_init: stack open, CAN TX=%d RX=%d, "
                "subscribed to PGN 127250 + 127251",
                PIN_CAN_TX, PIN_CAN_RX);
    } else {
        AR_LOGE(TAG,
                "nmea2000_consumer_init: NMEA2000.Open() failed (CAN driver "
                "init error). System will continue but no N2K traffic.");
    }
}

void nmea2000_consumer_start_task() {
    xTaskCreatePinnedToCore(RxTask, "n2k_rx", AR_TASK_STACK_N2K_RX, nullptr,
                            AR_TASK_PRIO_N2K_RX, nullptr, AR_TASK_CORE_COMMS);
}

HeadingSnapshot nmea2000_latest() {
    HeadingSnapshot copy;
    portENTER_CRITICAL(&g_mux);
    copy = g_snap;
    portEXIT_CRITICAL(&g_mux);
    return copy;
}

CogSogSnapshot nmea2000_cog_sog() {
    CogSogSnapshot copy;
    portENTER_CRITICAL(&g_mux);
    copy = g_cog_sog;
    portEXIT_CRITICAL(&g_mux);
    return copy;
}

NavDataSnapshot nmea2000_nav_data() {
    NavDataSnapshot copy;
    portENTER_CRITICAL(&g_mux);
    copy = g_nav_data;
    portEXIT_CRITICAL(&g_mux);
    return copy;
}

bool nmea2000_is_stale() {
    const auto s = nmea2000_latest();
    return !s.heading_valid;
}

bool nmea2000_cog_is_stale() {
    return !nmea2000_cog_sog().valid;
}

HeadingControlSnapshot nmea2000_htc() {
    HeadingControlSnapshot copy;
    portENTER_CRITICAL(&g_mux);
    copy = g_htc;
    portEXIT_CRITICAL(&g_mux);
    return copy;
}

bool nmea2000_htc_is_stale() {
    return !nmea2000_htc().valid;
}

}  // namespace arautopilot::protocols::nmea2000