% DISPERSION computes the dispersion function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% DESCRIPTION: DISPERSION computes the dispersion function
%
% CALL:        bs = dipersion(in,pl)
%
% INPUTS:      in      - matrix objects with input signals to the system
%              model   - symbolic models containing the transfer function model
%
%              pl      - parameter list
%
% OUTPUTS:     bs   - dispersion function AO
%
% <a href="matlab:utils.helper.displayMethodInfo('matrix', 'dispersion')">Parameters Description</a>
%
% VERSION:    $Id: dispersion.m,v 1.1 2011/06/22 09:54:19 miquel Exp $
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function varargout = dispersion(varargin)

% Check if this is a call for parameters
if utils.helper.isinfocall(varargin{:})
  varargout{1} = getInfo(varargin{3});
  return
end

import utils.const.*
utils.helper.msg(msg.PROC3, 'running %s/%s', mfilename('class'), mfilename);

% Method can not be used as a modifier
if nargout == 0
  error('### crb cannot be used as a modifier. Please give an output variable.');
end

% Collect input variable names
in_names = cell(size(varargin));
for ii = 1:nargin,in_names{ii} = inputname(ii);end

% Collect all AOs smodels and plists
[mtxs, mtxs_invars] = utils.helper.collect_objects(varargin(:), 'matrix', in_names);
pl              = utils.helper.collect_objects(varargin(:), 'plist', in_names);

% Combine plists
pl = parse(pl, getDefaultPlist);

% get params
params = find(pl,'FitParams');
numparams = find(pl,'paramsValues');
mmdl = find(pl,'model');
channel = find(pl,'channel');
mtxns = find(pl,'noise');
outModel = find(pl,'outModel');
bmdl = find(pl,'built-in');
f1 = find(pl,'f1');
f2 = find(pl,'f2');
pseudoinv = find(pl,'pinv');
tol = find(pl,'tol');
outNames = find(pl,'outNames');
inNames = find(pl,'inNames');

% Decide on a deep copy or a modify
fin = copy(mtxs, nargout);
n = copy(mtxns, nargout);
mdl = copy(mmdl,1);

% Get number of experiments
nexp = numel(fin);

% fft
% fin = fft(in);

% N should get before spliting, in order to convert correctly from psd to
% fft
N = length(fin(1).getObjectAtIndex(1).x);

% Get rid of fft f =0, reduce frequency range if needed
if ~isempty(f1) && ~isempty(f2)
  fin = split(fin,plist('frequencies',[f1 f2]));
end

FMall = zeros(numel(params),numel(params));
% loop over experiments
for k = 1:nexp
  
  utils.helper.msg(msg.IMPORTANT, sprintf('Analysis of experiment #%d',k), mfilename('class'), mfilename);
  
%   if (((numel(n(1).objs)) == 2) && (numel(fin(1).objs) == 2))
    
    % use signal fft to get frequency vector. Take into account signal
    % could be empty or set to zero
    % 1st channel
    if all(fin(k).getObjectAtIndex(1,1).y == 0) || isempty(fin(k).getObjectAtIndex(1,1).y)
      i1 = ao(plist('type','fsdata','xvals',0,'yvals',0));
    else
      i1 = fin(k).getObjectAtIndex(1,1);
      freqs = i1.x;
    end
    % 2nd channel
    if all(fin(k).getObjectAtIndex(2,1).y == 0) || isempty(fin(k).getObjectAtIndex(2,1).y)
      i2 = ao(plist('type','fsdata','xvals',0,'yvals',0));
    else
      i2 = fin(k).getObjectAtIndex(2,1);
      freqs = i2.x;
    end
    
    % Compute psd
    n1  = lpsd(n(k).getObjectAtIndex(1,1));
    n2  = lpsd(n(k).getObjectAtIndex(2,1));
    n12 = lcpsd(n(k).getObjectAtIndex(1,1),n(k).getObjectAtIndex(2,1));
    
    % interpolate to given frequencies
    % noise
    S11 = interp(n1,plist('vertices',freqs));
    S12 = interp(n12,plist('vertices',freqs));
    S22 = interp(n2,plist('vertices',freqs));
    S21 = conj(S12);
    
    % get some parameters used below
    fs = S11.fs;
    
    
%     if (isempty(outModel))
      
%       if ~isempty(bmdl)
        % compute built-in smodels
%         for i = 1:4
%           if strcmp(bmdl{i},'0');
%             h(i) = smodel('0');
%             h(i).setXvar('f');
%             h(i).setXvals(freqs);
%             h(i).setParams(params,numparams);
%           else
%             h(i) = smodel(plist('built-in',bmdl{i},'f',freqs));
%             % set all params to all models. It is not true but harmless
%             for ii = 1:numel(params)
%               vecparams(ii) = {numparams(ii)*ones(size(freqs))};
%             end
%             h(i).setParams(params,vecparams);
%           end
%         end
%       elseif ~isempty(mdl) && all(strcmp(class(mdl),'matrix'))
    
        for i = 1:numel(mdl.objs)
          % set Xvals
          h(i) = mdl.getObjectAtIndex(i).setXvals(freqs);
          % set alias
          h(i).assignalias(mdl.objs(i),plist('xvals',freqs));
          % set paramaters
          h(i).setParams(params,numparams);
        end
        % differentiate and eval
        for i = 1:length(params)
          utils.helper.msg(msg.IMPORTANT, sprintf('computing symbolic differentiation with respect %s',params{i}), mfilename('class'), mfilename);
          % differentiate symbolically
          dH11 = diff(h(1),params{i});
          dH12 = diff(h(3),params{i});  % taking into account matrix index convention h(2) > H(2,1)
          dH21 = diff(h(2),params{i});
          dH22 = diff(h(4),params{i});
          % evaluate
          d11(i) = eval(dH11,plist('output type','fsdata','output x',freqs));
          d12(i) = eval(dH12,plist('output type','fsdata','output x',freqs));
          d21(i) = eval(dH21,plist('output type','fsdata','output x',freqs));
          d22(i) = eval(dH22,plist('output type','fsdata','output x',freqs));
        end
        
%       elseif ~isempty(mdl) && all(strcmp(class(mdl),'ssm'))
%         
%         meval = copy(mdl,1);
%         % set parameter values
%         meval.doSetParameters(params, numparams);
%         
%         % make numeric
%         %         meval.doSubsParameters(params, true);
%         
%         % get the differentiation step
%         step = find(pl,'diffStep');
%         if isempty(step)
%           error('### Please input a step for the numerical differentiation')
%         end
%         
%         % differentiate and eval
%         for i = 1:length(params)
%           utils.helper.msg(msg.IMPORTANT, sprintf('computing numerical differentiation with respect %s, Step:%4.2d ',params{i},step(i)), mfilename('class'), mfilename);
%           % differentiate numerically
%           dH = meval.parameterDiff(plist('names', params(i),'values',step(i)));
%           % create plist with correct outNames (since parameterDiff change them)
%           out1 = strrep(outNames{1},'.', sprintf('_DIFF_%s.',params{i})); % 2x2 case
%           out2 =strrep(outNames{2},'.', sprintf('_DIFF_%s.',params{i}));
%           spl = plist('set', 'for bode', ...
%             'outputs', {out1,out2}, ...
%             'inputs', inNames, ...
%             'reorganize', true,...
%             'f', freqs);
%           % do bode
%           d  = bode(dH, spl);
%           % assign according matlab's matrix notation: H(1,1)->h(1)  H(2,1)->h(2)  H(1,2)->h(3)  H(2,2)->h(4)
%           d11(i) = d(1);
%           d21(i) = d(2);
%           d12(i) = d(3);
%           d22(i) = d(4);
%         end
%         
%       else
%         error('### please introduce models for the transfer functions')
%       end
      
%     elseif (~isempty(outModel))
%       
%       %   if(~isempty(outModel))
%       for lll=1:size(outModel,1)
%         for kkk=1:size(outModel,2)
%           outModel(lll,kkk) = split(outModel(lll,kkk),plist('frequencies',[f1 f2]));
%         end
%       end
%       %end
%       
%       % Avoid numerical differentiation (faster for the magnetic case)
%       Param{1} = [1 0;
%         0 0;];
%       Param{2} = [0 0;
%         0 1;];
%       
%       for pp = 1:length(params)
%         for ll = 1:size(outModel,1)
%           for kk = 1:size(Param{pp},2)
%             % index convention: H(1,1)->h(1)  H(2,1)->h(2)  H(1,2)->h(3)  H(2,2)->h(4)
%             tmp = 0;
%             for innerIndex = 1:size(outModel,2)
%               tmp = tmp + outModel(ll,innerIndex).y * Param{pp}(innerIndex,kk);
%             end
%             h{pp}(:,(kk-1)*size(outModel,1) + ll) = tmp;
%           end
%         end
%         
%       end
%       
%       for i = 1:length(params)
%         d11(i).y =  h{i}(:,1);
%         d21(i).y =  h{i}(:,2);
%         d12(i).y =  h{i}(:,3);
%         d22(i).y =  h{i}(:,4);
%       end
%       
%     end
    
    % scaling of PSD
    % PSD = 2/(N*fs) * FFT *conj(FFT)
    C11 = N*fs/2.*S11.y;
    C22 = N*fs/2.*S22.y;
    C12 = N*fs/2.*S12.y;
    C21 = N*fs/2.*S21.y;
    
    % compute elements of inverse cross-spectrum matrix
    InvS11 = (C22./(C11.*C22 - C12.*C21));
    InvS22 = (C11./(C11.*C22 - C12.*C21));
    InvS12 = (C21./(C11.*C22 - C12.*C21));
    InvS21 = (C12./(C11.*C22 - C12.*C21));
    
    
    % compute Fisher Matrix
    for i =1:length(params)
      for j =1:length(params)
        
        v1v1 = conj(d11(i).y.*i1.y + d12(i).y.*i2.y).*(d11(j).y.*i1.y + d12(j).y.*i2.y);
        v2v2 = conj(d21(i).y.*i1.y + d22(i).y.*i2.y).*(d21(j).y.*i1.y + d22(j).y.*i2.y);
        v1v2 = conj(d11(i).y.*i1.y + d12(i).y.*i2.y).*(d21(j).y.*i1.y + d22(j).y.*i2.y);
        v2v1 = conj(d21(i).y.*i1.y + d22(i).y.*i2.y).*(d11(j).y.*i1.y + d12(j).y.*i2.y);
        
        FisMat(i,j) = sum(real(InvS11.*v1v1 + InvS22.*v2v2 - InvS12.*v1v2 - InvS21.*v2v1));
      end
    end
    utils.helper.msg(msg.IMPORTANT, sprintf('rank(FisMat) = %d', rank(FisMat)));

    for kk = 1:numel(freqs)
      % create input signal with power at single freq. 
      % depending on input channel
      p = zeros(1,numel(freqs));
      if channel == 1
        p(kk) = sum(i1.y);
        % create aos
        i1single = ao(plist('Xvals',freqs,'Yvals',p));
        i2single = ao(plist('Xvals',freqs,'Yvals',zeros(1,numel(freqs))));
      elseif channel == 2
        p(kk) = sum(i2.y);
        % create aos
        i1single = ao(plist('Xvals',freqs,'Yvals',zeros(1,numel(freqs))));
        i2single = ao(plist('Xvals',freqs,'Yvals',p));
      else
        error('### wrong channel')
      end
      
      % compute Fisher Matrix for single frequencies
      for i =1:length(params)
        for j =1:length(params)
          
          v1v1 = conj(d11(i).y.*i1single.y + d12(i).y.*i2single.y).*(d11(j).y.*i1single.y + d12(j).y.*i2single.y);
          v2v2 = conj(d21(i).y.*i1single.y + d22(i).y.*i2single.y).*(d21(j).y.*i1single.y + d22(j).y.*i2single.y);
          v1v2 = conj(d11(i).y.*i1single.y + d12(i).y.*i2single.y).*(d21(j).y.*i1single.y + d22(j).y.*i2single.y);
          v2v1 = conj(d21(i).y.*i1single.y + d22(i).y.*i2single.y).*(d11(j).y.*i1single.y + d12(j).y.*i2single.y);
          
          FisMatsingle(i,j) = sum(real(InvS11.*v1v1 + InvS22.*v2v2 - InvS12.*v1v2 - InvS21.*v2v1));
        end
      end
      d(kk) = trace(FisMat\FisMatsingle)/numel(i1.x);    % had to divide for num. freqs
%         d(kk) = trace(pinv(FisMat)*FisMatsingle)/numel(i1.x);    % had to divide for num. freqs

    end
        
%     % store Fisher Matrix for this run
%     FM{k} = FisMat;
%     % adding up
%     FMall = FMall + FisMat;
    
%   elseif ((numel(n(1).objs) == 3) && (numel(in.objs) == 4) && ~isempty(outModel))
%     % this is only valid for the magnetic model, where we have 4 inputs
%     % (corresponding to the 4 conformator waveforms) and 3 outputs
%     % (corresponding to IFO.x12, IFO.eta1 and IFO.phi1). And there is a
%     % contribution of an outModel converting the conformator waveforms
%     % into forces and torques.
%     
%     
%     % For other cases not implemented yet.
%     
%     % use signal fft to get frequency vector. Take into account signal
%     % could be empty or set to zero
%     % 1st channel
%     freqs = fin.getObjectAtIndex(1,1).x;
%     
%     for ii = 1:numel(n.objs)
%       for jj = ii:numel(n.objs)
%         % Compute psd
%         if (ii==jj)
%           spec(ii,jj)  = psd(n(k).getObjectAtIndex(ii), pl);
%           S2(ii,jj) = interp(spec(ii,jj),plist('vertices',freqs,'method','linear'));
%         else
%           spec(ii,jj) = cpsd(n(k).getObjectAtIndex(ii),n(k).getObjectAtIndex(jj),pl);
%           S2(ii,jj) = interp(spec(ii,jj),plist('vertices',freqs,'method','linear'));
%           S2(jj,ii) = conj(S2(ii,jj));
%         end
%       end
%     end
%     
%     S = matrix(S2,plist('shape',[numel(n.objs) numel(n.objs)]));
%     
%     % get some parameters used below
%     fs = S.getObjectAtIndex(1,1).fs;
%     
%     
%     if(~isempty(outModel))
%       for lll=1:size(outModel,1)
%         for kkk=1:size(outModel,2)
%           outModel(lll,kkk) = split(outModel(lll,kkk),plist('frequencies',[f1 f2]));
%         end
%       end
%     end
%     
%     % Avoid numerical differentiation (faster for the magnetic case)
%     Param{1} = [  1 0 0 0;
%       0 0 0 0;
%       0 0 0 0;];
%     Param{2} = [  0 1 0 0;
%       0 0 0 0;
%       0 0 0 0;];
%     Param{3} = [  0 0 0 0;
%       0 0 1 0;
%       0 0 0 0;];
%     Param{4} = [  0 0 0 0;
%       0 0 0 0;
%       0 0 0 1;];
%     
%     % scaling of PSD
%     % PSD = 2/(N*fs) * FFT *conj(FFT)
%     for j = 1: numel(S.objs)
%       % spectra to variance
%       C(:,j) = (N*fs/2)*S.objs(j).data.getY;
%     end
%     
%     detm = (C(:,1).*C(:,5).*C(:,9) + ...
%       C(:,2).*C(:,6).*C(:,7) + ...
%       C(:,3).*C(:,4).*C(:,8) -...
%       C(:,7).*C(:,5).*C(:,3) -...
%       C(:,8).*C(:,6).*C(:,1) -...
%       C(:,9).*C(:,4).*C(:,2));
%     
%     InvS11 = (C(:,5).*C(:,9) - C(:,8).*C(:,6))./detm;
%     InvS12 = -(C(:,4).*C(:,9) - C(:,7).*C(:,6))./detm;
%     InvS13 = (C(:,4).*C(:,8) - C(:,7).*C(:,5))./detm;
%     InvS21 = -(C(:,2).*C(:,9) - C(:,8).*C(:,3))./detm;
%     InvS22 = (C(:,1).*C(:,9) - C(:,7).*C(:,3))./detm;
%     InvS23 = -(C(:,1).*C(:,8) - C(:,7).*C(:,2))./detm;
%     InvS31 = (C(:,2).*C(:,6) - C(:,5).*C(:,3))./detm;
%     InvS32 = -(C(:,1).*C(:,6) - C(:,4).*C(:,3))./detm;
%     InvS33 = (C(:,1).*C(:,5) - C(:,4).*C(:,2))./detm;
%     
%     for pp = 1:length(params)
%       for ll = 1:size(outModel,1)
%         for kk = 1:size(Param{pp},2)
%           % index convention: H(1,1)->h(1)  H(2,1)->h(2)  H(1,2)->h(3)  H(2,2)->h(4)
%           tmp = 0;
%           for innerIndex = 1:size(outModel,2)
%             tmp = tmp + outModel(ll,innerIndex).y * Param{pp}(innerIndex,kk);
%           end
%           h{pp}(:,(kk-1)*size(outModel,1) + ll) = tmp;
%         end
%       end
%       
%     end
%     
%     for kk = 1:numel(in.objs)
%       inV(:,kk) = fin.objs(kk).data.getY;
%     end
%     
%     
%     % compute Fisher Matrix
%     for i =1:length(params)
%       for j =1:length(params)
%         
%         for ll = 1:size(outModel,1)
%           tmp = 0;
%           for kk = 1:size(Param{1},2)
%             tmp = tmp + h{i}(:,(kk-1)*size(outModel,1) + ll).*inV(:,kk);
%           end
%           v{i}(:,ll) = tmp;
%         end
%         
%         
%         for ll = 1:size(outModel,1)
%           tmp = 0;
%           for kk = 1:size(Param{1},2)
%             tmp = tmp + h{j}(:,(kk-1)*size(outModel,1) + ll).*inV(:,kk);
%           end
%           v{j}(:,ll) = tmp;
%         end
%         
%         v1v1 = conj(v{i}(:,1)).*v{j}(:,1);
%         v1v2 = conj(v{i}(:,1)).*v{j}(:,2);
%         v1v3 = conj(v{i}(:,1)).*v{j}(:,3);
%         v2v1 = conj(v{i}(:,2)).*v{j}(:,1);
%         v2v2 = conj(v{i}(:,2)).*v{j}(:,2);
%         v2v3 = conj(v{i}(:,2)).*v{j}(:,3);
%         v3v1 = conj(v{i}(:,3)).*v{j}(:,1);
%         v3v2 = conj(v{i}(:,3)).*v{j}(:,2);
%         v3v3 = conj(v{i}(:,3)).*v{j}(:,3);
%         
%         FisMat(i,j) = sum(real(InvS11.*v1v1 +...
%           InvS12.*v1v2 +...
%           InvS13.*v1v3 +...
%           InvS21.*v2v1 +...
%           InvS22.*v2v2 +...
%           InvS23.*v2v3 +...
%           InvS31.*v3v1 +...
%           InvS32.*v3v2 +...
%           InvS33.*v3v3));
%       end
%     end
%     % store Fisher Matrix for this run
%     FM{k} = FisMat;
%     % adding up
%     FMall = FMall + FisMat;
%   else
%     error('Implemented cases: 2 inputs / 2outputs (TN3045 analysis), and 4 inputs / 3 outpus (magnetic complete analysis model. Other cases have not been implemented yet. Sorry for the inconvenience)');
%   end
  
  
% end

% inverse is the optimal covariance matrix
% if pseudoinv && isempty(tol)
%   cov = pinv(FMall);
% elseif pseudoinv
%   cov = pinv(FMall,tol);
% else
%   cov = FMall\eye(size(FMall));
% end


% create AO
out = ao(plist('Xvals',freqs,'Yvals',d,'type','fsdata','fs',fs,'name',''));
% spectrum in the procinfo
if channel == 1
out.setProcinfo(plist('S11',S11));
elseif channel == 2
out.setProcinfo(plist('S22',S22));
end
% Fisher Matrix in the procinfo
out.setProcinfo(plist('FisMat',FisMat));

varargout{1} = out;
end

end


%--------------------------------------------------------------------------
% Get Info Object
%--------------------------------------------------------------------------
function ii = getInfo(varargin)
if nargin == 1 && strcmpi(varargin{1}, 'None')
  sets = {};
  pls  = [];
else
  sets = {'Default'};
  pls  = getDefaultPlist;
end
% Build info object
ii = minfo(mfilename, 'ao', 'ltpda', utils.const.categories.sigproc, '$Id: dispersion.m,v 1.1 2011/06/22 09:54:19 miquel Exp $', sets, pls);
end

%--------------------------------------------------------------------------
% Get Default Plist
%--------------------------------------------------------------------------
function plout = getDefaultPlist()
persistent pl;
if exist('pl', 'var')==0 || isempty(pl)
  pl = buildplist();
end
plout = pl;
end

function pl = buildplist()
pl = plist.LPSD_PLIST;
pset(pl,'Navs',1)

p = plist({'f1', 'Initial frequency for the analysis'}, paramValue.EMPTY_DOUBLE);
pl.append(p);

p = plist({'f2', 'Final frequency for the analysis'}, paramValue.EMPTY_DOUBLE);
pl.append(p);

p = plist({'FitParamas', 'Parameters of the model'}, paramValue.EMPTY_STRING);
pl.append(p);

p = plist({'model','An array of matrix models'}, paramValue.EMPTY_STRING);
pl.append(p);

p = plist({'noise','An array of matrices with the cross-spectrum matrices'}, paramValue.EMPTY_STRING);
pl.append(p);

p = plist({'built-in','Symbolic models of the system as a string of built-in models'}, paramValue.EMPTY_STRING);
pl.append(p);

p = plist({'frequencies','Array of start/sop frequencies where the analysis is performed'}, paramValue.EMPTY_STRING);
pl.append(p);

p = plist({'pinv','Use the Penrose-Moore pseudoinverse'}, paramValue.TRUE_FALSE);
pl.append(p);

p = plist({'tol','Tolerance for the Penrose-Moore pseudoinverse'}, paramValue.EMPTY_DOUBLE);
pl.append(p);

p = plist({'diffStep','Numerical differentiation step for ssm models'}, paramValue.EMPTY_DOUBLE);
pl.append(p);

end