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

#include <Arduino.h>

Motor::Motor() {
  //   this->isMotor = true;
  type = Type::Motor;
}

// Motor::Motor(uint8_t pinIn1, uint8_t pinIn2) {
//   this->pinIn1 = pinIn1;
//   this->pinIn2 = pinIn2;

//   pinMode(pinIn1, OUTPUT);  // configure the in1 pin to output mode
//   pinMode(pinIn2, OUTPUT);  // configure the in2 pin to output mode
// }

// void Motor::SetDirection(Direction direction) {
//   digitalWrite(pinIn1, direction);
//   digitalWrite(pinIn2, !direction);  // This is the opposite of pinIn1
// }

// void Motor::SetSpeed(float speed) {  // 0..1
//   currentSpeed = speed;
//   uint8_t motorSignal = (uint8_t)(speed * 255);
//   analogWrite(pinIn1, speed);
//   analogWrite(pinIn2, 255 - speed);
// }

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

void Motor::SetSpeed(float speed) {
  this->currentSpeed = speed;
}

bool Motor::Drive(float distance) {
  if (!this->driving) {
    this->startTime = time(NULL);
    this->targetDistance = abs(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);
  return false;
}