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



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


test('3D Handgelenk Punkt prüfen 1', () => {
  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();

  // console.log("p = " + robot.pX.toString() +", "+robot.pY.toString() +", "+robot.pZ.toString() );
  expect(Math.abs(robot.pX)).toBeLessThanOrEqual(0.0001)
  expect(Math.abs(robot.pY)).toBe(590)
  expect(Math.abs(robot.pZ)).toBeLessThanOrEqual(0.0001)
});

test('3D Handgelenk Punkt prüfen 2', () => {
  robot = new Robot(300,290,10)

  robot.x = 0 ;
  robot.y = 400;
  robot.z = 0;
  robot.phi = -Math.PI/2;
  robot.theta = Math.PI/2;

  robot.calculateAngles3D();

  // console.log("p = " + robot.pX.toString() +", "+robot.pY.toString() +", "+robot.pZ.toString() );
  expect(Math.abs(robot.pX)).toBeLessThanOrEqual(0.0001)
  expect(Math.abs(robot.pY)).toBe(390)
  expect(Math.abs(robot.pZ)).toBeLessThanOrEqual(0.0001)
});

test('3D Handgelenk Punkt prüfen 3', () => {
  robot = new Robot(300,290,10)

  robot.x = 0 ;
  robot.y = 400;
  robot.z = 0;
  robot.phi = -Math.PI/2;
  robot.theta = 0;

  robot.calculateAngles3D();

  // console.log("p = " + robot.pX.toString() +", "+robot.pY.toString() +", "+robot.pZ.toString() );
  expect(Math.abs(robot.pX)).toBeLessThanOrEqual(0.0001)
  expect(Math.abs(robot.pY)).toBe(400)
  expect(Math.abs(robot.pZ)).toBe(10)
});

test('3D Handgelenk Punkt prüfen 4', () => {
  robot = new Robot(300,290,10)

  robot.x = 0 ;
  robot.y = 400;
  robot.z = 0;
  robot.phi = 0;
  robot.theta = Math.PI/2;

  robot.calculateAngles3D();

  // console.log("p = " + robot.pX.toString() +", "+robot.pY.toString() +", "+robot.pZ.toString() );
  expect(robot.pX).toBe(10)
  expect(robot.pY).toBe(400)
  expect(Math.abs(robot.pZ)).toBeLessThanOrEqual(0.0001)
});

/** 
// Griff mit ausgetrecktem Arm
test('3D Angles AlphaBeta', () => {
  robot = new Robot(300,290,10)

  robot.x = 0 ;
  robot.y = 600;
  robot.z = 0;
  robot.phi = -Math.PI / 2;
  robot.theta = 0;

  robot.calculateAngles3D();

  expect(robot.alpha).toBe(0);
  expect(robot.beta).toBe(0);
  expect(robot.a).toBe(0);
  expect(robot.b).toBe(0);
  expect(robot.c).toBe(0);
});

// Griff mit minimal gewinkeltem Arm
test('3D Angles AlphaBeta with slight angle', () => {
  robot = new Robot(300,290,10)

  robot.x = 0 ;
  robot.y = 550;
  robot.z = 0;
  robot.phi = -Math.PI / 2;
  robot.theta = 0;

  robot.calculateAngles3D();

  expect(robot.alpha).not.toBe(0);
  expect(robot.beta).not.toBe(0);
  expect(Math.abs(robot.a  % (Math.PI/2))).toBe(0);
  expect(robot.b).not.toBe(0);
  console.log(robot.b);
  expect(Math.abs(robot.c % (Math.PI/2))).toBe(0);
});

*/

/*
// Griff mit gewinkeltem Arm
test('3D Angles AlphaBeta', () => {
  robot = new Robot(300,290,10)

  robot.x = 0 ;
  robot.y = 300;
  robot.z = 0;
  robot.phi = -Math.PI / 2;
  robot.theta = Math.PI/3;

  robot.calculateAngles3D();

  console.log("alpha = " + robot.alpha.toString());
  console.log("beta  = " + robot.beta.toString());
  console.log("a = " + robot.a.toString());
  console.log("b = " + robot.b.toString());
  console.log("c = " + robot.c.toString());
  expect(robot.alpha).toBe(Math.PI/3);
  expect(robot.beta).toBe(-Math.PI/3);
});

*/