#include <ArduinoJson.h>
#include <HardwareSerial.h>

#include "sensor_node_receiver.h"

namespace {
constexpr int SENSOR_NODE_RS485_RX_PIN = 43;
constexpr int SENSOR_NODE_RS485_TX_PIN = 44;
constexpr uint32_t SENSOR_NODE_RS485_BAUD = 115200;
constexpr size_t SENSOR_NODE_RS485_MAX_LINE = 1900;
constexpr size_t SENSOR_NODE_RS485_RX_BUFFER = 4096;
constexpr unsigned long SENSOR_NODE_STALE_MS = 1500;

HardwareSerial sensorNodeSerial(1);
SensorNodeDashboardStatus sensorNodeStatus;
String receiveLine;
bool discardUntilNewline = false;
bool sequenceInitialized = false;

void recordSequence(uint32_t bootId, uint32_t sequence) {
    if (!sequenceInitialized || bootId != sensorNodeStatus.bootId) {
        if (sequenceInitialized && bootId != sensorNodeStatus.bootId) {
            sensorNodeStatus.nodeRestarts++;
        }
        sensorNodeStatus.bootId = bootId;
        sensorNodeStatus.frameSequence = sequence;
        sequenceInitialized = true;
        return;
    }

    uint32_t delta = sequence - sensorNodeStatus.frameSequence;
    if (delta > 1 && delta < 0x80000000UL) {
        sensorNodeStatus.framesDropped += delta - 1;
    }
    sensorNodeStatus.frameSequence = sequence;
}

void parseSensorNodeFrame(const String &line) {
    StaticJsonDocument<3072> doc;
    DeserializationError error = deserializeJson(doc, line);
    if (error) {
        sensorNodeStatus.parseErrors++;
        return;
    }

    const char *type = doc["type"] | "";
    if (strcmp(type, "sensor_status") != 0 || (int)(doc["schema_version"] | 0) != 1) {
        return;
    }

    uint32_t bootId = doc["boot_id"] | 0;
    uint32_t sequence = doc["frame_sequence"] | 0;
    recordSequence(bootId, sequence);

    JsonObject imu = doc["imu"];
    sensorNodeStatus.imuOnline = imu["online"] | false;
    sensorNodeStatus.imuValid = imu["valid"] | false;
    sensorNodeStatus.imuAccuracy = imu["accuracy"] | 0;
    sensorNodeStatus.stability = String((const char *)(imu["stability"] | "unknown"));

    JsonObject level = imu["level_calibration"];
    sensorNodeStatus.levelCalibrationValid = level["valid"] | false;
    sensorNodeStatus.pitchDegrees = sensorNodeStatus.levelCalibrationValid
        ? (float)(level["pitch_degrees"] | 0.0f)
        : (float)(imu["pitch_degrees"] | 0.0f);
    sensorNodeStatus.rollDegrees = sensorNodeStatus.levelCalibrationValid
        ? (float)(level["roll_degrees"] | 0.0f)
        : (float)(imu["roll_degrees"] | 0.0f);

    JsonObject gps = doc["gps"];
    sensorNodeStatus.gpsOnline = gps["online"] | false;
    sensorNodeStatus.gpsFix = gps["fix"] | false;
    sensorNodeStatus.satellites = gps["satellites"] | 0;
    sensorNodeStatus.hdop = gps["hdop"].isNull() ? 0 : (float)gps["hdop"];
    sensorNodeStatus.latitude = gps["latitude"] | 0.0;
    sensorNodeStatus.longitude = gps["longitude"] | 0.0;
    sensorNodeStatus.speedKph = gps["speed_kph"] | 0.0f;
    sensorNodeStatus.firmwareVersion = String((const char *)(doc["firmware_version"] | ""));
    sensorNodeStatus.frameReceived = true;
    sensorNodeStatus.framesReceived++;
    sensorNodeStatus.lastFrameMs = millis();
}
}

void initSensorNodeRs485() {
    receiveLine.reserve(SENSOR_NODE_RS485_MAX_LINE);
    sensorNodeSerial.setRxBufferSize(SENSOR_NODE_RS485_RX_BUFFER);
    sensorNodeSerial.begin(
        SENSOR_NODE_RS485_BAUD,
        SERIAL_8N1,
        SENSOR_NODE_RS485_RX_PIN,
        SENSOR_NODE_RS485_TX_PIN
    );
    Serial.println("Sensor node RS485: RX GPIO43 / TX GPIO44 @ 115200");
}

void processSensorNodeRs485() {
    while (sensorNodeSerial.available() > 0) {
        char value = (char)sensorNodeSerial.read();
        if (value == '\r') continue;

        if (value != '\n') {
            if (discardUntilNewline) continue;
            if (receiveLine.length() < SENSOR_NODE_RS485_MAX_LINE) {
                receiveLine += value;
            } else {
                receiveLine = "";
                discardUntilNewline = true;
                sensorNodeStatus.oversizedFrames++;
            }
            continue;
        }

        if (discardUntilNewline) {
            discardUntilNewline = false;
            receiveLine = "";
            continue;
        }

        if (receiveLine.length() > 0) {
            parseSensorNodeFrame(receiveLine);
        }
        receiveLine = "";
    }
}

bool sensorNodeRs485Connected() {
    return sensorNodeStatus.frameReceived &&
           millis() - sensorNodeStatus.lastFrameMs <= SENSOR_NODE_STALE_MS;
}

const SensorNodeDashboardStatus &getSensorNodeStatus() {
    return sensorNodeStatus;
}