// tests/test_flightstate.cpp

#include "vendor/catch_amalgamated.hpp"
#include "FlightState.h"

// Bring Approx into scope
using Catch::Approx;

TEST_CASE("FlightState basic behavior", "[flightstate]") {
    FlightState state;

    SECTION("Initial state values are zero or identity") {
        REQUIRE(state.getPosition()[0] == Approx(0.0));
        REQUIRE(state.getVelocity()[0] == Approx(0.0));
        REQUIRE(state.getAcceleration()[0] == Approx(0.0));
        REQUIRE(state.getOrientation()[0] == Approx(1.0)); // w component of unit quaternion
        REQUIRE(state.getOrientation()[1] == Approx(0.0));
        REQUIRE(state.getTime() == Approx(0.0));
    }

    SECTION("Can set and get position") {
        std::array<double, 3> pos = {10.0, 20.0, 30.0};
        state.setPosition(pos);

        REQUIRE(state.getPosition()[0] == Approx(10.0));
        REQUIRE(state.getPosition()[1] == Approx(20.0));
        REQUIRE(state.getPosition()[2] == Approx(30.0));
    }

    SECTION("Can set and get velocity") {
        std::array<double, 3> vel = {1.0, 2.0, 3.0};
        state.setVelocity(vel);

        REQUIRE(state.getVelocity()[0] == Approx(1.0));
        REQUIRE(state.getVelocity()[1] == Approx(2.0));
        REQUIRE(state.getVelocity()[2] == Approx(3.0));
    }

    SECTION("Can set and get acceleration") {
        std::array<double, 3> acc = {-1.0, -2.0, -3.0};
        state.setAcceleration(acc);

        REQUIRE(state.getAcceleration()[0] == Approx(-1.0));
        REQUIRE(state.getAcceleration()[1] == Approx(-2.0));
        REQUIRE(state.getAcceleration()[2] == Approx(-3.0));
    }

    SECTION("Can set and get orientation") {
        std::array<double, 4> quat = {0.707, 0.0, 0.707, 0.0}; // 90? rotation around Y axis
        state.setOrientation(quat);

        REQUIRE(state.getOrientation()[0] == Approx(0.707));
        REQUIRE(state.getOrientation()[2] == Approx(0.707));
    }

    SECTION("Can set and get angular velocity") {
        std::array<double, 3> angVel = {0.01, 0.02, 0.03};
        state.setAngularVelocity(angVel);

        REQUIRE(state.getAngularVelocity()[0] == Approx(0.01));
        REQUIRE(state.getAngularVelocity()[1] == Approx(0.02));
        REQUIRE(state.getAngularVelocity()[2] == Approx(0.03));
    }

    SECTION("Can set and get time") {
        state.setTime(5.5);

        REQUIRE(state.getTime() == Approx(5.5));
    }
}