function [ ] = bimat( )
clc; clf;
    mesh = uniform_mesh(30, 1, 10, 1, -5, -0.5);
    % assign level set values (top of domain is positive)
    mesh.phi = mesh.X(2,:);
    
    % Subdivides element into two for quadrature.
    mesh.quad = [-1, -1, 1, 1; -1,  1,-1, 1] / sqrt(3);
    mesh.quad = [mesh.quad; 1,1,1,1];
    qtop = mesh.quad; qbot = mesh.quad;
    qtop(2,:) = qtop(2,:)/2.0 + 0.5;
    qbot(2,:) = qbot(2,:)/2.0 - 0.5;
        
    mesh.quad = [qtop, qbot];
    mesh.quad(3,:) = mesh.quad(3,:) / 2.0;
        
    % Constructs consistent mass matrix.
    m = sparse(2*mesh.num_nodes, 2*mesh.num_nodes);
    for c = mesh.connectivity        
        phie = mesh.phi(:,c);
        xe = mesh.X(:,c);
        me = zeros(8,8);
        for q = mesh.quad
            N = shape(q);
            dN = gradshape(q);
            Hf = N*phie' > 0;
            HfI = phie>0;
            NN = [shape(q), N.*(Hf - HfI)];
            me = me + NN'*NN * (det(xe*dN)*q(3));
        end
        cc = [c, c+mesh.num_nodes];
        m(cc,cc) = m(cc,cc) + me;
    end    
    
    % Nodal variables.  
    % There are four degrees of freedom per node [ux, uy, qx, qy] where
    % qx and qy are the enriched degrees of freedom.
    mesh.u = zeros(4, mesh.num_nodes);
    mesh.v = zeros(4, mesh.num_nodes);
    mesh.a = zeros(4, mesh.num_nodes);    
    
    mesh.left = find(mesh.X(1,:) == min(mesh.X(1,:)));   
        
    dt = 0.05;
    num_steps = 100;
    for ts=1:num_steps
        
        % apply velocity BC on left nodes.
        mesh.v(1,mesh.left) = 0.05;
        mesh.a(1,mesh.left) = 0.00;
        mesh.u = mesh.u + dt*mesh.v + 0.5*dt*dt*mesh.a;
        
        plot_result(mesh);
        
        assert(norm(mesh.u)/length(mesh.u) < 10, 'disp is too big');        
        f = zeros(4, mesh.num_nodes);
                
        % compute internal forces
        for c = mesh.connectivity                        
            % Gather element variables.
            Xe   = mesh.X(:,c);
            xe   = Xe + mesh.u(1:2,c);
            qe   = mesh.u(3:4, c);            
            phie = mesh.phi(c);                        
            fint = zeros(4,4);
            for q = mesh.quad
                N = shape(q);
                dNdX = gradshape(q);
                J = Xe * dNdX;
                dNdX = dNdX / J;
                                
                Hf = N*phie' > 0;                
                HfI = phie>0;
                dQdX = diag((Hf - HfI)) * dNdX;

                F = xe*dNdX + qe*dQdX;
                E = 0.5*(F'*F - eye(2));
                
                if Hf > 0
                    lambda = 1;
                    mu = 0.5;
                else
                    lambda = 2;
                    mu = 1.0;
                end
                
                % Kirchoff material law.
                S = lambda*trace(E)*eye(2) + 2.0*mu*E;
                P = S*F';
                                                
                wt = det(J)*q(3);                
                fint(1:2,:) = fint(1:2,:) + (dNdX*P)' * wt;
                fint(3:4,:) = fint(3:4,:) + (dQdX*P)' * wt;                                                
            end
            
            f(:,c) = f(:,c) - fint;
        end        
                               
        mesh.v = mesh.v + 0.5*dt*mesh.a;
        
        % Compute new acceleration from nodal forces.
        ax = [f(1,:),f(3,:)] / m;
        ay = [f(2,:),f(4,:)] / m;
                
        n = mesh.num_nodes;
        mesh.a(1,:) = ax(:,1:n);
        mesh.a(2,:) = ay(:,1:n);
        mesh.a(3,:) = ax(:,n+1:end);
        mesh.a(4,:) = ay(:,n+1:end);                        
        
        mesh.v = mesh.v + 0.5*dt*mesh.a;                
    end   
end

function [N] = shape(x)
    N = 0.25 * [(1-x(1))*(1-x(2)), (1+x(1))*(1-x(2)),  ...
                (1+x(1))*(1+x(2)), (1-x(1))*(1+x(2))]; 
end

function [dN] = gradshape(x)
	dN = 0.25*[-1+x(2), -1+x(1);
                1-x(2), -1-x(1);
                1+x(2),  1+x(1);
               -1-x(2),  1-x(1)];
end

% plots the velocity field (vx) including discontinuities from xfem.
function [] = plot_result(mesh)
    clf        
    x0 = min(mesh.X(1,:));    x1 = max(mesh.X(1,:));
    y0 = min(mesh.X(2,:));    y1 = max(mesh.X(2,:));
    dx = mesh.X(1,2) - mesh.X(1,1);
        
    xx = linspace(x0, x1, 50);
    yy = linspace(y0, y1, 30);
    z = zeros(length(xx), length(yy));
    for i = 1:length(xx)
        x = xx(i);
        e = min(floor((x-x0)/dx) + 1, mesh.num_elements);                
        xi = 2.0*(x-x0-dx*(e-1))-1.0;
        c = mesh.connectivity(:,e);
        phie = mesh.phi(c);
        ve = mesh.v(:,c);                
        
        for j = 1:length(yy)
            y = yy(j);
            yi = 2.0*(y-y0)-1.0;
            
            N = shape([xi;yi]);
            Hf = N*phie' > 0;
            HfI = phie>0;
            v = ve(1:2,:) * N' + ve(3:4,:) * (N.*(Hf - HfI))';            
            z(i,j) = v(1);
        end
    end
    surf(xx, yy, z', 'facecolor','interp');
    view(180,45)
    pause(0.01);
    
end

% Creates a unifom mesh of ex by ey elements 
% with length of lx and ly in the x and y directions.
% The origin (lower left) can be optionally specified as x0, y0.
function [m] = uniform_mesh(ex, ey, lx, ly, x0, y0)
    % if origin is not specified, then set it to zero.
    if nargin < 5, x0 = 0; end
    if nargin < 6, y0 = 0; end       
        
    m.num_nodes = (ex+1)*(ey+1);
    % Nodal reference coordinates.
    m.X    = zeros(2, m.num_nodes);   
    for j=1:ey+1
        for i=1:ex+1
            m.X(:,i+(j-1)*(ex+1)) = [(i-1)*lx/ex + x0; (j-1)*ly/ey + y0];
        end
    end
    m.num_elements = ex*ey;
	m.connectivity = zeros(4, m.num_elements);
    for j=1:ey
        for i=1:ex            
            % first node in element
            n0 = i+(j-1)*(ex+1);            
            m.connectivity(:,i+(j-1)*ex) = [n0; n0+1; n0+ex+2; n0+1+ex];            
        end
    end
        
    % 4 point Gaussian quadrature rule.
    m.quad_points = [-1, 1, 1,-1;
                     -1,-1, 1, 1] / sqrt(3);
    m.quad_points = [m.quad_points; 1,1,1,1];
                        
end