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


#####################################Utils####################################################################
######### ELEMENT AND NODE NUMBERING IN 3D MESH 
function get_num(nx, ny, nz, i, j, k)
    num = (nx*ny)*(k-1) + nx*(j-1) + i;
    return num
end

######### GLOBAL DOFS FOR A GIVEN ELEMENT 
function  get_elem_dofs(nnx, nny, nnz, elx, ely, elz)
    n = get_num(nnx, nny, nnz, elx, ely, elz);
    N = [n; n+1; n+nnx+1; n+nnx; n+nnx*nny; n+nnx*nny+1; n+nnx*nny+nnx+1; n+nnx*nny+nnx];
    dofs = zeros(Int,24); 
    for j = 1:8; 
        for i = 1:3; 
            dofs[3*(j-1)+i] = Int(3*(N[j]-1)+i); 
        end
    end
    return dofs
end

function getDisplacement(xPhys,nelx,nely,nelz,getProblem)
    
    
    # Max and min stiffness
    Emin=1e-9
    Emax=1.0
    nu=0.3
    # dofs:
    ndof = 3*(nelx+1)*(nely+1)*(nelz+1)
    # Allocate design variables (as array), initialize and allocate sens.
    x=volfrac * ones(Float64,nely,nelx,nelz)
    xold=copy(x)
    
    
    g=0 # must be initialized to use the NGuyen/Paulino OC approach
    dc=zeros(Float64,(nely,nelx,nelz))
    
    # FE: Build the index vectors for the for coo matrix format.
    KE=lk_H8(nu)
    nele = nelx*nely*nelz
    
    nodenrs = reshape(1:(1+nelx)*(1+nely)*(1+nelz),1+nely,1+nelx,1+nelz)
    edofVec = reshape(3*nodenrs[1:end-1,1:end-1,1:end-1].+1,nelx*nely*nelz,1)
    edofMat = repeat(edofVec,1,24).+repeat([0 1 2 3*nely.+[3 4 5 0 1 2] -3 -2 -1 3*(nely+1)*(nelx+1).+[0 1 2 3*nely.+[3 4 5 0 1 2] -3 -2 -1]],nelx*nely*nelz,1)
        
    iK = convert(Array{Int},reshape(kron(edofMat,ones(24,1))',24*24*nele,1))
    jK = convert(Array{Int},reshape(kron(edofMat,ones(1,24))',24*24*nele,1))
    

    # # USER-DEFINED LOAD DOFs
    # il = [0 nelx]; jl = [nely nely]; kl = [0 0];
    # loadnid = kl .*(nelx+1) .*(nely+1) .+il .*(nely .+1) .+(nely+1 .-jl);
    # loaddof = 3 .*loadnid[:] .- 2;
    # din = loaddof[1]; dout = loaddof[2];
    # F = sparse(loaddof,[1;2],[1;-1],ndof,2);
    
    # # USER-DEFINED SUPPORT FIXED DOFs
    # # Top face
    # m = Matlab.meshgrid(0:nelx,0:nelz)
    # iif=m[1]
    # kf=m[2]
    # fixednid = kf .*(nelx+1) .*(nely+1) .+iif .*(nely+1) .+1;
    # fixeddof_t = 3 .*fixednid[:] .-1;
    # # Side face
    # m = Matlab.meshgrid(0:nelx,0:nely)
    # iif=m[1]
    # jf=m[2]
    # fixednid = iif .*(nely+1) .+(nely+1 .-jf);
    # fixeddof_s = 3*fixednid[:];
    # # Two pins
    # iif = [0;0]; jf = [0;1]; kf = [nelz;nelz];
    # fixednid = kf .*(nelx+1) .*(nely .+1) .+iif .*(nely .+1) .+(nely+1 .-jf)
    # fixeddof_p = [3 .*fixednid[:]; 3 .*fixednid[:].-1; 3 .*fixednid[:].-2] # DOFs
    # # Fixed DOFs
    # fixeddof = union(fixeddof_t,fixeddof_s);
    # fixeddof = union(fixeddof, fixeddof_p);
    # fixeddof = sort(fixeddof);
    
    # # Displacement vector
    # U = zeros(ndof,2);
    # freedofs = setdiff(1:ndof,fixeddof)

    U,F,freedofs,din,dout=getProblem(nelx,nely,nelz)

    iH=[]
    jH=[]
    sH=[]
    k = 0

    for k1 = 1:nelz
        for i1 = 1:nelx
            for j1 = 1:nely
                e1 = (k1-1)*nelx*nely + (i1-1)*nely+j1
                for k2 = max(k1-(ceil(rmin)-1),1):min(k1+(ceil(rmin)-1),nelz)
                    for i2 = max(i1-(ceil(rmin)-1),1):min(i1+(ceil(rmin)-1),nelx)
                        for j2 = max(j1-(ceil(rmin)-1),1):min(j1+(ceil(rmin)-1),nely)
                            e2 = (k2-1)*nelx*nely + (i2-1)*nely+j2;
                            k = k+1;
                            append!(iH, e1  )
                            append!(jH, e2  )
                            append!(sH, max(0.0,rmin-sqrt((i1-i2)^2+(j1-j2)^2+(k1-k2)^2) ))
                            
                        end
                    end
                end
            end
        end
    end

    iH=reshape(iH,length(iH),1)
    jH=reshape(jH,length(jH),1)
    sH=reshape(convert(Array{Float64}, sH),length(sH),1)
    H = sparse(vec(iH),vec(jH),vec(sH))
    Hs = sum(H,dims=2)
    ###################################################


    # FE-ANALYSIS
    sK = reshape(KE[:]*(Emin.+xPhys[:]'.^penal*(Emax-Emin)),24*24*nelx*nely*nelz,1)
    K = sparse(vec(iK),vec(jK),vec(sK)) 
    K[din,din] = K[din,din] + 0.1
    K[dout,dout] = K[dout,dout] + 0.1
    K = (K+K')/2


    @timed U[freedofs,:] = K[freedofs,freedofs] \ Array(F[freedofs,:])

    return U,ndof,freedofs
end

function getIndex(i,j,nelx,nely)
    return (i-1)*(nely+1)+(j-1)+1
end

# get index for 3d compliant
function getIndex3d(i,j,k,nelx,nely,nelz)
    return Int((nelx*nely)*(k-1) + nely*(i-1) + j);
end

# get index for 3d compliant
function getIndex3d1(i,j,k,dof,nelx,nely,nelz,ndof)
    return Int((ndof*nelx*nely)*(k-1) + ndof*nely*(i-1) + ndof*(j-1)+dof);
end

function getIndex3d(i,j,k,dof,nelx,nely,nelz,ndof)
    return Int.((ndof.*nelx.*nely).*(k.-1.0) .+ ndof.*nely.*(i.-1.0) .+ ndof.*(j.-1.0).+dof);
end