import processing.serial.*;

// definition of window size and framerate
int xWidth  = 900;
int yHeight = 700;
int fr = 24;

// variables for serial connection, portname and baudrate have to be set
Serial port;
String portname = "COM5";
int baudrate = 9600;

int[] dist_mm; // the values
float[] angle; // the angles in degrees each sensor is at
int[] x, y; // positions on screen of the endpoints
int[] start_y;
int top  = 200;
int left = 50;

PFont font;

void setup() {
	dist_mm = new int[5];
	angle = new float[5];
	x = new int[5];
	start_y = new int[5];

	// set size and framerate
	size(xWidth, yHeight); frameRate(fr);

	// establish serial port connection
	port = new Serial(this, portname, baudrate);

	font = loadFont("Monaco-14.vlw");
	
	angle[0] = 90;
	angle[1] = 67.8;
	angle[2] = 41.8;
	angle[3] = 20.2;
	angle[4] = 0;

	start_y[0] = top;
	start_y[1] = top;
	start_y[2] = top;
	start_y[3] = top;
	start_y[4] = top;
}




void draw() { // main loop
	String val;
	
	// listen to serial port and trigger serial event
	while (port.available() > 0) {
		val = port.readStringUntil('\n'); // 10=\n
		serialEvent(val);
	}
	
	drawBackground();
	drawData();
}






void serialEvent(String val) {
	// S::IR:100:150:700:30:56;
	String ra[];

	if (val != null) {
		if (val.length() > 6) {
			if (val.substring(0, 6).equals("S::IR:")) {
				val = val.substring(6); // remove S::IR:
				val = val.substring(0, val.length()-2); // remove ending ;
				ra = val.split(":");
				dist_mm[0] = int(ra[0]);
				dist_mm[1] = int(ra[1]);
				dist_mm[2] = int(ra[2]);
				dist_mm[3] = int(ra[3]);
				dist_mm[4] = int(ra[4]);
			}
		}
	}
}




void drawBackground() {
	background(255,255,255);

	int size = 8;
	int dist = 800; // 800mm max

	noFill();

	stroke(color(0,0,0));
	textFont(font);
	for (int i=0; i < 5; i++) {
		// draw the sensors
		ellipse(left,start_y[i], size,size); // 0

		// text labels
		text(i, left-17,start_y[i]+5);
	}
	
	// draw the rays
	stroke(color(200,200,200));
	line(left,start_y[0], left+dist*cos(radians(angle[0])),start_y[0]+dist*sin(radians(angle[0])));
	line(left,start_y[1], left+dist*cos(radians(angle[1])),start_y[1]+dist*sin(radians(angle[1])));
	line(left,start_y[2], left+dist*cos(radians(angle[2])),start_y[2]+dist*sin(radians(angle[2])));
	line(left,start_y[3], left+dist*cos(radians(angle[3])),start_y[3]+dist*sin(radians(angle[3])));
	line(left,start_y[4], left+dist*cos(radians(angle[4])),start_y[4]+dist*sin(radians(angle[4])));
}






int[] end_x = new int[5];
int[] end_y = new int[5];
void drawData() {
	String text;

	fill(color(255,0,0));
	textFont(font);
	int size = 4;

	// calculate the lines
	for (int i = 0; i < 5; i++) {
		if (dist_is_valid(dist_mm[i])) {
			end_x[i] = left       + int(dist_mm[i] * cos(radians(angle[i])) );
			end_y[i] = start_y[i] + int(dist_mm[i] * sin(radians(angle[i])) );
		}
	}

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



	// draw the lines
	for (int i = 0; i < 5; i++) {
		if (dist_is_valid(dist_mm[i])) {
			stroke(color(255,0,0));
			line(left,start_y[i], end_x[i],end_y[i]); // draw distance lines
			ellipse(end_x[i],end_y[i], size,size); // draw dots

			text(i, end_x[i]-4, end_y[i]-7);

			// draw the text of the distance
			text = str(dist_mm[i])+"mm";
			text(text, end_x[i]-20, end_y[i]+15);		
		}
	}

	
	// show angle calculations
	String s;
	if (angle_01 != -999) { s = "Angle 0,1 = "+str(angle_01)+" degrees"; } else { s = "Angle 0,1 = "; } text(s, 10, 15);
	if (angle_02 != -999) { s = "Angle 0,2 = "+str(angle_02)+" degrees"; } else { s = "Angle 0,2 = "; } text(s, 10, 15*2);
	if (angle_03 != -999) { s = "Angle 0,3 = "+str(angle_03)+" degrees"; } else { s = "Angle 0,3 = "; } text(s, 10, 15*3);
	if (angle_04 != -999) { s = "Angle 0,4 = "+str(angle_04)+" degrees"; } else { s = "Angle 0,4 = "; } text(s, 10, 15*4);
	if (angle_12 != -999) { s = "Angle 1,2 = "+str(angle_12)+" degrees"; } else { s = "Angle 1,2 = "; } text(s, 10, 15*5);
	if (angle_13 != -999) { s = "Angle 1,3 = "+str(angle_13)+" degrees"; } else { s = "Angle 1,3 = "; } text(s, 10, 15*6);
	if (angle_14 != -999) { s = "Angle 1,4 = "+str(angle_14)+" degrees"; } else { s = "Angle 1,4 = "; } text(s, 10, 15*7);
	if (angle_23 != -999) { s = "Angle 2,3 = "+str(angle_23)+" degrees"; } else { s = "Angle 2,3 = "; } text(s, 10, 15*8);
	if (angle_24 != -999) { s = "Angle 2,4 = "+str(angle_24)+" degrees"; } else { s = "Angle 2,4 = "; } text(s, 10, 15*9);
	if (angle_34 != -999) { s = "Angle 3,4 = "+str(angle_34)+" degrees"; } else { s = "Angle 3,4 = "; } text(s, 10, 15*10);


	
	// draw connecting angles
	for (int i = 0; i < 5; i++) {
		if (dist_is_valid(dist_mm[i])) {
			stroke(color(255,128,128));
			for (int j = i+1; j < 5; j++) {
				if (dist_is_valid(dist_mm[j])) {
					line(end_x[i],end_y[i], end_x[j],end_y[j]);
				}
			}
		}
	}


	// find the "best" angle
	int 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++; }
	int angle = int(float(sum)/float(count));

	int min_dist = 9999;
	for (int i = 0; i < 5; i++) {
		if (dist_is_valid(dist_mm[i])) {
			if (dist_mm[i] > 0) { // 0 are invalid too
				if (dist_mm[i] < min_dist) {
					min_dist = dist_mm[i];
				}
			}
		}
	}
	
	text("Overall Angle = "+str(angle)+" degrees @ min "+str(min_dist)+"mm", 10, 15*11);
	


}


int calc_angle(int a, int b) {
	// calculate the angle between two points
	int ret = -99999;
	int diff_x, diff_y;
	
	if (dist_is_valid(dist_mm[a]) && dist_is_valid(dist_mm[b])) {
		diff_x = end_x[b]-end_x[a];
		diff_y = end_y[a]-end_y[b];
		if (diff_x > 0) {
			ret = 90-int(degrees(atan(float(diff_y) / float(diff_x))));
		} else {
			ret = -(90+int(degrees(atan(float(diff_y) / float(diff_x)))));
		}
//print("diff_x="+str(diff_x)+"; diff_y="+str(diff_y)+"; angle_01="+str(angle_01)+"\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;
	}
}

/*
void drawPushButtonState()
{
	// read through the value buffer and shift the values to the left
	for (i = 1; i < num; i++) {
		valBuf[i-1] = valBuf[i];
	}
	
	// add new values to the end of the array
	valBuf[num-1] = actVal;
	noStroke();
	
	// reads through the value buffer and draws lines
	for(int i=0; i < num; i=i+2) {
		fill(int((valBuf[i]*255)/height), int((valBuf[i]*255)/height) , 255);
		rect(0, height-valBuf[i], width, 3);
		fill(int((valBuf[i+1]*255)/height), 255, 0 );
		rect(0, height-valBuf[i+1], width, 3);
	}

	// display value
	fill((0 ? 185 : 75));
	text( ""+(actVal), 96, height-actVal);
}
*/