U32 min_frequency = 10000;
U32 max_frequency = 70000;
U32 frequency;

boolean locked = FALSE;
boolean interrupted = FALSE;

U16 time_stamp_limit = 30;  // allowable phase difference, must be set to reality value
U16 time_stamp = time_stamp_limit + 1;


void
find_resonance()
{
	for ( frequency = min_frequency; frequency < max_frequency; frequency++ )
	{
		set_timer_output_frequency( frequency );
		PT_WAIT_UNTIL(pt, interrupted);	

		if ( time_stamp < time_stamp_limit )
		{
			locked = TRUE;
			exitloop;
		}
		interrupted = FALSE;		
	}
}


void
keep_resonance()
{
	PT_WAIT_UNTIL(pt, interrupted);	
	interrupted = FALSE;

	if ( time_stamp > time_stamp_limit )
	{
		// calculate new frequency;
		set_timer_output_frequency( frequency );
	}
	else
	if ( ( timer_top_value - time_stamp ) > time_stamp_limit ) 
	{
		// calculate new frequency;
		set_timer_output_frequency( frequency );
	}
}


input_compare_interrupt()
{
	time_stamp = input_capture_register;
	interrupted = TRUE;
}


timer_overflow_interrupt()
{
	interrupted = TRUE;
}

int
main()
{
	find_resonance();

	run_at_regular_intervals( keep_resonance );
}