#include <Servo.h>

#define numServos 3
//Servo servo1, servo2;
//
Servo brServo[numServos];
const int servoPins[numServos] = {5, 6, 9};
float angServo[numServos];

// demos de moviment
#define numDemo 4
float demosPos[numDemo][2] = {{30,30},{10,40},{10,15},{40,10}};

const int joyH_Pin = A1;
const int joyV_Pin = A0;
const int joyBoto_Pin = 2;


// mides del braçR
// M -> modul de la suma B1 + B2
float mB1 = 25, mB2 = 20, mB3 = 10, MF;

// coordenades moviment F
// F -> punta del braçR
float Fx, Fy;
float maxFx, maxFy, maxF = mB1 + mB2;
float Fxo = 30, Fyo = 30;

// angles de cada segment
float angle_B1, angle_B2, angle_B3;
// angle relatiu entre B1 i B2
float angle_B1_B2, angle_B2_B3;
// angle del modul Braç
float angle_MF;
// angles pel servos

#define RAD_to_DEG 57296 / 1000
#define DEG_to_RAD 1000 / 57296


void setup() {

  for( int i=0; i<numServos; i++){
    brServo[i].attach(servoPins[i]);
  }
  pinMode(joyBoto_Pin, INPUT_PULLUP);
  Serial.begin(9600);

  anar_Inici();
  maximF(Fx, Fy);
  calculaTriangle(Fx, Fy);  
  
}

void loop() {

  lecturaJoySticks();
  if(!digitalRead(joyBoto_Pin)) demo(); //anar_Inici();
  
  calculaTriangle(Fx, Fy);  
  //calculaTriangle(pinza);
  //mostraDadesCalculades();
  
  delay(50);
}

void anar_Inici(){
  Fx = Fxo;
  Fy = Fyo;
}
  
void demo(){

  for(int i=0; i<numDemo; i++){
    calculaTriangle(demosPos[i][0], demosPos[i][1]);  
    //moureServos(demosPos[i]);
    delay(1000);
  }
  Serial.println("Fi demo");
}

// Funcions Auxiliars
// ***********************
void lecturaJoySticks(){
  
  float jH = analogRead(A1);
  if (jH > 600) {
    Fx -= 1;
    if(Fx < 0) Fx = 0;
  }
  else if (jH < 400) {
    Fx += 1;
    maximF(Fx, Fy);
  }
  float jV = analogRead(A0);
  if (jV < 400) {
    Fy -= 1;
    if(Fy < 0) Fy = 0;
  }
  else if (jV > 600) {
    Fy += 1;
    maximF(Fx, Fy);
  }
    
}

//calcula el modul màxim que podem allargar el braç
float maximF(float& x, float& y){
  float modul = modulBrac(x, y);
  float argument = angleBrac(x, y);

  //Serial.println("COORDs   FX:" + String(x) + " FY: "+ String(y) + " MaxFx: " + String(maxFx) + " MaxFy: " + String(maxFy));
  if(x >= maxF) x = maxF-1;
  if(y >= maxF) y = maxF-1;
  
  while(x > maxFx){
    y--;
    argument = angleBrac(x, y);
    maxFx = maxF*cos(radians(argument));
    maxFy = maxF*sin(radians(argument));
  }
  while(y > maxFy){
    x--;
    argument = angleBrac(x, y);
    maxFx = maxF*cos(radians(argument));
    maxFy = maxF*sin(radians(argument));
  }

  return;

}

void calculaTriangle(float& x, float& y){
 float angServoB1, angServoB2, angServoB3;

  MF = modulBrac(x, y);
  angle_MF = angleBrac(x, y);
  
  angle_B1 = calculCosinus(MF, mB1, mB2);
  angle_B2 = calculCosinus(MF, mB2, mB1);
  
  angServoB1 = 90 - (angle_MF + angle_B1);
  //angServoB2 = angServoB1 + (180-angle_B1-angle_B2) - 180;
  angServoB2 = 90 - (angle_MF - angle_B2);

  angServo[0] = angServoB1;
  angServo[1] = angServoB2;
  // l'angle de la pinça
  angServo[2] = 180 - angServoB2;
  // moure servos
  moureServos(angServo);

}

void moureServos(float angle[]){

  //Serial.println("ANGs   B1:" + String(angServo[0]) + " B2: "+ String(angServo[1]) + " B3: " + String(angServo[2]));
  
  for(int i=0; i<numServos; i++){
    brServo[i].write(angle[i]);
  }
}

void mostraDadesCalculades(){

  Serial.print("Mòdul del Braç: ");
  Serial.println(MF);
  Serial.print(("Angle del Braç: "));
  Serial.println(angle_MF);

  Serial.println("   --- Triangle ---");
  Serial.print("angle base: ");
  Serial.println(angle_B1);
  Serial.print("angle segment2: ");
  Serial.println(angle_B2);

  Serial.print("Angle Servo1: ");
  Serial.println(angServo[0]);
  Serial.print("Angle Servo2: ");
  Serial.println(angServo[1]);
  Serial.print("Angle Servo3: ");
  Serial.println(angServo[2]);
  
  Serial.println("   --- --- --- ---");

}

// Culcula mòdul per patàgores 
float modulBrac(const float& lx, const float& ly){
  return sqrt(lx*lx + ly*ly);
}

// Càlcul ArcTangent
float angleBrac(const float& lx, const float& ly){
  return atan2(ly,lx) * RAD_to_DEG;
}

//Càlcul angle Teorema Cosino
float calculCosinus(const float& lc1, const float& lc2, const float& lo){
  const float angle = acos((lc1 * lc1 + lc2 * lc2 - lo * lo) / (2 * lc1 * lc2));
  return angle * RAD_to_DEG;
}