function extrapolate
%An implementation of quadratic extrapolation of a function along a signed
%distance field.

%The implementation most closely follows the description given by:
%Ng et al. 2009 "An efficient fluid–solid coupling algorithm for single-phase flows"
%But further details are available in:
%Min & Gibou 2007, "A second order accurate level set method on non-graded adaptive cartesian grids"
%Aslam 2003, "A partial differential equation approach to multidimensional extrapolation"

%The main difference is that for simplicity the time integration is 1st
%order RK (forward Euler) rather than TVD-RK2

domain_dims = [2*pi 2*pi];
domain_nodes = [70 70];
origin = [-pi -pi];
circle_origin = [0 0];
circle_rad = 2;
dx = domain_dims(1) / domain_nodes(1);
steps = 200;
dt = 0.5*dx;


%construct a sample signed distance field of a circle as test case (and its
%Heaviside function too)
phi = zeros(domain_nodes);
H0 = zeros(domain_nodes);
for i = 1:domain_nodes(1)
    for j = 1:domain_nodes(2)
        pos = origin + [i*dx j*dx];
        phi(i,j) = norm(pos - circle_origin) - circle_rad;
        if(phi(i,j) <= 0)
            H0(i,j) = 0;
        else
            H0(i,j) = 1;
        end
    end
end

%construct H1 Heaviside function
H1 = zeros(domain_nodes);
for i = 2:domain_nodes(1)-1
    for j = 2:domain_nodes(2)-1
        if(H0(i+1,j) == 0 && H0(i-1,j) == 0 && H0(i,j+1) == 0 && H0(i,j-1) == 0 )
            H1(i,j) = 0;
        else
            H1(i,j) = 1;
        end
    end
end

%construct H2 Heaviside function
H2 = zeros(domain_nodes);
for i = 3:domain_nodes(1)-2
    for j = 3:domain_nodes(2)-2
        if(H1(i+1,j) == 0 && H1(i-1,j) == 0 && H1(i,j+1) == 0 && H1(i,j-1) == 0 )
            H2(i,j) = 0;
        else
            H2(i,j) = 1;
        end
    end
end


%now set up some function on the interior
Q = zeros(domain_nodes);
for i = 1:domain_nodes(1)
    for j = 1:domain_nodes(2)
        pos = origin + [i*dx j*dx];
        if(phi(i,j) < 0)
            Q(i,j) = cos(pos(1))*sin(pos(2));
        else
            Q(i,j) = 0;
        end
        
    end
end

%visualize the input data
figure(1);
contourf(Q', 20);



% compute gradients of phi and Qn directional derivatives 
% with 2nd order central differences
n_x = zeros(domain_nodes);
n_y = zeros(domain_nodes);
Qn = zeros(domain_nodes);
for i = 2:domain_nodes(1)-1
    for j = 2:domain_nodes(2)-1
        phi_x = (phi(i+1,j) - phi(i-1,j)) / 2 / dx;
        phi_y = (phi(i,j+1) - phi(i,j-1)) / 2 / dx;
        length = sqrt(phi_x^2 + phi_y^2) + 1e-12;
        n_x(i,j) = phi_x / length;
        n_y(i,j) = phi_y / length;
        
        Q_x = (Q(i+1,j) - Q(i-1,j)) / 2 / dx;
        Q_y = (Q(i,j+1) - Q(i,j-1)) / 2 / dx;
        Qn(i,j) = [n_x(i,j) n_y(i,j)] * [Q_x Q_y]';
    end
end

%compute Qnn with central differences
Qnn = zeros(domain_nodes);
for i = 3:domain_nodes(1)-2
    for j = 3:domain_nodes(2)-2
        Qn_x = (Qn(i+1,j) - Qn(i-1,j)) / 2 / dx;
        Qn_y = (Qn(i,j+1) - Qn(i,j-1)) / 2 / dx;
        Qnn(i,j) = [n_x(i,j) n_y(i,j)] * [Qn_x Qn_y]';
    end
end


%extrapolate Qnn, with upwinding for derivatives
for step = 1:steps
    Q_old = Qnn;
    for i = 2:domain_nodes(1)-1
        for j = 2:domain_nodes(2)-1
            
            %upwind the first order differences
            if(n_x(i,j) < 0)
                grad_Q_x = (Q_old(i+1,j) - Q_old(i,j)) / dx;
            else
                grad_Q_x = (Q_old(i,j) - Q_old(i-1,j)) / dx;
            end
            
            if(n_y(i,j) < 0)
                grad_Q_y = (Q_old(i,j+1) - Q_old(i,j)) / dx;
            else
                grad_Q_y = (Q_old(i,j) - Q_old(i,j-1)) / dx;
            end
            
            %simple forward Euler
            Qnn(i,j) = Q_old(i,j) - dt * H2(i,j) * (n_x(i,j) * grad_Q_x + n_y(i,j) * grad_Q_y);
        end
    end
end

%extrapolate Qn with upwinding for derivatives
for step = 1:steps
    Q_old = Qn;
    for i = 2:domain_nodes(1)-1
        for j = 2:domain_nodes(2)-1
            
            %upwind the first order differences
            if(n_x(i,j) < 0)
                grad_Q_x = (Q_old(i+1,j) - Q_old(i,j)) / dx;
            else
                grad_Q_x = (Q_old(i,j) - Q_old(i-1,j)) / dx;
            end
            
            if(n_y(i,j) < 0)
                grad_Q_y = (Q_old(i,j+1) - Q_old(i,j)) / dx;
            else
                grad_Q_y = (Q_old(i,j) - Q_old(i,j-1)) / dx;
            end
            
            
            %simple forward Euler
            Qn(i,j) = Q_old(i,j) - dt * H1(i,j) * (n_x(i,j) * grad_Q_x + n_y(i,j) * grad_Q_y - Qnn(i,j));
        end
    end
end

%extrapolate Q itself, with 2nd order ENO for derivatives 
%(per Ron Fedkiw's course notes on convection from CS205b/CME306, Lecture 9)
for step = 1:steps
    Q_old = Q;
    
    for j = 3:domain_nodes(2)-2
        for i = 3:domain_nodes(1)-2        
            
            %first compute x-derivative
            %choose the upwind direction
            if(n_x(i,j) < 0)
                D1 = (Q_old(i+1,j) - Q_old(i,j)) / dx;
                k = i;

                %add either Q_old(i+2,j) or Q_old(i-1,j) to the approximation of the gradient
                D1next = (Q_old(i+2,j) - Q_old(i+1,j)) / dx;
                D1prev = (Q_old(i,j) - Q_old(i-1,j)) / dx;
            else
                D1 = (Q_old(i,j) - Q_old(i-1,j)) / dx;
                k = i - 1;

                %add either Q_old(i+1,j) or Q_old(i-2,j) to the approximation of the gradient
                D1next = (Q_old(i+1,j) - Q_old(i,j)) / dx;
                D1prev = (Q_old(i-1,j) - Q_old(i-2,j)) / dx;
            end

            %compute differences
            D2front = (D1next - D1)  / (2*dx);
            D2back =  (D1 - D1prev) / (2*dx);

            %choose the smaller of the two
            if(abs(D2front) <= abs(D2back)) 
                c = D2front;
            else
                c = D2back;
            end

            %compute the adjustment to get 2nd order accuracy
            Q2adjustment = c * (2*(i-k) - 1)*dx;

            %set the gradient accordingly
            gradient(1) = D1 + Q2adjustment;

            %now repeat the whole process in the y direction
            if(n_y(i,j) < 0)
                D1 = (Q_old(i,j+1) - Q_old(i,j)) / dx;
                k = i;
                D1next = (Q_old(i,j+2) - Q_old(i,j+1)) / dx;
                D1prev = (Q_old(i,j) - Q_old(i,j-1)) / dx;
            else
                D1 = (Q_old(i,j) - Q_old(i,j-1)) / dx;
                k = i - 1;
                D1next = (Q_old(i,j+1) - Q_old(i,j)) / dx;
                D1prev = (Q_old(i,j-1) - Q_old(i,j-2)) / dx;
            end
            D2front = (D1next - D1)  / (2*dx);
            D2back =  (D1 - D1prev) / (2*dx);
            if(abs(D2front) <= abs(D2back)) 
                c = D2front;
            else
                c = D2back;
            end
            Q2adjustment = c * (2*(i-k) - 1)*dx;

            %set the gradient accordingly
            gradient(2) = D1 + Q2adjustment;
            
            %simple forward Euler
            Q(i,j) = Q_old(i,j) - dt * H0(i,j) * (n_x(i,j) * gradient(1) + n_y(i,j) * gradient(2) - Qn(i,j));
        end
    end
end


%visualize it
figure(2);
contourf(Q', 20);