// Idle Waste Alarm
// Vije Miller
// MPU-6050 Sketch JohnChi

#include "Wire.h"

// I2C address of the MPU-6050
const int MPU_addr = 0x68;
int16_t AcY;

// Sensitivity
int high = 2700;
int low = -2700;
int set = 0;
int dif = 0;
// State
int state = 1;
// Time Variables
long echo = 0;
int dly = 100;
// 120 seconds
// Avg red light Wait 90 seconds
long sepone = 22500;
long septwo = 45000;
long septhree = 67500;
long sepfour = 90000;

// LEDs and Piezo
// Green
int one = 3;
int two = 4;
int three = 5;
// Yellow
int four = 6;
// Red and Piezo
int five = 7;

void setup() {

  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Serial.begin(9600);

  pinMode(one, OUTPUT);
  pinMode(two, OUTPUT);
  pinMode(three, OUTPUT);
  pinMode(four, OUTPUT);
  pinMode(five, OUTPUT);

  digitalWrite(one, HIGH);
  digitalWrite(two, LOW);
  digitalWrite(three, LOW);
  digitalWrite(four, LOW);
  digitalWrite(five, LOW);

}

void loop() {

  // GY-52 Communication
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true);

  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcY = Wire.read() << 8 | Wire.read();

  // Variance From Last Position
  dif = AcY - set;

  // Testing
  Serial.print("YAxi  ");
  Serial.print(AcY);
  Serial.print("   Difr   ");
  Serial.print(dif);
  Serial.print("   Mils   ");
  Serial.println(echo);

  // IF Motion Reset Minimums
  if (dif > high || dif < low) {
    digitalWrite(one, HIGH);
    digitalWrite(two, LOW);
    digitalWrite(three, LOW);
    digitalWrite(four, LOW);
    digitalWrite(five, LOW);
    state = 1;
    echo = 0;
  }
  else {
    if (state == 1) {
      if (echo >= sepone) {
        digitalWrite(two, HIGH);
        state = 2;
      }
    }
    else if (state == 2) {
      if (echo >= septwo) {
        digitalWrite(three, HIGH);
        state = 3;
      }
    }
    else if (state == 3) {
      if (echo >= septhree) {
        digitalWrite(four, HIGH);
        state = 4;
      }
    }
    else if (state == 4) {
      if (echo >= sepfour) {
        digitalWrite(five, HIGH);
      }
    }
  }

  // Reset Last Position
  set = AcY;

  // 1/10 Second
  echo = echo + dly;
  delay(dly);
}