#include "fesa/elements/mitc4_shell.hpp"

#include <gtest/gtest.h>

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

namespace {

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

fesa::Node node(std::int64_t label, Vector3 coordinates) {
    return {
        {"Shell-Instance", label, std::to_string(label)},
        coordinates,
        {"mitc4-shell.inp", static_cast<std::size_t>(label + 1)}};
}

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

fesa::ShellSection section(double thickness = 2.0) {
    return {"Section", thickness, 0U, {"mitc4-shell.inp", 20U}};
}

fesa::LinearElasticMaterial material(
    double youngsModulus = 120.0,
    double poissonRatio = 0.25) {
    return {
        "Material",
        youngsModulus,
        poissonRatio,
        {"mitc4-shell.inp", 21U}};
}

std::array<Vector3, 4> directors(Vector3 director = {0.0, 0.0, 1.0}) {
    return {director, director, director, director};
}

double dot(const Vector3& left, const Vector3& right) {
    return left[0] * right[0] + left[1] * right[1] + left[2] * right[2];
}

Vector3 cross(const Vector3& left, const Vector3& right) {
    return {
        left[1] * right[2] - left[2] * right[1],
        left[2] * right[0] - left[0] * right[2],
        left[0] * right[1] - left[1] * right[0]};
}

double norm(const Vector3& value) {
    return std::sqrt(dot(value, value));
}

void expectVectorNear(
    const Vector3& actual,
    const Vector3& expected,
    double tolerance = 1.0e-12) {
    for (std::size_t component = 0U; component < actual.size(); ++component) {
        EXPECT_NEAR(actual[component], expected[component], tolerance);
    }
}

void expectOrthonormalRightHanded(const fesa::Mitc4LocalFrame& frame) {
    EXPECT_NEAR(norm(frame.e1), 1.0, 1.0e-12);
    EXPECT_NEAR(norm(frame.e2), 1.0, 1.0e-12);
    EXPECT_NEAR(norm(frame.e3), 1.0, 1.0e-12);
    EXPECT_NEAR(dot(frame.e1, frame.e2), 0.0, 1.0e-12);
    EXPECT_NEAR(dot(frame.e1, frame.e3), 0.0, 1.0e-12);
    EXPECT_NEAR(dot(frame.e2, frame.e3), 0.0, 1.0e-12);
    expectVectorNear(cross(frame.e1, frame.e2), frame.e3);
}

void expectMatrixNear(
    const fesa::Matrix& actual,
    const fesa::Matrix& expected,
    double tolerance = 1.0e-12) {
    ASSERT_EQ(actual.rows(), expected.rows());
    ASSERT_EQ(actual.columns(), expected.columns());
    for (std::size_t row = 0U; row < actual.rows(); ++row) {
        for (std::size_t column = 0U; column < actual.columns(); ++column) {
            EXPECT_NEAR(actual(row, column), expected(row, column), tolerance)
                << "at (" << row << ", " << column << ")";
        }
    }
}

void expectSymmetric(const fesa::Matrix& matrix) {
    ASSERT_EQ(matrix.rows(), matrix.columns());
    for (std::size_t row = 0U; row < matrix.rows(); ++row) {
        for (std::size_t column = 0U; column < matrix.columns(); ++column) {
            EXPECT_NEAR(matrix(row, column), matrix(column, row), 1.0e-12);
        }
    }
}

bool hasPositiveCholeskyPivots(const fesa::Matrix& matrix) {
    if (matrix.rows() != matrix.columns()) {
        return false;
    }
    fesa::Matrix lower{matrix.rows(), matrix.columns()};
    for (std::size_t row = 0U; row < matrix.rows(); ++row) {
        for (std::size_t column = 0U; column <= row; ++column) {
            double value = matrix(row, column);
            for (std::size_t inner = 0U; inner < column; ++inner) {
                value -= lower(row, inner) * lower(column, inner);
            }
            if (row == column) {
                if (!std::isfinite(value) || !(value > 0.0)) {
                    return false;
                }
                lower(row, column) = std::sqrt(value);
            } else {
                lower(row, column) = value / lower(column, column);
            }
        }
    }
    return true;
}

std::array<fesa::Node, 4> planarNodes() {
    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})};
}

} // namespace

// MITC4-KIN-001
TEST(Mitc4ShellKinematics, ShapeFunctionsSatisfyNodalAndDerivativeIdentities) {
    constexpr std::array<Vector3, 4> naturalNodes{
        Vector3{-1.0, -1.0, 0.0},
        Vector3{1.0, -1.0, 0.0},
        Vector3{1.0, 1.0, 0.0},
        Vector3{-1.0, 1.0, 0.0}};

    for (std::size_t point = 0U; point < naturalNodes.size(); ++point) {
        const auto shape = fesa::Mitc4Shell::shapeFunctions(
            naturalNodes[point][0], naturalNodes[point][1]);
        for (std::size_t nodeIndex = 0U; nodeIndex < naturalNodes.size(); ++nodeIndex) {
            EXPECT_DOUBLE_EQ(shape.values[nodeIndex], point == nodeIndex ? 1.0 : 0.0);
        }
    }

    const auto shape = fesa::Mitc4Shell::shapeFunctions(0.25, -0.5);
    double valueSum = 0.0;
    double xiDerivativeSum = 0.0;
    double etaDerivativeSum = 0.0;
    for (std::size_t nodeIndex = 0U; nodeIndex < 4U; ++nodeIndex) {
        valueSum += shape.values[nodeIndex];
        xiDerivativeSum += shape.xiDerivatives[nodeIndex];
        etaDerivativeSum += shape.etaDerivatives[nodeIndex];
    }
    EXPECT_DOUBLE_EQ(valueSum, 1.0);
    EXPECT_DOUBLE_EQ(xiDerivativeSum, 0.0);
    EXPECT_DOUBLE_EQ(etaDerivativeSum, 0.0);
    EXPECT_EQ(
        shape.values,
        (std::array<double, 4>{0.28125, 0.46875, 0.15625, 0.09375}));
}

// MITC4-KIN-002
TEST(Mitc4ShellKinematics, BuildsRightHandedFramesAndSeparatePhysicalDrillingMaps) {
    const std::array<fesa::Node, 4> nodes{
        node(1, {0.0, -1.0, -1.0}),
        node(2, {0.0, 1.0, -1.0}),
        node(3, {0.0, 1.0, 1.0}),
        node(4, {0.0, -1.0, 1.0})};
    const auto candidate = fesa::Mitc4Shell::create(
        nodePointers(nodes), directors({1.0, 0.0, 0.0}), section(), material());
    ASSERT_TRUE(candidate.hasValue());
    const auto& shell = candidate.value();

    const auto frame = shell.localFrame(0.0, 0.0);
    expectVectorNear(frame.e1, {0.0, 1.0, 0.0});
    expectVectorNear(frame.e2, {0.0, 0.0, 1.0});
    expectVectorNear(frame.e3, {1.0, 0.0, 0.0});
    expectOrthonormalRightHanded(frame);

    const auto physical = shell.physicalTransformation20();
    const auto drilling = shell.drillingTransformation4();
    ASSERT_EQ(physical.rows(), 20U);
    ASSERT_EQ(physical.columns(), 24U);
    ASSERT_EQ(drilling.rows(), 4U);
    ASSERT_EQ(drilling.columns(), 24U);
    for (std::size_t nodeIndex = 0U; nodeIndex < 4U; ++nodeIndex) {
        const std::size_t physicalOffset = 5U * nodeIndex;
        const std::size_t globalOffset = 6U * nodeIndex;
        for (std::size_t component = 0U; component < 3U; ++component) {
            EXPECT_DOUBLE_EQ(
                physical(physicalOffset + component, globalOffset + component),
                1.0);
        }
        EXPECT_DOUBLE_EQ(physical(physicalOffset + 3U, globalOffset + 4U), 1.0);
        EXPECT_DOUBLE_EQ(physical(physicalOffset + 4U, globalOffset + 5U), 1.0);
        EXPECT_DOUBLE_EQ(drilling(nodeIndex, globalOffset + 3U), 1.0);

        for (std::size_t globalDof = 0U; globalDof < 24U; ++globalDof) {
            if (globalDof != globalOffset + 4U) {
                EXPECT_DOUBLE_EQ(physical(physicalOffset + 3U, globalDof), 0.0);
            }
            if (globalDof != globalOffset + 5U) {
                EXPECT_DOUBLE_EQ(physical(physicalOffset + 4U, globalDof), 0.0);
            }
            if (globalDof != globalOffset + 3U) {
                EXPECT_DOUBLE_EQ(drilling(nodeIndex, globalDof), 0.0);
            }
        }
    }

    auto invalidDirectors = directors({1.0, 0.0, 0.0});
    invalidDirectors[2] = {0.0, 0.0, 0.0};
    EXPECT_FALSE(fesa::Mitc4Shell::create(
                     nodePointers(nodes), invalidDirectors, section(), material())
                     .hasValue());
}

// MITC4-KIN-003
TEST(Mitc4ShellKinematics, FormsDirectColumnsAndAllCovariantTyingSamples) {
    const auto nodes = planarNodes();
    const auto candidate = fesa::Mitc4Shell::create(
        nodePointers(nodes), directors(), section(), material());
    ASSERT_TRUE(candidate.hasValue());
    const auto& shell = candidate.value();

    const auto direct = shell.directStrainDisplacement20(0.0, 0.0, 0.5);
    ASSERT_EQ(direct.rows(), 5U);
    ASSERT_EQ(direct.columns(), 20U);
    EXPECT_DOUBLE_EQ(direct(0U, 0U), -0.25);
    EXPECT_DOUBLE_EQ(direct(0U, 4U), -0.125);
    EXPECT_DOUBLE_EQ(direct(1U, 1U), -0.25);
    EXPECT_DOUBLE_EQ(direct(1U, 3U), 0.125);
    EXPECT_DOUBLE_EQ(direct(2U, 0U), -0.25);
    EXPECT_DOUBLE_EQ(direct(2U, 1U), -0.25);
    EXPECT_DOUBLE_EQ(direct(2U, 3U), 0.125);
    EXPECT_DOUBLE_EQ(direct(2U, 4U), -0.125);
    EXPECT_DOUBLE_EQ(direct(3U, 2U), -0.25);
    EXPECT_DOUBLE_EQ(direct(3U, 4U), 0.25);
    EXPECT_DOUBLE_EQ(direct(4U, 2U), -0.25);
    EXPECT_DOUBLE_EQ(direct(4U, 3U), -0.25);

    const auto samples = shell.covariantTyingShearSamples20();
    ASSERT_EQ(samples.rows(), 4U);
    ASSERT_EQ(samples.columns(), 20U);
    EXPECT_DOUBLE_EQ(samples(0U, 2U), -0.25);
    EXPECT_DOUBLE_EQ(samples(0U, 4U), 0.25);
    EXPECT_DOUBLE_EQ(samples(0U, 7U), 0.25);
    EXPECT_DOUBLE_EQ(samples(0U, 9U), 0.25);
    EXPECT_DOUBLE_EQ(samples(1U, 12U), 0.25);
    EXPECT_DOUBLE_EQ(samples(1U, 14U), 0.25);
    EXPECT_DOUBLE_EQ(samples(1U, 17U), -0.25);
    EXPECT_DOUBLE_EQ(samples(1U, 19U), 0.25);
    EXPECT_DOUBLE_EQ(samples(2U, 2U), -0.25);
    EXPECT_DOUBLE_EQ(samples(2U, 3U), -0.25);
    EXPECT_DOUBLE_EQ(samples(2U, 17U), 0.25);
    EXPECT_DOUBLE_EQ(samples(2U, 18U), -0.25);
    EXPECT_DOUBLE_EQ(samples(3U, 7U), -0.25);
    EXPECT_DOUBLE_EQ(samples(3U, 8U), -0.25);
    EXPECT_DOUBLE_EQ(samples(3U, 12U), 0.25);
    EXPECT_DOUBLE_EQ(samples(3U, 13U), -0.25);

    const auto weights = fesa::Mitc4Shell::tyingWeights(0.25, -0.5);
    EXPECT_EQ(weights.xiZeta, (std::array<double, 2>{0.75, 0.25}));
    EXPECT_EQ(weights.etaZeta, (std::array<double, 2>{0.375, 0.625}));

    const auto tied = shell.strainDisplacement20(0.0, 0.0, 0.0);
    EXPECT_DOUBLE_EQ(
        tied(3U, 4U),
        2.0 * (0.5 * samples(0U, 4U) + 0.5 * samples(1U, 4U)));
    EXPECT_DOUBLE_EQ(
        tied(4U, 3U),
        2.0 * (0.5 * samples(2U, 3U) + 0.5 * samples(3U, 3U)));
}

// MITC4-KIN-004
TEST(Mitc4ShellConstitutive, BuildsExactPositiveDefiniteSectionMatricesAndRescalesUnits) {
    const auto nodes = planarNodes();
    const auto candidate = fesa::Mitc4Shell::create(
        nodePointers(nodes), directors(), section(), material());
    ASSERT_TRUE(candidate.hasValue());
    const auto& shell = candidate.value();

    const auto cps = shell.planeStressConstitutive();
    const auto c5 = shell.materialConstitutive5();
    const auto a = shell.membraneSectionMatrix();
    const auto d = shell.bendingSectionMatrix();
    const auto as = shell.transverseShearSectionMatrix();
    EXPECT_EQ(cps.rows(), 3U);
    EXPECT_EQ(cps.columns(), 3U);
    EXPECT_EQ(c5.rows(), 5U);
    EXPECT_EQ(c5.columns(), 5U);
    EXPECT_EQ(a.rows(), 3U);
    EXPECT_EQ(d.rows(), 3U);
    EXPECT_EQ(as.rows(), 2U);
    EXPECT_DOUBLE_EQ(cps(0U, 0U), 128.0);
    EXPECT_DOUBLE_EQ(cps(0U, 1U), 32.0);
    EXPECT_DOUBLE_EQ(cps(2U, 2U), 48.0);
    EXPECT_DOUBLE_EQ(c5(3U, 3U), 40.0);
    EXPECT_DOUBLE_EQ(c5(4U, 4U), 40.0);
    EXPECT_DOUBLE_EQ(a(0U, 0U), 256.0);
    EXPECT_NEAR(d(0U, 0U), 256.0 / 3.0, 1.0e-12);
    EXPECT_DOUBLE_EQ(as(0U, 0U), 80.0);
    expectSymmetric(cps);
    expectSymmetric(c5);
    EXPECT_TRUE(hasPositiveCholeskyPivots(cps));
    EXPECT_TRUE(hasPositiveCholeskyPivots(c5));
    EXPECT_TRUE(hasPositiveCholeskyPivots(a));
    EXPECT_TRUE(hasPositiveCholeskyPivots(d));
    EXPECT_TRUE(hasPositiveCholeskyPivots(as));

    constexpr double forceScale = 7.0;
    constexpr double lengthScale = 3.0;
    const auto scaledCandidate = fesa::Mitc4Shell::create(
        nodePointers(nodes),
        directors(),
        section(2.0 * lengthScale),
        material(120.0 * forceScale / (lengthScale * lengthScale), 0.25));
    ASSERT_TRUE(scaledCandidate.hasValue());
    const auto& scaled = scaledCandidate.value();
    fesa::Matrix expectedCps{3U, 3U};
    fesa::Matrix expectedC5{5U, 5U};
    fesa::Matrix expectedA{3U, 3U};
    fesa::Matrix expectedD{3U, 3U};
    fesa::Matrix expectedAs{2U, 2U};
    for (std::size_t row = 0U; row < 3U; ++row) {
        for (std::size_t column = 0U; column < 3U; ++column) {
            expectedCps(row, column) =
                cps(row, column) * forceScale / (lengthScale * lengthScale);
            expectedA(row, column) = a(row, column) * forceScale / lengthScale;
            expectedD(row, column) = d(row, column) * forceScale * lengthScale;
        }
    }
    for (std::size_t row = 0U; row < 2U; ++row) {
        for (std::size_t column = 0U; column < 2U; ++column) {
            expectedAs(row, column) = as(row, column) * forceScale / lengthScale;
        }
    }
    for (std::size_t row = 0U; row < 5U; ++row) {
        for (std::size_t column = 0U; column < 5U; ++column) {
            expectedC5(row, column) =
                c5(row, column) * forceScale / (lengthScale * lengthScale);
        }
    }
    expectMatrixNear(scaled.planeStressConstitutive(), expectedCps);
    expectMatrixNear(scaled.materialConstitutive5(), expectedC5);
    expectMatrixNear(scaled.membraneSectionMatrix(), expectedA);
    expectMatrixNear(scaled.bendingSectionMatrix(), expectedD);
    expectMatrixNear(scaled.transverseShearSectionMatrix(), expectedAs);

    EXPECT_FALSE(fesa::Mitc4Shell::create(
                     nodePointers(nodes), directors(), section(0.0), material())
                     .hasValue());
    EXPECT_FALSE(fesa::Mitc4Shell::create(
                     nodePointers(nodes), directors(), section(), material(0.0, 0.25))
                     .hasValue());
    EXPECT_FALSE(fesa::Mitc4Shell::create(
                     nodePointers(nodes), directors(), section(), material(120.0, 0.5))
                     .hasValue());
}

// MITC4-KIN-005
TEST(Mitc4ShellKinematics, UsesOneFixedTwoByTwoByTwoQuadratureOrder) {
    const auto& points = fesa::Mitc4Shell::volumeQuadrature();
    ASSERT_EQ(points.size(), 8U);
    const double g = 1.0 / std::sqrt(3.0);
    const std::array<Vector3, 8> expected{
        Vector3{-g, -g, -g}, Vector3{-g, -g, g},
        Vector3{g, -g, -g}, Vector3{g, -g, g},
        Vector3{g, g, -g}, Vector3{g, g, g},
        Vector3{-g, g, -g}, Vector3{-g, g, g}};
    for (std::size_t point = 0U; point < points.size(); ++point) {
        EXPECT_EQ(points[point].naturalCoordinates, expected[point]);
        EXPECT_DOUBLE_EQ(points[point].weight, 1.0);
    }
}