'use strict';
// Tests für ToDo_9 Paket 4: Hardware-Sync-Command (M114 R).
//   - syncFromHardware liest MPos aller 3 Controller → Motorwerte → Pose
//   - bewegt den Roboter NICHT (kein sendCommand)
//   - robust gegen fehlende Rolle / ungültige/fehlende MPos

const RobotController = require('../robot/RobotController');
const { motorStateFromPorts, D } = require('../robot/portInverse');
const Robot = require('../robot/kinematics/Arm3SegmentLinearX');

// ── Fake-Sender mit Rolle und vorgegebenem MPos-Snapshot ─────────────────────
function fakeSender(role, machinePosition, { fail } = {}) {
    return {
        controllerRole: role,
        requestStatusReport: jest.fn(() =>
            fail
                ? Promise.reject(new Error(`${role}: timeout`))
                : Promise.resolve({ grblState: 'Idle', machinePosition, machinePositionType: 'MPos' })
        ),
    };
}

/** Roboter mit den drei Sendern, deren MPos die gegebenen Motorwerte kodieren. */
function robotWithMotors(m) {
    const robot = new Robot(250, 264, 100);
    // Produktiv-Verkabelung: base[x,y,z]=[xMotor, alpha·D, (beta−alpha)·D],
    // elbow[x]=a·D, hand[x,y,z]=[(c−b)·D, eMotor·D, b·D]
    const base  = [m.xMotor, m.alpha * D, (m.beta - m.alpha) * D];
    const elbow = [m.a * D];
    const hand  = [(m.c - m.b) * D, m.eMotor * D, m.b * D];
    robot.cmdReceivers = [
        fakeSender('base', base),
        fakeSender('elbow', elbow),
        fakeSender('hand', hand),
    ];
    return robot;
}

describe('RobotController.syncFromHardware (ToDo_9 Paket 4)', () => {

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

    test('rekonstruiert Motorwerte aus MPos und übernimmt sie', async () => {
        const motors = { xMotor: 100, alpha: 0.30, beta: 0.50, a: 0.20, b: 0.40, c: 0.90, eMotor: 1.0 };
        const robot = robotWithMotors(motors);

        await RobotController.syncFromHardware(robot);

        expect(robot.xMotor).toBeCloseTo(motors.xMotor, 6);
        expect(robot.alpha).toBeCloseTo(motors.alpha, 6);
        expect(robot.beta).toBeCloseTo(motors.beta, 6);
        expect(robot.a).toBeCloseTo(motors.a, 6);
        expect(robot.b).toBeCloseTo(motors.b, 6);
        expect(robot.c).toBeCloseTo(motors.c, 6);
        expect(robot.eMotor).toBeCloseTo(motors.eMotor, 6);
    });

    test('rechnet die Pose vor und gibt sie zurück (gleiche wie Referenz-Roboter)', async () => {
        const motors = { xMotor: 42, alpha: 0.10, beta: 0.10, a: 0.05, b: 1.20, c: 1.25, eMotor: 0.0 };
        const robot = robotWithMotors(motors);

        const pose = await RobotController.syncFromHardware(robot);

        const ref = new Robot(250, 264, 100);
        Object.assign(ref, motors);
        ref.calculatePositionFromMotorAngles();

        for (const k of ['x', 'y', 'z', 'phi', 'theta', 'psi']) {
            expect(pose[k]).toBeCloseTo(ref[k], 6);
        }
    });

    test('bewegt den Roboter NICHT (kein execCommand auf den Sendern)', async () => {
        const robot = robotWithMotors({ xMotor: 10, alpha: 0.1, beta: 0.2, a: 0.1, b: 0.2, c: 0.3, eMotor: 0 });
        // execCommand-Spy auf alle Sender; darf nie aufgerufen werden
        robot.cmdReceivers.forEach(s => { s.execCommand = jest.fn(); });

        await RobotController.syncFromHardware(robot);

        robot.cmdReceivers.forEach(s => expect(s.execCommand).not.toHaveBeenCalled());
    });

    test('setzt motorPosition zurück (nächster Move rechnet von echter Position)', async () => {
        const robot = robotWithMotors({ xMotor: 5, alpha: 0, beta: 0, a: 0, b: 0, c: 0, eMotor: 0 });
        robot.motorPosition = { dummy: true };
        robot.motorPositionOld = { dummy: true };

        await RobotController.syncFromHardware(robot);

        expect(robot.motorPosition).toBeNull();
        expect(robot.motorPositionOld).toBeNull();
    });

    test('fragt alle drei Controller aktiv ab (requestStatusReport)', async () => {
        const robot = robotWithMotors({ xMotor: 0, alpha: 0, beta: 0, a: 0, b: 0, c: 0, eMotor: 0 });
        await RobotController.syncFromHardware(robot);
        robot.cmdReceivers.forEach(s => expect(s.requestStatusReport).toHaveBeenCalledTimes(1));
    });

    test('wirft, wenn eine Controller-Rolle fehlt', async () => {
        const robot = new Robot(250, 264, 100);
        robot.cmdReceivers = [fakeSender('base', [0, 0, 0]), fakeSender('hand', [0, 0, 0])]; // elbow fehlt
        await expect(RobotController.syncFromHardware(robot)).rejects.toThrow(/elbow/);
    });

    test('wirft bei ungültiger MPos (zu wenige Achsen)', async () => {
        const robot = new Robot(250, 264, 100);
        robot.cmdReceivers = [
            fakeSender('base', [1, 2]),   // nur 2 statt 3 Werte
            fakeSender('elbow', [0]),
            fakeSender('hand', [0, 0, 0]),
        ];
        await expect(RobotController.syncFromHardware(robot)).rejects.toThrow(/base/);
    });

    test('wirft, wenn ein Controller nicht antwortet (Timeout propagiert)', async () => {
        const robot = new Robot(250, 264, 100);
        robot.cmdReceivers = [
            fakeSender('base', [0, 0, 0]),
            fakeSender('elbow', [0], { fail: true }),
            fakeSender('hand', [0, 0, 0]),
        ];
        await expect(RobotController.syncFromHardware(robot)).rejects.toThrow(/timeout/);
    });

    test('Dispatch: applyCommand(M114 R) liefert ein Promise', () => {
        const robot = robotWithMotors({ xMotor: 0, alpha: 0, beta: 0, a: 0, b: 0, c: 0, eMotor: 0 });
        const result = RobotController.applyCommand(robot, { command: 'M114', params: { R: true } });
        expect(result && typeof result.then).toBe('function');
        return result; // sauber auflösen lassen
    });

    test('receive(): bare M114 ohne R löst keinen Sync aus (synchron, undefined)', () => {
        const robot = robotWithMotors({ xMotor: 0, alpha: 0, beta: 0, a: 0, b: 0, c: 0, eMotor: 0 });
        robot.cmdReceivers.forEach(s => s.requestStatusReport.mockClear());
        const result = RobotController.receive(robot, 'M114');
        expect(result).toBeUndefined();
        robot.cmdReceivers.forEach(s => expect(s.requestStatusReport).not.toHaveBeenCalled());
    });
});