#include <Servo.h>

Servo myservo;

const int centerPos = 90;
const int maxAngle = 60; // ±60° per 120° totale
const unsigned long period = 1800; // Periodo in ms (1.8s per oscillazione completa)
const float pi = 3.14159265;

unsigned long startTime;
bool isRunning = false;

void setup() {
  myservo.attach(11);
  startTime = millis();
  isRunning = true;
}
void loop() {
  static float velocity = 3;
  static float acceleration = 3;
  static float position = 0;
  
  unsigned long currentTime = millis();
  static unsigned long lastTime = currentTime;
  float deltaTime = (currentTime - lastTime) / 1000.0;
  lastTime = currentTime;

  // Fisica semplificata del pendolo
  acceleration = -9.8 * sin(position) / 0.3; // 0.3 = lunghezza virtuale
  velocity += acceleration * deltaTime;
  position += velocity * deltaTime;

  // Energia costante (moto perpetuo)
  float energy = velocity*velocity + 19.6*(1-cos(position)); // 19.6 = 2*9.8
  if(energy < 19.6*(1-cos(radians(maxAngle)))) {
    velocity *= 1.001; // Piccola compensazione
  }

  // Converti e invia al servo
  int servoPos = centerPos + constrain(degrees(position), -maxAngle, maxAngle);
  myservo.write(servoPos);
  
  delay(10);
}