#include "engine.hpp"

#include <chrono>
#include <utility>

using namespace std;
using namespace std::chrono;

engine::engine(gpio&& pin_forward, gpio&& pin_reverse)
	: speed(0),
	  pin_forward(move(pin_forward)),
	  pin_reverse(move(pin_reverse)),
	  stop_thread(false),
	  measurement(string("engine") + to_string(this->pin_forward.get_pin()))
{
	this->pin_forward.set_value(false);
	this->pin_reverse.set_value(false);
	pwm_thread = thread(&engine::pwm_loop, this);
}

engine::~engine()
{
	stop_thread = true;
	pwm_thread.join();
}

void engine::set_speed(int speed)
{
	this->speed.store(speed, memory_order_relaxed);
}

void engine::pwm_loop()
{
	this_thread::sleep_for(milliseconds(100));
	const int sleep_msec = 1;
	int counter = 0;
	high_resolution_clock::time_point next_sleep_target = high_resolution_clock::now();
	while (!stop_thread)
	{
		measurement.start();
		direction target_direction;
		int speed_val = speed.load(memory_order_relaxed);
		if (speed_val < 0)
		{
			target_direction = direction::REVERSE;
			speed_val *= -1;
		}
		else
		{
			target_direction = direction::FORWARD;
		}
		counter += sleep_msec;
		if (counter >= 100)
			counter = 0;
		if (counter < speed_val)
		{
			if (target_direction == direction::FORWARD)
			{
				pin_reverse.set_value(false);
				pin_forward.set_value(true);
			}
			else
			{
				pin_forward.set_value(false);
				pin_reverse.set_value(true);
			}
		}
		else
		{
			pin_forward.set_value(false);
			pin_reverse.set_value(false);
		}
		measurement.stop();
		next_sleep_target += 500us;
		this_thread::sleep_until(next_sleep_target);
	}
	pin_forward.set_value(false);
	pin_reverse.set_value(false);
}