#include "ControlledMotor.h"

ControlledMotor::ControlledMotor() {
  this->type = (unsigned char)Type::ControlledMotor;
}

ControlledMotor::ControlledMotor(Motor *motor, Encoder *encoder)
    : ControlledMotor() {
  this->motor = motor;
  this->encoder = encoder;
  // this->rotationDirection = Direction::Forward;
}

#include <Arduino.h>

void ControlledMotor::SetTargetSpeed(float speed) {
  this->currentTargetSpeed = speed;
  // this->rotationDirection =
  //     (targetSpeed < 0) ? Direction::Reverse : Direction::Forward;
  // this->direction = (targetSpeed < 0) ? Motor::Direction::CounterClockwise
  //                                     : Motor::Direction::Clockwise;
}

void ControlledMotor::Update(unsigned long currentTimeMs) {
  this->actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs);
  if (this->currentTargetSpeed < 0)
    this->actualSpeed = -this->actualSpeed;

  float deltaTime = currentTimeMs - this->lastUpdateTime;

  float error = this->currentTargetSpeed - this->actualSpeed;
  float errorChange = (error - lastError) * deltaTime;

  float delta = error * pidP + errorChange * pidD;

  Serial.print("  actual Speed ");
  Serial.print(actualSpeed);
  Serial.print("  target Speed ");
  Serial.print(this->currentTargetSpeed);
  Serial.print("    motor target speed ");
  Serial.print(motor->currentTargetSpeed);
  Serial.print(" + ");
  Serial.print(error * pidP);
  Serial.print(" + ");
  Serial.print(errorChange * pidD);
  Serial.print(" = ");
  Serial.println(motor->currentTargetSpeed + delta);

  motor->SetTargetSpeed(motor->currentTargetSpeed +
                        delta); // or something like that
  this->lastUpdateTime = currentTimeMs;
}

float ControlledMotor::GetActualSpeed() { return actualSpeed; }

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;
}