#include "Motor.h"
#include <time.h>

// #include <Arduino.h>

Motor::Motor() {
  type = Thing::MotorType;
}

float Motor::GetSpeed() {
  return this->currentTargetSpeed;
}

void Motor::SetSpeed(float targetSpeed) {
  this->currentTargetSpeed = targetSpeed;
}

bool Motor::Drive(float distance) {
  if (!this->driving) {
    this->startTime = time(NULL);
    this->targetDistance = distance >= 0 ? distance : -distance;
    this->driving = true;
  }
  double duration = difftime(time(NULL), this->startTime);
  if (duration >= this->targetDistance) {
    this->driving = false;
    return true;
  }

  SetSpeed(distance < 0 ? -1 : 1);  // max speed
  return false;
}