#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_SSD1306.h>
#include <math.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
#define NUMFLAKES     10 // Number of snowflakes in the animation example


#define ENCODER_CLK 2
#define ENCODER_DT  4
#define ENCODER_SW  5

float counter = 0;

//MPU
const int MPU_ADDR = 0x68; // I2C address of the MPU-6050. If AD0 pin is set to HIGH, the I2C address will be 0x69.
int16_t accelerometer_x, accelerometer_y, accelerometer_z; // variables for accelerometer raw data
int16_t gyro_x, gyro_y, gyro_z; // variables for gyro raw data
int16_t temperature; // variables for temperature data
char tmp_str[7]; // temporary variable used in convert function
char* convert_int16_to_str(int16_t i) { // converts int16 to string. Moreover, resulting strings will have the same length in the debug monitor.
  sprintf(tmp_str, "%6d", i);
  return tmp_str;
}
//
//MPU
Adafruit_MPU6050 mpu;

void setup() {
  // Initialize OLED
  Serial.begin(115200);
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  
  display.display();
  delay(300);


  // Initialize encoder pins
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  pinMode(ENCODER_SW, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), readEncoder, FALLING);

  //MPU
  Wire.begin();
  Wire.beginTransmission(MPU_ADDR); // Begins a transmission to the I2C slave (GY-521 board)
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  //
  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    display.display(); 
    delay(1000);
  }
  Serial.println("MPU6050 ready!");
}

void readEncoder() {
  int dtValue = digitalRead(ENCODER_DT);
  if (dtValue == HIGH) {
    counter++; // Clockwise
  }
  if (dtValue == LOW) {
    counter--; // Counterclockwise
  }
}

// Get the counter value, disabling interrupts.
// This make sure readEncoder() doesn't change the value
// while we're reading it.
int getCounter() {
  float result;
  noInterrupts();
  result = counter;
  interrupts();
  return result;
}

void resetCounter() {
  noInterrupts();
  counter = 0;
  interrupts();
}
//MPU
sensors_event_t event;
void loop() {

  //OLED
  display.clearDisplay();
  display.setTextColor(WHITE);        // Draw white text
  display.setTextSize(2,2);             // Normal 1:1 pixel scale
  display.setCursor(0, 0);
  display.println("Angulo: ");
  display.setTextSize(2);
  display.setCursor(80, 0);
  display.println(getCounter());
  display.setCursor(80, 0);
  display.println("   ");
  
  

 

  //

   Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) [MPU-6000 and MPU-6050 Register Map and Descriptions Revision 4.2, p.40]
  Wire.endTransmission(false); // the parameter indicates that the Arduino will send a restart. As a result, the connection is kept active.
  Wire.requestFrom(MPU_ADDR, 14, true); // request a total of 7*2=14 registers
  
  // "Wire.read()<<8 | Wire.read();" means two registers are read and stored in the same variable
  accelerometer_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x3B (ACCEL_XOUT_H) and 0x3C (ACCEL_XOUT_L)
  accelerometer_y = Wire.read()<<8 | Wire.read(); // reading registers: 0x3D (ACCEL_YOUT_H) and 0x3E (ACCEL_YOUT_L)
  accelerometer_z = Wire.read()<<8 | Wire.read(); // reading registers: 0x3F (ACCEL_ZOUT_H) and 0x40 (ACCEL_ZOUT_L)
  temperature = Wire.read()<<8 | Wire.read(); // reading registers: 0x41 (TEMP_OUT_H) and 0x42 (TEMP_OUT_L)
  gyro_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x43 (GYRO_XOUT_H) and 0x44 (GYRO_XOUT_L)
  gyro_y = Wire.read()<<8 | Wire.read(); // reading registers: 0x45 (GYRO_YOUT_H) and 0x46 (GYRO_YOUT_L)
  gyro_z = Wire.read()<<8 | Wire.read(); // reading registers: 0x47 (GYRO_ZOUT_H) and 0x48 (GYRO_ZOUT_L)
  
   //print out data
  Serial.print("ACELERACION:");
  display.setTextColor(WHITE);        // Draw white text
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setCursor(0, 20);
  display.print("ACELERACION");
  display.setCursor(0, 30);
  display.print("X: ");
  display.print(accelerometer_x);
  Serial.print(convert_int16_to_str(accelerometer_x));Serial.print(","); 
  display.setCursor(0, 40);
  display.print("Y: ");
  display.print(accelerometer_y);
  Serial.print(convert_int16_to_str(accelerometer_y));Serial.print(","); 
  display.setCursor(0, 50);
  display.print("Z: ");
  display.print(accelerometer_z);
  Serial.print(convert_int16_to_str(accelerometer_z));Serial.print("  ");
  // the following equation was taken from the documentation [MPU-6000/MPU-6050 Register Map and Description, p.30]
  Serial.print("T:");
  Serial.print(temperature/340.00+36.53);
  Serial.print("  ");
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setCursor(70, 20);
  Serial.print("ROTACION: "); 
  display.print("ROTACION");
  display.setCursor(70, 30);
  display.print("X:");
  display.print(gyro_x);
  Serial.print(convert_int16_to_str(gyro_x));Serial.print(","); 
  display.setCursor(70, 40);
  display.print("Y: ");
  display.print(gyro_y);
  Serial.print(convert_int16_to_str(gyro_y)); Serial.print(","); 
  display.setCursor(70, 50);
  display.print("Z: ");
  display.print(gyro_z);
  Serial.print(convert_int16_to_str(gyro_z));  Serial.println();
  display.display();      // Show initial text
  delay(200);
    
}