/*
 From https://forum.arduino.cc/t/square-wave-generation-using-nano/973614/32

Output:

Freq: 2000.00 actual: 2000.00 prescale: 1 TOP: 7999
Freq: 3000.00 actual: 3000.19 prescale: 1 TOP: 5332
Freq: 4000.00 actual: 4000.00 prescale: 1 TOP: 3999
Freq: 5000.00 actual: 5000.00 prescale: 1 TOP: 3199
Freq: 6000.00 actual: 5999.25 prescale: 1 TOP: 2666
Freq: 7000.00 actual: 6999.12 prescale: 1 TOP: 2285z

*/

// Generating Variable-Frequency tones on Timer1 of an Arduino UNO (Pins 9 and/or 10)
// Good for many frequencies from 0.24 Hz to 8 MHz
// Written March 30th, 2022 by John Wasser

uint16_t TimerTOP = 0xFFFF;

bool SetTimer1Frequency(float frequency)
{
  byte prescaleBits; // 1, 2, 3, 4, 5
  uint16_t prescaleFactor;  // 1, 8, 64, 256, 1024
  uint32_t top32;

  // Clear the three clock select bits to stop the timer
  TCCR1B &= ~((1 << CS12) | (1 << CS11) | (1 << CS10));

  // Find the smallest prescale factor that will fit the TOP value within 16 bits.
  // frequency = F_CPU / (prescale * (TOP + 1))
  // TOP = (F_CPU / (prescale * frequency)) - 1;

  prescaleBits = 1;
  prescaleFactor = 1;  // Used for 123-65535 Hz
  top32 = (F_CPU / (prescaleFactor * frequency)) - 0.5;  // Round to next lower integer
  if (top32 < 1)
    return false;

  if (top32 > 65535UL) // Too many clocks to count in 16 bits?
  {
    prescaleBits = 2;
    prescaleFactor = 8;  // Used for 16-122 Hz
    top32 = (F_CPU / (prescaleFactor * frequency)) + 0.5;  // Round to nearest integer
    if (top32 > 65535UL) // Too many clocks to count in 16 bits?
    {
      prescaleBits = 3;
      prescaleFactor = 64;  // Used for 2-15 Hz
      top32 = (F_CPU / (prescaleFactor * frequency)) + 0.5;  // Round to nearest integer
      if (top32 > 65535UL) // Too many clocks to count in 16 bits?
      {
        prescaleBits = 4;
        prescaleFactor = 256; // Only used for 1 Hz
        top32 = (F_CPU / (prescaleFactor * frequency)) + 0.5;  // Round to nearest integer
        if (top32 > 65535UL) // Too many clocks to count in 16 bits?
        {
          prescaleBits = 5;
          prescaleFactor = 1024;
          top32 = (F_CPU / (prescaleFactor * frequency)) + 0.5;  // Round to nearest integer
          if (top32 > 65535UL) // Too many clocks to count in 16 bits?
          {
            return false;
          }
        }
      }
    }
  }

  float actualFrequency = F_CPU / float((top32 + 1) * prescaleFactor);

  Serial.print("Freq: ");
  Serial.print(frequency);
  Serial.print(" actual: ");
  Serial.print(actualFrequency);
  Serial.print(" prescale: ");
  Serial.print(prescaleFactor);
  Serial.print(" TOP: ");
  Serial.println(top32);

  TimerTOP = top32;

  OCR1A = TimerTOP / 2;
  OCR1B = TimerTOP / 2;

  ICR1 = TimerTOP;

  TCCR1B |= prescaleBits; // Set clock prescale bits to start the timer
  return true;
}

void setup()
{
  Serial.begin(115200);
  delay(200);

  digitalWrite(9, LOW);
  pinMode(9, OUTPUT);
  digitalWrite(10, LOW);
  pinMode(10, OUTPUT);

  // Stop Timer/Counter1
  TCCR1A = 0;  // Timer/Counter1 Control Register A
  TCCR1B = 0;  // Timer/Counter1 Control Register B
  TIMSK1 = 0;  // Timer/Counter1 Interrupt Mask Register

  // Set Timer/Counter1 to Waveform Generation Mode 14: Fast PWM with TOP set by ICR1
  TCCR1A |= (1 << WGM11);  // WGM=14
  TCCR1B |= (1 << WGM13) | (1 << WGM12);  // WGM=14
  TCCR1A |= (1 << COM1A1);  // Normal PWM on Pin 9
  TCCR1A |= (1 << COM1B1);  // Normal PWM on Pin 10

  SetTimer1Frequency(1000.0);  // Default to 1 kHz
}

void loop()
{
  if (Serial.available())
  {
    float frequency = Serial.parseFloat();
    if (!SetTimer1Frequency(frequency))
    {
      Serial.print("Frequency Set Failed: ");
      Serial.println(frequency);
    }

    delay(100);
    while (Serial.available())
    {
      Serial.read();
      delay(100);
    }
  }
}