#include "AccurateMotor.h"

void AccurateMotorClass::Init()
{
	car_running = false;
}

void AccurateMotorClass::SetCarRunning()
{
	AccurateMotor.car_running = true;
}

void AccurateMotorClass::SetCarNotRunning()
{
	AccurateMotor.car_running = false;
}

void AccurateMotorClass::TurnLeft(uint8_t degree)
{
	if (car_running)
		return;
	DCMotor.TurnLeft(car_speed, car_speed);
	SetCarRunning();
	MsTimer2::set(degree * 50, []()
		{
			{
				DCMotor.Stop();
				SetCarNotRunning();
				MsTimer2::stop();
			}
		});
	MsTimer2::start();
}

void AccurateMotorClass::ForceStop()
{
	DCMotor.Stop();
	SetCarNotRunning();
	MsTimer2::stop();
}

void AccurateMotorClass::TurnRight(uint8_t degree)
{
	if (car_running)
		return;
	DCMotor.TurnRight(car_speed, car_speed);
	SetCarRunning();
	MsTimer2::set(degree * 50, []()
		{
			{
				DCMotor.Stop();
				SetCarNotRunning();
				MsTimer2::stop();
			}
		});
	MsTimer2::start();
}

void AccurateMotorClass::RunForward(uint8_t distence)
{
	if (car_running)
		return;
	DCMotor.Go(car_speed);
	SetCarRunning();
	MsTimer2::set(distence * 100, []()
		{
			{
				DCMotor.Stop();
				SetCarNotRunning();
				MsTimer2::stop();
			}
		});
	MsTimer2::start();
}

bool AccurateMotorClass::IsCarRunning()
{
	return car_running;
}

void AccurateMotorClass::DelayUntilCarStop()
{
	while (car_running)
		delay(10);
}

void AccurateMotorClass::RunBackward(uint8_t distence)
{
	if (car_running)
		return;
	DCMotor.Back(car_speed);
	SetCarRunning();
	MsTimer2::set(distence * 100, []()
		{
			{
				DCMotor.Stop();
				SetCarNotRunning();
				MsTimer2::stop();
			}
		});
	MsTimer2::start();
}

AccurateMotorClass AccurateMotor;