function []=XCOR_2D()
clear all
% Define Mesh
NumX=10;
NumY=1;
delX=1.;
delY=1.;
numElem=NumX*NumY;
numNodes=(NumX+1)*(NumY+1);
Elength=(delX+delY)/2.;
[Node,Element]=buildMesh(NumX,NumY,delX,delY);
% Simulation Parameters
rho=1.;
Penalty=200.;
dtImp=0.01;
dtExp=0.001;
tsteps=10;
bandWidth=10.;
epsilon=0.00001;
visc=0.0005;
% Get Initial Level Set
LSetOld=initialLSet(Node,numNodes);
% plotLSet(NumX,NumY,delX,delY,LSet);
% Initial Conditions
Temp=zeros(numNodes*2,1);
for i=1:numNodes
    if LSetOld(i)<=0
        Temp(2*i-1)=1.;
    end
end
% Boundary Conditions
Bound=zeros(numNodes*2,1);
for i=1:numNodes
    if Node(i,1)<delX/10.
        Bound(2*i-1)=1.;
    end
end
% Loop through time steps
for ts=1:tsteps
    % Update Level Set
    LSetNew=updateLSet(Temp,Node,numNodes,Element,numElem,dtImp,dtExp,LSetOld,...
        Elength,bandWidth,epsilon,visc);
    % Solve for Temperature
    Temp=getTemp(Node,Element,numNodes,numElem,LSetNew,Bound,Temp,Penalty,rho,dtImp,LSetOld);
    LSetOld=LSetNew;
    LSetOld'
    POUT(ts)=LSetOld(1);
end
POUT';
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create a linear quadrilateral FE mesh
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Node,Element]=buildMesh(NumX,NumY,delX,delY)
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
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)+1;
        Element(index,4)=i+(NumX+1)*(j);
    end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function updates the level set
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [LSet]=updateLSet(Temp,Node,numNodes,Element,numElem,dtImp,dtExp,LSet,...
        Elength,bandWidth,epsilon,visc)
% parameters
charLen=epsilon*Elength;
for tstep=1:floor(dtImp/dtExp)
    % Identify Narrow Band Elements and Get Local Level Set
    [NBelem,NBElems,NGlobal,NLocal,NBNodes,LSetLocal]=getNarrowBand(bandWidth,...
        Elength,LSet,Element,numElem,numNodes);
    % Identify Scalar Velocity on Nodes Crossed By Interface - F
    F=getF(Temp,LSetLocal,NBElems,NBNodes,NLocal,NBelem,Node,Elength);
    % Get 'Stiffness' Matrix - A
    A=getA(Node,NLocal,NBelem,NBNodes,NBElems,LSetLocal);
    % Apply BCs
    RHS=-A*F;
    iindex=0;
    for i=1:NBNodes
        if F(i)==0.
            iindex=iindex+1;
            RHSred(iindex)=RHS(i);
            jindex=0;
            for j=1:NBNodes
                if F(j)==0.
                    jindex=jindex+1;
                    Ared(iindex,jindex)=A(i,j);
                end
            end
        end
    end
    if iindex>0
        % Solve for Fred
        Fred=(Ared^-1)*RHSred';
        % Get F
        iindex=0;
        for i=1:NBNodes
            if F(i)==0.
                iindex=iindex+1;
                F(i)=Fred(iindex);
            end
        end
    end
    % Get Level Set Equation Terms
    [M,MGLS,f1,f2,f3]=getTerms(Node,NLocal,NBelem,NBNodes,NBElems,LSetLocal,visc,charLen,F);
    LSetLocal=LSetLocal-((((M+MGLS)^-1)*dtExp)*(f1+f2+f3))';    
    % Reinitialize LS
    %LSetLocal=fastMarch(LSetLocal,NBelem,Node,NLocal,NBNodes,NBElems);
    for i=1:NBNodes
        LSet(NLocal(i))=LSetLocal(i);
    end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Find elements in narrow band and create map between
% global node labels and those in narrow band
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [NBelem,NBElems,NGlobal,NLocal,NBNodes,LSetLocal]=getNarrowBand(bandWidth,...
    ELength,LSet,Element,numElem,numNodes)
% Identify Narrow Band Elements
NBElems=0;
NBNodes=0;
NGlobal=zeros(numNodes);
for i=1:numElem
    check=0;
    for iNd=1:4
         if abs(LSet(Element(i,iNd)))<=bandWidth*ELength
             check=1;
         end
    end
    % If an element is in the narrow band split it into triangles
    if check==1
         for j=1:4
             if NGlobal(Element(i,j))==0
                 NBNodes=NBNodes+1;
                 NGlobal(Element(i,j))=NBNodes;
                 NLocal(NBNodes)=Element(i,j);                  
             end
         end
         NBElems=NBElems+1;
         NBelem(NBElems,1)=NGlobal(Element(i,1));
         NBelem(NBElems,2)=NGlobal(Element(i,2));
         NBelem(NBElems,3)=NGlobal(Element(i,3));                      
         NBElems=NBElems+1;
         NBelem(NBElems,1)=NGlobal(Element(i,1));
         NBelem(NBElems,2)=NGlobal(Element(i,3));
         NBelem(NBElems,3)=NGlobal(Element(i,4)); 
    end
end 
% Get local Level Set
for i=1:NBNodes
    LSetLocal(i)=LSet(NLocal(i));
end  
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Get Interface Normal Veloctiy 'F'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function F=getF(Temp,LSetLocal,NBElems,NBNodes,NLocal,NBelem,Node,ELength)  
F=zeros(NBNodes,1);
eStat=zeros(NBElems,1);
nData=zeros(NBNodes,2);
for i=1:NBElems
    for j=1:3
        L(j)=LSetLocal(NBelem(i,j)); 
    end
    x11=Node(NLocal(NBelem(i,1)),1);
    x12=Node(NLocal(NBelem(i,2)),1);
    x13=Node(NLocal(NBelem(i,3)),1);
    y11=Node(NLocal(NBelem(i,1)),2);
    y12=Node(NLocal(NBelem(i,2)),2);
    y13=Node(NLocal(NBelem(i,3)),2);
    count=0.;
    if sign(L(1)) ~= sign(L(2))
        eStat(i)=1;
        count=count+1;
        f=abs(L(1))/(abs(L(1))+abs(L(2)));
        xi(count)=f*(x12-x11)+x11;
        yi(count)=f*(y12-y11)+y11;
    end         
    if sign(L(1)) ~= sign(L(3)) 
        eStat(i)=1;
        count=count+1;               
        f=abs(L(1))/(abs(L(1))+abs(L(3)));
        xi(count)=f*(x13-x11)+x11;
        yi(count)=f*(y13-y11)+y11 ; 
    end   
    if sign(L(2)) ~= sign(L(3))
        eStat(i)=1;
        count=count+1;        
        f=abs(L(2))/(abs(L(2))+abs(L(3)));
        xi(count)=f*(x13-x12)+x12;
        yi(count)=f*(y13-y12)+y12 ;         
    end
    if eStat(i)==1
        n=[yi(2)-yi(1); xi(1)-xi(2)];
        n=n/norm(n);
        xd(1,1)=(xi(1)+xi(2))/2.;        
        xd(1,2)=(yi(1)+yi(2))/2.;
        xd(2,1)=0.15*ELength*n(1)+xd(1,1);
        xd(2,2)=0.15*ELength*n(2)+xd(1,2);
        % Check if xd2 is in element
        v0(1)=x11;
        v0(2)=y11;
        v1(1)=x12-x11;
        v1(2)=y12-y11;
        v2(1)=x13-x11;
        v2(2)=y13-y11;
        v(1)=xd(2,1);
        v(2)=xd(2,2);
        ra=((v(1)*v2(2)-v2(1)*v(2))-(v0(1)*v2(2)-v2(1)*v0(2)))/(v1(1)*v2(2)-v2(1)*v1(2));
        rb=-((v(1)*v1(2)-v1(1)*v(2))-(v0(1)*v1(2)-v1(1)*v0(2)))/(v1(1)*v2(2)-v2(1)*v1(2));
        check=0;
        if ra>0. && rb>0. && ra+rb<1.
            index=i;
            x21=x11;
            x22=x12;
            x23=x13;
            y21=y11;
            y22=y12;
            y23=y13;         
        else
            for j=1:NBElems
                 tx1=Node(NLocal(NBelem(j,1)),1);
                 tx2=Node(NLocal(NBelem(j,2)),1);
                 tx3=Node(NLocal(NBelem(j,3)),1);
                 ty1=Node(NLocal(NBelem(j,1)),2);
                 ty2=Node(NLocal(NBelem(j,2)),2);
                 ty3=Node(NLocal(NBelem(j,3)),2); 
                 v0(1)=tx1;
                 v0(2)=ty1;
                 v1(1)=tx2-tx1;
                 v1(2)=ty2-ty1;
                 v2(1)=tx3-tx1;
                 v2(2)=ty3-ty1;
                 v(1)=xd(2,1);
                 v(2)=xd(2,2);
                 ra=((v(1)*v2(2)-v2(1)*v(2))-(v0(1)*v2(2)-v2(1)*v0(2)))/(v1(1)*v2(2)-v2(1)*v1(2));
                 rb=-((v(1)*v1(2)-v1(1)*v(2))-(v0(1)*v1(2)-v1(1)*v0(2)))/(v1(1)*v2(2)-v2(1)*v1(2));
                 if ra>0. && rb>0. && ra+rb<1.
                    index=j;
                    x21=tx1;
                    x22=tx2;
                    x23=tx3;
                    y21=ty1;
                    y22=ty2;
                    y23=ty3;                     
                 end
            end
        end                           
        Ae=0.5*((x12*y13-x13*y12)+(y12-y13)*x11+(x13-x12)*y11);                     
        N1=(1./(2.*Ae))*((y12-y13)*(xd(1,1)-x12)+(x13-x12)*(xd(1,2)-y12));
        N2=(1./(2.*Ae))*((y13-y11)*(xd(1,1)-x13)+(x11-x13)*(xd(1,2)-y13));
        N3=(1./(2.*Ae))*((y11-y12)*(xd(1,1)-x11)+(x12-x11)*(xd(1,2)-y11));
        T1=Temp(2*NLocal(NBelem(i,1))-1);
        T2=Temp(2*NLocal(NBelem(i,2))-1);
        T3=Temp(2*NLocal(NBelem(i,3))-1);
        a1=Temp(2*NLocal(NBelem(i,1)));
        a2=Temp(2*NLocal(NBelem(i,2)));
        a3=Temp(2*NLocal(NBelem(i,3)));
        L1=LSetLocal(NBelem(i,1));
        L2=LSetLocal(NBelem(i,2));
        L3=LSetLocal(NBelem(i,3));
        LS=abs(N1*L1+L2*N2+L3*N3);
        p1=N1*(LS-abs(L1));
        p2=N2*(LS-abs(L2));
        p3=N3*(LS-abs(L3));
        T(1)=N1*T1+N2*T2+N3*T3+p1*a1+p2*a2+p3*a3;   
        Ae=0.5*((x22*y23-x23*y22)+(y22-y23)*x21+(x23-x22)*y21);  
        N1=(1./(2.*Ae))*((y22-y23)*(xd(2,1)-x22)+(x23-x22)*(xd(2,2)-y22));
        N2=(1./(2.*Ae))*((y23-y21)*(xd(2,1)-x23)+(x21-x23)*(xd(2,2)-y23));
        N3=(1./(2.*Ae))*((y21-y22)*(xd(2,1)-x21)+(x22-x21)*(xd(2,2)-y21));
        T1=Temp(2*NLocal(NBelem(index,1))-1);
        T2=Temp(2*NLocal(NBelem(index,2))-1);
        T3=Temp(2*NLocal(NBelem(index,3))-1);
        a1=Temp(2*NLocal(NBelem(index,1)));
        a2=Temp(2*NLocal(NBelem(index,2)));
        a3=Temp(2*NLocal(NBelem(index,3)));
        L1=LSetLocal(NBelem(index,1));
        L2=LSetLocal(NBelem(index,2));
        L3=LSetLocal(NBelem(index,3));
        LS=abs(N1*L1+L2*N2+L3*N3);
        p1=N1*(LS-abs(L1));
        p2=N2*(LS-abs(L2));
        p3=N3*(LS-abs(L3));
        T(2)=N1*T1+N2*T2+N3*T3+p1*a1+p2*a2+p3*a3           
        gradT=(T(2)-T(1))/(0.15*ELength);  
        for j=1:3
            nData(NBelem(i,j),1)=nData(NBelem(i,j),1)+1.;
            nData(NBelem(i,j),2)=nData(NBelem(i,j),2)+0.1*gradT;
        end
    end         
end
for i=1:NBNodes
    if nData(i,1)>0
        F(i)=nData(i,2)/nData(i,1);
    end
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Get 'Stiffness' Matrix 'A'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [A]=getA(Node,NLocal,NBelem,NBNodes,NBElems,LSetLocal)
A=zeros(NBNodes);
for i=1:NBElems            
    gx(1)=2./3.;
    gx(2)=1./6.;
    gx(3)=1./6.;
    hx(1)=1./6.;
    hx(2)=1./6.;
    hx(3)=2./3.;
    AfL=zeros(3);
    AfLGLS=zeros(3);
    x1=Node(NLocal(NBelem(i,1)),1);
    y1=Node(NLocal(NBelem(i,1)),2);
    x2=Node(NLocal(NBelem(i,2)),1);
    y2=Node(NLocal(NBelem(i,2)),2);
    x3=Node(NLocal(NBelem(i,3)),1);
    y3=Node(NLocal(NBelem(i,3)),2);
    for j=1:3
        g=gx(j);
        h=hx(j);            
        phi(1)=1.-g-h;
        phi(2)=g;
        phi(3)=h;
        phig(1)=-1.;
        phig(2)=1.;
        phig(3)=0.;    
        phih(1)=-1.;
        phih(2)=0.;
        phih(3)=1.;             
        djac=2*abs(x1*(y2-y3)+x2*(y3-y1)+x3*(y1-y2));
        for k=1:3
            phix(k)=(1./djac)*((-y1+y3)*phig(k)+(y1-y2)*phih(k));
            phiy(k)=(1./djac)*((x1-x3)*phig(k)+(-x1+x2)*phih(k));
        end
        delphi=[phix;phiy];
        nodalLset=[LSetLocal(NBelem(i,1));LSetLocal(NBelem(i,2));LSetLocal(NBelem(i,3))];
        set=phi*nodalLset;
        delset=delphi*nodalLset;
        AfL=AfL+(phi'*sign(set))*(delset'*delphi)/3.;
        AfLGLS=AfLGLS+(delphi'*delset)*(1./norm(delset))*(delset'*delphi)/3.;        
    end
    sum=AfL+AfLGLS;
    for k=1:3;
        for j=1:3;
           A(NBelem(i,j),NBelem(i,k))=A(NBelem(i,j),NBelem(i,k))+sum(j,k);
        end
    end
end    
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Get terms for LS equation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [M,MGLS,f1,f2,f3]=getTerms(Node,NLocal,NBelem,NBNodes,NBElems,LSetLocal,visc,charLen,F)
M=zeros(NBNodes);
MGLS=zeros(NBNodes);
f1=zeros(NBNodes,1);
f2=zeros(NBNodes,1);
f3=zeros(NBNodes,1);
for i=1:NBElems  
    ML=zeros(3);
    MGLSL=zeros(3);
    f1L=zeros(3,1);
    f2L=zeros(3,1);
    f3L=zeros(3,1);
    gx(1)=2./3.;
    gx(2)=1./6.;
    gx(3)=1./6.;
    hx(1)=1./6.;
    hx(2)=1./6.;
    hx(3)=2./3.;
    x1=Node(NLocal(NBelem(i,1)),1);
    y1=Node(NLocal(NBelem(i,1)),2);
    x2=Node(NLocal(NBelem(i,2)),1);
    y2=Node(NLocal(NBelem(i,2)),2);
    x3=Node(NLocal(NBelem(i,3)),1);
    y3=Node(NLocal(NBelem(i,3)),2);
    for j=1:3
        g=gx(j);
        h=hx(j);            
        phi(1)=1.-g-h;
        phi(2)=g;
        phi(3)=h;
        phig(1)=-1.;
        phig(2)=1.;
        phig(3)=0.;    
        phih(1)=-1.;
        phih(2)=0.;
        phih(3)=1.;             
        djac=abs(x1*(y2-y3)+x2*(y3-y1)+x3*(y1-y2));
        for k=1:3
            phix(k)=(1./djac)*((-y1+y3)*phig(k)+(y1-y2)*phih(k));
            phiy(k)=(1./djac)*((x1-x3)*phig(k)+(-x1+x2)*phih(k));
        end 
        delphi=[phix;phiy];
        nodalLset=[LSetLocal(NBelem(i,1));LSetLocal(NBelem(i,2));LSetLocal(NBelem(i,3))];
        nodalF=[F(NBelem(i,1));F(NBelem(i,2));F(NBelem(i,3))];
        delset=delphi*nodalLset;                              
        Floc=phi*nodalF;
        ML=ML+(phi'*phi)/3.;
        MGLSL=MGLSL+((delphi'*(delset/norm(delset)))*Floc*(charLen/abs(Floc)))*phi/3.;
        f1L=f1L+phi'*Floc*norm(delset)/3.;
        f2L=f2L+(delphi'*(delset/norm(delset))*Floc)*(charLen/abs(Floc))*Floc*norm(delset)/3.;
        vs=charLen*((abs(visc+Floc*norm(delset)))/(norm(Floc*delset)+charLen));
        f3L=f3L+vs*delphi'*delset/3.;
    end
    for k=1:3;
        for j=1:3;
           M(NBelem(i,j),NBelem(i,k))=M(NBelem(i,j),NBelem(i,k))+ML(j,k);
           MGLS(NBelem(i,j),NBelem(i,k))=MGLS(NBelem(i,j),NBelem(i,k))+MGLSL(j,k);           
        end
        f1(NBelem(i,k))=f1(NBelem(i,k))+f1L(k);
        f2(NBelem(i,k))=f2(NBelem(i,k))+f2L(k);
        f3(NBelem(i,k))=f3(NBelem(i,k))+f3L(k);       
    end
end   
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use Fast March Method to Reinitialize LS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function LSetLocal=fastMarch(LSetLocal,NBelem,Node,NLocal,NBNodes,NBElems)
newlSet=LSetLocal;
% Reinitialize LS
nstat=zeros(NBNodes,1);
for i=1:NBElems
    for j=1:3
        L(j)=sign(LSetLocal(NBelem(i,j)));
    end
    if L(1) ~= L(2) || L(1) ~= L(3)       
        for j=1:3
            nstat(NBelem(i,j))=1;
        end
    end
end  
maincheck=0;
while(maincheck==0)
    lmin=1000.;
    avlmin=1000.;
    eindex=0;
    nindex=0;
    maincheck=1;
    for i=1:NBElems
        if nstat(NBelem(i,1))+nstat(NBelem(i,2))+nstat(NBelem(i,3))==2
            maincheck=0;
            check=0;
            ltot=0.;
            for j=1:3
                if nstat(NBelem(i,j))==0
                    if abs(LSetLocal(NBelem(i,j)))<=lmin
                        check=1;
                        tempindex=j;
                    end
                end
                ltot=ltot+abs(LSetLocal(NBelem(i,j)));
            end
            if check==1 && ltot/3.<=avlmin
                eindex=i;
                nindex=tempindex;
                lmin=LSetLocal(NBelem(eindex,nindex));
                avlmin=ltot/3.;
            end
        end
    end
    if maincheck==0
        % Find New LS for point
        xp=Node(NLocal(NBelem(eindex,nindex)),1);
        yp=Node(NLocal(NBelem(eindex,nindex)),2);
        count=0;
        for i=1:3
            if i~=nindex
                count=count+1;
                x(count)=Node(NLocal(NBelem(eindex,i)),1);
                y(count)=Node(NLocal(NBelem(eindex,i)),2);
                lloc(count)=newlSet(NBelem(eindex,i));
            end
        end
        delxa=x(1)-xp;
        delya=y(1)-yp;
        delxb=x(2)-xp;
        delyb=y(2)-yp;
        N=[delxa delya; delxb delyb];
        M=N^-1;
        A=(M(1)*M(1)+M(2)*M(2));
        B=(M(3)*M(3)+M(4)*M(4));
        C=2.*(M(1)*M(3)+M(2)*M(4));
        a=A+B+C;
        b=-2.*lloc(1)*A-2.*lloc(2)*B-C*(lloc(1)+lloc(2));
        c=lloc(1)*lloc(1)*A+lloc(2)*lloc(2)*B+lloc(1)*lloc(2)*C-1.;
        templ1=(-b+sqrt(b*b-4.*a*c))/(2.*a);
        templ2=(-b-sqrt(b*b-4.*a*c))/(2.*a);
        if abs(templ1)>abs(templ2)
            newlSet(NBelem(eindex,nindex))=templ1;
        else
            newlSet(NBelem(eindex,nindex))=templ2;
        end
        nstat(NBelem(eindex,nindex))=1;  
    end
end
LSetLocal=newlSet;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve Implicit Porblem to Get Temperature
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Temp=getTemp(Node,Element,numNodes,numElem,LSet,Bound,Temp,Penalty,rho,dtImp,LSetOld)
K=zeros(numNodes*2,numNodes*2);
M=zeros(numNodes*2,numNodes*2);
MStar=zeros(numNodes*2,numNodes*2);
pforce=zeros(numNodes*2,1);
% Loop Through Elements
for e=1:numElem
    Ke=zeros(8);
    Me=zeros(8);
    MeStar=zeros(8);
    for icrd=1:4;
        crdnx(icrd)=Node(Element(e,icrd),1);
        crdny(icrd)=Node(Element(e,icrd),2);
        theta(icrd)=LSet(Element(e,icrd));
        thetaO(icrd)=LSetOld(Element(e,icrd));
    end 
    check=0;
    for i=1:3
        for j=i+1:4
            if sign(theta(1))~=sign(theta(j))
                check=1;
            end
        end
    end
    if check==1
        % possible enriched element
        npart=10;
        enr=npart*npart;       
        for sdx=1:npart
            for sdy=1:npart
                midx=-1.-1./npart+(2./npart)*sdx;
                midy=-1.-1./npart+(2./npart)*sdy;
                subindex=npart*(sdy-1)+sdx;
                gpos=1./(sqrt(3.)*npart);
                gx(subindex,1)=midx-gpos;
                gx(subindex,2)=midx+gpos;
                gx(subindex,3)=midx+gpos;
                gx(subindex,4)=midx-gpos;
                hx(subindex,1)=midy-gpos;
                hx(subindex,2)=midy-gpos;
                hx(subindex,3)=midy+gpos;
                hx(subindex,4)=midy+gpos;
            end
        end
        % check if int points are on different sides of front
         check=0;
         for i=1:enr
            for j=1:4
                g=gx(i,j);
                h=hx(i,j);                     
                phi(1)=0.25*(1.-g)*(1.-h);
                phi(3)=0.25*(1.+g)*(1.-h);
                phi(5)=0.25*(1.+g)*(1.+h);
                phi(7)=0.25*(1.-g)*(1.+h);
                phiO=phi;
                iLS=theta(1)*phi(1)+theta(2)*phi(3)+theta(3)*phi(5)+theta(4)*phi(7); 
                if i==1 && j==1
                    sgn=sign(iLS);
                else
                    if sign(iLS)~=sgn
                        check=1;
                    end
                end
            end
         end
         if check==0
            % regular element - fix extra dofs 
            enr=1;
            gpos=1/sqrt(3.);
            gx(1,1)=-gpos;
            gx(1,2)=gpos;
            gx(1,3)=gpos;
            gx(1,4)=-gpos;
            hx(1,1)=-gpos;
            hx(1,2)=-gpos;
            hx(1,3)=gpos;
            hx(1,4)=gpos;     
         end               
    else
        % regular element - fix extra dofs 
        enr=1;
        gpos=1/sqrt(3.);
        gx(1,1)=-gpos;
        gx(1,2)=gpos;
        gx(1,3)=gpos;
        gx(1,4)=-gpos;
        hx(1,1)=-gpos;
        hx(1,2)=-gpos;
        hx(1,3)=gpos;
        hx(1,4)=gpos;           
    end          
    for i=1:enr
        for j=1:4
            g=gx(i,j);
            h=hx(i,j);                     
            phi(1)=0.25*(1.-g)*(1.-h);
            phi(3)=0.25*(1.+g)*(1.-h);
            phi(5)=0.25*(1.+g)*(1.+h);
            phi(7)=0.25*(1.-g)*(1.+h);
            phiO=phi;
            iLS=theta(1)*phi(1)+theta(2)*phi(3)+theta(3)*phi(5)+theta(4)*phi(7);
            iLSO=thetaO(1)*phi(1)+thetaO(2)*phi(3)+thetaO(3)*phi(5)+thetaO(4)*phi(7);
            if iLS<0.
                cond=0.;
                spec=0.01;
            else
                cond=1.;
                spec=1.;
            end
            if iLSO<0.
                specO=0.01;
            else
                specO=1.;
            end                                                   
            for iter=1:4
                phi(2*iter)=phi(2*iter-1)*(abs(iLS)-abs(theta(iter)));
                phiO(2*iter)=phiO(2*iter-1)*(abs(iLSO)-abs(thetaO(iter)));
            end
            phig(1)=0.25*-(1.-h);
            phig(3)=0.25*(1.-h);
            phig(5)=0.25*(1.+h);
            phig(7)=0.25*-(1.+h);
            phih(1)=0.25*-(1.-g);
            phih(3)=0.25*-(1.+g);
            phih(5)=0.25*(1.+g);
            phih(7)=0.25*(1.-g);	
            diLSg=sign(iLS)*(phig(1)*theta(1)+phig(3)*theta(2)+phig(5)*theta(3)+phig(7)*theta(4));
            diLSh=sign(iLS)*(phih(1)*theta(1)+phih(3)*theta(2)+phih(5)*theta(3)+phih(7)*theta(4));
            for iter=1:4                      
                phig(2*iter)=phig(2*iter-1)*(abs(iLS)-abs(theta(iter)))+phi(2*iter-1)*diLSg;
                phih(2*iter)=phih(2*iter-1)*(abs(iLS)-abs(theta(iter)))+phi(2*iter-1)*diLSh;
            end           
            rjac=zeros(2,2);
            for iter=1:4
                rjac(1,1)=rjac(1,1)+phig(2*iter-1)*crdnx(iter);
                rjac(1,2)=rjac(1,2)+phig(2*iter-1)*crdny(iter);
                rjac(2,1)=rjac(2,1)+phih(2*iter-1)*crdnx(iter);
                rjac(2,2)=rjac(2,2)+phih(2*iter-1)*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:8              
                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))/double(enr);
            Me=Me+((we*rho*spec*phi'*phi)/dtImp)/double(enr);
            MeStar=MeStar+((we*rho*specO*phi'*phiO)/dtImp)/double(enr);
        end
    end  
    % Add penalty term and get temp gradient on interface
    if enr>1;
        count=0;
        if sign(theta(1))~=sign(theta(2))
            count=count+1;                  
            f=abs(theta(1))/(abs(theta(1))+abs(theta(2)));
            xi(count)=f*(crdnx(2)-crdnx(1))+crdnx(1);
            yi(count)=f*(crdny(2)-crdny(1))+crdny(1);
            gi(count)=(2.*xi(count)-(crdnx(1)+crdnx(2)))/(-crdnx(1)+crdnx(2));
            hi(count)=-1.;              
        end            
        if sign(theta(2))~=sign(theta(3))
            count=count+1;                   
            f=abs(theta(2))/(abs(theta(2))+abs(theta(3)));
            xi(count)=f*(crdnx(3)-crdnx(2))+crdnx(2);
            yi(count)=f*(crdny(3)-crdny(2))+crdny(2);
            gi(count)=1.;
            hi(count)=(2.*yi(count)-(crdny(2)+crdny(3)))/(-crdny(2)+crdny(3));                 
        end  
        if sign(theta(3))~=sign(theta(4))
            count=count+1;                   
            f=abs(theta(3))/(abs(theta(3))+abs(theta(4)));
            xi(count)=f*(crdnx(4)-crdnx(3))+crdnx(3);
            yi(count)=f*(crdny(4)-crdny(3))+crdny(3);
            gi(count)=(2.*xi(count)-(crdnx(4)+crdnx(3)))/(-crdnx(4)+crdnx(3));
            hi(count)=1.;                            
         end  
        if sign(theta(1))~=sign(theta(4))
            count=count+1;                   
            f=abs(theta(1))/(abs(theta(1))+abs(theta(4)));
            xi(count)=f*(crdnx(4)-crdnx(1))+crdnx(1);
            yi(count)=f*(crdny(4)-crdny(1))+crdny(1);
            gi(count)=-1.;
            hi(count)=(2.*yi(count)-(crdny(1)+crdny(4)))/(-crdny(4)+crdny(1));               
        end  
        c=zeros(2,1);
        c=(c+1.);
        for i=1:2;
            G(i,1)=0.25*(1.-gi(i))*(1.-hi(i));
            G(i,3)=0.25*(1.+gi(i))*(1.-hi(i));
            G(i,5)=0.25*(1.+gi(i))*(1.+hi(i));
            G(i,7)=0.25*(1.-gi(i))*(1.+hi(i)); 
            G(i,2)=-G(i,1)*abs(theta(1));
            G(i,4)=-G(i,3)*abs(theta(2));
            G(i,6)=-G(i,5)*abs(theta(3));
            G(i,8)=-G(i,7)*abs(theta(4));
        end 
        pen=Penalty*(G'*G);
        pfL=Penalty*G'*c;  
%        pen=zeros(8);
%        pfL=zeros(8,1); 
        Ke=Ke+pen;
    else
        pen=zeros(8);
        pfL=zeros(8,1);
    end    
    % Assemble Global Matrices
    gnum(1)=2*Element(e,1)-1;
    gnum(2)=2*Element(e,2)-1;
    gnum(3)=2*Element(e,3)-1;
    gnum(4)=2*Element(e,4)-1;      
    for i=1:4;
        for j=1:4;
            K(gnum(j),gnum(i))=K(gnum(j),gnum(i))+Ke(2*j-1,2*i-1);
            K(gnum(j)+1,gnum(i)+1)=K(gnum(j)+1,gnum(i)+1)+Ke(2*j,2*i);
            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);  
            MStar(gnum(j),gnum(i))=MStar(gnum(j),gnum(i))+MeStar(2*j-1,2*i-1);
            MStar(gnum(j)+1,gnum(i)+1)=MStar(gnum(j)+1,gnum(i)+1)+MeStar(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=MStar*Temp; 
A=K+M;
Sub=A*Bound; 
iindex=0;   
for i=1:2*numNodes;  
    if Bound(i)==0.;
        iindex=iindex+1;
        RHSred(iindex)=RHS(i)-Sub(i)+pforce(i);
        jindex=0;
        for j=1:2*numNodes; 
           if Bound(j)==0.; 
                jindex=jindex+1;
                Ared(iindex,jindex)=A(i,j);   
           end
        end
    end
end 
%Solve
Tempr=(Ared^-1)*RHSred';
iindex=0;   
for i=1:2*numNodes;
    if Bound(i)==0.;
        iindex=iindex+1;
        Temp(i)=Tempr(iindex);
    end       
end
Temp
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generates the initial level set
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [LSet]=initialLSet(Node,numNodes)
%centx=4.;
%centy=4.;
%rad=2.1;
%for i=1:numNodes;
%    dist=sqrt((Node(i,1)-centx)*(Node(i,1)-centx)+(Node(i,2)-centy)*(Node(i,2)-centy));
%    LSet(i)=dist-rad;
%end
for i=1:numNodes;
   dist=Node(i,1)-4.6;
   LSet(i)=dist;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot the level set
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function []=plotLSet(NumX,NumY,delX,delY,LSet)
[X Y]=meshgrid(0:delX:delX*NumX,0:delY:delY*NumY);
Z=zeros(NumX+1,NumY+1);
for i=1:(NumX+1)*(NumY+1)
    Z(i)=LSet(i);
end
surf(X,Y,Z)
end