// src/Integrator.cpp

#include "Integrator.h"
#include "Rocket.h"
#include "ForcesModel.h"
#include "FlightState.h"

#include <array>

// Constructor
Integrator::Integrator(std::shared_ptr<Rocket> rocket,
                       std::shared_ptr<ForcesModel> forcesModel)
    : rocket_(rocket),
      forcesModel_(forcesModel)
{}

// Public Methods

void Integrator::step(FlightState& state, const std::array<double, 3>& netForce, double deltaTime) {

    // Basic Euler integration for now
    eulerIntegration(state, netForce, deltaTime);
}

// Private Methods

void Integrator::eulerIntegration(FlightState& state,
                                   const std::array<double, 3>& netForce,
                                   double deltaTime) {
    // Get current values
    auto position = state.getPosition();
    auto velocity = state.getVelocity();
    auto acceleration = state.getAcceleration();
    double mass = rocket_->getTotalMass();

    // Update acceleration: a = F / m
    if (mass > 0.0) {
        for (int i = 0; i < 3; ++i) {
            acceleration[i] = netForce[i] / mass;
        }
    } else {
        acceleration = {0.0, 0.0, 0.0}; // Safety fallback
    }

    // Update velocity: v = v + a * dt
    for (int i = 0; i < 3; ++i) {
        velocity[i] += acceleration[i] * deltaTime;
    }

    // Update position: x = x + v * dt
    for (int i = 0; i < 3; ++i) {
        position[i] += velocity[i] * deltaTime;
    }

    // Update state
    state.setAcceleration(acceleration);
    state.setVelocity(velocity);
    state.setPosition(position);

}
