//#include "WProgram.h"
#include "InverseKinemetics.h"

void InverseKinemetics::RL3 (int i)
{
  l3 = sqrt(pow(abs(COR_2_IK[0+(i*3)]), 2) + pow(abs(COR_2_IK[2+(i*3)]), 2));
}

float InverseKinemetics::ZX (int i)
{
  RL3(i);
  float temp = degrees(acos((float)COR_2_IK[2+(i*3)] / l3));
  return temp;
}

float InverseKinemetics::TT (int i)
{
  int x = pow(abs(COR_2_IK[0+(i*3)]), 2);
  int y = pow(abs(COR_2_IK[1+(i*3)]), 2);
  int z = pow(abs(COR_2_IK[2+(i*3)]), 2);  
  s = sqrt(x + y + z);
  
  if (s > (l1 + l2) )
    R[i] = 3;
  
  float t1 = degrees(acos((pow(s, 2) + pow(COR_2_IK[1+(i*3)], 2) - pow(l3, 2))
                                  / (2 * s * COR_2_IK[1+(i*3)])));
                                  
  float t2 = degrees(acos((pow(s, 2) + pow(l1, 2) - pow(l2, 2))
                                  / (2 * s * l1)));
  
  float temp = (int)t1 + t2;
  //Serial.print("DEBUG: "); Serial.print(t1); Serial.print(", "); Serial.print(t2); Serial.print(", "); Serial.println(temp);
  
  return temp;
}

int InverseKinemetics::LL ()
{
  int temp = degrees(acos((float)(pow(l1, 2) + pow(l2, 2) - pow(s, 2)) / (2 * l1 * l2)));
  return temp;
}

void InverseKinemetics::setleg (int i)
{
  Serial.print("Set Leg "); 
  Serial.println(i);
  if (i >= 3)
  {
    //Error_Coordinate(i);
    DEG_2_CON[0+(i*3)] = 180 - ZX(i);
    DEG_2_CON[1+(i*3)] = 180 - TT(i);
    DEG_2_CON[2+(i*3)] = LL();
    Error_Angle(i);  
  }
  else
  {
    //Error_Coordinate(i);
    DEG_2_CON[0+(i*3)] = ZX(i);
    DEG_2_CON[1+(i*3)] = TT(i);
    DEG_2_CON[2+(i*3)] = 180 - LL();
    Error_Angle(i);
  }
}

void InverseKinemetics::setlegs ()
{
  for (int i=0; i<6; i++)
  {
    setleg(i);
  }
  for (int i=0; i<6; i++)
  {
    if (i != 0)
      Serial.print(", ");
    else
      Serial.print("Cor: ");
    Serial.print(COR_2_IK[0+(i*3)]);
    Serial.print(", ");
    Serial.print(COR_2_IK[1+(i*3)]);
    Serial.print(", ");
    Serial.print(COR_2_IK[2+(i*3)]);
  }
  Serial.println(";");
}

void InverseKinemetics::Error_Angle (int i)
{  
  for (int i2=0; i2<3; i2++)
    R[i2+(i*3)] = 0;
  if (DEG_2_CON[0+(i*3)] < 0) 
    R[0+(i*3)] = 1;
  if (DEG_2_CON[0+(i*3)] > 180) 
    R[0+(i*3)] = 2;
  if (DEG_2_CON[1+(i*3)] < 0) 
    R[1+(i*3)] = 1; 
  if (DEG_2_CON[1+(i*3)] > 180)
    R[1+(i*3)] = 2;
  if (DEG_2_CON[2+(i*3)] < 0)
    R[2+(i*3)] = 1;
  if (DEG_2_CON[2+(i*3)] > 180)
    R[2+(i*3)] = 2;
}

void InverseKinemetics::Error_Coordinate (int i)
{  
  Ra[i] = 0;
  float temp = sqrt(pow(COR_2_IK[0+(i*3)], 2) + pow(COR_2_IK[1+(i*3)], 2) + pow(COR_2_IK[2+(i*3)], 2));
  if (temp <= 5) {
    Ra[i] = 1;
  }    
  if (temp > l1 + l2) {
    Ra[i] = 2;
  }
}

int InverseKinemetics::CheckForErrorCor()
{
  for (int i=1;  i<19; i++)
    if (R[i-1]!=0)
      return i;
  return 0;
}

String InverseKinemetics::CheckForErrorCor_print()
{
  String temp = "Error Coordinate: ";
  for (int i=0; i<18; i++)
  {
    temp = temp + (int)DEG_2_CON[i] + ", ";
  }
  temp = temp + "\nCheckForErrorCor returnd: " + CheckForErrorCor();
  return temp;
}

int InverseKinemetics::CheckForErrorAng()
{
  for (int i=0;  i<6; i++)
    if (Ra[i]!=0)
      return i;
  return 0;
}