// =============================================================================
//  protocols/nmea2000_publisher.cpp -- NMEA 2000 publisher (Sprint 6)
// =============================================================================

#include "nmea2000_publisher.h"

#include <Arduino.h>
#include <math.h>

#include <N2kMessages.h>
#include <NMEA2000.h>

// NMEA2000_CAN.h defines the global tNMEA2000& NMEA2000 in the header,
// so it can only be included in one translation unit (nmea2000_consumer.cpp).
// Declare the same object here via extern.
extern tNMEA2000& NMEA2000;

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

namespace arautopilot::protocols::nmea2000 {

namespace {

constexpr const char* TAG = "AR/N2K/PUB";

static const double K_DEG2RAD = M_PI / 180.0;

// ----- PGN 127245: Rudder angle (10 Hz) -------------------------------------
void publish_rudder() {
    const auto rdr = hal::rudder_sensor_latest();
    if (!rdr.valid) return;

    tN2kMsg msg;
    SetN2kRudder(msg,
                 (double)rdr.angle_deg * K_DEG2RAD,
                 0,
                 N2kRDO_NoDirectionOrder,
                 N2kDoubleNA);
    NMEA2000.SendMsg(msg);
}

// ----- PGN 127237: Heading Track Control (1 Hz) -----------------------------
void publish_heading_track_control() {
    const bool engaged = !modes::is_standby();

    const tN2kSteeringMode steering_mode = engaged
        ? N2kSM_HeadingControl
        : N2kSM_MainSteering;

    double heading_to_steer = N2kDoubleNA;
    double vessel_heading   = N2kDoubleNA;

    // Current heading from consumer snapshot for VesselHeading field.
    {
        const auto n = nmea2000_latest();
        if (n.heading_valid) {
            vessel_heading = (double)n.heading_deg * K_DEG2RAD;
        }
    }

    if (engaged) {
        const modes::Mode mode = modes::current_mode();
        if (mode == modes::Mode::HEADING_HOLD) {
            heading_to_steer = (double)pid::pid_outer_heading_setpoint_deg() * K_DEG2RAD;
        } else if (mode == modes::Mode::TRUE_COURSE ||
                   mode == modes::Mode::TRACK_KEEPING) {
            heading_to_steer = (double)pid::pid_outer_cog_setpoint_deg() * K_DEG2RAD;
        }
    }

    tN2kMsg msg;
    SetN2kHeadingTrackControl(msg,
        N2kOnOff_Unavailable,       // RudderLimitExceeded
        N2kOnOff_Unavailable,       // OffHeadingLimitExceeded
        N2kOnOff_Unavailable,       // OffTrackLimitExceeded
        N2kOnOff_Off,               // Override
        steering_mode,              // SteeringMode
        N2kTM_RudderLimitControlled,// TurnMode
        N2khr_true,                 // HeadingReference
        N2kRDO_NoDirectionOrder,    // CommandedRudderDirection
        N2kDoubleNA,                // CommandedRudderAngle
        heading_to_steer,           // HeadingToSteerCourse
        N2kDoubleNA,                // Track
        N2kDoubleNA,                // RudderLimit
        N2kDoubleNA,                // OffHeadingLimit
        N2kDoubleNA,                // RadiusOfTurnOrder
        N2kDoubleNA,                // RateOfTurnOrder
        N2kDoubleNA,                // OffTrackLimit
        vessel_heading);            // VesselHeading
    NMEA2000.SendMsg(msg);
}

void PublisherTask(void* /*pv*/) {
    AR_LOGI(TAG, "nmea2000_publisher task started on core %d", xPortGetCoreID());

    TickType_t last_wake = xTaskGetTickCount();
    uint8_t    slow_tick = 0;

    for (;;) {
        publish_rudder();

        if (++slow_tick >= 10) {
            slow_tick = 0;
            publish_heading_track_control();
        }

        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(100));
    }
}

}  // namespace

void nmea2000_publisher_start_task() {
    xTaskCreatePinnedToCore(PublisherTask, "n2k_pub",
                            AR_TASK_STACK_N2K_RX,
                            nullptr,
                            AR_TASK_PRIO_N2K_RX - 1,
                            nullptr,
                            AR_TASK_CORE_COMMS);
}

}  // namespace arautopilot::protocols::nmea2000