#include <LiquidCrystal.h>
#include <MPU6050.h>
#include <math.h>

LiquidCrystal lcd(12, 11, 10, 9, 8, 7);
MPU6050 mpu;

float g_fSpeedX(0), g_fSpeedY(0), g_fSpeedZ(0);
float g_fdX(0), g_fdY(0), g_fdZ(0);
float g_fDegreeSpeedX(0), g_fDegreeSpeedY(0), g_fDegreeSpeedZ(0);
float g_fDegreeX(0), g_fDegreeY(0), g_fDegreeZ(0);
float g_fLastMillis(0);

const float MATH_G = 9.81f;

float floatMap(float fValue, float fMinInValue, float fMaxInValue, float fMinOutValue, float fMaxOutValue)
{
  return (fValue - fMinInValue) * (fMaxOutValue - fMinOutValue) / (fMaxInValue - fMinInValue) + fMinOutValue;
}
void setup()
{
  lcd.begin(16, 2);
  Serial.begin(9600);

  while (!mpu.testConnection())
  {
    lcd.clear();
    lcd.print("Device not Found");
    delay(1000);
  }
}

void normalizeAngles(float& fX, float& fY, float& fZ)
{
  if (fX >= 360.f) {
    fX -= 360.f;
  } else if (fX < 0) {
    fX += 360.f;
  }

  if (fY >= 360.f) {
    fY -= 360.f;
  } else if (fY < 0) {
    fY += 360.f;
  }

  if (fZ >= 360.f) {
    fZ -= 360.f;
  } else if (fZ < 0) {
    fZ += 360.f;
  }
}

void loop()
{
  lcd.setCursor(0, 0);
  lcd.clear();
  lcd.print(" X     Y     Z");

  int16_t iaX, iaY, iaZ;
  int16_t iDegreeX, iDegreeY, iDegreeZ;
  mpu.getMotion6(&iaX, &iaY, &iaZ, &iDegreeX, &iDegreeY, &iDegreeZ);

  float fMillisStart = millis();

  float faX = 0;
  if (iaX) {
    faX = floatMap((float) iaX, -32768, 32767, -2.f, 2.f) * MATH_G;
  }

  float faY = 0;
  if (iaY) {
    faY = floatMap((float) iaY, -32768, 32767, -2.f, 2.f) * MATH_G;
  }

  float faZ = 0;
  if (iaZ) {
    faZ = floatMap((float) iaZ, -32768, 32767, -2.f, 2.f) * MATH_G;
  }

  float fDegreeX = 0;
  if (iDegreeX) {
    fDegreeX = floatMap((float) iDegreeX, -32750, 32750, -250.f, 250.f);
  }

  float fDegreeY = 0;
  if (iDegreeY) {
    fDegreeY = floatMap((float) iDegreeY, -32750, 32750, -250.f, 250.f);
  }

  float fDegreeZ = 0;
  if (iDegreeZ) {
    fDegreeZ = floatMap((float) iDegreeZ, -32750, 32750, -250.f, 250.f);
  }

  float fTimeCoefficient = (float) (fMillisStart - g_fLastMillis) / 1000.f;

  g_fDegreeSpeedX += fDegreeX * fTimeCoefficient;
  g_fDegreeSpeedY += fDegreeY * fTimeCoefficient;
  g_fDegreeSpeedZ += fDegreeZ * fTimeCoefficient;
  normalizeAngles(g_fDegreeSpeedX, g_fDegreeSpeedY, g_fDegreeSpeedZ);

  g_fDegreeX += g_fDegreeSpeedX * fTimeCoefficient + (fDegreeX * fTimeCoefficient * fTimeCoefficient) / 2.f;
  g_fDegreeY += g_fDegreeSpeedY * fTimeCoefficient + (fDegreeY * fTimeCoefficient * fTimeCoefficient) / 2.f;
  g_fDegreeZ += g_fDegreeSpeedZ * fTimeCoefficient + (fDegreeZ * fTimeCoefficient * fTimeCoefficient) / 2.f;
  normalizeAngles(g_fDegreeX, g_fDegreeY, g_fDegreeZ);

  g_fSpeedX += faX * fTimeCoefficient;
  g_fSpeedY += faY * fTimeCoefficient;
  g_fSpeedZ += faZ * fTimeCoefficient;

  float fSimpleSpeedX = g_fSpeedX * fTimeCoefficient + (faX * fTimeCoefficient * fTimeCoefficient) / 2.f;
  float fSimpleSpeedY = g_fSpeedY * fTimeCoefficient + (faY * fTimeCoefficient * fTimeCoefficient) / 2.f;
  float fSimpleSpeedZ = g_fSpeedZ * fTimeCoefficient + (faZ * fTimeCoefficient * fTimeCoefficient) / 2.f;

  float fRadiansX = g_fDegreeX / M_PI;
  float fRadiansY = g_fDegreeY / M_PI;
  float fRadiansZ = g_fDegreeZ / M_PI;

  g_fdX += fSimpleSpeedZ * sin(fRadiansY)
    + fSimpleSpeedY * sin(fRadiansZ)
    + fSimpleSpeedX * (1 - cos(fRadiansY) - cos(fRadiansZ));
  g_fdY += fSimpleSpeedX * sin(fRadiansZ)
    + fSimpleSpeedZ * sin(fRadiansX)
    + fSimpleSpeedY * (1 - cos(fRadiansZ) - cos(fRadiansX));
  g_fdZ +=
    fSimpleSpeedY * sin(fRadiansX)
    + fSimpleSpeedX * sin(fRadiansY)
    + fSimpleSpeedZ * (1 - cos(fRadiansX) - cos(fRadiansY));

  Serial.print(" sX: ");
  Serial.print(fSimpleSpeedX);
  Serial.print(" sY: ");
  Serial.print(fSimpleSpeedY);
  Serial.print(" sZ: ");
  Serial.print(fSimpleSpeedZ);
  Serial.print(" dsX: ");
  Serial.print(g_fDegreeSpeedX);
  Serial.print(" dsY: ");
  Serial.print(g_fDegreeSpeedY);
  Serial.print(" dsZ: ");
  Serial.print(g_fDegreeSpeedZ);
  Serial.print(" dX: ");
  Serial.print(g_fDegreeX);
  Serial.print(" dY: ");
  Serial.print(g_fDegreeY);
  Serial.print(" dZ: ");
  Serial.println(g_fDegreeZ);

  lcd.setCursor(0, 1);
  lcd.print(g_fdX, 2);

  lcd.setCursor(6, 1);
  lcd.print(g_fdY, 2);

  lcd.setCursor(12, 1);
  lcd.print(g_fdZ, 2);

  g_fLastMillis = millis();
  delay(100);
}