#include <avr/io.h>
#include <stdint.h>
#include <avr/power.h>
#include <avr/sleep.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>

static void
timer1_init( void )
{
    /* Both outputs are clear on match, and set on top,
     * top is ICR2.
     */
    TCCR1A = 0xa2; // clear
    /* Mode configuration continued,
     * prescalar clock set to divide by 8.
     */
    TCCR1B = (3<<3) | (2);

    /* assuming a 8Mhz system clock,
     * a 0.020 millisecond period is
     * 0.020 / 0.000001 = 20000
     */
    ICR1 = 20000;

    DDRD |= _BV(5) | _BV(4); // enable timer1 pwm outputs
}

static void
timer1_pwm( uint16_t a, uint16_t b )
{
    OCR1A = a;
    OCR1B = b;
}

#define TIMER2_SKIPS (5)
#define PWM_A_MAX (2000)
#define PWM_A_MIN (1000)
#define PWM_B_MAX (2000)
#define PWM_B_MIN (1000)
#define DELTA (25)

static struct {
    uint8_t skips;
    uint16_t pwm_a;
    uint8_t a_dir;
    uint16_t pwm_b;
    uint8_t b_dir;
} timer2_state;


static void
timer2_init( uint8_t clockCnt )
{
    OCR2A = clockCnt;
    // CTC, OCR2A is top, TCNT2 cleared with OCR2A reached
    // prescaler set to 1024
    TCCR2A = _BV(WGM21);
    TCCR2B = _BV(CS22) | _BV(CS21) | _BV(CS20);
    // enable compare A interrupts
    TIMSK2 |= _BV( OCIE2A );

    timer2_state.pwm_a = PWM_A_MAX;
    timer2_state.a_dir = 0;
    timer2_state.pwm_b = PWM_B_MIN;
    timer2_state.b_dir = 1;
}

uint8_t cnt = 0;

ISR(TIMER2_COMPA_vect)
{
    if ( timer2_state.skips != 0 ) {
        timer2_state.skips--;
        return;
    }
    cnt++;
    PORTC = ~cnt;
    timer2_state.skips = TIMER2_SKIPS;
    if ( timer2_state.a_dir ) {
        timer2_state.pwm_a += DELTA;
        if ( timer2_state.pwm_a > PWM_A_MAX ) {
            timer2_state.pwm_a = PWM_A_MAX;
            timer2_state.a_dir = 0;
        }
    }
    else {
        timer2_state.pwm_a -= DELTA;
        if ( timer2_state.pwm_a < PWM_A_MIN ) {
            timer2_state.pwm_a = PWM_A_MIN;
            timer2_state.a_dir = 1;
        }
    }
    if ( timer2_state.b_dir ) {
        timer2_state.pwm_b += DELTA;
        if ( timer2_state.pwm_b > PWM_B_MAX ) {
            timer2_state.pwm_b = PWM_B_MAX;
            timer2_state.b_dir = 0;
        }
    }
    else {
        timer2_state.pwm_b -= DELTA;
        if ( timer2_state.pwm_b < PWM_B_MIN ) {
            timer2_state.pwm_b = PWM_B_MIN;
            timer2_state.b_dir = 1;
        }
    }
    timer1_pwm( timer2_state.pwm_a, timer2_state.pwm_b );
}

int
main( void )
{
    DDRC = 0xff;
    clock_prescale_set( clock_div_1 );

    timer1_init();
    timer2_init( 156 ); // approximately 0.02 seconds for 8MHz 

    sei(); // enable interrupts
    while ( 1 ) {
        sleep_mode();
    }
    return 0;
}