#include "motor.h"
#include "arduino.h"
#include "board_v10.h"

#include "MultiStepper.h"

c_motor motor0(MOT_STCK0, MOT_DIR0, MOT_EN0, MOT_RST0);
c_motor motor1(MOT_STCK1, MOT_DIR1, MOT_EN1, MOT_RST1);

MultiStepper motors;

void c_motor::init(bool setvref)
{
  pinMode(_rst, OUTPUT);
  pinMode(_en, OUTPUT);
  pinMode(_sclk, OUTPUT);
  pinMode(_dir, OUTPUT);

  digitalWrite(_rst, 0);
  digitalWrite(_sclk, 0);
  digitalWrite(_dir, 0);
  digitalWrite(_en, 0);

  delay(200);

  digitalWrite(_rst, 1);

  if (setvref)
  {
    pinMode(MOT_VREF1, OUTPUT);
    analogWrite(MOT_VREF1, 32);
  }
}

void c_motor::begin(void)
{
  _stepper.begin(RPM, MICROSTEPS);
  _stepper.enable();
}

void c_motor::decreasePos(void)
{
  if (ready())
  {
    _stepper.move(-OFFSET);
  }
}

void c_motor::increasePos(void)
{
  if (ready())
  {
    _stepper.move(OFFSET);
  }
}

bool c_motor::ready(void)
{
  return (_stepper.getCurrentState() == _stepper.State::STOPPED);
}

void handleMotors(void)
{
  motor0.run();
  motor1.run();
}

void initMotors(void)
{
  motor0.init(true);
  motor0.begin();

  motor1.init();
  motor1.begin();
}

void SetMotors(long pos0, long pos1)
{
  motor0.moveTo(pos0);
  motor1.moveTo(pos1);
}

void increaseMotor(e_motor motor)
{
  if (motor == MOTOR0)
  {
    motor0.increasePos();
  }
  if (motor == MOTOR1)
  {
    motor1.increasePos();
  }
}

void decreaseMotor(e_motor motor)
{
  if (motor == MOTOR0)
  {
    motor0.decreasePos();
  }
  if (motor == MOTOR1)
  {
    motor1.decreasePos();
  }
}