saltylab-firmware/ui/social-bot/src/hooks/useSettings.js

/**
 * useSettings.js — ROS2 parameter I/O + settings persistence + validation.
 *
 * Wraps rcl_interfaces/srv/GetParameters and SetParameters calls via
 * the rosbridge callService helper passed from useRosbridge.
 *
 * Known nodes and their parameters:
 *   adaptive_pid_node   — PID gains (empty/light/heavy), clamp bounds, overrides
 *   rtsp_server_node    — video fps, bitrate, resolution, RTSP port
 *   uwb_driver_node     — baudrate, range limits, Kalman filter noise
 *
 * Validation rules:
 *   kp_min 5–40, ki_min 0–5, kd_min 0–10
 *   override gains must be within [min, max]
 *   control_rate: 50–200 Hz
 *   balance_setpoint_rad: ±0.1 rad
 *
 * PID step-response simulator (pure JS, no ROS):
 *   Inverted pendulum: θ''(t) = g/L · θ(t) − 1/m·L² · u(t)
 *   Parameters: L≈0.40 m, m≈3 kg, g=9.81
 *   Simulate 120 steps at dt=0.02 s (2.4 s total) with 5° step input.
 */

import { useState, useCallback } from 'react';

const STORAGE_KEY = 'saltybot_settings_v1';

// ── Parameter catalogue ────────────────────────────────────────────────────────

export const PID_NODE = 'adaptive_pid_node';

export const PID_PARAMS = [
  { key: 'kp_empty',  default: 15.0, min: 0, max: 60, step: 0.5,  label: 'Kp empty' },
  { key: 'ki_empty',  default:  0.5, min: 0, max: 10, step: 0.05, label: 'Ki empty' },
  { key: 'kd_empty',  default:  1.5, min: 0, max: 20, step: 0.1,  label: 'Kd empty' },
  { key: 'kp_light',  default: 18.0, min: 0, max: 60, step: 0.5,  label: 'Kp light' },
  { key: 'ki_light',  default:  0.6, min: 0, max: 10, step: 0.05, label: 'Ki light' },
  { key: 'kd_light',  default:  2.0, min: 0, max: 20, step: 0.1,  label: 'Kd light' },
  { key: 'kp_heavy',  default: 22.0, min: 0, max: 60, step: 0.5,  label: 'Kp heavy' },
  { key: 'ki_heavy',  default:  0.8, min: 0, max: 10, step: 0.05, label: 'Ki heavy' },
  { key: 'kd_heavy',  default:  2.5, min: 0, max: 20, step: 0.1,  label: 'Kd heavy' },
  { key: 'kp_min',    default:  5.0, min: 0, max: 60, step: 0.5,  label: 'Kp min (clamp)' },
  { key: 'kp_max',    default: 40.0, min: 0, max: 80, step: 0.5,  label: 'Kp max (clamp)' },
  { key: 'ki_min',    default:  0.0, min: 0, max: 10, step: 0.05, label: 'Ki min (clamp)' },
  { key: 'ki_max',    default:  5.0, min: 0, max: 20, step: 0.05, label: 'Ki max (clamp)' },
  { key: 'kd_min',    default:  0.0, min: 0, max: 20, step: 0.1,  label: 'Kd min (clamp)' },
  { key: 'kd_max',    default: 10.0, min: 0, max: 40, step: 0.1,  label: 'Kd max (clamp)' },
  { key: 'override_enabled', default: false, type: 'bool', label: 'Override gains' },
  { key: 'override_kp', default: 15.0, min: 0, max: 60, step: 0.5,  label: 'Override Kp' },
  { key: 'override_ki', default:  0.5, min: 0, max: 10, step: 0.05, label: 'Override Ki' },
  { key: 'override_kd', default:  1.5, min: 0, max: 20, step: 0.1,  label: 'Override Kd' },
  { key: 'control_rate',         default: 100.0, min: 50, max: 200, step: 5, label: 'Control rate (Hz)' },
  { key: 'balance_setpoint_rad', default: 0.0,   min: -0.1, max: 0.1, step: 0.001, label: 'Balance setpoint (rad)' },
];

export const SENSOR_PARAMS = [
  { node: 'rtsp_server_node',  key: 'fps',         default: 15,    min: 5,  max: 60,   step: 1,   label: 'Camera FPS' },
  { node: 'rtsp_server_node',  key: 'bitrate_kbps',default: 4000,  min: 500,max: 20000,step: 500, label: 'Camera bitrate (kbps)' },
  { node: 'rtsp_server_node',  key: 'rtsp_port',   default: 8554,  min: 1024,max: 65535,step: 1,  label: 'RTSP port' },
  { node: 'rtsp_server_node',  key: 'use_nvenc',   default: true,  type: 'bool', label: 'NVENC hardware encode' },
  { node: 'uwb_driver_node',   key: 'max_range_m', default: 8.0,   min: 1,  max: 20,   step: 0.5, label: 'UWB max range (m)' },
  { node: 'uwb_driver_node',   key: 'kf_process_noise', default: 0.1, min: 0.001, max: 1.0, step: 0.001, label: 'UWB Kalman noise' },
];

// ── Validation ─────────────────────────────────────────────────────────────────

export function validatePID(gains) {
  const warnings = [];
  const { kp_empty, ki_empty, kd_empty, kp_max, ki_max, kd_max,
          override_enabled, override_kp, override_ki, override_kd,
          control_rate, balance_setpoint_rad } = gains;

  if (kp_empty > 35)
    warnings.push({ level: 'warn',  msg: `Kp empty (${kp_empty}) is high — risk of oscillation.` });
  if (ki_empty > 3)
    warnings.push({ level: 'warn',  msg: `Ki empty (${ki_empty}) is high — risk of integral windup.` });
  if (kp_empty > kp_max)
    warnings.push({ level: 'error', msg: `Kp empty (${kp_empty}) exceeds kp_max (${kp_max}).` });
  if (ki_empty > ki_max)
    warnings.push({ level: 'error', msg: `Ki empty (${ki_empty}) exceeds ki_max (${ki_max}).` });
  if (kp_empty > 0 && kd_empty / kp_empty < 0.05)
    warnings.push({ level: 'warn',  msg: `Low Kd/Kp ratio — under-damped response likely.` });
  if (override_enabled) {
    if (override_kp > kp_max)
      warnings.push({ level: 'error', msg: `Override Kp (${override_kp}) exceeds kp_max (${kp_max}).` });
    if (override_ki > ki_max)
      warnings.push({ level: 'error', msg: `Override Ki (${override_ki}) exceeds ki_max (${ki_max}).` });
    if (override_kd > kd_max)
      warnings.push({ level: 'error', msg: `Override Kd (${override_kd}) exceeds kd_max (${kd_max}).` });
  }
  if (control_rate > 150)
    warnings.push({ level: 'warn', msg: `Control rate ${control_rate} Hz — ensure STM32 UART can keep up.` });
  if (Math.abs(balance_setpoint_rad) > 0.05)
    warnings.push({ level: 'warn', msg: `Setpoint |${balance_setpoint_rad?.toFixed(3)}| rad > 3° — intentional lean?` });
  return warnings;
}

// ── Step-response simulation ───────────────────────────────────────────────────

export function simulateStepResponse(kp, ki, kd) {
  const dt   = 0.02;
  const N    = 120;
  const g    = 9.81;
  const L    = 0.40;
  const m    = 3.0;
  const step = 5 * Math.PI / 180;

  let theta = step;
  let omega = 0;
  let integral = 0;
  let prevErr = theta;
  const result = [];

  for (let i = 0; i < N; i++) {
    const t   = i * dt;
    const err = -theta;
    integral += err * dt;
    integral  = Math.max(-2, Math.min(2, integral));
    const deriv = (err - prevErr) / dt;
    prevErr = err;

    const u = kp * err + ki * integral + kd * deriv;
    const alpha = (g / L) * theta - u / (m * L * L);
    omega += alpha * dt;
    theta += omega * dt;

    result.push({ t, theta: theta * 180 / Math.PI, u: Math.min(100, Math.max(-100, u)) });

    if (Math.abs(theta) > Math.PI / 2) {
      for (let j = i + 1; j < N; j++)
        result.push({ t: j * dt, theta: theta > 0 ? 90 : -90, u: 0 });
      break;
    }
  }
  return result;
}

// ── Hook ──────────────────────────────────────────────────────────────────────

export function useSettings({ callService, subscribe } = {}) {
  const [gains, setGains] = useState(() => {
    try {
      const s = JSON.parse(localStorage.getItem(STORAGE_KEY));
      return s?.gains ?? Object.fromEntries(PID_PARAMS.map(p => [p.key, p.default]));
    } catch { return Object.fromEntries(PID_PARAMS.map(p => [p.key, p.default])); }
  });
  const [sensors, setSensors] = useState(() => {
    try {
      const s = JSON.parse(localStorage.getItem(STORAGE_KEY));
      return s?.sensors ?? Object.fromEntries(SENSOR_PARAMS.map(p => [p.key, p.default]));
    } catch { return Object.fromEntries(SENSOR_PARAMS.map(p => [p.key, p.default])); }
  });
  const [firmwareInfo, setFirmwareInfo] = useState(null);
  const [applying,     setApplying]     = useState(false);
  const [applyResult,  setApplyResult]  = useState(null);

  const persist = useCallback((newGains, newSensors) => {
    localStorage.setItem(STORAGE_KEY, JSON.stringify({
      gains:   newGains   ?? gains,
      sensors: newSensors ?? sensors,
      savedAt: Date.now(),
    }));
  }, [gains, sensors]);

  const buildSetRequest = useCallback((params) => ({
    parameters: params.map(({ name, value }) => {
      let type = 3; let v = { double_value: value };
      if (typeof value === 'boolean') { type = 1; v = { bool_value: value }; }
      else if (Number.isInteger(value)) { type = 2; v = { integer_value: value }; }
      return { name, value: { type, ...v } };
    }),
  }), []);

  const applyPIDGains = useCallback(async (overrideGains) => {
    const toApply = overrideGains ?? gains;
    setApplying(true); setApplyResult(null);
    const params = PID_PARAMS.map(p => ({ name: p.key, value: toApply[p.key] ?? p.default }));
    if (!callService) {
      setGains(toApply); persist(toApply, null); setApplying(false);
      setApplyResult({ ok: true, msg: 'Saved locally (not connected)' }); return;
    }
    try {
      await new Promise((resolve, reject) => {
        callService(`/${PID_NODE}/set_parameters`, 'rcl_interfaces/srv/SetParameters',
          buildSetRequest(params), (res) => {
            res.results?.every(r => r.successful) ? resolve() :
              reject(new Error(res.results?.find(r => !r.successful)?.reason ?? 'failed'));
          });
        setTimeout(() => reject(new Error('timeout')), 5000);
      });
      setGains(toApply); persist(toApply, null);
      setApplyResult({ ok: true, msg: 'Parameters applied to adaptive_pid_node' });
    } catch (e) {
      setApplyResult({ ok: false, msg: String(e.message) });
    }
    setApplying(false);
  }, [gains, callService, buildSetRequest, persist]);

  const applySensorParams = useCallback(async (overrideParams) => {
    const toApply = overrideParams ?? sensors;
    setApplying(true); setApplyResult(null);
    if (!callService) {
      setSensors(toApply); persist(null, toApply); setApplying(false);
      setApplyResult({ ok: true, msg: 'Saved locally (not connected)' }); return;
    }
    const byNode = {};
    SENSOR_PARAMS.forEach(p => {
      if (!byNode[p.node]) byNode[p.node] = [];
      byNode[p.node].push({ name: p.key, value: toApply[p.key] ?? p.default });
    });
    try {
      for (const [node, params] of Object.entries(byNode)) {
        await new Promise((resolve, reject) => {
          callService(`/${node}/set_parameters`, 'rcl_interfaces/srv/SetParameters',
            buildSetRequest(params), resolve);
          setTimeout(() => reject(new Error(`timeout on ${node}`)), 5000);
        });
      }
      setSensors(toApply); persist(null, toApply);
      setApplyResult({ ok: true, msg: 'Sensor parameters applied' });
    } catch (e) { setApplyResult({ ok: false, msg: String(e.message) }); }
    setApplying(false);
  }, [sensors, callService, buildSetRequest, persist]);

  const startFirmwareWatch = useCallback(() => {
    if (!subscribe) return () => {};
    return subscribe('/diagnostics', 'diagnostic_msgs/DiagnosticArray', (msg) => {
      const info = {};
      for (const status of msg.status ?? []) {
        for (const kv of status.values ?? []) {
          if (kv.key === 'stm32_fw_version') info.stm32Version  = kv.value;
          if (kv.key === 'jetson_sw_version') info.jetsonVersion = kv.value;
          if (kv.key === 'last_ota_date')     info.lastOtaDate   = kv.value;
          if (kv.key === 'jetson_hostname')   info.hostname      = kv.value;
          if (kv.key === 'ros_distro')        info.rosDistro     = kv.value;
          if (kv.key === 'uptime_s')          info.uptimeS       = parseFloat(kv.value);
        }
      }
      if (Object.keys(info).length > 0) setFirmwareInfo(fi => ({ ...fi, ...info }));
    });
  }, [subscribe]);

  const exportSettingsJSON = useCallback(() =>
    JSON.stringify({ gains, sensors, exportedAt: new Date().toISOString() }, null, 2),
  [gains, sensors]);

  const importSettingsJSON = useCallback((json) => {
    const data = JSON.parse(json);
    if (data.gains)   { setGains(data.gains);    persist(data.gains, null); }
    if (data.sensors) { setSensors(data.sensors); persist(null, data.sensors); }
  }, [persist]);

  const exportDiagnosticsBundle = useCallback((robotData, connections) => {
    const bundle = {
      exportedAt: new Date().toISOString(),
      settings: { gains, sensors },
      firmwareInfo,
      fleet: Object.entries(connections ?? {}).map(([id, c]) => ({ id, ...c, data: robotData?.[id] })),
    };
    const a = document.createElement('a');
    a.href = URL.createObjectURL(new Blob([JSON.stringify(bundle, null, 2)], { type: 'application/json' }));
    a.download = `saltybot-diagnostics-${Date.now()}.json`;
    a.click();
  }, [gains, sensors, firmwareInfo]);

  return {
    gains, setGains, sensors, setSensors, firmwareInfo,
    applying, applyResult,
    applyPIDGains, applySensorParams, startFirmwareWatch,
    exportSettingsJSON, importSettingsJSON, exportDiagnosticsBundle,
    validate: () => validatePID(gains),
  };
}