#include <MPU6050_light.h>
#include <Wire.h>

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

// GRIPPER POSITION CONSTRAINTS
// Setting the range of the XY Coordinate system as a box
// from the FOREARM_LENGTH to the point on the circle of
// reach that forms a square
#define MAX_RADIUS (SHOULDER_LENGTH + FOREARM_LENGTH) * sqrt(2) / 2
#define MIN_X FOREARM_LENGTH
#define MAX_X MAX_RADIUS
#define MIN_Y FOREARM_LENGTH
#define MAX_Y MAX_RADIUS

// MOVEMENT THRESHOLD AND SCALE
#define MOVEMENT_SCALE 50
#define ANGLE_THRESHOLD 35

// PIN DEFINITIONS
#define RESET_BUTTON 12
#define CALIBRATING_LED 13

// OBJECTS
MPU6050 mpu(Wire); // create mpu object to read data from the MPU6050

// GLOBAL VARIABLES
float gripperXPos = (MIN_X+MAX_X)/2;
float gripperYPos = (MIN_Y+MAX_Y)/2;
float angX = 0;
float angY = 0;
float angZ = 0;

unsigned long timer = 0;

void setup() {
  // GENERAL SETUP (pinModes and attachments)
  Serial.begin(38400);
  pinMode(CALIBRATING_LED, OUTPUT);
  pinMode(RESET_BUTTON, INPUT_PULLUP);

  // SENSOR SETUP
  Wire.begin();
  mpu.begin();
  calibrate();
}

void loop() {
  checkReset();
  readSensors();
  determineMovement();
  printData();
}

// CALL ON CALIBRATE METHOD FROM MPU LIBRARY
void calibrate() {
  digitalWrite(CALIBRATING_LED, HIGH);
  mpu.calcGyroOffsets();
  digitalWrite(CALIBRATING_LED, LOW);
}

// CALL ON CALIBRATE IF RESET BUTTON PRESSED
void checkReset() {
  if(!digitalRead(RESET_BUTTON)) {
    calibrate();
  }
}

// READ ANGLES FROM MPU6050, ADJUST SIGN DEPENDING ON MPU ORIENTATION
void readSensors() {
  mpu.update();
  angX = mpu.getAngleX();
  angY = mpu.getAngleY();
  angZ = -mpu.getAngleZ();
}

// PRINT CALCULATED ANGLES OF THE SENSOR AND XY-COORDINATES
void printData() {
  if((millis()-timer)>10) // print data every 10ms
  {
    Serial.print("P: ");
    Serial.print(angX);
    Serial.print(" R: ");
    Serial.print(angY);
    Serial.print(" Y: ");
    Serial.print(angZ);
    Serial.print(" XPos: ");
    Serial.print(gripperXPos);
    Serial.print(" YPos: ");
    Serial.print(gripperYPos);
    Serial.println();
    timer = millis();
  }
}

// CALCULATE XY-COORDINATES BASED ON CALCULATED ANGLES
void determineMovement() {
  gripperXPos = constrain(gripperXPos + floor(angX / ANGLE_THRESHOLD) / MOVEMENT_SCALE, MIN_X, MAX_X);
  gripperYPos = constrain(gripperYPos + floor(angY / ANGLE_THRESHOLD) / MOVEMENT_SCALE, MIN_Y, MAX_Y);
}