// physical stuff
#define NUM_SENSORS 5 // number of sensors connected
#define IR_AVG_RA_SIZE 8 // # of elements to use in the average IR value array.  increase this to smooth out sensors more
#define PIN_IR0 0 // 90 degrees - straight right
#define PIN_IR1 1 // 67
#define PIN_IR2 2 // 45
#define PIN_IR3 3 // 23
#define PIN_IR4 4 // 0 - straight ahead
#define ANGLE_IR0 90
#define ANGLE_IR1 67.8
#define ANGLE_IR2 41.8
#define ANGLE_IR3 20.2
#define ANGLE_IR4 0
#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   1000 // ms timing for full left
#define STEER_CENTER 1500 // ms timing for straight
#define STEER_RIGHT  2000 // ms timing for full right

#define D Serial.print

// Sensors
typedef struct {
	int pin_num; // ping number on the arduino board
	int dist_mm; // distance in mm to target
	int angle;   // angle the sensor is mounted at
	int end_x;   // calculated x distance in mm
	int end_y;   // calculated y distance in mm
	int ra_val[IR_AVG_RA_SIZE]; // array of last read values
	int ra_val_index; // current index in the array
} struct_ir;

struct_ir ir[NUM_SENSORS]; // main sensors
long angle_udegrees; // final calculated angle of the wall
int min_dist_mm; // closest distance

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

int temp_led_count; // heartbeat led




void setup() {
//	beginSerial(9600);
	temp_led_count = 0;
	pinMode(13, OUTPUT);
	digitalWrite(13, HIGH); // end pulse
	setup_sensors();
	setup_servos();
}

void loop() {
	readsensors(); // read the sensors
	behavior(); // decide what to do
//	sendservos(); // set the servos

	// AD = 100us delay
//	delay(250); // 250ms=4Hz
	delay(2);
}





void behavior() {
	// global angle_udegrees
	//  - flat wall ahead = 0
	//  - parallel wall = 90
	//  - wall on the left = - numbers
	//  - wall facing away = over 90
	// global min_dist_mm

	int pct; // -100 to 100 (L to R)

	// if parallel, don't change
	if (angle_udegrees == 90000) {
		pct = 0; // center
	
	
	// if wall goes away (> 90), turn right
	} else if (angle_udegrees > 90000) {
		if (angle_udegrees > 120000) {
			pct = 100;
		} else {
			pct = (angle_udegrees - 90000) / 300;
		}

	// if wall cuts us off (< 90), turn left
	} else {
		if (angle_udegrees < 60000) {
			pct = -100;
		} else {
			pct = -(90000 - angle_udegrees) / 300;
		}

	}

//D("angle_udegrees=");D(angle_udegrees);D("; min_dist_mm=");D(min_dist_mm);D("; pct=");D(pct);D(";\n");


	setSteer(pct);
}





//////////////////////////////////////////////////////////
///// Sensors
//////////////////////////////////////////////////////////
void setup_sensors() {
	int i, j;

	angle_udegrees = 0;
	min_dist_mm = 0;

	// inititalize arrays
	for (i = 0; i < NUM_SENSORS; i++) {
		ir[i].dist_mm = 0;
		ir[i].ra_val_index = 0;
		for (j = i; j < IR_AVG_RA_SIZE; j++) {
			ir[i].ra_val[j] = 0;
		}
	}

	ir[0].pin_num = PIN_IR0;
	ir[1].pin_num = PIN_IR1;
	ir[2].pin_num = PIN_IR2;
	ir[3].pin_num = PIN_IR3;
	ir[4].pin_num = PIN_IR4;

	ir[0].angle = ANGLE_IR0;
	ir[1].angle = ANGLE_IR1;
	ir[2].angle = ANGLE_IR2;
	ir[3].angle = ANGLE_IR3;
	ir[4].angle = ANGLE_IR4;
}


void readsensors() {
	// read all the sensors and store them in global vars
	// perform smoothing and bad data rejection
	int i;
		
	// read the sensors
	for (i = 0; i < NUM_SENSORS; i++) {
		ir[i].dist_mm = getSharp2Y0A021YK(ir[i].pin_num);
		ir[i].dist_mm = ir_tune_result(ir[i].ra_val, &ir[i].ra_val_index, ir[i].dist_mm);
	}
	calculate_wall(); // main wall calculation

//	Serial.print("S::IR3:");for (i = 0; i < NUM_SENSORS; i++) {Serial.print(ir[i].dist_mm); Serial.print(":");}Serial.print(":");Serial.print(angle_udegrees); Serial.print(":"); Serial.print(min_dist_mm); Serial.print(";\n");
}




int ir_tune_result(int *ra, int *ra_index, int new_val) {
	int i;

//	D("tune_result(): r_index="); D(*ra_index); D(";\n");
	// add it to the array
	ra[*ra_index] = new_val; // save val
	(*ra_index)++;
	if (*ra_index >= IR_AVG_RA_SIZE) { // reset position if looped
		*ra_index = 0;
	}

//	D("tune_result(): r_index="); D(*ra_index); D(";\n");
//	D("tune_result(): ra={ "); for (i = 0; i < IR_AVG_RA_SIZE; i++) { D(ra[i]); D(","); } D(" }\n");

	// find the average of the middle in the array, throw out the highest and the lowest, if multiple bad values in last 5 reads then it'll be a bit off still
	int min = 9999, max = 0;
	int i_min, i_max;
	for (i = 0; i < IR_AVG_RA_SIZE; i++) {
		if (ra[i] < min) {
			i_min = i;
			min = ra[i];
		}
		if (ra[i] > max) {
			i_max = i;
			max = ra[i];
		}
	}
	if (i_min == i_max) { // all values are the same
		i_min = 0; // just throw out the first two
		i_max = 1;
	}
//	D("tune_result(): min=");D(min);D("; max=");D(max);D("; i_min=");D(i_min);D("; i_max=");D(i_max);D(";\n");
	int sum = 0;
	for (i = 0; i < IR_AVG_RA_SIZE; i++) {
		if (i != i_min && i != i_max) {
			sum += ra[i];
		}
	}
	int avg = sum / (IR_AVG_RA_SIZE-2);
//	D("tune_result(): ra={ "); for (i = 0; i < IR_AVG_RA_SIZE; i++) { D(ra[i]); D(","); } D(" } sum=");D(sum);D("; avg=");D(avg);D(";\n");
	return avg;
}




unsigned int getSharp2Y0A021YK(int pin) { // in mm
	// read Sharp 2Y0A21 sensor on analog pin "pin"
	// Sharp IR Sensor: GP2Y0A02YK, marked 2Y0A21 on the unit
	// non-linear output
	// try a add 10uf cap on sensor lines - http://blog.withrona.com/entry/Alice-in-Wonderlandprogress4
	int dist_mm = 0;

	int voltage_mv = analogRead(pin) / 1023.0 * 5000; // 0v = 0, 1024 = 5v

	// convert voltage to non-linear dist_mm in meters
	dist_mm = 1085534.81 * (float)pow((float)voltage_mv, -1.2);

	// some limits
	if (dist_mm < 100) { // close limit
		dist_mm = 100;
	} else if (dist_mm > 800) { // far limit
		dist_mm = 800;
	}

//	D("getSharp(");D(pin);D("): voltage_mv=");D(voltage_mv);D("; dist_mm=");D(dist_mm);D(";\n");
//	D("getSharp(");D(pin);D("): dist_mm=");D(dist_mm);D(";\n");

	return dist_mm;
}





int calculate_wall() {
	// sets the global var for angle and distance (angle_udegrees and min_dist_mm)
	int i;

//		ir[i].dist_mm = ir_tune_result(ir[i].ra_val, &ir[i].ra_val_index, ir[i].dist_mm);
	// calculate the lines
	for (i = 0; i < NUM_SENSORS; i++) {
		// get all the ending points
		if (dist_is_valid(ir[i].dist_mm)) {
			ir[i].end_x = int(ir[i].dist_mm * cos(radians(ir[i].angle)) );
			ir[i].end_y = int(ir[i].dist_mm * sin(radians(ir[i].angle)) );
		}
	}

	// calculate angles in udegrees
	// with sensor looking right
	// flat wall ahead = 0
	// parallel wall = 90
	// wall on the left = - numbers
	// wall facing away = over 90
	long angle_01 = calc_angle(0, 1);
	long angle_02 = calc_angle(0, 2);
	long angle_03 = calc_angle(0, 3);
	long angle_04 = calc_angle(0, 4);
	long angle_12 = calc_angle(1, 2);
	long angle_13 = calc_angle(1, 3);
	long angle_14 = calc_angle(1, 4);
	long angle_23 = calc_angle(2, 3);
	long angle_24 = calc_angle(2, 4);
	long angle_34 = calc_angle(3, 4);


	// find the "best" angle
	long sum = 0;
	int count = 0;
	if (angle_01 != -999999) { sum += angle_01; count++; }
	if (angle_02 != -999999) { sum += angle_02; count++; }
	if (angle_03 != -999999) { sum += angle_03; count++; }
	if (angle_04 != -999999) { sum += angle_04; count++; }
	if (angle_12 != -999999) { sum += angle_12; count++; }
	if (angle_13 != -999999) { sum += angle_13; count++; }
	if (angle_14 != -999999) { sum += angle_14; count++; }
	if (angle_23 != -999999) { sum += angle_13; count++; }
	if (angle_24 != -999999) { sum += angle_14; count++; }
	if (angle_34 != -999999) { sum += angle_23; count++; }
	long angle = sum/count;

	int min_dist = 9999;
	for (i = 0; i < NUM_SENSORS; i++) {
		if (dist_is_valid(ir[i].dist_mm) && ir[i].dist_mm > 0 && ir[i].dist_mm < min_dist) {
			min_dist = ir[i].dist_mm;
		}
	}

	angle_udegrees = angle;
	min_dist_mm = min_dist;
}


long calc_angle(int a, int b) {
	// calculate the angle between two points
	long ret = -999999;
	int diff_x, diff_y;
	
	if (dist_is_valid(ir[a].dist_mm) && dist_is_valid(ir[b].dist_mm)) {
		diff_x = ir[b].end_x-ir[a].end_x;
		diff_y = ir[a].end_y-ir[b].end_y;
		if (diff_x > 0) {
			ret =  1000*long((90-degrees(atan(float(diff_y) / float(diff_x)))));
		} else {
			ret = -1000*long((90+degrees(atan(float(diff_y) / float(diff_x)))));
		}
//D("calc_angle(");D(a);D(",");D(b);D("): diff_x==");D(diff_x);D("; diff_y=");D(diff_y);D("; ret=");D(ret);D(";\n");
	}
	return ret;
}

boolean dist_is_valid(int dist_mm) {
	// is the distance less than the maximum we consider valid?
	int max_dist = 500; // max dist to detect at
	
	if (dist_mm <= max_dist) {
		return true;
	} else {
		return false;
	}
}




















///////////////////////////////
// SERVO FUNCTIONS
///////////////////////////////
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; // 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 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);
	}
//	D("setSteer(");D(pct);D("): time_steer=");D(time_steer);D(";\n");
}


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");
}




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++;

	// led output for a heartbeat
	if (temp_led_count == 100) {
		digitalWrite(13, LOW);
	} else if (temp_led_count >= 200) {
		digitalWrite(13, HIGH);
		temp_led_count = 0;
	}
	temp_led_count++;
}