#include "Servo.h"

#define SRV1 3
#define SRV2 5
#define SRV3 6
#define SRV4 9
#define RESETBTN 2
#define ARM_LENGTH 2

const float RAD2DEG = 180.0f / PI;
const float magicnum=sqrt(1.5);

Servo base;
Servo middle;
Servo mid_pitch;
Servo grabber;

int base_angle(double x, double y) {
  return (int)(atan2(x, y) * RAD2DEG + 360) % 360;
}

int mid_angle(double x, double y, double z) {
  double top = sqrt((x * x) + (y * y) + (z * z));
  return (int)(2 * asin((top / ARM_LENGTH) / 4) * RAD2DEG);
}

int mid_pitch_angle(double x,double y, double z){
  double bottom=sqrt((x * x) + (y * y) + (z * z));
  return(int)(2 * asin(((z*magicnum)/bottom) / 2) * RAD2DEG);
}

void setup() {


  Serial.begin(9600);
  Serial.println(base_angle(2, 2));
  Serial.println(mid_angle(2, 2, 2));
  Serial.println(mid_pitch_angle(2, 2, 2));

  pinMode(SRV1,OUTPUT);
  pinMode(SRV2,OUTPUT);
  pinMode(SRV3,OUTPUT);
  pinMode(SRV4,OUTPUT);


  base.attach(SRV1);
  middle.attach(SRV2);
  mid_pitch.attach(SRV3);
  grabber.attach(SRV4);

  base.write(0);
  middle.write(0);
  mid_pitch.write(0);
  grabber.write(0);

  Serial.println("START");
  delay(2000);
}

void loop() {
  int x = 2;
  int y = 3;
  int z = 5;
  int delaytime=1000;
  // put your main code here, to run repeatedly:
  base.write(base_angle(x, y));
  middle.write(mid_angle(x, y, z));
  mid_pitch.write(mid_pitch_angle(x, y, z));
  delay(delaytime);
  grabber.write(90);
}