# Amira Abdel-Rahman
# (c) Massachusetts Institute of Technology 2020

#####################################FEA KE####################################################################

function lk()
    E=1
    nu=0.3
    k=[1/2-nu/6,1/8+nu/8,-1/4-nu/12,-1/8+3*nu/8,-1/4+nu/12,-1/8-nu/8,nu/6,1/8-3*nu/8]
    KE = E/(1-nu^2)*[  k[0+1] k[1+1] k[2+1] k[3+1] k[4+1] k[5+1] k[6+1] k[7+1];
                       k[1+1] k[0+1] k[7+1] k[6+1] k[5+1] k[4+1] k[3+1] k[2+1];
                       k[2+1] k[7+1] k[0+1] k[5+1] k[6+1] k[3+1] k[4+1] k[1+1];
                       k[3+1] k[6+1] k[5+1] k[0+1] k[7+1] k[2+1] k[1+1] k[4+1];
                       k[4+1] k[5+1] k[6+1] k[7+1] k[0+1] k[1+1] k[2+1] k[3+1];
                       k[5+1] k[4+1] k[3+1] k[2+1] k[1+1] k[0+1] k[7+1] k[6+1];
                       k[6+1] k[3+1] k[4+1] k[1+1] k[2+1] k[7+1] k[0+1] k[5+1];
                       k[7+1] k[2+1] k[1+1] k[4+1] k[3+1] k[6+1] k[5+1] k[0+1] ];
    return (KE)
end

function lk_H8(nu)
    A = [32 6 -8 6 -6 4 3 -6 -10 3 -3 -3 -4 -8;
        -48 0 0 -24 24 0 0 0 12 -12 0 12 12 12];
    k = 1/144*A'*[1; nu];

    K1 = [k[1]   k[2]   k[2]   k[3]  k[5]  k[5];
            k[2]   k[1]   k[2]   k[4]  k[6]  k[7];
            k[2]   k[2]   k[1]   k[4]  k[7]  k[6];
            k[3]   k[4]   k[4]   k[1]  k[8]  k[8];
            k[5]   k[6]   k[7]   k[8]  k[1]  k[2];
            k[5]   k[7]   k[6]   k[8]  k[2]  k[1]];
    K2 = [k[9]   k[8]   k[12]  k[6]  k[4]  k[7];
            k[8]   k[9]   k[12]  k[5]  k[3]  k[5];
            k[10]  k[10]  k[13]  k[7]  k[4]  k[6];
            k[6]   k[5]   k[11]  k[9]  k[2]  k[10];
            k[4]   k[3]   k[5]   k[2]  k[9]  k[12]
            k[11]  k[4]   k[6]   k[12] k[10] k[13]];
    K3 = [k[6]   k[7]   k[4]   k[9]  k[12] k[8];
            k[7]   k[6]   k[4]   k[10] k[13] k[10];
            k[5]   k[5]   k[3]   k[8]  k[12] k[9];
            k[9]   k[10]  k[2]   k[6]  k[11] k[5];
            k[12]  k[13]  k[10]  k[11] k[6]  k[4];
            k[2]   k[12]  k[9]   k[4]  k[5]  k[3]];
    K4 = [k[14]  k[11]  k[11]  k[13] k[10] k[10];
            k[11]  k[14]  k[11]  k[12] k[9]  k[8];
            k[11]  k[11]  k[14]  k[12] k[8]  k[9];
            k[13]  k[12]  k[12]  k[14] k[7]  k[7];
            k[10]  k[9]   k[8]   k[7]  k[14] k[11];
            k[10]  k[8]   k[9]   k[7]  k[11] k[14]];
    K5 = [k[1]   k[2]   k[8]   k[3]  k[5]  k[4];
            k[2]   k[1]   k[8]   k[4]  k[6]  k[11];
            k[8]   k[8]   k[1]   k[5]  k[11] k[6];
            k[3]   k[4]   k[5]   k[1]  k[8]  k[2];
            k[5]   k[6]   k[11]  k[8]  k[1]  k[8];
            k[4]   k[11]  k[6]   k[2]  k[8]  k[1]];
    K6 = [k[14]  k[11]  k[7]   k[13] k[10] k[12];
            k[11]  k[14]  k[7]   k[12] k[9]  k[2];
            k[7]   k[7]   k[14]  k[10] k[2]  k[9];
            k[13]  k[12]  k[10]  k[14] k[7]  k[11];
            k[10]  k[9]   k[2]   k[7]  k[14] k[7];
            k[12]  k[2]   k[9]   k[11] k[7]  k[14]];
    KE = 1/((nu+1)*(1-2*nu))*[ K1  K2  K3  K4;K2'  K5  K6  K3';K3' K6  K5' K2';K4  K3  K2  K1'];

    return KE
end

## SUB FUNCTION: elementMatVec2D
function elementMatVec2D(a, b, DH)
    GaussNodes = [-1/sqrt(3); 1/sqrt(3)]; 
    GaussWeigh = [1 1];
    L = [1 0 0 0; 0 0 0 1; 0 1 1 0];
    Ke = zeros(8,8);
    for i = 1:2
        for j = 1:2
            GN_x = GaussNodes[i]; 
            GN_y = GaussNodes[j];

            dN_x = 1/4*[-(1-GN_x)  (1-GN_x) (1+GN_x) -(1+GN_x)];
            dN_y = 1/4*[-(1-GN_y) -(1+GN_y) (1+GN_y)  (1-GN_y)];

            J = [dN_x; dN_y]*[ -a  a  a  -a;  -b  -b  b  b]';
            G = [inv(J) zeros(size(J)); zeros(size(J)) inv(J)];
            dN=zeros(4,8)
            dN[1,1:2:8] = dN_x; 
            dN[2,1:2:8] = dN_y;
            dN[3,2:2:8] = dN_x; 
            dN[4,2:2:8] = dN_y;
            Be = L*G*dN;
            Ke = Ke + GaussWeigh[i]*GaussWeigh[j]*det(J)*Be'*DH*Be;
        end
    end
    return Ke
end
    
## SUB FUNCTION: elementMatVec3D
function elementMatVec3D(a, b, c, DH)
    GN_x=[-1/sqrt(3),1/sqrt(3)]; GN_y=GN_x; GN_z=GN_x; GaussWeigh=[1,1];
    Ke = zeros(24,24); L = zeros(6,9);
    L[1,1] = 1; L[2,5] = 1; L[3,9] = 1;
    L[4,2] = 1; L[4,4] = 1; L[5,6] = 1;
    L[5,8] = 1; L[6,3] = 1; L[6,7] = 1;
    # display(L)
    for ii=1:length(GN_x)
        for jj=1:length(GN_y)
            for kk=1:length(GN_z)
                x = GN_x[ii];y = GN_y[jj];z = GN_z[kk];
                
                dNx = 1/8*[-(1-y)*(1-z)  (1-y)*(1-z)  (1+y)*(1-z) -(1+y)*(1-z) -(1-y)*(1+z)  (1-y)*(1+z)  (1+y)*(1+z) -(1+y)*(1+z)];
                dNy = 1/8*[-(1-x)*(1-z) -(1+x)*(1-z)  (1+x)*(1-z)  (1-x)*(1-z) -(1-x)*(1+z) -(1+x)*(1+z)  (1+x)*(1+z)  (1-x)*(1+z)];
                dNz = 1/8*[-(1-x)*(1-y) -(1+x)*(1-y) -(1+x)*(1+y) -(1-x)*(1+y)  (1-x)*(1-y)  (1+x)*(1-y)  (1+x)*(1+y)  (1-x)*(1+y)];
                
                J = [dNx;dNy;dNz]*[ -a  a  a  -a  -a  a  a  -a ;  -b  -b  b  b  -b  -b  b  b; -c -c -c -c  c  c  c  c]';
                # display(a)
                # display(b)
                # display(c)
                # display(dNx)
                # display(dNy)
                # display(dNz)
                # display(J)
                
                G = [inv(J) zeros(3,3) zeros(3,3);zeros(3,3) inv(J) zeros(3,3);zeros(3,3) zeros(3,3) inv(J)];
                dN=zeros(9,24)
                dN[1,1:3:24] = dNx; dN[2,1:3:24] = dNy; dN[3,1:3:24] = dNz;
                dN[4,2:3:24] = dNx; dN[5,2:3:24] = dNy; dN[6,2:3:24] = dNz;
                dN[7,3:3:24] = dNx; dN[8,3:3:24] = dNy; dN[9,3:3:24] = dNz;
                
                
                Be = L*G*dN;
                # display((G))
                # display(size(dN))
                # display((Be))



                Ke .= Ke .+ GaussWeigh[ii]*GaussWeigh[jj]*GaussWeigh[kk]*det(J)*(Be'*DH*Be);
            end
        end
    end
    # display(Ke)

    return Ke
end

function evaluateCH(CH,dens)

    U,S,V = svd(CH);
    sigma = S;
    k = sum(sigma .> 1e-15);
    SH = (U[:,1:k] * diagm(0=>(1 ./sigma[1:k])) * V[:,1:k]')'; # more stable SVD (pseudo)inverse
    EH = [1/SH[1,1], 1/SH[2,2], 1/SH[3,3]]; # effective Young's modulus
    GH = [1/SH[4,4], 1/SH[5,5], 1/SH[6,6]]; # effective shear modulus
    vH = [-SH[2,1]/SH[1,1]  -SH[3,1]/SH[1,1]  -SH[3,2]/SH[2,2];
         -SH[1,2]/SH[2,2]  -SH[1,3]/SH[3,3]  -SH[2,3]/SH[3,3]]; # effective Poisson's ratio
        
    props = Dict("CH"=>CH, "SH"=>SH, "EH"=>EH, "GH"=>GH, "vH"=>vH, "density"=>dens);
   
        
    if true
        println("\n--------------------------EFFECTIVE PROPERTIES--------------------------\n")
        println("Youngs Modulus:____E11_____|____E22_____|____E33_____\n")
        println("               $(EH[1]) | $(EH[2]) | $(EH[3])\n\n")
        println("Shear Modulus:_____G23_____|____G31_____|____G12_____\n")
        println("               $(GH[1]) | $(GH[2]) | $(GH[3])\n\n")
        println("Poissons Ratio:____v12_____|____v13_____|____v23_____\n")
        println("               $(vH[1,1]) | $(vH[1,2]) | $(vH[1,3])\n\n")
        println("               ____v21_____|____v31_____|____v32_____\n")
        println("               $(vH[2,1]) | $(vH[2,2]) | $(vH[2,3])\n\n")
        println("------------------------------------------------------------------------")
    end
        
        
    return SH
end