// Example sketch for the MPU-6050.
//
// Version 1, 9 August 2021, by Koepel with help from Wokwi community.
//
// Start the simulation and click on the MPU-6050 module to change the values.
//
// Note:
//   This example does not use a library with quaternations for the 3D orientation.
//   The values are directly read from the MPU-6050 and are used to rotate a cube.
//   It is just a fun example. The rotation of the cube is not according to the
//   physical values of the accelerometer and the gyro.
//
// Based on:
//   (1)
//     3D_Cube for Arduino OLED module by Colin Ord, 9/1/2015
//     A port of my (Colin Ord) original JustBasic Cube_3D demo to the Arduino Uno using U8G library.
//     http://colinord.blogspot.com/2015/01/arduino-oled-module-with-3d-demo.html
//     (no known copyrights)
//
//   (2)
//     MPU-6050 Short Example Sketch
//     By Arduino User JohnChi
//     August 17, 2014
//     Public Domain
//     https://playground.arduino.cc/Main/MPU-6050/#short
// 
//   (3)
//     The Adafruit GFX library with the SSD1306 driver.
//

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

Adafruit_SSD1306 display( 128, 64);   // 128 pixels width, 64 pixels height

#define LED_PIN   6
#define LED_COUNT 1
Adafruit_NeoPixel neoPixel( LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

const int mpuAddress_back = 0x68;          // I2C address of the MPU-6050
const int mpuAddress_front = 0x69;

float xByGyro_back, yByGyro_back, zByGyro_back;      // Global variables for the rotation by gyro
float xByGyro_front, yByGyro_front, zByGyro_front;      // Global variables for the rotation by gyro


// Set the origin in the middle of the display
const int xOrigin_front = 30;
const int yOrigin_front = 30;

const int xOrigin_back = 90;
const int yOrigin_back = 30;

const float viewDistance = 150.0;     // higher for less perspective, lower for more.

// Vertices for a cube
// A cube has 8 corners and each coordinate has x,y,z values.
#define NUM_VERTICES 8
const int cube_vertex_front[NUM_VERTICES][3] = 
{
  { -20, -20,  20 },     // x, y, z
  {  20, -20,  20 },
  {  20,  20,  20 },
  { -20,  20,  20 },
  { -20, -20, -20 },
  {  20, -20, -20 },
  {  20,  20, -20 },
  { -20,  20, -20 }
};

const int cube_vertex_back[NUM_VERTICES][3] = 
{
  { -20, -20,  20 },     // x, y, z
  {  20, -20,  20 },
  {  20,  20,  20 },
  { -20,  20,  20 },
  { -20, -20, -20 },
  {  20, -20, -20 },
  {  20,  20, -20 },
  { -20,  20, -20 }
};

// The wirefram is to display the lines on the OLED display
// It contains the corners of the shape in 2D coordinates
int wireframe[NUM_VERTICES][2];


void setup()
{
  Serial.begin( 115200);

  Wire.begin();

  // Initialize the OLED display and test if it is connected.
  if( !display.begin( SSD1306_SWITCHCAPVCC, 0x3C)) 
  {
    Serial.println(F( "SSD1306 allocation failed"));
    for(;;);                                   // halt the sketch if error encountered
  }

  // Initialize the MPU-6050 and test if it is connected.
  Wire.beginTransmission( mpuAddress_back);
  Wire.write( 0x6B);                           // PWR_MGMT_1 register
  Wire.write( 0);                              // set to zero (wakes up the MPU-6050)
  auto error = Wire.endTransmission();
  if( error != 0)
  {
    Serial.println(F( "Error, MPU-6050 not found"));
    for(;;);                                   // halt the sketch if error encountered
  }


  // Initialize the NeoPixel
  neoPixel.begin();
}


void loop()
{
  Wire.beginTransmission( mpuAddress_back);
  Wire.write( 0x3B);                   // Starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission( false);        // No stop condition for a repeated start

  Wire.beginTransmission( mpuAddress_front);
  Wire.write( 0x3B);                   // Starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission( false); 

  // The MPU-6050 has the values as signed 16-bit integers.
  // There are 7 values in 14 registers.
  int16_t AcX_front, AcY_front, AcZ_front, Tmp_front, GyX_front, GyY_front, GyZ_front;
  int16_t AcX_back, AcY_back, AcZ_back, Tmp_back, GyX_back, GyY_back, GyZ_back;

  Wire.requestFrom( mpuAddress_front, 14);   // request a total of 14 bytes
  AcX_front = Wire.read()<<8 | Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY_front = Wire.read()<<8 | Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ_front = Wire.read()<<8 | Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp_front = Wire.read()<<8 | Wire.read();  // 0x41 (TEMP_OUT_H)   & 0x42 (TEMP_OUT_L)
  GyX_front = Wire.read()<<8 | Wire.read();  // 0x43 (GYRO_XOUT_H)  & 0x44 (GYRO_XOUT_L)
  GyY_front = Wire.read()<<8 | Wire.read();  // 0x45 (GYRO_YOUT_H)  & 0x46 (GYRO_YOUT_L)
  GyZ_front = Wire.read()<<8 | Wire.read();  // 0x47 (GYRO_ZOUT_H)  & 0x48 (GYRO_ZOUT_L)

  Wire.requestFrom( mpuAddress_back, 14);   // request a total of 14 bytes
  AcX_back = Wire.read()<<8 | Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY_back = Wire.read()<<8 | Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ_back = Wire.read()<<8 | Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp_back = Wire.read()<<8 | Wire.read();  // 0x41 (TEMP_OUT_H)   & 0x42 (TEMP_OUT_L)
  GyX_back = Wire.read()<<8 | Wire.read();  // 0x43 (GYRO_XOUT_H)  & 0x44 (GYRO_XOUT_L)
  GyY_back = Wire.read()<<8 | Wire.read();  // 0x45 (GYRO_YOUT_H)  & 0x46 (GYRO_YOUT_L)
  GyZ_back = Wire.read()<<8 | Wire.read();  // 0x47 (GYRO_ZOUT_H)  & 0x48 (GYRO_ZOUT_L)

  // The acceleration is directly mapped into the angles.
  // That is rather artificial.
  // The combined gravity could be used for an angle, while ignoring the strength.
  //
  // The gyro sets the rotation speed.
  // The angle created by the rotation speed is added to angle by the accelerometer.
  //
  // The conversion from the sensor values to the rotation is just a value
  // that makes it look good on the display.

  float xByAccel_back = (float) AcX_back * 0.0001;      // static angle by accelerometer
  float yByAccel_back = (float) AcY_back * 0.0001;
  float zByAccel_back = (float) AcZ_back * 0.0001;

  xByGyro_back += (float) GyX_back * 0.00001;           // moving angle by gyro
  yByGyro_back += (float) GyY_back * 0.00001;
  zByGyro_back += (float) GyZ_back * 0.00001;

  float x_back = xByAccel_back + xByGyro_back;               // combine both angles
  float y_back = yByAccel_back + yByGyro_back;
  float z_back = zByAccel_back + zByGyro_back;

  float xByAccel_front = (float) AcX_front * 0.0001;      // static angle by accelerometer
  float yByAccel_front = (float) AcY_front * 0.0001;
  float zByAccel_front = (float) AcZ_front * 0.0001;

  xByGyro_front += (float) GyX_front * 0.00001;           // moving angle by gyro
  yByGyro_front += (float) GyY_front * 0.00001;
  zByGyro_front += (float) GyZ_front * 0.00001;

  float x_front = xByAccel_front + xByGyro_front;               // combine both angles
  float y_front = yByAccel_front + yByGyro_front;
  float z_front = zByAccel_front + zByGyro_front;


  // Keep the radians in range (although the cos/sin functions accept every value)
  if( x_back < 0.0)
    x_back += 2.0 * M_PI;
  else if( x_back > 2.0 * M_PI)
    x_back -= 2.0 * M_PI;
  if( y_back < 0.0)
    y_back += 2.0 * M_PI;
  else if( y_back > 2.0 * M_PI)
    y_back -= 2.0 * M_PI;
  if( z_back < 0.0)
    z_back += 2.0 * M_PI;
  else if( z_back > 2.0 * M_PI)
    z_back -= 2.0 * M_PI;

  // Keep the radians in range (although the cos/sin functions accept every value)
  if( x_front < 0.0)
    x_front += 2.0 * M_PI;
  else if( x_front > 2.0 * M_PI)
    x_front -= 2.0 * M_PI;
  if( y_front < 0.0)
    y_front += 2.0 * M_PI;
  else if( y_front > 2.0 * M_PI)
    y_front -= 2.0 * M_PI;
  if( z_front < 0.0)
    z_front += 2.0 * M_PI;
  else if( z_front > 2.0 * M_PI)
    z_front -= 2.0 * M_PI;





  // Draw 3D picture
  for (int i = 0; i < NUM_VERTICES; i++) 
  {
    // Rotate Y
    float rotx = cube_vertex_front[i][2] * sin(y_front) + cube_vertex_front[i][0] * cos(y_front);
    float roty = cube_vertex_front[i][1];
    float rotz = cube_vertex_front[i][2] * cos(y_front) - cube_vertex_front[i][0] * sin(y_front);

    // Rotate X
    float rotxx = rotx;
    float rotyy = roty * cos(x_front) - rotz * sin(x_front);
    float rotzz = roty * sin(x_front) + rotz * cos(x_front);

    // Rotate Z
    float rotxxx = rotxx * cos(z_front) - rotyy * sin(z_front);
    float rotyyy = rotxx * sin(z_front) + rotyy * cos(z_front);
    float rotzzz = rotzz;

    // Add depth perspective
    rotxxx *= viewDistance / (viewDistance + rotzzz);
    rotyyy *= viewDistance / (viewDistance + rotzzz);

    // Bring to middle of screen
    rotxxx += (float) xOrigin_front;
    rotyyy += (float) yOrigin_front;

    // Store new vertices values for wireframe drawing
    wireframe[i][0] = (int) rotxxx;
    wireframe[i][1] = (int) rotyyy;
    wireframe[i][2] = (int) rotzzz;
  }
  draw_wireframe();

  for (int i = 0; i < NUM_VERTICES; i++) 
  {
    // Rotate Y
    float rotx = cube_vertex_back[i][2] * sin(y_back) + cube_vertex_back[i][0] * cos(y_back);
    float roty = cube_vertex_back[i][1];
    float rotz = cube_vertex_back[i][2] * cos(y_back) - cube_vertex_back[i][0] * sin(y_back);

    // Rotate X
    float rotxx = rotx;
    float rotyy = roty * cos(x_back) - rotz * sin(x_back);
    float rotzz = roty * sin(x_back) + rotz * cos(x_back);

    // Rotate Z
    float rotxxx = rotxx * cos(z_back) - rotyy * sin(z_back);
    float rotyyy = rotxx * sin(z_back) + rotyy * cos(z_back);
    float rotzzz = rotzz;

    // Add depth perspective
    rotxxx *= viewDistance / (viewDistance + rotzzz);
    rotyyy *= viewDistance / (viewDistance + rotzzz);

    // Bring to middle of screen
    rotxxx += (float) xOrigin_back;
    rotyyy += (float) yOrigin_back;

    // Store new vertices values for wireframe drawing
    wireframe[i][0] = (int) rotxxx;
    wireframe[i][1] = (int) rotyyy;
    wireframe[i][2] = (int) rotzzz;
  }
  draw_wireframe();


  // Set the color of the NeoPixel according to the temperature
  // Temperature by the MPU-6050 is -40 to 85.
  // According to the datasheet:
  //   Temperature in Celsius = (raw_value / 340) + 36.53
  // The Hue range for the NeoPixel is the full uint16_t range.
  float Celsius_front = ((float) Tmp_front / 340.00) + 36.53;
  float hue_front = (Celsius_front + 40.0) / 125.0 * 65535.0;
  uint32_t rgbcolor_front = neoPixel.ColorHSV( (uint16_t) hue_front);
  neoPixel.setPixelColor( 0, rgbcolor_front);

  float Celsius_back = ((float) Tmp_back / 340.00) + 36.53;
  float hue_back = (Celsius_back + 40.0) / 125.0 * 65535.0;
  uint32_t rgbcolor_back = neoPixel.ColorHSV( (uint16_t) hue_back);
  neoPixel.setPixelColor( 0, rgbcolor_back);



  neoPixel.show();                        // update new values to NeoPixel
}


void draw_wireframe(void)
{
  // Start with a empty buffer
  display.clearDisplay();

  // A cube has 8 points and 12 sides.
  // The wireframe contains the 8 points, and the 12 lines are drawn here.
  display.drawLine( wireframe[0][0], wireframe[0][1], wireframe[1][0], wireframe[1][1], SSD1306_WHITE);
  display.drawLine( wireframe[1][0], wireframe[1][1], wireframe[2][0], wireframe[2][1], SSD1306_WHITE);
  display.drawLine( wireframe[2][0], wireframe[2][1], wireframe[3][0], wireframe[3][1], SSD1306_WHITE);
  display.drawLine( wireframe[3][0], wireframe[3][1], wireframe[0][0], wireframe[0][1], SSD1306_WHITE);

  display.drawLine( wireframe[4][0], wireframe[4][1], wireframe[5][0], wireframe[5][1], SSD1306_WHITE);
  display.drawLine( wireframe[5][0], wireframe[5][1], wireframe[6][0], wireframe[6][1], SSD1306_WHITE);
  display.drawLine( wireframe[6][0], wireframe[6][1], wireframe[7][0], wireframe[7][1], SSD1306_WHITE);
  display.drawLine( wireframe[7][0], wireframe[7][1], wireframe[4][0], wireframe[4][1], SSD1306_WHITE);

  display.drawLine( wireframe[0][0], wireframe[0][1], wireframe[4][0], wireframe[4][1], SSD1306_WHITE);
  display.drawLine( wireframe[1][0], wireframe[1][1], wireframe[5][0], wireframe[5][1], SSD1306_WHITE);
  display.drawLine( wireframe[2][0], wireframe[2][1], wireframe[6][0], wireframe[6][1], SSD1306_WHITE);
  display.drawLine( wireframe[3][0], wireframe[3][1], wireframe[7][0], wireframe[7][1], SSD1306_WHITE);

  // Extra cross face on one side
  display.drawLine( wireframe[1][0], wireframe[1][1], wireframe[3][0], wireframe[3][1], SSD1306_WHITE);
  display.drawLine( wireframe[0][0], wireframe[0][1], wireframe[2][0], wireframe[2][1], SSD1306_WHITE);

  // Write the new picture to the display
  display.display();
}