#include <Wire.h>
unsigned int time = 0;
const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;

void setup() {
  Serial.begin(19200);
  Wire.begin();                      // Initialize comunication
  Wire.beginTransmission(MPU);       // Start communication with MPU6050 // MPU=0x68
  Wire.write(0x6B);                  // Talk to the register 6B
  Wire.write(0x00);                  // Make reset - place a 0 into the 6B register
  Wire.endTransmission(true);        //end the transmission
  /*
  // Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
  Wire.beginTransmission(MPU);
  Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)
  Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)
  Wire.endTransmission(true);
  // Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);                   // Talk to the GYRO_CONFIG register (1B hex)
  Wire.write(0x10);                   // Set the register bits as 00010000 (1000deg/s full scale)
  Wire.endTransmission(true);
  delay(20);
  */
  // Call this function if you need to get the IMU error values for your module
  
  delay(20);

  pinMode(A0, INPUT); // Set the A0 pin to a input mode
  pinMode(A1, INPUT); // Set the A1 pin to a input mode
  pinMode(A2, INPUT); // Set the A2 pin to a input mode
  pinMode(A3, INPUT); // Set the A3 pin to a input mode
  pinMode(2, INPUT_PULLUP); // Set the 3 pin to a input mode

}


int sport = 0;
int changeS = 0;
int raise = 0;
int lower = 0;
int changeUP;
int changeDWN;
int level;

// values below in g
float brakepoint1 = -0.2; // first threshold during braking
float brakepoint2 = -0.4; // second threshold during braking
float brakepoint3 = -0.6; // third threshold during braking
float accelpoint1 = 0.2; // first threshold during acceleration
float accelpoint2 = 0.4; // second threshold during acceleration
float accelpoint3 = 0.6; // third threshold during acceleration
float accelcorneringpoint1 = 0.2; // first threshold acceleration in cornering
float accelcorneringpoint2 = 0.4; // second threshold acceleration in cornering
float accelcorneringpoint3 = 0.6; // third threshold acceleration in cornering
float sinkingpoint1 = 0.1; // first threshold acceleration for sinking
float sinkingpoint2 = 0.2; // second threshold acceleration in sinking
float sinkingpoint3 = 0.3; // third threshold acceleration in sinking



void loop() {
// time = millis();
// === Read accelerometer data === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
  //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value


  //delay(500);

  changeS = sport;

  if (digitalRead(A2) == HIGH) sport = 1;
  if (digitalRead(A1) == HIGH) sport = 0;

   if (changeS != sport) {
     digitalWrite(12, HIGH);
     digitalWrite(11, HIGH);
     digitalWrite(10, HIGH);
     digitalWrite(9, HIGH);
     digitalWrite(8, HIGH);
     digitalWrite(7, HIGH);
    }
    
    // Print out the values
/*  
 
      Serial.print("X = ");
     Serial.print(AccX);
     Serial.print(", Y = ");
     Serial.print(AccY);
     Serial.print(", Z = ");
     Serial.println(AccZ);
  */   

  switch (sport) {
    case 1:
      digitalWrite(12, LOW);
      digitalWrite(11, LOW);
      digitalWrite(10, LOW);
      digitalWrite(9, LOW);
      digitalWrite(8, LOW);
      digitalWrite(7, LOW);
      break;

    case 0:

      // acceleration values in g
      // + Y acc is acceleration ; - Y acc is braking

      //Locking rear dampers during acceleration
      if (AccY >= 0.05) {
        if (AccY >= 0 && AccY < accelpoint1) {
          digitalWrite(8, HIGH);
          digitalWrite(7, HIGH);
        }
        if (AccY >= accelpoint1 && AccY < accelpoint2) {
          digitalWrite(8, LOW);
          digitalWrite(7, HIGH);
        }
        if (AccY >= accelpoint2 && AccY < accelpoint3)  {
          digitalWrite(7, LOW);
          digitalWrite(8, HIGH);
        }
        if (AccY >= accelpoint3) digitalWrite(8, LOW);
      }


      // Locking front dampers during braking

      if (AccY <= -0.05) {
        if (AccY <= 0 && AccY > brakepoint1) {
          digitalWrite(12, HIGH);
          digitalWrite(11, HIGH);
          digitalWrite(10, HIGH);
          digitalWrite(9, HIGH);
        }
        if (AccY <= brakepoint1 && AccY > brakepoint2) {
          digitalWrite(12, LOW);
          digitalWrite(11, HIGH);
          digitalWrite(10, LOW);
          digitalWrite(9, HIGH);
        }
        if (AccY <= brakepoint2 && AccY > brakepoint3)  {
          digitalWrite(11, LOW);
          digitalWrite(12, HIGH);
          digitalWrite(9, LOW);
          digitalWrite(10, HIGH);
        }
        if (AccY <= brakepoint3) {
          digitalWrite(12, LOW);
          digitalWrite(11, LOW);
          digitalWrite(10, LOW);
          digitalWrite(9, LOW);
        }
      }

      // + Z acc is to the left ; - Z acc is to the right

      // Locking left front damper during cornering to right
if (AccZ <= -0.05) {
      if (AccZ <= 0 && AccZ > -1 * (accelcorneringpoint1)) {
        digitalWrite(12, HIGH);
        digitalWrite(11, HIGH);
      }
      if (AccZ <= -1 * (accelcorneringpoint1) && AccZ > -1 * (accelcorneringpoint2)) {
        digitalWrite(12, LOW);
        digitalWrite(11, HIGH);
      }
      if (AccZ <= -1 * (accelcorneringpoint2) && AccZ > -1 * (accelcorneringpoint3))  {
        digitalWrite(11, LOW);
        digitalWrite(12, HIGH);
      }
      if (AccZ <= -1 * (accelcorneringpoint3)) {
        digitalWrite(12, LOW);
        digitalWrite(11, LOW);
      }
}

      // Locking right front damper during cornering to left
if (AccZ >= 0.05) {
      if (AccZ >= 0 && AccZ < accelcorneringpoint1) {
        digitalWrite(10, HIGH);
        digitalWrite(9, HIGH);
      }
      if (AccZ >= accelcorneringpoint1 && AccZ < accelcorneringpoint2) {
        digitalWrite(10, LOW);
        digitalWrite(9, HIGH);
      }
      if (AccZ >= accelcorneringpoint2 && AccZ < accelcorneringpoint3)  {
        digitalWrite(9, LOW);
        digitalWrite(10, HIGH);
      }
      if (AccZ >= accelcorneringpoint3) {
        digitalWrite(9, LOW);
        digitalWrite(10, LOW);
      }

}

      // + X acc is up ; - X acc is down
      // Locking dampers during sinking
      
            if (AccX <= 0.99 && AccX > 1-sinkingpoint1) {
              digitalWrite(12, HIGH);
              digitalWrite(11, HIGH);
              digitalWrite(10, HIGH);
              digitalWrite(9, HIGH);
              digitalWrite(8, HIGH);
              digitalWrite(7, HIGH);
            }
            if (AccX <= 1-sinkingpoint1 && AccX > 1-sinkingpoint2) {
              digitalWrite(12, LOW);
              digitalWrite(11, HIGH);
              digitalWrite(10, LOW);
              digitalWrite(9, HIGH);
              digitalWrite(8, LOW);
              digitalWrite(7, HIGH);
            }
            if (AccX <= 1-sinkingpoint2 && AccX > 1-sinkingpoint3)  {
              digitalWrite(11, LOW);
              digitalWrite(12, HIGH);
              digitalWrite(9, LOW);
              digitalWrite(10, HIGH);
              digitalWrite(8, HIGH);
              digitalWrite(7, LOW);
            }
            if (AccX <= 1-sinkingpoint3) {
              digitalWrite(12, LOW);
              digitalWrite(11, LOW);
              digitalWrite(10, LOW);
              digitalWrite(9, LOW);
              digitalWrite(8, LOW);
              digitalWrite(7, LOW);
            }

      
      break;
  }

  // raising & lowering vehicle for linear electric actuators - how long is pressed that long is raising or lowering

  raise = digitalRead(A3);
  if (raise == HIGH) {
    digitalWrite(6, HIGH);
    digitalWrite(5, HIGH);
  }
  else if (raise == LOW) {
    digitalWrite(6, LOW);
    digitalWrite(5, LOW);
  }

  lower = digitalRead(A0);
  if (lower == HIGH) {
    digitalWrite(6, HIGH);
    digitalWrite(4, HIGH);
  }
  else if (lower == LOW) {
    digitalWrite(6, LOW);
    digitalWrite(4, LOW);
  }

  // MIL lit if oil level drops

  int level = digitalRead(2);
  if (level == 1) {
    digitalWrite(3, HIGH);
  }
  else {
    digitalWrite(3, LOW);
  }

 // time = millis() - time;
 //Serial.println(time, DEC);
 //  delay(1000);
}