#include <avr/io.h>
#include <sys/cpu.h>
#include <sys/systick.h>

#ifdef __AVR_ATmega32__

#define TCCR0A	TCCR0
#define OCR0A	OCR0
#define TIMSK0	TIMSK

#define OCIE0A	OCIE0

#endif

#define PRESCALER_LEVELS 5
int prescale_shift[] = { 0, 3, 6, 8, 10 };
uint32_t	_st_prescale;

void systick_timer_init(uint32_t systick_us)
{
	uint64_t clocks = ((uint64_t)systick_us) * ((uint64_t)__freq_cpu) / 1000000LL;
	uint64_t ocr;
	uint8_t n_prescaler;
	
	
	for (n_prescaler=0;n_prescaler<PRESCALER_LEVELS;n_prescaler++){
		ocr = clocks >> prescale_shift[ n_prescaler ];
		if (ocr < 256){
			break;
		};
	};
	
	if (n_prescaler < PRESCALER_LEVELS) {
		
		TCCR0A = _BV(WGM01) | ((n_prescaler + 1) & 0x07);
		OCR0A = (uint8_t)ocr;
		TIMSK0 |= _BV(OCIE0A);
	
		_systick_us = 1000000LL * (1LL << prescale_shift[ n_prescaler ]) * (ocr + 1) / __freq_cpu;
		_st_prescale = (1L << prescale_shift[ n_prescaler ]);

#if defined(DEBUG_SCHEDULING)
		_ts_timer_resolution			= ocr;
		_ts_timer_cycles_per_increment	= _st_prescale;
		_ts_timer_increments_per_second	= __freq_cpu / _ts_timer_cycles_per_increment;
#endif

	};
};