// =============================================================================
//  protocols/n2k_bridge.cpp -- NMEA 2000 <-> NMEA 0183 conversion
// =============================================================================

#include "n2k_bridge.h"

#include <Arduino.h>
#include <NMEA2000_CAN.h>
#include <N2kMessages.h>
#include <cstring>
#include <cstdio>
#include <cmath>

namespace arconcentrador::protocols {

namespace {

// ---------------------------------------------------------------------------
//  Shared state (updated by PGN handlers, read by main loop)
// ---------------------------------------------------------------------------
static float    g_heading_deg  = 0.0f;
static bool     g_heading_valid = false;
static float    g_rot_dps      = 0.0f;
static float    g_sog_kn       = NAN;
static float    g_cog_deg      = NAN;
static float    g_depth_m      = NAN;
static float    g_water_temp_c = NAN;

static ApStatus g_ap_status{};

static portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;

// ---------------------------------------------------------------------------
//  Helpers
// ---------------------------------------------------------------------------
static float rad2deg_pos(double rad) {
    float d = (float)(rad * 180.0 / M_PI);
    while (d < 0.0f)    d += 360.0f;
    while (d >= 360.0f) d -= 360.0f;
    return d;
}

static uint8_t nmea_crc(const char* body) {
    uint8_t crc = 0;
    for (; *body; ++body) crc ^= (uint8_t)*body;
    return crc;
}

static int write_sentence(char* buf, size_t len, const char* body) {
    char tmp[256];
    snprintf(tmp, sizeof(tmp), "%s", body);
    const uint8_t crc = nmea_crc(tmp);
    return snprintf(buf, len, "$%s*%02X\r\n", body, crc);
}

// ---------------------------------------------------------------------------
//  PGN handlers (called from NMEA2000.ParseMessages())
// ---------------------------------------------------------------------------
static void on_heading(const tN2kMsg& msg) {
    unsigned char sid;
    double hdg = 0.0, dev = 0.0, var = 0.0;
    tN2kHeadingReference ref;
    if (!ParseN2kHeading(msg, sid, hdg, dev, var, ref)) return;
    if (hdg > 1e8) return;
    portENTER_CRITICAL(&g_mux);
    g_heading_deg   = rad2deg_pos(hdg);
    g_heading_valid = true;
    portEXIT_CRITICAL(&g_mux);
}

static void on_rot(const tN2kMsg& msg) {
    unsigned char sid;
    double rot = 0.0;
    if (!ParseN2kRateOfTurn(msg, sid, rot)) return;
    if (rot > 1e8 || rot < -1e8) return;
    // PGN 127251 is in rad/s; NMEA 0183 ROT sentence uses deg/min
    portENTER_CRITICAL(&g_mux);
    g_rot_dps = (float)(rot * 180.0 / M_PI);
    portEXIT_CRITICAL(&g_mux);
}

static void on_cog_sog(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;
    portENTER_CRITICAL(&g_mux);
    g_cog_deg = rad2deg_pos(cog);
    g_sog_kn  = (float)(sog * 1.94384f);
    portEXIT_CRITICAL(&g_mux);
}

static void on_depth(const tN2kMsg& msg) {
    unsigned char sid;
    double depth = 0.0, offset = 0.0, range = 0.0;
    if (!ParseN2kWaterDepth(msg, sid, depth, offset, range)) return;
    if (depth > 1e8) return;
    portENTER_CRITICAL(&g_mux);
    g_depth_m = (float)depth;
    portEXIT_CRITICAL(&g_mux);
}

static void on_water_temp(const tN2kMsg& msg) {
    unsigned char sid;
    tN2kTempSource src;
    double temp = 0.0, set_temp = 0.0;
    if (!ParseN2kTemperature(msg, sid, src, temp, set_temp)) return;
    if (src != N2kts_SeaTemperature) return;
    if (temp > 1e8) return;
    portENTER_CRITICAL(&g_mux);
    g_water_temp_c = (float)(temp - 273.15);  // K → °C
    portEXIT_CRITICAL(&g_mux);
}

static void on_htc(const tN2kMsg& msg) {
    // PGN 127237 from the autopilot: its current status.
    tN2kOnOff rl, ol, tl, ov;
    tN2kSteeringMode sm;
    tN2kTurnMode tm;
    tN2kHeadingReference hr;
    tN2kRudderDirectionOrder rdo;
    double cra = 0, hts = 0, trk = 0, rlim = 0, ohl = 0;
    double rot_ord = 0, xtel = 0, vhd = 0;
    if (!ParseN2kHeadingTrackControl(msg, rl, ol, tl, ov, sm, tm, hr,
                                     rdo, cra, hts, trk, rlim, ohl,
                                     rot_ord, rot_ord, xtel, vhd)) return;

    ApStatus s{};
    s.heading_deg   = (vhd < 1e8) ? rad2deg_pos(vhd) : g_heading_deg;
    s.commanded_deg = (hts < 1e8) ? rad2deg_pos(hts) : 0.0f;
    s.rudder_deg    = (cra < 1e8) ? (float)(cra * 180.0 / M_PI) : 0.0f;
    s.mode = (sm == N2kSM_HeadingControl) ? 1 :
             (sm == N2kSM_TrackControl)   ? 2 : 0;
    s.valid = true;
    portENTER_CRITICAL(&g_mux);
    g_ap_status = s;
    portEXIT_CRITICAL(&g_mux);
}

static void MessageHandler(const tN2kMsg& msg) {
    switch (msg.PGN) {
        case 127250L: on_heading(msg);    break;
        case 127251L: on_rot(msg);        break;
        case 127237L: on_htc(msg);        break;
        case 128267L: on_depth(msg);      break;
        case 129026L: on_cog_sog(msg);    break;
        case 130310L: on_water_temp(msg); break;
        default: break;
    }
}

}  // namespace

// ---------------------------------------------------------------------------
//  Public API
// ---------------------------------------------------------------------------

void n2k_bridge_init() {
    NMEA2000.SetProductInformation("AR-CONC-1", 300,
        "AR-Concentrador v1", "1.0.0", "AR-CONCENTRADOR v1.0");
    NMEA2000.SetDeviceInformation(3, 170, 25, 2046);
    NMEA2000.SetMode(tNMEA2000::N2km_ListenAndNode, 27);
    NMEA2000.EnableForward(false);
    NMEA2000.SetMsgHandler(&MessageHandler);
    NMEA2000.Open();
}

bool n2k_to_0183(const tN2kMsg& msg, char* out_buf, size_t out_len) {
    char body[200];

    portENTER_CRITICAL(&g_mux);
    const float hdg = g_heading_deg;
    const float rot = g_rot_dps;
    const float sog = g_sog_kn;
    const float cog = g_cog_deg;
    const float dep = g_depth_m;
    const float tmp = g_water_temp_c;
    portEXIT_CRITICAL(&g_mux);

    switch (msg.PGN) {
        case 127250L:  // Heading → $IIHDT
            snprintf(body, sizeof(body), "IIHDT,%.1f,T", hdg);
            write_sentence(out_buf, out_len, body);
            return true;

        case 127251L:  // ROT → $IIROT (deg/min)
            snprintf(body, sizeof(body), "IIROT,%.2f,A", rot * 60.0f);
            write_sentence(out_buf, out_len, body);
            return true;

        case 129026L:  // COG/SOG → $IIVHW
            if (!isnan(sog) && !isnan(cog)) {
                snprintf(body, sizeof(body),
                         "IIVHW,%.1f,T,,M,%.2f,N,,K", cog, sog);
                write_sentence(out_buf, out_len, body);
                return true;
            }
            break;

        case 128267L:  // Depth → $IIDPT
            if (!isnan(dep)) {
                snprintf(body, sizeof(body), "IIDPT,%.1f,0.0", dep);
                write_sentence(out_buf, out_len, body);
                return true;
            }
            break;

        case 130310L:  // Water temp → $IIMTW
            if (!isnan(tmp)) {
                snprintf(body, sizeof(body), "IIMTW,%.1f,C", tmp);
                write_sentence(out_buf, out_len, body);
                return true;
            }
            break;

        default:
            break;
    }
    return false;
}

bool parp_to_n2k(const ParpCommand& cmd, float current_heading_deg) {
    // Determine the target steering mode and commanded heading.
    tN2kSteeringMode mode = N2kSM_MainSteering;  // STANDBY default
    double heading_to_steer = N2kDoubleNA;

    if (strcmp(cmd.cmd, "ENGAGE") == 0) {
        mode = N2kSM_HeadingControl;
        // Engage on current heading if no specific heading commanded.
        heading_to_steer = (double)current_heading_deg * M_PI / 180.0;
    } else if (strcmp(cmd.cmd, "DISENGAGE") == 0) {
        mode = N2kSM_MainSteering;
    } else if (strcmp(cmd.cmd, "SETHEADING") == 0) {
        mode = N2kSM_HeadingControl;
        heading_to_steer = (double)cmd.value * M_PI / 180.0;
    } else if (strcmp(cmd.cmd, "PORTONE") == 0) {
        mode = N2kSM_HeadingControl;
        heading_to_steer = (double)(cmd.value - 1.0f) * M_PI / 180.0;
    } else if (strcmp(cmd.cmd, "STBDONE") == 0) {
        mode = N2kSM_HeadingControl;
        heading_to_steer = (double)(cmd.value + 1.0f) * M_PI / 180.0;
    } else if (strcmp(cmd.cmd, "PORTTEN") == 0) {
        mode = N2kSM_HeadingControl;
        heading_to_steer = (double)(cmd.value - 10.0f) * M_PI / 180.0;
    } else if (strcmp(cmd.cmd, "STBDTEN") == 0) {
        mode = N2kSM_HeadingControl;
        heading_to_steer = (double)(cmd.value + 10.0f) * M_PI / 180.0;
    } else {
        return false;  // station management commands — not forwarded to NMEA2000
    }

    tN2kMsg msg;
    SetN2kHeadingTrackControl(msg,
        N2kOnOff_Unavailable, N2kOnOff_Unavailable,
        N2kOnOff_Unavailable, N2kOnOff_Off,
        mode, N2kTM_RudderLimitControlled, N2khr_true,
        N2kRDO_NoDirectionOrder, N2kDoubleNA,
        heading_to_steer, N2kDoubleNA,
        N2kDoubleNA, N2kDoubleNA, N2kDoubleNA, N2kDoubleNA,
        N2kDoubleNA, N2kDoubleNA);
    return NMEA2000.SendMsg(msg);
}

float n2k_latest_heading_deg() {
    portENTER_CRITICAL(&g_mux);
    const float h = g_heading_deg;
    portEXIT_CRITICAL(&g_mux);
    return h;
}

ApStatus n2k_ap_status() {
    portENTER_CRITICAL(&g_mux);
    const ApStatus s = g_ap_status;
    portEXIT_CRITICAL(&g_mux);
    return s;
}

}  // namespace arconcentrador::protocols
