/*
 * Copyright (c) 2009, Jan Bramkamp
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY Jan Bramkamp ''AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Jan Bramkamp BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <avr/interrupt.h>
#include <avr/io.h>
#include <util/atomic.h>
#include <inttypes.h>

#define ever (;;)

volatile	 uint16_t cycles   = 0;
volatile     uint8_t  servo    = 0x01;
volatile	 uint8_t  idx      = 0;
static const uint16_t slot_len = 2400;

static const uint16_t range_min = 1000;
static const uint16_t range_max = 2000;
static const uint16_t delay     = 1*8;

volatile      int16_t delta[8] =
{
	1, 1, 1, 1,
	1, 1, 1, 1
};

volatile     uint16_t servo_pos[8] = 
{
	1500, 1500, 1500, 1500,
	1500, 1500, 1500, 1500
};


ISR(TIMER1_COMPA_vect) // Timer/Counter1 Compare Match A
{
	PORTB &= ~(servo);						// Disable current servo,
	servo  = (servo << 1) | (servo >> 7);	// rotate bitmask
	idx    = (idx + 1) & 7;					// and increment index.
}

ISR(TIMER1_COMPB_vect) // Timer/Counter1 Compare Match B
{
	PORTB |= servo;				// Activate current servo,
	OCR1A  = servo_pos[idx];	// load next pwm duty cycle
	TCNT1  = 0x0000;			// and reset counter;
	cycles++;
}

inline static void init_portb
( ) {
	PORTB = 0x01;	// Port_B[0]    = high, Port_B[1..7] = low
	DDRB  = 0xFF;	// Port_B[0..7] = output
}

inline static void init_timer1
( ) {
	// Enable both Timer1 Compare Match Interrupts.
	TIMSK = (1 << OCIE1A) | (1 << OCIE1B);
	
	// Start with first servo;
	OCR1A  = servo_pos[idx];
	
	// All servo slots have the same length;
	OCR1B  = slot_len;
	
	// Timer0/1 Prescaler = F_CPU / 1
	TCCR1B = (1 << CS10);	
}

int main (void)
{
	init_portb();
	init_timer1();
	
	sei(); // Enable Interrupts.
	
	for ever
	{
		uint16_t cycles_snapshot;
		
		ATOMIC_BLOCK(ATOMIC_FORCEON)
		{
			cycles_snapshot = cycles;
		}
		
		if ( cycles_snapshot > delay )
		{
			uint8_t i;
			
			ATOMIC_BLOCK(ATOMIC_FORCEON)
			{
				cycles = 0x0000;
			}
			
			for ( i = 0; i < 8; i++ )
			{
				uint16_t pos = servo_pos[i];
				
				if ( ( ( pos + delta[i] ) > range_max ) |
				     ( ( pos + delta[i] ) < range_min ) ) ATOMIC_BLOCK(ATOMIC_FORCEON)
				{
					delta[i] = -delta[i];
				}
				else ATOMIC_BLOCK(ATOMIC_FORCEON)
				{
					servo_pos[i] = pos + delta[i];
				}
			}
		}
	}
}

//20ms / 8 = 2.5ms