classdef Staggered1DAcousticsVariable < scheme.Scheme
   properties
        m % Number of points of primal grid in each direction, possibly a vector
        h % Grid spacing
        
        % Grids
        grid % Total grid object
        grid_primal 
        grid_dual

        order % Order accuracy for the approximation

        H  % Combined norm
        Hi % Inverse
        D % Semi-discrete approximation matrix

        D1_primal
        D1_dual

        % Pick out left or right boundary
        e_l
        e_r

        % Initial data
        v0

        % Pick out primal or dual component
        e_primal
        e_dual

        % System matrices
        A
        B

        %
        A_l, A_r
        B_l, B_r
    end


    methods
         % Scheme for A*u_t + B u_x = 0, 
         % u = [p, v];
         % A: Diagonal and A > 0,
         % A = [a1, 0;
         %      0,  a2]
         % 
         % B = B^T and with diagonal entries = 0.
         % B = [0 b 
         %     b 0] 
         % Here we store p on the primal grid and v on the dual
         function obj = Staggered1DAcousticsVariable(g, order, A, B)
            default_arg('B',{@(x)0*x, @(x)0*x+1; @(x)0*x+1, @(x)0*x});
            default_arg('A',{@(x)0*x+1, @(x)0*x; @(x)0*x, @(x)0*x+1});

            obj.order = order;
            obj.A = A;
            obj.B = B;

            % Grids
            obj.m = g.size();
            xl = g.getBoundary('l');
            xr = g.getBoundary('r');
            xlim = {xl, xr};

            % Boundary matrices
            obj.A_l = [A{1,1}(xl), A{1,2}(xl);....
                       A{2,1}(xl), A{2,2}(xl)]; 
            obj.A_r = [A{1,1}(xr), A{1,2}(xr);....
                       A{2,1}(xr), A{2,2}(xr)]; 
            obj.B_l = [B{1,1}(xl), B{1,2}(xl);....
                       B{2,1}(xl), B{2,2}(xl)]; 
            obj.B_r = [B{1,1}(xr), B{1,2}(xr);....
                       B{2,1}(xr), B{2,2}(xr)]; 
                
            obj.grid = g;
            obj.grid_primal = g.grids{1};
            obj.grid_dual = g.grids{2};
            x_primal = obj.grid_primal.points();
            x_dual = obj.grid_dual.points();
            
            % Get operators
            ops = sbp.D1StaggeredUpwind(obj.m, xlim, order);
            obj.h = ops.h;

            % Build combined operators
            H_primal = ops.H_primal;
            H_dual = ops.H_dual;
            obj.H = blockmatrix.toMatrix( {H_primal, []; [], H_dual } );
            obj.Hi = inv(obj.H);
        
            D1_primal = ops.D1_primal;
            D1_dual = ops.D1_dual;
            A11_B12 = spdiag(-1./A{1,1}(x_primal).*B{1,2}(x_primal), 0);
            A22_B21 = spdiag(-1./A{2,2}(x_dual).*B{2,1}(x_dual), 0);
            D = {[],                A11_B12*D1_primal;...
                 A22_B21*D1_dual,     []};
            obj.D = blockmatrix.toMatrix(D); 
            obj.D1_primal = D1_primal;
            obj.D1_dual = D1_dual;

            % Combined boundary operators
            e_primal_l = ops.e_primal_l;
            e_primal_r = ops.e_primal_r;
            e_dual_l = ops.e_dual_l;
            e_dual_r = ops.e_dual_r;
            e_l = {e_primal_l, [];...  
                   []       ,  e_dual_l};
            e_r = {e_primal_r, [];...  
                   []       ,  e_dual_r};
            obj.e_l = blockmatrix.toMatrix(e_l);
            obj.e_r = blockmatrix.toMatrix(e_r);

            % Pick out first or second component of solution
            N_primal = obj.grid_primal.N();
            N_dual = obj.grid_dual.N();
            obj.e_primal = [speye(N_primal, N_primal); sparse(N_dual, N_primal)];
            obj.e_dual = [sparse(N_primal, N_dual); speye(N_dual, N_dual)];


        end
        % Closure functions return the operators applied to the own domain to close the boundary
        % Penalty functions return the operators to force the solution. In the case of an interface it returns the operator applied to the other domain.
        %       boundary            is a string specifying the boundary e.g. 'l','r' or 'e','w','n','s'.
        %       type                is a string specifying the type of boundary condition if there are several.
        %       neighbour_scheme    is an instance of Scheme that should be interfaced to.
        %       neighbour_boundary  is a string specifying which boundary to interface to.
        function [closure, penalty] = boundary_condition(obj, boundary, type)
            
            default_arg('type','p');

            % type = 'p' => boundary condition for p
            % type = 'v' => boundary condition for v
            % No other types implemented yet

            % BC on the form Lu - g = 0;

            % Get boundary matrices
            switch boundary
                case 'l'
                    A = full(obj.A_l);
                    B = full(obj.B_l);
                case 'r'
                    A = full(obj.A_r);
                    B = full(obj.B_r);
            end

            % Diagonalize B
            [T, Lambda] = eig(B);
            lambda = diag(Lambda);

            % Identify in- and outgoing characteristic variables
            Iplus = lambda > 0;
            Iminus = lambda < 0;

            switch boundary
            case 'l'
                Iout = Iminus;
                Iin = Iplus;
            case 'r'
                Iout = Iplus;
                Iin = Iminus;
            end

            Tin = T(:,Iin);
            Tout = T(:,Iout);

            switch type
            case 'p'
                L = [1, 0];
            case 'v'
                L = [0, 1];
            case 'characteristic'
                L = Tin';
            otherwise
                error('Boundary condition not implemented.');
            end

            % Penalty parameters
            sigma = [0; 0];
            sigma(Iin) = lambda(Iin);
            switch boundary
            case 'l'
                tau = -1*obj.e_l * inv(A) * T * sigma * inv(L*Tin);  
                closure = obj.Hi*tau*L*obj.e_l';

            case 'r'
                tau = 1*obj.e_r * inv(A) * T * sigma * inv(L*Tin);  
                closure = obj.Hi*tau*L*obj.e_r';

            end
      
            penalty = -obj.Hi*tau;
                
         end
          
         function [closure, penalty] = interface(obj,boundary,neighbour_scheme,neighbour_boundary)

            error('Staggered1DAcoustics, interface not implemented');

             switch boundary
                 % Upwind coupling
                 case {'l','left'}
                     tau = -1*obj.e_l;
                     closure = obj.Hi*tau*obj.e_l';       
                     penalty = -obj.Hi*tau*neighbour_scheme.e_r';
                 case {'r','right'}
                     tau = 0*obj.e_r;
                     closure = obj.Hi*tau*obj.e_r';       
                     penalty = -obj.Hi*tau*neighbour_scheme.e_l';
             end
                 
         end
      
        function N = size(obj)
            N = obj.m;
        end

    end

    methods(Static)
        % Calculates the matrices needed for the inteface coupling between boundary bound_u of scheme schm_u
        % and bound_v of scheme schm_v.
        %   [uu, uv, vv, vu] = inteface_coupling(A,'r',B,'l')
        function [uu, uv, vv, vu] = interface_coupling(schm_u,bound_u,schm_v,bound_v)
            [uu,uv] = schm_u.interface(bound_u,schm_v,bound_v);
            [vv,vu] = schm_v.interface(bound_v,schm_u,bound_u);
        end
    end
end