function [E, D] = pcamat(vectors, firstEig, lastEig, s_interactive, ...
    s_verbose);
%PCAMAT - Calculates the pca for data
%
% [E, D] = pcamat(vectors, firstEig, lastEig, ...
%                 interactive, verbose);
%
% Calculates the PCA matrices for given data (row) vectors. Returns
% the eigenvector (E) and diagonal eigenvalue (D) matrices containing the
% selected subspaces. Dimensionality reduction is controlled with
% the parameters 'firstEig' and 'lastEig' - but it can also be done
% interactively by setting parameter 'interactive' to 'on' or 'gui'.
%
% ARGUMENTS
%
% vectors       Data in row vectors.
% firstEig      Index of the largest eigenvalue to keep.
%               Default is 1.
% lastEig       Index of the smallest eigenvalue to keep.
%               Default is equal to dimension of vectors.
% interactive   Specify eigenvalues to keep interactively. Note that if
%               you set 'interactive' to 'on' or 'gui' then the values
%               for 'firstEig' and 'lastEig' will be ignored, but they
%               still have to be entered. If the value is 'gui' then the
%               same graphical user interface as in FASTICAG will be
%               used. Default is 'off'.
% verbose       Default is 'on'.
%
%
% EXAMPLE
%       [E, D] = pcamat(vectors);
%
% Note
%       The eigenvalues and eigenvectors returned by PCAMAT are not sorted.
%
% This function is needed by FASTICA and FASTICAG

% For historical reasons this version does not sort the eigenvalues or
% the eigen vectors in any ways. Therefore neither does the FASTICA or
% FASTICAG. Generally it seams that the components returned from
% whitening is almost in reversed order. (That means, they usually are,
% but sometime they are not - depends on the EIG-command of matlab.)

% @(#)$Id: pcamat.m,v 1.5 2003/12/15 18:24:32 jarmo Exp $

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Default values:
if nargin < 5, s_verbose = 'on'; end
if nargin < 4, s_interactive = 'off'; end
if nargin < 3, lastEig = size(vectors, 1); end
if nargin < 2, firstEig = 1; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Check the optional parameters;
switch lower(s_verbose)
 case 'on'
  b_verbose = 1;
 case 'off'
  b_verbose = 0;
 otherwise
  error(sprintf('Illegal value [ %s ] for parameter: ''verbose''\n', s_verbose));
end

switch lower(s_interactive)
 case 'on'
  b_interactive = 1;
 case 'off'
  b_interactive = 0;
 case 'gui'
  b_interactive = 2;
 otherwise
  error(sprintf('Illegal value [ %s ] for parameter: ''interactive''\n', ...
		s_interactive));
end

oldDimension = size (vectors, 1);
if ~(b_interactive)
  if lastEig < 1 | lastEig > oldDimension
    error(sprintf('Illegal value [ %d ] for parameter: ''lastEig''\n', lastEig));
  end
  if firstEig < 1 | firstEig > lastEig
    error(sprintf('Illegal value [ %d ] for parameter: ''firstEig''\n', firstEig));
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculate PCA

% Calculate the covariance matrix.
if b_verbose, fprintf ('Calculating covariance...\n'); end
covarianceMatrix = cov(vectors', 1);

% Calculate the eigenvalues and eigenvectors of covariance
% matrix.
[E, D] = eig (covarianceMatrix);

% The rank is determined from the eigenvalues - and not directly by
% using the function rank - because function rank uses svd, which
% in some cases gives a higher dimensionality than what can be used
% with eig later on (eig then gives negative eigenvalues).
rankTolerance = 1e-7;
maxLastEig = sum (diag (D) > rankTolerance);
if maxLastEig == 0,
  fprintf (['Eigenvalues of the covariance matrix are' ...
	    ' all smaller than tolerance [ %g ].\n' ...
	    'Please make sure that your data matrix contains' ...
	    ' nonzero values.\nIf the values are very small,' ...
	    ' try rescaling the data matrix.\n'], rankTolerance);
  error ('Unable to continue, aborting.');
end

% Sort the eigenvalues - decending.
eigenvalues = flipud(sort(diag(D)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Interactive part - command-line
if b_interactive == 1

  % Show the eigenvalues to the user
  hndl_win=figure;
  bar(eigenvalues);
  title('Eigenvalues');

  % ask the range from the user...
  % ... and keep on asking until the range is valid :-)
  areValuesOK=0;
  while areValuesOK == 0
    firstEig = input('The index of the largest eigenvalue to keep? (1) ');
    lastEig = input(['The index of the smallest eigenvalue to keep? (' ...
                    int2str(oldDimension) ') ']);
    % Check the new values...
    % if they are empty then use default values
    if isempty(firstEig), firstEig = 1;end
    if isempty(lastEig), lastEig = oldDimension;end
    % Check that the entered values are within the range
    areValuesOK = 1;
    if lastEig < 1 | lastEig > oldDimension
      fprintf('Illegal number for the last eigenvalue.\n');
      areValuesOK = 0;
    end
    if firstEig < 1 | firstEig > lastEig
      fprintf('Illegal number for the first eigenvalue.\n');
      areValuesOK = 0;
    end
  end
  % close the window
  close(hndl_win);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Interactive part - GUI
if b_interactive == 2

  % Show the eigenvalues to the user
  hndl_win = figure('Color',[0.8 0.8 0.8], ...
    'PaperType','a4letter', ...
    'Units', 'normalized', ...
    'Name', 'FastICA: Reduce dimension', ...
    'NumberTitle','off', ...
    'Tag', 'f_eig');
  h_frame = uicontrol('Parent', hndl_win, ...
    'BackgroundColor',[0.701961 0.701961 0.701961], ...
    'Units', 'normalized', ...
    'Position',[0.13 0.05 0.775 0.17], ...
    'Style','frame', ...
    'Tag','f_frame');

b = uicontrol('Parent',hndl_win, ...
	'Units','normalized', ...
	'BackgroundColor',[0.701961 0.701961 0.701961], ...
	'HorizontalAlignment','left', ...
	'Position',[0.142415 0.0949436 0.712077 0.108507], ...
	'String','Give the indices of the largest and smallest eigenvalues of the covariance matrix to be included in the reduced data.', ...
	'Style','text', ...
	'Tag','StaticText1');
e_first = uicontrol('Parent',hndl_win, ...
	'Units','normalized', ...
	'Callback',[ ...
          'f=round(str2num(get(gcbo, ''String'')));' ...
          'if (f < 1), f=1; end;' ...
          'l=str2num(get(findobj(''Tag'',''e_last''), ''String''));' ...
          'if (f > l), f=l; end;' ...
          'set(gcbo, ''String'', int2str(f));' ...
          ], ...
	'BackgroundColor',[1 1 1], ...
	'HorizontalAlignment','right', ...
	'Position',[0.284831 0.0678168 0.12207 0.0542535], ...
	'Style','edit', ...
        'String', '1', ...
	'Tag','e_first');
b = uicontrol('Parent',hndl_win, ...
	'Units','normalized', ...
	'BackgroundColor',[0.701961 0.701961 0.701961], ...
	'HorizontalAlignment','left', ...
	'Position',[0.142415 0.0678168 0.12207 0.0542535], ...
	'String','Range from', ...
	'Style','text', ...
	'Tag','StaticText2');
e_last = uicontrol('Parent',hndl_win, ...
	'Units','normalized', ...
	'Callback',[ ...
          'l=round(str2num(get(gcbo, ''String'')));' ...
          'lmax = get(gcbo, ''UserData'');' ...
          'if (l > lmax), l=lmax; fprintf([''The selected value was too large, or the selected eigenvalues were close to zero\n'']); end;' ...
          'f=str2num(get(findobj(''Tag'',''e_first''), ''String''));' ...
          'if (l < f), l=f; end;' ...
          'set(gcbo, ''String'', int2str(l));' ...
          ], ...
	'BackgroundColor',[1 1 1], ...
	'HorizontalAlignment','right', ...
	'Position',[0.467936 0.0678168 0.12207 0.0542535], ...
	'Style','edit', ...
        'String', int2str(maxLastEig), ...
        'UserData', maxLastEig, ...
	'Tag','e_last');
% in the first version oldDimension was used instead of
% maxLastEig, but since the program would automatically
% drop the eigenvalues afte maxLastEig...
b = uicontrol('Parent',hndl_win, ...
	'Units','normalized', ...
	'BackgroundColor',[0.701961 0.701961 0.701961], ...
	'HorizontalAlignment','left', ...
	'Position',[0.427246 0.0678168 0.0406901 0.0542535], ...
	'String','to', ...
	'Style','text', ...
	'Tag','StaticText3');
b = uicontrol('Parent',hndl_win, ...
	'Units','normalized', ...
	'Callback','uiresume(gcbf)', ...
	'Position',[0.630697 0.0678168 0.12207 0.0542535], ...
	'String','OK', ...
	'Tag','Pushbutton1');
b = uicontrol('Parent',hndl_win, ...
	'Units','normalized', ...
	'Callback',[ ...
          'gui_help(''pcamat'');' ...
          ], ...
	'Position',[0.767008 0.0678168 0.12207 0.0542535], ...
	'String','Help', ...
	'Tag','Pushbutton2');

  h_axes = axes('Position' ,[0.13 0.3 0.775 0.6]);
  set(hndl_win, 'currentaxes',h_axes);
  bar(eigenvalues);
  title('Eigenvalues');

  uiwait(hndl_win);
  firstEig = str2num(get(e_first, 'String'));
  lastEig = str2num(get(e_last, 'String'));

  % close the window
  close(hndl_win);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% See if the user has reduced the dimension enought

if lastEig > maxLastEig
  lastEig = maxLastEig;
  if b_verbose
    fprintf('Dimension reduced to %d due to the singularity of covariance matrix\n',...
           lastEig-firstEig+1);
  end
else
  % Reduce the dimensionality of the problem.
  if b_verbose
    if oldDimension == (lastEig - firstEig + 1)
      fprintf ('Dimension not reduced.\n');
    else
      fprintf ('Reducing dimension...\n');
    end
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Drop the smaller eigenvalues
if lastEig < oldDimension
  lowerLimitValue = (eigenvalues(lastEig) + eigenvalues(lastEig + 1)) / 2;
else
  lowerLimitValue = eigenvalues(oldDimension) - 1;
end

lowerColumns = diag(D) > lowerLimitValue;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Drop the larger eigenvalues
if firstEig > 1
  higherLimitValue = (eigenvalues(firstEig - 1) + eigenvalues(firstEig)) / 2;
else
  higherLimitValue = eigenvalues(1) + 1;
end
higherColumns = diag(D) < higherLimitValue;

% Combine the results from above
selectedColumns = lowerColumns & higherColumns;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% print some info for the user
if b_verbose
  fprintf ('Selected [ %d ] dimensions.\n', sum (selectedColumns));
end
if sum (selectedColumns) ~= (lastEig - firstEig + 1),
  error ('Selected a wrong number of dimensions.');
end

if b_verbose
  fprintf ('Smallest remaining (non-zero) eigenvalue [ %g ]\n', eigenvalues(lastEig));
  fprintf ('Largest remaining (non-zero) eigenvalue [ %g ]\n', eigenvalues(firstEig));
  fprintf ('Sum of removed eigenvalues [ %g ]\n', sum(diag(D) .* ...
    (~selectedColumns)));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Select the colums which correspond to the desired range
% of eigenvalues.
E = selcol(E, selectedColumns);
D = selcol(selcol(D, selectedColumns)', selectedColumns);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Some more information
if b_verbose
  sumAll=sum(eigenvalues);
  sumUsed=sum(diag(D));
  retained = (sumUsed / sumAll) * 100;
  fprintf('[ %g ] %% of (non-zero) eigenvalues retained.\n', retained);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function newMatrix = selcol(oldMatrix, maskVector);

% newMatrix = selcol(oldMatrix, maskVector);
%
% Selects the columns of the matrix that marked by one in the given vector.
% The maskVector is a column vector.

% 15.3.1998

if size(maskVector, 1) ~= size(oldMatrix, 2),
  error ('The mask vector and matrix are of uncompatible size.');
end

numTaken = 0;

for i = 1 : size (maskVector, 1),
  if maskVector(i, 1) == 1,
    takingMask(1, numTaken + 1) = i;
    numTaken = numTaken + 1;
  end
end

newMatrix = oldMatrix(:, takingMask);


function [as,bs,vars] = sumARMA(varargin)

%SUMARMA Sum of ARMA-processes
%   [ar_s,ma_s,var_s] = sumARMA(ar1,ma1,var1,ar2,ma2,var2) yields
%   the sum of 2 ARMA-processes. The list of arguments can be
%   extended with more ARMA-processes.

%S. de Waele, 2001.

%Technical note:
%If this method does not work, the following procedure can be followed:
%- Calculate the covariance functions and add them;
%- Transform this covariance function into an AR process.

if rem(nargin,3),
    error('Incorrect number of inputs.')
end
np = nargin/3;

%Initialisation
a  = cell(np,1);
b  = cell(np,1);
var= cell(np,1);
gain = cell(np,1);
nomc = cell(np,1);
for i = 1:np,
    a{i}    = varargin{1+3*(i-1)};
    b{i}    = varargin{2+3*(i-1)};
    var{i}  = varargin{3+3*(i-1)};
    gain{i} = pgain(b{i},a{i});
    nomc{i} = 1;
end
den = 1;
vars = 0;
nom = 0;

%Calculation of the contributions of the processes to the nominator
for i = 1:np,
    for j = 1:np,
        if j~=i,
            nomc{j} = conv(nomc{j},a{i});
        else
            nomc{j} = conv(nomc{j},b{j});
        end %if j~i,
    end %for j = 1:np,
    vars = vars+var{i};
    den = conv(den,a{i});
end %for i = 1:np,
m = length(nomc{1});
for i = 2:np, m = max( [m length(nomc{i})] ); end
nom = zeros(1,m);

for i = 1:np,
    l = length(nomc{i})-1;
    nomc{i} = conv(nomc{i},fliplr(nomc{i}));
    nomc{i} = nomc{i}(l+1:2*l+1);
    nom(1:l+1) = nom(1:l+1)+var{i}*nomc{i}/gain{i};
end

as = den;
bs = cor2ma(nom,200*length(nom));


function varargout = homkermap(varargin)
% VL_HOMKERMAP Homogeneous kernel map
%   V = VL_HOMKERMAP(X, N) computes a 2*N+1 dimensional approximated
%   kernel map for the Chi2 kernel. X is an array of data points. Each
%   point is expanded into a vector of dimension 2*N+1 and saved to
%   the output V. The expanded feature vectors are stacked along the
%   first dimension, so that the output array V has the same
%   dimensions of the input array X except for the first one, which is
%   2*N+1 times larger.
%
%   The function accepts the following options:
%
%   Kernel:: KCHI2
%     One of KCHI2 (Chi2 kernel), KINTERS (intersection kernel), KJS
%     (Jensen-Shannon kernel). The 'Kernel' option name can be omitted,
%     i.e. VL_HOMKERMAP(..., 'kernel', 'kchi2') has the same effect of
%     VL_HOMKERMAP(..., 'kchi2').
%
%   Period:: [automatically tuned]
%     Set the period of the kernel specturm. The approximation is
%     based on periodicizing the kernel specturm. If not specified,
%     the period is automatically set based on the heuristic described
%     in [2].
%
%   Window:: [RECTANGULAR]
%     Set the window used to truncate the spectrum before The window
%     can be either RECTANGULAR or UNIFORM window. See [2] and the API
%     documentation for details.
%
%   Gamma:: [1]
%     Set the homogeneity degree of the kernel. The standard kernels
%     are 1-homogeneous, but sometimes smaller values perform better
%     in applications. See [2] for details.
%
%   Example::
%     The following code results in approximatively the same
%     similarities matrices between points X and Y:
%
%       x = rand(10,1) ;
%       y = rand(10,100) ;
%       psix = vl_homkermap(x, 3) ;
%       psiy = vl_homkermap(y, 3) ;
%       figure(1) ; clf ;
%       ker = vl_alldist(x, y, 'kchi2') ;
%       ker_ = psix' * psiy ;
%       plot([ker ; ker_]') ;
%
%   Note::
%     The homogeneous kernels K(X,Y) are normally defined for
%     non-negative data only. VL_HOMKERMAP defines them for both
%     positive and negative data by using the definition
%     SIGN(X)SIGN(Y)K(ABS(X),ABS(Y)) -- note that other extensions are
%     possible as well (see [2]).
%
%   REFERENCES::
%     [1] A. Vedaldi and A. Zisserman
%     `Efficient Additive Kernels via Explicit Feature Maps',
%     Proc. CVPR, 2010.
%
%     [2] A. Vedaldi and A. Zisserman
%     `Efficient Additive Kernels via Explicit Feature Maps',
%     PAMI, 2011 (submitted).
%
%   See also: VL_HELP().
[varargout{1:nargout}] = vl_homkermap(varargin{:});


%Finite Element Method 101-2
%National Taiwan University
%2D Elasticity problem

%%clear memory
close all; clc; clear all;

%% Load Mesh for Lab 10
[nodeCoordinates,elementNodes]=Mesh_Lab10('Q12');
% node coordinates are given in mm
NodePerElement=12;
numberNodes=size(nodeCoordinates,1);
numberElements=size(elementNodes,1);

%% Import BCs

% Essential BC's
GDof = 2*numberNodes;
prescribedDof = [1,2,3,4,5,6,7,8];

% Natural BC's
force = zeros(GDof,1);
force(end) = -10; % 10 [kN]

%% Import material and section properties
E = 3E7; % [GPa]
poisson = 0.3; %[-]
thickness = 1; % [mm]

%% Evalute force vector
%force=formForceVectorQ4(GDof,naturalBCs,surfaceOrientation,...
%    elementNodes,nodeCoordinates,thickness);

%% Construct Stiffness matrix for Q12 element
D=E/(1-poisson^2)*[1 poisson 0;poisson 1 0;0 0 (1-poisson)/2];

stiffness=formStiffness2D(GDof,numberElements,...
    elementNodes,numberNodes,nodeCoordinates,D,thickness);

%% solution
displacements=solution(GDof,prescribedDof,stiffness,force);

%% output displacements
outputDisplacements(displacements, numberNodes, GDof);
scaleFactor=1.E5;
drawingMesh(nodeCoordinates+scaleFactor*[displacements(1:2:2*numberNodes) ...
    displacements(2:2:2*numberNodes)],elementNodes,'Q4','r--');

%% B matrix & strain
% 4 by 4 quadrature
[gaussWeights,gaussLocations]=gauss2d('4x4');

for e=1:numberElements
  numNodePerElement = length(elementNodes(e,:));
  numEDOF = 2*numNodePerElement;
  elementDof=zeros(1,numEDOF);
  for i = 1:numNodePerElement
      elementDof(2*i-1)=2*elementNodes(e,i)-1;
      elementDof(2*i)=2*elementNodes(e,i);
  end

  % cycle for Gauss point
  for q=1:size(gaussWeights,1)
      GaussPoint=gaussLocations(q,:);
      xi=GaussPoint(1);
      eta=GaussPoint(2);

% shape functions and derivatives
    [shapeFunction,naturalDerivatives]=shapeFunctionQ12(xi,eta);

% Jacobian matrix, inverse of Jacobian,
% derivatives w.r.t. x,y
    [Jacob,invJacobian,XYderivatives]=...
        Jacobian(nodeCoordinates(elementNodes(e,:),:),naturalDerivatives);

%  B matrix
    B=zeros(3,numEDOF);
    B(1,1:2:numEDOF)       = XYderivatives(:,1)';
    B(2,2:2:numEDOF)  = XYderivatives(:,2)';
    B(3,1:2:numEDOF)       = XYderivatives(:,2)';
    B(3,2:2:numEDOF)  = XYderivatives(:,1)';

  elementNodes(e,:);
  dis(1:2:24)=displacements([2*(elementNodes(e,:))-1],1);
  dis(2:2:24)=displacements([2*(elementNodes(e,:))],1);
  strain=B*dis';
  stress(:,q)=D*strain;

  end
  for q=1:size(gaussWeights,1)
      GaussPoint=gaussLocations(q,:);
      xi=1/GaussPoint(1);
      eta=1/GaussPoint(2);
      [shapeFunction,naturalDerivatives]=shapeFunctionQ12(xi,eta);
      stressxx(e,q)=stress(1,1:12)*shapeFunction;
      stressyy(e,q)=stress(2,1:12)*shapeFunction;
      stressxy(e,q)=stress(3,1:12)*shapeFunction;
      vonmises(e,q)=sqrt(0.5*((stressxx(e,q)-stressyy(e,q))^2+(stressyy(e,q))^2+(stressxx(e,q))^2+6*(stressxy(e,q))^2));
      fprintf('\nStress in element %u\n',e)
      fprintf('\nStress in node %u\n',q)
      fprintf('Sigma_xx : %0.6f\n',stressxx(e,q))
      fprintf('Sigma_yy : %0.6f\n',stressyy(e,q))
      fprintf('Sigma_xy : %0.6f\n',stressxy(e,q))
      fprintf('Vonmises : %0.6f\n',vonmises(e,q))
  end

end


function phase=resolveLogScaleEccToPhase(Ecc,phaseZero,EccZero,noRings)

%   phase=resolveLogScaleEccToPhase(Ecc,phaseZero,EccZero,noRings)
%   PURPOSE: find the phase an eccentricity reflects when the ringmapping was logscaled.
%   (inverse of resolveLogScalePhaseToEcc)
%
%--------------------------------------------------------------------------
%   USAGE:  phase       = the phase that needs to be translated,
%           phaseZero   = the lowest phase (or reference phase)
%           EccZero     = the Ecc the phaseZero reflects
%           noRings     = number of stimuli in the ringmapping, usually 8,
%                         importent because noRing makes 2pi
%--------------------------------------------------------------------------
%   HISTORY
%
%   2005.03.14 by Mark Schira mark@ski.org

tau=noRings/pi/2;
phase=(log2(Ecc/EccZero)/tau)+phaseZero;


function b = r8gd_vxm ( n, ndiag, offset, a, x )

%*****************************************************************************80
%
%% R8GD_VXM multiplies a vector by a R8GD matrix.
%
%  Discussion:
%
%    The R8GD storage format is suitable for matrices whose only nonzero entries
%    occur along a few diagonals, but for which these diagonals are not all
%    close enough to the main diagonal for band storage to be efficient.
%
%    In that case, we assign the main diagonal the offset value 0.
%    Each successive superdiagonal gets an offset value 1 higher, until
%    the highest superdiagonal (the A(1,N) entry) is assigned the offset N-1.
%    Similarly, the subdiagonals are assigned offsets of -1 through -(N-1).
%
%    Now, assuming that only a few of these diagonals contain nonzeros,
%    then for the I-th diagonal to be saved, we stored its offset in
%    OFFSET(I), and its entries in column I of the matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer NDIAG, the number of diagonals of the matrix
%    that are stored in the array.
%    NDIAG must be at least 1, and no more than 2 * N - 1.
%
%    Input, integer OFFSET(NDIAG), the offsets for the diagonal storage.
%
%    Input, real A(N,NDIAG), the R8GD matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(N), the product X*A.
%
  b(1:n) = 0.0;

  for i = 1 : n
    for diag = 1 : ndiag
      j = i + offset(diag);
      if ( 1 <= j & j <= n )
        b(j) = b(j) + x(i) * a(i,diag);
      end
    end
  end

  return
end


function varargout = wtps(varargin)
% VL_WTPS  Thin-plate spline warping
%   [XP1,XP2]=VL_WTPS(PHI,YP) computes the thin-plate spline (TPS)
%   specified by the basis PHI and the warped control point Yp.
%
%   Yp is a 2xK matrix with one column per control point and the basis
%   PHI is calculated by means of the VL_TPS function.
%
%   The thin-palte spline is defined on a domain X1,X2 and specified
%   by a set of points Y and their warp YP. The spline passes
%   interpolates exaclty the control points.
%
%   The parameters X1,X2 and Y are used to compute the basis PHI. This
%   operation is fairily slow, but computing the spline for a given Yp
%   is then very quick, as the operation is just a linear combination
%   of the basis.
%
%   Example::
%     To calculate the warped grid [X1,X2] by moving the control points Y to
%     the control points YP use:
%       [xp1,xp2]=VL_WTPS(VL_TPS(x1,x2,Y),Yp).
%
%   See also: VL_TPS(), VL_HELP().
[varargout{1:nargout}] = vl_wtps(varargin{:});


%% S2AxisFieldHarmonic
%
% S2AxisFieldharmonic handles axis fields on the sphere.
% Axis can be understood as three-dimensional vectors without direction or length.
%%
% S2AxisFieldHarmonic handles functions of the form
%
% $$ f\colon \bf S^2\to\bf R^3_{/<\pm \mathrm{Id}>}. $$
%
%% Defining a S2AxisFieldHarmonic
%
%%
% *Definition via function values*
%
% At first you need some vertices
nodes = equispacedS2Grid('points', 1e5);
nodes = nodes(:);
%%
% Next you define function values for the vertices
y = vector3d(sin(5*nodes.x), 1, nodes.y, 'antipodal');
%%
% Now the actual command to get |sAF1| of type <S2AxisFieldHarmonic.S2AxisFieldHarmonic |S2AxisFieldHarmonic|>
sAF1 = S2AxisFieldHarmonic.approximation(nodes, y)

%%
% *Definition via function handle*
%
% If you have a function handle for the function you could create a
% |S2AxisFieldHarmonic| via quadrature. At first lets define a function
% handle which takes <vector3d.vector3d.html |vector3d|> as an argument and
% returns antipodal <vector3d.vector3d.html |vector3d|>:

f = @(v) vector3d(v.x, v.y, 0*v.x, 'antipodal');
%%
% Now you can call the quadrature command to get |sAF2| of type <S2AxisFieldHarmonic.S2AxisFieldHarmonic |S2AxisFieldHarmonic|>
sAF2 = S2AxisFieldHarmonic.quadrature(@(v) f(v))

%% Visualization
%
% One can use the default |plot|-command

plot(sAF1);
%%
% * same as quiver(sAF1)

%%
% or use the 3D plot of a sphere with the axis on itself
clf;
quiver3(sAF2);


function plotData(X, y)
%PLOTDATA Plots the data points X and y into a new figure
%   PLOTDATA(x,y) plots the data points with + for the positive examples
%   and o for the negative examples. X is assumed to be a Mx2 matrix.

% Create New Figure
figure; hold on;

% ====================== YOUR CODE HERE ======================
% Instructions: Plot the positive and negative examples on a
%               2D plot, using the option 'k+' for the positive
%               examples and 'ko' for the negative examples.
%

% Find indices of positive and negative examples
pos = find(y==1); neg = find(y==0);

% Plot examples
plot(X(pos,1), X(pos,2), 'r+', 'LineWidth', 2, 'MarkerSize', 7);
plot(X(neg,1), X(neg,2), 'ko', 'MarkerFaceColor', 'yellow', 'MarkerSize', 7);

% =========================================================================

hold off;

end


function [ftps] = mph2ftps(mph)
% Convert speed from miles per hour to feet per second
% Chad A. Greene 2012
ftps = mph*1.466666666667;


function transfo = createBasisTransform3d(source, target)
%CREATEBASISTRANSFORM3D Compute matrix for transforming a basis into another basis
%
%   TRANSFO = createBasisTransform3d(SOURCE, TARGET) will create a 4-by-4
%   transformation matrix representing the transformation from SOURCE basis
%   to TARGET basis.
%    SOURCE and TARGET are either standard 1-by-9 geom3d PLANE
%    representations of the form: [x0 y0 z0  ex1 ey1 ez1  ex2 ey2 ez2]
%     OR
%    SOURCE and TARGET may be any string such as 'global' or 'g' in which
%    case they represent the global plane [0 0 0 1 0 0 0 1 0].
%
%   The resulting TRANSFO matrix is such that a point expressed with
%   coordinates of the first basis will be represented by new coordinates
%   P2 = transformPoint3d(P1, TRANSFO) in the target basis.
%
%   Either (or both) SOURCE or TARGET may be an N-by-9 set of N planes. In
%   that case, TRANSFO will be a 4-by-4-by-N array of N transformation
%   matrices.
%
%   Example:
%     % Calculate local plane coords. of a point given in global coords
%     Plane = [10 10 10  1 0 0  0 1 0];
%     Tform = createBasisTransform3d('global', Plane);
%     PT_IN_PLANE = transformPoint3d([3 8 2], Tform)
%     PT_IN_PLANE =
%         13  18  12
%
%   See also
%     transforms3d, transformPoint3d, planePosition, createBasisTransform

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2010-12-03,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% HISTORY
% 2013-07-03 added support for multiple inputs (Sven Holcombe)
% 2017-10-16 rewrite

% size of input arguments
srcSz = size(source, 1);
tgtSz = size(target, 1);
maxSz = max(srcSz, tgtSz);

% check case of multiple inputs
if maxSz > 1
    [t1, t2] = deal( bsxfun(@times, eye(4), ones(1,1,maxSz)) );
    if srcSz > 1
        source = permute(source, [3 2 1]);
    end
    if tgtSz > 1
        target = permute(target, [3 2 1]);
    end
else
    [t1, t2] = deal(eye(4));
end

% Place source and target planes into t1 and t2 t-form matrices. If either
% input is non-numeric it is assumed to mean 'global', or identity t-form.
if isnumeric(source)
    if maxSz > 1 && srcSz == 1
        source = bsxfun(@times, source, ones(1,1,maxSz));
    end
    t1(1:3, 1, :) = source(1, 4:6, :);
    t1(1:3, 2, :) = source(1, 7:9, :);
    t1(1:3, 3, :) = crossProduct3d(source(1,4:6,:), source(1,7:9,:));
    t1(1:3, 4, :) = source(1, 1:3, :);
end
if isnumeric(target)
    if maxSz > 1 && tgtSz == 1
        target = bsxfun(@times, target, ones(1,1,maxSz));
    end
    t2(1:3, 1, :) = target(1, 4:6, :);
    t2(1:3, 2, :) = target(1, 7:9, :);
    t2(1:3, 3, :) = crossProduct3d(target(1,4:6,:), target(1,7:9,:));
    t2(1:3, 4, :) = target(1, 1:3, :);
end

% compute transform matrix
transfo = zeros(4, 4, maxSz);
for i = 1:maxSz
    % coordinate of four reference points in source basis
    po = t1(1:3, 4, i)';
    px = po + t1(1:3, 1, i)';
    py = po + t1(1:3, 2, i)';
    pz = po + t1(1:3, 3, i)';

    % express coordinates of reference points in the new basis
    t2i = inv(t2(:,:,i));
    pot = transformPoint3d(po, t2i);
    pxt = transformPoint3d(px, t2i);
    pyt = transformPoint3d(py, t2i);
    pzt = transformPoint3d(pz, t2i);

    % compute direction vectors in new basis
    vx = pxt - pot;
    vy = pyt - pot;
    vz = pzt - pot;

    % concatenate result in a 4-by-4 affine transform matrix
    transfo(:,:,i) = [vx' vy' vz' pot' ; 0 0 0 1];
end


function [scoreMat_viewpoint] = generateViewPointScoreMatrix(viewPointQ, viewPointT)

    vQ = [cos(viewPointQ) sin(viewPointQ)];
    vT = [cos(viewPointT) sin(viewPointT)];

    scoreMat_viewpoint = vQ * (vT');

end


%Demonstrates compressively sampling and regularization by denoising (RED) recovery of an image.
%Requires  matconvnet and fasta in the path
%Google's RED implementation can be found here: https://github.com/google/RED
% RED reference: Romano, Yaniv, Michael Elad, and Peyman Milanfar. "The little engine that could: Regularization by denoising (RED)." SIAM Journal on Imaging Sciences 10.4 (2017): 1804-1844.

addpath(genpath('..'))
addpath('~/matconvnet/matlab');
addpath(genpath('~/fasta-matlab'));
% addpath(genpath('~/gampmatlab'));

denoiser1='DnCNN';%Available options are NLM, Gauss, Bilateral, BLS-GSM, BM3D, fast-BM3D, BM3D-SAPCA, and DnCNN
denoiser2='DnCNN';
filename='boat.png';
SamplingRate=.1;
RED_iters=200;
AMP_iters=10;
% VAMP_iters=10;
imsize=128;
n_DnCNN_layers=20;%Other option is 17
LoadNetworkWeights(n_DnCNN_layers);

ImIn=double(imread(filename));
x_0=imresize(ImIn,imsize/size(ImIn,1));
[height, width]=size(x_0);
n=length(x_0(:));
m=round(n*SamplingRate);
errfxn = @(x_hat) PSNR(x_0,reshape(x_hat,[height width]));

% %Generate Gaussian Measurement Matrix
% M_matrix=1/sqrt(m)*randn(m,n);
% M=@(x) M_matrix*x(:);
% Mt=@(x) M_matrix'*x(:);
% U=[];
% Ut=[];
% d=[];

% %Generate Coded Diffraction Pattern Measurement Matrix
% signvec = exp(1i*2*pi*rand(n,1));
% inds=[1;randsample(n-1,m-1)+1];
% I=speye(n);
% SubsampM=I(inds,:);
% M=@(x) SubsampM*reshape(fft2(reshape(bsxfun(@times,signvec,x(:)),[height,width])),[n,1])*(1/sqrt(n))*sqrt(n/m);
% Mt=@(x) bsxfun(@times,conj(signvec),reshape(ifft2(reshape(SubsampM'*x(:),[height,width])),[n,1]))*sqrt(n)*sqrt(n/m);
% U=@(x) x(:);
% Ut= @(x) x(:);
% d=ones(m,1)*n/m;

% %Generate Real-valued Measurement Matrix with Fast Transformation and
% %approximately i.i.d. Gaussian distribution
% signvec = 2*round(rand(n,1))-1;
% inds=[1;randsample(n-1,m-1)+1];
% I=speye(n);
% SubsampM=I(inds,:);
% M=@(x) SubsampM*reshape(dct2(reshape(bsxfun(@times,signvec,x(:)),[height,width])),[n,1])*sqrt(n/m);
% Mt=@(x) bsxfun(@times,conj(signvec),reshape(idct2(reshape(SubsampM'*x(:),[height,width])),[n,1]))*sqrt(n/m);
% U=@(x) x(:);
% Ut= @(x) x(:);
% d=ones(m,1)*n/m;

%Generate (something close to) a Fast JL Transform matrix
signvec = 2*round(rand(n,1))-1;
inds=[1;randsample(n-1,m-1)+1];
I=speye(n);
SubsampM=I(inds,:);
M=@(x) SubsampM*reshape(dct(bsxfun(@times,signvec,x(:))),[n,1])*sqrt(n/m);
Mt=@(x) bsxfun(@times,conj(signvec),reshape(idct(SubsampM'*x(:)),[n,1]))*sqrt(n/m);
U=@(x) x(:);
Ut= @(x) x(:);
d=ones(m,1)*n/m;

% %Generate (something close to) a Fast JL Transform matrix
% signvec = 2*round(rand(n,1))-1;
% inds=[1;randsample(n-1,m-1)+1];
% I=speye(n);
% SubsampM=I(inds,:);
% M=@(x) SubsampM*reshape(fwht(bsxfun(@times,signvec,x(:))),[n,1])*n/sqrt(m);
% Mt=@(x) bsxfun(@times,conj(signvec),reshape(ifwht(SubsampM'*x(:)),[n,1]))*sqrt(1/m);
% U=@(x) x(:);
% Ut= @(x) x(:);
% d=ones(m,1)*n/m;

%Compressively sample the image
y=M(x_0(:));

%Recover signal using RED
%Set RED options
prox_opts=[];
prox_opts.width=width;
prox_opts.height=height;
prox_opts.denoiser=denoiser1;
prox_opts.prox_iters=1;
fasta_opts=[];
fasta_opts.maxIters=RED_iters/2;
fasta_opts.tol=1e-7;
fasta_opts.recordObjective=true;
% fasta_ops.function=errfxn;

x_init=100*rand(size(x_0(:)));

%Recover Signal using RED
prox_opts.lambda=1;
t0=tic;
x_hat1=x_init;
prox_opts.sigma_hat=50;
[x_hat1,out1]  = redCS( M,Mt,y,x_hat1(:),fasta_opts,prox_opts);
prox_opts.sigma_hat=15;
[x_hat1,out2]  = redCS( M,Mt,y,x_hat1(:),fasta_opts,prox_opts);
x_hat1=reshape(x_hat1,[height, width]);
t1=toc(t0);

% figure(4);
% subplot(1,2,1);plot(out1.objective);
% subplot(1,2,2);plot(out2.objective);

%Recover Signal using D-(V)AMP algorithms
t0=tic;[x_hat2,psnr2]  = DAMP(y,AMP_iters,height,width,denoiser1,M,Mt,errfxn);t2=toc(t0);
% t0=tic;[x_hat2,psnr2] = DVAMP(y,VAMP_iters,height,width,denoiser2,M,Mt,errfxn, U, Ut, d);t2=toc(t0);

%D(V)AMP Recovery Performance
performance1=PSNR(x_0,x_hat1);
performance2=PSNR(x_0,x_hat2);
display([num2str(SamplingRate*100),'% Sampling ', denoiser1, '-RED: PSNR=',num2str(performance1),', time=',num2str(t1)])
display([num2str(SamplingRate*100),'% Sampling ', denoiser2, '-AMP: PSNR=',num2str(performance2),', time=',num2str(t2)])

%Plot Recovered Signals
figure(1); clf;
subplot(1,3,1);
imshow(uint8(x_0));title('Original Image');
subplot(1,3,2);
imshow(uint8(x_hat1));title([denoiser1, '-RED']);
subplot(1,3,3);
imshow(uint8(x_hat2));title([denoiser2, '-AMP']);


% Demo for 2D neural field model.
% This demo inverts a linear 2D neural field model.
% NB: in this instance, the neural field is deterministic.

clear variables
close all

%---- Choose basic settings for simulations ----%

% dimensions
n_t             = 2e2;                      % number of time points
ns              = 5;                        % size of the square grid
deltat          = 5e-2;                     % time discretization
f_fname         = @f_2DneuralField;         % Evolution function
g_fname         = @g_Id;                    % Observation function

% Sinusoidal input to the approximate center of the neural field
u               = zeros(ns^2,n_t);
ohmega          = 0.91;
centre          = 0.5*ns*(ns-1);
u(centre-1:centre+1,2)     = 10;
u(centre-1:centre+1,:)     = repmat(1*sin(ohmega*deltat*[1:n_t]),3,1);

% parameters of the simulation
alpha           = Inf;%1e6;
sigma           = 1e3;%Inf;
theta           = [0.15;1;1];
phi             = [1;1;0.1];

% Build options structure for temporal integration of SDE
I = speye(ns,ns);
E = sparse(2:ns,1:ns-1,1,ns,ns);
D = E+E'-2*I;
inF.L           = kron(D,I)+kron(I,D);
inF.deltat      = deltat;
inG.ind         = 1:ns^2*2; % this makes states observable (not temporal derivatives)
options.inF     = inF;
options.inG     = inG;
% options.u0      = 0*ones(ns^2,1);

% Build priors for model inversion
priors.muX0 = 0*ones(ns^2*2,1);
priors.SigmaX0 = 0*speye(ns^2*2);%1e-1*speye(ns^2*2);
priors.muTheta = [.5;.5;.5];
priors.SigmaTheta = 1e0*speye(size(theta,1));
priors.muPhi = [0.1;0.9;0];
priors.SigmaPhi = 1e0*speye(size(phi,1));
priors.SigmaPhi(2,2) = 1e-4;
priors.a_alpha = Inf;%1e3;
priors.b_alpha = 0;%1e1;
priors.a_sigma = 1e3;
priors.b_sigma = 1e1;

% Build options and dim structures for model inversion
options.priors      = priors;
dim.n_theta         = size(theta,1);
dim.n_phi           = size(phi,1);
dim.n               = (ns^2)*2;

% Build time series of hidden states and observations
[y,x,x0,eta,e] = VBA_simulate (n_t,f_fname,g_fname,theta,phi,u,alpha,sigma,options,priors.muX0);

% display time series of hidden states and observations
displaySimulations(y,x,eta,e);
% disp('--paused--')
% pause

% Call inversion routine
% [posterior,out] = VBA_onlineWrapper(y,u,f_fname,g_fname,dim,options);
[posterior,out] = VBA_NLStateSpaceModel(y,u,f_fname,g_fname,dim,options);

% Display results
displayResults(posterior,out,y,x,x0,theta,phi,alpha,sigma);

% Make predictions
try
    options = out.options;
    [xs,ys,xhat,vx,yhat,vy] = VBA_comparePredictions(...
        n_t,theta,phi,zeros(size(u)),alpha,sigma,options,posterior,dim);
catch
    disp('------!!Unable to form predictions!!------')
end


function SER = scfde(SP)

numSymbols = SP.FFTsize;
H_channel = fft(SP.channel,SP.FFTsize);

for n = 1:length(SP.SNR),
    tic;
    errCount = 0;

    for k = 1:SP.numRun,
        tmp = round(rand(2,numSymbols));
        tmp = tmp*2 - 1;
        inputSymbols = (tmp(1,:) + i*tmp(2,:))/sqrt(2);

        TxSymbols = [inputSymbols(numSymbols-SP.CPsize+1:numSymbols) inputSymbols];

        RxSymbols = filter(SP.channel, 1, TxSymbols); % Multipath Channel

        tmp = randn(2, numSymbols+SP.CPsize);
        complexNoise = (tmp(1,:) + i*tmp(2,:))/sqrt(2);
        noisePower = 10^(-SP.SNR(n)/10);
        RxSymbols = RxSymbols + sqrt(noisePower)*complexNoise;

        EstSymbols = RxSymbols(SP.CPsize+1:numSymbols+SP.CPsize);
        Y = fft(EstSymbols, SP.FFTsize);

        if SP.equalizerType == 'ZERO'
            Y = Y./H_channel;
        elseif SP.equalizerType == 'MMSE'
            C = conj(H_channel)./(conj(H_channel).*H_channel + 10^(-SP.SNR(n)/10));
            Y = Y.*C;
        end

        EstSymbols = ifft(Y);

        EstSymbols = sign(real(EstSymbols)) + i*sign(imag(EstSymbols));
        EstSymbols = EstSymbols/sqrt(2);

        I = find((inputSymbols-EstSymbols) == 0);
        errCount = errCount + (numSymbols-length(I));
    end
    SER(n,:) = errCount / (numSymbols*SP.numRun);
    [SP.SNR(n) SER(n,:)]
    toc
end


[~,structs]=libphaseretprotofile;

S = libstruct('phaseret_legla_init_params',struct());
calllib('libphaseret','phaseret_legla_init_params_defaults',S);

f = greasy;
a = 256;
M = 2*1024;
M2 = floor(M/2) + 1;
gl = M;
L = dgtlength(size(f,1),a,M);
g = firwin('blackman',gl);
gd = long2fir(gabdual(g,a,M),gl);
N = L/a;
maxit = 200;

corig = dgtreal(f,{'blackman',gl},a,M);
s = abs(corig) + 1i*zeros(size(corig));
cinitPtr = libpointer('doublePtr',complex2interleaved(s));

cout = zeros(2*M2,N);
coutPtr = libpointer('doublePtr',cout);

tic;
calllib('libphaseret','phaseret_legla_d',cinitPtr,g,L,gl,1,a,M,maxit,coutPtr);
toc;
 cout2 = interleaved2complex(coutPtr.Value);
 %cout2 = phaselockreal(cout2,a,M);

frec = idgtreal(cout2,{'dual',{'blackman',gl}},a,M);
s2 = dgtreal(frec,{'blackman',gl},a,M);
magnitudeerrdb(s,s2)

tic;
cout2 = legla(s,g,a,M,'modtrunc','onthefly','flegla','maxit',maxit);
toc;

frec = idgtreal(cout2,{'dual',{'blackman',gl}},a,M);

s2 = dgtreal(frec,{'blackman',gl},a,M);
magnitudeerrdb(s,s2)

clear S;


%     Steffen Urban email: steffen.urban@kit.edu
%     Copyright (C) 2014  Steffen Urban
%
%     This program is free software; you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation; either version 2 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License along
%     with this program; if not, write to the Free Software Foundation, Inc.,
%     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

% 04.03.2014 by Steffen Urban
% this is a modified file from
% Davide Scaramuzzas Toolbox OcamCalib
% original filename: findcenter.m

function findcenterUrban(calib_data)

if isempty(calib_data.ima_proc) | isempty(calib_data.Xp_abs)
    fprintf(1,'\nNo corner data available. Extract grid corners before calibrating.\n\n');
    return;
end

fprintf(1,'\nComputing center coordinates.\n\n');

if isempty(calib_data.taylor_order),
    calib_data.taylor_order = calib_data.taylor_order_default;
end

%% ================
%  added code
options = optimset('Display','off','MaxIter',10000,'LargeScale','off');
x0   = [calib_data.ocam_model.xc, calib_data.ocam_model.yc];

[x0,~,~,~] = fminsearch(@errCenterUrban, x0, options, calib_data);

calib_data.ocam_model.xc = x0(1);
calib_data.ocam_model.yc = x0(2);
calib_data.ocam_model.c=1;
calib_data.ocam_model.d=0;
calib_data.ocam_model.e=0;

%% do calibration again
[calib_data.RRfin,calib_data.ocam_model.ss] = calibrate(calib_data.Xt, calib_data.Yt, calib_data.Xp_abs, calib_data.Yp_abs, calib_data.ocam_model.xc, calib_data.ocam_model.yc, calib_data.taylor_order, calib_data.ima_proc);
calib_data.calibrated = 1; %This flag i s1 when the camera has been calibrated

% reproject
M=[calib_data.Xt,calib_data.Yt,zeros(size(calib_data.Xt))];
[allerr,rms] = reprojectpoints_advUrban(calib_data.ocam_model, calib_data.RRfin, calib_data.ima_proc, calib_data.Xp_abs, calib_data.Yp_abs, M);
calib_data.rmsAfterCenter = rms;

ss = calib_data.ocam_model.ss;
ss

figure(3);
set(3,'Name','Calibration results','NumberTitle','off');
subplot(2,1,1);
plot(0:floor(calib_data.ocam_model.width/2),polyval([ss(end:-1:1)],[0:floor(calib_data.ocam_model.width/2)])); grid on; axis equal;
xlabel('Distance ''rho'' from the image center in pixels');
ylabel('f(rho)');
title('Forward projection function');
%
subplot(2,1,2);
plot(0:floor(calib_data.ocam_model.width/2),180/pi*atan2(0:floor(calib_data.ocam_model.width/2),-polyval([ss(end:-1:1)],[0:floor(calib_data.ocam_model.width/2)]))-90); grid on;
xlabel('Distance ''rho'' from the image center in pixels');
ylabel('Degrees');
title('Angle of optical ray as a function of distance from circle center (pixels)');

calib_data.calibrated = 1; %This flag is 1 when the camera has been calibrated


function [auc auh acc0 accm thrm thrs acc sens spec hull] = ...
    rocplot(scores, classes, plottype, Nthr)
%rocplot: plot a Receiver Operating Characteristic (ROC) curve
% ROC curves illustrate the performance on a binary classification problem
% where classification is based on simply thresholding a set of scores at
% varying levels. Low thresholds give high sensitivity but low specificity,
% high thresholds give high specificity but low sensitivity; the ROC curve
% plots this trade-off over a range of thresholds.
%
% Examples:
%   rocplot(scores, classes);
%   rocplot(scores, classes, plottype);
%   rocplot(scores, classes, plottype, Nthr);
%   [AUC AUH] = rocplot(scores, classes);
%   [AUC AUH acc0 accM thrM thr acc sens spec hull] = rocplot(...);
%
% classes is a Boolean vector of the same size as scores, which should
% be true where scores >= threshold should yield true, e.g. true for
% patients, and false for healthy control subjects, in medical diagnosis.
% If you have two vectors of scores, e.g. patient and control, first do:
%   scores  = [control(:); patient(:)];
%   classes = [false(numel(control), 1); true(numel(patient), 1)];
%
% plottype controls what is plotted, it defaults to 2, where:
%   0 gives no plot (useful to get AUC without creating a plot)
%   1 gives a standard ROC curve, with sensitivity vs (1 - specificity)
%   2 gives my preferred convention, with sensitivity vs specificity
% Nthr is an optional number of thresholds to consider (points in the ROC),
% if left off, all unique values in scores are considered, or, if there are
% more than 100 of those, then 100 values equally spaced from min to max of
% scores are used. Alternatively, Nthr can be a vector of thresholds.
%
% AUC is the area under the ROC curve, a measure of overall accuracy,
% which gives the probability that the classifier would rank a randomly
% chosen true instance (e.g. patient) higher than a random false one
% (e.g. control subject).
%
% AUH is the area under the convex hull of the ROC curve, which is of
% interest because it is theoretically possible to operate at any point on
% the convex hull of the points in an ROC curve (by using some proportional
% selection of classifiers that operate at two points defining the relevant
% section of the convex hull. This is just a more complex version of the
% logic that gives the null line for an ROC plot; if a threshold of -inf
% gives (spec=0,sens=1) and +inf gives (1,0), then using -inf for half of
% your data and +inf for the other half is expected to give (0.5,0.5) if
% you have equal numbers of true and false instances).
%
% Also recorded in the plot legend, and optionally returned, are acc0, the
% accuracy at a threshold of zero, which is of special importance
% in some algorithms, e.g. if your scores come from a linear classifier
% like a Support Vector Machine which can give scores(i) = w'*x(:, i) - b,
% as equivalent to testing for w'*x(:, i) > b, and accM, the max accuracy
% from the tested set of thresholds, which occurs at the threshold thrM.
%
% The function can also return a vector of all considered thresholds, along
% with the corresponding accuracies, sensitivities and specificities, in
% the variables thr, acc, sens, spec. The output hull contains the indices
% into sens and spec that give the convex hull. These arguments can then be
% used to plot multiple ROC curves and/or convex hulls on the same axis,
% e.g. after calling rocplot twice (with plottype 0), you could do:
%   plot(spec1,sens1,'b', spec2,sens2,'r', [0 1],[1 0],'g');
%
% See also: roc, prec_rec, get_concave_ROC, auroc, bookmaker
%
% These can be found in the MATLAB Central File Exchange:
% roc             http://www.mathworks.com/matlabcentral/fileexchange/19950
% roc             http://www.mathworks.com/matlabcentral/fileexchange/21318
% prec_rec        http://www.mathworks.com/matlabcentral/fileexchange/21528
% get_concave_ROC http://www.mathworks.com/matlabcentral/fileexchange/21382
% auroc           http://www.mathworks.com/matlabcentral/fileexchange/19468
% bookmaker       http://www.mathworks.com/matlabcentral/fileexchange/5648
% There is also a book "Ordinal Data Modeling", by Valen Johnson, with
% companion code  http://www.mathworks.com/matlabcentral/fileexchange/2264

% Copyright 2009 Ged Ridgway

scores = scores(:);
% need max thr greater than max(scores) so roc sens drops to zero
maxscoreplus = max(scores) + eps(max(scores));
classes = classes(:);
if any(double(logical(classes)) ~= double(classes))
    error('Classes vector must contain true (or 1) and false (or 0) only')
end

if ~exist('Nthr', 'var') || isempty(Nthr)
    if numel(scores) > 100
        Nthr = 100; % no need for more points in plot
    else
        thrs = unique([scores;maxscoreplus]);
        Nthr = numel(thrs);
    end
end
if ~exist('thrs', 'var')
    if ~isscalar(Nthr) % user-specified set of thrs to consider
        thrs = unique(Nthr); % (also sorts)
        Nthr = numel(thrs);
        % thrs(Nthr) = maxscoreplus; % commented out, assuming expert user.
    else
        thrs = linspace(min(scores), maxscoreplus, Nthr);
    end
end
thrs = thrs(:);

acc = zeros(Nthr, 1); sens = acc; spec = sens;
for n = 1:Nthr
    thr = thrs(n);
    result = scores >= thr;
    acc(n) = mean(result == classes);
    sens(n) = sum(result == 1 & classes == 1)/sum(classes == 1);
    spec(n) = sum(result == 0 & classes == 0)/sum(classes == 0);
end
% area under ROC curve
auc = trapz(spec, sens);
% test special case of 0 threshold, since training often assumes this
result0 = scores >= 0;
acc0 = mean(result0 == classes);
sens0 = sum(result0 == 1 & classes == 1)/sum(classes == 1);
spec0 = sum(result0 == 0 & classes == 0)/sum(classes == 0);
% threshold giving highest overall accuracy
[accm indm] = max(acc);
thrm = thrs(indm);
sensm = sens(indm);
specm = spec(indm);
% compute convex hull of ROC curve and area under hull
[hull auh] = convhull([spec;0], [sens;0]);
hull = unique(hull);
hull(end) = []; % don't close loop
% sanity check:
if abs(trapz(spec(hull), sens(hull)) - auh) > eps
    warning('rocplot:auh', 'Area under ROC convex hull may be incorrect');
end
if ~exist('plottype', 'var') || isempty(plottype)
    plottype = 2;
end
switch plottype
    case 0, return % no plot
    case 1 % standard TPR (= sens) vs FPR (= 1-spec)
        plot(1-spec,sens,'b', 1-spec(hull),sens(hull),'c', ...
            1-spec0,sens0,'ro', 1-specm,sensm,'ms', 1-[0 1],[1 0],'g');
        xlabel('False Positive Rate (1 - Specificity)');
        ylabel('True Positive Rate (Sensitivity)');
        legpos = 'SE';
        axis([0 1 0 1])
    case 2 % my convention, sens vs spec
        plot(spec,sens,'b', spec(hull),sens(hull),'c', ...
            spec0,sens0,'ro', specm,sensm,'ms', [0 1],[1 0],'g');
        xlabel('Specificity');
        ylabel('Sensitivity');
        legpos = 'SW';
        axis([0 1.05 0 1.05])
end
legend(sprintf('ROC Curve\nAUC = %.3g', auc), ...
    sprintf('Conv Hull\nAUC = %.3g', auh), ...
    sprintf('Acc(0) = %.3g', acc0), ...
    sprintf('Max Acc %.3g', accm), ... 'Chance', ...
    'Location', legpos)

if nargout == 0, clear auc, end % quieten rocplot(scores,classes) without ;


function stroud_test28 ( )

%*****************************************************************************80
%
%% TEST28 tests SIMPLEX_VOLUME_ND.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST28\n' );
  fprintf ( 1, '  SIMPLEX_VOLUME_ND computes the volume of a simplex\n' );
  fprintf ( 1, '    in N dimensions.\n' );
  fprintf ( 1, '\n' );

  for n = 2 : 4

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Spatial dimension N = %d\n', n );
%
%  Set the values of the simplex.
%
    v = setsim ( n );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Simplex vertices:\n' );
    fprintf ( 1, '\n' );

    for i = 1 : n+1
      for j = 1 : n
        fprintf ( 1, '  %4f', v(i,j) );
      end
      fprintf ( 1, '\n' );
    end

    volume = simplex_volume_nd ( n, v );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Volume is %f\n', volume );

  end

  return
end


%% Simulate some RF pulses and analyze their properties
%
% This example demonstrates the use of mr.simRf() function.

%% Create a system object
% we need it here to create fat-sat pulses
sys = mr.opts('B0', 2.89);
%seq=mr.Sequence(sys);

%% 30 degree slice selective SINC pulse
rf30_sinc = mr.makeSincPulse(pi/6,'system',sys,'Duration',3e-3,'use','excitation',...
    'PhaseOffset',pi/2,'apodization',0.3,'timeBwProduct',4);

[bw,f0,M_xy_sta,F1]=mr.calcRfBandwidth(rf30_sinc);
[M_z,M_xy,F2]=mr.simRf(rf30_sinc);

figure; plot(F1,abs(M_xy_sta),F2,abs(M_xy),F2,M_z);
axis([f0-2*bw, f0+2*bw, -0.1, 1.2]);
legend({'M_x_ySTA','M_x_ySIM','M_zSIM'});
xlabel('frequency offset / Hz');
ylabel('magnetisation');
title('STA vs. simulation, flip angle 30°');

figure; plot(F2,real(M_xy),F2,imag(M_xy));
axis([f0-2*bw, f0+2*bw, -1.2, 1.2]);
legend({'M_xSIM','M_ySIM'});
xlabel('frequency offset / Hz');
ylabel('magnetisation');
title('Real and imag. parts of transverse magnetisation, 30° flip');

%% 90 degree slice selective SINC pulse
rf90_sinc = mr.makeSincPulse(pi/2,'system',sys,'Duration',3e-3,'use','excitation',...
    'PhaseOffset',pi/2,'apodization',0.6,'timeBwProduct',8);

[bw,f0,M_xy_sta,F1]=mr.calcRfBandwidth(rf90_sinc);
[M_z,M_xy,F2]=mr.simRf(rf90_sinc);

%%
figure; plot(F1,abs(M_xy_sta),F2,abs(M_xy),F2,M_z);
axis([f0-2*bw, f0+2*bw, -0.1, 1.2]);
legend({'M_x_ySTA','M_x_ySIM','M_zSIM'});
xlabel('frequency offset / Hz');
ylabel('magnetisation');
title('STA vs. simulation, flip angle 90°');

figure; plot(F2,atan2(abs(M_xy),M_z)/pi*180);
axis([f0-2*bw, f0+2*bw, -5, 100]);
xlabel('frequency offset / Hz');
ylabel('flip ange [°]');
legend({'SINC'});
grid on;
title('Achieved flip angle for the nominal 90° flip');

figure; plot(F2,real(M_xy),F2,imag(M_xy));
axis([f0-2*bw, f0+2*bw, -1.2, 1.2]);
legend({'M_xSIM','M_ySIM'});
xlabel('frequency offset / Hz');
ylabel('magnetisation');
title('Real and imag. parts of transverse magnetisation, 90° flip');

%%
figure; plot(F2,angle(M_xy));
axis([f0-2*bw, f0+2*bw, -3.2, 3.2]);
xlabel('frequency offset / Hz');
ylabel('phase');
title('Phase of transverse magnetisation, 90° flip');

M_xy_masked=M_xy;
M_xy_masked(F2<f0-bw/2)=0;
M_xy_masked(F2>f0+bw/2)=0;

i_phase_slope=sum(M_xy_masked(2:end).*conj(M_xy_masked(1:end-1)));
i_phase_slope=1i*angle(i_phase_slope)/(F2(2)-F2(1));
i_phase_offset=sum(M_xy_masked.*exp(-i_phase_slope*F2));
i_phase_offset=i_phase_offset/abs(i_phase_offset);

hold on; plot(F2,angle(exp(i_phase_slope*F2)*i_phase_offset),'--');

xline(f0,'-');
xline(f0-bw/2,'--');
xline(f0+bw/2,'--');

legend({'SINC-phase','linear fit'});
fprintf('SINC90 rf center error: %g (%g %%)\n', abs(i_phase_slope)/2*pi/rf90_sinc.shape_dur/10, 100/0.5*abs(i_phase_slope)/2*pi/rf90_sinc.shape_dur/10); % no idea where this 10 comes from

%% 90 degree slice selective SLR pulse
rf_90slr= mr.makeSLRpulse(pi/2,'duration',3e-3,'timeBwProduct',4,'PhaseOffset',pi/2,'use','excitation',...
    'passbandRipple',1,'stopbandRipple',1e-2,'filterType','ms','system',sys);

[bw,f0,M_xy_sta,F1]=mr.calcRfBandwidth(rf_90slr);
[M_z,M_xy,F2]=mr.simRf(rf_90slr);

figure; plot(F1,abs(M_xy_sta),F2,abs(M_xy),F2,M_z);
axis([f0-2*bw, f0+2*bw, -0.1, 1.2]);
legend({'M_x_ySTA','M_x_ySIM','M_zSIM'});
xlabel('frequency offset / Hz');
ylabel('magnetisation');
title('SLR: STA vs. simulation, flip angle 90°');

figure; plot(F2,real(M_xy),F2,imag(M_xy));
axis([f0-2*bw, f0+2*bw, -1.2, 1.2]);
legend({'M_xSIM','M_ySIM'});
xlabel('frequency offset / Hz');
ylabel('magnetisation');
title('SLR: Real and imag. parts of transverse magnetisation, 90° flip');

figure; plot(F2,angle(M_xy));
axis([f0-2*bw, f0+2*bw, -3.2, 3.2]);
xlabel('frequency offset / Hz');
ylabel('phase');
legend({'SLR'});
title('SLR: Phase of transverse magnetisation, 90° flip');

figure; plot(F2,atan2(abs(M_xy),M_z)/pi*180);
axis([f0-2*bw, f0+2*bw, -5, 100]);
xlabel('frequency offset / Hz');
ylabel('flip ange [°]');
legend({'SLR'});
grid on;
title('SLR: Achieved flip angle for the nominal 90° flip');

%% 60 degree slice selective SINC pulse
rf60_block = mr.makeBlockPulse(pi/3,'system',sys,'Duration',0.5e-3,'use','excitation','PhaseOffset',pi/2);

[bw,f0,M_xy_sta,F1]=mr.calcRfBandwidth(rf60_block);
[M_z,M_xy,F2]=mr.simRf(rf60_block);

figure; plot(F1,abs(M_xy_sta),F2,abs(M_xy),F2,M_z);
axis([f0-2*bw, f0+2*bw, -0.1, 1.2]);
legend({'M_x_ySTA','M_x_ySIM','M_zSIM'});
xlabel('frequency offset / Hz');
ylabel('magnetisation');
title('STA vs. simulation, 60° hard pulse');

figure; plot(F2,real(M_xy),F2,imag(M_xy));
axis([f0-2*bw, f0+2*bw, -1.2, 1.2]);
legend({'M_xSIM','M_ySIM'});
xlabel('frequency offset / Hz');
ylabel('magnetisation');
title('Real and imag. parts of transverse magnetisation, 60° hard pulse');

%% fat-sat pulse
sat_ppm=-3.45;
sat_freq=sat_ppm*1e-6*sys.B0*sys.gamma;
rf_fs = mr.makeGaussPulse(110*pi/180,'system',sys,'Duration',8e-3,...
    'bandwidth',abs(sat_freq),'freqOffset',sat_freq,'use','saturation');
rf_fs.phaseOffset=-2*pi*rf_fs.freqOffset*mr.calcRfCenter(rf_fs); % compensate for the frequency-offset induced phase

[M_z,M_xy,F2]=mr.simRf(rf_fs);

figure; plot(F2,real(M_xy),F2,imag(M_xy),F2,M_z);
axis([sat_freq-900, sat_freq+900, -1.2, 1.2]);
legend({'M_x','M_y','M_z'});
xlabel('frequency offset / Hz');
ylabel('magnetisation');
title('Simulation, Gaussian fat-sat pulse');

%% 180 degree slice selective SINC pulse
rf180_sinc = mr.makeSincPulse(pi,'system',sys,'Duration',4e-3,'use','refocusing',...
    'apodization',0.3,'timeBwProduct',6);

[bw,f0,M_xy_sta,F1]=mr.calcRfBandwidth(rf180_sinc);
[M_z,M_xy,F2,ref_eff]=mr.simRf(rf180_sinc);

% figure; plot(F1,abs(M_xy_sta),F2,abs(M_xy),F2,M_z);
% axis([f0-2*bw, f0+2*bw, -1.2, 1.2]);
% legend({'M_x_ySTA','M_x_ySIM','M_zSIM'});
% xlabel('frequency offset / Hz');
% ylabel('magnetisation');
% title('STA vs. simulation, flip angle 180°');
%
% figure; plot(F2,real(M_xy),F2,imag(M_xy));
% axis([f0-2*bw, f0+2*bw, -1.2, 1.2]);
% legend({'M_xSIM','M_ySIM'});
% xlabel('frequency offset / Hz');
% ylabel('magnetisation');
% title('Real and imag. parts of transverse magnetisation, 180° flip');
%
% figure; plot(F2,angle(M_xy));
% axis([f0-2*bw, f0+2*bw, -3.2, 3.2]);
% xlabel('frequency offset / Hz');
% ylabel('phase');
% title('Phase of transverse magnetisation, 180° flip');

figure; plot(F2,atan2(abs(M_xy),M_z)/pi*180);
axis([f0-2*bw, f0+2*bw, -5, 190]);
xlabel('frequency offset / Hz');
ylabel('flip ange [°]');
legend({'SINC'});
grid on;
title('Achieved flip angle for the nominal 180° flip');

figure; plot(F2,abs(ref_eff));
axis([f0-2*bw, f0+2*bw, -0.1, 1.1]);
xlabel('frequency offset / Hz');
ylabel('efficiency');
legend({'SINC'});
title('refocusing efficiency');

figure; plot(F2,angle(ref_eff));
axis([f0-2*bw, f0+2*bw, -3.2, 3.2]);
xlabel('frequency offset / Hz');
ylabel('efficiency');
legend({'SINC'});
title('refocusing efficiency phase (~2x RF phase)');

%% 180 degree slice selective SLR pulse
rf180_slr= mr.makeSLRpulse(pi,'duration',4e-3,'timeBwProduct',6,'use','refocusing','filterType','ms','system',sys);

[bw,f0,M_xy_sta,F1]=mr.calcRfBandwidth(rf180_slr);
[M_z,M_xy,F2_slr,ref_eff_slr]=mr.simRf(rf180_slr);

figure; plot(F2_slr,atan2(abs(M_xy),M_z)/pi*180);
axis([f0-2*bw, f0+2*bw, -5, 190]);
xlabel('frequency offset / Hz');
ylabel('flip ange [°]');
legend({'SLR'});
grid on;
title('Achieved flip angle for the nominal 180° flip');

figure; plot(F2_slr,abs(ref_eff_slr));
axis([f0-2*bw, f0+2*bw, -0.1, 1.1]);
xlabel('frequency offset / Hz');
ylabel('efficiency');
legend({'SLR'});
title('refocusing efficiency');

figure; plot(F2_slr,angle(ref_eff_slr));
axis([f0-2*bw, f0+2*bw, -3.2, 3.2]);
xlabel('frequency offset / Hz');
ylabel('efficiency');
title('SLR: refocusing efficiency phase (~2x RF phase)');

figure; plot(F2,abs(ref_eff),F2_slr,abs(ref_eff_slr));
axis([f0-2*bw, f0+2*bw, -0.1, 1.1]);
xlabel('frequency offset / Hz');
ylabel('efficiency');
legend({'SINC','SLR'});
title('refocusing efficiency: SINC vs SLR');

%% adiabatic pulse
rf180_ad = mr.makeAdiabaticPulse('wurst','duration',4.6e-3,'bandwidth',6000,'n_fac',20,'use','inversion','system',sys);

[bw,f0,M_xy_sta,F1]=mr.calcRfBandwidth(rf180_ad);
[M_z,M_xy,F2,ref_eff,ref_mx,ref_my]=mr.simRf(rf180_ad,-0.5);

figure; plot(F1,abs(M_xy_sta),F2,abs(M_xy),F2,M_z);
axis([f0-2*bw, f0+2*bw, -1.2, 1.2]);
legend({'M_x_ySTA','M_x_ySIM','M_zSIM'});
xlabel('frequency offset / Hz');
ylabel('magnetisation');
title('STA vs. simulation, adiabatic pulse');

figure; plot(F2,real(M_xy),F2,imag(M_xy));
axis([f0-2*bw, f0+2*bw, -1.2, 1.2]);
legend({'M_xSIM','M_ySIM'});
xlabel('frequency offset / Hz');
ylabel('magnetisation');
title('Real and imag. parts of transverse magnetisation, adiabatic pulse');

figure; plot(F2,angle(M_xy));
axis([f0-2*bw, f0+2*bw, -3.2, 3.2]);
xlabel('frequency offset / Hz');
ylabel('phase');
legend({'WURST'});
title('Phase of transverse magnetisation, adiabatic pulse');

figure; plot(F2,atan2(abs(M_xy),M_z)/pi*180);
axis([f0-2*bw, f0+2*bw, -5, 190]);
xlabel('frequency offset / Hz');
ylabel('flip ange [°]');
legend({'WURST'});
grid on;
title('Achieved flip angle adiabatic pulse');

figure; plot(F2,abs(ref_eff));
axis([f0-2*bw, f0+2*bw, -0.1, 1.1]);
xlabel('frequency offset / Hz');
ylabel('efficiency');
legend({'WURST'});
title('refocusing efficiency');

figure; plot(F2,angle(ref_eff));
axis([f0-2*bw, f0+2*bw, -3.2, 3.2]);
xlabel('frequency offset / Hz');
ylabel('efficiency');
legend({'WURST'});
title('refocusing efficiency phase (~2x RF phase)');

%% spoiling simulation for the same pulse used and refocusing pulse

spoiling_factor=5; % area of the left/righ spoiler; reasonable range 1..10
cl=13; % convolution length to simulate intravoxel dephasing

[M_z,M_xy,F2,ref_eff,mxrf,myrf]=mr.simRf(rf180_ad,spoiling_factor,spoiling_factor);

mxrfc=conv(mxrf,ones(cl,1)/cl,'same');
myrfc=conv(myrf,ones(cl,1)/cl,'same');

figure;plot(F2,abs(abs(mxrfc)+1i*abs(myrfc))/2^0.5,F2,abs(ref_eff),F2,0.5-0.5*M_z,'--');
xlabel('frequency offset / Hz');
ylabel('signal');
legend({'spoiling','ref.eff.','.5-.5*M_z'});
title('signal with spoiling vs refocusing efficiency');

%% investigare RF center shift as a function of clip angle (is it an artifact?)

alphas=[5:5:150];
rfce=[];
for a=alphas
    rfAlpha = mr.makeSincPulse(a/180*pi,'system',sys,'Duration',3e-3,'use','excitation','apodization',0.3,'timeBwProduct',4);
    %rfAlpha = mr.makeGaussPulse(a*pi/180,'system',sys,'Duration',3e-3,'timeBwProduct',8,'use','excitation');

    [bw,f0,M_xy_sta,F1]=mr.calcRfBandwidth(rfAlpha);
    [M_z,M_xy,F2]=mr.simRf(rfAlpha);

    M_xy_masked=M_xy;
    M_xy_masked(F2<f0-bw/2)=0;
    M_xy_masked(F2>f0+bw/2)=0;

    i_phase_slope=sum(M_xy_masked(2:end).*conj(M_xy_masked(1:end-1)));
    i_phase_slope=1i*angle(i_phase_slope)/(F2(2)-F2(1));

    rfce=[rfce,abs(i_phase_slope)/2*pi/rf90_sinc.shape_dur/10]; % no idea where this 10 comes from
end

figure;plot(alphas,rfce/0.5*100); title('excess gradient refocusing needed in %');
xticks([0:30:alphas(end)]);
yticks([0:2:10]);
grid on;
xlabel('flip angle / °');
ylabel('refocusing / %');


% ANFIS-ART Feed-Forward operation.
function y = anfis_art_forward(x,u2,v2,gamma,ThetaL4)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%        								 							     				                                       %
%   			                 	NETWORK FUNCTIONALITY SECTION					   %
%      																					                                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
NumInVars    = size(x,1);
NumInTerms = size(u2,2);
NumRules     = NumInTerms;

% LAYER 1 - MF NODES
% Each node in this layer acts as a one-dimensional membership function.
% Out2 is a (NumInVars X NumInTerms) matrix.
 In1mat = x*ones(1,NumInTerms);
 % Matrix containing a copy of Out1 for each Layer 2 Term Node.
 Out1 = 1 - g(In1mat-v2,gamma) - g(u2-In1mat,gamma);

% LAYER 2 - PRECONDITION MATCHING OF FUZZY LOGIC RULES
% RULE NODES == NumInTerms
 Out2 = prod(Out1,1);
 S_2 = sum(Out2);

% LAYER 3 - NORMALIZATION NODES - All Node Activity Adds-up to unity.
if S_2~=0
     Out3 = Out2/S_2;
end

% LAYERS 4 - 5: CONSEQUENT NODES - SUMMING NODE
 Aux1 = [x; 1]*Out3;

 % New Input Training Data shaped as a column vector.
 a = reshape(Aux1,(NumInVars+1)*NumRules,1);
 y = a'*ThetaL4;


% makenoise.m
% written by: Duncan Po
% Date: August 24, 2002
% Generate additive Gaussian noise and add the noise to an image
% This function assumes that the image is stored in uint8 format with
% values from 0 to 255
% Usage: noisepic = makenoise(imname, imformat, nvar)
% Inputs:   imname      - name of the image file
%           imformat    - format of the image file
%           nvar        - variance of the noise normalized to the image range
% Output:   noisepic    - the resulting noisy image

function noisepic = makenoise(imname, imformat, nvar)

pic = imread(imname,imformat);
figure;
imshow(pic);
pic = double(pic);

% the following step is needed for the command imnoise (normalize)
pic2 = pic/255;

% adds zero mean Gaussian noise of power (0.1*255)^2 = 650.25
noisepic = imnoise(pic2, 'Gaussian', 0, nvar);

% converts the image back to a scale of 8 bits (0 to 255)
noisepic = noisepic*255;

% calculate initial MSE of the noisy image
MSE = sqrt(sum(sum((noisepic - pic).*(noisepic-pic)/(size(pic,1)*size(pic,2))...
    /(size(pic,1)*size(pic,2)))))
noisepic = uint8(noisepic);

figure;
imshow(noisepic);


function [mi3] = m32mi3(m3)
% Convert volume from cubic meters to cubic miles.
% Chad Greene 2012
mi3 = m3*2.3991275858e-10;


function res = imTangentCrop(img, pos, boxSize)
% Crop an image around a point based on local orientation.
%
%   RES = imTangentCrop(IMG, BOXCENTER, BOXSIZE)
%   Computes and orientated crop of the input image IMG, by considering all
%   pixels within a oriented box with centered given by BOXCENTER, size
%   given by BOXSIZE, and orientation evaluated from local gradient of the
%   image at the point POS.
%
%
%   Example
%
%   See also
%     imCropOrientedBox, imCropBox, imTangentCrop3d
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% INRAE - BIA Research Unit - BIBS Platform (Nantes)
% Created: 2022-06-01,    using Matlab 9.9.0.1570001 (R2020b) Update 4
% Copyright 2022 INRAE.

%% Compute transform

% evaluate gradient
grad = imLocalGradient(img, pos, 3);

% convert gradient to rotation angle
angle = atan2(grad(2), grad(1)) - pi/2;

% create the transform matrix that maps from box coords to global coords
transfo = createTranslation(pos) * createRotation(angle);

%% Sample points within box

% generate point coords along each box axis:
% * number of values equals to round(boxSize)
% * use single pixel spacing
radius = round(boxSize) / 2;
lx = -radius(1)+0.5:radius(1)-0.5;
ly = -radius(2)+0.5:radius(2)-0.5;

% map into global coordinate space
[x, y] = meshgrid(lx, ly);
[x, y] = transformPoint(x, y, transfo);

% evaluate within image
res = imEvaluate(img, x, y);


function G = compute_periodic_poisson(d, symmetrize)

% compute_periodic_poisson - solve poisson equation
%
%   G = compute_periodic_poisson(d,symmetrize);
%
%   Solve
%       Delta(G) = d
%   with periodic boundary condition.
%   G has zero mean.
%
%   Set symmetrize=1 (default 0) if the data divergence d is not periodic.
%   This will double the size and consider symmetric extension.
%
%   Copyright (c) 2007 Gabriel Peyre

n = size(d,1);

if nargin==2 && symmetrize
    d = perform_size_doubling(d);
    G = compute_periodic_poisson(d);
    G = G(1:n,1:n);
    return;
end

% solve for laplacian
[Y,X] = meshgrid(0:n-1,0:n-1);
mu = sin(X*pi/n).^2; mu = -4*( mu+mu' );
mu(1) = 1; % avoid division by 0

G = fft2(d) ./ mu; G(1) = 0;
G = real( ifft2( G ) );

%%
function g = perform_size_doubling(g)

g = [g;g(end:-1:1,:,:)];
g = [g,g(:,end:-1:1,:)];


function [B, L] = greedyExtremePoolBasis(model)
% Computes a non-negative basis for the left nullspace of the stoichiometric
% matrix using optimization to pick random extreme rays, then test a
% posteriori if each is linearly independent from the existing stored
% extreme rays.
%
% USAGE:
%
%    [B, L] = greedyExtremePoolBasis(model)
%
% INPUT:
%    model:    model structure
%
% OUTPUTS:
%    B, L:     non-negative basus fi the left nullspace

[inform,molecularVector]=checkStoichiometricConsistency(model,0); %check stoichiometric consistency
if inform~=1 %check if positive vector in left nullspace
    B=[];
    L=[]; % Returning empty vector for left nullspace so if it is expected matlab will keep running
    return;
end

[nMet,nRxn]=size(model.S);

%compute linear basis for left nullspace
printLevelL=0;
[L,rankS]=getNullSpace(model.S',printLevelL);
L=L';

B=sparse(nMet-rankS,nMet);

nPools=0;
nTry=0;
tic;
while nPools < (nMet-rankS)
    [x, output] = findExtremePool(model);
    B(nPools+1,:)=x';
    if nPools==0
        rankB=1;
    else
        nonZeroColumns=(B~=0);
        nonZeroColumns=sum(nonZeroColumns,1);
        rankB = getRankLUSOL(B(1:nPools+1,nonZeroColumns~=0));
        % %error as reports wrong rank if zero columns
        % rankB = getRankLUSOL(B(1:nPools+1,:));
    end
    if rankB==(nPools+1)
        nPools=nPools+1;
        fprintf('%s\n',[int2str(nPools) ' of ' int2str(nMet-rankS) ' linearly independent pool vectors, at time ' num2str(toc)]);
    else
        % fprintf('%s\n','Linearly dependent pool vector discarded');
    end
    nTry=nTry+1;
    if toc > 100
        B=B(end-1,:);
        fprintf('%s%u%s\n','Only ',nPools, ' computed.');
        break
    end
end
fprintf('%s%g\n','Hit fraction ',(nMet-rankS)/nTry);


function [c] = cellcov(x, y, dim, flag)

% [C] = CELLCOV(X, DIM) computes the covariance, across all cells in x along
% the dimension dim. When there are three inputs, covariance is computed between
% all cells in x and y
%
% X (and Y) should be linear cell-array(s) of matrices for which the size in at
% least one of the dimensions should be the same for all cells

if nargin<4 && iscell(y)
  flag = 1;
elseif nargin<4 && isnumeric(y)
  flag = dim;
end

if nargin<3 && iscell(y)
  scx1 = cellfun('size', x, 1);
  scx2 = cellfun('size', x, 2);
  if     all(scx2==scx2(1)), dim = 2; %let second dimension prevail
  elseif all(scx1==scx1(1)), dim = 1;
  else   error('no dimension to compute covariance for');
  end
elseif nargin<=3 && isnumeric(y)
  dim = y;
end

if isnumeric(y), y = []; end

nx = size(x);
if ~iscell(x) || length(nx)>2 || all(nx>1),
  error('incorrect input for cellcov');
end

if flag,
  mx   = cellmean(x, 2);
  x    = cellvecadd(x, -mx);
  if ~isempty(y),
    my = cellmean(y, 2);
    y  = cellvecadd(y, -my);
  end
end

nx   = max(nx);
nsmp = cellfun('size', x, dim);
if isempty(y),
  csmp = cellfun(@covc, x, repmat({dim},1,nx), 'UniformOutput', 0);
else
  csmp = cellfun(@covc, x, y, repmat({dim},1,nx), 'UniformOutput', 0);
end
nc   = size(csmp{1});
c    = sum(reshape(cell2mat(csmp), [nc(1) nc(2) nx]), 3)./sum(nsmp);

function [c] = covc(x, y, dim)

if nargin==2,
  dim = y;
  y   = x;
end

if dim==1,
  c = x'*y;
elseif dim==2,
  c = x*y';
end


function y = daub4_scale ( n, x )

%*****************************************************************************80
%
%% DAUB4_SCALE recursively evaluates the DAUB4 scaling function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the recursion level.
%
%    Input, real X, the point at which the function is to be evaluated.
%
%    Output, real Y, the estimated value of the function.
%
  c = [  0.4829629131445341E+00; ...
         0.8365163037378079E+00; ...
         0.2241438680420133E+00; ...
        -0.1294095225512603E+00 ];

  c = sqrt ( 2.0 ) * c;

  if ( 0 < n )
    y = ( c(1) * daub4_scale ( n - 1, 2 * x     ) ...
        + c(2) * daub4_scale ( n - 1, 2 * x - 1 ) ...
        + c(3) * daub4_scale ( n - 1, 2 * x - 2 ) ...
        + c(4) * daub4_scale ( n - 1, 2 * x - 3 ) );
  else
    y = ( 0 <= x & x < 1 );
  end

  return
end


function [CX, sse] = vgg_kmeans(X, nclus, varargin)

% VGG_KMEANS    initialize K-means clustering
%               [CX, sse] = vgg_kmeans(X, nclus, optname, optval, ...)
%
%               - X: input points (one per column)
%               - nclus: number of clusters
%               - opts (defaults):
%                    maxiters (inf): maxmimum number of iterations
%                    mindelta (eps): minimum change in SSE per iteration
%                       verbose (1): 1=print progress
%
%               - CX: cluster centers
%               - sse: SSE

% Author: Mark Everingham <me@robots.ox.ac.uk>
% Date: 13 Jan 03

opts = struct('maxiters', inf, 'mindelta', eps, 'verbose', 1);
if nargin > 2
    opts=vgg_argparse(opts,varargin);
end

perm=randperm(size(X,2));
CX=X(:,perm(1:nclus));

sse0 = inf;
iter = 0;
while iter < opts.maxiters

    tic;
    [CX, sse] = vgg_kmiter(X, CX);
    t=toc;

    if opts.verbose
        fprintf('iter %d: sse = %g (%g secs)\n', iter, sse, t)
    end

    if sse0-sse < opts.mindelta
        break
    end

    sse0=sse;
    iter=iter+1;

end


function [ transform ] = genTransMat( front, up )
%GENTRANSMAT

axes = cross(front,up);
axes = axes/norm(axes,2);
transform = [front(:)';up(:)';axes(:)'];

end


function Retinex = retinex_frankle_mccann(L, nIterations)

% RETINEX_FRANKLE_McCANN:
%         Computes the raw Retinex output from an intensity image, based on the
%         original model described in:
%         Frankle, J. and McCann, J., "Method and Apparatus for Lightness Imaging"
%         US Patent #4,384,336, May 17, 1983
%
% INPUT:  L           - logarithmic single-channel intensity image to be processed
%         nIterations - number of Retinex iterations
%
% OUTPUT: Retinex     - raw Retinex output
%
% NOTES:  - The input image is assumed to be logarithmic and in the range [0..1]
%         - To obtain the retinex "sensation" prediction, a look-up-table needs to
%         be applied to the raw retinex output
%         - For colour images, apply the algorithm individually for each channel
%
% AUTHORS: Florian Ciurea, Brian Funt and John McCann.
%          Code developed at Simon Fraser University.
%
% For information about the code see: Brian Funt, Florian Ciurea, and John McCann
% "Retinex in Matlab," by Proceedings of the IS&T/SID Eighth Color Imaging
% Conference: Color Science, Systems and Applications, 2000, pp 112-121.
%
% paper available online at http://www.cs.sfu.ca/~colour/publications/IST-2000/
%
% Copyright 2000. Permission granted to use and copy the code for research and
% educational purposes only.  Sale of the code is not permitted. The code may be
% redistributed so long as the original source and authors are cited.

global RR IP OP NP Maximum
RR = L;
Maximum = max(L(:));                                 % maximum color value in the image
[nrows, ncols] = size(L);

shift = 2^(fix(log2(min(nrows, ncols)))-1);          % initial shift
OP = Maximum*ones(nrows, ncols);                     % initialize Old Product

while (abs(shift) >= 1)
   for i = 1:nIterations
      CompareWith(0, shift);                         % horizontal step
      CompareWith(shift, 0);                         % vertical step
   end
   shift = -shift/2;                                 % update the shift
end
Retinex = NP;

function CompareWith(s_row, s_col)
global RR IP OP NP Maximum
IP = OP;
if (s_row + s_col > 0)
   IP((s_row+1):end, (s_col+1):end) = OP(1:(end-s_row), 1:(end-s_col)) + ...
   RR((s_row+1):end, (s_col+1):end) - RR(1:(end-s_row), 1:(end-s_col));
else
   IP(1:(end+s_row), 1:(end+s_col)) = OP((1-s_row):end, (1-s_col):end) + ...
   RR(1:(end+s_row),1:(end+s_col)) - RR((1-s_row):end, (1-s_col):end);
end
IP(IP > Maximum) = Maximum;                          % The Reset operation
NP = (IP + OP)/2;                                    % average with the previous Old Product
OP = NP;                                             % get ready for the next comparison


% demonstrate usage of prior distributions
%
% See also priorDistributions.m.
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2015-07-13.
%                                      File automatically generated using noweb.
clear all, close all

% 1) specify some priors
% a) univariate priors
mu = 1.0; s2 = 0.01^2; nu = 3;
pg = {@priorGauss,mu,s2};                          % Gaussian prior
pl = {'priorLaplace',mu,s2};                        % Laplace prior
pt = {@priorT,mu,s2,nu};                        % Student's t prior
p1 = {@priorSmoothBox1,0,3,15};  % smooth box constraints lin decay
p2 = {@priorSmoothBox2,0,2,15};  % smooth box constraints qua decay
pd = {'priorDelta'}; % fix value of prior exclude from optimisation
pc = {@priorClamped};                         % equivalent to above
lam = 1.05; k = 2.5;
pw = {@priorWeibull,lam,k};                         % Weibull prior

% b) meta priors
pmx = {@priorMix,[0.5,0.5],{pg,pl}};        % mixture of two priors
g = @exp; dg = @exp; ig = @log;
ptr = {@priorTransform,g,dg,ig,pg};    % Gaussian in the exp domain

% c) multivariate priors
m = [1;2]; V = [2,1;1,2];
pG = {@priorGaussMulti,m,V};                    % 2d Gaussian prior
pD = {'priorDeltaMulti'};   % fix value of prior exclude from optim
pC = {@priorClampedMulti};                    % equivalent to above

% 2) evaluation
% pri = pt;   hp = randn(1,3);
% pri = pmx;  hp = randn(1,3);
% pri = ptr;  hp = randn(1,3);
pri = pG;   hp = randn(2,3);

% a) draw a sample from the prior
feval(pri{:})

% b) evaluate prior and derivative if requires
[lp,dlp] = feval(pri{:},hp)

% 3) comprehensive example
x = (0:0.1:10)'; y = 2*x+randn(size(x));   % generate training data
mean = {@meanSum,{@meanConst,@meanLinear}}; % specify mean function
cov = {@covSEiso}; lik = {@likGauss};  % specify covariance and lik
hyp.cov = [log(1);log(1.2)]; hyp.lik = log(0.9); hyp.mean = [2;3];
par = {mean,cov,lik,x,y}; mfun = @minimize; % input for GP function

% a) plain marginal likelihood optimisation (maximum likelihood)
im = @infExact;                                  % inference method
hyp_plain = feval(mfun, hyp, @gp, -10, im, par{:});      % optimise

% b) regularised optimisation (maximum a posteriori) with 1d priors
prior.mean = {pg;pc};  % Gaussian prior for first, clamp second par
prior.cov  = {p1;[]}; % box prior for first, nothing for second par
im = {@infPrior,@infExact,prior};                % inference method
hyp_p1 = feval(mfun, hyp, @gp, -10, im, par{:});         % optimise

% c) regularised optimisation (maximum a posteriori) with Nd priors
prior = [];                                   % clear the structure
% multivariate Student's t prior on the first and second mean hyper
prior.multi{1} = {@priorTMulti,[mu;mu],diag([s2,s2]),nu,...
                 struct('mean',[1,2])};          % use hyper struct
% Equivalent shortcut (same mu and s2 for all dimensions)
prior.multi{1} = {@priorTMulti,mu,s2,nu,struct('mean',[1,2])};
% multivariate Gaussian prior jointly on 1st and 3rd hyper
prior.multi{2} = {@priorGaussMulti,[mu;mu],diag([s2,s2]),...
                 [1,3]};               % use unwrapped hyper vector
% Equivalent shortcut (same mu and s2 for all dimensions)
prior.multi{2} = {@priorGaussMulti,mu,s2,[1,3]};
im = {@infPrior,@infExact,prior};                % inference method
hyp_pN = feval(mfun, hyp, @gp, -10, im, par{:});         % optimise

[unwrap2vec(hyp), unwrap2vec(hyp_plain), unwrap2vec(hyp_p1), unwrap2vec(hyp_pN)]


function [d, dict, idict] = seg2dmat(im, outformat, res)
% SEG2DMAT  Local neighbourhood distance matrix between segmentation voxels
%
% D = SEG2DMAT(IM)
%
%   D is a sparse matrix where D(i,j) gives the distance between the i-th
%   and j-th voxels in the binary segmentation IM.
%
%   The index values i, j are computed with SUB2IND(), in the usual Matlab
%   way if you reshape IM into a vector, IM(:).
%
%   A 26-neighbourhood is assumed. That is, a voxel is only connected to
%   the 26 voxels that form a cube around it. That's why the sparse matrix
%   representation is convenient.
%
%   This function is fully vectorized and is thus very fast.
%
%   This function can be used with 2D images instead of 3D volumes too,
%   although some of the intermediate steps are not as efficient
%   memory-wise as they could be.
%
% [D, DICT, IDICT] = SEG2DMAT(IM, OUTFORMAT)
%
%   OUTFORMAT is a string. By default, D(i,j) gives the distance between
%   voxels i and j in IM. However, for large image volumes, it can be more
%   convenient to compute a smaller D where D(m, n) is the distance between
%   the m-th and n-th voxels of the segmentation:
%
%     'im' (default): Indices correspond to the whole image volume.
%
%     'seg': Indices correspond only to the segmentation (smaller matrix).
%
%   DICT and IDICT are column vectors used to convert to whole image and
%   segmentation indices. If OUTFORMAT='im', then i==j, so there's no need
%   for conversion and DICT and IDICT are returned empty.
%
%     DICT(i) is the matrix index for voxel i in the image
%     IDICT(i) is the image index for matrix index i.
%
% ... = SEG2DMAT(IM, OUTFORMAT, RES)
%
%    RES is a 3-vector with the voxel size given as [row, col, slice]. By
%    default, RES=[1 1 1].
%
% See also: im2imat.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2011 University of Oxford
% Version: 0.2.1
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
error(nargchk(1, 3, nargin, 'struct'));
error(nargoutchk(0, 3, nargout, 'struct'));

% defaults
if (nargin < 2 || isempty(outformat))
    outformat = 'im';
end
if (nargin < 3 || isempty(res))
    res = [1 1 1];
end

% total number of voxels in the image
N = numel(im);

% image size
sz = size(im);
if (length(sz) == 2)
    sz(3) = 1;
end

% get linear indices of segmented voxels
idx0 = find(im);

% create dictionary vector that will allow us to convert between indices
% referred to the whole image, and indices of segmented voxels
%
% note that if you do dict(i) == 0 means that voxel i is not segmented
% dict = zeros(numel(im),1);
% dict(idx0) = (1:length(idx0))';
dict = sparse(idx0, ones(length(idx0), 1), (1:length(idx0)));

% convert to r, c, s indices
idx = idx0;
[r, c, s] = ind2sub(sz, idx);

% compute neighbourhood with connectivity 26 around origin
[gr, gc, gs] = ndgrid(-1:1, -1:1, -1:1);

% compute distances from each point to the origin
dlocal = sqrt((res(1)*gr).^2 + (res(2)*gc).^2 + (res(3)*gs).^2);

% convert volume of distances and coordinates into vectors
dlocal = dlocal(:);
gr = gr(:);
gc = gc(:);
gs = gs(:);

% each row has the coordinates of a neighbourhood around a segmented voxel
r = repmat(gr', length(idx), 1) + repmat(r, 1, 27);
c = repmat(gc', length(idx), 1) + repmat(c, 1, 27);
s = repmat(gs', length(idx), 1) + repmat(s, 1, 27);

% convert out of range subscripts into NaNs, so that they are not
% considered later when computing distances
r(r < 1 | r > sz(1)) = nan;
c(c < 1 | c > sz(2)) = nan;
s(s < 1 | s > sz(3)) = nan;

% reduce the r, c, s matrices to a single matrix with linear indices for
% the neighbours. Now, we have vector idx and matrix nn. For example:
% idx(5) = 34864
% nn(5,:) = [NaN NaN NaN NaN NaN NaN NaN NaN NaN 34395 34396 34397 ...
%            34863 34864 34865 35331 35332 35333 108807 108808 ...
%            108809 109275 109276 109277 109743 109744 109745]
% means that voxel 34864 has only 18 neighbours (voxel 34864 is in the
% first slice, so the neighbourhood is constrained that way)
nn = sub2ind(sz, r, c, s);

% we are going to remove the central column, because we don't need to
% connect a voxel to itself with distance 0
nn = nn(:, [1:13 15:end]);
dlocal = dlocal([1:13 15:end]);

% replicate local distance and index vectors so that it corresponds with
% the nn matrix
dlocal = repmat(dlocal', length(idx), 1);
idx = repmat(idx, 1, 26);

% find the out of range connections
ok = ~isnan(nn);

% remove them
idx = idx(ok);
nn = nn(ok);
dlocal = dlocal(ok);

% find indices larger than the largest one in the segmentation
ok = (nn <= max(idx0));

% remove them
idx = idx(ok);
nn = nn(ok);
dlocal = dlocal(ok);

% find connections to voxels that are not part of the segmentation
ok = (dict(nn) ~= 0);

% remove them
idx = idx(ok);
nn = nn(ok);
dlocal = dlocal(ok);

switch outformat
    case 'im'
        % create sparse matrix for distances between all voxels in the
        % image
        d = sparse(idx, nn, dlocal, N, N);

        % in this case, the i-th position in the matrix corresponds to the
        % i-th voxel in the image, so no dictionary is necessary
        dict = [];
        idict = [];
    case 'seg'
        % create sparse matrix for distances between all voxels in the
        % segmentation
        d = sparse(dict(idx), dict(nn), dlocal);

        % create dictionary to translate from image linear indices to
        % distance matrix indices. For example, distances to voxel im(3)
        % are dict(dict(3), :)
        dict = sparse(idx0, ones(length(idx0), 1), (1:length(idx0)));

        % compute inverse dictionary
        idict = find(dict);
    otherwise
        error('Unrecognized output format string')
end


function test_vbgppcamv_cs(vbpca, ppca)

rand('state', 6);
randn('state', 6);

if nargin < 1
  vbpca = false;
end
if nargin < 2
  ppca = false;
end

inW = 1:200;
inX = 1:100;

M = length(inW);
N = length(inX);
D = 2;
X = zeros(D,N);
W = zeros(M,D);

% Covariance matrices
logthetaW = log(3);
logthetaX = log(3);

% Covariance functions
gpcovW = {@gpcovScale, @gpcovPP}; logthetaW = [3; logthetaW(:)];
%gpcovW = {@gpcovConstScale, 10, @gpcov};
%gpcovW = {@gpcov};
%gpcovX = {@gpcovScale, @gpcov}; logthetaX = [3; logthetaX(:)]
gpcovX = @gpcovPP;

% Generate latent variables
for d=1:D
  lsW = 10*ceil(exp(logthetaW(end)));
  lsX = 1*ceil(exp(logthetaX(end)));
  W(:,d) = filter(hamming(lsW), 1, randn(M,1));
  X(d,:) = filter(hamming(lsX), 1, randn(N,1));
  covfunW{d} = gpcovW;
  covfunX{d} = gpcovX;
  initthetaW{d} = logthetaW;
  initthetaX{d} = logthetaX;
end

W = W * diag( sqrt(M./rowsum(W.^2)) );
X = diag( sqrt(N./colsum(X.^2)) ) * X;

% $$$ tsplot(X)
% $$$ tsplot(W')
% $$$ return

% Generate data
Y = W*X;

% Noise level
s2 = 0.1;

% Make noisy observations
Yn = Y + sqrt(s2)*randn(M,N);  % noise
Ynm = Yn; Ynm(rand(M,N)<0.5) = nan; % missing values
Ytest = Yn; Ytest(~isnan(Ynm)) = nan;

% Learn GP PCA
maxiter = 1e1;
Q = vbgppcamv(Ynm,D,inW,inX,covfunW,initthetaW,covfunX,initthetaX, ...
              'maxiter',maxiter, 'pseudodensityx', 1, 'pseudodensityw', ...
              1, 'loglikelihood', true, 'updatehyper', 5, 'maxsearchx', ...
              3, 'maxsearchw', 3, 'updatepseudow', true, 'updatepseudox', ...
              true, 'checkgradx', true, 'checkgradw', false);

est_noise = 1/Q.tau
real_noise = s2

% Learn other models
if ppca
  Qppca = pca_full(Ynm,D,'maxiters',maxiter,'rotate2pca',true, ...
                   'algorithm','ppca');
  Yh_ppca = Qppca.A * Qppca.S + repmat(Qppca.Mu,1,N);
  testrmse_ppca = rmse(Ytest, Yh_ppca)
  noiselessrmse_ppca = rmse(Y, Yh_ppca)
end
if vbpca
  Qvbpca = vbpcamv(Ynm,M-1,'maxiters',maxiter);
  Yh_vbpca = Qvbpca.W * Qvbpca.X + repmat(Qvbpca.mu,1,N);
  testrmse_vbpca = rmse(Ytest, Yh_vbpca)
  noiselessrmse_vbpca = rmse(Y, Yh_vbpca)
end

Yh_gppca = Q.W * Q.X;
testrmse_gppca = rmse(Ytest, Yh_gppca)
noiselessrmse_gppca = rmse(Y, Yh_gppca)
figure
subplot(5,1,1);
plot(Y');
title('Original noiseless data')
subplot(5,1,2);
plot(Ynm');
title('Observed data')
subplot(5,1,3);
plot(Yh_gppca');
title('Reconstruction of GP VB PCA')
if vbpca
  subplot(5,1,4);
  plot(Yh_vbpca');
  title('Reconstruction of VB PCA')
end
if ppca
  subplot(5,1,5);
  plot(Yh_ppca');
  title('Reconstruction of PPCA')
end

vX = Q.varX;%diag(Q.CovX);
eX = 2*sqrt(vX);%sqrt( vX(reshape(1:(N*D), D, N)) );
vW = Q.varW;%diag(Q.CovW);
eW = 2*sqrt(vW);%sqrt( vW(reshape(1:(M*D), M, D)) );
tsgpplot(inX, Q.X', eX', 'pseudoinputs', {Q.pseudoX});
tsgpplot(inW, Q.W, eW, 'pseudoinputs', {Q.pseudoW});

thetaX1 = exp(Q.logthetaX{1})
thetaX2 = exp(Q.logthetaX{2})

% $$$ exp(Q.logthetaW{1})
% $$$ exp(Q.logthetaW{2})
% $$$
% $$$ exp(Q.logthetaX{1})
% $$$ exp(Q.logthetaX{2})

% $$$ % DEBUG: (test the effect of CovXp)
% $$$ X = zeros(D,N);
% $$$ varX = zeros(D,N);
% $$$ for d=1:D
% $$$   [X(d,:), varX(d,:)] = gppred(Q.pseudoX{d}, Q.Xp{d}, 0, inX, covfunX{d}, ...
% $$$                                Q.logthetaX{d});
% $$$ end
% $$$ varX;
% $$$ tsgpplot(inX, X', 2*sqrt(varX'), 'pseudoinputs', {Q.pseudoX});
% $$$ exp(Q.logthetaX{1})

% $$$ figure
% $$$ pcolor(Q.CovXp{1});
% $$$ figure
% $$$ pcolor(Q.CovWp{1});

return

%inv(funcK_noiseless(D,[10;10;1;10;10]));return
[p, loglike] = gplearn(ymv, @(p) funcK_noisy(D,p), [10;10;1;300;300;0.1])
%[p, loglike] = gplearn(ymv, @(p) funcK_noisy(D,p), [300;300;10;1;1])
%[p, loglike] = gplearn(ymv, @(p) funcK_noisy(D,p), [300;300;10;5;5;10;0.1])
%[p, loglike] = gplearn(ymv, @(p) funcK_noisy(D,p), [1000;100;0.1;3;100;0.059;1;0.1])
%[p, loglike] = gplearn(ymv, @(p) funcK_noisy(D,p), [100;100;3;100;0.059;1;0.1])

%p(1:4) = [2000;30;0.1;10]
%p(3) = 0.1;
[mu,Cov] = gppred(th,tmv,ymv, @(x1,x2) funcK_noiseless(gpdist(x1,x2),p(1:(end-1))), p(end));
yh = mnorm_rnd(mu, Cov, 10);

figure
clf
plot(th, yh, 'r')
figure
clf
gpplot(th,mu,Cov);
hold on
plot(tmv,ymv,'k+')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function gpmapplot(coord, y)
x = coord';
% Use haversine distance measure
mydist = @(x1,x2) gpdist(x1,x2,@dist_coord);
D = mydist(x,x);
funcK = @(p) gpK(@() gpK_ratquad(D,p(1),p(2),p(3)), ...
                 @() gpK_noise(length(D),p(4)));
[p, loglike] = gplearn(y, funcK, [10;5;1;1e-5])
funcK_noiseless = @(D) gpK(@() gpK_ratquad(D, p(1),p(2),p(3)));
[LONI, LATI] = get_grid(40, 40);
xh = [LONI(:)';LATI(:)'];
[mu,Cov] = gppred(xh,x,y, @(x1,x2) funcK_noiseless(mydist(x1,x2)), p(4));
yh = mu;
ZI = reshape(yh, size(LONI));
plot_map
hold on
m_pcolor(LONI, LATI, ZI);

% Set nice colormap
colormap(climcolmap);
shading flat;
lim = max( -min(yh), max(yh) );
set(gca, 'clim', [-lim lim]);

return

% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function D = gpdist(x1, x2, funcDist)
% $$$ if isvector(x1)
% $$$   x1 = x1(:)';
% $$$   x2 = x2(:)';
% $$$ end
% $$$ if nargin < 3
% $$$   funcDist = @(z1,z2) abs(z1-z2);
% $$$ end
% $$$ %[X2,X1] = meshgrid(x2,x1);
% $$$ n1 = size(x1,2);
% $$$ n2 = size(x2,2);
% $$$ D = zeros(n1,n2);
% $$$ for i=1:n1
% $$$   for j=1:n2
% $$$     D(i,j) = funcDist(x1(:,i),x2(:,j));
% $$$   end
% $$$ end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function d = dist_coord(coord1, coord2, varargin)
% d = dist(X1, X2)
% returns geographical distance in kilometers
% input vectors must be the same size and shape!!
% Approximates earth as a sphere.
% Distance is calculated for

if nargin > 2
  nargin
end

% Quadratic mean radius from Wikipedia
R_avg = 6372.795477598;

% Convert to radians
q = pi / 180;
lon1 = coord1(1,:) * q;
lat1 = coord1(2,:) * q;
lon2 = coord2(1,:) * q;
lat2 = coord2(2,:) * q;

% Distance calculation (haversine law)
dlat = lat2 - lat1;
dlon = lon2 - lon1;
a = sin(dlat/2).^2 + cos(lat1).*cos(lat2).*(sin(dlon/2).^2);
c = 2 * atan2(sqrt(a), sqrt(1-a));
d = R_avg * c;

return

% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [p,loglike] = gplearn(y, funcK, init_p)
% $$$ opts = optimset('GradObj', 'on');
% $$$ [p, negloglike] = fminunc(@(p) cost(y, funcK, p), init_p, opts);
% $$$ loglike = -negloglike;
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [mu,Cov] = gppred(xh, x, y, funcK, noise)
% $$$ if nargin == 1
% $$$   x = [];
% $$$   y = [];
% $$$ end
% $$$
% $$$ Kxhx = funcK(xh,x);
% $$$ invKxx = inv(funcK(x,x)+noise^2*eye(length(x)));
% $$$ Kxhxh = funcK(xh,xh);
% $$$ if isempty(Kxhx)
% $$$   Kxhx = 0;
% $$$ end
% $$$ if isempty(invKxx)
% $$$   invKxx = 0;
% $$$ end
% $$$ mu = Kxhx*invKxx*y;
% $$$ if isempty(mu)
% $$$   mu = zeros(size(xh));
% $$$ end
% $$$ Cov = Kxhxh - Kxhx*invKxx*Kxhx';

% $$$ % $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ % $$$ function gpplot(x, mu, Cov)
% $$$ % $$$ e = sqrt(diag(Cov));
% $$$ % $$$ X = [x(1:end); x(end:-1:1)];
% $$$ % $$$ Y = [mu(1:end)+e; mu(end:-1:1)-e(end:-1:1)];
% $$$ % $$$ C = [.65,.65,.65];
% $$$ % $$$ fill(X,Y,C,'EdgeColor',C);
% $$$ % $$$ hold on
% $$$ % $$$ plot(x,mu,'k');
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [K, dK] = gpK(varargin)
% $$$ K = 0;
% $$$ dK = [];
% $$$ for i=1:nargin
% $$$   if nargout == 1
% $$$     Knew = varargin{i}();
% $$$   else
% $$$     [Knew,dKnew] = varargin{i}();
% $$$     n = size(dKnew,3);
% $$$     if isempty(dK)
% $$$       dK = dKnew;
% $$$     else
% $$$       dK(:,:,end+(1:n)) = dKnew;
% $$$     end
% $$$   end
% $$$   K = K + Knew;
% $$$ end
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [K, dK] = gpK_noise(n, p1);
% $$$ K = p1^2 * eye(n);
% $$$ if nargout >= 2
% $$$   dK = K * 2/p1;
% $$$ end
% $$$
% $$$ % $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ % $$$ function [K, dK] = gpK_sqexp(D, p1, p2)
% $$$ % $$$ K = p1^2*exp(-0.5*D.^2/(p2^2));
% $$$ % $$$
% $$$ % $$$ if nargout >= 2
% $$$ % $$$   dK = zeros([size(D), 2]);
% $$$ % $$$   dK(:,:,1) = K .* 2 / p1;
% $$$ % $$$   dK(:,:,2) = K .* (-0.5*D.^2) .* (-2*p2^(-3));
% $$$ % $$$ end
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [K, dK] = gpK_decper(D, p1, p2, p3, p4)
% $$$ K = p1^2*exp(-0.5*(D.^2)/(p2^2)-2*sin(pi*D*p3).^2/(p4^2));
% $$$
% $$$ if nargout >= 2
% $$$   dK = zeros([size(D),4]);
% $$$   dK(:,:,1) = K .* 2 / p1;
% $$$   dK(:,:,2) = K .* (-0.5*D.^2) .* (-2*p2^(-3));
% $$$   dK(:,:,3) = K .* (-2*sin(pi*D*p3)*2) .* cos(pi*D*p3) .* (pi*D);
% $$$   dK(:,:,4) = K .* (-2*sin(pi*D*p3).^2) * (-2)*p4^(-3);
% $$$ end
% $$$
% $$$ % $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ % $$$ function [K,dK] = gpK_ratquad(D, p1, p2, p3)
% $$$ % $$$ %%% p3 = p3^2;
% $$$ % $$$ f = 1 + D.^2/(2*p3^2*p2^2);
% $$$ % $$$ K = p1^2*f.^(-p3^2);
% $$$ % $$$
% $$$ % $$$ if nargout >= 2
% $$$ % $$$   dK = zeros([size(D),3]);
% $$$ % $$$   dK(:,:,1) = K .* 2 / p1;
% $$$ % $$$   dK(:,:,2) = K .* (-p3^2).*f.^(-1) .* (-2).*D.^2/(2*p3^2)*p2^(-3);
% $$$ % $$$   dK(:,:,3) = K .* (-2*p3*log(f) + (-p3^2)./f.*D.^2/(2*p2^2) * (-2) * p3^(-3));
% $$$ % $$$ end
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [f, df] = cost(y,funcK,p)
% $$$ [K,dK] = funcK(p);
% $$$ [f,df] = loglikelihood(y,K,dK);
% $$$ f = -f;
% $$$ df = -df;
% $$$
% $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ function [loglike, dloglike] = loglikelihood(y,K,dK)
% $$$ invK = inv(K);
% $$$ logdetK = log(det(K));
% $$$ if logdetK < -1e100;
% $$$   logdetK = -1e100;
% $$$ end
% $$$ n = length(y);
% $$$
% $$$ loglike = -0.5*y'*invK*y - 0.5*logdetK - 0.5*n*log(2*pi);
% $$$
% $$$ if nargout >= 2
% $$$   m = size(dK,3);
% $$$   dloglike = zeros(m,1);
% $$$   a = invK * y;
% $$$   W = a*a' - invK;
% $$$   for i = 1:m
% $$$     dloglike(i) = 0.5*sum(sum(W.*dK(:,:,i)));
% $$$   end
% $$$ end
% $$$
% $$$
% $$$ % $$$ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $$$ % $$$ function y = mnorm_rnd(mu, Cov, n)
% $$$ % $$$ m = max(length(mu), length(Cov));
% $$$ % $$$
% $$$ % $$$ opts.issym = true;
% $$$ % $$$ opts.isreal = true;
% $$$ % $$$ [V,D] = svd(Cov);
% $$$ % $$$ D(D<0) = 0;
% $$$ % $$$ D = sqrt(D);
% $$$ % $$$ A = V * D;
% $$$ % $$$
% $$$ % $$$ y = repmat(mu,1,n) + A*randn(m,n);