/* 3-floor Elevator Controller for RVController
 * A product of Advanced Mesecons Devices, a Cheapie Systems company
 * This is free and unencumbered software released into the public domain.
 * See http://unlicense.org/ for more information */

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

int state = STATE_INIT;
bool sensor1state = false;
bool sensor2state = false;
bool sensor3state = false;
bool call1state = false;
bool call2state = false;
bool call3state = false;
int direction = DIRECTION_UP;
int timer;

char channelbuf[16];
char msgbuf[64];

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;
}

void sendSensorGetMessages(void) {
	digiline_send("sensor1","GET");
	digiline_send("sensor2","GET");
	digiline_send("sensor3","GET");
}

void processDigilinesMessages(void) {
	while (digiline_bufferlevel() > 0) {
		digiline_receive(channelbuf,16,msgbuf,64);
		if (streq(channelbuf,"sensor1")) {
			sensor1state = (streq(msgbuf,"mesecons_movestones:movestone_vertical"));
		} else if (streq(channelbuf,"sensor2")) {
			sensor2state = (streq(msgbuf,"mesecons_movestones:movestone_vertical"));
		} else if (streq(channelbuf,"sensor3")) {
			sensor3state = (streq(msgbuf,"mesecons_movestones:movestone_vertical"));
		} else if (streq(channelbuf,"button1") && (state != STATE_TIMING_1) && (state != STATE_IDLE_1)) {
			call1state = true;
			digiline_send("button1","light_on");
		} else if (streq(channelbuf,"button2") && (state != STATE_TIMING_2) && (state != STATE_IDLE_2)) {
			call2state = true;
			digiline_send("button2","light_on");
		} else if (streq(channelbuf,"button3") && (state != STATE_TIMING_3) && (state != STATE_IDLE_3)) {
			call3state = true;
			digiline_send("button3","light_on");
		}
	}
}

void main(void) {
	while (true) {
		sendSensorGetMessages();
		processDigilinesMessages();
		switch (state) {
			case STATE_INIT:
				printstr("\nElevator Controller\nfor RVController\nInitializing");
				digiline_send("door1","extend");
				digiline_send("door2","extend");
				digiline_send("door3","extend");
				digiline_send("sel","extend");
				digiline_send("down","extend");
				digiline_send("up","retract_sticky");
				digiline_send("button1","light_off");
				digiline_send("button2","light_off");
				digiline_send("button3","light_off");
				printstr("\nBottom Floor Demand");
				state = STATE_BOTTOM_FLOOR_DEMAND;
				break;
			case STATE_BOTTOM_FLOOR_DEMAND:
				if (sensor1state) {
					printstr("\nBFD Complete");
					digiline_send("down","retract_sticky");
					digiline_send("door1","retract_sticky");
					state = STATE_TIMING_1;
					timer = 5;
				}
				break;
			case STATE_TIMING_1:
				timer--;
				if (timer <= 0) {
					printstr("\nIdle at 1");
					state = STATE_IDLE_1;
				}
				break;
			case STATE_TIMING_2:
				timer--;
				if (timer <= 0) {
					printstr("\nIdle at 2");
					state = STATE_IDLE_2;
				}
				break;
			case STATE_TIMING_3:
				timer--;
				if (timer <= 0) {
					printstr("\nIdle at 3");
					state = STATE_IDLE_3;
				}
				break;
			case STATE_IDLE_1:
				if (call2state) {
					direction = DIRECTION_UP;
					printstr("\nMoving to 2");
					state = STATE_MOVING_2;
					digiline_send("door1","extend");
					digiline_send("sel","retract_sticky");
					digiline_send("up","extend");
				} else if (call3state) {
					direction = DIRECTION_UP;
					printstr("\nMoving to 3");
					state = STATE_MOVING_3;
					digiline_send("door1","extend");
					digiline_send("sel","extend");
					digiline_send("up","extend");
				}
				break;
			case STATE_IDLE_2:
				if (direction == DIRECTION_UP) {
					if (call3state) {
						direction = DIRECTION_UP;
						printstr("\nMoving to 3");
						state = STATE_MOVING_3;
						digiline_send("door2","extend");
						digiline_send("sel","extend");
						digiline_send("up","extend");
					} else if (call1state) {
						direction = DIRECTION_DOWN;
						printstr("\nMoving to 1");
						state = STATE_MOVING_1;
						digiline_send("door2","extend");
						digiline_send("sel","extend");
						digiline_send("down","extend");
					}
				} else {
					if (call1state) {
						direction = DIRECTION_DOWN;
						printstr("\nMoving to 1");
						state = STATE_MOVING_1;
						digiline_send("door2","extend");
						digiline_send("sel","extend");
						digiline_send("down","extend");
					} else if (call3state) {
						direction = DIRECTION_UP;
						printstr("\nMoving to 3");
						state = STATE_MOVING_3;
						digiline_send("door2","extend");
						digiline_send("sel","extend");
						digiline_send("up","extend");
					}
				}
				break;
			case STATE_IDLE_3:
				if (call2state) {
					direction = DIRECTION_DOWN;
					printstr("\nMoving to 2");
					state = STATE_MOVING_2;
					digiline_send("door3","extend");
					digiline_send("sel","retract_sticky");
					digiline_send("down","extend");
				} else if (call1state) {
					direction = DIRECTION_DOWN;
					printstr("\nMoving to 1");
					state = STATE_MOVING_1;
					digiline_send("door3","extend");
					digiline_send("sel","extend");
					digiline_send("down","extend");
				}
				break;
			case STATE_MOVING_1:
				if (sensor1state) {
					printstr("\nArrived at 1");
					digiline_send("button1","light_off");
					direction = DIRECTION_UP;
					digiline_send("down","retract_sticky");
					digiline_send("door1","retract_sticky");
					timer = 5;
					state = STATE_TIMING_1;
					call1state = false;
				}
				break;
			case STATE_MOVING_2:
				if (sensor2state) {
					printstr("\nArrived at 2");
					digiline_send("button2","light_off");
					digiline_send("up","retract_sticky");
					digiline_send("down","retract_sticky");
					digiline_send("door2","retract_sticky");
					timer = 5;
					state = STATE_TIMING_2;
					call2state = false;
				}
				break;
			case STATE_MOVING_3:
				if (sensor3state) {
					printstr("\nArrived at 3");
					digiline_send("button3","light_off");
					direction = DIRECTION_DOWN;
					digiline_send("up","retract_sticky");
					digiline_send("door3","retract_sticky");
					timer = 5;
					state = STATE_TIMING_3;
					call3state = false;
				}
				break;
		}
		if ((state == STATE_IDLE_1) || (state == STATE_IDLE_2) || (state == STATE_IDLE_3)) {
			lightweight_mode(1);
			sleep(1);
			lightweight_mode(0);
		}
	}
}