// main controller for the installation
// TODO: playback of the recordings, automation, switch from open-loop to closed-loop with the openCV tracker
(
var imageDist, micronsPerStep, automation, imgPositions, curPos, tarPos,
netAddress, serialPort, serialListener,
moveTo, jogControl, jogHorizontal, jogVertical,
imgSelect, imgCalibrate, automate, lastSelect, trackerPos, trackerOffsetBaseDist, trackerOffset;

// init global vars
imageDist = 300; // in microns
micronsPerStep = 0.0977;
automation = false;
imgPositions = 9.collect({nil});
curPos = Point.new(0, 0);
tarPos = Point.new(0, 0);
netAddress = NetAddr.new("127.0.0.1", 8080);
lastSelect = -1;
trackerOffsetBaseDist = 16000;
trackerOffset = [
	[trackerOffsetBaseDist * 1, trackerOffsetBaseDist * 1],
	[trackerOffsetBaseDist * 0, trackerOffsetBaseDist * 1],
	[trackerOffsetBaseDist * -1, trackerOffsetBaseDist * 1],

	[trackerOffsetBaseDist * 1, trackerOffsetBaseDist * 0],
	[trackerOffsetBaseDist * 0, trackerOffsetBaseDist * 0],
	[trackerOffsetBaseDist * -1, trackerOffsetBaseDist * 0],

	[trackerOffsetBaseDist * 1, trackerOffsetBaseDist * -1],
	[trackerOffsetBaseDist * 0, trackerOffsetBaseDist * -1],
	[trackerOffsetBaseDist * -1, trackerOffsetBaseDist * -1]
];


~serialPort = SerialPort("/dev/ttyACM0", baudrate: 115200, crtscts: true);

// recieve motor feedback
~serialListener = Routine({
	var byte, str, res, valArray,
	stepper, limitSwitchNeg, limitSwitchPos, safeMode, limitPos;

	safeMode = false;

	loop{
		byte = ~serialPort.read;
		if(byte==13, {
			if(str[1].asString == "[", {
				valArray = str.asString.interpret.postln;
				curPos = Point.new(valArray[0], valArray[1]);
				limitSwitchNeg = valArray[2];
				limitSwitchPos = valArray[3];
				if(safeMode && (limitSwitchNeg == limitSwitchPos), {
					safeMode = false;
					fork {
						netAddress.sendMsg("/STATE/SET", "{message: \"all clear\"}");
						2.wait;
						netAddress.sendMsg("/STATE/SET", "{message: \"\"}");
					}
				});
				if(automation, {
					var centerPos = nil, dist = 0;
					if(lastSelect != 0, {
						centerPos = imgPositions[lastSelect].deepCopy;
						dist = 300;
					}, {
						centerPos = imgPositions[4].deepCopy;
						dist = imageDist / micronsPerStep;
					});
					if((curPos.x - tarPos.x).abs < 100, {tarPos.x = centerPos.x + dist.rand2});
					if((curPos.y - tarPos.y).abs < 100, {tarPos.y = centerPos.y + dist.rand2});
					moveTo.value(tarPos);
				});
			}, {
				if(str[1..3].asString == "!!!", {
					netAddress.sendMsg("/STATE/SET", "{message: \"!!! limit switch still on after 1000 steps, this should NEVER happen\"}");
				}, {
					automation = false;
					safeMode = true;
					netAddress.sendMsg("/STATE/SET", "{message: \"!! limit hit, move the other direction\"}");
				});
			});
			str = "";
		}, {str = str++byte.asAscii});
	};
}).play(AppClock);

// send new coordinates to the arduino / motors
moveTo = {arg point;
	if((point.x.abs < 8000) && (curPos.y.abs < 8000), {
		"move to".postln;
		point.postln;
		~serialPort.putAll(point.x.asInteger.asString ++ " " ++ point.y.asInteger.asString);
		~serialPort.put(10);
	});
};

jogControl = {arg axis;
	var jog, count = 0, jogRate= 0, jogDirection = 1;
	jog = Task({
		loop{
			count = (count + 0.01).clip(0, 1);
			count.postln;
			jogRate = pow(count, 2) * 500;
			if(axis == '/jog_horizontal', {
				tarPos.x = curPos.x + (jogRate * jogDirection * -1);
			}, {
				tarPos.y = curPos.y + (jogRate * jogDirection);
			});
			curPos.postln;
			moveTo.value(tarPos);
			0.1.wait
		};
	});
	OSCFunc({arg msg;
		//tarPos.x = curPos.x + (1000 * msg[1]);
		//moveTo.value(tarPos);
		msg.postln;
		if(msg[1] == 0, {count = 0; jogRate = 0; jog.pause()}, {jogDirection = msg[1]; jog.play(AppClock)});
		automation = false;
		//netAddress.sendMsg("/STATE/SET", "{automate: 0}");
	}, axis, netAddress);
};

jogHorizontal = jogControl.value('/jog_horizontal');
jogVertical = jogControl.value('/jog_vertical');

imgSelect = {
	//var lastSelect = nil;
	OSCFunc({arg msg;
		var imgIndex;
		msg.postln;
		if((msg[1] > 0), {
			imgIndex = msg[1] - 1;
			imgIndex.postln;
			if(imgPositions[imgIndex] != nil, {tarPos = imgPositions[imgIndex].deepCopy; moveTo.value(tarPos)});
			9.do({arg i; if(imgIndex != i, {
				netAddress.sendMsg("/STATE/SET", "{img_" ++ (i + 1).asString ++ "_select: " ++ (i + 1).neg ++ "}")})});
			automation = false;
			//netAddress.sendMsg("/STATE/SET", "{automate: 0}");
			lastSelect = imgIndex;
		}, {
			lastSelect.postln;
			if(msg[1] == ((lastSelect + 1) * -1), {"here".postln; lastSelect = -1})
			//lastSelect = 0;
			/*
			imgIndex = msg[1].neg - 1;
			if(imgIndex == lastSelect, {
				if(imgPositions[imgIndex] != nil, {tarPos = imgPositions[imgIndex].deepCopy; moveTo.value(tarPos)});
				netAddress.sendMsg("/STATE/SET", "{img_" ++ (imgIndex + 1).asInteger.asString ++ "_select: " ++ (imgIndex + 1) ++ "}")});
			*/
		});
	}, '/img_select', netAddress)
}.value;

imgCalibrate = {
	var calibrateHold, imgIndex, setPos;
	calibrateHold = Routine({
		var imageCalibrationIndex = 0;
		20.do({0.1.wait});
		//imgPositions[imgIndex] = setPos.deepCopy;
		[-1, 0, 1].do({arg row;
			[-1, 0, 1].do({arg col;
				imgPositions[imageCalibrationIndex] = setPos.deepCopy + Point.new(6550 * col, 6550 * row);
				imgPositions[imageCalibrationIndex].postln;
				imageCalibrationIndex = imageCalibrationIndex + 1;
			});
		});

		imgPositions.postln;
		netAddress.sendMsg("/STATE/SET", "{message: \"image calibrated\"}");
	});

	OSCFunc({ arg msg;
		imgIndex = msg[1] - 1;
		if(imgIndex >= 0, {
			setPos = curPos.deepCopy;
			calibrateHold.play(AppClock);
		}, {
			calibrateHold.stop; calibrateHold.reset; netAddress.sendMsg("/STATE/SET", "{message: \"\"}");
		});
	}, '/img_calibrate', netAddress);
}.value;

automate = OSCFunc({arg msg;
	msg.postln;
	if(msg[1] == 1, {
		automation = true;
	}, {
		automation = false;
		tarPos = curPos.deepCopy;
		moveTo.value(tarPos);
	});
	9.do({arg i; netAddress.sendMsg("/STATE/SET", "{img_" ++ (i + 1).asString ++ "_select: " ++ (i + 1).neg ++ "}")});
}, '/automate', netAddress);

trackerPos = OSCFunc({arg msg;
	//imgPositions.postln[lastSelect.postln];
	if(lastSelect.postln > -1, {
		var distX, distY, isFocusing;
		distX = curPos.x - imgPositions[lastSelect].x;
		distY = curPos.y - imgPositions[lastSelect].y;
		isFocusing = false;
		if((imgPositions[lastSelect] != nil) && (distX.abs < 300) && ((msg[1] - trackerOffset[lastSelect][0]).abs > 3000), {
			tarPos.x = curPos.x + (10 * (msg[1] - trackerOffset[lastSelect][0]).sign);// * -1);
			isFocusing = true;
		});
		if((imgPositions[lastSelect] != nil) && (distY.abs < 300) && ((msg[2] - trackerOffset[lastSelect][1]).abs > 3000), {
			tarPos.y = curPos.y + (10 * (msg[2] - trackerOffset[lastSelect][1]).sign);// * -1);
			isFocusing = true;
		});
		if(isFocusing, {moveTo.value(tarPos)});
	})
	//msg.postln;
}, '/trackerpos');
)

//in case of emergency
~serialPort.close
~serialPort = SerialPort.new("/dev/ttyACM0", baudrate: 115200, crtscts: true);
~serialListener.reset
~serialListener.play(AppClock);

//This is legacy code - should be removed soonish
(
// TODO:
// set position to 0
// limit switch warnings
// More clean up and testing
var imageDist, rotation, micronsPerStep, curPos, tarPos, automate, imagePositions,
serialPort, serialListener, moveTo,
window, xOffset, yOffset,
userView, imageButtonRects,
dirKeyBlockTasks, jogTasks, jogRates,
moveButtons, curPosFields, tarPosFields,
calibrationSteps, wizardButtons, wizMoveBlock, curWizardStep, curWizardText;

// init global vars
imageDist = 25; // in microns
rotation = 0;    // in degrees
micronsPerStep = 0.0977;
curPos = Point.new(0, 0);
tarPos = Point.new(0, 0);
automate = false;
imagePositions = 3.collect({arg r; 3.collect({arg c; Point(imageDist * (c - 1), imageDist * (r - 1))})}).reverse.flat;

// connect to arduino
serialPort = SerialPort(
	"/dev/ttyACM0",    //edit to match the port (SerialPort.listDevice)
	baudrate: 115200,  //check that baudrate is the same as in arduino sketch
	crtscts: true);

// recieve motor feedback
serialListener = Routine({
	var byte, str, res, valArray,
	stepper, limitSwitchPos, limitSwitchNeg, safeMode, limitPos;
	loop{
		byte = serialPort.read;
		if(byte==13, {
			if(str[1].asString == "[", {

				valArray = str.asString.interpret;
				stepper = valArray[0];
				if(stepper == 1, {curPos.x = valArray[1]}, {curPos.y = valArray[1]});
				//tarPos = valArray[2];
				limitSwitchPos = valArray[3];
				limitSwitchNeg = valArray[4];
				safeMode = valArray[5];
				limitPos = valArray[6];

				// update all the curPos fields
				if(stepper == 2, {
					//curPos = curPos.rotate(rotation.neg * (pi / 180.0)) * micronsPerStep;
					curPos = curPos * micronsPerStep;
					curPosFields[0].string = (curPos.x).round(0.1).asString;
					curPosFields[1].string = (curPos.y).round(0.1).asString;
					curPosFields[2].string = (curPos.rho).round(0.1).asString;
					curPosFields[3].string = (if(curPos.theta >= 0, {0}, {360}) + (curPos.theta * (180 / pi))).round(0.1).asString;
					userView.refresh;

					// automate mode: select new point before the motor comes to a stop
					if(automate, {
						if((curPos.x - tarPos.x).abs < 5.0, {tarPos.x = imageDist.rand2.round(0.1)});
						if((curPos.y - tarPos.y).abs < 5.0, {tarPos.y = imageDist.rand2.round(0.1)});
						moveTo.value(tarPos);
					});
				});
			}, {
				(str).postln;
			});
			str = "";
		}, {str = str++byte.asAscii});
	};
});

// send new coordinates to the arduino / motors
moveTo = {arg point;
	var rotatedPoint, xMove, yMove;
	tarPosFields[0].string = tarPos.x.round(0.1).asString;
	tarPosFields[1].string = tarPos.y.round(0.1).asString;
	tarPosFields[2].string = tarPos.rho.round(0.1).asString;
	tarPosFields[3].string = (if(tarPos.theta >= 0, {0}, {360}) + (tarPos.theta * (180 / pi))).round(0.1).asString;
	//rotatedPoint = point.rotate(rotation * (pi / 180.0));
	rotatedPoint = point;
	xMove = (rotatedPoint.x / micronsPerStep).round(1).asInteger;
	yMove = (rotatedPoint.y / micronsPerStep).round(1).asInteger;
	serialPort.putAll(xMove.asString ++ " " ++ yMove.asString);
	serialPort.put(10);
};


// generate the gui
window = Window.new("", Rect(400, 400, 480, 650)).front;

xOffset = 240;
yOffset = 220;


// drawing and window key commands
userView = UserView(window, Rect(0, 0, 800, 600));
imageButtonRects = (({arg r; ({arg c; Rect.aboutPoint(Point(xOffset + (120 * (r - 1)), yOffset + (120 * (c - 1))), 5, 5)}) ! 3}) ! 3).flat;

userView.drawFunc = ({
	imageButtonRects.do({ arg rect, i;
		Pen.addOval(rect);
		Pen.color = Color.blue;
		Pen.draw;
	});

	Pen.addOval(Rect.aboutPoint(Point(xOffset + (curPos.x * (120 / imageDist)), yOffset + (curPos.y.neg * (120 / imageDist))), 5, 5));
	Pen.color = Color.black;
	Pen.draw;

	Pen.line(Point(xOffset, yOffset + 150), Point(xOffset, yOffset + 250));
	Pen.stroke;
});

userView.keyDownAction = ({arg view, char, mod, unicode, keycode, key;
	switch(key,
		16r1000012, {moveButtons[0].focus; dirKeyBlockTasks[0].stop; jogTasks[0].pause; jogTasks[0].play(AppClock)},
		16r1000013, {moveButtons[1].focus; dirKeyBlockTasks[1].stop; jogTasks[1].pause; jogTasks[1].play(AppClock)},
		16r1000014, {moveButtons[2].focus; dirKeyBlockTasks[2].stop; jogTasks[2].pause; jogTasks[2].play(AppClock)},
		16r1000015, {moveButtons[3].focus; dirKeyBlockTasks[3].stop; jogTasks[3].pause; jogTasks[3].play(AppClock)})
});


// create all the jog buttons and logic
dirKeyBlockTasks = [];
jogTasks = [];
jogRates = [0, 0, 0, 0, 0, 0, 0, 0];
moveButtons = ([[-1, 0], [0, -1], [1, 0], [0, 1], [-1, 0], [0, -1], [1, 0], [0, 1]].collect({arg m, i;
	var icons = ["◄", "▲", "►", "▼", "↻", "+", "↺", "-"], button;

	// speeds up the jog based on how long the button was pressed
	jogTasks = jogTasks.add(
		Task({
			dirKeyBlockTasks[i].stop;
			loop{
				jogRates[i] = (jogRates[i] + 0.1).clip(0, 10);
				if(i < 4, {
					// cartesian horizontal movement
					if(m[0].abs == 1, {tarPos.x = tarPos.x + (jogRates[i] * m[0])});
					// cartesian vertical movement
					if(m[1].abs == 1, {tarPos.y = tarPos.y + (jogRates[i] * m[1].neg);});
				}, {// polar change theta (rotate)
					if(m[0].abs == 1, {tarPos.theta = ((tarPos.theta * (180 / pi)) + (jogRates[i] * m[0])) * (pi / 180.0)});
					// polar change magnitude
					if(m[1].abs == 1, {tarPos.rho = tarPos.rho + (jogRates[i] * m[1].neg)});
				});
				moveTo.value(tarPos);
				0.2.wait
			};
		})
	);

	// hack to acount for a key held down
	dirKeyBlockTasks = dirKeyBlockTasks.add(Task({0.1.wait; jogRates[i] = 0;jogTasks[i].stop}));

	// create buttons
	button = Button(window,  Rect(xOffset - 12.5 + (25 * m[0]) + if(i < 4, {-175}, {175}), yOffset + 187.5 + (25 * m[1]), 25, 25))
	.states_([[icons[i]]])
	.mouseDownAction_({jogRates[i] = 0; jogTasks[i].play(AppClock)})
	.action_({jogTasks[i].stop(AppClock)})
	.enabled_(false)
	.keyDownAction_({arg butt, char, mod, unicode, keycode, key;
		switch(key,
			16r1000012, {moveButtons[0].focus; dirKeyBlockTasks[0].stop; jogTasks[0].pause; jogTasks[0].play(AppClock); true},
			16r1000013, {moveButtons[1].focus; dirKeyBlockTasks[1].stop; jogTasks[1].pause; jogTasks[1].play(AppClock); true},
			16r1000014, {moveButtons[2].focus; dirKeyBlockTasks[2].stop; jogTasks[2].pause; jogTasks[2].play(AppClock); true},
			16r1000015, {moveButtons[3].focus; dirKeyBlockTasks[3].stop; jogTasks[3].pause; jogTasks[3].play(AppClock); true},
			{false})})
	.keyUpAction_({arg butt, char, mod, unicode, keycode, key;
		switch(key,
			16r1000012, {dirKeyBlockTasks[0].start(AppClock); true},
			16r1000013, {dirKeyBlockTasks[1].start(AppClock); true},
			16r1000014, {dirKeyBlockTasks[2].start(AppClock); true},
			16r1000015, {dirKeyBlockTasks[3].start(AppClock); true},
			{false})})
}));


// position text fields
StaticText(window, Rect(xOffset - 82, yOffset + 150, 300, 20)).string_("cartesian");
StaticText(window, Rect(xOffset + 39, yOffset + 150, 300, 20)).string_("polar");
curPosFields = [];
tarPosFields = ["x", "y", "ρ", "θ"].collect({arg v, i;
	StaticText(window, Rect(xOffset + 22.5 + (55 * (i - 2)), yOffset + 170, 50, 20)).string_(v);
	curPosFields = curPosFields.add(StaticText(window, Rect(xOffset + 5 + (55 * (i - 2)), yOffset + 220, 50, 20)).string_("0.0"));
	TextField(window, Rect(xOffset + 2.5 + (55 * (i - 2)), yOffset + 190, 50, 20))
	.string_("0.0")
	.enabled_(false)
	.action_({arg field;
		if(i < 2, {
			tarPos.x = tarPosFields[0].string.asFloat;
			tarPos.y = tarPosFields[1].string.asFloat;
			tarPosFields[2].string = tarPos.rho.round(0.1).asString;
			tarPosFields[3].string = (if(tarPos.theta >= 0, {0}, {360}) + (tarPos.theta * (180 / pi))).round(0.1).asString;
		}, {
			tarPos.rho = tarPosFields[2].string.asFloat;
			tarPos.theta = tarPosFields[3].string.asFloat * (pi / 180);
			tarPosFields[0].string = tarPos.x.round(0.1).asString;
			tarPosFields[1].string = tarPos.y.round(0.1).asString;
		});
		moveTo.value(tarPos)})
});


// calibration wizard
calibrationSteps = [
	"1) find center image",
	"2) find northwest image \ntry first by using only the ↻ ↺ buttons to change θ",
	"3) compute all other points \nthis will erase previously saved points unless skipped",
	"4) find north image",
	"5) find northeast image",
	"6) find east image",
	"7) find southeast image",
	"8) find south image",
	"9) find southwest image",
	"10) find west image"
];

// disables everything till the point is reached between each step in the wizard
wizMoveBlock = Task({
	while({curPos.dist(tarPos) > 1}, {
		moveButtons.do({arg button; button.enabled = false});
		wizardButtons.do({arg button; button.enabled = false});
		tarPosFields.do({arg field; field.enabled = false});
		0.1.wait;
	});
	wizardButtons.do({arg button; button.enabled = true});
	wizardButtons[2].focus;
	moveButtons.do({arg button; button.enabled = true});
	tarPosFields.do({arg field; field.enabled = true});
});

// automate / calibrate button
Button.new(window, Rect.aboutPoint(Point(xOffset, yOffset + 270), 75, 12.5))
.states_([["calibrate"], ["automate"]])
.action_({arg button;
	if(button.value == 0, {
		automate = true;
		curWizardText.string = "";
		wizardButtons.do({arg button; button.visible = false});
	}, {
		automate = false;
		curWizardText.string = calibrationSteps[0];
		tarPos = imagePositions[4].deepCopy;
		moveTo.value(tarPos);
		wizMoveBlock.start(AppClock);
		curWizardStep = 0;
		wizardButtons.do({arg button; button.visible = true});
	});
	moveButtons.do({arg button; button.enabled = automate.not});
	tarPosFields.do({arg field; field.enabled = automate.not});
});

// wizard button logic
curWizardStep = 0;
curWizardText = StaticText(window, Rect.aboutPoint(Point(xOffset, yOffset + 310), 200, 20)).string_("").align_(\center);
wizardButtons = ["back", "skip", "next"].collect({arg t, i;
	var pointSeq, button;
	pointSeq = [4, 0, 0, 1, 2, 5, 8, 7, 6, 3, 4];
	button = Button(window,  Rect.aboutPoint(Point(xOffset - 60 + (60 * i), yOffset + 350), 25, 12.5))
	.states_([[t]])
	.action_({arg button;

		// code to automate populate all the points based on relation between two of the points
		if((curWizardStep == 2) && (i == 2), {
			if(imagePositions[0].rho == imageDist, {

			}, {

			});
			rotation = imagePositions[0].theta - (0.75 * pi);
			imagePositions[1].theta = (0.5 * pi) + rotation;
			imagePositions[2].theta = (0.25 * pi) + rotation;
			imagePositions[3].theta = pi + rotation;
			imagePositions[5].theta = rotation;
			imagePositions[6].theta = (1.25 * pi) + rotation;
			imagePositions[7].theta = (1.5 * pi) + rotation;
			imagePositions[8].theta = (1.75 * pi) + rotation;
		});

		if((curWizardStep == 0) && (i == 2), {serialPort.putAll("c")});
		if(i == 2, {imagePositions[pointSeq[curWizardStep]] = if(curWizardStep == 0, {Point(0, 0)}, {curPos.deepCopy})});
		curWizardStep = (curWizardStep + if(i == 0, {-1}, {1})) % 10;
		tarPos = imagePositions[pointSeq[curWizardStep]].deepCopy;
		moveTo.value(tarPos);
		wizMoveBlock.start(AppClock);
		//wizardButtons.do({arg button; button.enabled = true});
		//moveButtons.do({arg button; button.enabled = true});
		//tarPosFields.do({arg field; field.enabled = true});
		curWizardText.string = calibrationSteps[curWizardStep];
		//wizardButtons[1].visible = if(curWizardStep == 2, {true}, {false});
	})
	.visible_(false)
});

serialListener.play(AppClock);
)

