#include <Servo.h>

// ARM LENGTH CONSTANTS
#define BICEP_LENGTH 5 // shoulder/short length (screw to screw)
#define FOREARM_LENGTH 8 // long length + 1/2 gripper (screw to screw)

// GRIPPER POSITION CONSTRAINTS
#define MAX_RADIUS sqrt(BICEP_LENGTH * BICEP_LENGTH + FOREARM_LENGTH * FOREARM_LENGTH)
#define MIN_X FOREARM_LENGTH-BICEP_LENGTH
#define MAX_X MAX_RADIUS
#define MIN_Y FOREARM_LENGTH
#define MAX_Y MAX_RADIUS

// MOVEMENT THRESHOLD AND SCALE
#define MOVEMENT_SCALE 2
#define ROTATION_SPEED 2

// SERVOS
Servo baseServo;
Servo shoulderServo;
Servo elbowServo;
Servo gripperServo;

// PINS
#define SWIVEL_MOTOR 3
#define SHOULDER_JOINT 5
#define ELBOW_JOINT 6
#define GRIPPER_MOTOR 9
#define X_POT A0
#define Y_POT A1
#define GRIPPER_POT A2

// GLOBAL VARIABLES
float gripperXPos = (MIN_X+MAX_X)/2;
float gripperYPos = (MIN_Y+MAX_Y)/2;
float gripperOpenness = 0;
float xPot = 0;
float yPot = 0;
float gPot = 0;
float angShoulder = 0;
float angElbow = 0;

void setup() {
  // put your setup code here, to run once:
  pinMode(X_POT, INPUT);
  pinMode(Y_POT, INPUT);
  pinMode(GRIPPER_POT, INPUT);

  baseServo.attach(SWIVEL_MOTOR);
  shoulderServo.attach(SHOULDER_SERVO);
  elbowServo.attach(ELBOW_SERVO);
  gripperServo.attach(GRIPPER_SERVO);

}

void loop() {
  // put your main code here, to run repeatedly:
  readPots();
  determineMovement();
  controlArm();
  controlBase();
  controlGripper();
}

// Store the values found in the potentiometer controls
void readPots() {
  xPot = analogRead(X_POT);
  yPot = analogRead(Y_POT);
  gPot = analogRead(GRIPPER_POT);
  return;
}

// Determine the new position and state of the gripper
void determineMovement() {
  gripperXPos = constrain(map(xPot, 0, 1023, MIN_X, MAX_X), MIN_X, MAX_X);
  gripperYPos = constrain(map(yPot, 0, 1023, MIN_Y, MAX_Y), MIN_Y, MAX_Y);
  gripperOpenness = constrain(map(gPot, 0, 1023, 0, 100), 0, 100);
  return;
}

// Change the the shoulder and elbow servo angles based on the inverse kinematics
void controlArm() {
  shoulderServo.write(inverseKinematics(gripperXPos, gripperYPos, SHOULDER_SERVO));
  elbowServo.write(inverseKinematics(gripperXPos, gripperYPos, ELBOW_SERVO));
  return;
}

// Determine
void controlBase() {
  return;
}

void controlGripper() {
  return;
}

float inverseKinematics(float x, float y, byte servo_num) {
  double cb = (-sq(BICEP_LENGTH) - sq(FOREARM_LENGTH) + sq(x) + sq(y)) / (2 * (BICEP_LENGTH) * (FOREARM_LENGTH));
  double sbin = 1 - (sq(cb));
  double sb = sqrt(sbin);
  if (servo_num == SHOULDER_SERVO) {
    long double ca = ((x*BICEP_LENGTH)+(x*FOREARM_LENGTH*cb)+(y*FOREARM_LENGTH*sb))/(sq(x)+sq(y));
    double sain = 1 - (sq(ca));
    double sa = sqrt(sain);
    return (atan2(sa,ca)) * (180/PI);
  }
  else if (servo_num == ELBOW_SERVO) { 
    return (atan2(sb,cb)) * (180/PI);
  }
}