import { describe, expect, it } from "vitest";
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
import type { TrapProfileResult, TrapSample } from "../../src/kdl/types.js";

async function createRuntime() {
  const runtime = createKdlWorkerRuntime();
  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
  return runtime;
}

function expectMonotonic(samples: TrapSample[]) {
  for (let index = 1; index < samples.length; index += 1) {
    expect(samples[index]!.time).toBeGreaterThan(samples[index - 1]!.time);
    expect(samples[index]!.s).toBeGreaterThanOrEqual(samples[index - 1]!.s);
  }
}

describe("trapezoid velocity profile API", () => {
  it("creates a trapezoid profile when the path can reach max velocity", async () => {
    const runtime = await createRuntime();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 2,
      method: "makeTrapProfile",
      payload: [
        2,
        {
          maxVelocity: 1,
          maxAcceleration: 1,
          sampleTime: 0.25
        }
      ]
    });

    expect(response.ok).toBe(true);
    const profile = response.result as TrapProfileResult;
    expect(profile).toMatchObject({
      ok: true,
      type: "trapezoid",
      length: 2,
      duration: 3,
      tAccel: 1,
      tConst: 1,
      tDecel: 1,
      vPeak: 1,
      diagnostics: []
    });
    expect(profile.samples[0]).toMatchObject({ index: 0, time: 0, s: 0 });
    expect(profile.samples.at(-1)).toMatchObject({ time: 3, s: 1 });
    expect(profile.samples.find((sample) => sample.time === 1)?.s).toBeCloseTo(0.25);
    expect(profile.samples.find((sample) => sample.time === 1)?.sd).toBeCloseTo(0.5);
    expectMonotonic(profile.samples);
  });

  it("falls back to a triangle profile for short paths", async () => {
    const runtime = await createRuntime();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "makeTrapProfile",
      payload: [
        0.5,
        {
          maxVelocity: 2,
          maxAcceleration: 1,
          sampleTime: 0.1
        }
      ]
    });

    expect(response.ok).toBe(true);
    const profile = response.result as TrapProfileResult;
    expect(profile.type).toBe("triangle");
    expect(profile.tConst).toBe(0);
    expect(profile.vPeak).toBeCloseTo(Math.sqrt(0.5));
    expect(profile.duration).toBeCloseTo(2 * Math.sqrt(0.5));
    expect(profile.samples[0]?.s).toBe(0);
    expect(profile.samples.at(-1)?.s).toBe(1);
    expect(profile.diagnostics).toMatchObject([
      {
        severity: "info",
        code: "KDL_TRAP_TRIANGLE_PROFILE"
      }
    ]);
    expectMonotonic(profile.samples);
  });

  it("returns samples from sampleTrapProfile with strict endpoint samples", async () => {
    const runtime = await createRuntime();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 4,
      method: "sampleTrapProfile",
      payload: [
        1,
        {
          maxVelocity: 1,
          maxAcceleration: 2,
          sampleTime: 0.2
        }
      ]
    });

    expect(response.ok).toBe(true);
    const samples = response.result as TrapSample[];
    expect(samples[0]).toMatchObject({ index: 0, time: 0, s: 0 });
    expect(samples.at(-1)?.s).toBe(1);
    expect(samples.at(-1)?.time).toBeCloseTo(1.5);
    expectMonotonic(samples);
  });

  it("returns a structured diagnostic for invalid trap profile inputs", async () => {
    const runtime = await createRuntime();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 5,
      method: "makeTrapProfile",
      payload: [
        1,
        {
          maxVelocity: 0,
          maxAcceleration: 1,
          sampleTime: 0.01
        }
      ]
    });

    expect(response).toMatchObject({
      ok: false,
      error: {
        code: "KDL_INVALID_TRAP_PROFILE",
        diagnostics: [
          {
            severity: "error",
            code: "KDL_INVALID_TRAP_PROFILE"
          }
        ]
      }
    });
  });
});