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

#ifndef PERIPHERALS_MOTORS_HPP_
#define PERIPHERALS_MOTORS_HPP_

namespace Peripherals
{

template<typename T>
struct TwoChannelMotorData
{
  using pwm_channel = T;
  pwm_channel forward;
  pwm_channel backward;
};

class IMotors
{
public:
  virtual void Set(const std::int16_t left, const std::int16_t right) = 0;
  virtual void Coast() = 0;
  virtual void Break() = 0;
};

template<typename T, typename I>
class DualDrvMotors : public IMotors
{
public:
  using single_motor = TwoChannelMotorData<T>;
  using gpio = I;

  DualDrvMotors(const single_motor& left, const single_motor& right,
                const gpio& 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);
  }

  virtual void Set(const std::int16_t left, const std::int16_t right) override
  {
    if (left > 0)
    {
      _left.forward.SetCompare(left);
      _left.backward.SetCompare(0);
    } else
    {
      _left.forward.SetCompare(0);
      _left.backward.SetCompare(-1 * left);
    }

    if (right > 0)
    {
      _right.forward.SetCompare(right);
      _right.backward.SetCompare(0);
    } else
    {
      _right.forward.SetCompare(0);
      _right.backward.SetCompare(-1 * right);
    }
  }

  virtual void Coast() override
  {
    _left.forward.SetCompare(0);
    _left.backward.SetCompare(0);

    _right.forward.SetCompare(0);
    _right.backward.SetCompare(0);
  }

  virtual void Break() override
  {
    _left.forward.SetCompare(_left.forward.MaxValue());
    _left.backward.SetCompare(_left.backward.MaxValue());

    _right.forward.SetCompare(_right.forward.MaxValue());
    _right.backward.SetCompare(_right.backward.MaxValue());
  }

private:
  single_motor _left;
  single_motor _right;
  gpio _sleep_pin;
};

}// namespace Peripherals

#endif /* PERIPHERALS_MOTORS_HPP_ */