/*
 * peripherals_motors.cpp
 *
 *  Created on: Feb 24, 2021
 *      Author: erki
 */

#include "peripherals_motors.hpp"

namespace Peripherals
{

DualDrvMotors::DualDrvMotors(const IMotors::TwoChannelMotorData& left,
		const IMotors::TwoChannelMotorData& right, const IO& sleep_pin)
  : _left(left)
  , _right(right)
  , _sleep_pin(sleep_pin)
{
	_left.forward.Enable();
	_left.backward.Enable();
	_right.forward.Enable();
	_right.backward.Enable();

	Set(0, 0);
}

void DualDrvMotors::Set(const std::int16_t left, const std::int16_t right)
{
	if (left > 0)
	{
		__HAL_TIM_SET_COMPARE(_left.forward.timer, _left.forward.channel, left);
		__HAL_TIM_SET_COMPARE(_left.backward.timer, _left.backward.channel, 0);
	}
	else
	{
		__HAL_TIM_SET_COMPARE(_left.forward.timer, _left.forward.channel, 0);
		__HAL_TIM_SET_COMPARE(_left.backward.timer, _left.backward.channel, (-1 * left));
	}

	if (right > 0)
	{
		__HAL_TIM_SET_COMPARE(_right.forward.timer, _right.forward.channel, right);
		__HAL_TIM_SET_COMPARE(_right.backward.timer, _right.backward.channel, 0);
	}
	else
	{
		__HAL_TIM_SET_COMPARE(_right.forward.timer, _right.forward.channel, 0);
		__HAL_TIM_SET_COMPARE(_right.backward.timer, _right.backward.channel, (-1 * right));
	}
}

void DualDrvMotors::Coast()
{
	__HAL_TIM_SET_COMPARE(_left.forward.timer, _left.forward.channel, 0);
	__HAL_TIM_SET_COMPARE(_left.backward.timer, _left.backward.channel, 0);

	__HAL_TIM_SET_COMPARE(_right.forward.timer, _right.forward.channel, 0);
	__HAL_TIM_SET_COMPARE(_right.backward.timer, _right.backward.channel, 0);
}

void DualDrvMotors::Break()
{
	Set(0, 0);

	_left.forward.Disable();
	_left.backward.Disable();
	_right.forward.Disable();
	_right.backward.Disable();

	_left.forward.PinToGpio();
	_left.backward.PinToGpio();
	_right.forward.PinToGpio();
	_right.backward.PinToGpio();

	_left.forward.pin.Set(true);
	_left.backward.pin.Set(true);
	_right.forward.pin.Set(true);
	_right.backward.pin.Set(true);
}

void DualDrvMotors::Unbreak()
{
	_left.forward.PinToPwm();
	_left.backward.PinToPwm();
	_right.forward.PinToPwm();
	_right.backward.PinToPwm();

	_left.forward.Enable();
	_left.backward.Enable();
	_right.forward.Enable();
	_right.backward.Enable();
}

}