#include <algorithm>
#include <chrono>
#include <cmath>
#include <csignal>
#include <cstdint>
#include <iostream>
#include <random>
#include <vector>

#include "engine.hpp"
#include "measure.hpp"
#include "rfid_reader.hpp"
#include "ultrasound_sensor.hpp"
#include "emergency_stop.h"

using namespace std;

typedef chrono::high_resolution_clock clocktype;

const int tickrate = 100;
const chrono::milliseconds tick_delay = 1000ms / tickrate;
const uint32_t exit_tag = 0xAE0B1E2B;
const int no_tags = 3;
const uint32_t distance_until_turn = 180;
chrono::milliseconds backoff_time = 100ms;
chrono::milliseconds turn_right_follow_time = 150ms;
chrono::milliseconds turn_right_backoff_time = 500ms;

bool stop = false;

// Modes indicate what kawaii is doing at the moment
// Modes prefixed with LABYRINTH are for navigating the Labyrinth
// Modes prefixed with SEARCH are for performing rfid search in the open area
enum class mmode
{
	// Motors stopped, no operation at all
	STOP,
	// Drive straight until a wall is encountered
	LABYRINTH_FIND_WALL,
	// Drive backwards for a short period of time to build up distance to the wall
	LABYRINTH_BACKOFF,
	// Turn to the right - away from the wall followed - to produce a forward movement
	LABYRINTH_TURN_RIGHT,
	// Turn to the right to better align with the encountered wall
	LABYRINTH_TURN_AWAY,
	// Turn to the left until the currently followed wall is hit
	LABYRINTH_FOLLOW,
	// Move forward util encountering a wall or a rfid tag
	SEARCH_STRAIGHT,
	// Turn away from the wall to the right
	SEARCH_TURN_RIGHT,
	// Turn away from the wall to the left
	SEARCH_TURN_LEFT,
	// Perform a short stop to indicate detected rfid tag
	SEARCH_TAG_STOP,
	// Perform a short dance to celebrate successful operation
	HAPPY
};

void signal_handler(int sig)
{
	stop = true;
}

int main()
{
	signal(SIGINT, signal_handler);
	signal(SIGTERM, signal_handler);

	// Initialize everything needed during operation

	// Engines
	engine right(
			gpio(13, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
			gpio(20, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
			);
	engine left(
			gpio(19, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
			gpio(26, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
			);

	// Black & White Sensors
	gpio bwleftleft(18, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
	gpio bwleftright(4, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
	gpio bwrightleft(3, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
	gpio bwrightright(2, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);

	// Emergency stop button
	emergency_stop stop_button(25);

	// Ultrasound sensors
	ultrasound_sensor ultrasoundright(23, 24);
	ultrasound_sensor ultrasoundleft(12, 6);

	// RFID reader
	rfid_reader rfid;

	minstd_rand random; // This random is unseeded. This however isn't a problem for this application

	clocktype::time_point mmode_end = clocktype::now();
	mmode mode = mmode::STOP;
	mmode target_mode = mmode::LABYRINTH_FIND_WALL;
	clocktype::time_point mode_reset_time; // Inidicates how long the current mode should be active (does not apply to all modes)

	int last_tag = 0x00000000;
	vector<uint32_t> found_tags;
	found_tags.reserve(no_tags); // Since memory allocations during operations could cause problems we preallocate enough memory to store all tags in

	measure measurement("main");

	clocktype::time_point next_tick = clocktype::now();

	// Main loop repsonsible for the acutal operation
	while (!stop)
	{
		measurement.start();
		clocktype::time_point tick_start = clocktype::now();

		// First we check all relevant sensors and decide whether we need to change modes
		// If so we store the new mode in target_mode for later mode switching
		switch (mode)
		{
		case mmode::LABYRINTH_FOLLOW:
		case mmode::LABYRINTH_BACKOFF:
		case mmode::LABYRINTH_TURN_RIGHT:
		case mmode::LABYRINTH_TURN_AWAY:
		case mmode::LABYRINTH_FIND_WALL:
		{
			if (tick_start >= mode_reset_time)
			{
				// Some modes will run only a specific time before automaticly switching to the next mode
				switch (mode)
				{
				case mmode::LABYRINTH_BACKOFF:
					target_mode = mmode::LABYRINTH_TURN_AWAY;
					mode_reset_time = tick_start + turn_right_backoff_time;
					break;
				case mmode::LABYRINTH_TURN_AWAY:
					target_mode = mmode::LABYRINTH_FIND_WALL;
					break;
				case mmode::LABYRINTH_TURN_RIGHT:
					target_mode = mmode::LABYRINTH_FOLLOW;
					break;
				}
			}
			if (bwrightleft.get_value() || bwrightright.get_value())
			{
				// We want to follow the left-hand wall so we need to backoff a little if hitting a wall with the right sensor to turn
				target_mode = mmode::LABYRINTH_BACKOFF;
				mode_reset_time = tick_start + backoff_time;
			}
			else if (bwleftleft.get_value() || bwleftright.get_value())
			{
				// If the left sensor hits the wall, follow it
				// Don't switch to follow mode if the current mode is LABYRINTH_BACKOFF as this can cause bugs when left and right sensors hit
				// the wall at the same time which can cause kawaii to drive over the wall
				if (mode != mmode::LABYRINTH_BACKOFF)
				{
					target_mode = mmode::LABYRINTH_TURN_RIGHT;
					mode_reset_time = tick_start + turn_right_follow_time;
				}
			}
			int last_id = rfid.last_id();
			if (last_id == exit_tag)
			{
				// If we detected the labyrinth exit tag switch to seah mode
				target_mode = mmode::SEARCH_STRAIGHT;
				last_tag = last_id;
			}
		}
			break;
		case mmode::SEARCH_STRAIGHT:
			{
				auto usoundright = ultrasoundright.get_value();
				auto usoundleft = ultrasoundleft.get_value();
				if (min(usoundright, usoundleft) < distance_until_turn)
				{
					// If the left and right sensor measure a difference of under 5 cm the wall is roughly in front of kawaii
					if (std::abs((long long int) usoundright - usoundleft) < 50)
					{
						// If the wall is in front kawaii will turn anything between 90° and 270° to the right
						int angle = random() % 180 + 90;
						if (angle <= 180)
						{
							target_mode = mmode::SEARCH_TURN_RIGHT;
						}
						else
						{
							// If the angle is >180° turn to the left inststead to reach the desired angle
							target_mode = mmode::SEARCH_TURN_LEFT;
							angle = 360 - angle;
						}
						mode_reset_time = tick_start + 3ms * angle; // 3ms is the time kawaii needs to turn roughly 1°
					}
					// If not the wall is to kawaii's left or right - indicated by which sensor measures the lower value
					else
					{
						// If the wall is to the side turn anything between 90° and 180° away from the wall
						int angle = random() % 90 + 90;
						mode_reset_time = tick_start + 3ms * angle; // 3ms is the time kawaii needs to turn roughly 1°
						if (usoundright < usoundleft)
							target_mode = mmode::SEARCH_TURN_LEFT;
						else
							target_mode = mmode::SEARCH_TURN_RIGHT;
					}
				}

				int last_id = rfid.last_id();
				// Check wether the last detected rfid tag has changed and is not the exit tag
				if (last_id != last_tag && last_id != exit_tag)
				{
					last_tag = last_id;
					// Check if the tag is new and if so add it to the list of tags
					if (find(found_tags.begin(), found_tags.end(), last_tag) == found_tags.end())
					{
						found_tags.push_back(last_tag); // This won't perform any memory allocations since there was already enough memory for all tags allocated during initialization
						target_mode = mmode::SEARCH_TAG_STOP;
						mode_reset_time = tick_start + 200ms;
						if (found_tags.size() == no_tags)
						{
							target_mode = mmode::HAPPY;
							mode_reset_time = tick_start + 2s;
						}
					}
				}
			}
			break;
		case mmode::SEARCH_TURN_LEFT:
		case mmode::SEARCH_TURN_RIGHT:
		case mmode::SEARCH_TAG_STOP:
			if (tick_start >= mode_reset_time)
			{
				target_mode = mmode::SEARCH_STRAIGHT;
			}
			break;
		case mmode::HAPPY:
			if (tick_start >= mode_reset_time)
			{
				target_mode = mmode::STOP;
				stop = true;
			}
			break;
		}

		if (stop_button.get_state())
		{
			target_mode = mmode::STOP;
			stop = true;
		}

		// If the target_mode was modified adjust the motor speeds to the requirements of the new mode
		if (target_mode != mode)
		{
			mode = target_mode;
			switch (mode)
			{
			case mmode::STOP:
				left.set_speed(0);
				right.set_speed(0);
				break;
			case mmode::LABYRINTH_FIND_WALL:
				left.set_speed(60);
				right.set_speed(60);
				break;
			case mmode::LABYRINTH_BACKOFF:
				left.set_speed(-100);
				right.set_speed(-100);
				break;
			case mmode::LABYRINTH_TURN_AWAY:
			case mmode::LABYRINTH_TURN_RIGHT:
				left.set_speed(100);
				right.set_speed(-60);
				break;
			case mmode::LABYRINTH_FOLLOW:
				left.set_speed(0);
				right.set_speed(100);
				break;
			case mmode::SEARCH_STRAIGHT:
				left.set_speed(100);
				right.set_speed(100);
				break;
			case mmode::HAPPY:
			case mmode::SEARCH_TURN_LEFT:
				left.set_speed(-100);
				right.set_speed(100);
				break;
			case mmode::SEARCH_TURN_RIGHT:
				left.set_speed(100);
				right.set_speed(-100);
				break;
			case mmode::SEARCH_TAG_STOP:
				left.set_speed(-100);
				right.set_speed(-100);
				break;
			}
		}

		measurement.stop();

		if (!stop)
		{
			next_tick += tick_delay;
			this_thread::sleep_until(next_tick);
		}
	}

	return 0;
}