% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

%%begin

% In the field of robotics there are many possible ways of representing
% positions and orientations, but the homogeneous transformation is well
% matched to MATLABs powerful tools for matrix manipulation.
%
% Homogeneous transformations describe the relationships between Cartesian
% coordinate frames in terms of translation and orientation.

%  A pure translation of 0.5m in the X direction is represented by

transl(0.5, 0.0, 0.0)

% a rotation of 90degrees about the Y axis by

troty(pi/2)

% and a rotation of -90degrees about the Z axis by

trotz(-pi/2)

%  these may be concatenated by multiplication

t = transl(0.5, 0.0, 0.0) * troty(pi/2) * trotz(-pi/2)

%
% If this transformation represented the origin of a new coordinate frame with respect
% to the world frame origin (0, 0, 0), that new origin would be given by

t * [0 0 0 1]'

% the orientation of the new coordinate frame may be expressed in terms of
% Euler angles

tr2eul(t)

% or roll/pitch/yaw angles

tr2rpy(t)

% It is important to note that tranform multiplication is in general not
% commutative as shown by the following example

trotx(pi/2) * trotz(-pi/8)
trotz(-pi/8) * trotx(pi/2)


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 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 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


%% 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 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


%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']);


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 ;


%% 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 / %');


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 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 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


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_null_left ( )

%*****************************************************************************80
%
%% TEST_NULL_LEFT tests left null vectors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 March 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST_NULL_LEFT\n' );
  fprintf ( 1, '  A = a test matrix of order M by N;\n' );
  fprintf ( 1, '  x = an M vector, candidate for a left null vector.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ||A|| = Frobenius norm of A.\n' );
  fprintf ( 1, '  ||x|| = L2 norm of x.\n' );
  fprintf ( 1, '  ||A''*x||/||x|| = L2 norm of A''*x over L2 norm of x.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Title                    M     N      ' );
  fprintf ( 1, '||A||            ||x||        ||A''*x||/||x||\n' );
  fprintf ( 1, '\n' );
%
%  A123
%
  title = 'A123';
  m = 3;
  n = 3;
  a = a123 ( );
  x = a123_null_left ( );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  CHEBY_DIFF1
%
  title = 'CHEBY_DIFF1';
  m = 5;
  n = m;
  a = cheby_diff1 ( n );
  x = cheby_diff1_null_left ( m, n );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  CREATION
%
  title = 'CREATION';
  m = 5;
  n = m;
  a = creation ( m, n );
  x = creation_null_left ( m, n );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  DIF1
%  Only has null vectors for M odd.
%
  title = 'DIF1';
  m = 5;
  n = m;
  a = dif1 ( m, n );
  x = dif1_null_left ( m, n );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  DIF1CYCLIC
%
  title = 'DIF1CYCLIC';
  m = 5;
  n = m;
  a = dif1cyclic ( n );
  x = dif1cyclic_null_left ( m, n );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  DIF2CYCLIC
%
  title = 'DIF2CYCLIC';
  m = 5;
  n = m;
  a = dif2cyclic ( n );
  x = dif2cyclic_null_left ( m, n );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  EBERLEIN
%
  title = 'EBERLEIN';
  m = 5;
  n = 5;
  r8_lo = -5.0;
  r8_hi = +5.0;
  seed = 123456789;
  [ alpha, seed ] = r8_uniform_ab ( r8_lo, r8_hi, seed );
  a = eberlein ( alpha, n );
  x = eberlein_null_left ( m, n );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  FIBONACCI1
%
  title = 'FIBONACCI1';
  m = 5;
  n = m;
  r8_lo = -5.0;
  r8_hi = +5.0;
  seed = 123456789;
  [ f1, seed ] = r8_uniform_ab ( r8_lo, r8_hi, seed );
  [ f2, seed ] = r8_uniform_ab ( r8_lo, r8_hi, seed );
  a = fibonacci1 ( n, f1, f2 );
  x = fibonacci1_null_left ( m, n, f1, f2 );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  LAUCHLI
%
  title = 'LAUCHLI';
  m = 6;
  n = m - 1;
  r8_lo = -5.0;
  r8_hi = +5.0;
  seed = 123456789;
 [ alpha, seed ] = r8_uniform_ab ( r8_lo, r8_hi, seed );
  a = lauchli ( alpha, m, n );
  x = lauchli_null_left ( alpha, m, n );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  LINE_ADJ
%
  title = 'LINE_ADJ';
  m = 7;
  n = m;
  a = line_adj ( n );
  x = line_adj_null_left ( m, n );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  MOLER2
%
  title = 'MOLER2';
  m = 5;
  n = 5;
  a = moler2 ( );
  x = moler2_null_left ( );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  ONE
%
  title = 'ONE';
  m = 5;
  n = 5;
  a = one ( m, n );
  x = one_null_left ( m, n );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  RING_ADJ
%  N must be a multiple of 4 for there to be a null vector.
%
  title = 'RING_ADJ';
  m = 12;
  n = 12;
  a = ring_adj ( n );
  x = ring_adj_null_left ( m, n );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  ROSSER1
%
  title = 'ROSSER1';
  m = 8;
  n = 8;
  a = rosser1 ( );
  x = rosser1_null_left ( );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );
%
%  ZERO
%
  title = 'ZERO';
  m = 5;
  n = 5;
  a = zero ( m, n );
  x = zero_null_left ( m, n );
  error_l2 = r8mat_is_null_left ( m, n, a, x );
  norm_a_frobenius = r8mat_norm_fro ( m, n, a );
  norm_x_l2 = r8vec_norm_l2 ( m, x );
  fprintf ( 1, '  %-20s  %4d  %4d  %14g  %14g  %10.2g\n', ...
    title, m, n, norm_a_frobenius, norm_x_l2, error_l2 );

  return
end


function [R,T] = slabVibro(f, rho, c, e)
% Copyright (c) 2018-2019, Marc Bakry, Ecole Polytechnique
% GNU General Public License v3.0.
% Computation of the coeff. of reflection / transmission for an acaustic wave
% crossing three environments with the parameters rho_ {1, 2, 3} and
% c_ {1,2, 3} for a set of frequencies f
% e.g. ~/nonRegressionTest/vibroAcoustic/transmission_1d_calcul
% To be commented...
assert(length(rho) == 3 && size(c, 1) == length(f) && size(c, 2) == 3)
omega = 2*pi*f;
R = zeros(size(f)); T = zeros(size(f));
muk = (ones(length(f), 1)*rho) .* c;
for i=1:length(f)
    tmp = muk(i, :)*omega(i);
    k   = omega(i)./c(i, :);
    b   = [-1; tmp(1); 0; 0];
    A   = [1 -1 -1 0; ...
        tmp(1) -tmp(2) tmp(2) 0; ...
        0 exp(-1i*k(2)*e) exp(1i*k(2)*e) -exp(1i*k(3)*e); ...
        0 tmp(2)*exp(-1i*k(2)*e) ...
        -tmp(2)*exp(1i*k(2)*e) tmp(3)*exp(1i*k(3)*e)];
    x = A\b;
    R(i) = x(1);
    T(i) = x(4);
end
end


function L = log_marg_prob_node(CPD, self_ev, pev)
% LOG_MARG_PROB_NODE Compute prod_m log P(x(i,m)| x(pi_i,m)) for node i (linear_gaussian)
% L = log_marg_prob_node(CPD, self_ev, pev)
%
% This differs from log_prob_node because we integrate out the parameters.
% self_ev{m} is the evidence on this node in case m.
% pev{i,m} is the evidence on the i'th parent in case m
% We assume there is <= 1 case.

ncases = length(self_ev);

if ncases==0
  L = 0;
  return;
elseif ncases==1
  y = self_ev{1};
  x = cat(1, pev{:}); % column vector
  f = 1-x'*inv(x*x' + CPD.prior.n)*x;
  alpha = CPD.prior.alpha;
  L = log_student_pdf(y, x'*CPD.prior.theta, f*alpha/CPD.prior.beta, 2*alpha);
else
  error('can''t handle batch data');
end


%EXsparsity  Example script, deblurring without and with sparsity

% Silvia Gazzola, University of Bath
% Per Christian Hansen, Technical University of Denmark
% James G. Nagy, Emory University
% April, 2018.

% clear workspace and window.
clear, clc

% Choose if you would like to see the results displayed in a single figure
% window ('subplots') or in multiple figure windows ('manyplots').
% dispres = 'subplots';
dispres = 'manyplots';

rng(0) % make sure this test is repeatable

% Test problem: Gaussian deblurring of a sparse 'dotk' image (starry night).
n = 256;                        % Image is n-by-n.
NoiseLevel = 0.10;              % Relative noise level in data.
PRoptions.trueImage = 'dotk';   % Sparse test image.
[A, b, x, ProbInfo] = PRblur(n,PRoptions);
bn = PRnoise(b,'gauss',NoiseLevel);

% Compute the CGLS reconstruction.
options = IRset('MaxIter', 80, 'x_true', x, 'NoStop', 'on');
[Xcgls, info_cgls] = IRcgls(A, bn, options);

% Compute ell1 sparse reconstructions with default (GCV) inner stopping rule.
[Xell1_GCV, info_ell1_GCV] = IRell1(A, bn, options);

% Compute ell1 sparse reconstruction with DP as inner stopping rule.
options = IRset(options, 'RegParam', 'discrep', 'NoiseLevel', NoiseLevel,...
                'eta', 1.1);
[Xell1_DP, info_ell1_DP] = IRell1(A, bn, options);

options.NoStopOut = 'on';
K = 80;
[Xirn_DP, info_irn_DP] = IRirn(A, bn, K, options);

% Display the reconstructions;
% uncomment as appropriate to avoid displaying titles and legends.
if strcmp(dispres, 'subplots')
    figure(1), clf
    subplot(2,3,1)
    imagesc(reshape(x,n,n)), axis image off
    title('Exact image','fontsize',16,'interpreter','latex')
    subplot(2,3,2)
    imagesc(reshape(b,n,n)), axis image off
    title('Blurred image','fontsize',16,'interpreter','latex')
    subplot(2,3,3)
    imagesc(reshape(max(0,info_cgls.BestReg.X),n,n)), axis image off
    title(['Best IRcgls after ',num2str(info_cgls.BestReg.It),' iterations'],'fontsize',16,'interpreter','latex')
    subplot(2,3,4)
    imagesc(reshape(max(0,info_ell1_GCV.BestReg.X),n,n)), axis image off
    title(['Best IRell1 w/ GCV after ',num2str(info_ell1_GCV.BestReg.It),...
          ' iterations'],'fontsize',16,'interpreter','latex')
    subplot(2,3,5)
    imagesc(reshape(max(0,info_ell1_DP.BestReg.X),n,n)), axis image off
    title(['Best IRell1 w/ DP ',num2str(info_ell1_DP.BestReg.It),...
          ' iterations'],'fontsize',16,'interpreter','latex')
    subplot(2,3,6)
    imagesc(reshape(max(0,info_irn_DP.BestReg.X),n,n)), axis image off
    title(['Best IRirn w/ DP after', num2str(info_irn_DP.BestReg.It),...
         ' iterations'],'fontsize',16,'interpreter','latex')
elseif strcmp(dispres, 'manyplots')
    figure(1), clf
    XX = reshape(x,n,n);
    imagesc(XX), axis image off
    title('Exact image','fontsize',16,'interpreter','latex')
    axes('units','normalized','position',[0.17 0.1 1.15 0.25]);
    imagesc(XX(227:256,227:256)), axis image, caxis([0 max(XX(:))])
    set(gca,'Xtick',[],'Ytick',[],'LineWidth',2,'Xcolor','red','Ycolor','red')
    %
    figure(2), clf
    BB = reshape((max(b,0)),n,n);
    imagesc(BB), axis image off
    title('Blurred image','fontsize',16,'interpreter','latex')
    axes('units','normalized','position',[0.17 0.1 1.15 0.25]);
    imagesc(BB(227:256,227:256)), axis image, caxis([0 max(BB(:))])
    set(gca,'Xtick',[],'Ytick',[],'LineWidth',2,'Xcolor','red','Ycolor','red')
    %
    figure(3), clf
    XCG = reshape((max(0,info_cgls.BestReg.X)),n,n);
    imagesc(XCG), axis image off
    title(['Best IRcgls after ',num2str(info_cgls.BestReg.It),' iterations'],'fontsize',16,'interpreter','latex')
    axes('units','normalized','position',[0.17 0.1 1.15 0.25]);
    imagesc(XCG(227:256,227:256)), axis image, caxis([0 max(XCG(:))])
    set(gca,'Xtick',[],'Ytick',[],'LineWidth',2,'Xcolor','red','Ycolor','red')
    %
    figure(4), clf
    I = reshape((max(0,info_ell1_DP.BestReg.X)),n,n);
    imagesc(I), axis image off
    title(['Best IRell1 w/ GCV after ',num2str(info_ell1_GCV.BestReg.It),...
          ' iterations'],'fontsize',16,'interpreter','latex')
    axes('units','normalized','position',[0.17 0.1 1.15 0.25]);
    imagesc(I(227:256,227:256)), axis image, caxis([0 max(I(:))])
    set(gca,'Xtick',[],'Ytick',[],'LineWidth',2,'Xcolor','red','Ycolor','red')
    %
    figure(5), clf
    II = reshape((max(0,info_ell1_DP.BestReg.X)),n,n);
    imagesc(II), axis image off
    title(['Best IRell1 w/ DP after ',num2str(info_ell1_DP.BestReg.It),...
          ' iterations'],'fontsize',16,'interpreter','latex')
    axes('units','normalized','position',[0.17 0.1 1.15 0.25]);
    imagesc(II(227:256,227:256)), axis image, caxis([0 max(II(:))])
    set(gca,'Xtick',[],'Ytick',[],'LineWidth',2,'Xcolor','red','Ycolor','red')
    %
    figure(6), clf
    III = reshape((max(0,info_irn_DP.BestReg.X)),n,n);
    imagesc(III), axis image off
    title(['Best IRirn w/ DP after ',num2str(info_irn_DP.BestReg.It),...
          ' iterations'],'fontsize',16,'interpreter','latex')
    axes('units','normalized','position',[0.17 0.1 1.15 0.25]);
    imagesc(III(227:256,227:256)), axis image, caxis([0 max(III(:))])
    set(gca,'Xtick',[],'Ytick',[],'LineWidth',2,'Xcolor','red','Ycolor','red')
end

return

% A number of instructions useful to save the displayed figures follow;
% the defualt is not to execute them. If you wish to save the displayed
% figures in the dedicated 'Results' folder, please comment the above
% return statement
oldcd = cd;
if strcmp(dispres, 'subplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    figure(1), print -dpng -r300 EXsparsity
elseif strcmp(dispres, 'manyplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    figure(1), print -depsc -r300 EXsparsity_a
    figure(2), print -depsc -r300 EXsparsity_b
    figure(3), print -depsc -r300 EXsparsity_c
    figure(4), print -depsc -r300 EXsparsity_d
    figure(5), print -depsc -r300 EXsparsity_e
    figure(6), print -depsc -r300 EXsparsity_f
end
cd(oldcd)

% Uncomment the following return statement if you wish to save the
% displayed figures as MATLAB figures

% return

oldcd = cd;
if strcmp(dispres, 'subplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    figure(1), saveas('EXsparsity.fig')
elseif strcmp(dispres, 'manyplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    saveas(figure(1), 'EXsparsity_a.fig')
    saveas(figure(2), 'EXsparsity_b.fig')
    saveas(figure(3), 'EXsparsity_c.fig')
    saveas(figure(4), 'EXsparsity_d.fig')
    saveas(figure(5), 'EXsparsity_e.fig')
    saveas(figure(6), 'EXsparsity_f.fig')
end
cd(oldcd)


function [dlnPart, m] = gradLnDiffErfs(x1, x2, fact1, fact2),

% GRADLNDIFFERFS Compute the gradient of the log difference of two erfs.
%
%
% COPYRIGHT : Antti Honkela, 2007

% NDLUTIL

m = min(x1.^2, x2.^2);
dlnPart = 2/sqrt(pi) * (exp(-x1.^2 + m) .* fact1 ...
			- exp(-x2.^2 + m) .* fact2);


function [f, g] = obj_infometric(L, sX, sY, tX, lambda)

% Input:
% L: D*d transformation matrix
% sX: instance matrix for the source domain data. size: N*D
% sY: label vector for the source domain data. size: N*1
% tX: instance matrix for the target domain data. size: M*D
% lambda: regularization parameter
%
% Output:
% f: objective function value
% g: gradient on L
%
% yuanshi@usc.edu
% 2013/9/5
%

% mutual info in the target domain
% do "discriminative clustering" on the target domain
[mi, grad] = compute_mutual_infoL(L, sX, sY, sX);

% mutual info in both domains
% make it hard to distinguish the two domains
all_data = [sX; tX];
all_label = [ones( size(sX,1), 1); zeros( size(tX,1), 1)];  % domain label

if lambda > 0
    [mi2, grad2] = compute_mutual_infoL(L, all_data, all_label, all_data);

    f = -(mi - lambda*mi2);
    g = -(grad - lambda*grad2);
else
    f = -mi;
    g = -grad;
end


function point = intersectEdges(edge1, edge2, varargin)
%INTERSECTEDGES Return all intersections between two set of edges.
%
%   P = intersectEdges(E1, E2);
%   returns the intersection point of edges E1 and E2.
%   E1 and E2 are 1-by-4 arrays, containing parametric representation of
%   each edge (in the form [x1 y1 x2 y2], see 'createEdge' for details).
%
%   In case of colinear edges, the result P contains [Inf Inf].
%   In case of parallel but not colinear edges, the result P contains
%   [NaN NaN].
%
%   If each input is N-by-4 array, the result is a N-by-2 array containing
%   the intersection of each couple of edges.
%   If one of the input has N rows and the other 1 row, the result is a
%   N-by-2 array.
%
%   P = intersectEdges(E1, E2, TOL);
%   Specifies a tolerance parameter to determine parallel and colinear
%   edges, and if a point belongs to an edge or not. The latter test is
%   performed on the relative position of the intersection point over the
%   edge, that should lie within [-TOL; 1+TOL].
%
%   See also
%   edges2d, intersectLines
%

% ------
% Author: David Legland
% E-mail: david.legland@nantes.inra.fr
% Created: 2003-10-31
% Copyright 2003-2022 INRA - Cepia Software Platform

% tolerance for precision
tol = 1e-14;

if nargin > 2
    tol = varargin{1};
end

%% Initialisations

% ensure input arrays are same size
N1  = size(edge1, 1);
N2  = size(edge2, 1);

% ensure input have same size
if N1 ~= N2
    if N1 == 1
        edge1 = repmat(edge1, [N2 1]);
        N1 = N2;
    elseif N2 == 1
        edge2 = repmat(edge2, [N1 1]);
    end
end

% initialize result array
x0  = zeros(N1, 1);
y0  = zeros(N1, 1);

%% Detect parallel and colinear cases

% indices of parallel edges
%par = abs(dx1.*dy2 - dx1.*dy2)<tol;
par = isParallel(edge1(:,3:4)-edge1(:,1:2), edge2(:,3:4)-edge2(:,1:2));

% indices of colinear edges
% equivalent to: |(x2-x1)*dy1 - (y2-y1)*dx1| < eps
col = abs(  (edge2(:,1)-edge1(:,1)) .* (edge1(:,4)-edge1(:,2)) - ...
            (edge2(:,2)-edge1(:,2)) .* (edge1(:,3)-edge1(:,1)) ) < tol & par;

% Parallel edges have no intersection -> return [NaN NaN]
x0(par & ~col) = NaN;
y0(par & ~col) = NaN;

%% Process colinear edges

% colinear edges may have 0, 1 or infinite intersection
% Discrimnation based on position of edge2 vertices on edge1
if sum(col) > 0
    % array for storing results of colinear edges
    resCol = Inf * ones(size(col));

    % compute position of edge2 vertices wrt edge1
    t1 = edgePosition(edge2(col, 1:2), edge1(col, :));
    t2 = edgePosition(edge2(col, 3:4), edge1(col, :));

    % control location of vertices: we want t1<t2
    if t1 > t2
        tmp = t1;
        t1  = t2;
        t2  = tmp;
    end

    % edge totally before first vertex or totally after last vertex
    resCol(col(t2 < -tol))  = NaN;
    resCol(col(t1 > 1+tol)) = NaN;

    % set up result into point coordinate
    x0(col) = resCol(col);
    y0(col) = resCol(col);

    % touches on first point of first edge
    touch = col(abs(t2) < tol);
    x0(touch) = edge1(touch, 1);
    y0(touch) = edge1(touch, 2);

    % touches on second point of first edge
    touch = col(abs(t1-1) < tol);
    x0(touch) = edge1(touch, 3);
    y0(touch) = edge1(touch, 4);
end

%% Process non parallel cases

% process edges whose supporting lines intersect
i = find(~par);

% use a test to avoid process empty arrays
if sum(i) > 0
    % extract base parameters of supporting lines for non-parallel edges
    x1  = edge1(i,1);
    y1  = edge1(i,2);
    dx1 = edge1(i,3) - x1;
    dy1 = edge1(i,4) - y1;
    x2  = edge2(i,1);
    y2  = edge2(i,2);
    dx2 = edge2(i,3) - x2;
    dy2 = edge2(i,4) - y2;

    % compute intersection points of supporting lines
    delta = (dx2.*dy1 - dx1.*dy2);
    x0(i) = ((y2-y1).*dx1.*dx2 + x1.*dy1.*dx2 - x2.*dy2.*dx1) ./ delta;
    y0(i) = ((x2-x1).*dy1.*dy2 + y1.*dx1.*dy2 - y2.*dx2.*dy1) ./ -delta;

    % compute position of intersection points on each edge
    % t1 is position on edge1, t2 is position on edge2
    t1  = ((y0(i)-y1).*dy1 + (x0(i)-x1).*dx1) ./ (dx1.*dx1+dy1.*dy1);
    t2  = ((y0(i)-y2).*dy2 + (x0(i)-x2).*dx2) ./ (dx2.*dx2+dy2.*dy2);

    % check position of points on edges.
    % it should be comprised between 0 and 1 for both t1 and t2.
    % if not, the edges do not intersect
    out = t1<-tol | t1>1+tol | t2<-tol | t2>1+tol;
    x0(i(out)) = NaN;
    y0(i(out)) = NaN;
end

%% format output arguments

point = [x0 y0];


function e = calcError(pf,rec,varargin)
% RP and mean square error
%
% *calcError(pf,rec)* calculates reconstruction error between meassured
% intensities and the recalcuated ODF or between two meassured pole
% figures. It can be specified whether the RP
% error or the mean square error is calculated. The scaling coefficients
% are calculated by the function PoleFigure/calcNormalization
%
% Syntax
%   e = calcError(pf,pf2) % compares two different @PoleFigure with same @S2Grid
%   e = calcError(pf,rec) % compares @PoleFigure with the Recalculated @SO3Fun
%
% Input
%  pf,pf2 - @PoleFigure
%  rec    - @SO3Fun
%
% Output
%  e - error
%
% Flags
%  RP    - RP value (default) |pfmeas - pfcalc|./ pfcalc
%  l1    - l1 error           |pfmeas - pfcalc|
%  l2    - l2 error           |pfmeas - pfcalc|.^2
%
% See also
% SO3Fun/calcError PoleFigure/calcNormalization PoleFigure/scale

argin_check(rec,{'SO3Fun','PoleFigure'});

% calc difference PoleFigure
errorpf = calcErrorPF(pf,rec,varargin{:});

% calc error
e = zeros(1,pf.numPF);
for i = 1:pf.numPF

  e(i) = mean(errorpf.allI{i}(:)); % RP error

  if check_option(varargin,'l1')
    e(i) = e(i)/mean(abs(pf.allI{i}(:))); % L^1 error
  elseif check_option(varargin,'l2')
    e(i) = e(i)/mean((pf.allI{i}(:)).^2); % L^2 error
  end
end

% TODO: implement a nice default output
%if nargout == 0
%  disp('TODO')
%  clear e;
%end


function fem2d_bvp_serene_test01 ( )

%*****************************************************************************80
%
%% FEM2D_BVP_SERENE_TEST01 carries out test case #1.
%
%  Discussion:
%
%    Use A1, C1, F1, EXACT1, EXACT_UX1, EXACT_UY1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 June 2014
%
%  Author:
%
%    John Burkardt
%
  nx = 5;
  ny = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM2D_BVP_SERENE_TEST01\n' );
  fprintf ( 1, '  Solve - del ( A del U ) + C U = F \n' );
  fprintf ( 1, '  on the unit square with zero boundary conditions.\n' );
  fprintf ( 1, '  A1(X,Y) = 1.0\n' );
  fprintf ( 1, '  C1(X,Y) = 0.0\n' );
  fprintf ( 1, '  F1(X,Y) = 2*X*(1-X)+2*Y*(1-Y).\n' );
  fprintf ( 1, '  U1(X,Y) = X * ( 1 - X ) * Y * ( 1 - Y )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The grid uses %d by %d nodes.\n', nx, ny );
  node_num = fem2d_bvp_serene_node_num ( nx, ny );
  fprintf ( 1, '  The number of nodes is %d\n', node_num );
%
%  Geometry definitions.
%
  x = linspace ( 0.0, 1.0, nx );
  y = linspace ( 0.0, 1.0, ny );

  show11 = 0;
  u = fem2d_bvp_serene ( nx, ny, @a1, @c1, @f1, x, y, show11 );

  if ( nx * ny <= 25 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '     I     J    X         Y         U         Uexact    Error\n' );
    fprintf ( 1, '\n' );

    k = 0;

    for j = 1 : ny

      if ( mod ( j, 2 ) == 1 )
        inc = 1;
      else
        inc = 2;
      end

      for i = 1 : inc : nx
        k = k + 1;
        uexact = exact1 ( x(i), y(j) );
        fprintf ( 1, '  %4d  %4d  %8f  %8f  %8f  %8f  %8e\n', ...
          i, j, x(i), y(j), u(k), uexact, abs ( u(k) - uexact ) );
      end
    end

  end

  e1 = fem2d_l1_error_serene ( nx, ny, x, y, u, @exact1 );
  e2 = fem2d_l2_error_serene ( nx, ny, x, y, u, @exact1 );
  h1s = fem2d_h1s_error_serene ( nx, ny, x, y, u, @exact_ux1, @exact_uy1 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  l1 error   = %g\n', e1 );
  fprintf ( 1, '  L2 error   = %g\n', e2 );
  fprintf ( 1, '  H1S error  = %g\n', h1s );

  return
end
function value = a1 ( x, y )

%*****************************************************************************80
%
%% A1 evaluates A function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of A(X).
%
  value = 1.0;

  return
end
function value = c1 ( x, y )

%*****************************************************************************80
%
%% C1 evaluates C function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of C(X).
%
  value = 0.0;

  return
end
function value = exact1 ( x, y )

%*****************************************************************************80
%
%% EXACT1 evaluates exact solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of the solution.
%
  value = x .* ( 1.0 - x ) .* y .* ( 1.0 - y );

  return
end
function value = exact_ux1 ( x, y )

%*****************************************************************************80
%
%% EXACT_UX1 evaluates the derivative dUdX of exact solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of dUdX.
%
  value = ( 1.0 - 2.0 * x ) .* ( y - y .* y );

  return
end
function value = exact_uy1 ( x, y )

%*****************************************************************************80
%
%% EXACT_UY1 evaluates the derivative dUdY of exact solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of dUdX.
%
  value = ( x - x .* x ) .* ( 1.0 - 2.0 * y );

  return
end
function value = f1 ( x, y )

%*****************************************************************************80
%
%% F1 evaluates right hand side function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of the right hand side.
%
  value = 2.0 * x .* ( 1.0 - x ) ...
        + 2.0 * y .* ( 1.0 - y );

  return
end


function [ suborder_xyz, suborder_w ] = tetrahedron_ncc_subrule ( rule, suborder_num )

%*****************************************************************************80
%
%% TETRAHEDRON_NCC_SUBRULE returns a compressed NCC rule.
%
%  Discussion:
%
%    In order for these compressed rules to be "unwrapped" correctly,
%    it's necessary that the values in SUBORDER_XYZ_N be listed
%    in a particular order for each kind of symmetry.  Basically,
%    the repeated equal values must come first.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Peter Silvester,
%    Symmetric Quadrature Formulae for Simplexes,
%    Mathematics of Computation,
%    Volume 24, Number 109, January 1970, pages 95-100.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Input, integer SUBORDER_NUM, the number of suborders of the rule.
%
%    Output, real SUBORDER_XYZ(3,SUBORDER_NUM),
%    the barycentric coordinates of the abscissas.
%
%    Output, real SUBORDER_W(SUBORDER_NUM), the suborder weights.
%
  if ( rule == 1 )

    suborder_xyz_n(1:4,1:suborder_num) = [ ...
      1,  1, 1, 1 ]';

    suborder_xyz_d = 4;

    suborder_w_n(1:suborder_num) = [ 1 ];

    suborder_w_d = 1;

  elseif ( rule == 2 )

    suborder_xyz_n(1:4,1:suborder_num) = [ ...
      0, 0, 0, 1 ]';

    suborder_xyz_d = 1;

    suborder_w_n(1:suborder_num) = [ 1 ];

    suborder_w_d = 4;

  elseif ( rule == 3 )

    suborder_xyz_n(1:4,1:suborder_num) = [ ...
      0, 0, 0, 2; ...
      1, 1, 0, 0 ]';

    suborder_xyz_d = 2;

    suborder_w_n(1:suborder_num) = [ -1, 4 ];

     suborder_w_d = 20;

  elseif ( rule == 4 )

    suborder_xyz_n(1:4,1:suborder_num) = [ ...
      0, 0, 0, 3; ...
      0, 0, 1, 2; ...
      1, 1, 1, 0 ]';

    suborder_xyz_d = 3;

    suborder_w_n(1:suborder_num) = [ 1, 0, 9 ];

     suborder_w_d = 40;

  elseif ( rule == 5 )

    suborder_xyz_n(1:4,1:suborder_num) = [ ...
      0, 0, 0, 4; ...
      0, 0, 3, 1; ...
      2, 2, 0, 0; ...
      1, 1, 0, 2; ...
      1, 1, 1, 1 ]';

    suborder_xyz_d = 4;

    suborder_w_n(1:suborder_num) = [ -5, 16, -12, 16, 128 ];

     suborder_w_d = 420;

  elseif ( rule == 6 )

    suborder_xyz_n(1:4,1:suborder_num) = [ ...
      0, 0, 0, 5; ...
      0, 0, 4, 1; ...
      0, 0, 3, 2; ...
      1, 1, 0, 3; ...
      2, 2, 1, 0; ...
      1, 1, 1, 2  ]';

    suborder_xyz_d = 5;

    suborder_w_n(1:suborder_num) = [ 33, -35, 35, 275, -75, 375 ];

    suborder_w_d = 4032;

  elseif ( rule == 7 )

    suborder_xyz_n(1:4,1:suborder_num) = [ ...
      0, 0, 0, 6; ...
      0, 0, 5, 1; ...
      0, 0, 4, 2; ...
      1, 1, 0, 4; ...
      3, 3, 0, 0; ...
      3, 2, 1, 0; ...
      1, 1, 1, 3; ...
      2, 2, 2, 0; ...
      2, 2, 1, 1 ]';

    suborder_xyz_d = 6;

    suborder_w_n(1:suborder_num) = [ ...
      -7, 24, -30, 0, 40, 30, 180, -45, 0 ];

    suborder_w_d = 1400;

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'TETRAHEDRON_NCC_SUBRULE - Fatal error!\n' );
    fprintf ( 1, '  Illegal RULE = %d\n', rule );
    error ( 'TETRAHEDRON_NCC_SUBRULE - Fatal error!' )

  end

  suborder_xyz(1:4,1:suborder_num) = ...
    suborder_xyz_n(1:4,1:suborder_num) / suborder_xyz_d;

  suborder_w(1:suborder_num) = ...
    suborder_w_n(1:suborder_num) / suborder_w_d;

  return
end


%Parametric ReLU function
x = -10:0.01:10;
a = input('Enter the value of the slope')
y = @(x) (x).*((x > 0)) + (a*x).*((x < 0)) ;
fplot(y);
xlabel('x');
ylabel('y');
grid on


function obj = addAttribute(obj, attribute)
% ADDATTRIBUTE Add an attribute to the object.
% ------------------------------------------------------------------------------
% INPUT
% 1 [attribute]
%   Name of attribute which should be added. Actually, possible choices are:
%     * 'slope'
%        Slope (from 0 to 100 gradian) of each point based on its normal vector.
%        This attribute requires the components of the normal vectors nx, ny,
%        and nz for each point. They can be calculated with the method 'normals'
%        (see 'help pointCloud.normals').
%     * 'exposition'
%        Exposition (from 0 to 400 gradian) of each point based on its normal
%        vector. This attribute requires the components of the normal vectors
%        nx, ny, and nz for each point. They can be calculated with the method
%        'normals' (see 'help pointCloud.normals').
% ------------------------------------------------------------------------------
% EXAMPLES
% 1 Calculate and visualize the slope attribute of a point cloud.
%   pc = pointCloud('Lion.xyz', 'Attributes', {'nx' 'ny' 'nz' 'roughness'});
%   pc.addAttribute('slope');
%   pc.plot('Color', 'A.slope', 'MarkerSize', 5);
%
% 2 Calculate and visualize the exposition attribute of a point cloud.
%   pc = pointCloud('Lion.xyz', 'Attributes', {'nx' 'ny' 'nz' 'roughness'});
%   pc.addAttribute('exposition');
%   pc.plot('Color', 'A.exposition', 'MarkerSize', 5);
% ------------------------------------------------------------------------------
% philipp.glira@gmail.com
% ------------------------------------------------------------------------------

% Input parsing ----------------------------------------------------------------

validAttribute = {'slope' 'exposition'};

p = inputParser;
p.addRequired('attribute', @(x) any(strcmpi(x, validAttribute)));
p.parse(attribute);
p = p.Results;
% Clear required input to avoid confusion
clear attribute

% Start ------------------------------------------------------------------------

procHierarchy = {'POINTCLOUD' 'ADDATTRIBUTE'};
msg('S', procHierarchy);
msg('I', procHierarchy, sprintf('Point cloud label = ''%s''', obj.label));

% Add attribute ----------------------------------------------------------------

% Slope
if strcmpi(p.attribute, 'slope')

    % Points with no normal (i.e. with normal components equal to NaN)
    idxNoNormal = isnan(obj.A.nx) | isnan(obj.A.ny) | isnan(obj.A.nz);

    obj.A.slope = NaN(size(obj.X,1),1); % initialize with NaNs
    obj.A.slope(~idxNoNormal) = acosg(abs(obj.A.nz(~idxNoNormal))); % attention: abs introduced to make sure, slope ranges between 0 and 100, but in TLS slope > 100 may be useful

% Exposition
elseif strcmpi(p.attribute, 'exposition')

    % Points with no normal (i.e. with normal components equal to NaN)
    idxNoNormal = isnan(obj.A.nx) | isnan(obj.A.ny) | isnan(obj.A.nz);

    polar = xyz2polar([obj.A.nx(~idxNoNormal) obj.A.ny(~idxNoNormal) obj.A.nz(~idxNoNormal)]);
    obj.A.exposition = NaN(size(obj.X,1),1); % initialize with NaNs
    obj.A.exposition(~idxNoNormal) = polar(:,2);

    % Set exposition to zero if normal points upwards (otherwise it is equal to NaN)
    obj.A.exposition(obj.A.nx == 0 & obj.A.ny == 0 & obj.A.nz == 1) = 0;

end

% End --------------------------------------------------------------------------

msg('E', procHierarchy);

end