#ifndef InverseKinemetics_H
#define InverseKinemetics_H

#include "WProgram.h"
#include <math.h>
#include "Data_Manager.h"

class InverseKinemetics
{
public:
  //
  void RL3(int i);
  float ZX (int i);
  float TT (int i);
  int LL ();
  void setlegs ();
  void setleg (int i);

  /* // Input
   void setLeftFrontLeg	(int ix, int iy, int iz);	  //	clockwise,		t3=clockwise,		t3=counterclockwise
   void setLeftMiddleLeg	(int ix, int iy, int iz);	  //	clockwise,		t3=clockwise,		t3=counterclockwise
   void setLeftBackLeg	(int ix, int iy, int iz);         //	clockwise,		t3=clockwise,		t3=counterclockwise
   void setRightFrontLeg	(int ix, int iy, int iz);	  //	counterclockwise,	t3=counterclockwise,	t3=clockwise
   void setRightMiddleLeg(int ix, int iy, int iz);         //	counterclockwise,	t3=counterclockwise,	t3=clockwise
   void setRightBackLeg	(int ix, int iy, int iz);	  //	counterclockwise,	t3=counterclockwise,	t3=clockwise*/

  // 0 | 3
  // 1 | 4
  // 2 | 5

  // 
  void Error_Angle  (int i);
  void Error_Coordinate (int i);
  
  int CheckForErrorCor();
  String CheckForErrorCor_print();
  int CheckForErrorAng();

  // Variable's
  int s;
  float l3;
  byte R[18];
  byte Ra[6];
};


#endif