// =============================================================================
//  AR BNO085 Compact Node v1 — main.cpp
// =============================================================================
//
//  Reads heading (ARVR Stabilized Rotation Vector) and yaw rate (Gyroscope)
//  from the BNO085 via I2C and publishes them on the NMEA 2000 backbone:
//    PGN 127250 — Vessel Heading        (100 Hz)
//    PGN 127251 — Rate of Turn          (100 Hz)
//
//  The main autopilot board (ar_autopilot_v1) receives these PGNs from the
//  backbone via its nmea2000_consumer and feeds them into the PID outer loop.
// =============================================================================

#include <Arduino.h>
#include <Wire.h>

// --- NMEA 2000 ---
#define ESP32_CAN_TX_PIN GPIO_NUM_23
#define ESP32_CAN_RX_PIN GPIO_NUM_4
#include <NMEA2000_CAN.h>
#include <N2kMessages.h>

// --- BNO085 ---
#include <SparkFun_BNO08x_Arduino_Library.h>

// =============================================================================
//  Pin definitions
// =============================================================================
static constexpr int PIN_I2C_SDA  = 21;
static constexpr int PIN_I2C_SCL  = 22;
static constexpr int PIN_BNO_INT  = 34;   // data-ready, active low
static constexpr int PIN_BNO_NRST = 13;   // reset, active low

// =============================================================================
//  BNO085 instance
// =============================================================================
static BNO08x imu;

// Latest measurements (updated from BNO085 reports)
static volatile float g_heading_rad = 0.0f;  // true heading, radians
static volatile float g_rot_rad_s   = 0.0f;  // yaw rate, rad/s (+ve = stbd)
static volatile bool  g_heading_valid = false;
static volatile bool  g_rot_valid     = false;

// =============================================================================
//  NMEA 2000 setup
// =============================================================================
static void nmea2000_init() {
    NMEA2000.SetProductInformation(
        "AR-BNO-1",
        200,
        "AR-BNO085-NODE v1",
        "1.0.0",
        "AR-BNO085-NODE v1.0"
    );
    NMEA2000.SetDeviceInformation(
        2,     // Unique number (different from autopilot node = 1)
        140,   // Device function: Attitude sensor
        60,    // Device class: Navigation
        2046   // Manufacturer code (test)
    );
    NMEA2000.SetMode(tNMEA2000::N2km_NodeOnly, 26);
    NMEA2000.EnableForward(false);
    NMEA2000.Open();
}

// =============================================================================
//  BNO085 initialisation
// =============================================================================
static bool bno085_init() {
    // Hard reset — hold NRST low for 10 ms, then release.
    pinMode(PIN_BNO_NRST, OUTPUT);
    digitalWrite(PIN_BNO_NRST, LOW);
    delay(15);
    digitalWrite(PIN_BNO_NRST, HIGH);
    delay(100);  // wait for BNO085 startup (~50 ms typical)

    Wire.begin(PIN_I2C_SDA, PIN_I2C_SCL);
    Wire.setClock(400000);  // 400 kHz Fast Mode

    if (!imu.begin(0x4A, Wire, PIN_BNO_INT)) {
        Serial.println("[BNO085] begin() failed — check wiring / I2C address");
        return false;
    }

    // ARVR Stabilized Rotation Vector → heading (tilt-compensated), 100 Hz
    if (!imu.enableARVRStabilizedRotationVector(10000)) {  // 10 ms = 100 Hz
        Serial.println("[BNO085] enableARVRStabilizedRotationVector() failed");
        return false;
    }

    // Calibrated Gyroscope → yaw rate, 100 Hz
    if (!imu.enableGyro(10000)) {
        Serial.println("[BNO085] enableGyro() failed");
        return false;
    }

    Serial.println("[BNO085] init OK — heading + yaw rate @ 100 Hz");
    return true;
}

// =============================================================================
//  Read BNO085 reports (call frequently)
// =============================================================================
static void bno085_read() {
    if (!imu.dataAvailable()) return;

    // Rotation Vector → heading (yaw around Z)
    if (imu.getSensorEventID() == SENSOR_REPORTID_ARVR_STABILIZED_ROTATION_VECTOR) {
        // SparkFun library: getYaw() returns yaw in radians, -pi..+pi
        const float yaw = imu.getYaw();
        // Convert to 0..2pi (nautical convention, 0 = North, + = clockwise)
        float hdg = -yaw;  // BNO085: +yaw = CCW (math convention); nautical = CW
        if (hdg < 0.0f)    hdg += 2.0f * (float)M_PI;
        if (hdg >= 2.0f * (float)M_PI) hdg -= 2.0f * (float)M_PI;
        g_heading_rad   = hdg;
        g_heading_valid = true;
    }

    // Gyroscope Z → yaw rate (+ = clockwise = starboard turn)
    if (imu.getSensorEventID() == SENSOR_REPORTID_GYROSCOPE_CALIBRATED) {
        // getGyroZ(): rad/s, +Z = looking down = CW from above = starboard
        g_rot_rad_s = imu.getGyroZ();
        g_rot_valid = true;
    }
}

// =============================================================================
//  Publish NMEA 2000 PGNs
// =============================================================================
static void publish_pgns() {
    if (g_heading_valid) {
        tN2kMsg msg;
        SetN2kHeading(msg,
            0,                    // SID
            (double)g_heading_rad,
            N2kDoubleNA,          // Deviation
            N2kDoubleNA,          // Variation
            N2khr_true);          // Reference: true north (BNO085 is absolute)
        NMEA2000.SendMsg(msg);
    }

    if (g_rot_valid) {
        tN2kMsg msg;
        // PGN 127251: rate in rad/s
        SetN2kRateOfTurn(msg, 0, (double)g_rot_rad_s);
        NMEA2000.SendMsg(msg);
    }
}

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

    nmea2000_init();

    if (!bno085_init()) {
        Serial.println("[AR-BNO085-NODE] FATAL: IMU init failed. Halting.");
        for (;;) delay(1000);
    }

    Serial.println("[AR-BNO085-NODE] ready.");
}

void loop() {
    // Read sensor at ~100 Hz (non-blocking, interrupt-driven via INT pin)
    bno085_read();

    // Publish to NMEA 2000 backbone at 10 Hz
    static uint32_t last_pub = 0;
    const uint32_t now = millis();
    if (now - last_pub >= 100) {
        last_pub = now;
        publish_pgns();
    }

    // Keep the NMEA2000 stack alive
    NMEA2000.ParseMessages();
}