#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("Ped Hybrid Beacon\n");
	printstr("for RVController\nInitializing HW\n");
	digiline_send("beacon","OFF");
	digiline_send("ped","RED");
	digiline_clearbuffer();
	while (true) {
		printstr("Idle\n");
		lightweight_mode(1);
		printstr("Lightweight: On\n");
		while (!checkdetector()) {}
		printstr("Ped detected\nLightweight: Off\n");
		lightweight_mode(0);
		printstr("Beacon: Flash Yellow\n");
		digiline_send("beacon","FLASHYELLOW");
		sleep(3);
		printstr("Beacon: Yellow\n");
		digiline_send("beacon","YELLOW");
		sleep(5);
		printstr("Beacon: Red\n");
		digiline_send("beacon","RED");
		sleep(1);
		printstr("Ped: Walk\n");
		digiline_send("ped","GREEN");
		sleep(7);
		digiline_clearbuffer();
		printstr("Ped: Flash DW\n");
		digiline_send("ped","FLASHRED");
		printstr("Beacon: Flash Red\n");
		digiline_send("beacon","FLASHRED");
		sleep(15);
		printstr("Ped: Don't Walk\n");
		digiline_send("ped","RED");
		sleep(1);
		printstr("Beacon: Off\n");
		digiline_send("beacon","OFF");
		sleep(15);
	}
}