#include <Arduino.h>
#include "gps.h"
#include "imu.h"
#include "rs485_transport.h"
#include "sensor_node_config.h"

HardwareSerial gpsSerial(0), rs485Serial(1);
ImuState imuState;
GpsState gpsState;
unsigned long lastTelemetryMs = 0;

void setup() {
    Serial.begin(115200); delay(250);
    Serial.println("XIAO ESP32-C6 sensor node booting");
    initGps(gpsSerial); initRs485(rs485Serial);
    imuState.online = initImu();
    Serial.println(imuState.online ? "BNO086 ready" : "BNO086 not detected");
}

void loop() {
    updateGps(gpsSerial, gpsState); updateImu(imuState);
    unsigned long now = millis();
    if (now - lastTelemetryMs >= TELEMETRY_INTERVAL_MS) {
        publishSensorStatus(rs485Serial, imuState, gpsState); lastTelemetryMs = now;
    }
    delay(1);
}