/*
 * system_state.c
 *
 *  Created on: Sep 19, 2016
 *      Author: tkl
 */

#include <string.h>
#include <stdbool.h>
#include <stdint.h>

#include "collision_ctrl.h"
#include "drive_ctrl.h"

#include "driver.h"
#include "stack.h"
#include "queue.h"
#include "list.h"
#include "shell.h"
#include "kernel.h"

#include "board_devices.h"

#include "system_state.h"

#pragma pack(push)
#pragma pack(1)
struct collision_timer {
	bool running;
	unsigned long tick;
};
#pragma pack(pop)

struct system_state_object {
	enum system_state system_state;
	enum system_state last_state;
	enum system_state saved_state;
	enum system_state force_state;
	struct collision_timer collision_timer;
};

static struct system_state_object system_state_object = {
		.system_state = SYS_INIT,
		.last_state = SYS_MAX,
		.saved_state = SYS_MAX,
		.force_state = SYS_MAX,
		.collision_timer.running = false,
		.collision_timer.tick = 0,
};

int system_state_init(void)
{
	return 0;
}

static bool is_transition(void)
{
	return (system_state_object.system_state != system_state_object.last_state) ? true : false;
}

static enum system_state handle_state_init(void)
{
	enum system_state next_state = SYS_INIT;
	if(is_transition()) {
		system_state_object.last_state = system_state_object.system_state;
		char print_buffer[] = "#SYS_ST: INIT\r\n";
		shell_write(print_buffer, strlen(print_buffer));
		drive_ctrl_halt();
	}
	// TODO: check various sensor states e.g. battery, position state etc...
	next_state = SYS_IDLE;
	return next_state;
}

static enum system_state handle_state_idle(void)
{
	enum system_state next_state = SYS_IDLE;
	if(is_transition()) {
		system_state_object.last_state = system_state_object.system_state;
		char print_buffer[] = "#SYS_ST: IDLE\r\n";
		shell_write(print_buffer, strlen(print_buffer));
		drive_ctrl_halt();
	}
	if(system_state_object.force_state == SYS_DRIVING) {
		next_state = SYS_DRIVING;
		system_state_object.force_state = SYS_MAX;
	}
	return next_state;
}

static enum system_state handle_state_driving(void)
{
	enum system_state next_state = SYS_DRIVING;
	if(is_transition()) {
		system_state_object.last_state = system_state_object.system_state;
		char print_buffer[] = "#SYS_ST: DRIVING\r\n";
		shell_write(print_buffer, strlen(print_buffer));
		drive_ctrl_forward();
	}
	if(collision_ctrl_is_collision()) {
		next_state = SYS_COLLISION_DETECTED;
		system_state_object.saved_state = system_state_object.system_state;
	}
	if(system_state_object.force_state == SYS_IDLE) {
		next_state = SYS_IDLE;
		system_state_object.force_state = SYS_MAX;
	}

	return next_state;
}

static enum system_state handle_state_collision_detected(void)
{
	enum system_state next_state = SYS_COLLISION_DETECTED;
	if(is_transition()) {
		system_state_object.last_state = system_state_object.system_state;
		drive_ctrl_halt();
		char print_buffer[] = "#SYS_ST: COLLISION_DETECTED\r\n";
		shell_write(print_buffer, strlen(print_buffer));
	}
	next_state = SYS_COLLISION_DRIVE_BACK;
	if(system_state_object.force_state == SYS_IDLE) {
		next_state = SYS_IDLE;
		system_state_object.force_state = SYS_MAX;
	}
	return next_state;
}

static enum system_state handle_state_collision_drive_back(void)
{
	enum system_state next_state = SYS_COLLISION_DRIVE_BACK;
	if(is_transition()) {
		system_state_object.last_state = system_state_object.system_state;
		char print_buffer[] = "#SYS_ST: COLLISION_DRIVE_BACK\r\n";
		shell_write(print_buffer, strlen(print_buffer));
		system_state_object.collision_timer.tick = sys_tick_get_ms() + 500;
		system_state_object.collision_timer.running = true;
	}
	if(system_state_object.collision_timer.running &&
			(system_state_object.collision_timer.tick < sys_tick_get_ms())) {
		drive_ctrl_halt();
		next_state = SYS_COLLISION_TURN;
		system_state_object.collision_timer.running = false;
	}
	if(system_state_object.force_state == SYS_IDLE) {
		next_state = SYS_IDLE;
		system_state_object.force_state = SYS_MAX;
		system_state_object.collision_timer.running = false;
	}
	return next_state;
}

static enum system_state handle_state_collision_turn(void)
{
	enum system_state next_state = SYS_COLLISION_TURN;
	if(is_transition()) {
		system_state_object.last_state = system_state_object.system_state;
		char print_buffer[] = "#SYS_ST: COLLISION_TURN\r\n";
		shell_write(print_buffer, strlen(print_buffer));
		drv_open(&rng);
		// random number between 500 and 4000
		unsigned int sleep = (unsigned int)drv_read(&rng, NULL, 0) % 3500 + 500;
		drv_close(&rng);
		system_state_object.collision_timer.tick = sys_tick_get_ms() + sleep;
		system_state_object.collision_timer.running = true;
		// randomize turn direction
		if(sleep % 2)
			drive_ctrl_turn_left();
		else
			drive_ctrl_turn_right();
	}
	if(system_state_object.collision_timer.running &&
			(system_state_object.collision_timer.tick < sys_tick_get_ms())) {
		drive_ctrl_halt();
		next_state = system_state_object.saved_state;
		system_state_object.collision_timer.running = false;
	}
	if(system_state_object.force_state == SYS_IDLE) {
		next_state = SYS_IDLE;
		system_state_object.force_state = SYS_MAX;
		system_state_object.collision_timer.running = false;
	}
	return next_state;
}

int system_state_poll(void)
{
	enum system_state next_state = SYS_MAX;
	switch(system_state_object.system_state) {
	case SYS_INIT:
		next_state = handle_state_init();
		break;
	case SYS_IDLE:
		next_state = handle_state_idle();
		break;
	case SYS_DRIVING:
		next_state = handle_state_driving();
		break;
	case SYS_COLLISION_DETECTED:
		next_state = handle_state_collision_detected();
		break;
	case SYS_COLLISION_DRIVE_BACK:
		next_state = handle_state_collision_drive_back();
		break;
	case SYS_COLLISION_TURN:
		next_state = handle_state_collision_turn();
		break;
	case SYS_MAX:
		break;
	}
	if(next_state < SYS_MAX) {
		system_state_object.system_state = next_state;
		return 0;
	}
	return -1;
}

int system_state_force(enum system_state state)
{
	system_state_object.force_state = state;
	return 0;
}