#include "ServoMotor.h"

#include "LinearAlgebra/FloatSingle.h"

ServoMotor::ServoMotor()
    : Thing(0) {  // for now, id should be set properly later
  this->type = Thing::ServoType;
  this->controlMode = ControlMode::Position;
  this->targetAngle = 0;
  this->hasTargetAngle = false;
}

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

  if (maxVelocity == 0.0F || hasTargetAngle == false) {
    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->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;
}

void ServoMotor::Update(unsigned long currentTimeMs) {
  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
    Thing* child = this->GetChild(childIx);
    if (child != nullptr && child->type == Thing::ServoType) {
      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 (maxVelocity == 0.0F || hasTargetAngle == false) {
      this->lastUpdateTimeMs = currentTimeMs;
      return;

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

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

      this->lastUpdateTimeMs = currentTimeMs;
      return;
    }

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

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

    this->lastUpdateTimeMs = currentTimeMs;
  }
}

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