#include <Servo.h>

Servo pendulumServo;

const int servoPin = 11;
const int vibPin = 2;     // Pin collegato al sensore di vibrazione
const int ledPin = 13;    // Pin collegato al LED
const int buzzerPin = 7;  // Pin collegato al buzzer

const float g = 9.81;       // accelerazione gravitazionale
const float L = 0.2;        // lunghezza del pendolo in metri
const float thetaMax = 40.0; // ampiezza massima in gradi
const int rampDuration = 15000; // durata rampa in millisecondi (30 sec)
unsigned long startTime;

void setup() {
  pendulumServo.attach(servoPin);
  startTime = millis();
  pinMode(vibPin, INPUT);     // Imposta il pin del sensore come input
  pinMode(ledPin, OUTPUT);    // Imposta il pin del LED come output
  pinMode(buzzerPin, OUTPUT); // Imposta il pin del buzzer come output
  Serial.begin(9600);         // Avvia la comunicazione seriale
}

void loop() {
  unsigned long currentTime = millis();
  float t = (currentTime - startTime) / 1000.0; // tempo in secondi

  // Calcolo della frequenza angolare
  float omega = sqrt(g / L);

  // Calcolo dell'ampiezza attuale in base alla rampa
  float rampProgress = min((currentTime - startTime) / (float)rampDuration, 1.0);
  float currentThetaMax = thetaMax * rampProgress;

  // Calcolo dell'angolo del pendolo
  float angle = currentThetaMax * cos(omega * t);

  // Mappatura dell'angolo per il servo (0-180 gradi)
  int servoAngle = map(angle, -thetaMax, thetaMax, 60, 120); // centro a 90°
  pendulumServo.write(servoAngle);

  int vibValue = digitalRead(vibPin);  // Legge lo stato del sensore

  if (vibValue == HIGH) {
    digitalWrite(ledPin, HIGH);        // Accende il LED
    digitalWrite(buzzerPin, HIGH);     // Attiva il buzzer
    Serial.println("Vibrazione rilevata!");
  } else {
    digitalWrite(ledPin, LOW);         // Spegne il LED
    digitalWrite(buzzerPin, LOW);      // Spegne il buzzer
  }

  delay(100);  // Piccola pausa per stabilizzare la lettura
}