# Author:  JV Carstensen, CEE, MIT (JK Guest, Civil Eng, JHU)
# Revised: Aug 22 2017, JVC
# Revised: Amira Abdel-Rahman
# =======================================================================

function FEM_frame(data,A)

    # FEM solver for frame elements
    # Author:  JV Carstensen, CEE, MIT (JK Guest, Civil Eng, JHU)
    # Revised: Aug 22 2017, JVC
    # Revised: Amira Abdel-Rahman

    # =======================================================================



    # ---- READ IN DATA -----------------------------------------------------
    # read in external file
    E,f,dis,idb,ien,ndf,nel,nen,nnp,nsd,xn,len,Ke,Te= data;
    g=zeros(size(dis))
    g.=dis


    # ---- NUMBER THE EQUATIONS ---------------------------------------------
    # line 380
    id,neq = number_eq(idb,ndf,nnp);


    # ---- FORM THE ELEMENT STIFFNESS MATRICES ------------------------------
    # line 324
    nee = ndf*nen;                   # number of element equations
    Ke  = zeros(nee,nee,nel);
    # Te  = zeros(nen*1,nen*nsd,nel);  # *1 is specific to frame
    Te  = zeros(nee,nee,nel);
    for i = 1:nel
        Ke[:,:,i],Te[:,:,i] = Ke_frame(A[i],E[i],ien[:,i],nee,nsd,xn);
    end

    # display("Element Stiffness Matrix")
    # display(Ke)


    # ---- PERFORM GLOBAL TO LOCAL MAPPING ----------------------------------
    # line 237
    LM  = zeros(nee,nel);
    for i = 1:nel
        LM[:,i] = get_local_id(id,ien[:,i],ndf,nee,nen);
    end


    # ---- IF THERE IS FREE DEGREES OF FREEDOM, THEN SOLVE THE EQUILIBRIUM --
    if (neq > 0)

        # get global force vector (line 275)
        F = globalF(f,g,id,ien,Ke,LM,ndf,nee,nel,nen,neq,nnp);

        # solve Kd - F = 0 (line 521)
        d,K = solveEQ(F,LM,Ke,nee,nel,neq);
    end

    # display("F")
    # display(F)


    # ---- POST-PROCESS THE RESULTS -----------------------------------------
    # line 419
    dcomp,axial,stress,strain,Fe = post_processing(A,d,E,g,id,ien,Ke,ndf,nee,nel,nen,nnp,Te);
    # display("dcomp")
    # display(dcomp)

    # display("axial")
    # display(axial)

    # display("stress")
    # display(stress)
    # ---- COMPUTE REACTION FORCES ------------------------------------------
    # line 482
    Rcomp,idr = reactions(idb,ien,Fe,ndf,nee,nel,nen,nnp);
    # display("Rcomp")
    # display(Rcomp)

    # ---- PLOT THE STRUCTURE -----------------------------------------------
    # read in external file

    # set the plot factor for the thickness of the frame elements
    #  plot_fac_bar = 1/A[i];
    #  A_min        = 0;
    #  plot_frame(A,A_min,f,idr,ien,nel,nnp,nsd,plot_fac_bar,xn);
    return K,F,d,stress,dcomp,g
    # =======================================================================
        
end
# =======================================================================
function add_d2dcomp(dcomp,d,id,ndf,nnp)

    # function that adds the displacements of the free degrees of freedom to
    # the nodal displacements
    # -----------------------------------------------------------------------
    # dcomp(ndf,nnp)  = nodal displacements
    # d(neq,1)        = displacement at free degrees of freedom
    # id(ndf,nnp)     = equation numbers of degrees of freedom
    # ndf             = number of degrees of freedom per node
    # nnp             = number of nodal points

    #------------------------------------------------------------------------

    # loop over nodes and degrees of freedom
    for n=1:nnp
        for i=1:ndf

            # if it is a free dof then add the global displacement
            if (id[i,n]>0)       
                dcomp[Int(i),Int(n)] = dcomp[Int(i),Int(n)]+d[Int(id[Int(i),Int(n)])];
            end
        end
    end
    return dcomp
end
# =======================================================================

function add_loads_to_force(F,f,id,ndf,nnp)

    # function that adds nodal forces to the global force vector
    # -----------------------------------------------------------------------
    # F(neq,1)        = global force vector
    # f(ndf,nnp)      = prescribed nodal forces
    # id(ndf,nnp)     = equation numbers of degrees of freedom
    # ndf             = number of degrees of freedom per node
    # nnp             = number of nodal points

    #------------------------------------------------------------------------

    # loop over nodes and degrees of freedom
    for n = 1:nnp
        for i = 1:ndf

            # get the global equation number 
            M = id[i,n];  

            # if free degree of freedom, then add nodal load to global force 
            # vector
            if (M > 0)        
                F[Int(floor(M))] = F[Int(floor(M))] + f[Int(floor(i)),Int(floor(n))];
            end
        end
    end
    return F

end
# =======================================================================

function addforce(F,Fe,LM,nee)

    # function that adds element forces to the global force vector
    # -----------------------------------------------------------------------
    # F(neq,1)        = global force vector
    # Fe(nee,1)       = element force vector
    # LM(nee,nel)     = global to local map for the element
    # nee             = number of element equations

    # =======================================================================

    # loop over rows of Fe
    for i = 1:nee

        # get the global equation number for local equation i
        M = LM[i];     

        # if free dof (eqn number > 0) add to F vector
        if (M > 0)         
            F[Int(M)]=F[Int(M)]+Fe[Int(i)];
        end
    end
    return F


end
# =======================================================================

function get_de_from_dcomp(dcomp,ien,ndf,nen)

    #  extracts element displacement vector from complete displacement vector
    #------------------------------------------------------------------------
    # dcomp(ndf,nnp)  = nodal displacements
    # ien(nen,1)      = element connectivity
    # ndf             = number of degrees of freedom per node
    # nen             = number of element equations
    #
    # de(nen,1)       = element displacements
    # 
    #------------------------------------------------------------------------
    
    de = zeros((nen-1)*ndf+ndf,1);

    # loop over number of element nodes
    for i = 1:nen

        # loop over number of degrees of freedom per node
        for j = 1:ndf

            # get the local element number and place displacement in de
            leq     = (i-1)*ndf+j;  
            
            de[leq] = dcomp[j,Int(floor(ien[i]))];
        end
    end
    return de
end
# =======================================================================

function get_local_id(id,ien,ndf,nee,nen)

    # functions that performs the global to local mapping of equation numbers
    # -----------------------------------------------------------------------
    # id(ndf,nnp)     = equation numbers of degrees of freedom
    # ien(nen,1)      = element connectivity
    # ndf             = number of degrees of freedom per node
    # nee             = number of element equations
    # nen             = number of element equations
    #
    # LM(nee,1)       = global to local map for element

    # =======================================================================

    # initialize global-local mapping matrix
    LM  = zeros(nee,1);        

    # initialize local equation number counter
    k = 0;                      

    # loop over element nodes
    for i = 1:nen         

        # loop over degrees of freedom at each node
        for j = 1:ndf

            # update counter and prescribe global equation number
            k     = k+1;
            LM[k] = id[j,Int(floor(ien[i]))];
        end
    end
    return LM
end
# =======================================================================

function globalF(f,g,id,ien,Ke,LM,ndf,nee,nel,nen,neq,nnp) 

    # function that assembles the global load vector
    # -----------------------------------------------------------------------
    # id(ndf,nnp)     = equation numbers of degrees of freedom
    # f(ndf,nnp)      = prescribed nodal forces
    # g(ndf,nnp)      = prescribed nodal displacements
    # ien(nen,nel)    = element connectivities
    # Ke(nee,nee,nel) = element stiffness matrices
    # ndf             = number of degrees of freedom per node
    # nee             = number of element equations
    # nel             = number of elements
    # nen             = number of element equations
    # neq             = number of equations
    # nnp             = number of nodal points
    #
    # F(neq,1)        = global force vector

    # =======================================================================

    # initialize
    F = zeros(neq,1);

    # Insert applied loads into F
    F = add_loads_to_force(F,f,id,ndf,nnp);

    # Compute forces from applied displacements (ds~=0) and insert into F
    Fse = zeros(nee,nel);  

    # loop over elements
    for i = 1:nel

        # get dse for current element
        dse      = get_de_from_dcomp(g,ien[:,i],ndf,nen);  

        # compute element force
        Fse[:,i] = -Ke[:,:,i]*dse;

        # assemble elem force into global force vector
        F        = addforce(F,Fse[:,i],LM[:,i],nee);     
    end

    return F
end
# =======================================================================

function  Ke_frame(A,E,ien,nee,nsd,xn)

    # function that computes the global element stiffness matrix for a frame
    # element
    # -----------------------------------------------------------------------
    # A(1,1)          = cross-sectional area of elements
    # E(1,1)          = Young's modulus of elements
    # ien(nen,1)      = element connectivity
    # nee             = number of element equations
    # nsd             = number of spacial dimensions
    # xn(nsd,nnp)     = nodal coordinates
    #
    # Ke(nee,nee,1)   = global element stiffness matrix
    # Te(nee,nee,1)   = global to local transformation matrix for element

    # =======================================================================

    # form vector along axis of element using nodal coordinates
    v = xn[:,Int(floor(ien[2]))]-xn[:,Int(floor(ien[1]))];
    

    # compute the length of the element
    Le = norm(v,2);

    # rotation of parent domain
    #   rot=[ cos(theta_x)  cos(theta_y)  cos(theta_z) ]'
    rot = v/Le;

    l=rot[1]
    m=rot[2]
    n=rot[3]

    D=sqrt(l^2+m^2+n^2)
    # D=1.0

    b=sqrt(A)
    h=sqrt(A)

    G=E * 1 / 2 #todo shear_modulus
    ixx = b*h^3/12
    iyy = b*h^3/12
    j=b*h*(b*b+h*h)/12;#todo check
    l0=Le
    l02 = l0 * l0
    l03 = l0 * l0 * l0
    

    # local element stiffness matrix
    # ke = E*A/Le*[  1  -1
    #     -1   1 ];

    ke = [  [E*A/l0  0  0  0  0  0  -E*A/l0  0  0  0  0  0];
            [0  12*E*ixx/l03  0  0  0  6*E*ixx/l02  0  -12*E*ixx/l03  0  0  0  6*E*ixx/l02];
            [0  0  12*E*iyy/l03  0  -6*E*iyy/l02  0  0  0  -12*E*iyy/l03  0  -6*E*iyy/l02  0];
            [0  0  0  G*j/l0  0  0  0  0  0  -G*j/l0  0  0];
            [0  0  -6*E*iyy/l02  0  4*E*iyy/l0  0  0  0  6*E*iyy/l02  0  2*E*iyy/l0  0];
            [0  6*E*ixx/l02  0  0  0  4*E*ixx/l0  0  -6*E*ixx/l02  0  0  0  2*E*ixx/l0];
            [-E*A/l0  0  0  0  0  0  E*A/l0  0  0  0  0  0];
            [0  -12*E*ixx/l03  0  0  0  -6*E*ixx/l02  0  12*E*ixx/l03  0  0  0  -6*E*ixx/l02];
            [0  0  -12*E*iyy/l03  0  6*E*iyy/l02  0  0  0  12*E*iyy/l03  0  6*E*iyy/l02  0];
            [0  0  0  -G*j/l0  0  0  0  0  0  G*j/l0  0  0];
            [0  0  -6*E*iyy/l02  0  2*E*iyy/l0  0  0  0  6*E*iyy/l02  0  4*E*iyy/l0  0];
            [0  6*E*ixx/l02  0  0  0  2*E*ixx/l0  0  -6*E*ixx/l02  0  0  0  4*E*ixx/l0]];


    # Transformation matrix: global to local coordinate system
    if (nsd == 2)   # 2D case

        # # frame Te is nen x ndf*nen array
        # Te = [ rot[1]  rot[2]       0       0
        #             0       0  rot[1]  rot[2] ];
        # Frame Te is nen x ndf*nen array
        Te = [ rot[1]  rot[2]   0          0       0  0
              -rot[2]  rot[1]   0          0       0  0
                   0       0    1          0       0  0
                   0       0    0     rot[1]  rot[2]  0
                   0       0    0    -rot[2]  rot[1]  0
                   0       0    0          0       0  1];

    elseif (nsd == 3)   # 3D case

        # # frame Te is nen x ndf*nen array
        # Te = [ rot[1]  rot[2]  rot[3]       0       0       0  
        #             0       0       0  rot[1]  rot[2]  rot[3] ];
        # Frame Te is nen x ndf*nen array
        # Te = [ rot[1]  rot[2]  rot[3]       0       0       0  
        #             0       0       0  rot[1]  rot[2]  rot[3] ];


        Te = [       l       m  n      0       0 0       0       0 0       0      0 0
                  -m/D     l/D  0      0       0 0       0       0 0       0      0 0
                -l*n/D  -m*n/D  D      0       0 0       0       0 0       0      0 0
                     0       0  0      l       m n       0       0 0       0      0 0
                     0       0  0   -m/D     l/D 0       0       0 0       0      0 0
                     0       0  0 -l*n/D  -m*n/D D       0       0 0       0      0 0
                     0       0  0      0       0 0       l       m n       0      0 0
                     0       0  0      0       0 0    -m/D     l/D 0       0      0 0
                     0       0  0      0       0 0  -l*n/D  -m*n/D D       0      0 0
                     0       0  0      0       0 0       0       0 0       l      m n
                     0       0  0      0       0 0       0       0 0    -m/D    l/D 0
                     0       0  0      0       0 0       0       0 0  -l*n/D -m*n/D D];
    end
    # compute the global element stiffness matrix
    Ke = zeros(nee,nee);
    Ke = Te'*ke*Te;
    # println(size(Ke))
    # println(size(Te))

    return Ke,Te
end
# =======================================================================

function number_eq(idb,ndf,nnp)

    # function that numbers the unknown degrees of freedom (equations)
    # -----------------------------------------------------------------------
    # idb(ndf,nnp)    = 1 if the degree of freedom is prescribed, 0 otherwise
    # ndf             = number of degrees of freedom per node
    # nnp             = number of nodal points
    #
    # id(ndf,nnp)     = equation numbers of degrees of freedom
    # neq             = number of equations (tot number of degrees of freedom)

    # =======================================================================

    # initialize id and neq
    id  = zeros(ndf,nnp);  
    neq = 0;              

    # loop over nodes
    for n = 1:nnp

        # loop over degrees of freedom
        for i = 1:ndf
            if idb[i,n] == 0 

                # udate # of equations
                neq = neq + 1;      

                # if no prescribed displacement at dof i of node n
                #   => give an equation # different from 0
                id[i,n] = neq;      

            end
        end
    end
    return id,neq
end

# =======================================================================
function post_processing(A,d,E,g,id,ien,Ke,ndf,nee,nel,nen,nnp,Te)

    # function that performs post processing for frame elements
    # -----------------------------------------------------------------------
    # A(nel,1)        = cross-sectional area of elements
    # d(neq,1)        = displacements at free degrees of freedom
    # E(nel,1)        = Young's modulus of elements
    # g(ndf,nnp)      = prescribed nodal displacements
    # id(ndf,nnp)     = equation numbers of degrees of freedom
    # ien(nen,nel)    = element connectivities
    # Ke(nee,nee,nel) = element stiffness matrices
    # ndf             = number of degrees of freedom per node
    # nee             = number of element equations
    # nel             = number of elements
    # nen             = number of element equations
    # nnp             = number of nodes
    # Te(nee,nee,nel) = element transformation matrices
    #
    # dcomp(ndf,nnp)  = nodal displacements
    # axial(nel,1)    = axial element forces
    # stress(nel,1)   = element stresses
    # strain(nel,1)   = element strains
    # Fe(nee,nel)     = element forces

    # =======================================================================

    # get the total displacement of the structure in matrix form dcomp(nsd,nnp)
    dcomp = add_d2dcomp(g,d,id,ndf,nnp);

    # initalize evaluation of global element forces Fe, local element forces 
    # fe, axial forces, element stresses and strains
    Fe     = zeros(nee,nel);      
    de     = zeros(nee,nel);  
    fe     = zeros(nee,nel);  # element local force vector 
    axial  = zeros(nel,1); 
    stress = zeros(nel,1); 
    strain = zeros(nel,1); # element axial, stress, strain

    

    # loop over elements
    for i=1:nel

        # get the element displacaments
        de[:,i] = get_de_from_dcomp(dcomp,ien[:,i],ndf,nen);

        # compute the element forces
        Fe[:,i] = Ke[:,:,i]*de[:,i];

        # transform Fe to the local coordinate system
        fe[:,i] = Te[:,:,i]*Fe[:,i];

        # Compute the axial force, stress, strain
        axial[i] = fe[7,i] ;         # Use second entry for frame element
        stress[i] = axial[i]/A[i];   
        strain[i] = stress[i]/E[i];
    end

    # display(fe[:,1])

    # display("Fe")
    # display(Fe)
    return dcomp,axial,stress,strain,Fe
end
# =======================================================================

function reactions(idb,ien,Fe,ndf,nee,nel,nen,nnp)

    # function that computes the reaction forces on the structure
    # -----------------------------------------------------------------------
    # idb(ndf,nnp)    = 1 if the degree of freedom is prescribed, 0 otherwise
    # ien(nen,nel)    = element connectivities
    # Fe(nee,nel)     = element forces
    # ndf             = number of degrees of freedom per node
    # nee             = number of element equations
    # nel             = number of elements
    # nen             = number of element equations
    # nnp             = number of nodes
    #
    # Rcomp(ndf,nnp)  = nodal reactions
    # idbr(ndf,nnp)   = 0 if the degree of freedom is prescribed,  otherwise

    # =======================================================================

    # switch BC marker and number the equations for the reaction forces
    idbr = 1 .- idb;  
    idr,neqr = number_eq(idbr,ndf,nnp);

    # assemble reactions R from element force vectors Fe 
    R   = zeros(neqr,1);  
    for i = 1:nel
        LMR = get_local_id(idr,ien[:,i],ndf,nee,nen);
        R   = addforce(R,Fe[:,i],LMR,nee);
    end

    # organize the reactions in matrix array Rcomp(ndf,nnp)
    Rcomp = zeros(ndf,nnp);
    Rcomp = add_d2dcomp(Rcomp,R,idr,ndf,nnp);

    


    return Rcomp,idr
end
# =======================================================================

function addstiff(K,Ke,LM,nee)
    
    # function that solves the equilibrium condition
    # -----------------------------------------------------------------------
    # K(neq,neq)      = global stiffness matrix
    # Ke(nee,nee,1)   = element stiffness matrix
    # LM(nee,nel)     = global to local map for the element
    # nee             = number of element equations

    # =======================================================================

    # loop over rows of Ke
    for i =1:nee

        # loop over columns of Ke

        for j = 1:nee                        

            Mr = LM[i];
            Mc = LM[j];

            if(Mr > 0 && Mc > 0)             
                # if equation #'s are non-zero add element contribution to the 
                # stiffness matrix
                K[Int(Mr),Int(Mc)] = K[Int(Mr),Int(Mc)] +Ke[Int(i),Int(j)];
            end

        end
    
    end
    
    return K
    

end 

# =======================================================================

function solveEQ(F,LM,Ke,nee,nel,neq) 

    # function that solves the equilibrium condition
    # -----------------------------------------------------------------------
    # F(neq,1)        = global force vector
    # LM(nee,nel)     = global to local maps
    # Ke(nee,nee,nel) = element stiffness matrices
    # nee             = number of element equations
    # nel             = number of elements
    # neq             = number of equations
    #
    # d(neq,1)        = displacements at free degrees of freedom

    # =======================================================================

    # assemble global stiffness matrix
    # K = zeros(neq,neq);   # Use 'sparse' for more efficient memory usage
    K=spzeros(neq,neq)
    for i = 1:nel
        K = addstiff(K,Ke[:,:,i],LM[:,i],nee);
    end

    # display("K")
    # display(K)

    # solve the equlibrium
    d = K\F;

    # display("d")
    # display(d)

    return d,K
    
end
# =======================================================================

function mapp(value, x1, y1, x2, y2)
	return (value - x1) * (y2 - x2) / (y1 - x1) + x2;
end
# =======================================================================

function plotFrame(problem,X,scale,threshold=0)
    nel=length(X)
    E,f,g,idb,ien,ndf,nel,nen,nnp,nsd,xn,len,Ke,Te=problem;
    K,F,d,stress,dcomp,g=FEM_frame(problem,X);
    p=plot(axis=nothing,ticks=nothing, border=nothing, aspect_ratio=:equal)
    for i in 1:nel
        if X[i]>threshold
            if threshold>0
                p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                    [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                    color=RGB(0.0,0.0,0.0),
                    linewidth = 3.0)
                    
            else
                p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                    [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                    color=RGB(mapp(stress[i], minimum(stress[:]), maximum(stress[:]), 1, 0),0.0, mapp(stress[i], minimum(stress[:]), maximum(stress[:]), 0, 1)),
                    linewidth = X[i]*scale)
                    # linewidth = mapp(X[i], minimum(X[:]), maximum(X[:])+0.0001, scale/10.0, scale))
                    # p=annotate!((xn[1,Int(ien[1,i])]+xn[1,Int(ien[2,i])])/2.0, (xn[2,Int(ien[1,i])]+xn[2,Int(ien[2,i])])/2.0, "$(floor(stress[i]/1000))*e3", 8)
            end
        else
            p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                    [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                    color=RGB(1.0,1.0,1.0),
                    linewidth = 0.0)
        end

    end
    # plot!(axis=nothing,ticks=nothing, border=nothing)
    p
end
# =======================================================================

function getSetup(fileName)
    setup = Dict()
    open(fileName, "r") do f
        dicttxt = read(f,String)  # file information to string
        setup=JSON.parse(dicttxt)  # parse and transform data
    end
    return setup
    
end

# =======================================================================

function plotFrameDeformed(problem,X,scale,threshold=0,exageration=100.0)
    nel=length(X)
    E,f,g,idb,ien,ndf,nel,nen,nnp,nsd,xn,len,Ke,Te=problem;
    K,F,d,stress,dcomp,g=FEM_frame(problem,X);
    p=plot(axis=nothing,ticks=nothing, border=nothing, aspect_ratio=:equal)
    xnn=zeros(size(xn))
    xnn.=xn .+ exageration.*dcomp
    for i in 1:nel
        if X[i]>threshold
            if threshold>0
                p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                    [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                    color=RGB(0.0,0.0,0.0),
                    linewidth = 3.0)
                p=plot!([xnn[1,Int(ien[1,i])],xnn[1,Int(ien[2,i])]],
                    [xnn[2,Int(ien[1,i])],xnn[2,Int(ien[2,i])]],label="",
                    color=RGB(0.0,1.0,0.0),
                    linewidth = 3.0)
                    
            else
                p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                    [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                    color=RGB(mapp(stress[i], minimum(stress[:]), maximum(stress[:]), 1, 0),0.0, mapp(stress[i], minimum(stress[:]), maximum(stress[:]), 0, 1)),
                    linewidth = X[i]*scale)
                    # linewidth = mapp(X[i], minimum(X[:]), maximum(X[:])+0.0001, scale/10.0, scale))
                    # p=annotate!((xn[1,Int(ien[1,i])]+xn[1,Int(ien[2,i])])/2.0, (xn[2,Int(ien[1,i])]+xn[2,Int(ien[2,i])])/2.0, "$(floor(stress[i]/1000))*e3", 8)
            end
        else
            p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                    [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                    color=RGB(1.0,1.0,1.0),
                    linewidth = 0.0)
        end

    end
    # plot!(axis=nothing,ticks=nothing, border=nothing)
    p
end

# =======================================================================

function plotFrameDeformed3D(problem,X,scale,threshold=0,exageration=100.0)
    nel=length(X)
    E,f,g,idb,ien,ndf,nel,nen,nnp,nsd,xn,len,Ke,Te=problem;
    K,F,d,stress,dcomp,g=FEM_frame(problem,X);
    p=plot(axis=nothing,ticks=nothing, border=nothing, aspect_ratio=:equal)
    xnn=zeros(size(xn))
    xnn.=xn .+ exageration.*dcomp[1:3, :]
    for i in 1:nel
        if X[i]>threshold
            if threshold>0
                p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                        [xn[3,Int(ien[1,i])],xn[3,Int(ien[2,i])]],
                        [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                        color=RGB(0.0,0.0,0.0),
                        linewidth = 3.0)
                p=plot!([xnn[1,Int(ien[1,i])],xnn[1,Int(ien[2,i])]],
                        [xnn[3,Int(ien[1,i])],xnn[3,Int(ien[2,i])]],
                        [xnn[2,Int(ien[1,i])],xnn[2,Int(ien[2,i])]],label="",
                        color=RGB(0.0,1.0,0.0),
                        linewidth = 3.0)
                    
            else
                p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                        [xn[3,Int(ien[1,i])],xn[3,Int(ien[2,i])]],
                        [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                        color=RGB(mapp(stress[i], minimum(stress[:]), maximum(stress[:]), 1, 0),0.0, mapp(stress[i], minimum(stress[:]), maximum(stress[:]), 0, 1)),
                        linewidth = X[i]*scale)
                        # linewidth = mapp(X[i], minimum(X[:]), maximum(X[:])+0.0001, scale/10.0, scale))
                        # p=annotate!((xn[1,Int(ien[1,i])]+xn[1,Int(ien[2,i])])/2.0, (xn[2,Int(ien[1,i])]+xn[2,Int(ien[2,i])])/2.0, "$(floor(stress[i]/1000))*e3", 8)
            end
        else
            p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                    [xn[3,Int(ien[1,i])],xn[3,Int(ien[2,i])]],
                    [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                    color=RGB(1.0,1.0,1.0),
                    linewidth = 0.0)
        end

    end
    # plot!(axis=nothing,ticks=nothing, border=nothing)
    p
end

# =======================================================================

function plotFrame3D(problem,X,scale,threshold=0)
    nel=length(X)
    E,f,g,idb,ien,ndf,nel,nen,nnp,nsd,xn,len,Ke,Te=problem;
    K,F,d,stress,dcomp,g=FEM_frame(problem,X);
    p=plot(axis=nothing,ticks=nothing, border=nothing, aspect_ratio=:equal)
    for i in 1:nel
        if X[i]>threshold
            if threshold>0
                p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                        [xn[3,Int(ien[1,i])],xn[3,Int(ien[2,i])]],
                        [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                        color=RGB(0.0,0.0,0.0),
                        linewidth = 3.0)
                    
            else
                p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                        [xn[3,Int(ien[1,i])],xn[3,Int(ien[2,i])]],
                        [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                        color=RGB(mapp(stress[i], minimum(stress[:]), maximum(stress[:]), 1, 0),0.0, mapp(stress[i], minimum(stress[:]), maximum(stress[:]), 0, 1)),
                        linewidth = X[i]*scale)
                        # linewidth = mapp(X[i], minimum(X[:]), maximum(X[:])+0.0001, scale/10.0, scale))
                        # p=annotate!((xn[1,Int(ien[1,i])]+xn[1,Int(ien[2,i])])/2.0, (xn[2,Int(ien[1,i])]+xn[2,Int(ien[2,i])])/2.0, "$(floor(stress[i]/1000))*e3", 8)
            end
        else
            p=plot!([xn[1,Int(ien[1,i])],xn[1,Int(ien[2,i])]],
                    [xn[3,Int(ien[1,i])],xn[3,Int(ien[2,i])]],
                    [xn[2,Int(ien[1,i])],xn[2,Int(ien[2,i])]],label="",
                    color=RGB(1.0,1.0,1.0),
                    linewidth = 0.0)
        end

    end
    # plot!(axis=nothing,ticks=nothing, border=nothing)
    p
end

# =======================================================================