#include <Adafruit_MPU6050.h>
#include <Adafruit_SSD1306.h>
#include <Wire.h>


Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);
Adafruit_MPU6050 accelrator;
sensors_event_t a, g, temp, prev_a, prev_g;

const unsigned short int displayCenter[2] = { 64, 32 };
const unsigned short int coordLimit = 32;
const unsigned short int vertexAxis = 3;
const unsigned short int figureVertexes = 4;
unsigned short int points[figureVertexes][vertexAxis] = {
  { 1, 1, 10 },  //max coord - 32
  { 10, 4, 3 },
  { 3, 2, 0 },
  { 34, 0, 1 }
};

void setup(void) {
  Serial.begin(9600);
  AcceleratorInit();
  DisplayInit();

  delay(100);
}

void loop() {
  AcceleratorEvent();
  delay(10);
}

void AcceleratorEvent() {
  accelrator.getEvent(&a, &g, &temp);

  if (!SE_Equal(a, prev_a, true) || !SE_Equal(g, prev_g, false)) {
    PrintMPUinfo();
    //DrawFigure();
  }

  SE_Set(prev_a, a, true);
  SE_Set(prev_g, g, false);

}

void DrawFigure() {
  float zNormalize = 0;

  for (int i = 0; i < figureVertexes - 1; i++) {  //-1 -> last vert has no pair

    for (int j = 0; j < vertexAxis; j++)
      if (points[i][j] > coordLimit)  //visual limit
        points[i][j] = coordLimit;


    zNormalize = (float)points[i][2] / coordLimit;
    //x = x1*(32-z)+xCentr*z
    //y = y1*(32-z)+yCentr*z
    display.drawLine(points[i][0] * (coordLimit - zNormalize) + displayCenter[0] * zNormalize,
                     points[i][1] * (coordLimit - zNormalize) + displayCenter[1] * zNormalize,
                     points[i + 1][0] * (coordLimit - zNormalize) + displayCenter[0] * zNormalize,
                     points[i + 1][1] * (coordLimit - zNormalize) + displayCenter[1] * zNormalize,
                     WHITE);
  }
}

void PrintMPUinfo() {

  /*display.setTextColor(WHITE, BLACK);
    display.setCursor(0, 56);
    display.print(a.acceleration.x);
    display.display();*/

  Serial.print(a.acceleration.x);
  Serial.print(",");
  Serial.print(a.acceleration.y);
  Serial.print(",");
  Serial.print(a.acceleration.z);
  Serial.println("");
  Serial.print(g.gyro.x);
  Serial.print(",");
  Serial.print(g.gyro.y);
  Serial.print(",");
  Serial.print(g.gyro.z);
  Serial.println("");
  delay(10);
}

void AcceleratorInit() {
  if (!accelrator.begin(0x68)) {
    Serial.println("Failed to find MPU6050 chip\n");

    while (1)  //infinity waiting
      delay(10);
  }

  accelrator.setAccelerometerRange(MPU6050_RANGE_16_G);
  accelrator.setGyroRange(MPU6050_RANGE_250_DEG);
  accelrator.setFilterBandwidth(MPU6050_BAND_21_HZ);
}

void DisplayInit() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.display();  //disp logo
  delay(2000);
  display.clearDisplay();

  display.drawRect(0, 0, 128, 64, WHITE);  //draw frame
  display.display();
}

bool SE_Equal(sensors_event_t a, sensors_event_t b, bool isAcceleration) {
  if (isAcceleration)
    return a.acceleration.x == b.acceleration.x
           && a.acceleration.y == b.acceleration.y
           && a.acceleration.z == b.acceleration.z;

  else
    return a.gyro.x == b.gyro.x
           && a.gyro.y == b.gyro.y
           && a.gyro.z == b.gyro.z;
}

void SE_Set(sensors_event_t a, sensors_event_t b, bool isAcceleration) {
  if (isAcceleration) {
    a.acceleration.x = b.acceleration.x;
    a.acceleration.y = b.acceleration.y;
    a.acceleration.z = b.acceleration.z;
  }
  else {
    a.gyro.x = b.gyro.x;
    a.gyro.y = b.gyro.y;
    a.gyro.z = b.gyro.z;
  }

}