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

Adafruit_MPU6050 mpu;

const int triggerPin = 13; 
const int resetBtn = "reset";     // Reset button connected to Digital Pin 2

float filtered_y = 0;
float prev_y = 0;
unsigned long windowStartTime = 0;
int zeroCrossings = 0;
bool wasPositive = false;
float maxAmplitude = 0;
bool systemActive = true;   // Flag to track if the system is manually reset

void setup() {
  Serial.begin(115200);
  pinMode(triggerPin, OUTPUT);
  pinMode(resetBtn, INPUT_PULLUP); // Use internal pull-up resistor
  
  digitalWrite(triggerPin, LOW);

  if (!mpu.begin()) {
    Serial.println("MPU6050 NOT DETECTED!");
    while (1);
  }
  
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); 
  windowStartTime = millis();
}

void loop() {
  // 1. Check Reset Button (Active Low)
  if (digitalRead(resetBtn) == LOW) {
    systemActive = false;
    digitalWrite(triggerPin, LOW);
    Serial.println("!!! MANUAL RESET: Stimulation Stopped !!!");
    delay(10); // Debounce delay
  }

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // X, Y, Z (Total Acceleration)
  float raw_accel = sqrt(a.acceleration.x * a.acceleration.x + 
                         a.acceleration.y * a.acceleration.y + 
                         a.acceleration.z * a.acceleration.z) - 9.8;

  // High-Pass Filter
  filtered_y = 0.9 * filtered_y + 0.9 * (raw_accel - prev_y);
  prev_y = raw_accel;

  if (abs(filtered_y) > maxAmplitude) {
    maxAmplitude = abs(filtered_y);
  }

  // Improved Zero-Crossing Sensitivity (0.001 instead of 0.01)
  bool isPositive = (filtered_y > 0.001); 
  if (isPositive != wasPositive) {
    zeroCrossings++;
    wasPositive = isPositive;
  }

  if (millis() - windowStartTime >= 1000) {
    float frequency = zeroCrossings / 2.0; 
    
    Serial.print("Freq: "); Serial.print(frequency);
    Serial.print(" Hz, Amp: "); Serial.print(maxAmplitude);

    // 2. Modified Trigger Logic with Reset Check
    // System triggers ONLY if it hasn't been manually reset
    if (systemActive && frequency >= 4.0 && frequency <= 12.0 && maxAmplitude > 0.5) {
      digitalWrite(triggerPin, HIGH);
      Serial.println(" --> TREMOR DETECTED!");
    } else {
      digitalWrite(triggerPin, LOW);
      Serial.println(systemActive ? " --> No Tremor" : " --> SYSTEM RESET PAUSE");
      
      // 3. Auto-Reactivate Logic
      // If the hand is still (frequency < 0.5), reactivate the system for next time
      if (!systemActive && frequency < 0.5) {
        systemActive = true;
        Serial.println("System Ready - Monitoring active.");
      }
    }

    zeroCrossings = 0;
    maxAmplitude = 0;
    windowStartTime = millis();
  }
  delay(10);
}