function [] = 2DSimp()
% MATLAB based 2-D XFEM Solver
% J. Grogan (2012)
clear all
% Define Mesh
NumX=3;
NumY=3;
delX=1.;
delY=1.;
for j=1:NumY+1
   for i=1:NumX+1
       index=i+(NumX+1)*(j-1);
       Node(index,1)=single((i-1.))*delX;
       Node(index,2)=single((j-1.))*delY;
   end
end
numNodes=(NumX+1)*(NumY+1);
for j=1:NumY
    for i=1:NumX
        index=i+NumX*(j-1);
        Element(index,1)=i+(NumX+1)*(j-1);
        Element(index,2)=i+(NumX+1)*(j-1)+1;
        Element(index,3)=i+(NumX+1)*(j);
        Element(index,4)=i+(NumX+1)*(j)+1;
    end
end
numElem=(NumX)*(NumY);
% dofs per node
ndof=1;
% Define Section Properties
rho=1.;
% initial interface position
dpos=0.25;
% Initial temperatures
Tnew=zeros(numNodes,1);
Bound=zeros(numNodes,1);
stored(1)=dpos;
for e=1:numElem
    for n=1:4
        crdn=Node(Element(e,n),1);
        if crdn<=dpos
            Tnew(Element(e,n))=1.;
            Bound(Element(e,n))=1.;
        end
    end
end
% Define Time Step
dtime=0.05;
tsteps=20;
time=0.;
% Loop through time steps
for ts=1:tsteps
    K=zeros(numNodes*ndof,numNodes*ndof);
    M=zeros(numNodes*ndof,numNodes*ndof);
    % Loop Through Elements
    for e=1:numElem
        Ke=zeros(4*ndof);
        Me=zeros(4*ndof);
        for icrd=1:4;
            crdnx(icrd)=Node(Element(e,icrd),1);
            crdny(icrd)=Node(Element(e,icrd),2);
        end   
        % regular element - fix extra dofs    
        gpos=1/sqrt(3.);
        gx(1)=-gpos;
        gx(2)=gpos;
        gx(3)=gpos;
        gx(4)=-gpos;
        hx(1)=-gpos;
        hx(2)=-gpos;
        hx(3)=-gpos;
        hx(4)=gpos;        
        % Loop Through Int Points
        for i=1:4;
            g=gx(i);
            h=hx(i);                     
            phi(1)=0.25*(1.-g)*(1.-h);
            phi(2)=0.25*(1.+g)*(1.-h);
            phi(3)=0.25*(1.+g)*(1.+h);
            phi(4)=0.25*(1.-g)*(1.+h);  
            cond=1.;
            spec=1.;
			phig(1)=0.25*-(1.-h);
			phig(2)=0.25*(1.-h);
			phig(3)=0.25*(1.+h);
			phig(4)=0.25*-(1.+h);
			phih(1)=0.25*-(1.-g);
			phih(2)=0.25*-(1.+g);
			phih(3)=0.25*(1.+g);
			phih(4)=0.25*(1.-g);	          
			rjac=zeros(2,2);
			for iter=1:4
				rjac(1,1)=rjac(1,1)+phig(iter)*crdnx(iter);
				rjac(1,2)=rjac(1,2)+phig(iter)*crdny(iter);
				rjac(2,1)=rjac(2,1)+phih(iter)*crdnx(iter);
				rjac(2,2)=rjac(2,2)+phih(iter)*crdny(iter);
            end
			djac=rjac(1,1)*rjac(2,2)-rjac(1,2)*rjac(2,1);
			rjaci(1,1)= rjac(2,2)/djac;
			rjaci(2,2)= rjac(1,1)/djac;
			rjaci(1,2)=-rjac(1,2)/djac;
			rjaci(2,1)=-rjac(2,1)/djac ; 
            for iter=1:4              
                phix(iter)=rjaci(1,1)*phig(iter)+rjaci(1,2)*phih(iter);
                phiy(iter)=rjaci(2,1)*phig(iter)+rjaci(2,2)*phih(iter);
            end	           
            we=djac;
            Ke=Ke+we*cond*(phix'*phix+phiy'*phiy);
            Me=Me+(we*rho*spec*phi'*phi)/dtime;
        end  
        % Add penalty term and get temp gradient on interface
        % Assemble Global Matrices
        gnum(1)=Element(e,1);
        gnum(2)=Element(e,2);
        gnum(3)=Element(e,3);
        gnum(4)=Element(e,4);      
        for i=1:4;
            for j=1:4;
                K(gnum(j),gnum(i))=K(gnum(j),gnum(i))+Ke(j,2*i-1);
                M(gnum(j),gnum(i))=M(gnum(j),gnum(i))+Me(2*j-1,2*i-1);
                M(gnum(j)+1,gnum(i)+1)=M(gnum(j)+1,gnum(i)+1)+Me(2*j,2*i);                
            end
        end
        for i=1:4;
            pforce(gnum(i))=pforce(gnum(i))+pfL(2*i-1);
            pforce(gnum(i)+1)=pforce(gnum(i)+1)+pfL(2*i);
        end
    end
    %Remove inactive DOFs(Reduce Matrices)
    RHS=M*Tnew; 
    A=K+M;
    Sub=A*Bound; 
    iindex=0;   
    for i=1:ndof*numNodes;  
        if Bound(i)==0.;
            iindex=iindex+1;
            RHSred(iindex)=RHS(i)-Sub(i)+pforce(i);
            jindex=0;
            for j=1:ndof*numNodes; 
               if Bound(j)==0.; 
                    jindex=jindex+1;
                    Ared(iindex,jindex)=A(i,j);   
               end
            end
        end
    end 
    %Solve
    StiffI=Ared^-1;
    Tnewr=(Ared^-1)*RHSred';
    iindex=0;   
    for i=1:ndof*numNodes;
        if Bound(i)==0.;
            iindex=iindex+1;
            Tnew(i)=Tnewr(iindex);
        end       
    end
    Tnew
end
stored'