function I = sum3(f)
%SUM3   Triple integral of a BALLFUN over its domain.
%   I = SUM3(F) returns the double definite integral of a BALLFUN.
%
% See also SUM, SUM2.

% Copyright 2019 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Empty check
if ( isempty(f) )
    I = [];
    return
end

% Second argument: integral over the sphere of radius -1
[m,n,p] = size(f);
m = m+2; p = p+2;
F = coeffs3(f,m,n,p);

% Extract the 0-th Fourier mode
F = reshape(F(:,floor(n/2)+1,:), m, p);

% Increase the discretization by 2 in the r and theta direction
F = [zeros(m,1),F,zeros(m,1);zeros(2,p+2)];
m = m+2;
p = p+2;

% Multiply f par r^2sin(theta) (= Jacobian)
Msin = trigspec.multmat(p, [0.5i;0;-0.5i] );
Mr2 = ultraS.multmat(m, [0.5; 0; 0.5], 0 );
F = Mr2*F*(Msin.');

% Coefficients of integration between 0 and 1 of the chebyshev polynomials
IntChebyshev = zeros(1,m);
for i = 0:m-1
    if mod(i,4)==0
        IntChebyshev(i+1) = -1/(i^2-1);
    elseif mod(i,4)==1
        IntChebyshev(i+1) = 1/(i+1);
    elseif mod(i,4)==2
        IntChebyshev(i+1) = -1/(i^2-1);
    else
        IntChebyshev(i+1) = -1/(i-1);
    end
end

% Coefficients of integration between 0 and pi of the theta Fourier
% function
Listp = (1:p).' - floor(p/2)-1;
IntTheta = -1i*((-1).^Listp-1)./Listp;
IntTheta(floor(p/2)+1) = pi;

% Integrate over lambda
IntTheta = 2*pi*IntTheta;

% Integrate over the sphere of radius -1
if nargin>1
    IntChebyshev = (-1).^(0:m-1).*IntChebyshev;
end

% Return the integral of f over the ballfun
I = IntChebyshev*F*IntTheta;

% Return real value if the function is real
if f.isReal
   I = real(I);
end
end


%  Using 2D or 3D affine matrix to rotate, translate, scale, reflect and
%  shear a 2D image or 3D volume. 2D image is represented by a 2D matrix,
%  3D volume is represented by a 3D matrix, and data type can be real
%  integer or floating-point.
%
%  You may notice that MATLAB has a function called 'imtransform.m' for
%  2D spatial transformation. However, keep in mind that 'imtransform.m'
%  assumes y for the 1st dimension, and x for the 2nd dimension. They are
%  equivalent otherwise.
%
%  In addition, if you adjust the 'new_elem_size' parameter, this 'affine.m'
%  is equivalent to 'interp2.m' for 2D image, and equivalent to 'interp3.m'
%  for 3D volume.
%
%  Usage: [new_img new_M] = ...
%	affine(old_img, old_M, [new_elem_size], [verbose], [bg], [method]);
%
%  old_img  -	original 2D image or 3D volume. We assume x for the 1st
%		dimension, y for the 2nd dimension, and z for the 3rd
%		dimension.
%
%  old_M  -	a 3x3 2D affine matrix for 2D image, or a 4x4 3D affine
%		matrix for 3D volume. We assume x for the 1st dimension,
%		y for the 2nd dimension, and z for the 3rd dimension.
%
%  new_elem_size (optional)  -  size of voxel along x y z direction for
%		a transformed 3D volume, or size of pixel along x y for
%		a transformed 2D image. We assume x for the 1st dimension
%		y for the 2nd dimension, and z for the 3rd dimension.
%		'new_elem_size' is 1 if it is default or empty.
%
%		You can increase its value to decrease the resampling rate,
%		and make the 2D image or 3D volume more coarse. It works
%		just like 'interp3'.
%
%  verbose (optional) - 1, 0
%		1:  show transforming progress in percentage
%		2:  progress will not be displayed
%		'verbose' is 1 if it is default or empty.
%
%  bg (optional)  -	background voxel intensity in any extra corner that
%		is caused by the interpolation. 0 in most cases. If it is
%		default or empty, 'bg' will be the average of two corner
%		voxel intensities in original data.
%
%  method (optional)  -	1, 2, or 3
%		1:  for Trilinear interpolation
%		2:  for Nearest Neighbor interpolation
%		3:  for Fischer's Bresenham interpolation
%		'method' is 1 if it is default or empty.
%
%  new_img  -	transformed 2D image or 3D volume
%
%  new_M  -	transformed affine matrix
%
%  Example 1 (3D rotation):
%	load mri.mat;   old_img = double(squeeze(D));
%	old_M = [0.88 0.5 3 -90; -0.5 0.88 3 -126; 0 0 2 -72; 0 0 0 1];
%	new_img = affine(old_img, old_M, 2);
%	[x y z] = meshgrid(1:128,1:128,1:27);
%	sz = size(new_img);
%	[x1 y1 z1] = meshgrid(1:sz(2),1:sz(1),1:sz(3));
%	figure; slice(x, y, z, old_img, 64, 64, 13.5);
%	shading flat; colormap(map); view(-66, 66);
%	figure; slice(x1, y1, z1, new_img, sz(1)/2, sz(2)/2, sz(3)/2);
%	shading flat; colormap(map); view(-66, 66);
%
%  Example 2 (2D interpolation):
%	load mri.mat;   old_img=D(:,:,1,13)';
%	old_M = [1 0 0; 0 1 0; 0 0 1];
%	new_img = affine(old_img, old_M, [.2 .4]);
%	figure; image(old_img); colormap(map);
%	figure; image(new_img); colormap(map);
%
%  This program is inspired by:
%  SPM5 Software from Wellcome Trust Centre for Neuroimaging
%	http://www.fil.ion.ucl.ac.uk/spm/software
%  Fischer, J., A. del Rio (2004). A Fast Method for Applying Rigid
%	Transformations to Volume Data, WSCG2004 Conference.
%	http://wscg.zcu.cz/wscg2004/Papers_2004_Short/M19.pdf
%
%  - Jimmy Shen (jimmy@rotman-baycrest.on.ca)
%
function [new_img, new_M] = affine(old_img, old_M, new_elem_size, verbose, bg, method)

   if ~exist('old_img','var') | ~exist('old_M','var')
      error('Usage: [new_img new_M] = affine(old_img, old_M, [new_elem_size], [verbose], [bg], [method]);');
   end

   if ndims(old_img) == 3
      if ~isequal(size(old_M),[4 4])
         error('old_M should be a 4x4 affine matrix for 3D volume.');
      end
   elseif ndims(old_img) == 2
      if ~isequal(size(old_M),[3 3])
         error('old_M should be a 3x3 affine matrix for 2D image.');
      end
   else
      error('old_img should be either 2D image or 3D volume.');
   end

   if ~exist('new_elem_size','var') | isempty(new_elem_size)
      new_elem_size = [1 1 1];
   elseif length(new_elem_size) < 2
      new_elem_size = new_elem_size(1)*ones(1,3);
   elseif length(new_elem_size) < 3
      new_elem_size = [new_elem_size(:); 1]';
   end

   if ~exist('method','var') | isempty(method)
      method = 1;
   elseif ~exist('bresenham_line3d.m','file') & method == 3
      error([char(10) char(10) 'Please download 3D Bresenham''s line generation program from:' char(10) char(10) 'http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=21057' char(10) char(10) 'to test Fischer''s Bresenham interpolation method.' char(10) char(10)]);
   end

   %  Make compatible to MATLAB earlier than version 7 (R14), which
   %  can only perform arithmetic on double data type
   %
   old_img = double(old_img);
   old_dim = size(old_img);

   if ~exist('bg','var') | isempty(bg)
      bg = mean([old_img(1) old_img(end)]);
   end

   if ~exist('verbose','var') | isempty(verbose)
      verbose = 1;
   end

   if ndims(old_img) == 2
      old_dim(3) = 1;
      old_M = old_M(:, [1 2 3 3]);
      old_M = old_M([1 2 3 3], :);
      old_M(3,:) = [0 0 1 0];
      old_M(:,3) = [0 0 1 0]';
   end

   %  Vertices of img in voxel
   %
   XYZvox = [	1		1		1
		1		1		old_dim(3)
		1		old_dim(2)	1
		1		old_dim(2)	old_dim(3)
		old_dim(1)	1		1
		old_dim(1)	1		old_dim(3)
		old_dim(1)	old_dim(2)	1
		old_dim(1)	old_dim(2)	old_dim(3)   ]';

   old_R = old_M(1:3,1:3);
   old_T = old_M(1:3,4);

   %  Vertices of img in millimeter
   %
   XYZmm = old_R*(XYZvox-1) + repmat(old_T, [1, 8]);

   %  Make scale of new_M according to new_elem_size
   %
   new_M = diag([new_elem_size 1]);

   %  Make translation so minimum vertex is moved to [1,1,1]
   %
   new_M(1:3,4) = round( min(XYZmm,[],2) );

   %  New dimensions will be the maximum vertices in XYZ direction (dim_vox)
   %  i.e. compute   dim_vox   via   dim_mm = R*(dim_vox-1)+T
   %  where, dim_mm = round(max(XYZmm,[],2));
   %
   new_dim = ceil(new_M(1:3,1:3) \ ( round(max(XYZmm,[],2))-new_M(1:3,4) )+1)';

   %  Initialize new_img with new_dim
   %
   new_img = zeros(new_dim(1:3));

   %  Mask out any changes from Z axis of transformed volume, since we
   %  will traverse it voxel by voxel below. We will only apply unit
   %  increment of mask_Z(3,4) to simulate the cursor movement
   %
   %  i.e. we will use   mask_Z * new_XYZvox   to replace   new_XYZvox
   %
   mask_Z = diag(ones(1,4));
   mask_Z(3,3) = 0;

   %  It will be easier to do the interpolation if we invert the process
   %  by not traversing the original volume. Instead, we traverse the
   %  transformed volume, and backproject each voxel in the transformed
   %  volume back into the original volume. If the backprojected voxel
   %  in original volume is within its boundary, the intensity of that
   %  voxel can be used by the cursor location in the transformed volume.
   %
   %  First, we traverse along Z axis of transformed volume voxel by voxel
   %
   for z = 1:new_dim(3)

      if verbose & ~mod(z,10)
         fprintf('%.2f percent is done.\n', 100*z/new_dim(3));
      end

      %  We need to find out the mapping from voxel in the transformed
      %  volume (new_XYZvox) to voxel in the original volume (old_XYZvox)
      %
      %  The following equation works, because they all equal to XYZmm:
      %  new_R*(new_XYZvox-1) + new_T  ==  old_R*(old_XYZvox-1) + old_T
      %
      %  We can use modified new_M1 & old_M1 to substitute new_M & old_M
      %      new_M1 * new_XYZvox       ==       old_M1 * old_XYZvox
      %
      %  where: M1 = M;   M1(:,4) = M(:,4) - sum(M(:,1:3),2);
      %  and:             M(:,4) == [T; 1] == sum(M1,2)
      %
      %  Therefore:   old_XYZvox = old_M1 \ new_M1 * new_XYZvox;
      %
      %  Since we are traverse Z axis, and   new_XYZvox   is replaced
      %  by   mask_Z * new_XYZvox, the above formula can be rewritten
      %  as:    old_XYZvox = old_M1 \ new_M1 * mask_Z * new_XYZvox;
      %
      %  i.e. we find the mapping from new_XYZvox to old_XYZvox:
      %  M = old_M1 \ new_M1 * mask_Z;
      %
      %  First, compute modified old_M1 & new_M1
      %
      old_M1 = old_M;   old_M1(:,4) = old_M(:,4) - sum(old_M(:,1:3),2);
      new_M1 = new_M;   new_M1(:,4) = new_M(:,4) - sum(new_M(:,1:3),2);

      %  Then, apply unit increment of mask_Z(3,4) to simulate the
      %  cursor movement
      %
      mask_Z(3,4) = z;

      %  Here is the mapping from new_XYZvox to old_XYZvox
      %
      M = old_M1 \ new_M1 * mask_Z;

      switch method
      case 1
         new_img(:,:,z) = trilinear(old_img, new_dim, old_dim, M, bg);
      case 2
         new_img(:,:,z) = nearest_neighbor(old_img, new_dim, old_dim, M, bg);
      case 3
         new_img(:,:,z) = bresenham(old_img, new_dim, old_dim, M, bg);
      end

   end;			% for z

   if ndims(old_img) == 2
      new_M(3,:) = [];
      new_M(:,3) = [];
   end

   return;					% affine

%--------------------------------------------------------------------
function img_slice = trilinear(img, dim1, dim2, M, bg)

   img_slice = zeros(dim1(1:2));
   TINY = 5e-2;					% tolerance

   %  Dimension of transformed 3D volume
   %
   xdim1 = dim1(1);
   ydim1 = dim1(2);

   %  Dimension of original 3D volume
   %
   xdim2 = dim2(1);
   ydim2 = dim2(2);
   zdim2 = dim2(3);

   %  initialize new_Y accumulation
   %
   Y2X = 0;
   Y2Y = 0;
   Y2Z = 0;

   for y = 1:ydim1

      %  increment of new_Y accumulation
      %
      Y2X = Y2X + M(1,2);		% new_Y to old_X
      Y2Y = Y2Y + M(2,2);		% new_Y to old_Y
      Y2Z = Y2Z + M(3,2);		% new_Y to old_Z

      %  backproject new_Y accumulation and translation to old_XYZ
      %
      old_X = Y2X + M(1,4);
      old_Y = Y2Y + M(2,4);
      old_Z = Y2Z + M(3,4);

      for x = 1:xdim1

         %  accumulate the increment of new_X, and apply it
         %  to the backprojected old_XYZ
         %
         old_X = M(1,1) + old_X  ;
         old_Y = M(2,1) + old_Y  ;
         old_Z = M(3,1) + old_Z  ;

         %  within boundary of original image
         %
         if (	old_X > 1-TINY & old_X < xdim2+TINY & ...
		old_Y > 1-TINY & old_Y < ydim2+TINY & ...
		old_Z > 1-TINY & old_Z < zdim2+TINY	)

            %  Calculate distance of old_XYZ to its neighbors for
            %  weighted intensity average
            %
            dx = old_X - floor(old_X);
            dy = old_Y - floor(old_Y);
            dz = old_Z - floor(old_Z);

            x000 = floor(old_X);
            x100 = x000 + 1;

            if floor(old_X) < 1
               x000 = 1;
               x100 = x000;
            elseif floor(old_X) > xdim2-1
               x000 = xdim2;
               x100 = x000;
            end

            x010 = x000;
            x001 = x000;
            x011 = x000;

            x110 = x100;
            x101 = x100;
            x111 = x100;

            y000 = floor(old_Y);
            y010 = y000 + 1;

            if floor(old_Y) < 1
               y000 = 1;
               y100 = y000;
            elseif floor(old_Y) > ydim2-1
               y000 = ydim2;
               y010 = y000;
            end

            y100 = y000;
            y001 = y000;
            y101 = y000;

            y110 = y010;
            y011 = y010;
            y111 = y010;

            z000 = floor(old_Z);
            z001 = z000 + 1;

            if floor(old_Z) < 1
               z000 = 1;
               z001 = z000;
            elseif floor(old_Z) > zdim2-1
               z000 = zdim2;
               z001 = z000;
            end

            z100 = z000;
            z010 = z000;
            z110 = z000;

            z101 = z001;
            z011 = z001;
            z111 = z001;

            x010 = x000;
            x001 = x000;
            x011 = x000;

            x110 = x100;
            x101 = x100;
            x111 = x100;

            v000 = double(img(x000, y000, z000));
            v010 = double(img(x010, y010, z010));
            v001 = double(img(x001, y001, z001));
            v011 = double(img(x011, y011, z011));

            v100 = double(img(x100, y100, z100));
            v110 = double(img(x110, y110, z110));
            v101 = double(img(x101, y101, z101));
            v111 = double(img(x111, y111, z111));

            img_slice(x,y) = v000*(1-dx)*(1-dy)*(1-dz) + ...
               v010*(1-dx)*dy*(1-dz) + ...
               v001*(1-dx)*(1-dy)*dz + ...
               v011*(1-dx)*dy*dz + ...
               v100*dx*(1-dy)*(1-dz) + ...
               v110*dx*dy*(1-dz) + ...
               v101*dx*(1-dy)*dz + ...
               v111*dx*dy*dz;

         else
            img_slice(x,y) = bg;

         end	% if boundary

      end	% for x
   end		% for y

   return;					% trilinear

%--------------------------------------------------------------------
function img_slice = nearest_neighbor(img, dim1, dim2, M, bg)

   img_slice = zeros(dim1(1:2));

   %  Dimension of transformed 3D volume
   %
   xdim1 = dim1(1);
   ydim1 = dim1(2);

   %  Dimension of original 3D volume
   %
   xdim2 = dim2(1);
   ydim2 = dim2(2);
   zdim2 = dim2(3);

   %  initialize new_Y accumulation
   %
   Y2X = 0;
   Y2Y = 0;
   Y2Z = 0;

   for y = 1:ydim1

      %  increment of new_Y accumulation
      %
      Y2X = Y2X + M(1,2);		% new_Y to old_X
      Y2Y = Y2Y + M(2,2);		% new_Y to old_Y
      Y2Z = Y2Z + M(3,2);		% new_Y to old_Z

      %  backproject new_Y accumulation and translation to old_XYZ
      %
      old_X = Y2X + M(1,4);
      old_Y = Y2Y + M(2,4);
      old_Z = Y2Z + M(3,4);

      for x = 1:xdim1

         %  accumulate the increment of new_X and apply it
         %  to the backprojected old_XYZ
         %
         old_X = M(1,1) + old_X  ;
         old_Y = M(2,1) + old_Y  ;
         old_Z = M(3,1) + old_Z  ;

         xi = round(old_X);
         yi = round(old_Y);
         zi = round(old_Z);

         %  within boundary of original image
         %
         if (	xi >= 1 & xi <= xdim2 & ...
		yi >= 1 & yi <= ydim2 & ...
		zi >= 1 & zi <= zdim2	)

            img_slice(x,y) = img(xi,yi,zi);

         else
            img_slice(x,y) = bg;

         end	% if boundary

      end	% for x
   end		% for y

   return;					% nearest_neighbor

%--------------------------------------------------------------------
function img_slice = bresenham(img, dim1, dim2, M, bg)

   img_slice = zeros(dim1(1:2));

   %  Dimension of transformed 3D volume
   %
   xdim1 = dim1(1);
   ydim1 = dim1(2);

   %  Dimension of original 3D volume
   %
   xdim2 = dim2(1);
   ydim2 = dim2(2);
   zdim2 = dim2(3);

   for y = 1:ydim1

      start_old_XYZ = round(M*[0     y 0 1]');
      end_old_XYZ   = round(M*[xdim1 y 0 1]');

      [X Y Z] = bresenham_line3d(start_old_XYZ, end_old_XYZ);

      %  line error correction
      %
%      del = end_old_XYZ - start_old_XYZ;
 %     del_dom = max(del);
  %    idx_dom = find(del==del_dom);
   %   idx_dom = idx_dom(1);
    %  idx_other = [1 2 3];
     % idx_other(idx_dom) = [];
      %del_x1 = del(idx_other(1));
%      del_x2 = del(idx_other(2));
 %     line_slope = sqrt((del_x1/del_dom)^2 + (del_x2/del_dom)^2 + 1);
  %    line_error = line_slope - 1;
% line error correction removed because it is too slow

      for x = 1:xdim1

         %  rescale ratio
         %
         i = round(x * length(X) / xdim1);

         if i < 1
            i = 1;
         elseif i > length(X)
            i = length(X);
         end

         xi = X(i);
         yi = Y(i);
         zi = Z(i);

         %  within boundary of the old XYZ space
         %
         if (	xi >= 1 & xi <= xdim2 & ...
		yi >= 1 & yi <= ydim2 & ...
		zi >= 1 & zi <= zdim2	)

            img_slice(x,y) = img(xi,yi,zi);

%            if line_error > 1
 %              x = x + 1;

%               if x <= xdim1
 %                 img_slice(x,y) = img(xi,yi,zi);
  %                line_error = line_slope - 1;
   %            end
    %        end		% if line_error
% line error correction removed because it is too slow

         else
            img_slice(x,y) = bg;

         end	% if boundary

      end	% for x
   end		% for y

   return;					% bresenham


function [P_MPP,U_MPP,I_MPP,U_OC_New,I_SC_New,U_PV,I_PV,P_PV] = PV_Module_Simulator_Function(G,T_amb)
%------ Solar Module simulation according to DIN EN 50530:2010 ------------
% This function can be used to simulate an solar cell to a PV-field by
% entering the number of modules (cells) in parallel and the number of
% module in serise.
% The equations used in this function are taken from the DIN EN 50530:2010
% norm.
% This function need two inputs:
% 1- G: The new solar irradiation at which the new working points
% (U_MPP,P_MPP,...) will be calculated.
% 2- T_amb: The new ambiant temperature at which the new working points
% (U_MPP,P_MPP,...) will be calculated.
% This function outputs are:
% 1- P_MPP,U_MPP,I_MPP: are the power, voltage, current at the maximum
% power point at the new conditions respectivily
% 2- U_OC_New,I_SC_New: are the open circuit voltage and the short circuit
% current respectivily calculated for the new weather(input) conditions.
% 3- U_PV,I_PV,P_PV: are the new characteristic curves values for the
% studied input condition.
% This function will also ask the user for addtional informations which
% need to be given in order to be able to simulate the characteristics of
% the selected PV-mdouel.
% Those information cal be obtained from the datasheet of the selected PV
% modules. if not given use the default values already given.
%Author: Aubai Alkhatib, date: 25.09.2013

prompt1 = {'Please Enter The STC Temperatur in C','Please Enter The STC Open Voltage in V','Please Enter The STC Short circut Current in A','Please Enter The Voltage temperatur coefficient Beta in %/C','Please Enter The Curent temperatur coefficient Alpha in %/C ','Please Enter The STC Irradiantion in W/m2','Please Eneter The Voltage of the MPP in V','Please Eneter The Current of the MPP in A','Please Eneter The Technology correction factor CG in W/m2','Please Eneter The Technology correction factor CU in pu','Please Eneter The Technology correction factor CR in m2/W','Please Eneter The number of PV-Modules in Parallel P','Please Eneter The number of PV-Modules in Seris R','Please Enter The Open circut Voltage of the LS'};
name = 'Initial Input data';
numlines = 1;
%defaultanswer1 = {'25','36.7','8.18','-0.32','0.04','1000','29.9','7.53','2.514','8.593','1.088','22','384','1200','Oldenburg,Germany','20130701','X:\Loc_4631\2013\07'};
defaultanswer1 = {'25','36.7','8.18','-0.32','0.04','1000','29.9','7.53','2.514','8.593','1.088','22','450','1600'};
%defaultanswer1 = {'4','1700','105','20','400','50','1'};
answer1 = inputdlg(prompt1,name,numlines,defaultanswer1);
T_STC = str2num(answer1{1})+273;
Uoc_STC =  str2num(answer1{2});
Isc_STC =  str2num(answer1{3});
V_T_C = str2num(answer1{4})/100;
I_T_C = str2num(answer1{5})/100;
E0 = str2num(answer1{6});
Umpp_STC = str2num(answer1{7});
Impp_STC = str2num(answer1{8});
CG = str2num(answer1{9})/1000;
CU = str2num(answer1{10})/100;
CR = str2num(answer1{11})/10000;
R = str2num(answer1{12});
P = str2num(answer1{13});
Ts = str2num(answer1{14});%#ok<*ST2NM>
G_STC = E0;
Uoc_STC = Uoc_STC * R;
Isc_STC = Isc_STC * P;
Umpp_STC = Umpp_STC * R;
Impp_STC = Impp_STC * P;
if 0 == 1
    T_0 = -3;
    k = 0.03;%km^2/w
    Tau = 5;% in min
    T_PV = T_amb + T_0 + ((k*1000000)/(1+(Tau*60)))*G;
else
    T_NOCT = 45;
    T_PV = T_amb + ((T_NOCT-20) * (G/800));
end
Alpha = I_T_C;
Isc_New = (Isc_STC*(G/G_STC)) * (1 + (Alpha * (T_PV - T_STC)));
Beta = V_T_C;
Uoc_New = Uoc_STC*(1+Beta*(T_PV-T_STC))*(log((G/CG)+1)*CU-CR*G);
FF_U = Umpp_STC/Uoc_STC;
FF_I = Impp_STC/Isc_STC;
I_0 = (Isc_STC*(1-FF_I)^(1/(1-FF_U)))*(G/G_STC);
CAQ = (FF_U-1)/(log(1-FF_I));
U_PV = 0:1:Ts;
I_PV = Isc_New - I_0*(exp(U_PV/(Uoc_New*CAQ))-1);
P_PV = U_PV.*I_PV;
P_MPP = max(P_PV);
U_MPP = U_PV(P_PV == P_MPP);
I_MPP = I_PV(U_MPP);
indx = find(I_PV < 0);
U_OC_New = U_PV(indx(1));
I_SC_New = I_PV(1);
figure(1);
subplot(2,1,1);
plot(U_PV,I_PV);xlabel('DC-Voltage in V');ylabel('DC-Current in A');title('PV-Module Charactaristic curves');grid on;ylim([0,I_MPP+((20/100)*I_MPP)]);xlim([0,U_OC_New+10]);
subplot (2,1,2);
plot(U_PV,P_PV);xlabel('DC-Voltage in V');ylabel('DC-Power in W');grid on;ylim([0,P_MPP+((5/100)*P_MPP)]);xlim([0,U_OC_New+10]);
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%

% problem 7 - convolution of x[n] and h[n]

n=0:10;
u=ones(size(n));
n1=0:3;
u4_1=zeros(size(n1));
n2=4:10;
u4_2=ones(size(n2));
u4=[u4_1 u4_2];
x=u-u4;
h=0.7.^n;
y=conv(x,h);
stem(0:20,y);
xlim([-1 21]);
legend('y[n]')


function f = projectOntoBMCII(f)
%PROJECTONTOBMCII   Projection onto BMC-II symmetry.
%   F = projectOntoBMCII(F) is the orthogonal projection of f onto BMC-II
%   symmetry, i.e., a function that is
%   1. even in r for every even wave number in theta.
%   2. odd in r for every odd wave number in theta.
%   Additionally, for all but the k=0 mode in theta, the resulting
%   projection enforces the diskfun is zero at the origin.
%
%   The projection is orthogonal, i.e., the correction matrix to fix up the
%   structure has the smallest possible Frobenius norm.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Even part
feven = f;
feven.cols = feven.cols(:, feven.idxPlus);
feven.rows = feven.rows(:, feven.idxPlus);
feven = projectOntoEvenBMCII(feven);

% Odd part
fodd = f;
fodd.cols = fodd.cols(:, fodd.idxMinus);
fodd.rows = fodd.rows(:, fodd.idxMinus);
fodd = projectOntoOddBMCII(fodd);

% Put pieces back together.
f.cols(:, f.idxPlus) = feven.cols;
f.rows(:, f.idxPlus) = feven.rows;

f.cols(:, f.idxMinus) = fodd.cols;
f.rows(:, f.idxMinus) = fodd.rows;

end

function f = projectOntoEvenBMCII(f)
% Project a diskfun to have even BMC-II symmetry, i.e., a diskfun that
% is pi-periodic in theta and even in r. The projection is orthogonal,
% i.e., the correction matrix to fix up the structure has the smallest
% possible Frobenius norm.

% Nothing to project
if isempty(f)
    return;
end

% Operate on the column coefficients first.
X = f.cols.funs{1}.onefun.coeffs;

% Get size:
[m, n] = size(X);

 % First we enforce that the every function in r is even:
 X(2:2:end, :) = 0;

% The rest of the code now needs to operate on the remaining even,
% non-zero modes in theta.
evenModes = 2:n;

if ( ~isempty(evenModes) )
    % to enforce that f(r) = 0 for each column function,
    % we want C with smallest 2-norm such that  A*(X(1:2:end, 2:end)+C) = 0,
    % where A =[1 -1 1 -1..] .
    % The solution is
    % C = A'*((A*A')\(A*X)).
    %odd modes in r are zero, they won't contribute.
    even = 1:2:m;
    Xe = X(even, evenModes);
    factor = 1/length(even)*(sum(Xe(1:2:end, :),1)-sum(Xe(2:2:end, :),1));
    C = ((-1*ones(length(even), 1)).^((2:length(even)+1)'))*factor;
    %now add C to X
    X(even, evenModes) = Xe+C;

end

ctechs = chebtech2({'',X});
f.cols.funs{1}.onefun = ctechs;

% Now operate on the rows. The coefficients for the rows of an even BMC-II
% function should only contain even wave numbers. The projection is to
% simply zero out the odd wave numbers.
X = f.rows.funs{1}.onefun.coeffs;
n = size(X, 1);
zeroMode = floor(n/2) + 1;
oddModes = [fliplr(zeroMode-1:-2:1) zeroMode+1:2:n];
X(oddModes, :) = 0;
rtechs = real(trigtech({'', X}));
f.rows.funs{1}.onefun = rtechs;

% Weird feval behavior in chebfun requires this
f.cols.pointValues = feval(ctechs, [-1; 1]);
f.rows.pointValues = feval(rtechs, [-1; 1]);

end

function f = projectOntoOddBMCII(f)
% Project a diskfun to have odd BMC-II symmetry, i.e., a diskfun that is
% pi-anti-periodic in theta and even in r. The projection is orthogonal,
% i.e., the correction matrix to fix up the structure has the smallest
% possible Frobenius norm.

% Nothing to project
if isempty(f)
    return;
end

% to enforce odd, simply zero out even coeffs:

% Operate on the column coefficients first to project them onto odd
% functions.
X = f.cols.funs{1}.onefun.coeffs;
X(1:2:end, :) = 0;

ctechs = chebtech2({'',X});
f.cols.funs{1}.onefun = ctechs;

% Now operate on the rows. The coefficients for the rows of an odd BMC-II
% function should only contain odd wave numbers. The projection is to
% simply zero out the even wave numbers.
X = f.rows.funs{1}.onefun.coeffs;
n = size(X, 1);
zeroMode = floor(n/2) + 1;
evenModes = [fliplr(zeroMode-2:-2:1) zeroMode:2:n];
X(evenModes, :) = 0;

rtechs = real(trigtech({'', X}));
f.rows.funs{1}.onefun = rtechs;

% Weird feval behavior in chebfun requires this
f.cols.pointValues = feval(ctechs, [-1; 1]);
f.rows.pointValues = feval(rtechs, [-1; 1]);

end


function G = divgrad(M,options)

% divgrad - compute either gradient or divergence.
%
%   G = divgrad(M);
%
%   if M is a 2D array, compute gradient,
%   if M is a 3D array, compute divergence.
%   Use centered finite differences.
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
if size(M,3)==2
    G = mydiv(M,options);
else
    G = mygrad(M,options);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function d = mydiv(g,options)

bound = getoptions(options, 'bound', 'sym');

n = size(g,1);
d = zeros(n);

if strcmp(bound,'sym')
    d(2:end-1,:) = d(2:end-1,:) + ( g(3:end,:,1)-g(1:end-2,:,1) )/2;
    d(1,:) = d(1,:) + g(2,:,1)-g(1,:,1);
    d(end,:) = d(end,:) + g(end,:,1)-g(end-1,:,1);

    d(:,2:end-1) = d(:,2:end-1) + ( g(:,3:end,2)-g(:,1:end-2,2) )/2;
    d(:,1) = d(:,1) + g(:,2,2)-g(:,1,2);
    d(:,end) = d(:,end) + g(:,end,2)-g(:,end-1,2);
else
    sel1 = [2:n 1];
    sel2 = [n 1:n-1];
    d = g(sel1,:,1)-g(sel2,:,1) + g(:,sel1,2)-g(:,sel2,2);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function g = mygrad(M,options)

bound = getoptions(options, 'bound', 'sym');

n = size(M,1);
g = zeros(n,n,2);

if strcmp(bound,'sym')
    % on x
    g(2:end-1,:,1) = ( M(3:end,:)-M(1:end-2,:) )/2;
    g(1,:,1) = M(2,:)-M(1,:);
    g(end,:,1) = M(end,:)-M(end-1,:);
    % on y
    g(:,2:end-1,2) = ( M(:,3:end)-M(:,1:end-2,:) )/2;
    g(:,1,1) = M(2,:)-M(1,:);
    g(:,end,1) = M(:,end)-M(:,end-1);
else
    sel1 = [2:n 1];
    sel2 = [n 1:n-1];
    g = cat( 3, M(sel1,:)-M(sel2,:), M(:,sel1)-M(:,sel2) )/2;
end


function J = jacobian( F )
%JACOBIAN   Jacobian determinant of a CHEBFUN2V.
%   J = JACOBIAN(F) computes the determinant of the Jacobian matrix associated
%   to the vector-valued CHEBFUN2V F. The CHEBFUN2V must have two components.
%
%   Note we return the determinant of the Jacobian matrix and not the Jacobian
%   matrix itself.
%
% See also CHEBFUN2/GRADIENT.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Empty check.
if ( isempty(F) )
    J = [];
    return
end

if ( F.nComponents == 3 )
    error('CHEBFUN:CHEBFUN2V:jacobian:notSquare', ...
        'Jacobian matrix is not square.')
end

% Determinant formula:
Fx = diff( F, 1, 2 );
Fy = diff( F, 1, 1 );

% Jacobian:
Fxc = Fx.components;
Fyc = Fy.components;
J = Fxc{1} .* Fyc{2} - Fyc{1} .* Fxc{2};

end


function n=im3Dnorm(img,normind,varargin)
% IMAGE3DNORM computes the desired image norm
%   IMAGE3DNORM(IMG,NORMIND) computes the norm if image IMG using the norm
%   defined in NORMING
%
%   IMG         A 3D image
%   NORMIND     'LN': N norm
%               'TV': TV norm
%
%
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------
if ~ischar(normind)
      error('CBCT:image3Dnorm:InvalidInput','Norm option has to be a string');
end
if length(normind)~=2
      error('CBCT:image3Dnorm:InvalidInput','Unknown norm option');
end

if strcmp(normind(1),'L')
    Nnorm=str2double(normind(2:end));
    n=norm(img(:),Nnorm);
    return;
end

if strcmp(normind,'TV')
    typeGrad=varargin{1};
    if strcmpi(typeGrad,'central')
        [gx,gy,gz]=gradient(img);
    end
    if strcmpi(typeGrad,'forward')
        gx=diff(img,1,1);
        gy=diff(img,1,2);
        gz=diff(img,1,3);
        gx=cat(1,gx,zeros(size(gx(end,:,:))));
        gy=cat(2,gy,zeros(size(gy(:,end,:))));
        gz=cat(3,gz,zeros(size(gz(:,:,end))));

    end
    if strcmpi(typeGrad,'backward')
        gx=diff(img,1,1);
        gy=diff(img,1,2);
        gz=diff(img,1,3);
        gx=cat(1,zeros(size(gx(1,:,:))),gx);
        gy=cat(2,zeros(size(gy(:,1,:))),gy);
        gz=cat(3,zeros(size(gz(:,:,1))),gz);
    end
    if ~exist('gx','var')
        error('CBCT:image3Dnorm:InvalidInput','Unknown gradient option for the TV norm');
    end
    g=sqrt(gx.^2+gy.^2+gz.^2); clear gx gy gz;
    n=sum(g(:));
    return;
end

error('CBCT:image3Dnorm:InvalidInput','Unknown norm option');
end


function odf = calcKernelODF(ori,varargin)
% calculate ODF from individuel orientations via kernel density estimation
%
% *calcKernelODF* is one of the core function of the MTEX toolbox.
% It estimates an ODF from a set of individual crystal orientations by
% <EBSD2ODF.html kernel density estimation>.
%
% The function *calcKernelODF* has several options to control the halfwidth
% of the kernel functions, the resolution, etc. Most important the
% estimated ODF is affected by the *halfwidth* of the kernel function.
%
% If the halfwidth is large the estimated ODF is smooth whereas a small
% halfwidth results in a sharp ODF. It depends on your prior information
% about the ODF to choose this parameter right. Look at this
% <EBSD2ODF.html description> for an exhausive discussion.
%
% Syntax
%   odf = calcKernelODF(ori)
%   odf = calcKernelODF(grains.meanOrientation,'weigths',grains.area)
%   odf = calcKernelODF(ebsd.orientations,'halfwidth',5*degree)
%
% Input
%  ori - @orientation
%
% Output
%  odf - @SO3Fun
%
% Options
%  halfwidth  - halfwidth of the kernel function
%  resolution - resolution of the grid where the ODF is approximated
%  kernel     - kernel function (default -- de la Valee Poussin kernel)
%  weights    - list of weights for the orientations
%
% Flags
%  exact      - no approximation to a corser grid
%
% See also
% ebsd_demo EBSD2odf EBSDSimulation_demo EBSD/load EBSD/calcKernel kernel/kernel

% maybe there is nothing to do
if isempty(ori), odf = ODF; return, end

% extract weights
if check_option(varargin,'weights')
  weights = get_option(varargin,'weights');
  varargin = delete_option(varargin,'weights',1);
else
  weights = ones(1,length(ori));
end

% remove nan orientations and weights
weights = weights(~isnan(ori));
ori = subSet(ori,~isnan(ori));

% normalize weights
weights = weights ./ sum(weights(:));

% extract kernel function
psi = SO3DeLaValleePoussinKernel('halfwidth',10*degree,varargin{:});
psi = get_option(varargin,'kernel',psi);
hw = psi.halfwidth;

% if we have to many orientation approximate them on a grid
if length(ori) > 1000 && ~check_option(varargin,'exact')

  [ori,weights] = gridify(ori,'weights',weights,...
    'resolution',max(0.75*degree,hw / 2), varargin{:});

end

% set up exact ODF
odf = unimodalODF(ori,psi,ori.CS,ori.SS,'weights',weights);

end


function [E,J]=SynthMeasWatsonSHCylSingleRadGPD_PGSE(x, grad_dirs, G, delta, smalldel, fibredir, roots)
% Substrate: Impermeable cylinders with one radius in a homogeneous background.
% Orientation distribution: Watson's distribution with SH approximation
% Pulse sequence: Pulsed gradient spin echo
% Signal approximation: Gaussian phase distribution.
%
% [E,J]=SynthMeasWatsonSHCylSingleRadGPD_PGSE(x, grad_dirs, G, delta, smalldel, fibredir, roots)
% returns the measurements E according to the model and the Jacobian J of the
% measurements with respect to the parameters.  The Jacobian does not
% include derivates with respect to the fibre direction.
%
% x is the list of model parameters in SI units:
% x(1) is the volume fraction of the intracellular space.
% x(2) is the free diffusivity of the material inside and outside the cylinders.
% x(3) is the hindered diffusivity outside the cylinders in perpendicular directions.
% x(4) is the radius of the cylinders.
% x(5) is the concentration parameter of the Watson's distribution
%
% grad_dirs is the gradient direction for each measurement.  It has size [N
% 3] where N is the number of measurements.
%
% G, delta and smalldel are the gradient strength, pulse separation and
% pulse length of each measurement in the protocol.  Each has
% size [N 1].
%
% fibredir is a unit vector along the symmetry axis of the Watson's
% distribution.  It must be in Cartesian coordinates [x y z]' with size [3 1].
%
% roots contains solutions to the Bessel function equation from function
% BesselJ_RootsCyl.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

% Duplication with SynthMeasDistributedRadVG remains, because of the
% derivative computation.

f=x(1);
dPar=x(2);
dPerp=x(3);
R=x(4);
kappa=x(5);

% build the input x vector for hindered compartment
x_h = [dPar dPerp kappa];

% build the input x vector for restricted diffusion in Neuman cylinder model
% set diffusion coeff in restricted compartment same as parallel one in
% hindered.
x_r = [dPar R kappa];

% Synthesize measurements from model
if (nargout>1)
	[E_h, J_h] = SynthMeasWatsonHinderedDiffusion_PGSE(x_h, grad_dirs, G, delta, smalldel, fibredir);
	[E_r, J_r] = SynthMeasWatsonSHCylNeuman_PGSE(x_r, grad_dirs, G, delta, smalldel, fibredir, roots);
else
	E_h = SynthMeasWatsonHinderedDiffusion_PGSE(x_h, grad_dirs, G, delta, smalldel, fibredir);
	E_r = SynthMeasWatsonSHCylNeuman_PGSE(x_r, grad_dirs, G, delta, smalldel, fibredir, roots);
end

E=(1-f)*E_h+f*E_r;

% Compute the Jacobian matrix
if(nargout>1)

    % dE_tot/df = E_r - E_h
    dEtdf = E_r - E_h;

    % dE_tot/ddPar
    dEtddPar = (1-f)*J_h(:,1) + f*J_r(:,1);

    % dE_tot/ddPerp
    dEtddPerp = (1-f)*J_h(:,2);

    % dE_tot/dR
    dEtdr = f*J_r(:,2);

    % dE_tot/dk
    dEtdk = (1-f)*J_h(:,3) + f*J_r(:,3);

    % Construct the jacobian matrix.
    J = zeros(length(E), 5);
    J(:,1) = dEtdf;
    J(:,2) = dEtddPar;
    J(:,3) = dEtddPerp;
    J(:,4) = dEtdr;
    J(:,5) = dEtdk;
end


function [f,g] = spsurfun(y, z)
% SPSURFUN Evaluate the sparse grid interpolant at single point
%    IP = SPSURFUN(Y,Z)  Computes the interpolated value IP at
%    the single point [Y1, ..., YD] for the sparse grid
%    interpolant Z.
%
%    [IP,IPGRAD] = SPSURFUN(Y,Z)  Computes the interpolated
%    value IP and the gradient vector IPGRAD.
%
%    Example:
%       f = inline('x.^2 + y.^2 - 2.*z');
%       g1 = inline('2*x + 0*y + 0*z');
%       g2 = inline('2*y + 0*x + 0*z');
%       g3 = inline('-2  + 0*x + 0*y + 0*z');
%       z = spvals(f,3,[],spset('GridType','Chebyshev'));
%       [ip,ipgrad] = spsurfun([0.5, 0.2, 0.2], z)
%       f_exact = f(0.5, 0.2, 0.2)
%       g_exact = [g1(0.5, 0.2, 0.2); ...
%                  g2(0.5, 0.2, 0.2); ...
%                  g3(0.5, 0.2, 0.2)]
%
%    See also SPINTERP, SPVALS.
%
%    Note:
%       SPSURFUN is provided for conveniece to be used as an
%       alternative to SPINTERP, where the point Y to be
%       evaluated is given as a row or column vector. This
%       functional form is often adopted by multivariate
%       optimization algorithms (such as fminsearch) in
%       Matlab.
%       Note that this form allows the evaluation of the sparse
%       grid interpolant at a single point only. Therefore, It
%       is recommended to use SPINTERP instead if multiple
%       evaluations of the interpolant can be performed
%       simultaneously.

% Author : Andreas Klimke
% Version: 1.0
% Date   : September 8, 2006

% Change log:
% V1.0   : September 8, 2006
%          Initial version

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

if nargout <= 1
  f = spinterp(z, y);
elseif nargout == 2
 [f,g] = spinterp(z, y);
 g = g{1,1};
end


function linpack_s_test23 ( )

%*****************************************************************************80
%
%% TEST23 tests SQRDC and SQRSL.
%
%  Discussion:
%
%    SQRDC and SQRSL compute the QR factorization, and use it
%    to solve linear systems.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;
  p = 3;
  lda = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST23\n' );
  fprintf ( 1, '  For a general matrix,\n' );
  fprintf ( 1, '  SQRDC computes the QR decomposition of a\n' );
  fprintf ( 1, '  matrix, but does not return Q and R explicitly.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Show how Q and R can be recovered using SQRSL.\n' );
%
%  Set the matrix A.
%
  a = [ ...
    1.0, 1.0, 0.0; ...
    1.0, 0.0, 1.0; ...
    0.0, 1.0, 1.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The original matrix A:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : p
      fprintf ( 1, '  %14f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Decompose the matrix.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Decompose the matrix.\n' );

  job = 0;
  ipvt(1:p) = 0;

  [ a, qraux, ipvt ] = sqrdc ( a, lda, n, p, ipvt, job );
%
%  Print out what SQRDC has stored in A...
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The packed matrix A which describes Q and R:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : p
      fprintf ( 1, '  %14f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end
%
%  ...and in QRAUX.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The QRAUX vector, containing some additional\n' );
  fprintf ( 1, '  information defining Q:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %14f', qraux(i) );
  end
  fprintf ( 1, '\n' );
%
%  Print out the resulting R factor.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The R factor:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : p

      if ( j < i )
        r(i,j) = 0.0;
      else
        r(i,j) = a(i,j);
      end

    end

    for j = 1 : p
      fprintf ( 1, '  %14f', r(i,j) );
    end
    fprintf ( 1, '\n' );

  end
%
%  Call SQRSL to extract the information about the Q matrix.
%  We do this, essentially, by asking SQRSL to tell us the
%  value of Q*Y, where Y is a column of the identity matrix.
%
  job = 10000;

  for i = 1 : n
%
%  Set the vector Y.
%
    y(1:n) = 0.0;

    y(i) = 1.0;
%
%  Ask SQRSL to tell us what Q*Y is.
%
    [ qy, qty, b, rsd, xb, info ] = sqrsl ( a, lda, n, p, qraux, y, job );

    if ( info ~= 0 )
      fprintf ( 1, '  Error!  SQRSL returns INFO = %d\n', info );
      return
    end
%
%  Copy QY into the appropriate column of Q.
%
    q(1:n,i) = qy(1:n);

  end
%
%  Now print out the Q matrix we have extracted.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The Q factor:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %14f', q(i,j) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Compute Q*R to verify that it equals A.
%
  b(1:n,1:p) = q(1:n,1:n) * r(1:n,1:p);
%
%  Print the result.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The product Q * R:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : p
      fprintf ( 1, '  %14f', b(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function out=prox_max(x,alpha)
%PROX_MAX computes the proximal operator of the function alpha*max(x(:))
%
%  Usage:
%  out = PROX_MAX(x,alpha)
%  ===========================================
%  INPUT:
%  x - point to be projected (vector/matrix)
%  alpha - positive scalar
%  ===========================================
%  Output:
%  out - proximal operator at x

% This file is part of the FOM package - a collection of first order methods for solving convex optimization problems
% Copyright (C) 2017 Amir and Nili Beck
%
% 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.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

if (nargin < 2)
    error ('usage: prox_max(x,alpha)') ;
end

if (alpha < 0)
    error('usage: prox_max(x,alpha) - alpha should be positive')
end

out = x - alpha * proj_simplex (x/alpha,1,'eq') ;

end


function [U] = hyperNfindr(M, q)
% HYPERNFINDR Performs the N-FINDR (endmember extraction) algorithm
%   Performs the N-FINDR algorithm to find q endmembers. If only M is
%  given as input, this function calls hyperHfcVd to estimate the number
%  of endmembers (q) and then hyperPct to reduce dimensionality to (q-1).
%
% Usage
%   [U] = hyperNfindr(M)
%   [U] = hyperNfindr(M, q)
% Inputs
%   M - 2d matrix of HSI data (p x N)
%   q - Number of endmembers to find
%       -- if not given, q is obtained from hyperHfcVd(M, 10^-3)
% Outputs
%   U - Recovered endmembers (p x q)
%
% References
%   M. Winter, "N-findr: an algorithm for fast autonomous
% spectral endmember determination in hyperspectral data," SPIE’s
% International Symposium on Optical Science, Engineering, and
% Instrumentation, pages 266–275. International Society for Optics
% and Photonics, 1999.

% Error trapping
if ndims(M) ~= 2
    warning('WarnTests:dim', ...
            'Input image must be p x N.\n',...
            'Converting with hyperConvert2d.\n')
    M = hyperConvert2d(M);
end

M_orig = M;
[p, N] = size(M);

if nargin == 1
    fprintf('Implementing hyperHfcVd to determine the number of endmembers.\n')
    q = hyperHfcVd(M_orig, [10^-3]);
    fprintf('Reducing dimensionality to (q-1) using hyperPct.\n')
    M = hyperPct(M, q-1);
elseif q < p+1
    warning('WarnTests:dim', ...
    strcat('N-FINDR requires (q-1) spectral bands.\n',...
           'Performing PCA to reduce dimensionality.\n'))
    M = hyperPct(M, q-1);
elseif q > p+1
    warning('WarnTests:dim', ...
    strcat('N-FINDR requires (q-1) spectral bands.\n',...
           'Performing PCA to reduce dimensionality.\n'))
    error('ErrTests:dim', ...
        strcat('N-FINDR cannot find more than (p+1) endmembers (q),\n', ...
               'where p is the number of available spectral bands.\n'))
end

% Initialize
U_idx = randperm(N,q); % Random endmember selection
E     = M(:,U_idx);    % Endmember matrix
V     = abs(det([ones(1,q); E])) / factorial(q-1); % Simplex volume
vols  = zeros(q,1);

% Search for maximum volume simplex
for j = 1:N;
    % Replace each column of E with sample vector M(:,j)
    % and compute the volume for each
    for k = 1:q;
        E_tmp      = E;
        E_tmp(:,k) = M(:,j);
        vols(k)    = abs(det([ones(1,q); E_tmp])) / factorial(q-1);
    end
    % If max volume is greater than previous V, update E and V
    [V_tmp,k_idx] = max(vols);
    if V_tmp > V
        V            = V_tmp;
        E(:,k_idx)   = M(:,j);
        U_idx(k_idx) = j;
    end
end

% Return endmembers
U = M_orig(:, U_idx);


function [p sres sres_ns] = ResidScan(res, FWHM)
% function [p sres sres_ns] = ResidScan(res, FWHM)
%
% Calculates P(M>=t) where M is the max value of the smoothed residuals.
% In this implementation the residuals are smoothed using a Gaussian
% kernel.
%
% INPUT:
%
% res - residual time course
% FWHM - Full Width Half Maximum (in time units)
%
% OUTPUT:
%
% p - pvalues
% sres - smoothed residuals
% sres_ns - smoothed residuals (non standardized)
%
% By Martin Lindquist & Ji-Meng Loh, July 2007
%
% Edited by ML on 10/02/09

res_ns = res;
res = res./std(res);
len = length(res);

% Create Gaussian Kernel
sig = ceil(FWHM/(2*sqrt(2*log(2))));
klen = 3*sig;
kern = normpdf((-klen:klen),0,sig);
kern = kern./sqrt(sum(kern.^2));

% Convolve
x = conv(res,kern);
sres = x((klen + 1):(end-klen));

x = conv(res_ns,kern/sum(kern));
sres_ns = x((klen + 1):(end-klen));

% Find Max value
[a,location] = max(abs(sres));

% Find p-values using Gaussian Random Field theory
z = Euler_p(1, a, len, FWHM);
z = 2*z;        %Two-sided test
p = min(1, z);

end

% END MAIN FUNCTION

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subfunctions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function pval = Euler_p(myDim, value, N, fwhm)
% function z = Euler_p(myDim, value, N, fwhm)
%
% Finds the p value using the expected Euler characteristic.
%
% This function returns P(M \ge value) using the approximation
% \sum_{d=0}^D R_d(V) \rho_d(value) following Worsley et al's "A Unified
% Statistical Approach for Determining Significant Signals in Images of
% Cerebral Activation".
%
% INPUTS:
%
% myDim - the number of dimensions in the data
% value - the value of the maximum.
% N     - the number of (time) points in that 1 dimension
% fwhm  - the full width half maximum
%
% OUTPUTS:
%
% pval - the p-value

% NOTE: CURRENTLY THIS FUNCTION IS ONLY IMPLEMENTED FOR THE 1D CASE

  % Constants
  myfactor = 4*log(2);
  pi2 = 2*pi;
  exptsq = exp(-(value^2)/2);

  % Euler Characteristc Densties
  rho = zeros(5,1);
  rho(1) = 1-normcdf(value);
  rho(2) = myfactor^(0.5)*exptsq/pi2;
  rho(3) = myfactor * exptsq * value / (pi2 ^ (1.5));
  rho(4) = myfactor ^ (1.5) * exptsq * (value^2-1) / (pi2 ^2);
  rho(5) = myfactor ^2 * exptsq * (value^3-3*value) / (pi2 ^ (5/2));

  % Resel Count
  R0 = 1;
  R1 = N/fwhm;

  % P-value
  pval = R0 * rho(1) + R1 * rho(2);

end


function [ g , mu ] = gsp_design_translates(G, g0,N )
%GSP_DESIGN_TRANSLATES Create a filterbank by uniformly translating a window
%   Usage: g = gsp_design_translates( G, g0, Ntrans );
%
%   Inputs parameters:
%       G       : Graph structure
%       g0      : Mother window (anonymous function)
%       N       : Number of translate
%
%   Outputs parameters:
%       g       : filterbank
%       mu      : Centers of the filters
%
%   This function construct a filter bank of *N* uniformly translated
%   filter from the mother filter *g0*.
%

% Author : Nathanael Perraudin
% Date: 6 January 2016

if isstruct(G)
    if ~isfield(G,'lmax')
        if param.verbose
            fprintf('GSP_DESIGN_TRANSLATE has to compute lmax \n')
        end
        G = gsp_estimate_lmax(G);
    end
   lmax = G.lmax;
else
   lmax = G;
end

mu = linspace(0,lmax,N);

g = cell(length(mu),1);

for ii = 1:length(mu)
    g{ii} = @(x) g0(x-mu(ii));
end

end


function b = r83_mv ( m, n, a, x )

%*****************************************************************************80
%
%% R83_MV multiplies an R83 matrix times an R8VEC.
%
%  Discussion:
%
%    The R83 storage format is used for a tridiagonal matrix.
%    The superdiagonal is stored in entries (1,2:N), the diagonal in
%    entries (2,1:N), and the subdiagonal in (3,1:N-1).  Thus, the
%    original matrix is "collapsed" vertically into the array.
%
%  Example:
%
%    Here is how an R83 matrix of order 5 would be stored:
%
%       *  A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(3,N), the R83 matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(M), the product A * x.
%
  b = zeros ( m, 1 );

  mn = min ( m, n );

  if ( n == 1 )
    b(1,1) = a(2,1) * x(1,1);
    if ( 1 < m )
      b(2,1) = a(3,1) * x(1,1);
    end
    return
  end

  b(1,1)      = a(2,1)       * x(1,1) ...
              + a(1,2)       * x(2,1);

  b(2:mn-1,1) = a(3,1:mn-2)' .* x(1:mn-2,1) ...
              + a(2,2:mn-1)' .* x(2:mn-1,1) ...
              + a(1,3:mn)'   .* x(3:mn,1);

  b(mn,1)     = a(3,mn-1)    * x(mn-1,1) ...
              + a(2,mn)      * x(mn,1);

  if ( n < m )
    b(mn+1,1) = b(mn+1,1) + a(3,mn) * x(mn,1);
  end

  if ( m < n )
    b(mn,1) = b(mn,1) + a(1,mn+1) * x(mn+1,1);
  end

  return
end


function [ final_dist ] = re_ranking( feat, M, W, query_num, k1, k2, lambda)
% k-reciprocal re-ranking

%% initial ranking list
original_dist = MahDist(M, feat' * W, feat' * W);
[~, initial_rank] = sort(original_dist, 2, 'ascend');
gallery_num = size(original_dist,2);
%% compute k-reciprocal feature
V = zeros(size(original_dist), 'single');
original_dist = original_dist./ repmat(max(original_dist, [], 2), 1, size(original_dist, 2));
for i = 1:size(original_dist, 1)
    %% k-reciprocal neighbors
    forward_k_neigh_index = initial_rank(i, 1: k1 + 1);
    backward_k_neigh_index  = initial_rank(forward_k_neigh_index, 1: k1+1);
    [fi, ~, ~]= find(backward_k_neigh_index == i);
    k_reciprocal_index  = forward_k_neigh_index(fi);
    k_reciprocal_expansion_index = k_reciprocal_index;
    for j = 1: length(k_reciprocal_index)
        candidate = k_reciprocal_index(j);
        candidate_forward_k_neigh_index = initial_rank(candidate, 1: round((k1+1)/2));
        candidate_backward_k_neigh_index = initial_rank(candidate_forward_k_neigh_index, 1: round((k1+1)/2));
        [fi_candidate, ~, ~]= find(candidate_backward_k_neigh_index == candidate);
        candidate_k_reciprocal_index = candidate_forward_k_neigh_index(fi_candidate);
        if length(intersect(k_reciprocal_index, candidate_k_reciprocal_index)) > 2/3*length(candidate_k_reciprocal_index)
            k_reciprocal_expansion_index = [k_reciprocal_expansion_index candidate_k_reciprocal_index];
        end
    end
    k_reciprocal_expansion_index = unique(k_reciprocal_expansion_index);
    %% feature encoding
    weight = exp(-original_dist(i, k_reciprocal_expansion_index));
    V(i, k_reciprocal_expansion_index) = weight/sum(weight);
end
%% local query expansion
if k2 ~=1
    V_qe = zeros(size(V), 'single');
    for i = 1:size(V, 1)
        V_qe(i, :) = single(mean(V(initial_rank(i, 1:k2), :), 1));
    end
    V = V_qe;
    V_qe = [];
end

%% Inverted Index
% Inpsired by Song Bai, Xiang Bai,
% Sparse Contextual Activation for Efficient Visual Re-ranking, TIP, 2016.
% We apply the inverted index to quickly compute the Jaccard distance.

invIndex = cell(gallery_num, 1);
for i = 1:gallery_num
    invIndex{i} = find(V(:, i) ~=0);
end

jaccard_dist = zeros(size(original_dist), 'single');

for i = 1:query_num
    temp_min = zeros(1, gallery_num, 'single');
    indNonZero = find( V( i, : ) ~= 0 );
    indImages = invIndex( indNonZero );
    for j = 1 : length( indNonZero )
        temp_min( 1, indImages{j} ) = temp_min( 1, indImages{j} )...
            + single( min( V(i, indNonZero(j)), V(indImages{j}, indNonZero(j)) ) )';
    end
    jaccard_dist(i, :) = bsxfun(@minus, 1, temp_min./(2 - temp_min));
end

final_dist = jaccard_dist*(1-lambda) + original_dist*lambda;
final_dist = final_dist(1:query_num,query_num+1:end)';

end


function disp (F)
%DISP displays the factorization F
%
% Example
%   F = factorize (A)
%   disp (F)
%
% See also factorize.

% Copyright 2009, Timothy A. Davis, University of Florida

fprintf ('  A:\n') ;
disp (F.A) ;

if (~isempty (F.L))
    fprintf ('  L:\n') ;
    disp (F.L) ;
end

if (~isempty (F.U))
    fprintf ('  U:\n') ;
    disp (F.U) ;
end

if (~isempty (F.Q))
    fprintf ('  Q:\n') ;
    disp (F.Q) ;
end

if (~isempty (F.R))
    fprintf ('  R:\n') ;
    disp (F.R) ;
end

if (~isempty (F.p))
    fprintf ('  p:\n') ;
    disp (F.p) ;
end

if (~isempty (F.q))
    fprintf ('  q:\n') ;
    disp (F.q) ;
end

fprintf ('  is_inverse: %d kind: %d\n', F.is_inverse, F.kind) ;

% print the kind of factorization that F contains

switch F.kind

    case 1

        fprintf ('  Q-less economy sparse QR factorization: ') ;
        fprintf ('(A*q)''*A*q = R''*R\n');

    case 2

        fprintf ('  dense economy QR factorization: A = Q*R\n') ;

    case 3

        fprintf ('  Q-less economy sparse QR factorization: ') ;
        fprintf ('(p*A)*(p*A)'' = R''*R\n') ;

    case 4

        fprintf ('  dense economy QR factorization: A'' = Q*R\n') ;

    case 5

        fprintf ('  sparse Cholesky factorization: ')  ;
        fprintf ('q*A*q'' = L*L''\n');

    case 6

        fprintf ('  dense Cholesky factorization: A = L*L''\n') ;

    case 7

        fprintf ('  sparse LU factorization: p*A*q = L*U\n') ;

    case 8

        fprintf ('  dense LU factorization: p*A = L*U\n') ;

end


function new = colormap_optimization(cmap)
%colormap_optimisation: minimally modify colormap to satisfy constraints.
% In this case, the constraints are for linear grayscale luminance and a
% neutral mid-gray central color.
%
% Example:
%  sprung = colormap_optimization(spring(5));
%  colormap_visualization(spring(5))
%  colormap_visualization(sprung)
%
% See also: colormap_investigation, colormap_visualization, bipolar

% Copyright 2009 Ged Ridgway at gmail com

%% Conversion from RGB to grayscale brightness/luminance

% From http://www.poynton.com/ColorFAQ.html (Q.9)
% coef = [0.2126 0.7152 0.0722]'

% From rgb2gray:
% T = inv([1.0 0.956 0.621; 1.0 -0.272 -0.647; 1.0 -1.106 1.703]);
% coef = T(1,:)';

% From  http://gimp-savvy.com/BOOK/index.html?node54.html
coef = [0.3 0.59 0.11]';
% Note, the Gimp coef is almost identical to that from rgb2gray, and these
% seem nicer in a print('-dps') grayscale copy than the Poynton version.

%% Constraints
m = size(cmap, 1);
mid = zeros(1, m);
mid(round((m+1)/2)) = 1;
Aeq = [
    kron(coef', eye(m)) % linearity in luminance
    kron(eye(2,3), mid) % centrality
    ];
l = 0.1; % lower luminance (avoid black, to give contrast with text/lines)
u = 0.9; % upper luminance (avoid white, to give contrast with background)
beq = [
    linspace(l, u, m)'  % linearity in luminance
    ones(2, 1) / 2      % centrality
    ];

%% Optimisation (using MATLAB optimisation toolbox)
orig = warning('off', 'optim:lsqlin:LinConstraints');
x = lsqlin(eye(3*m), cmap(:), [], [], Aeq, beq, zeros(3*m, 1), ones(3*m, 1));
x = max(min(x, 1), 0); % enforce bounds, since lsqlin can give e.g. -1e-18
new = reshape(x, m, 3);
warning(orig);


function [ SV ] = moveFV(V,F,S)
% MOVEFV  Move a scalar field defined on faces to vertices by averaging
%
% [ SV ] = moveFV(V,F,S)
%
% Input:
%   V,F  mesh
%   S  scalar field defined on faces, Fx1
%
% Output:
%   SV  scalar field defined on vertices

SV = zeros(size(V,1),size(S,2));
COUNT = zeros(size(V,1),1);

for i=1:size(F,1)
    SV(F(i,:)',:) = SV(F(i,:)',:) + repmat(S(i,:),size(F,2),1);
    COUNT(F(i,:)') = COUNT(F(i,:)') + 1;
end

SV = SV ./ repmat(COUNT, 1, size(S,2));

end


function [M, L, Y, C] = lmnn(X, labels)
%LMNN Learns a metric using large-margin nearest neighbor metric learning
%
%   [M, L, Y, C] = lmnn(X, labels)
%
% The function uses large-margin nearest neighbor (LMNN) metric learning to
% learn a metric on the data set specified by the NxD matrix X and the
% corresponding Nx1 vector labels. The metric is returned in M.
%
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    % Initialize some variables
    [N, D] = size(X);
    assert(length(labels) == N);
    [lablist, ~, labels] = unique(labels);
    K = length(lablist);
    label_matrix = false(N, K);
    label_matrix(sub2ind(size(label_matrix), (1:length(labels))', labels)) = true;
    same_label = logical(double(label_matrix) * double(label_matrix'));
    M = eye(D);
    C = Inf; prev_C = Inf;

    % Set learning parameters
    min_iter = 50;          % minimum number of iterations
    max_iter = 1000;        % maximum number of iterations
    eta = .1;               % learning rate
    mu = .5;                % weighting of pull and push terms
    tol = 1e-3;             % tolerance for convergence
    best_C = Inf;           % best error obtained so far
    best_M = M;             % best metric found so far
    no_targets = 3;         % number of target neighbors

    % Select target neighbors
    sum_X = sum(X .^ 2, 2);
    DD = bsxfun(@plus, sum_X, bsxfun(@plus, sum_X', -2 * (X * X')));
    DD(~same_label) = Inf; DD(1:N + 1:end) = Inf;
    [~, targets_ind] = sort(DD, 2, 'ascend');
    targets_ind = targets_ind(:,1:no_targets);
    targets = false(N, N);
    targets(sub2ind([N N], vec(repmat((1:N)', [1 no_targets])), vec(targets_ind))) = true;

    % Compute pulling term between target neigbhors to initialize gradient
    slack = zeros(N, N, no_targets);
    G = zeros(D, D);
    for i=1:no_targets
        G = G + (1 - mu) .* (X - X(targets_ind(:,i),:))' * (X - X(targets_ind(:,i),:));
    end

    % Perform main learning iterations
    iter = 0;
    while (prev_C - C > tol || iter < min_iter) && iter < max_iter

        % Compute pairwise distances under current metric
        XM = X * M;
        sum_X = sum(XM .* X, 2);
        DD = bsxfun(@plus, sum_X, bsxfun(@plus, sum_X', -2 * (XM * X')));

        % Compute value of slack variables
        old_slack = slack;
        for i=1:no_targets
            slack(:,:,i) = ~same_label .* max(0, bsxfun(@minus, 1 + DD(sub2ind([N N], (1:N)', targets_ind(:,i))), DD));
        end

        % Compute value of cost function
        prev_C = C;
        C = (1 - mu) .* sum(DD(targets)) + ...  % push terms between target neighbors
                 mu  .* sum(slack(:));          % pull terms between impostors

        % Maintain best solution found so far (subgradient method)
        if C < best_C
            best_C = C;
            best_M = M;
        end

        % Perform gradient update
        for i=1:no_targets

            % Add terms for new violations
            [r, c] = find(slack(:,:,i) > 0 & old_slack(:,:,i) == 0);
            G = G + mu .* ((X(r,:) - X(targets_ind(r, i),:))' * ...
                           (X(r,:) - X(targets_ind(r, i),:)) - ...
                           (X(r,:) - X(c,:))' * (X(r,:) - X(c,:)));

            % Remove terms for resolved violations
            [r, c] = find(slack(:,:,i) == 0 & old_slack(:,:,i) > 0);
            G = G - mu .* ((X(r,:) - X(targets_ind(r, i),:))' * ...
                           (X(r,:) - X(targets_ind(r, i),:)) - ...
                           (X(r,:) - X(c,:))' * (X(r,:) - X(c,:)));
        end
        M = M - (eta ./ N) .* G;

        % Project metric back onto the PSD cone
        [V, L] = eig(M);
        V = real(V); L = real(L);
        ind = find(diag(L) > 0);
        if isempty(ind)
            warning('Projection onto PSD cone failed. All eigenvalues were negative.'); break
        end
        M = V(:,ind) * L(ind, ind) * V(:,ind)';
        if any(isinf(M(:)))
            warning('Projection onto PSD cone failed. Metric contains Inf values.'); break
        end
        if any(isnan(M(:)))
            warning('Projection onto PSD cone failed. Metric contains NaN values.'); break
        end

        % Update learning rate
        if prev_C > C
            eta = eta * 1.01;
        else
            eta = eta * .5;
        end

        % Print out progress
        iter = iter + 1;
        no_slack = sum(slack(:) > 0);
        if rem(iter, 10) == 0
            [~, sort_ind] = sort(DD, 2, 'ascend');
            disp(['Iteration ' num2str(iter) ': error is ' num2str(C ./ N) ...
                  ', nearest neighbor error is ' num2str(sum(labels(sort_ind(:,2)) ~= labels) ./ N) ...
                  ', number of constraints: ' num2str(no_slack)]);
        end
    end

    % Return best metric and error
    M = best_M;
    C = best_C;

    % Compute mapped data
    [L, S, ~] = svd(M);
    L = bsxfun(@times, sqrt(diag(S)), L);
    Y = X * L;
end

function x = vec(x)
    x = x(:);
end


function [ CostMatric,Cost ] =  MinimalDirectedMSF( CostMatric )
% MST on a directed graph
% Chu-Liu/Edmonds Algorithm:
%1 Discard the arcs entering the root if any; For each node other than the root, select the entering arc with the highest cost; Let the selected n-1 arcs be the set S.
%  If no cycle formed, G( N,S ) is a MST. Otherwise, continue.
%2 For each cycle formed, contract the nodes in the cycle into a pseudo-node (k), and modify the cost of each arc which enters a node (j) in the cycle from some node (i)
%  outside the cycle according to the following equation.  c(i,k)=c(i,j)-(c(x(j),j)-min_{j}(c(x(j),j)) where c(x(j),j) is the cost of the arc in the cycle which enters j.
%3 For each pseudo-node, select the entering arc which has the smallest modified cost; Replace the arc which enters the same real node in S by the new selected arc.
%4 Go to step 2 with the contracted graph.

% Pay attention, we extend the Chu-Liu algorithm into considering
% forest.And we consider of discarding the premise of giving the root here.
% CostMatric must be symmetric

  [ Number, Component ] = conncomp(  biograph( CostMatric ),'Weak', true );
  for k = 1:Number
      LocalNodes = find( Component == k );
      Dim = size( LocalNodes,2 );
      if Dim == 1
         continue
      elseif Dim == 2
          if CostMatric( LocalNodes(1),LocalNodes(2) ) > CostMatric( LocalNodes(2),LocalNodes(1) )
              CostMatric( LocalNodes(2),LocalNodes(1) ) = 0;
          else
              CostMatric( LocalNodes(1),LocalNodes(2) ) = 0;
          end
      elseif Dim > 2
      ComponentCost = CostMatric( LocalNodes,LocalNodes );
      MinTree = zeros( Dim ); MinTreeCost = Inf;
      for m = 1:Dim
          Root = m;
          LocalCostMatric = ComponentCost;
          LocalCostMatric( :,Root ) = zeros( Dim,1 );
          [ LocalTree,LocalCostTree ] =  DirectedMinimalSpanningTree( LocalCostMatric,Root );
          if MinTreeCost > LocalCostTree
             MinTree = LocalTree;
             MinTreeCost = LocalCostTree ;
          end
      end
      CostMatric( LocalNodes,LocalNodes ) = MinTree;
      end
   end
   Cost = sum( sum( CostMatric ));
end


% NOLM_Attractor - Plot a chaotic attractor for the NOLM.
% Copyright Springer 2013 A.L. Steele and S. Lynch.
clear
N=50000;P=100;start=500;
lambda=1.55E-6;n2=3.2E-20;Aeff=30E-12;L=80;
E1(1)=0;phi=0*pi;
Eout(1:N)=0;
phif=0*pi; E6p=0;

kappa1=0.25;kappa2=0.8;kappa3=0.8;
rootk1 = sqrt(kappa1); irootk1 = 1i*sqrt(1-kappa1);
rootk2 = sqrt(kappa2); irootk2 = 1i*sqrt(1-kappa2);
rootk3 = sqrt(kappa3); irootk3 = 1i*sqrt(1-kappa3);

G=sqrt((1-kappa2)*(1-kappa3));
% iterate
for n=1:N

    E1 = rootk3*sqrt(P)+irootk3*E6p;
    P1 = abs(E1)^2;

    E3 = rootk1*E1;
    E4 = irootk1*E1;

    phic=2*pi*n2*L*(2-kappa1)*P1/(lambda*Aeff);
    phicc=2*pi*n2*L*(1+kappa1)*P1/(lambda*Aeff);

    E3p = E3*exp(-1i*(phi+phic));
    E4p = E4*exp(-1i*(phi+phicc));

    E5 = rootk1*E3p+irootk1*E4p;

    Eout(n) = rootk2*E5;
    x(n)=real(Eout(n));
    y(n)=imag(Eout(n));
    E6 = irootk2*E5;

    E6p = E6*exp(1i*phif);

end
axis([-10 10 -10 10])
axis equal
plot(x(start:N),y(start:N),'.','MarkerSize',1);
fsize=15;
set(gca,'xtick',-10:5:10,'FontSize',fsize)
set(gca,'ytick',-10:5:10,'FontSize',fsize)
xlabel('Real E','FontSize',fsize)
ylabel('Imag E','FontSize',fsize)


function E = energyVector(C,x,V,P,varargin)
% Calculates Energy velocity vector (km/s)
%
% Description
% Energy velocity for lossless elastic medium (i.e. no attenuation)
% Good proxy for group velocity, which typically has some energy loss
% The formula is given by
% F.I. Fedorov(1968)Theory of Elastic Waves in Crystals, 375 pp. New York: Penum Press.
%
% Ve_i = C_ijkl P_j P_l X_k / rho*V
%
% N.B. E_magnitude should be equal or more than plane wave velocity vp, vs1 or vs2
%
% David Mainprice 6/02/2018
%
% Syntax
%   E = energyVector(C,x,v,p)
%   E = energyVector(C,x,v,p,rho)
%   E = energyVector(C,[],vFun,pFun)
%   E = energyVector(C,[],vFun,pFun,rho)
%
% Input
%  C - @stiffnessTensor (units GPa)
%  x - @vector3d propagation direction
%  v - plane wave velocity (unit km/s) e.g. vp,vs1 or vs2
%  p - @vector3d plane wave polarization vector e.g. pp,ps1 or ps2
%  vFun - @S2Fun plane wave velocity (unit km/s) e.g. vp,vs1 or vs2
%  pFun - @S2AxisField plane wave polarization vector e.g. pp,ps1 or ps2
%  rho - density in g/cm3
%
% Output
%  E - Energy velocity vector (units km/s)
%

% return a function if required
if isempty(x)
  if isa(V,'S2FunTri')
    E = S2VectorFieldTri(V.tri,energyVector(C,V.vertices,V.values,P.values,varargin{:}));
  else
    E = S2VectorFieldHarmonic.quadrature(@(x) energyVector(C,x,V,P,varargin{:}),'bandwidth',128,C.CS);
  end
  return
end

% make X and P to be unit vectors
x = x.normalize;

if ~isa(P,'vector3d'), P = P.eval(x); end
if ~isnumeric(V), V = V.eval(x); end
P = P.normalize;

% get density
if nargin == 5
  rho = varargin{1};
elseif isfield(C.opt,'density')
  rho = C.opt.density;
else
  rho = 1;
  warning('No density given! I''m going to use the density rho=1.');
end

% E_vector
E = vector3d(EinsteinSum(C,[1 -2 -3 -4],P(:),-2,P(:),-4,x(:),-3))./rho ./V(:);

end


% vgg_line3d_from_lP_nonlin  Non-linear estimation of (possibly constrained) 3D line segment from image line segments.
%
% SYNOPSIS
% L = vgg_line3d_from_lP_nonlin(s,P [,imsize] [,L0] [,X] [,nonlin_opt])
%
% s ... cell(K) of double(3,3), inv. covariance matrices of the K image line segments:-
%   - If the segments are estimated from edges, it is s(:,k) = x*x',
%     where x (3-by-N) are homog. coordinates of the edgels with last components 1.
%   - If only end points are available, s(:,k) = d*x*y' where x, y (column 2-vectors)
%     are the segment's end points and d its length.
%
% P ... K-cell with 3-by-4 camera matrices
%
% imsize ... size (2,K), image size(s) for preconditiong.
%   Omit if s and P are already preconditioned.
%
% L0 ... double(4,2), initial scene line (optional). Homogeneous points L0(:,i) span
%   the line. If omitted, linear estimation is done first.
%
% X ... constraint on L :-
%   - if X is omitted:     no constraint on L
%   - if X is double(4,1): L goes thru point X
%   - if X is double(4,2): L goes thru 3D line spanned by X(:,i)
%
% nonlin_opt ... options for Levenberg-Marquardt. It is comma-separated list
%   of pairs ['option',value]. Possible options are :-
% opt ... options structure with possible fields :-
%   - verbose ... 1 or 0 (default: 0)
%   - niter_term ... maximum number of iterations
%   - rmsstep_term ... terminating step of rms of residuals
%   - lambda_term ... terminating value of lambda (default: 1e10)
%   - lambda_init ... initial value of lambda
% E.g., vgg_line3d_from_lP_nonlin(...,'lambda_init',1e-9,'niter_term',5).
%
% L ... double(4,2), estimated 3D line. Points L(:,i) span the line.
%
% Note: use [] if you want to omit a parameter and use a later one, e.g.
%   vgg_line3d_from_lP_nonlin(s,P,imsize,[],[],'verbose',1,'lam_init',1e-9)
%
% ALGORITHM
% - Minimization is done by Levenberg-Marquardt.
% - 3D line L is parameterized by image lines in the first two images.
%   The positions of these image lines are possibly constrained by X.

% T.Werner, Feb 2002

function L = vgg_line3d_from_lP_nonlin(s,P,imsize,L,X,varargin)

if nargin < 3, imsize = []; end
if nargin < 4, L = []; end
if nargin < 5, X = []; end

if isempty(L)
  L = vgg_line3d_from_lP_lin(s,P,imsize);
end
K = length(P); % number of images
if K<2
  error('Cannot reconstruct 3D line from 1 image');
end
if isempty(X) & K==2 % no need for non-linear minimization
  return
end

% Prepare square root of covariance matrices; now s{k}(:,n) has meaning of 3 homogeneous image points
for k = 1:K
  [us,ss,vs] = svd(s{k},0);
  s{k} = us*sqrt(ss);
end

% Preconditioning
if ~isempty(imsize)
  for k = 1:K
    H = vgg_conditioner_from_image(imsize(:,k));
    P{k} = H*P{k};
    s{k} = H*s{k};
    scale(k) = H(1,1); % save the scales for evaluating objective function
  end
else
  scale = ones(1,K);
end

switch size(X,2)
 case 0
  % Scene line L is unconstrained, having thus 4 DOF.
  % L is parameterized by two image lines in images 1 and 2, each having 2 DOF, as follows:
  %   l1 = l0(1,:) + p(1:2)'*ldelta{1}
  %   l2 = l0(2,:) + p(3:4)'*ldelta{2}
  % where row 4-vector p represents 4 DOF of L.
  for k = 1:2
    l0(k,:) = normx(vgg_wedge(P{k}*L)')';
    ldelta{k} = null(l0(k,:))';
  end

  % optimization
  p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta},...
              @normsolve,...
              [0;0;0;0],...
              varargin{:});
  l = l12_from_p(p,l0,ldelta);

 case 1
  % Scene line L is constrained to intersect the scene point X, having thus 2 DOF.
  % L is parameterized by two image lines in images 1 and 2, each having 1 DOF, as follows:
  %   l1 = l0(1,:) + p(1)*ldelta{1}
  %   l2 = l0(2,:) + p(2)*ldelta{2}
  % where 2-vector p represents 2 DOF of L.
  for k = 1:2
    l0(k,:) = normx(vgg_wedge(P{k}*L)')';
    x = P{k}*X;

    % Since L might not intersect X, move l0(k,:) 'as little as possible' to intersect x.
    l0(k,:) = l0(k,:) - (l0(k,:)*x)/(x'*x).*x';

    Q = null(x')';
    ldelta{k} = null(l0(k,:)*pinv(Q))'*Q;
  end

  % optimization
  p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta},...
              @normsolve,...
              [0;0],...
              varargin{:});
  l = l12_from_p(p,l0,ldelta);

 case 2
  % Scene line L is constrained to intersect the scene line given by 2 points X.
  % This constraint is given by
  %   l1*G*l2' = 0
  % where G is 3x3 rank 2 matrix (analogical in fact to fundamental matrix) and
  %   G = P{1}*M*P{2}'
  % where M is Pluecker matrix of line given by X, M = X(:,1)*X(:,2)'-X(:,2)*X(:,1).
  %
  % This constraint can be written as
  %   p(1:2)*D*p(3:4)' + p(1:2)*d{2}' + d{1}*p(3:4)' + c = 0
  % where D, d, c are given below and 4-vector p are 4 parameters of L.
  %
  % L is parameterized by two image lines in images 1 and 2, each having 2 DOF, as follows:
  %   l1 = l0(1,:) + p(1:2)'*ldelta{1}
  %   l2 = l0(2,:) + p(3:4)'*ldelta{2}
  % where p(1:3) are chosen freely and p(4) is computed from the above formula as
  %   p(4) = -(p(1:2)'*(D(:,1)*p(3)+d{1})+p(3)*d{2}(1)+c)/(p(1:2)'*D(:,2)+d{2}(2)).
  Lpm = X(:,1)*X(:,2)' - X(:,2)*X(:,1)';
  G = P{1}*Lpm*P{2}';
  for k = 1:2
    l0(k,:) = normx(vgg_wedge(P{k}*L)')';
  end

  % As L might not intersect line X, move l0(2,:) 'as little as possible' to enforce l0(1,:)*G*l0(2,:)'==0.
  x = (l0(1,:)*G)';
  l0(2,:) = l0(2,:) - (l0(2,:)*x)/(x'*x).*x';

  for k = 1:2
    ldelta{k} = null(l0(k,:))';
  end

  D = ldelta{1}*G*ldelta{2}';
  d{1} = ldelta{1}*G*l0(2,:)';
  d{2} = ldelta{2}*G'*l0(1,:)';
  c = l0(1,:)*G*l0(2,:)';

  % optimization
  p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta,D,d,c},...
              @normsolve,...
              [0;0;0],...
              varargin{:});
  l = l12_from_p(p,l0,ldelta,D,d,c);

end
if all(~isnan(l(:)))
  L = null([l(1,:)*P{1}; l(2,:)*P{2}]);
end

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% objective function

% Objective function of Levenberg-Marquardt
function [y,w,J] = F(p,P,s,scale,varargin)
K = length(s);

% l := lines in images 1, 2 from p
l = l12_from_p(p,varargin{:});

% l := reprojection of lines in images 1, 2 to all images
if all(abs(p) < inf)
  [dummy,dummy,L]= svd([l(1,:)*P(1:3,:); l(2,:)*P(4:6,:)],0);
else
  L = inf*ones(4);
end
l = norml(vgg_wedge(reshape(P*L(:,3),[3 K]),reshape(P*L(:,4),[3 K])));

% compute residual function
y = [];
for k = 1:K
  y = [y l(k,:)*s{k}];
end
y = y';

w = [1;1;1] * (1./scale);
w = w(:);

% else, compute also jacobian
if nargout < 2
  return
end
dif = 1e-6;
J = zeros(length(y),length(p));
for i = 1:length(p)
  pdif = p;
  pdif(i) = pdif(i) + dif;
  J(:,i) = (F(pdif,P,s,scale,varargin{:}) - y)/dif;
end
return

% The following function computes lines in the first two images
% from parameters p. Explanation see above.
function l = l12_from_p(p,l0,ldelta,D,d,c)
switch length(p)
 case 4 % unconstrained
  l = [l0(1,:) + p(1:2)'*ldelta{1}
       l0(2,:) + p(3:4)'*ldelta{2}];
 case 2 % going thru X
  l = [l0(1,:) + p(1)*ldelta{1}
       l0(2,:) + p(2)*ldelta{2}];
 case 3 % going thru L
  p(4) = -(p(1:2)'*(D(:,1)*p(3)+d{1})+p(3)*d{2}(1)+c)/(p(1:2)'*D(:,2)+d{2}(2));
  l = [l0(1,:) + p(1:2)'*ldelta{1}
       l0(2,:) + p(3:4)'*ldelta{2}];
end
return

function dp = normsolve(J,Y,w,lambda)
OLDWARN = warning('off');
dp = -( J'*diag(w)*J + lambda*eye(size(J,2)) ) \ ( J'*(Y.*w) );
warning(OLDWARN);
return

function x = normx(x)
if ~isempty(x)
  x = x./(ones(size(x,1),1)*sqrt(sum(x.*x)));
end

function l = norml(l)
% l = norml(l)  Multiplies hyperplane l by scalar so that for each n, norm(l(1:end-1,n))==1.
l = l./(sqrt(sum(l(:,1:end-1).^2,2))*ones(1,size(l,2)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% a = levmarq(@RES,PARAMS,@NORMSOLVE,a [,opt])  Non-linear least-squares by Levenberg-Marquardt.
%
% Minimizes f(a)'*W*f(a) over a.
%
% @RES ... residual function f called like [e,w,J] = RES(a,PARAMS{:}), where
%    - a ... double(M,1), parameter vector
%    - e ... double(N,1), residual vector
%    - J ... double(N,M), derivative of e wrt a
%    - w ... double(N,1), weights of e; covariance matrix of e is diag(1/e.^2).
%            Use 1 instead of ones(N,1).
% For efficiency, RES should not compute the jacobian if called with two output parameters only.
%
% @NORMSOLVE ... function solving normal equations, called like da = NORMSOLVE(J,e,W,lambda).
% a ... initial parameter vector
% opt ... options structure, see code

function [a,w] = levmarq(RES,PARAMS,NORMSOLVE,a0,varargin)

% options
[opt,rem_opt] = vgg_argparse( { 'niter_term',     +inf,...
                                'drmsrel_term',    0,...
                                'loglambda_term',  6,...
                                'loglambda_init', -4,...
                                'verbose',         0 },...
                              varargin );
if ~isempty(rem_opt), if ~isempty(fieldnames(rem_opt))
  error(['Unknown option(s) ' fieldnames(rem_opt)]);
end, end

% Initial statistics
if opt.verbose
  [e0,w0] = feval(RES,a0,PARAMS{:});
  ssd0 = sum( (e0.*w0).^2 );
  fprintf( '                         [rms=%14.12g] [maxabs=%14.12g]\n',...
          sqrt(ssd0/length(e0)),...
          max(abs(e0.*w0)) );
end

loglambda = opt.loglambda_init;
niter = 0;
while 1

  % Compute actual residual and jacobian
  [e0,w0,J] = feval(RES,a0,PARAMS{:});

  % Update a as a := a0 + da, by finding
  % optimal lambda and solving normal equations for da.
  nfail = 1;
  while (loglambda < opt.loglambda_term)
    niter = niter + 1;

    a = a0 + feval(NORMSOLVE,J,e0,w0,10^loglambda);
    [e,w] = feval(RES,a,PARAMS{:});

    if sum((e.*w).^2) < sum((e0.*w0).^2) % success
      a0 = a;
      loglambda = loglambda - 1;
      break
    end

    if opt.verbose
      fprintf('%4i.%.2i: [loglambda=%3i] [REJECTED]\n',niter,nfail,loglambda);
    end

    loglambda = loglambda + 1;
    nfail = nfail + 1;
  end

  % Print statistic after successful iteration
  ssd0 = sum( (e0.*w0).^2 );
  ssd = sum( (e.*w).^2 );
  if opt.verbose
    fprintf( '%4i   : [loglambda=%3i] [rms=%14.12g] [maxabs=%14.12g] [drmsrel=%4g%%]\n',...
             niter,...
             round(loglambda),...
             sqrt(ssd/length(e)),...
             max(abs(e.*w)),...
             100*(1-sqrt(ssd/ssd0)) );
  end

  % Termination criteria
  test(1) = loglambda <  opt.loglambda_term;
  test(2) = ssd0-ssd  >= opt.drmsrel_term^2*ssd0;
  test(3) = niter     <  opt.niter_term;
  if any(test==0)
    break
  end
end

if opt.verbose
  onoff = {'YES','no'};
  fprintf( ' Levenberg-Marquardt finished succesfully.\n Reason for termination:\n' );
  fprintf( '   lambda  = %s\n', onoff{test(1)+1} );
  fprintf( '   drmsrel = %s\n', onoff{test(2)+1} );
  fprintf( '   niter   = %s\n', onoff{test(3)+1} );
end

return

function print_statistics(niter,loglambda,e0,w0,e,w,opt)
if opt.verbose
  ssd0 = sum( (e0.*w0).^2 );
  ssd = sum( (e.*w).^2 );
  fprintf( '%4i   : [loglambda=%3i] [rms=%14.12g] [maxabs=%14.12g] [drmsrel=%11.5g]\n',...
           niter,...
           round(loglambda),...
           sqrt(ssd/length(e)),...
           max(abs(e.*w)),...
           sqrt(1-ssd/ssd0) );
end
return


function phiFaceAverage = tvdMean2D(phi, u, FL)
% This function gets the value of the field variable phi defined
% over the MeshStructure and calculates the TVD average on
% the cell faces, based on the direction of the velocity vector for a uniform mesh.
%
% SYNOPSIS:
%
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Written by Ali A. Eftekhari
% See the license file

% extract the velocity data
% note: size(ux) = [1:m+1, 1:n] and size(uy) = [1:m, 1:n+1]
ux = u.xvalue;
uy = u.yvalue;

% check the size of the variable and the mesh dimension
Nx = u.domain.dims(1);
Ny = u.domain.dims(2);
dx=repmat(0.5*(u.domain.cellsize.x(1:end-1)+u.domain.cellsize.x(2:end)), 1, Ny);
dy=repmat(0.5*(u.domain.cellsize.y(1:end-1)+u.domain.cellsize.y(2:end))', Nx, 1);

%
phiX_p = zeros(Nx+1,Ny);
phiX_m = zeros(Nx+1,Ny);
phiY_p = zeros(Nx,Ny+1);
phiY_m = zeros(Nx,Ny+1);

% calculate the upstream to downstream gradient ratios for u>0 (+ ratio)
% x direction
dphiX_p = (phi.value(2:Nx+2, 2:Ny+1)-phi.value(1:Nx+1, 2:Ny+1))./dx;
rX_p = dphiX_p(1:end-1,:)./fsign(dphiX_p(2:end,:));
phiX_p(2:Nx+1,:) = phi.value(2:Nx+1, 2:Ny+1)+0.5*FL(rX_p).* ...
    (phi.value(3:Nx+2,2:Ny+1)-phi.value(2:Nx+1, 2:Ny+1));
phiX_p(1, :) = (phi.value(1, 2:Ny+1)+phi.value(2, 2:Ny+1))/2; % left boundary
% y direction
dphiY_p = (phi.value(2:Nx+1, 2:Ny+2)-phi.value(2:Nx+1, 1:Ny+1))./dy;
rY_p = dphiY_p(:,1:end-1)./fsign(dphiY_p(:,2:end));
phiY_p(:,2:Ny+1) = phi.value(2:Nx+1, 2:Ny+1)+0.5*FL(rY_p).* ...
    (phi.value(2:Nx+1,3:Ny+2)-phi.value(2:Nx+1, 2:Ny+1));
phiY_p(:,1) = (phi.value(2:Nx+1,1)+phi.value(2:Nx+1,2))/2; % Bottom boundary

% calculate the upstream to downstream gradient ratios for u<0 (- ratio)
% x direction
rX_m = dphiX_p(2:end,:)./fsign(dphiX_p(1:end-1,:));
phiX_m(1:Nx,:) = phi.value(2:Nx+1, 2:Ny+1)+0.5*FL(rX_m).* ...
    (phi.value(1:Nx, 2:Ny+1)-phi.value(2:Nx+1, 2:Ny+1));
phiX_m(Nx+1,:) = (phi.value(end, 2:Ny+1)+phi.value(end-1, 2:Ny+1))/2; % right boundary
% y direction
rY_m = dphiY_p(:,2:end)./fsign(dphiY_p(:,1:end-1));
phiY_m(:,1:Ny) = phi.value(2:Nx+1, 2:Ny+1)+0.5*FL(rY_m).* ...
    (phi.value(2:Nx+1, 1:Ny)-phi.value(2:Nx+1, 2:Ny+1));
phiY_m(:, Ny+1) = (phi.value(2:Nx+1, end)+phi.value(2:Nx+1, end-1))/2; % top boundary

% calculate the average value
xvalue = (ux>0).*phiX_p+ ...
                        (ux<0).*phiX_m+ ...
                        0.5*(ux==0).*(phi.value(1:Nx+1,2:Ny+1)+phi.value(2:Nx+2,2:Ny+1));
yvalue = (uy>0).*phiY_p+ ...
                        (uy<0).*phiY_m+ ...
                        0.5*(uy==0).*(phi.value(2:Nx+1,1:Ny+1)+phi.value(2:Nx+1,2:Ny+2));
phiFaceAverage=FaceVariable(phi.domain, xvalue, yvalue, []);
end

function phi_out = fsign(phi_in)
% This function checks the value of phi_in and assigns an eps value to the
% elements that are less than or equal to zero, while keeping the signs of
% the nonzero elements
    phi_out = (abs(phi_in)>=eps).*phi_in+eps*(phi_in==0)+eps*(abs(phi_in)<eps).*sign(phi_in);
end


function [e,cnt] = my_normest(S,St,n,tol, maxiter)
% [norm,iter_count] = my_normest( A, At, n, [tol], [maxiter] )
%   estimates the spectral norm of A using the power method,
%   where A is a function handle to compute A(x) = A*x
%   and At is a function handle to compute At(x) = A'*x
%
% Copied from MATLAB's "normest" function, but allows function handles, not just sparse matrices
    if nargin < 4, tol = 1.e-6; end
    if nargin < 5, maxiter = 20; end
    if isempty(St)
        St = S;  % we assume the matrix is symmetric;
    end
    x = ones(n,1);
    cnt = 0;
    e = norm(x);
    if e == 0, return, end
    x = x/e;
    e0 = 0;
    while abs(e-e0) > tol*e && cnt < maxiter
       e0 = e;
       Sx = S(x);
       if nnz(Sx) == 0
          Sx = rand(size(Sx));
       end
       e = norm(Sx);
       x = St(Sx);
       x = x/norm(x);
       cnt = cnt+1;
    end
end


function I = mi_mixture_gd_vec(x, y, Ym)
% MI_MIXTURE_GD_VEC Vectorized MI calculation between multiple Gaussian variables
%         and a common discrete variable in bits from Gaussian mixture.
%   I = mi_mixture_gd_vec(x,y,Ym) returns the MI between the (possibly multidimensional)
%   Gaussian variables x and the discrete variable y.
%   size(x) = [Ntrl Nvec Ndim]
%   so each output I(i) = mi_mixture_gd(squeeze(x(:,i,:)), y, Ym);
%
%   y should contain integer values in the range [0 Ym-1] (inclusive).
%
%   biascorrect : true / false option (default true) which specifies whether
%   bias correction should be applied to the esimtated MI.
%   demeaned : false / true option (default false) which specifies whether the
%   input data already has zero mean (true if it has been copula-normalized)
%   See also: MI_MIXTURE_GD, MI_MODEL_GD_VEC

% ensure samples first axis for vectors
if isvector(x)
    x = x(:);
end
if ndims(x)>3
    error('mi_model_gd: input arrays should be 3d')
end
if isvector(y)
    y = y(:);
else
    error('mi_model_gd: only univariate discrete variable supported');
end

Ntrl = size(x,1);
Nvec = size(x,2);
Xdim = size(x,3);

if size(y,1) ~= Ntrl
    error('mi_model_gd: number of trials do not match');
end

I = zeros(Nvec,1);

% y = y-1;
% for vi=1:Nvec
%   I(vi) = mi_model_gd(squeeze(x(:,vi,:)),y,Ym,true,true);
% end
%
% return

% one-hot encoding of Y
Yhot = indexed2boolean(y);

% remove class means
[Xcen class_means] = removeclassmeans(x, Yhot);

% allocate memory for class-conditional entropies and covariances
Ntrl_y = sum(Yhot);
Hcond  = zeros(Nvec,Ym);
Cm     = zeros(Nvec,Xdim,Xdim,Ym);

for vi1=1:Xdim
  % all voxels for this dimension
  x1 = Xcen(:,:,vi1);
  for yi=1:Ym
    tmp = x1(Yhot(:,yi),:);
    Cm(:,vi1,vi1,yi) = sum(tmp.^2);
  end

  for vi2=(vi1+1):Xdim
    x2  = Xcen(:,:,vi2);
    for yi=1:Ym
      tmp = transpose(sum(x1(Yhot(:,yi),:).*x2(Yhot(:,yi),:)));
      Cm(:,vi1,vi2,yi) = tmp;
      Cm(:,vi2,vi1,yi) = tmp;
    end
  end
end

for yi=1:Ym
  Cm(:,:,:,yi) = Cm(:,:,:,yi) / (Ntrl_y(yi) - 1);
  Cm(:,:,:,yi) = vecchol(Cm(:,:,:,yi));
  for vi=1:Xdim
    Hcond(:,yi) = Hcond(:,yi)+log(Cm(:,vi,vi,yi));
  end
end
% class weights
w = Ntrl_y ./ Ntrl;
% normalise entropy properly
Hcond = Hcond + 0.5*Xdim*(log(2*pi)+1);

% mixture entropy via unscented transform
% See:
% Huber, Bailey, Durrant-Whyte and Hanebeck
% "On entropy approximation for Gaussian mixture random vectors"
% http://dx.doi.org/10.1109/MFI.2008.4648062
%
% Goldberger, Gordon, Greenspan
% "An efficient image similarity measure based on approximations of
% KL-divergence between two Gaussian mixtures"
% http://dx.doi.org/10.1109/ICCV.2003.1238387

D = Xdim;
Ds = sqrt(Xdim);
Hmix = zeros(Nvec,1);
chC = Cm;

m = reshape(class_means,[Nvec Xdim Ym]);
for yi=1:Ym
    % vector transpose
    Ps = Ds * permute(chC(:,:,:,yi), [1 3 2]);
    % unscented points for this class
    usc = cat(3,bsxfun(@plus,Ps,m(:,:,yi)), bsxfun(@minus,m(:,:,yi),Ps));

    % class log-likelihoods at unscented points
    log_lik = zeros(Ym,Nvec,2*Xdim);
    for mi=1:Ym
        % demean points
        dx = bsxfun(@minus,usc,m(:,:,mi));
        % gaussian likelihood
        log_lik(mi,:,:) = bsxfun(@minus,norm_innerv(dx, chC(:,:,:,mi)), Hcond(:,mi)) + 0.5*Xdim;
    end
    % log mixture likelihood for these unscented points
    logmixlik = maxstar(log_lik, w);
    % add to entropy estimate
    Hmix = Hmix + w(yi)*sum(logmixlik,2);
end
Hmix = -Hmix/(2*D);

% compute mutual information
I = Hmix - Hcond*w';

% convert to bits
I = I / log(2);

function [Xcen, class_means] = removeclassmeans(X, design)
[Ntrl, Nvec, Ndim] = size(X);
Xcen = X(:,:);
class_means = zeros(Nvec*Ndim,size(design,2));
for k = 1:size(design,2)
  sel = design(:,k);
  tmp = Xcen(sel,:);
  class_means(:,k) = mean(tmp,1);
  Xcen(sel,:) = bsxfun(@minus,tmp,class_means(:,k).');
end
Xcen = reshape(Xcen,[Ntrl Nvec Ndim]);

function Y = indexed2boolean(X)
uX = unique(X);
Y  = false(numel(X),numel(uX));
for k = 1:size(Y,2)
  Y(X==uX(k),k) = true;
end

function w = norm_innerv(x, chC)
% normalised innervations
Nvec = size(chC,1);
w = zeros(Nvec,size(x,3));
for vi=1:Nvec
  m = (squeeze(chC(vi,:,:))')\(squeeze(x(vi,:,:)));
  w(vi,:) = -0.5 *sum(m.*m,1);
end


% system process model in ecef frame with dcm formulation, Cse is the
% transformation between the sensor and e-frame, with the assumption that
% the sensor is also the body frame, we have Cse=Cbe.
%implement this function as sys_llh_phipsi_v000
function [STM Qd]=sys_ecef_dcm_v001(xyz_imu, Cse, acc, gyro, dt, imutype, modelNo)
% for modelNo=5 and imutype=5
% position   (1-3)
% velocity   (4-6)
% attitude   (7-9)
% acc bias drift
% gyro bias drift
% acc scale factor
% gyro scale factor
% acc turn on bias
% gyro turn on bias
% for modelNo=1 and imutype=5 the acc and gyro turn on bias are removed
WIE_E=7292115e-11;  %Earth rotation rate
g = 9.7803267714; % nominal g on earth surface
R=6317000; % earth average radius
%system disturbance coefs
N=zeros(9,6);
N(7:9,4:6)=-Cse; %attitude
N(4:6,1:3)=Cse; %velocity

%system matrix
A=zeros(9);
acc_e=Cse*acc;

%Position
A(1,4)=1;
A(2,5)=1;
A(3,6)=1;

%Velocity
unit=xyz_imu/norm(xyz_imu,2);
A(4:6,1:3)=-g*R^2/norm(xyz_imu,2)^3*(eye(3)-3*(unit*unit'))-skew([0;0;WIE_E])*skew([0;0;WIE_E]);
A(4:6,4:6)=-2*skew([0;0;WIE_E]);
A(4:6,7:9)=skew(acc_e);

%Attitude
A(7:9,7:9)=-skew([0;0;WIE_E]);
Anav=A;
Nnav=N;
% X(k+1) = ffun[X(k),U(k),V(k)]
% X(k+1) = Ak*X(k)+Gk*Vk

%%%%Imu error model parameters
[Aimu_d, Qimu_d, Cimu, Rimu]=imu_err_model_v001(acc, gyro, dt, imutype, modelNo);

%%%%Combine and discretize nav and imu models
% this discretization can also be accomplished by Loan's matrix exponential
% method, see sys_metric_phipsi_v000.m
Anav_d=eye(9)+dt*Anav;  %Use 1st order taylor series to discretize Anav
Qnav=Nnav*Rimu*Nnav';
Qnav_d=dt/2*(Anav_d*Qnav+Qnav*Anav_d');      %Use trapezoidal rule to discretize Rimu

STM=zeros(9+size(Aimu_d,1));
STM(1:9,1:9)=Anav_d;
STM(1:9,10:end)=Nnav*Cimu*dt;
STM(10:end,10:end)=Aimu_d;

Qd=zeros(9+size(Aimu_d,1));
Qd(1:9,1:9)=Qnav_d;
Qd(10:end,10:end)=Qimu_d;
Qd(1:9,10:end)=Nnav*Cimu*Qimu_d*dt/2;
Qd(10:end,1:9)=Qd(1:9,10:end)';
end


function c=ref_fcdgt(f,g,a,M,m_t,m_f,w_t,w_f)
%REF_CDGT  Reference centered DGT
%   Usage:  c=ref_dgtiv(f,g,a,M,c_t,c_f,c_w);
%
%   Linear algebra version of the algorithm. Create big matrix
%   containing all the basis functions and multiply with the transpose.
%
%   For easy work, m_t,m_f,w_t,w_f are all just 0/1 indicator variables.

L=size(f,1);
N=L/a;
b=L/M;

m_t=m_t*.5;
w_t=w_t*floor(a/2);
w_f=w_f*ceil(b/2);

F=zeros(L,M*N);

l=(0:L-1).';

if m_f==0

  for n=0:N-1
    for m=0:M-1
      F(:,M*n+m+1)=exp(2*pi*i*(m*b+w_f)*(l+m_t)/L).*circshift(g,n*a+w_t);
    end;
  end;

else

  for n=0:N-1
    for m=0:M-1
      F(:,M*n+m+1)=exp(2*pi*i*(m*b+.5+w_f)*(l+m_t-n*a)/L).*circshift(g,n*a+w_t);
    end;
  end;

end;

c=F'*f;


% ct_beta_study
% study how to adjust beta when pixel size changes in X-ray CT recon.
% results:
% changing nx has neglible effect, as expected
% changing pixel size has minimal effect when fwhm is converted to mm!
% values used CT studies:
% l2b=9, fov=500, fwhm = 1.62 mm
% l2b=9, fov=250, fwhm = 1.63 mm
% so we can  use the same beta regardless of pixel size and get nearly same fwhm
% caution: if other factors change, such as the # of views, may need to revisit!

%nx = 256;
%nx = 128;
nx = 64;
ny = nx;

if ~isvar('R')
	mask = true(nx,ny);
	l2b = 9;
	R = Robject(mask, 'edge_type', 'tight', 'beta', 2^l2b);
end

pixel_size_list = [500/512 250/512 0.25];
for ii=1:length(pixel_size_list)
	pixel_size = pixel_size_list(ii);

	sys = ct_sys('nx', nx, 'ny', ny, 'pixel_size', pixel_size);

	G = Gtomo2_dscmex(sys, 'nthread', 2);

%	psf = qpwls_psf(G, C, 2^l2b, mask, 1);
	psf = qpwls_psf(G, R, 1, mask, 1);
	fwhm = fwhm2(psf);
	printf('fwhm in mm = %g', fwhm * pixel_size)
end


function [LMTRad,LMT1,LMT2]=TT2LMT(TT1,TT2,rObsITRS,deltaT,xpyp)
%%TT2LMT Convert from terrestrial time to local mean solar time (LMT). This
%        is a measure of the time at a place on the Earth given the mean
%        location of the Sun, not a high-precision ephemeris.
%
%INPUTS: Jul1,Jul2 Two parts of a Julian date given in TT. The
%                  units of the date are days. The full date is the sum of
%                  both terms. The date is broken into two parts to
%                  provide more bits of precision. It does not matter how
%                  the date is split.
%         rObsITRS The 3X1 location of the observer in the International
%                  Terrestrial Reference System (ITRS). Only the direction
%                  matters, not the magnitude.
%           deltaT An optional parameter specifying the offset between TT
%                  and UT1 in seconds. If this parameter is omitted or if
%                  an empty matrix is passed, then the value of the
%                  function getEOP will be used.
%             xpyp xpyp=[xp;yp] are the polar motion coordinates in radians
%                  including the effects of tides and librations. If this
%                  parameter is omitted or if an empty matrix is passed,
%                  the value from the function getEOP will be used.
%
%OUTPUTS: LMTRad The local apparent solar time in radians. 0 radians is
%                solar midnight. pi radians is solar noon. The mapping is
%                is 2*pi radians for 24 hours. Thus, LATRad*(24/(2*pi))
%                gives the time of day in hours.
%      LMT1,LMT2 Two parts of the local mean solar time in Julian days. The
%                date is split so that LMT1 is the integer part and LMT2 is
%                the fractional part. Like UT1, a zero fractional part of
%                the day corresponds to noon, not midnight.
%
%The local mean solar time is UT1 plus the East longitude of the observer
%in radians in the Terrestrial Intermediate Reference System (TIRS). The
%East longitude is added using the convention that 360 degrees equals 24
%hours of 60 minutes and 60 seconds. For LMT in radians, an additional 12
%hours (pi radians) is added to adhere to the convention that 0 hours/ 0
%radians is midnight, not noon.
%
%The use of UT1 as a definition of Greenwhich mean solar time is given in
%[1]. Since UT1 is defined as a rotation in the TIRS, it only makes sense
%that a local mean solar time is defined by using the longitude offset in
%the TIRS.
%
%REFERENCES:
%[1] D. D. McCarthy, "Evolution of timescales from astronomy to physical
%    metrology," Metrologia, vol. 48, no. 4, pp. S132-S144, Aug. 2011.
%
%April 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Get any Earth orientation parameters that were not provided.
if(nargin<5||isempty(deltaT)||isempty(xpyp))
    [UTC1,UTC2]=TT2UTC(TT1,TT2);
    [xpypNew,~,~,deltaTTUT1]=getEOP(UTC1,UTC2);

    if(nargin<4||isempty(deltaT))
       deltaT=deltaTTUT1;
    end

    if(nargin<5||isempty(xpyp))
        xpyp=xpypNew;
    end
end

[UT11,UT12]=TT2UT1(TT1,TT2,deltaT);
rObsTIRS=ITRS2TIRS(rObsITRS,TT1,TT2,xpyp);
rObsSphere=Cart2Sphere(rObsTIRS);

%2*pi radians per day.
deltaUT1=rObsSphere(2)/(2*pi);

%Add preserving precision.
if(UT11<UT12)
    LMT2=UT11+deltaUT1;
    LMT1=UT12;
else
    LMT2=UT12+deltaUT1;
    LMT1=UT11;
end

%Put the fractional part into LMT2 and the integer part into LMT1.
frac1=LMT1-fix(LMT1);
frac2=LMT2-fix(LMT2);
sumVal=frac1+frac2;
sumInt=fix(sumVal);
sumFrac=sumVal-sumInt;
LMT1=fix(LMT2)+fix(LMT1)+sumInt;
LMT2=sumFrac;

%The local mean solar time in radians. This is equivalent to
%wrapRange(2*pi*(LMT1+LMT2)+pi,-pi,pi); However, since LMT1 is always
%integers, it can just be omitted.
LMTRad=wrapRange(2*pi*LMT2+pi,-pi,pi);

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function r = logdet(M)
% Compute log(det(A)) without the usual numerical inaccuracies.

% Copyright (C) Christian Kothe, SCCN, 2011, christian@sccn.ucsd.edu
%
% This program is free software; you can redistribute it and/or modify it under the terms of the GNU
% General Public License as published by the Free Software Foundation; either version 2 of the
% License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
% even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% General Public License for more details.
%
% You should have received a copy of the GNU General Public License along with this program; if not,
% write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
% USA

r = 2 * sum(log(diag(chol(M))), 1);


function [A] = spm_matrix(P, order)
% returns an affine transformation matrix
% FORMAT [A] = spm_matrix(P, order)
% P(1)  - x translation
% P(2)  - y translation
% P(3)  - z translation
% P(4)  - x rotation about - {pitch} (radians)
% P(5)  - y rotation about - {roll}  (radians)
% P(6)  - z rotation about - {yaw}   (radians)
% P(7)  - x scaling
% P(8)  - y scaling
% P(9)  - z scaling
% P(10) - x affine
% P(11) - y affine
% P(12) - z affine
%
% order (optional) application order of transformations.
%
% A     - affine transformation matrix
%___________________________________________________________________________
%
% spm_matrix returns a matrix defining an orthogonal linear (translation,
% rotation, scaling or affine) transformation given a vector of
% parameters (P).  By default, the transformations are applied in the
% following order (i.e., the opposite to which they are specified):
%
% 1) shear
% 2) scale (zoom)
% 3) rotation - yaw, roll & pitch
% 4) translation
%
% This order can be changed by calling spm_matrix with a string as a
% second argument. This string may contain any valid MATLAB expression
% that returns a 4x4 matrix after evaluation. The special characters 'S',
% 'Z', 'R', 'T' can be used to reference the transformations 1)-4)
% above. The default order is 'T*R*Z*S', as described above.
%
% SPM uses a PRE-multiplication format i.e. Y = A*X where X and Y are 4 x n
% matrices of n coordinates.
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_matrix.m 1149 2008-02-14 14:29:04Z volkmar $

% pad P with 'null' parameters
%---------------------------------------------------------------------------
q  = [0 0 0 0 0 0 1 1 1 0 0 0];
P  = [P q((length(P) + 1):12)];

% default multiplication order if not specified
%---------------------------------------------------------------------------
if nargin < 2
    order = 'T*R*Z*S';
end;

T  =   [1   0   0   P(1);
        0   1   0   P(2);
        0   0   1   P(3);
        0   0   0   1];

R1  =  [1    0      0          0;
        0    cos(P(4))  sin(P(4))  0;
        0   -sin(P(4))  cos(P(4))  0;
        0    0      0          1];

R2  =  [cos(P(5))  0    sin(P(5))  0;
        0          1    0      0;
       -sin(P(5))  0    cos(P(5))  0;
        0          0    0          1];

R3  =  [cos(P(6))   sin(P(6))   0  0;
       -sin(P(6))   cos(P(6))   0  0;
        0           0           1  0;
        0           0       0  1];

R   = R1*R2*R3;

Z   =  [P(7)    0       0       0;
        0       P(8)    0       0;
        0       0       P(9)    0;
        0       0       0       1];

S   =  [1       P(10)   P(11)   0;
        0       1   P(12)   0;
        0       0       1   0;
        0       0       0       1];

A = eval(sprintf('%s;', order));
if ~isnumeric(A) || ndims(A) ~= 2 || any(size(A) ~= 4)
    error('Order expression ''%s'' did not return a valid 4x4 matrix.', ...
          order);
end;