#include <Arduino.h>
#include <Streaming.h>
#include <util/atomic.h>
Print &cout {Serial};

constexpr uint8_t PWM_OUT {9};
constexpr uint8_t ANALOG_IN {A0};
constexpr uint16_t HERTZ {1};   // Min is 1 / Max is 65535!

// Dont change anything below ------ PSC = Prescaler Timer 1 ----------------------------------------------------------
constexpr uint32_t PSC_INDEX() {
  return (F_CPU / (1 * HERTZ)) <= 65535U      ? 0
         : (F_CPU / (8 * HERTZ)) <= 65535U    ? 1
         : (F_CPU / (64 * HERTZ)) <= 65535U   ? 2
         : (F_CPU / (256 * HERTZ)) <= 65535U  ? 3
         : (F_CPU / (1024 * HERTZ)) <= 65535U ? 4
                                              : 5;
}
constexpr uint16_t PSC[6] {1, 8, 64, 256, 1024, 0};
constexpr uint8_t SET_PSC[6] {_BV(CS10), _BV(CS11), (_BV(CS11) | _BV(CS10)), _BV(CS12), (_BV(CS12) | _BV(CS10)), 0};
constexpr uint16_t ICR1_TOP {(F_CPU / (2UL * PSC[PSC_INDEX()] * HERTZ)) - 1};
// --------------------------------------------------------------------------------------------------------------------

void setup() {
  Serial.begin(115200);
 
  pinMode(PWM_OUT, OUTPUT);
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    TCCR1B = 0;   // Stop Timer first, for safety
    TCCR1A = 0;
    TCCR1B = _BV(WGM13) | SET_PSC[PSC_INDEX()];   // Prescaler
    TCCR1A = _BV(COM1A1) | _BV(WGM11);            // phase correct, clear ocr1a on compare match
    ICR1 = ICR1_TOP;                              // TOP VAlue for 2kHz
    OCR1A = (ICR1_TOP / 2) + 1;                   // 50% Duty Cycle
  }
}

void loop() {
  static int16_t prevResult {1025};
  int16_t result = analogRead(A0);
  if (abs(result - prevResult) > 4) {
    prevResult = result;
    uint16_t duty = map(result, 0, 1023, 0, ICR1_TOP);
    cout << _WIDTH(duty,5) << " " << _WIDTH(ICR1_TOP,5) << " "  << _WIDTH(duty * 100UL / ICR1_TOP,3)<< "%" << endl;
    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { OCR1A = duty; }
  }
  delay(200);
}