//Wokwi Serial terminal simulation - Wokwi Arduino Simulator

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

// Create three instances of the MPU6050 class
Adafruit_MPU6050 mpu1;

float angle1;
int buttoncheck();
int stompcheck();



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

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

	// set accelerometer range to +-8G for all three modules
	mpu1.setAccelerometerRange(MPU6050_RANGE_8_G);


	// set gyro range to +- 500 deg/s for all three modules
	mpu1.setGyroRange(MPU6050_RANGE_500_DEG);


	// set filter bandwidth to 21 Hz for all three modules
	mpu1.setFilterBandwidth(MPU6050_BAND_21_HZ);

  angle1 = 0;

  pinMode(13, OUTPUT);
  pinMode(12, INPUT);

	delay(100);
}


void loop() {

  float dt = 10;
  float gyroadjust = 370/M_PI;

  if (stompcheck() == 1){
    Serial.println("stomp detected");
    digitalWrite(13, HIGH);
    delay(3);

    while(1){

      if (buttoncheck() == 0){
        break;
      }
    }

    delay(5);

    digitalWrite(13, LOW);
    Serial.println("button pressed, action will now be detected");

  } else {

    //Extra writing for visdible confirmation of no stomp detection
    Serial.println("Stomp not detected");
    Serial.println("Next iteration will begin");
  }

  delay(5);

  sensors_event_t a1, g1, temp1;
  mpu1.getEvent(&a1, &g1, &temp1);
  


  angle1 += (g1.gyro.z*gyroadjust) *((dt+5)/1000);

  //If the angle goes past zero downwards it is now at 360
  if ((angle1 - 360) > 0){
    angle1 = 0;
  }

  //If the angle goes past 360 upwards it gets set to zero
  if (angle1 < 0){
    angle1 = 360;
  }

  //Delay for dt
  delay(dt);

  //Print the angle
  Serial.println(angle1);
}

int stompcheck(){

 
  //Pull the accelearation values
  sensors_event_t a1, g1, temp1;
  mpu1.getEvent(&a1, &g1, &temp1);


  //Acceleration definitions
  float xaccel = a1.acceleration.x;
  float yaccel = a1.acceleration.y;
  float zaccel = a1.acceleration.z;

  //Resulting acceleration in the movement plane
  float planeaccel = sqrt(pow(xaccel, 2)+pow(yaccel, 2));

  //If acceleration satisfies the conditions, stomp is true
  if ((zaccel > 19) && (planeaccel < 8)){

    return 1;
    
  } else return 0;

}

int buttoncheck(){
  if (digitalRead(12) == HIGH){
    return 0;
  } else return 1;
}