#include <stdint.h>
#include <stdbool.h>
#include "rvcontroller-ecalls.h"

bool streq(char *a, char *b) {
	for (int i=0;a[i] != 0 || b[i] != 0;i++) {
		if (a[i] != b[i]) {
			return false;
		}
	}
	return true;
}

void sleep(uint32_t delay) {
	//This will start having problems after about 130 years
	//of uptime, but that's probably acceptable
	uint32_t endtime = rdtime() + delay;
	while (rdtime() < endtime) {}
	return;
}

bool checkdetector(void) {
	char channelbuf[16];
	char msgbuf[16];
	while (digiline_bufferlevel() > 0) {
		lightweight_mode(0);
		digiline_receive(channelbuf,16,msgbuf,16);
		if (streq(channelbuf,"detect")) {
			return true;
		}
	}
	lightweight_mode(1);
	return false;
}

void main() {
	printstr("Railroad Crossing\nfor RVController\nInitializing HW\n");
	digiline_send("light","OFF");
	digiline_send("bell","off");
	digiline_send("gate","up");
	digiline_clearbuffer();
	while (true) {
		printstr("Idle\n");
		lightweight_mode(1);
		printstr("Lightweight: On\n");
		while (!checkdetector()) {}
		printstr("Train detected\nLightweight: Off\n");
		lightweight_mode(0);
		digiline_send("light","YELLOW");
		printstr("Light: Yellow\n");
		sleep(2);
		digiline_send("bell","on");
		printstr("Bell: On\n");
		sleep(2);
		digiline_send("light","RED");
		printstr("Light: Red\n");
		sleep(3);
		digiline_send("gate","down");
		printstr("Gate: Down\n");
		sleep(4);
		digiline_send("bell","off");
		printstr("Bell: Off\n");

		digiline_clearbuffer();
		for (uint8_t time = 15;time > 0;time--) {
			printstr("Timeout in ");
			printint(time);
			printstr("s\n");
			sleep(1);
			bool train = checkdetector();
			lightweight_mode(0); //checkdetector() turns this on
			if (train) {
				digiline_clearbuffer();
				time = 25;
				printstr("Time reset by train\n");
			}
		}
		printstr("Timed out\n");

		digiline_send("gate","up");
		printstr("Gate: Up\n");
		sleep(2);
		digiline_send("light","OFF");
		printstr("Light: Off\n");
	}
}