#include "ControlledMotor.h"
#include "LinearAlgebra/FloatSingle.h"

namespace RoboidControl {

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

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

void ControlledMotor::Update(bool recurse) {
  unsigned long currentTimeMs = GetTimeMs();
  float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
  this->lastUpdateTime = currentTimeMs;
  if (timeStep <= 0)
    return;

  // encoder->Update(false);

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

  float error = this->targetVelocity - this->actualVelocity;
  float p_term = error * pidP;
  this->integral += error * timeStep;
  float i_term = pidI * this->integral;
  float d_term = pidD * (error - this->lastError) / timeStep;
  this->lastError = error;

  float output = p_term + i_term + d_term;
  std::cout << "target " << this->targetVelocity << " actual "
            << this->actualVelocity << " output = " << output << std::endl;
  // float acceleration =
  //     error * timeStep * pidP;  // Just P is used at this moment
  // std::cout << "motor acc. " << acceleration << std::endl;

  // float newTargetVelocity = motor->targetVelocity + acceleration;
  output = LinearAlgebra::Float::Clamp(output, -1, 1);
  motor->SetTargetVelocity(output);  // 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