% CRB computes the inverse of the Fisher Matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% DESCRIPTION: CRB computes the inverse of the Fisher Matrix
%
% CALL:        bs = crb(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   - covariance matrix AO
%
% <a href="matlab:utils.helper.displayMethodInfo('matrix', 'crb')">Parameters Description</a>
%
% VERSION:    $Id: crb.m,v 1.17 2011/10/07 08:19:55 miquel Exp $
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function varargout = crb(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');
mdl = find(pl,'model');
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
in = copy(mtxs, nargout);
n = copy(mtxns, nargout);

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

% 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)) == 1) && (numel(in(1).objs) == 1))
        
        % use signal fft to get frequency vector.
        i1 = fin(k).getObjectAtIndex(1,1);
        freqs = i1.x;
        
        FisMat = utils.math.fisher_1x1(i1,n(k),mdl,params,numparams,freqs,N,pl,inNames,outNames);
        % store Fisher Matrix for this run
        FM{k} = FisMat;
        % adding up
        FMall = FMall + FisMat;
        
    elseif (((numel(n(1).objs)) == 2) && (numel(in(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
    
    FisMat = utils.math.fisher_2x2(i1,i2,n(k),mdl,params,numparams,freqs,N,pl,inNames,outNames);
    % 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(cov);
% Fisher Matrix in the procinfo
out.setProcinfo(plist('FisMat',FM));

varargout{1} = out;
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: crb.m,v 1.17 2011/10/07 08:19:55 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.WELCH_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({'step','Numerical differentiation step for ssm models'}, paramValue.EMPTY_DOUBLE);
pl.append(p);

p = plist({'ngrid','Number of points in the grid to compute the optimal differentiation step for ssm models'}, paramValue.EMPTY_DOUBLE);
pl.append(p);

p = plist({'stepRanges','An array with upper and lower values for the parameters ranges. To be used to compute the optimal differentiation step for ssm models.'}, paramValue.EMPTY_DOUBLE);
pl.append(p);
end