// src/FlightSimulator.cpp

#include "FlightSimulator.h"
#include "Rocket.h"
#include "Environment.h"
#include "ForcesModel.h"
#include "FlightState.h"
#include "Integrator.h"
#include "Stage.h"
#include "Motor.h"

#include <iostream> // Optional for debug output

// Constructor
FlightSimulator::FlightSimulator(std::shared_ptr<Rocket> rocket,
                                 std::shared_ptr<Environment> environment)
    : rocket_(rocket),
      environment_(environment),
      forcesModel_(std::make_unique<ForcesModel>(rocket, environment)),
      integrator_(std::make_unique<Integrator>(rocket, forcesModel_)),
      hasLaunched_(false) // <-- initialize launch flag
{}

// Public Methods

void FlightSimulator::run(double maxSimulationTime, double timeStep) {
    initialize();

    FlightState state;
    state.setTime(0.0);

    // Initial position: launch pad
    state.setPosition({0.0, 0.0, environment_->getLaunchAltitude()});
    state.setVelocity({0.0, 0.0, 0.0});
    state.setAcceleration({0.0, 0.0, 0.0});

    flightLog_.clear();
    flightLog_.push_back(state);

    double currentTime = 0.0;

    while (currentTime < maxSimulationTime) {
        step(timeStep);

        const auto& latestState = flightLog_.back();
        if (checkTermination(latestState)) {
            break;
        }

        currentTime += timeStep;
    }
}

const std::vector<FlightState>& FlightSimulator::getFlightLog() const {
    return flightLog_;
}

// Private Methods

void FlightSimulator::initialize() {
    rocket_->prepareForFlight(*environment_);
}

void FlightSimulator::step(double deltaTime) {
    FlightState currentState = flightLog_.back(); // Copy latest state

    updateMotors(deltaTime); // <-- motors must advance ignition timers

    // Compute forces and integrate motion
    auto netForce = forcesModel_->computeNetForce(currentState);
    integrator_->step(currentState, netForce, deltaTime);

    rocket_->applyFlightState(currentState);

    handleEvents(currentState);

    currentState.setTime(currentState.getTime() + deltaTime);

    flightLog_.push_back(currentState);
}

void FlightSimulator::updateMotors(double deltaTime) {
    for (const auto& stage : rocket_->getStages()) {
        for (auto& motor : stage->getMotorsForTesting()) {
            motor->update(deltaTime);
        }
    }
}

void FlightSimulator::handleEvents(FlightState& /*state*/) {
    // TODO: Extend this to handle recovery deployment and staging events.
}

bool FlightSimulator::checkTermination(const FlightState& state) {
    const double altitude = state.getPosition()[2];
    const double verticalVelocity = state.getVelocity()[2];

    if (!hasLaunched_) {
        // Detect real liftoff: significant velocity or altitude gain
        if (verticalVelocity > 5.0 || altitude > environment_->getLaunchAltitude() + 5.0) {
            hasLaunched_ = true;
        }
        return false; // Always continue until launched
    }

    // After launch, normal landing detection
    if (altitude <= environment_->getLaunchAltitude() + 1.0 && verticalVelocity <= 1.0) {
        return true; // Landed
    }

    return false; // Still flying
}
