/* #define SERVO_PIN 9
#define BUTTON_PIN 2

volatile int position = 0; // Posizione iniziale del servo
volatile bool buttonPressed = false;

void setup() {
  // Configura il pin del servo (SERVO_PIN) come output
  DDRB |= (1 << DDB1); // SERVO_PIN corrisponde a PB1 (pin 9 su Arduino UNO)

  // Configura il pin del pulsante (BUTTON_PIN) come input
  DDRD &= ~(1 << DDD2); // BUTTON_PIN corrisponde a PD2 (pin 2 su Arduino UNO)

  // Attiva il pull-up interno sul pin del pulsante
  PORTD |= (1 << PORTD2);



  
  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), handleButtonPress, RISING);
  setupTimer1();
}

*/



#define SERVO_PIN 9
#define BUTTON_PIN 2

volatile int position = 0; // Posizione iniziale del servo
volatile bool buttonPressed = false;

void setup() {
  // Configura il pin del servo (SERVO_PIN) come output
  DDRB |= (1 << DDB1); // SERVO_PIN corrisponde a PB1 (pin 9 su Arduino UNO)

  // Configura il pin del pulsante (BUTTON_PIN) come input
  DDRD &= ~(1 << DDD2); // BUTTON_PIN corrisponde a PD2 (pin 2 su Arduino UNO)

  // Attiva il pull-up interno sul pin del pulsante
  PORTD |= (1 << PORTD2);

  // Configura l'interrupt esterno per il pulsante (INT0)
  EICRA |= (1 << ISC01) | (1 << ISC00); // Configura per il fronte di salita (RISING)
  EIMSK |= (1 << INT0); // Abilita l'interrupt INT0
  
  // Configura il Timer1 per controllare il servo
  TCCR1A = (1 << COM1A1) | (1 << WGM11); // Modo Fast PWM, con TOP ICR1
  TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); // Prescaler 8
  ICR1 = 39999; // Periodo di 20ms (50Hz)
  
  // Posizione iniziale del servo (1.5 ms per posizione centrale)
  OCR1A = 3000;

  // Abilita gli interrupt globali impostando il bit I nel registro di stato SREG
  SREG |= (1 << 7);
}

ISR(INT0_vect) {
  // Incrementa la posizione del servo di 1 grado
  position += 1;
  if (position > 180) {
    position = 0;
  }
  
  // Calcola manualmente il valore di OCR1A per il PWM
  // 1000 è il valore per 0 gradi (1 ms), 5000 è il valore per 180 gradi (2 ms)
  OCR1A = 1000 + ((position * 25) / 9); 
  // Questa formula deriva dal fatto che (4000 / 180) = 22.22, arrotondato a 25/9 per maggiore precisione
}

void loop() {
  // Vuoto, tutto è gestito dagli interrupt
}

void handleButtonPress() {
  buttonPressed = true;
}

void setupTimer1() {
 // noInterrupts(); // Disabilita gli interrupt
  TCCR1A = 0; // Imposta il registro di controllo A a 0
  TCCR1B = 0; // Imposta il registro di controllo B a 0
  TCNT1 = 0; // Inizializza il contatore a 0
  OCR1A = 39999; // Imposta il valore di confronto per ottenere 50Hz (20ms periodo)
  TCCR1B |= (1 << WGM12); // Modalità CTC
  TCCR1B |= (1 << CS11) | (1 << CS10); // Prescaler 64
  TIMSK1 |= (1 << OCIE1A); // Abilita l'interrupt di confronto
  //interrupts(); // Abilita gli interrupt
}

ISR(TIMER1_COMPA_vect) {
  static bool state = false;
  static uint16_t pulse_width;
  static uint16_t high_time;

  // Calcola il tempo di impulso in microsecondi senza usare map()
  pulse_width = 544 + ((position * (2400 - 544)) / 180);

  if (state) {
    // Imposta il pin SERVO_PIN su HIGH
    PORTB |= (1 << PORTB1); // SERVO_PIN è il pin 9 su Arduino UNO (PB1)
    
    // Calcola il tempo di impulso per questo ciclo (incluso il tempo alto)
    high_time = pulse_width;

    // Abilita il prossimo ciclo di ritardo del PWM
    // Calcola il tempo per cui il pin deve rimanere LOW
    OCR1A = ICR1 - high_time; // Tempo durante il quale il pin rimane LOW
  } else {
    // Imposta il pin SERVO_PIN su LOW
    PORTB &= ~(1 << PORTB1); // SERVO_PIN è il pin 9 su Arduino UNO (PB1)
    
    // Calcola il tempo per cui il pin deve rimanere HIGH
    OCR1A = high_time; // Tempo durante il quale il pin rimane HIGH
  }

  state = !state;

  if (buttonPressed) {
    position += 10; // Incrementa la posizione del servo di 10 gradi
    if (position > 180) position = 0; // Reset posizione se supera 180 gradi
    buttonPressed = false;
  }
}

int mapValue(int x, int in_min, int in_max, int out_min, int out_max) {
  // Calcola la differenza degli intervalli
  int input_range = in_max - in_min;
  int output_range = out_max - out_min;

  // Calcola la proporzione rispetto all'intervallo di input
  int scaled_value = (x - in_min) * output_range;

  // Effettua la divisione per adattare l'intervallo di output
  int mapped_value = scaled_value / input_range;

  // Aggiungi il minimo dell'intervallo di output
  return mapped_value + out_min;
}



/*
#define SERVO_PIN 9
#define BUTTON_PIN 2

volatile int position = 90; // Posizione iniziale del servo
volatile int clickCount = 0;
volatile unsigned long lastClickTime = 0;
volatile bool buttonPressed = false;

void setup() {
  pinMode(SERVO_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT);
  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), handleButtonPress, RISING);
  setupTimer1();
}

void loop() {
  // Funzione loop vuota
}

void handleButtonPress() {
  unsigned long currentTime = micros();
  if (currentTime - lastClickTime > 50000) { // Debounce 50ms
    clickCount++;
    lastClickTime = currentTime;
  }
}

void setupTimer1() {
  noInterrupts(); // Disabilita gli interrupt
  TCCR1A = 0; // Imposta il registro di controllo A a 0
  TCCR1B = 0; // Imposta il registro di controllo B a 0
  TCNT1 = 0; // Inizializza il contatore a 0
  OCR1A = 39999; // Imposta il valore di confronto per ottenere 50Hz (20ms periodo)
  TCCR1B |= (1 << WGM12); // Modalità CTC
  TCCR1B |= (1 << CS11) | (1 << CS10); // Prescaler 64
  TIMSK1 |= (1 << OCIE1A); // Abilita l'interrupt di confronto
  interrupts(); // Abilita gli interrupt
}

ISR(TIMER1_COMPA_vect) {
  static bool state = false;
  if (state) {
    digitalWrite(SERVO_PIN, HIGH);
    delayMicroseconds(map(position, 0, 180, 544, 2400));
    digitalWrite(SERVO_PIN, LOW);
  }
  state = !state;

  if (clickCount == 1 && (micros() - lastClickTime) > 300000) {
    // Singolo click
    moveServo(1);
    clickCount = 0;
  } else if (clickCount == 2 && (micros() - lastClickTime) > 300000) {
    // Doppio click
    moveServo(-1);
    clickCount = 0;
  } else if (clickCount == 3 && (micros() - lastClickTime) > 300000) {
    // Triplo click
    resetServo();
    clickCount = 0;
  }
}

void moveServo(int step) {
  position += step;
  if (position > 180) position = 0; // Reset posizione se supera 180 gradi
  if (position < 0) position = 180; // Reset posizione se scende sotto 0 gradi
  analogWrite(SERVO_PIN, map(position, 0, 180, 544, 2400));
}

void resetServo() {
  position = 90; // Posizione iniziale
  analogWrite(SERVO_PIN, map(position, 0, 180, 544, 2400));
}

int map(int x, int in_min, int in_max, int out_min, int out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
*/