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

#define LED_PIN 23
#define NUMPIXELS 16
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C

Adafruit_MPU6050 mpu;
Adafruit_NeoPixel pixels(NUMPIXELS, LED_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup(void)
{
  mpu.begin();
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  pixels.begin();
  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);

  delay(100);
}

void loop()
{
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  displayBankAngle(&a);
  displayAccelerationVector(&a);
  delay(50);
}

void displayBankAngle(sensors_event_t * a)
{
  float acceleration = sqrt(a->acceleration.y * a->acceleration.y + a->acceleration.z * a->acceleration.z);
  float angle = acos(a->acceleration.z/acceleration);
  if(a->acceleration.y < 0)
  {
    angle = -angle;
  }
  angle *= 180/PI;
  int pin = (int)((angle+180)/360.0*16) + 4;

  pixels.clear();
  if(pin == 12 || pin == 4)
  {
    pixels.setPixelColor((pin + 8)%16, pixels.Color(0, 255, 0));
    pixels.setPixelColor((pin)%16, pixels.Color(0, 255, 0));
  }
  else if((pin >= 10 && pin <= 14) || (pin >= 2 && pin <= 6))
  {
    pixels.setPixelColor((pin + 8)%16, pixels.Color(255, 255, 0));
    pixels.setPixelColor((pin)%16, pixels.Color(255, 255, 0));
  }
  else
  {
    pixels.setPixelColor((pin + 8)%16, pixels.Color(255, 0, 0));
    pixels.setPixelColor((pin)%16, pixels.Color(255, 0, 0));
  }
  pixels.show();
}

void displayAccelerationVector(sensors_event_t * a)
{
  // Clear display
  display.clearDisplay();
  // Draw axis arrows
  display.fillCircle(5, display.height()-5, 3, SSD1306_WHITE);
  display.drawLine(5, display.height()-5, 5, display.height()-20, SSD1306_WHITE);
  display.drawLine(5, display.height()-20, 2, display.height()-17, SSD1306_WHITE);
  display.drawLine(5, display.height()-20, 8, display.height()-17, SSD1306_WHITE);
  display.drawLine(5, display.height()-5, 20, display.height()-5, SSD1306_WHITE);
  display.drawLine(20, display.height()-5, 17, display.height()-2, SSD1306_WHITE);
  display.drawLine(20, display.height()-5, 17, display.height()-8, SSD1306_WHITE);
  // Draw axis names
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(3, display.height()-30);
  display.write('y');
  display.setCursor(23, display.height()-9);
  display.write('x');
  // Draw scale
  display.drawCircle(display.width()/2, display.height()/2, 2, SSD1306_WHITE);
  display.drawCircle(display.width()/2, display.height()/2, 8, SSD1306_WHITE);
  display.drawCircle(display.width()/2, display.height()/2, 16, SSD1306_WHITE);
  display.drawCircle(display.width()/2, display.height()/2, 24, SSD1306_WHITE);
  display.drawLine(display.width()/2, 4, display.width()/2, display.height()-4, SSD1306_WHITE);
  display.drawLine(display.width()/2-28, display.height()/2, display.width()/2+28, display.height()/2, SSD1306_WHITE);
  // Draw point
  int x_position = (int)(display.width()/2 + a->acceleration.x/(2.0*9.81)*24);
  int y_position = (int)(display.height()/2 - a->acceleration.y/(2.0*9.81)*24);
  display.fillCircle(x_position, y_position, 3, SSD1306_WHITE);
  display.display();
}