% Examination of the response of a forcing function with two input
% frequencies to several different spring force (capacitance)
% coefficients for the linear constant-coefficient non-homogeneous
% system 
%    a y'' + b y' + c y = -f0 cos(w1 t) - f1 cos(w2 t).
% We assume that a, b and c are positive.
%
% We graph in one subplot the input: the forcing function and its
% components; and in another, the long-term response
% (particular solution) for the system.  This is done for several
% different values of c, with pauses between them.
%
% by Gavin LaRose, 10/10/2012
% 
% (c)2012 Regents of the University of Michigan
% This work is licensed under the
% Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.
% To view a copy of this license, visit
% http://creativecommons.org/licenses/by-nc-sa/3.0/
% or send a letter to
% Creative Commons,
% 444 Castro Street, Suite 900,
% Mountain View, CA, 94041, USA.

function Response_Multiple()
%-------------------------------------------------------------------------------
% configuration variables

% the default values (a = 1, b = 0.1, c = 1, f0 = 1, f1 = 4/5, w0 = 1, 
% w1 = 5) are taken from Borelli & Coleman, Differential Equations, a
% Modeling Perspective, 2nd ed. New York:Wiley, 2004.

% we are solving a y'' + b y' + c y = -f0 cos(w1 t) - f1 cos(w2 t)
%    values of c are set from the list c_values, below
    a = 1;
    b = 0.1;

% initial conditions used in generating the solution: y(0) = y0,
%    y'(0) = yp0
    y0  = 1;
    yp0 = 0;

    f0 = 1;
    f1 = 4;

% the frequencies
    w0  = 1;
    w1  = 5;
% values of c to consider (an input frequency is selected when 
%    c = a w^2)
    c_values = [ 0.1 1 25 81 ];
    
% the range of times on which to plot the solutions
    t_range = [ 0 30 ];
    
% color choices
    full_color   = 'k';
    partic_color = 'g';
    force_color    = 'c';
    background_grey      = [ 0.6 0.6 0.6 ];
    
%-------------------------------------------------------------------------------
% initialization

    scrsz = get(0,'ScreenSize');
    figh = figure('Position',[1 scrsz(4)/4 scrsz(3)/4 scrsz(4)/2]);
    axes( 'Parent', figh, 'Color', background_grey );
    hold on;
    
% set up subplots
    forc_sub_plot = subplot(2,1,1, 'Parent', figh, 'Color', background_grey );
    resp_sub_plot = subplot(2,1,2, 'Parent', figh, 'Color', background_grey );
    hold( forc_sub_plot );
    hold( resp_sub_plot );

% label these
    function c=num2charspc(v)
        if v == 1
            c = ' ';
        else
            c = [ num2str(v) ' ' ];
        end
    end
    if b == 0
        term2 = ' ';
    else
        term2 = [ '+ ' num2charspc(b) 'y\prime' ];
    end
    term3 = [ '+ c y\prime' ];

    xlabel( 't', 'FontSize', 16, 'Parent', forc_sub_plot );
    ylabel( 'y', 'FontSize', 16, 'Parent', forc_sub_plot );
    xlabel( 't', 'FontSize', 16, 'Parent', resp_sub_plot );
    ylabel( 'y', 'FontSize', 16, 'Parent', resp_sub_plot );

    title( [ 'Forcing f(t) = -' num2charspc(f0) 'cos(' num2str(w0) ' t) - ' ...
             num2charspc(f1) 'cos(' num2str(w1) ' t)' ], ...
           'FontSize', 16, 'Parent', forc_sub_plot );
    title( [ 'Particular solution to ' ...
             num2charspc(a) 'y\prime\prime' term2 term3 ' = f(t)' ], ...
           'FontSize', 16, 'Parent', resp_sub_plot );
    
% the maximum amplitude is 
    max_amp = max( amp( a, b, c_values, [f0 f1], [w0 w1] ) );

% we can then set the axes for the subplots
    force_amp = round(sqrt(f0^2 + f1^2));
    axis( forc_sub_plot, [ t_range(1) t_range(2) -force_amp-1 force_amp+1 ] );
    axis( resp_sub_plot, [ t_range(1) t_range(2) -max_amp-1 max_amp+1 ] );

% the vector of t values on which to graph the solutions
    t_vals  = linspace( t_range(1), t_range(2), 5000 );

%-------------------------------------------------------------------------------
% do graphing

    pause on;

% go through plots for each value of c
    for k=1:length(c_values)
        cv = c_values(k);
        partic_solution = partic_soln( a, b, cv, [f0 f1], [w0 w1], t_vals );
        full_solution   = full_soln( a, b, cv, [f0 f1], [w0 w1], ...
                                     y0, yp0, t_vals );
        force_term1     = -f0*cos(w0*t_vals);
        force_term2     = -f1*cos(w1*t_vals);
        force_term      = force_term1 + force_term2;

        forc_plot_handles = ...
            plot( t_vals, force_term1, ['--' force_color], ...
                  t_vals, force_term2, ['-.' force_color], ...
                  t_vals, force_term,  ['-' full_color], ...
                  'LineWidth', 2, 'Parent', forc_sub_plot );
        sol_legend_handle = legend( forc_plot_handles, ...
                                    { [ '-' num2str(f0) 'cos(' num2str(w0) ...
                                        ' t)' ], ...
                                      [ '-' num2str(f1) 'cos(' num2str(w1) ...
                                        ' t)' ], ...
                                      [ 'f(t)' ] } );
        % resp_plot_handles = ...
        %     plot( t_vals, partic_solution, ['-' partic_color], ...
        %           t_vals, full_solution, ['-' full_color], ...
        %           'LineWidth', 2, 'Parent', resp_sub_plot );
        resp_plot_handles = ...
            plot( t_vals, partic_solution, ['-' partic_color], ...
                  'LineWidth', 2, 'Parent', resp_sub_plot );
        % resp_legend_handle = legend( resp_plot_handles, ...
        %                              { [ 'c=' num2str(cv) ...
        %                                  ' particular solution y_p' ], ...
        %                                  [ 'c=' num2str(cv) ...
        %                                  ' full solution y_p + y_c' ] 
        %                              } );
        resp_legend_handle = legend( resp_plot_handles, ...
                                     { [ 'c=' num2str(cv) ...
                                         ' particular solution y_p' ] } );

        if k ~= length(c_values)
            disp( [ 'Press any key to continue ' ] );
            pause;
            delete( forc_plot_handles );
            delete( resp_plot_handles );
        end
    
        % delete( sol_legend_handle );
        % delete( resp_legend_handle );
    end
    
%-------------------------------------------------------------------------------
% helper functions to find amplitude and solutions

% the amplitude of the particular solution (or zero, if it's unbounded)
%    this allows for c, f and w to be vectors, with f and w having two
%    entries each. 
    function out = amp( a, b, c, f, w )
        f0 = f(1);
        f1 = f(2);
        w0 = w(1);
        w1 = w(2);
        if b > 0
            out = sqrt( f0^2./(c.^2 + b^2*w0^2 - 2*a*c.*w0^2 + a^2*w0^4) ) + ...
                  sqrt( f1^2./(c.^2 + b^2*w1^2 - 2*a*c.*w1^2 + a^2*w1^4) );
        else
            if ismember( 0, c - a*w0^2 ) | ismember( 0, c - a*w1^2 )
    		out = abs( f0.*(1 - ((c - a*w0^2)==0)) ./ ...
                           ((c - a*w0^2) + (c - a*w0^2 == 0)) ) + ...
                      abs( f1.*(1 - ((c - a*w1^2)==0)) ./ ...
                           ((c - a*w1^2) + (c - a*w1^2 == 0)) );
            else
                out = abs( f0./(c - a*w0^2) ) + abs( f1./(c - a*w1^2) );
            end
 	end
    end

% the particular solution to the equation; all parameters are scalar
%    save the range of t values, f and w, which have two values each
    function out = partic_soln( a, b, cv, f, w, tvals )
        if b > 0
            out = zeros( 1, length(tvals) );
            for j=1:2
                den = (cv - a*w(j)^2)^2 + (b*w(j))^2;
                a0  = (cv - a*w(j)^2)*(-1*f(j))/den;
                b0 = (-1*b*w(j)*f(j))/den;
            
                out = out + a0*cos(w(j)*tvals) + b0*sin(w(j)*tvals);
            end
            
            % den1 = (cv - a*w(1)^2)^2 + (b*w(1))^2;
            % a01  = (cv - a*w(1)^2)*(-1*f(1))/den1;
            % b01 = (-1*b*w(1)*f(1))/den1;
            
            % den2 = (cv - a*w(2)^2)^2 + (b*w(2))^2;
            % a02  = (cv - a*w(2)^2)*(-1*f(2))/den2;
            % b02 = (-1*b*w(2)*f(2))/den2;

            % out = a01*cos(w(1)*tvals) + b01*sin(w(1)*tvals) + ...
            %       a02*cos(w(2)*tvals) + b02*sin(w(2)*tvals);
        else
            out = zeros( 1, length(tvals) );
            for j=1:2
                if cv - a*w(j)^2 ~= 0
                    out = out + (-1*f(j)/(cv - a*w(j)^2))*cos(w(j)*tvals);
                else
                    out = out + (-1*f(j)/(2*sqrt(cv*a)))*tvals.*sin(w(j)*tvals);
                end
            end
        end
    end

% the full solution to the problem; all parameters are scalar save the
%    range of t values, f and w, which have two values each
    function out = full_soln( a, b, c, f, w, y0, yp0, tvals )

    % the particular solution is
        ypartic = partic_soln( a, b, c, f, w, tvals );

    % find roots of characteristic polynomial
        char_roots = roots( [a b c] ); 
        realr = real( char_roots(1) );
        imagr = real( char_roots(1) );

        if b > 0
            p0  = 0;
            pp0 = 0;
            for j=1:2
                den = (c - a*w(j)^2) + (b*w(j))^2;
                % the values of the particular solution and its derivative
                %    at zero are
                p0  = p0 + (c - a*w(j)^2)*(-1*f(j)) / den;
                pp0 = pp0 + (-1*b*w(j)^2*f(j)) / den;
            end
            
            if imagr ~= 0
            % solve for c1, c2
                tmpc = [1 0; realr imagr]^-1 * [y0 - p0; yp0 - pp0];
                out = exp(realr*tvals).*( tmpc(1)*cos(imagr*tvals) + ...
                                          tmpc(2)*sin(imagr*tvals) ) + ...
                      ypartic;
            else
                r1 = real( char_roots(1) );
                r2 = real( char_roots(2) );
                
                if r1 ~= r2
                    tmpc = [1 1; r1 r2]^-1 * [y0 - p0; yp0 - pp0];
                    out = tmpc(1)*exp(r1*tvals) + tmpc(2)*exp(r2* ...
                                                              tvals) + ...
                          ypartic;
                else
                    tmpc = [1 0; r1 1]^1 * [y0 - p0; yp0 - pp0];
                    out = tmpc(1)*exp(r1*tvals) + ...
                          tmpc(2)*tvals.*exp(r1*tvals) + ...
                          ypartic;
                end
            end
        else
            p0  = 0;
            pp0 = 0;
            for j=1:2
                if c - a*w(j)^2 ~= 0
                    p0 = p0 + -1*f(j)/(c - a*w(j)^2);
                end
            end
            
            tmpc = [1 0; realr imagr]^-1 * [y0 - p0; yp0];
            out = tmpc(1)*cos(imagr*tvals) + tmpc(2)*sin(imagr*tvals) + ...
                  ypartic;
        end
    end
end

% all done
%-------------------------------------------------------------------------------
