#include <gtest/gtest.h>

#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <limits>
#include <memory>
#include <optional>
#include <stdexcept>
#include <utility>
#include <vector>

#include "analysis/hht_dynamic_internal.h"
#include "fesa/analysis/analysis_model.h"
#include "fesa/assembly/parallel_for.h"
#include "fesa/math/matrix.h"
#include "fesa/solvers/linear/mkl_pardiso_solver.h"
#include "fesa/solvers/linear/mkl_pardiso_tangent_solver.h"

namespace {

struct Fixture {
  std::unique_ptr<fesa::Domain> domain;
  fesa::DofManager dofs;
};

Fixture MakeFixture(const std::size_t free_nodes = 1U,
                    const bool partial_rotation = false,
                    const bool translations_free = true,
                    const double youngs_modulus = 1000.0,
                    const bool all_rotations_constrained = false) {
  fesa::ModelDefinition definition{};
  definition.source_path = "hht-algebra.inp";
  definition.nodes = {
      {{"Plate", 1, "1"}, {-1, -1, 0}, {definition.source_path, 1U}},
      {{"Plate", 2, "2"}, {1, -1, 0}, {definition.source_path, 2U}},
      {{"Plate", 3, "3"}, {1, 1, 0}, {definition.source_path, 3U}},
      {{"Plate", 4, "4"}, {-1, 1, 0}, {definition.source_path, 4U}}};
  definition.materials = {
      {"Elastic", youngs_modulus, 0.3, {definition.source_path, 5U}}};
  definition.densities = {{0U, 1.0, {definition.source_path, 6U}}};
  definition.shell_sections = {
      {"Plate", 0.2, 0U, {definition.source_path, 7U}}};
  definition.shell_elements = {{{"Plate", 1, "1"},
                                fesa::ShellSourceElementType::kS4,
                                {0U, 1U, 2U, 3U},
                                0U,
                                0U,
                                {definition.source_path, 8U}}};
  fesa::DynamicStepDefinition step{};
  step.source_step_name_raw = "Dynamic";
  step.source_step_name_canonical = "DYNAMIC";
  step.initial_increment = 0.1;
  step.time_period = 0.1;
  step.increment_count = 1U;
  step.maximum_increments = 1U;
  for (std::size_t i = 0; i < 4; ++i) {
    definition.shell_node_initial_frames.push_back(
        {static_cast<fesa::EntityIndex>(i), {0, 0, 1}, {1, 0, 0}, {0, 1, 0}});
    const std::string name = "Node" + std::to_string(i);
    definition.node_sets.push_back({name,
                                    std::nullopt,
                                    {static_cast<fesa::EntityIndex>(i)},
                                    {definition.source_path, 9U}});
    if (i >= free_nodes) {
      step.boundaries.push_back(
          {name, 1, 6, 0.0, {definition.source_path, 10U}});
    } else {
      if (all_rotations_constrained)
        step.boundaries.push_back(
            {name, 4, 6, 0.0, {definition.source_path, 10U}});
      else if (partial_rotation)
        step.boundaries.push_back(
            {name, 4, 5, 0.0, {definition.source_path, 10U}});
      if (!translations_free)
        step.boundaries.push_back(
            {name, 1, 3, 0.0, {definition.source_path, 10U}});
    }
  }
  definition.dynamic_step = step;
  auto result = fesa::Domain::Create(std::move(definition));
  if (!result.HasValue()) throw std::runtime_error{"Invalid algebra domain."};
  auto domain = std::make_unique<fesa::Domain>(std::move(result.Value()));
  auto model = fesa::AnalysisModel::Create(*domain);
  if (!model.HasValue()) throw std::runtime_error{"Invalid algebra model."};
  auto dofs = fesa::DofManager::Create(model.Value());
  if (!dofs.HasValue()) throw std::runtime_error{"Invalid algebra DOFs."};
  return {std::move(domain), std::move(dofs.Value())};
}

fesa::SparseMatrix Sparse(const fesa::Matrix& matrix) {
  std::vector<fesa::CooContribution> entries;
  for (std::size_t i = 0; i < matrix.Rows(); ++i) {
    for (std::size_t j = 0; j < matrix.Columns(); ++j) {
      entries.push_back({i, j, matrix(i, j), 0U, entries.size()});
    }
  }
  auto result = fesa::hht_internal::MakeSparse(matrix.Rows(), matrix.Columns(),
                                               std::move(entries));
  EXPECT_TRUE(result.HasValue());
  return std::move(result.Value());
}

fesa::hht_internal::DynamicAssembly Assembly(
    const double second_stiffness = 3.0, const double mass_scale = 1.0) {
  fesa::DynamicAcceptedFrameState frame;
  frame.is_initial = true;
  for (auto* vector :
       {&frame.generalized_coordinates, &frame.velocity, &frame.acceleration,
        &frame.algorithmic_acceleration, &frame.inertia_force,
        &frame.internal_force, &frame.external_force, &frame.residual,
        &frame.residual_scale})
    *vector = fesa::Vector{24U};
  frame.external_force[0] = 2.0;
  frame.residual_scale[0] = 2.0;
  fesa::Matrix mass{24U, 24U};
  fesa::Matrix position{24U, 24U};
  fesa::Matrix velocity{24U, 24U};
  fesa::Matrix stiffness{24U, 24U};
  fesa::Matrix metric{24U, 24U};
  fesa::Matrix hidden_gradient{4U, 24U};
  std::vector<fesa::DynamicRotationKinematics> rotations;
  for (std::size_t i = 0; i < 4; ++i) {
    const std::size_t base = 6U * i;
    for (std::size_t j = 0; j < 5; ++j)
      mass(base + j, base + j) = mass_scale * (j < 3U ? 2.0 : 1.0);
    const double k = i == 1U ? second_stiffness : 3.0;
    stiffness(base, base) = 8.0 + k * 0.7 * 0.7;
    stiffness(base, base + 5U) = -0.7 * k;
    stiffness(base + 5U, base) = -0.7 * k;
    stiffness(base + 5U, base + 5U) = k;
    metric(base + 5U, base + 5U) = 3.0;
    hidden_gradient(i, base) = -0.7 * k;
    hidden_gradient(i, base + 5U) = k;
    auto rotation = fesa::EvaluateDynamicRotation({}, {0, 0, 1}, {}, {});
    EXPECT_TRUE(rotation.HasValue());
    rotations.push_back(std::move(rotation.Value()));
  }
  return {std::move(frame),
          Sparse(mass),
          Sparse(position),
          Sparse(velocity),
          Sparse(stiffness),
          Sparse(stiffness),
          Sparse(metric),
          fesa::Vector{24U},
          fesa::Vector{24U},
          std::move(rotations),
          0.0,
          0.0,
          fesa::Vector{4U},
          fesa::Vector{4U},
          Sparse(hidden_gradient)};
}

fesa::DynamicAcceptedFrameState EmptyTrial(const Fixture& fixture) {
  fesa::DynamicAcceptedFrameState frame{};
  for (auto* values :
       {&frame.generalized_coordinates, &frame.velocity, &frame.acceleration,
        &frame.algorithmic_acceleration, &frame.external_force})
    *values = fesa::Vector(fixture.dofs.FullDofCount());
  return frame;
}

TEST(HhtNewtonStart, ValidPredictorPreservesRatesFixedDofsAndOldState) {
  const auto fixture = MakeFixture(4U, true);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(
      *fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  auto old = EmptyTrial(fixture);
  old.velocity[2U] = 0.2;
  old.velocity[5U] = 0.1;
  old.acceleration[2U] = 0.3;
  old.algorithmic_acceleration[2U] = 0.3;
  old.iteration_count = 7U;
  const auto parameters = fesa::MakeHhtParameters(-0.05).Value();
  const auto predictor = fesa::PredictHht(
      old.generalized_coordinates, old.velocity, old.algorithmic_acceleration,
      0.1, parameters).Value();
  fesa::SerialParallelFor parallel;
  const auto start = fesa::hht_internal::EvaluateNewtonStart(
      *fixture.domain, fixture.dofs, shells.Value(), old, predictor,
      old.acceleration, fesa::Vector(24U), 0.1, parameters, parallel);
  ASSERT_TRUE(start.HasValue());
  const auto& frame = start.Value().frame;
  EXPECT_GT(frame.generalized_coordinates[2U], 0.0);
  EXPECT_EQ(frame.iteration_count, 0U);
  for (std::size_t i = 0U; i < 24U; ++i) {
    EXPECT_DOUBLE_EQ(frame.generalized_coordinates[i], predictor.coordinates[i]);
    EXPECT_DOUBLE_EQ(frame.velocity[i], predictor.velocity[i]);
    EXPECT_DOUBLE_EQ(frame.algorithmic_acceleration[i], 0.0);
    EXPECT_DOUBLE_EQ(frame.acceleration[i],
                     parameters.alpha * old.acceleration[i] /
                         (1.0 + parameters.alpha));
    EXPECT_DOUBLE_EQ(old.generalized_coordinates[i], 0.0);
  }
  for (const auto i : fixture.dofs.ConstrainedDofs()) {
    EXPECT_DOUBLE_EQ(frame.generalized_coordinates[i], 0.0);
    EXPECT_DOUBLE_EQ(frame.velocity[i], 0.0);
    EXPECT_DOUBLE_EQ(frame.acceleration[i], 0.0);
    EXPECT_DOUBLE_EQ(frame.algorithmic_acceleration[i], 0.0);
  }
  EXPECT_DOUBLE_EQ(old.velocity[2U], 0.2);
  EXPECT_DOUBLE_EQ(old.acceleration[2U], 0.3);
  EXPECT_EQ(old.iteration_count, 7U);
}

TEST(HhtNewtonStart, InvalidPredictorGeometryUsesUnchangedAcceptedCoordinates) {
  const auto fixture = MakeFixture(4U);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(
      *fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const auto old = EmptyTrial(fixture);
  const auto parameters = fesa::MakeHhtParameters(-0.05).Value();
  fesa::SerialParallelFor parallel;
  for (const int invalid_case : {0, 1, 2}) {
    SCOPED_TRACE(invalid_case);
    fesa::HhtPredictor predictor{fesa::Vector(24U), fesa::Vector(24U)};
    if (invalid_case == 0) {
      predictor.coordinates[5U] = 3.5;
    } else if (invalid_case == 1) {
      predictor.coordinates[6U] = -2.0;
      predictor.coordinates[12U] = -2.0;
    } else {
      predictor.coordinates[3U] =
          std::acos(-1.0) - 2.0 * std::sqrt(std::numeric_limits<double>::epsilon());
    }
    const auto start = fesa::hht_internal::EvaluateNewtonStart(
        *fixture.domain, fixture.dofs, shells.Value(), old, predictor,
        old.acceleration, fesa::Vector(24U), 0.1, parameters, parallel);
    ASSERT_TRUE(start.HasValue());
    for (std::size_t i = 0U; i < 24U; ++i) {
      EXPECT_DOUBLE_EQ(start.Value().frame.generalized_coordinates[i], 0.0);
      EXPECT_DOUBLE_EQ(old.generalized_coordinates[i], 0.0);
      const double auxiliary =
          -predictor.coordinates[i] / (parameters.beta * 0.1 * 0.1);
      EXPECT_DOUBLE_EQ(start.Value().frame.algorithmic_acceleration[i], auxiliary);
      EXPECT_DOUBLE_EQ(start.Value().frame.acceleration[i],
                       auxiliary / (1.0 + parameters.alpha));
    }
  }
}

TEST(HhtNewtonStart, ValidChartRateOverflowIsNotReplacedByAcceptedGeometry) {
  const auto fixture = MakeFixture(4U);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(
      *fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const auto old = EmptyTrial(fixture);
  fesa::HhtPredictor predictor{fesa::Vector(24U), fesa::Vector(24U)};
  predictor.coordinates[3U] = 0.1;
  predictor.velocity[3U] = 1e200;
  fesa::SerialParallelFor parallel;
  const auto start = fesa::hht_internal::EvaluateNewtonStart(
      *fixture.domain, fixture.dofs, shells.Value(), old, predictor,
      old.acceleration, fesa::Vector(24U), 1e-160,
      fesa::MakeHhtParameters(-0.05).Value(), parallel);
  ASSERT_FALSE(start.HasValue());
  EXPECT_EQ(start.GetStatus().Diagnostics().front().code,
            "invalid-dynamic-rotation");
}

TEST(HhtNewtonStart, LaterOutsideChartNodeDoesNotHideEarlierRateOverflow) {
  const auto fixture = MakeFixture(4U);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(
      *fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const auto old = EmptyTrial(fixture);
  fesa::HhtPredictor predictor{fesa::Vector(24U), fesa::Vector(24U)};
  predictor.coordinates[3U] = 0.1;
  predictor.velocity[3U] = 1e200;
  predictor.coordinates[11U] = 3.5;
  fesa::SerialParallelFor parallel;
  const auto start = fesa::hht_internal::EvaluateNewtonStart(
      *fixture.domain, fixture.dofs, shells.Value(), old, predictor,
      old.acceleration, fesa::Vector(24U), 1e-160,
      fesa::MakeHhtParameters(-0.05).Value(), parallel);
  ASSERT_FALSE(start.HasValue());
  EXPECT_EQ(start.GetStatus().Diagnostics().front().code,
            "invalid-dynamic-rotation");
}

TEST(HhtNewtonStart, UsesTheExistingPrincipalChartMarginForInitialSelection) {
  const auto fixture = MakeFixture(4U);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(
      *fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const auto old = EmptyTrial(fixture);
  const auto parameters = fesa::MakeHhtParameters(-0.05).Value();
  const double margin = std::sqrt(std::numeric_limits<double>::epsilon());
  fesa::SerialParallelFor parallel;
  for (const bool inside : {true, false}) {
    SCOPED_TRACE(inside);
    fesa::HhtPredictor predictor{fesa::Vector(24U), fesa::Vector(24U)};
    const double angle = std::acos(-1.0) - (inside ? 2.0 : 0.5) * margin;
    for (std::size_t node = 0U; node < 4U; ++node) {
      const auto& x = fixture.domain->Nodes()[node].coordinates;
      predictor.coordinates[6U * node] =
          (std::cos(angle) - 1.0) * x[0U] - std::sin(angle) * x[1U];
      predictor.coordinates[6U * node + 1U] =
          std::sin(angle) * x[0U] + (std::cos(angle) - 1.0) * x[1U];
      predictor.coordinates[6U * node + 5U] = angle;
    }
    const auto start = fesa::hht_internal::EvaluateNewtonStart(
        *fixture.domain, fixture.dofs, shells.Value(), old, predictor,
        old.acceleration, fesa::Vector(24U), 0.1, parameters, parallel);
    ASSERT_TRUE(start.HasValue());
    EXPECT_DOUBLE_EQ(start.Value().frame.generalized_coordinates[5U],
                     inside ? predictor.coordinates[5U] : 0.0);
  }
}

TEST(HhtNewtonStart, CommonEnergyScalingSelectsTheSmallerFreeResidual) {
  const auto fixture = MakeFixture(4U);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(
      *fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const auto old = EmptyTrial(fixture);
  auto probe = old;
  probe.generalized_coordinates[5U] = 0.25;
  const auto parameters = fesa::MakeHhtParameters(-0.05).Value();
  fesa::SerialParallelFor parallel;
  const auto evaluated = fesa::hht_internal::EvaluateAssembly(
      *fixture.domain, fixture.dofs, shells.Value(), probe, fesa::Vector(24U),
      0.1, parameters, parallel, false);
  ASSERT_TRUE(evaluated.HasValue());
  double drill_residual = 0.0;
  for (std::size_t i = 3U; i < 24U; i += 6U)
    for (std::size_t j = 0U; j < 3U; ++j)
      drill_residual = std::max(
          drill_residual, std::abs(evaluated.Value().frame.residual[i + j]));
  ASSERT_GT(drill_residual, 0.0);
  // A rigid translation gives m*a per node, m=rho*t*area/4=0.2.
  // The accepted-start force is 0.75 of the predictor torque. Multiplying
  // force by ell=2 reverses the unscaled force-versus-torque comparison.
  const double translation =
      0.75 * drill_residual * (1.0 + parameters.alpha) * parameters.beta *
      0.1 * 0.1 / 0.2;
  fesa::HhtPredictor predictor{probe.generalized_coordinates, fesa::Vector(24U)};
  for (std::size_t node = 0U; node < 4U; ++node)
    predictor.coordinates[6U * node] = translation;
  const auto start = fesa::hht_internal::EvaluateNewtonStart(
      *fixture.domain, fixture.dofs, shells.Value(), old, predictor,
      old.acceleration, fesa::Vector(24U), 0.1, parameters, parallel);
  ASSERT_TRUE(start.HasValue());
  EXPECT_DOUBLE_EQ(start.Value().frame.generalized_coordinates[5U], 0.25);
}

TEST(HhtNewtonStart, ConstrainedReactionDoesNotChangeNontrivialFreeResidualTie) {
  const auto fixture = MakeFixture();
  const auto shells = fesa::hht_internal::BuildRuntimeShells(
      *fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  auto old = EmptyTrial(fixture);
  auto probe = old;
  probe.generalized_coordinates[5U] = 0.25;
  const auto parameters = fesa::MakeHhtParameters(-0.05).Value();
  fesa::SerialParallelFor parallel;
  const auto evaluated = fesa::hht_internal::EvaluateAssembly(
      *fixture.domain, fixture.dofs, shells.Value(), probe, fesa::Vector(24U),
      0.1, parameters, parallel, false);
  ASSERT_TRUE(evaluated.HasValue());
  fesa::Vector load_scale(24U);
  for (std::size_t i = 0U; i < 24U; ++i) {
    old.external_force[i] = 0.5 * evaluated.Value().frame.internal_force[i];
    load_scale[i] = std::abs(old.external_force[i]);
  }
  // Pure drill spin has zero physical inertia at both starting points, so
  // the free residuals are exact opposites. A support reaction is excluded.
  old.external_force[6U] = load_scale[6U] = 1e308;
  fesa::HhtPredictor predictor{probe.generalized_coordinates, fesa::Vector(24U)};
  const auto start = fesa::hht_internal::EvaluateNewtonStart(
      *fixture.domain, fixture.dofs, shells.Value(), old, predictor,
      old.acceleration, load_scale, 0.1, parameters, parallel);
  ASSERT_TRUE(start.HasValue());
  EXPECT_DOUBLE_EQ(start.Value().frame.generalized_coordinates[5U], 0.25);
}

TEST(HhtNewtonStart, ScaledResidualOverflowRemainsAFailure) {
  const auto fixture = MakeFixture(4U);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(
      *fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  auto old = EmptyTrial(fixture);
  old.external_force[0U] = 1e308;
  fesa::Vector load_scale(24U);
  load_scale[0U] = 1e308;
  fesa::HhtPredictor predictor{fesa::Vector(24U), fesa::Vector(24U)};
  predictor.coordinates[5U] = 0.25;
  fesa::SerialParallelFor parallel;
  const auto start = fesa::hht_internal::EvaluateNewtonStart(
      *fixture.domain, fixture.dofs, shells.Value(), old, predictor,
      old.acceleration, load_scale, 0.1,
      fesa::MakeHhtParameters(-0.05).Value(), parallel);
  ASSERT_FALSE(start.HasValue());
  EXPECT_EQ(start.GetStatus().Diagnostics().front().code,
            "dynamic-invalid-state");
}

TEST(HhtNewtonStart, AcceptedStartArithmeticFailureIsNotIgnoredDuringComparison) {
  const auto fixture = MakeFixture(4U);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(
      *fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const auto old = EmptyTrial(fixture);
  fesa::HhtPredictor predictor{fesa::Vector(24U), fesa::Vector(24U)};
  for (std::size_t node = 0U; node < 4U; ++node)
    predictor.coordinates[6U * node] = 1e10;
  const double time_increment = 1e-150;
  const auto parameters = fesa::MakeHhtParameters(-0.05).Value();
  fesa::SerialParallelFor parallel;
  auto probe = old;
  probe.generalized_coordinates = predictor.coordinates;
  const auto valid_predictor = fesa::hht_internal::EvaluateAssembly(
      *fixture.domain, fixture.dofs, shells.Value(), probe, fesa::Vector(24U),
      time_increment, parameters, parallel, false);
  ASSERT_TRUE(valid_predictor.HasValue());
  const auto start = fesa::hht_internal::EvaluateNewtonStart(
      *fixture.domain, fixture.dofs, shells.Value(), old, predictor,
      old.acceleration, fesa::Vector(24U), time_increment, parameters, parallel);
  ASSERT_FALSE(start.HasValue());
  EXPECT_EQ(start.GetStatus().Diagnostics().front().code,
            "dynamic-invalid-state");
}

TEST(HhtNewtonStart, RejectedPredictorDoesNotHideInvalidAcceptedStartRates) {
  const auto fixture = MakeFixture(4U);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(
      *fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const auto old = EmptyTrial(fixture);
  fesa::HhtPredictor predictor{fesa::Vector(24U), fesa::Vector(24U)};
  predictor.coordinates[5U] = 3.5;
  fesa::SerialParallelFor parallel;
  const auto start = fesa::hht_internal::EvaluateNewtonStart(
      *fixture.domain, fixture.dofs, shells.Value(), old, predictor,
      old.acceleration, fesa::Vector(24U), 1e-160,
      fesa::MakeHhtParameters(-0.05).Value(), parallel);
  ASSERT_FALSE(start.HasValue());
  EXPECT_EQ(start.GetStatus().Diagnostics().front().code,
            "dynamic-invalid-state");
  EXPECT_DOUBLE_EQ(old.generalized_coordinates[5U], 0.0);
}

TEST(HhtNewtonStart, ValidGeometryStressOverflowRemainsARecoveryFailure) {
  const auto fixture = MakeFixture(4U, false, true, 1e307);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(
      *fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const auto old = EmptyTrial(fixture);
  fesa::HhtPredictor predictor{fesa::Vector(24U), fesa::Vector(24U)};
  for (std::size_t node = 0U; node < 4U; ++node)
    for (std::size_t component = 0U; component < 2U; ++component)
      predictor.coordinates[6U * node + component] =
          9.0 * fixture.domain->Nodes()[node].coordinates[component];
  fesa::SerialParallelFor parallel;
  const auto start = fesa::hht_internal::EvaluateNewtonStart(
      *fixture.domain, fixture.dofs, shells.Value(), old, predictor,
      old.acceleration, fesa::Vector(24U), 0.1,
      fesa::MakeHhtParameters(-0.05).Value(), parallel);
  ASSERT_FALSE(start.HasValue());
  EXPECT_EQ(start.GetStatus().Diagnostics().front().code,
            "invalid-nonlinear-shell-recovery");
}

TEST(HhtDynamicAlgebra,
     InitializesCoupledMasslessAccelerationFromHiddenEquation) {
  const auto fixture = MakeFixture();
  auto assembly = Assembly();
  fesa::MklPardisoSolver positive;
  fesa::MklPardisoTangentSolver tangent;
  const auto acceleration = fesa::hht_internal::InitializeAcceleration(
      *fixture.domain, fixture.dofs, assembly, 2.0, positive, tangent);
  ASSERT_TRUE(acceleration.HasValue());
  EXPECT_NEAR(acceleration.Value()[0], 1.0, 1.0e-12);
  EXPECT_NEAR(acceleration.Value()[5], 0.7, 1.0e-12);
  for (const auto full : fixture.dofs.ConstrainedDofs())
    EXPECT_EQ(acceleration.Value()[full], 0.0);
  const auto nodes =
      fesa::hht_internal::NullNodes(fixture.dofs, assembly.rotations);
  ASSERT_EQ(nodes, std::vector<std::size_t>{0U});
  assembly.frame.acceleration = acceleration.Value();
  auto frame = assembly.frame;
  EXPECT_TRUE(fesa::hht_internal::CompleteConstraintEvidence(
                  *fixture.domain, fixture.dofs, assembly, 2.0, nodes, positive,
                  tangent, &frame)
                  .IsOk());
  ASSERT_EQ(frame.hidden_acceleration_defect.Size(), 1U);
  EXPECT_NEAR(frame.hidden_acceleration_defect[0], 0.0, 1.0e-12);
  EXPECT_GT(frame.hidden_acceleration_defect_scale[0], 0.0);
  frame.acceleration[5] = 0.0;
  const auto pseudoinverse = fesa::hht_internal::CompleteConstraintEvidence(
      *fixture.domain, fixture.dofs, assembly, 2.0, nodes, positive, tangent,
      &frame);
  EXPECT_FALSE(pseudoinverse.IsOk());
}

TEST(HhtDynamicAlgebra, RejectsMissingCanonicalHiddenGradient) {
  const auto fixture = MakeFixture();
  auto assembly = Assembly();
  assembly.hidden_gradient.reset();
  fesa::MklPardisoSolver positive;
  fesa::MklPardisoTangentSolver tangent;
  const auto acceleration = fesa::hht_internal::InitializeAcceleration(
      *fixture.domain, fixture.dofs, assembly, 2.0, positive, tangent);
  ASSERT_FALSE(acceleration.HasValue());
  ASSERT_FALSE(acceleration.GetStatus().Diagnostics().empty());
  EXPECT_EQ(acceleration.GetStatus().Diagnostics().front().code,
            "dynamic-algebraic-gradient");
}

TEST(HhtDynamicAlgebra, ChecksInitialConstraintAndVelocityWithoutRepair) {
  const auto fixture = MakeFixture();
  fesa::MklPardisoSolver positive;
  fesa::MklPardisoTangentSolver tangent;
  auto inconsistent = Assembly();
  inconsistent.frame.internal_force[5] = 0.3;
  inconsistent.frame.residual_scale[5] = 0.3;
  EXPECT_FALSE(
      fesa::hht_internal::InitializeAcceleration(
          *fixture.domain, fixture.dofs, inconsistent, 2.0, positive, tangent)
          .HasValue());
  inconsistent = Assembly();
  inconsistent.frame.velocity[5] = 0.1;
  EXPECT_FALSE(
      fesa::hht_internal::InitializeAcceleration(
          *fixture.domain, fixture.dofs, inconsistent, 2.0, positive, tangent)
          .HasValue());
}

TEST(HhtDynamicAlgebra, KeepsAcceptedHiddenRateDefectsAsDiagnostics) {
  const auto fixture = MakeFixture();
  auto assembly = Assembly();
  assembly.frame.is_initial = false;
  assembly.frame.velocity[5] = 0.1;
  assembly.frame.acceleration[5] = -0.2;
  auto frame = assembly.frame;
  fesa::MklPardisoSolver positive;
  fesa::MklPardisoTangentSolver tangent;
  const auto nodes =
      fesa::hht_internal::NullNodes(fixture.dofs, assembly.rotations);
  const auto result = fesa::hht_internal::CompleteConstraintEvidence(
      *fixture.domain, fixture.dofs, assembly, 2.0, nodes, positive, tangent,
      &frame);
  ASSERT_TRUE(result.IsOk());
  EXPECT_NEAR(frame.hidden_velocity_defect[0], 0.3, 1.0e-12);
  EXPECT_NEAR(frame.hidden_acceleration_defect[0], -0.6, 1.0e-12);
}

TEST(HhtDynamicAlgebra, RejectsNegativePhysicalMassAndAlgebraicRankLoss) {
  const auto fixture = MakeFixture(2U);
  fesa::MklPardisoSolver positive;
  fesa::MklPardisoTangentSolver tangent;
  const auto negative = Assembly(3.0, -1.0);
  EXPECT_FALSE(
      fesa::hht_internal::InitializeAcceleration(
          *fixture.domain, fixture.dofs, negative, 2.0, positive, tangent)
          .HasValue());
  const auto singular = Assembly(0.0);
  EXPECT_FALSE(
      fesa::hht_internal::InitializeAcceleration(
          *fixture.domain, fixture.dofs, singular, 2.0, positive, tangent)
          .HasValue());
}

TEST(HhtDynamicAlgebra, AppliesTheExactRegularityConditionBoundary) {
  const auto fixture = MakeFixture(2U);
  fesa::MklPardisoSolver positive;
  fesa::MklPardisoTangentSolver tangent;
  for (const int exponent : {31, 32, 33}) {
    auto assembly = Assembly();
    // The boundary oracle is exactly diag(1,c), with W=I. Avoid a rounded
    // sqrt(3) congruence when testing a binary-exact inclusive threshold.
    fesa::Matrix stiffness{24U, 24U};
    fesa::Matrix metric{24U, 24U};
    for (std::size_t node = 0; node < 4; ++node) {
      const std::size_t drill = 6U * node + 5U;
      stiffness(drill, drill) = node == 1U ? std::ldexp(1.0, -exponent) : 1.0;
      metric(drill, drill) = 1.0;
    }
    assembly.internal_tangent = Sparse(stiffness);
    assembly.drilling_metric = Sparse(metric);
    fesa::Matrix hidden_gradient{4U, 24U};
    for (std::size_t node = 0U; node < 4U; ++node)
      hidden_gradient(node, 6U * node + 5U) =
          node == 1U ? std::ldexp(1.0, -exponent) : 1.0;
    assembly.hidden_gradient = Sparse(hidden_gradient);
    const auto result = fesa::hht_internal::InitializeAcceleration(
        *fixture.domain, fixture.dofs, assembly, 2.0, positive, tangent);
    EXPECT_EQ(result.HasValue(), exponent <= 32)
        << exponent
        << (result.HasValue() ? ""
                              : result.GetStatus().Diagnostics()[0].message);
    if (exponent == 33) {
      ASSERT_FALSE(result.HasValue());
      EXPECT_EQ(result.GetStatus().Diagnostics()[0].code,
                "dynamic-algebraic-conditioning");
    }
  }
}

TEST(HhtDynamicAlgebra, AllFixedModelHasAnEmptyValidAccelerationSolve) {
  const auto fixture = MakeFixture(0U);
  auto assembly = Assembly();
  fesa::MklPardisoSolver positive;
  fesa::MklPardisoTangentSolver tangent;
  const auto result = fesa::hht_internal::InitializeAcceleration(
      *fixture.domain, fixture.dofs, assembly, 2.0, positive, tangent);
  ASSERT_TRUE(result.HasValue());
  EXPECT_EQ(result.Value().Norm(), 0.0);
  auto frame = assembly.frame;
  EXPECT_TRUE(fesa::hht_internal::CompleteConstraintEvidence(
                  *fixture.domain, fixture.dofs, assembly, 2.0, {}, positive,
                  tangent, &frame)
                  .IsOk());
  EXPECT_EQ(frame.hidden_constraint.Size(), 0U);
}

TEST(HhtDynamicAlgebra, PartialBoundaryRetainsEveryNonzeroPhysicalDirection) {
  for (const bool translations_free : {false, true}) {
    const auto fixture = MakeFixture(1U, true, translations_free);
    for (const double p : {0.0, 1.0 / 2000.0, 1.0e-8}) {
      auto assembly = Assembly();
      assembly.frame.external_force[0] = 0.0;
      fesa::Matrix mass{24U, 24U};
      const double nz = std::sqrt(1.0 - p * p);
      for (std::size_t node = 0; node < 4; ++node) {
        for (std::size_t j = 0; j < 3; ++j)
          mass(6U * node + j, 6U * node + j) = 2.0;
        mass(6U * node + 3U, 6U * node + 3U) = nz * nz;
        mass(6U * node + 4U, 6U * node + 4U) = 1.0;
        mass(6U * node + 5U, 6U * node + 5U) = p * p;
        mass(6U * node + 3U, 6U * node + 5U) = -p * nz;
        mass(6U * node + 5U, 6U * node + 3U) = -p * nz;
        assembly.rotations[node].drill_null = {p, 0, nz};
      }
      assembly.mass = Sparse(mass);
      const auto nodes =
          fesa::hht_internal::NullNodes(fixture.dofs, assembly.rotations);
      EXPECT_EQ(nodes.size(), p == 0.0 ? 1U : 0U);
      fesa::MklPardisoSolver positive;
      fesa::MklPardisoTangentSolver tangent;
      const auto result = fesa::hht_internal::InitializeAcceleration(
          *fixture.domain, fixture.dofs, assembly, 2.0, positive, tangent);
      const bool expected = !(translations_free && p == 1.0e-8);
      EXPECT_EQ(result.HasValue(), expected) << "p=" << p;
      if (!expected) {
        ASSERT_FALSE(result.HasValue());
        EXPECT_EQ(result.GetStatus().Diagnostics()[0].code,
                  "dynamic-positive-mass-conditioning");
      }
    }
  }
}

TEST(HhtDynamicAlgebra, RejectsAdmissibleNullRankChangeBeforeAcceptingFrame) {
  const auto fixture = MakeFixture(1U, true);
  auto assembly = Assembly();
  const auto initial_nodes =
      fesa::hht_internal::NullNodes(fixture.dofs, assembly.rotations);
  ASSERT_EQ(initial_nodes.size(), 1U);
  assembly.rotations[0].drill_null = {1.0e-8, 0, std::sqrt(1.0 - 1.0e-16)};
  auto frame = assembly.frame;
  frame.hidden_constraint = fesa::Vector{1U, 23.0};
  fesa::MklPardisoSolver positive;
  fesa::MklPardisoTangentSolver tangent;
  const auto result = fesa::hht_internal::CompleteConstraintEvidence(
      *fixture.domain, fixture.dofs, assembly, 2.0, initial_nodes, positive,
      tangent, &frame);
  ASSERT_FALSE(result.IsOk());
  EXPECT_EQ(result.Diagnostics()[0].code, "dynamic-mass-null-rank-change");
  EXPECT_EQ(frame.hidden_constraint[0], 23.0);
}

TEST(HhtEndpointFields, MeasuresActualFullChartAcrossNonlinearRootAndPartialBoundary) {
  const auto fixture = MakeFixture(1U, true);
  auto trial = EmptyTrial(fixture), old = EmptyTrial(fixture);
  trial.residual = fesa::Vector{24U};
  fesa::Vector previous{24U};
  // Independent nonlinear graph q_phiZ=p^2: p .1 -> .2. The
  // previous tangent lift is .02, while the actual endpoint change is .03.
  previous[0] = .1; previous[5] = .01;
  trial.generalized_coordinates[0] = .2;
  trial.generalized_coordinates[5] = .04;
  trial.residual[0] = 3; trial.residual[5] = -5;
  // Large constrained values and spatial support moments are not free
  // generalized endpoint evidence and cannot change either scalar maximum.
  trial.generalized_coordinates[3] = 1000;
  trial.residual[3] = 2000;
  trial.reaction = fesa::Vector{24U}; trial.reaction[5] = 3000;
  const auto result = fesa::hht_internal::MeasureEndpointFields(fixture.dofs, trial, old, previous);
  ASSERT_TRUE(result.HasValue());
  EXPECT_TRUE(result.Value()[0].active); EXPECT_TRUE(result.Value()[1].active);
  EXPECT_DOUBLE_EQ(result.Value()[0].residual, 3);
  EXPECT_DOUBLE_EQ(result.Value()[1].residual, 5);
  EXPECT_DOUBLE_EQ(result.Value()[0].increment, .2);
  EXPECT_DOUBLE_EQ(result.Value()[1].increment, .04);
  EXPECT_DOUBLE_EQ(*result.Value()[0].correction, .2 - .1);
  EXPECT_DOUBLE_EQ(*result.Value()[1].correction, .04 - .01);
  EXPECT_NE(*result.Value()[1].correction, 2 * .1 * (.2 - .1));
  const auto initial = fesa::hht_internal::MeasureEndpointFields(fixture.dofs, trial, old, std::nullopt);
  ASSERT_TRUE(initial.HasValue());
  EXPECT_FALSE(initial.Value()[0].correction.has_value());
  EXPECT_FALSE(initial.Value()[1].correction.has_value());
  previous[0] = -std::numeric_limits<double>::max();
  trial.generalized_coordinates[0] = std::numeric_limits<double>::max();
  EXPECT_FALSE(fesa::hht_internal::MeasureEndpointFields(fixture.dofs, trial, old, previous).HasValue());
}

}  // namespace

TEST(HhtDynamicAlgebra, NonDirectionalAssemblyRetainsValidEmptyNullBoundary) {
  for (const std::size_t free_nodes : {4U, 0U}) {
    SCOPED_TRACE(free_nodes);
    const auto fixture = MakeFixture(free_nodes, false, true, 1000.0, true);
    const auto shells = fesa::hht_internal::BuildRuntimeShells(*fixture.domain,
                                                                fixture.dofs);
    ASSERT_TRUE(shells.HasValue());
    auto trial = EmptyTrial(fixture);
    trial.external_force[0U] = 0.4;
    fesa::Vector load_scale(24U);
    load_scale[0U] = 0.4;
    fesa::SerialParallelFor serial;
    const auto parameters = fesa::MakeHhtParameters(-0.05);
    ASSERT_TRUE(parameters.HasValue());
    const auto assembly = fesa::hht_internal::EvaluateAssembly(
        *fixture.domain, fixture.dofs, shells.Value(), trial, load_scale,
        0.1, parameters.Value(), serial, false);
    ASSERT_TRUE(assembly.HasValue());
    ASSERT_TRUE(assembly.Value().frame.mitc4_drilling);
    EXPECT_EQ(assembly.Value().frame.mitc4_drilling->mode,
              fesa::Mitc4DrillingMode::kDirectSpring);
    EXPECT_EQ(assembly.Value().hidden_constraint.Size(), 0U);
    EXPECT_FALSE(assembly.Value().hidden_gradient.has_value());
    const auto nodes = fesa::hht_internal::NullNodes(fixture.dofs,
                                                    assembly.Value().rotations);
    ASSERT_TRUE(nodes.empty());
    fesa::MklPardisoSolver positive;
    fesa::MklPardisoTangentSolver tangent;
    const auto acceleration = fesa::hht_internal::InitializeAcceleration(
        *fixture.domain, fixture.dofs, assembly.Value(), 2.0, positive, tangent);
    EXPECT_TRUE(acceleration.HasValue())
        << (acceleration.GetStatus().Diagnostics().empty() ? "" :
            acceleration.GetStatus().Diagnostics()[0U].message);
    auto frame = assembly.Value().frame;
    if (acceleration.HasValue()) {
      frame.acceleration = acceleration.Value();
      // rho*t*area/4 = .2 per node; the .4 force gives acceleration 2.
      EXPECT_NEAR(acceleration.Value()[0U], free_nodes == 0U ? 0.0 : 2.0, 1e-12);
      for (const auto full : fixture.dofs.ConstrainedDofs())
        EXPECT_DOUBLE_EQ(acceleration.Value()[full], 0.0);
    }
    const auto complete = fesa::hht_internal::CompleteConstraintEvidence(
        *fixture.domain, fixture.dofs, assembly.Value(), 2.0, nodes,
        positive, tangent, &frame);
    EXPECT_TRUE(complete.IsOk())
        << (complete.Diagnostics().empty() ? "" : complete.Diagnostics()[0U].message);
    if (complete.IsOk()) {
      EXPECT_EQ(frame.hidden_constraint.Size(), 0U);
      EXPECT_EQ(frame.hidden_velocity_defect.Size(), 0U);
      EXPECT_EQ(frame.hidden_acceleration_defect.Size(), 0U);
    }
  }
}