#include "ServoMotor.h"

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

namespace RoboidControl {

ServoMotor::ServoMotor(Participant* participant) : Thing(participant, Thing::Servo, 0) {
  this->controlMode = ControlMode::Position;
  this->targetAngle = Angle16();
  this->hasTargetAngle = false;
}

ServoMotor::ServoMotor(Thing* parent) : ServoMotor(parent->owner) {
  std::cout << "new parent " << parent->name << " owner " << parent->owner << std::endl;
  this->parent = parent;
}

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, bool recurse) {
  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
    Thing* child = this->GetChild(childIx);
    if (child != nullptr && child->type == Thing::Servo) {
      ServoMotor* servo = (ServoMotor*)child;
      servo->Update(currentTimeMs, recurse);
    }
  }

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

}  // namespace RoboidControl