#include <Servo.h>

int saklar1,saklar2,
    pot1,pot2,pot3,pot4,pot5,
    stat;

Servo join1;
Servo join2; 
Servo join3; 
Servo join4;
Servo join5;
Servo join6;

const float cx=9; //coxa
const float fm=15; //femur
const float tb=17; // tibia
float L, L1;
float alpha, alpha1,alpha2,beta,gama;

void setup()
{
  Serial.begin(9600); 

  join1.attach(3);
  join2.attach(4);
  join3.attach(5);
  join4.attach(6);
  join5.attach(7);
  join6.attach(8);

//  join1.write(120);
//  join2.write(60);
//  join3.write(90);
//  join4.write(90);
//  join5.write(00);
//  join6.write(90);

  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);
  pinMode(A6, INPUT);
}

void loop()
{
  saklar1 = analogRead(A0);
  saklar2 = analogRead(A1);

  Serial.print(stat);
  Serial.print(" || ");
  Serial.print(pot1);
  Serial.print(" || ");
  Serial.print(pot2);
  Serial.print(" || ");
  Serial.print(pot3);
  Serial.print(" || ");
  Serial.print(pot4);
  Serial.print(" || ");
  Serial.println(pot5);
  
  if(saklar1 >= 1023 && saklar2 <= 1023)
  {
    stat = 1;
  }
  if(saklar1 <= 1023 && saklar2 >= 1023)
  {
    stat = 2;
  }

  if(stat == 1)
  {
    pot1 = analogRead(A2);
    pot1 = map(pot1, 0 ,1023, 0, 25);
    
    pot2 = analogRead(A3);
    pot2 = map(pot2, 0 ,1023, 0, 25);
    
    pot3 = analogRead(A4);
    pot3 = map(pot3, 0 ,1023, 0, 25);
    
    trigono_xyz(pot1, pot2, pot3); //contoh x,y,z
    Serial.print("gama= ");
    Serial.print(gama);
    Serial.print(", alpha= ");
    Serial.print(alpha);
    Serial.print(", beta= ");
    Serial.print(beta);
    Serial.println();
  }
  
  if(stat == 2)
  {
    pot1 = analogRead(A2);
    pot1 = map(pot1, 0 ,1023, 0, 180);
    
    pot2 = analogRead(A3);
    pot2 = map(pot2, 0 ,1023, 0, 180);
    
    pot3 = analogRead(A4);
    pot3 = map(pot3, 0 ,1023, 0, 180);
    
    pot4 = analogRead(A5);
    pot4 = map(pot4, 0 ,1023, 0, 180);
    
    pot5 = analogRead(A6);
    pot5 = map(pot5, 0 ,1023, 0, 180);
    
    join1.write(pot1);
    join2.write(pot2);
    join6.write(pot3);
    join4.write(pot4);
    join3.write(pot5);
  }
  
  /*
   * cx=9
   * fm=15
   * tb=17
   */
//   trigono_xyz(15, 15, 15); //contoh x,y,z
//   Serial.print("gama= ");
//   Serial.print(gama);
//   Serial.print(", alpha= ");
//   Serial.print(alpha);
//   Serial.print(", beta= ");
//   Serial.print(beta);
//   Serial.println();

  delay(100);
}

void trigono_xyz(float x, float y, float z)
{
  L1=sqrt(sq(x)+sq(y));
  gama=atan(x/y)/PI*180;
  L=sqrt(sq(L1-cx)+sq(z));
  beta=acos((sq(tb)+sq(fm)-sq(L))/(2*tb*fm))/PI*180;
  alpha1=acos(z/L)/PI*180;
  alpha2=acos((sq(fm)+sq(L)-sq(tb))/(2*fm*L))/PI*180;
  alpha=alpha1+alpha2;

  join1.write(beta);    //Y
  join2.write(gama);    //X
  join3.write(beta);    //X
  join5.write(alpha);   //Z end effector
}