/*
 * 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 (;;)

// each execution of TIMER1_COMPB_vect increments this
// providing the rest of the code with a strictly monotonic
// increasing tick counter.
volatile     uint16_t cycles   = 0;

/*
 * volatile uint8_t reads and writes are always atomic
 * their for don't require access to them
 * to be protected by a ATOMIC_BLOCK.
 */

// bitmask "pointing" to the currently procced pin
volatile     uint8_t  servo    = 0x01;
// index in delta[8] and servo_pos[8] corresponding to this pin
volatile     uint8_t  idx      = 0x00;
// length of each time slot
static const uint16_t slot_len = 2500;

// minimal length of each duty cycle
static const uint16_t range_min =  1000;
// maximal length of each duty cycle
static const uint16_t range_max =  2000;
// how many ticks to wait before updating servo_pos[0..7]
static const uint16_t delay     = 1 * 8;

// how fast to move each servo
volatile      int16_t delta[8] = {
	1, 1, 1, 1, 1, 1, 1, 1
};

// current duty cycle length for each servo.
volatile     uint16_t servo_pos[8] = {
	1500, 1500, 1500, 1500,
	1500, 1500, 1500, 1500
};

// ISR(TIMER1_COMPB_vect) has to be executed before
// ISR(TIMER1_COMPA_vect) in each pwm cycle.
// SEE ALSO: TIMER1_COMPB_vect
ISR( TIMER1_COMPB_vect ) {
	PORTB &= ~(servo);						// Disable current servo
											// and move to next one by
	servo  = (servo << 1) | (servo >> 7);	// rotating the bitmask
	idx    = (idx + 1) & 7;					// and incrementing the index.
}

// ISR(TIMER1_COMPB_vect) has to be executed before
// ISR(TIMER1_COMPA_vect) in each pwm cycle.
// SEE ALSO: TIMER1_COMPA_vect
ISR( TIMER1_COMPA_vect ) {
	PORTB |= servo;				// Activate current servo,
	OCR1B  = servo_pos[idx];	// load pwm duty cycle
	cycles++;
}

ISR( USART_RX_vect ) {
	
}

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;
	OCR1B  = servo_pos[idx];
	
	// All servo slots have the same length;
	OCR1A  = slot_len;
	
	// Timer0/1 Prescaler = F_CPU / 1
	// CTC Mode = Reset Counter1 on interrupt
	TCCR1B = (1 << CS10) | (1 << WGM12);	
}

void main ( void ) {
	init_portb();
	init_timer1();
	
	sei(); // Enable Interrupts.
	
	for ever {
		uint16_t cycles_snapshot;	// we need a copy of the global tick counter
		
		ATOMIC_BLOCK( ATOMIC_FORCEON )
			cycles_snapshot = cycles; // a correct copy
		
		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