#include <Arduino_LSM9DS1.h>
#include "SensorFusion.h" //SF

#include <BLEMIDI_Transport.h>
//#include <hardware/BLEMIDI_ESP32_NimBLE.h>
//#include <hardware/BLEMIDI_ESP32.h>
//#include <hardware/BLEMIDI_nRF52.h>
#include <hardware/BLEMIDI_ArduinoBLE.h>

BLEMIDI_CREATE_DEFAULT_INSTANCE()

unsigned long tm = millis();
unsigned long t0 = millis();
bool isConnected = false;

// midi
// offsets CC:
// 0-63: left hand
// 64-127: right hand
enum side {LEFT, RIGHT=64};
int hand = LEFT;
int midichannel = 1;

// accelerometer
float accx, accy, accz;
float maccx, maccy, maccz; // midi mapped
float accmag; // magnitude

// gyroscope
float gyrox, gyroy, gyroz;
float mgyrox, mgyroy, mgyroz;
// magnetometer
float magx, magy, magz;
float mmagx, mmagy, mmagz;
// sensor fusion 
float mroll, mpitch, myaw;

float maxx, maxy, maxz;

//SF - 
SF fusion;

float gx, gy, gz, ax, ay, az, mx, my, mz;
float pitch, roll, yaw;
float deltat;

float gxoff = 0;
float gyoff = 0;
float gzoff = 0;

float fmap(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

byte highbits(float f) {
  return int(f) % 128;
}

byte lowbits(float f) {
  return int(f * 128) % 128;
};

unsigned long loops = 0;
unsigned long loopmillis = 0;

unsigned long midis = 0;
unsigned long midimillis = 0;


// -----------------------------------------------------------------------------
// When BLE connected, LED will turn on (indication that connection was successful)
// When receiving a NoteOn, LED will go out, on NoteOff, light comes back on.
// This is an easy and conveniant way to show that the connection is alive and working. 
// -----------------------------------------------------------------------------
void setup()
{
  Serial.begin(115200);

// while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB
//  }

  loopmillis = millis();
  midimillis = millis();

  Serial.println("MIDI-BLE TEST 3");
  Serial.println("2022-06-16");


  MIDI.begin();

  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);

  BLEMIDI.setHandleConnected([]() {
    isConnected = true;
    digitalWrite(LED_BUILTIN, HIGH);
    Serial.println("MIDI-BLE CONNECTED");

  });

  BLEMIDI.setHandleDisconnected([]() {
    isConnected = false;
    digitalWrite(LED_BUILTIN, LOW);
    Serial.println("MIDI-BLE DISCONNECTED");

  });

  MIDI.setHandleNoteOn([](byte channel, byte note, byte velocity) {
    digitalWrite(LED_BUILTIN, LOW);
  });
  MIDI.setHandleNoteOff([](byte channel, byte note, byte velocity) {
    digitalWrite(LED_BUILTIN, HIGH);
  });


// gyro calibration
 Serial.print("Loading calibration: ");
  if (loadCalibration()) {
    gxoff=getGyroXCal();
    gyoff=getGyroYCal();
    gyoff=getGyroZCal();  
    Serial.println("success :)");
  } else {
      Serial.println("failed :(");
  }

  printCalibration();

  // motion sensor
  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }
  
  Serial.print("Accelerometer sample rate = ");
  Serial.print(IMU.accelerationSampleRate());
  Serial.println(" Hz");
  Serial.println();
  Serial.println("Acceleration in G's");
  Serial.println("X\tY\tZ");
  
  Serial.print("Gyroscope sample rate = ");
  Serial.print(IMU.gyroscopeSampleRate());
  Serial.println(" Hz");
  Serial.println();
  Serial.println("Gyroscope in degrees/second");
  Serial.println("X\tY\tZ");
  
  Serial.print("Magnetic field sample rate = ");
  Serial.print(IMU.magneticFieldSampleRate());
  Serial.println(" uT");
  Serial.println();
  Serial.println("Magnetic Field in uT");
  Serial.println("X\tY\tZ");

  Serial.print("Setup took (ms): ");
  Serial.println(millis()-loopmillis);
  loopmillis = millis();
  midimillis = millis();
  
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void loop()
{

 // now you should read the gyroscope, accelerometer (and magnetometer if you have it also)
    // NOTE: the gyroscope data have to be in radians
    // if you have them in degree convert them with: DEG_TO_RAD example: gx * DEG_TO_RAD


  if (IMU.magneticFieldAvailable()) {
    IMU.readMagneticField(mx, my, mz);
  }

  if (IMU.accelerationAvailable()&&IMU.gyroscopeAvailable()) {
 tm = millis();
 
 loops++;

 if (!(loops%1000)) {
  Serial.print("1000 loops took (ms): ");
  Serial.println(tm-loopmillis);
  loopmillis=tm;
 }

  MIDI.read();

    IMU.readAcceleration(ax, ay, az);
  /*
    ax *= 9.81;  
    ay *= 9.81;  
    az *= 9.81;  
  */
  
    IMU.readGyroscope(gx, gy, gz);

    gx -= gxoff;
    gy -= gxoff;
    gz -= gxoff;

    gx *= DEG_TO_RAD;
    gy *= DEG_TO_RAD;
    gz *= DEG_TO_RAD;
  
   
    deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
    //choose only one of these two:
    //fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltat);  //mahony is suggested if there isn't the mag and the mcu is slow
    fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, -mx, my, mz, deltat);  //else use the magwick, it is slower but more accurate



  if (isConnected && (tm - t0) > 6)
  {
    t0 = tm;

    switch (midis) {



      case 0:

        
        
    maccx = fmap(ax,-4.0,4.0, 0.0,127.0);
        MIDI.sendControlChange(0 + hand, maccx, midichannel);
        break;

      case 1:
    maccy = fmap(ay,-4.0,4.0, 0.0,127.0);
    MIDI.sendControlChange(1 + hand, maccy, midichannel);
break;

case 2:
    maccz = fmap(az,-4.0,4.0, 0.0,127.0);
    MIDI.sendControlChange(2 + hand, maccz, midichannel);
break;

case 3:
    mgyrox = map(gx, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0);
    MIDI.sendControlChange(3 + hand, mgyrox, midichannel);
break;

case 4:
    mgyroy = map(gy, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0);
    MIDI.sendControlChange(4 + hand, mgyroy, midichannel);
break;

case 5:
    mgyroz = map(gz, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0);
    MIDI.sendControlChange(5 + hand, mgyroz, midichannel);
break;

case 6:
    accmag = sqrt(ax*ax+ay*ay+az*az);
//    accmag = map(accmag, 0, 1000, 0,127);
    
    MIDI.sendControlChange(6 + hand, accmag * 100, midichannel);
break;

case 7:
    mmagx = fmap(mx, 0,60, 0,127);
    MIDI.sendControlChange(7 + hand, mmagx, midichannel);
break;

case 8:
    mmagy = fmap(my, 0,60, 0,127);
    MIDI.sendControlChange(8 + hand, mmagy, midichannel);
break;

case 9:
    mmagz = fmap(mz, 0,60, 0,127);
    MIDI.sendControlChange(9 + hand, mmagz, midichannel);
break;

case 10:
    roll = fusion.getRoll();    //you could also use getRollRadians() ecc
    mroll = fmap(roll,-180,180,0,127);
    MIDI.sendControlChange(10 + hand, mroll, midichannel);
break;

case 11:
    pitch = fusion.getPitch();
    mpitch = fmap(pitch,-90,90,0,127);
    MIDI.sendControlChange(11 + hand, mpitch, midichannel);
break;

case 12:
    MIDI.sendControlChange(12 + hand, myaw, midichannel);
    yaw = fusion.getYaw();
    myaw = fmap(yaw,0,360,0,127);
break;

case 13:
    roll = fusion.getRoll();    //you could also use getRollRadians() ecc
    mroll = fmap(roll,-180,180,0,127);
    MIDI.sendControlChange(13 + hand, lowbits(mroll), midichannel);
break;

case 14:
    pitch = fusion.getPitch();
    mpitch = fmap(pitch,-90,90,0,127);
    MIDI.sendControlChange(14 + hand, lowbits(mpitch), midichannel);
break;

case 15:
    MIDI.sendControlChange(15 + hand, lowbits(myaw), midichannel);
    yaw = fusion.getYaw();
    myaw = fmap(yaw,0,360,0,127);
break;

    }

    //mgyrox = gyrox/2000 * 64 + 64;
    //mgyroy = gyroy/2000 * 64 + 64;
    //mgyroz = gyroz/2000 * 64 + 64;
//    mz = max(10, abs(z) * 1000);

//    maxx = max(magx, maxx);
//    maxy = max(magy, maxy);
//    maxz = max(magz, maxz);

//    Serial.print(mx);
//    Serial.print('\t');
//    Serial.print(my);
//    Serial.print(y);
//    Serial.print('\t');
//    Serial.println(z);
//    Serial.println();


//    MIDI.sendNoteOn (my, mx, 1); // note 60, velocity 100 on channel 1
   // Serial.print(mmagx);
   // Serial.print('\t');
   // Serial.print(mmagy);
   // Serial.print('\t');
   // Serial.println(mmagz);
//    Serial.print('\t');
//    Serial.println(accmag * 100);
//    Serial.println("ping");
//    delay(mz);
//    MIDI.sendNoteOff(my, mx, 1);

    midis=(midis+1)%16;  

    if (midis==0) {

    Serial.print("MIDI sending took (ms): ");
    Serial.println(tm-midimillis);
    midimillis=tm;
    }
  }




  
  }
  
  

  

}
