#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 = 0x68;          // I2C address of the MPU-6050

float xByGyro, yByGyro, zByGyro;      // Global variables for the rotation by gyro

// Set the origin in the middle of the display
const int xOrigin = 64;
const int yOrigin = 32;

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[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];
const unsigned char epd_bitmap_123upp [] PROGMEM = {
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x07, 0xf0, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x01, 0x80, 0x03, 0xc0, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0xf8, 0x1d, 0x2c, 0x0f, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xfe, 0x03, 0x03, 0xc6, 0xe3, 0x00, 0x70, 0x3f, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xe0, 0xe0, 0xf0, 0x6c, 0x30, 0x07, 0x83, 0x87, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xfe, 0x0c, 0x0e, 0x01, 0xd9, 0x3d, 0x02, 0x98, 0x38, 0x7f, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xf0, 0xc1, 0x60, 0x00, 0x01, 0x82, 0x06, 0x23, 0xc3, 0x0f, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0x8e, 0x15, 0x99, 0x80, 0x77, 0xf8, 0x36, 0x06, 0x78, 0x21, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xfc, 0x21, 0xdc, 0x81, 0x1f, 0x1f, 0xf1, 0x62, 0x08, 0x8e, 0x0c, 0x3f, 0xff, 0xff, 
  0xff, 0xff, 0xe1, 0x0e, 0x43, 0xe6, 0x00, 0xcf, 0xee, 0x00, 0x01, 0x77, 0xf1, 0x87, 0xff, 0xff, 
  0xff, 0xff, 0x0c, 0x0d, 0x30, 0x14, 0x01, 0xcc, 0x25, 0x00, 0x04, 0x00, 0x3c, 0x30, 0xff, 0xff, 
  0xff, 0xfc, 0x63, 0x6c, 0x21, 0x11, 0x00, 0x01, 0x04, 0x00, 0x0c, 0xfe, 0x0b, 0xc6, 0x3f, 0xff, 
  0xff, 0xe1, 0x0c, 0x09, 0x8f, 0x34, 0x40, 0x86, 0xe2, 0x01, 0x98, 0xf8, 0x0e, 0x71, 0x87, 0xff, 
  0xff, 0x8c, 0x23, 0x41, 0x98, 0x40, 0x01, 0x7f, 0xfd, 0x22, 0x53, 0xc0, 0x01, 0x8a, 0x31, 0xff, 
  0xff, 0x32, 0x08, 0x90, 0x00, 0x3c, 0x7e, 0x00, 0x00, 0x69, 0x00, 0x0f, 0xe1, 0x22, 0x4c, 0xff, 
  0xff, 0x2c, 0x86, 0x98, 0x3c, 0x46, 0x80, 0x3f, 0x3e, 0x07, 0xb0, 0x0e, 0x80, 0xa0, 0x36, 0xff, 
  0xff, 0x4c, 0x30, 0x18, 0x03, 0x3e, 0x00, 0xff, 0xdf, 0xf1, 0xfe, 0x38, 0x00, 0x18, 0x92, 0x7f, 
  0xfe, 0x58, 0x49, 0x81, 0xc1, 0x90, 0x00, 0xff, 0xe7, 0xfe, 0x1b, 0x00, 0x7f, 0x0a, 0x4a, 0x7f, 
  0xfe, 0xdc, 0x39, 0x80, 0x16, 0xc0, 0x00, 0xff, 0xf9, 0xff, 0x8d, 0x03, 0xfc, 0x0a, 0x92, 0x7f, 
  0xfc, 0x91, 0x80, 0x8f, 0x1c, 0x00, 0x1c, 0xff, 0xfe, 0xf9, 0xe2, 0x43, 0x80, 0x0e, 0x79, 0x7f, 
  0xfc, 0xb4, 0x4c, 0x00, 0x27, 0x00, 0x1f, 0xff, 0x7f, 0x7e, 0x79, 0x10, 0x04, 0x01, 0x09, 0x3f, 
  0xfd, 0xb0, 0x8e, 0x3c, 0x94, 0xff, 0x47, 0xfc, 0x1e, 0x3f, 0xbc, 0xf8, 0x3f, 0xe2, 0x05, 0x3f, 
  0xfd, 0x20, 0x86, 0x61, 0x05, 0xfe, 0x67, 0x80, 0x0c, 0x1f, 0x9e, 0x4c, 0x78, 0x00, 0x05, 0xbf, 
  0xf9, 0x24, 0xb0, 0x01, 0x99, 0xfe, 0x07, 0xc0, 0x08, 0x9f, 0xcf, 0x64, 0x00, 0x01, 0x04, 0xbf, 
  0xf9, 0x64, 0x9c, 0x72, 0x0b, 0xfe, 0x40, 0xe0, 0x1c, 0x18, 0xc0, 0x24, 0x13, 0xa1, 0x84, 0x9f, 
  0xf9, 0x41, 0x04, 0x83, 0x13, 0xfe, 0xf8, 0x7f, 0xfd, 0x9e, 0x00, 0x2a, 0x3e, 0xf0, 0x82, 0x9f, 
  0xfb, 0x49, 0x20, 0x32, 0xb3, 0xfe, 0xff, 0xe0, 0x0c, 0xcf, 0xe0, 0x2a, 0x00, 0x00, 0x82, 0xdf, 
  0xfb, 0x43, 0xb8, 0xe2, 0x32, 0xfe, 0xff, 0xff, 0xff, 0xdf, 0xe0, 0x2a, 0x00, 0x01, 0xaa, 0x5f, 
  0xfa, 0x60, 0x00, 0x09, 0x19, 0xbf, 0x7f, 0xff, 0xfe, 0x03, 0x6f, 0x33, 0x1f, 0xc3, 0x2a, 0x4f, 
  0xfa, 0x71, 0x20, 0x7b, 0x08, 0x3f, 0x7f, 0xff, 0xf0, 0x00, 0x0f, 0x12, 0x1e, 0xf8, 0x86, 0x4f, 
  0xfa, 0x41, 0x1c, 0x40, 0x1c, 0x3f, 0xbf, 0xff, 0xf0, 0x00, 0x00, 0x64, 0x00, 0x00, 0xc2, 0x4f, 
  0xfa, 0x40, 0x00, 0x0c, 0x8c, 0x0f, 0x9f, 0xff, 0xf0, 0x00, 0x04, 0xcc, 0x7f, 0x01, 0xca, 0x5f, 
  0xfb, 0x40, 0x38, 0x1c, 0x0f, 0x06, 0x6f, 0xff, 0xf8, 0x30, 0x01, 0xb8, 0x3f, 0x82, 0x0a, 0xdf, 
  0xf9, 0x60, 0x8e, 0x30, 0x70, 0x83, 0xf7, 0xff, 0xff, 0x7f, 0x03, 0x80, 0x01, 0xe7, 0x06, 0x9f, 
  0xf9, 0x60, 0x40, 0x07, 0x14, 0x01, 0xff, 0xff, 0xfe, 0xff, 0xcc, 0xc3, 0xd8, 0x06, 0x94, 0x9f, 
  0xf9, 0x34, 0x47, 0x8e, 0x0c, 0xa0, 0x7f, 0x7f, 0xfd, 0xfe, 0x12, 0x01, 0xf8, 0x06, 0x14, 0x9f, 
  0xf9, 0x24, 0xb0, 0x00, 0xe9, 0x2e, 0x0f, 0xff, 0xe8, 0x08, 0xec, 0x00, 0x1e, 0x08, 0x4d, 0x3f, 
  0xfd, 0xb4, 0x18, 0x00, 0xc0, 0x61, 0x83, 0xe0, 0x00, 0x07, 0x84, 0x38, 0x06, 0x28, 0x49, 0x3f, 
  0xfc, 0xaa, 0x18, 0xf8, 0x8c, 0xc1, 0x78, 0x00, 0x00, 0xff, 0x48, 0x1f, 0x80, 0x60, 0x39, 0x3f, 
  0xfc, 0x98, 0x0c, 0x00, 0x0c, 0xc0, 0x00, 0xff, 0xff, 0x00, 0x20, 0x03, 0x80, 0x40, 0xb3, 0x7f, 
  0xfe, 0xd5, 0x06, 0x0f, 0x01, 0x80, 0x00, 0x11, 0xc0, 0x00, 0x88, 0x01, 0xce, 0xc0, 0x5a, 0x7f, 
  0xfe, 0x5e, 0x00, 0x03, 0x81, 0xf8, 0x00, 0x00, 0x02, 0xc0, 0x10, 0x00, 0x0d, 0x00, 0x32, 0xff, 
  0xfe, 0x4d, 0x83, 0x60, 0x78, 0x07, 0x07, 0xfe, 0xfe, 0x00, 0xe0, 0x00, 0x05, 0x03, 0x64, 0xff, 
  0xff, 0x31, 0x83, 0x9f, 0x2f, 0x00, 0x7c, 0x00, 0x00, 0xfc, 0x01, 0x00, 0xe0, 0x06, 0x8c, 0xff, 
  0xff, 0xc6, 0x3c, 0x7f, 0xc6, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xef, 0xf8, 0x0e, 0x23, 0xff, 
  0xff, 0xf1, 0x8e, 0x03, 0xbc, 0x5e, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x27, 0x86, 0x31, 0x8f, 0xff, 
  0xff, 0xfe, 0x31, 0x80, 0xdf, 0xb0, 0x04, 0x00, 0x00, 0x80, 0x03, 0xf0, 0x03, 0xc6, 0x3f, 0xff, 
  0xff, 0xff, 0x84, 0x3b, 0xfa, 0xfc, 0x3a, 0x80, 0x05, 0xfc, 0x3f, 0xff, 0x8e, 0x31, 0xff, 0xff, 
  0xff, 0xff, 0xf1, 0x8e, 0x7f, 0x4f, 0xf4, 0x29, 0x10, 0x3f, 0xfc, 0xfc, 0xf0, 0x87, 0xff, 0xff, 
  0xff, 0xff, 0xfc, 0x31, 0x87, 0xf2, 0xff, 0xf2, 0xdf, 0xff, 0x4f, 0xf7, 0x8c, 0x3f, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0x86, 0x3a, 0x3f, 0xeb, 0xdf, 0xfd, 0x67, 0xfa, 0x68, 0xe1, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xf0, 0xc3, 0xe0, 0xff, 0xda, 0xed, 0xfe, 0x0f, 0x84, 0x0f, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xfe, 0x0c, 0x1f, 0x4b, 0xff, 0xfe, 0x81, 0xf0, 0x60, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xe0, 0xc0, 0x3c, 0x80, 0x00, 0xfe, 0x0c, 0x0f, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xfe, 0x07, 0x03, 0x7e, 0x87, 0x80, 0xe0, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x38, 0x1f, 0xf0, 0x78, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x01, 0xc0, 0x03, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x07, 0xc0, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};
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);
  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);
  Wire.write( 0x3B);                   // Starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission( false);        // No stop condition for a repeated start

  // The MPU-6050 has the values as signed 16-bit integers.
  // There are 7 values in 14 registers.
  int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

  Wire.requestFrom( mpuAddress, 14);   // request a total of 14 bytes
  AcX = Wire.read()<<8 | Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY = Wire.read()<<8 | Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read()<<8 | Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read()<<8 | Wire.read();  // 0x41 (TEMP_OUT_H)   & 0x42 (TEMP_OUT_L)
  GyX = Wire.read()<<8 | Wire.read();  // 0x43 (GYRO_XOUT_H)  & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read()<<8 | Wire.read();  // 0x45 (GYRO_YOUT_H)  & 0x46 (GYRO_YOUT_L)
  GyZ = 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 = (float) AcX * 0.0001;      // static angle by accelerometer
  float yByAccel = (float) AcY * 0.0001;
  float zByAccel = (float) AcZ * 0.0001;

  xByGyro += (float) GyX * 0.00001;           // moving angle by gyro
  yByGyro += (float) GyY * 0.00001;
  zByGyro += (float) GyZ * 0.00001;

  float x = xByAccel + xByGyro;               // combine both angles
  float y = yByAccel + yByGyro;
  float z = zByAccel + zByGyro;

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

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

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

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

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

    // Bring to middle of screen
    rotxxx += (float) xOrigin;
    rotyyy += (float) yOrigin;

    // 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 = ((float) Tmp / 340.00) + 36.53;
  float hue = (Celsius + 40.0) / 125.0 * 65535.0;
  uint32_t rgbcolor = neoPixel.ColorHSV( (uint16_t) hue);
  neoPixel.setPixelColor( 0, rgbcolor);
  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();
   // Initialize the NeoPixel
  neoPixel.begin();
  display.clearDisplay(); //clear sebelum tampilan baru
  display.drawBitmap(0, 0, epd_bitmap_123upp, 128, 64, WHITE); 
  display.display(); //tampilkan data
  delay(7000);
}