//
// Created by Wei Chen on 3/2/22
//

#include <iostream>
#include <spdlog/spdlog.h>

#include "Material.hpp"

namespace ssim {

    void Material::computeN_tet(const Eigen::RowVector3d &P, const Eigen::Matrix<double, 4, 3> &X,
                                Eigen::Matrix<double, 3, 12> &N) {

        Eigen::Matrix<double, 4, 4> H;
        H.col(0).setOnes();
        H.rightCols<3>() = X;
        double V6 = H.determinant();

        Eigen::Matrix<int, 4, 3> index;
        index << 1, 2, 3,
                0, 2, 3,
                0, 1, 3,
                0, 1, 2;

        Eigen::RowVector4d a;
        Eigen::RowVector4d b;
        Eigen::RowVector4d c;
        Eigen::RowVector4d d;

        auto SubMatrix = [](const Eigen::Matrix4d &original,
                            const Eigen::Vector3i &rowIdx,
                            const Eigen::Vector3i &colIdx) {
            Eigen::Matrix3d ret;
            for (int i = 0; i < 3; ++i) {
                int rowI = rowIdx(i);
                for (int j = 0; j < 3; ++j) {
                    int colI = colIdx(j);
                    ret(i, j) = original(rowI, colI);
                }
            }
            return ret;
        };

        a(0) = SubMatrix(H, index.row(0), index.row(0)).determinant();
        a(1) = -SubMatrix(H, index.row(1), index.row(0)).determinant();
        a(2) = SubMatrix(H, index.row(2), index.row(0)).determinant();
        a(3) = -SubMatrix(H, index.row(3), index.row(0)).determinant();

        b(0) = -SubMatrix(H, index.row(0), index.row(1)).determinant();
        b(1) = SubMatrix(H, index.row(1), index.row(1)).determinant();
        b(2) = -SubMatrix(H, index.row(2), index.row(1)).determinant();
        b(3) = SubMatrix(H, index.row(3), index.row(1)).determinant();

        c(0) = SubMatrix(H, index.row(0), index.row(2)).determinant();
        c(1) = -SubMatrix(H, index.row(1), index.row(2)).determinant();
        c(2) = SubMatrix(H, index.row(2), index.row(2)).determinant();
        c(3) = -SubMatrix(H, index.row(3), index.row(2)).determinant();

        d(0) = -SubMatrix(H, index.row(0), index.row(3)).determinant();
        d(1) = SubMatrix(H, index.row(1), index.row(3)).determinant();
        d(2) = -SubMatrix(H, index.row(2), index.row(3)).determinant();
        d(3) = SubMatrix(H, index.row(3), index.row(3)).determinant();

        Eigen::RowVector4d NN;
        NN(0) = (a(0) + b(0) * P(0) + c(0) * P(1) + d(0) * P(2)) / V6;
        NN(1) = (a(1) + b(1) * P(0) + c(1) * P(1) + d(1) * P(2)) / V6;
        NN(2) = (a(2) + b(2) * P(0) + c(2) * P(1) + d(2) * P(2)) / V6;
        NN(3) = (a(3) + b(3) * P(0) + c(3) * P(1) + d(3) * P(2)) / V6;

        N.setZero();
        for (int i = 0; i < 4; ++i) {
            N(0, i * 3) = NN(i);
            N(1, i * 3 + 1) = NN(i);
            N(2, i * 3 + 2) = NN(i);
        }

    }

    void Material::computeB_tet(const Eigen::Matrix<double, 4, 3> &X, Eigen::Matrix<double, 6, 12> &B) {

        Eigen::Matrix<double, 4, 4> H;
        H.col(0).setOnes();
        H.rightCols<3>() = X;
        double V6 = H.determinant();

        Eigen::Matrix<int, 4, 3> index;
        index << 1, 2, 3,
                0, 2, 3,
                0, 1, 3,
                0, 1, 2;

        Eigen::RowVector4d b;
        Eigen::RowVector4d c;
        Eigen::RowVector4d d;

        auto SubMatrix = [](const Eigen::Matrix4d &original,
                            const Eigen::Vector3i &rowIdx,
                            const Eigen::Vector3i &colIdx) {
            Eigen::Matrix3d ret;
            for (int i = 0; i < 3; ++i) {
                int rowI = rowIdx(i);
                for (int j = 0; j < 3; ++j) {
                    int colI = colIdx(j);
                    ret(i, j) = original(rowI, colI);
                }
            }
            return ret;
        };

        b(0) = -SubMatrix(H, index.row(0), index.row(1)).determinant();
        b(1) = SubMatrix(H, index.row(1), index.row(1)).determinant();
        b(2) = -SubMatrix(H, index.row(2), index.row(1)).determinant();
        b(3) = SubMatrix(H, index.row(3), index.row(1)).determinant();

        c(0) = SubMatrix(H, index.row(0), index.row(2)).determinant();
        c(1) = -SubMatrix(H, index.row(1), index.row(2)).determinant();
        c(2) = SubMatrix(H, index.row(2), index.row(2)).determinant();
        c(3) = -SubMatrix(H, index.row(3), index.row(2)).determinant();

        d(0) = -SubMatrix(H, index.row(0), index.row(3)).determinant();
        d(1) = SubMatrix(H, index.row(1), index.row(3)).determinant();
        d(2) = -SubMatrix(H, index.row(2), index.row(3)).determinant();
        d(3) = SubMatrix(H, index.row(3), index.row(3)).determinant();

        B.setZero();
        for (int i = 0; i < 4; ++i) {
            B(0, 3 * i) = b(i);
            B(1, 3 * i + 1) = c(i);
            B(2, 3 * i + 2) = d(i);
            B(3, 3 * i) = c(i);
            B(3, 3 * i + 1) = b(i);
            B(4, 3 * i + 1) = d(i);
            B(4, 3 * i + 2) = c(i);
            B(5, 3 * i) = d(i);
            B(5, 3 * i + 2) = b(i);
        }
        B /= V6;

    }


    void Material::computeKe_tet(const Eigen::Matrix<double, 4, 3> &X, const Eigen::Matrix<double, 6, 6> &D,
                                 Eigen::Matrix<double, 12, 12> &Ke, double &Vol) {

        Eigen::Matrix<double, 4, 4> H;
        H.col(0).setOnes();
        H.rightCols<3>() = X;
        double V6 = H.determinant();
        Vol = V6 / 6.0;

        Eigen::Matrix<int, 4, 3> index;
        index << 1, 2, 3,
                0, 2, 3,
                0, 1, 3,
                0, 1, 2;

        Eigen::RowVector4d b;
        Eigen::RowVector4d c;
        Eigen::RowVector4d d;

        auto SubMatrix = [](const Eigen::Matrix4d &original,
                            const Eigen::Vector3i &rowIdx,
                            const Eigen::Vector3i &colIdx) {
            Eigen::Matrix3d ret;
            for (int i = 0; i < 3; ++i) {
                int rowI = rowIdx(i);
                for (int j = 0; j < 3; ++j) {
                    int colI = colIdx(j);
                    ret(i, j) = original(rowI, colI);
                }
            }
            return ret;
        };

        b(0) = -SubMatrix(H, index.row(0), index.row(1)).determinant();
        b(1) = SubMatrix(H, index.row(1), index.row(1)).determinant();
        b(2) = -SubMatrix(H, index.row(2), index.row(1)).determinant();
        b(3) = SubMatrix(H, index.row(3), index.row(1)).determinant();

        c(0) = SubMatrix(H, index.row(0), index.row(2)).determinant();
        c(1) = -SubMatrix(H, index.row(1), index.row(2)).determinant();
        c(2) = SubMatrix(H, index.row(2), index.row(2)).determinant();
        c(3) = -SubMatrix(H, index.row(3), index.row(2)).determinant();

        d(0) = -SubMatrix(H, index.row(0), index.row(3)).determinant();
        d(1) = SubMatrix(H, index.row(1), index.row(3)).determinant();
        d(2) = -SubMatrix(H, index.row(2), index.row(3)).determinant();
        d(3) = SubMatrix(H, index.row(3), index.row(3)).determinant();

        Eigen::Matrix<double, 6, 12> B;
        B.setZero();
        for (int i = 0; i < 4; ++i) {
            B(0, 3 * i) = b(i);
            B(1, 3 * i + 1) = c(i);
            B(2, 3 * i + 2) = d(i);
            B(3, 3 * i) = c(i);
            B(3, 3 * i + 1) = b(i);
            B(4, 3 * i + 1) = d(i);
            B(4, 3 * i + 2) = c(i);
            B(5, 3 * i) = d(i);
            B(5, 3 * i + 2) = b(i);
        }
        B /= V6;

        Ke = Vol * (B.transpose() * D * B);
    }

} // namespace SIM