#include "ControlledMotor.h"

namespace RoboidControl {

ControlledMotor::ControlledMotor(Motor* motor,
                                 RelativeEncoder* encoder,
                                 Thing* parent)
    : Motor(parent), motor(motor), encoder(encoder) {
  this->type = Type::ControlledMotor;
  // encoder.SetParent(*this);
  //  Thing parent = motor.GetParent();
  //  this->SetParent(parent);
}

void ControlledMotor::SetTargetVelocity(float velocity) {
  this->targetVelocity = velocity;
  this->rotationDirection =
      (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
}

void ControlledMotor::Update(bool recurse) {
  encoder->Update(false);

  this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;

  unsigned long currentTimeMs = GetTimeMs();
  float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
  this->lastUpdateTime = currentTimeMs;

  float error = this->targetVelocity - this->actualVelocity;
  float acceleration =
      error * timeStep * pidP;  // Just P is used at this moment
  std::cout << "motor acc. " << acceleration << std::endl;
  motor->SetTargetVelocity(targetVelocity +
                          acceleration);  // or something like that

  motor->Update(false);
}

// float ControlledMotor::GetActualVelocity() {
//   return (int)rotationDirection * encoder->rotationSpeed;
// }

// bool ControlledMotor::Drive(float distance) {
//   if (!driving) {
//     targetDistance = distance;
//     startDistance = encoder->GetDistance();
//     driving = true;
//   }
//   float totalDistance = encoder->GetDistance() - startDistance;
//   bool completed = totalDistance > targetDistance;
//   return completed;
// }
}  // namespace RoboidControl