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

# Topology Optimization Implementation in Julia from various sources
# based on https://paulino.ce.gatech.edu/conferences/papers/11pereira_anefficientandcompact.pdf and http://www.topopt.mek.dtu.dk/apps-and-software and https://github.com/blademwang11/Topopt/blob/master/top.jl

#######################################2d##################################################################
function topologyOptimization(nelx,nely,volfrac,rmin,penal)
    anim=Animation()
    
    display("Minimum compliance problem with OC")
    display("ndes: $nelx x $nely")
    display("volfrac: $volfrac rmin: $rmin penal: $penal")
    # Max and min stiffness
    Emin=1e-9
    Emax=1.0
    # dofs:
    ndof = 2*(nelx+1)*(nely+1)
    # Allocate design variables (as array), initialize and allocate sens.
    x=volfrac * ones(Float64,nely,nelx)
    xold=copy(x)
    xPhys=copy(x)
    g=0 # must be initialized to use the NGuyen/Paulino OC approach
    dc=zeros(Float64,(nely,nelx))
    
    # FE: Build the index vectors for the for coo matrix format.
    KE=lk()
    nodenrs = reshape(1:(1+nelx)*(1+nely),1+nely,1+nelx)
    edofVec = reshape(2*nodenrs[1:end-1,1:end-1].+1,nelx*nely,1)
    edofMat = repeat(edofVec,1,8).+repeat([0 1 2*nely.+[2 3 0 1] -2 -1],nelx*nely,1)
    iK = convert(Array{Int},reshape(kron(edofMat,ones(8,1))',64*nelx*nely,1))
    jK = convert(Array{Int},reshape(kron(edofMat,ones(1,8))',64*nelx*nely,1))
    # DEFINE LOADS AND SUPPORTS (HALF MBB-BEAM)
    F = sparse([2],[1],[-1.0],2*(nely+1)*(nelx+1),1)
    U = zeros(2*(nely+1)*(nelx+1),1)
    fixeddofs = union(1:2:2*(nely+1),2*(nelx+1)*(nely+1))
    alldofs = 1:(2*(nely+1)*(nelx+1))
    freedofs = setdiff(alldofs,fixeddofs)
    # Prepare filter
    iH = ones(convert(Int,nelx*nely*(2*(ceil(rmin)-1)+1)^2),1)
    jH = ones(Int,size(iH))
    sH = zeros(size(iH))
    k = 0;
    for i1 = 1:nelx
        for j1 = 1:nely
            e1 = (i1-1)*nely+j1
            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 = (i2-1)*nely+j2
                    k = k+1
                    iH[k] = e1
                    jH[k] = e2
                    sH[k] = max(0,rmin-sqrt((i1-i2)^2+(j1-j2)^2))
                end
            end
        end
    end
    H = sparse(vec(iH),vec(jH),vec(sH))
    Hs = sum(H,dims=2)
    ###################################################
    loop = 0
    change = 1
    maxIter=1000
    # Start iteration
    for i =1:maxIter
        if (change > 0.01)
            # Start iteration
            loop += 1
            # FE-ANALYSIS
            sK = reshape(KE[:]*(Emin.+xPhys[:]'.^penal*(Emax-Emin)),64*nelx*nely,1)
            K = sparse(vec(iK),vec(jK),vec(sK)); K = (K+K')/2
            @timed U[freedofs] = K[freedofs,freedofs] \ Array(F[freedofs])
            # Objective function and sensitivity analysis
            ce = reshape(sum((U[edofMat]*KE).*U[edofMat],dims=2),nely,nelx)
            c = sum(sum((Emin.+xPhys.^penal*(Emax-Emin)).*ce))
            
            dc = -penal*(Emax-Emin)*xPhys.^(penal-1).*ce
            dv = ones(nely,nelx)
            dc[:] = H*(dc[:]./Hs)
            dv[:] = H*(dv[:]./Hs)
            # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES AND PHYSICAL DENSITIES
            l1 = 0; l2 = 1e9; move = 0.2; xnew = 0
            while (l2-l1)/(l1+l2) > 1e-3
                lmid = 0.5*(l2+l1)
                xnew = max.(0,max.(x.-move,min.(1,min.(x.+move,x.*sqrt.((0.0.-dc)./dv./lmid)))))
                xPhys[:] = (H*xnew[:])./Hs
                if sum(xPhys[:]) > volfrac*nelx*nely
                    l1 = lmid
                else
                    l2 = lmid
                end
            end
            change = maximum(abs.(xnew[:].-x[:]))
            x = xnew
            m=mean(xPhys[:])
            display(" It:$loop Obj:$c Vol:$m ch:$change ")
            if loop<20||mod(loop,10)==0
                xxx=1 .- clamp.(xPhys,0,1)
                display(heatmap(xxx,xaxis=nothing,yaxis=nothing,legend=nothing,fc=:grays,clims=(0.0, 1.0),aspect_ratio=:equal))
                heatmap(xxx,xaxis=nothing,yaxis=nothing,legend=nothing,fc=:grays,clims=(0.0, 1.0),aspect_ratio=:equal)
                frame(anim)
            end

            xPhys = copy(x)
        end
    end
    return xPhys,anim
end

#########################################################################################################

function CompliantTopologyOptimization(nelx,nely,volfrac,rmin,penal,maxIter,Load,Support,Spring,DOut)
    anim=Animation()
    display("Minimum compliance problem with OC")
    display("ndes: $nelx x $nely")
    display("volfrac: $volfrac rmin: $rmin penal: $penal")
    # Max and min stiffness
    Emin=1e-9
    Emax=1.0
    change = 1
    # dofs:
    ndof = 2*(nelx+1)*(nely+1)
    # Allocate design variables (as array), initialize and allocate sens.
    x=volfrac * ones(Float64,nely,nelx)
    xold=copy(x)
    xPhys=copy(x)
    g=0 # must be initialized to use the NGuyen/Paulino OC approach
    dc=zeros(Float64,(nely,nelx))
    
    # FE: Build the index vectors for the for coo matrix format.
    KE=lk()
    nodenrs = reshape(1:(1+nelx)*(1+nely),1+nely,1+nelx)
    edofVec = reshape(2*nodenrs[1:end-1,1:end-1].+1,nelx*nely,1)
    edofMat = repeat(edofVec,1,8).+repeat([0 1 2*nely.+[2 3 0 1] -2 -1],nelx*nely,1)
    iK = convert(Array{Int},reshape(kron(edofMat,ones(8,1))',64*nelx*nely,1))
    jK = convert(Array{Int},reshape(kron(edofMat,ones(1,8))',64*nelx*nely,1))
    
    
    # DEFINE LOADS AND SUPPORTS 
    F = sparse(2 .*Load[:,1] .-2 .+ Load[:,2],ones(size(Load,1)),Load[:,3],2*(nely+1)*(nelx+1),2)
    DofDOut = 2 * DOut[1] - 2 +DOut[2]; #only one
    F=Array(F)
    F[DofDOut,2]=-1
    fixeddofs = 2 .*Support[:,1] .-2 .+ Support[:,2]
    NSpring = size(Spring,1);
    s = sparse(2 .*Spring[:,1] .-2 .+ Spring[:,2],ones(size(Spring,1)),Spring[:,3],2*(nely+1)*(nelx+1),2)
    m=Array(s)[:,1]
    S= sparse(diagm(m))
    
    U = zeros(2*(nely+1)*(nelx+1),2)

    alldofs = 1:(2*(nely+1)*(nelx+1))
    freedofs = setdiff(alldofs,fixeddofs)
    
    # Prepare filter
    iH = ones(convert(Int,nelx*nely*(2*(ceil(rmin)-1)+1)^2),1)
    jH = ones(Int,size(iH))
    sH = zeros(size(iH))
    k = 0;
    for i1 = 1:nelx
        for j1 = 1:nely
            e1 = (i1-1)*nely+j1
            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 = (i2-1)*nely+j2
                    k = k+1
                    iH[k] = e1
                    jH[k] = e2
                    sH[k] = max(0,rmin-sqrt((i1-i2)^2+(j1-j2)^2))
                end
            end
        end
    end
    H = sparse(vec(iH),vec(jH),vec(sH))
    Hs = sum(H,dims=2)
    
    change= 1
    loop = 0
    changeStable=0
    # Start iteration
    for penal in 1.0:0.5:4
        display(" Penalty: $penal")
        loop = 0
        for i =1:maxIter
            if (change < 0.01)
                changeStable+=1
                display(" Change Stable for $changeStable iterations")
            else
                changeStable=0
            end
            if (changeStable<10)
                # Start iteration
                loop += 1
                # FE-ANALYSIS
                sK = reshape(KE[:]*(Emin.+xPhys[:]'.^penal*(Emax-Emin)),64*nelx*nely,1)
                K = sparse(vec(iK),vec(jK),vec(sK))+S;K = (K+K')/2
                @timed U[freedofs,:] = K[freedofs,freedofs] \ Array(F[freedofs,:])
                U1=U[:,1]
                U2=U[:,2]
                # Objective function and sensitivity analysis
                #ce = reshape(sum((U1[edofMat]*KE).*U2[edofMat],dims=2),nely,nelx)

                ce=reshape(-sum((U1[edofMat]*KE).*U2[edofMat],dims=2),nely,nelx)

                #c = sum((Emin.+xPhys[:].^penal*(Emax-Emin)).*ce)

                c=U[DofDOut,1]

                dc = -penal.*(Emax-Emin).*xPhys.^(penal-1).*ce
                dv = ones(nely,nelx)
                dc[:] = H*(dc[:]./Hs)
                dv[:] = H*(dv[:]./Hs)

                # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES AND PHYSICAL DENSITIES
                l1 = 0; l2 = 1e9; move = 0.05; xnew = 0 #move=0.2
                while (l2-l1)/(l2+l1) > 1e-4 && l2>1e-40
                #while (l2-l1)/(l1+l2) > 1e-3
                    lmid = 0.5*(l2+l1)
                    xnew = max.(0,max.(x.-move,min.(1,min.(x.+move,x.*sqrt.(max.(1e-10,-dc./dv./lmid))))))
                    #xnew = max.(0,max.(x.-move,min.(1,min.(x.+move,x.*sqrt.(-dc./dv./lmid)))))
                    xPhys[:] = (H*xnew[:])./Hs
                    if sum(xPhys[:]) > volfrac*nelx*nely
                        l1 = lmid
                    else
                        l2 = lmid
                    end
                end
                change = maximum(abs.(xnew[:].-x[:]))
                x = xnew

                # print result
                m=mean(xPhys[:])
                display(" It:$loop Obj:$c Vol:$m ch:$change ")
                if loop<20||mod(loop,5)==0
                    xxx=vcat(xPhys[end:-1:1,:],xPhys)
                    xxx=1 .- xxx
                    display(heatmap(xxx,xaxis=nothing,yaxis=nothing,legend=nothing,fc=:grays,clims=(0.0, 1.0)))
                    heatmap(xxx,xaxis=nothing,yaxis=nothing,legend=nothing,fc=:grays,clims=(0.0, 1.0))
                    frame(anim)
                end

                xPhys = copy(x)
            end
        end
    end
    # heatmap(xPhys, legend=false, axis=nothing, foreground_color_subplot=colorant"white",fc=:grays,clims=(0.0, 1.0))
    # heatmap(xPhys, legend=false,fc=:grays,clims=(0.0, 1.0))
    return xPhys,anim
end


#########################################3d################################################################
function topologyOptimization3d(nelx,nely,nelz,volfrac,rmin,penal)
    anim=Animation()
    
    display("Minimum compliance problem with OC")
    display("ndes: $nelx x $nely")
    display("volfrac: $volfrac rmin: $rmin penal: $penal")
    # 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)
    xPhys=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
    m= Matlab.meshgrid(nelx:nelx, 0:0, 0:nelz)
    il=m[1]
    jl=m[2]
    kl=m[3]
    loadnid = kl.*(nelx+1).*(nely+1).+il.*(nely+1).+(nely+1 .-jl)
    loaddof = 3 .*loadnid[:] .- 1; 
    # USER-DEFINED SUPPORT FIXED DOFs
    m= Matlab.meshgrid(0:0,0:nely,0:nelz)# Coordinates
    iif=m[1]
    jf=m[2]
    kf=m[3]
    fixednid = kf.*(nelx+1).*(nely+1) .+iif .*(nely .+1) .+(nely .+1 .-jf) # Node IDs
    fixeddof = [3 .*fixednid[:]; 3 .*fixednid[:].-1; 3 .*fixednid[:].-2] # DOFs
    
    
    # DEFINE LOADS AND SUPPORTS (HALF MBB-BEAM)
    F= sparse(loaddof,fill(1,length(loaddof)),fill(-1,length(loaddof)),ndof,1);
    U = zeros(ndof,1)
    alldofs = 1:ndof
    freedofs = setdiff(1:ndof,fixeddof)
    
    
    
    # Prepare filter
    #iH = ones(convert(Int,nele*(2*(ceil(rmin)-1)+1)^2),1)
    #jH = ones(Int,size(iH))
    #sH = zeros(size(iH))
    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)
    ###################################################
    loop = 0
    change = 1
    maxIter=1000
    # Start iteration
    for i =1:maxIter
        if (change > 0.01)
            # Start iteration
            loop += 1
            # 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 = (K+K')/2
            @timed U[freedofs] = K[freedofs,freedofs] \ Array(F[freedofs])
            # Objective function and sensitivity analysis
            ce = reshape(sum((U[edofMat]*KE).*U[edofMat],dims=2),nely,nelx,nelz)
            c = sum(sum(sum((Emin.+xPhys.^penal*(Emax-Emin)).*ce)))
            
            dc = -penal*(Emax-Emin)*xPhys.^(penal-1).*ce
            dv = ones(nely,nelx,nelz)
            dc[:] = H*(dc[:]./Hs)
            dv[:] = H*(dv[:]./Hs)
            # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES AND PHYSICAL DENSITIES
            l1 = 0; l2 = 1e9; move = 0.2; xnew = 0
            while (l2-l1)/(l1+l2) > 1e-3
                lmid = 0.5*(l2+l1)
                xnew = max.(0,max.(x.-move,min.(1,min.(x.+move,x.*sqrt.((0.0.-dc)./dv./lmid)))))
                xPhys[:] = (H*xnew[:])./Hs
                if sum(xPhys[:]) > volfrac*nelx*nely
                    l1 = lmid
                else
                    l2 = lmid
                end
            end
            change = maximum(abs.(xnew[:].-x[:]))
            x = xnew
            m=mean(xPhys[:])
            display(" It:$loop Obj:$c Vol:$m ch:$change ")
            if mod(loop,2)==0
                display(topplot3d(xPhys))
                topplot3d(xPhys)
                frame(anim)
            end

            xPhys = copy(x)
        end
    end
    return xPhys,anim
end

#########################################3d KE################################################################
function topologyOptimization3dKE(nelx,nely,nelz,volfrac,rmin,penal,CE)
    anim=Animation()
    
    display("Minimum compliance problem with OC")
    display("ndes: $nelx x $nely")
    display("volfrac: $volfrac rmin: $rmin penal: $penal")
    # 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)
    xPhys=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)

    KE = elementMatVec3D(0.5, 0.5, 0.5, CE);

    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
    m= Matlab.meshgrid(nelx:nelx, 0:0, 0:nelz)
    il=m[1]
    jl=m[2]
    kl=m[3]
    loadnid = kl.*(nelx+1).*(nely+1).+il.*(nely+1).+(nely+1 .-jl)
    loaddof = 3 .*loadnid[:] .- 1; 
    # USER-DEFINED SUPPORT FIXED DOFs
    m= Matlab.meshgrid(0:0,0:nely,0:nelz)# Coordinates
    iif=m[1]
    jf=m[2]
    kf=m[3]
    fixednid = kf.*(nelx+1).*(nely+1) .+iif .*(nely .+1) .+(nely .+1 .-jf) # Node IDs
    fixeddof = [3 .*fixednid[:]; 3 .*fixednid[:].-1; 3 .*fixednid[:].-2] # DOFs
    
    
    # DEFINE LOADS AND SUPPORTS (HALF MBB-BEAM)
    F= sparse(loaddof,fill(1,length(loaddof)),fill(-1,length(loaddof)),ndof,1);
    U = zeros(ndof,1)
    alldofs = 1:ndof
    freedofs = setdiff(1:ndof,fixeddof)
    
    
    
    # Prepare filter
    #iH = ones(convert(Int,nele*(2*(ceil(rmin)-1)+1)^2),1)
    #jH = ones(Int,size(iH))
    #sH = zeros(size(iH))
    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)
    ###################################################
    loop = 0
    change = 1
    maxIter=1000
    # Start iteration
    for i =1:maxIter
        if (change > 0.01)
            # Start iteration
            loop += 1
            # 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 = (K+K')/2
            @timed U[freedofs] = K[freedofs,freedofs] \ Array(F[freedofs])
            # Objective function and sensitivity analysis
            ce = reshape(sum((U[edofMat]*KE).*U[edofMat],dims=2),nely,nelx,nelz)
            c = sum(sum(sum((Emin.+xPhys.^penal*(Emax-Emin)).*ce)))
            
            dc = -penal*(Emax-Emin)*xPhys.^(penal-1).*ce
            dv = ones(nely,nelx,nelz)
            dc[:] = H*(dc[:]./Hs)
            dv[:] = H*(dv[:]./Hs)
            # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES AND PHYSICAL DENSITIES
            l1 = 0; l2 = 1e9; move = 0.2; xnew = 0
            while (l2-l1)/(l1+l2) > 1e-3
                lmid = 0.5*(l2+l1)
                xnew = max.(0,max.(x.-move,min.(1,min.(x.+move,x.*sqrt.((0.0.-dc)./dv./lmid)))))
                xPhys[:] = (H*xnew[:])./Hs
                if sum(xPhys[:]) > volfrac*nelx*nely
                    l1 = lmid
                else
                    l2 = lmid
                end
            end
            change = maximum(abs.(xnew[:].-x[:]))
            x = xnew
            m=mean(xPhys[:])
            display(" It:$loop Obj:$c Vol:$m ch:$change ")
            if mod(loop,5)==0
                display(topplot3d(xPhys))
                topplot3d(xPhys)
                frame(anim)
            end

            xPhys = copy(x)
        end
    end
    return xPhys,anim
end

#########################################################################################################

function compliantOptimization3d(nelx,nely,nelz,volfrac,rmin,penal,maxIter=500,getProblem=inverter)
    anim=Animation()

    display("Compliance sythesyz problem with OC")
    display("ndes: $nelx x $nely x $nelz")
    display("volfrac: $volfrac rmin: $rmin penal: $penal")
    # 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)
    xPhys=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
    # m= Matlab.meshgrid(nelx:nelx, 0:0, 0:nelz)
    # il=m[1]
    # jl=m[2]
    # kl=m[3]
    # loadnid = kl.*(nelx+1).*(nely+1).+il.*(nely+1).+(nely+1 .-jl)
    # loaddof = 3 .*loadnid[:] .- 1; 
    # # USER-DEFINED SUPPORT FIXED DOFs
    # m= Matlab.meshgrid(0:0,0:nely,0:nelz)# Coordinates
    # iif=m[1]
    # jf=m[2]
    # kf=m[3]
    # fixednid = kf.*(nelx+1).*(nely+1) .+iif .*(nely .+1) .+(nely .+1 .-jf) # Node IDs
    # fixeddof = [3 .*fixednid[:]; 3 .*fixednid[:].-1; 3 .*fixednid[:].-2] # DOFs
    
    
    # # DEFINE LOADS AND SUPPORTS (HALF MBB-BEAM)
    # F= sparse(loaddof,fill(1,length(loaddof)),fill(-1,length(loaddof)),ndof,1);
    # U = zeros(ndof,1)
    # alldofs = 1:ndof
    # freedofs = setdiff(1:ndof,fixeddof)
    # 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)

    
    
    # Prepare filter
    #iH = ones(convert(Int,nele*(2*(ceil(rmin)-1)+1)^2),1)
    #jH = ones(Int,size(iH))
    #sH = zeros(size(iH))
    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)
    ###################################################
    loop = 0
    change = 1
    
    # Start iteration
    for i =1:maxIter
        if (change > 0.01)
            # Start iteration
            loop += 1
            # 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])
            @timed U[freedofs,:] = K[freedofs,freedofs] \ Array(F[freedofs,:])
            U1 = U[:,1]
            U2 = U[:,2]
            
            # Objective function and sensitivity analysis
            
            #ce = reshape(sum((U[edofMat]*KE).*U[edofMat],dims=2),nely,nelx,nelz)
            ce = reshape(sum((U1[edofMat]*KE).*U2[edofMat],dims=2),nely,nelx,nelz);
            
            #c = sum(sum(sum((Emin.+xPhys.^penal*(Emax-Emin)).*ce)))
            c  = U[dout,1]
            
            dc = penal*(Emax-Emin)*xPhys.^(penal-1).*ce
            dv = ones(nely,nelx,nelz)
            dc[:] = H*(dc[:]./Hs)
            dv[:] = H*(dv[:]./Hs)
            # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES AND PHYSICAL DENSITIES
            #l1 = 0; l2 = 1e9; move = 0.2; xnew = 0
            l1 = 0; l2 = 1e9; move = 0.1;xnew = 0
            #while (l2-l1)/(l1+l2) > 1e-3
            while (l2-l1)/(l1+l2) > 1e-4 && l2 > 1e-40
                lmid = 0.5*(l2+l1)
                #xnew = max.(0,max.(x.-move,min.(1,min.(x.+move,x.*sqrt.((0.0.-dc)./dv./lmid)))))
                xnew = max.(0,max.(x.-move,min.(1,min.(x.+move,x.*(max.(1e-10,-dc./dv./lmid)).^0.3))));

                xPhys[:] = (H*xnew[:])./Hs
                if sum(xPhys[:]) > volfrac*nelx*nely
                    l1 = lmid
                else
                    l2 = lmid
                end
            end
            change = maximum(abs.(xnew[:].-x[:]))
            x = xnew
            m=mean(xPhys[:])
            display(" It:$loop Obj:$c Vol:$m ch:$change ")
            if mod(loop,5)==0
                display(topplot3d(xPhys))
                topplot3d(xPhys)
                frame(anim)
            end

            xPhys = copy(x)
        end
    end
    return xPhys,anim
end

#############################################multimaterial############################################################

#Based on: https://link.springer.com/article/10.1007/s00158-013-0999-1
function multitop(nx,ny,tol_out,tol_f,iter_max_in,iter_max_out,p,q,e,v,rf)
    anim=Animation()
    alpha = zeros(Float64,nx*ny,p)
    for i = 1:p
        alpha[:,i] .= v[i]
    end
    
    # MAKE FILTER
    H,Hs = make_filter(nx,ny,rf)
    obj=0
    change_out = 2*tol_out; iter_out = 0;
    while (iter_out < iter_max_out) && (change_out > tol_out)
        alpha_old = copy(alpha)
        for a = 1:p
            for b = a+1:p
                obj,alpha = bi_top(a,b,nx,ny,p,v,e,q,alpha,H,Hs,iter_max_in);
            end
        end
        iter_out = iter_out + 1;
        change_out = norm(alpha[:].-alpha_old[:],Inf);
        display("Iter:$iter_out Obj.:$obj change:$change_out");
        #  UPDATE FILTER
        if (change_out < tol_f) && (rf>3)
            tol_f = 0.99*tol_f; rf = 0.99*rf; 
            H,Hs = make_filter(nx,ny,rf);
        end
        #  SCREEN OUT TEMPORAL TOPOLOGY EVERY 5 ITERATIONS
        if mod(iter_out,5)==0
            I = make_bitmap(p,nx,ny,alpha);
            display(RGB.(I[:,:,1],I[:,:,2],I[:,:,3]))
            I = permutedims(I, [3, 1, 2])
            img = colorview(RGB, I)
            heatmap(img,xaxis=nothing,yaxis=nothing,aspect_ratio=:equal)
            frame(anim)
        end
    end
    return anim,alpha
end

function bi_top(a,b,nx,ny,p,v,e,q,alpha_old,H,Hs,maxIter)
    alpha = copy(alpha_old)
    nu = 0.3;
    
    # PREPARE FINITE ELEMENT ANALYSIS
    A11 = [12 3 -6 -3; 3 12 3 0; -6 3 12 -3; -3 0 -3 12];
    A12 = [-6 -3 0 3; -3 -6 -3 -6; 0 -3 -6 3; 3 -6 3 -6];
    B11 = [-4 3 -2 9; 3 -4 -9 4; -2 -9 -4 -3; 9 4 -3 -4];
    B12 = [ 2 -3 4 -9; -3 2 9 -2; 4 9 2 3; -9 -2 3 2];
    KE = 1/(1-nu^2)/24*([A11 A12;A12' A11]+nu*[B11 B12;B12' B11]);
    
    nelx=nx
    nely=ny
    
    nodenrs = reshape(1:(1+nelx)*(1+nely),1+nely,1+nelx)
    edofVec = reshape(2*nodenrs[1:end-1,1:end-1].+1,nelx*nely,1)
    edofMat = repeat(edofVec,1,8).+repeat([0 1 2*nely.+[2 3 0 1] -2 -1],nelx*nely,1)
    iK = convert(Array{Int},reshape(kron(edofMat,ones(8,1))',64*nelx*nely,1))
    jK = convert(Array{Int},reshape(kron(edofMat,ones(1,8))',64*nelx*nely,1))
    
    # DEFINE LOADS AND SUPPORTS (HALF MBB-BEAM)
    F = sparse([2],[1],[-1.0],2*(nely+1)*(nelx+1),1)
    U = zeros(2*(nely+1)*(nelx+1),1)
    fixeddofs = union(1:2:2*(nely+1),2*(nelx+1)*(nely+1))
    alldofs = 1:(2*(nely+1)*(nelx+1))
    freedofs = setdiff(alldofs,fixeddofs)
    
    o=0;alpha_a=0
    # inner iteration
    for i =0: (maxIter-1)
        # FE-ANALYSIS
        E = e[1]*alpha[:,1].^q
        for phase = 2:p
            E = E + e[phase]*alpha[:,phase].^q;
        end

        sK = reshape(KE[:]*E[:]',64*nelx*nely,1)
        K = sparse(vec(iK),vec(jK),vec(sK)); K = (K+K')/2
        @timed U[freedofs] = K[freedofs,freedofs] \ Array(F[freedofs])
    

        # Objective function and sensitivity analysis
        ce = sum((U[edofMat]*KE).*U[edofMat],dims=2)
        o = sum(sum(E.*ce))
        
        # FILTERING OF SENSITIVITIES 
        dc = 0.0 .-(q .*(e[a].-e[b]).*alpha[:,a].^(q-1)).*ce;
        dc = H*(alpha[:,a].*dc)./Hs./max.(1e-3,alpha[:,a])
        dc = min.(dc,0.0)

        # UPDATE LOWER AND UPPER BOUNDS OF DESIGN VARIABLES
        move = 0.2;
        r = ones(nx*ny,1)
  
        for k = 1:p
            #if (k ~= a) && (k ~= b)
            if (k != a) && (k != b)
                # display("k $k a $a b $b")
                r = r .- alpha[:,k]
            end
        end
        l = max.(0,alpha[:,a] .-move)
        u = min.(r,alpha[:,a] .+move)
        # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES
        l1 = 0; l2 = 1e9
        while (l2-l1)/(l1+l2) > 1e-3
            lmid = 0.5*(l2+l1)
            alpha_a = max.(l,min.(u,alpha[:,a].*sqrt.(-dc./lmid)));
            if sum(alpha_a) > nx*ny*v[a]
                l1 = lmid
            else
                l2 = lmid 
            end
        end
        alpha[:,a] = alpha_a
        alpha[:,b] = r .-alpha_a

    end
    return o,alpha
end

########

function multitop_compliant(nx,ny,tol_out,tol_f,iter_max_in,iter_max_out,p,q,e,v,rf,Load,Support,Spring,DOut)
    anim=Animation()
    alpha = zeros(Float64,nx*ny,p)
    for i = 1:p
        alpha[:,i] .= v[i]
    end
    
    # MAKE FILTER
    H,Hs = make_filter(nx,ny,rf)
    obj=0
    change_out = 2*tol_out; iter_out = 0;
    while (iter_out < iter_max_out) && (change_out > tol_out)
        alpha_old = copy(alpha)
        for a = 1:p
            for b = a+1:p
                obj,alpha = bi_top_compliant(a,b,nx,ny,p,v,e,q,alpha,H,Hs,iter_max_in);
            end
        end
        iter_out = iter_out + 1;
        change_out = norm(alpha[:].-alpha_old[:],Inf);
        display("Iter:$iter_out Obj.:$obj change:$change_out");
        #  UPDATE FILTER
        if (change_out < tol_f) && (rf>3)
            tol_f = 0.99*tol_f; rf = 0.99*rf; 
            H,Hs = make_filter(nx,ny,rf);
        end
        #  SCREEN OUT TEMPORAL TOPOLOGY EVERY 5 ITERATIONS
        if mod(iter_out,5)==0
            I = make_bitmap_compliant(p,nx,ny,alpha);
            display(RGB.(I[:,:,1],I[:,:,2],I[:,:,3]))
            I = permutedims(I, [3, 1, 2])
            img = colorview(RGB, I)
            heatmap(img,xaxis=nothing,yaxis=nothing,aspect_ratio=:equal)
            frame(anim)
        end
    end
    return anim,alpha
end

function bi_top_compliant(a,b,nx,ny,p,v,e,q,alpha_old,H,Hs,maxIter,Load,Support,Spring,DOut)
    alpha = copy(alpha_old)
    nu = 0.3;
    
    # PREPARE FINITE ELEMENT ANALYSIS
    A11 = [12 3 -6 -3; 3 12 3 0; -6 3 12 -3; -3 0 -3 12];
    A12 = [-6 -3 0 3; -3 -6 -3 -6; 0 -3 -6 3; 3 -6 3 -6];
    B11 = [-4 3 -2 9; 3 -4 -9 4; -2 -9 -4 -3; 9 4 -3 -4];
    B12 = [ 2 -3 4 -9; -3 2 9 -2; 4 9 2 3; -9 -2 3 2];
    KE = 1/(1-nu^2)/24*([A11 A12;A12' A11]+nu*[B11 B12;B12' B11]);
    
    nelx=nx
    nely=ny
    
    nodenrs = reshape(1:(1+nelx)*(1+nely),1+nely,1+nelx)
    edofVec = reshape(2*nodenrs[1:end-1,1:end-1].+1,nelx*nely,1)
    edofMat = repeat(edofVec,1,8).+repeat([0 1 2*nely.+[2 3 0 1] -2 -1],nelx*nely,1)
    iK = convert(Array{Int},reshape(kron(edofMat,ones(8,1))',64*nelx*nely,1))
    jK = convert(Array{Int},reshape(kron(edofMat,ones(1,8))',64*nelx*nely,1))

    

    # # DEFINE LOADS AND SUPPORTS (HALF MBB-BEAM)
    # F = sparse([2],[1],[-1.0],2*(nely+1)*(nelx+1),1)
    # U = zeros(2*(nely+1)*(nelx+1),1)
    # fixeddofs = union(1:2:2*(nely+1),2*(nelx+1)*(nely+1))
    # alldofs = 1:(2*(nely+1)*(nelx+1))
    # freedofs = setdiff(alldofs,fixeddofs)

    # DEFINE LOADS AND SUPPORTS 
    F = sparse(2 .*Load[:,1] .-2 .+ Load[:,2],ones(size(Load,1)),Load[:,3],2*(nely+1)*(nelx+1),2)
    DofDOut = 2 * DOut[1] - 2 +DOut[2]; #only one
    F=Array(F)
    F[DofDOut,2]=-1
    fixeddofs = 2 .*Support[:,1] .-2 .+ Support[:,2]
    NSpring = size(Spring,1);
    s = sparse(2 .*Spring[:,1] .-2 .+ Spring[:,2],ones(size(Spring,1)),Spring[:,3],2*(nely+1)*(nelx+1),2)
    m=Array(s)[:,1]
    S= sparse(diagm(m))
    
    U = zeros(2*(nely+1)*(nelx+1),2)

    alldofs = 1:(2*(nely+1)*(nelx+1))
    freedofs = setdiff(alldofs,fixeddofs)
    
    o=0;alpha_a=0
    # inner iteration
    for i =0: (maxIter-1)
        # FE-ANALYSIS
        E = e[1]*alpha[:,1].^q
        for phase = 2:p
            E = E + e[phase]*alpha[:,phase].^q;
        end

        sK = reshape(KE[:]*E[:]',64*nelx*nely,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])
        @timed U[freedofs,:] = K[freedofs,freedofs] \ Array(F[freedofs,:])
        U1=U[:,1]
        U2=U[:,2]


        # Objective function and sensitivity analysis
        # ce = sum((U[edofMat]*KE).*U[edofMat],dims=2)
        ce =-sum((U1[edofMat]*KE).*U2[edofMat],dims=2)
        # o = sum(sum(E.*ce))

        o=U[DofDOut,1]
        
        # FILTERING OF SENSITIVITIES 
        dc = 0.0 .-(q .*(e[a].-e[b]).*alpha[:,a].^(q-1)).*ce;
        dc = H*(alpha[:,a].*dc)./Hs./max.(1e-3,alpha[:,a])
        dc = min.(dc,0.0)

        # UPDATE LOWER AND UPPER BOUNDS OF DESIGN VARIABLES
        move=0.1;
        r = ones(nx*ny,1)
  
        for k = 1:p
            #if (k ~= a) && (k ~= b)
            if (k != a) && (k != b)
                # display("k $k a $a b $b")
                r = r .- alpha[:,k]
            end
        end
        l = max.(0,alpha[:,a] .-move)
        u = min.(r,alpha[:,a] .+move)
        # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES
        l1 = 0; l2 = 1e9
        while (l2-l1)/(l2+l1) > 1e-4 && l2>1e-40
        # while (l2-l1)/(l1+l2) > 1e-3
            lmid = 0.5*(l2+l1)
            alpha_a = max.(l,min.(u,alpha[:,a].*((max.(1e-10,-dc./lmid)).^0.3 )));

            if sum(alpha_a) > nx*ny*v[a]
                l1 = lmid
            else
                l2 = lmid 
            end
        end
        alpha[:,a] = alpha_a
        alpha[:,b] = r .-alpha_a

    end
    return o,alpha
end

########

function multitop3D(nx,ny,nz,tol_out,tol_f,iter_max_in,iter_max_out,p,q,e,v,rf)
    anim=Animation()
    alpha = zeros(Float64,nx*ny*nz,p)
    for i = 1:p
        alpha[:,i] .= v[i]
    end
    
    # MAKE FILTER
    H,Hs = make_filter3D(nx,ny,nz,rf)
    obj=0
    change_out = 2*tol_out; iter_out = 0;
    while (iter_out < iter_max_out) && (change_out > tol_out)
        alpha_old = copy(alpha)
        for a = 1:p
            for b = a+1:p
                obj,alpha = bi_top3D(a,b,nx,ny,nz,p,v,e,q,alpha,H,Hs,iter_max_in);
            end
        end
        iter_out = iter_out + 1;
        change_out = norm(alpha[:].-alpha_old[:],Inf);
        display("Iter:$iter_out Obj.:$obj change:$change_out");
        #  UPDATE FILTER
        if (change_out < tol_f) && (rf>3)
            tol_f = 0.99*tol_f; rf = 0.99*rf; 
            H,Hs = make_filter3D(nx,ny,nz,rf)
        end
        #  SCREEN OUT TEMPORAL TOPOLOGY EVERY 5 ITERATIONS
        if mod(iter_out,5)==0
            I = make_bitmap_3d(p,nx,ny,nz,alpha)
            threshold=0.05
            display(topplotmulti3d(nx,ny,nz,I,threshold))
            frame(anim)
        end
    end
    return anim,alpha
end

function bi_top3D(a,b,nx,ny,nz,p,v,e,q,alpha_old,H,Hs,maxIter)
    alpha = copy(alpha_old)
    nu = 0.3;

    
    
    # PREPARE FINITE ELEMENT ANALYSIS
    KE=lk_H8(nu)
    
    nelx=nx
    nely=ny
    nelz=nz
    nele = nelx*nely*nelz
    ndof = 3*(nelx+1)*(nely+1)*(nelz+1)

    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
    m= Matlab.meshgrid(nelx:nelx, 0:0, 0:nelz)
    il=m[1]
    jl=m[2]
    kl=m[3]
    loadnid = kl.*(nelx+1).*(nely+1).+il.*(nely+1).+(nely+1 .-jl)
    loaddof = 3 .*loadnid[:] .- 1; 
    # USER-DEFINED SUPPORT FIXED DOFs
    m= Matlab.meshgrid(0:0,0:nely,0:nelz)# Coordinates
    iif=m[1]
    jf=m[2]
    kf=m[3]
    fixednid = kf.*(nelx+1).*(nely+1) .+iif .*(nely .+1) .+(nely .+1 .-jf) # Node IDs
    fixeddof = [3 .*fixednid[:]; 3 .*fixednid[:].-1; 3 .*fixednid[:].-2] # DOFs
    
    
    # DEFINE LOADS AND SUPPORTS (HALF MBB-BEAM)
    F= sparse(loaddof,fill(1,length(loaddof)),fill(-1,length(loaddof)),ndof,1);
    U = zeros(ndof,1)
    alldofs = 1:ndof
    freedofs = setdiff(1:ndof,fixeddof)
    
    o=0;alpha_a=0
    # inner iteration
    for i =0: (maxIter-1)
        # FE-ANALYSIS
        E = e[1]*alpha[:,1].^q
        for phase = 2:p
            E = E + e[phase]*alpha[:,phase].^q;
        end

        sK = reshape(KE[:]*E[:]',24*24*nelx*nely*nelz,1)
        K = sparse(vec(iK),vec(jK),vec(sK)); K = (K+K')/2
        @timed U[freedofs] = K[freedofs,freedofs] \ Array(F[freedofs])
    

        # Objective function and sensitivity analysis
        ce = sum((U[edofMat]*KE).*U[edofMat],dims=2)
        o = sum(sum(E.*ce))
        
        # FILTERING OF SENSITIVITIES 
        dc = 0.0 .-(q .*(e[a].-e[b]).*alpha[:,a].^(q-1)).*ce;
        dc = H*(alpha[:,a].*dc)./Hs./max.(1e-3,alpha[:,a])
        dc = min.(dc,0.0)

        # UPDATE LOWER AND UPPER BOUNDS OF DESIGN VARIABLES
        move = 0.2;
        r = ones(nx*ny*nz,1)
  
        for k = 1:p
            #if (k ~= a) && (k ~= b)
            if (k != a) && (k != b)
                # display("k $k a $a b $b")
                r = r .- alpha[:,k]
            end
        end
        l = max.(0,alpha[:,a] .-move)
        u = min.(r,alpha[:,a] .+move)
        # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES
        l1 = 0; l2 = 1e9
        while (l2-l1)/(l1+l2) > 1e-3
            lmid = 0.5*(l2+l1)
            alpha_a = max.(l,min.(u,alpha[:,a].*sqrt.(-dc./lmid)));
            if sum(alpha_a) > nx*ny*v[a]
                l1 = lmid
            else
                l2 = lmid 
            end
        end
        alpha[:,a] = alpha_a
        alpha[:,b] = r .-alpha_a

    end
    return o,alpha
end

########

function multitop3D_compliant(nx,ny,nz,tol_out,tol_f,iter_max_in,iter_max_out,p,q,e,v,rf,getProblem=inverter)
    anim=Animation()
    alpha = zeros(Float64,nx*ny*nz,p)
    for i = 1:p
        alpha[:,i] .= v[i]
    end
    
    # MAKE FILTER
    H,Hs = make_filter3D(nx,ny,nz,rf)
    obj=0
    change_out = 2*tol_out; iter_out = 0;
    while (iter_out < iter_max_out) && (change_out > tol_out)
        alpha_old = copy(alpha)
        for a = 1:p
            for b = a+1:p
                obj,alpha = bi_top3D_compliant(a,b,nx,ny,nz,p,v,e,q,alpha,H,Hs,iter_max_in,getProblem);
            end
        end
        iter_out = iter_out + 1;
        change_out = norm(alpha[:].-alpha_old[:],Inf);
        display("Iter:$iter_out Obj.:$obj change:$change_out");
        #  UPDATE FILTER
        if (change_out < tol_f) && (rf>3)
            tol_f = 0.99*tol_f; rf = 0.99*rf; 
            H,Hs = make_filter3D(nx,ny,nz,rf)
        end
        #  SCREEN OUT TEMPORAL TOPOLOGY EVERY 5 ITERATIONS
        if mod(iter_out,5)==0
            I = make_bitmap_3d(p,nx,ny,nz,alpha)
            threshold=0.05
            display(topplotmulti3d(nx,ny,nz,I,threshold))
            frame(anim)
        end
    end
    return anim,alpha
end

function bi_top3D_compliant(a,b,nx,ny,nz,p,v,e,q,alpha_old,H,Hs,maxIter,getProblem=inverter)
    alpha = copy(alpha_old)
    nu = 0.3;

    
    
    # PREPARE FINITE ELEMENT ANALYSIS
    KE=lk_H8(nu)
    
    nelx=nx
    nely=ny
    nelz=nz
    nele = nelx*nely*nelz
    ndof = 3*(nelx+1)*(nely+1)*(nelz+1)

    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)
    
    
    o=0;alpha_a=0
    # inner iteration
    for i =0: (maxIter-1)
        # FE-ANALYSIS
        E = e[1]*alpha[:,1].^q
        for phase = 2:p
            E = E + e[phase]*alpha[:,phase].^q;
        end

        sK = reshape(KE[:]*E[:]',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])
        @timed U[freedofs,:] = K[freedofs,freedofs] \ Array(F[freedofs,:])
        U1 = U[:,1]
        U2 = U[:,2]
    

        # Objective function and sensitivity analysis
        ce = -sum((U1[edofMat]*KE).*U2[edofMat],dims=2)

        # o = sum(sum(E.*ce))
        o  = U[dout,1]
        
        # FILTERING OF SENSITIVITIES 
        dc = 0.0 .-(q .*(e[a].-e[b]).*alpha[:,a].^(q-1)).*ce;
        dc = H*(alpha[:,a].*dc)./Hs./max.(1e-3,alpha[:,a])
        dc = min.(dc,0.0)

        # UPDATE LOWER AND UPPER BOUNDS OF DESIGN VARIABLES
        move = 0.1;
        r = ones(nx*ny*nz,1)
  
        for k = 1:p
            #if (k ~= a) && (k ~= b)
            if (k != a) && (k != b)
                # display("k $k a $a b $b")
                r = r .- alpha[:,k]
            end
        end
        l = max.(0,alpha[:,a] .-move)
        u = min.(r,alpha[:,a] .+move)
        # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES
        l1 = 0; l2 = 1e9
        # while (l2-l1)/(l1+l2) > 1e-3
        while (l2-l1)/(l1+l2) > 1e-4 && l2 > 1e-40
            lmid = 0.5*(l2+l1)
            # alpha_a = max.(l,min.(u,alpha[:,a].*sqrt.(-dc./lmid)));
            alpha_a = max.(l,min.(u,alpha[:,a].*((max.(1e-10,-dc./lmid)).^0.3 )));
            if sum(alpha_a) > nx*ny*v[a]
                l1 = lmid
            else
                l2 = lmid 
            end
        end
        alpha[:,a] = alpha_a
        alpha[:,b] = r .-alpha_a

    end
    return o,alpha
end

########
function multitop3DKE(nx,ny,nz,tol_out,tol_f,iter_max_in,iter_max_out,p,q,KEs,Es,v,rf)
    anim=Animation()
    alpha = zeros(Float64,nx*ny*nz,p)
    for i = 1:p
        alpha[:,i] .= v[i]
    end
    
    # MAKE FILTER
    H,Hs = make_filter3D(nx,ny,nz,rf)
    obj=0
    change_out = 2*tol_out; iter_out = 0;
    while (iter_out < iter_max_out) && (change_out > tol_out)
        alpha_old = copy(alpha)
        for a = 1:p
            for b = a+1:p
                obj,alpha = bi_top3DKE(a,b,nx,ny,nz,p,v,KEs,Es,q,alpha,H,Hs,iter_max_in);
            end
        end
        iter_out = iter_out + 1;
        change_out = norm(alpha[:].-alpha_old[:],Inf);
        display("Iter:$iter_out Obj.:$obj change:$change_out");
        #  UPDATE FILTER
        if (change_out < tol_f) && (rf>3)
            tol_f = 0.99*tol_f; rf = 0.99*rf; 
            H,Hs = make_filter3D(nx,ny,nz,rf)
        end
        #  SCREEN OUT TEMPORAL TOPOLOGY EVERY 5 ITERATIONS
        if mod(iter_out,5)==0
            I = make_bitmap_3d(p,nx,ny,nz,alpha)
            threshold=0.05*2.0
            display(topplotmulti3d(nx,ny,nz,I,threshold))
            frame(anim)
        end
    end
    return anim,alpha
end

function bi_top3DKE(a,b,nx,ny,nz,p,v,KEs,Es,q,alpha_old,H,Hs,maxIter)
    alpha = copy(alpha_old)
    nu = 0.3;

    # PREPARE FINITE ELEMENT ANALYSIS
    # KE=lk_H8(nu)
    
    nelx=nx
    nely=ny
    nelz=nz
    nele = nelx*nely*nelz
    ndof = 3*(nelx+1)*(nely+1)*(nelz+1)
    nnx = nelx+1; nny = nely+1; nnz = nelz+1;

    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
    m= Matlab.meshgrid(nelx:nelx, 0:0, 0:nelz)
    il=m[1]
    jl=m[2]
    kl=m[3]
    loadnid = kl.*(nelx+1).*(nely+1).+il.*(nely+1).+(nely+1 .-jl)
    loaddof = 3 .*loadnid[:] .- 1; 
    # USER-DEFINED SUPPORT FIXED DOFs
    m= Matlab.meshgrid(0:0,0:nely,0:nelz)# Coordinates
    iif=m[1]
    jf=m[2]
    kf=m[3]
    fixednid = kf.*(nelx+1).*(nely+1) .+iif .*(nely .+1) .+(nely .+1 .-jf) # Node IDs
    fixeddof = [3 .*fixednid[:]; 3 .*fixednid[:].-1; 3 .*fixednid[:].-2] # DOFs
    
    
    # DEFINE LOADS AND SUPPORTS (HALF MBB-BEAM)
    F= sparse(loaddof,fill(1,length(loaddof)),fill(-1,length(loaddof)),ndof,1);
    U = zeros(ndof,1)
    alldofs = 1:ndof
    freedofs = setdiff(1:ndof,fixeddof)
    
    o=0;alpha_a=0
    # inner iteration
    for i =0: (maxIter-1)
        # FE-ANALYSIS
        # KE[:]*E[:]'
        KEE = KEs[1][:]*alpha[:,1][:]'.^q
        for phase = 2:p
            KEE = KEE + KEs[phase][:]*alpha[:,phase][:]'.^q;
        end

        EE = mean(Es[1])*alpha[:,1].^q
        for phase = 2:p
            EE = EE + mean(Es[phase])*alpha[:,phase].^q;
        end

        

        sK = reshape(KEE[:],24*24*nelx*nely*nelz,1)
        K = sparse(vec(iK),vec(jK),vec(sK)); K = (K+K')/2
        @timed U[freedofs] = K[freedofs,freedofs] \ Array(F[freedofs])


        

        KE=reshape(sum(KEE,dims=2),24,24)
        # dc=zeros(nelx* nely* nelz)
        # for i=1:Int(nelx* nely* nelz)
        #     KE=(q .*(KEs[a].-KEs[b]).*alpha[i,a].^(q-1))
        #     # ce[i]=sum((U[edofMat][i,:]'*KE).*U[edofMat][i,:]')
        #     dc[i]=-sum((U[edofMat][i,:]'*KE).*U[edofMat][i,:]')
        # end

        ce=zeros(nelx* nely* nelz)
        for i=1:Int(nelx* nely* nelz)
            KE = KEs[1].*alpha[i,1]
            for phase = 2:p
                KE = KE + KEs[phase].*alpha[i,phase]
            end
            ce[i]=sum((U[edofMat][i,:]'*KE).*U[edofMat][i,:]')
        end

        # display(size(KEE))
        # OBJECTIVE FUNCTION AND SENSITIVITY ANALYSIS
        # cc = 0.0; 
        # dc = zeros(nelx, nely, nelz);
        # for elz = 1:nelz
        #     for ely = 1:nely 
        #         for elx = 1:nelx
        #             # KE   = get_KE(truss, x, Es, vs, Gs, elx, ely, elz, false);
        #             # DKE  = get_KE(truss, x, Es, vs, Gs, elx, ely, elz, true);
        #             # KE=reshape(KEEE[:,get_num(nelx, nely, nelz, elx, ely, elz)] ,24,24)
        #             # display(get_num1(nelx, nely, nelz, elx, ely, elz))
        #             # display(get_num(nnx, nny, nnz, elx, ely, elz))

        #             # KE = (KEs[a].*reshape(alpha[:,a][:],nelx, nely, nelz)[elx, ely, elz])-(KEs[b].*reshape(alpha[:,b][:],nelx, nely, nelz)[elx, ely, elz])


        #             # KE = KEs[1].*reshape(alpha[:,1][:],nelx, nely, nelz)[elx, ely, elz]
        #             # for phase = 2:p
        #             #     KE = KE + KEs[phase].*reshape(alpha[:,phase][:],nelx, nely, nelz)[elx, ely, elz];
        #             # end
        #             DKE=KE
        #             dofs1 = get_elem_dofs(nnx, nny, nnz, elx, ely, elz);
        #             dofs1 = get_elem_dofs(nelx, nely, nelz, elx, ely, elz);
        #             # display(dofs1)
        #             Ue = U[dofs1];
        #             cc = cc + Ue'*KE*Ue;
        #             dc[elx,ely,elz] = Ue'*DKE*Ue;
        #         end
        #     end
        # end

        # display(size(dc))
        # display(c)

        # ce=dc[:]
        # o=cc


        # display(ce)
        # display(sum(o))
        # dc=dc[:]

        

        
        # Objective function and sensitivity analysis
        # ce= sum((U[edofMat]*KE).*U[edofMat],dims=2)
        o = sum(sum(EE.*ce))

        # o = sum(sum(-dc))

        # display(ce)
        # display(sum(o1))

        # display(size(ce))
        # display(size(EE))
        
        # FILTERING OF SENSITIVITIES 
        dc = 0.0 .-(q .*mean(Es[a].-Es[b]).*alpha[:,a].^(q-1)).*ce;
        dc = H*(alpha[:,a].*dc)./Hs./max.(1e-3,alpha[:,a])
        dc = min.(dc,0.0)

        # UPDATE LOWER AND UPPER BOUNDS OF DESIGN VARIABLES
        move = 0.2;
        r = ones(nx*ny*nz,1)
  
        for k = 1:p
            #if (k ~= a) && (k ~= b)
            if (k != a) && (k != b)
                # display("k $k a $a b $b")
                r = r .- alpha[:,k]
            end
        end
        l = max.(0,alpha[:,a] .-move)
        u = min.(r,alpha[:,a] .+move)
        # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES
        l1 = 0; l2 = 1e9
        while (l2-l1)/(l1+l2) > 1e-3
            lmid = 0.5*(l2+l1)
            alpha_a = max.(l,min.(u,alpha[:,a].*sqrt.(-dc./lmid)));
            if sum(alpha_a) > nx*ny*v[a]
                l1 = lmid
            else
                l2 = lmid 
            end
        end
        alpha[:,a] = alpha_a
        alpha[:,b] = r .-alpha_a

    end
    return o,alpha
end

function bi_top3DKEOldKindofWorking(a,b,nx,ny,nz,p,v,KEs,Es,q,alpha_old,H,Hs,maxIter)
    alpha = copy(alpha_old)
    nu = 0.3;

    # PREPARE FINITE ELEMENT ANALYSIS
    # KE=lk_H8(nu)
    
    nelx=nx
    nely=ny
    nelz=nz
    nele = nelx*nely*nelz
    ndof = 3*(nelx+1)*(nely+1)*(nelz+1)
    nnx = nelx+1; nny = nely+1; nnz = nelz+1;

    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
    m= Matlab.meshgrid(nelx:nelx, 0:0, 0:nelz)
    il=m[1]
    jl=m[2]
    kl=m[3]
    loadnid = kl.*(nelx+1).*(nely+1).+il.*(nely+1).+(nely+1 .-jl)
    loaddof = 3 .*loadnid[:] .- 1; 
    # USER-DEFINED SUPPORT FIXED DOFs
    m= Matlab.meshgrid(0:0,0:nely,0:nelz)# Coordinates
    iif=m[1]
    jf=m[2]
    kf=m[3]
    fixednid = kf.*(nelx+1).*(nely+1) .+iif .*(nely .+1) .+(nely .+1 .-jf) # Node IDs
    fixeddof = [3 .*fixednid[:]; 3 .*fixednid[:].-1; 3 .*fixednid[:].-2] # DOFs
    
    
    # DEFINE LOADS AND SUPPORTS (HALF MBB-BEAM)
    F= sparse(loaddof,fill(1,length(loaddof)),fill(-1,length(loaddof)),ndof,1);
    U = zeros(ndof,1)
    alldofs = 1:ndof
    freedofs = setdiff(1:ndof,fixeddof)
    
    o=0;alpha_a=0
    # inner iteration
    for i =0: (maxIter-1)
        # FE-ANALYSIS
        # KE[:]*E[:]'
        KEE = KEs[1][:]*alpha[:,1][:]'.^q
        for phase = 2:p
            KEE = KEE + KEs[phase][:]*alpha[:,phase][:]'.^q;
        end

        EE = mean(Es[1])*alpha[:,1].^q
        for phase = 2:p
            EE = EE + mean(Es[phase])*alpha[:,phase].^q;
        end

        

        sK = reshape(KEE[:],24*24*nelx*nely*nelz,1)
        K = sparse(vec(iK),vec(jK),vec(sK)); K = (K+K')/2
        @timed U[freedofs] = K[freedofs,freedofs] \ Array(F[freedofs])


        

        KE=reshape(sum(KEE,dims=2),24,24)
        ce=zeros(nelx* nely* nelz)

        # display(size(U[edofMat]))

        for i=1:Int(nelx* nely* nelz)
            KE = KEs[1].*alpha[i,1]
            for phase = 2:p
                KE = KE + KEs[phase].*alpha[i,phase]
            end
            # ce[i]=sum((U[edofMat][i,:]'*reshape(KE[:,i],24,24)).*U[edofMat][i,:]')
            ce[i]=sum((U[edofMat][i,:]'*KE).*U[edofMat][i,:]')
        end

        # display(size(KEE))
        # OBJECTIVE FUNCTION AND SENSITIVITY ANALYSIS
        # cc = 0.0; 
        # dc = zeros(nelx, nely, nelz);
        # for elz = 1:nelz
        #     for ely = 1:nely 
        #         for elx = 1:nelx
        #             # KE   = get_KE(truss, x, Es, vs, Gs, elx, ely, elz, false);
        #             # DKE  = get_KE(truss, x, Es, vs, Gs, elx, ely, elz, true);
        #             # KE=reshape(KEEE[:,get_num(nelx, nely, nelz, elx, ely, elz)] ,24,24)
        #             # display(get_num1(nelx, nely, nelz, elx, ely, elz))
        #             # display(get_num(nnx, nny, nnz, elx, ely, elz))

        #             # KE = (KEs[a].*reshape(alpha[:,a][:],nelx, nely, nelz)[elx, ely, elz])-(KEs[b].*reshape(alpha[:,b][:],nelx, nely, nelz)[elx, ely, elz])


        #             # KE = KEs[1].*reshape(alpha[:,1][:],nelx, nely, nelz)[elx, ely, elz]
        #             # for phase = 2:p
        #             #     KE = KE + KEs[phase].*reshape(alpha[:,phase][:],nelx, nely, nelz)[elx, ely, elz];
        #             # end
        #             DKE=KE
        #             dofs1 = get_elem_dofs(nnx, nny, nnz, elx, ely, elz);
        #             dofs1 = get_elem_dofs(nelx, nely, nelz, elx, ely, elz);
        #             # display(dofs1)
        #             Ue = U[dofs1];
        #             cc = cc + Ue'*KE*Ue;
        #             dc[elx,ely,elz] = Ue'*DKE*Ue;
        #         end
        #     end
        # end

        # display(size(dc))
        # display(c)

        # ce=dc[:]
        # o=cc


        # display(ce)
        # display(sum(o))
        # dc=dc[:]

        

        
        # Objective function and sensitivity analysis
        # ce= sum((U[edofMat]*KE).*U[edofMat],dims=2)
        o = sum(sum(EE.*ce))

        # display(ce)
        # display(sum(o1))

        # display(size(ce))
        # display(size(EE))
        
        # FILTERING OF SENSITIVITIES 
        dc = 0.0 .-(q .*mean(Es[a].-Es[b]).*alpha[:,a].^(q-1)).*ce;
        dc = H*(alpha[:,a].*dc)./Hs./max.(1e-3,alpha[:,a])
        dc = min.(dc,0.0)

        # UPDATE LOWER AND UPPER BOUNDS OF DESIGN VARIABLES
        move = 0.2;
        r = ones(nx*ny*nz,1)
  
        for k = 1:p
            #if (k ~= a) && (k ~= b)
            if (k != a) && (k != b)
                # display("k $k a $a b $b")
                r = r .- alpha[:,k]
            end
        end
        l = max.(0,alpha[:,a] .-move)
        u = min.(r,alpha[:,a] .+move)
        # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES
        l1 = 0; l2 = 1e9
        while (l2-l1)/(l1+l2) > 1e-3
            lmid = 0.5*(l2+l1)
            alpha_a = max.(l,min.(u,alpha[:,a].*sqrt.(-dc./lmid)));
            if sum(alpha_a) > nx*ny*v[a]
                l1 = lmid
            else
                l2 = lmid 
            end
        end
        alpha[:,a] = alpha_a
        alpha[:,b] = r .-alpha_a

    end
    return o,alpha
end

####
########

function multitop3D_compliantKE(nx,ny,nz,tol_out,tol_f,iter_max_in,iter_max_out,p,q,KEs,Es,v,rf,getProblem=inverter)
    anim=Animation()
    alpha = zeros(Float64,nx*ny*nz,p)
    for i = 1:p
        alpha[:,i] .= v[i]
    end
    
    # MAKE FILTER
    H,Hs = make_filter3D(nx,ny,nz,rf)
    obj=0
    change_out = 2*tol_out; iter_out = 0;
    while (iter_out < iter_max_out) && (change_out > tol_out)
        alpha_old = copy(alpha)
        for a = 1:p
            for b = a+1:p
                obj,alpha = bi_top3D_compliantKE(a,b,nx,ny,nz,p,v,KEs,Es,q,alpha,H,Hs,iter_max_in,getProblem);
            end
        end
        iter_out = iter_out + 1;
        change_out = norm(alpha[:].-alpha_old[:],Inf);
        display("Iter:$iter_out Obj.:$obj change:$change_out");
        #  UPDATE FILTER
        if (change_out < tol_f) && (rf>3)
            tol_f = 0.99*tol_f; rf = 0.99*rf; 
            H,Hs = make_filter3D(nx,ny,nz,rf)
        end
        #  SCREEN OUT TEMPORAL TOPOLOGY EVERY 5 ITERATIONS
        if mod(iter_out,5)==0
            I = make_bitmap_3d(p,nx,ny,nz,alpha)
            threshold=0.05
            display(topplotmulti3d(nx,ny,nz,I,threshold))
            frame(anim)
        end
    end
    return anim,alpha
end

function bi_top3D_compliantKE(a,b,nx,ny,nz,p,v,KEs,Es,q,alpha_old,H,Hs,maxIter,getProblem=inverter)
    alpha = copy(alpha_old)
    nu = 0.3;

    
    
    # PREPARE FINITE ELEMENT ANALYSIS
    # KE=lk_H8(nu)
    
    nelx=nx
    nely=ny
    nelz=nz
    nele = nelx*nely*nelz
    ndof = 3*(nelx+1)*(nely+1)*(nelz+1)

    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)

    
    
    o=0;alpha_a=0
    # inner iteration
    for i =0: (maxIter-1)
        # FE-ANALYSIS
        # E = e[1]*alpha[:,1].^q
        # for phase = 2:p
        #     E = E + e[phase]*alpha[:,phase].^q;
        # end

        KEE = KEs[1][:]*alpha[:,1][:]'.^q
        for phase = 2:p
            KEE = KEE + KEs[phase][:]*alpha[:,phase][:]'.^q
        end

        # EE = mean(Es[1])*alpha[:,1].^q
        # for phase = 2:p
        #     EE = EE + mean(Es[phase])*alpha[:,phase].^q;
        # end

        

        sK = reshape(KEE[:],24*24*nelx*nely*nelz,1)
        

        # sK = reshape(KE[:]*E[:]',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])
        @timed U[freedofs,:] = K[freedofs,freedofs] \ Array(F[freedofs,:])
        U1 = U[:,1]
        U2 = U[:,2]

        KE=reshape(sum(KEE,dims=2),24,24)
        # ce=zeros(nelx* nely* nelz)
        # for i=1:Int(nelx* nely* nelz)
        #     KE = KEs[1].*alpha[i,1]
        #     for phase = 2:p
        #         KE = KE + KEs[phase].*alpha[i,phase]
        #     end
        #     ce[i]=-sum((U1[edofMat][i,:]'*KE).*U2[edofMat][i,:]')
        # end

        ce=zeros(nelx* nely* nelz)
        for i=1:Int(nelx* nely* nelz)
            KE = KEs[1].*alpha[i,1]
            for phase = 2:p
                KE = KE + KEs[phase].*alpha[i,phase]
            end
            ce[i]=-sum((U1[edofMat][i,:]'*KE).*U2[edofMat][i,:]')
        end
    

        # Objective function and sensitivity analysis
        # ce = -sum((U1[edofMat]*KE).*U2[edofMat],dims=2)

        # o = sum(sum(E.*ce))
        o  = U[dout,1]
        
        # FILTERING OF SENSITIVITIES 
        # dc = 0.0 .-(q .*(e[a].-e[b]).*alpha[:,a].^(q-1)).*ce;
        dc = 0.0 .-(q .*mean(Es[a].-Es[b]).*alpha[:,a].^(q-1)).*ce;
        dc = H*(alpha[:,a].*dc)./Hs./max.(1e-3,alpha[:,a])
        dc = min.(dc,0.0)

        # UPDATE LOWER AND UPPER BOUNDS OF DESIGN VARIABLES
        move = 0.1;
        r = ones(nx*ny*nz,1)
  
        for k = 1:p
            #if (k ~= a) && (k ~= b)
            if (k != a) && (k != b)
                # display("k $k a $a b $b")
                r = r .- alpha[:,k]
            end
        end
        l = max.(0,alpha[:,a] .-move)
        u = min.(r,alpha[:,a] .+move)
        # OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES
        l1 = 0; l2 = 1e9
        # while (l2-l1)/(l1+l2) > 1e-3
        while (l2-l1)/(l1+l2) > 1e-4 && l2 > 1e-40
            lmid = 0.5*(l2+l1)
            # alpha_a = max.(l,min.(u,alpha[:,a].*sqrt.(-dc./lmid)));
            alpha_a = max.(l,min.(u,alpha[:,a].*((max.(1e-10,-dc./lmid)).^0.3 )));
            if sum(alpha_a) > nx*ny*v[a]
                l1 = lmid
            else
                l2 = lmid 
            end
        end
        alpha[:,a] = alpha_a
        alpha[:,b] = r .-alpha_a

    end
    return o,alpha
end

################################Probelms###################
function inverter(nelx,nely,nelz)
    ndof = 3*(nelx+1)*(nely+1)*(nelz+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;

    loaddof=[getIndex3d(1,1,1,1,nelx+1,nely+1,nelz+1,3),
        getIndex3d(nelx+1,1,1,1,nelx+1,nely+1,nelz+1,3)]
    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);

    return U,F,freedofs,din,dout
end

function wing(nelx,nely,nelz)
    ndof = 3*(nelx+1)*(nely+1)*(nelz+1)
    # USER-DEFINED LOAD DOFs
    # loaddof=[getIndex3d1(1,nely+1,1,1,nelx+1,nely+1,nelz+1,3),
    #     getIndex3d1(nelx+1,nely+1,1,1,nelx+1,nely+1,nelz+1,3)]
    # din = loaddof[1]; 
    # dout = loaddof[2];

    # F = sparse(loaddof,[1;2],[1;-1],ndof,2);

    loaddof=[getIndex3d(1,nely+1,1,1,nelx+1,nely+1,nelz+1,3),
        getIndex3d(nelx+1,nely+1,1,2,nelx+1,nely+1,nelz+1,3)]
    din = loaddof[1]; 
    dout = loaddof[2];

    F = sparse(loaddof,[1;2],[1;1],ndof,2);

    # F = fill(0.0,ndof,2);

    # F[loaddof[1],1]=1
    # F[loaddof[2],2]=-1
    # F[getIndex3d(1,1,1,2,nelx+1,nely+1,nelz+1,3):3:getIndex3d(nelx+1,nely+1,nelz+1,2,nelx+1,nely+1,nelz+1,3),1] .+= -0.01
    # F[getIndex3d(1,1,1,2,nelx+1,nely+1,nelz+1,3),1] = 1
    # F[loaddof[1],1].=F[loaddof[1],1].+1.0
    # F = sparse(loaddof,[1;2],[1;1],ndof,2);
    # F[:]


    # USER-DEFINED SUPPORT FIXED DOFs
    # m= Matlab.meshgrid(0:0,0:nely,0:nelz)# Coordinates
    # iif=m[1]
    # jf=m[2]
    # kf=m[3]
    # fixednid = kf.*(nelx+1).*(nely+1) .+iif .*(nely .+1) .+(nely .+1 .-jf) # Node IDs
    # fixeddof = [3 .*fixednid[:]; 3 .*fixednid[:].-1; 3 .*fixednid[:].-2] # DOFs

    # 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[:];

    fixeddof=union(fixeddof_s,getIndex3d(1,1,(nelz+1),1:3,nelx+1,nely+1,nelz+1,3))
    fixeddof=union(fixeddof,getIndex3d(1,2,(nelz+1),1:3,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(getIndex3d(1,1,1:(nelz+1),1,nelx+1,nely+1,nelz+1,3),getIndex3d(1,1,1:(nelz+1),2,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,1,1:(nelz+1),3,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,2,1:(nelz+1),1,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,2,1:(nelz+1),2,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,2,1:(nelz+1),3,nelx+1,nely+1,nelz+1,3))
    fixeddof = sort(fixeddof);


    # Displacement vector
    U = zeros(ndof,2);
    freedofs = setdiff(1:ndof,fixeddof);

    return U,F,freedofs,din,dout
end

function wing1(nelx,nely,nelz)
    ndof = 3*(nelx+1)*(nely+1)*(nelz+1)
    # USER-DEFINED LOAD DOFs
    # loaddof=[getIndex3d1(1,nely+1,1,1,nelx+1,nely+1,nelz+1,3),
    #     getIndex3d1(nelx+1,nely+1,1,1,nelx+1,nely+1,nelz+1,3)]
    # din = loaddof[1]; 
    # dout = loaddof[2];

    # F = sparse(loaddof,[1;2],[1;-1],ndof,2);

    loaddof=[getIndex3d(nelx/2,nely+1,1,1,nelx+1,nely+1,nelz+1,3),
        getIndex3d(nelx+1,nely+1,1,2,nelx+1,nely+1,nelz+1,3)]
    din = loaddof[1]; 
    dout = loaddof[2];

    F = sparse(loaddof,[1;2],[-1;-1],ndof,2);


    # 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[:];

    fixeddof=union(fixeddof_s,getIndex3d(1,1,(nelz+1),1:3,nelx+1,nely+1,nelz+1,3))
    fixeddof=union(fixeddof,getIndex3d(1,2,(nelz+1),1:3,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(getIndex3d(1,1,1:(nelz+1),1,nelx+1,nely+1,nelz+1,3),getIndex3d(1,1,1:(nelz+1),2,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,1,1:(nelz+1),3,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,2,1:(nelz+1),1,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,2,1:(nelz+1),2,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,2,1:(nelz+1),3,nelx+1,nely+1,nelz+1,3))
    fixeddof = sort(fixeddof);


    # Displacement vector
    U = zeros(ndof,2);
    freedofs = setdiff(1:ndof,fixeddof);

    return U,F,freedofs,din,dout
end

function wing2(nelx,nely,nelz)
    ndof = 3*(nelx+1)*(nely+1)*(nelz+1)
    # USER-DEFINED LOAD DOFs
    # loaddof=[getIndex3d1(1,nely+1,1,1,nelx+1,nely+1,nelz+1,3),
    #     getIndex3d1(nelx+1,nely+1,1,1,nelx+1,nely+1,nelz+1,3)]
    # din = loaddof[1]; 
    # dout = loaddof[2];

    # F = sparse(loaddof,[1;2],[1;-1],ndof,2);

    loaddof=[getIndex3d(nelx+1,nely+1,1,2,nelx+1,nely+1,nelz+1,3),getIndex3d(1,nely+1,1,1,nelx+1,nely+1,nelz+1,3)]
    din = loaddof[1]; 
    dout = loaddof[2];

    F = sparse(loaddof,[1;2],[2;1],ndof,2);

    # F = fill(0.0,ndof,2);

    # F[loaddof[1],1]=1
    # F[loaddof[2],2]=-1
    # F[getIndex3d(1,1,1,2,nelx+1,nely+1,nelz+1,3):3:getIndex3d(nelx+1,nely+1,nelz+1,2,nelx+1,nely+1,nelz+1,3),1] .+= -0.01
    # F[getIndex3d(1,1,1,2,nelx+1,nely+1,nelz+1,3),1] = 1
    # F[loaddof[1],1].=F[loaddof[1],1].+1.0
    # F = sparse(loaddof,[1;2],[1;1],ndof,2);
    # F[:]


    # USER-DEFINED SUPPORT FIXED DOFs
    # m= Matlab.meshgrid(0:0,0:nely,0:nelz)# Coordinates
    # iif=m[1]
    # jf=m[2]
    # kf=m[3]
    # fixednid = kf.*(nelx+1).*(nely+1) .+iif .*(nely .+1) .+(nely .+1 .-jf) # Node IDs
    # fixeddof = [3 .*fixednid[:]; 3 .*fixednid[:].-1; 3 .*fixednid[:].-2] # DOFs

    # 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[:];

    fixeddof=union(fixeddof_s,getIndex3d(1,1,(nelz+1),1:3,nelx+1,nely+1,nelz+1,3))
    fixeddof=union(fixeddof,getIndex3d(1,2,(nelz+1),1:3,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(getIndex3d(1,1,1:(nelz+1),1,nelx+1,nely+1,nelz+1,3),getIndex3d(1,1,1:(nelz+1),2,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,1,1:(nelz+1),3,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,2,1:(nelz+1),1,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,2,1:(nelz+1),2,nelx+1,nely+1,nelz+1,3))
    # fixeddof=union(fixeddof,getIndex3d(1,2,1:(nelz+1),3,nelx+1,nely+1,nelz+1,3))
    fixeddof = sort(fixeddof);


    # Displacement vector
    U = zeros(ndof,2);
    freedofs = setdiff(1:ndof,fixeddof);

    return U,F,freedofs,din,dout
end

#############################################2d Periodic############################################################
# Based on https://link.springer.com/article/10.1007/s00158-015-1294-0
## PERIODIC MATERIAL MICROSTRUCTURE DESIGN
function topX(nelx,nely,volfrac,penal,rmin,ft,optType="bulk")
    ## MATERIAL PROPERTIES
    E0 = 1;
    Emin = 1e-9;
    nu = 0.3;
    ## PREPARE FINITE ELEMENT ANALYSIS
    A11 = [12  3 -6 -3;  3 12  3  0; -6  3 12 -3; -3  0 -3 12]'
    A12 = [-6 -3  0  3; -3 -6 -3 -6;  0 -3 -6  3;  3 -6  3 -6]'
    B11 = [-4  3 -2  9;  3 -4 -9  4; -2 -9 -4 -3;  9  4 -3 -4]'
    B12 = [ 2 -3  4 -9; -3  2  9 -2;  4  9  2  3; -9 -2  3  2]'
    KE = 1/(1-nu^2)/24*([A11 A12;A12' A11]+nu*[B11 B12;B12' B11])

    
    nodenrs = reshape(1:(1+nelx)*(1+nely),1+nely,1+nelx)
    edofVec = reshape(2*nodenrs[1:end-1,1:end-1].+1,nelx*nely,1)
    edofMat = repeat(edofVec,1,8).+repeat([0 1 2*nely.+[2 3 0 1] -2 -1],nelx*nely,1)

    iK = convert(Array{Int},reshape(kron(edofMat,ones(8,1))',64*nelx*nely,1))
    jK = convert(Array{Int},reshape(kron(edofMat,ones(1,8))',64*nelx*nely,1))
    
    ## PREPARE FILTER
    iH = ones(convert(Int,nelx*nely*(2*(ceil(rmin)-1)+1)^2),1)
    jH = ones(Int,size(iH))
    sH = zeros(size(iH))
    
    k = 0;
    for i1 = 1:nelx
        for j1 = 1:nely
            e1 = (i1-1)*nely+j1
            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 = (i2-1)*nely+j2
                    k = k+1
                    iH[k] = e1
                    jH[k] = e2
                    sH[k] = max(0,rmin-sqrt((i1-i2)^2+(j1-j2)^2))
                end
            end
        end
    end
    H = sparse(vec(iH),vec(jH),vec(sH))
    Hs = sum(H,dims=2)
    
    ## PERIODIC BOUNDARY CONDITIONS
    e0 = Matrix(1.0I, 3, 3);
    ufixed = zeros(8,3);
    
    U = zeros(2*(nely+1)*(nelx+1),3);
    alldofs = [1:2*(nely+1)*(nelx+1)];
    
    n1 = vcat(nodenrs[end,[1,end]],nodenrs[1,[end,1]]);
    d1 = vec(reshape([(2 .* n1 .-1) 2 .*n1]',1,8));
    n3 = [vec(nodenrs[2:(end-1),1]');vec(nodenrs[end,2:(end-1)])];
    d3 = vec(reshape([(2 .*n3 .-1) 2 .*n3]',1,2*(nelx+nely-2)));
    n4 = [vec(nodenrs[2:end-1,end]');vec(nodenrs[1,2:end-1])];
    d4 = vec(reshape([(2 .*n4 .-1) 2 .*n4]',1,2*(nelx+nely-2)));
    d2 = setdiff(vcat(alldofs...),union(union(d1,d3),d4));
    
        
    
    for j = 1:3
        ufixed[3:4,j] = [e0[1,j] e0[3,j]/2 ; e0[3,j]/2 e0[2,j]]*[nelx;0];
        ufixed[7:8,j] = [e0[1,j] e0[3,j]/2 ; e0[3,j]/2 e0[2,j]]*[0;nely];
        ufixed[5:6,j] = ufixed[3:4,j] .+ ufixed[7:8,j];
    end
    wfixed = [repeat(ufixed[3:4,:],nely-1,1); repeat(ufixed[7:8,:],nelx-1,1)];
    

    
    ## INITIALIZE ITERATION
    qe = Array{Any,2}(undef, 3, 3);
    Q = zeros(3,3);
    dQ = Array{Any,2}(undef, 3, 3);
    x = volfrac.*ones(nely,nelx)
    
    for i = 1:nelx
        for j = 1:nely
            vall=3
            if optType=="poisson"
                vall=6
            end
            if sqrt((i-nelx/2-0.5)^2+(j-nely/2-0.5)^2) < min(nelx,nely)/vall
                x[j,i] = volfrac/2.0;
            end
        end
    end
    xPhys = copy(x);
    change = 1;
    loop = 0;
    xnew=zeros(size(x))
    ## START ITERATION
    while (change > 0.01)
        loop = loop +1;
        ## FE-ANALYSIS
        sK = reshape(KE[:]*(Emin .+ xPhys[:]'.^penal*(80 .- Emin)),64*nelx*nely,1);
        
        K = sparse(vec(iK),vec(jK),vec(sK)); 
        K = (K.+K')./2.0;
        Kr = vcat(hcat(K[d2,d2] , K[d2,d3]+K[d2,d4]),hcat((K[d3,d2]+K[d4,d2]),(K[d3,d3]+K[d4,d3]+K[d3,d4]+K[d4,d4])));
        U[d1,:] .= ufixed;
        U[[d2;d3],:] = Kr\(-[K[d2,d1]; K[d3,d1]+K[d4,d1]]*ufixed-[K[d2,d4]; K[d3,d4]+K[d4,d4]]*wfixed);
        U[d4,:] = U[d3,:]+wfixed;
        
        
        ## OBJECTIVE FUNCTION AND SENSITIVITY ANALYSIS
        for i = 1:3
            for j = 1:3
                U1 = U[:,i]; U2 = U[:,j];
                qe[i,j] = reshape(sum((U1[edofMat]*KE).*U2[edofMat],dims=2),nely,nelx)./(nelx*nely);
                Q[i,j] = sum(sum((Emin .+ xPhys.^penal*(E0 .-Emin)).*qe[i,j]));
                dQ[i,j] = penal*(E0-Emin)*xPhys.^(penal-1).*qe[i,j];
            end
        end
        if optType=="bulk"
            #bulk
            c = -(Q[1,1]+Q[2,2]+Q[1,2]+Q[2,1]);
            dc = -(dQ[1,1]+dQ[2,2]+dQ[1,2]+dQ[2,1]);
        elseif optType=="shear"
            #shear
            c=-Q[3,3];
            dc=-dQ[3,3];
        elseif optType=="poisson"
            c = Q[1,2]-(0.8^loop)*(Q[1,1]+Q[2,2]);
            dc = dQ[1,2]-(0.8^loop)*(dQ[1,1]+dQ[2,2]);
        end
        
        dv = ones(nely,nelx);
        ## FILTERING/MODIFICATION OF SENSITIVITIES
        if ft == 1
            dc[:] = H*(x[:].*dc[:])./Hs./max.(1e-3,x[:]);
        elseif ft == 2
            dc[:] = H*(dc[:]./Hs);
            dv[:] = H*(dv[:]./Hs);
        end
        ## OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES AND PHYSICAL DENSITIES
            
        if optType=="poisson"
            l1=0; l2=1e9; move = 0.1;
        else
            l1 =0; l2 = 1e9; move = 0.2;
        end
        while (l2-l1 > 1e-9)
            lmid = 0.5*(l2+l1);
            if optType=="poisson"
                xnew = max.(0,max.(x.-move,min.(1.0,min.(x.+move,x.*(-dc./dv./lmid)))));
            else
                xnew =max.(0.0,max.(x.-move,min.(1.0,min.(x.+move,x.*sqrt.(0.0.-dc./dv./lmid)))));
            end
            if ft == 1
                xPhys = copy(xnew);
            elseif ft == 2
                xPhys[:] = (H*xnew[:])./Hs;
            end
            if mean(xPhys[:]) > volfrac
                l1 = lmid;
            else
                l2 = lmid;
            end
        end
        change = maximum(abs.(xnew[:].-x[:]))
        x = xnew;
        ## PRINT RESULTS
        display(" It:$loop Obj:$c Vol:$(mean(xPhys[:])) ch:$change ")

        ## PLOT DENSITIES
        heatmap(xPhys, aspect_ratio=:equal, legend=false, axis=nothing, foreground_color_subplot=colorant"white",fc=:grays,clims=(0.0, 1.0))
        frame(anim)
    end
    return xnew
end

######################3d Periodic#########################################
function topX3D(nelx,nely,nelz,volfrac,penal,rmin,ft,optType="bulk")
    ## MATERIAL PROPERTIES
    E0 = 1;
    Emin = 1e-9;
    nu = 0.3;
    ## PREPARE FINITE ELEMENT ANALYSIS
    KE=lk_H8(nu)

    Num_node = (1+nely)*(1+nelx)*(1+nelz);
    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 = reshape(kron(edofMat,ones(24,1))',24*24*nele,1);
    jK = reshape(kron(edofMat,ones(1,24))',24*24*nele,1);

    ## PREPARE FILTER
    iH = ones(convert(Int,nelx*nely*(2*(ceil(rmin)-1)+1)^2),1)
    jH = ones(Int,size(iH))
    sH = zeros(size(iH))
    
    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;
                            iH[k] = e1;
                            jH[k] = e2;
                            sH[k] = max(0,rmin-sqrt((i1-i2)^2+(j1-j2)^2+(k1-k2)^2));
                        end
                    end
                end
            end
        end
    end
    H = sparse(vec(iH),vec(jH),vec(sH))
    Hs = sum(H,dims=2)
    
    ## PERIODIC BOUNDARY CONDITIONS
    e0 = Matrix(1.0I, 6, 6);
    ufixed = zeros(24,6);
    
    U = zeros(2*(nely+1)*(nelx+1),3);
    alldofs = [1:2*(nely+1)*(nelx+1)];
    
    n1 = vcat(nodenrs[end,[1,end]],nodenrs[1,[end,1]]);
    d1 = vec(reshape([(2 .* n1 .-1) 2 .*n1]',1,8));
    n3 = [vec(nodenrs[2:(end-1),1]');vec(nodenrs[end,2:(end-1)])];
    d3 = vec(reshape([(2 .*n3 .-1) 2 .*n3]',1,2*(nelx+nely-2)));
    n4 = [vec(nodenrs[2:end-1,end]');vec(nodenrs[1,2:end-1])];
    d4 = vec(reshape([(2 .*n4 .-1) 2 .*n4]',1,2*(nelx+nely-2)));
    d2 = setdiff(vcat(alldofs...),union(union(d1,d3),d4));

    # n1 = [nodenrs[end,[1,end]],nodenrs[1,[end,1]]];
    # d1 = reshape( [(2*n1-1);2*n1],1,8);
    # n3 = [nodenrs[2:end-1,1]',nodenrs[end,2:end-1]];
    # d3 = reshape( [(2*n3-1);2*n3],1,2*(nelx+nely-2));
    # n4 = [nodenrs[2:end-1,end]',nodenrs[1,2:end-1]];
    # d4 = reshape( [(2*n4-1);2*n4],1,2*(nelx+nely-2));
    # d2 = setdiff(alldofs,[d1,d3,d4] );

    n1 = vcat(nodenrs[end, [1 end], 1], nodenrs[1, [end 1], 1], nodenrs[end, [1 end], end], nodenrs[1, [end 1], end]);
    d1 = vec(reshape( [3 .*n1-2 3 .*n1-1 3 .*n1]',3*numel(n1),1));
    n3 = vcat(reshape(squeeze(nodenrs[end,1,2:end-1] ),1,numel(squeeze(nodenrs[end,1,2:end-1] ))),               # AE
          reshape(squeeze(nodenrs[1, 1, 2:end-1] ),1,numel(squeeze(nodenrs[1, 1, 2:end-1] ))),               # DH
          reshape(squeeze(nodenrs[end,2:end-1,1] ),1,numel(squeeze(nodenrs[end,2:end-1,1] ))) ,              # AB
          reshape(squeeze(nodenrs[1, 2:end-1, 1] ),1,numel(squeeze(nodenrs[1, 2:end-1, 1] ))) ,              # DC
          reshape(squeeze(nodenrs[2:end-1, 1, 1] ),1,numel(squeeze(nodenrs[2:end-1, 1, 1] ))),               # AD
          reshape(squeeze(nodenrs[2:end-1,1,end] ),1,numel(squeeze(nodenrs[2:end-1,1,end] ))) ,              # EH
          reshape(squeeze(nodenrs[2:end-1, 2:end-1, 1] ),1,numel(squeeze(nodenrs[2:end-1, 2:end-1, 1] ))) ,  # ABCD
          reshape(squeeze(nodenrs[2:end-1, 1, 2:end-1] ),1,numel(squeeze(nodenrs[2:end-1, 1, 2:end-1] ))) ,  # ADHE
          reshape(squeeze(nodenrs[end,2:end-1,2:end-1] ),1,numel(squeeze(nodenrs[end,2:end-1,2:end-1] ))));   # ABFE                   
    d3 = vec(reshape( [3 .*n3-2 3 .*n3-1 3 .*n3]',3*numel(n3),1));
    n4 = vcat(reshape(squeeze(nodenrs[1, end, 2:end-1] ),1,numel(squeeze(nodenrs[1, end, 2:end-1] ))),           # CG
          reshape(squeeze(nodenrs[end,end,2:end-1] ),1,numel(squeeze(nodenrs[end,end,2:end-1] ))) ,          # BF
          reshape(squeeze(nodenrs[1, 2:end-1, end] ),1,numel(squeeze(nodenrs[1, 2:end-1, end] ))) ,          # HG
          reshape(squeeze(nodenrs[end,2:end-1,end] ),1,numel(squeeze(nodenrs[end,2:end-1,end] ))) ,          # EF
          reshape(squeeze(nodenrs[2:end-1,end,end] ),1,numel(squeeze(nodenrs[2:end-1,end,end] ))) ,          # FG
          reshape(squeeze(nodenrs[2:end-1, end, 1] ),1,numel(squeeze(nodenrs[2:end-1, end, 1] ))) ,          # BC
          reshape(squeeze(nodenrs[2:end-1,2:end-1,end] ),1,numel(squeeze(nodenrs[2:end-1,2:end-1,end] ))) ,  # EFGH
          reshape(squeeze(nodenrs[2:end-1,end,2:end-1] ),1,numel(squeeze(nodenrs[2:end-1,end,2:end-1] ))) ,  # BCGF
          reshape(squeeze(nodenrs[1, 2:end-1, 2:end-1] ),1,numel(squeeze(nodenrs[1, 2:end-1, 2:end-1] ))));   # DCGH
    d4 = vec(reshape( [3*n4-2 3*n4-1 3*n4]',3*numel(n4),1));
    n2 = setdiff(nodenrs[:],union(n1[:],union(n3[:],n4[:])) ); 
    d2 = vec(reshape( [3*n2-2 3*n2-1 3*n2]',3*numel(n2),1));
    
    # for j = 1:3
    #     ufixed[3:4,j] = [e0[1,j] e0[3,j]/2 ; e0[3,j]/2 e0[2,j]]*[nelx;0];
    #     ufixed[7:8,j] = [e0[1,j] e0[3,j]/2 ; e0[3,j]/2 e0[2,j]]*[0;nely];
    #     ufixed[5:6,j] = ufixed[3:4,j] .+ ufixed[7:8,j];
    # end
    # wfixed = [repeat(ufixed[3:4,:],nely-1,1); repeat(ufixed[7:8,:],nelx-1,1)];
    

    vert_cor = [0  lx lx  0  0 lx lx  0;
                0   0 ly ly  0  0 ly ly;
                0   0  0  0 lz lz lz lz];
    for i = 1:6
        epsilon = [  e[i,1] e[i,4]/2 e[i,6]/2;
                   e[i,4]/2   e[i,2] e[i,5]/2;
                   e[i,6]/2 e[i,5]/2   e[i,3]];
        ufixed[:,i] = reshape(epsilon*vert_cor,24);
    end
    # 3D boundary constraint equations
    wfixed = [repeat(ufixed[  7:9,:],numel(squeeze(nodenrs[end,1,2:end-1] )),1);                    # C
              repeat(ufixed[  4:6,:]-ufixed[10:12,:],numel(squeeze(nodenrs[1, 1, 2:end-1] )),1);    # B-D
              repeat(ufixed[22:24,:],numel(squeeze(nodenrs[end,2:end-1,1] )),1);                    # H
              repeat(ufixed[13:15,:]-ufixed(10:12,:),numel(squeeze(nodenrs[1, 2:end-1, 1] )),1);    # E-D
              repeat(ufixed[16:18,:],numel(squeeze(nodenrs[2:end-1, 1, 1] )),1);                    # F
              repeat(ufixed[  4:6,:]-ufixed[13:15,:],numel(squeeze(nodenrs[2:end-1,1,end] )),1);    # B-E
              repeat(ufixed[13:15,:],numel(squeeze(nodenrs[2:end-1, 2:end-1, 1] )),1);              # E
              repeat(ufixed[  4:6,:],numel(squeeze(nodenrs[2:end-1, 1, 2:end-1] )),1);              # B
              repeat(ufixed[10:12,:],numel(squeeze(nodenrs[end,2:end-1,2:end-1] )),1)];             # D
    

    
    ## INITIALIZE ITERATION
    qe = Array{Any,2}(undef, 6, 6);
    Q = zeros(6,6);
    dQ = Array{Any,2}(undef, 6, 6);
    x = volfrac.*ones(nely,nelx)
    
    for i = 1:nelx
        for j = 1:nely
            vall=3
            if optType=="poisson"
                vall=6
            end
            if sqrt((i-nelx/2-0.5)^2+(j-nely/2-0.5)^2) < min(nelx,nely)/vall
                x[j,i] = volfrac/2.0;
            end
        end
    end
    xPhys = copy(x);
    change = 1;
    loop = 0;
    xnew=zeros(size(x))
    ## START ITERATION
    while (change > 0.01)
        loop = loop +1;
        ## FE-ANALYSIS
        sK = reshape(KE[:]*(Emin .+ xPhys[:]'.^penal*(80 .- Emin)),24*24*nelx*nely,1);
        # sK = reshape(Ke[:]*(Emin+den[:]'.^penal*(1-Emin)),24*24*nele,1);

        K = sparse(vec(iK),vec(jK),vec(sK)); 
        K = (K.+K')./2.0;
        Kr = vcat(hcat(K[d2,d2] , K[d2,d3]+K[d2,d4]),hcat((K[d3,d2]+K[d4,d2]),(K[d3,d3]+K[d4,d3]+K[d3,d4]+K[d4,d4])));
        U[d1,:] .= ufixed;
        U[[d2;d3],:] = Kr\(-[K[d2,d1]; K[d3,d1]+K[d4,d1]]*ufixed-[K[d2,d4]; K[d3,d4]+K[d4,d4]]*wfixed);
        U[d4,:] = U[d3,:]+wfixed;
        
        
        ## OBJECTIVE FUNCTION AND SENSITIVITY ANALYSIS
        for i = 1:6
            for j = 1:6
                U1 = U[:,i]; U2 = U[:,j];
                qe[i,j] = reshape(sum((U1[edofMat]*KE).*U2[edofMat],dims=2),nely,nelx)./(nelx*nely);
                Q[i,j] = sum(sum((Emin .+ xPhys.^penal*(E0 .-Emin)).*qe[i,j]));
                dQ[i,j] = penal*(E0-Emin)*xPhys.^(penal-1).*qe[i,j];
            end
        end
        if optType=="bulk"
            #bulk
            c = -(Q[1,1]+Q[2,2]+Q[1,2]+Q[2,1]);
            dc = -(dQ[1,1]+dQ[2,2]+dQ[1,2]+dQ[2,1]);
        elseif optType=="shear"
            #shear
            c=-Q[3,3];
            dc=-dQ[3,3];
        elseif optType=="poisson"
            c = Q[1,2]-(0.8^loop)*(Q[1,1]+Q[2,2]);
            dc = dQ[1,2]-(0.8^loop)*(dQ[1,1]+dQ[2,2]);
        end
        
        dv = ones(nely,nelx);
        ## FILTERING/MODIFICATION OF SENSITIVITIES
        if ft == 1
            dc[:] = H*(x[:].*dc[:])./Hs./max.(1e-3,x[:]);
        elseif ft == 2
            dc[:] = H*(dc[:]./Hs);
            dv[:] = H*(dv[:]./Hs);
        end
        ## OPTIMALITY CRITERIA UPDATE OF DESIGN VARIABLES AND PHYSICAL DENSITIES
            
        if optType=="poisson"
            l1=0; l2=1e9; move = 0.1;
        else
            l1 =0; l2 = 1e9; move = 0.2;
        end
        while (l2-l1 > 1e-9)
            lmid = 0.5*(l2+l1);
            if optType=="poisson"
                xnew = max.(0,max.(x.-move,min.(1.0,min.(x.+move,x.*(-dc./dv./lmid)))));
            else
                xnew =max.(0.0,max.(x.-move,min.(1.0,min.(x.+move,x.*sqrt.(0.0.-dc./dv./lmid)))));
            end
            if ft == 1
                xPhys = copy(xnew);
            elseif ft == 2
                xPhys[:] = (H*xnew[:])./Hs;
            end
            if mean(xPhys[:]) > volfrac
                l1 = lmid;
            else
                l2 = lmid;
            end
        end
        change = maximum(abs.(xnew[:].-x[:]))
        x = xnew;
        ## PRINT RESULTS
        display(" It:$loop Obj:$c Vol:$(mean(xPhys[:])) ch:$change ")

        ## PLOT DENSITIES
        heatmap(xPhys, aspect_ratio=:equal, legend=false, axis=nothing, foreground_color_subplot=colorant"white",fc=:grays,clims=(0.0, 1.0))
        frame(anim)
    end
    return xnew
end

#####################################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

function make_filter(nelx,nely,rmin)
    iH = ones(convert(Int,nelx*nely*(2*(ceil(rmin)-1)+1)^2),1)
    jH = ones(Int,size(iH))
    sH = zeros(size(iH))
    k = 0;
    for i1 = 1:nelx
        for j1 = 1:nely
            e1 = (i1-1)*nely+j1
            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 = (i2-1)*nely+j2
                    k = k+1
                    iH[k] = e1
                    jH[k] = e2
                    sH[k] = max(0,rmin-sqrt((i1-i2)^2+(j1-j2)^2))
                end
            end
        end
    end
    H = sparse(vec(iH),vec(jH),vec(sH))
    Hs = sum(H,dims=2)
    return H,Hs
end

function make_filter3D(nelx,nely,nelz,rmin)
    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)
    return H,Hs
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]';
                
                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;
                # display(dN)
                # display(G)
                
                Be = L*G*dN;
                Ke = Ke + GaussWeigh[ii]*GaussWeigh[jj]*GaussWeigh[kk]*det(J)*(Be'*DH*Be);
            end
        end
    end
    return Ke
end

######### 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

#####################################Ployt####################################################################
function topplot3d(xPhys)
    ix=[]
    iy=[]
    iz=[]
    for j in 1:nely
        for i in 1:nelx
            for k in 1:nelz
                if(xPhys[j,i,k]>0.0)
                    append!(ix,i)
                    append!(iy,j)
                    append!(iz,k)
                end
            end
        end
    end
    # r = 4.0
    # lim = FRect3D((-4,-4,-4*r),(8,8,8*r))
    return scatter(ix,iz,iy,color="black",label="",markersize =4, aspect_ratio=:equal,markerstrokealpha = 0.2,markeralpha = 0.6,markershape = :square,camera = (30, 60),xlim=(0,nelx),zlim=(0,nely),ylim=(-10,10))#,markershape = :square
end

function plotDisplacement(nelx,nely,nelz,xPhysC,getProblem=inverter,factor=0.04,cameraX=30,cameraY=60)
    U,ndof,freedofs=getDisplacement(xPhysC,nelx,nely,nelz,getProblem)
    displacement=reshape((U[:,2]),3,(nely+1),(nelx+1),(nelz+1));

    anim1=Animation()
    for step in 0:10
        ix=[]
        iy=[]
        iz=[]
        exg=factor*step
        for j in 1:nely
            for i in 1:nelx
                for k in 1:nelz
                    if(xPhysC[j,i,k]>0.0)
                        append!(ix,i+exg*(displacement[1,j,i,k]))
                        append!(iy,j+exg*(displacement[2,j,i,k]))
                        append!(iz,k+exg*(displacement[3,j,i,k]))
                    end
                end
            end
        end
        # r = 4.0
        # lim = FRect3D((-4,-4,-4*r),(8,8,8*r))
        scatter(ix,iz,iy,color="black",label="",markersize =4, aspect_ratio=:equal,markerstrokealpha = 0.2,markeralpha = 0.6,markershape = :square,camera = (cameraX,cameraY),xlim=(0,nelx*1.5),zlim=(-0.5*nely,nely*1.5),ylim=(-10,10))#,markershape = :square
        frame(anim1)
    end
    return anim1
    
end

function make_bitmap(p,nx,ny,alpha)
    color = [1 0 0; 0 0 .45; 0 1 0; 0 0 0; 1 1 1];
    I = zeros(nx*ny,3);
    for j = 1:p
        I[:,1:3] = I[:,1:3] + alpha[:,j] .*color[j,1:3]';
    end
    # I = imresize(reshape(I,ny,nx,3),10,'bilinear');
    I=reshape(I,ny,nx,3)
    II=hcat(I[:,end:-1:1,:],I)
    return II
end

function make_bitmap_compliant(p,nx,ny,alpha)
    color = [1 0 0; 0 0 .45; 0 1 0; 0 0 0; 1 1 1];
    I = zeros(nx*ny,3);
    for j = 1:p
        I[:,1:3] = I[:,1:3] + alpha[:,j] .*color[j,1:3]';
    end
    # I = imresize(reshape(I,ny,nx,3),10,'bilinear');
    I=reshape(I,ny,nx,3)
    # II=hcat(I[:,end:-1:1,:],I)
    II=vcat(I[end:-1:1,:,:],I)
    return II
end

function make_bitmap_3d(p,nx,ny,nz,alpha)
    color = [1 0 0; 0 0 1; 0 1 0; 0 0 0; 1 1 1];
    I = zeros(nx*ny*nz,3);
    for j = 1:p
        I[:,1:3] = I[:,1:3] + alpha[:,j] .*color[j,1:3]';
    end
    # I = imresize(reshape(I,ny,nx,3),10,'bilinear');
    I=reshape(I,ny,nx,nz,3)
    II=hcat(I[:,end:-1:1,:,:],I)
    return II
end

function topplotmulti3d(nelx,nely,nelz,I,threshold)
    ixr=[]
    iyr=[]
    izr=[]
    
    ixg=[]
    iyg=[]
    izg=[]
    
    ixb=[]
    iyb=[]
    izb=[]
    for j in 1:nely
        for i in 1:nelx
            for k in 1:nelz
                if(I[j,i,k,1]>threshold)
                    append!(ixr,i)
                    append!(iyr,j)
                    append!(izr,k)
                end
                if(I[j,i,k,2]>threshold)
                    append!(ixg,i)
                    append!(iyg,j)
                    append!(izg,k)
                end
                if(I[j,i,k,3]>threshold)
                    append!(ixb,i)
                    append!(iyb,j)
                    append!(izb,k)
                end
            end
        end
    end
    p=scatter(ixr,izr,iyr,color="red",label="",markersize =4, aspect_ratio=:equal,markerstrokealpha = 0.2,markeralpha = 0.6,markershape = :square,camera = (30, 60),xlim=(0,nelx),zlim=(0,nely),ylim=(-10,10))#,markershape = :square
    p=scatter!(ixg,izg,iyg,color="green",label="",markersize =4, aspect_ratio=:equal,markerstrokealpha = 0.2,markeralpha = 0.6,markershape = :square,camera = (30, 60),xlim=(0,nelx),zlim=(0,nely),ylim=(-10,10))#,markershape = :square
    p=scatter!(ixb,izb,iyb,color="blue",label="",markersize =4, aspect_ratio=:equal,markerstrokealpha = 0.2,markeralpha = 0.6,markershape = :square,camera = (30, 60),xlim=(0,nelx),zlim=(0,nely),ylim=(-10,10))#,markershape = :square
    return p

end

function topplotmulti3d_mirror(nelx,nely,nelz,I,threshold,vertical)
    ixr=[]
    iyr=[]
    izr=[]
    
    ixg=[]
    iyg=[]
    izg=[]
    
    ixb=[]
    iyb=[]
    izb=[]
    if(vertical)
        for j in 1:nely
            for i in 1:nelx
                for k in 1:nelz
                    if(I[j,i,k,1]>threshold)
                        append!(ixr,i)
                        append!(ixr,i)
                        append!(iyr,nely-j)
                        append!(iyr,j+nely-1)
                        append!(izr,k)
                        append!(izr,k)
                    end
                    if(I[j,i,k,2]>threshold)
                        append!(ixg,i)
                        append!(ixg,i)
                        append!(iyg,nely-j)
                        append!(iyg,j+nely-1)
                        append!(izg,k)
                        append!(izg,k)
                    end
                    if(I[j,i,k,3]>threshold)
                        append!(ixb,i)
                        append!(ixb,i)
                        append!(iyb,nely-j)
                        append!(iyb,j+nely-1)
                        append!(izb,k)
                        append!(izb,k)
                    end
                end
            end
        end
    else
        for j in 1:nely
            for i in 1:nelx
                for k in 1:nelz
                    if(I[j,i,k,1]>threshold)
                        append!(ixr,nelx-i+nelx-1)
                        append!(ixr,i)
                        append!(iyr,j)
                        append!(izr,k)
                        append!(iyr,j)
                        append!(izr,k)
                    end
                    if(I[j,i,k,2]>threshold)
                        append!(ixg,nelx-i+nelx-1)
                        append!(ixg,i)
                        append!(iyg,j)
                        append!(izg,k)
                        append!(iyg,j)
                        append!(izg,k)
                    end
                    if(I[j,i,k,3]>threshold)
                        append!(ixb,nelx-i+nelx-1)
                        append!(ixb,i)
                        append!(iyb,j)
                        append!(izb,k)
                        append!(iyb,j)
                        append!(izb,k)
                        
                    end
                end
            end
        end
    end
    p=scatter(ixr,izr,iyr,color="red",label="",markersize =4, aspect_ratio=:equal,markerstrokealpha = 0.2,markeralpha = 0.6,markershape = :square,camera = (30, 60),xlim=(0,nelx),zlim=(0,nely),ylim=(-10,10))#,markershape = :square
    p=scatter!(ixg,izg,iyg,color="green",label="",markersize =4, aspect_ratio=:equal,markerstrokealpha = 0.2,markeralpha = 0.6,markershape = :square,camera = (30, 60),xlim=(0,nelx),zlim=(0,nely),ylim=(-10,10))#,markershape = :square
    p=scatter!(ixb,izb,iyb,color="blue",label="",markersize =4, aspect_ratio=:equal,markerstrokealpha = 0.2,markeralpha = 0.6,markershape = :square,camera = (30, 60),xlim=(0,nelx),zlim=(0,nely),ylim=(-10,10))#,markershape = :square
    return p

end

function plotBoundaryConditions3D(nelx,nely,nelz,U,F,freedofs,z=1)
    display("Supports")
    UU=U[:,1]
    UU[freedofs].=1.0
    UUU=reshape(UU,3,nely+1,nelx+1,nelz+1)[1,:,:,z]
    display(heatmap(UUU,aspect_ratio=:equal))
    UUU=reshape(UU,3,nely+1,nelx+1,nelz+1)[2,:,:,z]
    display(heatmap(UUU,aspect_ratio=:equal))
    UUU=reshape(UU,3,nely+1,nelx+1,nelz+1)[3,:,:,z]
    display(heatmap(UUU,aspect_ratio=:equal))


    UU=U[:,2]
    UU[freedofs].=1.0
    UUU=reshape(UU,3,nely+1,nelx+1,nelz+1)[1,:,:,z]
    display(heatmap(UUU,aspect_ratio=:equal))
    UUU=reshape(UU,3,nely+1,nelx+1,nelz+1)[2,:,:,z]
    display(heatmap(UUU,aspect_ratio=:equal))
    UUU=reshape(UU,3,nely+1,nelx+1,nelz+1)[3,:,:,z]
    display(heatmap(UUU,aspect_ratio=:equal))

    display("Forces")
    display("din")
    FF=F[:,1]
    FFF=Array(reshape(FF,3,nely+1,nelx+1,nelz+1)[1,:,:,z])
    display(heatmap(FFF,aspect_ratio=:equal))
    FFF=Array(reshape(FF,3,nely+1,nelx+1,nelz+1)[2,:,:,z])
    display(heatmap(FFF,aspect_ratio=:equal))
    FFF=Array(reshape(FF,3,nely+1,nelx+1,nelz+1)[3,:,:,z])
    display(heatmap(FFF,aspect_ratio=:equal))

    display("dout")
    FF=F[:,2]
    FFF=Array(reshape(FF,3,nely+1,nelx+1,nelz+1)[1,:,:,z])
    display(heatmap(FFF,aspect_ratio=:equal))
    FFF=Array(reshape(FF,3,nely+1,nelx+1,nelz+1)[2,:,:,z])
    display(heatmap(FFF,aspect_ratio=:equal))
    FFF=Array(reshape(FF,3,nely+1,nelx+1,nelz+1)[3,:,:,z])
    display(heatmap(FFF,aspect_ratio=:equal))
end
#####################################Utils####################################################################
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