#include <SPI.h>
#include <Wire.h>
#include <MIDI.h>

#define MPU 0x68  // I2C address of the MPU-6050
const int DEFAULT_MIDI_CHANNEL = 1;
MIDI_CREATE_DEFAULT_INSTANCE();


double AcX,AcY,AcZ;
int Pitch, Roll;
int Yf;
int Yb;
int Xf;
int Xb;


void setup(){
   MIDI.begin(MIDI_CHANNEL_OMNI);

  Serial.begin(115200);

  init_MPU(); // Inizializzazione MPU6050
}
 
void loop()
{

int mc1 =0;
int mc2  =0;
int mc3  =0;
int mc4  =0;
  
  FunctionsMPU(); // Acquisisco assi AcX, AcY, AcZ.
    
  Roll = FunctionsPitchRoll(AcX, AcY, AcZ);   //Calcolo angolo Roll
  Pitch = FunctionsPitchRoll(AcY, AcX, AcZ);  //Calcolo angolo Pitch

  int xposition = map(Roll, -90, 90, 0, 179);
  int yposition = map(Pitch, -90, 90, 179, 0);
 
 

if (yposition < 80 && yposition > 10) {

int Yf = yposition;

  mc1 = map(Yf, 79, 11, 0, 127);

}

if (yposition > 100 && yposition < 170) {

int Yb = yposition;

  mc2 = map(Yb, 101, 169, 0, 127);

}

if (xposition < 80 && xposition > 10) {

int xf = xposition;

  mc3 = map(xf, 79, 11, 0, 127);

}

if (xposition > 100 && xposition < 170) {

int xb = xposition;

  mc4 = map(xb, 101, 169, 0, 127);

}



 MIDI.sendControlChange(1, mc1, DEFAULT_MIDI_CHANNEL);
 MIDI.sendControlChange(2, mc2, DEFAULT_MIDI_CHANNEL);
MIDI.sendControlChange(3, mc3, DEFAULT_MIDI_CHANNEL);
 MIDI.sendControlChange(4, mc4, DEFAULT_MIDI_CHANNEL);



  //Serial.print("Y: "); Serial.print(mc2);
 // Serial.print("\t");
//Serial.print("X: "); Serial.print(mc1);
 //Serial.print("\n");

}

void init_MPU(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  delay(1000);
}

//Function to calculate angles
double FunctionsPitchRoll(double A, double B, double C){
  double DatoA, DatoB, Value;
  DatoA = A;
  DatoB = (B*B) + (C*C);
  DatoB = sqrt(DatoB);
  
  Value = atan2(DatoA, DatoB);
  Value = Value * 180/3.14;
  
  return (int)Value;
}

//Function to get  X,Y,Z from MPU6050
void FunctionsMPU(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
}