// examples/basic_flight_simulation.cpp

#include <iostream>
#include <memory>

// Include headers
#include "Rocket.h"
#include "Stage.h"
#include "Motor.h"
#include "Airframe.h"
#include "FinSet.h"
#include "RecoverySystem.h"
#include "FlightSimulator.h"
#include "Environment.h"
#include "FlightState.h"
#include "ForcesModel.h"
#include "Integrator.h"

#include "plotting/MatplotlibPlotter.h"

int main() {
    // Create environment: sea level, 15?C
    auto environment = std::make_shared<Environment>(
        101325.0, // surface pressure (Pa)
        288.15,   // surface temperature (K)
        0.0       // launch altitude (m)
    );

    // Build Rocket
    auto rocket = std::make_shared<Rocket>("Test Rocket");

    // Build Stage
    auto stage = std::make_unique<Stage>("First Stage");

    // Add Motor
    auto motor = std::make_unique<Motor>("F50",
        0.050,    // 50 grams of propellant
        50.0      // total impulse (Ns)
    );
    // Simple thrust curve (linear ramp-up and down)
    motor->addThrustDataPoint(0.0, 0.0);
    motor->addThrustDataPoint(0.1, 50.0);
    motor->addThrustDataPoint(1.5, 50.0);
    motor->addThrustDataPoint(1.6, 0.0);

    stage->addMotor(std::move(motor));

    // Add Airframe
    auto airframe = std::make_unique<Airframe>("Standard Tube",
        1.0,     // length (m)
        0.1,     // diameter (m)
        1.2,     // dry mass (kg)
        0.75     // drag coefficient (unitless)
    );
    stage->setAirframe(std::move(airframe));

    // Add FinSet
    auto finset = std::make_unique<FinSet>("Tri-Fins",
        3,       // number of fins
        0.15,    // root chord (m)
        0.10,    // tip chord (m)
        0.10,    // span (m)
        0.05,    // sweep length (m)
        0.003,   // thickness (m)
        500.0    // material density (kg/m3)
    );
    stage->setFinSet(std::move(finset));

    // Add Recovery System
    auto recovery = std::make_unique<RecoverySystem>("Main Parachute",
        RecoverySystem::DeploymentType::Apogee,
        0.0,    // trigger value not needed for apogee deploy
        1.5,    // drag coefficient (very large for parachutes)
        1.0     // reference area (m2)
    );
    stage->setRecoverySystem(std::move(recovery));

    // Finalize Rocket
    rocket->addStage(std::move(stage));
    rocket->updateMassProperties();

    // Setup Simulation
    FlightSimulator simulator(rocket, environment);

    // Run simulation
    simulator.run(30.0, 0.01); // 30 seconds max, 10ms timestep

    // Output simple telemetry
    const auto& flightLog = simulator.getFlightLog();
    MatplotlibPlotter plotter;

    // Build flight data vectors
    std::vector<double> time, altitude, velocity, acceleration;
    for (const auto& state : flightLog) {
        time.push_back(state.getTime());
        altitude.push_back(state.getPosition()[2]);
        velocity.push_back(state.getVelocity()[2]);
        acceleration.push_back(state.getAcceleration()[2]);
    }
    
    // --- Detect Apogee ---
    double maxAltitude = 0.0;
    double apogeeTime = 0.0;
    for (const auto& state : flightLog) {
        double alt = state.getPosition()[2];
        if (alt > maxAltitude) {
            maxAltitude = alt;
            apogeeTime = state.getTime();
        }
    }
    plotter.addMarker(apogeeTime, "Apogee");
    
    // --- Detect Motor Burnout ---
    // (Simple version: find when thrust becomes near zero)
    
    bool burnoutDetected = false;
    for (size_t i = 0; i < flightLog.size(); ++i) {
        if (i == 0) continue; // skip first entry
    
        double velPrev = flightLog[i-1].getVelocity()[2];
        double velCurr = flightLog[i].getVelocity()[2];
    
        // If velocity was increasing and then starts decreasing significantly
        if (velPrev > velCurr + 1.0) { // "significant" drop
            plotter.addMarker(flightLog[i].getTime(), "Motor Burnout (inferred)");
            burnoutDetected = true;
            break;
        }
    }
    
    if (!burnoutDetected) {
        std::cout << "Warning: Motor burnout not detected automatically.\n";
    }
    
    // --- Plot Altitude vs Time ---
    plotter.plot2D(time, altitude, "Altitude vs Time", "Time (s)", "Altitude (m)");
    
    // --- Plot Velocity vs Time ---
    plotter.plot2D(time, velocity, "Velocity vs Time", "Time (s)", "Velocity (m/s)");
    
    // --- Plot Acceleration vs Time ---
    plotter.plot2D(time, acceleration, "Acceleration vs Time", "Time (s)", "Acceleration (m/s2)");

    plotter.plotPosVelAcc(time, altitude, velocity, acceleration);

    return 0;
}