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

#define F_CPU 16000000UL  // 16 MHz Clock Frequency

void Timer1_init() {
    // Set Fast PWM mode with ICR1 as TOP (Mode 14)
    TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM11);
    TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); // Prescaler = 8

    ICR1 = 40000; // 50Hz PWM (16MHz / 8 / 40000 = 50Hz)

    // Set default servo positions to center (1.5ms pulse width)
    OCR1A = 3000;  // Servo on Pin 11 (PB5)
    OCR1B = 3000;  // Servo on Pin 12 (PB6)

    // Set PB5 (Pin 11) and PB6 (Pin 12) as output
    DDRB |= (1 << PB6) | (1 << PB5);
}

void setServoPosition(uint8_t servo, uint16_t angle) {
    // Convert angle (0-180°) to pulse width (1ms to 2ms)
    uint16_t pulse_width = 2000 + (angle * 11);

    if (servo == 1) {
        OCR1A = pulse_width; // Update Servo on Pin 13
    } else if (servo == 2) {
        OCR1B = pulse_width; // Update Servo on Pin 12
    }
}

int main() {
    Timer1_init();  // Initialize Timer1 for PWM

    while (1) {
        setServoPosition(1, 0);    // Move Servo on Pin 13 to 0°
        setServoPosition(2, 180);  // Move Servo on Pin 12 to 180°
        _delay_ms(1000);

        setServoPosition(1, 90);   // Move Servo on Pin 13 to 90°
        setServoPosition(2, 90);   // Move Servo on Pin 12 to 90°
        _delay_ms(1000);

        setServoPosition(1, 180);  // Move Servo on Pin 13 to 180°
        setServoPosition(2, 0);    // Move Servo on Pin 12 to 0°
        _delay_ms(1000);
    }
}