function [] = FESolveSimp()
% MATLAB based 1-D XFEM Solver
% J. Grogan (2012)
clear all
% Define Geometry
len=1.;
% Define Section Properties
rho=1.;
% Generate Mesh
numElem=10;
charlen=len/numElem;
ndCoords=linspace(0,len,numElem+1);
numNodes=size(ndCoords,2);
indx=1:numElem;
elemNodes(:,1)=indx;
elemNodes(:,2)=indx+1;
% dofs per node
ndof=1;
% Initial temperatures
Tnew=zeros(numNodes,1);
Bound=zeros(numNodes,1);
Tnew(1)=1.;
Bound(1)=1.;
% Define Time Step
dtime=0.0001;
tsteps=100;
time=0.;
% Loop through time steps
for ts=1:tsteps
    K=zeros(numNodes,numNodes);
    M=zeros(numNodes,numNodes);
    % Loop Through Elements
    for e=1:numElem
        Ke=zeros(2*ndof);
        Me=zeros(2*ndof);
        crdn1=ndCoords(elemNodes(e,1));
        crdn2=ndCoords(elemNodes(e,2));
        elen=abs(crdn2-crdn1);
        ajacob=elen/2.;     
        gpx(1)=-1/sqrt(3.);
        gpx(2)=1/sqrt(3.);
        w(1)=1.;
        w(2)=1.;         
        % Loop Through Int Points
        for i=1:2;
            c=gpx(i);
            phi(1)=(1.-c)/2.;
            phi(2)=(1.+c)/2.;
            cond=1.;
            spec=1.;          
            phic(1)=-0.5;
            phic(2)=0.5;               
            phix(1)=phic(1)/ajacob;
            phix(2)=phic(2)/ajacob;
            we=ajacob*w(i);
            Ke=Ke+we*cond*phix'*phix;
            Me=Me+(we*rho*spec*phi'*phi)/dtime;
        end 
        if enr==5;
            Ke(1,2)=0.;
            Me(1,2)=0.;
            Ke(2,1)=0.;
            Me(2,1)=0.;
            Ke(1,4)=0.;
            Me(1,4)=0.;
            Ke(4,1)=0.;
            Me(4,1)=0.;
            Ke(3,2)=0.;
            Me(3,2)=0.;
            Ke(2,3)=0.;
            Me(2,3)=0.;
            Ke(4,3)=0.;
            Me(4,3)=0.;
            Ke(3,4)=0.;
            Me(3,4)=0.;
        end        
        % Add penalty term and get temp gradient on interface
        if enr==4;
            xi=point;
            gm(1)=(1.-xi)/2.;
            gm(3)=(1.+xi)/2.;
            term=theta(1)*gm(1)+theta(2)*gm(3);
            gm(2)=gm(1)*(abs(term)-abs(theta(1)));
            gm(4)=gm(3)*(abs(term)-abs(theta(2)));
            pen=beta*(gm'*gm);
            pfL=beta*1*gm';  
            Ke=Ke+pen;
        else
            pen=zeros(4);
            pfL=zeros(4,1);
        end
        % Assemble Global Matrices
        gnum=2.*elemNodes(e,1)-1.;
        for i=1:4;
            for j=1:4;
                K(gnum+j-1,gnum+i-1)=K(gnum+j-1,gnum+i-1)+Ke(j,i);
                M(gnum+j-1,gnum+i-1)=M(gnum+j-1,gnum+i-1)+Me(j,i);
            end
            pforce(gnum+i-1)=pforce(gnum+i-1)+pfL(i);
        end
    end
    %Remove NON-ENHANCED DOFs(Reduce Matrices)
    iindex=0.;
    for i=1:ndof*numNodes;  
        check=0;
        if mod(i,2)==0 && eNodes(i)~=1
            check=1;
        end   
        if check==0
            iindex=iindex+1;
            TR1(iindex)=Tnew(i);
            BR1(iindex)=Bound(i);
            pforceR1(iindex)=pforce(i);
            jindex=0;
            for j=1:ndof*numNodes; 
               check=0;                  
               if mod(j,2)==0 && eNodes(j)~=1
                     check=1;
               end
               if check==0
                     jindex=jindex+1;
                     MR1(iindex,jindex)=M(i,j);   
                     KR1(iindex,jindex)=K(i,j);
               end
            end
        end
    end 
    AR1=KR1+MR1
    SubR1=AR1*BR1'
    MR1
    TR1'
    MR1*TR1'
    pforceR1'  
    RHSR1=MR1*TR1'-SubR1+pforceR1'    
    % Apply Boundary Conditions       
    Biindex=0.;
    for i=1:iindex;  
        if BR1(i)==0.;
            Biindex=Biindex+1;
            RHSR2(Biindex)=RHSR1(i);
            jindex=0;
            for j=1:iindex; 
               check=0;                  
               if BR1(j)==0.; 
                    jindex=jindex+1;
                    AR2(Biindex,jindex)=AR1(i,j);   
               end
            end
         end
    end         
    %Solve
    Tnewr=(AR2^-1)*RHSR2'
    % Restore Matrices
    Biindex=0;
    for i=1:iindex;
        if BR1(i)==0.;          
              Biindex=Biindex+1;
              TR1(i)=Tnewr(Biindex);
        end       
    end       
    iindex=0;   
    for i=1:ndof*numNodes;
        check=0;
        if mod(i,2)==0 && eNodes(i)~=1
            check=1;
        end   
        if check==0           
             iindex=iindex+1;
             Tnew(i)=TR1(iindex);
        end     
    end
    Tnew
end
stored'