#include "fesa/loads/shell_surface_pressure.h"

#include <gtest/gtest.h>

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

namespace {

using Contribution = fesa::ShellSurfacePressureContribution;
using Input = fesa::ShellSurfacePressureInput;
using Vector3 = fesa::Vector3;

constexpr std::size_t kNodeCount = 4U;
constexpr std::size_t kDofsPerNode = 6U;
constexpr std::size_t kDofCount = kNodeCount * kDofsPerNode;

Input PlanarInput(
    const fesa::SurfacePressureConfiguration configuration =
        fesa::SurfacePressureConfiguration::kReference) {
  return {{{Vector3{0.0, 0.0, 0.0}, Vector3{2.0, 0.0, 0.0},
            Vector3{2.0, 1.0, 0.0}, Vector3{0.0, 1.0, 0.0}}},
          {},
          fesa::ShellSurfaceSide::kSpos,
          3.0,
          0.5,
          configuration,
          {"shell-pressure.inp", 17U},
          "Part-1-1; element=10; side=SPOS"};
}

Input DistortedCurrentInput() {
  Input input = PlanarInput(fesa::SurfacePressureConfiguration::kCurrent);
  input.reference_coordinates = {
      Vector3{0.0, 0.0, 0.0}, Vector3{2.1, 0.2, 0.1},
      Vector3{1.7, 1.4, 0.5}, Vector3{-0.2, 1.1, -0.2}};
  input.translational_displacements = {
      Vector3{0.10, -0.04, 0.03}, Vector3{-0.02, 0.08, -0.01},
      Vector3{0.05, 0.02, 0.06}, Vector3{-0.04, -0.03, 0.02}};
  input.pressure_magnitude = 2.7;
  input.load_factor = 0.8;
  return input;
}

Contribution EvaluateOrThrow(const Input& input) {
  const auto result = fesa::EvaluateShellSurfacePressure(input);
  if (!result.HasValue()) {
    throw std::runtime_error{result.GetStatus().Diagnostics().front().code};
  }
  return result.Value();
}

Vector3 ForceAt(const Contribution& contribution, const std::size_t node) {
  const std::size_t offset = node * kDofsPerNode;
  return {contribution.force[offset], contribution.force[offset + 1U],
          contribution.force[offset + 2U]};
}

Vector3 SumForce(const Contribution& contribution) {
  Vector3 sum{};
  for (std::size_t node = 0U; node < kNodeCount; ++node) {
    sum = sum + ForceAt(contribution, node);
  }
  return sum;
}

Vector3 MomentAboutOrigin(const Input& input,
                          const Contribution& contribution) {
  Vector3 moment{};
  for (std::size_t node = 0U; node < kNodeCount; ++node) {
    Vector3 position = input.reference_coordinates[node];
    if (input.configuration ==
        fesa::SurfacePressureConfiguration::kCurrent) {
      position = position + input.translational_displacements[node];
    }
    moment = moment + position.Cross(ForceAt(contribution, node));
  }
  return moment;
}

double Norm(const Vector3& value) { return value.Norm(); }

template <std::size_t Size>
double Norm(const std::array<double, Size>& values) {
  double squared_norm = 0.0;
  for (const double value : values) {
    squared_norm += value * value;
  }
  return std::sqrt(squared_norm);
}

void ExpectVectorRelative(const Vector3& actual, const Vector3& expected,
                          const double scale, const double tolerance) {
  const double error = Norm(actual - expected);
  if (scale == 0.0) {
    EXPECT_EQ(error, 0.0);
    return;
  }
  EXPECT_LE(error / scale, tolerance);
}

template <std::size_t Size>
void ExpectArraysRelative(const std::array<double, Size>& actual,
                          const std::array<double, Size>& expected,
                          const double scale, const double tolerance) {
  std::array<double, Size> difference{};
  for (std::size_t index = 0U; index < difference.size(); ++index) {
    difference[index] = actual[index] - expected[index];
  }
  if (scale == 0.0) {
    EXPECT_EQ(Norm(difference), 0.0);
    return;
  }
  EXPECT_LE(Norm(difference) / scale, tolerance);
}

std::array<double, kDofCount> TangentAction(
    const Contribution& contribution,
    const std::array<double, kDofCount>& direction) {
  std::array<double, kDofCount> action{};
  for (std::size_t row = 0U; row < kDofCount; ++row) {
    for (std::size_t column = 0U; column < kDofCount; ++column) {
      action[row] +=
          contribution.external_tangent[row * kDofCount + column] *
          direction[column];
    }
  }
  return action;
}

double ChordLength(const Input& input) {
  double length = 0.0;
  for (std::size_t first = 0U; first < kNodeCount; ++first) {
    Vector3 first_position = input.reference_coordinates[first];
    if (input.configuration ==
        fesa::SurfacePressureConfiguration::kCurrent) {
      first_position = first_position + input.translational_displacements[first];
    }
    for (std::size_t second = first + 1U; second < kNodeCount; ++second) {
      Vector3 second_position = input.reference_coordinates[second];
      if (input.configuration ==
          fesa::SurfacePressureConfiguration::kCurrent) {
        second_position =
            second_position + input.translational_displacements[second];
      }
      length = (std::max)(length, Norm(first_position - second_position));
    }
  }
  return length;
}

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

Vector3 Rotate(const Rotation& rotation, const Vector3& value) {
  return {rotation[0U][0U] * value.X() + rotation[0U][1U] * value.Y() +
              rotation[0U][2U] * value.Z(),
          rotation[1U][0U] * value.X() + rotation[1U][1U] * value.Y() +
              rotation[1U][2U] * value.Z(),
          rotation[2U][0U] * value.X() + rotation[2U][1U] * value.Y() +
              rotation[2U][2U] * value.Z()};
}

Rotation ProperRotation() {
  const Vector3 axis{1.0, 2.0, 3.0};
  const Vector3 unit_axis = axis / axis.Norm();
  const double angle = 2.0 * std::acos(-1.0) / 3.0;
  const double cosine = std::cos(angle);
  const double sine = std::sin(angle);
  const double one_minus_cosine = 1.0 - cosine;
  return {{{cosine + one_minus_cosine * unit_axis.X() * unit_axis.X(),
            one_minus_cosine * unit_axis.X() * unit_axis.Y() -
                sine * unit_axis.Z(),
            one_minus_cosine * unit_axis.X() * unit_axis.Z() +
                sine * unit_axis.Y()},
           {one_minus_cosine * unit_axis.Y() * unit_axis.X() +
                sine * unit_axis.Z(),
            cosine + one_minus_cosine * unit_axis.Y() * unit_axis.Y(),
            one_minus_cosine * unit_axis.Y() * unit_axis.Z() -
                sine * unit_axis.X()},
           {one_minus_cosine * unit_axis.Z() * unit_axis.X() -
                sine * unit_axis.Y(),
            one_minus_cosine * unit_axis.Z() * unit_axis.Y() +
                sine * unit_axis.X(),
            cosine + one_minus_cosine * unit_axis.Z() * unit_axis.Z()}}};
}

Input RotateInput(const Input& input, const Rotation& rotation) {
  Input rotated = input;
  for (std::size_t node = 0U; node < kNodeCount; ++node) {
    rotated.reference_coordinates[node] =
        Rotate(rotation, input.reference_coordinates[node]);
    rotated.translational_displacements[node] =
        Rotate(rotation, input.translational_displacements[node]);
  }
  return rotated;
}

void ExpectFailure(const Input& input, const std::string& code) {
  const auto result = fesa::EvaluateShellSurfacePressure(input);
  ASSERT_FALSE(result.HasValue());
  ASSERT_EQ(result.GetStatus().Diagnostics().size(), 1U);
  EXPECT_EQ(result.GetStatus().Diagnostics().front().code, code);
  EXPECT_EQ(result.GetStatus().Diagnostics().front().location.file,
            input.location.file);
  EXPECT_EQ(result.GetStatus().Diagnostics().front().location.line,
            input.location.line);
  EXPECT_EQ(result.GetStatus().Diagnostics().front().entity_identity,
            input.entity_identity);
}

TEST(ShellSurfacePressureKernel,
     PartitionsUnityAndIntegratesPlanarResultantAndMoment) {
  const Input input = PlanarInput();
  const Contribution contribution = EvaluateOrThrow(input);
  const Vector3 expected_resultant{0.0, 0.0, -3.0};
  const Vector3 expected_moment{-1.5, 3.0, 0.0};
  const double force_scale = 3.0;
  const double moment_scale = std::sqrt(11.25);

  for (std::size_t node = 0U; node < kNodeCount; ++node) {
    ExpectVectorRelative(ForceAt(contribution, node),
                         0.25 * expected_resultant, force_scale, 1.0e-12);
  }
  ExpectVectorRelative(SumForce(contribution), expected_resultant, force_scale,
                       1.0e-12);
  ExpectVectorRelative(MomentAboutOrigin(input, contribution), expected_moment,
                       moment_scale, 1.0e-12);
}

TEST(ShellSurfacePressureKernel,
     ReversesExactlyForSposSnegAndNegativeMagnitude) {
  Input positive = PlanarInput(fesa::SurfacePressureConfiguration::kCurrent);
  positive.load_factor = 1.0;
  const Contribution spos = EvaluateOrThrow(positive);

  Input sneg_input = positive;
  sneg_input.side = fesa::ShellSurfaceSide::kSneg;
  const Contribution sneg = EvaluateOrThrow(sneg_input);

  Input negative_input = positive;
  negative_input.pressure_magnitude = -positive.pressure_magnitude;
  const Contribution negative = EvaluateOrThrow(negative_input);

  for (std::size_t index = 0U; index < kDofCount; ++index) {
    EXPECT_DOUBLE_EQ(sneg.force[index], -spos.force[index]);
    EXPECT_DOUBLE_EQ(negative.force[index], -spos.force[index]);
  }
  for (std::size_t index = 0U;
       index < spos.external_tangent.size(); ++index) {
    EXPECT_DOUBLE_EQ(sneg.external_tangent[index],
                     -spos.external_tangent[index]);
    EXPECT_DOUBLE_EQ(negative.external_tangent[index],
                     -spos.external_tangent[index]);
  }
}

TEST(ShellSurfacePressureKernel,
     UsesReferenceGeometryWithExactZeroLinearTangent) {
  Input reference = PlanarInput();
  reference.translational_displacements = {
      Vector3{10.0, -4.0, 3.0}, Vector3{7.0, 8.0, -2.0},
      Vector3{-2.0, 4.0, 9.0}, Vector3{3.0, -5.0, 1.0}};
  const Contribution with_displacement = EvaluateOrThrow(reference);

  reference.translational_displacements = {};
  const Contribution without_displacement = EvaluateOrThrow(reference);
  EXPECT_EQ(with_displacement.force, without_displacement.force);
  for (const double value : with_displacement.external_tangent) {
    EXPECT_EQ(value, 0.0);
  }
}

TEST(ShellSurfacePressureKernel, UsesCurrentAreaAndNormalForFollowerForce) {
  Input input = PlanarInput(fesa::SurfacePressureConfiguration::kCurrent);
  for (std::size_t node = 0U; node < kNodeCount; ++node) {
    input.translational_displacements[node] =
        0.25 * input.reference_coordinates[node];
  }
  const Contribution contribution = EvaluateOrThrow(input);
  const Vector3 expected_resultant{0.0, 0.0, -3.0 * 1.25 * 1.25};
  ExpectVectorRelative(SumForce(contribution), expected_resultant,
                       Norm(expected_resultant), 1.0e-12);
}

TEST(ShellSurfacePressureKernel,
     HasExactZeroRotationalForceRowsAndTangentBlocks) {
  const Contribution contribution = EvaluateOrThrow(DistortedCurrentInput());
  for (std::size_t row = 0U; row < kDofCount; ++row) {
    const bool rotation_row = row % kDofsPerNode >= 3U;
    if (rotation_row) {
      EXPECT_EQ(contribution.force[row], 0.0);
    }
    for (std::size_t column = 0U; column < kDofCount; ++column) {
      if (rotation_row || column % kDofsPerNode >= 3U) {
        EXPECT_EQ(contribution.external_tangent[row * kDofCount + column],
                  0.0);
      }
    }
  }
}

TEST(ShellSurfacePressureKernel, IsTranslationAndRigidRotationObjective) {
  const Input input = DistortedCurrentInput();
  const Contribution original = EvaluateOrThrow(input);

  Input translated = input;
  for (auto& displacement : translated.translational_displacements) {
    displacement = displacement + Vector3{3.0, -4.0, 2.0};
  }
  const Contribution translated_contribution = EvaluateOrThrow(translated);
  ExpectArraysRelative(translated_contribution.force, original.force,
                       (std::max)(Norm(translated_contribution.force),
                                  Norm(original.force)),
                       1.0e-12);
  ExpectArraysRelative(translated_contribution.external_tangent,
                       original.external_tangent,
                       (std::max)(Norm(translated_contribution.external_tangent),
                                  Norm(original.external_tangent)),
                       1.0e-12);

  const Rotation rotation = ProperRotation();
  const Contribution rotated = EvaluateOrThrow(RotateInput(input, rotation));
  for (std::size_t node = 0U; node < kNodeCount; ++node) {
    ExpectVectorRelative(ForceAt(rotated, node),
                         Rotate(rotation, ForceAt(original, node)),
                         Norm(ForceAt(original, node)), 1.0e-12);
  }
  for (std::size_t row_node = 0U; row_node < kNodeCount; ++row_node) {
    for (std::size_t column_node = 0U; column_node < kNodeCount;
         ++column_node) {
      for (std::size_t row_component = 0U; row_component < 3U;
           ++row_component) {
        for (std::size_t column_component = 0U; column_component < 3U;
             ++column_component) {
          double expected = 0.0;
          for (std::size_t left = 0U; left < 3U; ++left) {
            for (std::size_t right = 0U; right < 3U; ++right) {
              expected +=
                  rotation[row_component][left] *
                  original.external_tangent
                      [(row_node * kDofsPerNode + left) * kDofCount +
                       column_node * kDofsPerNode + right] *
                  rotation[column_component][right];
            }
          }
          const double actual =
              rotated.external_tangent
                  [(row_node * kDofsPerNode + row_component) * kDofCount +
                   column_node * kDofsPerNode + column_component];
          EXPECT_NEAR(actual, expected, 1.0e-12);
        }
      }
    }
  }
}

TEST(ShellSurfacePressureKernel, IsCovariantUnderCyclicNodeRelabeling) {
  const Input input = DistortedCurrentInput();
  const Contribution original = EvaluateOrThrow(input);
  Input cyclic = input;
  for (std::size_t node = 0U; node < kNodeCount; ++node) {
    cyclic.reference_coordinates[node] =
        input.reference_coordinates[(node + 1U) % kNodeCount];
    cyclic.translational_displacements[node] =
        input.translational_displacements[(node + 1U) % kNodeCount];
  }
  const Contribution relabeled = EvaluateOrThrow(cyclic);
  for (std::size_t row = 0U; row < kDofCount; ++row) {
    const std::size_t original_row =
        ((row / kDofsPerNode + 1U) % kNodeCount) * kDofsPerNode +
        row % kDofsPerNode;
    EXPECT_NEAR(relabeled.force[row], original.force[original_row], 1.0e-12);
    for (std::size_t column = 0U; column < kDofCount; ++column) {
      const std::size_t original_column =
          ((column / kDofsPerNode + 1U) % kNodeCount) * kDofsPerNode +
          column % kDofsPerNode;
      EXPECT_NEAR(relabeled.external_tangent[row * kDofCount + column],
                  original.external_tangent[original_row * kDofCount +
                                            original_column],
                  1.0e-12);
    }
  }
}

TEST(ShellSurfacePressureKernel,
     MatchesCenteredFiniteDifferenceForForceAndResidualSigns) {
  const Input input = DistortedCurrentInput();
  const Contribution base = EvaluateOrThrow(input);
  std::array<double, kDofCount> direction{};
  direction[0U] = 0.4;
  direction[1U] = -0.2;
  direction[2U] = 0.1;
  direction[6U] = -0.1;
  direction[7U] = 0.3;
  direction[8U] = -0.4;
  direction[12U] = 0.2;
  direction[13U] = 0.1;
  direction[14U] = 0.5;
  direction[18U] = -0.3;
  direction[19U] = -0.4;
  direction[20U] = -0.2;

  double direction_scale = 0.0;
  for (std::size_t node = 0U; node < kNodeCount; ++node) {
    direction_scale =
        (std::max)(direction_scale,
                   Norm(Vector3{direction[node * kDofsPerNode],
                                direction[node * kDofsPerNode + 1U],
                                direction[node * kDofsPerNode + 2U]}));
  }
  ASSERT_GT(direction_scale, 0.0);
  const double chord_length = ChordLength(input);
  for (std::size_t node = 0U; node < kNodeCount; ++node) {
    for (std::size_t component = 0U; component < 3U; ++component) {
      direction[node * kDofsPerNode + component] *=
          chord_length / direction_scale;
    }
  }
  constexpr double kStep = 1.0e-5;
  Input plus = input;
  Input minus = input;
  for (std::size_t node = 0U; node < kNodeCount; ++node) {
    plus.translational_displacements[node] =
        plus.translational_displacements[node] +
        kStep * Vector3{direction[node * kDofsPerNode],
                        direction[node * kDofsPerNode + 1U],
                        direction[node * kDofsPerNode + 2U]};
    minus.translational_displacements[node] =
        minus.translational_displacements[node] -
        kStep * Vector3{direction[node * kDofsPerNode],
                        direction[node * kDofsPerNode + 1U],
                        direction[node * kDofsPerNode + 2U]};
  }
  const Contribution plus_contribution = EvaluateOrThrow(plus);
  const Contribution minus_contribution = EvaluateOrThrow(minus);
  std::array<double, kDofCount> finite_difference{};
  for (std::size_t index = 0U; index < kDofCount; ++index) {
    finite_difference[index] =
        (plus_contribution.force[index] - minus_contribution.force[index]) /
        (2.0 * kStep);
  }
  const std::array<double, kDofCount> analytical =
      TangentAction(base, direction);
  const double scale = (std::max)(Norm(finite_difference), Norm(analytical));
  ASSERT_GT(scale, 0.0);
  ExpectArraysRelative(finite_difference, analytical, scale, 1.0e-8);

  std::array<double, kDofCount> residual_difference{};
  std::array<double, kDofCount> residual_analytical{};
  for (std::size_t index = 0U; index < kDofCount; ++index) {
    residual_difference[index] = -finite_difference[index];
    residual_analytical[index] = -analytical[index];
  }
  ExpectArraysRelative(residual_difference, residual_analytical, scale,
                       1.0e-8);
}

TEST(ShellSurfacePressureKernel,
     PreservesGenerallyNonsymmetricFollowerTangent) {
  Input input = PlanarInput(fesa::SurfacePressureConfiguration::kCurrent);
  input.load_factor = 1.0;
  const Contribution contribution = EvaluateOrThrow(input);
  const double xz = contribution.external_tangent[0U * kDofCount + 2U];
  const double zx = contribution.external_tangent[2U * kDofCount + 0U];
  EXPECT_NE(xz, 0.0);
  EXPECT_NE(zx, 0.0);
  EXPECT_DOUBLE_EQ(xz, -zx);
}

TEST(ShellSurfacePressureKernel,
     RejectsNonfiniteCollapsedFoldedAndInvertedGeometryAtEveryRequiredPoint) {
  Input nonfinite = PlanarInput();
  nonfinite.reference_coordinates[0U] =
      Vector3{std::numeric_limits<double>::infinity(), 0.0, 0.0};
  ExpectFailure(nonfinite, "invalid-reference-surface-geometry");

  Input collapsed = PlanarInput();
  collapsed.reference_coordinates = {Vector3{}, Vector3{}, Vector3{},
                                     Vector3{}};
  ExpectFailure(collapsed, "invalid-reference-surface-geometry");

  Input folded = PlanarInput();
  folded.reference_coordinates = {
      Vector3{0.0, 0.0, 0.0}, Vector3{1.0, 0.0, 0.0},
      Vector3{0.0, 1.0, 0.0}, Vector3{1.0, 1.0, 0.0}};
  ExpectFailure(folded, "invalid-reference-surface-geometry");

  Input inverted = PlanarInput(fesa::SurfacePressureConfiguration::kCurrent);
  inverted.translational_displacements[2U] = Vector3{-3.0, 0.0, 0.0};
  ExpectFailure(inverted, "invalid-current-surface-geometry");
}

TEST(ShellSurfacePressureKernel,
     ValidatesGeometryEvenForZeroPressureOrFactor) {
  Input zero_pressure = PlanarInput();
  zero_pressure.pressure_magnitude = 0.0;
  zero_pressure.reference_coordinates = {Vector3{}, Vector3{}, Vector3{},
                                         Vector3{}};
  ExpectFailure(zero_pressure, "invalid-reference-surface-geometry");

  Input zero_factor =
      PlanarInput(fesa::SurfacePressureConfiguration::kCurrent);
  zero_factor.load_factor = 0.0;
  zero_factor.translational_displacements[2U] = Vector3{-3.0, 0.0, 0.0};
  ExpectFailure(zero_factor, "invalid-current-surface-geometry");
}

}  // namespace