const Robot = require('../robot/kinematics/Arm3SegmentLinearX');
const MotorPosition = require('../robot/RobotMotorPosition');

describe('Robot.calculateSpeeds (ToDo_6a)', () => {

  beforeAll(() => {
    jest.spyOn(console, 'log').mockImplementation(() => {});
  });
  afterAll(() => {
    jest.restoreAllMocks();
  });

  function makeRobot() {
    const robot = new Robot(250, 264, 100);
    robot.useSpeedCalc = true;
    robot.feedrate = 1000;
    return robot;
  }

  test('erzeugt keine NaN-Werte (Property-Bug behoben) und setzt moveTime', () => {
    const robot = makeRobot();
    robot.xMotor = 10; robot.alpha = 1; robot.beta = 1;
    robot.a = 0.5; robot.b = 0.5; robot.c = 0.5; robot.eMotor = 0.2;

    const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0);
    const newPos = new MotorPosition(10, 1, 1, 0.5, 0.5, 0.5, 0.2);

    robot.calculateSpeeds(oldPos, newPos);

    for (const k of ['x', 'y', 'z', 'a', 'b', 'c', 'e']) {
      expect(Number.isFinite(robot.motorSpeeds[k])).toBe(true);
    }
    expect(robot.motorSpeeds.x).toBeGreaterThan(0);
    expect(robot.lastMoveTime).toBeGreaterThan(0);
  });

  test('exakte Werte: motorSpeeds = Delta/Zeit, moveTime = Distanz/Feedrate', () => {
    const robot = makeRobot();
    // reine X-Motor-Bewegung um 30 mm
    robot.xMotor = 30; robot.alpha = 0; robot.beta = 0;

    const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0);
    const newPos = new MotorPosition(30, 0, 0, 0, 0, 0, 0);

    robot.calculateSpeeds(oldPos, newPos);

    // Distanz = 30, feedrate = 1000 → moveTime = 0.03 min
    expect(robot.lastMoveTime).toBeCloseTo(0.03, 6);
    // motorSpeeds.x = 30 / 0.03 = 1000
    expect(robot.motorSpeeds.x).toBeCloseTo(1000, 4);
    // unbewegte Achsen → 0
    expect(robot.motorSpeeds.y).toBeCloseTo(0, 6);
    expect(robot.motorSpeeds.z).toBeCloseTo(0, 6);
  });

  test('Handgelenk-Zweig: xyz-Motoren unverändert, Handgelenk-Punkt bewegt sich', () => {
    const robot = makeRobot();
    robot.xMotor = 0; robot.alpha = 0; robot.beta = 0;

    const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0);
    oldPos.pX = 0; oldPos.pY = 0; oldPos.pZ = 0;

    const newPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0);
    newPos.pX = 4; newPos.pY = 0; newPos.pZ = 3; // Handgelenk-Distanz = 5

    robot.calculateSpeeds(oldPos, newPos);

    // xyz-Distanz ist 0 → der Handgelenk-Zweig greift: moveTime = 5 / 1000
    expect(robot.lastMoveTime).toBeCloseTo(0.005, 6);
  });

  test('Finger-Zweig: nur e bewegt sich', () => {
    const robot = makeRobot();
    robot.eMotor = 2;

    const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0);
    const newPos = new MotorPosition(0, 0, 0, 0, 0, 0, 2);

    robot.calculateSpeeds(oldPos, newPos);

    expect(Number.isFinite(robot.motorSpeeds.e)).toBe(true);
    expect(robot.lastMoveTime).toBeCloseTo(0.002, 6); // 2 / 1000
  });

  test('Guard: useSpeedCalc deaktiviert → keine Änderung (Legacy)', () => {
    const robot = makeRobot();
    robot.useSpeedCalc = false;
    robot.xMotor = 10;

    const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0);
    const newPos = new MotorPosition(10, 0, 0, 0, 0, 0, 0);

    robot.calculateSpeeds(oldPos, newPos);

    expect(robot.motorSpeeds.x).toBe(0);
    expect(robot.lastMoveTime).toBe(0);
  });

  test('Guard: feedrate <= 0 → keine Berechnung', () => {
    const robot = makeRobot();
    robot.feedrate = 0;
    robot.xMotor = 10;

    const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0);
    const newPos = new MotorPosition(10, 0, 0, 0, 0, 0, 0);

    robot.calculateSpeeds(oldPos, newPos);

    expect(robot.motorSpeeds.x).toBe(0);
    expect(robot.lastMoveTime).toBe(0);
  });

  test('Guard: fehlende Positionen → kein Fehler, keine Änderung', () => {
    const robot = makeRobot();
    expect(() => robot.calculateSpeeds(null, null)).not.toThrow();
    expect(robot.motorSpeeds.x).toBe(0);
  });

  test('keine Bewegung (alle Distanzen 0) → moveTime bleibt 0', () => {
    const robot = makeRobot();
    const pos = new MotorPosition(5, 0.1, 0.2, 0, 0, 0, 0);
    robot.xMotor = 5; robot.alpha = 0.1; robot.beta = 0.2;

    robot.calculateSpeeds(pos, pos);

    expect(robot.lastMoveTime).toBe(0);
  });
});