// =============================================================================
//  AR Concentrador NMEA2000-USB v1 — main.cpp
// =============================================================================
//
//  Bidirectional NMEA 2000 <-> NMEA 0183 USB gateway with multi-station
//  command authority management.
//
//  Hardware:
//    UART1 TX (GPIO17) → 4x CH340N → USB-OUT1..4  (broadcast NMEA 0183)
//    UART2 RX (GPIO16) ← 4x CH340N ← USB-IN1..4   (receive $PARP commands)
//    CAN TX   (GPIO21) → MCP2562T   → NMEA 2000 backbone
//    CAN RX   (GPIO22) ← MCP2562T   ← NMEA 2000 backbone
//
//  Protocol: docs/concentrador_protocol.md
// =============================================================================

#include <Arduino.h>

#define ESP32_CAN_TX_PIN GPIO_NUM_21
#define ESP32_CAN_RX_PIN GPIO_NUM_22
#include <NMEA2000_CAN.h>
#include <N2kMessages.h>

#include "protocols/n2k_bridge.h"
#include "protocols/nmea0183_parser.h"
#include "station/station_mgr.h"

using namespace arconcentrador::protocols;
using namespace arconcentrador::station;

// =============================================================================
//  Serial port configuration
// =============================================================================
#define UART_OUT  Serial1   // UART1 TX → CH340N OUT ports
#define UART_IN   Serial2   // UART2 RX ← CH340N IN  ports

static constexpr int PIN_UART1_TX = 17;
static constexpr int PIN_UART1_RX = -1;   // not used (TX only)
static constexpr int PIN_UART2_TX = -1;   // not used (RX only)
static constexpr int PIN_UART2_RX = 16;
static constexpr int UART_BAUD    = 115200;

// =============================================================================
//  Broadcast helpers
// =============================================================================

/// Write a sentence to all OUT ports (UART1 TX).
static void broadcast(const char* sentence) {
    if (sentence && sentence[0]) {
        UART_OUT.print(sentence);
    }
}

/// Build and broadcast the $PARP,STATUS sentence (2 Hz heartbeat).
static void broadcast_status(uint8_t commander) {
    const ApStatus ap = n2k_ap_status();
    const float hdg   = n2k_latest_heading_deg();

    const char* mode_str = "STANDBY";
    if (ap.valid) {
        if (ap.mode == 1) mode_str = "HEADING_HOLD";
        else if (ap.mode == 2) mode_str = "TRACK";
    }

    char body[128];
    snprintf(body, sizeof(body),
             "PARP,STATUS,%s,%.1f,%.1f,%.1f,%02d",
             mode_str,
             ap.valid ? ap.commanded_deg : 0.0f,
             hdg,
             ap.valid ? ap.rudder_deg   : 0.0f,
             commander);

    char sentence[160];
    uint8_t crc = 0;
    for (const char* p = body; *p; ++p) crc ^= (uint8_t)*p;
    snprintf(sentence, sizeof(sentence), "$%s*%02X\r\n", body, crc);
    broadcast(sentence);
}

// =============================================================================
//  NMEA 2000 message handler (pumped by NMEA2000.ParseMessages())
// =============================================================================
static const uint32_t PGNS_TO_FORWARD[] = {
    127250L,  // Heading
    127251L,  // ROT
    129026L,  // COG/SOG
    128267L,  // Depth
    130310L,  // Water temp
    0
};

static void on_n2k_message(const tN2kMsg& msg) {
    // Forward selected PGNs to all USB-OUT ports as NMEA 0183 sentences.
    for (int i = 0; PGNS_TO_FORWARD[i]; ++i) {
        if (msg.PGN == PGNS_TO_FORWARD[i]) {
            char buf[128];
            if (n2k_to_0183(msg, buf, sizeof(buf))) {
                broadcast(buf);
            }
            break;
        }
    }
}

// =============================================================================
//  Command processor (reads from UART2 RX)
// =============================================================================
static void process_uart_in() {
    while (UART_IN.available()) {
        const char c = (char)UART_IN.read();
        ParpCommand cmd{};
        if (!parp_feed(c, cmd)) continue;

        // --- Station management commands ---
        char station_broadcast[128];
        station_process(cmd.cmd, cmd.station_id,
                        station_broadcast, sizeof(station_broadcast));
        if (station_broadcast[0]) {
            broadcast(station_broadcast);
        }

        // --- Autopilot commands (only from current commander) ---
        const uint8_t cmdr = current_commander();
        const bool is_commander = (cmdr == cmd.station_id);
        const bool is_ap_cmd = (strcmp(cmd.cmd, "ENGAGE")      == 0 ||
                                strcmp(cmd.cmd, "DISENGAGE")   == 0 ||
                                strcmp(cmd.cmd, "SETHEADING")  == 0 ||
                                strcmp(cmd.cmd, "PORTONE")     == 0 ||
                                strcmp(cmd.cmd, "STBDONE")     == 0 ||
                                strcmp(cmd.cmd, "PORTTEN")     == 0 ||
                                strcmp(cmd.cmd, "STBDTEN")     == 0);

        if (is_ap_cmd && is_commander) {
            const float hdg = n2k_latest_heading_deg();
            parp_to_n2k(cmd, hdg);
            Serial.printf("[MAIN] cmd %s val=%.1f sta=%d → NMEA2000\n",
                          cmd.cmd, cmd.value, cmd.station_id);
        } else if (is_ap_cmd && !is_commander) {
            Serial.printf("[MAIN] cmd %s rechazado: sta=%d no tiene el mando (mando=%d)\n",
                          cmd.cmd, cmd.station_id, cmdr);
        }
    }
}

// =============================================================================
//  Arduino entry points
// =============================================================================
void setup() {
    Serial.begin(115200);
    Serial.println("[AR-CONCENTRADOR] booting...");

    // UART1: TX only (broadcast to CH340N OUT ports)
    UART_OUT.begin(UART_BAUD, SERIAL_8N1, PIN_UART1_RX, PIN_UART1_TX);

    // UART2: RX only (receive from CH340N IN ports)
    UART_IN.begin(UART_BAUD, SERIAL_8N1, PIN_UART2_RX, PIN_UART2_TX);

    // Station manager
    station_init();

    // NMEA 2000 bridge
    n2k_bridge_init();
    NMEA2000.SetMsgHandler(on_n2k_message);

    Serial.println("[AR-CONCENTRADOR] ready.");
    Serial.println("[AR-CONCENTRADOR] UART1 TX=GPIO17 (OUT) | UART2 RX=GPIO16 (IN)");
    Serial.println("[AR-CONCENTRADOR] CAN TX=GPIO21 | CAN RX=GPIO22");
}

void loop() {
    // 1. Pump NMEA 2000 (calls on_n2k_message for each received PGN)
    NMEA2000.ParseMessages();

    // 2. Process incoming $PARP commands from USB-IN ports
    process_uart_in();

    // 3. Station management tick (REQCMD timeout)
    station_tick();

    // 4. Broadcast autopilot status at 2 Hz
    static uint32_t last_status = 0;
    const uint32_t now = millis();
    if (now - last_status >= 500) {
        last_status = now;
        broadcast_status(current_commander());
    }
}
