#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <Servo.h>
#include "pitches.h"

#define SPEAKER_PIN 8
Servo arm;
Adafruit_MPU6050 mpu;
float pos = 0.0; // Variable where the arm's position will be stored (in degrees)
float step = 5; // Variable used for the arm's position step
const int pinR = 9;
const int pinG = 10;
const int pinB = 11;
const int button = 5;
int pitch;
void setup(void) {

  pinMode( 2, OUTPUT);
  pinMode( 8, OUTPUT);
  pinMode( 9, OUTPUT);
  pinMode( 10, OUTPUT);
  pinMode( 11, OUTPUT);
  pinMode(A1, INPUT_PULLUP); // Set the A1 pin to a pushbutton in pullup mode
  arm.attach(2); // Attache the arm to the pin 2
  arm.write(pos); // Initialize the arm's position to 0 (leftmost)
  
  Serial.begin(115200);
  while (!Serial)
    delay(10); 

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  // Serial.print("Accelerometer range set to: ");
  // switch (mpu.getAccelerometerRange()) {
  // case MPU6050_RANGE_2_G:
  //   Serial.println("+-2G");
  //   break;
  // case MPU6050_RANGE_4_G:
  //   Serial.println("+-4G");
  //   break;
  // case MPU6050_RANGE_8_G:
  //   Serial.println("+-8G");
  //   break;
  // case MPU6050_RANGE_16_G:
  //   Serial.println("+-16G");
  //   break;
  // }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  // Serial.print("Gyro range set to: ");
  // switch (mpu.getGyroRange()) {
  // case MPU6050_RANGE_250_DEG:
  //   Serial.println("+- 250 deg/s");
  //   break;
  // case MPU6050_RANGE_500_DEG:
  //   Serial.println("+- 500 deg/s");
  //   break;
  // case MPU6050_RANGE_1000_DEG:
  //   Serial.println("+- 1000 deg/s");
  //   break;
  // case MPU6050_RANGE_2000_DEG:
  //   Serial.println("+- 2000 deg/s");
  //   break;
  // }

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  // Serial.print("Filter bandwidth set to: ");
  // switch (mpu.getFilterBandwidth()) {
  // case MPU6050_BAND_260_HZ:
  //   Serial.println("260 Hz");
  //   break;
  // case MPU6050_BAND_184_HZ:
  //   Serial.println("184 Hz");
  //   break;
  // case MPU6050_BAND_94_HZ:
  //   Serial.println("94 Hz");
  //   break;
  // case MPU6050_BAND_44_HZ:
  //   Serial.println("44 Hz");
  //   break;
  // case MPU6050_BAND_21_HZ:
  //   Serial.println("21 Hz");
  //   break;
  // case MPU6050_BAND_10_HZ:
  //   Serial.println("10 Hz");
  //   break;
  // case MPU6050_BAND_5_HZ:
  //   Serial.println("5 Hz");
  //   break;
  // }

  Serial.println("");
  delay(100);
}

void loop() {
  
  digitalWrite(9, 0);
  digitalWrite(10, 0);
  digitalWrite(11, 0);
  pos = 45;
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  /* Print out the values */
  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  Serial.println("");
  int newpitch = 0;

  if(a.acceleration.x > a.acceleration.y && a.acceleration.x > a.acceleration.z)
    newpitch = NOTE_C4;


  if(a.acceleration.x < a.acceleration.y && a.acceleration.y > a.acceleration.z)
    newpitch = NOTE_F4;


  if(a.acceleration.z > a.acceleration.y && a.acceleration.x < a.acceleration.z)
    newpitch = NOTE_C5;

  tone(SPEAKER_PIN, newpitch);
  delay(100);
  noTone(SPEAKER_PIN);

  if (digitalRead(button)==HIGH) // Check for the yellow button input
  { //Serial.println(" in ");
     
    if (pos>0) // Check that the position won't go lower than 0°
    {
      if (g.gyro.x > g.gyro.y && g.gyro.x > g.gyro.z)
      { 
          if (pos < 60)
            pos = 89;
          step = 10;
          
      }
      if (g.gyro.x < g.gyro.y && g.gyro.z < g.gyro.y)
      { 
          if (pos <30 || pos >60)
            pos = 59; 
          step = 10;
           
      }
      if (g.gyro.x < g.gyro.z && g.gyro.z > g.gyro.y)
      { 
          if (pos > 30)
            pos = 0;
          step = -10;
      }

      arm.write(pos); // Set the arm's position to "pos" value
      pos-=step; // Decrement "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
    }
  }
  if(digitalRead(button)==LOW) 
  {
    if (g.gyro.x > g.gyro.y && g.gyro.x > g.gyro.z)
    { 
        digitalWrite(9, 1);
    }
    if (g.gyro.x < g.gyro.y && g.gyro.z < g.gyro.y)
    { 
        digitalWrite(10, 1);
    }
    if (g.gyro.x < g.gyro.z && g.gyro.z > g.gyro.y)
    { 
        digitalWrite(11, 1);
    }

  }

  
  delay(100);
}