#define D Serial.print

char timer_state; // the state the timer is in, 0=new, 1=servo 1, 2=servo 
int pin;      // which pin to run first
int val;      // current us delay



void setup() {
	beginSerial(9600);

	pin = 10;
	setup_servos();

	D("Servo calibration ready, ? for help\n");
	D("Current: pin=");D(pin);D("; val=");D(val);D(";\n");
}


void loop() {
	int incomingByte = 0;	// for incoming serial data
	if (Serial.available() > 0) {
		// read the incoming byte:
		incomingByte = Serial.read();
	
		switch (incomingByte) {
			case 'a': val += 1;   break;
			case 'A': val += 100; break;
			case 'z': val -= 1;   break;
			case 'Z': val -= 100; break;
			case '?':
			default:
				D("..Help..\n");
				D("a = up by 1\n");
				D("A = up by 100\n");
				D("z = down by 100\n");
				D("Z = down by 100\n");
				break;		
		}
		D("Current: pin=");D(pin);D("; val=");D(val);D(";\n");
	}
	delay(25);
}





void setup_servos() {
	pinMode(pin, OUTPUT);
	val = 1500; // stop
	timer_state = 0; // initital state

	// set up the timer
	TCCR2A = 0; // timer mode
	TCCR2B = 1<<CS22 | 0<<CS21 | 1<<CS20; // 101 = /1024 prescalar

	// Timer2 Overflow Interrupt Enable
	TIMSK2 = 1<<TOIE2; // enable callback to ISR(TIMER2_OVF_vect)
	
	set_timer(1000);
}




void set_timer(int us) {
	// timer (TCNT2) is an 8-bit register, triggers ISR at 0xff
	// TIMER_CLOCK_FREQ = the freq tcnt2 increases at
	// TIMER_CLOCK_US = microseconds per tick in tcnt2

	// 16MHz model =  /0    = us max = 127us
	//                /8    = us  max = 255us
	//                /64   = us  max = 510us
	//                /256  = us  max = 1020us
	//                /1024 = 8us  max = 2040us
	// time range we need: 1000us to 2000us (plus a bit of a buffer)
	// servo refresh time = 20,000us (20ms)

	int result;
	result = 255 - us/8;
	if (result <= 0) {
		result = 1;
	} else if (result > 255) {
		result = 255;
	}
		
	TCNT2 = result;
//	D("set_timer(");D(us);D("); result=");D((int)result);D("; TCNT2=");D((int)TCNT2);D("\n");
}




//Timer2 overflow interrupt vector handler
ISR(TIMER2_OVF_vect) {
	// timer ISR
//D("timer_state=");D((int)timer_state);D("\n");
	if (timer_state == 0) {
		// all servos start pulse
		digitalWrite(pin, HIGH);
		set_timer(val);

	} else if (timer_state == 1) {
		// ending longest timer
		digitalWrite(pin, LOW); // end pulse
		set_timer(2040-val);
	} else if (timer_state == 2)  { set_timer(2040); // ends at 0+ms
	} else if (timer_state == 3)  { set_timer(2040); // ends at 0+ms
	} else if (timer_state == 4)  { set_timer(2040); // ends at 0+ms
	} else if (timer_state == 5)  { set_timer(2040); // ends at 0+ms
	} else if (timer_state == 6)  { set_timer(2040); // ends at 0+ms
	} else if (timer_state == 7)  { set_timer(2040); // ends at 0+ms
//	} else if (timer_state == 8)  { set_timer(2040); // ends at 0+ms
//	} else if (timer_state == 9)  { set_timer(2040); // ends at 0+ms
//	} else if (timer_state == 10) { set_timer(2040); // ends at 0+ms
//	} else if (timer_state == 11) { set_timer(2040); // ends at 0+ms
//	} else if (timer_state == 12) { set_timer(2040); // ends at 0+ms
	} else {
		timer_state = -1;
	}
	timer_state++;
}