#include "ServoMotor.h"

#include "LinearAlgebra/FloatSingle.h"

ServoMotor::ServoMotor() : Thing() {
  this->type = (unsigned char)Type::Servo;
  this->controlMode = ControlMode::Position;
  this->targetAngle = Angle16();
  this->hasTargetAngle = false;
}

void ServoMotor::SetTargetAngle(Angle16 angle) {
  angle = Angle16::Clamp(angle, minAngle, maxAngle);

  if (maxSpeed == 0.0F) {
    SetAngle(angle);
    this->limitedTargetAngle = angle;
  }

  this->controlMode = ControlMode::Position;
  this->targetAngle = angle;
  this->hasTargetAngle = true;
}

Angle16 ServoMotor::GetTargetAngle() { return this->targetAngle; }

void ServoMotor::SetMaximumVelocity(float maxVelocity) {
  this->maxSpeed = maxVelocity;
}

void ServoMotor::SetTargetVelocity(float targetVelocity) {
  targetVelocity = Float::Clamp(targetVelocity, -this->maxSpeed, maxSpeed);

  this->controlMode = ControlMode::Velocity;
  this->targetVelocity = targetVelocity;
  this->hasTargetAngle = false; // can't we use the controlMode for this?
}

float ServoMotor::GetTargetVelocity() { return this->targetVelocity; }

void ServoMotor::Update(unsigned long currentTimeMs) {
  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
    Thing *child = this->GetChildByIndex(childIx);
    if (child != nullptr && child->type == (unsigned char)Type::Servo) {
      ServoMotor *servo = (ServoMotor *)child;
      servo->Update(currentTimeMs);
    }
  }

  if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) {
    this->lastUpdateTimeMs = currentTimeMs;
    return;
  }

  float deltaTime = (currentTimeMs - this->lastUpdateTimeMs) / 1000.0F;

  // Position control
  if (controlMode == ControlMode::Position) {
    if (maxSpeed == 0.0F || hasTargetAngle == false) {
      this->lastUpdateTimeMs = currentTimeMs;
      return;

    } else {
      float angleStep = maxSpeed * deltaTime;
      float deltaAngle =
          this->targetAngle.InDegrees() - this->limitedTargetAngle.InDegrees();
      float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle;

      if (absDeltaAngle < angleStep) {
        this->limitedTargetAngle = this->targetAngle;
        SetAngle(targetAngle);
      } else {
        if (deltaAngle < 0)
          limitedTargetAngle = limitedTargetAngle - Angle16::Degrees(angleStep);
        else
          limitedTargetAngle = limitedTargetAngle + Angle16::Degrees(angleStep);
      }
      SetAngle(limitedTargetAngle);

      this->lastUpdateTimeMs = currentTimeMs;
      return;
    }

    // Velocity control
  } else {
    float newAngle = this->targetAngle.InDegrees() + targetVelocity * deltaTime;
    newAngle =
        Float::Clamp(newAngle, minAngle.InDegrees(), maxAngle.InDegrees());

    Angle16 targetAngle = Angle16::Degrees(newAngle);
    ServoMotor::SetTargetAngle(targetAngle);
    SetAngle(targetAngle);

    this->lastUpdateTimeMs = currentTimeMs;
  }
}

void ServoMotor::SetAngle(Angle16 angle) { this->actualAngle = angle; };