import fixture from '../../../../contracts/fixtures/lekiwi-kinematics.json';
import { bodyToWheels, LEKIWI_PROFILE } from './lekiwi';

it('matches the real LeRobot 0.6.1 forward kinematics and proportional saturation within half an encoder tick', () => {
  expect(LEKIWI_PROFILE.wheels).toEqual(fixture.wheelOrder);
  const radPerTick = (2 * Math.PI) / 4096;
  for (const { body, wheelRaw } of fixture.cases) {
    const wheel = bodyToWheels(body[0], body[1], (body[2] * Math.PI) / 180);
    wheel.forEach((value, i) =>
      expect(Math.abs(value / radPerTick - wheelRaw[i])).toBeLessThanOrEqual(0.500001),
    );
  }
});