const Robot = require('../robot/Robot.js');

test('Grade ausgestreckt', () => {
    robot = new Robot(300,290,10)
  
    robot.x = 0 ;
    robot.y = 600;
    robot.z = 0;
    robot.phi = -Math.PI/2;
    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;
    robot.z = 0;
    robot.phi = -Math.PI/2;
    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;
    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);
});