// Arduino clock frequency is 16 MHz

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

#define CLOCK_FREQ 16000000U
#define PRESCALER 256
#define TICKS_PER_SEC (CLOCK_FREQ / PRESCALER)
#define MIN_PULSE_WIDTH 544    // Minimum pulse width for servos in microseconds
#define MAX_PULSE_WIDTH 2400   // Maximum pulse width for servos in microseconds

int servo1Pin = 11;
int servo2Pin = 3;

/// @brief Keeps a value within a specified range.
int clip(int val, int min, int max)
{
    if (val < min) return min;
    if (val > max) return max;
    return val;
}

/// @brief Maps the input value from one range to another.
int map(int val, int in_low, int in_high, int out_low, int out_high)
{
    val = clip(val, in_low, in_high);
    return (val - in_low) * (out_high - out_low) / (in_high - in_low) + out_low;
}

/// @brief Configures Timer2 for Phase Correct PWM mode.
void timer2_init()
{
    // Set Timer2 to Phase Correct PWM mode
    TCCR2A |= (1 << WGM20); // Phase Correct PWM mode
    TCCR2B |= (1 << WGM22); // Phase Correct PWM mode
    TCCR2A |= (1 << COM2A1) | (1 << COM2B1); // PWM on OC2A (Pin 11) and OC2B (Pin 3)
    
    // Set prescaler to 256
    TCCR2B |= (1 << CS22);
}

/// @brief Writes a PWM signal to a servo pin based on pulse width in microseconds.
void servo_write_us(int pin, int micro_secs)
{
    // Convert microseconds to timer ticks
    unsigned int ticks = (micro_secs * (CLOCK_FREQ / PRESCALER)) / 1000000;
    
    // Set the OCR register for the corresponding pin
    if (pin == 11) {
        OCR2A = clip(ticks, 0, 255); // Clip to fit within 8-bit range
    } else if (pin == 3) {
        OCR2B = clip(ticks, 0, 255); // Clip to fit within 8-bit range
    }
}

/// @brief Writes a PWM signal to a servo pin based on an angle.
void servo_write_angle(int pin, int angle)
{
    // Map the angle (0-180) to the pulse width (MIN_PULSE_WIDTH - MAX_PULSE_WIDTH)
    int pulse_width = map(angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
    servo_write_us(pin, pulse_width);
}

int main()
{
    // Initialize Timer2
    timer2_init();

    // Set Pin Modes for servos
    DDRB |= (1 << DDB3); // Pin 11 as output
    DDRD |= (1 << DDD3); // Pin 3 as output

    // Enable global interrupts (not strictly needed here but kept for safety)
    sei();

    while (1)
    {
        // Control using pulse width
        servo_write_us(servo1Pin, MIN_PULSE_WIDTH);
        servo_write_us(servo2Pin, MAX_PULSE_WIDTH);
        _delay_ms(1000);

        servo_write_us(servo1Pin, MAX_PULSE_WIDTH);
        servo_write_us(servo2Pin, MIN_PULSE_WIDTH);
        _delay_ms(1000);

        // Control using angles
        servo_write_angle(servo1Pin, 0);
        servo_write_angle(servo2Pin, 180);
        _delay_ms(1000);

        servo_write_angle(servo1Pin, 90);
        servo_write_angle(servo2Pin, 90);
        _delay_ms(1000);

        servo_write_angle(servo1Pin, 180);
        servo_write_angle(servo2Pin, 0);
        _delay_ms(1000);

        // Sweep motion
        for (int angle = 180; angle >= 0; angle--)
        {
            servo_write_angle(servo1Pin, angle);
            servo_write_angle(servo2Pin, 180 - angle);
            _delay_ms(10);
        }
        _delay_ms(500);
        for (int angle = 0; angle <= 180; angle++)
        {
            servo_write_angle(servo1Pin, angle);
            servo_write_angle(servo2Pin, 180 - angle);
            _delay_ms(10);
        }
        _delay_ms(500);
    }
}