#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/wdt.h>
#include <math.h>

#define true 1
#define false 0

#define RED		(1<<PD0)
#define GREEN	(1<<PD1)
#define BLUE	(1<<PD2)

uint16_t counter;
uint8_t running;

uint16_t r, g, b;
uint16_t nr, ng, nb, nh;

uint16_t h;
uint16_t s;
/*const uint16_t table[] PROGMEM = {0,0,1,2,2,3,3,4,5,5,6,6,7,7,8,9,9,10,10,11,12,12,13,13,14,15,15,16,16,17,17,18,19,19,20,20,21,22,22,23,23,24,24,25,26,26,27,27,28,29,29,30,30,31,32,32,33,33,34,34,35,36,36,37,37,38,39,39,40,40,41,41,42,43,43,44,44,45,46,46,47,47,48,48,49,50,50,51,51,52,53,53,54,54,55,55,56,57,57,58,58,59,60,60,61,61,62,62,63,64,64,65,65,66,66,67,68,68,69,69,70,71,71,72,72,73,73,74,75,75,76,76,77,78,78,79,79,80,80,81,82,82,83,83,84,84,85,86,86,87,87,88,89,89,90,90,91,91,92,93,93,94,94,95,96,96,97,97,98,98,99,100,100,101,101,102,102,103,104,104,105,105,106,107,107,108,108,109,109,110,111,111,112,112,113,113,114,115,115,116,116,117,118,118,119,119,120,120,121,122,122,123,123,124,125,125,126,126,127,127,128,129,129,130,130,131,131,132,133,133,134,134,135,136,136,137,137,138,138,139,140,140,141,141,142,143,143,144,144,145,145,146,147,147,148,148,149,149,150,151,151,152,152,153,154,154,155,155,156,156,157,158,158,159,159,160,161,161,162,162,163,164,164,165,165,166,166,167,168,168,169,169,170,
                                   171,171,172,172,173,173,174,175,175,176,176,177,178,178,179,179,180,181,181,182,182,183,183,184,185,185,186,186,187,188,188,189,189,190,191,191,192,192,193,194,194,195,195,196,196,197,198,198,199,199,200,201,201,202,202,203,204,204,205,205,206,207,207,208,208,209,210,210,211,211,212,213,213,214,214,215,216,216,217,217,218,219,219,220,220,221,222,222,223,223,224,225,225,226,227,227,228,228,229,230,230,231,231,232,233,233,234,234,235,236,236,237,237,238,239,239,240,241,241,242,242,243,244,244,245,246,246,247,247,248,249,249,250,250,251,252,252,253,254,254,255,255,256,257,257,258,259,259,260,260,261,262,262,263,264,264,265,266,266,267,267,268,269,269,270,271,271,272,273,273,274,274,275,276,276,277,278,278,279,280,280,281,282,282,283,283,284,285,285,286,287,287,288,289,289,290,291,291,292,293,293,294,295,295,296,297,297,298,299,299,300,301,301,302,303,303,304,305,305,306,307,307,308,309,309,310,311,311,312,313,313,314,315,315,316,317,318,318,319,320,320,321,322,322,323,324,324,325,
								   326,327,327,328,329,329,330,331,331,332,333,334,334,335,336,336,337,338,339,339,340,341,341,342,343,344,344,345,346,347,347,348,349,349,350,351,352,352,353,354,355,355,356,357,358,358,359,360,361,361,362,363,364,364,365,366,367,367,368,369,370,371,371,372,373,374,374,375,376,377,378,378,379,380,381,381,382,383,384,385,385,386,387,388,389,389,390,391,392,393,393,394,395,396,397,398,398,399,400,401,402,403,403,404,405,406,407,408,408,409,410,411,412,413,413,414,415,416,417,418,419,420,420,421,422,423,424,425,426,427,427,428,429,430,431,432,433,434,435,436,436,437,438,439,440,441,442,443,444,445,446,447,447,448,449,450,451,452,453,454,455,456,457,458,459,460,461,462,463,464,465,466,467,468,469,470,471,472,473,474,475,476,477,478,479,480,481,482,483,484,485,486,487,488,489,490,491,492,493,494,495,497,498,499,500,501,502,503,504,505,506,507,509,510,511,512,513,514,515,516,517,519,520,521,522,523,524,526,527,528,529,530,531,533,534,535,536,537,538,540,541,542,543,545,546,547,548,549,
								   551,552,553,554,556,557,558,559,561,562,563,565,566,567,568,570,571,572,574,575,576,578,579,580,582,583,584,586,587,588,590,591,593,594,595,597,598,600,601,602,604,605,607,608,610,611,612,614,615,617,618,620,621,623,624,626,627,629,630,632,633,635,636,638,640,641,643,644,646,647,649,651,652,654,655,657,659,660,662,664,665,667,669,670,672,674,675,677,679,680,682,684,686,687,689,691,693,694,696,698,700,701,703,705,707,709,711,712,714,716,718,720,722,724,725,727,729,731,733,735,737,739,741,743,745,747,749,751,753,755,757,759,761,763,765,767,769,771,773,775,777,780,782,784,786,788,790,792,795,797,799,801,803,806,808,810,812,815,817,819,822,824,826,829,831,833,836,838,840,843,845,848,850,852,855,857,860,862,865,867,870,872,875,877,880,882,885,888,890,893,895,898,901,903,906,909,911,914,917,919,922,925,928,931,933,936,939,942,945,947,950,953,956,959,962,965,968,971,974,977,980,983,986,989,992,995,998,1001,1004,1007,1011,1014,1017,1020,1023};*/

/*const uint16_t table[] PROGMEM = {0,0,0,1,1,2,2,2,3,3,4,4,4,5,5,5,6,6,7,7,7,8,8,9,9,9,10,10,10,11,11,12,12,12,13,13,13,14,14,15,15,15,16,16,17,17,17,18,18,18,19,19,20,20,20,21,21,21,22,22,23,23,23,24,24,24,25,25,26,26,26,27,27,27,28,28,29,29,29,30,30,30,31,31,32,32,32,33,33,33,34,34,35,35,35,36,36,36,37,37,38,38,38,39,39,39,40,40,41,41,41,42,42,42,43,43,43,44,44,45,45,45,46,46,46,47,47,48,48,48,49,49,49,50,50,51,51,51,52,52,52,53,53,54,54,54,55,55,55,56,56,56,57,57,58,58,58,59,59,59,60,60,61,61,61,62,62,62,63,63,63,64,64,65,65,65,66,66,66,67,67,68,68,68,69,69,69,70,70,71,71,71,72,72,72,73,73,73,74,74,75,75,75,76,76,76,77,77,78,78,78,79,79,79,80,80,80,81,81,82,82,82,83,83,83,84,84,85,85,85,86,86,86,87,87,88,88,88,89,89,89,90,90,91,91,91,92,92,92,93,93,94,94,94,95,95,95,96,96,96,97,97,98,98,98,99,99,99,100,100,101,101,101,102,102,103,103,103,104,104,104,105,105,106,106,106,107,107,107,108,108,109,109,109,110,110,110,111,111,112,112,112,113,113,113,114,114,115,115,115,116,116,117,117,117,118,118,118,119,119,120,120,120,
121,121,122,122,122,123,123,124,124,124,125,125,125,126,126,127,127,127,128,128,129,129,129,130,130,131,131,131,132,132,133,133,133,134,134,135,135,135,136,136,137,137,137,138,138,139,139,139,140,140,141,141,141,142,142,143,143,143,144,144,145,145,145,146,146,147,147,148,148,148,149,149,150,150,150,151,151,152,152,152,153,153,154,154,155,155,155,156,156,157,157,158,158,158,159,159,160,160,161,161,161,162,162,163,163,164,164,164,165,165,166,166,167,167,168,168,168,169,169,170,170,171,171,172,172,172,173,173,174,174,175,175,176,176,176,177,177,178,178,179,179,180,180,181,181,182,182,182,183,183,184,184,185,185,186,186,187,187,188,188,189,189,190,190,191,191,192,192,192,193,193,194,194,195,195,196,196,197,197,198,198,199,199,200,200,201,201,202,202,203,203,204,205,205,206,206,207,207,208,208,209,209,210,210,211,211,212,212,213,213,214,215,215,216,216,217,217,218,218,219,219,220,221,221,222,222,223,223,224,224,225,226,226,227,227,228,228,229,230,230,231,231,232,232,233,234,234,235,235,236,237,237,
238,238,239,240,240,241,241,242,243,243,244,245,245,246,246,247,248,248,249,250,250,251,252,252,253,253,254,255,255,256,257,257,258,259,259,260,261,261,262,263,263,264,265,266,266,267,268,268,269,270,270,271,272,273,273,274,275,275,276,277,278,278,279,280,281,281,282,283,284,284,285,286,287,287,288,289,290,290,291,292,293,294,294,295,296,297,297,298,299,300,301,302,302,303,304,305,306,306,307,308,309,310,311,312,312,313,314,315,316,317,318,318,319,320,321,322,323,324,325,326,327,327,328,329,330,331,332,333,334,335,336,337,338,339,340,341,342,343,344,345,346,347,348,349,350,351,352,353,354,355,356,357,358,359,360,361,362,363,364,365,366,367,368,369,371,372,373,374,375,376,377,378,379,381,382,383,384,385,386,387,389,390,391,392,393,395,396,397,398,399,401,402,403,404,406,407,408,409,411,412,413,414,416,417,418,420,421,422,423,425,426,427,429,430,432,433,434,436,437,438,440,441,443,444,445,447,448,450,451,453,454,456,457,459,460,462,463,465,466,468,469,471,472,474,475,477,478,480,482,483,485,486,
488,490,491,493,495,496,498,500,501,503,505,506,508,510,511,513,515,517,518,520,522,524,526,527,529,531,533,535,537,538,540,542,544,546,548,550,552,554,556,557,559,561,563,565,567,569,571,573,575,577,580,582,584,586,588,590,592,594,596,599,601,603,605,607,609,612,614,616,618,621,623,625,627,630,632,634,637,639,641,644,646,649,651,653,656,658,661,663,666,668,671,673,676,678,681,683,686,689,691,694,696,699,702,704,707,710,713,715,718,721,723,726,729,732,735,738,740,743,746,749,752,755,758,761,764,767,770,773,776,779,782,785,788,791,794,798,801,804,807,810,813,817,820,823,827,830,833,837,840,843,847,850,854,857,860,864,867,871,875,878,882,885,889,892,896,900,903,907,911,915,918,922,926,930,934,938,941,945,949,953,957,961,965,969,973,977,981,986,990,994,998,1002,1006,1011,1015,1019,1023};
*/

const uint16_t table[] PROGMEM = {0,1,2,2,3,4,5,5,6,7,8,9,9,10,11,12,13,13,14,15,16,17,17,18,19,20,21,21,22,23,24,25,25,26,27,28,28,29,30,31,32,32,33,34,35,36,36,37,38,39,40,40,41,42,43,44,44,45,46,47,48,48,49,50,51,51,52,53,54,55,55,56,57,58,59,59,60,61,62,63,63,64,65,66,67,67,68,69,70,70,71,72,73,74,74,75,76,77,78,78,79,80,81,82,82,83,84,85,86,86,87,88,89,89,90,91,92,93,93,94,95,96,97,97,98,99,100,101,101,102,103,104,104,105,106,107,108,108,109,110,111,112,112,113,114,115,116,116,117,118,119,119,120,121,122,123,123,124,125,126,127,127,128,129,130,131,131,132,133,134,134,135,136,137,138,138,139,140,141,142,142,143,144,145,146,146,147,148,149,150,150,151,152,153,153,154,155,156,157,157,158,159,160,161,161,162,163,164,165,165,166,167,168,168,169,170,171,172,172,173,174,175,176,176,177,178,179,180,180,181,182,183,183,184,185,186,187,187,188,189,190,191,191,192,193,194,195,195,196,197,198,199,199,200,201,202,202,203,204,205,206,206,207,208,209,210,210,211,212,213,214,214,215,216,217,218,218,219,220,
221,221,222,223,224,225,225,226,227,228,229,229,230,231,232,233,233,234,235,236,237,237,238,239,240,240,241,242,243,244,244,245,246,247,248,248,249,250,251,252,252,253,254,255,256,256,257,258,259,260,260,261,262,263,264,264,265,266,267,268,268,269,270,271,272,272,273,274,275,276,276,277,278,279,280,280,281,282,283,284,284,285,286,287,288,288,289,290,291,292,292,293,294,295,296,296,297,298,299,300,300,301,302,303,304,304,305,306,307,308,308,309,310,311,312,312,313,314,315,316,316,317,318,319,320,320,321,322,323,324,324,325,326,327,328,329,329,330,331,332,333,333,334,335,336,337,337,338,339,340,341,341,342,343,344,345,346,346,347,348,349,350,350,351,352,353,354,355,355,356,357,358,359,359,360,361,362,363,363,364,365,366,367,368,368,369,370,371,372,373,373,374,375,376,377,377,378,379,380,381,382,382,383,384,385,386,387,387,388,389,390,391,391,392,393,394,395,396,396,397,398,399,400,401,401,402,403,404,405,406,406,407,408,409,410,411,412,412,413,414,415,416,417,417,418,
419,420,421,422,422,423,424,425,426,427,428,428,429,430,431,432,433,433,434,435,436,437,438,439,439,440,441,442,443,444,445,445,446,447,448,449,450,451,451,452,453,454,455,456,457,457,458,459,460,461,462,463,463,464,465,466,467,468,469,470,470,471,472,473,474,475,476,477,477,478,479,480,481,482,483,484,484,485,486,487,488,489,490,491,492,492,493,494,495,496,497,498,499,500,501,501,502,503,504,505,506,507,508,509,510,510,511,512,513,514,515,516,517,518,519,520,520,521,522,523,524,525,526,527,528,529,530,531,531,532,533,534,535,536,537,538,539,540,541,542,543,544,545,545,546,547,548,549,550,551,552,553,554,555,556,557,558,559,560,561,562,563,563,564,565,566,567,568,569,570,571,572,573,574,575,576,577,578,579,580,581,582,583,584,585,586,587,588,589,590,591,592,593,594,595,596,597,598,599,600,601,602,603,604,605,606,607,608,609,610,611,612,613,614,615,616,617,618,619,620,621,622,623,624,626,627,628,629,630,631,632,633,634,635,636,637,638,639,640,641,642,644,645,646,647,
648,649,650,651,652,653,654,655,657,658,659,660,661,662,663,664,665,666,668,669,670,671,672,673,674,675,677,678,679,680,681,682,683,685,686,687,688,689,690,692,693,694,695,696,697,699,700,701,702,703,704,706,707,708,709,710,712,713,714,715,716,718,719,720,721,723,724,725,726,727,729,730,731,732,734,735,736,737,739,740,741,742,744,745,746,748,749,750,751,753,754,755,757,758,759,761,762,763,764,766,767,768,770,771,772,774,775,776,778,779,781,782,783,785,786,787,789,790,792,793,794,796,797,799,800,801,803,804,806,807,808,810,811,813,814,816,817,819,820,822,823,824,826,827,829,830,832,833,835,836,838,839,841,842,844,846,847,849,850,852,853,855,856,858,860,861,863,864,866,867,869,871,872,874,875,877,879,880,882,884,885,887,889,890,892,894,895,897,899,900,902,904,905,907,909,911,912,914,916,918,919,921,923,925,926,928,930,932,934,935,937,939,941,943,944,946,948,950,952,954,956,957,959,961,963,965,967,969,971,973,975,977,979,981,982,984,986,988,990,992,994,996,998,1000,1002,
1005,1007,1009,1011,1013,1015,1017,1019,1021,1023};


void hsv_to_rgb(void)	
{
 uint16_t R,G,B;			

 uint32_t i,f;
 uint16_t p, q, t;

 if( s == 0 ) 
 	{
    r=1023;										// Auf 8 Bit RGB skalieren, nacher nur noch kopieren
    g=1023;
    b=1023;	
	return;
	}
 
 i = (((uint32_t)h) * 6) & 0xFF0000;
 f = ((((uint32_t)h) * 6) - i)>>8;
 i >>= 16;
 p = 65535 - s * 256;
 q = 65535 - s * f;
 t = 65535 - s * (256 - f);
 
 switch( i )
 	 	{
		case 0:
			R = 65535; G = t; B = p; break;
		case 1:
			R = q; G = 65535; B = p; break;
		case 2:
			R = p; G = 65535; B = t; break;
		case 3:
			R = p; G = q; B = 65535; break;			
		case 4:
			R = t; G = p; B = 65535; break;
		default:								
			R = 65535; G = p; B = q; break;				// case 5:					
		}

  nr=pgm_read_word(&table[R>>6]);
  ng=pgm_read_word(&table[G>>6]);
  nb=pgm_read_word(&table[B>>6]);
}


SIGNAL(TIMER0_OVF_vect) {
	
	if(counter>=1024) {
		counter = 0;
		r = nr;
		g = ng;
		b = nb;
		h += nh;
	} 
		
	if(running == true) {
	
	  if(counter<r)
			PORTD|=RED;
		else
			PORTD&=~RED;
		
		if(counter<g)
			PORTD|=GREEN;
		else
			PORTD&=~GREEN;
		
		if(counter<b)
			PORTD|=BLUE;
		else
			PORTD&=~BLUE;
		
	} else {
		PORTD&=~RED;
		PORTD&=~GREEN;
		PORTD&=~BLUE;
	}
	
	counter++;
}


int	main(void)
{
uint16_t speed;
uint16_t cctr;
uint16_t sat;

	OSCCAL=0xFF;
	TCCR0=1<<CS20;
	TIMSK=1<<TOIE0;

	DDRD = RED | GREEN | BLUE; // outputs
	PORTD = 0;
	
	r = 0;
	g = 0;
	b = 0;
	
	h=0;
	s=128;
	
	running = true;
	
	// set up A/D converter for single shot mode
	ADMUX = 0;
	ADCSRA = (1<<ADEN) | (1<<ADPS1) | (1<<ADPS0);
	
	sei();
	while(1) {
		// Get saturation
		ADMUX = 0;
		ADCSRA |= (1<<ADSC);		// start dummy conversion
		while(ADCSRA & (1<<ADSC));
		sat=0;
		for(cctr=0;cctr<64;cctr++) {
			ADCSRA |= (1<<ADSC);		// start conversion
			while(ADCSRA & (1<<ADSC));
			sat += ADCW;
		}
		s = (sat >> 6) >> 2;
		if(s>250) s = 255;

		// Get speed
		ADMUX = 1;
		ADCSRA |= (1<<ADSC);		// start dummy conversion
		while(ADCSRA & (1<<ADSC));

		speed=0;
		for(cctr=0;cctr<64;cctr++) {
			ADCSRA |= (1<<ADSC);		// start conversion
			while(ADCSRA & (1<<ADSC));
			speed += ADCW;
		}
		speed >>= 8;
		if(speed<8) running=false; else running=true;
		if(speed > 32) nh=(speed-32)>>1;	// Change division factor for limiting max speed
	
		hsv_to_rgb();
	}
	return 0;
}