#include "fesa/elements/nonlinear_mitc4_inertia.h"

#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <limits>
#include <string>
#include <utility>

#include "fesa/elements/finite_rotation.h"

namespace fesa {
namespace {

constexpr std::size_t kNodes = 4U;
constexpr std::size_t kDofs = 24U;
const std::array<double, 3> kGauss{-std::sqrt(3.0 / 5.0), 0.0,
                                   std::sqrt(3.0 / 5.0)};
constexpr std::array<double, 3> kWeights{5.0 / 9.0, 8.0 / 9.0, 5.0 / 9.0};

template <class T>
Result<T> Failure(const std::string& code, const SourceLocation& location,
                  const std::string& identity, std::string message) {
  return Result<T>::Failure(Status::Failure(
      FailureCategory::kModel, {{Severity::kError, code, location, "*ELEMENT",
                                 identity, std::move(message)}}));
}

std::string Identity(const std::array<const Node*, kNodes>& nodes) {
  std::string identity;
  for (const Node* node : nodes) {
    if (!identity.empty()) {
      identity += "-";
    }
    identity += node == nullptr ? "?" : node->source_id.source_label_text;
  }
  return identity;
}

Matrix FrameMatrix(const ShellNodeInitialFrame& frame) {
  Matrix result{3U, 3U};
  for (std::size_t row = 0U; row < 3U; ++row) {
    result(row, 0U) = frame.tangent_a[row];
    result(row, 1U) = frame.tangent_b[row];
    result(row, 2U) = frame.director[row];
  }
  return result;
}

bool IsFinite(const Matrix& matrix) {
  for (std::size_t row = 0U; row < matrix.Rows(); ++row) {
    for (std::size_t column = 0U; column < matrix.Columns(); ++column) {
      if (!std::isfinite(matrix(row, column))) {
        return false;
      }
    }
  }
  return true;
}

bool IsFinite(const Vector& vector) {
  for (std::size_t index = 0U; index < vector.Size(); ++index) {
    if (!std::isfinite(vector[index])) {
      return false;
    }
  }
  return true;
}

/// @brief Carries all derivatives through order three in one nodal rotation.
struct Jet {
  double value{};
  std::array<double, 3> d{};
  std::array<std::array<double, 3>, 3> dd{};
  std::array<std::array<std::array<double, 3>, 3>, 3> ddd{};
};

Jet Constant(const double value) { return Jet{value, {}, {}, {}}; }

Jet Variable(const double value, const std::size_t component) {
  Jet result = Constant(value);
  result.d[component] = 1.0;
  return result;
}

Jet operator+(const Jet& left, const Jet& right) {
  Jet result{};
  result.value = left.value + right.value;
  for (std::size_t i = 0U; i < 3U; ++i) {
    result.d[i] = left.d[i] + right.d[i];
    for (std::size_t j = 0U; j < 3U; ++j) {
      result.dd[i][j] = left.dd[i][j] + right.dd[i][j];
      for (std::size_t k = 0U; k < 3U; ++k) {
        result.ddd[i][j][k] = left.ddd[i][j][k] + right.ddd[i][j][k];
      }
    }
  }
  return result;
}

Jet operator-(const Jet& left, const Jet& right) {
  Jet result{};
  result.value = left.value - right.value;
  for (std::size_t i = 0U; i < 3U; ++i) {
    result.d[i] = left.d[i] - right.d[i];
    for (std::size_t j = 0U; j < 3U; ++j) {
      result.dd[i][j] = left.dd[i][j] - right.dd[i][j];
      for (std::size_t k = 0U; k < 3U; ++k) {
        result.ddd[i][j][k] = left.ddd[i][j][k] - right.ddd[i][j][k];
      }
    }
  }
  return result;
}

Jet operator*(const Jet& left, const Jet& right) {
  Jet result{};
  result.value = left.value * right.value;
  for (std::size_t i = 0U; i < 3U; ++i) {
    result.d[i] = left.d[i] * right.value + left.value * right.d[i];
    for (std::size_t j = 0U; j < 3U; ++j) {
      result.dd[i][j] = left.dd[i][j] * right.value + left.d[i] * right.d[j] +
                        left.d[j] * right.d[i] + left.value * right.dd[i][j];
      for (std::size_t k = 0U; k < 3U; ++k) {
        result.ddd[i][j][k] =
            left.ddd[i][j][k] * right.value + left.dd[i][j] * right.d[k] +
            left.dd[i][k] * right.d[j] + left.dd[j][k] * right.d[i] +
            left.d[i] * right.dd[j][k] + left.d[j] * right.dd[i][k] +
            left.d[k] * right.dd[i][j] + left.value * right.ddd[i][j][k];
      }
    }
  }
  return result;
}

Jet Reciprocal(const Jet& value) {
  Jet result{};
  result.value = 1.0 / value.value;
  const double squared = result.value * result.value;
  const double cubed = squared * result.value;
  const double fourth = cubed * result.value;
  for (std::size_t i = 0U; i < 3U; ++i) {
    result.d[i] = -value.d[i] * squared;
    for (std::size_t j = 0U; j < 3U; ++j) {
      result.dd[i][j] =
          2.0 * value.d[i] * value.d[j] * cubed - value.dd[i][j] * squared;
      for (std::size_t k = 0U; k < 3U; ++k) {
        result.ddd[i][j][k] =
            -6.0 * value.d[i] * value.d[j] * value.d[k] * fourth +
            2.0 *
                (value.dd[i][j] * value.d[k] + value.dd[i][k] * value.d[j] +
                 value.dd[j][k] * value.d[i]) *
                cubed -
            value.ddd[i][j][k] * squared;
      }
    }
  }
  return result;
}

Jet InverseSqrt(const Jet& value) {
  Jet result{};
  result.value = 1.0 / std::sqrt(value.value);
  const double cubed = result.value * result.value * result.value;
  const double fifth = cubed * result.value * result.value;
  const double seventh = fifth * result.value * result.value;
  for (std::size_t i = 0U; i < 3U; ++i) {
    result.d[i] = -0.5 * cubed * value.d[i];
    for (std::size_t j = 0U; j < 3U; ++j) {
      result.dd[i][j] =
          0.75 * fifth * value.d[i] * value.d[j] - 0.5 * cubed * value.dd[i][j];
      for (std::size_t k = 0U; k < 3U; ++k) {
        result.ddd[i][j][k] =
            -1.875 * seventh * value.d[i] * value.d[j] * value.d[k] +
            0.75 * fifth *
                (value.dd[i][j] * value.d[k] + value.dd[i][k] * value.d[j] +
                 value.dd[j][k] * value.d[i]) -
            0.5 * cubed * value.ddd[i][j][k];
      }
    }
  }
  return result;
}

Jet Scale(const double scalar, const Jet& value) {
  return Constant(scalar) * value;
}

using JetVector = std::array<Jet, 3>;
using JetMatrix = std::array<std::array<Jet, 3>, 3>;

JetVector Multiply(const JetMatrix& matrix, const JetVector& vector) {
  JetVector result{Constant(0.0), Constant(0.0), Constant(0.0)};
  for (std::size_t row = 0U; row < 3U; ++row) {
    for (std::size_t column = 0U; column < 3U; ++column) {
      result[row] = result[row] + matrix[row][column] * vector[column];
    }
  }
  return result;
}

JetMatrix Multiply(const JetMatrix& left, const JetMatrix& right) {
  JetMatrix result{};
  for (std::size_t row = 0U; row < 3U; ++row) {
    for (std::size_t column = 0U; column < 3U; ++column) {
      result[row][column] = Constant(0.0);
      for (std::size_t index = 0U; index < 3U; ++index) {
        result[row][column] =
            result[row][column] + left[row][index] * right[index][column];
      }
    }
  }
  return result;
}

JetMatrix IdentityMatrix() {
  JetMatrix result{};
  for (std::size_t index = 0U; index < 3U; ++index) {
    result[index][index] = Constant(1.0);
  }
  return result;
}

JetMatrix Skew(const JetVector& value) {
  JetMatrix result{};
  result[0U][1U] = Scale(-1.0, value[2U]);
  result[0U][2U] = value[1U];
  result[1U][0U] = value[2U];
  result[1U][2U] = Scale(-1.0, value[0U]);
  result[2U][0U] = Scale(-1.0, value[1U]);
  result[2U][1U] = value[0U];
  return result;
}

Jet Dot(const JetVector& left, const JetVector& right) {
  Jet result = Constant(0.0);
  for (std::size_t component = 0U; component < 3U; ++component) {
    result = result + left[component] * right[component];
  }
  return result;
}

JetMatrix Inverse(const JetMatrix& matrix) {
  JetMatrix cofactor{};
  for (std::size_t row = 0U; row < 3U; ++row) {
    for (std::size_t column = 0U; column < 3U; ++column) {
      const std::size_t a = (row + 1U) % 3U;
      const std::size_t b = (row + 2U) % 3U;
      const std::size_t c = (column + 1U) % 3U;
      const std::size_t d = (column + 2U) % 3U;
      cofactor[row][column] =
          matrix[a][c] * matrix[b][d] - matrix[a][d] * matrix[b][c];
      // Cyclic minor ordering already includes the cofactor sign.
    }
  }
  Jet determinant = Constant(0.0);
  for (std::size_t column = 0U; column < 3U; ++column) {
    determinant = determinant + matrix[0U][column] * cofactor[0U][column];
  }
  const Jet inverse_determinant = Reciprocal(determinant);
  JetMatrix result{};
  for (std::size_t row = 0U; row < 3U; ++row) {
    for (std::size_t column = 0U; column < 3U; ++column) {
      result[row][column] = cofactor[column][row] * inverse_determinant;
    }
  }
  return result;
}

JetMatrix LeftJacobian(const JetVector& phi) {
  const JetMatrix skew = Skew(phi);
  JetMatrix result = IdentityMatrix();
  JetMatrix term = IdentityMatrix();
  double factorial = 1.0;
  for (std::size_t order = 1U; order <= 32U; ++order) {
    term = Multiply(term, skew);
    factorial *= static_cast<double>(order + 1U);
    for (std::size_t row = 0U; row < 3U; ++row) {
      for (std::size_t column = 0U; column < 3U; ++column) {
        result[row][column] =
            result[row][column] + Scale(1.0 / factorial, term[row][column]);
      }
    }
  }
  return result;
}

/// @brief Uses a converged exponential series so D1/D2/D3 share one analytic
/// map.
JetMatrix ExpSkew(const JetVector& phi) {
  const JetMatrix skew = Skew(phi);
  JetMatrix result = IdentityMatrix();
  JetMatrix term = IdentityMatrix();
  double factorial = 1.0;
  for (std::size_t order = 1U; order <= 32U; ++order) {
    term = Multiply(term, skew);
    factorial *= static_cast<double>(order);
    for (std::size_t row = 0U; row < 3U; ++row) {
      for (std::size_t column = 0U; column < 3U; ++column) {
        result[row][column] =
            result[row][column] + Scale(1.0 / factorial, term[row][column]);
      }
    }
  }
  return result;
}

double Determinant(const Vector3& a, const Vector3& b, const Vector3& c) {
  return a.Dot(b.Cross(c));
}

/// @brief Checks the scaled physical twenty-dimensional mass without a rank
/// cut.
Status CheckPhysicalMass(const Matrix& mass,
                         const std::array<Vector3, kNodes>& nulls,
                         const double length, const SourceLocation& location,
                         const std::string& identity) {
  const auto fail = [&](const std::string& code, const std::string& message) {
    return Status::Failure(
        FailureCategory::kModel,
        {{Severity::kError, code, location, "*ELEMENT", identity, message}});
  };
  constexpr std::size_t kPhysical = 20U;
  Matrix scaled{kDofs, kDofs};
  double scale = 0.0;
  for (std::size_t row = 0U; row < kDofs; ++row) {
    for (std::size_t column = 0U; column < kDofs; ++column) {
      scaled(row, column) = mass(row, column) * (row % 6U < 3U ? length : 1.0) *
                            (column % 6U < 3U ? length : 1.0);
      if (mass(row, column) != 0.0 && scaled(row, column) == 0.0) {
        return fail("invalid-nonlinear-inertia-positive-mass",
                    "Physical mass scaling underflowed a nonzero entry.");
      }
      scale = std::max(scale, std::abs(scaled(row, column)));
    }
  }
  if (!IsFinite(scaled) || !std::isfinite(scale) || !(scale > 0.0)) {
    return fail("invalid-nonlinear-inertia-positive-mass",
                "Scaled physical mass must have a finite positive scale.");
  }
  // Scaling by a positive entry preserves conditioning and avoids density
  // overflow in the Cholesky and inverse-action norms.
  for (std::size_t row = 0U; row < kDofs; ++row) {
    for (std::size_t column = 0U; column < kDofs; ++column) {
      scaled(row, column) /= scale;
    }
  }
  Matrix complement{kDofs, kPhysical};
  for (std::size_t node = 0U; node < kNodes; ++node) {
    const Vector3& normal = nulls[node];
    std::array<std::size_t, 3> axes{0U, 1U, 2U};
    std::stable_sort(axes.begin(), axes.end(),
                     [&](const std::size_t a, const std::size_t b) {
                       return std::abs(normal[a]) < std::abs(normal[b]);
                     });
    std::array<Vector3, 2> tangent{};
    for (std::size_t column = 0U; column < 2U; ++column) {
      std::array<double, 3> axis{};
      axis[axes[column]] = 1.0;
      Vector3 direction{axis};
      direction = direction - direction.Dot(normal) * normal;
      for (std::size_t previous = 0U; previous < column; ++previous) {
        direction =
            direction - direction.Dot(tangent[previous]) * tangent[previous];
      }
      const double norm = direction.Norm();
      if (!std::isfinite(norm) || !(norm > 0.0)) {
        return fail("invalid-nonlinear-inertia-null",
                    "The analytic physical complement must remain finite.");
      }
      tangent[column] = direction / norm;
    }
    for (std::size_t row = 0U; row < 3U; ++row) {
      complement(6U * node + row, 5U * node + row) = 1.0;
      for (std::size_t column = 0U; column < 2U; ++column) {
        complement(6U * node + 3U + row, 5U * node + 3U + column) =
            tangent[column][row];
      }
    }
  }
  double mass_norm = 0.0;
  double symmetry_norm = 0.0;
  double null_action_norm = 0.0;
  for (std::size_t row = 0U; row < kDofs; ++row) {
    for (std::size_t column = 0U; column < kDofs; ++column) {
      mass_norm = std::hypot(mass_norm, scaled(row, column));
      symmetry_norm =
          std::hypot(symmetry_norm, scaled(row, column) - scaled(column, row));
    }
    for (std::size_t node = 0U; node < kNodes; ++node) {
      double action = 0.0;
      for (std::size_t component = 0U; component < 3U; ++component) {
        action +=
            scaled(row, 6U * node + 3U + component) * nulls[node][component];
      }
      null_action_norm = std::hypot(null_action_norm, action);
    }
  }
  double gram_error = 0.0;
  double orthogonality_error = 0.0;
  for (std::size_t row = 0U; row < kPhysical; ++row) {
    for (std::size_t column = 0U; column < kPhysical; ++column) {
      double entry = row == column ? -1.0 : 0.0;
      for (std::size_t inner = 0U; inner < kDofs; ++inner) {
        entry += complement(inner, row) * complement(inner, column);
      }
      gram_error = std::hypot(gram_error, entry);
    }
    for (std::size_t node = 0U; node < kNodes; ++node) {
      double entry = 0.0;
      for (std::size_t component = 0U; component < 3U; ++component) {
        entry += complement(6U * node + 3U + component, row) *
                 nulls[node][component];
      }
      orthogonality_error = std::hypot(orthogonality_error, entry);
    }
  }
  double null_gram_error = 0.0;
  for (const Vector3& normal : nulls) {
    null_gram_error = std::hypot(null_gram_error, normal.Dot(normal) - 1.0);
  }
  const double full_gram_error = std::hypot(
      gram_error,
      std::hypot(null_gram_error, std::sqrt(2.0) * orthogonality_error));
  if (!(symmetry_norm / mass_norm <= 1.0e-12) ||
      !(null_action_norm / mass_norm / 2.0 <= 1.0e-12) ||
      !(full_gram_error <= 1.0e-12)) {
    return fail("invalid-nonlinear-inertia-null",
                "Scaled mass symmetry and analytic null identities failed.");
  }
  Matrix block{kPhysical, kPhysical};
  Matrix mass_complement{kDofs, kPhysical};
  for (std::size_t row = 0U; row < kDofs; ++row) {
    for (std::size_t column = 0U; column < kPhysical; ++column) {
      for (std::size_t inner = 0U; inner < kDofs; ++inner) {
        mass_complement(row, column) +=
            scaled(row, inner) * complement(inner, column);
      }
    }
  }
  double block_norm = 0.0;
  for (std::size_t row = 0U; row < kPhysical; ++row) {
    double row_sum = 0.0;
    for (std::size_t column = 0U; column < kPhysical; ++column) {
      for (std::size_t inner = 0U; inner < kDofs; ++inner) {
        block(row, column) +=
            complement(inner, row) * mass_complement(inner, column);
      }
      row_sum += std::abs(block(row, column));
    }
    block_norm = std::max(block_norm, row_sum);
  }
  Matrix lower{kPhysical, kPhysical};
  for (std::size_t row = 0U; row < kPhysical; ++row) {
    for (std::size_t column = 0U; column <= row; ++column) {
      double entry = block(row, column);
      for (std::size_t inner = 0U; inner < column; ++inner) {
        entry -= lower(row, inner) * lower(column, inner);
      }
      if (!std::isfinite(entry) || (row == column && !(entry > 0.0))) {
        return fail(
            "invalid-nonlinear-inertia-positive-mass",
            "Physical mass requires strictly positive Cholesky pivots.");
      }
      lower(row, column) =
          row == column ? std::sqrt(entry) : entry / lower(column, column);
    }
  }
  std::array<double, kPhysical> inverse_row_sums{};
  double reconstruction_error = 0.0;
  double block_frobenius = 0.0;
  for (std::size_t row = 0U; row < kPhysical; ++row) {
    for (std::size_t column = 0U; column < kPhysical; ++column) {
      double reconstructed = 0.0;
      for (std::size_t inner = 0U; inner <= std::min(row, column); ++inner) {
        reconstructed += lower(row, inner) * lower(column, inner);
      }
      reconstruction_error =
          std::hypot(reconstruction_error, reconstructed - block(row, column));
      block_frobenius = std::hypot(block_frobenius, block(row, column));
    }
  }
  if (!(reconstruction_error / block_frobenius <= 1.0e-12)) {
    return fail("invalid-nonlinear-inertia-positive-mass",
                "Physical mass Cholesky reconstruction failed.");
  }
  for (std::size_t column = 0U; column < kPhysical; ++column) {
    std::array<double, kPhysical> solution{};
    for (std::size_t row = 0U; row < kPhysical; ++row) {
      double rhs = row == column ? 1.0 : 0.0;
      for (std::size_t inner = 0U; inner < row; ++inner) {
        rhs -= lower(row, inner) * solution[inner];
      }
      solution[row] = rhs / lower(row, row);
    }
    for (std::size_t reverse = kPhysical; reverse > 0U; --reverse) {
      const std::size_t row = reverse - 1U;
      double rhs = solution[row];
      for (std::size_t inner = row + 1U; inner < kPhysical; ++inner) {
        rhs -= lower(inner, row) * solution[inner];
      }
      solution[row] = rhs / lower(row, row);
      inverse_row_sums[row] += std::abs(solution[row]);
    }
  }
  const double inverse_norm =
      *std::max_element(inverse_row_sums.begin(), inverse_row_sums.end());
  const double reciprocal_condition = 1.0 / block_norm / inverse_norm;
  if (!std::isfinite(inverse_norm) || !std::isfinite(reciprocal_condition) ||
      !(reciprocal_condition >= std::ldexp(1.0, -32))) {
    return fail("ill-conditioned-nonlinear-inertia-positive-mass",
                "Physical mass reciprocal infinity condition is below 2^-32.");
  }
  return Status::Ok();
}

}  // namespace

Result<DynamicRotationKinematics> EvaluateDynamicRotation(
    const Vector3& phi_value, const Vector3& initial_director,
    const Vector3& velocity, const Vector3& acceleration) {
  if (!phi_value.IsFinite() || !initial_director.IsFinite() ||
      !velocity.IsFinite() || !acceleration.IsFinite() ||
      !(std::abs(initial_director.Norm() - 1.0) <= 1.0e-12) ||
      !(std::acos(-1.0) - phi_value.Norm() >
        std::sqrt(std::numeric_limits<double>::epsilon()))) {
    return Failure<DynamicRotationKinematics>(
        "invalid-dynamic-rotation", {{}, 0U}, "DYNAMIC_ROTATION",
        "Dynamic rotation requires finite principal coordinates and a unit "
        "initial director.");
  }
  JetVector phi{};
  for (std::size_t component = 0U; component < 3U; ++component) {
    phi[component] = Variable(phi_value[component], component);
  }
  const JetVector initial{Constant(initial_director.X()),
                          Constant(initial_director.Y()),
                          Constant(initial_director.Z())};
  const JetVector director_jet = Multiply(ExpSkew(phi), initial);
  const JetMatrix left_jacobian = LeftJacobian(phi);
  JetVector null_jet = Multiply(Inverse(left_jacobian), director_jet);
  const Jet null_norm_squared = Dot(null_jet, null_jet);
  if (!std::isfinite(null_norm_squared.value) ||
      !(null_norm_squared.value > 0.0)) {
    return Failure<DynamicRotationKinematics>(
        "invalid-dynamic-rotation", {{}, 0U}, "DYNAMIC_ROTATION",
        "Dynamic rotation produced a nonpositive drilling-null norm.");
  }
  const Jet inverse_norm = InverseSqrt(null_norm_squared);
  for (Jet& component : null_jet) {
    component = component * inverse_norm;
  }
  std::array<double, 3> directional_second{};
  std::array<double, 3> angular_velocity{};
  std::array<double, 3> angular_acceleration{};
  Matrix gradient{3U, 3U};
  for (std::size_t row = 0U; row < 3U; ++row) {
    for (std::size_t column = 0U; column < 3U; ++column) {
      gradient(row, column) = null_jet[row].d[column];
    }
  }
  for (std::size_t row = 0U; row < 3U; ++row) {
    double second = 0.0;
    double omega = 0.0;
    double omega_dot = 0.0;
    for (std::size_t column = 0U; column < 3U; ++column) {
      omega += left_jacobian[row][column].value * velocity[column];
      omega_dot += left_jacobian[row][column].value * acceleration[column];
      for (std::size_t other = 0U; other < 3U; ++other) {
        second += null_jet[row].dd[column][other] * velocity[column] *
                  velocity[other];
        omega_dot += left_jacobian[row][column].d[other] * velocity[other] *
                     velocity[column];
      }
    }
    directional_second[row] = second;
    angular_velocity[row] = omega;
    angular_acceleration[row] = omega_dot;
  }
  const Vector3 director{director_jet[0U].value, director_jet[1U].value,
                         director_jet[2U].value};
  const Vector3 drill_null{null_jet[0U].value, null_jet[1U].value,
                           null_jet[2U].value};
  const Vector3 directional_second_value{directional_second};
  const Vector3 angular_velocity_value{angular_velocity};
  const Vector3 angular_acceleration_value{angular_acceleration};
  if (!director.IsFinite() || !drill_null.IsFinite() ||
      !directional_second_value.IsFinite() ||
      !angular_velocity_value.IsFinite() ||
      !angular_acceleration_value.IsFinite() || !IsFinite(gradient)) {
    return Failure<DynamicRotationKinematics>(
        "invalid-dynamic-rotation", {{}, 0U}, "DYNAMIC_ROTATION",
        "Dynamic rotation evaluation must remain finite.");
  }
  DynamicRotationKinematics result{director,
                                   drill_null,
                                   std::move(gradient),
                                   directional_second_value,
                                   angular_velocity_value,
                                   angular_acceleration_value,
                                   {},
                                   {},
                                   {}};
  for (std::size_t i = 0U; i < 3U; ++i) {
    result.director_first[i] = {director_jet[0].d[i], director_jet[1].d[i],
                                director_jet[2].d[i]};
    for (std::size_t j = 0U; j < 3U; ++j) {
      result.director_second[i][j] = {director_jet[0].dd[i][j],
                                      director_jet[1].dd[i][j],
                                      director_jet[2].dd[i][j]};
      for (std::size_t k = 0U; k < 3U; ++k) {
        result.director_third[i][j][k] = {director_jet[0].ddd[i][j][k],
                                          director_jet[1].ddd[i][j][k],
                                          director_jet[2].ddd[i][j][k]};
      }
    }
  }
  return Result<DynamicRotationKinematics>::Success(std::move(result));
}

NonlinearMitc4Inertia::NonlinearMitc4Inertia(
    std::array<Vector3, 4> coordinates, std::array<Matrix, 4> initial_frames,
    std::array<Vector3, 4> initial_directors, const double thickness,
    const double density, SourceLocation source_location, std::string identity)
    : coordinates_(std::move(coordinates)),
      initial_frames_(std::move(initial_frames)),
      initial_directors_(std::move(initial_directors)),
      thickness_(thickness),
      density_(density),
      source_location_(std::move(source_location)),
      identity_(std::move(identity)) {}

Result<NonlinearMitc4Inertia> NonlinearMitc4Inertia::Create(
    std::array<const Node*, 4> nodes,
    std::array<ShellNodeInitialFrame, 4> initial_frames,
    const ShellSection& section, const LinearElasticMaterial& material,
    const double density) {
  const std::string identity = Identity(nodes);
  if (!std::isfinite(density) || !(density > 0.0)) {
    return Failure<NonlinearMitc4Inertia>(
        "invalid-nonlinear-inertia-density", section.location, identity,
        "Nonlinear MITC4 inertia requires a finite positive density.");
  }
  const auto shell =
      NonlinearMitc4Shell::Create(nodes, initial_frames, section, material);
  if (!shell.HasValue()) {
    return Result<NonlinearMitc4Inertia>::Failure(shell.GetStatus());
  }
  std::array<Vector3, kNodes> coordinates{};
  std::array<Matrix, kNodes> frames{Matrix{3U, 3U}, Matrix{3U, 3U},
                                    Matrix{3U, 3U}, Matrix{3U, 3U}};
  std::array<Vector3, kNodes> directors{};
  for (std::size_t node = 0U; node < kNodes; ++node) {
    coordinates[node] = Vector3{nodes[node]->coordinates};
    frames[node] = FrameMatrix(initial_frames[node]);
    directors[node] = Vector3{initial_frames[node].director};
  }
  NonlinearMitc4Inertia inertia{
      coordinates, std::move(frames),   directors, section.thickness,
      density,     nodes[0U]->location, identity};
  const auto initial = inertia.Evaluate({}, Vector{kDofs}, Vector{kDofs});
  if (!initial.HasValue()) {
    return Result<NonlinearMitc4Inertia>::Failure(initial.GetStatus());
  }
  return Result<NonlinearMitc4Inertia>::Success(std::move(inertia));
}

Result<NonlinearMitc4InertiaEvaluation> NonlinearMitc4Inertia::Evaluate(
    const NonlinearMitc4TrialDofs& trial, const Vector& velocity,
    const Vector& acceleration) const {
  if (velocity.Size() != kDofs || acceleration.Size() != kDofs ||
      !IsFinite(velocity) || !IsFinite(acceleration)) {
    return Failure<NonlinearMitc4InertiaEvaluation>(
        "invalid-nonlinear-inertia-rate", source_location_, identity_,
        "Nonlinear MITC4 inertia requires 24 finite rates.");
  }
  std::array<DynamicRotationKinematics, kNodes> rotations{};
  std::array<Vector3, kNodes> nulls{};
  double length = 0.0;
  for (std::size_t node = 0U; node < kNodes; ++node) {
    length = std::max(
        length,
        (coordinates_[(node + 1U) % kNodes] - coordinates_[node]).Norm());
    if (!trial.displacement[node].IsFinite() ||
        !MakeFiniteRotationState(trial.rotation_vector[node],
                                 initial_frames_[node])
             .HasValue()) {
      return Failure<NonlinearMitc4InertiaEvaluation>(
          "invalid-nonlinear-inertia-trial", source_location_, identity_,
          "Nonlinear MITC4 inertia trial must be in the principal chart.");
    }
    const auto rotation = EvaluateDynamicRotation(
        trial.rotation_vector[node], initial_directors_[node], {}, {});
    if (!rotation.HasValue()) {
      return Result<NonlinearMitc4InertiaEvaluation>::Failure(
          rotation.GetStatus());
    }
    rotations[node] = rotation.Value();
    nulls[node] = rotation.Value().drill_null;
    for (std::size_t previous = 0U; previous < node; ++previous) {
      if (!(rotations[node].director.Dot(rotations[previous].director) >
            -1.0 + 1.0e-8)) {
        return Failure<NonlinearMitc4InertiaEvaluation>(
            "invalid-nonlinear-inertia-director", source_location_, identity_,
            "Nonlinear MITC4 inertia trial directors must not be opposed.");
      }
    }
  }
  if (!std::isfinite(length) || !(length > 0.0)) {
    return Failure<NonlinearMitc4InertiaEvaluation>(
        "invalid-nonlinear-inertia-geometry", source_location_, identity_,
        "Nonlinear MITC4 inertia requires a finite positive initial edge.");
  }

  NonlinearMitc4InertiaEvaluation result{Matrix{kDofs, kDofs},
                                         Matrix{kDofs, kDofs},
                                         Matrix{kDofs, kDofs},
                                         Vector{kDofs},
                                         Vector{kDofs},
                                         0.0,
                                         {},
                                         {},
                                         {},
                                         {}};
  for (std::size_t xi = 0U; xi < kGauss.size(); ++xi) {
    for (std::size_t eta = 0U; eta < kGauss.size(); ++eta) {
      const Mitc4ShapeFunctions shape =
          Mitc4Shell::ShapeFunctions(kGauss[xi], kGauss[eta]);
      for (std::size_t zeta = 0U; zeta < kGauss.size(); ++zeta) {
        const double z = 0.5 * thickness_ * kGauss[zeta];
        const double weight = kWeights[xi] * kWeights[eta] * kWeights[zeta];
        Vector3 ref_xi{};
        Vector3 ref_eta{};
        Vector3 ref_zeta{};
        Vector3 current_xi{};
        Vector3 current_eta{};
        Vector3 current_zeta{};
        Vector3 position{};
        Vector3 velocity_point{};
        Vector3 acceleration_point{};
        Vector3 convective_point{};
        std::array<Vector3, kDofs> h{};
        std::array<Vector3, kDofs> velocity_derivative{};
        std::array<Vector3, kDofs> position_derivative{};
        for (std::size_t node = 0U; node < kNodes; ++node) {
          const std::size_t base = 6U * node;
          const auto& rotation = rotations[node];
          const Vector3 reference =
              coordinates_[node] + z * initial_directors_[node];
          const Vector3 current = coordinates_[node] +
                                  trial.displacement[node] +
                                  z * rotation.director;
          ref_xi = ref_xi + shape.xi_derivatives[node] * reference;
          ref_eta = ref_eta + shape.eta_derivatives[node] * reference;
          ref_zeta = ref_zeta + 0.5 * thickness_ * shape.values[node] *
                                    initial_directors_[node];
          current_xi = current_xi + shape.xi_derivatives[node] * current;
          current_eta = current_eta + shape.eta_derivatives[node] * current;
          current_zeta = current_zeta + 0.5 * thickness_ * shape.values[node] *
                                            rotation.director;
          position = position + shape.values[node] * current;
          for (std::size_t component = 0U; component < 3U; ++component) {
            std::array<double, 3> translation{};
            translation[component] = shape.values[node];
            h[base + component] = Vector3{translation};
            h[base + 3U + component] =
                z * shape.values[node] * rotation.director_first[component];
            for (std::size_t other = 0U; other < 3U; ++other) {
              const Vector3 second = z * shape.values[node] *
                                     rotation.director_second[component][other];
              convective_point =
                  convective_point + second * velocity[base + 3U + component] *
                                         velocity[base + 3U + other];
              velocity_derivative[base + 3U + component] =
                  velocity_derivative[base + 3U + component] +
                  2.0 * second * velocity[base + 3U + other];
              position_derivative[base + 3U + component] =
                  position_derivative[base + 3U + component] +
                  second * acceleration[base + 3U + other];
              for (std::size_t third = 0U; third < 3U; ++third) {
                position_derivative[base + 3U + component] =
                    position_derivative[base + 3U + component] +
                    z * shape.values[node] *
                        rotation.director_third[component][other][third] *
                        velocity[base + 3U + other] *
                        velocity[base + 3U + third];
              }
            }
          }
        }
        const double jacobian = Determinant(ref_xi, ref_eta, ref_zeta);
        const double current_jacobian =
            Determinant(current_xi, current_eta, current_zeta);
        if (!std::isfinite(jacobian) || !(jacobian > 0.0) ||
            !std::isfinite(current_jacobian) || !(current_jacobian > 0.0)) {
          return Failure<NonlinearMitc4InertiaEvaluation>(
              "invalid-nonlinear-inertia-geometry", source_location_, identity_,
              "Inertia requires positive reference and current mass-point "
              "Jacobians.");
        }
        for (std::size_t row = 0U; row < kDofs; ++row) {
          velocity_point = velocity_point + h[row] * velocity[row];
          acceleration_point = acceleration_point + h[row] * acceleration[row];
        }
        acceleration_point = acceleration_point + convective_point;
        const double scale = density_ * weight * jacobian;
        if (!std::isfinite(scale) || !(scale > 0.0)) {
          return Failure<NonlinearMitc4InertiaEvaluation>(
              "invalid-nonlinear-inertia-positive-mass", source_location_,
              identity_,
              "Mass-point integration weight must be finite and positive.");
        }
        result.kinetic_energy +=
            0.5 * scale * velocity_point.Dot(velocity_point);
        result.momentum = result.momentum + scale * velocity_point;
        result.momentum_rate =
            result.momentum_rate + scale * acceleration_point;
        result.angular_momentum =
            result.angular_momentum + scale * position.Cross(velocity_point);
        result.angular_momentum_rate =
            result.angular_momentum_rate +
            scale * position.Cross(acceleration_point);
        result.momentum_rate_scale += scale * acceleration_point.Norm();
        result.angular_momentum_rate_scale +=
            scale * position.Cross(acceleration_point).Norm();
        for (std::size_t row = 0U; row < kDofs; ++row) {
          result.force[row] += scale * h[row].Dot(acceleration_point);
          // Keep c independent of a; subtracting M*a destroys small c values.
          result.convective_force[row] += scale * h[row].Dot(convective_point);
          for (std::size_t column = 0U; column < kDofs; ++column) {
            result.mass(row, column) += scale * h[row].Dot(h[column]);
            result.velocity_tangent(row, column) +=
                scale * h[row].Dot(velocity_derivative[column]);
            double position_entry = h[row].Dot(position_derivative[column]);
            if (row / 6U == column / 6U && row % 6U >= 3U &&
                column % 6U >= 3U) {
              const std::size_t node = row / 6U;
              position_entry +=
                  z * shape.values[node] *
                  rotations[node]
                      .director_second[row % 6U - 3U][column % 6U - 3U]
                      .Dot(acceleration_point);
            }
            result.position_tangent(row, column) += scale * position_entry;
          }
        }
      }
    }
  }
  if (!IsFinite(result.mass) || !IsFinite(result.position_tangent) ||
      !IsFinite(result.velocity_tangent) || !IsFinite(result.force) ||
      !IsFinite(result.convective_force) ||
      !std::isfinite(result.kinetic_energy) || !result.momentum.IsFinite() ||
      !result.angular_momentum.IsFinite() || !result.momentum_rate.IsFinite() ||
      !result.angular_momentum_rate.IsFinite() ||
      !std::isfinite(result.momentum_rate_scale) ||
      !std::isfinite(result.angular_momentum_rate_scale)) {
    return Failure<NonlinearMitc4InertiaEvaluation>(
        "invalid-nonlinear-inertia-evaluation", source_location_, identity_,
        "Nonlinear MITC4 inertia evaluation must remain finite.");
  }
  const Status physical_mass = CheckPhysicalMass(result.mass, nulls, length,
                                                 source_location_, identity_);
  if (!physical_mass.IsOk()) {
    return Result<NonlinearMitc4InertiaEvaluation>::Failure(physical_mass);
  }
  return Result<NonlinearMitc4InertiaEvaluation>::Success(std::move(result));
}

}  // namespace fesa