function [] = F2DLevelSetFMM()
clear all
% Define Main Solution Mesh
NumX=32;
NumY=32;
delX=0.25;
delY=0.25;
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)+1;
        Element(index,4)=i+(NumX+1)*(j);
    end
end
numElem=(NumX)*(NumY);
% Define Initial Level Set
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)-0.1;
%    lSet(i)=dist;
%end
% Plot initial level set
[X Y]=meshgrid(0:0.25:8);
Z=zeros(33);
for i=1:1089
    Z(i)=lSet(i);
end
surf(X,Y,Z)
% LS Algorithm Parameters
lSet'
bandwidth=10;
% Loop through timesteps
for tstep=1:10
    % 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*delX
                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   
    % Velocity BC
    F=zeros(NBNodes,1);
    for i=1:NBElems 
        L1=sign(lSetLocal(NBelem(i,1)));
        L2=sign(lSetLocal(NBelem(i,2)));
        L3=sign(lSetLocal(NBelem(i,3)));      
        if L1 ~= L2 || L1 ~= L3
            F(NBelem(i,1))= 1.;
            F(NBelem(i,2))= 1.;
            F(NBelem(i,3))= 1.;
        end
    end
    % Assemble 'Stiffness' Matrices
    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
    % Apply BCs
    RHS=zeros(NBNodes,1);
    Sub=A*F;
    iindex=0;
    for i=1:NBNodes
        if F(i)==0.
            iindex=iindex+1;
            RHSred(iindex)=RHS(i)-Sub(i);
            Fred=0.;
            jindex=0;
            for j=1:NBNodes
                if F(j)==0.
                    jindex=jindex+1;
                    Ared(iindex,jindex)=A(i,j);
                end
            end
        end
    end
    % 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
    % Update level set
    mMat=zeros(NBNodes);
    mMatGLS=zeros(NBNodes);
    f1=zeros(NBNodes,1);
    f2=zeros(NBNodes,1);
    f3=zeros(NBNodes,1);
    h2=0.00001;
    visc=0.0005;
    for i=1:NBElems  
        mMatL=zeros(3);
        mMatGLSL=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;
            mMatL=mMatL+(phi'*phi)/3.;
            mMatGLSL=mMatGLSL+((delphi'*(delset/norm(delset)))*Floc*(h2/abs(Floc)))*phi/3.;
            f1L=f1L+phi'*Floc*norm(delset)/3.;
            f2L=f2L+(delphi'*(delset/norm(delset))*Floc)*(h2/abs(Floc))*Floc*norm(delset)/3.;
            vs=h2*((abs(visc+Floc*norm(delset)))/(norm(Floc*delset)+h2));
            f3L=f3L+vs*delphi'*delset/3.;
        end
        for k=1:3;
            for j=1:3;
               mMat(NBelem(i,j),NBelem(i,k))=mMat(NBelem(i,j),NBelem(i,k))+mMatL(j,k);
               mMatGLS(NBelem(i,j),NBelem(i,k))=mMatGLS(NBelem(i,j),NBelem(i,k))+mMatGLSL(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
    dt=0.01;
    lSetLocal=lSetLocal-((((mMat+mMatGLS)^-1)*dt)*(f1+f2+f3))';
    newlSet=lSetLocal;
    % Reinitialize LS
    nstat=zeros(NBNodes,1);
    for i=1:NBElems
        L1=sign(lSetLocal(NBelem(i,1)));
        L2=sign(lSetLocal(NBelem(i,2)));
        L3=sign(lSetLocal(NBelem(i,3)));      
        if L1 ~= L2 || L1 ~= L3       
            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;
    % Update Global Level Set
    for i=1:NBNodes
        lSet(NLocal(i))=lSetLocal(i);
    end        
end
lSet'
[X Y]=meshgrid(0:0.25:8);
Z=zeros(33);
for i=1:1089
    Z(i)=lSet(i);
end
surf(X,Y,Z)