#include <avr/io.h>
#include <avr/interrupt.h>

void setupTimer1(double frequency) {
  // Disabilita temporaneamente il timer1
  TCCR1A = 0; // Reset controllo
  TCCR1B = 0; // Reset clock

  // Calcolo del top e del prescaler
  const uint32_t f_cpu = F_CPU; // Frequenza della CPU (16 MHz)
  uint16_t top = 0;
  uint16_t prescaler = 0;

  if (f_cpu / (1 * frequency) <= 65536) {
    prescaler = 1;
    top = f_cpu / (1 * frequency) - 1;
  } else if (f_cpu / (8 * frequency) <= 65536) {
    prescaler = 8;
    top = f_cpu / (8 * frequency) - 1;
  } else if (f_cpu / (64 * frequency) <= 65536) {
    prescaler = 64;
    top = f_cpu / (64 * frequency) - 1;
  } else if (f_cpu / (256 * frequency) <= 65536) {
    prescaler = 256;
    top = f_cpu / (256 * frequency) - 1;
  } else if (f_cpu / (1024 * frequency) <= 65536) {
    prescaler = 1024;
    top = f_cpu / (1024 * frequency) - 1;
  } else {
    Serial.println("Errore: frequenza non impostabile");
    return;
  }

  // Imposta il valore TOP
  ICR1 = top;

  // Modalità CTC (Clear Timer on Compare Match) con TOP definito in ICR1
  TCCR1A = (1 << WGM11);
  TCCR1B = (1 << WGM13) | (1 << WGM12);

  // Imposta il prescaler
  switch (prescaler) {
    case 1:
      TCCR1B |= (1 << CS10);
      break;
    case 8:
      TCCR1B |= (1 << CS11);
      break;
    case 64:
      TCCR1B |= (1 << CS11) | (1 << CS10);
      break;
    case 256:
      TCCR1B |= (1 << CS12);
      break;
    case 1024:
      TCCR1B |= (1 << CS12) | (1 << CS10);
      break;
  }

  // Abilita gli interrupt di overflow
  TIMSK1 |= (1 << TOIE1);
  sei();                  
}

ISR(TIMER1_OVF_vect) {
  digitalWrite(13, HIGH);
  digitalWrite(13, LOW);
}

void setup() {
  // Configura D13 come output
  pinMode(13, OUTPUT);
  Serial.begin(115200);
}

uint16_t oldFrequency;

void loop() {
  uint16_t frequency = map(analogRead(A0), 0, 1023, 50, 2500);

  if (frequency != oldFrequency) {
    oldFrequency = frequency;
    setupTimer1(frequency);
    // Serial.print("Setpoint: ");
    // Serial.println(frequency);
  }

}