// physical stuff
#define PIN_IR_LR 0 // left side, pointing right
#define PIN_IR_LL 1 // left side, pointing left
#define PIN_IR_C  2 // center, facing forward
#define PIN_IR_RR 3 // right side, pointing right
#define PIN_IR_RL 4 // right side, pointing left

// behavior options
#define IR_AVG_RA_SIZE 8 // # of elements to use in the average IR value array

#define D Serial.print

typedef struct {
	int pin_num; // ping number on the arduino board
	int dist_mm; // distance in mm to target
	int ra_val[IR_AVG_RA_SIZE]; // array of last read values
	int ra_val_index; // current index in the array
} ir;

//global vars
// sensors

ir ir_lr;
ir ir_ll;
ir ir_c;
ir ir_rr;
ir ir_rl;

void setup()
{
	beginSerial(9600);

	setup_sensors();
}


void loop()
{
	readsensors();

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








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

	ir_lr.dist_mm = 0;
	ir_ll.dist_mm = 0;
	ir_c.dist_mm  = 0;
	ir_rr.dist_mm = 0;
	ir_rl.dist_mm = 0;

	// inititalize arrays
	for (i = i; i < IR_AVG_RA_SIZE; i++)
	{
		ir_lr.ra_val[i] = 0;
		ir_ll.ra_val[i] = 0;
		ir_c.ra_val[i]  = 0;
		ir_rr.ra_val[i] = 0;
		ir_rl.ra_val[i] = 0;
	}
	ir_lr.ra_val_index = 0;
	ir_ll.ra_val_index = 0;
	ir_c.ra_val_index  = 0;
	ir_rr.ra_val_index = 0;
	ir_rl.ra_val_index = 0;

	ir_lr.pin_num = PIN_IR_LR;
	ir_ll.pin_num = PIN_IR_LL;
	ir_c.pin_num  = PIN_IR_C;
	ir_rr.pin_num = PIN_IR_RR;
	ir_rl.pin_num = PIN_IR_RL;
}


void readsensors()
{
	// read all the sensors and store them in global vars
	// perform smoothing and bad data rejection
		
	ir_lr.dist_mm = getSharp2Y0A021YK(ir_lr.pin_num);
	ir_ll.dist_mm = getSharp2Y0A021YK(ir_ll.pin_num);
	ir_c.dist_mm  = getSharp2Y0A021YK(ir_c.pin_num);
	ir_rr.dist_mm = getSharp2Y0A021YK(ir_rr.pin_num);
	ir_rl.dist_mm = getSharp2Y0A021YK(ir_rl.pin_num);

	ir_lr.dist_mm = ir_tune_result(ir_lr.ra_val, &ir_lr.ra_val_index, ir_lr.dist_mm);
	ir_ll.dist_mm = ir_tune_result(ir_ll.ra_val, &ir_ll.ra_val_index, ir_ll.dist_mm);
	ir_c.dist_mm  = ir_tune_result(ir_c.ra_val,  &ir_c.ra_val_index,  ir_c.dist_mm);
	ir_rr.dist_mm = ir_tune_result(ir_rr.ra_val, &ir_rr.ra_val_index, ir_rr.dist_mm);
	ir_rl.dist_mm = ir_tune_result(ir_rl.ra_val, &ir_rl.ra_val_index, ir_rl.dist_mm);

	int angle_udegrees = 0; // angle the wall on the right makes to the bot

	Serial.print("S::IR:"); Serial.print(ir_lr.dist_mm); Serial.print(":"); Serial.print(ir_ll.dist_mm); Serial.print(":"); Serial.print(ir_c.dist_mm); Serial.print(":"); Serial.print(ir_rr.dist_mm); Serial.print(":"); Serial.print(ir_rl.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;
}