/*
AVR C Servo Control on Arduino Nano 16mHz clock, Timer 1
*/

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

// Timer1 is counting 0.5 µs ticks.
/* Should be these values for 0, 90 and 180
#define PULSE_MIN  2000 // = 1ms value
#define PULSE_MID  3000 // = 1.5ms value
#define PULSE_MAX  4000 // = 2ms value
*/
// True values for Wokwi
#define PULSE_MIN  1100 // = 1ms value
#define PULSE_MID  2950 // = 1.5ms value
#define PULSE_MAX  4800 // = 2ms value


static inline void initTimer1Servo(void) {
  // Set up Timer1 (16bit) to give a pulse every 20ms
  // Fast PWM, TOP = ICR1 (Mode 14)
  TCCR1A = 0; // Clear registers
  TCCR1B = 0;
  TCNT1  = 0;

  TCCR1A |= (1 << WGM11);
  TCCR1B |= (1 << WGM12) | (1 << WGM13);

  TCCR1B |= (1 << CS11); // devide by 8 prescaling -- counting in microseconds
  TCCR1A |= (1 << COM1A1); // Direct output on PB1 / OC1A

  ICR1 = 20000; // TOP value = 20ms
  
  DDRB |= (1 << PB1); // set pin for output
}

static inline void showOff(void) {
  Serial.println("90 degrees");
  OCR1A = PULSE_MID;
  _delay_ms(1500);
  Serial.println("0 degrees");
  OCR1A = PULSE_MIN;
  _delay_ms(1500);
  Serial.println("180 degrees");
  OCR1A = PULSE_MAX;
  _delay_ms(1500);
  Serial.println("90 degrees");
  OCR1A = PULSE_MID;
  _delay_ms(1500);
}

void setup() {
  Serial.begin(115200);
  initTimer1Servo();

  Serial.println(OCR1A);

  OCR1A = 1100; // 1100 = value to reach 0
  Serial.println(OCR1A);
  _delay_ms(1500);

  OCR1A = 2950; // 2950 = value to reach 90
  Serial.println(OCR1A);
  _delay_ms(1500);

  OCR1A = 4800; // 4800 = value to reach 180
  Serial.println(OCR1A);
  _delay_ms(1500);
  
  showOff();
}

void loop() {

  for (int i = PULSE_MIN; i < PULSE_MAX; i++) {
    OCR1A = i;
    _delay_ms(1);
    Serial.println(OCR1A);
  }
  OCR1A = PULSE_MIN;
  _delay_ms(1500);
}