function   varargout = ltpda_free_dynamics(varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% DESCRIPTION: DATA_REDUCTION converts interferometer data to acceleration
% data. This function returns the free dynamics without the action of the
% controller circuit. The commanded force has to be subtracted in order to
% obtain the output force noise
%
%
% CALL:                 b = ltpda_free_dynamics(varargin);
%                       ltpda_free_dynamics(varargin);
%
% INPUTS:      
%       - Aos containing the interferometer data. Aos must be supplied in
%       couples.
%       - plist containing the parasitic stiffness and the cross talk
%       coefficient. The default structure of the plist is:
%           plo = plist('pstiff1', -13e-7,'pstiff2', -20e-7,'cross_talk',
%           -1e-4, 'METHOD', 'PARFIT')
%           - 'pstiff1' is the square of the parasitic stiffness per unit of mass of TM1
%           - 'pstiff2' is the square of the parasitic stiffness per unit of mass of TM2
%           - 'cross_talk' is the sensitivity of the differential channel
%           to x1
%           - 'METHOD' define the method you want to use for the
%           calculation of the derivatives. Possible values are:
%               - 'PARFIT' makes use of the five points stencil parabolic
%               fit approximation [1] to calculate the derivatives
%               - 'SERIES' makes use of the five points stencil taylor
%               series approximation [2] to calculate the derivatives
% 
%       Es: b = ltpda_free_dynamics(ao1, ao2);
% 
%       Es: b = ltpda_free_dynamics(ao1, ao2, pl);
%              
%
%
% OUTPUTS:     
%       a vector of Aos containing the data obtained by the application of
%       the free dynamics to the input data (interferometer data)
%
% REFERENCES:
%           [1] L. Carbone et al., Physical Review D 75, 042001 (2007)
%           [2] S. E. Koonin and D. C. Meredith, COMPUTATIONAL PHYSICS -
%           Fortran Version, 1990, Westview Press
%
% VERSION:     $Id: ltpda_free_dynamics.m,v 1.1 2008/04/24 16:13:12 luigi Exp $
% 
% HISTORY:     18-03-2008 L Ferraioli
%                 Creation
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



%% Standard history variables

ALGONAME = mfilename;
VERSION  = '$Id: ltpda_free_dynamics.m,v 1.1 2008/04/24 16:13:12 luigi Exp $';
CATEGORY = 'Signal Processing';

%% Check if this is a call for parameters, the CVS version string
% or the method category
if nargin == 1 && ischar(varargin{1})
  in = char(varargin{1});
  if strcmpi(in, 'Params')
    varargout{1} = getDefaultPL();
    return
  elseif strcmpi(in, 'Version')
    varargout{1} = VERSION;
    return
  elseif strcmpi(in, 'Category')
    varargout{1} = CATEGORY;
    return
  end
end

%% Collect input ao's, plist's and ao variable names
in_names = {};
for ii = 1:nargin
  in_names{end+1} = inputname(ii);
end

[as, pl, invars] = collect_inputs(varargin, in_names);

% Checks that the input AOs come in pairs
if rem(length(as),2)
  warning('The input AOs must come in pairs!');
  return
end

% produce one parameter list
if isa(pl, 'plist')
  pl = combine(pl, getDefaultPL());
else
  pl = getDefaultPL();
end

%% Initialize outputs
% bs = [];

%% Defining Constants

fs = get(as(1),'fs');
wpx1 = find(pl, 'pstiff1');
wpx2 = find(pl, 'pstiff2');
dSD1 = find(pl, 'cross_talk');
T = 1/fs;

%% Defining plist for derivative functions

% In this case we need only second order derivatives and zero order in
% case we want to use the parabolic fit method
pl_2diff = plist('NAME', 'Second deriv plist', 'ORDER', 'SECOND');
pl_0diff = plist('NAME', 'Zero deriv plist', 'ORDER', 'ZERO');

%% Going through analysis objects

for jj = 1:2:numel(as)/2
          
    % Extracts the input data
    a1 = as(jj);
    a2 = as(jj+1);
    
    %% Applying "free dynamics" to data using the differentiation method
    %% defined in the input plist
    switch find(pl, 'METHOD')
        case 'PARFIT'
            d2_a1 = ltpda_parfit_derivative(a1, pl_2diff);
            d0_a1 = ltpda_parfit_derivative(a1, pl_0diff);
            d2_a2 = ltpda_parfit_derivative(a2, pl_2diff);
            d0_a2 = ltpda_parfit_derivative(a2, pl_0diff);
            
            b1 = d2_a1 + wpx1.*d0_a1;
            b12 = -dSD1.*d2_a1 + (wpx2-wpx1-(dSD1*wpx2)).*d0_a1;
            b22 = d2_a2 + wpx2.*d0_a2;
            b2 = b12 + b22;
            clear b12 b22
            
        case 'SERIES'
            d2_a1 = ltpda_series_derivative(a1, pl_2diff);
            d2_a2 = ltpda_series_derivative(a2, pl_2diff);
            
            b1 = d2_a1 + wpx1.*a1;
            b12 = -dSD1.*d2_a1 + (wpx2-wpx1-(dSD1*wpx2)).*a1;
            b22 = d2_a2 + wpx2.*a2;
            b2 = b12 + b22;
            clear b12 b22
    end

    % name, mfilename, description for these objects
    b1 = setnh(b1, 'name', sprintf('ltpda_free_dynamics(%s)', invars{jj}),...
       'description', find(pl,'description'));
    b2 = setnh(b2, 'name', sprintf('ltpda_free_dynamics(%s)', invars{jj+1}),...
        'description', find(pl,'description'));
%     bs = [bs b1 b2];
%     clear('b1', 'b2');
    
end

% %% Reshape the ouput to the same size of the input
% 
% varargout{1} = reshape(bs, size(as));

%% outputs
if nargout == 2
   varargout{1} = b1;
   varargout{2} = b2;
elseif nargout == 1
   varargout{1} = [b1 b2];
end
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION:    getDefaultPL
%
% DESCRIPTION: Get default params
%
% HISTORY:     01-02-2008 M Hueller
%                 Creation
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function plo = getDefaultPL()

plo = plist('pstiff1', -13e-7,'pstiff2', -20e-7,'cross_talk', -1e-4,'METHOD', 'PARFIT', ... 
    'DESCRIPTION','Application of the free dynamics to interferometer data');