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

enum class mmode
{
	STOP,
	LABYRINTH_FIND_WALL,
	LABYRINTH_BACKOFF,
	LABYRINTH_TURN_RIGHT,
	LABYRINTH_TURN_AWAY,
	LABYRINTH_FOLLOW,
	SEARCH_STRAIGHT,
	SEARCH_TURN_RIGHT,
	SEARCH_TURN_LEFT,
	SEARCH_TAG_STOP,
	HAPPY
};

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

int main()
{
	signal(SIGINT, signal_handler);
	signal(SIGTERM, signal_handler);
	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)
			);
	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 stop_button(25);
	ultrasound_sensor ultrasoundright(23, 24);
	ultrasound_sensor ultrasoundleft(12, 6);
	rfid_reader rfid;
	// TODO This random is unseeded
	minstd_rand random;

	clocktype::time_point mmode_end = clocktype::now();
	mmode mode = mmode::STOP;
	mmode target_mode = mmode::LABYRINTH_FIND_WALL;
	clocktype::time_point mode_reset_time;

	int last_tag = 0x00000000;
	vector<uint32_t> found_tags;
	found_tags.reserve(no_tags);

	measure measurement("main");

	clocktype::time_point next_tick = clocktype::now();
	while (!stop)
	{
		measurement.start();
		clocktype::time_point tick_start = clocktype::now();
		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)
			{
				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())
			{
				target_mode = mmode::LABYRINTH_BACKOFF;
				mode_reset_time = tick_start + backoff_time;
			}
			else if (bwleftleft.get_value() || bwleftright.get_value())
			{
				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)
			{
				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 (std::abs((long long int) usoundright - usoundleft) < 50)
					{
						int angle = random() % 180 + 90;
						if (angle <= 180)
						{
							target_mode = mmode::SEARCH_TURN_RIGHT;
						}
						else
						{
							target_mode = mmode::SEARCH_TURN_LEFT;
							angle = 360 - angle;
						}
						mode_reset_time = tick_start + 3ms * angle;
					}
					else
					{
						int angle = random() % 90 + 90;
						mode_reset_time = tick_start + 3ms * angle;
						if (usoundright < usoundleft)
							target_mode = mmode::SEARCH_TURN_LEFT;
						else
							target_mode = mmode::SEARCH_TURN_RIGHT;
					}
				}
				int last_id = rfid.last_id();
				if (last_id != last_tag && last_id != exit_tag)
				{
					last_tag = last_id;
					if (find(found_tags.begin(), found_tags.end(), last_tag) == found_tags.end())
					{
						found_tags.push_back(last_tag);
						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 (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;
}