#include "fesa/analysis/hht_time_integration.h"

#include <cmath>

#include <gtest/gtest.h>

namespace fesa {
namespace {

TEST(HhtTimeIntegrationTest, KeepsPhysicalAndAlgorithmicAccelerationDistinct) {
  const auto parameters = MakeHhtParameters(-0.05);
  ASSERT_TRUE(parameters.HasValue());
  Vector q(1U);
  Vector v(1U);
  Vector old_a(1U);
  old_a[0U] = 1.0;
  const auto predictor = PredictHht(q, v, old_a, 0.1, parameters.Value());
  ASSERT_TRUE(predictor.HasValue());
  q[0U] = predictor.Value().coordinates[0U] +
          parameters.Value().beta * 0.01 * 2.0;
  const auto rates = EvaluateHhtRates(q, predictor.Value(), old_a, 0.1,
                                     parameters.Value());
  ASSERT_TRUE(rates.HasValue());
  EXPECT_NEAR(rates.Value().algorithmic_acceleration[0U], 2.0, 1e-14);
  EXPECT_NEAR(rates.Value().acceleration[0U], 1.95 / 0.95, 1e-14);
  EXPECT_NEAR(rates.Value().velocity[0U], 0.155, 1e-14);
}

TEST(HhtTimeIntegrationTest, RejectsInvalidParametersAndTime) {
  EXPECT_FALSE(MakeHhtParameters(0.01).HasValue());
  EXPECT_FALSE(MakeHhtParameters(-0.34).HasValue());
  ASSERT_TRUE(MakeHhtParameters(-1.0 / 3.0).HasValue());
  const auto parameters = MakeHhtParameters(0.0);
  ASSERT_TRUE(parameters.HasValue());
  EXPECT_EQ(parameters.Value().beta, 0.25);
  EXPECT_EQ(parameters.Value().gamma, 0.5);
  const Vector zero(1U);
  EXPECT_FALSE(PredictHht(zero, zero, zero, 0.0, parameters.Value()).HasValue());
}

// C02 is a right-continuous STEP load with a nonzero physical a0.
TEST(HhtTimeIntegrationTest, StepOscillatorHasSecondOrderMaximumError) {
  const auto parameters = MakeHhtParameters(-0.05);
  ASSERT_TRUE(parameters.HasValue());
  double previous_q = 0.0;
  double previous_v = 0.0;
  double previous_a = 0.0;
  for (unsigned int power = 4U; power <= 8U; ++power) {
    const unsigned int count = 1U << power;
    const double h = 1.0 / static_cast<double>(count);
    Vector q(1U);
    Vector v(1U);
    Vector a(1U);
    Vector auxiliary(1U);
    a[0U] = auxiliary[0U] = 1.0;
    double error_q = 0.0;
    double error_v = 0.0;
    double error_a = 0.0;
    for (unsigned int n = 1U; n <= count; ++n) {
      const auto predictor = PredictHht(q, v, auxiliary, h, parameters.Value());
      ASSERT_TRUE(predictor.HasValue());
      const double coefficient =
          1.0 / ((1.0 + parameters.Value().alpha) *
                 parameters.Value().beta * h * h);
      q[0U] = (2.0 + 2.0 * coefficient * predictor.Value().coordinates[0U] -
               2.0 * parameters.Value().alpha * a[0U] /
                   (1.0 + parameters.Value().alpha)) /
              (8.0 + 2.0 * coefficient);
      const auto rates = EvaluateHhtRates(q, predictor.Value(), a, h,
                                         parameters.Value());
      ASSERT_TRUE(rates.HasValue());
      v = rates.Value().velocity;
      a = rates.Value().acceleration;
      auxiliary = rates.Value().algorithmic_acceleration;
      const double time = static_cast<double>(n) * h;
      error_q = (std::max)(error_q, std::abs(q[0U] - 0.25 * (1.0 - std::cos(2.0 * time))) / 0.5);
      error_v = (std::max)(error_v, std::abs(v[0U] - 0.5 * std::sin(2.0 * time)) / 0.5);
      error_a = (std::max)(error_a, std::abs(a[0U] - std::cos(2.0 * time)));
    }
    if (power >= 7U) {
      EXPECT_GE(std::log2(previous_q / error_q), 1.8);
      EXPECT_GE(std::log2(previous_v / error_v), 1.8);
      EXPECT_GE(std::log2(previous_a / error_a), 1.8);
    }
    if (power == 8U) {
      EXPECT_LT(error_q, 1e-4);
      EXPECT_LT(error_v, 1e-4);
      EXPECT_LT(error_a, 1e-4);
    }
    previous_q = error_q;
    previous_v = error_v;
    previous_a = error_a;
  }
}

}  // namespace
}  // namespace fesa