#include <gtest/gtest.h>

#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <limits>
#include <stdexcept>
#include <string>

#include "fesa/elements/finite_rotation.h"
#include "fesa/elements/nonlinear_mitc4_inertia.h"
#include "fesa/elements/nonlinear_mitc4_shell.h"
#include "fesa/model/model_types.h"

namespace {

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

using Coordinates = std::array<double, 3>;

fesa::Node Node(const std::int64_t label, const Coordinates coordinates) {
  return {{"Shell-Instance", label, std::to_string(label)},
          coordinates,
          {"nonlinear-mitc4-tangent.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> NodePointers(
    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::NonlinearMitc4Shell Shell(const std::array<fesa::Node, 4>& nodes) {
  const fesa::ShellSection section{
      "Section", 0.2, 0U, {"nonlinear-mitc4-tangent.inp", 20U}};
  const fesa::LinearElasticMaterial material{
      "Material", 120.0, 0.25, {"nonlinear-mitc4-tangent.inp", 21U}};
  const auto shell = fesa::NonlinearMitc4Shell::Create(
      NodePointers(nodes), Frames(), section, material);
  if (!shell.HasValue()) {
    throw std::runtime_error{shell.GetStatus().Diagnostics()[0U].code};
  }
  return shell.Value();
}

fesa::NonlinearMitc4TrialDofs Trial() {
  fesa::NonlinearMitc4TrialDofs trial{};
  trial.displacement[0U] = {-0.02, 0.01, 0.03};
  trial.displacement[1U] = {0.01, -0.03, -0.01};
  trial.displacement[2U] = {0.03, 0.02, 0.04};
  trial.displacement[3U] = {-0.01, 0.04, -0.02};
  trial.rotation_vector[0U] = {0.03, -0.02, 0.01};
  trial.rotation_vector[1U] = {-0.01, 0.04, -0.02};
  trial.rotation_vector[2U] = {0.02, 0.01, 0.03};
  trial.rotation_vector[3U] = {-0.03, 0.02, -0.01};
  return trial;
}

fesa::NonlinearMitc4TrialDofs AddScaled(
    const fesa::NonlinearMitc4TrialDofs& trial,
    const fesa::NonlinearMitc4TrialDofs& direction, const double scale) {
  fesa::NonlinearMitc4TrialDofs result = trial;
  for (std::size_t node = 0U; node < 4U; ++node) {
    result.displacement[node] =
        result.displacement[node] + scale * direction.displacement[node];
    result.rotation_vector[node] =
        result.rotation_vector[node] + scale * direction.rotation_vector[node];
  }
  return result;
}

fesa::NonlinearMitc4TrialDofs Direction() {
  fesa::NonlinearMitc4TrialDofs direction{};
  direction.displacement[0U] = {0.5, -0.2, 0.1};
  direction.displacement[1U] = {-0.1, 0.3, -0.4};
  direction.displacement[2U] = {0.2, 0.1, 0.5};
  direction.displacement[3U] = {-0.3, -0.4, -0.2};
  direction.rotation_vector[0U] = {0.2, -0.1, 0.3};
  direction.rotation_vector[1U] = {-0.3, 0.2, -0.1};
  direction.rotation_vector[2U] = {0.1, 0.3, -0.2};
  direction.rotation_vector[3U] = {-0.2, -0.3, 0.1};
  return direction;
}

fesa::Vector ToVector(const fesa::NonlinearMitc4TrialDofs& trial) {
  fesa::Vector result{24U};
  for (std::size_t node = 0U; node < 4U; ++node) {
    for (std::size_t component = 0U; component < 3U; ++component) {
      result[6U * node + component] = trial.displacement[node][component];
      result[6U * node + 3U + component] =
          trial.rotation_vector[node][component];
    }
  }
  return result;
}

double ReferenceLength(const std::array<fesa::Node, 4>& nodes) {
  double length = 0.0;
  for (std::size_t first = 0U; first < nodes.size(); ++first) {
    for (std::size_t second = first + 1U; second < nodes.size(); ++second) {
      length = (std::max)(length, (fesa::Vector3{nodes[first].coordinates} -
                                   fesa::Vector3{nodes[second].coordinates})
                                      .Norm());
    }
  }
  return length;
}

fesa::NonlinearMitc4TrialDofs WorkConjugateDirection(
    const std::array<fesa::Node, 4>& nodes) {
  auto direction = Direction();
  double maximum_component = 0.0;
  for (std::size_t node = 0U; node < 4U; ++node) {
    for (std::size_t component = 0U; component < 3U; ++component) {
      maximum_component =
          (std::max)(maximum_component,
                     std::abs(direction.displacement[node][component]));
      maximum_component =
          (std::max)(maximum_component,
                     std::abs(direction.rotation_vector[node][component]));
    }
  }
  const double length = ReferenceLength(nodes);
  for (std::size_t node = 0U; node < 4U; ++node) {
    direction.displacement[node] =
        length * direction.displacement[node] / maximum_component;
    direction.rotation_vector[node] =
        direction.rotation_vector[node] / maximum_component;
  }
  return direction;
}

fesa::Vector WorkConjugate(const fesa::Vector& vector, const double length) {
  fesa::Vector result = vector;
  for (std::size_t node = 0U; node < 4U; ++node) {
    for (std::size_t component = 0U; component < 3U; ++component) {
      result[6U * node + component] *= length;
    }
  }
  return result;
}

double ChannelNorm(const fesa::Vector& vector, const bool translation) {
  double sum = 0.0;
  const std::size_t offset = translation ? 0U : 3U;
  for (std::size_t node = 0U; node < 4U; ++node) {
    for (std::size_t component = 0U; component < 3U; ++component) {
      const double value = vector[6U * node + offset + component];
      sum += value * value;
    }
  }
  return std::sqrt(sum);
}

double FrobeniusNorm(const fesa::Matrix& matrix) {
  double sum = 0.0;
  for (std::size_t row = 0U; row < matrix.Rows(); ++row) {
    for (std::size_t column = 0U; column < matrix.Columns(); ++column) {
      sum += matrix(row, column) * matrix(row, column);
    }
  }
  return std::sqrt(sum);
}

fesa::Matrix WorkConjugateTangent(const fesa::Matrix& tangent,
                                  const double length) {
  fesa::Matrix result = tangent;
  for (std::size_t row = 0U; row < tangent.Rows(); ++row) {
    const double row_scale = row % 6U < 3U ? length : 1.0;
    for (std::size_t column = 0U; column < tangent.Columns(); ++column) {
      const double column_scale = column % 6U < 3U ? length : 1.0;
      result(row, column) *= row_scale * column_scale;
    }
  }
  return result;
}

void ExpectRelative(const double actual, const double expected) {
  if (expected == 0.0) {
    EXPECT_LE(std::abs(actual), kNumericalTolerance);
    return;
  }
  EXPECT_LE(std::abs(actual - expected) / std::abs(expected), kNumericalTolerance);
}

// NSNR-ELEM-TAN-001
TEST(NonlinearMitc4Residual, EvaluatesResidualTangentAndRecoveryAtOneTrial) {
  const auto nodes = Nodes();
  const auto shell = Shell(nodes);
  const auto trial = Trial();
  const auto recovery = shell.RecoverState(trial);
  const auto result = fesa::EvaluateInternalResidualAndTangent(shell, trial);

  ASSERT_TRUE(recovery.HasValue());
  ASSERT_TRUE(result.HasValue());
  EXPECT_EQ(result.Value().internal_residual.Size(), 24U);
  EXPECT_EQ(result.Value().internal_tangent.Rows(), 24U);
  EXPECT_EQ(result.Value().internal_tangent.Columns(), 24U);
  EXPECT_DOUBLE_EQ(result.Value().physical_strain_energy,
                   result.Value().recovery.physical_strain_energy);
  EXPECT_DOUBLE_EQ(result.Value().recovery.physical_strain_energy,
                   recovery.Value().physical_strain_energy);
  for (std::size_t index = 0U; index < 24U; ++index) {
    EXPECT_DOUBLE_EQ(result.Value().internal_residual[index],
                     result.Value().internal_force[index]);
  }
  for (std::size_t point = 0U;
       point < result.Value().recovery.gauss_points.size(); ++point) {
    EXPECT_EQ(result.Value().recovery.gauss_points[point].generalized_strain,
              recovery.Value().gauss_points[point].generalized_strain);
    EXPECT_EQ(result.Value().recovery.gauss_points[point].section_resultant,
              recovery.Value().gauss_points[point].section_resultant);
  }
}

// NSNR-ELEM-TAN-002 and NSNR-ELEM-TAN-003
TEST(NonlinearMitc4Tangent, MatchesTheApprovedDirectionalSchedule) {
  const auto nodes = Nodes();
  const auto shell = Shell(nodes);
  const auto trial = Trial();
  const auto direction = WorkConjugateDirection(nodes);
  const double length = ReferenceLength(nodes);
  const auto equilibrium =
      fesa::EvaluateInternalResidualAndTangent(shell, trial);
  ASSERT_TRUE(equilibrium.HasValue());

  const fesa::Vector direction_vector = ToVector(direction);
  const fesa::Vector analytical = WorkConjugate(
      equilibrium.Value().internal_tangent.Multiply(direction_vector), length);
  const std::array<double, 8> steps{1.0e-3, 3.0e-4, 1.0e-4, 3.0e-5,
                                    1.0e-5, 3.0e-6, 1.0e-6, 3.0e-7};
  std::array<double, 8> force_errors{};
  std::array<double, 8> moment_errors{};
  ASSERT_GT(ChannelNorm(analytical, true), 0.0);
  ASSERT_GT(ChannelNorm(analytical, false), 0.0);
  for (std::size_t index = 0U; index < steps.size(); ++index) {
    const auto plus = fesa::EvaluateInternalResidualAndTangent(
        shell, AddScaled(trial, direction, steps[index]));
    const auto minus = fesa::EvaluateInternalResidualAndTangent(
        shell, AddScaled(trial, direction, -steps[index]));
    ASSERT_TRUE(plus.HasValue());
    ASSERT_TRUE(minus.HasValue());
    fesa::Vector derivative = plus.Value().internal_residual;
    derivative.Axpy(-1.0, minus.Value().internal_residual);
    derivative.Scale(0.5 / steps[index]);
    derivative = WorkConjugate(derivative, length);
    derivative.Axpy(-1.0, analytical);
    force_errors[index] =
        ChannelNorm(derivative, true) / ChannelNorm(analytical, true);
    moment_errors[index] =
        ChannelNorm(derivative, false) / ChannelNorm(analytical, false);
  }

  std::size_t force_order_count = 0U;
  std::size_t moment_order_count = 0U;
  for (std::size_t index = 1U; index < force_errors.size(); ++index) {
    if (force_errors[index - 1U] > 0.0 && force_errors[index] > 0.0) {
      const double order =
          std::log(force_errors[index - 1U] / force_errors[index]) /
          std::log(steps[index - 1U] / steps[index]);
      if (order >= 1.5) {
        ++force_order_count;
      }
    }
    if (moment_errors[index - 1U] > 0.0 && moment_errors[index] > 0.0) {
      const double order =
          std::log(moment_errors[index - 1U] / moment_errors[index]) /
          std::log(steps[index - 1U] / steps[index]);
      if (order >= 1.5) {
        ++moment_order_count;
      }
    }
  }
  EXPECT_GE(force_order_count, 2U);
  EXPECT_GE(moment_order_count, 2U);
  EXPECT_LE(force_errors[3U], kNumericalTolerance);
  EXPECT_LE(moment_errors[3U], kNumericalTolerance);
}

// NSNR-ELEM-TAN-004 and NSNR-ELEM-TAN-005
TEST(NonlinearMitc4Tangent, PreservesRigidTranslationNullspaceAndSymmetry) {
  const auto nodes = Nodes();
  const auto equilibrium =
      fesa::EvaluateInternalResidualAndTangent(Shell(nodes), {});
  ASSERT_TRUE(equilibrium.HasValue());
  const double length = ReferenceLength(nodes);

  fesa::NonlinearMitc4TrialDofs rigid_translation{};
  for (auto& displacement : rigid_translation.displacement) {
    displacement = {0.3, -0.2, 0.4};
  }
  const fesa::Vector null_action =
      WorkConjugate(equilibrium.Value().internal_tangent.Multiply(
                        ToVector(rigid_translation)),
                    length);
  EXPECT_LE(null_action.Norm(), kNumericalTolerance);

  const fesa::Matrix scaled_tangent =
      WorkConjugateTangent(equilibrium.Value().internal_tangent, length);
  fesa::Matrix antisymmetric = scaled_tangent;
  for (std::size_t row = 0U; row < antisymmetric.Rows(); ++row) {
    for (std::size_t column = 0U; column < antisymmetric.Columns(); ++column) {
      antisymmetric(row, column) -= scaled_tangent(column, row);
    }
  }
  ASSERT_GT(FrobeniusNorm(scaled_tangent), 0.0);
  EXPECT_LE(FrobeniusNorm(antisymmetric) / FrobeniusNorm(scaled_tangent),
            kNumericalTolerance);
}

// NSNR-ELEM-TAN-006
TEST(NonlinearMitc4Residual, DoesNotAcceptExternalLoadShares) {
  const auto nodes = Nodes();
  const auto first =
      fesa::EvaluateInternalResidualAndTangent(Shell(nodes), Trial());
  const auto second =
      fesa::EvaluateInternalResidualAndTangent(Shell(nodes), Trial());
  ASSERT_TRUE(first.HasValue());
  ASSERT_TRUE(second.HasValue());
  for (std::size_t index = 0U; index < 24U; ++index) {
    EXPECT_DOUBLE_EQ(first.Value().internal_residual[index],
                     second.Value().internal_residual[index]);
  }
}

// NSNR-ELEM-TAN-007
TEST(NonlinearMitc4Objectivity, FiniteRigidMotionHasZeroInternalForce) {
  const auto nodes = Nodes();
  const fesa::Vector3 rotation{0.3, -0.2, 0.1};
  const fesa::Matrix tensor = fesa::ExpSkew(rotation);
  fesa::NonlinearMitc4TrialDofs trial{};
  for (std::size_t node = 0U; node < nodes.size(); ++node) {
    const fesa::Vector3 initial{nodes[node].coordinates};
    const fesa::Vector3 rotated{
        tensor(0U, 0U) * initial.X() + tensor(0U, 1U) * initial.Y() +
            tensor(0U, 2U) * initial.Z(),
        tensor(1U, 0U) * initial.X() + tensor(1U, 1U) * initial.Y() +
            tensor(1U, 2U) * initial.Z(),
        tensor(2U, 0U) * initial.X() + tensor(2U, 1U) * initial.Y() +
            tensor(2U, 2U) * initial.Z()};
    trial.displacement[node] =
        rotated + fesa::Vector3{0.7, -0.4, 1.2} - initial;
    trial.rotation_vector[node] = rotation;
  }
  const auto result =
      fesa::EvaluateInternalResidualAndTangent(Shell(nodes), trial);
  ASSERT_TRUE(result.HasValue());
  EXPECT_LE(result.Value().internal_residual.Norm(), kNumericalTolerance);
  EXPECT_LE(result.Value().physical_strain_energy, kNumericalTolerance);
}

// NSNR-ELEM-TAN-008
TEST(NonlinearMitc4Patch, RetainsPhysicalRecoveryIdentities) {
  const auto nodes = Nodes();
  const auto shell = Shell(nodes);
  constexpr double kMembraneStrain = 1.0e-2;
  const double green_strain =
      kMembraneStrain + 0.5 * kMembraneStrain * kMembraneStrain;
  constexpr double kYoungsModulus = 120.0;
  constexpr double kPoissonRatio = 0.25;
  constexpr double kThickness = 0.2;
  const double plane_stress =
      kYoungsModulus / (1.0 - kPoissonRatio * kPoissonRatio);
  const double s11 = plane_stress * green_strain;
  const double s22 = plane_stress * kPoissonRatio * green_strain;
  fesa::NonlinearMitc4TrialDofs trial{};
  for (std::size_t node = 0U; node < nodes.size(); ++node) {
    trial.displacement[node] = {kMembraneStrain * nodes[node].coordinates[0U],
                                0.0, 0.0};
  }
  const auto result = fesa::EvaluateInternalResidualAndTangent(shell, trial);
  ASSERT_TRUE(result.HasValue());
  for (const auto& point : result.Value().recovery.gauss_points) {
    ExpectRelative(point.generalized_strain[0U], green_strain);
    for (std::size_t component = 1U; component < 8U; ++component) {
      ExpectRelative(point.generalized_strain[component], 0.0);
    }
    ExpectRelative(point.section_resultant[0U], kThickness * s11);
    ExpectRelative(point.section_resultant[1U], kThickness * s22);
    for (std::size_t component = 2U; component < 8U; ++component) {
      ExpectRelative(point.section_resultant[component], 0.0);
    }
    for (const auto& stress : point.stress) {
      ExpectRelative(stress.components[0U], s11);
      ExpectRelative(stress.components[1U], s22);
      ExpectRelative(stress.components[2U], 0.0);
    }
  }
}

// NSNR-ELEM-TAN-009
TEST(NonlinearMitc4Residual, RejectsNonfiniteTrialData) {
  const auto nodes = Nodes();
  auto trial = Trial();
  trial.rotation_vector[2U] = {std::numeric_limits<double>::quiet_NaN(), 0.0,
                               0.0};
  const auto result =
      fesa::EvaluateInternalResidualAndTangent(Shell(nodes), trial);
  ASSERT_FALSE(result.HasValue());
  EXPECT_EQ(result.GetStatus().Diagnostics()[0U].code,
            "invalid-nonlinear-shell-trial");

  auto overflow = Trial();
  overflow.displacement[1U] = {1.0e300, 0.0, 0.0};
  const auto overflow_result =
      fesa::EvaluateInternalResidualAndTangent(Shell(nodes), overflow);
  EXPECT_FALSE(overflow_result.HasValue());
}

void CheckThirdSweep(const fesa::NonlinearMitc4TrialDofs& trial) {
  const auto nodes = Nodes();
  const auto shell = Shell(nodes);
  auto direction = Direction();
  double norm = 0.0;
  const auto unscaled = ToVector(direction);
  for (std::size_t i = 0U; i < 24U; ++i) {
    norm = std::hypot(norm, unscaled[i]);
  }
  for (std::size_t node = 0U; node < 4U; ++node) {
    direction.displacement[node] = 2.0 * direction.displacement[node] / norm;
    direction.rotation_vector[node] = direction.rotation_vector[node] / norm;
  }
  const auto velocity = ToVector(direction);
  const auto exact =
      fesa::EvaluateInternalResidualAndTangent(shell, trial, &velocity);
  ASSERT_TRUE(exact.HasValue());
  unsigned int consecutive = 0U;
  bool passed = false;
  for (int power = 8; power <= 20; power += 2) {
    const double h = std::ldexp(1.0, -power);
    const auto plus = fesa::EvaluateInternalResidualAndTangent(
        shell, AddScaled(trial, direction, h));
    const auto minus = fesa::EvaluateInternalResidualAndTangent(
        shell, AddScaled(trial, direction, -h));
    ASSERT_TRUE(plus.HasValue());
    ASSERT_TRUE(minus.HasValue());
    double error = 0.0;
    double analytic_norm = 0.0;
    double fd_norm = 0.0;
    for (std::size_t i = 0U; i < 24U; ++i) {
      double fd = 0.0;
      for (std::size_t j = 0U; j < 24U; ++j) {
        fd += (plus.Value().internal_tangent(i, j) -
               minus.Value().internal_tangent(i, j)) *
              velocity[j] / (2.0 * h);
      }
      const double d = i % 6U < 3U ? 2.0 : 1.0;
      const double analytic = exact.Value().force_directional_second[i];
      error = std::hypot(error, d * (analytic - fd));
      analytic_norm = std::hypot(analytic_norm, d * analytic);
      fd_norm = std::hypot(fd_norm, d * fd);
    }
    if (power >= 10 && power <= 18 &&
        error / (analytic_norm + fd_norm) <= kNumericalTolerance) {
      ++consecutive;
      passed = passed || consecutive >= 2U;
    } else {
      consecutive = 0U;
    }
  }
  EXPECT_TRUE(passed);
  EXPECT_GE(exact.Value().drilling_energy, 0.0);
  for (std::size_t i = 0U; i < 24U; ++i) {
    EXPECT_DOUBLE_EQ(
        exact.Value().internal_force[i],
        exact.Value().physical_force[i] + exact.Value().drilling_force[i]);
  }
}

TEST(NonlinearMitc4Residual,
     ZeroTrialOnTranslatedDecimalMeshHasNoArtificialForce) {
  auto nodes = Nodes();
  nodes[0].coordinates = {3.84615385, 2.30769231, 0.0};
  nodes[1].coordinates = {4.23076923, 2.30769231, 0.0};
  nodes[2].coordinates = {4.23076923, 2.69230769, 0.0};
  nodes[3].coordinates = {3.84615385, 2.69230769, 0.0};
  const auto shell = Shell(nodes);
  const auto evaluated = fesa::EvaluateInternalResidualAndTangent(shell, {});
  ASSERT_TRUE(evaluated.HasValue());
  for (std::size_t i = 0; i < 24U; ++i)
    EXPECT_DOUBLE_EQ(evaluated.Value().internal_force[i], 0.0);
  EXPECT_DOUBLE_EQ(evaluated.Value().drilling_energy, 0.0);
}

TEST(NonlinearMitc4Residual, ExactDirectionalThirdGradientMatchesTangentSweep) {
  CheckThirdSweep(Trial());
}

TEST(NonlinearMitc4Residual, SmallRotationPhysicalForceRetainsDrillingNullIdentity) {
  // Exact local state of C08b h=1/32768, increment4: the rotation has just
  // crossed the original 1e-4 coefficient branch and its physical null leaked.
  auto nodes = Nodes();
  nodes[1U].coordinates = {0.0, -1.0, 0.0};
  nodes[2U].coordinates = {0.0, 0.0, 0.0};
  nodes[3U].coordinates = {-1.0, 0.0, 0.0};
  const fesa::ShellSection section{"Centered", 0.2, 0U, {}};
  const fesa::LinearElasticMaterial material{"Elastic", 1000.0, 0.3, {}};
  const auto shell = fesa::NonlinearMitc4Shell::Create(NodePointers(nodes), Frames(),
                                                      section, material);
  ASSERT_TRUE(shell.HasValue());
  const std::array<std::array<double, 6>, 4> coordinates{{
      {6.4086914072366947e-05, -7.9345703110834861e-05, -1.4343267109683367e-05, 9.7657633876829583e-05, 7.324020770488069e-05, 4.8828020264083388e-05},
      {6.4086914054701282e-05, -3.0517578102879528e-05, -9.0026855471989683e-05, 9.7658037630745924e-05, 7.3240207702551634e-05, 4.8828064949797786e-05},
      {1.5258789062740791e-05, -3.0517578125180581e-05, 7.6293945312499653e-06, 9.7658037632949931e-05, 7.3239803947312715e-05, 4.8828124536813954e-05},
      {1.5258789080115012e-05, -7.9345703132764625e-05, 8.0871582026927324e-05, 9.7657633879033603e-05, 7.3239803949641825e-05, 4.8828079851099556e-05}
  }};
  fesa::NonlinearMitc4TrialDofs trial{};
  for (std::size_t node = 0U; node < 4U; ++node) {
    const auto& q = coordinates[node];
    trial.displacement[node] = {q[0U], q[1U], q[2U]};
    trial.rotation_vector[node] = {q[3U], q[4U], q[5U]};
  }
  const fesa::Vector direction(24U);
  const auto response = fesa::EvaluateInternalResidualAndTangent(shell.Value(), trial, &direction);
  ASSERT_TRUE(response.HasValue());
  for (std::size_t node = 0U; node < 4U; ++node) {
    SCOPED_TRACE(node);
    const auto rotation = fesa::EvaluateDynamicRotation(trial.rotation_vector[node],
                                                       {0.0, 0.0, 1.0}, {}, {});
    ASSERT_TRUE(rotation.HasValue());
    double physical = 0.0, physical_scale = 0.0, projected = 0.0, total_scale = 0.0;
    for (std::size_t c = 0U; c < 3U; ++c) {
      const auto full = 6U * node + 3U + c;
      const double n = rotation.Value().drill_null[c];
      physical += n * response.Value().physical_force[full];
      physical_scale += std::abs(n * response.Value().physical_force[full]);
      projected += n * response.Value().internal_force[full];
      total_scale += std::abs(n * response.Value().internal_force[full]);
    }
    ASSERT_GT(physical_scale, 0.0);
    ASSERT_GT(total_scale, 0.0);
    EXPECT_LE(std::abs(physical) / physical_scale, kNumericalTolerance);
    EXPECT_LE(std::abs(projected - response.Value().drilling_constraint[node]) / total_scale,
              kNumericalTolerance);
  }
}

TEST(NonlinearMitc4Residual, ThirdGradientIsStableAcrossSmallAngleBranches) {
  for (const double angle : {1e-4 * (1 - std::ldexp(1., -10)),
                             1e-4 * (1 + std::ldexp(1., -10)), 2e-4, 1e-3,
                             .5 * (1 - std::ldexp(1., -12)),
                             .5 * (1 + std::ldexp(1., -12))}) {
    SCOPED_TRACE(angle);
    auto trial = Trial();
    for (auto& phi : trial.rotation_vector) phi = {angle, 0., 0.};
    CheckThirdSweep(trial);
  }
}

TEST(NonlinearMitc4Residual,
     ExactDrillingConstraintHasIndependentForceGradientAndHessian) {
  for (bool warped : {false, true}) {
    auto nodes = Nodes();
    if (warped) nodes[2U].coordinates[2U] = 0.1;
    const auto shell = Shell(nodes);
    auto trial = Trial();
    trial.rotation_vector[0U] = {.6, -.2, .1};
    const auto direction = Direction();
    const auto velocity = ToVector(direction);
    const auto exact =
        fesa::EvaluateInternalResidualAndTangent(shell, trial, &velocity);
    ASSERT_TRUE(exact.HasValue());
    for (std::size_t node = 0U; node < 4U; ++node) {
      const auto rotation = fesa::EvaluateDynamicRotation(
          trial.rotation_vector[node], {0., 0., 1.},
          direction.rotation_vector[node], {});
      ASSERT_TRUE(rotation.HasValue());
      double psi = 0.;
      for (std::size_t c = 0U; c < 3U; ++c)
        psi += rotation.Value().drill_null[c] *
               exact.Value().internal_force[6U * node + 3U + c];
      EXPECT_NEAR(exact.Value().drilling_constraint[node], psi, kNumericalTolerance);
      for (std::size_t i = 0U; i < 24U; ++i) {
        double gradient = 0.;
        for (std::size_t c = 0U; c < 3U; ++c) {
          gradient += rotation.Value().drill_null[c] *
                      exact.Value().internal_tangent(6U * node + 3U + c, i);
          if (i / 6U == node && i % 6U >= 3U)
            gradient += rotation.Value().null_gradient(c, i % 6U - 3U) *
                        exact.Value().internal_force[6U * node + 3U + c];
        }
        EXPECT_NEAR(exact.Value().drilling_constraint_gradient(node, i),
                    gradient, kNumericalTolerance);
      }
    }
    unsigned consecutive = 0U;
    bool passed = false;
    for (int power = 8; power <= 20; power += 2) {
      const double h = std::ldexp(1., -power);
      const auto plus = fesa::EvaluateInternalResidualAndTangent(
          shell, AddScaled(trial, direction, h), &velocity);
      const auto minus = fesa::EvaluateInternalResidualAndTangent(
          shell, AddScaled(trial, direction, -h), &velocity);
      ASSERT_TRUE(plus.HasValue());
      ASSERT_TRUE(minus.HasValue());
      double error = 0., anorm = 0., fnorm = 0.;
      for (std::size_t node = 0U; node < 4U; ++node)
        for (std::size_t i = 0U; i < 24U; ++i) {
          double analytic = 0.;
          for (std::size_t j = 0U; j < 24U; ++j)
            analytic += exact.Value().drilling_constraint_hessian[node](i, j) *
                        velocity[j];
          const double fd =
              (plus.Value().drilling_constraint_gradient(node, i) -
               minus.Value().drilling_constraint_gradient(node, i)) /
              (2 * h);
          const double scale = i % 6U < 3U ? 2. : 1.;
          error = std::hypot(error, scale * (analytic - fd));
          anorm = std::hypot(anorm, scale * analytic);
          fnorm = std::hypot(fnorm, scale * fd);
        }
      if (power >= 10 && power <= 18 && error / (anorm + fnorm) <= kNumericalTolerance) {
        ++consecutive;
        passed = passed || consecutive >= 2U;
      } else
        consecutive = 0U;
    }
    EXPECT_TRUE(passed);
  }
}

TEST(NonlinearMitc4Residual,
     ExposesExactDrillingConstraintThirdDirectionalGradient) {
  const auto nodes = Nodes();
  const auto shell = Shell(nodes);
  const auto trial = Trial();
  const auto direction = Direction();
  const auto velocity = ToVector(direction);
  const auto exact =
      fesa::EvaluateInternalResidualAndTangent(shell, trial, &velocity);
  ASSERT_TRUE(exact.HasValue());
  constexpr double h = 1.0e-4;
  const auto plus = fesa::EvaluateInternalResidualAndTangent(
      shell, AddScaled(trial, direction, h), &velocity);
  const auto minus = fesa::EvaluateInternalResidualAndTangent(
      shell, AddScaled(trial, direction, -h), &velocity);
  ASSERT_TRUE(plus.HasValue());
  ASSERT_TRUE(minus.HasValue());

  for (std::size_t node = 0U; node < 4U; ++node) {
    ASSERT_EQ(exact.Value().drilling_constraint_third_directional_gradient[node]
                  .Size(),
              24U);
    for (std::size_t row = 0U; row < 24U; ++row) {
      double plus_hessian_direction = 0.0;
      double minus_hessian_direction = 0.0;
      for (std::size_t column = 0U; column < 24U; ++column) {
        plus_hessian_direction +=
            plus.Value().drilling_constraint_hessian[node](row, column) *
            velocity[column];
        minus_hessian_direction +=
            minus.Value().drilling_constraint_hessian[node](row, column) *
            velocity[column];
      }
      const double finite_difference =
          (plus_hessian_direction - minus_hessian_direction) / (2.0 * h);
      EXPECT_NEAR(
          exact.Value().drilling_constraint_third_directional_gradient[node]
              [row],
          finite_difference, kNumericalTolerance);
    }
  }
}

TEST(NonlinearMitc4Residual,
     ExternalDrillingConstraintIncludesForceTimeAndCurvatureTerms) {
  const auto nodes = Nodes();
  const auto shell = Shell(nodes);
  const fesa::Vector3 phi{.6, -.2, .1};
  const fesa::Vector3 phi_dot{.2, -.1, .3};
  const fesa::Vector3 force{1.7, -.8, .4};
  const fesa::Vector3 force_rate{-.3, .6, .2};
  const fesa::Vector3 force_acceleration{.5, -.4, .1};
  const auto exact = shell.EvaluateExternalDrillingConstraint(
      0U, phi, phi_dot, force, force_rate, force_acceleration);
  ASSERT_TRUE(exact.HasValue());
  constexpr double h = 1.0e-4;
  const auto at_time = [&](const double time) {
    return shell.EvaluateExternalDrillingConstraint(
        0U, phi + time * phi_dot, phi_dot,
        force + time * force_rate + 0.5 * time * time * force_acceleration,
        force_rate + time * force_acceleration, force_acceleration);
  };
  const auto plus = at_time(h);
  const auto minus = at_time(-h);
  ASSERT_TRUE(plus.HasValue());
  ASSERT_TRUE(minus.HasValue());
  EXPECT_NEAR(exact.Value().directional_first,
              (plus.Value().value - minus.Value().value) / (2.0 * h),
              kNumericalTolerance);
  EXPECT_NEAR(exact.Value().directional_second,
              (plus.Value().value - 2.0 * exact.Value().value +
               minus.Value().value) /
                  (h * h),
              kNumericalTolerance);
  for (std::size_t row = 0U; row < 3U; ++row) {
    EXPECT_NEAR(exact.Value().gradient_directional_first[row],
                (plus.Value().gradient[row] - minus.Value().gradient[row]) /
                    (2.0 * h),
                kNumericalTolerance);
    EXPECT_NEAR(
        exact.Value().gradient_directional_second[row],
        (plus.Value().gradient[row] - 2.0 * exact.Value().gradient[row] +
         minus.Value().gradient[row]) /
            (h * h),
        kNumericalTolerance);
  }
  for (std::size_t column = 0U; column < 3U; ++column) {
    fesa::Vector3 basis{};
    if (column == 0U) basis = {1.0, 0.0, 0.0};
    if (column == 1U) basis = {0.0, 1.0, 0.0};
    if (column == 2U) basis = {0.0, 0.0, 1.0};
    const auto positive = shell.EvaluateExternalDrillingConstraint(
        0U, phi + h * basis, phi_dot, force, force_rate, force_acceleration);
    const auto negative = shell.EvaluateExternalDrillingConstraint(
        0U, phi - h * basis, phi_dot, force, force_rate, force_acceleration);
    ASSERT_TRUE(positive.HasValue());
    ASSERT_TRUE(negative.HasValue());
    for (std::size_t row = 0U; row < 3U; ++row) {
      EXPECT_NEAR(exact.Value().hessian(row, column),
                  (positive.Value().gradient[row] -
                   negative.Value().gradient[row]) /
                      (2.0 * h),
                  kNumericalTolerance);
    }
  }
}

TEST(NonlinearMitc4Residual,
     ExternalDrillingConstraintRejectsInvalidRotationAndHasZeroForceValue) {
  const auto shell = Shell(Nodes());
  const auto zero = shell.EvaluateExternalDrillingConstraint(
      0U, {.2, -.1, .3}, {.1, .2, -.3}, {}, {}, {});
  ASSERT_TRUE(zero.HasValue());
  EXPECT_DOUBLE_EQ(zero.Value().value, 0.0);
  EXPECT_DOUBLE_EQ(zero.Value().directional_first, 0.0);
  EXPECT_DOUBLE_EQ(zero.Value().directional_second, 0.0);
  for (std::size_t row = 0U; row < 3U; ++row) {
    EXPECT_DOUBLE_EQ(zero.Value().gradient[row], 0.0);
    EXPECT_DOUBLE_EQ(zero.Value().gradient_directional_first[row], 0.0);
    EXPECT_DOUBLE_EQ(zero.Value().gradient_directional_second[row], 0.0);
  }
  const auto invalid = shell.EvaluateExternalDrillingConstraint(
      0U, {std::acos(-1.0), 0.0, 0.0}, {}, {}, {}, {});
  EXPECT_FALSE(invalid.HasValue());
  EXPECT_EQ(invalid.GetStatus().Diagnostics()[0U].code,
            "invalid-nonlinear-shell-rotation");
}

TEST(NonlinearMitc4Residual, TinyRotationPreservesPhysicalForceAndRecoveryEnergy) {
  for (const bool rotated : {false, true}) {
    for (const bool warped : {false, true}) {
      SCOPED_TRACE(rotated);
      SCOPED_TRACE(warped);
      auto nodes = Nodes();
      if (warped) nodes[2U].coordinates[2U] = 0.1;
      auto frames = Frames();
      const auto rotation = fesa::ExpSkew(
          rotated ? fesa::Vector3{0.4, -0.2, 0.7} : fesa::Vector3{});
      const auto transform = [&](const Coordinates& values) {
        Coordinates result{};
        for (std::size_t i = 0U; i < 3U; ++i)
          for (std::size_t j = 0U; j < 3U; ++j)
            result[i] += rotation(i, j) * values[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 shell = fesa::NonlinearMitc4Shell::Create(
          NodePointers(nodes), frames, section, material);
      ASSERT_TRUE(shell.HasValue());
      fesa::NonlinearMitc4TrialDofs tiny{};
      fesa::Vector increment{24U};
      for (std::size_t node = 0U; node < 4U; ++node) {
        tiny.rotation_vector[node] =
            1e-17 * fesa::Vector3{frames[node].tangent_a};
        for (std::size_t c = 0U; c < 3U; ++c)
          increment[6U * node + 3U + c] = tiny.rotation_vector[node][c];
      }
      const auto zero =
          fesa::EvaluateInternalResidualAndTangent(shell.Value(), {});
      const auto actual =
          fesa::EvaluateInternalResidualAndTangent(shell.Value(), tiny);
      ASSERT_TRUE(zero.HasValue());
      ASSERT_TRUE(actual.HasValue());
      EXPECT_DOUBLE_EQ(zero.Value().physical_strain_energy, 0.0);
      double error = 0.0;
      double scale = 0.0;
      double quadratic_energy = 0.0;
      for (std::size_t row = 0U; row < 24U; ++row) {
        EXPECT_DOUBLE_EQ(zero.Value().physical_force[row], 0.0);
        double expected = 0.0;
        for (std::size_t column = 0U; column < 24U; ++column)
          expected += (zero.Value().internal_tangent(row, column) -
                       zero.Value().drilling_tangent(row, column)) *
                      increment[column];
        error = std::max(error,
                         std::abs(actual.Value().physical_force[row] - expected));
        scale = std::max(scale, std::abs(expected));
        quadratic_energy += 0.5 * increment[row] * expected;
      }
      ASSERT_GT(scale, 0.0);
      ASSERT_GT(quadratic_energy, 0.0);
      EXPECT_LE(error / scale, kNumericalTolerance);
      EXPECT_GT(actual.Value().physical_strain_energy, 0.0);
      EXPECT_LE(std::abs(actual.Value().physical_strain_energy - quadratic_energy) /
                    quadratic_energy,
                kNumericalTolerance);
      if (!warped) {
        // Uniform director tilt has .5 * (5/6) * G * t * area * theta^2.
        constexpr double shear_energy = 1.6e-33;
        EXPECT_LE(std::abs(actual.Value().physical_strain_energy - shear_energy) /
                      shear_energy,
                  kNumericalTolerance);
      }
    }
  }
}

}  // namespace