#include <avr/interrupt.h>
#include <avr/sleep.h>
#include <avr/io.h>

#define F_CPU 1000000UL  // 1 MHz
#include <util/delay.h>

#define GETBIT( pattern, bit ) ( ( pattern >> bit ) & 1 )
#define SETBIT( pattern, bit ) ( pattern |= ( 1 << bit ) )
#define CLRBIT( pattern, bit ) ( pattern &= ~( 1 << bit ) )
#define FLPBIT( pattern, bit ) ( pattern ^= ( 1 << bit) )

#define TIMER_CLK_DIV8			0x02	///< Timer clocked at F_CPU/8
#define TIMER_CLK_DIV64			0x03

#define TIMER0PRESCALE		TIMER_CLK_DIV8
#define TIMER1PRESCALE		TIMER_CLK_DIV64	
#define TIMER_PRESCALE_MASK		0x07

typedef unsigned char  u08;
typedef unsigned short u16;

typedef void (*funcptr)(void);

struct threaddata {
	u08 enabled;
	u08 id;

	u16 sp;
	u08 sreg;
	u08 pointers[6];
	u08 registers[32];

	funcptr entry;

};

u16 my_threads = 0;
struct threaddata threads[5];
struct threaddata *currthread = threads;



int launchThread ( funcptr entry )
{
	++my_threads;
	threads[my_threads].entry = entry;
	return my_threads;
}

void timer1Init(u08 prescale, u16 topcount)
{
	// set prescaler on timer 1
	TCCR1B = (TCCR1B & ~TIMER_PRESCALE_MASK) | prescale;

	// reset TCNT1
	TCNT1H = 0;
	TCNT1L = 0;

	// enable TCNT1 overflow
	SETBIT(TIMSK, TOIE1);

	// set PWM mode with ICR top-count
	CLRBIT(TCCR1A,WGM10);
	SETBIT(TCCR1A,WGM11);
	SETBIT(TCCR1B,WGM12);
	SETBIT(TCCR1B,WGM13);
	
	// set top count value
	ICR1H = (u08)(topcount>>8);
	ICR1L = (u08)topcount;
}

void launchThread1 ( )
{
	++my_threads;
	//threads[my_threads].entry = entry;
	threads[my_threads].id = 42;
}




void thread1 ()
{
	while( 1 )
	{
		_delay_ms( 100 );
		FLPBIT( PORTC, PC3 );
	}
}

void thread2 ()
{
	while( 1 )
	{
		_delay_ms( 150 );
		FLPBIT( PORTC, PC2 );
	}
}


int main( void )
{
//	SETBIT( DDRC, DDC5 );

	timer1Init( TIMER_CLK_DIV8, 1000);

	threads[0].enabled = 1;

	launchThread1( );
	launchThread( thread1 );
//	launchThread( thread2 );

	timer1Init( TIMER_CLK_DIV8, 500);

	sei();	// enable interrupts

//	sleep_enable();
//	sleep_cpu();;	cli()	// disable interrupts
//	sleep_disable();

	while( 1 ) {
	
		_delay_ms( 100 );
	}
	return 0;
}

//! Interrupt handler for tcnt1 overflow interrupt
SIGNAL(SIG_OVERFLOW1)
{
	if( 0 != currthread )
	{
		currthread->sp = SP;
		currthread->sreg = SREG;

		currthread->entry = threads[0].entry;
//		currthread->registers[0] = r10;
	}

	FLPBIT( PORTC, PC5 );
}