import type { ReachyMiniInstance } from "@pollen-robotics/reachy-mini-sdk";

/** Client-side tweened gestures (guide §14): requestAnimationFrame keyframes
 *  fed to setHeadRpyDeg / setAntennasDeg / setBodyYawDeg. Degrees stay at this
 *  boundary; the SDK converts below. One gesture at a time — a new gesture
 *  cancels the running one and starts from neutral-ish interpolation. */

interface Pose {
  roll: number;
  pitch: number;
  yaw: number;
  antennaRight: number;
  antennaLeft: number;
  bodyYaw: number;
}

interface Keyframe extends Partial<Pose> {
  /** Seconds from gesture start. */
  at: number;
}

const NEUTRAL: Pose = { roll: 0, pitch: 0, yaw: 0, antennaRight: 0, antennaLeft: 0, bodyYaw: 0 };

function easeInOut(u: number): number {
  return u < 0.5 ? 2 * u * u : 1 - (-2 * u + 2) ** 2 / 2;
}

export class RobotGestures {
  private reachy: ReachyMiniInstance;
  private rafId: number | null = null;
  private current: Pose = { ...NEUTRAL };

  constructor(reachy: ReachyMiniInstance) {
    this.reachy = reachy;
  }

  cancel(): void {
    if (this.rafId !== null) {
      cancelAnimationFrame(this.rafId);
      this.rafId = null;
    }
  }

  private apply(pose: Pose): void {
    this.current = pose;
    this.reachy.setHeadRpyDeg(pose.roll, pose.pitch, pose.yaw);
    this.reachy.setAntennasDeg(pose.antennaRight, pose.antennaLeft);
    this.reachy.setBodyYawDeg(pose.bodyYaw);
  }

  /** Play keyframes; resolves when done. A new call cancels the previous one. */
  private play(frames: Keyframe[]): Promise<void> {
    this.cancel();
    const start = performance.now();
    const from = { ...this.current };
    // Prepend the current pose so the gesture blends in from wherever we are.
    const timeline: (Pose & { at: number })[] = [{ ...from, at: 0 }];
    for (const frame of frames) {
      const prev = timeline[timeline.length - 1];
      timeline.push({ ...prev, ...frame });
    }

    return new Promise((resolve) => {
      const tick = (now: number) => {
        const tSec = (now - start) / 1000;
        const last = timeline[timeline.length - 1];
        if (tSec >= last.at) {
          this.apply({ ...last });
          this.rafId = null;
          resolve();
          return;
        }
        let next = 1;
        while (next < timeline.length - 1 && timeline[next].at < tSec) next++;
        const a = timeline[next - 1];
        const b = timeline[next];
        const span = Math.max(b.at - a.at, 1e-6);
        const u = easeInOut(Math.min(Math.max((tSec - a.at) / span, 0), 1));
        this.apply({
          roll: a.roll + (b.roll - a.roll) * u,
          pitch: a.pitch + (b.pitch - a.pitch) * u,
          yaw: a.yaw + (b.yaw - a.yaw) * u,
          antennaRight: a.antennaRight + (b.antennaRight - a.antennaRight) * u,
          antennaLeft: a.antennaLeft + (b.antennaLeft - a.antennaLeft) * u,
          bodyYaw: a.bodyYaw + (b.bodyYaw - a.bodyYaw) * u,
        });
        this.rafId = requestAnimationFrame(tick);
      };
      this.rafId = requestAnimationFrame(tick);
    });
  }

  /** Quick antenna wiggle — an item was added to the inventory. */
  antennaWiggle(): Promise<void> {
    return this.play([
      { at: 0.15, antennaRight: 25, antennaLeft: -25 },
      { at: 0.35, antennaRight: -25, antennaLeft: 25 },
      { at: 0.55, antennaRight: 25, antennaLeft: -25 },
      { at: 0.75, antennaRight: -25, antennaLeft: 25 },
      { at: 1.0, antennaRight: 0, antennaLeft: 0 },
    ]);
  }

  /** Happy nod with a little body sway — a meal plan was generated. */
  happyDance(): Promise<void> {
    return this.play([
      { at: 0.3, pitch: 12, antennaRight: 20, antennaLeft: 20 },
      { at: 0.6, pitch: -8, bodyYaw: 12, antennaRight: -10, antennaLeft: -10 },
      { at: 0.9, pitch: 12, bodyYaw: -12, antennaRight: 20, antennaLeft: 20 },
      { at: 1.2, pitch: -8, bodyYaw: 12, antennaRight: -10, antennaLeft: -10 },
      { at: 1.5, pitch: 12, bodyYaw: -12, antennaRight: 20, antennaLeft: 20 },
      { at: 2.0, pitch: 0, bodyYaw: 0, antennaRight: 0, antennaLeft: 0 },
    ]);
  }

  /** Concerned head tilt — something is about to expire. */
  concernedTilt(): Promise<void> {
    return this.play([
      { at: 0.4, roll: 16, pitch: 8, antennaRight: -15, antennaLeft: -15 },
      { at: 1.3, roll: 16, pitch: 8 },
      { at: 1.8, roll: 0, pitch: 0, antennaRight: 0, antennaLeft: 0 },
    ]);
  }

  /** Sweep look left-right — "scanning the kitchen" while generating lists. */
  sweepLook(): Promise<void> {
    return this.play([
      { at: 0.5, yaw: 35, pitch: 6 },
      { at: 1.1, yaw: 35 },
      { at: 1.8, yaw: -35 },
      { at: 2.4, yaw: -35 },
      { at: 3.0, yaw: 0, pitch: 0 },
    ]);
  }

  /** Small greeting on connect. */
  greet(): Promise<void> {
    return this.play([
      { at: 0.3, pitch: 10, antennaRight: 30, antennaLeft: 5 },
      { at: 0.6, antennaRight: -10 },
      { at: 0.9, antennaRight: 30 },
      { at: 1.3, pitch: 0, antennaRight: 0, antennaLeft: 0 },
    ]);
  }
}