import { describe, expect, test } from 'bun:test';

import { computeSegment, trajectory } from '../src/motion.js';

import { scurvePlan, scurveTime, striangle } from '../src/plan.js';

import { connected, rng } from './helpers.js';

const STEP_HR = 1 / 2032; // one motor-axis step, inches (16x microstepping)

describe('S-curve math', () => {

  test('distance to reach a speed, and speed reachable in a distance, are inverses', () => {

    const rand = rng(5);

    for (let i = 0; i < 200; i++) {

      const j = 1000 + rand() * 30000;

      const v0 = rand() * 3;

      const vMax = v0 + 0.01 + rand() * 6;

      const d = scurvePlan(v0, vMax, j, null);

      expect(scurvePlan(v0, vMax, j, d * 0.999999)).toBeLessThanOrEqual(vMax);

      expect(scurvePlan(v0, 1e9, j, d)).toBeCloseTo(vMax, 6);

      expect(scurveTime(v0, vMax, j)).toBeCloseTo(2 * Math.sqrt((vMax - v0) / j), 12);

    }

  });

  test('triangle peak speed covers the requested distance', () => {

    const j = 15000;

    const dist = 0.05;

    const v = striangle(0, 0, 8, j, dist);

    expect(2 * scurvePlan(0, v, j, null)).toBeCloseTo(dist, 4);

  });

});

describe('segment planning on a connected machine', () => {

  for (const handling of [1, 2, 3, 4]) {

    test(`random segments end exactly where planned (handling ${handling})`, async () => {

      const { nd } = await connected({}, (n) => { n.options.homing = false; n.options.handling = handling; });

      const rand = rng(100 + handling);

      const step = 1 / nd.stepScale; // one motor step, in inches of (x+y) or (x-y)

      for (let i = 0; i < 150; i++) {

        const x = rand() * 11.81;

        const y = rand() * 8.58;

        nd.pen.phys.zUp = rand() < 0.5;

        const [moves, data] = computeSegment(nd, [x, y, 0, 0, false]);

        if (moves == null) continue;

        const end = data[0];

        // Timed moves land within a couple of motor steps of the target (as in

        // the Python planner); the tracked end position itself is exact.

        expect(Math.abs((end.xpos + end.ypos) - (x + y))).toBeLessThanOrEqual(step * 2.5);

        expect(Math.abs((end.xpos - end.ypos) - (x - y))).toBeLessThanOrEqual(step * 2.5);

        for (const [kind, p] of moves) {

          expect(['T3', 'TD']).toContain(kind);

          for (const v of p) expect(Number.isInteger(v)).toBe(true);

          expect(p[0]).toBeGreaterThanOrEqual(1);

        }

        Object.assign(nd.pen.phys, { xpos: end.xpos, ypos: end.ypos, accum1: end.accum1, accum2: end.accum2 });

      }

    });

  }

  test('tiny moves under one step are skipped', async () => {

    const { nd } = await connected({}, (n) => { n.options.homing = false; });

    expect(computeSegment(nd, [STEP_HR / 10, 0, 0, 0, false])).toEqual([null, null]);

  });

  test('trajectory lowers, moves, and raises', async () => {

    const { nd } = await connected({}, (n) => { n.options.homing = false; });

    const t = trajectory(nd, [[0, 0], [1, 0], [1, 1], [0, 1], [0, 0]]);

    expect(t[0]).toEqual(['lower']);

    expect(t[t.length - 1]).toEqual(['raise']);

    const last = t[t.length - 2][2][1];

    expect(last.xpos).toBeCloseTo(0, 3);

    expect(last.ypos).toBeCloseTo(0, 3);

  });

  test('a path that trims to one usable segment is still drawn', async () => {

    // Vertices closer together than one step are dropped; one real segment remains.

    const { nd } = await connected({}, (n) => { n.options.homing = false; });

    const t = trajectory(nd, [[0, 0], [0.00001, 0], [1, 0]]);

    const motion = t.filter((m) => m[0] === 'T3' || m[0] === 'TD');

    expect(motion.length).toBeGreaterThan(0);

    expect(motion[motion.length - 1][2][1].xpos).toBeCloseTo(1, 3);

  });

  test('out-of-bounds targets are clamped and warned about', async () => {

    const { nd, messages } = await connected({}, (n) => { n.options.homing = false; });

    const [, data] = computeSegment(nd, [20, -3, 0, 0, false]);

    expect(data[0].xpos).toBeCloseTo(11.81, 3);

    expect(data[0].ypos).toBeCloseTo(0, 3);

    expect(messages.join(' ')).toContain('clipped');

  });

});