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

#ifndef SKULLC_PERIPHERALS_MOTORS_HPP_
#define SKULLC_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 setBreak() = 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 setBreak() 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 /* SKULLC_PERIPHERALS_MOTORS_HPP_ */