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

test('Grade ausgestreckt', () => {
    robot = new Robot(300,290,10)
  
    robot.x = 0 ;
    robot.y = -600;            // -y: voll ausgestreckte Grundstellung
    robot.z = 0;
    robot.phi = Math.PI/2;     // gespiegelt zu -y (siehe doc/Info_Koordinaten.md)
    robot.theta = Math.PI/2;


    robot.calculateAngles3D();

    expect(robot.pX).toBeLessThanOrEqual(0.00001)
    expect(robot.pY).toBe(-590)
    expect(robot.pZ).toBeLessThanOrEqual(0.00001)

    expect(robot.alpha).toBeLessThanOrEqual(0.00001)
    expect(robot.beta).toBeLessThanOrEqual(0.00001)
});

test('Grade gewinkelt', () => {
    robot = new Robot(300,290,10)
  
    robot.x = 0 ;
    robot.y = -300;            // -y
    robot.z = 0;
    robot.phi = Math.PI/2;     // gespiegelt zu -y
    robot.theta = Math.PI/2 - Math.PI/3;

    
    robot.calculateAngles3D();

    expect(robot.pX).toBeLessThanOrEqual(0.00001)
    expect(robot.pZ).toBe(10 * Math.cos(robot.theta))

    expect(robot.alpha - Math.PI/3).toBeLessThanOrEqual(0.00002)
    expect(robot.beta + Math.PI/3).toBeLessThanOrEqual(0.00002)
});

test('schräg gewinkelt 1', () => {
    robot = new Robot(300,300,10)
  
    robot.x = 0 ;
    robot.y = -310;            // -y (phi=0 ist spiegel-invariant)
    robot.z = 0;
    robot.phi = 0;
    robot.theta = Math.PI/2;

    
    robot.calculateAngles3D();

    expect(robot.pX).toBe(10);
    expect(robot.pZ).toBeLessThanOrEqual(0.001);

    expect(robot.alpha - Math.PI/3).toBeLessThanOrEqual(0.02);
    expect(robot.beta + Math.PI/3).toBeLessThanOrEqual(0.02);
});