/*
  MPU6050 and LCD 20x4 with Franzininho
  by Anderson Costa with ❤ for the Wokwi community
  Visit https://wokwi.com to learn about the Wokwi Simulator
  Visit https://franzininho.com.br to learn about the Franzininho
*/

#include <TinyWireM.h>
#include <LiquidCrystal_I2C.h>

#define MPU_ADDR 0x68                    // Endereco I2C do MPU6050
#include<Servo.h>
Servo Myservo;
float  x,y,z;
LiquidCrystal_I2C lcd(0x27, 20, 4);      // Define endereco e 20 caracteres/4 linhas

// Variaveis para armazenar valores dos sensores
int accelX, accelY, accelZ, temp;
int gyroX, gyroY, gyroZ;

// Variaveis para corrigir o espaço do valor anterior
int lastAccelX, lastAccelY, lastAccelZ, lastTemp;
int lastGyroX, lastGyroY, lastGyroZ;

unsigned long timerRequest;

void setup() {
  // Inicializa o LCD
  lcd.begin(20, 4);
  lcd.init();                            // Inicializa o display
  lcd.backlight();                       // Acende a luz de fundo do display
  lcd.clear();                           // Limpa o display
  Myservo.attach(1);
  // Inicializa o MPU-6050
  TinyWireM.begin();
  TinyWireM.beginTransmission(MPU_ADDR);
  TinyWireM.write(0x6B);                 // Endereço de configuração de energia
  TinyWireM.write(0);                    // Desperta o MPU-6050
  TinyWireM.endTransmission();

/*  // Informações iniciais do display
  lcd.setCursor(0, 0);
  lcd.print("Acelerometro");
  lcd.setCursor(0, 2);
  lcd.print("Giroscopio");*/
}

void loop() {
  if (millis() - timerRequest >= 200) {
    // Reseta o temporizador de requisição
    timerRequest = millis();

    TinyWireM.beginTransmission(MPU_ADDR);
    TinyWireM.write(0x3B);                 // Iniciando no registrador 0x3B
    TinyWireM.endTransmission();

    // Solicita os dados do sensor
    TinyWireM.requestFrom(MPU_ADDR, 14);   // Requisição de 14 bytes

    // Armazena os valores retornados nas variaveis correspondentes
    accelX = TinyWireM.read() << 8 | TinyWireM.read(); // 0x3B - ACCEL_XOUT_H | 0x3C - ACCEL_XOUT_L
    accelY = TinyWireM.read() << 8 | TinyWireM.read(); // 0x3D - ACCEL_YOUT_H | 0x3E - ACCEL_YOUT_L
    accelZ = TinyWireM.read() << 8 | TinyWireM.read(); // 0x3F - ACCEL_ZOUT_H | 0x40 - ACCEL_ZOUT_L
    temp = TinyWireM.read() << 8 | TinyWireM.read();   // 0x41 - TEMP_OUT_H   | 0x42 - TEMP_OUT_L
    gyroX = TinyWireM.read() << 8 | TinyWireM.read();  // 0x43 - GYRO_XOUT_H  | 0x44 - GYRO_XOUT_L
    gyroY = TinyWireM.read() << 8 | TinyWireM.read();  // 0x45 - GYRO_YOUT_H  | 0x46 - GYRO_YOUT_L
    gyroZ = TinyWireM.read() << 8 | TinyWireM.read();  // 0x47 - GYRO_ZOUT_H  | 0x48 - GYRO_ZOUT_L
/*
    // Corrige o espaço usado pelo último valor de accelX
    if (lastAccelX != accelX) {
      lastAccelX = accelX;
      lcd.setCursor(2, 1);
      lcd.print("     ");
    }

    // Corrige o espaço usado pelo último valor de accelY
    if (lastAccelY != accelY) {
      lastAccelY = accelY;
      lcd.setCursor(9, 1);
      lcd.print("    ");
    }

    // Corrige o espaço usado pelo último valor de accelZ
    if (lastAccelZ != accelZ) {
      lastAccelZ = accelZ;
      lcd.setCursor(15, 1);
      lcd.print("     ");
    }

    // Corrige o espaço usado pelo último valor da temperatura
    if (lastTemp != temp) {
      lastTemp = temp;
      lcd.setCursor(15, 0);
      lcd.print("     ");
    }

    // Corrige o espaço usado pelo último valor de gyroX
    if (lastGyroX != gyroX) {
      lastGyroX = gyroX;
      lcd.setCursor(2, 3);
      lcd.print("     ");
    }

    // Corrige o espaço usado pelo último valor de gyroY
    if (lastGyroY != gyroY) {
      lastGyroY = gyroY;
      lcd.setCursor(9, 3);
      lcd.print("    ");
    }

    // Corrige o espaço usado pelo último valor de gyroZ
    if (lastGyroZ != gyroZ) {
      lastGyroZ = gyroZ;
      lcd.setCursor(15, 3);
      lcd.print("     ");
    }*/
    lcd.clear();
    lcd.setCursor(0, 1);
    lcd.print("X=");
    lcd.setCursor(2, 1);
    // Envia valor X do acelerômetro para o LCD
    lcd.print(mapd(accelX, -2.0, 2.0));
    
    lcd.setCursor(7, 1);
    lcd.print("Y=");
    lcd.setCursor(9, 1);
    // Envia valor Y do acelerômetro para o LCD
    lcd.print(mapd(accelY, -2.0, 2.0));

    lcd.setCursor(13, 1);
    lcd.print("Z=");
    lcd.setCursor(15, 1);
    // Envia valor Z do acelerômetro para o LCD
    lcd.print(mapd(accelZ, -2.0, 2.0));

    lcd.setCursor(13, 0);
    lcd.print("T:");
    lcd.setCursor(15, 0);
    // Envia valor da temperatura para o LCD
    // Calcula a temperatura em graus Celsius
    lcd.print(temp / 340.00 + 36.53);

    lcd.setCursor(0, 3);
    lcd.print("X=");
    lcd.setCursor(2, 3);
    // Envia valor X do giroscópio para o LCD
    lcd.print(mapi(gyroX, -250, 250)); // 2 bytes

    lcd.setCursor(7, 3);
    lcd.print("Y=");
    lcd.setCursor(9, 3);
    // Envia valor Y do giroscópio para o LCD
    lcd.print(mapi(gyroY, -250, 250));

    lcd.setCursor(13, 3);
    lcd.print("Z=");
    lcd.setCursor(15, 3);
    // Envia valor Z do giroscópio para o LCD
    lcd.print(mapi(gyroZ, -250, 250));
    delay(1000);
x=mapd(accelX, -2.0,2.0);
y=mapd(accelY,-2.0,2.0);
z=mapd(accelZ,-2.0,2.0);
x=map(x,-2.0,2.0,0,180);
y=map(y,-2.0,2.0,0,180);
z=map(z,-2.0,2.0,0,180);
lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("X=");
    lcd.setCursor(3, 0);
    // Envia valor X do acelerômetro para o LCD
    
    lcd.print(x);
    
    lcd.setCursor(8, 0);
    lcd.print("Y=");
    lcd.setCursor(10, 0);
    // Envia valor Y do acelerômetro para o LCD
    lcd.print(y);

    lcd.setCursor(15, 0);
    lcd.print("Z=");
    lcd.setCursor(17, 0);
    // Envia valor Z do acelerômetro para o LCD
    lcd.print(z);
  
  delay(1000);
 
  float heading = atan2(y, x);
  float declinationAngle = 0.22;
  heading += declinationAngle;
  if (heading < 0) {
    heading += 2 * PI;
  }
  if (heading > 2 * PI) {
    heading -= 2 * PI;
  }
  float headingDegrees = heading * 180 / M_PI;
  lcd.clear();
  lcd.setCursor(0, 0);
    lcd.print("Heading : ");
    lcd.setCursor(0, 1);
    // Envia valor Z do acelerômetro para o LCD
    lcd.print(headingDegrees);
    Myservo.write(headingDegrees);
   delay(1000);


  }
}

// map double (2 bytes)
double mapd(int x, float out_min, float out_max) {
  return (x - -32768) * (out_max - out_min) / (32767 - -32768) + out_min;
}

// map int (2 bytes)
int mapi(int x, int out_min, int out_max) {
  return (x - -32768) * (out_max + 1 - out_min) / (32767 - -32768) + out_min;


}