#include "InverseKinemetics.h"
//  V3.0

//Variables
boolean error;

void RL3 (int i, Data *DataLine)
{
  float x = DataLine->ServoData[i].x;
  float z = DataLine->ServoData[i].z;

  if ((x == 0) && (z == 0)) 
    DataLine->l3 = 0;
  else DataLine->l3 = hypot(z, x);

  if (x < 0)
    DataLine->l3 = DataLine->l3 * -1;
}

float O1 (int i, Data *DataLine)
{
  RL3(i, DataLine);

  float x = DataLine->ServoData[i].x;
  float z = DataLine->ServoData[i].z;
  float r;

  if ((x != 0) & (z != 0)){
    double a = (z / x);
    return atan(a);
  }
  else
    return atan(0.0); // Math Error Protection.
}

float T1 (int i, Data *DataLine)
{
  float x = DataLine->ServoData[i].x;
  float y = DataLine->ServoData[i].y;
  float z = DataLine->ServoData[i].z;
  float x2 = x * x;
  float y2 = y * y;
  float z2 = z * z;
  float l32 = DataLine->l3 * DataLine->l3;

  DataLine->s = sqrt(x2 + y2 + z2);

  float t1 = 0;

  float s2 = DataLine->s * DataLine->s;

  if ((y == 0) && (DataLine->l3 == 0))
    t1 = 0; // Math Error Protection.
  else
    t1 = (atan2(float(DataLine->l3), float(y)));

  float a = (l1 * l1) - (l2*l2) + s2;
  float b = 2 * l1 * DataLine->s;
  float t2 = acos(a / b);

  float ret = 0;

  ret = t1 + abs(t2);

  return ret;
}

float T3 (Data *DataLine)
{
  return (acos(((l1*l1) + (l2*l2) - (DataLine->s*DataLine->s)) / (2 * l1 * l2)));
}

void setleg (int i, Data *DataLine)
{
  Segment Seg;

  if (i < 3)
  {
    float a = ((float)90 - degrees(O1(i, DataLine)));
    float b = ((float)180 - degrees(T1(i, DataLine)));
    float c = degrees(T3(DataLine));

    if ((a >= 0) & (a <= 180)
      & (b >= 0) & (b <= 180)
      & (c >= 0) & (c <= 180))
    {
      DataLine->ServoData[i].o1 = a;
      DataLine->ServoData[i].o2 = b;
      DataLine->ServoData[i].o3 = c;
    }
    else
    {
      error = true;
      Seg.sevenSegWrite('e');
    }
  }
  else
  {
    float a = ((float)90 + degrees(O1(i, DataLine)));
    float b = degrees(T1(i, DataLine));
    float c = ((float)180 - degrees(T3(DataLine)));

    if ((a >= 0) & (a <= 180)
      & (b >= 0) & (b <= 180)
      & (c >= 0) & (c <= 180))
    {
      DataLine->ServoData[i].o1 = a;
      DataLine->ServoData[i].o2 = b;
      DataLine->ServoData[i].o3 = c;
    }
    else
    {
      error = true;
      Seg.sevenSegWrite('e');
    }
  }
}

void InverseKinemetics::IK(Data *DataLine)  
{
  Segment Seg;
  Seg.sevenSegWrite('o');

  error = false;

  for (int i=0; i<6; i++)
    setleg(i, DataLine);

#ifdef DEBUGIK
  for (int i=0; i<6; i++)
  {
    if (i != 0)
      Serial.print(", ");
    else
      Serial.print("### InverseKinemetics->ServosPosition's are: ");

    Serial.print(DataLine->ServoData[i].x);
    Serial.print(", ");
    Serial.print(DataLine->ServoData[i].y);
    Serial.print(", ");
    Serial.print(DataLine->ServoData[i].z);
  }
  if (error == true)
    Serial.println("; ERROR");
  else
    Serial.println(";");
#endif
}