// Define pin connections
#define SERVO_PIN PB1 // Pin 9 on Arduino Nano

// Setup function to configure Timer1 and pin direction
void setup() {
  // Set the servo pin as output (DDRB register for Port B)
  DDRB |= (1 << SERVO_PIN);

  // Set Timer1 for Fast PWM mode with ICR1 as top
  TCCR1A = (1 << COM1A1) | (1 << WGM11);  // Non-inverting mode on OC1A
  TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11);  // Fast PWM, ICR1 as top, prescaler = 8

  // Set ICR1 for a 20 ms period corresponding to a 50 Hz servo control signal
  ICR1 = 39999;

  // Set OCR1A for a 1.5 ms pulse width initially (neutral position)
  OCR1A = 3000;
}

// Global timing variables
volatile unsigned long timerTicks = 0;
unsigned long lastChangeTime = 0;
int servoPosition = 0; // 0: Neutral, 1: 180 degrees, 2: 0 degrees

// Timer1 overflow interrupt service routine to keep track of passing time
ISR(TIMER1_OVF_vect) {
  timerTicks++; // Increment our counter of overflows
}

void loop() {
  // Check if one second has passed (based on timer overflows)
  // Each overflow is 4.096 ms, so roughly 244 overflows are approximately one second
  if ((timerTicks - lastChangeTime) >= 244) {
    lastChangeTime = timerTicks;

    if (servoPosition == 0) {
      // Rotate servo to 180 degrees
      OCR1A = 4000;
      servoPosition = 1;
    } else if (servoPosition == 1) {
      // Rotate servo to 0 degrees
      OCR1A = 2000;
      servoPosition = 2;
    } else {
      // Return to neutral position
      OCR1A = 3000;
      servoPosition = 0;
    }
  }
}

int main(void) {
  setup();
  
  // Enable Timer1 overflow interrupt
  TIMSK1 |= (1 << TOIE1);

  // Global interrupt enable
  sei();

  while (1) {
    loop();
  }
}