#include "Quadcopter.h"

Quadcopter::Quadcopter() {}

void Quadcopter::SetTwistSpeed(float forward, float yaw) {
  this->velocity = Vector3::forward * forward;
  this->yawSpeed = yaw;
}

void Quadcopter::SetTwistSpeed(Vector2 linear, float yaw) {
  this->velocity = Vector3(linear.x, 0.0F, linear.y);
  this->yawSpeed = yaw;
}

void Quadcopter::SetTwistSpeed(Vector3 velocity,
                               float yaw,
                               float pitch,
                               float roll) {
  this->velocity = velocity;
  this->yawSpeed = yaw;
  this->rollSpeed = roll;
  this->pitchSpeed = pitch;
}

Vector3 Quadcopter::GetTargetVelocity() {
  return this->velocity;
}

float Quadcopter::GetPitchSpeed() {
  return this->pitchSpeed;
}

float Quadcopter::GetYawSpeed() {
  return this->yawSpeed;
}
float Quadcopter::GetRollSpeed() {
  return this->rollSpeed;
}