// src/FlightState.cpp

#include "FlightState.h"

// Constructor
FlightState::FlightState()
    : position_{0.0, 0.0, 0.0},
      velocity_{0.0, 0.0, 0.0},
      acceleration_{0.0, 0.0, 0.0},
      orientation_{1.0, 0.0, 0.0, 0.0}, // Unit quaternion by default
      angularVelocity_{0.0, 0.0, 0.0},
      time_(0.0)
{}

// Public Methods

const std::array<double, 3>& FlightState::getPosition() const {
    return position_;
}

void FlightState::setPosition(const std::array<double, 3>& pos) {
    position_ = pos;
}

const std::array<double, 3>& FlightState::getVelocity() const {
    return velocity_;
}

void FlightState::setVelocity(const std::array<double, 3>& vel) {
    velocity_ = vel;
}

const std::array<double, 3>& FlightState::getAcceleration() const {
    return acceleration_;
}

void FlightState::setAcceleration(const std::array<double, 3>& acc) {
    acceleration_ = acc;
}

const std::array<double, 4>& FlightState::getOrientation() const {
    return orientation_;
}

void FlightState::setOrientation(const std::array<double, 4>& quat) {
    orientation_ = quat;
}

const std::array<double, 3>& FlightState::getAngularVelocity() const {
    return angularVelocity_;
}

void FlightState::setAngularVelocity(const std::array<double, 3>& angVel) {
    angularVelocity_ = angVel;
}

double FlightState::getTime() const {
    return time_;
}

void FlightState::setTime(double time) {
    time_ = time;
}
