#include "Motor.h"

#include <algorithm> // for std::lower_bound

// Constructor
Motor::Motor(const std::string& name, double initialPropellantMass, double totalImpulse)
    : name_(name),
      initialPropellantMass_(initialPropellantMass),
      remainingPropellantMass_(initialPropellantMass),
      totalImpulse_(totalImpulse),
      ignitionTime_(0.0),
      ignited_(false),
      burnedOut_(false),
      currentThrust_(0.0)
{}

// Public Methods

void Motor::addThrustDataPoint(double time, double thrust) {
    thrustCurve_.emplace_back(time, thrust);
}

void Motor::ignite() {
    ignitionTime_ = 0.0;
    ignited_ = true;
    burnedOut_ = false;
}

void Motor::update(double deltaTime) {
    if (!ignited_ || burnedOut_) {
        currentThrust_ = 0.0;
        return;
    }

    ignitionTime_ += deltaTime;

    // Check if we exceed thrust curve duration
    if (!thrustCurve_.empty() && ignitionTime_ > thrustCurve_.back().first) {
        burnedOut_ = true;
        currentThrust_ = 0.0;
        remainingPropellantMass_ = 0.0;
        return;
    }

    // Update current thrust using interpolation
    currentThrust_ = interpolateThrust(ignitionTime_);

    // Estimate propellant burn proportional to impulse usage
    double impulsePerSecond = totalImpulse_ / (thrustCurve_.back().first); // approximate
    double propellantConsumptionRate = initialPropellantMass_ / (totalImpulse_ / impulsePerSecond);
    double deltaPropellant = propellantConsumptionRate * deltaTime;

    remainingPropellantMass_ = std::max(remainingPropellantMass_ - deltaPropellant, 0.0);
}

double Motor::getCurrentThrust() const {
    return currentThrust_;
}

bool Motor::isBurnedOut() const {
    return burnedOut_;
}

double Motor::getRemainingPropellantMass() const {
    return remainingPropellantMass_;
}

const std::string& Motor::getName() const {
    return name_;
}

// Private Methods

double Motor::interpolateThrust(double time) const {
    if (thrustCurve_.empty()) {
        return 0.0;
    }

    // If time is outside bounds
    if (time <= thrustCurve_.front().first) {
        return thrustCurve_.front().second;
    }
    if (time >= thrustCurve_.back().first) {
        return thrustCurve_.back().second;
    }

    // Find the two points around the current time
    auto it = std::lower_bound(thrustCurve_.begin(), thrustCurve_.end(), std::make_pair(time, 0.0),
        [](const std::pair<double, double>& a, const std::pair<double, double>& b) {
            return a.first < b.first;
        });

    if (it == thrustCurve_.begin()) {
        return it->second;
    }

    auto itPrev = std::prev(it);

    double t0 = itPrev->first;
    double t1 = it->first;
    double f0 = itPrev->second;
    double f1 = it->second;

    // Linear interpolation
    double thrust = f0 + (f1 - f0) * (time - t0) / (t1 - t0);
    return thrust;
}