// physical stuff
#define NUM_SENSORS 5 // number of sensors connected
#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;

// behavior options
#define IR_AVG_RA_SIZE 8 // # of elements to use in the average IR value array.  increase this to smooth out sensors more

#define D Serial.print

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




void setup() {
	beginSerial(9600);
	setup_sensors();
}

void loop() {
	readsensors();

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








//////////////////////////////////////////////////////////
///// 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::IR:"); Serial.print(ir0.dist_mm); Serial.print(":"); Serial.print(ir1.dist_mm); Serial.print(":"); Serial.print(ir2.dist_mm); Serial.print(":"); Serial.print(ir3.dist_mm); Serial.print(":"); Serial.print(ir4.dist_mm); Serial.print(";\n");
//	Serial.print("S::IR2:"); Serial.print(angle_udegrees); Serial.print(":"); Serial.print(min_dist_mm); Serial.print(";\n");
	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;
	}
}