telemetry_row.cpp

// telemetry_row.cpp
#include "telemetry_row.h"
#include <ctype.h>

// Cache the hardware id so we don't AT spam every row
static char g_hwId[24] = {0};     // IMEI is 15 digits; extra space for safety
static bool g_hwIdReady = false;

// Cache LTE signal so we don't AT spam every row
static int  g_csq = -1;                 // 0..31, 99 unknown
static long g_csqLastMs = 0;
static const long CSQ_REFRESH_MS = 30000; // refresh every 30s

// -------------------------- PID fallbacks (in case headers don't define them) --------------------------
#ifndef PID_RPM
  #define PID_RPM 0x0C
#endif
#ifndef PID_SPEED
  #define PID_SPEED 0x0D
#endif
#ifndef PID_FUEL_LEVEL
  #define PID_FUEL_LEVEL 0x2F
#endif
#ifndef PID_THROTTLE
  #define PID_THROTTLE 0x11
#endif
#ifndef PID_ENGINE_LOAD
  #define PID_ENGINE_LOAD 0x04
#endif
#ifndef PID_COOLANT_TEMP
  #define PID_COOLANT_TEMP 0x05
#endif
#ifndef PID_DISTANCE
  #define PID_DISTANCE 0x31 // Distance since codes cleared (Mode 01 PID 31)
#endif

// Valuable extras
#ifndef PID_CONTROL_MODULE_VOLTAGE
  #define PID_CONTROL_MODULE_VOLTAGE 0x42
#endif
#ifndef PID_INTAKE_AIR_TEMP
  #define PID_INTAKE_AIR_TEMP 0x0F
#endif
#ifndef PID_AMBIENT_AIR_TEMP
  #define PID_AMBIENT_AIR_TEMP 0x46
#endif
#ifndef PID_MAF_FLOW
  #define PID_MAF_FLOW 0x10
#endif
#ifndef PID_FUEL_RATE
  #define PID_FUEL_RATE 0x5E
#endif
#ifndef PID_BAROMETRIC_PRESSURE
  #define PID_BAROMETRIC_PRESSURE 0x33
#endif
#ifndef PID_INTAKE_MANIFOLD_PRESSURE
  #define PID_INTAKE_MANIFOLD_PRESSURE 0x0B
#endif
#ifndef PID_TIMING_ADVANCE
  #define PID_TIMING_ADVANCE 0x0E
#endif
#ifndef PID_ENGINE_RUNTIME
  #define PID_ENGINE_RUNTIME 0x1F
#endif

// -------------------------- Helpers --------------------------

// Extract first 15-digit-ish numeric token from modem response
static bool extractImeiFromResp(const char* resp, char* out, size_t outSize) {
  if (!resp || outSize < 16) return false;

  // Collect digits in order; stop after 17 to be safe
  char digits[24] = {0};
  size_t di = 0;

  for (const char* p = resp; *p; p++) {
    if (isdigit((unsigned char)*p)) {
      if (di + 1 < sizeof(digits)) digits[di++] = *p;
    } else {
      // If we already collected a plausible IMEI, we can stop at first break
      if (di >= 14) break;
    }
  }

  // Typical IMEI is 15 digits
  if (di < 14) return false;

  digits[di] = '\0';
  strncpy(out, digits, outSize);
  out[outSize - 1] = '\0';
  return true;
}

const char* getModemHardwareId(CellAT& cell) {
  if (g_hwIdReady && g_hwId[0]) return g_hwId;

  // Try common commands. Some SIMCom variants return IMEI on AT+GSN, others AT+CGSN.
  cell.atp("AT+GSN\r", 2000);
  if (extractImeiFromResp(cell.lastResp(), g_hwId, sizeof(g_hwId))) {
    g_hwIdReady = true;
    return g_hwId;
  }

  cell.atp("AT+CGSN\r", 2000);
  if (extractImeiFromResp(cell.lastResp(), g_hwId, sizeof(g_hwId))) {
    g_hwIdReady = true;
    return g_hwId;
  }

  // If nothing worked, avoid retrying constantly
  strncpy(g_hwId, "UNKNOWN", sizeof(g_hwId));
  g_hwIdReady = true;
  return g_hwId;
}

// Parse +CSQ: <rssi>,<ber>
static bool parseCSQ(const char* resp, int& outRssi) {
  if (!resp) return false;

  // Find "+CSQ:"
  const char* p = strstr(resp, "+CSQ:");
  if (!p) return false;

  p += 5; // past "+CSQ:"
  while (*p == ' ' || *p == '\t') p++;

  // Parse first integer
  int rssi = -1;
  bool neg = false;
  if (*p == '-') { neg = true; p++; }
  if (!isdigit((unsigned char)*p)) return false;

  int val = 0;
  while (isdigit((unsigned char)*p)) {
    val = val * 10 + (*p - '0');
    p++;
  }
  rssi = neg ? -val : val;

  outRssi = rssi;
  return true;
}

// Refresh CSQ no more than every CSQ_REFRESH_MS
static int getLTECSQ(CellAT& cell) {
  long now = (long)millis();
  if (g_csqLastMs == 0 || (now - g_csqLastMs) >= CSQ_REFRESH_MS) {
    g_csqLastMs = now;
    cell.atp("AT+CSQ\r", 1500);
    int rssi = -1;
    if (parseCSQ(cell.lastResp(), rssi)) {
      g_csq = rssi;
    } else {
      g_csq = -1;
    }
  }
  return g_csq;
}

static int csqToDbm(int csq) {
  // Per 3GPP: 0..31 => -113 + 2*csq dBm, 99 unknown
  if (csq < 0) return 0;
  if (csq == 99) return 0;
  if (csq > 31) return 0;
  return -113 + (2 * csq);
}

// Some Freematics builds return PID_CONTROL_MODULE_VOLTAGE as mV,
// others as raw-ish int. This makes a best-effort float voltage.
static float bestEffortVoltageV(int vctlRaw) {
  if (vctlRaw <= 0) return 0.0f;

  // If it looks like millivolts (typical 11000..15000)
  if (vctlRaw >= 3000 && vctlRaw <= 25000) {
    return (float)vctlRaw / 1000.0f;
  }

  // If it looks like centivolts (e.g., 1260 => 12.60V)
  if (vctlRaw >= 300 && vctlRaw <= 2500) {
    return (float)vctlRaw / 100.0f;
  }

  // Otherwise treat as volts already (rare)
  if (vctlRaw <= 60) {
    return (float)vctlRaw;
  }

  // Unknown scaling
  return (float)vctlRaw;
}

// -------------------------- Main Row Builder --------------------------

bool buildRowJson(
  FreematicsESP32& sys,
  COBD& obd,
  CellAT& cell,
  const char* vin,
  char* outRow,
  int outSize
) {
  // Core PIDs
  int rpm = 0, spd = 0, fuel = 0, thr = 0, load = 0, temp = 0, dist = 0;

  // Valuable extras
  int vctl_raw = 0;        // control module voltage (raw from lib)
  int iat = 0;             // intake air temp (°C typically)
  int amb = 0;             // ambient air temp (°C typically)
  int maf = 0;             // mass air flow (often grams/sec scaled by lib)
  int fuel_rate = 0;       // fuel rate (if supported by vehicle)
  int baro = 0;            // kPa
  int map = 0;             // kPa
  int timing = 0;          // degrees (if supported by lib)
  int runtime = 0;         // seconds since engine start (if supported)

  if (obd.getState() == OBD_CONNECTED) {
    // Read PIDs (ignore individual failures; leave defaults)
    obd.readPID(PID_RPM, rpm);
    obd.readPID(PID_SPEED, spd);
    obd.readPID(PID_FUEL_LEVEL, fuel);
    obd.readPID(PID_THROTTLE, thr);
    obd.readPID(PID_ENGINE_LOAD, load);
    obd.readPID(PID_COOLANT_TEMP, temp);
    obd.readPID(PID_DISTANCE, dist);

    // Extras (these will quietly stay 0 if unsupported)
    obd.readPID(PID_CONTROL_MODULE_VOLTAGE, vctl_raw);
    obd.readPID(PID_INTAKE_AIR_TEMP, iat);
    obd.readPID(PID_AMBIENT_AIR_TEMP, amb);
    obd.readPID(PID_MAF_FLOW, maf);
    obd.readPID(PID_FUEL_RATE, fuel_rate);
    obd.readPID(PID_BAROMETRIC_PRESSURE, baro);
    obd.readPID(PID_INTAKE_MANIFOLD_PRESSURE, map);
    obd.readPID(PID_TIMING_ADVANCE, timing);
    obd.readPID(PID_ENGINE_RUNTIME, runtime);
  }

  float vctl_v = bestEffortVoltageV(vctl_raw);

  // Keep your derived metric
  float engine_efficiency = (rpm > 0) ? (float)(load * rpm) / 5000.0f : 0.0f;

  // GPS
  GPS_DATA* gd = nullptr;
  float lat = 0.0f, lng = 0.0f, alt = 0.0f;
  int sat = 0;
  float gps_spd_kph = 0.0f;
  int gps_fix = 0;

  if (sys.gpsGetData(&gd) && gd) {
    lat = gd->lat;
    lng = gd->lng;
    alt = gd->alt;
    sat = gd->sat;

    // If GPS_DATA doesn't have speed, remove this line too.
    gps_spd_kph = gd->speed;

    // ✅ GPS_DATA doesn't have "valid" in your build.
    // Use a conservative fix heuristic: lat/lng non-zero AND sats >= 3.
    gps_fix = (sat >= 3 && (lat != 0.0f || lng != 0.0f)) ? 1 : 0;
  }

  // LTE status + signal (CSQ)
  bool lte_connected = cell.isLTEConnected();
  int csq = getLTECSQ(cell);
  int rssi_dbm = csqToDbm(csq);

  // Timestamp (uptime ms)
  unsigned long ts_ms = millis();

  const char* hardwareId = getModemHardwareId(cell);

  // JSON output
  int n = snprintf(
    outRow, outSize,
    "{"
      "\"vin\":\"%s\","
      "\"hardware_id\":\"%s\","
      "\"ts_ms\":%lu,"

      "\"rpm\":%d,"
      "\"spd\":%d,"
      "\"fuel\":%d,"
      "\"thr\":%d,"
      "\"load\":%d,"
      "\"temp\":%d,"
      "\"dist\":%d,"

      "\"vctl_raw\":%d,"
      "\"vctl_v\":%.2f,"
      "\"iat\":%d,"
      "\"amb\":%d,"
      "\"maf\":%d,"
      "\"fuel_rate\":%d,"
      "\"baro\":%d,"
      "\"map\":%d,"
      "\"timing\":%d,"
      "\"runtime\":%d,"

      "\"dtc\":\"NONE\","

      "\"lat\":%.6f,"
      "\"lng\":%.6f,"
      "\"alt\":%.1f,"
      "\"sat\":%d,"
      "\"gps_spd_kph\":%.1f,"
      "\"gps_fix\":%d,"

      "\"acc_x\":0.00,"

      "\"os_version\":\"%s\","
      "\"device\":\"%s\","

      "\"engine_efficiency\":%.2f,"

      "\"lte_connected\":%s,"
      "\"lte_csq\":%d,"
      "\"lte_rssi_dbm\":%d"
    "}",
    vin,
    hardwareId,
    ts_ms,

    rpm, spd, fuel, thr, load, temp, dist,

    vctl_raw, vctl_v, iat, amb, maf, fuel_rate, baro, map, timing, runtime,

    lat, lng, alt, sat, gps_spd_kph, gps_fix,

    OS_VERSION, DEVICE_NAME,

    engine_efficiency,

    lte_connected ? "true" : "false",
    csq,
    rssi_dbm
  );

  if (n < 0) {
    LOGE("❌ snprintf error building row JSON");
    return false;
  }
  if (n >= outSize) {
    LOGE("❌ Row JSON truncated! needed=%d buf=%d", n, outSize);
    return false;
  }

  return true;
}