// C++ code
//
#include <Servo.h>
#include <I2Cdev.h>
#include <MPU6050.h>

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// shoulder not used since wrist pitch and roll are more accurate;
// further explained in the video: https://youtu.be/dQw4w9WgXcQ
const int basePin = 3;
const int elbowPin = 5;
const int wristPitchPin = 6;
const int wristRollPin = 9; 
const int gripperPin = 10;

Servo base;
Servo elbow;
Servo wristPitch;
Servo wristRoll;
Servo gripper;
MPU6050 accelgyro;

// refresh delay in millis
const int interval = 10;

unsigned long previousMillis;
unsigned long currentMillis;

int16_t ax, ay, az;
int16_t gx0, gy0, gz0;
int16_t gx, gy, gz;
float baseG = 90;
float wristPitchG = 90;
float wristRollG = 90;

// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated
// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read,
// not so easy to parse, and slow(er) over UART.
#define OUTPUT_READABLE_ACCELGYRO

void setup()
{

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  base.attach(basePin);
  elbow.attach(elbowPin);
  wristPitch.attach(wristPitchPin);
  wristRoll.attach(wristRollPin);
  gripper.attach(gripperPin);

  Serial.begin(115200);

   // initialize device
  Serial.println("Initializing I2C devices...");
  accelgyro.initialize();

  // verify connection
  Serial.println("Testing device connections...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  base.write(90);
  elbow.write(90);
  wristPitch.write(90);
  wristRoll.write(90);
  gripper.write(90);

}

float gyroServo(float val, int16_t g0, int16_t g) {
  
  val = val + (float(map(((float(g0) + float(g))/2), -32750, 32750, -250, 250))/(1000/interval));
    
    if (val < 0) {
      val = 0;
    }
    
    if (val > 180) {
      val = 180;
    }
    
  g0 = g;
  
  return val;

}

void loop()
{
    
  // read raw accel/gyro measurements from device
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  currentMillis = millis();
  
  if ((previousMillis + interval) >= currentMillis){
        
    wristPitchG = gyroServo(wristPitchG, gy0, gy);
    wristPitch.write(wristPitchG);
    
    wristRollG = gyroServo(wristRollG, gz0, gz);
    wristRoll.write(wristRollG);

    previousMillis = currentMillis;

  }

  // display tab-separated accel/gyro x/y/z values
  #ifdef OUTPUT_READABLE_ACCELGYRO
      Serial.print("a/g:\t");
      Serial.print(ax); Serial.print("\t");
      Serial.print(ay); Serial.print("\t");
      Serial.print(az); Serial.print("\t");
      Serial.print(gx); Serial.print("\t");
      Serial.print(gy); Serial.print("\t");
      Serial.println(gz);
  #endif

}