// Telemetry service
//   * Orientation: supervises the configured Python MPU script and tracks it with an explicit state machine.
//   * Position:    talks to gpsd over its JSON TCP protocol (no extra dependency).
//   * Time:        the OS clock (already disciplined by RTC/GPS), exposed via now().
//
// Sensor state machine
//
//   stopped ──start()──▶ starting ──first sample──▶ running ◀──sample──┐
//                           │                          │                │
//                           │ exit / no data           │ no sample      │
//                           ▼                          ▼ for staleMs    │
//                        backoff ◀──── exit / stale too long ──────── stale
//                           │
//                           └─ delay (1s,2s,4s … 30s) ─▶ starting
//
// Events: 'orientation', 'sensor-state', 'gps-state', 'gps'
import { EventEmitter } from 'node:events';
import { spawn } from 'node:child_process';
import readline from 'node:readline';
import net from 'node:net';

const SENSOR_TRANSITIONS = {
  stopped: ['starting'],
  starting: ['running', 'backoff', 'stopped'],
  running: ['stale', 'backoff', 'stopped'],
  stale: ['running', 'backoff', 'stopped'],
  backoff: ['starting', 'stopped'],
};

const isFiniteNum = (v) => typeof v === 'number' && Number.isFinite(v);

function eulerToQuat(yaw, pitch, roll) {
  const y = (yaw * Math.PI) / 360;
  const p = (pitch * Math.PI) / 360;
  const r = (roll * Math.PI) / 360;
  const cy = Math.cos(y), sy = Math.sin(y);
  const cp = Math.cos(p), sp = Math.sin(p);
  const cr = Math.cos(r), sr = Math.sin(r);
  return [
    sr * cp * cy - cr * sp * sy,
    cr * sp * cy + sr * cp * sy,
    cr * cp * sy - sr * sp * cy,
    cr * cp * cy + sr * sp * sy,
  ];
}


export class TelemetryService extends EventEmitter {
  #cfg;
  #log;

  #sensor = {
    state: 'stopped',
    proc: null,
    startedAt: 0,
    lastSampleAt: 0,
    runningSince: 0,
    failures: 0,
    restarts: 0,
    lastError: null,
    backoffTimer: null,
  };
  #orientation = null;
  #batteryTimer = null;
  #batteryPolling = false;
  #battery = {
    state: 'unavailable', available: false, source: 'pisugar-server', socketPath: '/tmp/pisugar-server.sock',
    voltageV: null, percent: null, percentEstimate: null, percentIsEstimate: false,
    charging: null, powerPlugged: null, chargingAllowed: null, updatedAt: null, error: 'not polled yet',
  };

  #gps = {
    socket: null,
    connected: false,
    retryTimer: null,
    connectFailures: 0,
    lastTpvAt: 0,
    tpv: null,
    sky: null,
    lastGoodFix: null,
    emittedState: 'disconnected',
  };

  #watchdog = null;
  #stopping = true;

  constructor(cfg, { log }) {
    super();
    this.#cfg = cfg;
    this.#log = log;
  }

  // ───────────────────────────── lifecycle ─────────────────────────────

  start() {
    if (!this.#stopping) return;
    this.#stopping = false;
    this.#startSensor();
    this.#connectGps();
    this.#pollBattery().catch(() => {});
    if (this.#cfg.pisugar?.enabled !== false) {
      this.#batteryTimer = setInterval(() => this.#pollBattery().catch(() => {}), this.#cfg.pisugar?.pollMs || 5000);
      this.#batteryTimer.unref();
    }
    this.#watchdog = setInterval(() => this.#tick(), 1000);
    this.#watchdog.unref();
  }

  async stop() {
    this.#stopping = true;
    clearInterval(this.#watchdog);
    clearInterval(this.#batteryTimer);
    clearTimeout(this.#sensor.backoffTimer);
    clearTimeout(this.#gps.retryTimer);
    this.#transitionSensor('stopped', 'service stopped');

    const proc = this.#sensor.proc;
    this.#sensor.proc = null;
    if (proc) {
      await new Promise((resolve) => {
        const t = setTimeout(() => proc.kill('SIGKILL'), 2000);
        proc.once('close', () => {
          clearTimeout(t);
          resolve();
        });
        proc.kill('SIGTERM');
      });
    }
    this.#gps.socket?.destroy();
    this.#gps.socket = null;
  }

  // ───────────────────────────── public API ─────────────────────────────

  /** Current time. The OS clock is already synced to RTC/GPS. */
  now() {
    return new Date();
  }

  /**
   * Latest orientation, or null if the sensor is not delivering fresh data.
   * @param {{maxAgeMs?: number}} [opts]
   */
  getOrientation({ maxAgeMs = this.#cfg.staleMs } = {}) {
    const o = this.#orientation;
    if (!o) return null;
    const ageMs = Date.now() - o.ts;
    if (ageMs > maxAgeMs) return null;
    return { yaw: o.yaw, pitch: o.pitch, roll: o.roll, quat: o.quat, ageMs, ts: o.ts };
  }

  /** Last good GPS fix (kept even if the fix is currently lost) or null if there has never been one. */
  getPosition() {
    const f = this.#gps.lastGoodFix;
    if (!f) return null;
    const ageMs = Date.now() - f.at;
    return {
      lat: f.lat,
      lon: f.lon,
      altM: f.altM ?? 0,
      mode: f.mode,
      ageMs,
      live: ageMs < this.#cfg.gpsStaleMs,
    };
  }

  getSensorState() {
    return this.#sensor.state;
  }

  getGpsState() {
    return this.#gpsStateNow();
  }

  getSnapshot() {
    const now = Date.now();
    const s = this.#sensor;
    const o = this.#orientation;
    const g = this.#gps;
    const pos = this.getPosition();
    return {
      time: { iso: new Date(now).toISOString(), epochMs: now },
      sensor: {
        state: s.state,
        sampleAgeMs: s.lastSampleAt ? now - s.lastSampleAt : null,
        restarts: s.restarts,
        lastError: s.lastError,
      },
      orientation: o
        ? { yaw: o.yaw, pitch: o.pitch, roll: o.roll, quat: o.quat, ageMs: now - o.ts }
        : null,
      battery: { ...this.#battery },
      gps: {
        state: this.#gpsStateNow(),
        position: pos,
        satellites: g.sky ? { used: g.sky.used, seen: g.sky.seen } : null,
        dop: g.sky ? { hdop: g.sky.hdop, vdop: g.sky.vdop, pdop: g.sky.pdop } : null,
        speedMps: g.tpv?.speed ?? null,
        // GPS time minus system time. Should be a few ms if the OS sync is healthy.
        clockSkewMs: g.tpv?.skewMs ?? null,
        errorM: g.tpv ? { x: g.tpv.epx ?? null, y: g.tpv.epy ?? null, v: g.tpv.epv ?? null } : null,
      },
    };
  }

  // ───────────────────────────── PiSugar battery ─────────────────────────────

  getBattery() {
    return { ...this.#battery };
  }

  async #pisugarCommand(command) {
    const path = this.#cfg.pisugar?.socketPath || '/tmp/pisugar-server.sock';
    return await new Promise((resolve, reject) => {
      const socket = net.createConnection({ path });
      let output = '';
      let settled = false;
      const finish = (err, value) => {
        if (settled) return;
        settled = true;
        clearTimeout(timer);
        socket.destroy();
        if (err) reject(err);
        else resolve(value.trim());
      };
      const timer = setTimeout(() => finish(new Error(`PiSugar socket timed out for: ${command}`)), 1500);
      socket.setEncoding('utf8');
      socket.on('connect', () => socket.write(`${command}\n`));
      socket.on('data', (chunk) => {
        output += chunk;
        const newline = output.indexOf('\n');
        if (newline >= 0) finish(null, output.slice(0, newline));
      });
      socket.on('error', (err) => finish(err));
      socket.on('end', () => finish(null, output));
    });
  }

  async #pollBattery() {
    const cfg = this.#cfg.pisugar || {};
    if (cfg.enabled === false || this.#batteryPolling) return;
    this.#batteryPolling = true;

    try {
      const read = async (name) => {
        const response = await this.#pisugarCommand(`get ${name}`);
        const match = response.match(/:\s*(.+?)\s*$/);
        if (!match) throw new Error(`Unexpected PiSugar response for get ${name}: ${response}`);
        return match[1];
      };
      const [percentText, voltageText, pluggedText, allowChargingText] = await Promise.all([
        read('battery'), read('battery_v'), read('battery_power_plugged'), read('battery_allow_charging'),
      ]);
      const percent = Number(percentText);
      const voltageV = Number(voltageText);
      const powerPlugged = pluggedText.toLowerCase() === 'true';
      const chargingAllowed = allowChargingText.toLowerCase() === 'true';
      if (!Number.isFinite(percent) || !Number.isFinite(voltageV)) throw new Error('PiSugar returned invalid battery values');

      this.#battery = {
        state: 'ok', available: true, source: 'pisugar-server',
        socketPath: cfg.socketPath || '/tmp/pisugar-server.sock',
        voltageV: Math.round(voltageV * 1000) / 1000,
        percent: Math.round(percent), percentEstimate: Math.round(percent), percentIsEstimate: false,
        powerPlugged, chargingAllowed, charging: powerPlugged && chargingAllowed,
        updatedAt: new Date().toISOString(), error: null,
      };
      this.emit('battery', this.getBattery());
    } catch (err) {
      const message = err.code === 'ENOENT'
        ? `PiSugar socket unavailable (${cfg.socketPath || '/tmp/pisugar-server.sock'}); is pisugar-server running?`
        : err.message;
      const shouldLog = this.#battery.error !== message;
      this.#battery = {
        ...this.#battery,
        state: this.#battery.updatedAt ? 'stale' : 'unavailable',
        available: Boolean(this.#battery.updatedAt),
        source: 'pisugar-server', socketPath: cfg.socketPath || '/tmp/pisugar-server.sock', error: message,
      };
      if (shouldLog) this.#log.warn(`PiSugar battery read failed: ${message}`);
    } finally {
      this.#batteryPolling = false;
    }
  }

  // ───────────────────────────── sensor ─────────────────────────────

  #transitionSensor(next, reason) {
    const s = this.#sensor;
    if (s.state === next) return;
    if (!SENSOR_TRANSITIONS[s.state]?.includes(next)) {
      this.#log.debug(`ignored illegal sensor transition ${s.state} -> ${next}`);
      return;
    }
    const prev = s.state;
    s.state = next;
    if (next === 'running') s.runningSince = Date.now();
    this.#log.info(`sensor ${prev} -> ${next}${reason ? ` (${reason})` : ''}`);
    this.emit('sensor-state', { state: next, prev, reason });
  }

  #startSensor() {
    if (this.#stopping) return;
    const s = this.#sensor;
    this.#transitionSensor('starting');
    s.startedAt = Date.now();
    s.lastSampleAt = 0;
    s.runningSince = 0;

    let proc;
    try {
      proc = spawn(this.#cfg.python, [this.#cfg.script], {
        stdio: ['ignore', 'pipe', 'pipe'],
        env: { ...process.env, BNO_ADDR: String(this.#cfg.bnoAddress), BNO_HZ: String(this.#cfg.bnoHz) },
      });
    } catch (err) {
      this.#sensorDied(null, `spawn failed: ${err.message}`);
      return;
    }
    s.proc = proc;

    readline.createInterface({ input: proc.stdout }).on('line', (l) => this.#onSensorLine(l));
    readline.createInterface({ input: proc.stderr }).on('line', (l) => this.#onSensorStderr(l));
    proc.on('error', (err) => this.#sensorDied(proc, `process error: ${err.message}`));
    proc.on('exit', (code, signal) => this.#sensorDied(proc, `exited (code=${code}, signal=${signal})`));
  }

  #onSensorLine(line) {
    let d;
    try {
      d = JSON.parse(line);
    } catch {
      this.#log.debug(`non-JSON from sensor: ${line.slice(0, 120)}`);
      return;
    }
    if (![d.yaw, d.pitch, d.roll].every(isFiniteNum)) return;
    const q = Array.isArray(d.quat) && d.quat.length === 4 && d.quat.every(isFiniteNum)
      ? d.quat
      : eulerToQuat(d.yaw, d.pitch, d.roll);

    const now = Date.now();
    this.#orientation = { yaw: d.yaw, pitch: d.pitch, roll: d.roll, quat: q, ts: now };
    this.#sensor.lastSampleAt = now;

    const st = this.#sensor.state;
    if (st === 'starting' || st === 'stale') this.#transitionSensor('running', 'receiving samples');
    this.emit('orientation', this.#orientation);
  }

  #onSensorStderr(line) {
    let msg = line;
    let level = 'warn';
    try {
      const d = JSON.parse(line);
      if (d.error) {
        level = 'error';
        msg = d.error;
        this.#sensor.lastError = String(d.error);
      } else if (d.warning) {
        msg = d.warning;
      }
    } catch {
      /* plain traceback line */
    }
    this.#log[level](`bno: ${msg}`);
  }

  /** Called when the python process exits/errors. `proc` guards against double-handling. */
  #sensorDied(proc, reason) {
    const s = this.#sensor;
    if (proc && s.proc !== proc) return; // already handled
    s.proc = null;
    if (this.#stopping) return;

    s.lastError = reason;
    // A process that ran healthily for a while resets the backoff.
    if (s.runningSince && Date.now() - s.runningSince > 15000) s.failures = 0;
    s.failures += 1;
    s.restarts += 1;

    const delay = Math.min(30000, 1000 * 2 ** Math.min(s.failures - 1, 5));
    this.#transitionSensor('backoff', `${reason}; retry in ${delay / 1000}s`);
    clearTimeout(s.backoffTimer);
    s.backoffTimer = setTimeout(() => this.#startSensor(), delay);
  }

  // ───────────────────────────── gpsd ─────────────────────────────

  #connectGps() {
    if (this.#stopping) return;
    const g = this.#gps;
    const { gpsdHost: host, gpsdPort: port } = this.#cfg;
    const sock = net.createConnection({ host, port });
    sock.setEncoding('utf8');
    sock.setKeepAlive(true, 10000);
    g.socket = sock;

    let buf = '';
    sock.on('connect', () => {
      g.connected = true;
      g.connectFailures = 0;
      this.#log.info(`gpsd connected (${host}:${port})`);
      sock.write('?WATCH={"enable":true,"json":true}\n');
      this.#emitGpsStateIfChanged();
    });
    sock.on('data', (chunk) => {
      buf += chunk;
      let i;
      while ((i = buf.indexOf('\n')) >= 0) {
        const line = buf.slice(0, i).trim();
        buf = buf.slice(i + 1);
        if (line) this.#onGpsLine(line);
      }
      if (buf.length > 1_000_000) buf = '';
    });
    sock.on('error', (err) => {
      // Don't spam the dashboard if gpsd simply isn't running.
      if (g.connectFailures === 0 || g.connectFailures % 20 === 0) {
        this.#log.warn(`gpsd connection error: ${err.code || err.message}`);
      }
    });
    sock.on('close', () => {
      if (g.socket === sock) g.socket = null;
      const wasConnected = g.connected;
      g.connected = false;
      g.connectFailures += 1;
      if (wasConnected) this.#log.warn('gpsd disconnected');
      this.#emitGpsStateIfChanged();
      if (this.#stopping) return;
      clearTimeout(g.retryTimer);
      g.retryTimer = setTimeout(() => this.#connectGps(), Math.min(10000, 2000 + g.connectFailures * 1000));
    });
  }

  #onGpsLine(line) {
    let d;
    try {
      d = JSON.parse(line);
    } catch {
      return;
    }
    const g = this.#gps;
    if (d.class === 'TPV') {
      const now = Date.now();
      const tpv = { ...d, receivedAt: now };
      if (d.time) {
        const t = Date.parse(d.time);
        if (Number.isFinite(t)) tpv.skewMs = Math.round(t - now);
      }
      g.tpv = tpv;
      g.lastTpvAt = now;
      if (d.mode >= 2 && isFiniteNum(d.lat) && isFiniteNum(d.lon)) {
        g.lastGoodFix = {
          lat: d.lat,
          lon: d.lon,
          altM: [d.altMSL, d.altHAE, d.alt].find(isFiniteNum) ?? null,
          mode: d.mode,
          at: now,
        };
        this.emit('gps', this.getPosition());
      }
      this.#emitGpsStateIfChanged();
    } else if (d.class === 'SKY') {
      const sats = Array.isArray(d.satellites) ? d.satellites : null;
      g.sky = {
        used: d.uSat ?? sats?.filter((s) => s.used).length ?? null,
        seen: d.nSat ?? sats?.length ?? null,
        hdop: d.hdop ?? null,
        vdop: d.vdop ?? null,
        pdop: d.pdop ?? null,
      };
    }
  }

  /** disconnected | no_gpsd_data | no_fix | fix_2d | fix_3d */
  #gpsStateNow() {
    const g = this.#gps;
    if (!g.connected) return 'disconnected';
    if (!g.tpv) return 'no_gpsd_data';
    if (Date.now() - g.lastTpvAt > this.#cfg.gpsStaleMs) return 'no_gpsd_data';
    if (g.tpv.mode === 3) return 'fix_3d';
    if (g.tpv.mode === 2) return 'fix_2d';
    return 'no_fix';
  }

  #emitGpsStateIfChanged() {
    const now = this.#gpsStateNow();
    if (now === this.#gps.emittedState) return;
    const prev = this.#gps.emittedState;
    this.#gps.emittedState = now;
    this.#log.info(`gps ${prev} -> ${now}`);
    this.emit('gps-state', { state: now, prev });
  }

  // ───────────────────────────── watchdog ─────────────────────────────

  #tick() {
    const now = Date.now();
    const s = this.#sensor;
    const { staleMs, restartAfterMs, startTimeoutMs } = this.#cfg;

    if (s.state === 'running' && now - s.lastSampleAt > staleMs) {
      this.#transitionSensor('stale', `no sample for ${now - s.lastSampleAt} ms`);
    } else if (s.state === 'stale' && now - s.lastSampleAt > restartAfterMs) {
      this.#log.warn('sensor stale too long; restarting python');
      s.proc?.kill('SIGKILL'); // 'exit' handler moves us to backoff
    } else if (s.state === 'starting' && now - s.startedAt > startTimeoutMs) {
      this.#log.warn('sensor produced no data after start; restarting python');
      s.proc?.kill('SIGKILL');
    }

    this.#emitGpsStateIfChanged(); // catches "TPV stopped arriving"
  }
}