#include "fesa/elements/nonlinear_mitc4_inertia.h"

#include <gtest/gtest.h>

#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <iostream>
#include <limits>
#include <string>
#include <vector>

#include "fesa/elements/finite_rotation.h"
#include "fesa/model/model_types.h"
#include "fesa/model/shell_geometry.h"

namespace {

// User-approved physical/numerical comparison tolerance for the local experiment.
constexpr double kNumericalTolerance = 1.0e-3;

fesa::Node Node(const std::int64_t label,
                const std::array<double, 3>& coordinates) {
  return {{"Shell-Instance", label, std::to_string(label)},
          coordinates,
          {"nonlinear-inertia.inp", static_cast<std::size_t>(label)}};
}

std::array<fesa::Node, 4> Nodes() {
  return {Node(1, {-1.0, -1.0, 0.0}), Node(2, {1.0, -1.0, 0.0}),
          Node(3, {1.0, 1.0, 0.0}), Node(4, {-1.0, 1.0, 0.0})};
}

std::array<const fesa::Node*, 4> Pointers(
    const std::array<fesa::Node, 4>& nodes) {
  return {&nodes[0], &nodes[1], &nodes[2], &nodes[3]};
}

std::array<fesa::ShellNodeInitialFrame, 4> Frames() {
  return {{{0U, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}},
           {1U, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}},
           {2U, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}},
           {3U, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}}}};
}

fesa::NonlinearMitc4Inertia Inertia(const std::array<fesa::Node, 4>& nodes) {
  const fesa::ShellSection section{
      "Section", 0.2, 0U, {"nonlinear-inertia.inp", 20U}};
  const fesa::LinearElasticMaterial material{
      "Material", 120.0, 0.25, {"nonlinear-inertia.inp", 21U}};
  const auto inertia = fesa::NonlinearMitc4Inertia::Create(
      Pointers(nodes), Frames(), section, material, 1.0);
  EXPECT_TRUE(inertia.HasValue());
  return inertia.Value();
}

fesa::NonlinearMitc4TrialDofs AddRotation(fesa::NonlinearMitc4TrialDofs trial,
                                          const fesa::Vector& direction,
                                          const double step) {
  for (std::size_t node = 0U; node < 4U; ++node) {
    trial.displacement[node] =
        trial.displacement[node] + fesa::Vector3{direction[6U * node],
                                                 direction[6U * node + 1U],
                                                 direction[6U * node + 2U]} *
                                       step;
    trial.rotation_vector[node] =
        trial.rotation_vector[node] + fesa::Vector3{direction[6U * node + 3U],
                                                    direction[6U * node + 4U],
                                                    direction[6U * node + 5U]} *
                                          step;
  }
  return trial;
}

fesa::Vector Difference(const fesa::Vector& left, const fesa::Vector& right,
                        const double denominator) {
  fesa::Vector result = left;
  result.Axpy(-1.0, right);
  result.Scale(1.0 / denominator);
  return result;
}

TEST(NonlinearMitc4Inertia, IntegratesTheRectangleNodalLumpedMassAndNull) {
  const auto nodes = Nodes();
  const auto evaluation =
      Inertia(nodes).Evaluate({}, fesa::Vector{24U}, fesa::Vector{24U});
  ASSERT_TRUE(evaluation.HasValue());
  const fesa::Matrix& mass = evaluation.Value().mass;
  for (std::size_t row = 0U; row < 4U; ++row) {
    for (std::size_t column = 0U; column < 4U; ++column) {
      EXPECT_NEAR(mass(6U * row, 6U * column), row == column ? 0.2 : 0.0,
                  kNumericalTolerance);
      EXPECT_NEAR(mass(6U * row + 3U, 6U * column + 3U),
                  row == column ? 0.2 * 0.2 * 0.2 / 12.0 : 0.0, kNumericalTolerance);
    }
    EXPECT_NEAR(mass(6U * row + 5U, 6U * row + 5U), 0.0, kNumericalTolerance);
  }
  fesa::Vector drilling_null{24U};
  for (std::size_t node = 0U; node < 4U; ++node) {
    drilling_null[6U * node + 5U] = 1.0;
  }
  EXPECT_LE(mass.Multiply(drilling_null).Norm(), kNumericalTolerance);
}

TEST(NonlinearMitc4Inertia, UsesPhysicalAccelerationAndExactCurrentForce) {
  const auto nodes = Nodes();
  fesa::NonlinearMitc4TrialDofs trial{};
  for (std::size_t node = 0U; node < 4U; ++node) {
    trial.rotation_vector[node] = {0.05 * (node + 1U), -0.03 * (node + 1U),
                                   0.02 * (node + 1U)};
  }
  fesa::Vector acceleration{24U};
  for (std::size_t index = 0U; index < acceleration.Size(); ++index) {
    acceleration[index] = 0.01 * static_cast<double>(index + 1U);
  }
  const auto evaluation =
      Inertia(nodes).Evaluate(trial, fesa::Vector{24U}, acceleration);
  ASSERT_TRUE(evaluation.HasValue());
  const fesa::Vector expected = evaluation.Value().mass.Multiply(acceleration);
  for (std::size_t index = 0U; index < expected.Size(); ++index) {
    EXPECT_NEAR(evaluation.Value().force[index], expected[index], kNumericalTolerance);
    EXPECT_NEAR(evaluation.Value().convective_force[index], 0.0, kNumericalTolerance);
  }
}

TEST(NonlinearMitc4Inertia,
     PreservesTinyAngularAccelerationAlongsideRigidTranslationAcceleration) {
  for (const bool rotated : {false, true}) {
    SCOPED_TRACE(rotated);
    auto nodes = Nodes();
    auto frames = Frames();
    const auto rotation = fesa::ExpSkew(
        rotated ? fesa::Vector3{0.4, -0.2, 0.7} : fesa::Vector3{});
    const auto transform = [&](const std::array<double, 3>& value) {
      std::array<double, 3> result{};
      for (std::size_t i = 0U; i < 3U; ++i)
        for (std::size_t j = 0U; j < 3U; ++j)
          result[i] += rotation(i, j) * value[j];
      return result;
    };
    for (std::size_t node = 0U; node < 4U; ++node) {
      nodes[node].coordinates = transform(nodes[node].coordinates);
      frames[node].director = transform(frames[node].director);
      frames[node].tangent_a = transform(frames[node].tangent_a);
      frames[node].tangent_b = transform(frames[node].tangent_b);
    }
    const fesa::ShellSection section{"Section", 0.2, 0U, {"tiny.inp", 1U}};
    const fesa::LinearElasticMaterial material{
        "Material", 120.0, 0.25, {"tiny.inp", 2U}};
    const auto inertia = fesa::NonlinearMitc4Inertia::Create(
        Pointers(nodes), frames, section, material, 1.0);
    ASSERT_TRUE(inertia.HasValue());
    fesa::Vector acceleration{24U};
    for (std::size_t node = 0U; node < 4U; ++node)
      for (std::size_t c = 0U; c < 3U; ++c) {
        acceleration[6U * node + c] = c == 0U ? 15.0 : (c == 1U ? 12.0 : 0.0);
        acceleration[6U * node + 3U + c] = 1e-17 * frames[node].tangent_a[c];
      }
    const auto actual = inertia.Value().Evaluate({}, fesa::Vector{24U}, acceleration);
    ASSERT_TRUE(actual.HasValue());
    // Nodal m=rho*A*t/4=.2, rotary inertia=m*t^2/12; c=0 at v=0.
    constexpr double nodal_rotary_inertia = 0.2 * 0.2 * 0.2 / 12.0;
    for (std::size_t node = 0U; node < 4U; ++node) {
      double error = 0.0;
      double scale = 0.0;
      for (std::size_t c = 0U; c < 3U; ++c) {
        const auto full = 6U * node + 3U + c;
        const double expected = nodal_rotary_inertia * acceleration[full];
        error = std::max(error, std::abs(actual.Value().force[full] - expected));
        scale = std::max(scale, std::abs(expected));
        EXPECT_DOUBLE_EQ(actual.Value().convective_force[full], 0.0);
      }
      ASSERT_GT(scale, 0.0);
      EXPECT_LE(error / scale, kNumericalTolerance);
    }
  }
}

// At phi=0, Dq(M*a)[tangent_b]=-.5*j*epsilon*director for
// a_phi=epsilon*tangent_a.  An origin at the active midpoint also isolates
// L=j*v_phi and Ldot=j*a_phi from orbital contributions.
TEST(NonlinearMitc4Inertia, MixedScaleRotationDerivativeAndAngularMomentum) {
  for (const bool rotated : {false, true}) {
    auto nodes = Nodes();
    auto frames = Frames();
    const auto rotation = fesa::ExpSkew(
        rotated ? fesa::Vector3{0.4, -0.2, 0.7} : fesa::Vector3{});
    const auto transform = [&](const std::array<double, 3>& value) {
      std::array<double, 3> result{};
      for (std::size_t i = 0U; i < 3U; ++i)
        for (std::size_t j = 0U; j < 3U; ++j)
          result[i] += rotation(i, j) * value[j];
      return result;
    };
    for (std::size_t node = 0U; node < 4U; ++node) {
      nodes[node].coordinates = transform(nodes[node].coordinates);
      frames[node].director = transform(frames[node].director);
      frames[node].tangent_a = transform(frames[node].tangent_a);
      frames[node].tangent_b = transform(frames[node].tangent_b);
    }
    const auto origin = nodes[0].coordinates;
    for (auto& node : nodes)
      for (std::size_t c = 0U; c < 3U; ++c)
        node.coordinates[c] -= origin[c];
    const fesa::ShellSection section{"Section", 0.2, 0U, {"tiny.inp", 1U}};
    const fesa::LinearElasticMaterial material{
        "Material", 120.0, 0.25, {"tiny.inp", 2U}};
    const auto inertia = fesa::NonlinearMitc4Inertia::Create(
        Pointers(nodes), frames, section, material, 1.0);
    ASSERT_TRUE(inertia.HasValue());
    constexpr double nodal_rotary_inertia = 0.2 * 0.2 * 0.2 / 12.0;
    for (int power = 7; power <= 20; ++power) {
      SCOPED_TRACE(rotated);
      SCOPED_TRACE(power);
      const double epsilon = std::pow(10.0, -power);
      fesa::Vector acceleration{24U};
      acceleration[0] = 15.0;
      acceleration[1] = 12.0;
      for (std::size_t c = 0U; c < 3U; ++c)
        acceleration[3U + c] = epsilon * frames[0].tangent_a[c];
      const auto actual = inertia.Value().Evaluate(
          {}, fesa::Vector{24U}, acceleration);
      ASSERT_TRUE(actual.HasValue());
      double derivative_error = 0.0;
      double rate_error = 0.0;
      for (std::size_t c = 0U; c < 3U; ++c) {
        double derivative = 0.0;
        for (std::size_t k = 0U; k < 3U; ++k)
          derivative += actual.Value().position_tangent(3U + c, 3U + k) *
                        frames[0].tangent_b[k];
        derivative_error = std::max(derivative_error,
            std::abs(derivative + 0.5 * nodal_rotary_inertia * epsilon *
                                      frames[0].director[c]));
        rate_error = std::max(rate_error,
            std::abs(actual.Value().angular_momentum_rate[c] -
                     nodal_rotary_inertia * acceleration[3U + c]));
      }
      EXPECT_LE(derivative_error / (nodal_rotary_inertia * epsilon), kNumericalTolerance);
      EXPECT_LE(rate_error / (nodal_rotary_inertia * epsilon), kNumericalTolerance);
      const auto moving = inertia.Value().Evaluate(
          {}, acceleration, fesa::Vector{24U});
      ASSERT_TRUE(moving.HasValue());
      double momentum_error = 0.0;
      for (std::size_t c = 0U; c < 3U; ++c)
        momentum_error = std::max(momentum_error,
            std::abs(moving.Value().angular_momentum[c] -
                     nodal_rotary_inertia * acceleration[3U + c]));
      EXPECT_LE(momentum_error / (nodal_rotary_inertia * epsilon), kNumericalTolerance);
    }
  }
}

using V3 = fesa::Vector3;

V3 Multiply(const fesa::Matrix& matrix, const V3& value) {
  return {matrix(0, 0) * value.X() + matrix(0, 1) * value.Y() +
              matrix(0, 2) * value.Z(),
          matrix(1, 0) * value.X() + matrix(1, 1) * value.Y() +
              matrix(1, 2) * value.Z(),
          matrix(2, 0) * value.X() + matrix(2, 1) * value.Y() +
              matrix(2, 2) * value.Z()};
}

// exp of the 24x24 subset-block matrix applied to (d0,0,...,0).
// Its off-diagonal blocks are the three direction skew matrices; the
// subset-7 block is the mixed third derivative. No Jl scalar is reused.
V3 BlockExponentialDerivative(const V3& phi, const V3& initial,
                              const std::vector<V3>& directions) {
  std::array<V3, 8> term{};
  std::array<V3, 8> sum{};
  term[0] = initial;
  sum[0] = initial;
  const std::size_t blocks = std::size_t{1U} << directions.size();
  for (std::size_t order = 1U; order <= 64U; ++order) {
    std::array<V3, 8> next{};
    for (std::size_t subset = 0U; subset < blocks; ++subset) {
      next[subset] = phi.Cross(term[subset]);
      for (std::size_t axis = 0U; axis < directions.size(); ++axis) {
        const std::size_t bit = std::size_t{1U} << axis;
        if ((subset & bit) != 0U) {
          next[subset] =
              next[subset] + directions[axis].Cross(term[subset ^ bit]);
        }
      }
      next[subset] = next[subset] / static_cast<double>(order);
      sum[subset] = sum[subset] + next[subset];
    }
    term = next;
  }
  return sum[blocks - 1U];
}

V3 Second(const fesa::DynamicRotationKinematics& value, const V3& p,
          const V3& r) {
  V3 result{};
  for (std::size_t i = 0; i < 3; ++i) {
    for (std::size_t j = 0; j < 3; ++j) {
      result = result + value.director_second[i][j] * p[i] * r[j];
    }
  }
  return result;
}

V3 Third(const fesa::DynamicRotationKinematics& value, const V3& p, const V3& r,
         const V3& t) {
  V3 result{};
  for (std::size_t i = 0; i < 3; ++i) {
    for (std::size_t j = 0; j < 3; ++j) {
      for (std::size_t k = 0; k < 3; ++k) {
        result = result + value.director_third[i][j][k] * p[i] * r[j] * t[k];
      }
    }
  }
  return result;
}

double MatrixNorm(const fesa::Matrix& matrix) {
  double norm = 0.0;
  for (std::size_t i = 0; i < matrix.Rows(); ++i) {
    for (std::size_t j = 0; j < matrix.Columns(); ++j) {
      norm = std::hypot(norm, matrix(i, j));
    }
  }
  return norm;
}

struct Fixture {
  std::array<fesa::Node, 4> nodes = Nodes();
  std::array<fesa::ShellNodeInitialFrame, 4> frames = Frames();
  double thickness = 0.2;
  double density = 1.0;
  double modulus = 1000.0;

  double Length() const {
    double length = 0.0;
    for (std::size_t i = 0; i < 4; ++i) {
      length = std::max(length, (V3{nodes[(i + 1U) % 4U].coordinates} -
                                 V3{nodes[i].coordinates})
                                    .Norm());
    }
    return length;
  }

  fesa::Result<fesa::NonlinearMitc4Inertia> Create() const {
    return fesa::NonlinearMitc4Inertia::Create(
        Pointers(nodes), frames,
        {"Section", thickness, 0U, {"nonlinear-inertia.inp", 20U}},
        {"Material", modulus, 0.3, {"nonlinear-inertia.inp", 21U}}, density);
  }
};

Fixture Warped() {
  Fixture fixture;
  fixture.nodes[2].coordinates[2] = 0.1;
  const fesa::Mitc4ShellDefinition element{{"Shell-Instance", 1, "1"},
                                           fesa::ShellSourceElementType::kS4,
                                           {0U, 1U, 2U, 3U},
                                           0U,
                                           0U,
                                           {"nonlinear-inertia.inp", 10U}};
  const auto geometry = fesa::PreprocessShellGeometry(
      std::vector<fesa::Node>{fixture.nodes.begin(), fixture.nodes.end()},
      {element}, {{"Section", 0.2, 0U, {"nonlinear-inertia.inp", 20U}}});
  EXPECT_TRUE(geometry.HasValue());
  for (std::size_t i = 0; i < 4; ++i) {
    fixture.frames[i] = geometry.Value().nodal_frames[i];
  }
  return fixture;
}

Fixture WarpedNonuniformDirectors() {
  Fixture fixture = Warped();
  for (const std::size_t node : {0U, 3U, 1U, 2U}) {
    const double angle = (node == 0U || node == 3U) ? -0.1 : 0.1;
    const double cosine = std::cos(angle);
    const double sine = std::sin(angle);
    fixture.frames[node].tangent_a = {cosine, 0.0, -sine};
    fixture.frames[node].tangent_b = {0.0, 1.0, 0.0};
    fixture.frames[node].director = {sine, 0.0, cosine};
  }
  return fixture;
}

V3 RotateFrame(const V3& value) {
  constexpr double kCosZ = 0.7648421872844885;
  constexpr double kSinZ = 0.6442176872376910;
  constexpr double kCosX = 0.9210609940028851;
  constexpr double kSinX = 0.3894183423086505;
  return {kCosZ * value.X() - kSinZ * kCosX * value.Y() +
              kSinZ * kSinX * value.Z(),
          kSinZ * value.X() + kCosZ * kCosX * value.Y() -
              kCosZ * kSinX * value.Z(),
          kSinX * value.Y() + kCosX * value.Z()};
}

Fixture FlatRotatedLargeOffset() {
  Fixture fixture;
  for (std::size_t node = 0U; node < 4U; ++node) {
    fixture.nodes[node].coordinates =
        (RotateFrame(V3{fixture.nodes[node].coordinates}) +
         V3{1.0e8, -2.0e8, 3.0e8})
            .Components();
    fixture.frames[node].director =
        RotateFrame(V3{fixture.frames[node].director}).Components();
    fixture.frames[node].tangent_a =
        RotateFrame(V3{fixture.frames[node].tangent_a}).Components();
    fixture.frames[node].tangent_b =
        RotateFrame(V3{fixture.frames[node].tangent_b}).Components();
  }
  return fixture;
}

struct State {
  fesa::NonlinearMitc4TrialDofs q{};
  fesa::Vector v{24U};
  fesa::Vector a{24U};
};

State FiniteState() {
  State state;
  for (std::size_t node = 0; node < 4; ++node) {
    const double i = static_cast<double>(node + 1U);
    state.q.displacement[node] = {0.01 * i, -0.01 * i, 0.005 * i};
    state.q.rotation_vector[node] = {0.05 * i, -0.03 * i, 0.02 * i};
    const std::array<double, 6> v{0.1, 0.05 * i, -0.03 * i,
                                  0.2, -0.1 * i, 0.04 * i};
    const std::array<double, 6> a{0.3, -0.2, 0.1 * i, -0.07 * i, 0.06, 0.05};
    for (std::size_t j = 0; j < 6; ++j) {
      state.v[6U * node + j] = v[j];
      state.a[6U * node + j] = a[j];
    }
  }
  return state;
}

fesa::Vector ApplyD(fesa::Vector vector, const double length) {
  for (std::size_t i = 0; i < vector.Size(); ++i) {
    if (i % 6U < 3U) {
      vector[i] *= length;
    }
  }
  return vector;
}

TEST(NonlinearMitc4Inertia,
     FlatRotatedLargeOffsetKeepsCenteredRotationalInertiaExactlyZero) {
  const Fixture fixture = FlatRotatedLargeOffset();
  const auto inertia = fixture.Create();
  ASSERT_TRUE(inertia.HasValue());
  fesa::Vector acceleration{24U};
  for (std::size_t node = 0U; node < 4U; ++node) {
    acceleration[6U * node] = 0.3;
    acceleration[6U * node + 1U] = -0.2;
    acceleration[6U * node + 2U] = 0.1;
  }
  const auto value = inertia.Value().Evaluate({}, fesa::Vector{24U}, acceleration);
  ASSERT_TRUE(value.HasValue());
  const auto mass_times_acceleration = value.Value().mass.Multiply(acceleration);
  for (std::size_t node = 0U; node < 4U; ++node) {
    for (std::size_t component = 0U; component < 3U; ++component) {
      EXPECT_DOUBLE_EQ(value.Value().force[6U * node + 3U + component], 0.0);
      EXPECT_DOUBLE_EQ(mass_times_acceleration[6U * node + 3U + component],
                       0.0);
    }
  }
}

TEST(NonlinearMitc4Inertia, WarpedGeometryRetainsNonzeroFirstThicknessMoment) {
  const auto inertia = WarpedNonuniformDirectors().Create();
  ASSERT_TRUE(inertia.HasValue());
  const auto value = inertia.Value().Evaluate({}, fesa::Vector{24U}, fesa::Vector{24U});
  ASSERT_TRUE(value.HasValue());
  double coupling = 0.0;
  for (std::size_t row = 0U; row < 24U; ++row) {
    for (std::size_t column = 0U; column < 24U; ++column) {
      if (row % 6U >= 3U && column % 6U < 3U)
        coupling = std::hypot(coupling, value.Value().mass(row, column));
    }
  }
  EXPECT_GT(coupling, 0.0);
}

fesa::Matrix ScaledMatrix(const fesa::Matrix& matrix, const double length) {
  fesa::Matrix result{matrix.Rows(), matrix.Columns()};
  for (std::size_t i = 0; i < matrix.Rows(); ++i) {
    for (std::size_t j = 0; j < matrix.Columns(); ++j) {
      result(i, j) = matrix(i, j) * (i % 6U < 3U ? length : 1.0) *
                     (j % 6U < 3U ? length : 1.0);
    }
  }
  return result;
}

TEST(DynamicRotation, UsesCubicDirectorAndJacobianAtUnitAxisRotation) {
  const auto value = fesa::EvaluateDynamicRotation(
      {1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}, {0.0, 1.0, 0.0}, {});
  ASSERT_TRUE(value.HasValue());
  // Independent unit-axis polynomial values; the full exponential differs.
  EXPECT_NEAR(value.Value().director.X(), 0.0, kNumericalTolerance);
  EXPECT_NEAR(value.Value().director.Y(), -5.0 / 6.0, kNumericalTolerance);
  EXPECT_NEAR(value.Value().director.Z(), 0.5, kNumericalTolerance);
  EXPECT_NEAR(value.Value().angular_velocity.X(), 0.0, kNumericalTolerance);
  EXPECT_NEAR(value.Value().angular_velocity.Y(), 5.0 / 6.0, kNumericalTolerance);
  EXPECT_NEAR(value.Value().angular_velocity.Z(), 11.0 / 24.0, kNumericalTolerance);
}

TEST(DynamicRotation, InverseIdentityAndCurrentAnalyticNullAtFiniteRotation) {
  for (const V3 phi : {V3{0.5, 0.0, 0.0}, V3{0.2, -0.3, 0.1}}) {
    const auto rotation = fesa::ExpSkew(phi);
    const auto jacobian = fesa::LeftJacobianSO3(phi);
    for (std::size_t column = 0; column < 3; ++column) {
      // R^T e_j is a unit initial director; its current director is e_j.
      const V3 initial{rotation(column, 0), rotation(column, 1),
                       rotation(column, 2)};
      const auto value = fesa::EvaluateDynamicRotation(phi, initial, {}, {});
      ASSERT_TRUE(value.HasValue());
      const V3 mapped = Multiply(jacobian, value.Value().drill_null);
      EXPECT_GT(mapped[column], 0.0);
      EXPECT_NEAR(value.Value().drill_null.Norm(), 1.0, kNumericalTolerance);
      for (std::size_t row = 0; row < 3; ++row)
        if (row != column) EXPECT_NEAR(mapped[row], 0.0, kNumericalTolerance);
    }
  }
  for (const Fixture fixture : {Fixture{}, Warped()}) {
    const auto inertia = fixture.Create();
    ASSERT_TRUE(inertia.HasValue());
    for (const bool uniform : {false, true}) {
      State state = FiniteState();
      if (uniform) {
        state.q.rotation_vector.fill(V3{0.2, -0.3, 0.1});
      }
      const auto value = inertia.Value().Evaluate(state.q, state.v, state.a);
      ASSERT_TRUE(value.HasValue());
      const auto mass = ScaledMatrix(value.Value().mass, fixture.Length());
      for (std::size_t node = 0; node < 4; ++node) {
        const auto rotation = fesa::EvaluateDynamicRotation(
            state.q.rotation_vector[node], V3{fixture.frames[node].director},
            {}, {});
        ASSERT_TRUE(rotation.HasValue());
        fesa::Vector n{24U};
        for (std::size_t j = 0; j < 3; ++j) {
          n[6U * node + 3U + j] = rotation.Value().drill_null[j];
        }
        EXPECT_LE(mass.Multiply(n).Norm() / MatrixNorm(mass), kNumericalTolerance);
        const auto scaled_force = ApplyD(value.Value().force, fixture.Length());
        EXPECT_LE(std::abs(n.Dot(scaled_force)) / scaled_force.Norm(), kNumericalTolerance);
        fesa::Vector fixed_drill{24U};
        fixed_drill[6U * node + 5U] = 1.0;
        EXPECT_GT(mass.Multiply(fixed_drill).Norm() / MatrixNorm(mass), 1.0e-8);
      }
    }
  }
}

TEST(NonlinearMitc4Inertia,
     RejectsIllConditionedPhysicalMassWithoutDeletingIt) {
  for (const double density : {1.0e-180, 1.0, 1.0e180}) {
    Fixture normal;
    normal.density = density;
    EXPECT_TRUE(normal.Create().HasValue());
    Fixture thin = normal;
    thin.thickness = 1.0e-5;
    const auto rejected = thin.Create();
    ASSERT_FALSE(rejected.HasValue());
    EXPECT_EQ(rejected.GetStatus().Diagnostics()[0].code,
              "ill-conditioned-nonlinear-inertia-positive-mass");
  }
}

TEST(NonlinearMitc4Inertia,
     ConvectiveForceDoesNotLoseSmallValuesToAcceleration) {
  const auto inertia = Fixture{}.Create();
  ASSERT_TRUE(inertia.HasValue());
  State state = FiniteState();
  const auto ordinary = inertia.Value().Evaluate(state.q, state.v, state.a);
  state.a.Scale(1.0e15);
  const auto large = inertia.Value().Evaluate(state.q, state.v, state.a);
  ASSERT_TRUE(ordinary.HasValue());
  ASSERT_TRUE(large.HasValue());
  EXPECT_GT(ordinary.Value().convective_force.Norm(), 0.0);
  for (std::size_t i = 0; i < 24; ++i) {
    EXPECT_EQ(ordinary.Value().convective_force[i],
              large.Value().convective_force[i]);
  }
}

TEST(NonlinearMitc4Inertia,
     PartialAndEffectiveTangentsPassTheScaledFixedSweep) {
  constexpr std::array<int, 7> kExponents{8, 10, 12, 14, 16, 18, 20};
  for (const Fixture fixture : {Fixture{}, Warped()}) {
    const auto inertia = fixture.Create();
    ASSERT_TRUE(inertia.HasValue());
    const State state = FiniteState();
    const auto value = inertia.Value().Evaluate(state.q, state.v, state.a);
    ASSERT_TRUE(value.HasValue());
    for (std::size_t channel = 0; channel < 3; ++channel) {
      fesa::Vector p{24U};
      for (std::size_t i = 0; i < 24; ++i) {
        if (channel == 2U || (channel == 0U) == (i % 6U < 3U)) {
          p[i] = static_cast<double>(i + 1U) * (i % 2U == 0U ? 1.0 : -1.0);
        }
      }
      p.Scale(1.0 / p.Norm());
      const auto direction = ApplyD(p, fixture.Length());
      // q, v, a partials and the two HHT dependent-rate effective tangents.
      for (std::size_t mode = 0; mode < 5; ++mode) {
        const double h = mode == 4U ? 0.025 : 0.1;
        const double cv =
            mode >= 3U ? 0.55 / (0.275625 * h) : (mode == 1U ? 1.0 : 0.0);
        const double ca = mode >= 3U ? 1.0 / (0.95 * 0.275625 * h * h)
                                     : (mode == 2U ? 1.0 : 0.0);
        const double cq = mode == 0U || mode >= 3U ? 1.0 : 0.0;
        fesa::Matrix tangent{24U, 24U};
        for (std::size_t i = 0; i < 24; ++i) {
          for (std::size_t j = 0; j < 24; ++j) {
            tangent(i, j) = cq * value.Value().position_tangent(i, j) +
                            cv * value.Value().velocity_tangent(i, j) +
                            ca * value.Value().mass(i, j);
          }
        }
        const auto scaled = ScaledMatrix(tangent, fixture.Length());
        const auto analytical = scaled.Multiply(p);
        bool previous_pass = false;
        bool consecutive = false;
        for (const int exponent : kExponents) {
          const double step = std::ldexp(1.0, -exponent);
          State plus = state;
          State minus = state;
          plus.q = AddRotation(state.q, direction, cq * step);
          minus.q = AddRotation(state.q, direction, -cq * step);
          plus.v.Axpy(cv * step, direction);
          minus.v.Axpy(-cv * step, direction);
          plus.a.Axpy(ca * step, direction);
          minus.a.Axpy(-ca * step, direction);
          const auto vp = inertia.Value().Evaluate(plus.q, plus.v, plus.a);
          const auto vm = inertia.Value().Evaluate(minus.q, minus.v, minus.a);
          ASSERT_TRUE(vp.HasValue());
          ASSERT_TRUE(vm.HasValue());
          const auto fd =
              ApplyD(Difference(vp.Value().force, vm.Value().force, 2.0 * step),
                     fixture.Length());
          const double error = Difference(analytical, fd, 1.0).Norm();
          const double scale = analytical.Norm() > 0.0
                                   ? analytical.Norm() + fd.Norm()
                                   : MatrixNorm(scaled);
          const double metric = scale > 0.0 ? error / scale : error;
          const double tolerance = kNumericalTolerance;
          std::cout << "C06 tangent channel=" << channel << " mode=" << mode
                    << " j=" << exponent << " raw=" << error
                    << " metric=" << metric << '\n';
          const bool pass =
              metric <= tolerance && exponent >= 10 && exponent <= 18;
          consecutive = consecutive || (previous_pass && pass);
          previous_pass = pass;
        }
        EXPECT_TRUE(consecutive) << "channel=" << channel << " mode=" << mode;
      }
    }
  }
}

struct Integral {
  std::array<std::array<std::array<double, 4>, 4>, 3> moments{};
  fesa::Matrix mass{24U, 24U};
  fesa::Vector force{24U};
  double energy = 0.0;
  double power = 0.0;
  double total_mass = 0.0;
  V3 momentum{};
  V3 angular_momentum{};
  V3 momentum_rate{};
  V3 angular_momentum_rate{};
};

Integral IntegrateFivePoints(const Fixture& fixture, const State& state,
                             const bool lumped = true) {
  const double r = std::sqrt(10.0 / 7.0);
  const double outer = std::sqrt(5.0 + 2.0 * r) / 3.0;
  const double inner = std::sqrt(5.0 - 2.0 * r) / 3.0;
  const double wouter = (322.0 - 13.0 * std::sqrt(70.0)) / 900.0;
  const double winner = (322.0 + 13.0 * std::sqrt(70.0)) / 900.0;
  const std::array<double, 5> points{-outer, -inner, 0.0, inner, outer};
  const std::array<double, 5> weights{wouter, winner, 128.0 / 225.0, winner,
                                      wouter};
  const std::array<double, 4> sx{-1, 1, 1, -1};
  const std::array<double, 4> sy{-1, -1, 1, 1};
  std::array<V3, 4> directors{};
  std::array<std::array<V3, 3>, 4> first{};
  std::array<V3, 4> convective{};
  for (std::size_t node = 0; node < 4; ++node) {
    const V3 phi = state.q.rotation_vector[node];
    const V3 d0{fixture.frames[node].director};
    directors[node] = BlockExponentialDerivative(phi, d0, {});
    const V3 vr{state.v[6U * node + 3U], state.v[6U * node + 4U],
                state.v[6U * node + 5U]};
    convective[node] = BlockExponentialDerivative(phi, d0, {vr, vr});
    for (std::size_t j = 0; j < 3; ++j) {
      std::array<double, 3> basis{};
      basis[j] = 1.0;
      first[node][j] = BlockExponentialDerivative(phi, d0, {V3{basis}});
    }
  }
  Integral integral;
  for (std::size_t xi = 0; xi < 5; ++xi) {
    for (std::size_t eta = 0; eta < 5; ++eta) {
      for (std::size_t zeta = 0; zeta < 5; ++zeta) {
        for (std::size_t mass_node = 0; mass_node < (lumped ? 4U : 1U);
             ++mass_node) {
          std::array<double, 4> kinetic_shape{};
          kinetic_shape[mass_node] = 1.0;
          const double z = 0.5 * fixture.thickness * points[zeta];
          std::array<double, 4> shape{};
          std::array<V3, 24> h{};
          V3 a1{};
          V3 a2{};
          V3 a3{};
          V3 position{};
          V3 velocity{};
          V3 acceleration{};
          for (std::size_t node = 0; node < 4; ++node) {
            shape[node] = 0.25 * (1.0 + sx[node] * points[xi]) *
                          (1.0 + sy[node] * points[eta]);
            if (!lumped) kinetic_shape[node] = shape[node];
            const double dx = 0.25 * sx[node] * (1.0 + sy[node] * points[eta]);
            const double dy = 0.25 * sy[node] * (1.0 + sx[node] * points[xi]);
            const V3 d0{fixture.frames[node].director};
            const V3 x0{fixture.nodes[node].coordinates};
            a1 = a1 + dx * (x0 + z * d0);
            a2 = a2 + dy * (x0 + z * d0);
            a3 = a3 + 0.5 * fixture.thickness * shape[node] * d0;
            position = position +
                       kinetic_shape[node] * (x0 + state.q.displacement[node] +
                                              z * directors[node]);
            acceleration =
                acceleration + z * kinetic_shape[node] * convective[node];
            for (std::size_t j = 0; j < 3; ++j) {
              std::array<double, 3> axis{};
              axis[j] = kinetic_shape[node];
              h[6U * node + j] = V3{axis};
              h[6U * node + 3U + j] = z * kinetic_shape[node] * first[node][j];
            }
          }
          const double weight = fixture.density * weights[xi] * weights[eta] *
                                weights[zeta] * a1.Dot(a2.Cross(a3)) *
                                (lumped ? shape[mass_node] : 1.0);
          EXPECT_GT(weight, 0.0);
          integral.total_mass += weight;
          for (std::size_t i = 0; i < 24; ++i) {
            velocity = velocity + h[i] * state.v[i];
            acceleration = acceleration + h[i] * state.a[i];
          }
          integral.energy += 0.5 * weight * velocity.Dot(velocity);
          integral.power += weight * velocity.Dot(acceleration);
          integral.momentum = integral.momentum + weight * velocity;
          integral.angular_momentum =
              integral.angular_momentum + weight * position.Cross(velocity);
          integral.momentum_rate =
              integral.momentum_rate + weight * acceleration;
          integral.angular_momentum_rate =
              integral.angular_momentum_rate +
              weight * position.Cross(acceleration);
          for (std::size_t i = 0; i < 24; ++i) {
            integral.force[i] += weight * h[i].Dot(acceleration);
            for (std::size_t j = 0; j < 24; ++j) {
              integral.mass(i, j) += weight * h[i].Dot(h[j]);
            }
          }
          for (std::size_t i = 0; i < 4; ++i) {
            for (std::size_t j = 0; j < 4; ++j) {
              double moment =
                  lumped ? (i == mass_node && j == mass_node ? weight : 0.0)
                         : weight * shape[i] * shape[j];
              for (std::size_t k = 0; k < 3; ++k) {
                integral.moments[k][i][j] += moment;
                moment *= z;
              }
            }
          }
        }
      }
    }
  }
  return integral;
}

TEST(NonlinearMitc4Inertia,
     MatchesIndependentWarpedNodalMomentsEnergyAndPower) {
  for (const Fixture fixture :
       {Fixture{}, Warped(), WarpedNonuniformDirectors()}) {
    const auto inertia = fixture.Create();
    ASSERT_TRUE(inertia.HasValue());
    const State state = FiniteState();
    const auto value = inertia.Value().Evaluate(state.q, state.v, state.a);
    ASSERT_TRUE(value.HasValue());
    const Integral oracle = IntegrateFivePoints(fixture, state);
    const auto actual_mass = ScaledMatrix(value.Value().mass, fixture.Length());
    const auto expected_mass = ScaledMatrix(oracle.mass, fixture.Length());
    double mass_error = 0.0;
    double symmetry_error = 0.0;
    for (std::size_t i = 0; i < 24; ++i) {
      for (std::size_t j = 0; j < 24; ++j) {
        mass_error =
            std::hypot(mass_error, actual_mass(i, j) - expected_mass(i, j));
        symmetry_error =
            std::hypot(symmetry_error, actual_mass(i, j) - actual_mass(j, i));
      }
    }
    EXPECT_LE(mass_error / MatrixNorm(expected_mass), kNumericalTolerance);
    EXPECT_LE(symmetry_error / MatrixNorm(expected_mass), kNumericalTolerance);
    EXPECT_LE(
        std::abs(value.Value().kinetic_energy - oracle.energy) / oracle.energy,
        kNumericalTolerance);
    EXPECT_LE(std::abs(0.5 * state.v.Dot(value.Value().mass.Multiply(state.v)) -
                       oracle.energy) /
                  oracle.energy,
              kNumericalTolerance);
    EXPECT_LE(std::abs(state.v.Dot(value.Value().force) - oracle.power) /
                  std::abs(oracle.power),
              kNumericalTolerance);
    EXPECT_LE(Difference(ApplyD(value.Value().force, fixture.Length()),
                         ApplyD(oracle.force, fixture.Length()), 1.0)
                      .Norm() /
                  ApplyD(oracle.force, fixture.Length()).Norm(),
              kNumericalTolerance);
    EXPECT_LE((value.Value().momentum - oracle.momentum).Norm() /
                  oracle.momentum.Norm(),
              kNumericalTolerance);
    EXPECT_LE(
        (value.Value().angular_momentum - oracle.angular_momentum).Norm() /
            oracle.angular_momentum.Norm(),
        kNumericalTolerance);
    EXPECT_LE((value.Value().momentum_rate - oracle.momentum_rate).Norm() /
                  oracle.momentum_rate.Norm(),
              kNumericalTolerance);
    EXPECT_LE(
        (value.Value().angular_momentum_rate - oracle.angular_momentum_rate)
                .Norm() /
            oracle.angular_momentum_rate.Norm(),
        kNumericalTolerance);
    // Recover C0/C1/C2 from their known tensor factors, with the prescribed
    // m_e*(t/2)^k scales even when a centered C1 is exactly zero.
    for (std::size_t i = 0; i < 4; ++i) {
      const auto bi = fesa::EvaluateDynamicRotation(
          state.q.rotation_vector[i], V3{fixture.frames[i].director}, {}, {});
      ASSERT_TRUE(bi.HasValue());
      for (std::size_t j = 0; j < 4; ++j) {
        const auto bj = fesa::EvaluateDynamicRotation(
            state.q.rotation_vector[j], V3{fixture.frames[j].director}, {}, {});
        ASSERT_TRUE(bj.HasValue());
        std::array<double, 3> numerator{};
        std::array<double, 3> denominator{1.0, 0.0, 0.0};
        numerator[0] = value.Value().mass(6U * i, 6U * j);
        for (std::size_t a = 0; a < 3; ++a) {
          for (std::size_t b = 0; b < 3; ++b) {
            const double b1 = bj.Value().director_first[b][a];
            const double b2 =
                bi.Value().director_first[a].Dot(bj.Value().director_first[b]);
            numerator[1] +=
                value.Value().mass(6U * i + a, 6U * j + 3U + b) * b1;
            denominator[1] += b1 * b1;
            numerator[2] +=
                value.Value().mass(6U * i + 3U + a, 6U * j + 3U + b) * b2;
            denominator[2] += b2 * b2;
          }
        }
        double scale = oracle.total_mass;
        for (std::size_t k = 0; k < 3; ++k) {
          EXPECT_LE(std::abs(numerator[k] / denominator[k] -
                             oracle.moments[k][i][j]) /
                        scale,
                    kNumericalTolerance);
          scale *= fixture.thickness / 2.0;
        }
      }
    }
    State uniform;
    for (std::size_t i = 0; i < 4; ++i) {
      uniform.v[6U * i] = 0.3;
      uniform.v[6U * i + 1U] = -0.4;
      uniform.v[6U * i + 2U] = 0.5;
    }
    const auto translation = inertia.Value().Evaluate({}, uniform.v, uniform.a);
    ASSERT_TRUE(translation.HasValue());
    EXPECT_NEAR(translation.Value().kinetic_energy / oracle.total_mass, 0.25,
                kNumericalTolerance);
  }
}

TEST(NonlinearMitc4Inertia,
     InitialMassMatchesEigenLumpingAndFiniteEnergyObeysJensen) {
  for (const Fixture fixture : {Fixture{}, Warped()}) {
    std::array<std::array<double, 3>, 4> directors{};
    for (std::size_t node = 0; node < 4; ++node)
      directors[node] = fixture.frames[node].director;
    const auto shell = fesa::Mitc4Shell::Create(
        Pointers(fixture.nodes), directors,
        {"Section", fixture.thickness, 0U, {"mass-test", 1U}},
        {"Material", fixture.modulus, 0.25, {"mass-test", 2U}});
    ASSERT_TRUE(shell.HasValue());
    const auto eigen = shell.Value().Mass(fixture.density);
    ASSERT_TRUE(eigen.HasValue());
    const auto inertia = fixture.Create();
    ASSERT_TRUE(inertia.HasValue());
    const auto initial =
        inertia.Value().Evaluate({}, fesa::Vector{24U}, fesa::Vector{24U});
    ASSERT_TRUE(initial.HasValue());
    for (std::size_t i = 0; i < 24; ++i) {
      for (std::size_t j = 0; j < 24; ++j) {
        EXPECT_NEAR(initial.Value().mass(i, j),
                    eigen.Value().lumped_global24(i, j), kNumericalTolerance);
        if (i / 6U != j / 6U) EXPECT_EQ(initial.Value().mass(i, j), 0.0);
      }
    }
    const State state = FiniteState();
    const auto lumped = IntegrateFivePoints(fixture, state, true);
    const auto consistent = IntegrateFivePoints(fixture, state, false);
    EXPECT_GT(lumped.energy - consistent.energy, 0.0);
    EXPECT_NEAR(lumped.total_mass, consistent.total_mass, kNumericalTolerance);
  }
}

TEST(DynamicRotation, ThirdDerivativesMatchAllPermutationsAndInteriorSweep) {
  const std::array<V3, 5> states{
      V3{}, V3{1.0e-6, -2.0e-6, 3.0e-6}, V3{0.2, -0.3, 0.4},
      V3{1.0e-4 * (1.0 - std::ldexp(1.0, -10)), 0.0, 0.0},
      V3{1.0e-4 * (1.0 + std::ldexp(1.0, -10)), 0.0, 0.0}};
  const std::array<V3, 4> directions{V3{1, 0, 0}, V3{0, 1, 0}, V3{0, 0, 1},
                                     V3{1, 2, -1} / std::sqrt(6.0)};
  const V3 initial{0, 0, 1};
  for (std::size_t state_index = 0; state_index < states.size();
       ++state_index) {
    const auto value =
        fesa::EvaluateDynamicRotation(states[state_index], initial, {}, {});
    ASSERT_TRUE(value.HasValue());
    for (std::size_t p = 0; p < 4; ++p) {
      std::array<std::array<bool, 4>, 4> previous{};
      std::array<std::array<bool, 4>, 4> consecutive{};
      for (const int exponent : {8, 10, 12, 14, 16, 18, 20}) {
        const double step = std::ldexp(1.0, -exponent);
        const auto plus = fesa::EvaluateDynamicRotation(
            states[state_index] + step * directions[p], initial, {}, {});
        const auto minus = fesa::EvaluateDynamicRotation(
            states[state_index] - step * directions[p], initial, {}, {});
        ASSERT_TRUE(plus.HasValue());
        ASSERT_TRUE(minus.HasValue());
        for (std::size_t r = 0; r < 4; ++r) {
          for (std::size_t t = 0; t < 4; ++t) {
            const V3 analytic = Third(value.Value(), directions[p],
                                      directions[r], directions[t]);
            const V3 fd =
                (Second(plus.Value(), directions[r], directions[t]) -
                 Second(minus.Value(), directions[r], directions[t])) /
                (2.0 * step);
            const V3 oracle = BlockExponentialDerivative(
                states[state_index], initial,
                {directions[p], directions[r], directions[t]});
            EXPECT_LE((analytic - oracle).Norm(), kNumericalTolerance);
            EXPECT_LE((analytic - Third(value.Value(), directions[t],
                                        directions[p], directions[r]))
                          .Norm(),
                      kNumericalTolerance);
            const double error = (analytic - fd).Norm();
            const double scale =
                analytic.Norm() > 0.0 ? analytic.Norm() + fd.Norm() : 1.0;
            const double metric = error / scale;
            const double tolerance = kNumericalTolerance;
            std::cout << "C06 D3 state=" << state_index << " triple=" << p << r
                      << t << " j=" << exponent << " raw=" << error
                      << " metric=" << metric << '\n';
            const bool pass =
                metric <= tolerance && exponent >= 10 && exponent <= 18;
            consecutive[r][t] = consecutive[r][t] || (previous[r][t] && pass);
            previous[r][t] = pass;
          }
        }
      }
      for (std::size_t r = 0; r < 4; ++r) {
        for (std::size_t t = 0; t < 4; ++t) {
          EXPECT_TRUE(consecutive[r][t])
              << "state=" << state_index << " triple=" << p << r << t;
        }
      }
    }
  }
}

TEST(DynamicRotation, NormalizedNullDerivativesIncludeNormalizationCurvature) {
  const V3 initial{0, 0, 1};
  const V3 direction = V3{1, -2, 3} / std::sqrt(14.0);
  for (const V3 phi : {V3{}, V3{1.0e-6, -2.0e-6, 3.0e-6}, V3{0.2, -0.3, 0.4}}) {
    const auto value =
        fesa::EvaluateDynamicRotation(phi, initial, direction, {});
    ASSERT_TRUE(value.HasValue());
    const V3 first = Multiply(value.Value().null_gradient, direction);
    EXPECT_NEAR(value.Value().drill_null.Norm(), 1.0, kNumericalTolerance);
    EXPECT_NEAR(value.Value().drill_null.Dot(first), 0.0, kNumericalTolerance);
    EXPECT_NEAR(
        value.Value().drill_null.Dot(value.Value().null_directional_second),
        -first.Dot(first), kNumericalTolerance);
    bool previous = false;
    bool consecutive = false;
    for (const int exponent : {8, 10, 12, 14, 16, 18, 20}) {
      const double step = std::ldexp(1.0, -exponent);
      const auto plus = fesa::EvaluateDynamicRotation(phi + step * direction,
                                                      initial, direction, {});
      const auto minus = fesa::EvaluateDynamicRotation(phi - step * direction,
                                                       initial, direction, {});
      ASSERT_TRUE(plus.HasValue());
      ASSERT_TRUE(minus.HasValue());
      const V3 fd1 =
          (plus.Value().drill_null - minus.Value().drill_null) / (2.0 * step);
      const V3 fd2 = (Multiply(plus.Value().null_gradient, direction) -
                      Multiply(minus.Value().null_gradient, direction)) /
                     (2.0 * step);
      const double error1 = (fd1 - first).Norm() / (fd1.Norm() + first.Norm());
      const double error2 =
          (fd2 - value.Value().null_directional_second).Norm() /
          (fd2.Norm() + value.Value().null_directional_second.Norm());
      std::cout << "C06 null j=" << exponent << " D1=" << error1
                << " D2=" << error2 << '\n';
      const bool pass = error1 <= kNumericalTolerance && error2 <= kNumericalTolerance &&
                        exponent >= 10 && exponent <= 18;
      consecutive = consecutive || (previous && pass);
      previous = pass;
    }
    EXPECT_TRUE(consecutive);
  }
  EXPECT_FALSE(fesa::EvaluateDynamicRotation({}, {0, 0, 2}, {}, {}).HasValue());
  EXPECT_FALSE(
      fesa::EvaluateDynamicRotation(
          {std::numeric_limits<double>::quiet_NaN(), 0, 0}, initial, {}, {})
          .HasValue());
}

TEST(NonlinearMitc4Inertia, UnitAndGlobalFrameChangesPreservePhysicalMetrics) {
  const Fixture fixture = Warped();
  const State state = FiniteState();
  const auto inertia = fixture.Create();
  ASSERT_TRUE(inertia.HasValue());
  const auto value = inertia.Value().Evaluate(state.q, state.v, state.a);
  ASSERT_TRUE(value.HasValue());
  constexpr double kLength = 1.0e-3;
  constexpr double kMass = 1.0e-6;
  constexpr double kTime = 1.0e2;
  Fixture units = fixture;
  State converted = state;
  units.thickness *= kLength;
  units.density *= kMass / (kLength * kLength * kLength);
  units.modulus *= kMass / (kLength * kTime * kTime);
  for (std::size_t node = 0; node < 4; ++node) {
    for (double& coordinate : units.nodes[node].coordinates) {
      coordinate *= kLength;
    }
    converted.q.displacement[node] = kLength * state.q.displacement[node];
    for (std::size_t j = 0; j < 6; ++j) {
      const double length_factor = j < 3U ? kLength : 1.0;
      converted.v[6U * node + j] *= length_factor / kTime;
      converted.a[6U * node + j] *= length_factor / (kTime * kTime);
    }
  }
  const auto converted_inertia = units.Create();
  ASSERT_TRUE(converted_inertia.HasValue());
  const auto uv =
      converted_inertia.Value().Evaluate(converted.q, converted.v, converted.a);
  ASSERT_TRUE(uv.HasValue());
  const double energy_scale = kMass * kLength * kLength / (kTime * kTime);
  EXPECT_LE(std::abs(uv.Value().kinetic_energy / energy_scale -
                     value.Value().kinetic_energy) /
                value.Value().kinetic_energy,
            kNumericalTolerance);
  const auto original_scaled =
      ScaledMatrix(value.Value().mass, fixture.Length());
  const auto converted_scaled = ScaledMatrix(uv.Value().mass, units.Length());
  double error = 0.0;
  for (std::size_t i = 0; i < 24; ++i) {
    for (std::size_t j = 0; j < 24; ++j) {
      error = std::hypot(error,
                         converted_scaled(i, j) / (kMass * kLength * kLength) -
                             original_scaled(i, j));
    }
  }
  EXPECT_LE(error / MatrixNorm(original_scaled), kNumericalTolerance);
  const auto original_force = ApplyD(value.Value().force, fixture.Length());
  auto converted_force = ApplyD(uv.Value().force, units.Length());
  converted_force.Scale(1.0 / energy_scale);
  EXPECT_LE(Difference(original_force, converted_force, 1.0).Norm() /
                original_force.Norm(),
            kNumericalTolerance);

  const auto qx = fesa::ExpSkew(V3{0.4, 0, 0});
  const auto qz = fesa::ExpSkew(V3{0, 0, 0.7});
  const auto rotate = [&](const V3& v) {
    return Multiply(qz, Multiply(qx, v));
  };
  Fixture frame = fixture;
  State rotated = state;
  for (std::size_t node = 0; node < 4; ++node) {
    frame.nodes[node].coordinates =
        rotate(V3{fixture.nodes[node].coordinates}).Components();
    frame.frames[node].director =
        rotate(V3{fixture.frames[node].director}).Components();
    frame.frames[node].tangent_a =
        rotate(V3{fixture.frames[node].tangent_a}).Components();
    frame.frames[node].tangent_b =
        rotate(V3{fixture.frames[node].tangent_b}).Components();
    rotated.q.displacement[node] = rotate(state.q.displacement[node]);
    rotated.q.rotation_vector[node] = rotate(state.q.rotation_vector[node]);
    for (std::size_t offset = 0; offset < 6; offset += 3) {
      const std::size_t base = 6U * node + offset;
      const V3 v =
          rotate(V3{state.v[base], state.v[base + 1U], state.v[base + 2U]});
      const V3 a =
          rotate(V3{state.a[base], state.a[base + 1U], state.a[base + 2U]});
      for (std::size_t j = 0; j < 3; ++j) {
        rotated.v[base + j] = v[j];
        rotated.a[base + j] = a[j];
      }
    }
  }
  const auto frame_inertia = frame.Create();
  ASSERT_TRUE(frame_inertia.HasValue());
  const auto fv =
      frame_inertia.Value().Evaluate(rotated.q, rotated.v, rotated.a);
  ASSERT_TRUE(fv.HasValue());
  EXPECT_LE(std::abs(fv.Value().kinetic_energy - value.Value().kinetic_energy) /
                value.Value().kinetic_energy,
            kNumericalTolerance);
  EXPECT_LE((fv.Value().momentum - rotate(value.Value().momentum)).Norm() /
                value.Value().momentum.Norm(),
            kNumericalTolerance);
  EXPECT_LE(
      (fv.Value().angular_momentum - rotate(value.Value().angular_momentum))
              .Norm() /
          value.Value().angular_momentum.Norm(),
      kNumericalTolerance);
  for (std::size_t base = 0; base < 24; base += 3) {
    const V3 expected =
        rotate(V3{value.Value().force[base], value.Value().force[base + 1U],
                  value.Value().force[base + 2U]});
    const V3 actual{fv.Value().force[base], fv.Value().force[base + 1U],
                    fv.Value().force[base + 2U]};
    EXPECT_LE((actual - expected).Norm() / expected.Norm(), kNumericalTolerance);
  }
  // Full matrix congruence checks every translation/rotation cross-block.
  double covariance_error = 0.0;
  for (std::size_t i = 0; i < 24; i += 3) {
    for (std::size_t j = 0; j < 24; j += 3) {
      for (std::size_t column = 0; column < 3; ++column) {
        std::array<double, 3> axis{};
        axis[column] = 1.0;
        const V3 global_axis = rotate(V3{axis});
        V3 result{};
        std::array<double, 3> components{};
        for (std::size_t row = 0; row < 3; ++row) {
          for (std::size_t k = 0; k < 3; ++k) {
            components[row] += fv.Value().mass(i + row, j + k) * global_axis[k];
          }
        }
        result = V3{components};
        const V3 expected = rotate(V3{value.Value().mass(i, j + column),
                                      value.Value().mass(i + 1U, j + column),
                                      value.Value().mass(i + 2U, j + column)});
        covariance_error =
            std::hypot(covariance_error, (result - expected).Norm());
      }
    }
  }
  EXPECT_LE(covariance_error / MatrixNorm(value.Value().mass), kNumericalTolerance);
}

TEST(DynamicRotation, MatchesEightyDigitIndependentBlockExponentialAnchors) {
  // Constants were evaluated with Python decimal precision 80 by applying
  // exp(A) to (e3,0,...,0), with A the 24x24 subset-block matrix described
  // above, summing 99 matrix-power terms. This oracle uses no Rodrigues/Jl
  // coefficients and retains high precision even on MSVC (long double=double).
  const std::array<std::array<V3, 3>, 5> expected{
      std::array<V3, 3>{V3{0, 1, 0}, V3{0, 0, 0},
                        V3{-0.81649658092772603, 0.40824829046386302, 0}},
      std::array<V3, 3>{
          V3{-7.5000039999910002e-07, 0.9999999999982, 9.9999999999883338e-07},
          V3{1.6666686666628888e-07, -8.3332933333111116e-08,
             2.6666666666643096e-19},
          V3{-0.8164969891729138, 0.40824706571842007,
             -1.6329931618530299e-06}},
      std::array<V3, 3>{
          V3{-0.10876310154778675, 0.95485599102728924, 0.19478607292927094},
          V3{0.029022554703521119, -0.0080127091261333588,
             0.0010465349668405597},
          V3{-0.80418976262207553, 0.24008088229171029, -0.21102407953866634}},
      std::array<V3, 3>{V3{0, 0.99999999500976089, 9.9902343583821137e-05},
                        V3{0, -8.325195306960704e-06, 1.111111111020202e-108},
                        V3{-0.81648298485418402, 0.40825508659919563,
                           3.7386214252198052e-05}},
      std::array<V3, 3>{V3{0, 0.99999999499022962, 0.00010009765608284457},
                        V3{0, -8.3414713485948191e-06, 8.5714285692063495e-99},
                        V3{-0.81648295827126682, 0.40825509988321224,
                           3.7459305579990838e-05}}};
  const std::array<V3, 5> states{
      V3{}, V3{1.0e-6, -2.0e-6, 3.0e-6}, V3{0.2, -0.3, 0.4},
      V3{1.0e-4 * (1.0 - std::ldexp(1.0, -10)), 0.0, 0.0},
      V3{1.0e-4 * (1.0 + std::ldexp(1.0, -10)), 0.0, 0.0}};
  const V3 e1{1, 0, 0};
  const V3 e2{0, 1, 0};
  const V3 e3{0, 0, 1};
  const V3 mixed = V3{1, 2, -1} / std::sqrt(6.0);
  for (std::size_t i = 0; i < states.size(); ++i) {
    const auto value = fesa::EvaluateDynamicRotation(states[i], e3, {}, {});
    ASSERT_TRUE(value.HasValue());
    EXPECT_LE((Third(value.Value(), e1, e1, e1) - expected[i][0]).Norm(),
              kNumericalTolerance);
    EXPECT_LE((Third(value.Value(), e1, e2, e3) - expected[i][1]).Norm(),
              kNumericalTolerance);
    EXPECT_LE(
        (Third(value.Value(), mixed, mixed, mixed) - expected[i][2]).Norm(),
        kNumericalTolerance);
  }
}

TEST(NonlinearMitc4Inertia, IndependentOracleDetectsEachRequiredOmission) {
  const Fixture fixture = Warped();
  const auto inertia = fixture.Create();
  ASSERT_TRUE(inertia.HasValue());
  const State state = FiniteState();
  const auto value = inertia.Value().Evaluate(state.q, state.v, state.a);
  ASSERT_TRUE(value.HasValue());
  const Integral oracle = IntegrateFivePoints(fixture, state);
  fesa::Vector p{24U};
  for (std::size_t i = 0; i < 24; ++i) {
    if (i % 6U >= 3U) {
      p[i] = static_cast<double>(i + 1U) * (i % 2U == 0U ? 1.0 : -1.0);
    }
  }
  p.Scale(1.0 / p.Norm());
  std::array<V3, 4> first_acceleration{};
  std::array<V3, 4> third{};
  std::array<std::array<V3, 3>, 4> first{};
  std::array<std::array<V3, 3>, 4> second{};
  for (std::size_t node = 0; node < 4; ++node) {
    const std::size_t b = 6U * node + 3U;
    const V3 phi = state.q.rotation_vector[node];
    const V3 d0{fixture.frames[node].director};
    const V3 v{state.v[b], state.v[b + 1U], state.v[b + 2U]};
    const V3 a{state.a[b], state.a[b + 1U], state.a[b + 2U]};
    const V3 direction{p[b], p[b + 1U], p[b + 2U]};
    first_acceleration[node] = BlockExponentialDerivative(phi, d0, {a}) +
                               BlockExponentialDerivative(phi, d0, {v, v});
    third[node] = BlockExponentialDerivative(phi, d0, {direction, v, v});
    for (std::size_t j = 0; j < 3; ++j) {
      std::array<double, 3> axis{};
      axis[j] = 1.0;
      first[node][j] = BlockExponentialDerivative(phi, d0, {V3{axis}});
      second[node][j] =
          BlockExponentialDerivative(phi, d0, {direction, V3{axis}});
    }
  }
  fesa::Vector transposed_h_derivative{24U};
  fesa::Vector third_derivative{24U};
  for (std::size_t i = 0; i < 4; ++i) {
    for (std::size_t j = 0; j < 4; ++j) {
      const V3 au{state.a[6U * j], state.a[6U * j + 1U], state.a[6U * j + 2U]};
      for (std::size_t c = 0; c < 3; ++c) {
        transposed_h_derivative[6U * i + 3U + c] +=
            oracle.moments[1][i][j] * second[i][c].Dot(au) +
            oracle.moments[2][i][j] * second[i][c].Dot(first_acceleration[j]);
        third_derivative[6U * i + c] += oracle.moments[1][i][j] * third[j][c];
        third_derivative[6U * i + 3U + c] +=
            oracle.moments[2][i][j] * first[i][c].Dot(third[j]);
      }
    }
  }
  const double step = std::ldexp(1.0, -12);
  const auto plus =
      inertia.Value().Evaluate(AddRotation(state.q, p, step), state.v, state.a);
  const auto minus = inertia.Value().Evaluate(AddRotation(state.q, p, -step),
                                              state.v, state.a);
  ASSERT_TRUE(plus.HasValue());
  ASSERT_TRUE(minus.HasValue());
  const auto fd =
      ApplyD(Difference(plus.Value().force, minus.Value().force, 2.0 * step),
             fixture.Length());
  const auto correct =
      ApplyD(value.Value().position_tangent.Multiply(p), fixture.Length());
  for (const auto& omitted : {transposed_h_derivative, third_derivative}) {
    auto mutant = correct;
    mutant.Axpy(-1.0, ApplyD(omitted, fixture.Length()));
    EXPECT_GT(Difference(mutant, fd, 1.0).Norm() / (mutant.Norm() + fd.Norm()),
              1.0e-6);
  }
  auto no_convective = value.Value().force;
  no_convective.Axpy(-1.0, value.Value().convective_force);
  EXPECT_GT(Difference(ApplyD(no_convective, fixture.Length()),
                       ApplyD(oracle.force, fixture.Length()), 1.0)
                    .Norm() /
                ApplyD(oracle.force, fixture.Length()).Norm(),
            1.0e-6);
  fesa::Vector vp = state.v;
  fesa::Vector vm = state.v;
  vp.Axpy(step, p);
  vm.Axpy(-step, p);
  const auto vplus = inertia.Value().Evaluate(state.q, vp, state.a);
  const auto vminus = inertia.Value().Evaluate(state.q, vm, state.a);
  ASSERT_TRUE(vplus.HasValue());
  ASSERT_TRUE(vminus.HasValue());
  const auto vfd =
      ApplyD(Difference(vplus.Value().force, vminus.Value().force, 2.0 * step),
             fixture.Length());
  EXPECT_GT(vfd.Norm(), 0.0);
  const fesa::Vector missing_dv{24U};
  EXPECT_GT(Difference(missing_dv, vfd, 1.0).Norm() / vfd.Norm(), 1.0e-6);
}

}  // namespace