#include <Servo.h>

const int JOY_R_VERT = A4;
const int JOY_R_HORIZ = A5;
const int JOY_L_VERT = A0;
const int JOY_L_HORIZ = A1;

const int CLAW = 3;
const int ELBOW = 5;
const int BASE = 6;
const int SHOULDER = 9;

Servo clawMotor;
Servo shoulderMotor;
Servo elbowMotor;
Servo baseMotor;

int getJoystickPosition(int pin) {
  int amount = analogRead(pin);
  int result = 0;

  if(amount > 600)
    result = -1;
  else if(amount < 450)
    result = 1;
  
  return result;
}

int setMotorPosition(Servo motor, int position) {
  if(position < 0) {
    position = 0;
  }else if(position > 180) {
    position = 180;
  }
  motor.write(position);
  // delay(30);
}

void setup() {
  pinMode(JOY_R_VERT, INPUT);
  pinMode(JOY_R_HORIZ, INPUT);
  pinMode(JOY_R_HORIZ, INPUT);
  pinMode(JOY_L_HORIZ, INPUT);
  Serial.begin(9600);

  clawMotor.attach(CLAW);
  shoulderMotor.attach(SHOULDER);
  elbowMotor.attach(ELBOW);
  baseMotor.attach(BASE);

  setMotorPosition(clawMotor, 0);
  setMotorPosition(baseMotor, 90);
  setMotorPosition(shoulderMotor, 0);
  setMotorPosition(elbowMotor, 0);
}

void loop() {
  int motorSpeed = 5;

  int clawAmt = getJoystickPosition(JOY_L_HORIZ);
  int clawPos = clawMotor.read() + motorSpeed * clawAmt;
  setMotorPosition(clawMotor, clawPos);

  int baseAmt = getJoystickPosition(JOY_R_HORIZ);
  int basePos = baseMotor.read() + motorSpeed * baseAmt;
  setMotorPosition(baseMotor, basePos);

  int shoulderAmt = getJoystickPosition(JOY_R_VERT);
  int shoulderPos = shoulderMotor.read() + motorSpeed * shoulderAmt;
  setMotorPosition(shoulderMotor, shoulderPos);

  int elbowAmt = getJoystickPosition(JOY_L_VERT);
  int elbowPos = elbowMotor.read() + motorSpeed * elbowAmt;
  setMotorPosition(elbowMotor, elbowPos);
}