#define PIN_SERVO_STEER 10 // pin the steering servo is on
#define PIN_SERVO_SPEED 11 // pin the throttle servo is on

#define STEER_LEFT   1200 // ms timing for full left
#define STEER_CENTER 1500 // ms timing for straight
#define STEER_RIGHT  1850 // ms timing for full right
//#define SPEED_FORWARD  1200 // ms timing for full speed ahead
//#define SPEED_STOP     1500 // ms timing for stop
//#define SPEED_BACKWARD 1850 // ms timing for full speed reverse

#define D Serial.print

char timer_state; // the state the timer is in, 0=new, 1=servo 1, 2=servo 
int time_steer = STEER_CENTER;



void setup() {
	beginSerial(9600);

	setup_servos();
}

int mode = 0;
void loop() {
	// steer 0
	// steer left
	// steer 0
	// steer right
	// steer 0
	// throttle 0 -> full
	// throttle full -> 0
	// throttle 0 -> reverse
	// throttle reverse -> 0
	char text[20];
	switch (mode) {
		case 0: strcpy(text, "Steer Center");    setSteer(0);    break;
		case 1: strcpy(text, "Steer Left");      setSteer(-100); break;
		case 2: strcpy(text, "Steer Center");    setSteer(0);    break;
		case 3: strcpy(text, "Steer Right");     setSteer(100);  break;
		case 4: strcpy(text, "Steer Center");    setSteer(0);    break;
		case 5: strcpy(text, "Steer Left 50%");  setSteer(-50);  break;
		case 6: strcpy(text, "Steer Center");    setSteer(0);    break;
		case 7: strcpy(text, "Steer Right 50%"); setSteer(50);   break;
	}

	D("mode=");D(mode);D(": ");D(text);D(";\n");
	
	mode++; // set the next mode
	if (mode > 7) { mode = 0; } // reset loop

	delay(1000);
}



void setSteer(int pct) { // -100 to 100
	if (pct < -100) { pct = -100; }
	if (pct > 100)  { pct = 100;  }
	
	if (pct == 0) { // center
		time_steer = STEER_CENTER;
	} else if (pct < 0) { // left
		time_steer = STEER_CENTER - (STEER_CENTER - STEER_LEFT) * float(-float(pct)/100);
	} else { // right
		time_steer = STEER_CENTER + (STEER_RIGHT - STEER_CENTER) * float(float(pct)/100);
	}
}




void setup_servos() {
	pinMode(PIN_SERVO_STEER, OUTPUT); // have to set this to get 5V out, otherwise it's 1.3V
	pinMode(PIN_SERVO_SPEED, OUTPUT);
	
	time_steer = STEER_CENTER;
	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_SERVO_STEER, HIGH);
		set_timer(time_steer);

	} else if (timer_state == 1) {
		// ending longest timer
		digitalWrite(PIN_SERVO_STEER, LOW); // end pulse
		set_timer(2040-time_steer);
	} 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++;
}