DriverLIdar/include/lidar_diagnostics.hpp

#pragma once
// xlidar-driver — device self-diagnostics decoded from the data stream.
//
// The public surface is vendor-neutral: every driver reports through the same
// Diagnostics struct — a list of DiagnosticIssue with stable cross-vendor
// codes, plus a raw field map for vendor-specific passthrough. Hosts never
// need per-vendor accessors; serialize with to_json() when a string API is
// more convenient.
#include <cstdint>
#include <cstdio>
#include <map>
#include <string>
#include <vector>

namespace xlidar {

struct ExtraInfo;  // lidar_interface.hpp

// ── Raw wire constants (document the values in Diagnostics::raw) ────────────

// OLEI Family A (0xFAF0) error_status bits, header byte [5]. Bits 3-7 are
// reserved on the wire; a nonzero reserved bit is still reported as a fault.
inline constexpr uint8_t kFaultMonitor     = 1u << 0;  // monitor / motor abnormal
inline constexpr uint8_t kFaultVoltage     = 1u << 1;  // supply voltage out of range
inline constexpr uint8_t kFaultTemperature = 1u << 2;  // internal temperature abnormal

// SICK TiM LMDscandata device status (low word; Telegram Listing).
inline constexpr uint16_t kSickStatusError            = 1u << 0;
inline constexpr uint16_t kSickStatusPollutionWarning = 1u << 1;
inline constexpr uint16_t kSickStatusPollutionError   = 1u << 2;

// SICK nanoScan3 General System State byte 0 (layout from sick_safetyscanners;
// NOT verified on real hardware).
inline constexpr uint8_t kNanoStateRunMode              = 1u << 0;
inline constexpr uint8_t kNanoStateStandby              = 1u << 1;
inline constexpr uint8_t kNanoStateContaminationWarning = 1u << 2;
inline constexpr uint8_t kNanoStateContaminationError   = 1u << 3;
inline constexpr uint8_t kNanoStateReferenceContour     = 1u << 4;
inline constexpr uint8_t kNanoStateManipulation         = 1u << 5;

// RPLIDAR SDK health status values (sl_lidar_response_device_health_t.status).
inline constexpr uint8_t kRplidarHealthOk      = 0;
inline constexpr uint8_t kRplidarHealthWarning = 1;
inline constexpr uint8_t kRplidarHealthError   = 2;

// ── Common diagnostics structure ────────────────────────────────────────────

// Fault = device says something is wrong now, stop trusting the data;
// Warning = degraded but still measuring (dirty optics, weak motor) —
// schedule cleaning/service.
enum class DiagSeverity { Warning, Fault };

inline const char* to_string(DiagSeverity s) {
    return s == DiagSeverity::Fault ? "fault" : "warning";
}

// One decoded device issue. `code` is a stable, machine-readable identifier
// shared across vendors:
//   "motor"          — motor/monitor subsystem abnormal
//   "voltage"        — supply voltage out of range
//   "temperature"    — internal temperature abnormal
//   "optics_dirty"   — pollution/contamination of the optics window
//                      (warning: clean soon; fault: data no longer reliable)
//   "manipulation"   — safety scanner suspects tampering/covering
//   "device_error"   — device-level fault the vendor doesn't break down
//   "device_warning" — device-level warning the vendor doesn't break down
// `detail` is human-readable, names the vendor, and may carry the raw value.
struct DiagnosticIssue {
    DiagSeverity severity = DiagSeverity::Fault;
    std::string  code;
    std::string  detail;
};

// Device self-diagnostics decoded from the data stream. valid stays false
// until the driver has decoded one full scan; issues is empty while the
// device reports healthy. Vendor-specific raw fields appear in `raw` keyed
// by stable names ("olei.error_status", "sick.device_status",
// "nano.general_state", "espe.error_status", "rplidar.health_status",
// "rplidar.error_code", ...) — only fields present on the wire are set.
struct Diagnostics {
    bool        valid = false;
    std::string model = "AUTO";
    std::string firmware;                 // e.g. "fw 1.32 hw 18"; empty if unknown
    uint32_t    device_timestamp_ms = 0;  // device clock; 0 if not on the wire

    std::vector<DiagnosticIssue> issues;
    std::map<std::string, uint32_t> raw;

    bool has_fault() const {
        for (const auto& i : issues)
            if (i.severity == DiagSeverity::Fault) return true;
        return false;
    }
    bool has_warning() const {
        for (const auto& i : issues)
            if (i.severity == DiagSeverity::Warning) return true;
        return false;
    }
    bool healthy() const { return valid && !has_fault(); }
};

// Decode the diagnostic fields of one scan; sets valid = true.
// Defined inline in lidar_interface.hpp (needs the ExtraInfo definition, and
// every plugin .so must carry its own copy).
Diagnostics decode_diagnostics(const ExtraInfo& info);

// One-line log summary: "no data" / "ok" / "WARN: optics_dirty" /
// "FAULT: voltage temperature | WARN: optics_dirty".
inline std::string to_string(const Diagnostics& d) {
    if (!d.valid)        return "no data";
    if (d.issues.empty()) return "ok";
    std::string faults, warnings;
    for (const auto& i : d.issues)
        (i.severity == DiagSeverity::Fault ? faults : warnings) += " " + i.code;
    std::string s;
    if (!faults.empty())   s += "FAULT:" + faults;
    if (!warnings.empty()) s += (s.empty() ? "WARN:" : " | WARN:") + warnings;
    return s;
}

namespace detail {
// Minimal JSON string escaping (quotes, backslash, control characters) —
// model/firmware come off the wire and may hold arbitrary bytes.
inline std::string json_escape(const std::string& in) {
    std::string out;
    out.reserve(in.size());
    for (unsigned char c : in) {
        switch (c) {
            case '"':  out += "\\\""; break;
            case '\\': out += "\\\\"; break;
            case '\n': out += "\\n";  break;
            case '\r': out += "\\r";  break;
            case '\t': out += "\\t";  break;
            default:
                if (c < 0x20) {
                    char buf[8];
                    std::snprintf(buf, sizeof(buf), "\\u%04X", c);
                    out += buf;
                } else {
                    out += static_cast<char>(c);
                }
        }
    }
    return out;
}
} // namespace detail

// Full JSON snapshot, e.g. for a REST/telemetry payload:
//   {"valid":true,"model":"C1","firmware":"fw 1.32 hw 18",
//    "device_timestamp_ms":0,"healthy":false,
//    "issues":[{"severity":"fault","code":"voltage","detail":"..."}],
//    "raw":{"olei.error_status":2}}
inline std::string to_json(const Diagnostics& d) {
    std::string s = "{\"valid\":";
    s += d.valid ? "true" : "false";
    s += ",\"model\":\"" + detail::json_escape(d.model) + "\"";
    s += ",\"firmware\":\"" + detail::json_escape(d.firmware) + "\"";
    s += ",\"device_timestamp_ms\":" + std::to_string(d.device_timestamp_ms);
    s += ",\"healthy\":";
    s += d.healthy() ? "true" : "false";
    s += ",\"issues\":[";
    for (size_t i = 0; i < d.issues.size(); ++i) {
        const DiagnosticIssue& issue = d.issues[i];
        if (i) s += ',';
        s += "{\"severity\":\"";
        s += to_string(issue.severity);
        s += "\",\"code\":\"" + detail::json_escape(issue.code) + "\"";
        s += ",\"detail\":\"" + detail::json_escape(issue.detail) + "\"}";
    }
    s += "],\"raw\":{";
    bool first = true;
    for (const auto& [key, value] : d.raw) {
        if (!first) s += ',';
        first = false;
        s += "\"" + detail::json_escape(key) + "\":" + std::to_string(value);
    }
    s += "}}";
    return s;
}

} // namespace xlidar