#include "ServoMotor.h"

#include "VectorAlgebra/FloatSingle.h"

ServoMotor::ServoMotor() {
  this->type = Thing::ServoType;
  this->controlMode = ControlMode::Position;
  this->targetAngle = 0;
  this->hasTargetAngle = false;
}

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

  if (maxVelocity == 0.0F || hasTargetAngle == false) {
    SetAngle(angle);
    this->limitedTargetAngle = angle;
  } else if (angle != this->targetAngle) {
    // if the new target is the same, the limited angle is not overwritten
    this->limitedTargetAngle = this->targetAngle;
  }

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

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

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

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

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

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

#include <Arduino.h>

void ServoMotor::Update(float currentTimeMs) {
  if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) {
    this->lastUpdateTimeMs = currentTimeMs;
    return;
  }

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

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

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

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

      this->lastUpdateTimeMs = currentTimeMs;
      return;
    }

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

    ServoMotor::SetTargetAngle(newAngle);
    SetAngle(newAngle);

    this->lastUpdateTimeMs = currentTimeMs;
  }

  Serial.println(this->childCount);
  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
    Thing *child = this->GetChild(childIx);
    if (child->type == Thing::ServoType) {
      ServoMotor *servo = (ServoMotor *)child;
      servo->Update(currentTimeMs);
    }
  }
}