%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% bdm1d.m
% Authors: David Chao, Arizona State University 
%          Jay Oswald, Arizona State University 
% E-mail:  j-oswald@asu.edu
% Description: This code gives an example of the bridging domain method for
% one-dimensional wave propagation.  In this problem, atoms and elements
% exist on the whole domain, but are deactivated if the corresponding
% weight is zero.  The Lagrange multiplier constraints can be diagonalized
% in order to absorb high frequency energy at the coupling interface.  The
% boundary conditions are an applied force at the left end of the domain
% and zero displacement on the right end of the domain.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [] = bdm1d()
clc
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % User settings:
    atom_count     = 201;     % Number of atoms (in whole domain).
    domain_length  = 10.0;    % Size of domain.
    dt_factor      = 0.95;    % Fraction dt is of critical timestep.
    node_count     = 21;      % Number of nodes (in whole domain)
    A0             = 1e-4;    % area: m^2    
    sim_time       = 100.0;   % Duration of simulation.         
    atoms.m        = 1.0;     % Mass of atoms.
    bond_stiffness = 5.0;     % Stiffness of atomic bonds.
	tau            = 5.0;     % Time constant for load.
    % Switch to determine whether compatibility constraint is diagonalized.
    diagonalize_constraint = 1;
    % Applied force (on leftmost node/atom)
    applied_force  = @(t) -1.0e-3*cos(2*pi*t/tau)*exp(-t/tau);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
    % Number of elements.
    bond_length    = domain_length/(atom_count-1);
    chain_length   = bond_length*(atom_count-1);
    ne      = node_count-1;    
    dt_crit = sqrt(atoms.m/bond_stiffness);
    dt      = dt_crit * dt_factor;    
    nsteps  = ceil(sim_time / dt);
    
    % Don't plot every step (aim for 100 plots during sim time).
    plot_interval = max(1,floor(nsteps/100));
    
    % Compute density and modulus to match atomic properties.
    total_mass = atom_count * atoms.m;
    rho0 = total_mass / chain_length / A0;
    youngs = bond_stiffness*bond_length / A0;    
    
    % Generate initial atom coordinates.
    atoms.X = linspace(0, chain_length, atom_count)';
    mesh.X  = linspace(0, chain_length, node_count)';
	mesh.connect = [1:ne; 2:node_count];
    shape = @(xi) 0.5*[1-xi, 1+xi];
	
    % Very small MD weight factor for continuum region.
    MIN_MD_WEIGHT = 1e-8;
    md_elements = ne-floor(ne/3-1):ne;   % Make last 1/3 of elements FE.
                
    mesh.md_weight = (1.0-MIN_MD_WEIGHT)*ones(node_count, 1);
    for e = md_elements                
        mesh.md_weight(mesh.connect(:,e)) = MIN_MD_WEIGHT;
    end
    
    figure(1);
    plot(mesh.X, mesh.md_weight, 'b.-', mesh.X, 1-mesh.md_weight, 'r:');
    xlabel('Position');  ylabel('Weight');
    legend('MD weight', 'FE weight');
    axis([mesh.X(1), mesh.X(end), -1, 2]);               
        
    % Determine which element each atom is in.
    atoms.element = floor(atoms.X/(chain_length/ne))+1;          
    atoms.element(atoms.element > ne) = ne;               
    % Determine the Lagrangian coordinate of each atom (in parent
    % coordinates).
    atoms.parent_coord = zeros(length(atoms.element), 1);
    for i=1:length(atoms.element)
        xIe = mesh.X(mesh.connect(:,atoms.element(i)));
        xi = atoms.X(i) - xIe(1);        
        xi = 2.0*xi / range(xIe) - 1.0;        
        atoms.parent_coord(i) = xi;
    end                        
    shape_atom = @(a) shape(atoms.parent_coord(a));
    
    % First assign all atoms to FE region. (0=FE, 1=MD, 2=coupled).
    atoms.type = zeros(length(atoms.element), 1);
    atoms.md_weight = zeros(length(atoms.element),1);
    for i=1:length(atoms.element)        
        wIe = mesh.md_weight(mesh.connect(:, atoms.element(i)));
        N = shape(atoms.parent_coord(i));
        atoms.md_weight(i) = N*wIe;
        if atoms.md_weight(i) > 1.0-1.01*MIN_MD_WEIGHT                    
            atoms.type(i) = 1.0;
        elseif atoms.md_weight(i) > 1.01*MIN_MD_WEIGHT
            atoms.type(i) = 2.0;
        end            
    end    
    % Classify atom types.
    atoms.full_md = find(atoms.type == 1);
    atoms.coupled = find(atoms.type == 2);
    
    % Classify element types (0=FE, 1=MD, 2=coupled).
    mesh.element_type = zeros(length(mesh.connect));
    for e=1:length(mesh.connect)
       fe_wt = shape(0) * mesh.md_weight(mesh.connect(:,e));
       if fe_wt > 1.0 - 1.01*MIN_MD_WEIGHT
           mesh.element_type(e) = 1.0;
       elseif fe_wt > 1.01*MIN_MD_WEIGHT
           mesh.element_type(e) = 2.0;
       end
    end
    mesh.full_fe = find(mesh.element_type == 1);
    mesh.coupled = find(mesh.element_type == 2);    
    
    % Set initial conditions.    
    atoms.x = atoms.X;
    mesh.x  = mesh.X;        
    atoms.v = zeros(atom_count, 1);
    atoms.a = zeros(atom_count, 1);
    mesh.v  = zeros(node_count, 1);
    mesh.a  = zeros(node_count, 1);
                
    M = zeros(node_count, 1);
    for conn = mesh.connect
        Me = zeros(2,2);
        Le = range(mesh.X(conn));
        fe_wt = 1.0 - mesh.md_weight(conn);
        for xi = [1.0, -1.0]/sqrt(3);
            N  = shape(xi);
            w  = (N * fe_wt)*rho0*A0 * Le/2;
            Me = Me + N'*N*w;            
        end
        M(conn) = M(conn) + sum(Me)';
    end                
    
    % Compute constraint matrix (A).
    A = zeros(length(atoms.coupled));
    for i = 1:length(atoms.coupled)
        a  = atoms.coupled(i);
        Na = shape_atom(a);        
        nodes_a = mesh.connect(:,atoms.element(a));        
        for j = 1:length(atoms.coupled)
            b  = atoms.coupled(j);
            Nb = shape_atom(b);
            nodes_b = mesh.connect(:,atoms.element(a));                        
            A(i,j) = A(i,j) + Nb(nodes_b==nodes_a(1))*Na(1)/M(nodes_a(1));
            A(i,j) = A(i,j) + Nb(nodes_b==nodes_a(2))*Na(2)/M(nodes_a(2));
        end
        A(i,i) = A(i,i) + 1.0 / (atoms.m*atoms.md_weight(a));
    end
    
    % This command will lump the constraint matrix.
    if (diagonalize_constraint)
        A = diag(sum(A));
    end   
   
    figure(2);    
    % Integrate time with velocity verlet.
    for ts = 1:nsteps                            
        % Update atom coordinates.
        atoms.x = atoms.x + dt*atoms.v + 0.5*dt*dt*atoms.a;
        mesh.x  = mesh.x  + dt*mesh.v  + 0.5*dt*dt*mesh.a;
               
        % Compute force atom i makes on atom i + 1.
        f = bond_stiffness * (bond_length - diff(atoms.x));       
        % Newton's third law.
        f = [0; f] - [f; 0];
        f = f .* atoms.md_weight;
              
        f_node = zeros(node_count,1);
        for conn = mesh.connect
            xIe = mesh.x(conn);       % current coords
            XIe = mesh.X(conn);       % original coords
            Le = range(mesh.X(conn)); % original length
            
            uIe = xIe - XIe;            
            
            % Derivative of N with respect to X
            B   = [-0.5, 0.5] * 2.0/Le;  
            eps = B*uIe;
            P   = youngs * eps;
                                                                       
            w = A0*Le * shape(0.0) * (1.0-mesh.md_weight(conn));
            fe_int = B'*P*w;
            
            f_node(conn) = f_node(conn) - fe_int;            
        end                     
        fext = applied_force(ts*dt);
        f_node(1) = f_node(1) + fext * (1.0-mesh.md_weight(1));        
        f(1)      = f(1) + fext * atoms.md_weight(1);
        
        % Compute trial accelerations for MD and FE.        
        a_tr_md = f./(atoms.m * atoms.md_weight);
        a_tr_fe = f_node./M;
        
        % Compute trial velocities for MD and FE.
        v_tr_md = atoms.v + 0.5*dt*(atoms.a + a_tr_md);
        v_tr_fe = mesh.v  + 0.5*dt*(mesh.a  + a_tr_fe);
                
        % Solve for unknown Lagrange multipliers to satisfy constraint.
        g = zeros(length(atoms.coupled),1);
        for i = 1:length(g)
            a = atoms.coupled(i);
            N = shape_atom(a);
            conn = mesh.connect(:,atoms.element(a));
            g(i) = v_tr_md(a) - N*(v_tr_fe(conn));
        end                                        
        lambda = -(2.0/dt)*(A\g);                        

        % Compute corrected accelerations.
        afix_md = lambda ./ (atoms.m*atoms.md_weight(atoms.coupled));
        a_tr_md(atoms.coupled) = a_tr_md(atoms.coupled) + afix_md;
                
        for i = 1:length(lambda);
            a = atoms.coupled(i);
            N = shape_atom(a);
            conn = mesh.connect(:,atoms.element(a));
            a_tr_fe(conn) = a_tr_fe(conn) - (N'*lambda(i))./M(conn);        
        end
        a_tr_md(end) = 0;
        a_tr_fe(end) = 0;
        
        % Update atomic and finite element velocities.
        atoms.v = atoms.v + 0.5*dt*(atoms.a + a_tr_md);        
        mesh.v  = mesh.v + 0.5*dt*(mesh.a + a_tr_fe);
        atoms.a = a_tr_md;
        mesh.a  = a_tr_fe;                                               
               
        if mod(ts,plot_interval) == 0
            plot_result(mesh, atoms);
        end               
    end  
end

function [] = plot_result(mesh, atoms)
    % Plot results.
    clf; hold on;        
    % Plot only full fe or coupled elements (in different colors).
    u = mesh.x - mesh.X;        
    for e=1:length(mesh.connect)
        c = mesh.connect(:,e);
        if mesh.element_type(e) ~= 1
            plot(mesh.X(c), u(c), 's-');        
        end
    end

    % Plot only full md or coupled atoms (in different colors.
    plot(atoms.X(atoms.full_md), atoms.x(atoms.full_md)-atoms.X(atoms.full_md), 'k.');
    plot(atoms.X(atoms.coupled), atoms.x(atoms.coupled)-atoms.X(atoms.coupled), 'g.');
    xlabel('position');  ylabel('displacement');
    axis([0,max(mesh.X), -1e-3, 1e-3]);
    pause(0.01);        
end
