#include <boost/asio.hpp>

#include <sys/socket.h>
#include <netdb.h>
#include <cstdint>
#include <thread>
#include <atomic>
#include <queue>
#include <mutex>
#include <chrono>
#include <condition_variable>

#include "engine.hpp"

using namespace std;
using boost::asio::ip::tcp;
using boost::asio::io_service;

template <typename T>
class prodcon
{
private:
	mutex mut;
	condition_variable condition;
	queue<T> q;
public:
	void push(T data)
	{
		unique_lock<mutex> lock(mut);
		q.push(data);
		condition.notify_one();
	}
	template<typename _Rep, typename _Period>
	bool pop(T &result, const chrono::duration<_Rep, _Period> __rtime)
	{
		unique_lock<mutex> lock(mut);
		bool success = condition.wait_for(lock, __rtime, [this] {return !this->q.empty();});
		if (!success)
			return false;
		result = q.front();
		q.pop();
		return true;
	}
};

class remote_server
{
private:
	tcp::acceptor acceptor;
	io_service &io;
	thread engine_thread;
	atomic<bool> stop_thread;
	prodcon<pair<int8_t, int8_t>> commands;
public:
	remote_server(io_service &io_service);
	void run();
	void engine_loop();
};

int main()
{
	io_service io;
	remote_server server(io);
	server.run();
	return 0;
}

remote_server::remote_server(io_service &io_service)
	: acceptor(io_service),
	  io(io_service),
	  stop_thread(false)
{
	// Nothing to do
}

void remote_server::run()
{
	tcp::endpoint endpoint(tcp::v4(), 1337);
	acceptor.open(endpoint.protocol());
	acceptor.set_option(tcp::acceptor::reuse_address(true));
	acceptor.bind(endpoint);
	acceptor.listen(1);
	while (true)
	{
		tcp::socket socket(io);
		acceptor.accept(socket);
		stop_thread = false;
		engine_thread = thread(&remote_server::engine_loop, this);
		while (true)
		{
			int8_t speed_left, speed_right;
			socket.read_some(boost::asio::buffer(&speed_left, 1));
			socket.read_some(boost::asio::buffer(&speed_right, 1));
			commands.push(pair<int8_t, int8_t>(speed_left, speed_right));
		}
	}
}

void remote_server::engine_loop()
{
	engine left(
			gpio(13, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
			gpio(20, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
			);
	engine right(
			gpio(19, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
			gpio(26, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
			);
	while (!stop_thread)
	{
		pair<int8_t, int8_t> command;
		if (commands.pop(command, 1s))
		{
			left.set_speed(command.first);
			right.set_speed(command.second);
		}
		else
		{
			left.set_speed(0);
			right.set_speed(0);
		}
	}
}