#include <Arduino.h> // If using platformio


#include <MPU9250_asukiaaa.h>

#ifdef _ESP32_HAL_I2C_H_
    #define SDA_PIN 21
    #define SCL_PIN 22
#endif 

MPU9250_asukiaaa giroscopio;

float aX = 0, aY, aZ, aSqrt, gX, gY, gZ, mDirection, mX, mY, mZ;
float grausX = 0;

//' prototipos de funções // 
float Quadrante(float rad), DegToRad(float deg), RadToDeg(float rad);


void setup()
{
   Serial.begin(9600);
   while (!Serial);

   #ifdef _ESP32_HAL_I2C_H_ // For ESP32 
   Wire.begin(SDA_PIN, SCL_PIN);
   giroscopio.setWire(&Wire);
   #else
   Wire.begin();
   giroscopio.setWire(&Wire);
   #endif

   giroscopio.beginAccel();
}

void loop()
{
    if (1)//giroscopio.accelUpdate() == 0)
    {
        grausX += 1;
        Serial.println("GrausX = " + String(grausX));
        float grausY = grausX +90;

        aX = cos(DegToRad( grausX ));//giroscopio.accelX(); //? Wokwi
        aY = cos(DegToRad( grausY ));//giroscopio.accelY(); //? Wokwi

        // aZ = giroscopio.accelZ();
        // aSqrt = giroscopio.accelSqrt();

        Serial.println("Deg X: " + String(RadToDeg( DegToRad( grausX ) )) + " || Rad X: " + String(DegToRad( grausX )) + " || Cos X: " + String( aX ));
        Serial.println("Deg Y: " + String(RadToDeg( DegToRad( grausY ) )) + " || Rad X: " + String(DegToRad( grausY )) + " || Cos X: " + String( aY ));

        Serial.println("\naccelX: " + String(aX) + "||" + "accelY: " + String(aY) + "||" + "accelZ: " + String(aZ));
        // Serial.print("\taccelSqrt: " + String(aSqrt));

        float angX = Quadrante(acos(aX));

        Serial.println("AnguloX: " + String(angX) + "||" + "AnguloY: " + String(angX + 90) + "\n");
    }

   delay(200);
} 



float Quadrante(float rad)
{
   if (aX >= 0 and aY >= 0) //' ambos são positivos
   {
    Serial.println("++");

    return 270 + (90 - RadToDeg(rad));
   }
   else if (aX >= 0 or aY >= 0) //' só um deles é positivo
   {
      if (aX >= 0) //' X é positivo
        {
          Serial.println("+-");

          return RadToDeg(rad);
        }
      else //' Y é positivo
        {
          Serial.println("-+");

          return  270 + (90 - RadToDeg(rad)); //! 180 + (RadToDeg(rad));
        }
    }
    else // ambos são negativos
    {
      Serial.println("--");

      return 90 + (90 - RadToDeg(rad));
    }

   return rad; // retorna o valor lido
}


float DegToRad(float deg)
{
  return deg * PI/180;
}
float RadToDeg(float rad)
{
  return rad * 180/PI;
}


/*
#include <Arduino.h> // If using platformio


#include <MPU9250_asukiaaa.h>

#ifdef _ESP32_HAL_I2C_H_
    #define SDA_PIN 21
    #define SCL_PIN 22
#endif

MPU9250_asukiaaa giroscopio;
float aX, aY, aZ, aSqrt, gX, gY, gZ, mDirection, mX, mY, mZ;

//' prototipos de funções //
float Quadrante(int var); float RadToDeg(float var);


void setup()
{
  Serial.begin(9600);
  while (!Serial);

  #ifdef _ESP32_HAL_I2C_H_ // For ESP32
    Wire.begin(SDA_PIN, SCL_PIN);
    giroscopio.setWire(&Wire);
  #else
    Wire.begin();
    giroscopio.setWire(&Wire);
  #endif

  giroscopio.beginAccel();
}

void loop()
{
    if (1)//(giroscopio.accelUpdate() == 0) //? Wokwi
    {
        float grausX = 180;
        float grausY = grausX + 90;

        aX = cos(DegToRad( grausX ));//giroscopio.accelX(); //? Wokwi
        aY = cos(DegToRad( grausY ));//giroscopio.accelY(); //? Wokwi

        Serial.println("deg X: " + String(RadToDeg( DegToRad( grausX ) )) + " || Rad X: " + String(DegToRad( grausX )) + " || cos X: " + String( aX ));
        Serial.println("deg Y: " + String(RadToDeg( DegToRad( grausY ) )) + " || Rad X: " + String(DegToRad( grausY )) + " || cos X: " + String( aY ));

        //aZ = giroscopio.accelZ();
        // aSqrt = giroscopio.accelSqrt();
        Serial.println("\naccelX: " + String(aX) + "||" + "accelY: " + String(aY) + "||" + "accelZ: " + String(aZ));
        // Serial.print("\taccelSqrt: " + String(aSqrt));

        float angX = Quadrante(acos(aX));
        
        Serial.println("AnguloX: " + String(angX) + "||" + "AnguloY: " + String(angX + 90));
    }

    delay(80000);
}

float Quadrante(float rad)
{
    if (aX >= 0 and aY >= 0) //' ambos são positivos
    {
      Serial.println("++");

      return 270 + (90 - RadToDeg(rad));
    }
    else if (aX >= 0 or aY >= 0) //' só um deles é positivo
    {
        if (aX >= 0) //' X é positivo
        {
          Serial.println("+-");

          return RadToDeg(rad);
        }
        else //' Y é positivo
        {
          Serial.println("-+");

          return  270 + (90 - RadToDeg(rad)); //! 180 + (RadToDeg(rad));
        }
    }
    else // ambos são negativos
    {
      Serial.println("--");

      return 90 + (90 - RadToDeg(rad));
    }

    return rad; // retorna o valor lido
}

float DegToRad(float deg)
{
  return deg * PI/180;
}
float RadToDeg(float rad)
{
  return rad * 180/PI;
}
*/