// Basic demo for accelerometer readings from Adafruit MPU6050

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <CircularBuffer.h>

#define LED1_RED 3
#define LED1_GRN 5
#define LED1_BLU 6

#define LED2_RED 9
#define LED2_GRN 10
#define LED2_BLU 11

#define LED_BRIGHT 100

Adafruit_MPU6050 mpu;

double x_Buff;
double y_Buff;
double z_Buff;
int roll;
int pitch;
int prevRoll = 0;

CircularBuffer<int,10> values;
CircularBuffer<int,10> averageDifferences;

void setup(void) {

  pinMode(LED1_RED, OUTPUT);
  pinMode(LED1_GRN, OUTPUT);
  pinMode(LED1_BLU, OUTPUT);
  pinMode(LED2_RED, OUTPUT);
  pinMode(LED2_GRN, OUTPUT);
  pinMode(LED2_BLU, OUTPUT);
  set_led(0,0,0);
  
  
  Serial.begin(115200);
  while (!Serial) {
    delay(10); // will pause Zero, Leonardo, etc until serial console opens
  }

  // Try to initialize!
  if (!mpu.begin()) {
    //Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_10_HZ);
  //Serial.println("");
  delay(100);
  set_led(0,0,0);
}

void loop() {

  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  /* Print out the values */
  /*Serial.print(a.acceleration.x);
  Serial.print(",");
  Serial.print(a.acceleration.y);
  Serial.print(",");
  Serial.print(a.acceleration.z);
  Serial.print(", ");
  Serial.print(g.gyro.x);
  Serial.print(",");
  Serial.print(g.gyro.y);
  Serial.print(",");
  Serial.print(g.gyro.z);
  Serial.println("");
  */

  x_Buff = float(a.acceleration.x);
  y_Buff = float(a.acceleration.y);
  z_Buff = float(a.acceleration.z);
  roll = atan2(y_Buff , z_Buff) * 57.3;
  //pitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;

  values.push(roll);
  int average = (values[0]+values[1]+values[2]+values[3]+values[4]+values[5]+values[6]+values[7]+values[8]+values[9])/10;

  for (int i=0; i<10; i++)
  {
    averageDifferences.push(abs(values[i] - average));
  }

  int differences = averageDifferences[0]+averageDifferences[1]+averageDifferences[2]+averageDifferences[3]+averageDifferences[4]+averageDifferences[5]+averageDifferences[6]+averageDifferences[7]+averageDifferences[8]+averageDifferences[9];
  
  if (differences < 2 && abs(prevRoll - average)>1)
  {
    prevRoll = average;
    Serial.print(roll+180);
    set_led(0,LED_BRIGHT,0);
  }
  else if (differences >= 2)
  {
    set_led(LED_BRIGHT,0,0);
  }
  else
  {
    set_led(0,0,LED_BRIGHT);
  }

  delay(50);
}

void set_led(uint8_t r, uint8_t g, uint8_t b)
{
  analogWrite(LED1_RED, 255-r);
  analogWrite(LED1_GRN, 255-g);
  analogWrite(LED1_BLU, 255-b);
  analogWrite(LED2_RED, 255-r);
  analogWrite(LED2_GRN, 255-g);
  analogWrite(LED2_BLU, 255-b);
}