#include <Wire.h>
#include <Adafruit_SSD1306.h>

//direcciones MPU
#define MPU_ADDR 0x68
#define PWR_MGMT_1 0x6B
#define WHO_AM_I 0x75
#define ACCEL_XOUT_H 0x3B
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C

//variables MPU
float aRes = 2.0/32768.0;
float gRes = 250.0/32768.0;
float accelBias[3], gyroBias[3];

int16_t aRaw[3], gRaw[3];
float aSc[3], gSc[3];

long tiempo_prev, dt;
float a_angX, a_angY;
float g_angX, g_angY, g_angX_prev, g_angY_prev;

//Display settings
#define SCREEN_WIDTH 128  // OLED display width, in pixels
#define SCREEN_HEIGHT 64  // OLED display height, in pixels
#define OLED_RESET    -1  // Reset pin # (or -1 if sharing reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void writeByte(uint8_t address, uint8_t subAddress, uint8_t data){
  Wire.beginTransmission(address);
  Wire.write(subAddress); 
  Wire.write(data); 
  Wire.endTransmission(true);
}

uint8_t readByte(uint8_t address, uint8_t subAddress){ 
  Wire.beginTransmission(address);
  Wire.write(subAddress);
  Wire.endTransmission(false);
  Wire.requestFrom(address, 1, true); 
  return Wire.read();   
}

void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest){
  Wire.beginTransmission(address);
  Wire.write(subAddress);
  Wire.endTransmission(false);
  uint8_t i = 0;
  Wire.requestFrom(address, count);
  while(Wire.available()){
    dest[i++] = Wire.read();
    }
}

void initMPU(){
  /*Inicia el sensor, 
  selecciona como reloj interno un PLL 
  con referencia del eje x del girsocopio*/
  writeByte(MPU_ADDR,PWR_MGMT_1,0x01);
  /*Ajusta la velocidad de muestreo del MPU a 200Hz*/
  writeByte(MPU_ADDR,SMPLRT_DIV,0x04);
  /*Ajusta el filtro pasa bajos del giroscopio 
  y del acelerómetro a 42Hz y 44Hz, respectivamente*/
  writeByte(MPU_ADDR,CONFIG,0x03);
  /*Ajusta el rango de medición del giroscopio a +/- 250°/s*/
  writeByte(MPU_ADDR,GYRO_CONFIG,0x00);
  /*Ajusta el rango de sensor del acelerómetro a +/- 2g*/
  writeByte(MPU_ADDR,ACCEL_CONFIG,0x00);
}

void calibrateMPU(float* accel_bias, float* gyro_bias){
  uint8_t data[14];
  int nSamples = 40;

  for(int i=0; i<nSamples; i++){
    int16_t accel_temp[3] = {0,0,0}, gyro_temp[3] = {0,0,0};
    readBytes(MPU_ADDR, ACCEL_XOUT_H, 14, data);

    // Form signed 16-bit integer for each sample
    accel_temp[0] = data[0] << 8 | data[1];  
    accel_temp[1] = data[2] << 8 | data[3];
    accel_temp[2] = data[4] << 8 | data[5];    
    gyro_temp[0]  = data[8] << 8 | data[9];
    gyro_temp[1]  = data[10] << 8 | data[11];
    gyro_temp[2]  = data[12] << 8 | data[13];
    
    //Suma de las mediciones
    accel_bias[0] += accel_temp[0]; 
    accel_bias[1] += accel_temp[1];
    accel_bias[2] += accel_temp[2];
    gyro_bias[0]  += gyro_temp[0];
    gyro_bias[1]  += gyro_temp[1];
    gyro_bias[2]  += gyro_temp[2];
  }

  //Promedio de las mediciones
  accel_bias[0] /= nSamples; 
  accel_bias[1] /= nSamples;
  accel_bias[2] /= nSamples;
  gyro_bias[0]  /= nSamples;
  gyro_bias[1]  /= nSamples;
  gyro_bias[2]  /= nSamples;

  if(accel_bias[2]>0L){accel_bias[2] -= (int32_t)16384;}
  else{accel_bias[2] += (int32_t)16384;}
}

uint8_t readWhoAmI(){
  uint8_t id  = readByte(MPU_ADDR, WHO_AM_I);
  return id; 
}

void readData() {
  uint8_t data[14];
  readBytes(MPU_ADDR, ACCEL_XOUT_H, 14, data);
  //0x3B (ACCEL_XOUT_H)|0x3C (ACCEL_XOUT_L)
  aRaw[0] = data[0] << 8 | data[1];
  //0x3D (ACCEL_YOUT_H)|0x3E (ACCEL_YOUT_L)
  aRaw[1] = data[2] << 8 | data[3];
  //0x3F (ACCEL_ZOUT_H)|0x40 (ACCEL_ZOUT_L) 
  aRaw[2] = data[4] << 8 | data[5];
  //0x43 (GYRO_XOUT_H)|0x44 (GYRO_XOUT_L) 
  gRaw[0] = data[8] << 8 | data[9]; 
  //0x45 (GYRO_YOUT_H)|0x46 (GYRO_YOUT_L)
  gRaw[1] = data[10] << 8 | data[11]; 
  //0x47 (GYRO_ZOUT_H)|0x48 (GYRO_ZOUT_L)
  gRaw[2] = data[12] << 8 | data[13]; 
}

void displayWhoAmI(uint8_t id){
  display.clearDisplay();
  display.setTextColor(WHITE);
  //display.startscrollright(0x00, 0x0F);
  display.setTextSize(1);
  display.setCursor(0,0);
  display.print("i am 0x"); 
  display.println(id, HEX);

  switch(id){
    case 0x68:
      display.print("MPU 6000/6050/9150");
      break;
    case 0x70:
      display.print("MPU 6500");
      break;
    case 0x71:
      display.print("MPU 9250");
      break;
    case 0x73:
      display.print("MPU 9255");
      break;
  }

  display.display();
}

void displayData(int16_t Acc[], int16_t Gyr[]){
  display.clearDisplay();
  display.setCursor(0,0);
  display.println("Acelerometro ");
  display.print(Acc[0]);
  display.print(", ");
  display.print(Acc[1]);
  display.print(", ");
  display.println(Acc[2]);
  display.println("Giroscopio ");
  display.print(Gyr[0]);
  display.print(", ");
  display.print(Gyr[1]);
  display.print(", ");
  display.println(Gyr[2]);
  display.display();
}

void displayData(float Acc[], float Gyr[]){
  display.clearDisplay();
  display.setCursor(0,0);
  display.println("Acelerometro ");
  display.print(Acc[0]);
  display.print(", ");
  display.print(Acc[1]);
  display.print(", ");
  display.println(Acc[2]);
  display.println("Giroscopio ");
  display.print(Gyr[0]);
  display.print(", ");
  display.print(Gyr[1]);
  display.print(", ");
  display.println(Gyr[2]);
  display.display();
}

void displayData(float Acc[], float Gyr[], float angX, float angY){
  display.clearDisplay();
  display.setCursor(0,0);
  display.println("Acelerometro ");
  display.print(Acc[0]);
  display.print(", ");
  display.print(Acc[1]);
  display.print(", ");
  display.println(Acc[2]);
  display.println("Giroscopio ");
  display.print(Gyr[0]);
  display.print(", ");
  display.print(Gyr[1]);
  display.print(", ");
  display.println(Gyr[2]);
  display.println("Rotacion en x: ");
  display.println(angX);
  display.println("Rotacion en y: ");
  display.println(angY);
  display.display();
}

void setup() {
  Serial.begin(115200);
  /*Setup I2C*/
  Wire.begin();
  /*Initialize sensor*/
  initMPU();
  /*Initialize Display*/
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);

  /*Identify Sensor*/
  uint8_t id = readWhoAmI();
  displayWhoAmI(id);
  delay(2000);
  calibrateMPU(accelBias, gyroBias);
}

void loop() {

  dt = millis()-tiempo_prev;

  if(dt >= 5)
  {
    readData();
    aSc[0] = (aRaw[0] - accelBias[0])*aRes;
    aSc[1] = (aRaw[1] - accelBias[1])*aRes;
    aSc[2] = (aRaw[2] - accelBias[2])*aRes;
    gSc[0] = (gRaw[0] - gyroBias[0])*gRes;
    gSc[1] = (gRaw[1] - gyroBias[1])*gRes;
    gSc[2] = (gRaw[2] - gyroBias[2])*gRes;

    a_angX = atan(aSc[0]/sqrt(pow(aSc[1],2) + pow(aSc[2],2)))*(180.0/3.1416);
    a_angY = atan(aSc[1]/sqrt(pow(aSc[0],2) + pow(aSc[2],2)))*(180.0/3.1416);
    
    g_angX = 0.98*(gSc[0]*dt/1000 + g_angX_prev) + 0.02*a_angX;
    g_angY = 0.98*(gSc[1]*dt/1000 + g_angY_prev) + 0.02*a_angY;

    g_angX_prev = g_angX;
    g_angY_prev = g_angY;

    displayData(aSc,gSc,g_angX,g_angY);

    tiempo_prev = millis();
  }

}