#include <ArduinoJson.h>
#include "rs485_transport.h"
#include "sensor_node_config.h"
#include "calibration.h"

namespace {
String commandBuffer;
bool discardCommandUntilNewline = false;
void sendJson(HardwareSerial& serial, JsonDocument& doc) {
    digitalWrite(RS485_DE_PIN, HIGH); delayMicroseconds(20);
    serializeJson(doc, serial); serial.write('\n'); serial.flush();
    digitalWrite(RS485_DE_PIN, LOW);
#if USB_TELEMETRY_ENABLED
    serializeJson(doc, Serial); Serial.println();
#endif
}
}

void initRs485(HardwareSerial& serial) {
    pinMode(RS485_DE_PIN, OUTPUT); digitalWrite(RS485_DE_PIN, LOW);
    serial.begin(RS485_BAUD, SERIAL_8N1, RS485_RX_PIN, RS485_TX_PIN);
}

void publishSensorStatus(HardwareSerial& serial, const ImuState& imu, const GpsState& gps) {
    StaticJsonDocument<1024> doc;
    doc["type"] = "sensor_status"; doc["schema_version"] = SENSOR_NODE_SCHEMA_VERSION;
    doc["node_id"] = SENSOR_NODE_NAME; doc["firmware_version"] = SENSOR_NODE_FIRMWARE_VERSION;
    doc["uptime_ms"] = millis();
    JsonObject i = doc.createNestedObject("imu");
    i["online"] = imu.online; i["valid"] = imu.valid && millis() - imu.lastUpdateMs <= SENSOR_STALE_MS;
    i["accuracy"] = imu.accuracy;
    JsonObject q = i.createNestedObject("quaternion"); q["i"] = imu.i; q["j"] = imu.j; q["k"] = imu.k; q["real"] = imu.real;
    i["yaw_degrees"] = imu.yawDegrees; i["pitch_degrees"] = imu.pitchDegrees; i["roll_degrees"] = imu.rollDegrees;
    JsonObject level = i.createNestedObject("level_calibration");
    level["valid"] = levelCalibration.valid; level["active"] = levelCalibration.active;
    level["pitch_offset_degrees"] = levelCalibration.pitchOffsetDegrees;
    level["roll_offset_degrees"] = levelCalibration.rollOffsetDegrees;
    level["pitch_degrees"] = calibratedPitchDegrees(imu);
    level["roll_degrees"] = calibratedRollDegrees(imu);
    JsonObject g = doc.createNestedObject("gps");
    g["online"] = gps.online; g["fix"] = gps.fix; g["satellites"] = gps.satellites;
    g["baud"] = gps.baud;
    JsonObject nmea = g.createNestedObject("nmea");
    nmea["characters"] = gps.charactersProcessed;
    nmea["sentences_passed"] = gps.sentencesPassed;
    nmea["sentences_failed"] = gps.sentencesFailed;
    g["latitude"] = gps.latitude; g["longitude"] = gps.longitude; g["altitude_meters"] = gps.altitudeMeters;
    g["speed_kph"] = gps.speedKph; g["course_degrees"] = gps.courseDegrees;
    JsonObject utc = g.createNestedObject("utc"); utc["year"] = gps.year; utc["month"] = gps.month; utc["day"] = gps.day;
    utc["hour"] = gps.hour; utc["minute"] = gps.minute; utc["second"] = gps.second;
    sendJson(serial, doc);
}

void publishCalibrationResult(HardwareSerial& serial, const String& result) {
    StaticJsonDocument<384> doc;
    doc["type"] = "calibration_response";
    doc["calibration"] = "level";
    doc["result"] = result;
    doc["ok"] = result == "started" || result == "complete" || result == "cleared";
    doc["valid"] = levelCalibration.valid;
    doc["pitch_offset_degrees"] = levelCalibration.pitchOffsetDegrees;
    doc["roll_offset_degrees"] = levelCalibration.rollOffsetDegrees;
    sendJson(serial, doc);
}

void pollRs485Commands(HardwareSerial& serial, const ImuState& imu) {
    // Bound RX work so an electrically echoed or noisy bus cannot starve sensors.
    int bytesRemaining = RS485_COMMAND_MAX_LENGTH * 2;
    while (serial.available() && bytesRemaining-- > 0) {
        char c = serial.read();
        if (c == '\r') continue;
        if (c != '\n') {
            if (discardCommandUntilNewline) continue;
            if (commandBuffer.length() < RS485_COMMAND_MAX_LENGTH) {
                commandBuffer += c;
            } else {
                commandBuffer = "";
                discardCommandUntilNewline = true;
            }
            continue;
        }

        if (discardCommandUntilNewline) {
            discardCommandUntilNewline = false;
            commandBuffer = "";
            continue;
        }

        StaticJsonDocument<192> request;
        DeserializationError error = deserializeJson(request, commandBuffer);
        commandBuffer = "";
        if (error) { publishCalibrationResult(serial, "invalid_json"); continue; }
        String type = request["type"] | "";
        // Some RS485 boards echo transmitted frames into RX. Ignore our own output.
        if (type == "sensor_status" || type == "calibration_response") continue;
        if (type == "calibrate_level") {
            publishCalibrationResult(serial, startLevelCalibration(imu) ? "started" : "imu_unavailable");
        } else if (type == "clear_level_calibration") {
            clearLevelCalibration(); publishCalibrationResult(serial, "cleared");
        } else {
            publishCalibrationResult(serial, "unknown_command");
        }
    }
}