#include <iostream>
#include <random>
#include <array>
#include <vector>
#include <thread>
#include <functional>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <chrono>

std::random_device rd;  //Will be used to obtain a seed for the random number engine
std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
std::uniform_int_distribution<> dis(1, 500);

constexpr int TherholdMin = 10;
constexpr int TherholdMedium = 100;
constexpr int TherholdMax = 250;

constexpr int MotorSpeedMin = 180;
constexpr int MotorSpeedMax = 230;

constexpr int SensorsCount = 5;

std::array<int, SensorsCount> sensorsArray{};

std::mutex taskQueueMutex;
std::condition_variable timerExpired;

void pinSetup()
{
	std::cout << "Executing pin setup" << std::endl;
}

void moveForward()
{
	std::cout << "Move forward!" << std::endl;
}

void moveBackward()
{
	std::cout << "Move backward!" << std::endl;
}

void moveRight()
{
	std::cout << "Move right!" << std::endl;
}

void moveLeft()
{
	std::cout << "Move left!" << std::endl;
}

void stop()
{
	std::cout << "Car stopped!" << std::endl;
}

void setMotorSpeed(int _motorNumber, size_t _motorSpeed)
{
	std::cout << "Motor speed set to: " << _motorSpeed << " for motor number: " << _motorNumber << std::endl;
}

namespace Tasks
{
	void readHallSensor()
	{
		int sensorValue = dis(gen);
		if (sensorValue < TherholdMin)
		{
			moveBackward();
			moveRight();
		}
		std::cout << "Read hall sensor: "<< sensorValue << std::endl;
	}

	void readSensorValues()
	{
		std::for_each(
				sensorsArray.begin()
			,	sensorsArray.end()
			,	[](int& _sensor)
				{
					int sensorValue = dis(gen);
					_sensor = sensorValue;
				}
		);
	}


	void task3_auto_tarcking()
	{
		if (sensorsArray[2] >= TherholdMax)
		{ //The middle sensor is on the black line
			if (sensorsArray[1] <= TherholdMedium && sensorsArray[3] <= TherholdMedium)
			{ //Other sensors are in the white area
				moveForward();  //straight
				setMotorSpeed( 4, MotorSpeedMin );
			}
			else if (sensorsArray[1] >= TherholdMax && sensorsArray[3] <= TherholdMin)
			{ //The second sensor is also on the black line
				moveLeft();  //Turn left
				setMotorSpeed( MotorSpeedMin,0 );
			}
			else if (sensorsArray[1] <= TherholdMin && sensorsArray[3] >= TherholdMax)
			{ //The fourth sensor is also on the black line
				moveRight();  //Turn right
				setMotorSpeed(3, MotorSpeedMin );
			}
		}
		else if (sensorsArray[2] >= TherholdMax)
		{ //The middle sensor is in the white area
			if (sensorsArray[1] >= TherholdMax && sensorsArray[3] >= TherholdMedium)
			{ //Turn left
				moveLeft();
				setMotorSpeed(3, MotorSpeedMax );
			}
			else if (sensorsArray[1] >= TherholdMedium && sensorsArray[3] >= TherholdMax)
			{ //Turn right
				moveRight();
				setMotorSpeed(2, MotorSpeedMin );
			}
		}
		else {
			moveBackward();
			setMotorSpeed(1, MotorSpeedMin );
		}

		if (sensorsArray[1] >= TherholdMax)
		{ //The second sensor is on the black line
			if (sensorsArray[0] >= TherholdMax && sensorsArray[2] <= TherholdMin)
			{ //Accelerate left turn
				moveLeft();
				setMotorSpeed(0, MotorSpeedMax );
			}
			else
			{ //Turn left
				moveLeft();
				setMotorSpeed(0, MotorSpeedMax );
			}
		}
		if (sensorsArray[3] >= TherholdMax)
		{ //The fourth sensor is on the black line
			if (sensorsArray[2] <= TherholdMin && sensorsArray[4] >= TherholdMax)
			{ //Accelerate right turn
				moveRight();
				setMotorSpeed( 0, MotorSpeedMax );
			}
			else
			{ //Turn right
				moveRight();
				setMotorSpeed( 0, MotorSpeedMin );
			}
		}
	}

};

using TasksQueue = std::vector<std::function<void()>>;
TasksQueue tasksQueue{
		Tasks::readHallSensor
	,   Tasks::readSensorValues
	,   Tasks::task3_auto_tarcking
};

void tasksProceeder()
{
	size_t taskIndex{};
	const size_t tasksCount{ tasksQueue.size() };

	while(true)
	{
		std::unique_lock<std::mutex> queueLock( taskQueueMutex );
		timerExpired.wait(queueLock);

		tasksQueue.at( taskIndex )();
		taskIndex = taskIndex == tasksCount - 1 ? 0 : ++taskIndex;

		queueLock.unlock();
		timerExpired.notify_one();
	}
}


void timerThread()
{
	using namespace std::chrono_literals;
	constexpr auto TimerPeriod = 100ms;

	while (true)
	{
		timerExpired.notify_one();
		std::this_thread::sleep_for( TimerPeriod );
	}
}

int main()
{

	std::thread queueWorker(tasksProceeder);
	std::thread timer(timerThread);

	queueWorker.join();
	timer.join();


	return 0;
}