//
// 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(Eigen::all, Eigen::seq(1, Eigen::last)) = 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;

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

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

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

    d(0) = -H(index.row(0), index.row(3)).determinant();
    d(1) = H(index.row(1), index.row(3)).determinant();
    d(2) = -H(index.row(2), index.row(3)).determinant();
    d(3) = 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();
    N(0, Eigen::seq(0, Eigen::last, 3)) = NN;
    N(1, Eigen::seq(1, Eigen::last, 3)) = NN;
    N(2, Eigen::seq(2, Eigen::last, 3)) = NN;

}

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(Eigen::all, Eigen::seq(1, Eigen::last)) = 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;

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

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

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

    B.setZero();
    B(0, Eigen::seq(0, Eigen::last, 3)) = b;
    B(1, Eigen::seq(1, Eigen::last, 3)) = c;
    B(2, Eigen::seq(2, Eigen::last, 3)) = d;
    B(3, Eigen::seq(0, Eigen::last, 3)) = c;
    B(3, Eigen::seq(1, Eigen::last, 3)) = b;
    B(4, Eigen::seq(1, Eigen::last, 3)) = d;
    B(4, Eigen::seq(2, Eigen::last, 3)) = c;
    B(5, Eigen::seq(0, Eigen::last, 3)) = d;
    B(5, Eigen::seq(2, Eigen::last, 3)) = b;
    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(Eigen::all, Eigen::seq(1, Eigen::last)) = 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;

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

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

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

    Eigen::Matrix<double, 6, 12> B;
    B.setZero();
    B(0, Eigen::seq(0, Eigen::last, 3)) = b;
    B(1, Eigen::seq(1, Eigen::last, 3)) = c;
    B(2, Eigen::seq(2, Eigen::last, 3)) = d;
    B(3, Eigen::seq(0, Eigen::last, 3)) = c;
    B(3, Eigen::seq(1, Eigen::last, 3)) = b;
    B(4, Eigen::seq(1, Eigen::last, 3)) = d;
    B(4, Eigen::seq(2, Eigen::last, 3)) = c;
    B(5, Eigen::seq(0, Eigen::last, 3)) = d;
    B(5, Eigen::seq(2, Eigen::last, 3)) = b;
    B /= V6;

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

} // namespace SIM