const RobotController = require('../robot/RobotController');
const createDummyRobot = require('./helpers/createDummyRobot');

describe('RobotController (ToDo_6)', () => {

  test('applyCommand: G90 setzt Absolutmodus, ohne Bewegung', () => {
    const robot = createDummyRobot();
    robot.moveRelative = true;

    RobotController.applyCommand(robot, { command: 'G90', params: {} });

    expect(robot.moveRelative).toBe(false);
    expect(robot.calculateAngles3D).not.toHaveBeenCalled();
    expect(robot.sendCommand).not.toHaveBeenCalled();
  });

  test('applyCommand: G1 absolut setzt Koordinaten und bewegt', () => {
    const robot = createDummyRobot();
    robot.moveRelative = false;

    RobotController.applyCommand(robot, { command: 'G1', params: { X: 10, Y: 20, Z: 30 } });

    expect(robot.x).toBe(10);
    expect(robot.y).toBe(20);
    expect(robot.z).toBe(30);
    expect(robot.calculateAngles3D).toHaveBeenCalledTimes(1);
    expect(robot.sendCommand).toHaveBeenCalledTimes(1);
  });

  test('applyCommand: G1 relativ addiert', () => {
    const robot = createDummyRobot();
    robot.moveRelative = true;
    robot.x = 5;

    RobotController.applyCommand(robot, { command: 'G1', params: { X: 2 } });

    expect(robot.x).toBe(7);
  });

  test('applyCommand: M1 verändert Motorwerte und rechnet vorwärts', () => {
    const robot = createDummyRobot();
    robot.moveRelative = false;

    RobotController.applyCommand(robot, { command: 'M1', params: { X: 1, Y: 2, Z: 3, A: 4, B: 5, C: 6 } });

    expect(robot.xMotor).toBe(1);
    expect(robot.alpha).toBe(2);
    expect(robot.beta).toBe(3);
    expect(robot.a).toBe(4);
    expect(robot.calculatePositionFromMotorAngles).toHaveBeenCalled();
    expect(robot.sendCommand).toHaveBeenCalled();
  });

  test('applyCommand: M92 setzt Motorwerte absolut und sendet G92', () => {
    const robot = createDummyRobot();
    robot.createMotorPosition = jest.fn();

    RobotController.applyCommand(robot, { command: 'M92', params: { X: 5, Y: 0.5, A: 0.3 } });

    expect(robot.createMotorPosition).toHaveBeenCalledTimes(1);
    expect(robot.xMotor).toBe(5);
    expect(robot.alpha).toBe(0.5);
    expect(robot.a).toBe(0.3);
    expect(robot.calculatePositionFromMotorAngles).toHaveBeenCalled();
    expect(robot.sendCommand).toHaveBeenCalledWith('G92');
  });

  test('applyCommand: G92 interpretiert Winkel als Grad (→ rad), X bleibt mm', () => {
    const robot = createDummyRobot();
    robot.createMotorPosition = jest.fn();

    // G92 nutzt die G-Code-Konvention (Grad). Intern landen die Winkel in Radiant,
    // X (lineare Schiene) bleibt unverändert in mm. Vgl. M92 oben (Roh-Radiant).
    RobotController.applyCommand(robot, { command: 'G92', params: { X: 5, Y: 0.5, A: 0.3 } });

    const DEG2RAD = Math.PI / 180;
    expect(robot.createMotorPosition).toHaveBeenCalledTimes(1);
    expect(robot.xMotor).toBe(5);
    expect(robot.alpha).toBeCloseTo(0.5 * DEG2RAD, 10);
    expect(robot.a).toBeCloseTo(0.3 * DEG2RAD, 10);
    expect(robot.calculatePositionFromMotorAngles).toHaveBeenCalled();
    expect(robot.sendCommand).toHaveBeenCalledWith('G92');
  });

  test('applyCommand: G92 E setzt Greifer-Öffnung (mm) und leitet eMotor ab', () => {
    const robot = createDummyRobot();
    robot.createMotorPosition = jest.fn();
    robot.b = 0.2;   // Handgelenk-Knick
    robot.c = -0.5;  // Hand-Dreher

    // E ist mm (keine Grad/rad-Umrechnung). eMotor wird über die Greifer-Kopplung
    // aus e, b, c abgeleitet — sonst bliebe der an FluidNC gesendete Wert stale.
    RobotController.applyCommand(robot, { command: 'G92', params: { E: 10, B: 0.2 * (180 / Math.PI), C: -0.5 * (180 / Math.PI) } });

    expect(robot.e).toBe(10);
    expect(robot.eMotor).toBeCloseTo(10 - robot.b - robot.c, 10); // = 10 - 0.2 - (-0.5) = 10.3
    expect(robot.sendCommand).toHaveBeenCalledWith('G92');
  });

  test('applyCommand: ungültiger Befehl wird ignoriert', () => {
    const robot = createDummyRobot();
    RobotController.applyCommand(robot, null);
    RobotController.applyCommand(robot, { command: 'G99', params: {} });
    expect(robot.sendCommand).not.toHaveBeenCalled();
  });

  test('receive: parst rohe Nachricht und wendet mehrere Befehle an', () => {
    const robot = createDummyRobot();
    robot.moveRelative = true;

    RobotController.receive(robot, 'G90 G1 X12 Y34');

    expect(robot.moveRelative).toBe(false);
    expect(robot.x).toBe(12);
    expect(robot.y).toBe(34);
    expect(robot.sendCommand).toHaveBeenCalledTimes(1);
  });
});