#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
#define MAX_PULSE_WIDTH 2400

int servo1Pin = 11;
int servo2Pin = 3;

/// @brief Fixes the value within a range.
// Keeps it at the extreme if the value goes out of the 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 the input range to the output range
int map(int val, int in_low, int in_high, int out_low, int out_high)
{
    val = clip(val, in_low, in_high);
    return out_low + (val - in_low) * (out_high - out_low) / (in_high - in_low);
}

void servo_write_us(int pin, int micro_secs)
{
    // Convert microseconds to timer ticks (based on clock and prescaler)
    int ticks = (micro_secs * (CLOCK_FREQ / 1000000)) / PRESCALER;

    if (pin == 11)
    {
        OCR1A = clip(ticks, MIN_PULSE_WIDTH * TICKS_PER_SEC / 1000000, MAX_PULSE_WIDTH * TICKS_PER_SEC / 1000000);
    }
    else if (pin == 3)
    {
        OCR2A = clip(ticks, MIN_PULSE_WIDTH * TICKS_PER_SEC / 1000000, MAX_PULSE_WIDTH * TICKS_PER_SEC / 1000000);
    }
}

void servo_write_angle(int pin, int angle)
{
    int pulse_width = map(angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
    servo_write_us(pin, pulse_width);
}

int main()
{
    // TASK: Set the PRESCALER for Timer2 to 256
    TCCR1B |= (1 << CS12); // Timer1 prescaler 256
    TCCR2B |= (1 << CS22) | (1 << CS21); // Timer2 prescaler 256

    // TASK: Set Wave Generation mode to Phase correct PWM
    TCCR1A |= (1 << WGM10);
    TCCR1B &= ~(1 << WGM12); // Clear WGM bits for phase-correct PWM in Timer1
    TCCR2A |= (1 << WGM20);
    TCCR2B &= ~(1 << WGM22); // Clear WGM bits for phase-correct PWM in Timer2

    // TASK: Set pin modes for D11 and D3
    DDRB |= (1 << PB3); // Set D11 (PB3) as output
    DDRD |= (1 << PD3); // Set D3 (PD3) as output

    // Driver code
    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 angle
        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
        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);
    };
}