#include <Servo.h>
Servo servo1_arm1;
Servo servo2_arm2;
Servo servo3_base;
Servo servo4_grip;
const int r1 = 20;
const int r2 = 10;
const int grip = 10;
const float pi = 3.14159; 
float increment_y = 0.1;
float increment_x = 0.1;
int base_position = 0;
int grip_position = 0;
float y = 0.00;
float x = 40.00;
float x_old;  
int pin1 = 11;
int pin2 = 10;
int pin3 = 6;
int pin4 = 5;
int y_key_1 = A0;
int x_key_1 = A1;
int y_key_2 = A2;
int x_key_2 = A3;
int grip_key_up = 13;
int grip_key_down = 12;
int x_pos_1;
int y_pos_1;
int x_pos_2;
int y_pos_2;

float theta_2 (float x, float y,int r1, int r2, int grip) {
  float arm2_position_rad = acos((pow(x,2) + pow(y,2) - pow(r1,2) - (r2+grip)*(r2+grip))/ (2*r1*(r2+grip)));
  return arm2_position_rad;
}

float theta_1 (float x, float y,int r1, int r2, int grip){
  float arm2_position_rad = theta_2(x, y, r1, r2, grip);
  float arm1_position_rad = atan(y/x) - asin(((r2+grip)*sin(arm2_position_rad))/sqrt(x*x + y*y));
  if (arm1_position_rad < 0) {
    arm1_position_rad = (arm1_position_rad + 2*pi) - pi;
    return arm1_position_rad;
  }
  else {
    return arm1_position_rad;
  }
}

void setup() {
  servo1_arm1.attach(pin1);
  servo2_arm2.attach(pin2);
  servo3_base.attach(pin3);
  servo4_grip.attach(pin4);
  servo1_arm1.write(90);
  servo2_arm2.write(90);
  servo3_base.write(0);
  servo4_grip.write(0);
  pinMode(x_key_1, INPUT);
  pinMode(y_key_1, INPUT);
  pinMode(x_key_2, INPUT);
  pinMode(y_key_2, INPUT);
  pinMode(grip_key_up, INPUT_PULLUP);
  pinMode(grip_key_down, INPUT_PULLUP);
}

void loop() {
  x_pos_1 = analogRead(x_key_1);
  y_pos_1 = analogRead(y_key_1);
  x_pos_2 = analogRead(x_key_2);
  y_pos_2 = analogRead(y_key_2);
  if (x_pos_1 == 0 && y_pos_1 == 512 && x<40.00 && x<=x_old){
    x = x + increment_x ; 
    y = y;
    float arm1_position = theta_1(x, y, r1, r2, grip);
    float arm1_position_deg = (arm1_position/pi) * 180;
    servo1_arm1.write(arm1_position_deg);
    float arm2_position = theta_2(x, y, r1, r2, grip);
    float arm2_position_deg = (arm2_position/pi) *180;
    servo2_arm2.write(arm2_position_deg);
    delay(50); 
  }
  if (x_pos_1 == 1023 && y_pos_1 == 512 && x>0){
    x = x - increment_x ;
    y = y;
    float arm1_position = theta_1(x, y, r1, r2, grip);
    float arm1_position_deg = (arm1_position/pi) * 180;
    servo1_arm1.write(arm1_position_deg);
    float arm2_position = theta_2(x, y, r1, r2, grip);
    float arm2_position_deg = (arm2_position/pi) *180;
    servo2_arm2.write(arm2_position_deg);
    delay(50);
  }
  if (x_pos_1 == 512 && y_pos_1 == 1023 && y <40.00){
    y = y + increment_y ;
    x = x - increment_x;
    x_old = x;   
    float arm1_position = theta_1(x, y, r1, r2, grip);
    float arm1_position_deg = (arm1_position/pi) * 180;
    servo1_arm1.write(arm1_position_deg);
    float arm2_position = theta_2(x, y, r1, r2, grip);
    float arm2_position_deg = (arm2_position/pi) *180;
    servo2_arm2.write(arm2_position_deg);
    delay(50);
  }
  if (x_pos_1 == 512 && y_pos_1 == 0 && y > 0.00){
    y = y - increment_y ;
    x = x + increment_x;
    x_old = x; 
    float arm1_position = theta_1(x, y, r1, r2, grip);
    float arm1_position_deg = (arm1_position/pi) * 180;
    servo1_arm1.write(arm1_position_deg);
    float arm2_position = theta_2(x, y, r1, r2, grip);
    float arm2_position_deg = (arm2_position/pi) *180;
    servo2_arm2.write(arm2_position_deg);
    delay(50);
  }
  if (x_pos_2 == 0 && y_pos_2 == 512){
    base_position = base_position + 1;
    servo3_base.write(base_position);
    delay(50);
  }
  if (x_pos_2 == 1023 && y_pos_2 == 512){
    base_position = base_position - 1;
    servo3_base.write(base_position);
    delay(50);
  }
  if (digitalRead(grip_key_up) == LOW && grip_position <90){
    grip_position = grip_position + 1; 
    servo4_grip.write(grip_position);
    delay(50);
  }

  if (digitalRead(grip_key_down)== LOW && grip_position > 0 ){
    grip_position = grip_position - 1; 
    servo4_grip.write(grip_position);
    delay(50);
  }
}