/** Behavioural reference ONLY. Not connected to the current VESC PPM path. */
import {axisKeys, bounded, mix, parseProfile, type Axes, type ControlProfile, type Sides} from './profile';

export interface Sample { value: number; at: number; sequence: number }
export interface DriveEvidence { at: number; healthy: boolean; stopped: boolean }
export interface CoreCommand {
  token: string; sequence: number; at: number; expires: number; demand: Sides;
}
export interface Observation {
  now: number;
  // These are trusted receiver timestamps and link status, NOT USB read times.
  link: { state: 'live' | 'lost' | 'unknown'; at: number };
  axes: Partial<Record<keyof Axes, Sample>>;
  drives: Record<string, DriveEvidence>;
  command?: CoreCommand;
  requestCore?: {owner: 'remote' | 'autonomy'; sequence: number; at: number};
}
export interface Policy {
  maxAgeMs: number; neutralMs: number; maxCommandMs: number;
  /** Verified RC controls that can take over, including non-driving stick axes. */
  monitoredAxes: readonly (keyof Axes)[];
}
export type State = 'hold' | 'rc-ready' | 'rc-manual' | 'core';
export interface Decision {
  state: State; reason: string; demand: Sides; token: string | null;
  owner: 'remote' | 'autonomy' | 'rc' | null;
  stopAll: boolean; flushMotionQueue: boolean; cancelMotionTasks: boolean;
}
const zero = (): Sides => ({left: 0, right: 0});

export class AuthorityModel {
  private state: State = 'hold';
  private epoch = 0;
  private token: string | null = null;
  private owner: Decision['owner'] = null;
  private neutralSince: number | null = null;
  private lastNow = -Infinity;
  private lastCommand = -1;
  private lastRequest = -1;
  private samples: Partial<Record<keyof Axes, Sample>> = {};
  private readonly profile: ControlProfile;
  private readonly motors: string[];
  private readonly policy: Policy;

  constructor(profile: ControlProfile, motors: readonly string[], policy: Policy, private readonly bootId: string) {
    this.profile = parseProfile(profile);
    this.motors = [...motors]; this.policy = {...policy, monitoredAxes:[...policy.monitoredAxes]};
    if (!bootId || !motors.length || new Set(motors).size !== motors.length || motors.some(id => !id)
        || !bounded(policy.maxAgeMs, 1, 10000) || !bounded(policy.neutralMs, 1, 10000)
        || !bounded(policy.maxCommandMs, 1, 10000)
        || new Set(policy.monitoredAxes).size !== policy.monitoredAxes.length
        || policy.monitoredAxes.some(key => !['leftY','rightY','leftX','rightX'].includes(key))
        || axisKeys(this.profile).some(key => !policy.monitoredAxes.includes(key))) throw Error('Invalid authority policy');
  }
  private result(reason: string, demand = zero(), revoke = false): Decision {
    return {state: this.state, reason, demand, token: this.token, owner: this.owner,
      stopAll: this.state === 'hold', flushMotionQueue: revoke, cancelMotionTasks: revoke};
  }
  private hold(reason: string): Decision {
    const revoke = this.state === 'core';
    if (this.state !== 'hold') this.epoch++;
    this.state = 'hold'; this.token = null; this.owner = null;
    this.neutralSince = null; this.lastCommand = -1;
    return this.result(reason, zero(), revoke);
  }
  step(input: Observation): Decision {
    const now = input.now;
    if (!Number.isFinite(now) || now < 0 || now < this.lastNow) return this.hold('clock-invalid');
    const gap = now - this.lastNow; this.lastNow = now;
    const fresh = (at: number) => Number.isFinite(at) && at <= now && now - at <= this.policy.maxAgeMs;
    const request = input.requestCore;
    const newRequest = !!request && Number.isSafeInteger(request.sequence) && request.sequence > this.lastRequest;
    // Consume even a premature request: it must never become effective later.
    if (newRequest) this.lastRequest = request.sequence;
    // A gap cannot count towards continuous observed neutral.
    if (gap > this.policy.maxAgeMs) {
      const first = gap === Infinity;
      if (!first) return this.hold('observation-gap');
      this.neutralSince = null;
    }
    if (input.link?.state !== 'live' || !fresh(input.link.at)) return this.hold('receiver-unverified');
    const axes = {leftY: 0, rightY: 0, leftX: 0, rightX: 0};
    let neutral = true;
    let observedThrough = input.link.at;
    const candidates: Partial<Record<keyof Axes, Sample>> = {};
    for (const key of this.policy.monitoredAxes) {
      const sample = input.axes[key], prev = this.samples[key];
      if (!sample || !bounded(sample.value, -1, 1) || !fresh(sample.at)
          || !Number.isSafeInteger(sample.sequence) || sample.sequence < 0
          || (prev && (sample.sequence < prev.sequence || sample.at < prev.at
            || (sample.sequence === prev.sequence && (sample.value !== prev.value || sample.at !== prev.at)))))
        return this.hold('axis-invalid');
      candidates[key] = {...sample}; axes[key] = sample.value;
      observedThrough = Math.min(observedThrough, sample.at);
      neutral &&= Math.abs(sample.value) <= this.profile.deadband;
    }
    this.samples = candidates;
    let stopped = true;
    for (const id of this.motors) {
      const drive = input.drives[id];
      if (!drive || drive.healthy !== true || !fresh(drive.at)) return this.hold('drive-unverified');
      observedThrough = Math.min(observedThrough, drive.at);
      stopped &&= drive.stopped === true;
    }
    // RC intent is evaluated before any Core command or reacquisition request.
    if (this.state === 'core' && !neutral) return this.hold('rc-takeover');
    if (neutral && stopped) this.neutralSince ??= now;
    else this.neutralSince = null;
    const stableNeutral = this.neutralSince !== null && observedThrough - this.neutralSince >= this.policy.neutralMs;
    if (this.state === 'hold') {
      if (!stableNeutral) return this.result(stopped ? 'await-neutral' : 'await-stop');
      this.state = 'rc-ready'; this.owner = 'rc';
      // A request queued before reaching neutral cannot acquire Core authority.
      return this.result('rc-ready');
    }
    if (newRequest && request && fresh(request.at) && this.state !== 'core' && stableNeutral) {
      if (!['remote', 'autonomy'].includes(request.owner)) return this.hold('source-invalid');
      this.epoch++; this.token = `${this.bootId}:${this.epoch}:${this.profile.revision}`;
      this.owner = request.owner; this.state = 'core'; this.lastCommand = -1;
      return this.result('core-granted');
    }
    if (this.state === 'core') {
      const c = input.command;
      if (!c || c.token !== this.token || !Number.isSafeInteger(c.sequence) || c.sequence <= this.lastCommand
          || !fresh(c.at) || !Number.isFinite(c.expires) || c.expires <= now
          || c.expires - c.at > this.policy.maxCommandMs || c.expires < c.at
          || !bounded(c.demand.left, -1, 1) || !bounded(c.demand.right, -1, 1)) return this.hold('core-command-invalid');
      this.lastCommand = c.sequence;
      return this.result('core-command', {left: c.demand.left * this.profile.outputScale, right: c.demand.right * this.profile.outputScale});
    }
    if (this.state === 'rc-ready' && !neutral) this.state = 'rc-manual';
    return this.result(neutral ? 'rc-neutral' : 'rc-command', mix(this.profile, axes));
  }
}