#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include "ysecTimer2.h"
ysecTimer2 ysectmr2 = ysecTimer2();

// static 
ISR(TIMER2_OVF_vect)
{
	if (!ysectmr2.wrap)
	{
		TCNT2 = ysectmr2.tick0Count; // let's run first the rest
		ysectmr2.tickCount++;
		ysectmr2.wrap=ysectmr2.tickHCount;
		ysectmr2.f();
	}
	else
	{
		TCNT2 = ysectmr2.tcnt2;
		ysectmr2.wrap--;
	}
}



ysecTimer2::ysecTimer2()
{
}

ysecTimer2::~ysecTimer2()
{
}


// ticksInc in usec
void ysecTimer2::set(unsigned long ysecs, void (*callback)(void))
{
	float prescaler = 0.0;

	stop();
	f=callback;

	TCCR2A &= ~((1<<WGM21) | (1<<WGM20));
	TCCR2B &= ~(1<<WGM22);
	ASSR &= ~(1<<AS2);
	TIMSK2 &= ~(1<<OCIE2A);

	if ((F_CPU >= 1000000UL) && (F_CPU <= 16000000UL)) {	// prescaler set to 64
		TCCR2B |= (1<<CS22);
		TCCR2B &= ~((1<<CS21) | (1<<CS20));
		prescaler = 64.0;
	} else if (F_CPU < 1000000UL) {	// prescaler set to 8
		TCCR2B |= (1<<CS21);
		TCCR2B &= ~((1<<CS22) | (1<<CS20));
		prescaler = 8.0;
	} else { // F_CPU > 16Mhz, prescaler set to 128
		TCCR2B |= ((1<<CS22) | (1<<CS20));
		TCCR2B &= ~(1<<CS21);
		prescaler = 128.0;
	}

	float ysecsPerTick=1000000.0/F_CPU*prescaler;
	int totalticks=ysecs/ysecsPerTick;
	int wraps;
	int cnt2;

	if (totalticks < 256)
	{
		wraps=0;
		cnt2=totalticks;
		tick0Count=cnt2;
	}
	else
	{
		cnt2=250;
		wraps=totalticks/cnt2;
		tick0Count=totalticks-cnt2*wraps;
		while (tick0Count<20)
		{
			cnt2-=wraps*(20-tick0Count)/4+1;
			wraps=totalticks/cnt2;
			tick0Count=totalticks-cnt2*wraps;
		}
	}
	tickHCount=wraps;
	char outb[64];
	//sprintf(outb,"wraps=%d tt=%d t0=%d tc2=%d",wraps,totalticks,tick0Count,tcnt2);
	//Serial.println(outb);
	tick0Count=256-tick0Count;
	tcnt2 = 256-cnt2;
}

void ysecTimer2::start() {
	tickCount=0;
	TCNT2 = tcnt2;
	TIMSK2 |= (1<<TOIE2);
}

void ysecTimer2::stop() {
	TIMSK2 &= ~(1<<TOIE2);
}