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

Adafruit_MPU6050 mpu1;

 double currentGYROValue; //The slowed down math
    #define Jack_Up 48 //Jack _Up relay
    #define Jack_Down 47 //Jack_Down relay
    #define JackButton 36

//-------------------------------------------------Jack_Upordown
//This needs to turn on when the button it high and turn off it the button is low.
void Jack_Up_or_Down ()
  {   sensors_event_t a1, g1, temp1;
      mpu1.getEvent(&a1, &g1, &temp1);
      double Tongue_Level = a1.acceleration.y; // get the true instant reading
      double currentGYROValue  = Tongue_Level; // 0.9* currentGYROValue  + 0.1 * Tongue_Level; // 90% of the old value, 10% of the new value
  
    Serial.println("outside loop"+ digitalRead(JackButton));
    while (true) //digitalRead(JackButton)==HIGH)
    {
        Serial.println("inside loop"+ digitalRead(JackButton));
    if (currentGYROValue < -1) 
        {
        digitalWrite(Jack_Up, HIGH);
        Serial.println("UP");
        Serial.println(currentGYROValue);
        digitalWrite(Jack_Down, LOW);
        delay(1000);
      //  digitalRead(JackButton)==HIGH;
        }
 
     else if (currentGYROValue > 1) 
        {
        digitalWrite(Jack_Down, HIGH);
        Serial.println("Down");
        Serial.println(currentGYROValue);
        digitalWrite(Jack_Up, LOW);
        delay(1000);
       // digitalRead(JackButton)==HIGH;
        }
 
      else {digitalWrite(Jack_Up, LOW);
        digitalWrite(Jack_Down, LOW); delay(500); break;}

      sensors_event_t a1, g1, temp1;
      mpu1.getEvent(&a1, &g1, &temp1);
      Tongue_Level = a1.acceleration.y; // get the true instant reading
      currentGYROValue  = 0.5* currentGYROValue  + 0.5 * Tongue_Level; // 90% of the old value, 10% of the new value
  
    }
    digitalWrite(Jack_Up, LOW);
    digitalWrite(Jack_Down, LOW);
  }

void setup(void) {
  Serial.begin(115200);

  pinMode(Jack_Up, OUTPUT);                   //pin 48 "This relay will level the front jack up"
  pinMode(Jack_Down, OUTPUT);                 //pin 47 "This relay will level the front jack down"
  pinMode(JackButton, INPUT); 


mpu1.begin();

    mpu1.setAccelerometerRange(MPU6050_RANGE_16_G);
    mpu1.setGyroRange(MPU6050_RANGE_250_DEG);
    mpu1.setFilterBandwidth(MPU6050_BAND_21_HZ);
    Serial.println("mpu1 started");
}



void loop() {
 Jack_Up_or_Down();

}