#include <gtest/gtest.h>

#include <algorithm>
#include <cmath>
#include <iostream>
#include <limits>
#include <vector>

#include "fesa/analysis/analysis_model.h"
#include "fesa/analysis/solid_hht_dynamic_procedure.h"
#include "fesa/assembly/parallel_for.h"
#include "fesa/elements/hex8_element.h"
#include "fesa/solvers/linear/mkl_pardiso_solver.h"
#include "fesa/solvers/linear/mkl_pardiso_tangent_solver.h"
#include "solid_hht_test_fixture.h"

namespace {
using M3 = std::array<std::array<double, 3>, 3>;
struct AffineMotion {
  M3 b{}, velocity{}, acceleration{}, piola{};
};
AffineMotion Motion(double time, bool nonlinear) {
  const double q = 1 - std::cos(2 * time);
  const double dq = 2 * std::sin(2 * time);
  const double ddq = 4 * std::cos(2 * time);
  AffineMotion m;
  constexpr double lambda = 4.8;
  constexpr double mu = 4.8;
  if (!nonlinear) {
    const M3 d{{{.015, .008, -.004}, {.006, -.01, .003}, {.005, .004, .012}}};
    double trace = 0;
    for (unsigned i = 0; i < 3; ++i) trace += q * d[i][i];
    for (unsigned i = 0; i < 3; ++i)
      for (unsigned j = 0; j < 3; ++j) {
        m.b[i][j] = q * d[i][j];
        m.velocity[i][j] = dq * d[i][j];
        m.acceleration[i][j] = ddq * d[i][j];
        m.piola[i][j] =
            (i == j ? lambda * trace : 0) + mu * q * (d[i][j] + d[j][i]);
      }
  } else {
    const double s = .004 * q, ds = .004 * dq, dds = .004 * ddq;
    const double theta = .5 * q, dt = .5 * dq, ddt = .5 * ddq;
    const double c = std::cos(theta), sn = std::sin(theta);
    const M3 r{{{c, -sn, 0}, {sn, c, 0}, {0, 0, 1}}};
    const M3 jr{{{-sn, -c, 0}, {c, -sn, 0}, {0, 0, 0}}};
    const M3 jjr{{{-c, sn, 0}, {-sn, -c, 0}, {0, 0, 0}}};
    const double eg = .5 * ((1 + s) * (1 + s) - 1);
    const double stress = (3 * lambda + 2 * mu) * eg;
    for (unsigned i = 0; i < 3; ++i)
      for (unsigned j = 0; j < 3; ++j) {
        m.b[i][j] = (1 + s) * r[i][j] - (i == j ? 1 : 0);
        m.velocity[i][j] = ds * r[i][j] + (1 + s) * dt * jr[i][j];
        m.acceleration[i][j] = dds * r[i][j] + 2 * ds * dt * jr[i][j] +
                               (1 + s) * (ddt * jr[i][j] + dt * dt * jjr[i][j]);
        m.piola[i][j] = (1 + s) * r[i][j] * stress;
      }
  }
  return m;
}
std::array<double, 3> Apply(const M3& matrix, const std::array<double, 3>& x) {
  std::array<double, 3> result{};
  for (unsigned i = 0; i < 3; ++i)
    for (unsigned j = 0; j < 3; ++j) result[i] += matrix[i][j] * x[j];
  return result;
}
fesa::ModelDefinition TwoBricks(double h, unsigned count, bool nonlinear) {
  auto d = solid_hht_test::Brick(h, count, nonlinear);
  const std::array<std::array<double, 3>, 4> added{
      {{2, 0, 0}, {2, 1, 0}, {2, 0, 1}, {2, 1, 1}}};
  for (unsigned j = 0; j < 4; ++j) {
    unsigned n = 8 + j;
    d.nodes.push_back({{"I", n + 1, std::to_string(n + 1)}, added[j], {}});
    d.node_sets.push_back({"N" + std::to_string(n), {}, {n}, {}});
  }
  d.solid_elements.emplace_back(
      fesa::SourceEntityId{"I", 9, "9"},
      std::array<fesa::EntityIndex, 8>{1, 8, 9, 2, 5, 10, 11, 6}, 0, 0,
      fesa::SourceLocation{});
  return d;
}
// Closed polynomial brick integrals: integral Na*X and integral grad Na.
// These do not call the production stiffness, mass, quadrature or response.
std::vector<double> ExactForce(const fesa::ModelDefinition& d, double t,
                               bool nonlinear) {
  const auto m = Motion(t, nonlinear);
  std::vector<double> f(3 * d.nodes.size());
  for (unsigned e = 0; e < 2; ++e) {
    const auto& nodes = d.solid_elements[e].node_indices;
    for (unsigned a = 0; a < 8; ++a) {
      const auto& x = d.nodes[nodes[a]].coordinates;
      std::array<double, 3> integral_x{}, gradient{};
      for (unsigned c = 0; c < 3; ++c) {
        const double origin = c == 0 ? static_cast<double>(e) : 0;
        const double local = x[c] - origin;
        integral_x[c] = 2 * (origin / 8 + (1 + local) / 24);
        gradient[c] = (2 * local - 1) / 4;
      }
      const auto inertia = Apply(m.acceleration, integral_x);
      const auto internal = Apply(m.piola, gradient);
      for (unsigned c = 0; c < 3; ++c)
        f[3 * nodes[a] + c] += inertia[c] + internal[c];
    }
  }
  return f;
}
std::vector<double> ExactMassProduct(const fesa::Domain& d,
                                     const fesa::Vector& v) {
  std::vector<double> result(v.Size());
  for (unsigned e = 0; e < 2; ++e) {
    const auto& nodes = d.SolidElements()[e].node_indices;
    for (unsigned a = 0; a < 8; ++a)
      for (unsigned b = 0; b < 8; ++b) {
        double coefficient = 2.0 / 216;
        for (unsigned c = 0; c < 3; ++c)
          coefficient *= d.Nodes()[nodes[a]].coordinates[c] ==
                                 d.Nodes()[nodes[b]].coordinates[c]
                             ? 2
                             : 1;
        for (unsigned c = 0; c < 3; ++c)
          result[3 * nodes[a] + c] += coefficient * v[3 * nodes[b] + c];
      }
  }
  return result;
}
void AddCross(const std::array<double, 3>& x, const std::array<double, 3>& f,
              std::array<double, 3>* result) {
  (*result)[0] += x[1] * f[2] - x[2] * f[1];
  (*result)[1] += x[2] * f[0] - x[0] * f[2];
  (*result)[2] += x[0] * f[1] - x[1] * f[0];
}
std::array<double, 3> ManufacturedRun(double h, unsigned count,
                                      bool nonlinear,
                                      bool compare_parallel = false) {
  auto d = TwoBricks(h, count, nonlinear);
  for (unsigned i = 0; i < 3 * d.nodes.size(); ++i) {
    fesa::TabularAmplitudeDefinition amplitude;
    amplitude.name_raw = "A" + std::to_string(i);
    amplitude.name_canonical = amplitude.name_raw;
    for (unsigned n = 0; n <= count; ++n) {
      const double t = static_cast<double>(n) * h;
      amplitude.points.push_back({t, ExactForce(d, t, nonlinear)[i]});
    }
    d.amplitudes.push_back(amplitude);
    d.dynamic_step->loads.push_back({"N" + std::to_string(i / 3),
                                     static_cast<int>(i % 3 + 1),
                                     1,
                                     {},
                                     i,
                                     amplitude.name_raw,
                                     {}});
  }
  auto domain = fesa::Domain::Create(std::move(d));
  EXPECT_TRUE(domain.HasValue());
  if (!domain.HasValue()) return {1, 1, 1};
  fesa::SerialParallelFor parallel;
  fesa::MklPardisoSolver mass;
  fesa::MklPardisoTangentSolver tangent;
  auto model = fesa::AnalysisModel::Create(domain.Value());
  auto dofs = fesa::DofManager::Create(model.Value());
  auto state =
      fesa::SolidDynamicAnalysisState::Create(domain.Value(), dofs.Value());
  auto status = fesa::SolidHhtDynamicProcedureExecutor(parallel, mass, tangent)
                    .ExecuteInto(domain.Value(), state.Value());
  EXPECT_TRUE(status.IsOk()) << solid_hht_test::Message(status);
  if (!status.IsOk()) {
    const auto& e = state.Value().IterationRecords().back().evidence;
    std::cout << "Failed force metric " << e.residual_metric << " update "
              << e.update_metric << '\n';
    return {1, 1, 1};
  }
  if (count == 30 || compare_parallel) {
    // Both element workers have distinct index-owned buffers; compare every
    // accepted field, not merely the final displacement norm.
    auto parallel_state =
        fesa::SolidDynamicAnalysisState::Create(domain.Value(), dofs.Value());
    fesa::TbbParallelFor tbb;
    fesa::MklPardisoSolver parallel_mass;
    fesa::MklPardisoTangentSolver parallel_tangent;
    auto parallel_status =
        fesa::SolidHhtDynamicProcedureExecutor(tbb, parallel_mass,
                                               parallel_tangent)
            .ExecuteInto(domain.Value(), parallel_state.Value());
    EXPECT_TRUE(parallel_status.IsOk())
        << solid_hht_test::Message(parallel_status);
    if (parallel_status.IsOk()) {
      const auto& serial_frames = state.Value().AcceptedFrames();
      const auto& parallel_frames = parallel_state.Value().AcceptedFrames();
      EXPECT_EQ(serial_frames.size(), parallel_frames.size());
      bool has_nonzero_low = false;
      for (std::size_t n = 0; n < serial_frames.size(); ++n) {
        const auto& sf = serial_frames[n];
        const auto& pf = parallel_frames[n];
        EXPECT_DOUBLE_EQ(sf.kinetic_energy, pf.kinetic_energy);
        EXPECT_DOUBLE_EQ(sf.strain_energy, pf.strain_energy);
        EXPECT_DOUBLE_EQ(sf.external_work, pf.external_work);
        EXPECT_EQ(sf.angular_momentum, pf.angular_momentum);
        EXPECT_EQ(sf.low.strain_energy, pf.low.strain_energy);
        EXPECT_EQ(sf.low.kinetic_energy, pf.low.kinetic_energy);
        EXPECT_EQ(sf.low.external_work, pf.low.external_work);
        EXPECT_EQ(sf.low.energy_balance_error, pf.low.energy_balance_error);
        EXPECT_EQ(sf.low.linear_momentum, pf.low.linear_momentum);
        EXPECT_EQ(sf.low.angular_momentum, pf.low.angular_momentum);
        EXPECT_EQ(sf.low.linear_momentum_rate, pf.low.linear_momentum_rate);
        EXPECT_EQ(sf.low.angular_momentum_rate, pf.low.angular_momentum_rate);
        EXPECT_EQ(sf.low.global_equilibrium, pf.low.global_equilibrium);
        EXPECT_EQ(sf.low.linearization_current_internal_moment,
                  pf.low.linearization_current_internal_moment);
        EXPECT_EQ(sf.low.reference_internal_moment,
                  pf.low.reference_internal_moment);
        EXPECT_EQ(sf.low.current_free_residual_moment,
                  pf.low.current_free_residual_moment);
        for (auto member : {&fesa::SolidDynamicFrameLowParts::displacement,
                            &fesa::SolidDynamicFrameLowParts::velocity,
                            &fesa::SolidDynamicFrameLowParts::acceleration,
                            &fesa::SolidDynamicFrameLowParts::algorithmic_acceleration,
                            &fesa::SolidDynamicFrameLowParts::inertia_force,
                            &fesa::SolidDynamicFrameLowParts::internal_force,
                            &fesa::SolidDynamicFrameLowParts::external_force,
                            &fesa::SolidDynamicFrameLowParts::residual,
                            &fesa::SolidDynamicFrameLowParts::reaction,
                            &fesa::SolidDynamicFrameLowParts::residual_scale}) {
          const auto& sl = sf.low.*member;
          const auto& pl = pf.low.*member;
          EXPECT_EQ(sl.Size(), pl.Size());
          if (sl.Size() != pl.Size()) continue;
          for (std::size_t i = 0; i < sl.Size(); ++i) {
            EXPECT_EQ(sl[i], pl[i]);
            has_nonzero_low = has_nonzero_low || sl[i] != 0;
          }
        }
        for (std::size_t i = 0; i < sf.displacement.Size(); ++i) {
          EXPECT_DOUBLE_EQ(sf.displacement[i], pf.displacement[i]);
          EXPECT_DOUBLE_EQ(sf.velocity[i], pf.velocity[i]);
          EXPECT_DOUBLE_EQ(sf.acceleration[i], pf.acceleration[i]);
          EXPECT_DOUBLE_EQ(sf.algorithmic_acceleration[i],
                           pf.algorithmic_acceleration[i]);
          EXPECT_DOUBLE_EQ(sf.residual[i], pf.residual[i]);
        }
        for (std::size_t point = 0; point < sf.solid_results.size(); ++point) {
          EXPECT_EQ(sf.solid_results[point].cauchy_stress,
                    pf.solid_results[point].cauchy_stress);
          EXPECT_EQ(sf.solid_results[point].strain,
                    pf.solid_results[point].strain);
        }
      }
      EXPECT_TRUE(has_nonzero_low);
    }
  }
  std::array<double, 3> errors{}, scales{};
  const auto exact = Motion(static_cast<double>(count) * h, nonlinear);
  const auto& final = state.Value().CommittedState();
  for (unsigned node = 0; node < domain.Value().Nodes().size(); ++node) {
    const auto u = Apply(exact.b, domain.Value().Nodes()[node].coordinates);
    const auto v =
        Apply(exact.velocity, domain.Value().Nodes()[node].coordinates);
    const auto a =
        Apply(exact.acceleration, domain.Value().Nodes()[node].coordinates);
    for (unsigned c = 0; c < 3; ++c) {
      const std::array<double, 3> truth{u[c], v[c], a[c]};
      const std::array<double, 3> actual{final.displacement[3 * node + c],
                                         final.velocity[3 * node + c],
                                         final.acceleration[3 * node + c]};
      for (unsigned j = 0; j < 3; ++j) {
        errors[j] += (actual[j] - truth[j]) * (actual[j] - truth[j]);
        scales[j] += truth[j] * truth[j];
      }
    }
  }
  for (unsigned j = 0; j < 3; ++j) errors[j] = std::sqrt(errors[j] / scales[j]);
  for (const auto& f : state.Value().AcceptedFrames()) {
    const auto mv = ExactMassProduct(domain.Value(), f.velocity);
    const auto ma = ExactMassProduct(domain.Value(), f.acceleration);
    std::array<double, 3> p{}, angular{}, pdot{}, ldot{}, vv{};
    double kinetic = 0, energy_scale = 0, momentum_scale = 0;
    double rate_scale = 0, moment_scale = 0, angular_scale = 0, vv_scale = 0;
    double free_residual_sum = 0;
    for (unsigned n = 0; n < domain.Value().Nodes().size(); ++n) {
      auto x = domain.Value().Nodes()[n].coordinates;
      std::array<double, 3> nmv{}, nma{}, v{};
      for (unsigned c = 0; c < 3; ++c) {
        x[c] += f.displacement[3 * n + c];
        v[c] = f.velocity[3 * n + c];
        nmv[c] = mv[3 * n + c];
        nma[c] = ma[3 * n + c];
        p[c] += nmv[c];
        pdot[c] += nma[c];
        kinetic += .5 * v[c] * nmv[c];
        energy_scale += .5 * std::abs(v[c] * nmv[c]);
        momentum_scale += std::abs(nmv[c]);
        rate_scale += std::abs(nma[c]) + std::abs(f.internal_force[3 * n + c]) +
                      std::abs(f.external_force[3 * n + c]) +
                      std::abs(f.reaction[3 * n + c]);
        free_residual_sum +=
            std::abs(f.residual[3 * n + c] - f.reaction[3 * n + c]);
      }
      double arm = 0, velocity_scale = 0, node_mv = 0, node_rate = 0;
      for (unsigned c = 0; c < 3; ++c) {
        arm +=
            std::abs(x[c]) + std::abs(domain.Value().Nodes()[n].coordinates[c]);
        velocity_scale += std::abs(v[c]);
        node_mv += std::abs(nmv[c]);
        node_rate += std::abs(nma[c]) + std::abs(f.internal_force[3 * n + c]) +
                     std::abs(f.external_force[3 * n + c]) +
                     std::abs(f.reaction[3 * n + c]);
      }
      angular_scale += arm * node_mv;
      moment_scale += arm * node_rate;
      vv_scale += velocity_scale * node_mv;
      AddCross(x, nmv, &angular);
      AddCross(x, nma, &ldot);
      AddCross(v, nmv, &vv);
    }
    const double roundoff = 512 * std::numeric_limits<double>::epsilon();
    EXPECT_NEAR(f.kinetic_energy, kinetic, roundoff * energy_scale);
    for (unsigned c = 0; c < 3; ++c) {
      EXPECT_NEAR(f.linear_momentum[c], p[c], roundoff * momentum_scale);
      EXPECT_NEAR(f.angular_momentum[c], angular[c], roundoff * angular_scale);
      EXPECT_NEAR(f.linear_momentum_rate[c], pdot[c], roundoff * rate_scale);
      EXPECT_NEAR(f.angular_momentum_rate[c], ldot[c], roundoff * moment_scale);
      EXPECT_NEAR(vv[c], 0, roundoff * vv_scale);
      EXPECT_NEAR(f.global_equilibrium[c], 0,
                  free_residual_sum + roundoff * rate_scale);
      const double identity = f.reference_internal_moment[c] +
                              f.linearization_current_internal_moment[c] -
                              f.current_free_residual_moment[c];
      EXPECT_NEAR(f.global_equilibrium[3 + c], identity,
                  roundoff * moment_scale);
      if (nonlinear)
        EXPECT_NEAR(f.reference_internal_moment[c] +
                        f.linearization_current_internal_moment[c],
                    0, roundoff * moment_scale);
      else
        EXPECT_NEAR(f.reference_internal_moment[c], 0, roundoff * moment_scale);
    }
  }
  return errors;
}
}  // namespace

TEST(SolidHhtTemporal, PairedSerialAndTbbPreserveEveryLowComponent) {
  for (bool nonlinear : {false, true}) {
    SCOPED_TRACE(nonlinear);
    ManufacturedRun(.02, 2, nonlinear, true);
  }
}

TEST(SolidHhtTemporal, FreeConstantForceHasAnalyticalInitialAcceleration) {
  auto d = solid_hht_test::Brick();
  d.dynamic_step->boundaries = {{"ALL", 2, 3, 0, {}}};
  d.dynamic_step->loads = {{"ALL", 1, 0.25, {}}};
  auto domain = fesa::Domain::Create(std::move(d));
  ASSERT_TRUE(domain.HasValue());
  fesa::SerialParallelFor parallel;
  fesa::MklPardisoSolver mass;
  fesa::MklPardisoTangentSolver tangent;
  auto result = fesa::SolidHhtDynamicProcedureExecutor(parallel, mass, tangent)
                    .Execute(domain.Value());
  ASSERT_TRUE(result.HasValue()) << solid_hht_test::Message(result.GetStatus());
  const auto& initial = result.Value().state.AcceptedFrames().front();
  // Brick mass row sum rho V/8 = 1/4; rigid translation has zero stiffness.
  for (unsigned node = 0; node < 8; ++node) {
    EXPECT_NEAR(initial.acceleration[3 * node], 1, 1e-13);
    EXPECT_DOUBLE_EQ(initial.displacement[3 * node], 0);
  }
}

TEST(SolidHhtTemporal,
     AllFreeUniformForceRetainsExactRigidTranslationBehavior) {
  auto d = solid_hht_test::Brick();
  d.dynamic_step->loads = {{"ALL", 1, .25, {}}};
  auto domain = fesa::Domain::Create(std::move(d));
  ASSERT_TRUE(domain.HasValue());
  fesa::SerialParallelFor parallel;
  fesa::MklPardisoSolver mass;
  fesa::MklPardisoTangentSolver tangent;
  auto model = fesa::AnalysisModel::Create(domain.Value());
  auto dofs = fesa::DofManager::Create(model.Value());
  auto state =
      fesa::SolidDynamicAnalysisState::Create(domain.Value(), dofs.Value());
  auto status = fesa::SolidHhtDynamicProcedureExecutor(parallel, mass, tangent)
                    .ExecuteInto(domain.Value(), state.Value());
  if (!status.IsOk()) {
    const auto& e = state.Value().IterationRecords().back().evidence;
    std::cout << "Rigid-force failure worst DOF " << e.worst_full_dof
              << " force ratio " << e.residual_metric
              << " terms r,Ma,f,F,scale ";
    for (auto term : e.worst_force_terms) std::cout << term << ' ';
    std::cout << '\n';
    for (const auto& rec : state.Value().IterationRecords()) {
      if (rec.evidence.iteration_index < 10) {
        std::cout << "Trace " << rec.increment_index << ' '
                  << rec.evidence.iteration_index << ' ' << rec.outcome
                  << " ratio=" << rec.evidence.residual_metric
                  << " worst=" << rec.evidence.worst_full_dof
                  << " correction=" << rec.evidence.correction_metrics[0]
                  << '\n';
      }
    }
  }
  ASSERT_TRUE(status.IsOk()) << solid_hht_test::Message(status);
  EXPECT_TRUE(std::any_of(
      state.Value().IterationRecords().begin(),
      state.Value().IterationRecords().end(),
      [](const auto& record) { return record.outcome == "paired-candidate"; }));
  const double roundoff = 512 * std::numeric_limits<double>::epsilon();
  for (const auto& f : state.Value().AcceptedFrames())
    for (unsigned n = 0; n < 8; ++n) {
      EXPECT_NEAR(f.displacement[3 * n], .5 * f.time * f.time,
                  roundoff * .5 * f.time * f.time);
      EXPECT_NEAR(f.velocity[3 * n], f.time, roundoff * f.time);
      EXPECT_NEAR(f.acceleration[3 * n], 1, roundoff);
      for (unsigned c = 1; c < 3; ++c) {
        EXPECT_NEAR(f.displacement[3 * n + c], 0,
                    roundoff * .5 * f.time * f.time);
        EXPECT_NEAR(f.velocity[3 * n + c], 0, roundoff * f.time);
        EXPECT_NEAR(f.acceleration[3 * n + c], 0, roundoff);
      }
    }
}

TEST(SolidHhtTemporal,
     PairedNewtonKeepsAllOriginalComponentsAcrossAxesAndScales) {
  const double roundoff = 512 * std::numeric_limits<double>::epsilon();
  for (unsigned axis = 0; axis < 3; ++axis)
    for (double acceleration : {1e-4, 1.0, 1000.0}) {
      SCOPED_TRACE(axis);
      SCOPED_TRACE(acceleration);
      auto d = solid_hht_test::Brick(.01, 8);
      d.dynamic_step->loads = {
          {"ALL", static_cast<int>(axis + 1), .25 * acceleration, {}}};
      auto domain = fesa::Domain::Create(std::move(d));
      ASSERT_TRUE(domain.HasValue());
      fesa::SerialParallelFor parallel;
      fesa::MklPardisoSolver mass;
      fesa::MklPardisoTangentSolver tangent;
      auto model = fesa::AnalysisModel::Create(domain.Value());
      ASSERT_TRUE(model.HasValue());
      auto dofs = fesa::DofManager::Create(model.Value());
      ASSERT_TRUE(dofs.HasValue());
      auto state =
          fesa::SolidDynamicAnalysisState::Create(domain.Value(), dofs.Value());
      ASSERT_TRUE(state.HasValue());
      const auto status =
          fesa::SolidHhtDynamicProcedureExecutor(parallel, mass, tangent)
              .ExecuteInto(domain.Value(), state.Value());
      if (!status.IsOk()) {
        const auto& records = state.Value().IterationRecords();
        for (const auto& record : records) {
          if (record.increment_index != records.back().increment_index ||
              (record.evidence.iteration_index > 8 &&
               record.evidence.iteration_index < 95))
            continue;
          std::cout << "Rigid paired Newton trace "
                    << record.evidence.iteration_index << ' ' << record.outcome
                    << " metric=" << record.evidence.residual_metric
                    << " worst=" << record.evidence.worst_full_dof << " terms=";
          for (double term : record.evidence.worst_force_terms)
            std::cout << term << ' ';
          std::cout << " corrections=";
          for (double metric : record.evidence.correction_metrics)
            std::cout << metric << ' ';
          std::cout << '\n';
        }
      }
      ASSERT_TRUE(status.IsOk()) << solid_hht_test::Message(status);
      for (const auto& frame : state.Value().AcceptedFrames()) {
        EXPECT_LE(frame.iterations.back().residual_metric, 1e-10);
        EXPECT_LE(frame.iterations.back().update_metric, 1e-12);
        for (unsigned node = 0; node < 8; ++node)
          for (unsigned c = 0; c < 3; ++c) {
            const double a = c == axis ? acceleration : 0;
            EXPECT_NEAR(frame.displacement[3 * node + c],
                        .5 * a * frame.time * frame.time,
                        roundoff * .5 * acceleration * frame.time * frame.time);
            EXPECT_NEAR(frame.velocity[3 * node + c], a * frame.time,
                        roundoff * acceleration * frame.time);
            EXPECT_NEAR(frame.acceleration[3 * node + c], a,
                        roundoff * acceleration);
          }
      }
    }
}

TEST(SolidHhtTemporal,
     ActualTwoBrickLinearAndRotatingSmallStrainTlAreSecondOrder) {
  for (bool nonlinear : {false, true}) {
    SCOPED_TRACE(nonlinear);
    const auto coarse = ManufacturedRun(.04, 30, nonlinear);
    const auto medium = ManufacturedRun(.02, 60, nonlinear);
    const auto fine = ManufacturedRun(.01, 120, nonlinear);
    for (unsigned j = 0; j < 3; ++j) {
      const double p1 = std::log(coarse[j] / medium[j]) / std::log(2.0);
      const double p2 = std::log(medium[j] / fine[j]) / std::log(2.0);
      std::cout << "Hex8 " << (nonlinear ? "TL" : "linear") << " u/v/a channel "
                << j << " errors " << coarse[j] << ' ' << medium[j] << ' '
                << fine[j] << " orders " << p1 << ' ' << p2 << '\n';
      EXPECT_GT(fine[j],
                1e-8);  // Meaningful truncation interval, away from noise.
      EXPECT_LT(coarse[j], .1);
      EXPECT_GE(p1, 1.8);
      EXPECT_GE(p2, 1.8);
    }
  }
}

TEST(SolidHhtTemporal, IndependentThreeDofBrickOscillatorHasSecondOrderUVA) {
  // Node0's three translations are free. Exact polynomial integrals yield
  // M=(rho/27)I, Kii=(lambda+4mu)/9, Kij=(lambda+mu)/12.
  std::array<std::array<double, 3>, 3> errors{};
  for (unsigned level = 0; level < 3; ++level) {
    const double h = .02 / static_cast<double>(1U << level);
    const unsigned count = 32U << level;
    auto d = solid_hht_test::Brick(h, count);
    for (unsigned n = 1; n < 8; ++n)
      d.dynamic_step->boundaries.push_back(
          {"N" + std::to_string(n), 1, 3, 0, {}});
    d.dynamic_step->loads = {
        {"N0", 1, .01, {}}, {"N0", 2, .02, {}}, {"N0", 3, .03, {}}};
    auto domain = fesa::Domain::Create(std::move(d));
    ASSERT_TRUE(domain.HasValue());
    fesa::SerialParallelFor parallel;
    fesa::MklPardisoSolver mass;
    fesa::MklPardisoTangentSolver tangent;
    auto result =
        fesa::SolidHhtDynamicProcedureExecutor(parallel, mass, tangent)
            .Execute(domain.Value());
    ASSERT_TRUE(result.HasValue())
        << solid_hht_test::Message(result.GetStatus());
    const auto& f = result.Value().state.CommittedState();
    const double m = 2.0 / 27;
    const double diagonal = (4.8 + 4 * 4.8) / 9;
    const double off = (4.8 + 4.8) / 12;
    const std::array<double, 2> k{diagonal + 2 * off, diagonal - off};
    for (unsigned c = 0; c < 3; ++c) {
      std::array<double, 3> truth{};
      const std::array<double, 2> force{.02, .01 * (c + 1) - .02};
      for (unsigned mode = 0; mode < 2; ++mode) {
        const double w = std::sqrt(k[mode] / m);
        truth[0] += force[mode] / k[mode] * (1 - std::cos(w * f.time));
        truth[1] += force[mode] / (m * w) * std::sin(w * f.time);
        truth[2] += force[mode] / m * std::cos(w * f.time);
      }
      const std::array<double, 3> actual{f.displacement[c], f.velocity[c],
                                         f.acceleration[c]};
      for (unsigned j = 0; j < 3; ++j)
        errors[level][j] += std::pow(actual[j] - truth[j], 2);
    }
  }
  for (unsigned j = 0; j < 3; ++j) {
    const double p1 =
        .5 * std::log(errors[0][j] / errors[1][j]) / std::log(2.0);
    const double p2 =
        .5 * std::log(errors[1][j] / errors[2][j]) / std::log(2.0);
    std::cout << "Independent MDOF channel " << j << " orders " << p1 << ' '
              << p2 << '\n';
    EXPECT_GE(p1, 1.8);
    EXPECT_GE(p2, 1.8);
    EXPECT_GT(errors[2][j], 1e-15);
  }
}

TEST(SolidHhtTemporal, IndependentScalarBrickOscillatorHasSecondOrderUVA) {
  std::array<std::array<double, 3>, 3> errors{};
  const double m = 2.0 / 27;
  const double k = (4.8 + 4 * 4.8) / 9;
  const double w = std::sqrt(k / m);
  for (unsigned level = 0; level < 3; ++level) {
    const double h = .02 / static_cast<double>(1U << level);
    auto d = solid_hht_test::Brick(h, 32U << level);
    d.dynamic_step->boundaries.push_back({"N0", 2, 3, 0, {}});
    for (unsigned n = 1; n < 8; ++n)
      d.dynamic_step->boundaries.push_back(
          {"N" + std::to_string(n), 1, 3, 0, {}});
    d.dynamic_step->loads = {{"N0", 1, .01, {}}};
    auto domain = fesa::Domain::Create(std::move(d));
    ASSERT_TRUE(domain.HasValue());
    fesa::SerialParallelFor parallel;
    fesa::MklPardisoSolver mass;
    fesa::MklPardisoTangentSolver tangent;
    auto result =
        fesa::SolidHhtDynamicProcedureExecutor(parallel, mass, tangent)
            .Execute(domain.Value());
    ASSERT_TRUE(result.HasValue())
        << solid_hht_test::Message(result.GetStatus());
    const auto& f = result.Value().state.CommittedState();
    const std::array<double, 3> truth{.01 / k * (1 - std::cos(w * f.time)),
                                      .01 / (m * w) * std::sin(w * f.time),
                                      .01 / m * std::cos(w * f.time)};
    const std::array<double, 3> actual{f.displacement[0], f.velocity[0],
                                       f.acceleration[0]};
    for (unsigned j = 0; j < 3; ++j)
      errors[level][j] = std::abs(actual[j] - truth[j]);
    EXPECT_NEAR(
        f.kinetic_energy, .5 * m * f.velocity[0] * f.velocity[0],
        512 * std::numeric_limits<double>::epsilon() * f.kinetic_energy);
    EXPECT_NEAR(f.strain_energy, .5 * k * f.displacement[0] * f.displacement[0],
                512 * std::numeric_limits<double>::epsilon() * f.strain_energy);
  }
  for (unsigned j = 0; j < 3; ++j) {
    const double p1 = std::log(errors[0][j] / errors[1][j]) / std::log(2.0);
    const double p2 = std::log(errors[1][j] / errors[2][j]) / std::log(2.0);
    std::cout << "Independent SDOF channel " << j << " orders " << p1 << ' '
              << p2 << '\n';
    EXPECT_GE(p1, 1.8);
    EXPECT_GE(p2, 1.8);
    EXPECT_GT(errors[2][j], 1e-8);
  }
}

TEST(SolidHhtTemporal,
     BendingMeshRefinementQuantifiesFullIntegrationShearLocking) {
  constexpr double thickness = .125;
  constexpr double curvature = .01;
  constexpr double lambda = 4.8;
  constexpr double mu = 4.8;
  const double continuum = .5 * (lambda + 2 * mu) * curvature * curvature *
                           thickness * thickness * thickness / 12;
  double previous_error = 0;
  for (unsigned nx : {1U, 2U, 4U, 8U}) {
    double numerical = 0;
    for (unsigned e = 0; e < nx; ++e) {
      auto d = solid_hht_test::Brick();
      fesa::Hex8Element::Coordinates x{};
      fesa::ElementDofLayout layout{
          {"I", e + 1, std::to_string(e + 1)},
          {0, 1, 2, 3, 4, 5, 6, 7},
          {fesa::DofComponent::kUx, fesa::DofComponent::kUy,
           fesa::DofComponent::kUz}};
      fesa::Vector u(24);
      for (unsigned a = 0; a < 8; ++a) {
        x[a] = {static_cast<double>(e) / nx + d.nodes[a].coordinates[0] / nx,
                thickness * (d.nodes[a].coordinates[1] - .5),
                d.nodes[a].coordinates[2]};
        u[3 * a] = -curvature * x[a][0] * x[a][1];
        u[3 * a + 1] = .5 * curvature * x[a][0] * x[a][0];
      }
      auto element = fesa::Hex8Element::Create(e, layout, x, d.materials[0], 2);
      ASSERT_TRUE(element.HasValue());
      auto response = element.Value().EvaluateLinear(u);
      ASSERT_TRUE(response.HasValue());
      numerical += response.Value().strain_energy;
    }
    const double error = numerical / continuum - 1;
    const double exact_parasitic =
        mu / (lambda + 2 * mu) / std::pow(nx * thickness, 2);
    EXPECT_NEAR(error, exact_parasitic, 2e-11);
    if (previous_error != 0) EXPECT_NEAR(previous_error / error, 4, 1e-10);
    std::cout << "Bending nx=" << nx
              << " energy/continuum=" << numerical / continuum << '\n';
    previous_error = error;
  }
}