#include "ArduinoPositioner.hpp"

using namespace vera;

ArduinoPositioner::ArduinoPositioner(Ptr<schneide::SerialConnection> terminal,
  int measurementPosition, int loadingPosition,
  std::chrono::duration<double> waitTime)
: mTerminal(terminal)
, mMeasurementPosition(measurementPosition)
, mLoadingPosition(loadingPosition)
, mWaitTime(waitTime)
{
}

void ArduinoPositioner::moveMotor(Interactor& interactor, int where)
{
  // Set motor target to the desired position
  mTerminal->query("MOTOR {0}", where);
  // Wait for it to actually get there
  interactor.wait(mWaitTime);
}

void ArduinoPositioner::measurementPosition(Interactor& interactor)
{
  moveMotor(interactor, mMeasurementPosition);
}

void ArduinoPositioner::loadingPosition(Interactor& interactor)
{
  moveMotor(interactor, mLoadingPosition);
}
