#include <gtest/gtest.h>

#include <algorithm>
#include <array>
#include <cmath>
#include <cstdint>
#include <string>

#include "fesa/elements/mitc4_shell.h"
#include "fesa/model/model_types.h"

namespace {

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

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

double MaximumEdgeLength(const std::array<fesa::Node, 4>& nodes) {
  double result = 0.0;
  for (std::size_t node = 0U; node < nodes.size(); ++node) {
    const auto& first = nodes[node].coordinates;
    const auto& second = nodes[(node + 1U) % nodes.size()].coordinates;
    result = (std::max)(result, std::hypot(
        std::hypot(first[0U] - second[0U], first[1U] - second[1U]),
        first[2U] - second[2U]));
  }
  return result;
}

Coordinates Rotate(const Coordinates& value) {
  return {-value[1U],
          0.6 * value[0U] - 0.8 * value[2U],
          0.8 * value[0U] + 0.6 * value[2U]};
}

Coordinates DirectorCross(const Coordinates& director,
                          const Coordinates& rotation) {
  return {rotation[1U] * director[2U] - rotation[2U] * director[1U],
          rotation[2U] * director[0U] - rotation[0U] * director[2U],
          rotation[0U] * director[1U] - rotation[1U] * director[0U]};
}

double Dot(const Coordinates& left, const Coordinates& right) {
  return left[0U] * right[0U] + left[1U] * right[1U] +
         left[2U] * right[2U];
}

fesa::Matrix RotateCongruently(const fesa::Matrix& original) {
  fesa::Matrix result{24U, 24U};
  for (std::size_t row = 0U; row < 24U; ++row) {
    for (std::size_t column = 0U; column < 24U; ++column) {
      const std::size_t row_base = 6U * (row / 6U) +
                                   (row % 6U < 3U ? 0U : 3U);
      const std::size_t column_base = 6U * (column / 6U) +
                                      (column % 6U < 3U ? 0U : 3U);
      const std::size_t row_component = row % 3U;
      const std::size_t column_component = column % 3U;
      for (std::size_t left = 0U; left < 3U; ++left) {
        for (std::size_t right = 0U; right < 3U; ++right) {
          const Coordinates basis_left{
              left == 0U ? 1.0 : 0.0, left == 1U ? 1.0 : 0.0,
              left == 2U ? 1.0 : 0.0};
          const Coordinates basis_right{
              right == 0U ? 1.0 : 0.0, right == 1U ? 1.0 : 0.0,
              right == 2U ? 1.0 : 0.0};
          result(row, column) += Rotate(basis_left)[row_component] *
                                 original(row_base + left, column_base + right) *
                                 Rotate(basis_right)[column_component];
        }
      }
    }
  }
  return result;
}

void ExpectScaledCovariance(const fesa::Matrix& original,
                            const fesa::Matrix& rotated,
                            const double length) {
  const auto expected = RotateCongruently(original);
  fesa::Matrix difference{24U, 24U};
  for (std::size_t row = 0U; row < 24U; ++row)
    for (std::size_t column = 0U; column < 24U; ++column)
      difference(row, column) = rotated(row, column) - expected(row, column);
  const double ratio = FrobeniusNorm(difference, length) /
                       (FrobeniusNorm(rotated, length) +
                        FrobeniusNorm(expected, length));
  EXPECT_LE(ratio, 1e-12);
}

fesa::Node Node(std::int64_t label, Coordinates coordinates) {
  return {{"I", label, std::to_string(label)}, coordinates, {"mass.inp", 1U}};
}

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

fesa::Mitc4Shell WarpedShell() {
  const std::array<fesa::Node, 4> nodes{
      Node(1, {-1.0, -1.0, 0.1}), Node(2, {1.0, -1.0, -0.1}),
      Node(3, {1.0, 1.0, 0.2}), Node(4, {-1.0, 1.0, -0.1})};
  const std::array<Coordinates, 4> directors{
      {{{0.0, 0.0, 1.0}},
       {{0.6, 0.0, 0.8}},
       {{2.0 / 3.0, 1.0 / 3.0, 2.0 / 3.0}},
       {{0.0, 0.6, 0.8}}}};
  const fesa::ShellSection section{"S", 0.5, 0U, {"mass.inp", 2U}};
  const fesa::LinearElasticMaterial material{"M", 12.0, 0.0, {"mass.inp", 3U}};
  const auto result =
      fesa::Mitc4Shell::Create(Pointers(nodes), directors, section, material);
  EXPECT_TRUE(result.HasValue());
  return result.Value();
}

double Quadratic(const fesa::Matrix& matrix, const fesa::Vector& vector) {
  return vector.Dot(matrix.Multiply(vector));
}

// E2-C02: all values derive independently in numerical-review-evidence.md.
TEST(Mitc4ShellMassContract, PreservesWarpedMomentsRowsAndFirstSpatialMoment) {
  const auto mass = WarpedShell().Mass(3.0);
  ASSERT_TRUE(mass.HasValue());
  const auto& value = mass.Value();
  const double expected[3][4][4] = {
      {{17609.0 / 28800.0, 3337.0 / 11520.0, 1547.0 / 11520.0,
         37.0 / 128.0},
        {3337.0 / 11520.0, 15793.0 / 28800.0, 7177.0 / 28800.0,
         1547.0 / 11520.0},
        {1547.0 / 11520.0, 7177.0 / 28800.0, 4297.0 / 9600.0,
         14239.0 / 57600.0},
        {37.0 / 128.0, 1547.0 / 11520.0, 14239.0 / 57600.0,
         15683.0 / 28800.0}},
      {{11003.0 / 1440000.0, 5053.0 / 1440000.0, 9839.0 / 5760000.0,
         21667.0 / 5760000.0},
        {5053.0 / 1440000.0, 27769.0 / 4320000.0, 53411.0 / 17280000.0,
         9839.0 / 5760000.0},
        {9839.0 / 5760000.0, 53411.0 / 17280000.0, 5687.0 / 960000.0,
         28673.0 / 8640000.0},
        {21667.0 / 5760000.0, 9839.0 / 5760000.0, 28673.0 / 8640000.0,
         63913.0 / 8640000.0}},
      {{88177.0 / 6912000.0, 27847.0 / 4608000.0, 7747.0 / 2764800.0,
         41693.0 / 6912000.0},
        {27847.0 / 4608000.0, 1757.0 / 153600.0, 1331.0 / 256000.0,
         7747.0 / 2764800.0},
        {7747.0 / 2764800.0, 1331.0 / 256000.0, 21521.0 / 2304000.0,
         23773.0 / 4608000.0},
        {41693.0 / 6912000.0, 7747.0 / 2764800.0, 23773.0 / 4608000.0,
         5237.0 / 460800.0}}};
  constexpr double element_mass = 7743.0 / 1600.0;
  constexpr double half_thickness = 0.25;
  for (std::size_t moment = 0U; moment < 3U; ++moment) {
    const double scale = element_mass * std::pow(half_thickness, moment);
    for (std::size_t row = 0U; row < 4U; ++row) {
      double expected_row = 0.0;
      for (std::size_t column = 0U; column < 4U; ++column) {
        EXPECT_LE(std::abs(value.consistent_moments[moment](row, column) -
                           expected[moment][row][column]) /
                      scale,
                  1e-12)
            << "moment=" << moment << " row=" << row
            << " column=" << column;
        expected_row += expected[moment][row][column];
      }
      EXPECT_LE(std::abs(value.nodal_moments[row][moment] - expected_row) /
                    scale,
                1e-12)
          << "moment=" << moment << " row=" << row;
    }
  }
  const std::array<Coordinates, 4> coordinates{{
      {{-1.0, -1.0, 0.1}}, {{1.0, -1.0, -0.1}},
      {{1.0, 1.0, 0.2}}, {{-1.0, 1.0, -0.1}}}};
  const std::array<Coordinates, 4> directors{{
      {{0.0, 0.0, 1.0}}, {{0.6, 0.0, 0.8}},
      {{2.0 / 3.0, 1.0 / 3.0, 2.0 / 3.0}}, {{0.0, 0.6, 0.8}}}};
  const Coordinates reference = coordinates.front();
  const Coordinates expected_first{{-79747.0 / 360000.0,
                                    -685951.0 / 2880000.0,
                                    49651.0 / 320000.0}};
  Coordinates actual_first{};
  for (std::size_t node = 0U; node < 4U; ++node) {
    for (std::size_t component = 0U; component < 3U; ++component) {
      actual_first[component] +=
          value.nodal_moments[node][0U] *
              (coordinates[node][component] - reference[component]) +
          value.nodal_moments[node][1U] * directors[node][component];
    }
  }
  double difference = 0.0;
  for (std::size_t component = 0U; component < 3U; ++component)
    difference = std::hypot(difference, actual_first[component] -
                                            (expected_first[component] -
                                             element_mass * reference[component]));
  EXPECT_LE(difference / (element_mass * std::hypot(2.0, 0.3)), 1e-12);
}

// E2-C05: rebuild both masses after a proper rotation and translation.
TEST(Mitc4ShellMassContract, RecomputesWarpedMassWithScaledCovariance) {
  const std::array<fesa::Node, 4> nodes{
      Node(1, {-1.0, -1.0, 0.1}), Node(2, {1.0, -1.0, -0.1}),
      Node(3, {1.0, 1.0, 0.2}), Node(4, {-1.0, 1.0, -0.1})};
  const std::array<Coordinates, 4> directors{{
      {{0.0, 0.0, 1.0}}, {{0.6, 0.0, 0.8}},
      {{2.0 / 3.0, 1.0 / 3.0, 2.0 / 3.0}}, {{0.0, 0.6, 0.8}}}};
  std::array<fesa::Node, 4> transformed_nodes = nodes;
  std::array<Coordinates, 4> transformed_directors{};
  for (std::size_t node = 0U; node < nodes.size(); ++node) {
    transformed_nodes[node].coordinates = Rotate(nodes[node].coordinates);
    transformed_nodes[node].coordinates[0U] += 7.0;
    transformed_nodes[node].coordinates[1U] -= 3.0;
    transformed_nodes[node].coordinates[2U] += 5.0;
    transformed_directors[node] = Rotate(directors[node]);
  }
  const fesa::ShellSection section{"S", 0.5, 0U, {"mass.inp", 2U}};
  const fesa::LinearElasticMaterial material{"M", 12.0, 0.0,
                                               {"mass.inp", 3U}};
  const auto original = fesa::Mitc4Shell::Create(Pointers(nodes), directors,
                                                  section, material);
  const auto transformed = fesa::Mitc4Shell::Create(
      Pointers(transformed_nodes), transformed_directors, section, material);
  ASSERT_TRUE(original.HasValue());
  ASSERT_TRUE(transformed.HasValue());
  const auto original_mass = original.Value().Mass(3.0);
  const auto transformed_mass = transformed.Value().Mass(3.0);
  ASSERT_TRUE(original_mass.HasValue());
  ASSERT_TRUE(transformed_mass.HasValue());
  const double length = MaximumEdgeLength(nodes);
  ExpectScaledCovariance(original_mass.Value().consistent_global24,
                         transformed_mass.Value().consistent_global24, length);
  ExpectScaledCovariance(original_mass.Value().lumped_global24,
                         transformed_mass.Value().lumped_global24, length);
}

// E2-C03: scalar row-sum lumping retains uniform translation energy and is
// never below the consistent kinetic energy.
TEST(Mitc4ShellMassContract, SatisfiesUniformAndJensenEnergyContracts) {
  const auto mass = WarpedShell().Mass(3.0);
  ASSERT_TRUE(mass.HasValue());
  fesa::Vector translation{24U};
  fesa::Vector field{24U};
  for (std::size_t node = 0U; node < 4U; ++node) {
    translation[6U * node] = 1.0;
    field[6U * node] = static_cast<double>(node + 1U);
    field[6U * node + 1U] = -0.5 * static_cast<double>(node);
    field[6U * node + 3U] = 0.25;
  }
  EXPECT_NEAR(Quadratic(mass.Value().consistent_global24, translation),
              Quadratic(mass.Value().lumped_global24, translation), 1e-12);
  EXPECT_GE(Quadratic(mass.Value().lumped_global24, field) + 1e-12,
            Quadratic(mass.Value().consistent_global24, field));
}

// E2-C03: construct both 24-by-24 mass variants from the independently
// checked scalar moments, including the C1 translation/rotation coupling.
TEST(Mitc4ShellMassContract, MapsScalarMomentsIntoBothMassBlocks) {
  const auto mass = WarpedShell().Mass(3.0);
  ASSERT_TRUE(mass.HasValue());
  const std::array<Coordinates, 4> directors{{
      {{0.0, 0.0, 1.0}}, {{0.6, 0.0, 0.8}},
      {{2.0 / 3.0, 1.0 / 3.0, 2.0 / 3.0}}, {{0.0, 0.6, 0.8}}}};
  fesa::Matrix expected_consistent{24U, 24U};
  fesa::Matrix expected_lumped{24U, 24U};
  for (std::size_t first = 0U; first < 4U; ++first) {
    for (std::size_t second = 0U; second < 4U; ++second) {
      for (std::size_t row = 0U; row < 3U; ++row) {
        for (std::size_t column = 0U; column < 3U; ++column) {
          const double identity = row == column ? 1.0 : 0.0;
          const Coordinates unit_column{{column == 0U ? 1.0 : 0.0,
                                         column == 1U ? 1.0 : 0.0,
                                         column == 2U ? 1.0 : 0.0}};
          const Coordinates unit_row{{row == 0U ? 1.0 : 0.0,
                                      row == 1U ? 1.0 : 0.0,
                                      row == 2U ? 1.0 : 0.0}};
          const double sj = DirectorCross(directors[second], unit_column)[row];
          const double si = DirectorCross(directors[first], unit_row)[column];
          expected_consistent(6U * first + row, 6U * second + column) =
              mass.Value().consistent_moments[0U](first, second) * identity;
          expected_consistent(6U * first + row, 6U * second + 3U + column) =
              mass.Value().consistent_moments[1U](first, second) * sj;
          expected_consistent(6U * first + 3U + row, 6U * second + column) =
              mass.Value().consistent_moments[1U](first, second) * si;
          expected_consistent(6U * first + 3U + row,
                              6U * second + 3U + column) =
              mass.Value().consistent_moments[2U](first, second) *
              Dot(DirectorCross(directors[first], unit_row),
                  DirectorCross(directors[second], unit_column));
        }
      }
    }
    for (std::size_t row = 0U; row < 3U; ++row) {
      for (std::size_t column = 0U; column < 3U; ++column) {
        const double identity = row == column ? 1.0 : 0.0;
        const Coordinates unit_column{{column == 0U ? 1.0 : 0.0,
                                       column == 1U ? 1.0 : 0.0,
                                       column == 2U ? 1.0 : 0.0}};
        const Coordinates unit_row{{row == 0U ? 1.0 : 0.0,
                                    row == 1U ? 1.0 : 0.0,
                                    row == 2U ? 1.0 : 0.0}};
        const double sj = DirectorCross(directors[first], unit_column)[row];
        const double si = DirectorCross(directors[first], unit_row)[column];
        expected_lumped(6U * first + row, 6U * first + column) =
            mass.Value().nodal_moments[first][0U] * identity;
        expected_lumped(6U * first + row, 6U * first + 3U + column) =
            mass.Value().nodal_moments[first][1U] * sj;
        expected_lumped(6U * first + 3U + row, 6U * first + column) =
            mass.Value().nodal_moments[first][1U] * si;
        expected_lumped(6U * first + 3U + row, 6U * first + 3U + column) =
            mass.Value().nodal_moments[first][2U] *
            Dot(DirectorCross(directors[first], unit_row),
                DirectorCross(directors[first], unit_column));
      }
    }
  }
  fesa::Vector velocity{24U};
  for (std::size_t node = 0U; node < 4U; ++node) {
    velocity[6U * node] = static_cast<double>(node + 1U);
    velocity[6U * node + 1U] = -0.5 * static_cast<double>(node);
    velocity[6U * node + 3U] = 0.25;
  }
  for (std::size_t row = 0U; row < 24U; ++row) {
    for (std::size_t column = 0U; column < 24U; ++column) {
      EXPECT_NEAR(mass.Value().consistent_global24(row, column),
                  expected_consistent(row, column), 1e-12);
      EXPECT_NEAR(mass.Value().lumped_global24(row, column),
                  expected_lumped(row, column), 1e-12);
    }
  }
  fesa::Matrix consistent_difference{24U, 24U};
  fesa::Matrix lumped_difference{24U, 24U};
  for (std::size_t row = 0U; row < 24U; ++row) {
    for (std::size_t column = 0U; column < 24U; ++column) {
      consistent_difference(row, column) =
          mass.Value().consistent_global24(row, column) -
          expected_consistent(row, column);
      lumped_difference(row, column) = mass.Value().lumped_global24(row, column) -
                                       expected_lumped(row, column);
    }
  }
  const double length = std::hypot(2.0, 0.3);
  EXPECT_LE(FrobeniusNorm(consistent_difference, length) /
                (FrobeniusNorm(mass.Value().consistent_global24, length) +
                 FrobeniusNorm(expected_consistent, length)),
            1e-12);
  EXPECT_LE(FrobeniusNorm(lumped_difference, length) /
                (FrobeniusNorm(mass.Value().lumped_global24, length) +
                 FrobeniusNorm(expected_lumped, length)),
            1e-12);
  double pairwise_work = 0.0;
  for (std::size_t first = 0U; first < 4U; ++first) {
    for (std::size_t second = 0U; second < 4U; ++second) {
      Coordinates displacement_difference{};
      Coordinates director_velocity_difference{};
      for (std::size_t component = 0U; component < 3U; ++component) {
        displacement_difference[component] =
            velocity[6U * first + component] -
            velocity[6U * second + component];
      }
      Coordinates first_rotation{{velocity[6U * first + 3U],
                                   velocity[6U * first + 4U],
                                   velocity[6U * first + 5U]}};
      Coordinates second_rotation{{velocity[6U * second + 3U],
                                    velocity[6U * second + 4U],
                                    velocity[6U * second + 5U]}};
      const auto first_director_velocity =
          DirectorCross(directors[first], first_rotation);
      const auto second_director_velocity =
          DirectorCross(directors[second], second_rotation);
      for (std::size_t component = 0U; component < 3U; ++component)
        director_velocity_difference[component] =
            first_director_velocity[component] -
            second_director_velocity[component];
      pairwise_work += 0.5 * (
          mass.Value().consistent_moments[0U](first, second) *
              Dot(displacement_difference, displacement_difference) +
          2.0 * mass.Value().consistent_moments[1U](first, second) *
              Dot(displacement_difference, director_velocity_difference) +
          mass.Value().consistent_moments[2U](first, second) *
              Dot(director_velocity_difference, director_velocity_difference));
    }
  }
  const double consistent_work =
      Quadratic(mass.Value().consistent_global24, velocity);
  const double lumped_work = Quadratic(mass.Value().lumped_global24, velocity);
  EXPECT_LE(std::abs((lumped_work - consistent_work) - pairwise_work) /
                (lumped_work + consistent_work),
            1e-12);
}

// E2-C04: assemble the physical velocity work over successively finer meshes.
TEST(Mitc4ShellMassContract, ReproducesAffineSquareVelocityWork) {
  const std::array<Coordinates, 4> directors{{{{0.0, 0.0, 1.0}},
                                              {{0.0, 0.0, 1.0}},
                                              {{0.0, 0.0, 1.0}},
                                              {{0.0, 0.0, 1.0}}}};
  const fesa::ShellSection section{"S", 1.0, 0U, {"mass.inp", 2U}};
  const fesa::LinearElasticMaterial material{"M", 12.0, 0.0, {"mass.inp", 3U}};
  for (const int mesh : {1, 2, 4}) {
    double consistent_work = 0.0, lumped_work = 0.0;
    const double edge = 2.0 / mesh;
    for (int row = 0; row < mesh; ++row) {
      for (int column = 0; column < mesh; ++column) {
        const double x = -1.0 + edge * column, y = -1.0 + edge * row;
        const std::array<fesa::Node, 4> nodes{
            Node(1, {x, y, 0.0}), Node(2, {x + edge, y, 0.0}),
            Node(3, {x + edge, y + edge, 0.0}), Node(4, {x, y + edge, 0.0})};
        const auto shell = fesa::Mitc4Shell::Create(Pointers(nodes), directors,
                                                    section, material);
        ASSERT_TRUE(shell.HasValue());
        const auto mass = shell.Value().Mass(1.0);
        ASSERT_TRUE(mass.HasValue());
        fesa::Vector velocity{24U};
        for (std::size_t node = 0U; node < 4U; ++node)
          velocity[6U * node] = nodes[node].coordinates[0U];
        consistent_work +=
            Quadratic(mass.Value().consistent_global24, velocity);
        lumped_work += Quadratic(mass.Value().lumped_global24, velocity);
      }
    }
    EXPECT_NEAR(consistent_work, 4.0 / 3.0, 1e-12) << mesh;
    EXPECT_NEAR(lumped_work - consistent_work, 8.0 / (3.0 * mesh * mesh), 1e-12)
        << mesh;
  }
}

}  // namespace