function [] = GetF2D()
clear all
% Define Main Solution Mesh
NumX=10;
NumY=10;
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)+1;
        Element(index,4)=i+(NumX+1)*(j);
    end
end
numElem=(NumX)*(NumY);
% Define Initial Level Set
centx=5.;
centy=5.;
rad=2.5;
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
% Plot initial level set
[X,Y]=meshgrid(0:1.:10);
Z=zeros(11);
for i=1:numNodes
    Z(i)=lSet(i);
end
surf(X,Y,Z)
% LS Algorithm Parameters
bandwith=10.;
% Loop through timesteps
for tstep=1:1
    % Identify Narrow Band Elements
    NBindex=0;
    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
            NBindex=NBindex+1;
            NBelem(NBindex,1)=Element(i,1);
            NBelem(NBindex,2)=Element(i,2);
            NBelem(NBindex,3)=Element(i,3);
            NBindex=NBindex+1;
            NBelem(NBindex,1)=Element(i,1);
            NBelem(NBindex,2)=Element(i,3);
            NBelem(NBindex,3)=Element(i,4);  
        end
    end 
    % Velocity BC
    F=zeros(numNodes,1);
    for i=1:NBindex  
        if lSet(NBelem(i,1))~=lSet(NBelem(i,2)) || lSet(NBelem(i,1))~=lSet(NBelem(i,3))
            F(NBelem(i,1))= 0.0005;
            F(NBelem(i,2))= 0.0005;
            F(NBelem(i,3))= 0.0005;
        end
    end
    % Assemble 'Stiffness' Matrices
    A=zeros(numNodes);
    for i=1:NBindex 
        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;
        AfL=zeros(4);
        AfLGLS=zeros(4);
        for j=1:4
            g=gx(j);
            h=hx(j);            
            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);
 			phig(1)=0.25*-(1.-h);
			phig(2)=0.25*(1.-h);
			phig(3)=0.25*(1.+h);
			phig(4)=0.25*-(1.+h);           
            rset=phi(1)*lSet(Element(i,1))+phi(2)*lSet(Element(i,2))+phi(3)*lSet(Element(i,3))+phi(4)*lSet(Element(i,4));
            dls=phig(1)*lSet(Element(i,1))+phig(2)*lSet(Element(i,2))+phig(3)*lSet(Element(i,3))+phig(4)*lSet(Element(i,4));
            AfL=AfL+(phi'*sign(rset))*(dls*phig);
            AfLGLS=AfLGLS+(phig'*dls)*(1./abs(dls))*(dls*phig);
        end
        for k=1:4;
            for j=1:4;
               A(Element(i,j),Element(i,k))=A(Element(i,j),Element(i,k))+AfL(j,k)+AfLGLS(j,k)
            end
        end
    end
    % Apply BCs
    RHS=zeros(numNodes,1);
    Sub=A*F;
    iindex=0;
    for i=1:numNodes
        if F(i)==0.
            iindex=iindex+1;
            RHSred(iindex)=RHS(i)-Sub(i);
            Fred=0.;
            jindex=0;
            for j=1:numNodes
                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:numNodes
        if F(i)==0.
            iindex=iindex+1;
            F(i)=Fred(iindex);
        end
    end
    % Update level set
    mMat=zeros(numNodes);
    mMatGLS=zeros(numNodes);
    f1=zeros(numNodes,1);
    f2=zeros(numNodes,1);
    f3=zeros(numNodes,1);
    h=1.;
    visc=0.000;
    for i=1:numElem
        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;
        mMatL=zeros(4);
        mMatGLSL=zeros(4);
        f1L=zeros(4,1);
        f2L=zeros(4,1);
        f3L=zeros(4,1);
        for j=1:4
            g=gx(j);
            h=hx(j);            
            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);
 			phig(1)=0.25*-(1.-h);
			phig(2)=0.25*(1.-h);
			phig(3)=0.25*(1.+h);
			phig(4)=0.25*-(1.+h);    
            Floc=phi(1)*F(Element(i,1))+phi(2)*F(Element(i,2))+phi(3)*F(Element(i,3))+phi(4)*F(Element(i,4));
            rset=phi(1)*lSet(Element(i,1))+phi(2)*lSet(Element(i,2))+phi(3)*lSet(Element(i,3))+phi(4)*lSet(Element(i,4));
            dls=phig(1)*lSet(Element(i,1))+phig(2)*lSet(Element(i,2))+phig(3)*lSet(Element(i,3))+phig(4)*lSet(Element(i,4));
            mMatL=mMatL+phi'*phi;
            mMatGLSL=mMatGLSL+((phig'*(dls/abs(dls)))*Floc*(h/abs(Floc)))*phi;
            f1L=f1L+phi'*Floc*abs(dls);
            f2L=f2L+(phig'*(dls/abs(dls))*Floc)*(h/abs(Floc))*Floc*abs(dls);
            vs=h*((abs(visc+Floc*abs(dls)))/(abs(Floc*dls)+h));
            f3L=f3L+vs*phig'*dls;
        end
        for k=1:4;
            for j=1:4;
               mMat(Element(i,j),Element(i,k))=mMat(Element(i,j),Element(i,k))+mMatL(j,k);
               mMatGLS(Element(i,j),Element(i,k))=mMatGLS(Element(i,j),Element(i,k))+mMatGLSL(j,k);           
            end
            f1(Element(i,k))=f1(Element(i,k))+f1L(k);
            f2(Element(i,k))=f2(Element(i,k))+f2L(k);
            f3(Element(i,k))=f3(Element(i,k))+f3L(k);       
        end
    end
    dt=0.01;
    lSet=lSet-((((mMat+mMatGLS)^-1)/dt)*(f1+f2+f3))';
end
%scatter3(Node(:,1), Node(:,2), lSet');