#include "fesa/analysis/hht_time_integration.h"

#include <cmath>
#include <utility>

namespace fesa {
namespace {

/// @brief Reports invalid or unrepresentable HHT kinematics.
Status Failure() {
  return Status::Failure(
      FailureCategory::kSolver,
      {{Severity::kError, "dynamic-invalid-state", {}, "DYNAMIC", {},
        "HHT requires finite compatible rates, positive time, and coupled "
        "coefficients in the supported range."}});
}

/// @brief Validates the coupled coefficient and time denominators.
bool Valid(const double h, const HhtParameters& p) {
  return std::isfinite(h) && h > 0.0 && std::isfinite(h * h) &&
         h * h > 0.0 && std::isfinite(p.alpha) &&
         p.alpha >= -1.0 / 3.0 && p.alpha <= 0.0 &&
         p.beta == 0.25 * (1.0 - p.alpha) * (1.0 - p.alpha) &&
         p.gamma == 0.5 - p.alpha && p.beta * h * h > 0.0;
}

}  // namespace

Result<HhtParameters> MakeHhtParameters(const double alpha) {
  if (!std::isfinite(alpha) || alpha < -1.0 / 3.0 || alpha > 0.0) {
    return Result<HhtParameters>::Failure(Failure());
  }
  return Result<HhtParameters>::Success(
      {alpha, 0.25 * (1.0 - alpha) * (1.0 - alpha), 0.5 - alpha});
}

Result<HhtPredictor> PredictHht(const Vector& coordinates,
                              const Vector& velocity,
                              const Vector& algorithmic_acceleration,
                              const double time_increment,
                              const HhtParameters& parameters) {
  if (!Valid(time_increment, parameters) ||
      coordinates.Size() != velocity.Size() ||
      coordinates.Size() != algorithmic_acceleration.Size()) {
    return Result<HhtPredictor>::Failure(Failure());
  }
  HhtPredictor predictor{Vector(coordinates.Size()), Vector(coordinates.Size())};
  for (std::size_t i = 0U; i < coordinates.Size(); ++i) {
    predictor.coordinates[i] =
        coordinates[i] + time_increment * velocity[i] +
        time_increment * time_increment * (0.5 - parameters.beta) *
            algorithmic_acceleration[i];
    predictor.velocity[i] =
        velocity[i] + time_increment * (1.0 - parameters.gamma) *
                          algorithmic_acceleration[i];
    if (!std::isfinite(predictor.coordinates[i]) ||
        !std::isfinite(predictor.velocity[i])) {
      return Result<HhtPredictor>::Failure(Failure());
    }
  }
  return Result<HhtPredictor>::Success(std::move(predictor));
}

Result<HhtRates> EvaluateHhtRates(const Vector& coordinates,
                                const HhtPredictor& predictor,
                                const Vector& old_acceleration,
                                const double time_increment,
                                const HhtParameters& parameters) {
  if (!Valid(time_increment, parameters) ||
      coordinates.Size() != predictor.coordinates.Size() ||
      coordinates.Size() != predictor.velocity.Size() ||
      coordinates.Size() != old_acceleration.Size()) {
    return Result<HhtRates>::Failure(Failure());
  }
  HhtRates rates{Vector(coordinates.Size()), Vector(coordinates.Size()),
                 Vector(coordinates.Size())};
  for (std::size_t i = 0U; i < coordinates.Size(); ++i) {
    const double increment = coordinates[i] - predictor.coordinates[i];
    rates.algorithmic_acceleration[i] =
        increment / (parameters.beta * time_increment * time_increment);
    rates.velocity[i] = predictor.velocity[i] +
                        parameters.gamma * increment /
                            (parameters.beta * time_increment);
    rates.acceleration[i] =
        (rates.algorithmic_acceleration[i] +
         parameters.alpha * old_acceleration[i]) /
        (1.0 + parameters.alpha);
    if (!std::isfinite(rates.algorithmic_acceleration[i]) ||
        !std::isfinite(rates.acceleration[i]) ||
        !std::isfinite(rates.velocity[i])) {
      return Result<HhtRates>::Failure(Failure());
    }
  }
  return Result<HhtRates>::Success(std::move(rates));
}

}  // namespace fesa