function x = daub12_transform_inverse ( n, y )

%*****************************************************************************80
%
%% DAUB12_TRANSFORM_INVERSE inverts the DAUB12 transform of a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the vector.
%    N must be a power of 2 and at least 4.
%
%    Input, real Y(N), the transformed vector.
%
%    Output, real X(N), the original vector.
%
  c = [ ...
    0.1115407433501095; ...
    0.4946238903984533; ...
    0.7511339080210959; ...
    0.3152503517091982; ...
   -0.2262646939654400; ...
   -0.1297668675672625; ...
    0.0975016055873225; ...
    0.0275228655303053; ...
   -0.0315820393174862; ...
    0.0005538422011614; ...
    0.0047772575109455; ...
   -0.0010773010853085 ];
  p = 11;
  x(1:n,1) = y(1:n);
  m = 4;
  q = floor ( ( p - 1 ) / 2 );

  while ( m <= n )

    z(1:m,1) = 0.0;

    j = 1;

    mh = floor ( m / 2 );

    for i = - q + 1 : mh - q

      for k = 0 : 2 : p - 1
        i0 = i4_wrap ( i      + k / 2,     1,      mh );
        i1 = i4_wrap ( i + mh + k / 2,     mh + 1, m  );
        z(j,1)   = z(j,1)   + c(p-k)   * x(i0) + c(k+2) * x(i1);
        z(j+1,1) = z(j+1,1) + c(p-k+1) * x(i0) - c(k+1) * x(i1);
      end

      j = j + 2;

    end

    x(1:m,1) = z(1:m);

    m = m * 2;

  end

  return
end


function   [dc,dk] = bspderiv(d,c,k)
%
% Function Name:
%
%   bspdeval - Evaluate the control points and knot sequence of the derivative
%              of a univariate B-Spline.
%
% Calling Sequence:
%
%   [dc,dk] = bspderiv(d,c,k)
%
% Parameters:
%
%   d	: Degree of the B-Spline.
%
%   c	: Control Points, matrix of size (dim,nc).
%
%   k	: Knot sequence, row vector of size nk.
%
%   dc	: Control points of the derivative
%
%   dk	: Knot sequence of the derivative
%
% Description:
%
%   Evaluate the derivative of univariate B-Spline, which is itself a B-Spline.
%   This function provides an interface to a toolbox 'C' routine.
[mc,nc] = size(c);
nk = numel(k);
                                                     %
                                                     % int bspderiv(int d, double *c, int mc, int nc, double *k, int nk, double *dc,
                                                     %              double *dk)
                                                     % {
                                                     %   int ierr = 0;
                                                     %   int i, j, tmp;
                                                     %
                                                     %   // control points
                                                     %   double **ctrl = vec2mat(c,mc,nc);
                                                     %
                                                     %   // control points of the derivative
dc = zeros(mc,nc-1);                                 %   double **dctrl = vec2mat(dc,mc,nc-1);
                                                     %
for i=0:nc-2                                         %   for (i = 0; i < nc-1; i++) {
   tmp = d / (k(i+d+2) - k(i+2));                    %     tmp = d / (k[i+d+1] - k[i+1]);
   for j=0:mc-1                                      %     for (j = 0; j < mc; j++) {
       dc(j+1,i+1) = tmp*(c(j+1,i+2) - c(j+1,i+1));  %       dctrl[i][j] = tmp * (ctrl[i+1][j] - ctrl[i][j]);
   end                                               %     }
end                                                  %   }
                                                     %
dk = zeros(1,nk-2);                                  %   j = 0;
for i=1:nk-2                                         %   for (i = 1; i < nk-1; i++)
   dk(i) = k(i+1);                                   %     dk[j++] = k[i];
end                                                  %
                                                     %   freevec2mat(dctrl);
                                                     %   freevec2mat(ctrl);
                                                     %
                                                     %   return ierr;
                                                     % }</pre>


function [model] = svmTrain(X, Y, C, kernelFunction, ...
                            tol, max_passes)
%SVMTRAIN Trains an SVM classifier using a simplified version of the SMO
%algorithm.
%   [model] = SVMTRAIN(X, Y, C, kernelFunction, tol, max_passes) trains an
%   SVM classifier and returns trained model. X is the matrix of training
%   examples.  Each row is a training example, and the jth column holds the
%   jth feature.  Y is a column matrix containing 1 for positive examples
%   and 0 for negative examples.  C is the standard SVM regularization
%   parameter.  tol is a tolerance value used for determining equality of
%   floating point numbers. max_passes controls the number of iterations
%   over the dataset (without changes to alpha) before the algorithm quits.
%
% Note: This is a simplified version of the SMO algorithm for training
%       SVMs. In practice, if you want to train an SVM classifier, we
%       recommend using an optimized package such as:
%
%           LIBSVM   (http://www.csie.ntu.edu.tw/~cjlin/libsvm/)
%           SVMLight (http://svmlight.joachims.org/)
%
%

if ~exist('tol', 'var') || isempty(tol)
    tol = 1e-3;
end

if ~exist('max_passes', 'var') || isempty(max_passes)
    max_passes = 5;
end

% Data parameters
m = size(X, 1);
n = size(X, 2);

% Map 0 to -1
Y(Y==0) = -1;

% Variables
alphas = zeros(m, 1);
b = 0;
E = zeros(m, 1);
passes = 0;
eta = 0;
L = 0;
H = 0;

% Pre-compute the Kernel Matrix since our dataset is small
% (in practice, optimized SVM packages that handle large datasets
%  gracefully will _not_ do this)
%
% We have implemented optimized vectorized version of the Kernels here so
% that the svm training will run faster.
if strcmp(func2str(kernelFunction), 'linearKernel')
    % Vectorized computation for the Linear Kernel
    % This is equivalent to computing the kernel on every pair of examples
    K = X*X';
elseif strfind(func2str(kernelFunction), 'gaussianKernel')
    % Vectorized RBF Kernel
    % This is equivalent to computing the kernel on every pair of examples
    X2 = sum(X.^2, 2);
    K = bsxfun(@plus, X2, bsxfun(@plus, X2', - 2 * (X * X')));
    K = kernelFunction(1, 0) .^ K;
else
    % Pre-compute the Kernel Matrix
    % The following can be slow due to the lack of vectorization
    K = zeros(m);
    for i = 1:m
        for j = i:m
             K(i,j) = kernelFunction(X(i,:)', X(j,:)');
             K(j,i) = K(i,j); %the matrix is symmetric
        end
    end
end

% Train
fprintf('\nTraining ...');
dots = 12;
while passes < max_passes,

    num_changed_alphas = 0;
    for i = 1:m,

        % Calculate Ei = f(x(i)) - y(i) using (2).
        % E(i) = b + sum (X(i, :) * (repmat(alphas.*Y,1,n).*X)') - Y(i);
        E(i) = b + sum (alphas.*Y.*K(:,i)) - Y(i);

        if ((Y(i)*E(i) < -tol && alphas(i) < C) || (Y(i)*E(i) > tol && alphas(i) > 0)),

            % In practice, there are many heuristics one can use to select
            % the i and j. In this simplified code, we select them randomly.
            j = ceil(m * rand());
            while j == i,  % Make sure i \neq j
                j = ceil(m * rand());
            end

            % Calculate Ej = f(x(j)) - y(j) using (2).
            E(j) = b + sum (alphas.*Y.*K(:,j)) - Y(j);

            % Save old alphas
            alpha_i_old = alphas(i);
            alpha_j_old = alphas(j);

            % Compute L and H by (10) or (11).
            if (Y(i) == Y(j)),
                L = max(0, alphas(j) + alphas(i) - C);
                H = min(C, alphas(j) + alphas(i));
            else
                L = max(0, alphas(j) - alphas(i));
                H = min(C, C + alphas(j) - alphas(i));
            end

            if (L == H),
                % continue to next i.
                continue;
            end

            % Compute eta by (14).
            eta = 2 * K(i,j) - K(i,i) - K(j,j);
            if (eta >= 0),
                % continue to next i.
                continue;
            end

            % Compute and clip new value for alpha j using (12) and (15).
            alphas(j) = alphas(j) - (Y(j) * (E(i) - E(j))) / eta;

            % Clip
            alphas(j) = min (H, alphas(j));
            alphas(j) = max (L, alphas(j));

            % Check if change in alpha is significant
            if (abs(alphas(j) - alpha_j_old) < tol),
                % continue to next i.
                % replace anyway
                alphas(j) = alpha_j_old;
                continue;
            end

            % Determine value for alpha i using (16).
            alphas(i) = alphas(i) + Y(i)*Y(j)*(alpha_j_old - alphas(j));

            % Compute b1 and b2 using (17) and (18) respectively.
            b1 = b - E(i) ...
                 - Y(i) * (alphas(i) - alpha_i_old) *  K(i,j)' ...
                 - Y(j) * (alphas(j) - alpha_j_old) *  K(i,j)';
            b2 = b - E(j) ...
                 - Y(i) * (alphas(i) - alpha_i_old) *  K(i,j)' ...
                 - Y(j) * (alphas(j) - alpha_j_old) *  K(j,j)';

            % Compute b by (19).
            if (0 < alphas(i) && alphas(i) < C),
                b = b1;
            elseif (0 < alphas(j) && alphas(j) < C),
                b = b2;
            else
                b = (b1+b2)/2;
            end

            num_changed_alphas = num_changed_alphas + 1;

        end

    end

    if (num_changed_alphas == 0),
        passes = passes + 1;
    else
        passes = 0;
    end

    fprintf('.');
    dots = dots + 1;
    if dots > 78
        dots = 0;
        fprintf('\n');
    end
    if exist('OCTAVE_VERSION')
        fflush(stdout);
    end
end
fprintf(' Done! \n\n');

% Save the model
idx = alphas > 0;
model.X= X(idx,:);
model.y= Y(idx);
model.kernelFunction = kernelFunction;
model.b= b;
model.alphas= alphas(idx);
model.w = ((alphas.*Y)'*X)';

end


%%                   runSignalReconstructionDemo.m
%
% This script will create phaseless measurements from a 1d test signal, and
% then recover the image using phase retrieval methods.  We now describe
% the details of the simple recovery problem that this script implements.
%
%                         Recovery Problem
% This script creates a complex-valued random Gaussian signal. Measurements
% of the signal are then obtained by applying a linear operator to the
% signal, and computing the magnitude (i.e., removing the phase) of
% the results.
%
%                       Measurement Operator
% Measurement are obtained using a linear operator, called 'A', that
% contains random Gaussian entries.
%
%                      The Recovery Algorithm
% The image is recovered by calling the method 'solvePhaseRetrieval', and
% handing the measurement operator and linear measurements in as arguments.
% A struct containing options is also handed to 'solvePhaseRetrieval'.
% The entries in this struct specify which recovery algorithm is used.
%
% For more details, see the Phasepack user guide.
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

function runSignalReconstructionDemo()

%% Specify the signal length, and number of measurements
n = 100;  % The signal length
m = 8*n;  % The number of measurements

%% Build the target signal
x_true = randn(n,1)+1i*randn(n,1);

%% Create the measurement operator
% Note: we use a dense matrix in this example, but PhasePack also supports
% function handles.  See the more complex 'runImageReconstructionDemo.m'
% script for an example using the fast Fourier transform.
A = randn(m,n)+1i*randn(m,n);

%% Compute phaseless measurements
b = abs(A*x_true);

%% Set options for PhasePack - this is where we choose the recovery algorithm
opts = struct;                  % Create an empty struct to store options
opts.algorithm = 'Fienup';      % Use the Fienup method to solve the retrieval problem.  Try changing this to 'twf' for truncated Wirtinger flow.
opts.initMethod = 'optimal';    % Use the optimal spectral initializer method to generate an initial starting point for the solver
opts.tol = 1e-3;                % The tolerance - make this smaller for more accurate solutions, or larger for faster runtimes
opts.verbose = 2;               % Print out lots of information as the solver runs (set this to 1 or 0 for less output)

%% Run the Phase retrieval Algorithm
fprintf('Running %s algorithm\n',opts.algorithm);
% Call the solver using the measurement operator 'A', the
% measurements 'b', the length of the signal to be recovered, and the
% options.  Note, the measurement operator can be either a function handle
% or a matrix.   Here, we use a matrix.  In this case, we have omitted the
% second argument. If 'A' had been a function handle, we would have
% handed the transpose of 'A' in as the second argument.
[x, outs] = solvePhaseRetrieval(A, [], b, n, opts);
% Note: 'outs' is a struct containing convergene information.

%% Remove phase ambiguity
% Phase retrieval can only recover images up to a phase ambiguity.
% Let's apply a phase rotation to align the recovered signal with the
% original so they look the same when we plot them.
rotation = sign(x'*x_true(:));
x = x*rotation;

% Print some useful info to the console
fprintf('Signal recovery required %d iterations (%f secs)\n',outs.iterationCount, outs.solveTimes(end));

%% Plot results
figure;
% Plot the true vs recovered signal.  Ideally, this scatter plot should be
% clustered around the 45-degree line.
subplot(1,2,1);
scatter(real(x_true),real(x));
xlabel('Original signal value');
ylabel('Recovered signal value');
title('Original vs recovered signal');

% Plot a convergence curve
subplot(1,3,3);
convergedCurve = semilogy(outs.solveTimes, outs.residuals);
set(convergedCurve, 'linewidth',1.75);
grid on;
xlabel('Time (sec)');
ylabel('Error');
title('Convergence Curve');
set(gcf,'units','points','position',[0,0,1200,300]);

end


function dspace = dspacing(h)
% space between crystal planes

dspace = 1./norm(h);


M = 5;
S1 = 4;
S2 = 4;
S = S1 + S2;
K = 2;
Y = zeros(M, S);
Y(1, 1:4) = [1 2 3 4];
Y(2, 1:4) = [3 3 2 6];
Y(3, 5:8) = [3 3 6 6];
Y(4, 5:8) = [4 5 6 7];
Y
Y = spx.norm.normalize_l2(Y)
C = spx.fast.batch_flipped_omp_spr(Y, K, 0)

Y * C
abs(Y - Y * C)


%computes the novelty measure used by laroche
%> called by ::ComputeNoveltyFunction
%>
%> @param X: spectrogram (dimension FFTLength X Observations)
%> @param f_s: sample rate of audio data (unused)
%>
%> @retval d_lar novelty measure
% ======================================================================
function [d_lar] = NoveltyLaroche (X, f_s)

    % difference spectrum
    afDeltaX    = diff([sqrt(X(:,1)), sqrt(X)],1,2);

    % half-wave rectification
    afDeltaX(afDeltaX<0) = 0;

    % flux
    d_lar       = sum(afDeltaX)/size(X,1);
end


classdef KalmanFilter < handle
    %KALMANFILTER  Kalman filter class
    %
    % The class implements a standard
    % [Kalman filter](https://en.wikipedia.org/wiki/Kalman_filter), [Welch95].
    % However, you can modify `transitionMatrix`, `controlMatrix`, and
    % `measurementMatrix` to get an extended Kalman filter functionality.
    %
    % ## Example
    %
    %     % initialization
    %     kf = cv.KalmanFilter(4,2);
    %     kf.statePre = [10;20;0;0]; % initial state prediction
    %     kf.transitionMatrix = [1,0,1,0; 0,1,0,1; 0,0,1,0; 0,0,0,1];
    %     kf.measurementMatrix([1,4]) = 1;
    %     kf.processNoiseCov = eye(4) * 1e-4;
    %     kf.measurementNoiseCov = eye(2) * 1e-1;
    %     kf.errorCovPost = eye(4) * 0.1;
    %
    %     % dynamics
    %     p_pred = kf.predict();       % update internal state
    %     measure = [11;21];           % measurement
    %     p_est = kf.correct(measure); % correct
    %
    % ## References
    % [Welch95]:
    % > Greg Welch and Gary Bishop. An introduction to the kalman filter, 1995.
    % > [PDF](http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf)
    %
    % See also: cv.KalmanFilter.init, cv.KalmanFilter.predict,
    %  cv.KalmanFilter.correct, vision.KalmanFilter
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    properties (Dependent)
        % predicted state `(x'(k)): x(k)=A*x(k-1)+B*u(k)`
        statePre
        % corrected state `(x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))`
        statePost
        % state transition matrix `(A)`
        transitionMatrix
        % control matrix `(B)` (not used if there is no control)
        controlMatrix
        % measurement matrix `(H)`
        measurementMatrix
        % process noise covariance matrix `(Q)`
        processNoiseCov
        % measurement noise covariance matrix `(R)`
        measurementNoiseCov
        % priori error estimate covariance matrix
        % `(P'(k)): P'(k)=A*P(k-1)*At + Q`
        errorCovPre
        % Kalman gain matrix `(K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R`)
        gain
        % posteriori error estimate covariance matrix
        % `(P(k)): P(k)=(I-K(k)*H)*P'(k)`
        errorCovPost
    end

    methods
        function this = KalmanFilter(varargin)
            %KALMANFILTER  KalmanFilter constructor
            %
            %     kf = cv.KalmanFilter()
            %     kf = cv.KalmanFilter(dynamParams, measureParams)
            %     kf = cv.KalmanFilter(..., 'OptionName', optionValue, ...)
            %
            % ## Input
            % * __dynamParams__ Dimensionality of the state.
            % * __measureParams__ Dimensionality of the measurement.
            %
            % ## Options
            % * __ControlParams__ Dimensionality of the control vector.
            %   default 0
            % * __Type__ Type of the created matrices that should be `single`
            %   or `double`. default `single`
            %
            % The constructor invokes the cv.KalmanFilter.init method to
            % initialize the object with the passed parameters.
            %
            % See also: cv.KalmanFilter, cv.KalmanFilter.init
            %
            this.id = KalmanFilter_(0, 'new');
            if nargin>0, this.init(varargin{:}); end
        end

        function delete(this)
            %DELETE  Destructor
            %
            %     kf.delete()
            %
            % See also: cv.KalmanFilter
            %
            if isempty(this.id), return; end
            KalmanFilter_(this.id, 'delete');
        end

        function init(this, dynamParams, measureParams, varargin)
            %INIT  Re-initializes Kalman filter. The previous content is destroyed
            %
            %     kf.init(dynamParams, measureParams)
            %     kf.init(..., 'OptionName', optionValue, ...)
            %
            % ## Input
            % * __dynamParams__ Dimensionality of the state.
            % * __measureParams__ Dimensionality of the measurement.
            %
            % ## Options
            % * __ControlParams__ Dimensionality of the control vector.
            %   default 0
            % * __Type__ Type of the created matrices that should be `single`
            %   or `double` (default).
            %
            % See also: cv.KalmanFilter.KalmanFilter
            %
            KalmanFilter_(this.id, 'init', dynamParams, measureParams, varargin{:});
        end

        function s = predict(this, varargin)
            %PREDICT  Computes a predicted state
            %
            %     s = kf.predict('OptionName', optionValue, ...)
            %
            % ## Output
            % * __s__ Output predicted state.
            %
            % ## Options
            % * __Control__ The optional input control. Not set by default
            %
            % See also: cv.KalmanFilter.correct
            %
            s = KalmanFilter_(this.id, 'predict', varargin{:});
        end

        function s = correct(this, measurement)
            %CORRECT  Updates the predicted state from the measurement
            %
            %     s = kf.correct(measurement)
            %
            % ## Input
            % * __measurement__ The measured system parameters.
            %
            % ## Output
            % * __s__ Output corrected state.
            %
            % See also: cv.KalmanFilter.predict
            %
            s = KalmanFilter_(this.id, 'correct', measurement);
        end
    end

    %% Getters/Setters
    methods
        function value = get.statePre(this)
            value = KalmanFilter_(this.id, 'get', 'statePre');
        end
        function set.statePre(this, value)
            KalmanFilter_(this.id, 'set', 'statePre', value);
        end

        function value = get.statePost(this)
            value = KalmanFilter_(this.id, 'get', 'statePost');
        end
        function set.statePost(this, value)
            KalmanFilter_(this.id, 'set', 'statePost', value);
        end

        function value = get.transitionMatrix(this)
            value = KalmanFilter_(this.id, 'get', 'transitionMatrix');
        end
        function set.transitionMatrix(this, value)
            KalmanFilter_(this.id, 'set', 'transitionMatrix', value);
        end

        function value = get.controlMatrix(this)
            value = KalmanFilter_(this.id, 'get', 'controlMatrix');
        end
        function set.controlMatrix(this, value)
            KalmanFilter_(this.id, 'set', 'controlMatrix', value);
        end

        function value = get.measurementMatrix(this)
            value = KalmanFilter_(this.id, 'get', 'measurementMatrix');
        end
        function set.measurementMatrix(this, value)
            KalmanFilter_(this.id, 'set', 'measurementMatrix', value);
        end

        function value = get.processNoiseCov(this)
            value = KalmanFilter_(this.id, 'get', 'processNoiseCov');
        end
        function set.processNoiseCov(this, value)
            KalmanFilter_(this.id, 'set', 'processNoiseCov', value);
        end

        function value = get.measurementNoiseCov(this)
            value = KalmanFilter_(this.id, 'get', 'measurementNoiseCov');
        end
        function set.measurementNoiseCov(this, value)
            KalmanFilter_(this.id, 'set', 'measurementNoiseCov', value);
        end

        function value = get.errorCovPre(this)
            value = KalmanFilter_(this.id, 'get', 'errorCovPre');
        end
        function set.errorCovPre(this, value)
            KalmanFilter_(this.id, 'set', 'errorCovPre', value);
        end

        function value = get.gain(this)
            value = KalmanFilter_(this.id, 'get', 'gain');
        end
        function set.gain(this, value)
            KalmanFilter_(this.id, 'set', 'gain', value);
        end

        function value = get.errorCovPost(this)
            value = KalmanFilter_(this.id, 'get', 'errorCovPost');
        end
        function set.errorCovPost(this, value)
            KalmanFilter_(this.id, 'set', 'errorCovPost', value);
        end
    end

end


classdef DenoisingProblem < handle

    properties (GetAccess = public, SetAccess = private)
        optimizedImage
        plottingData
    end

    properties (Access = private)
        originalImage
        imageSize
        discreteGradient
        noisyImage
        noisyImageNorm

        optimizer
        designVariable
        iterator

        energyFunction
        l1Proximal
        dualGap

        lambda
    end

    methods (Access = public)

        function obj = DenoisingProblem(cParams)
            obj.init(cParams);
            obj.createDiscreteGradient();
            obj.createNoisyImage(cParams);
            obj.computeNoisyImageNorm();
            obj.createDesignVariable();
            obj.createIterator(cParams);
            obj.createEnergyFunction(cParams);
            obj.createL1Proximal(cParams);
            obj.createOptimizer(cParams);
        end

        function solve(obj)
            while obj.iterator.hasNotFinished()
                obj.optimizer.update();
                obj.designVariable.update();
                obj.energyFunction.computeCost();
                obj.computeU();
                obj.computeDualGap();
                obj.updatePlottingData();
                obj.iterator.update;
            end
        end

    end

    methods (Access = private)

        function updatePlottingData(obj)
            p = obj.plottingData;
            i = obj.iterator.value;
            p.cost(i) = obj.energyFunction.value;
            p.dualGap(i) = obj.dualGap;
            obj.plottingData = p;
        end

        function init(obj,cParams)
            im = obj.readImage(cParams);
            obj.computeImageSize(im);
            obj.transformImageInVectorForm(im)
            obj.lambda = cParams.totalVariationWeigth;
        end

        function computeImageSize(obj,image)
            [m,n] = size(image);
            obj.imageSize.rows = m;
            obj.imageSize.columns = n;
            obj.imageSize.rowsTimesColumns = m*n;
        end

        function transformImageInVectorForm(obj,image)
            obj.originalImage = image(:);
        end

        function createDiscreteGradient(obj)
            m  = obj.imageSize.rows;
            n  = obj.imageSize.columns;
            mn = obj.imageSize.rowsTimesColumns;
            I  = reshape(1:m*n,m,n);
            east  = [I(:,2:end), I(:,end)];
            north = [I(2:end,:); I(end,:)];
            D1 = sparse(I,east,1,mn,mn)  -speye(mn,mn);
            D2 = sparse(I,north,1,mn,mn) -speye(mn,mn);
            obj.discreteGradient = [D1 ; D2];
        end

        function createNoisyImage(obj,cParams)
            u0    = obj.originalImage;
            L     = cParams.lipschitzConstant;
            a     = cParams.noiseAmplitud;
            sigma = a/L;
            uN    = u0 + sigma*randn(size(u0));
            obj.noisyImage = uN;
        end

        function computeNoisyImageNorm(obj)
            g = obj.noisyImage;
            obj.noisyImageNorm = 0.5*(g'*g);
        end

        function createDesignVariable(obj)
            mn = 2*obj.imageSize.rowsTimesColumns;
            s.xLength = mn;
            obj.designVariable = DesignImagVariable(s);
        end

        function computeU(obj)
            g = obj.noisyImage;
            D = obj.discreteGradient;
            p = obj.designVariable.value;
            obj.optimizedImage = g(:) - D'*p;
        end

        function createEnergyFunction(obj,cParams)
            s.lipschitzConstant = cParams.lipschitzConstant;
            s.A = obj.discreteGradient';
            s.b = obj.noisyImage;
            s.designVariable = obj.designVariable;
            c = QuadraticFunction(s);
            obj.energyFunction = c;
        end

        function createL1Proximal(obj,cParams)
            s.lambda = cParams.totalVariationWeigth;
            s.imageSize = obj.imageSize;
            s.designVariable = obj.designVariable;
            obj.l1Proximal = L1VectorNormProximal(s);
        end

        function createIterator(obj,cParams)
            s.maxIter = cParams.maxIter;
            obj.iterator = Iterator(s);
        end

        function computeDualGap(obj)
            lam = obj.lambda;
            ut  = obj.optimizedImage;
            p   = obj.designVariable.value;
            D   = obj.discreteGradient;
            gN  = obj.noisyImageNorm;
            q   = D'*p;
            gap = lam*sum(abs(D*ut)) + q'*ut;
            obj.dualGap = gap/gN;
        end

        function createOptimizer(obj,cParams)
            sg.designVariable = obj.designVariable;
            sg.differentiableFunction = obj.energyFunction;
            sg.designVariable = obj.designVariable;
            s.gradientMethodParams = sg;

            sm.designVariable = obj.designVariable;
            sm.iterator       = obj.iterator;
            s.momentumParams  = sm;

            s.proximal = obj.l1Proximal;
            s.type     = cParams.optimizer;
            s.iterator = obj.iterator;
            obj.optimizer = SplittingAlgorithm.create(s);
        end

    end

    methods (Access = private, Static)

        function im = readImage(cParams)
            image = cParams.imageFile;
            im = double(imread(image));
        end

    end

end


function dStates = Dynamics(States, Inputs, Params)

%This function is used to compute the dynamics of the tractor trailer truck
%system. The equations are derived by Derive_EoM.m

%States is a (4xN) matrix of states
%Inputs is a (2xN) matrix of inputs
%Params is a structure of parameters

%x = States(1,:);
%y = States(2,:);
th = States(3,:);
phi = States(4,:);

v = Inputs(1,:);
psi = Inputs(2,:)';

Lt = Params.Dyn.Lt;
Lc = Params.Dyn.Lc;

%[dStates, A, B] = Derive_EoM()

dStates =[...

                      -v.*cos(phi).*cos(psi).*sin(th);
                      v.*cos(phi).*cos(psi).*cos(th);
                         (v.*cos(psi).*sin(phi))/Lt;
 (v.*(Lt.*sin(psi) - Lc.*cos(psi).*sin(phi)))/(Lc*Lt)];

end


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


% State transition function for the UNGM-model.
%
% Copyright (C) 2007 Jouni Hartikainen
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function x_n = ungm_f(x,param)
n = param(1);
x_n = 0.5*x(1,:) + 25*x(1,:)./(1+x(1,:).*x(1,:)) + 8*cos(1.2*(n-1));
if size(x,1) > 1
   x_n = x_n + x(2,:);
end


function [clrmap,clrIX_x,clrIX_y] = MapXYto2Dcolormap(gIX_in,X,Y,Xrange,Yrange,cmap2D)
% gIX_in corresponds to the 2D values X and Y.
% The output clrmap is a linear colormap of the same length as numK
% (=max(gIX_in)), and same as clrIX_x and clrIX_y.
% clrIX_x and clrIX_y are the coordinates in the 2D colormap for the range of values in gIX_in.

if nargin < 4
    Xrange = [min(X),max(X)];
end
if nargin < 5
    Yrange = [min(Y),max(Y)];
end

if nargin < 6
    cmap2D = MakeDiagonal2Dcolormap(huex,satmin,pw);
end
res = size(cmap2D,1);

% check input dimensions
if size(X,1)~=size(Y,1)
    if size(X,1)>1
        Y = Y';
    else
        X = X';
    end
end
assert(isequal(size(X),size(Y)));

% set data range
X(X<Xrange(1)) = Xrange(1);
Y(Y<Yrange(1)) = Yrange(1);
X(X>Xrange(2)) = Xrange(2);
Y(Y>Yrange(2)) = Yrange(2);

clrIX_x = round((X-Xrange(1))/(Xrange(2)-Xrange(1))*(res-1))+1;
clrIX_y = round((Y-Yrange(1))/(Yrange(2)-Yrange(1))*(res-1))+1;

%%
clrmap = zeros(length(clrIX_x),3);
clrmap_2D_flat = reshape(cmap2D,res*res,3); % for efficient indexing

U = unique(gIX_in);

ix = (clrIX_x(U)-1)*res+clrIX_y(U);
clrmap(U,:) = clrmap_2D_flat(ix,:);
% for i = 1:length(U)
% %     ix = sub2ind([res,res],clrIX_y(U(i))',clrIX_x(U(i))');
%     ix = (clrIX_x(U(i))'-1)*res+clrIX_y(U(i))';
% %     center_ix = (M_xyz(j,2)-1)*dimv(1)+M_xyz(j,1); % linear pixel index, faster equivalent of:
%         %     center_ix = sub2ind([dimv(1),dimv(2)],M_xyz(j,1),M_xyz(j,2));
%
%
%     clrmap(U(i),:) = clrmap_2D_flat(ix,:);
% 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 w = ymdf_to_weekday_hebrew ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_WEEKDAY_HEBREW returns the weekday of a Hebrew YMDF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, integer W, is the week day number of the date, with
%    1 for Sunday, through 7 for Saturday.
%
  jed = ymdf_to_jed_hebrew ( y, m, d, f );

  [ w, f2 ] = jed_to_weekday ( jed );

  return
end


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


function [yn,en,S] = FXLMSadapt(un,dn,S)

% FXLMSadapt        Filtered-x LMS (FXLMS) Algorithm
%
%                   Perform over the entire length of the input sequence.
%                   The history of output, square error and coefficients of FIR
%                   filters are passed out to extenal
% Arguments:
% un                Input signal
% dn                Desired signal
% S                 Adptive filter parameters as defined in LMSinit.m
% yn                History of output signal
% en                History of error signal
%
% by Lee, Gan, and Kuo, 2008
% Subband Adaptive Filtering: Theory and Implementation
% Publisher: John Wiley and Sons, Ltd

M = length(S.coeffs);                     % Length of FIR filter
mu = S.step;                              % Step size of the NLMS algorithm
leak = S.leakage;                         % Leaky factor for the leaky LMS algorithm
AdaptStart = S.AdaptStart;
w = S.coeffs;                             % Coefficients of FIR filter
u = zeros(M,1);
fu = zeros(M,1);

ITER = length(un);                        % Length of input sequence
yn = zeros(1,ITER);                       % Initialize output sequence to zero
en = zeros(1,ITER);                       % Initialize error sequence to zero
%filt_un = zeros(1,ITER);                 % Initialize filtered input to zero
%est_filt_un = zeros(1,ITER);             % Initialize filtered input to zero (est)
filt_un = filter(S.sec_num,S.sec_den,un); % Filtered input
est_filt_un = filter(S.estsec,1,un);      % Filtered input (est)

if isfield(S,'unknownsys')
    b = S.unknownsys;
    norm_b = norm(b);
    eml = zeros(1,ITER);
    ComputeEML = 1;
else
    ComputeEML = 0;
end

for n = 1:ITER
   u = [filt_un(n); u(1:end-1)];          % Input signal vector (through the actual
                                          %   secondary path)
   fu = [est_filt_un(n); fu(1:end-1)];    % Filtered input signal vector (through the
                                          %   estimated secondary path)
   yn(n) = w'*u;                          % Compute output using inner product of w and u
   en(n) = dn(n)-yn(n);                   % Error computation
   if ComputeEML == 1;
        eml(n) = norm(b-w)/norm_b;        % System error norm (normalized)
   end
   if n >= AdaptStart
        w = (1-mu*leak)*w + (mu*en(n))*fu;% LMS algorithm in leaky mode
        S.iter = S.iter + 1;
   end
end

S.coeffs = w;                             % Coefficient values at the final iteration
if ComputeEML == 1;
    S.eml = eml;
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]')


%% ===== Secondary sources ===============================================
conf = SFS_config;
conf.secondary_sources.size = 3;

% === linear ===
conf.secondary_sources.geometry = 'line';
conf.secondary_sources.number = 21;
x0 = secondary_source_positions(conf);
figure;
figsize(540,404,'px');
draw_loudspeakers(x0,conf);
axis([-2 2 -2 1]);
pause(1)
print_png('secondary_sources_linear.png');

conf.secondary_sources.logspread = 3.5;
x0 = secondary_source_positions(conf);
figure;
figsize(540,404,'px');
draw_loudspeakers(x0,conf);
axis([-2 2 -2 1]);
pause(1)
print_png('secondary_sources_linear_log.png');

% === circular ===
conf.secondary_sources.geometry = 'circle';
conf.secondary_sources.number = 56;
x0 = secondary_source_positions(conf);
figure;
figsize(540,404,'px');
draw_loudspeakers(x0,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('secondary_sources_circle.png');

% === box shaped ===
conf.secondary_sources.geometry = 'box';
conf.secondary_sources.number = 84;
x0 = secondary_source_positions(conf);
figure;
figsize(540,404,'px');
draw_loudspeakers(x0,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('secondary_sources_box.png');

% === box shaped with smoothed edges ===
conf.secondary_sources.geometry = 'rounded-box';
conf.secondary_sources.number = 84;
conf.secondary_sources.corner_radius = 0.3;
x0 = secondary_source_positions(conf);
figure;
figsize(540,404,'px');
draw_loudspeakers(x0,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('secondary_sources_rounded-box.png');

% === spherical array ===
conf.secondary_sources.geometry = 'sphere'; % or 'spherical'
conf.secondary_sources.number = 225;
x0 = secondary_source_positions(conf);
figure;
figsize(540,404,'px');
draw_loudspeakers(x0,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('secondary_sources_sphere.png');

% === arbitrary shaped arrays ===
% create a stadium like shape by combining two half circles with two linear
% arrays
% first getting a full circle with 56 loudspeakers
conf.secondary_sources.geometry = 'circle';
conf.secondary_sources.number = 56;
conf.secondary_sources.x0 = [];
x0 = secondary_source_positions(conf);
% store the first half cricle and move it up
x01 = x0(2:28,:);
x01(:,2) = x01(:,2) + ones(size(x01,1),1)*0.5;
% store the second half circle and move it down
x03 = x0(30:56,:);
x03(:,2) = x03(:,2) - ones(size(x03,1),1)*0.5;
% create a linear array
conf.secondary_sources.geometry = 'linear';
conf.secondary_sources.number = 7;
conf.secondary_sources.size = 1;
x0 = secondary_source_positions(conf);
% rotate it and move it left
R = rotation_matrix(pi/2);
x02 = [(R*x0(:,1:3)')' (R*x0(:,4:6)')'];
x02(:,1) = x02(:,1) - ones(size(x0,1),1)*1.5;
x02(:,7) = x0(:,7);
% rotate it the other way around and move it right
R = rotation_matrix(-pi/2);
x04 = [(R*x0(:,1:3)')' (R*x0(:,4:6)')'];
x04(:,1) = x04(:,1) + ones(size(x0,1),1)*1.5;
x04(:,7) = x0(:,7);
% combine everything
conf.secondary_sources.geometry = 'custom';
conf.secondary_sources.x0 = [x01; x02; x03; x04];
% if we gave the conf.secondary_sources.x0 to the secondary_source_positions
% function it will simply return the defined x0 matrix
x0 = secondary_source_positions(conf);
figure;
figsize(540,404,'px');
draw_loudspeakers(x0,conf);
axis([-2 2 -2.5 2.5]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('secondary_sources_arbitrary.png');
conf.plot.realloudspeakers = true;
figure;
figsize(540,404,'px');
draw_loudspeakers(x0,conf);
axis([-2 2 -2.5 2.5]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('secondary_sources_arbitrary_realloudspeakers.png');

%% ===== Monochromatic sound fields ======================================
% === WFS 3D ===
conf = SFS_config;
conf.dimension = '3D';
conf.secondary_sources.size = 3;
conf.secondary_sources.number = 225;
conf.secondary_sources.geometry = 'sphere';
% [P,x,y,z,x0,win] = sound_field_mono_wfs_25d(X,Y,Z,xs,src,fconf);
sound_field_mono_wfs([-2 2],[-2 2],0,[0 -1 0],'pw',800,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('sound_field_wfs_3d_xy.png');
sound_field_mono_wfs([-2 2],0,[-2 2],[0 -1 0],'pw',800,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('sound_field_wfs_3d_xz.png');
sound_field_mono_wfs(0,[-2 2],[-2 2],[0 -1 0],'pw',800,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('sound_field_wfs_3d_yz.png');
conf.resolution = 100;
sound_field_mono_wfs([-2 2],[-2 2],[-2 2],[0 -1 0],'pw',800,conf);
print_png('sound_field_wfs_3d_xyz.png');

% === WFS 2.5D ===
% simulating 2.5D WFS with circular array and a point source
conf = SFS_config;
conf.dimension = '2.5D';
conf.plot.useplot = true;
conf.plot.normalisation = 'center';
% [P,x,y,z,x0] = sound_field_mono_wfs(X,Y,Z,xs,src,f,conf);
[P,~,~,~,x0] = sound_field_mono_wfs([-2 2],[-2 2],0,[0 2.5 0],'ps',800,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('sound_field_wfs_25d.png');
% plotting WFS with all secondary sources
x0_all = secondary_source_positions(conf);
[~,idx] = secondary_source_selection(x0_all,[0 2.5 0],'ps');
x0_all(:,7) = zeros(1,size(x0_all,1));
x0_all(idx,7) = x0(:,7);
conf.plot.realloudspeakers = true;
plot_sound_field(P,[-2 2],[-2 2],0,x0_all,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('sound_field_wfs_25d_with_all_sources.png');
% simulating 2.5D NFCHOA with circular array and a plane wave
conf = SFS_config;
conf.dimension = '2.5D';
% sound_field_mono_nfchoa(X,Y,Z,xs,src,f,conf);
sound_field_mono_nfchoa([-2 2],[-2 2],0,[0 -1 0],'pw',800,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('sound_field_nfchoa_25d.png');

% === 2D local WFS with box shaped array and circular virtual array ===
X = [-1 1];
Y = [-1 1];
Z = 0;
xs = [1 -1 0];
src = 'pw';
f = 7000;
conf = SFS_config;
conf.resolution = 1000;
conf.dimension = '2D';
conf.secondary_sources.geometry = 'box';
conf.secondary_sources.number = 4*56;
conf.secondary_sources.size = 2;
conf.localwfs_vss.size = 0.4;
conf.localwfs_vss.center = [0 0 0];
conf.localwfs_vss.geometry = 'circular';
conf.localwfs_vss.number = 56;
sound_field_mono_localwfs_vss(X,Y,Z,xs,src,f,conf);
axis([-1.1 1.1 -1.1 1.1]);
print_png('sound_field_localwfs_2d.png');

% === stereo setup ===
conf = SFS_config;
conf.plot.normalisation = 'center';
x0 = [-1 2 0 0 -1 0 1;1 2 0 0 -1 0 1];
% [P,x,y,z] = sound_field_mono(X,Y,Z,x0,src,D,f,conf)
sound_field_mono([-2 2],[-1 3],0,x0,'ps',[1 1],800,conf)
print_png('sound_field_stereo.png');

%% ===== spatio-temporal snapshots of the sound field ====================
conf = SFS_config;
conf.dimension = '2.5D';
conf.plot.useplot = true;
% sound_field_imp_nfchoa(X,Y,Z,xs,src,t,conf)
[p,x,y,z,x0] = sound_field_imp_nfchoa([-2 2],[-2 2],0,[0 2 0],'ps',0.005,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('sound_field_imp_nfchoa_25d.png');
conf.plot.usedb = true;
plot_sound_field(p,[-2 2],[-2 2],0,x0,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('sound_field_imp_nfchoa_25d_dB.png');
conf.plot.useplot = false;
conf.t0 = 'source';
t_40cm = 0.4/conf.c; % time to travel 40 cm in s
t0 = 0.0005; % start time of focused source in s
[p_ps,~,~,~,x0_ps] = ...
    sound_field_imp_wfs([-2 2],[-2 2],0,[1.9 0 0],'ps',t0+t_40cm,conf);
[p_pw,~,~,~,x0_pw] = ...
    sound_field_imp_wfs([-2 2],[-2 2],0,[1 -2 0],'pw',t0-t_40cm,conf);
[p_fs,~,~,~,x0_fs] = ...
    sound_field_imp_wfs([-2 2],[-2 2],0,[0 -1 0 0 1 0],'fs',t0,conf);
plot_sound_field(p_ps+p_pw+p_fs,[-2 2],[-2 2],0,[x0_ps; x0_pw; x0_fs],conf)
hold;
scatter(0,0,'k','x');   % origin of plane wave
scatter(1.9,0,'k','o'); % point source
scatter(0,-1,'k','o');  % focused source
hold off;
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('sound_field_imp_multiple_sources_dB.png');

%% ===== custom grids ====================================================
conf = SFS_config;
conf.dimension = '3D';
conf.secondary_sources.number = 225;
conf.secondary_sources.geometry = 'sphere';
conf.resolution = 100;
conf.plot.normalisation = 'center';
X = randi([-2000 2000],125000,1)/1000;
Y = randi([-2000 2000],125000,1)/1000;
Z = randi([-2000 2000],125000,1)/1000;
sound_field_mono_wfs(X,Y,Z,[0 -1 0],'pw',800,conf);
print_png('sound_field_wfs_3d_xyz_custom_grid.png');
conf.plot.usedb = true;
conf.dimension = '2.5D';
conf.secondary_sources.number = 64;
conf.secondary_sources.geometry = 'circle';
sound_field_imp_nfchoa(X,Y,0,[0 2 0],'ps',0.005,conf);
axis([-2 2 -2 2]);
set(gca, 'XTick', -2:1:2); set(gca, 'YTick', -2:1:2);
print_png('sound_field_imp_nfchoa_25d_dB_custom_grid.png');

%% ===== modal windows ===================================================
conf = SFS_config;
conf.dimension = '2.5D';
conf.secondary_sources.number = 16;
conf.secondary_sources.geometry = 'circular';
conf.secondary_sources.size = 3;
conf.resolution = 300;
conf.plot.usedb = true;
conf.t0 = 'source';
X = [-2,2];
Y = [-2,2];
Z = 0;
conf.modal_window = 'rect';  % default
sound_field_imp_nfchoa(X,Y,Z,[0 -1 0],'pw',0,conf);
print_png('sound_field_imp_nfchoa_25d_dB_rect.png');
conf.modal_window = 'max-rE';
sound_field_imp_nfchoa(X,Y,Z,[0 -1 0],'pw',0,conf);
print_png('sound_field_imp_nfchoa_25d_dB_max-rE.png');
conf.modal_window = 'kaiser';
conf.modal_window_parameter = 1.0;
sound_field_imp_nfchoa(X,Y,Z,[0 -1 0],'pw',0,conf);
print_png('sound_field_imp_nfchoa_25d_dB_kaiser.png');
conf.modal_window = 'tukey';
conf.modal_window_parameter = 0.5;
sound_field_imp_nfchoa(X,Y,Z,[0 -1 0],'pw',0,conf);
print_png('sound_field_imp_nfchoa_25d_dB_tukey.png');

%% ===== impulse response of a spatial audio system ======================
conf = SFS_config;
conf.t0 = 'source';
X = [0 0 0];
phi = 0;
xs = [2.5 0 0];
src = 'ps';
t = (1:1000)/conf.fs*1000;
hrtf = dummy_irs(conf);
[ir,~,delay] = ir_wfs(X,phi,xs,src,hrtf,conf);
figure;
figsize(540,404,'px');
plot(t,ir(1:1000,1),'-g');
hold on;
offset = round(delay*conf.fs);
plot(t,ir(1+offset:1000+offset,1),'-b');
hold off;
xlabel('time / ms');
ylabel('amplitude');
print_png('impulse_response_wfs_25d.png');
X = [0 0 0];
head_orientation = [0 0];
xs = [2.5 0 0];
src = 'ps';
conf = SFS_config;
conf.N = 1000;
conf.t0 = 'source';
time_response_wfs(X,xs,src,conf)
axis([0 25 -0.005 0.025]);
print_png('impulse_response_wfs_25d_imp.png');

%% ===== frequency response of a spatial audio system ====================
X = [0 0 0];
head_orientation = [pi/2 0];
xs = [0 2.5 0];
src = 'ps';
hrtf = dummy_irs(conf);
conf = SFS_config;
conf.ir.usehcomp = false;
conf.wfs.usehpre = false;
[ir1,x0] = ir_wfs(X,head_orientation,xs,src,hrtf,conf);
conf.wfs.usehpre = true;
conf.wfs.hprefhigh = aliasing_frequency(x0,conf);
ir2 = ir_wfs(X,head_orientation,xs,src,hrtf,conf);
[a1,p,f] = spectrum_from_signal(norm_signal(ir1(:,1)),conf);
a2 = spectrum_from_signal(norm_signal(ir2(:,1)),conf);
figure;
figsize(540,404,'px');
semilogx(f,20*log10(a1),'-b',f,20*log10(a2),'-r');
axis([10 20000 -80 -40]);
set(gca,'XTick',[10 100 250 1000 5000 20000]);
legend('w/o pre-filter','w pre-filter');
xlabel('frequency / Hz');
ylabel('magnitude / dB');
print_png('frequency_response_wfs_25d.png');
% alternative variant
X = [0 0 0];
xs = [0 2.5 0];
src = 'ps';
conf = SFS_config;
freq_response_wfs(X,xs,src,conf);
axis([10 20000 -40 0]);
print_png('frequency_response_wfs_25d_mono.png');


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


% bootstrapping trial

%% make test Data matrix (from data examined by hand)
Data = cell(1,3);

i_fish = 8;
gIX = FC{i_fish}.gIX;
IX = find(gIX <100);
Data{1}.cIX = FC{i_fish}.cIX(IX);
Data{1}.xyz_norm = FC{i_fish}.xyz_norm(IX,:);

i_fish = 9;
Data{2}.cIX = FC{i_fish}.cIX;
Data{2}.xyz_norm = FC{i_fish}.xyz_norm;

i_fish = 10;
gIX = FC{i_fish}.gIX;
IX = find(gIX <100);
Data{3}.cIX = FC{i_fish}.cIX(IX);
Data{3}.xyz_norm = FC{i_fish}.xyz_norm(IX,:);

%%

%%
thres_prc = 5;
numDim = 3;
numFish = 3;

TF = zeros(numFish,numFish,numDim);

for i_ref = 1:numFish,
    numRand = 100;
    means = zeros(numRand,3);
    for i = 1:numRand,
        IX = ceil(100*rand(1,numRand));
        means(i,:) = mean(Data{i_ref}.xyz_norm(IX,:),1);
    end
    for i_test = 1:numFish,
        if i_test ~= i_ref,
            for i_dim = 1:numDim,
                mean_test = mean(Data{i_test}.xyz_norm(:,i_dim),1);
                lowerbound = prctile(means(:,i_dim),thres_prc/2);
                upperbound = prctile(means(:,i_dim),100-thres_prc/2);
                TF(i_ref,i_test,i_dim) = mean_test>lowerbound & mean_test<upperbound;
            end
        else
            TF(i_ref,i_test,:) = NaN;
        end
    end
end


function C = dot_product(A,B)

% Computes the dot product of the corresponding rows of the matrices A and B

C = sum(A.*B,2);


% This example is from Page.143 of "Probabilistic Networks and Expert Systems",
% Cowell, Dawid, Lauritzen and Spiegelhalter, 1999, Springer.

X = 1; Y = 2; Z = 3;
n = 3;

dag = zeros(n);
dag(X, Y)=1;
dag(Y, Z)=1;

ns = ones(1, n);
dnodes = [];

bnet = mk_bnet(dag, ns, dnodes);
bnet.CPD{X} = gaussian_CPD(bnet, X, 'mean', 0, 'cov', 1);
bnet.CPD{Y} = gaussian_CPD(bnet, Y, 'mean', 0, 'cov', 1, 'weights', 1);
bnet.CPD{Z} = gaussian_CPD(bnet, Z, 'mean', 0, 'cov', 1, 'weights', 1);

engines = {};
engines{end+1} = jtree_inf_engine(bnet);
engines{end+1} = stab_cond_gauss_inf_engine(bnet);
nengines = length(engines);

evidence = cell(1,n);
evidence{Y} = 1.5;

for e=1:nengines
  engines{e} = enter_evidence(engines{e}, evidence);
  margX = marginal_nodes(engines{e}, X);
  assert(approxeq(margX.mu, 0.75))
  assert(approxeq(margX.Sigma, 0.5))

  margZ = marginal_nodes(engines{e}, Z);
  assert(approxeq(margZ.mu, 1.5))
  assert(approxeq(margZ.Sigma, 1))
end

evidence = cell(1,n);
evidence{Z} = 1.5;

for e=1:nengines
  engines{e} = enter_evidence(engines{e}, evidence);
  margX = marginal_nodes(engines{e}, X);
  assert(approxeq(margX.mu, 1/2))
  assert(approxeq(margX.Sigma, 2/3))

  margY = marginal_nodes(engines{e}, Y);
  assert(approxeq(margY.mu, 1))
  assert(approxeq(margY.Sigma, 2/3))
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 drawCube( origin, size,color,alpha,a)
% From
% http://www.mathworks.com/matlabcentral/newsreader/view_thread/235581
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% 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
%--------------------------------------------------------------------------
x=([0 1 1 0 0 0;1 1 0 0 1 1;1 1 0 0 1 1;0 1 1 0 0 0]-0.5)*size(1)+origin(1);
y=([0 0 1 1 0 0;0 1 1 0 0 0;0 1 1 0 1 1;0 0 1 1 1 1]-0.5)*size(2)+origin(2);
z=([0 0 0 0 0 1;0 0 0 0 0 1;1 1 1 1 0 1;1 1 1 1 0 1]-0.5)*size(3)+origin(3);
arad=a*pi/180;
R=[cos(arad) -sin(arad) 0; sin(arad) cos(arad) 0; 0 0 1];

for i=1:6
    h=patch(x(:,i),y(:,i),z(:,i),color);
    set(h,'facealpha',alpha)
    rotate(h,[0 0 1],a,[0 0 0]);
end

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 exact = p38_exact ( )

%*****************************************************************************80
%
%% P38_EXACT returns the exact integral for problem 38.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the value of the integral.
%
  alpha = p38_param_get ();

  x = 2.0^alpha;

  exact = pi * besselj ( 0, x );

  return
end


function [norm_im, mask] = segmentation(or_im, blksze, thr)

    fun = inline('std(x(:))*ones(size(x))');

    std_devim = blkproc(or_im, [blksze blksze], fun);

    mask = std_devim > thr;
    mask_i = find(mask);

    or_im = or_im - mean(or_im(mask_i));
    norm_im = or_im / std(or_im(mask_i));

    %imshow(normim);


function v = normc(v)
% normalize along first axis (works for GPU arrays)
v = v./repmat(sum(v.^2, 1), size(v,1),1).^.5;


%%
%  函数imfilter的应用
f = imread('Fig0216(a).tif');
imshow(f);

w = ones(31);  %  生成滤波模板
gd = imfilter(f,w);
figure,imshow(gd,[ ])

gr = imfilter(f,w,'replicate');
figure,imshow(gr,[ ])

gs = imfilter(f,w,'symmetric');
figure,imshow(gs,[ ])

gc = imfilter(f,w,'circular');
figure,imshow(gc,[ ])

f8 = im2uint8(f);
g8r = imfilter(f,w,'replicate');
figure,imshow(g8r,[ ])


function [D, Dsm] = distMatrixShow( D, IDX, show )
% Useful visualization of a distance matrix of clustered points.
%
% D is sorted into k blocks, where the ith block contains all the points in
% cluster i. When D is displayed the blocks are shown explicitly.  Hence
% for a good clustering (under a spherical gaussian assumption) the
% 'diagonal' blocks ought to be mostly dark, and all other block ought to be
% relatively white.  One can thus quickly visualize the quality of the
% clustering, or even how clusterable the points are.  Outliers (according
% to IDX) are removed from D.
%
% USAGE
%  [D, Dsm] = distMatrixShow( D, IDX, [show] )
%
% INPUTS
%  D       - nxn distance matrix
%  IDX     - cluster membership [see kmeans2.m]
%  show    - [1] will display results in figure(show)
%
% OUTPUTS
%  D       - sorted nxn distance matrix
%  Dsm     - sorted and smoothed nxn distance matrix
%
% EXAMPLE
%  % not the best example since points are already ordered
%  [X,IDX] = demoGenData(100,0,5,2,10,2,0);
%  distMatrixShow( pdist2(X,X), IDX );
%
% See also VISUALIZEDATA, KMEANS2
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.0
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<3 || isempty(show) ); show=1; end

k = max(IDX);
n = size(D,1);

%%% remove outliers from D and IDX
inliers = IDX>0;
D = D( inliers, inliers );
IDX = IDX( inliers );

%%% get order of points and rearrange D and IDX
order = IDX2order( IDX );
IDX = IDX( order );
D = D( order, order );

%%% compute smoothed version of D
cnts = zeros(1,k); for i=1:k; cnts(i)=sum(IDX==i); end
cumCnts = cumsum(cnts);  cumCnts2=[0 cumCnts];  Dsm = D;
inds = 1:k; inds = inds( cnts>0 );
for i=inds
  rs = cumCnts2(i)+1:cumCnts2(i+1);
  for j=inds
    cs = cumCnts2(j)+1:cumCnts2(j+1);
    ds = D( rs, cs  );
    Dsm( rs, cs ) = mean(ds(:));
  end;
end;

%%% show D and lines seperating super clusters.
if(show)
  figure(show); clf;
  subplot(1,2,1); im(D); hold('on')
  for i=1:k-1
    line( [.5,n+.5], [cumCnts(i)+.5,cumCnts(i)+.5] );
    line( [cumCnts(i)+.5,cumCnts(i)+.5], [.5,n+.5] );
  end;
  hold('off');
  subplot(1,2,2); im( Dsm );
end


function [kcal] = cal2kcal(cal)
% Convert energy or work from calories to kilocalories.
% Note: these calories are different from the capitalized Calories found on
% American cereal boxes.  I suppose we'd be afraid to eat a candy bar with
% 250,000 calories in it.  Because then we'd feel like fatties.
% Chad A. Greene 2012
kcal = cal/1000;


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 = SVT(X,tau)
    [U,S,V] = svd(X,'econ');
    out = U*shrink(S,tau)*V';
end


function mask = AngularMask(r, c)
%
%        mask = AngularMask(r, c)
%
%        This function creates a mask for a Angular/Spherical map
%
%        Input:
%           -r: rows of the image in the Angular/Spherical format
%           -c: columns of the image in the Angular/Spherical format
%        Output:
%           -mask: a mask where the Angular/Spherical is defined
%
%     Copyright (C) 2011-15  Francesco Banterle
%
%     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/>.
%

[X,Y] = meshgrid(1:c, 1:r);
X = X / c * 2 - 1;
Y = Y / r * 2 - 1;
R = sqrt(X.^2 + Y.^2);

tmpMask = ones(r, c);
tmpMask(R > 1) = 0;

mask = zeros(r, c, 3);

for i=1:3
    mask(:,:,i) = tmpMask;
end

end


function [x,fval,exitflag,info,Opt] = opti_lsqcurvefit(fun,x0,xdata,ydata,lb,ub,opts)
%OPTI_LSQCURVEFIT Solve a NLS using an OPTI NLS Solver (Matlab Overload)
%
%   [x,fval,exitflag,info] = opti_lsqcurvefit(fun,x0,xdata,ydata,lb,ub)
%   solves the nonlinear least squares problem sum[(f(x,xdata)-ydata)^2]
%   where fun is the nonlinear function to be fitted [fun(x,xdata)],
%   starting at x0, to the sample data ydata. Optional bounds lb and ub can
%   be placed on the decision variables x.
%
%   [x,fval,exitflag,info] = opti_lsqcurvefit(fun,...,ub,opts) allows the
%   user to specify optiset options. This includes specifying a solver via
%   the 'solver' field of optiset.
%
%   [x,...,info,Opt] = opti_lsqcurvefit(fun,...) returns the internally
%   built OPTI object.

%   Copyright (C) 2011 Jonathan Currie (I2C2)

% Handle missing arguments
if nargin < 7, opts = optiset; end
if nargin < 6, ub = []; end
if nargin < 5, lb = []; end
if nargin < 4, error('You must supply at least 4 arguments to opti_lsqcurvefit'); end

%Build OPTI Object
Opt = opti('fun',fun,'data',xdata,ydata,'bounds',lb,ub,'x0',x0,'options',opts);

%Solve
[x,fval,exitflag,info] = solve(Opt);


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