#include "Telemetry.hpp"
#include "NetMgr.hpp"

namespace Telemetry {

bool readGNSS(double& lat, double& lng, double& alt, double& speed, double& heading) {
  lat=lng=alt=speed=heading=0;
  String r = NetMgr::sendAT("AT+CGNSINF", 3000);
  // +CGNSINF: 1,1,yyyyMMddhhmmss.sss,lat,lng,alt, ... ,speed_over_ground, ... ,course
  int p = r.indexOf("+CGNSINF:");
  if (p < 0) return false;
  // crude split
  int first = r.indexOf('\n', p); // skip header if present
  String line = (first!=-1) ? r.substring(p, first) : r.substring(p);
  // safer: take last line
  int lastNl = r.lastIndexOf('\n');
  line = (lastNl!=-1) ? r.substring(lastNl+1) : line;
  line.trim();

  // tokenization
  int commas[10]; int count=0;
  for (int i=0;i<(int)line.length() && count<10;i++) if (line[i]==',') commas[count++]=i;
  if (count < 6) return false;

  lat = line.substring(commas[1]+1, commas[2]).toDouble();
  lng = line.substring(commas[2]+1, commas[3]).toDouble();
  alt = line.substring(commas[3]+1, commas[4]).toDouble();
  // speed (km/h) field may be later; do a find:
  int next = line.indexOf(',', commas[4]+1);
  next = line.indexOf(',', next+1);
  next = line.indexOf(',', next+1);
  int speedStart = next+1;
  int speedEnd = line.indexOf(',', speedStart);
  if (speedStart>0 && speedEnd>speedStart) speed = line.substring(speedStart, speedEnd).toDouble();
  // heading/course similarly if needed
  return true;
}

String buildJson(const String& imei, double lat, double lng, double alt, double speed, double heading) {
  String j = String("{\"device_id\":\"") + imei + "\","
           + "\"lat\":" + String(lat,6) + ","
           + "\"lng\":" + String(lng,6) + ","
           + "\"speed\":" + String(speed,2) + ","
           + "\"altitude\":" + String(alt,2) + "}";
  return j;
}

String buildNoPowerJson(const String& imei) {
  return String("{\"device_id\":\"") + imei + "\",\"car_power\":false}";
}

} // namespace Telemetry namespace Telemetry