///////////////////////////////////////////////////////////
// TILT SENSOR

void set_last_read_angle_data(unsigned long t, int16_t x, int16_t y, int16_t z, int16_t x_gyro, int16_t y_gyro, int16_t z_gyro) {
  last_read_time = t;
  last_x_angle = x;
  last_y_angle = y;
  last_z_angle = z;
  last_gyro_x_angle = x_gyro;
  last_gyro_y_angle = y_gyro;
  last_gyro_z_angle = z_gyro;
}

//Function used to detect motion. Tolerance variable adjusts the sensitivity of movement detected.
boolean checkMotion() {
  boolean tempB = false;
  xVal = last_x_angle;
  yVal = last_y_angle;
  zVal = last_z_angle;

  if (xVal > (xMax + tolerance) || xVal < (xMin - tolerance)) {
    tempB = true;
  }

  if (yVal > (yMax + tolerance) || yVal < (yMin - tolerance)) {
    tempB = true;
  }

  if (zVal > (zMax + tolerance) || zVal < (zMin - tolerance)) {
    tempB = true;
  }

  return tempB;
}

void calibrateTILT() {
  // reset alarm
  moveDetected = false;

  tiltReading = 0;

  //initialise x,y,z variables
  xVal = last_x_angle;
  xMin = xVal;
  xMax = xVal;

  yVal = last_y_angle;
  yMin = yVal;
  yMax = yVal;

  zVal = last_z_angle;
  zMin = zVal;
  zMax = zVal;

  //calibrate the Accelerometer (should take about 0.5 seconds)
  for (unsigned int i = 0; i < 50; i++) {
    // Calibrate X Values
    xVal = last_x_angle;
    if (xVal > xMax) {
      xMax = xVal;
    } else if (xVal < xMin) {
      xMin = xVal;
    }

    // Calibrate Y Values
    yVal = last_y_angle;
    if (yVal > yMax) {
      yMax = yVal;
    } else if (yVal < yMin) {
      yMin = yVal;
    }

    // Calibrate Z Values
    zVal = last_z_angle;
    if (zVal > zMax) {
      zMax = zVal;
    } else if (zVal < zMin) {
      zMin = zVal;
    }

  }

  calibrated = true;

}

void updateTILT(){

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float gyro_x = (gx - base_x_gyro) / 131.0;
  float gyro_y = (gy - base_y_gyro) / 131.0;
  float gyro_z = (gz - base_z_gyro) / 131.0;

  float accel_x = ax;
  float accel_y = ay;
  float accel_z = az;

  float accel_angle_y = atan(-1 * accel_x / sqrt(pow(accel_y, 2) + pow(accel_z, 2))) * (180 / 3.14159);
  float accel_angle_x = atan(accel_y / sqrt(pow(accel_x, 2) + pow(accel_z, 2))) * (180 / 3.14159);
  float accel_angle_z = 0;

  // Compute the (filtered) gyro angles
  unsigned long t_now = millis();
  float dt = (t_now - last_read_time) / 1000.0;
  float gyro_angle_x = gyro_x * dt + last_x_angle;
  float gyro_angle_y = gyro_y * dt + last_y_angle;
  float gyro_angle_z = gyro_z * dt + last_z_angle;

  // Compute the drifting gyro angles
  float unfiltered_gyro_angle_x = gyro_x * dt + last_gyro_x_angle;
  float unfiltered_gyro_angle_y = gyro_y * dt + last_gyro_y_angle;
  float unfiltered_gyro_angle_z = gyro_z * dt + last_gyro_z_angle;

  // Apply the complementary filter to figure out the change in angle
  float angle_x = 0.96 * gyro_angle_x + (1.0 - 0.96) * accel_angle_x;
  float angle_y = 0.96 * gyro_angle_y + (1.0 - 0.96) * accel_angle_y;
  float angle_z = gyro_angle_z;  //Accelerometer doesn't give z-angle

  set_last_read_angle_data(t_now, (int16_t)angle_x, (int16_t)angle_y, (int16_t)angle_z, (int16_t)unfiltered_gyro_angle_x, (int16_t)unfiltered_gyro_angle_y, (int16_t)unfiltered_gyro_angle_z);
  
  if (calibrated) {
    if (checkMotion()) {
      timeTilt = millis();
      moveDetected = true;
    }
  }

  if (moveDetected) {
    tiltReading = 1;
    //don't check for movement until recalibrated again
    calibrated = false;
    if (millis() - timeTilt > tiltWait) {
      calibrateTILT();
    }
  }
  
}