#include "fesa/Element/Element.hpp"
#include "fesa/Element/MITC4Stiffness.hpp"
#include "fesa/Material/MITC4PlaneStressMaterial.hpp"
#include "fesa/Material/Material.hpp"

#include <array>
#include <cmath>
#include <stdexcept>
#include <vector>

namespace {

void check(bool value, const char* message) {
  if (!value) {
    throw std::runtime_error(message);
  }
}

void checkNear(fesa::Real actual, fesa::Real expected, fesa::Real tolerance, const char* message) {
  if (std::fabs(actual - expected) > tolerance) {
    throw std::runtime_error(message);
  }
}

fesa::Real matrixEnergy(const fesa::DenseMatrix& matrix, const std::vector<fesa::Real>& values) {
  const auto product = matrix.multiply(values);
  fesa::Real energy = 0.0;
  for (std::size_t i = 0; i < values.size(); ++i) {
    energy += values[i] * product[i];
  }
  return energy;
}

}  // namespace

int main() {
  const auto law = fesa::mitc4PlaneStressMaterialMatrix(1000.0, 0.25);
  check(law.ok(), "valid MITC4 plane-stress material should remain accepted");
  check(fesa::mitc4StrainComponentLabels()[0] == "eps11", "MITC4 strain labels changed");
  const auto eps11 = fesa::strainComponentIndex(fesa::MITC4StrainComponent::Eps11);
  const auto eps22 = fesa::strainComponentIndex(fesa::MITC4StrainComponent::Eps22);
  const auto gamma13 = fesa::strainComponentIndex(fesa::MITC4StrainComponent::Gamma13);
  checkNear(law.matrix[eps11][eps11], 1066.6666666666667, 1.0e-10, "plane-stress D11 changed");
  checkNear(law.matrix[eps11][eps22], 266.6666666666667, 1.0e-10, "plane-stress D12 changed");
  checkNear(law.matrix[gamma13][gamma13], (5.0 / 6.0) * 400.0, 1.0e-10,
            "plane-stress shear correction changed");

  fesa::MITC4StrainVector strain{};
  strain[eps11] = 0.01;
  strain[eps22] = -0.02;
  strain[gamma13] = 0.03;
  const auto stress = fesa::multiplyMITC4MaterialMatrix(law.matrix, strain);
  check(fesa::dotMITC4Vector(strain, stress) > 0.0, "MITC4 material energy changed");

  const auto invalid = fesa::mitc4PlaneStressMaterialMatrix(-1.0, 0.25);
  check(!invalid.ok(), "invalid MITC4 material should remain diagnosed");
  check(fesa::containsDiagnostic(invalid.diagnostics, "FESA-MITC4-MATERIAL"),
        "MITC4 invalid material diagnostic changed");

  const auto points = fesa::mitc4GaussQuadrature2x2x2();
  check(points.size() == 8, "MITC4 integration point count changed");
  for (const auto& point : points) {
    checkNear(point.weight, 1.0, 1.0e-15, "MITC4 integration weight changed");
  }

  const std::array<fesa::Vec3, 4> coords = {{{0, 0, 0}, {1, 0, 0}, {1, 1, 0}, {0, 1, 0}}};
  const auto geometry = fesa::buildMITC4Geometry(coords, 0.2);
  check(geometry.ok(), "flat MITC4 geometry should remain valid");
  const auto integration = fesa::mitc4BuildMaterialIntegrationData(geometry, 1000.0, 0.25);
  check(integration.ok(), "MITC4 material integration data should remain valid");
  check(integration.samples.size() == 8, "MITC4 material integration sample count changed");
  for (const auto& sample : integration.samples) {
    check(sample.ok(), "MITC4 material integration sample should remain valid");
    check(sample.basis.ok(), "MITC4 material integration basis should remain valid");
  }

  const auto stiffness = fesa::mitc4ElementStiffness(coords, 1000.0, 0.25, 0.2);
  check(stiffness.ok(), "MITC4 stiffness should remain valid");
  check(stiffness.global.rows() == 24 && stiffness.global.cols() == 24, "MITC4 global stiffness size changed");
  check(stiffness.integration_point_count == 8, "MITC4 stiffness integration count changed");
  checkNear(stiffness.drilling_stiffness_scale, 1.0e-3, 1.0e-15, "MITC4 drilling scale changed");
  check(stiffness.drilling_reference_diagonal > 0.0, "MITC4 drilling reference diagonal changed");
  check(stiffness.drilling_stiffness > 0.0, "MITC4 drilling stiffness changed");
  for (fesa::LocalIndex i = 0; i < 24; ++i) {
    for (fesa::LocalIndex j = 0; j < 24; ++j) {
      checkNear(stiffness.global(i, j), stiffness.global(j, i), 1.0e-8, "MITC4 stiffness symmetry changed");
    }
  }

  std::vector<fesa::Real> displacement(24, 0.0);
  displacement[0] = 0.01;
  displacement[7] = -0.02;
  displacement[14] = 0.03;
  displacement[21] = 0.04;
  const auto internal_force = fesa::mitc4ElementInternalForce(stiffness, displacement);
  check(internal_force.size() == displacement.size(), "MITC4 internal force size changed");
  const auto expected_internal_force = stiffness.global.multiply(displacement);
  for (std::size_t i = 0; i < internal_force.size(); ++i) {
    checkNear(internal_force[i], expected_internal_force[i], 1.0e-12, "MITC4 internal force changed");
  }
  check(matrixEnergy(stiffness.global, displacement) > 0.0, "MITC4 stiffness energy changed");

  fesa::ElementStiffnessOptions options;
  options.drilling_stiffness_scale = 2.0e-3;
  const auto scaled = fesa::mitc4ElementStiffness(coords, 1000.0, 0.25, 0.2, options);
  check(scaled.ok(), "scaled MITC4 stiffness should remain valid");
  checkNear(scaled.drilling_reference_diagonal, stiffness.drilling_reference_diagonal, 1.0e-10,
            "MITC4 drilling reference changed with scale");
  checkNear(scaled.drilling_stiffness, 2.0 * stiffness.drilling_stiffness, 1.0e-10,
            "MITC4 drilling scale response changed");

  fesa::MITC4ElementKernel kernel;
  const auto kernel_stiffness = kernel.stiffness(coords, 1000.0, 0.25, 0.2);
  check(kernel_stiffness.rows() == 24 && kernel_stiffness.cols() == 24, "MITC4 kernel stiffness size changed");

  return 0;
}