#include <gtest/gtest.h>

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

#if defined(_M_X64) || defined(_M_IX86) || defined(__SSE__)
#include <xmmintrin.h>
#endif

#include "analysis/hht_reduced_internal.h"
#include "fesa/analysis/analysis_model.h"
#include "fesa/analysis/hht_time_integration.h"
#include "fesa/assembly/parallel_for.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 bool partial_rotation, const bool warped_shared = false) {
  fesa::ModelDefinition definition{};
  definition.source_path = "hht-reduced.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", 1000.0, 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}}};
  if (warped_shared) {
    definition.nodes[2U].coordinates[2U] = 0.1;
    definition.nodes.push_back(
        {{"Plate", 5, "5"}, {3, -1, 0}, {definition.source_path, 11U}});
    definition.nodes.push_back(
        {{"Plate", 6, "6"}, {3, 1, 0}, {definition.source_path, 12U}});
    definition.shell_elements.push_back(
        {{"Plate", 2, "2"}, fesa::ShellSourceElementType::kS4,
         {1U, 4U, 5U, 2U}, 0U, 0U, {definition.source_path, 13U}});
  }
  fesa::DynamicStepDefinition step{};
  step.source_step_name_raw = "Dynamic";
  step.source_step_name_canonical = "DYNAMIC";
  step.initial_increment = step.time_period = 0.1;
  step.increment_count = step.maximum_increments = 1U;
  for (std::size_t i = 0U; i < 4U; ++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 (partial_rotation && i == 0U)
      step.boundaries.push_back({name, 6, 6, 0.0, {definition.source_path, 10U}});
  }
  if (warped_shared) {
    for (std::size_t i = 4U; i < 6U; ++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, 14U}});
    }
  }
  definition.dynamic_step = step;
  auto created = fesa::Domain::Create(std::move(definition));
  if (!created.HasValue()) throw std::runtime_error{"Invalid reduced fixture."};
  auto domain = std::make_unique<fesa::Domain>(std::move(created.Value()));
  auto model = fesa::AnalysisModel::Create(*domain);
  if (!model.HasValue()) throw std::runtime_error{"Invalid reduced model."};
  auto dofs = fesa::DofManager::Create(model.Value());
  if (!dofs.HasValue()) throw std::runtime_error{"Invalid reduced dofs."};
  return {std::move(domain), std::move(dofs.Value())};
}

std::vector<fesa::DynamicRotationKinematics> Rotations(const std::size_t nodes) {
  std::vector<fesa::DynamicRotationKinematics> rotations;
  for (std::size_t node = 0U; node < nodes; ++node) {
    auto rotation = fesa::EvaluateDynamicRotation({}, {0, 0, 1}, {}, {});
    if (!rotation.HasValue()) throw std::runtime_error{"Invalid rotation."};
    rotations.push_back(std::move(rotation.Value()));
  }
  return rotations;
}

fesa::Result<fesa::hht_internal::DynamicAssembly> Assemble(
    const Fixture& fixture,
    const std::vector<fesa::hht_internal::RuntimeShell>& shells,
    const fesa::Vector& q, const fesa::Vector& v, const fesa::Vector& a,
    const fesa::Vector& external, const bool directional) {
  fesa::DynamicAcceptedFrameState frame{};
  frame.generalized_coordinates = q;
  frame.velocity = v;
  frame.acceleration = a;
  frame.algorithmic_acceleration = fesa::Vector(q.Size());
  frame.external_force = external;
  fesa::SerialParallelFor parallel;
  const auto parameters = fesa::MakeHhtParameters(-0.05);
  if (!parameters.HasValue())
    return fesa::Result<fesa::hht_internal::DynamicAssembly>::Failure(
        parameters.GetStatus());
  return fesa::hht_internal::EvaluateAssembly(
      *fixture.domain, fixture.dofs, shells, std::move(frame),
      fesa::Vector(q.Size()), 0.1, parameters.Value(), parallel, directional);
}

void SetComponentColumns(fesa::hht_reduced_internal::FixedChart* chart) {
  chart->p_columns.clear();
  chart->z_columns.clear();
  for (const auto full : chart->p_dofs)
    chart->p_columns.push_back({full / 6U, full % 6U < 3U ? 0U : 1U,
                                false, {{full, 1.0}}, {0.0}});
  for (const auto full : chart->z_dofs)
    chart->z_columns.push_back({full / 6U, 1U, false, {{full, 1.0}}, {0.0}});
}

std::size_t PIndex(const fesa::hht_reduced_internal::FixedChart& chart,
                   const std::size_t full) {
  const auto found = std::find(chart.p_dofs.begin(), chart.p_dofs.end(), full);
  if (found == chart.p_dofs.end()) throw std::runtime_error{"Missing p DOF."};
  return static_cast<std::size_t>(found - chart.p_dofs.begin());
}

std::array<double, 3U> Hidden(
    const fesa::hht_reduced_internal::ConstraintRowOperators& row,
    const fesa::Vector& velocity, const fesa::Vector& acceleration) {
  double first = row.time_first;
  double second = row.time_second;
  for (const auto& entry : row.gradient) {
    first += entry.value * velocity[entry.dof];
    second += entry.value * acceleration[entry.dof];
  }
  for (const auto& entry : row.time_gradient)
    second += 2.0 * entry.value * velocity[entry.dof];
  for (const auto& entry : row.hessian)
    second += entry.value * velocity[entry.row] * velocity[entry.column];
  return {row.value, first, second};
}

TEST(HhtReducedChart, SelectsCanonicalPivotAndLiftsCoordinates) {
  const auto fixture = MakeFixture(false);
  const auto chart = fesa::hht_reduced_internal::BuildFixedChart(
      fixture.dofs, Rotations(fixture.domain->Nodes().size()));
  ASSERT_TRUE(chart.HasValue());
  ASSERT_EQ(chart.Value().null_nodes.size(), 4U);
  for (std::size_t node = 0U; node < 4U; ++node) {
    EXPECT_EQ(chart.Value().pivot_components[node], 2U);
    EXPECT_EQ(chart.Value().z_dofs[node], 6U * node + 5U);
  }
  fesa::Vector p{chart.Value().p_dofs.size()};
  fesa::Vector z{chart.Value().z_dofs.size()};
  for (std::size_t i = 0U; i < p.Size(); ++i) p[i] = static_cast<double>(i + 1U);
  for (std::size_t i = 0U; i < z.Size(); ++i) z[i] = -static_cast<double>(i + 1U);
  const auto full = fesa::hht_reduced_internal::Lift(chart.Value(), p, z);
  ASSERT_TRUE(full.HasValue());
  for (std::size_t i = 0U; i < p.Size(); ++i)
    EXPECT_DOUBLE_EQ(full.Value()[chart.Value().p_dofs[i]], p[i]);
  for (std::size_t i = 0U; i < z.Size(); ++i)
    EXPECT_DOUBLE_EQ(full.Value()[chart.Value().z_dofs[i]], z[i]);
}

TEST(HhtReducedChart, RetainsNodeWhenItsPartialBoundaryCutsTheNull) {
  const auto fixture = MakeFixture(true);
  const auto chart = fesa::hht_reduced_internal::BuildFixedChart(
      fixture.dofs, Rotations(fixture.domain->Nodes().size()));
  ASSERT_TRUE(chart.HasValue());
  ASSERT_EQ(chart.Value().null_nodes.size(), 3U);
  EXPECT_TRUE(std::find(chart.Value().null_nodes.begin(), chart.Value().null_nodes.end(),
                        0U) == chart.Value().null_nodes.end());
  EXPECT_TRUE(std::find(chart.Value().p_dofs.begin(), chart.Value().p_dofs.end(),
                        3U) != chart.Value().p_dofs.end());
  EXPECT_TRUE(std::find(chart.Value().p_dofs.begin(), chart.Value().p_dofs.end(),
                        4U) != chart.Value().p_dofs.end());
  const auto shells = fesa::hht_internal::BuildRuntimeShells(*fixture.domain,
                                                              fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const fesa::Vector zero(fixture.dofs.FullDofCount());
  const auto assembly = Assemble(fixture, shells.Value(), zero, zero, zero,
                                 zero, true);
  ASSERT_TRUE(assembly.HasValue());
  const auto operators = fesa::hht_reduced_internal::EvaluateConstraintOperators(
      *fixture.domain, fixture.dofs, shells.Value(), assembly.Value(),
      chart.Value(), zero, zero);
  ASSERT_TRUE(operators.HasValue());
  EXPECT_EQ(operators.Value().rows.size(), 3U);
}

TEST(HhtReducedChart, UsesActualFiniteInitialNullAndPreservesTransposeActions) {
  const auto fixture = MakeFixture(false);
  auto rotations = Rotations(4U);
  for (auto& rotation : rotations) {
    const auto finite = fesa::EvaluateDynamicRotation({0.31, -0.22, 0.17},
                                                      {0, 0, 1}, {}, {});
    ASSERT_TRUE(finite.HasValue());
    rotation = finite.Value();
  }
  const auto built = fesa::hht_reduced_internal::BuildFixedChart(fixture.dofs, rotations);
  ASSERT_TRUE(built.HasValue());
  const auto& chart = built.Value();
  const auto& raw = rotations.front().drill_null;
  const double norm = std::hypot(raw[0U], raw[1U], raw[2U]);
  std::size_t axis = 0U;
  for (std::size_t i = 1U; i < 3U; ++i)
    if (std::abs(raw[i]) < std::abs(raw[axis])) axis = i;
  EXPECT_EQ(chart.minimum_axes.front(), axis);
  ASSERT_EQ(chart.z_columns.front().support.size(), 3U);
  for (std::size_t i = 0U; i < 3U; ++i) {
    const auto& column = chart.z_columns.front();
    EXPECT_NEAR(column.support[i].value, raw[i] / norm, 1e-15);
    EXPECT_TRUE(std::isfinite(column.support_radii[i]));
    EXPECT_GE(column.support_radii[i], 0.0);
  }
  EXPECT_GT(std::abs(chart.z_columns.front().support[0U].value), 0.01);
  fesa::Vector p(chart.p_columns.size()), z(chart.z_columns.size());
  for (std::size_t i = 0U; i < p.Size(); ++i) p[i] = 0.015 * (i + 1U);
  for (std::size_t i = 0U; i < z.Size(); ++i) z[i] = -0.02 * (i + 1U);
  const auto full = fesa::hht_reduced_internal::Lift(chart, p, z);
  ASSERT_TRUE(full.HasValue());
  const auto projected_p = fesa::hht_reduced_internal::ProjectPhysical(chart, full.Value());
  const auto projected_z = fesa::hht_reduced_internal::ProjectAlgebraic(chart, full.Value());
  ASSERT_TRUE(projected_p.HasValue());
  ASSERT_TRUE(projected_z.HasValue());
  for (std::size_t i = 0U; i < p.Size(); ++i)
    EXPECT_NEAR(projected_p.Value()[i], p[i], 1e-15);
  for (std::size_t i = 0U; i < z.Size(); ++i)
    EXPECT_NEAR(projected_z.Value()[i], z[i], 1e-15);
}

TEST(HhtReducedChart, MinimumAxisTieAndPartialBoundaryRetainExactZeros) {
  const auto fixture = MakeFixture(true);
  const auto chart = fesa::hht_reduced_internal::BuildFixedChart(fixture.dofs, Rotations(4U));
  ASSERT_TRUE(chart.HasValue());
  for (const auto axis : chart.Value().minimum_axes) EXPECT_EQ(axis, 0U);
  const auto full = fesa::hht_reduced_internal::Lift(
      chart.Value(), fesa::Vector(chart.Value().p_columns.size(), 0.4),
      fesa::Vector(chart.Value().z_columns.size(), -0.2));
  ASSERT_TRUE(full.HasValue());
  EXPECT_DOUBLE_EQ(full.Value()[5U], 0.0);
  for (const auto& column : chart.Value().p_columns)
    if (column.node == 0U) EXPECT_FALSE(column.fixed_plane);
}

TEST(HhtReducedChart, ProperRotationRelatesRetainedPlanesByConstantOrthogonalMap) {
  const auto fixture = MakeFixture(false);
  const std::array<std::array<double, 3U>, 3U> q{{
      {{0.36, -0.48, 0.8}}, {{0.8, 0.6, 0.0}}, {{-0.48, 0.64, 0.6}}}};
  const auto rotate = [&q](const std::array<double, 3U>& x) {
    std::array<double, 3U> y{};
    for (std::size_t i = 0U; i < 3U; ++i)
      for (std::size_t j = 0U; j < 3U; ++j) y[i] += q[i][j] * x[j];
    return y;
  };
  const std::array<double, 3U> phi{{0.31, -0.22, 0.17}};
  const auto phi_q = rotate(phi), director_q = rotate({0.0, 0.0, 1.0});
  const auto original = fesa::EvaluateDynamicRotation(
      {phi[0U], phi[1U], phi[2U]}, {0, 0, 1}, {}, {});
  const auto transformed = fesa::EvaluateDynamicRotation(
      {phi_q[0U], phi_q[1U], phi_q[2U]},
      {director_q[0U], director_q[1U], director_q[2U]}, {}, {});
  ASSERT_TRUE(original.HasValue());
  ASSERT_TRUE(transformed.HasValue());
  const auto source = fesa::hht_reduced_internal::BuildFixedChart(
      fixture.dofs, std::vector<fesa::DynamicRotationKinematics>(4U, original.Value()));
  const auto target = fesa::hht_reduced_internal::BuildFixedChart(
      fixture.dofs, std::vector<fesa::DynamicRotationKinematics>(4U, transformed.Value()));
  ASSERT_TRUE(source.HasValue());
  ASSERT_TRUE(target.HasValue());
  std::array<std::array<double, 3U>, 2U> source_plane{}, target_plane{};
  std::size_t si = 0U, ti = 0U;
  for (const auto& column : source.Value().p_columns)
    if (column.node == 0U && column.channel == 1U) {
      for (const auto& entry : column.support)
        source_plane[si][entry.dof - 3U] = entry.value;
      ++si;
    }
  for (const auto& column : target.Value().p_columns)
    if (column.node == 0U && column.channel == 1U) {
      for (const auto& entry : column.support)
        target_plane[ti][entry.dof - 3U] = entry.value;
      ++ti;
    }
  ASSERT_EQ(si, 2U);
  ASSERT_EQ(ti, 2U);
  std::array<std::array<double, 2U>, 2U> t{};
  for (std::size_t j = 0U; j < 2U; ++j) {
    const auto rotated = rotate(source_plane[j]);
    for (std::size_t i = 0U; i < 2U; ++i)
      for (std::size_t k = 0U; k < 3U; ++k) t[i][j] += target_plane[i][k] * rotated[k];
    for (std::size_t k = 0U; k < 3U; ++k)
      EXPECT_NEAR(target_plane[0U][k] * t[0U][j] + target_plane[1U][k] * t[1U][j],
                  rotated[k], 1e-14);
  }
  for (std::size_t i = 0U; i < 2U; ++i)
    for (std::size_t j = 0U; j < 2U; ++j)
      EXPECT_NEAR(t[0U][i] * t[0U][j] + t[1U][i] * t[1U][j],
                  i == j ? 1.0 : 0.0, 1e-14);
}

TEST(HhtReducedChart, RejectsSubnormalNormWithoutAValidCoefficientCertificate) {
  const auto fixture = MakeFixture(false);
  auto rotations = Rotations(4U);
  const double tiny = std::numeric_limits<double>::denorm_min();
  rotations.front().drill_null = {tiny, tiny, tiny};
  EXPECT_FALSE(fesa::hht_reduced_internal::BuildFixedChart(fixture.dofs, rotations).HasValue());
}

TEST(HhtReducedChart, NativeCoefficientRadiiExportFixedInputReplay) {
  const auto fixture = MakeFixture(false);
  const auto finite = fesa::EvaluateDynamicRotation({0.31, -0.22, 0.17},
                                                    {0, 0, 1}, {}, {});
  ASSERT_TRUE(finite.HasValue());
  const double tiny = std::numeric_limits<double>::denorm_min();
  const std::array<std::array<double, 3U>, 7U> inputs{{
      {{0.0, 0.0, 1.0}}, {{1.0, 1.0, 1.0}}, finite.Value().drill_null.Components(),
      {{1e300, -7e299, 3e299}}, {{1e-300, 7e-301, -3e-301}},
      {{1.0, tiny, -1e-320}}, {{-1.0, 1e-160, 1e-170}}}};
  std::cout << "FBNATIVE environment round=" << std::fegetround();
#if defined(_M_X64) || defined(_M_IX86) || defined(__SSE__)
  std::cout << " mxcsr=" << _mm_getcsr();
#endif
  std::cout << '\n';
  for (std::size_t sample = 0U; sample < inputs.size(); ++sample) {
    auto rotations = Rotations(4U);
    rotations.front().drill_null = fesa::Vector3(inputs[sample]);
    const auto chart = fesa::hht_reduced_internal::BuildFixedChart(fixture.dofs, rotations);
    ASSERT_TRUE(chart.HasValue()) << sample;
    std::cout << "FBNATIVE case=" << sample << " axis=" << chart.Value().minimum_axes.front()
              << std::hexfloat << " raw=";
    for (const auto value : inputs[sample]) std::cout << value << ',';
    std::array<const fesa::hht_reduced_internal::FixedBasisColumn*, 3U> basis{};
    std::size_t column = 0U;
    for (const auto& p : chart.Value().p_columns)
      if (p.node == 0U && p.channel == 1U) basis[column++] = &p;
    ASSERT_EQ(column, 2U);
    basis[2U] = &chart.Value().z_columns.front();
    std::cout << " values=";
    for (const auto* p : basis) {
      ASSERT_EQ(p->support.size(), 3U);
      for (const auto& entry : p->support) std::cout << entry.value << ',';
    }
    std::cout << " radii=";
    for (const auto* p : basis)
      for (const auto radius : p->support_radii) {
        EXPECT_TRUE(std::isfinite(radius));
        EXPECT_GE(radius, 0.0);
        std::cout << radius << ',';
      }
    std::cout << std::defaultfloat << '\n';
  }
  auto invalid = Rotations(4U);
  invalid.front().drill_null = {};
  EXPECT_FALSE(fesa::hht_reduced_internal::BuildFixedChart(fixture.dofs, invalid).HasValue());
}

TEST(HhtReducedChart, InvertibleGraphDoesNotCertifySelectorTransversality) {
  const auto fixture = MakeFixture(false);
  const auto chart = fesa::hht_reduced_internal::BuildFixedChart(fixture.dofs, Rotations(4U));
  ASSERT_TRUE(chart.HasValue());
  fesa::hht_reduced_internal::ConstraintOperators operators;
  operators.rows.resize(4U);
  for (std::size_t i = 0U; i < 4U; ++i)
    operators.rows[i].gradient.push_back({6U * i + 5U, 1.0});
  fesa::MklPardisoTangentSolver solver;
  EXPECT_TRUE(fesa::hht_reduced_internal::SolveGraphSystem(
      fesa::hht_reduced_internal::GraphJacobian(operators, chart.Value()),
      fesa::Vector(4U, 1.0), solver).HasValue());
  const auto shells = fesa::hht_internal::BuildRuntimeShells(*fixture.domain, fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const fesa::Vector zero(fixture.dofs.FullDofCount());
  const auto assembled = Assemble(fixture, shells.Value(), zero, zero, zero, zero, false);
  ASSERT_TRUE(assembled.HasValue());
  auto assembly = assembled.Value();
  assembly.rotations = Rotations(4U);
  for (auto& rotation : assembly.rotations) rotation.drill_null = {1.0, 0.0, 0.0};
  EXPECT_FALSE(fesa::hht_reduced_internal::CheckSelectorTransversality(
      assembly, chart.Value(), solver).HasValue());
  for (auto& rotation : assembly.rotations) rotation.drill_null = {1.0, 0.0, 1e-12};
  EXPECT_FALSE(fesa::hht_reduced_internal::CheckSelectorTransversality(
      assembly, chart.Value(), solver).HasValue());
}

TEST(HhtReducedOperators, ScatterShellDerivativesByConstraintRow) {
  const auto fixture = MakeFixture(false);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(*fixture.domain,
                                                              fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  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());
  const auto parameters = fesa::MakeHhtParameters(-0.05);
  ASSERT_TRUE(parameters.HasValue());
  fesa::SerialParallelFor parallel;
  const auto assembly = fesa::hht_internal::EvaluateAssembly(
      *fixture.domain, fixture.dofs, shells.Value(), frame,
      fesa::Vector(fixture.dofs.FullDofCount()), 0.1, parameters.Value(),
      parallel, true);
  ASSERT_TRUE(assembly.HasValue());
  const auto chart = fesa::hht_reduced_internal::BuildFixedChart(
      fixture.dofs, assembly.Value().rotations);
  ASSERT_TRUE(chart.HasValue());
  const auto operators = fesa::hht_reduced_internal::EvaluateConstraintOperators(
      *fixture.domain, fixture.dofs, shells.Value(), assembly.Value(),
      chart.Value(), fesa::Vector(fixture.dofs.FullDofCount()),
      fesa::Vector(fixture.dofs.FullDofCount()));
  ASSERT_TRUE(operators.HasValue());
  ASSERT_EQ(operators.Value().rows.size(), 4U);
  fesa::MklPardisoTangentSolver solver;
  const auto graph_condition =
      fesa::hht_reduced_internal::CheckGraphRegularity(
          assembly.Value(), chart.Value(), operators.Value(), solver);
  ASSERT_TRUE(graph_condition.HasValue());
  EXPECT_GE(graph_condition.Value(), std::ldexp(1.0, -32));
  for (std::size_t row_index = 0U; row_index < operators.Value().rows.size();
       ++row_index) {
    const auto& row = operators.Value().rows[row_index];
    EXPECT_DOUBLE_EQ(row.value, 0.0);
    double expected_scale = 0.0;
    const auto node = chart.Value().null_nodes[row_index];
    for (std::size_t component = 0U; component < 3U; ++component)
      expected_scale +=
          std::abs(assembly.Value().rotations[node].drill_null[component]) *
          assembly.Value().frame.residual_scale[6U * node + 3U + component];
    EXPECT_DOUBLE_EQ(row.value_scale, expected_scale);
    EXPECT_FALSE(row.gradient.empty());
    EXPECT_FALSE(row.hessian.empty());
    EXPECT_LT(row.gradient.size(), fixture.dofs.FullDofCount());
    for (const auto& entry : row.hessian) {
      EXPECT_LT(entry.row, fixture.dofs.FullDofCount());
      EXPECT_LT(entry.column, fixture.dofs.FullDofCount());
    }
  }
}

TEST(HhtReducedEmbedding, RejectsNonfiniteGraphResidualBeforeAcceptance) {
  const auto fixture = MakeFixture(false);
  const auto chart = fesa::hht_reduced_internal::BuildFixedChart(
      fixture.dofs, Rotations(fixture.domain->Nodes().size()));
  ASSERT_TRUE(chart.HasValue());
  fesa::MklPardisoTangentSolver solver;
  const auto root = fesa::hht_reduced_internal::SolveEmbeddingRoot(
      chart.Value(), fesa::Vector(chart.Value().p_dofs.size()),
      fesa::Vector(chart.Value().z_dofs.size()),
      [&chart](const fesa::Vector&) {
        fesa::hht_reduced_internal::ConstraintOperators operators;
        operators.rows.resize(chart.Value().z_dofs.size());
        operators.rows.front().value =
            std::numeric_limits<double>::quiet_NaN();
        return fesa::Result<fesa::hht_reduced_internal::ConstraintOperators>::
            Success(std::move(operators));
      },
      solver);
  ASSERT_FALSE(root.HasValue());
  ASSERT_FALSE(root.GetStatus().Diagnostics().empty());
  EXPECT_EQ(root.GetStatus().Diagnostics().front().code,
            "dynamic-invalid-state");
}

TEST(HhtReducedEmbedding, ReturnsExactTwoCycleForFullHiddenCertification) {
  // This deliberately nonphysical graph models a binary64 Newton two-cycle.
  // The root helper must return an already evaluated state; it must not claim
  // graph convergence.  The procedure's original hidden-row gates remain the
  // only authority that can accept the returned trial.
  fesa::hht_reduced_internal::FixedChart chart{};
  chart.full_dof_count = 1U;
  chart.z_dofs = {0U};
  SetComponentColumns(&chart);
  fesa::MklPardisoTangentSolver solver;
  const auto root = fesa::hht_reduced_internal::SolveEmbeddingRoot(
      chart, fesa::Vector(0U), fesa::Vector{1U, 1.0},
      [](const fesa::Vector& q) {
        fesa::hht_reduced_internal::ConstraintOperators operators;
        operators.rows.resize(1U);
        operators.rows[0U].value = 2.0 * q[0U];
        operators.rows[0U].gradient.push_back({0U, 1.0});
        return fesa::Result<fesa::hht_reduced_internal::ConstraintOperators>::
            Success(std::move(operators));
      },
      solver);
  ASSERT_TRUE(root.HasValue());
  EXPECT_DOUBLE_EQ(root.Value().z[0U], -1.0);
  EXPECT_DOUBLE_EQ(root.Value().q[0U], -1.0);
  EXPECT_DOUBLE_EQ(root.Value().operators.rows[0U].value, -2.0);
}

TEST(HhtReducedEmbedding, ReturnsEvaluatedTrialWhenLiftAbsorbsAChangedPrivateRoot) {
  const auto fixture = MakeFixture(false);
  auto rotations = Rotations(4U);
  const double c = std::sqrt(0.5);
  rotations.front().drill_null = {c, -c, 0.0};
  const auto built = fesa::hht_reduced_internal::BuildFixedChart(fixture.dofs, rotations);
  ASSERT_TRUE(built.HasValue());
  const auto& chart = built.Value();
  fesa::Vector p(chart.p_columns.size()), seed(chart.z_columns.size());
  std::size_t angular = 0U;
  for (std::size_t i = 0U; i < chart.p_columns.size(); ++i)
    if (chart.p_columns[i].node == 0U && chart.p_columns[i].channel == 1U)
      if (angular++ == 1U) p[i] = 0.4;
  ASSERT_EQ(angular, 2U);
  const auto original = fesa::hht_reduced_internal::Lift(chart, p, seed);
  ASSERT_TRUE(original.HasValue());
  auto changed = seed;
  changed[0U] = 1e-20 / 0.003;
  EXPECT_NE(changed[0U], seed[0U]);
  const auto absorbed = fesa::hht_reduced_internal::Lift(chart, p, changed);
  ASSERT_TRUE(absorbed.HasValue());
  for (std::size_t i = 0U; i < original.Value().Size(); ++i)
    EXPECT_EQ(original.Value()[i], absorbed.Value()[i]);
  fesa::MklPardisoTangentSolver solver;
  const auto root = fesa::hht_reduced_internal::SolveEmbeddingRoot(
      chart, p, seed, [&chart](const fesa::Vector& q) {
        fesa::hht_reduced_internal::ConstraintOperators operators;
        operators.rows.resize(chart.z_columns.size());
        for (std::size_t row = 0U; row < operators.rows.size(); ++row) {
          const double factor = row == 0U ? 0.003 : 1.0;
          auto& out = operators.rows[row];
          for (const auto& entry : chart.z_columns[row].support) {
            out.gradient.push_back({entry.dof, factor * entry.value});
            out.value += factor * entry.value * q[entry.dof];
          }
          if (row == 0U) out.value -= 1e-20;
          out.value_scale = std::abs(out.value);
        }
        return fesa::Result<fesa::hht_reduced_internal::ConstraintOperators>::Success(
            std::move(operators));
      }, solver);
  ASSERT_TRUE(root.HasValue());
  // A returned arithmetic-stagnation trial is not a converged graph. The
  // procedure must still apply the original full endpoint/hidden gates.
  EXPECT_EQ(root.Value().z[0U], seed[0U]);
  EXPECT_NE(root.Value().operators.rows[0U].value, 0.0);
  EXPECT_GT(root.Value().graph_residual_relative, 1e-10);
  EXPECT_EQ(root.Value().graph_iteration_count, 0U);
  for (std::size_t i = 0U; i < original.Value().Size(); ++i)
    EXPECT_EQ(root.Value().q[i], original.Value()[i]);
}

TEST(HhtReducedEmbedding, StopsAtExistingScaledPositionCertificate) {
  fesa::hht_reduced_internal::FixedChart chart{};
  chart.full_dof_count = 1U;
  chart.z_dofs = {0U};
  SetComponentColumns(&chart);
  fesa::MklPardisoTangentSolver solver;
  const auto root = fesa::hht_reduced_internal::SolveEmbeddingRoot(
      chart, fesa::Vector(0U), fesa::Vector{1U, 0.0},
      [](const fesa::Vector& q) {
        fesa::hht_reduced_internal::ConstraintOperators operators;
        operators.rows.resize(1U);
        operators.rows[0U].value = 5.0e-11;
        operators.rows[0U].value_scale = 1.0;
        operators.rows[0U].gradient.push_back({0U, 1.0});
        return fesa::Result<fesa::hht_reduced_internal::ConstraintOperators>::
            Success(std::move(operators));
      },
      solver);
  ASSERT_TRUE(root.HasValue());
  EXPECT_DOUBLE_EQ(root.Value().z[0U], 0.0);
  EXPECT_DOUBLE_EQ(root.Value().graph_residual_infinity, 5.0e-11);
  EXPECT_DOUBLE_EQ(root.Value().graph_residual_relative, 5.0e-11);
  EXPECT_DOUBLE_EQ(root.Value().graph_correction_infinity, 0.0);
}

TEST(HhtReducedEmbedding, RequiresExactZeroWhenPositionScaleIsZero) {
  fesa::hht_reduced_internal::FixedChart chart{};
  chart.full_dof_count = 1U;
  chart.z_dofs = {0U};
  SetComponentColumns(&chart);
  fesa::MklPardisoTangentSolver solver;
  const auto root = fesa::hht_reduced_internal::SolveEmbeddingRoot(
      chart, fesa::Vector(0U), fesa::Vector{1U, 1.0},
      [](const fesa::Vector& q) {
        fesa::hht_reduced_internal::ConstraintOperators operators;
        operators.rows.resize(1U);
        operators.rows[0U].value = 5.0e-11 * q[0U];
        operators.rows[0U].gradient.push_back({0U, 5.0e-11});
        return fesa::Result<fesa::hht_reduced_internal::ConstraintOperators>::
            Success(std::move(operators));
      },
      solver);
  ASSERT_TRUE(root.HasValue());
  EXPECT_DOUBLE_EQ(root.Value().z[0U], 0.0);
  EXPECT_DOUBLE_EQ(root.Value().operators.rows[0U].value, 0.0);
}

TEST(HhtReducedEmbedding,
     ReconstructsRatesAcrossSelectedPartitionCancellation) {
  // The exact RHS has the binary64 terms (+1e16, +1, -1e16). A plain
  // left-to-right sum loses +1; the private rate partition must retain it.
  fesa::hht_reduced_internal::FixedChart chart{};
  chart.full_dof_count = 3U;
  chart.p_dofs = {0U, 1U};
  chart.z_dofs = {2U};
  SetComponentColumns(&chart);
  fesa::hht_reduced_internal::EmbeddingRoot root{};
  root.p = fesa::Vector{2U};
  root.z = fesa::Vector{1U};
  root.q = fesa::Vector{3U};
  root.operators.rows.resize(1U);
  auto& row = root.operators.rows.front();
  row.time_first = -1.0e16;
  row.time_second = 1.0e16;
  row.gradient = {{2U, 1.0}, {0U, -1.0}, {1U, 1.0e16}};
  row.time_gradient = {{0U, -0.5}};
  row.hessian = {{0U, 0U, -1.0e16}};
  fesa::Vector vp{2U};
  vp[0U] = vp[1U] = 1.0;
  const fesa::Vector ap{2U};
  double plain_velocity_rhs = -row.time_first;
  for (const auto& entry : row.gradient)
    if (entry.dof < 2U) plain_velocity_rhs -= entry.value * vp[entry.dof];
  double plain_acceleration_rhs = -row.hessian.front().value * vp[0U] * vp[0U];
  plain_acceleration_rhs -= 2.0 * row.time_gradient.front().value * vp[0U];
  plain_acceleration_rhs -= row.time_second;
  EXPECT_DOUBLE_EQ(plain_velocity_rhs, 0.0);
  EXPECT_DOUBLE_EQ(plain_acceleration_rhs, 0.0);
  fesa::MklPardisoTangentSolver solver;
  const auto rates = fesa::hht_reduced_internal::ReconstructPhysicalRates(
      chart, root, vp, ap, solver);
  ASSERT_TRUE(rates.HasValue());
  EXPECT_DOUBLE_EQ(rates.Value().velocity[0U], 1.0);
  EXPECT_DOUBLE_EQ(rates.Value().velocity[1U], 1.0);
  EXPECT_DOUBLE_EQ(rates.Value().velocity[2U], 1.0);
  EXPECT_DOUBLE_EQ(rates.Value().acceleration[0U], 0.0);
  EXPECT_DOUBLE_EQ(rates.Value().acceleration[1U], 0.0);
  EXPECT_DOUBLE_EQ(rates.Value().acceleration[2U], 1.0);
  EXPECT_DOUBLE_EQ(rates.Value().z_velocity[0U], 1.0);
  EXPECT_DOUBLE_EQ(rates.Value().z_acceleration[0U], 1.0);
}

TEST(HhtReducedEmbedding, ReconstructsActualShellRatesWithMixedTimeLoad) {
  const auto fixture = MakeFixture(false, true);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(*fixture.domain,
                                                              fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const fesa::Vector zero(fixture.dofs.FullDofCount());
  const auto initial = Assemble(fixture, shells.Value(), zero, zero, zero,
                                zero, true);
  ASSERT_TRUE(initial.HasValue());
  auto initial_rotations = initial.Value().rotations;
  for (auto& rotation : initial_rotations) {
    auto finite = fesa::EvaluateDynamicRotation({0.12, -0.08, 0.05},
                                                {0, 0, 1}, {}, {});
    ASSERT_TRUE(finite.HasValue());
    rotation = finite.Value();
  }
  const auto chart = fesa::hht_reduced_internal::BuildFixedChart(
      fixture.dofs, initial_rotations);
  ASSERT_TRUE(chart.HasValue());
  fesa::Vector force_rate(fixture.dofs.FullDofCount());
  fesa::Vector force_acceleration(fixture.dofs.FullDofCount());
  fesa::Vector force(fixture.dofs.FullDofCount());
  // A finite dead drilling moment keeps the external n(q)^T F and its
  // position derivatives active without driving this local graph beyond its
  // principal-coordinate branch during the off-graph tangent probe.
  force[5U] = 1.0e-2;
  force_rate[5U] = 0.2;
  force_acceleration[5U] = -0.1;
  const auto position_operator = [&](const fesa::Vector& q) {
    const auto position = Assemble(fixture, shells.Value(), q, zero, zero,
                                   force, true);
    if (!position.HasValue())
      return fesa::Result<fesa::hht_reduced_internal::ConstraintOperators>::
          Failure(position.GetStatus());
    return fesa::hht_reduced_internal::EvaluateConstraintOperators(
        *fixture.domain, fixture.dofs, shells.Value(), position.Value(),
        chart.Value(), force_rate, force_acceleration);
  };
  fesa::Vector p(chart.Value().p_dofs.size());
  p[PIndex(chart.Value(), 2U)] = 0.01;
  p[PIndex(chart.Value(), 3U)] = 0.02;
  fesa::MklPardisoTangentSolver solver;
  const auto root = fesa::hht_reduced_internal::SolveEmbeddingRoot(
      chart.Value(), p, fesa::Vector(chart.Value().z_dofs.size()),
      position_operator, solver);
  ASSERT_TRUE(root.HasValue())
      << (root.HasValue() || root.GetStatus().Diagnostics().empty()
              ? ""
              : root.GetStatus().Diagnostics().front().message);
  EXPECT_DOUBLE_EQ(root.Value().q[2U], 0.01);
  fesa::Vector vp(chart.Value().p_dofs.size());
  fesa::Vector ap(chart.Value().p_dofs.size());
  vp[PIndex(chart.Value(), 3U)] = 0.03;
  ap[PIndex(chart.Value(), 4U)] = -0.02;
  const auto rates = fesa::hht_reduced_internal::ReconstructPhysicalRates(
      chart.Value(), root.Value(), vp, ap, solver);
  ASSERT_TRUE(rates.HasValue());
  const auto actual = Assemble(fixture, shells.Value(), root.Value().q,
                               rates.Value().velocity,
                               rates.Value().acceleration, force, true);
  ASSERT_TRUE(actual.HasValue());
  const auto operators = fesa::hht_reduced_internal::EvaluateConstraintOperators(
      *fixture.domain, fixture.dofs, shells.Value(), actual.Value(),
      chart.Value(), force_rate, force_acceleration);
  ASSERT_TRUE(operators.HasValue());
  bool saw_time_term = false;
  for (const auto& row : operators.Value().rows) {
    const auto hidden = Hidden(row, rates.Value().velocity,
                               rates.Value().acceleration);
    saw_time_term = saw_time_term || row.time_first != 0.0 ||
                    row.time_second != 0.0;
    EXPECT_NEAR(hidden[0U], 0.0, 1.0e-9);
    EXPECT_NEAR(hidden[1U], 0.0, 1.0e-9);
    EXPECT_NEAR(hidden[2U], 0.0, 1.0e-8);
  }
  EXPECT_TRUE(saw_time_term);
  const auto parameters = fesa::MakeHhtParameters(-0.05);
  ASSERT_TRUE(parameters.HasValue());
  const double cv = parameters.Value().gamma / (parameters.Value().beta * 0.1);
  const double ca = 1.0 / ((1.0 + parameters.Value().alpha) *
                           parameters.Value().beta * 0.01);
  const auto reference_length = fesa::hht_internal::ReferenceLength(*fixture.domain);
  ASSERT_TRUE(reference_length.HasValue());
  const double h = fixture.domain->Dynamic()->initial_increment;
  const auto system = fesa::hht_reduced_internal::AssembleAugmentedReducedSystem(
      actual.Value(), chart.Value(), operators.Value(), cv, ca,
      reference_length.Value(), h);
  ASSERT_TRUE(system.HasValue());
  ASSERT_EQ(system.Value().physical_count, chart.Value().p_dofs.size());
  ASSERT_EQ(system.Value().algebraic_count, chart.Value().z_dofs.size());
  fesa::Vector correction(system.Value().rhs.Size());
  correction[0U] = 0.004;
  correction[system.Value().physical_count] = -0.003;
  correction[system.Value().physical_count + system.Value().algebraic_count] =
      0.002;
  correction[system.Value().physical_count +
             2U * system.Value().algebraic_count] = -0.001;
  for (std::size_t i = 0U; i < correction.Size(); ++i)
    correction[i] /= system.Value().unknown_scale[i];
  const auto mapped = fesa::hht_reduced_internal::MapAugmentedCorrection(
      chart.Value(), system.Value(), correction, cv, ca);
  ASSERT_TRUE(mapped.HasValue());
  EXPECT_DOUBLE_EQ(mapped.Value().coordinate[0U], 0.004);
  EXPECT_DOUBLE_EQ(mapped.Value().velocity[0U], cv * 0.004);
  EXPECT_DOUBLE_EQ(mapped.Value().acceleration[0U], ca * 0.004);
  const auto& z_column = chart.Value().z_columns.front();
  for (const auto& entry : z_column.support) {
    EXPECT_NEAR(mapped.Value().coordinate[entry.dof], -0.003 * entry.value, 1e-17);
    EXPECT_NEAR(mapped.Value().velocity[entry.dof], 0.002 * entry.value, 1e-17);
    EXPECT_NEAR(mapped.Value().acceleration[entry.dof], -0.001 * entry.value, 1e-17);
  }
  const auto residual = [&](const fesa::Vector& q, const fesa::Vector& v,
                            const fesa::Vector& a) -> fesa::Result<fesa::Vector> {
    const auto trial = Assemble(fixture, shells.Value(), q, v, a, force, true);
    if (!trial.HasValue()) return fesa::Result<fesa::Vector>::Failure(trial.GetStatus());
    const auto trial_operators = fesa::hht_reduced_internal::EvaluateConstraintOperators(
        *fixture.domain, fixture.dofs, shells.Value(), trial.Value(), chart.Value(),
        force_rate, force_acceleration);
    if (!trial_operators.HasValue())
      return fesa::Result<fesa::Vector>::Failure(trial_operators.GetStatus());
    fesa::Vector result(system.Value().rhs.Size());
    for (std::size_t i = 0U; i < chart.Value().p_columns.size(); ++i)
      for (const auto& entry : chart.Value().p_columns[i].support)
        result[i] += entry.value * trial.Value().frame.residual[entry.dof];
    for (std::size_t row = 0U; row < chart.Value().z_dofs.size(); ++row) {
      const auto hidden = Hidden(trial_operators.Value().rows[row], v, a);
      result[system.Value().physical_count + row] = hidden[0U];
      result[system.Value().physical_count + system.Value().algebraic_count + row] =
          hidden[1U];
      result[system.Value().physical_count + 2U * system.Value().algebraic_count + row] =
          hidden[2U];
    }
    for (std::size_t i = 0U; i < result.Size(); ++i)
      result[i] *= system.Value().row_scale[i];
    return fesa::Result<fesa::Vector>::Success(std::move(result));
  };
  const auto base = residual(root.Value().q, rates.Value().velocity,
                             rates.Value().acceleration);
  ASSERT_TRUE(base.HasValue());
  const auto projected_base = fesa::hht_reduced_internal::ProjectPhysical(
      chart.Value(), actual.Value().frame.residual);
  ASSERT_TRUE(projected_base.HasValue());
  for (std::size_t i = 0U; i < projected_base.Value().Size(); ++i) {
    double uncancelled = 0.0;
    for (const auto& entry : chart.Value().p_columns[i].support)
      uncancelled += std::abs(entry.value * actual.Value().frame.residual[entry.dof]);
    EXPECT_NEAR(base.Value()[i], system.Value().row_scale[i] * projected_base.Value()[i],
                1e-13 * system.Value().row_scale[i] * uncancelled);
  }
  const fesa::Vector input_scale(system.Value().rhs.Size(), 1.0);
  const fesa::Vector output_scale(system.Value().rhs.Size(), 1.0);
  const auto block_norm = [&](const fesa::Vector& values, const std::size_t block,
                              const fesa::hht_reduced_internal::AugmentedReducedSystem& active) {
    const std::size_t begin = block == 0U ? 0U
        : active.physical_count + (block - 1U) * active.algebraic_count;
    const std::size_t end = block == 0U ? active.physical_count
        : begin + active.algebraic_count;
    double squared = 0.0;
    for (std::size_t row = begin; row < end; ++row)
      squared += std::pow(output_scale[row] * values[row], 2.0);
    return std::sqrt(squared);
  };
  const auto sweep = [&](const char* name, const fesa::Vector& direction,
                         const fesa::hht_reduced_internal::AugmentedReducedSystem& active,
                         const fesa::Vector& base_velocity,
                         const fesa::Vector& base_acceleration,
                         const bool expected_pass) {
    fesa::Vector normalized(direction);
    double direction_norm = 0.0;
    for (std::size_t i = 0U; i < normalized.Size(); ++i)
      direction_norm += std::pow(input_scale[i] * normalized[i], 2.0);
    direction_norm = std::sqrt(direction_norm);
    ASSERT_GT(direction_norm, 0.0) << name;
    for (std::size_t i = 0U; i < normalized.Size(); ++i)
      normalized[i] /= direction_norm;
    const auto mapped_direction = fesa::hht_reduced_internal::MapAugmentedCorrection(
        chart.Value(), active, normalized, cv, ca);
    ASSERT_TRUE(mapped_direction.HasValue()) << name;
    const auto analytic = active.tangent.Multiply(normalized);
    double tangent_frobenius = 0.0;
    for (std::size_t row = 0U; row < active.tangent.Rows(); ++row)
      for (std::size_t offset = active.tangent.RowOffsets()[row];
           offset < active.tangent.RowOffsets()[row + 1U]; ++offset) {
        const auto column = active.tangent.ColumnIndices()[offset];
        const double value = output_scale[row] * active.tangent.Values()[offset] /
                             input_scale[column];
        tangent_frobenius += value * value;
      }
    tangent_frobenius = std::sqrt(tangent_frobenius);
    ASSERT_GT(tangent_frobenius, 0.0) << name;
    std::array<bool, 7U> passed{};
    const std::array<int, 7U> exponents{{8, 10, 12, 14, 16, 18, 20}};
    for (std::size_t sample = 0U; sample < exponents.size(); ++sample) {
      const double step = std::ldexp(1.0, -exponents[sample]);
      fesa::Vector plus_q(root.Value().q), minus_q(root.Value().q);
      fesa::Vector plus_v(base_velocity), minus_v(base_velocity);
      fesa::Vector plus_a(base_acceleration), minus_a(base_acceleration);
      for (std::size_t i = 0U; i < plus_q.Size(); ++i) {
        plus_q[i] += step * mapped_direction.Value().coordinate[i];
        minus_q[i] -= step * mapped_direction.Value().coordinate[i];
        plus_v[i] += step * mapped_direction.Value().velocity[i];
        minus_v[i] -= step * mapped_direction.Value().velocity[i];
        plus_a[i] += step * mapped_direction.Value().acceleration[i];
        minus_a[i] -= step * mapped_direction.Value().acceleration[i];
      }
      const auto plus = residual(plus_q, plus_v, plus_a);
      const auto minus = residual(minus_q, minus_v, minus_a);
      ASSERT_TRUE(plus.HasValue()) << name << " h=" << step;
      ASSERT_TRUE(minus.HasValue()) << name << " h=" << step;
      fesa::Vector difference(analytic.Size());
      for (std::size_t row = 0U; row < difference.Size(); ++row)
        difference[row] = (plus.Value()[row] - minus.Value()[row]) / (2.0 * step);
      fesa::Vector error(analytic.Size());
      for (std::size_t row = 0U; row < error.Size(); ++row)
        error[row] = analytic[row] - difference[row];
      passed[sample] = true;
      std::cout << "N5 " << name << " j=" << exponents[sample] << " h=" << step;
      for (std::size_t block = 0U; block < 4U; ++block) {
        const double analytic_norm = block_norm(analytic, block, active);
        const double fd_norm = block_norm(difference, block, active);
        const double error_norm = block_norm(error, block, active);
        const double metric = analytic_norm == 0.0
                                  ? error_norm / tangent_frobenius
                                  : error_norm / (analytic_norm + fd_norm);
        const bool block_pass = analytic_norm == 0.0
                                    ? metric <= 1.0e-10
                                    : metric <= 1.0e-6;
        passed[sample] = passed[sample] && block_pass;
        std::cout << " b" << block << "=(a:" << analytic_norm
                  << ",fd:" << fd_norm << ",e:" << error_norm
                  << ",m:" << metric << ")";
      }
      std::cout << '\n';
    }
    bool consecutive = false;
    for (std::size_t sample = 1U; sample <= 4U; ++sample)
      consecutive = consecutive || (passed[sample] && passed[sample + 1U]);
    EXPECT_EQ(consecutive, expected_pass) << name;
  };
  const auto total = system.Value().rhs.Size();
  fesa::Vector u_only(total), phi_only(total), z_only(total), vz_only(total),
      az_only(total), mixed(total);
  u_only[PIndex(chart.Value(), 0U)] = 0.7;
  phi_only[PIndex(chart.Value(), 3U)] = -0.6;
  z_only[system.Value().physical_count] = 0.5;
  vz_only[system.Value().physical_count + system.Value().algebraic_count] = -0.4;
  az_only[system.Value().physical_count + 2U * system.Value().algebraic_count] = 0.3;
  mixed[PIndex(chart.Value(), 0U)] = 0.25;
  mixed[PIndex(chart.Value(), 3U)] = -0.35;
  mixed[system.Value().physical_count] = 0.2;
  mixed[system.Value().physical_count + system.Value().algebraic_count] = -0.15;
  mixed[system.Value().physical_count + 2U * system.Value().algebraic_count] = 0.1;
  sweep("u", u_only, system.Value(), rates.Value().velocity,
        rates.Value().acceleration, true);
  sweep("phi", phi_only, system.Value(), rates.Value().velocity,
        rates.Value().acceleration, true);
  sweep("z", z_only, system.Value(), rates.Value().velocity,
        rates.Value().acceleration, true);
  sweep("vz", vz_only, system.Value(), rates.Value().velocity,
        rates.Value().acceleration, true);
  sweep("az", az_only, system.Value(), rates.Value().velocity,
        rates.Value().acceleration, true);
  sweep("mixed", mixed, system.Value(), rates.Value().velocity,
        rates.Value().acceleration, true);
  // Keep the finite F-position derivative large enough that deleting it is
  // observable under the existing all-block, consecutive-step N5 gate. The
  // root and graph-regularity probe for this warped shared-node state
  // converges at this load without changing its principal-coordinate branch.
  const auto no_force_actual = Assemble(
      fixture, shells.Value(), root.Value().q, rates.Value().velocity,
      rates.Value().acceleration, zero, true);
  ASSERT_TRUE(no_force_actual.HasValue());
  const auto no_force_operators =
      fesa::hht_reduced_internal::EvaluateConstraintOperators(
          *fixture.domain, fixture.dofs, shells.Value(),
          no_force_actual.Value(), chart.Value(), force_rate,
          force_acceleration);
  ASSERT_TRUE(no_force_operators.HasValue());
  auto omitted_external_gradient = operators.Value();
  ASSERT_EQ(omitted_external_gradient.rows.size(),
            no_force_operators.Value().rows.size());
  bool saw_external_position_gradient = false;
  for (std::size_t row = 0U; row < omitted_external_gradient.rows.size();
       ++row) {
    auto& omitted = omitted_external_gradient.rows[row].gradient;
    const auto& no_force = no_force_operators.Value().rows[row].gradient;
    ASSERT_EQ(omitted.size(), no_force.size());
    for (std::size_t entry = 0U; entry < omitted.size(); ++entry) {
      ASSERT_EQ(omitted[entry].dof, no_force[entry].dof);
      saw_external_position_gradient =
          saw_external_position_gradient ||
          omitted[entry].value != no_force[entry].value;
      omitted[entry].value = no_force[entry].value;
    }
  }
  ASSERT_TRUE(saw_external_position_gradient);
  const auto omitted_external_gradient_system =
      fesa::hht_reduced_internal::AssembleAugmentedReducedSystem(
          actual.Value(), chart.Value(), omitted_external_gradient, cv, ca, reference_length.Value(), h);
  ASSERT_TRUE(omitted_external_gradient_system.HasValue());
  sweep("mutant-omit-external-gradient", mixed,
        omitted_external_gradient_system.Value(), rates.Value().velocity,
        rates.Value().acceleration, false);
  const auto zero_rates = fesa::Vector(fixture.dofs.FullDofCount());
  const auto ha_actual = Assemble(fixture, shells.Value(), root.Value().q,
                                  zero_rates, rates.Value().acceleration,
                                  force, true);
  ASSERT_TRUE(ha_actual.HasValue());
  const auto ha_operators = fesa::hht_reduced_internal::EvaluateConstraintOperators(
      *fixture.domain, fixture.dofs, shells.Value(), ha_actual.Value(),
      chart.Value(), force_rate, force_acceleration);
  ASSERT_TRUE(ha_operators.HasValue());
  const auto ha_system = fesa::hht_reduced_internal::AssembleAugmentedReducedSystem(
      ha_actual.Value(), chart.Value(), ha_operators.Value(), cv, ca, reference_length.Value(), h);
  ASSERT_TRUE(ha_system.HasValue());
  sweep("Ha", mixed, ha_system.Value(), zero_rates,
        rates.Value().acceleration, true);
  auto omitted_ha = ha_operators.Value();
  for (auto& row : omitted_ha.rows) row.hessian.clear();
  const auto omitted_ha_system =
      fesa::hht_reduced_internal::AssembleAugmentedReducedSystem(
          ha_actual.Value(), chart.Value(), omitted_ha, cv, ca, reference_length.Value(), h);
  ASSERT_TRUE(omitted_ha_system.HasValue());
  sweep("mutant-omit-Ha", mixed, omitted_ha_system.Value(), zero_rates,
        rates.Value().acceleration, false);
  const auto hw_actual = Assemble(fixture, shells.Value(), root.Value().q,
                                  rates.Value().velocity, zero_rates,
                                  force, true);
  ASSERT_TRUE(hw_actual.HasValue());
  const auto hw_operators = fesa::hht_reduced_internal::EvaluateConstraintOperators(
      *fixture.domain, fixture.dofs, shells.Value(), hw_actual.Value(),
      chart.Value(), force_rate, force_acceleration);
  ASSERT_TRUE(hw_operators.HasValue());
  const auto hw_system = fesa::hht_reduced_internal::AssembleAugmentedReducedSystem(
      hw_actual.Value(), chart.Value(), hw_operators.Value(), cv, ca, reference_length.Value(), h);
  ASSERT_TRUE(hw_system.HasValue());
  sweep("HW-2W", mixed, hw_system.Value(), rates.Value().velocity,
        zero_rates, true);
  auto omitted_hw = hw_operators.Value();
  for (auto& row : omitted_hw.rows) row.hessian.clear();
  const auto omitted_hw_system =
      fesa::hht_reduced_internal::AssembleAugmentedReducedSystem(
          hw_actual.Value(), chart.Value(), omitted_hw, cv, ca, reference_length.Value(), h);
  ASSERT_TRUE(omitted_hw_system.HasValue());
  sweep("mutant-omit-HW-2W", mixed, omitted_hw_system.Value(),
        rates.Value().velocity, zero_rates, false);
  auto omitted_third = operators.Value();
  for (auto& row : omitted_third.rows) row.third_directional_gradient.clear();
  const auto third_system = fesa::hht_reduced_internal::AssembleAugmentedReducedSystem(
      actual.Value(), chart.Value(), omitted_third, cv, ca, reference_length.Value(), h);
  ASSERT_TRUE(third_system.HasValue());
  sweep("mutant-omit-third", mixed, third_system.Value(), rates.Value().velocity,
        rates.Value().acceleration, false);
  auto omitted_mixed_time = operators.Value();
  for (auto& row : omitted_mixed_time.rows) row.time_gradient.clear();
  const auto mixed_time_system =
      fesa::hht_reduced_internal::AssembleAugmentedReducedSystem(
          actual.Value(), chart.Value(), omitted_mixed_time, cv, ca, reference_length.Value(), h);
  ASSERT_TRUE(mixed_time_system.HasValue());
  sweep("mutant-omit-psi-qt", mixed, mixed_time_system.Value(),
        rates.Value().velocity, rates.Value().acceleration, false);
  auto omitted_second_time_root = root.Value();
  for (auto& row : omitted_second_time_root.operators.rows) row.time_second = 0.0;
  const auto omitted_second_time_rates =
      fesa::hht_reduced_internal::ReconstructPhysicalRates(
          chart.Value(), omitted_second_time_root, vp, ap, solver);
  ASSERT_TRUE(omitted_second_time_rates.HasValue());
  double omitted_second_time_hidden = 0.0;
  for (const auto& row : operators.Value().rows)
    omitted_second_time_hidden = (std::max)(
        omitted_second_time_hidden,
        std::abs(Hidden(row, omitted_second_time_rates.Value().velocity,
                        omitted_second_time_rates.Value().acceleration)[2U]));
  EXPECT_GT(omitted_second_time_hidden, 1.0e-8);
}

TEST(HhtReducedChannelNewton, RequiresEveryTranslationGateAndThaws) {
  fesa::hht_reduced_internal::FixedChart chart{};
  chart.full_dof_count = 6U;
  chart.p_dofs = {0U, 3U, 4U};
  chart.z_dofs = {5U};
  SetComponentColumns(&chart);
  const std::array<double, 4U> zero{};
  const auto refine = [&](const std::array<double, 2U>& residual,
                          const std::array<double, 4U>& correction,
                          const bool private_pass = true) {
    return fesa::hht_reduced_internal::ShouldRefineAngular(
        chart, residual, correction, private_pass);
  };
  EXPECT_TRUE(refine({1e-10, 2e-10}, zero));
  EXPECT_FALSE(refine({2e-10, 2e-10}, zero));  // Fresh U failure thaws.
  EXPECT_FALSE(refine({0.0, 1e-10}, zero));   // Phi correction-only is excluded.
  EXPECT_FALSE(refine({0.0, 2e-10}, zero, false));
  for (std::size_t i = 0U; i < zero.size(); ++i) {
    auto correction = zero;
    correction[i] = std::nextafter(1e-10, 1.0);
    EXPECT_FALSE(refine({0.0, 2e-10}, correction));
    correction[i] = std::numeric_limits<double>::quiet_NaN();
    EXPECT_FALSE(refine({0.0, 2e-10}, correction));
  }
  EXPECT_FALSE(refine({std::numeric_limits<double>::infinity(), 2e-10}, zero));
  EXPECT_FALSE(refine({0.0, std::numeric_limits<double>::quiet_NaN()}, zero));
  chart.p_dofs = {3U, 4U};
  SetComponentColumns(&chart);
  EXPECT_FALSE(refine({0.0, 2e-10}, zero));
  chart.p_dofs = {0U};
  chart.z_dofs.clear();
  SetComponentColumns(&chart);
  EXPECT_FALSE(refine({0.0, 2e-10}, zero));
}

TEST(HhtReducedChannelNewton, RetainsAllAuxiliaryBlocksAndFullModelDefect) {
  fesa::hht_reduced_internal::FixedChart chart{};
  chart.full_dof_count = 6U;
  chart.p_dofs = {0U, 3U, 4U};
  chart.z_dofs = {5U};
  SetComponentColumns(&chart);
  std::vector<fesa::CooContribution> entries;
  for (std::size_t i = 0U; i < 6U; ++i)
    entries.push_back({i, i, static_cast<double>(i + 2U), 0U, i});
  entries.push_back({0U, 1U, 0.25, 0U, 6U});
  entries.push_back({1U, 0U, 0.5, 0U, 7U});
  entries.push_back({3U, 1U, 0.7, 0U, 8U});
  entries.push_back({1U, 3U, -0.2, 0U, 9U});
  entries.push_back({2U, 4U, 1.5, 0U, 10U});
  entries.push_back({4U, 2U, 0.1, 0U, 11U});
  entries.push_back({5U, 1U, 0.3, 0U, 12U});
  auto tangent = fesa::hht_internal::MakeSparse(6U, 6U, std::move(entries));
  ASSERT_TRUE(tangent.HasValue());
  fesa::Vector expected{6U};
  expected[1U] = 2.0;
  expected[2U] = -1.0;
  expected[3U] = 0.4;
  expected[4U] = 0.2;
  expected[5U] = -0.3;
  auto rhs = tangent.Value().Multiply(expected);
  rhs[0U] += 1.0;  // Omitted row is not silently certified by the J solve.
  const fesa::hht_reduced_internal::AugmentedReducedSystem full{
      tangent.Value(), rhs, 3U, 1U, fesa::Vector(6U, 1.0),
      fesa::Vector(6U, 1.0)};
  auto selected = fesa::hht_reduced_internal::SelectAugmentedSolve(full, chart, true);
  ASSERT_TRUE(selected.HasValue());
  EXPECT_EQ(selected.Value().full_indices,
            (std::vector<std::size_t>{1U, 2U, 3U, 4U, 5U}));
  fesa::MklPardisoTangentSolver solver;
  ASSERT_TRUE(solver.FactorizeFresh(selected.Value().tangent).IsOk());
  fesa::Vector step{5U};
  ASSERT_TRUE(solver.SolveCorrection(selected.Value().rhs, step).IsOk());
  EXPECT_TRUE(fesa::hht_internal::CheckLinearSolve(
      selected.Value().tangent, selected.Value().rhs, step).IsOk());
  auto embedded = fesa::hht_reduced_internal::EmbedAugmentedSolution(
      selected.Value(), step, 6U);
  ASSERT_TRUE(embedded.HasValue());
  EXPECT_DOUBLE_EQ(embedded.Value()[0U], 0.0);
  for (std::size_t i = 1U; i < 6U; ++i)
    EXPECT_NEAR(embedded.Value()[i], expected[i], 1e-14);
  EXPECT_FALSE(fesa::hht_internal::CheckLinearSolve(
      full.tangent, full.rhs, embedded.Value()).IsOk());
  auto mapped = fesa::hht_reduced_internal::MapAugmentedCorrection(
      chart, full, embedded.Value(), 2.0, 3.0);
  ASSERT_TRUE(mapped.HasValue());
  EXPECT_DOUBLE_EQ(mapped.Value().coordinate[0U], 0.0);
  EXPECT_DOUBLE_EQ(mapped.Value().velocity[0U], 0.0);
  EXPECT_DOUBLE_EQ(mapped.Value().acceleration[0U], 0.0);
  EXPECT_NEAR(mapped.Value().coordinate[5U], 0.4, 1e-14);
  EXPECT_NEAR(mapped.Value().velocity[5U], 0.2, 1e-14);
  EXPECT_NEAR(mapped.Value().acceleration[5U], -0.3, 1e-14);
  auto thawed = fesa::hht_reduced_internal::SelectAugmentedSolve(full, chart, false);
  ASSERT_TRUE(thawed.HasValue());
  EXPECT_EQ(thawed.Value().full_indices.size(), 6U);
  EXPECT_DOUBLE_EQ(thawed.Value().rhs[0U], rhs[0U]);
  auto invalid = full;
  invalid.rhs[0U] = std::numeric_limits<double>::infinity();
  EXPECT_FALSE(fesa::hht_reduced_internal::SelectAugmentedSolve(
      invalid, chart, true).HasValue());
  step[0U] = std::numeric_limits<double>::quiet_NaN();
  EXPECT_FALSE(fesa::hht_reduced_internal::EmbedAugmentedSolution(
      selected.Value(), step, 6U).HasValue());
}

TEST(HhtReducedChannelNewton, RestrictedSingularityIsNotFullInvertibility) {
  fesa::hht_reduced_internal::FixedChart chart{};
  chart.full_dof_count = 6U;
  chart.p_dofs = {0U, 3U};
  SetComponentColumns(&chart);
  auto matrix = fesa::hht_internal::MakeSparse(
      2U, 2U, {{0U, 1U, 1.0, 0U, 0U}, {1U, 0U, 1.0, 0U, 1U}});
  ASSERT_TRUE(matrix.HasValue());
  const fesa::hht_reduced_internal::AugmentedReducedSystem full{
      matrix.Value(), fesa::Vector{2U}, 2U, 0U, fesa::Vector(2U, 1.0),
      fesa::Vector(2U, 1.0)};
  auto restricted = fesa::hht_reduced_internal::SelectAugmentedSolve(full, chart, true);
  ASSERT_TRUE(restricted.HasValue());
  fesa::MklPardisoTangentSolver solver;
  ASSERT_TRUE(solver.FactorizeFresh(full.tangent).IsOk());
  EXPECT_FALSE(solver.FactorizeFresh(restricted.Value().tangent).IsOk());
}

TEST(HhtReducedOperators, NullMetricKeepsFiniteLeftAssociatedProducts) {
  const auto fixture = MakeFixture(false);
  const auto shells = fesa::hht_internal::BuildRuntimeShells(*fixture.domain,
                                                           fixture.dofs);
  ASSERT_TRUE(shells.HasValue());
  const fesa::Vector zero(fixture.dofs.FullDofCount());
  const auto assembled = Assemble(fixture, shells.Value(), zero, zero, zero,
                                  zero, false);
  ASSERT_TRUE(assembled.HasValue());
  auto assembly = assembled.Value();
  const double maximum = (std::numeric_limits<double>::max)();
  const double epsilon = std::ldexp(1.0, -26);
  const double t = std::sqrt(1.0 - epsilon * epsilon);
  const std::array<double, 3U> n{epsilon, 0.6 * t, 0.8 * t};
  const std::array<double, 9U> block{0.0, maximum, maximum,
                                    maximum, 0.0, 0.0,
                                    maximum, 0.0, 0.0};
  double original_metric = 0.0;
  for (std::size_t i = 0U; i < 3U; ++i)
    for (std::size_t j = 0U; j < 3U; ++j)
      original_metric += (n[i] * block[3U * i + j]) * n[j];
  ASSERT_TRUE(std::isfinite(original_metric));
  ASSERT_GT(original_metric, 0.0);
  ASSERT_FALSE(std::isfinite(block[1U] * n[1U] + block[2U] * n[2U]));
  std::vector<fesa::CooContribution> entries;
  for (std::size_t node = 0U; node < assembly.rotations.size(); ++node) {
    assembly.rotations[node].drill_null = fesa::Vector3(n);
    for (std::size_t i = 0U; i < 3U; ++i)
      for (std::size_t j = 0U; j < 3U; ++j)
        entries.push_back({6U * node + 3U + i, 6U * node + 3U + j,
                           block[3U * i + j], node, 3U * i + j});
  }
  const auto metric = fesa::hht_internal::MakeSparse(
      fixture.dofs.FullDofCount(), fixture.dofs.FullDofCount(),
      std::move(entries));
  ASSERT_TRUE(metric.HasValue());
  assembly.drilling_metric = metric.Value();
  const auto chart = fesa::hht_reduced_internal::BuildFixedChart(
      fixture.dofs, assembly.rotations);
  ASSERT_TRUE(chart.HasValue());
  fesa::hht_reduced_internal::ConstraintOperators operators;
  operators.rows.resize(chart.Value().null_nodes.size());
  for (std::size_t row = 0U; row < operators.rows.size(); ++row)
    for (std::size_t component = 0U; component < 3U; ++component)
      operators.rows[row].gradient.push_back(
          {6U * chart.Value().null_nodes[row] + 3U + component,
           original_metric * n[component]});
  fesa::MklPardisoTangentSolver solver;
  const auto checked = fesa::hht_reduced_internal::CheckGraphRegularity(
      assembly, chart.Value(), operators, solver);
  ASSERT_TRUE(checked.HasValue());
  EXPECT_GE(checked.Value(), std::ldexp(1.0, -32));
}

}  // namespace