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



test('Initialisiere den Robot', () => {
  robot = new Robot(300,290,10)
  expect(robot.x).toBe(0);
});

test('Handgelenk Position', () => {
  robot = new Robot(300,300,30)

  robot.x = 300 + robot.l3;
  robot.y = 0;
  robot.z = 0;    

  robot.theta = 0;
  robot.calculateAngles3D();
  expect(Math.abs(robot.Hz)).toBeLessThanOrEqual(0.0001)
  expect(Math.abs(robot.Hr - 300)).toBeLessThanOrEqual(0.0001)


  robot.theta = Math.PI/4;
  robot.calculateAngles3D();
  expect(Math.abs(robot.Hz - robot.l3/Math.sqrt(2))).toBeLessThanOrEqual(0.0001)
  expect(Math.abs(robot.r - robot.Hr - robot.l3/Math.sqrt(2))).toBeLessThanOrEqual(0.0001)

});

test('Winkel Oberarm/Unterarm Dreieck', () => {

  robot = new Robot(300,300,30)

  robot.x = 150 + robot.l3;
  robot.y = 0;
  robot.z = 0;    

  robot.theta = 0;
  robot.calculateAngles3D();

  expect(Math.abs(robot.alpha*2 + robot.beta - Math.PI)).toBeLessThanOrEqual(0.0001)
});

test('Winkel Oberarm/Unterarm Dreieck nach unten greifend', () => {

  robot = new Robot(300,300,30)

  robot.x = robot.l1 + robot.l3;
  robot.y = 0;
  robot.z = robot.l2;    

  robot.theta = 0;
  robot.calculateAngles3D();

  expect(Math.abs(robot.alpha*2 + robot.beta - Math.PI)).toBeLessThanOrEqual(0.0001)

  expect(Math.abs(robot.alpha - Math.PI/4)).toBeLessThanOrEqual(0.0001)
  expect(Math.abs(robot.beta - Math.PI/2)).toBeLessThanOrEqual(0.0001)

  console.log("alpha=", robot.alpha*180/Math.PI, " beta=", robot.beta*180/Math.PI);
});