// MPU6050 example
// https://wokwi.com/arduino/projects/305937248748044864

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

Adafruit_MPU6050 mpu;
const int ledPin = 11;
unsigned long forrigeTid = 0;
const int frek = 249;
const int toleranse = 10000;

unsigned long fTid_2 = 0;
const int vent = 499;

bool bevegelseAktiv = false;
unsigned long sistBevegelse = 0;

void setup(void) {
  Serial.begin(115200);
  pinMode(ledPin, OUTPUT);
  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");
}

sensors_event_t event;

void loop() {
  mpu.getAccelerometerSensor()->getEvent(&event);
  float a_x = event.acceleration.x;
  float a_y = event.acceleration.y;
  float a_z = event.acceleration.z;

  unsigned long tidTeller = millis();

  bool aksOver = abs(a_x) > 0.8 || abs(a_y) > 0.8;

  if (aksOver) {
    sistBevegelse = tidTeller;

    if (tidTeller - forrigeTid >= frek) {
      forrigeTid = tidTeller;
      digitalWrite(ledPin, !digitalRead(ledPin));
      //Serial.println(tidTeller);
    } 
  } else if (!aksOver && (tidTeller - sistBevegelse <= toleranse) && (tidTeller - forrigeTid >= frek) && (tidTeller >= toleranse)) {
    forrigeTid = tidTeller;
    digitalWrite(ledPin, !digitalRead(ledPin));
    Serial.println(tidTeller);
  } else if (!aksOver && (tidTeller - sistBevegelse >= toleranse)) {
    digitalWrite(ledPin, LOW);
    Serial.println("Har stått stille i over 10 sekunder");
  }

  unsigned long teller_2 = millis();

  if (teller_2 - fTid_2 >= vent) {
    fTid_2 = teller_2;
    Serial.print("[");
    Serial.print(teller_2);
    Serial.print("] X: ");
    Serial.print(a_x);
    Serial.print(", Y: ");
    Serial.print(a_y);
    Serial.print(", Z: ");
    Serial.print(a_z);
    Serial.println(" m/s^2");
  }
}