 classdef matRad_MinMaxDVH < DoseConstraints.matRad_DoseConstraint
    % matRad_MinMaxDVH Implements a MinMaxDVH constraint
    %   See matRad_DoseConstraint for interface description
    %
    % References
    %   -
    %
    % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    % Copyright 2020 the matRad development team.
    %
    % This file is part of the matRad project. It is subject to the license
    % terms in the LICENSE file found in the top-level directory of this
    % distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
    % of the matRad project, including this file, may be copied, modified,
    % propagated, or distributed except according to the terms contained in the
    % LICENSE file.
    %
    % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    properties (Constant)
        name = 'DVH constraint';
        parameterNames = {'d^{ref}', 'V^{min}', 'V^{max}'};
        %parameterIsDose = logical([1 0 0]);
        parameterTypes = {'dose','numeric','numeric'};
    end

    properties
        voxelScalingRatio = 1;
        referenceScalingVal = 0.01;
        parameters = {30,0,100};
    end

    methods
        function obj = matRad_MinMaxDVH(dRef,vMin,vMax)

            %If we have a struct in first argument
            if nargin == 1 && isstruct(dRef)
                inputStruct = dRef;
                initFromStruct = true;
            else
                initFromStruct = false;
                inputStruct = [];
            end

            %Call Superclass Constructor (for struct initialization)
            obj@DoseConstraints.matRad_DoseConstraint(inputStruct);

            %now handle initialization from other parameters
            if ~initFromStruct
                if nargin == 3 && isscalar(vMax)
                    obj.parameters{3} = vMax;
                end

                if nargin >= 1 && isscalar(dRef)
                    obj.parameters{1} = dRef;
                end

                if nargin >= 2 && isscalar(vMin)
                    obj.parameters{2} = vMin;
                end
            end
        end

        %Overloads the struct function to add constraint specific
        %parameters
        function s = struct(obj)
            s = struct@DoseConstraints.matRad_DoseConstraint(obj);
            s.voxelScalingRatio = 1;
            s.referenceScalingVal = 0.01;
        end

        function cu = upperBounds(obj,n)
            cu = obj.parameters{3} / 100;
        end
        function cl = lowerBounds(obj,n)
            cl = obj.parameters{2} / 100;
        end
        %% Calculates the Constraint Function value
        function cDose = computeDoseConstraintFunction(obj,dose)

            %Fast DVH point calculation
            cDose = sum(dose >= obj.parameters{1})/numel(dose);

            %cDose = 100 * cDose; %In Percent

            % alternative constraint calculation 3/4 %
            % % get reference Volume
            % refVol = cst{j,6}(k).volume/100;
            %
            % % calc deviation
            % deviation = d_i - d_ref;
            %
            % % calc d_ref2: V(d_ref2) = refVol
            % d_ref2 = matRad_calcInversDVH(refVol,d_i);
            %
            % % apply lower and upper dose limits
            % if isequal(cst{j,6}(k).type, 'max DVH constraint')
            %    deviation(d_i < d_ref | d_i > d_ref2) = 0;
            % elseif isequal(cst{j,6}(k).type, 'min DVH constraint')
            %    deviation(d_i > d_ref | d_i < d_ref2) = 0;
            % end
            %
            % %c = sum(deviation);                              % linear deviation
            % %c = deviation'*deviation;                        % square devioation
            % c = (1/size(cst{j,4},1))*(deviation'*deviation); % square deviation with normalization
            % %c = (deviation).^2'*(deviation).^2;               % squared square devioation
            % alternative constraint calculation 3/4 %
        end

        %% Calculates the Constraint jacobian
        function cDoseJacob  = computeDoseConstraintJacobian(obj,dose)
            %logistic approximation

            %Do we really need to sort two times?
            dose_sort = sort(dose);

            % calculate scaling
            NoVoxels     = max(obj.voxelScalingRatio*numel(dose),10);
            absDiffsort  = sort(abs(obj.parameters{1} - dose_sort));

            deltaDoseMax = absDiffsort(min(ceil(NoVoxels/2),numel(dose)));

            % calclulate DVHC scaling
            DVHCScaling = min((log(1/obj.referenceScalingVal-1))/(2*deltaDoseMax),250);

            d_diff = dose - obj.parameters{1};

            cDoseJacob = (2/numel(dose))*DVHCScaling*exp(2*DVHCScaling*d_diff)./(exp(2*DVHCScaling*d_diff)+1).^2;

            % alternative constraint calculation 4/4 %
            % % get reference Volume
            % refVol = cst{j,6}(k).volume/100;
            %
            % % calc deviation
            % deviation = d_i - d_ref;
            %
            % % calc d_ref2: V(d_ref2) = refVol
            % d_ref2 = matRad_calcInversDVH(refVol,d_i);
            %
            % % apply lower and upper dose limits
            % if isequal(cst{j,6}(k).type, 'max DVH constraint')
            %      deviation(d_i < d_ref | d_i > d_ref2) = 0;
            % elseif isequal(cst{j,6}(k).type, 'min DVH constraint')
            %      deviation(d_i > d_ref | d_i < d_ref2) = 0;
            % end
            %
            % %jacobVec = ones(size(cst{j,4}));             % linear deviation
            % %jacobVec = 2*deviation;                      % square deviation
            % jacobVec = (1/size(cst{j,4},1))*2*deviation; % square deviation with normalization
            % %jacobVec = 4*(deviation).^3;                  % squared square devioation
            % alternative constraint calculation 4/4 %
        end
    end

end


filename='Gripping_quad_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'nonadjoint_compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
constraint_case = 'INEQUALITY';
optimizer = 'IPOPT'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.75;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function [Opt, R, E] = RetroTS(SN)
%    [Opt, OptR, OptE] = RetroTS(Opt)
%This function creates slice-based regressors for regressing out
% components of heart rate, respiration and respiration volume per time.
%
%  Opt is the options structure with the following fields
%     Mandatory:
%     ----------
%     Respfile: Respiration data file
%     Cardfile: Cardiac data file
%     PhysFS: Physioliogical signal sampling frequency in Hz.
%     Nslices: Number of slices
%     VolTR: Volume TR in seconds
%     Optional:
%     ---------
%     Prefix: Prefix of output file
%     SliceOffset: Vector of slice acquisition time offsets in seconds.
%                  (default is equivalent of alt+z)
%     RVTshifts: Vector of shifts in seconds of RVT signal.
%                (default is [0:5:20])
%     RespCutoffFreq: Cut off frequency in Hz for respiratory lowpass filter
%                     (default 3 Hz)
%     CardCutoffFreq: Cut off frequency in Hz for cardiac lowpass filter
%                     (default 3 Hz)
%     ResamKernel: Resampling kernel.
%                 (default is 'linear', see help interp1 for more options)
%     FIROrder: Order of FIR filter. (default is 40)
%     Quiet: [1]/0  flag. (defaut is 1) Show talkative progress as the program runs
%     Demo: [1]/0 flag. (default is 0)
%     RVT_out: [1]/0 flag for writing RVT regressors
%     Card_out: [1]/0 flag for writing Card regressors
%     Resp_out: [1]/0 flag for writing Resp regressors
%     SliceOrder: ['alt+z']/'alt-z'/'seq+z'/'seq-z'/'Custom'/filename.1D
%                 Slice timing information in seconds. The default is
%                 alt+z. See 3dTshift help for more info. 'Custom' allows
%                 the program to use the values stored in the
%                 Opt.SliceOffset array. If a value is placed into the
%                 SliceOrder field other than these, it is assumed to be
%                 the name of a 1D / text file containing the times for
%                 each slice (also in seconds).
%
%Example:
%
%  Opt.Respfile = 'Resp_epiRT_scan_14.dat'
%  Opt.Cardfile = 'ECG_epiRT_scan_14.dat'
%  Opt.VolTR = 2
%  Opt.Nslices = 20;
%  Opt.PhysFS = 1./0.02;   %20 msec sampling period, 50 samples per second
%  RetroTS(Opt);
%

% Input Mode 2 (for testing purposes only):
%  Opt: Scan number for file triplet to be processed.
%      Files called Resp*SN*, ECG*SN*, and scan*SN* are presumed to
%      exist in the directory from which RetroTS is called.
%      Many parameters' value are hard coded to defaults
%
% Output:
%  Opt: Structure of options including default settings.
%

% This option is not to be used because window width calculations do not use it
%     ResampFS: Frequency of resampled signal (default is same as PhysFS)

%Implementation Notes:
%%%%%%%%%%%%%%%%%%%%%%
% The script is intended as a prototype for development in C or Python
% The important routines are:
%    hilbert: Easily implemented with fft and ifft
%    interp1: A table lookup interpolation
%    fir: Tool for designing filters (we can just take it's coefficients)
%    filter: function to apply fir filter parameters (easy)
%
% All of the above can be easily implemented in C. However, I find it
% very useful to be able to plot the various steps in the process as we
% will undoubtedly face problems in the future. So I would vote for
% Python, assuming library vintage is not an issue.
%

if (nargin < 1),
   fprintf(2,'Need some input.\n');
   return;
end

R = struct([]);
E = struct([]);

if (~isstruct(SN)), %mode 1, toy mode
   iscan = 12;

   lll = zglobb({ sprintf('Resp*%d*',iscan),...
                  sprintf('ECG*%d*',iscan),...
                  sprintf('scan*%d*', iscan)});

   %Get some info from header file and set params
   f = fopen(lll(3).name, 'r');
   s = fscanf(f,'%c');
   fclose(f);
   ns = length(s);
   pat = 'RT Physio:\W*sampling\W*';
   Opt.PhysFS = 1000/str2num(strtok(s(regexp(s,pat,'end'):ns)));
   Opt.Nslices = 20;
   Opt.VolTR = 2;
   Opt.SliceMajor = 1;
   Opt.ResampFS = Opt.PhysFS; %upsampling frequency in Hz
   Opt.Quiet = 1;
   Opt.ResamKernel = 'linear';   %resampling filter for envelopes and phase
   Opt.FIROrder = 40;  %order of fir filter
   Opt.RVTshifts = [0:5:20];  %shifts, in seconds, applied to RVT curve
   Opt.Demo = 0;
   Opt.zerophaseoffset = 0;
   Opt.fcutoff = 3; %cut off frequency for filter
   Opt.RespCutoffFreq = 3;
   Opt.CardCutoffFreq = 3;
   Opt.Respfile = lll(1).name;
   Opt.Cardfile = lll(1).name;
   Opt.SliceOffset = ...
      [0:Opt.VolTR./Opt.Nslices:Opt.VolTR-Opt.VolTR./Opt.Nslices];
   Opt.Prefix = sprintf('%d',iscan);
   Opt.SepDups = 0;
   clear ('s');
   clear ('SN');
else,
   Opt = SN; clear ('SN');
   Opt.err = 1; Opt.zerophaseoffset = 0;
   if ( (~isfield(Opt,'Respfile') | isempty(Opt.Respfile))),
      Opt.Respfile = '';
      Opt.Resp_out = 0;
      Opt.RVT_out = 0;
   end
   if ( (~isfield(Opt,'Cardfile') | isempty(Opt.Cardfile))),
      Opt.Cardfile = '';
      Opt.Card_out = 0;
   end
   if ( (~isfield(Opt,'Respfile') | isempty(Opt.Respfile)) & (~isfield(Opt,'Cardfile') | isempty(Opt.Cardfile))),
      fprintf(2,'No Respfile or Cardfile\n');
      return;
   end
   if ( ~isfield(Opt,'PhysFS') | isempty(Opt.PhysFS)),
      fprintf(2,'Missing field PhysFS\n');
      return;
   end
   if ( ~isfield(Opt,'SliceMajor') | isempty(Opt.SliceMajor)),
      Opt.SliceMajor = 1;
   end
   if ( ~isfield(Opt,'Nslices') | isempty(Opt.Nslices)),
      fprintf(2,'Missing field Nslices\n');
      return;
   end
   if ( ~isfield(Opt,'VolTR') | isempty(Opt.VolTR)),
      fprintf(2,'Missing field VolTR\n');
      return;
   end
   if ( ~isfield(Opt,'RVTshifts') | isempty(Opt.RVTshifts)),
      Opt.RVTshifts=[0:5:20];
   end
   if ( ~isfield(Opt,'ResampFS') | isempty(Opt.ResampFS)),
      Opt.ResampFS=Opt.PhysFS;
   end
   if ( ~isfield(Opt,'RespCutoffFreq') | isempty(Opt.RespCutoffFreq)),
      Opt.RespCutoffFreq=3;
   end
   if ( ~isfield(Opt,'CardCutoffFreq') | isempty(Opt.CardCutoffFreq)),
      Opt.CardCutoffFreq=3;
   end
   if ( ~isfield(Opt,'ResamKernel') | isempty(Opt.ResamKernel)),
      Opt.ResamKernel='linear';
   end

   if ( ~isfield(Opt,'FIROrder') | isempty(Opt.FIROrder)),
      Opt.FIROrder=40;
   end
   if ( ~isfield(Opt,'Quiet') | isempty(Opt.Quiet)),
      Opt.Quiet=1;
   end
   if ( ~isfield(Opt,'Demo') | isempty(Opt.Demo)),
      Opt.Demo=0;
   end
   if ( ~isfield(Opt,'Prefix') | isempty(Opt.Prefix)),
      Opt.Prefix = 'oba';
   end
   if ( ~isfield(Opt,'Resp_out') | isempty(Opt.Resp_out)),
      Opt.Resp_out = 1;
   end
   if ( ~isfield(Opt,'Card_out') | isempty(Opt.Card_out)),
      Opt.Card_out = 1;
   end
   if ( ~isfield(Opt,'RVT_out') | isempty(Opt.RVT_out)),
      Opt.RVT_out = 1;
   end
   if ( ~isfield(Opt,'SepDups') | isempty(Opt.SepDups)),
      Opt.SepDups = 0;
   end

   dtt = Opt.VolTR/Opt.Nslices; tt = 0.0;

   % & ~isfield(Opt, 'SliceOffset')
   % & (Opt.SliceOrder ~= 'alt+z')

      % default slice offset times are for alt+z (alternating slice timing)
   if ( ~isfield(Opt,'SliceOffset') | isempty(Opt.SliceOffset))
      Opt.SliceOffset=zeros(Opt.Nslices,1);
   end
   if(~isfield(Opt,'SliceOrder'))
      Opt.SliceOrder = 'alt+z'
   end

   if (isfield(Opt,'SliceOrder'))
      Opt.SliceOffset=zeros(Opt.Nslices,1);
      if(strcmpi(Opt.SliceOrder,'alt+z'))
         for (i=1:2:Opt.Nslices),
            Opt.SliceOffset(i) = tt; tt = tt+dtt;
         end
         for (i=2:2:Opt.Nslices),
            Opt.SliceOffset(i) = tt; tt = tt+dtt;
         end
      elseif(strcmpi(Opt.SliceOrder, 'alt+z2'))
         for (i=2:2:Opt.Nslices),
            Opt.SliceOffset(i) = tt; tt = tt+dtt;
         end
         for (i=1:2:Opt.Nslices),
            Opt.SliceOffset(i) = tt; tt = tt+dtt;
         end
      elseif(strcmpi(Opt.SliceOrder, 'seq+z'))
         for (i=1:1:Opt.Nslices),
            Opt.SliceOffset(i) = tt; tt = tt+dtt;
         end
      elseif(strcmpi(Opt.SliceOrder,'seq-z'))
         for (i=Opt.Nslices:-1:1),
            Opt.SliceOffset(i) = tt; tt = tt+dtt;
         end
      elseif(strcmpi(Opt.SliceOrder,'alt-z'))
         for (i=Opt.Nslices:-2:1),
            Opt.SliceOffset(i) = tt; tt = tt+dtt;
         end
         for (i=Opt.Nslices-1:-2:1),
            Opt.SliceOffset(i) = tt; tt = tt+dtt;
         end
      elseif(strcmpi(Opt.SliceOrder,'Custom'))
          % timing already set in Opt.SliceOffset, do nothing
      else
         % read in time offsets from a file (SliceOrder is actually a
         % filename)
         readopt.verb = 0;
         [err, Opt.SliceOffset] = Read_1D(Opt.SliceOrder,readopt);
         if(length(Opt.SliceOffset)~=Opt.Nslices)
            fprintf('Could not read enough slice offsets from file');
            exit(1);
         end
      end
   end
   if(~Opt.Quiet)
      fprintf('Slice timing:'); Opt.SliceOffset
   end
   if ( ~isfield(Opt,'ShowGraphs') | isempty(Opt.ShowGraphs)),
      Opt.ShowGraphs = 1; % show graphs by default
   end
end

if (Opt.SepDups),
   fprintf(1,'WARNING: SepDups should not be used\n');
   fprintf(1,'         It is kept in the code for debugging\n');
   fprintf(1,'         purposes.\n');
end

%create option copy for each type of signal
   OptR = Opt;
      OptR.fcutoff = Opt.RespCutoffFreq;
      OptR.AmpPhase = 1;   %amplitude based phase for respiration
      %OptR.as_percover = 50; %percent overlap of windows for fft
      %OptR.as_windwidth = 0; %window width in seconds for fft, 0 for full window
      %OptR.as_fftwin = 0 ; %1 == hamming window. 0 == no windowing
   OptE = Opt;
      OptE.fcutoff = Opt.CardCutoffFreq;
      OptE.AmpPhase = 0;   %time based phase for cardiac signal

%Get the peaks for R and E
if (~isempty(Opt.Respfile)),
   [R,e]= PeakFinder(Opt.Respfile,OptR);
   if (e), fprintf(2,'Died in PeakFinder\n'); return; end
else
   R = struct([]);
end
if (~isempty(Opt.Cardfile)),
   [E,e] = PeakFinder(Opt.Cardfile,OptE);
   if (e), fprintf(2,'Died in PeakFinder\n'); return; end
else
   E = struct([]);
end
%get the phase
if (~isempty(R)),
   fprintf(2, 'Estimating phase for R\n');
   R = PhaseEstimator(R,OptR);
end
if (~isempty(E)),
   fprintf(2, 'Estimating phase for E\n');
   E = PhaseEstimator(E,OptE);
end

%Now do the RVT for Respiration
if (~isempty(R)),
   fprintf(2,'Computing RVT from peaks\n');
   R = RVT_from_PeakFinder(R, OptR);
end

%Show some results
if(Opt.ShowGraphs)
   if (~isempty(R)),
      fprintf(2, 'Showing RVT Peaks for R\n');
      Show_RVT_Peak(R,1);
   end
end

if (0),
   %write retroicor regressors
   for (i=1:1:Opt.Nslices),
      fname = sprintf('%s.RetroCard.slc%02d.1D', Opt.Prefix, i);
      wryte3(E.phz_slc_reg(:,:,i), fname, 1);
      fname = sprintf('%s.RetroResp.slc%02d.1D', Opt.Prefix, i);
      wryte3(R.phz_slc_reg(:,:,i), fname, 1);
   end

   %and write the RVT puppy, plus or minus a few seconds delay
   fname = sprintf('%s.RetroRVT.1D', Opt.Prefix);
   wryte3(R.RVTRS_slc, fname, 1);
end

%also generate files as 3dREMLfit likes them
nn = 0;
nRv = 0; nRp = 0; nE = 0;
if (~isempty(R)),
   nn = length(R.tst);
   nRp = size(R.phz_slc_reg,2);
   nRv = size(R.RVTRS_slc,2);
end
if (~isempty(E)), %must have E
   nn = length(E.tst); %ok to overwrite length(R.tst), should be same.
   nE = size(E.phz_slc_reg,2);
end

if ( ~Opt.Card_out & ~Opt.Resp_out & ~Opt.RVT_out ),
   fprintf(2, 'Options Card_out, Resp_out, and RVT_out all 0.\nNo output required.\n');
   return;
end
Opt.RemlOut = zeros(  nn,...
                  Opt.Nslices .* ...
                     (  (Opt.RVT_out~=0) .*nRv + ...
                        (Opt.Resp_out~=0).*nRp + ...
                        (Opt.Card_out~=0).*nE ) );
cnt = 0;
head = sprintf([ '# <RetroTSout\n',...
                  '# ni_type = "%d*double"\n'...
                  '# ni_dimen = "%d"\n'...
                  '# ColumnLabels = "'],...
                  size(Opt.RemlOut,2), size(Opt.RemlOut,1) );
tail = sprintf('"\n# >\n');
tailclose = sprintf('# </RetroTSout>\n');

label = head;

if (Opt.SliceMajor == 0), %old approach, not handy for 3dREMLfit
   %RVT
   if (Opt.RVT_out),
      for (j=1:1:size(R.RVTRS_slc,2)),
         for (i=1:1:Opt.Nslices),
            cnt = cnt + 1;
            Opt.RemlOut(:,cnt) = R.RVTRS_slc(:,j); %same for each slice
            label = sprintf('%s s%d.RVT%d ;', label, i-1, j-1);
          end
      end
   end
   %Resp
   if (Opt.Resp_out),
      for (j=1:1:size(R.phz_slc_reg,2)),
         for (i=1:1:Opt.Nslices),
            cnt = cnt + 1;
            Opt.RemlOut(:,cnt) = R.phz_slc_reg(:,j,i);
            label = sprintf('%s s%d.Resp%d ;', label, i-1, j-1);
         end
      end
   end
   %Card
   if (Opt.Card_out),
      for (j=1:1:size(E.phz_slc_reg,2)),
         for (i=1:1:Opt.Nslices),
            cnt = cnt + 1;
            Opt.RemlOut(:,cnt) = E.phz_slc_reg(:,j,i);
            label = sprintf('%s s%d.Card%d ;', label, i-1, j-1);
         end
      end
   end
   fid = fopen(sprintf('%s.retrots.1D', Opt.Prefix),'w');
else
   for (i=1:1:Opt.Nslices),
      if (Opt.RVT_out),
         %RVT
         for (j=1:1:size(R.RVTRS_slc,2)),
            cnt = cnt + 1;
            Opt.RemlOut(:,cnt) = R.RVTRS_slc(:,j); %same regressor for each slice
            label = sprintf('%s s%d.RVT%d ;', label, i-1, j-1);
         end
      end
      if (Opt.Resp_out),
         %Resp
         for (j=1:1:size(R.phz_slc_reg,2)),
            cnt = cnt + 1;
            Opt.RemlOut(:,cnt) = R.phz_slc_reg(:,j,i);
            label = sprintf('%s s%d.Resp%d ;', label, i-1, j-1);
         end
      end
      if (Opt.Card_out),
         %Card
         for (j=1:1:size(E.phz_slc_reg,2)),
            cnt = cnt + 1;
            Opt.RemlOut(:,cnt) = E.phz_slc_reg(:,j,i);
            label = sprintf('%s s%d.Card%d ;', label, i-1, j-1);
         end
      end
   end
   fid = fopen(sprintf('%s.slibase.1D', Opt.Prefix),'w');
end

%remove very last ';'
label = label(1:end-1);

fprintf(fid,'%s',label);
fprintf(fid,'%s ',tail);
for(i=1:1:size(Opt.RemlOut,1)),
   fprintf(fid,'%g ', Opt.RemlOut(i,:));
   fprintf(fid,'\n ');
end
fprintf(fid,'%s',tailclose);
fclose(fid);

Opt.err = 0;

return;


function [ cfg ] = default_parameters_dat()
%DEFAULT_PARAMETERS_DAT Default parametrization
  cfg = struct('show_figures', false);

  % Image scaling
  cfg.img_scale_target_diagonal = 75; % Length of object hypothesis diagonal (Used to downscale image)

  % Search and surrounding regions
  cfg.search_win_padding = 2; % Search win = hypothesis + X * max(w,h of hypothesis)
  cfg.surr_win_factor = 1.9;  % Surrounding win = X * surr_win_factor

  % Appearance model
  cfg.color_space = 'rgb';                % 'rgb', 'hsv', or 'lab'
  cfg.num_bins = 16;                      % Number of bins per channel
  cfg.bin_mapping = getBinMapping(cfg.num_bins); % Maps pixel values from [0, 255] to the corresponding bins
  cfg.prob_lut_update_rate = .05;         % Update rate for LUT
  cfg.distractor_aware = true;            % Toggle distractor-awareness
  cfg.adapt_thresh_prob_bins = 0:0.05:1;  % Bins for adaptive threshold.

  % Motion estimation
  cfg.motion_estimation_history_size = 5;  % Motion based on past X frames (Set to 0 to disable motion estimation)

  % NMS-based localization
  cfg.nms_scale = 1;                  % NMS yields rects scaled by X - no scaling typically achieves best localization (Lower scales yield more distractors - so choose wisely)
  cfg.nms_overlap = .9;               % Overlap between candidate rectangles for NMS
  cfg.nms_score_factor = .5;          % Report all rectangles with score >= X * best_score
  cfg.nms_include_center_vote = true; % Prefer hypothesis with highly confident center regions

  % camera correction part
  cfg.transform = '';
end


function g = dnetGradient(params, model)

% DNETGRADIENT Density Network gradient wrapper.
% FORMAT
% DESC is a wrapper function for the gradient of the negative log
% likelihood of an Density Network model with respect to the latent postions
% and parameters.
% ARG params : vector of parameters and latent postions where the
% gradient is to be evaluated.
% ARG model : the model structure into which the latent positions
% and the parameters will be placed.
% RETURN g : the gradient of the negative log likelihood with
% respect to the latent positions and the parameters at the given
% point.
%
% SEEALSO : dnetLogLikeGradients, dnetExpandParam
%
% COPYRIGHT : Neil D. Lawrence, 2008

% MLTOOLS

model = dnetExpandParam(model, params);
g = - dnetLogLikeGradients(model);


 function y = reale(x, arg2, arg3)
%| Return real part of complex data (with error checking).
%function y = reale(x, arg2, arg3)
%|
%| y = reale(x)
%| y = reale(x, tol) [default tol is 1e-13 for double, else 1e-6]
%| y = reale(x, 'warn', 'message')
%| y = reale(x, 'error')
%| y = reale(x, 'report')
%| y = reale(x, 'prompt')
%| y = reale(x, 'disp')
%|
%| Checks that imaginary part is negligible (warns etc. if not).
%|
%| Copyright Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if nargin == 1 && streq(x, 'test'), reale_test, return, end

com = 'error';
if isa(x, 'double')
	tol = 1e-13;
else
	tol = 1e-6;
end

if nargin > 1
	if ischar(arg2)
		com = arg2;
	elseif isnumeric(arg2)
		tol = arg2;
	end
end

switch com
case {'disp','prompt', 'report'}
	;
case 'warn'
	onlywarn = 1;
case 'error'
	onlywarn = 0;
otherwise
	fail('bad argument "%s"', com)
end

max_abs_x = max(abs(x(:)));
if max_abs_x == 0
	if any(imag(x(:)) ~= 0)
		fail 'max real 0, but imaginary!'
	else
		y = real(x);
		return
	end
end

frac = max(abs(imag(x(:)))) / max_abs_x;
if streq(com, 'report')
	printm('imaginary part %g%%', frac * 100)
	return
end

if frac > tol
	[cname line] = caller_name;
	t = sprintf('%s(%d): %s: imaginary fraction of %s [class %s] is %g', ...
		cname, line, mfilename, inputname(1), class(x), frac);
	if isvar('arg3')
		t = [t ', ' arg3];
	end
	if streq(com, 'disp')
		disp(t)

	elseif streq(com, 'prompt')
		printm('reale() called for input with imaginary part %g%%', frac * 100)
		printm('reale() called in context where a large imaginary part')
		printm('is likely an *error*.  proceed with caution!')
		t = input('proceed? [y|n]: ', 's');
		if isempty(t) || t(1) ~= 'y'
			printm('ok, aborting is probably wise!')
			error ' '
		end

	elseif onlywarn
		disp(t)
	else
		fail(t)
	end
end
y = real(x);

function reale_test
x = 7 + 1i*eps;
reale(x, 'warn');
reale(x, 'prompt');
%reale(x, 'report'); % check error reporting
%reale(x, eps/100) % check error faulting


function test_issue1952

% WALLTIME 00:20:00
% MEM 4gb

% DEPENDENCY ft_denoise_ssp

%% PATHS

% path = '/home/lau/Dokumenter/wakeman_henson_face_data/ds117/sub001/MEG';
% dataset = fullfile(path, 'run_01_raw.fif');

dataset = dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/epilepsy/raw/case1/neuromag/case1_cHPI_raw.fif');
if ~exist(dataset, 'file')
  % probably not on filesystem at Donders
  datadir = tempdir;
  s = ftp('ftp.fieldtriptoolbox.org');
  cd(s,'pub/fieldtrip/tutorial/epilepsy/raw/case1/neuromag');
  mget(s, 'case1_cHPI_raw.fif', datadir);
  close(s);
  dataset = fullfile(datadir, 'case1_cHPI_raw.fif');
end

%% SEGMENT

cfg                         = [];
cfg.dataset                 = dataset;

hdr = ft_read_header(dataset, 'checkmaxfilter', false);
fs  = hdr.Fs;

cfg.trl = [(fs:fs:100*fs)+1;(2*fs:fs:101*fs)]';
cfg.trl(:,3) = 0;

% cfg.trialdef.eventtype      = 'STI101';
% cfg.trialdef.eventvalue     = 5;
% cfg.trialdef.prestim        = 0.200;
% cfg.trialdef.poststim       = 0.600;
% cfg.continuous              = 'yes';
% cfg = ft_definetrial(cfg);

% preprocess the data
cfg.channel                 = {'MEG'};
cfg.demean                  = 'yes';     % apply baselinecorrection
%cfg.baselinewindow          = [-0.200 0]; % on basis of mean signal between -0.05 and 0
cfg.checkmaxfilter = false;
data_fif = ft_preprocessing(cfg);

%% TIMELOCK

cfg = [];

timelock = ft_timelockanalysis(cfg, data_fif);

%% PLOT TIMELOCK

cfg = [];
cfg.layout = 'neuromag306mag_helmet.mat';
ft_multiplotER(cfg, timelock);

%% DENOISE WITH SSP

cfg = [];
cfg.ssp = 'all';

denoised_timelock = ft_denoise_ssp(cfg, timelock);

%% DENOISE WITH SSP CELL ARRAY

cfg = [];
% cfg.ssp = {'mag_ssp_upright_fif___pca_mags_v1', ...
%            'mag_ssp_upright_fif___pca_mags_v2', ...
%            'mag_ssp_upright_fif___pca_mags_v3'};
cfg.ssp = {'magssp68iason_fif___pca_v1', ...
  'magssp68iason_fif___pca_v2'};

denoised_timelock_specific = ft_denoise_ssp(cfg, timelock);

%% PLOT SINGLE PLOT

cfg = [];
cfg.channel = 'MEG2611';
cfg.layout = 'neuromag306mag_helmet.mat';

ft_singleplotER(cfg, timelock, denoised_timelock, denoised_timelock_specific);

%% PLOT DENOISED TIMELOCK

cfg = [];
cfg.layout = 'neuromag306mag_helmet.mat';
ft_multiplotER(cfg, timelock, denoised_timelock, denoised_timelock_specific); %% fails


view=getSelectedVolume;
classFileName = [view.leftPath(1:end-5) '.class'];
c = readClassFile(classFileName);
anatSize = size(view.anat);

voi = c.header.voi;
anat = uint8(zeros([anatSize, 3]));
wm = repmat(logical(0), anatSize);
wm([voi(3):voi(4)], [voi(1):voi(2)], [voi(5):voi(6)]) = permute(c.data, [2,1,3])==c.type.white;
for(ii=1:3)
    anat(:,:,:,ii) = view.anat;
end
tmp = anat(:,:,:,1);
tmp(wm) = uint8(double(tmp(wm))+20);
anat(:,:,:,1) = tmp;

mHires = makeMontage3(anat(:,:,:,1), anat(:,:,:,2), anat(:,:,:,3), [voi(5):voi(6)], view.mmPerVox(1), 0);


function [recalls, allRecalls]= testCore(db, qFeat, dbFeat, varargin)
    opts= struct(...
        'nTestSample', inf, ...
        'recallNs', [1:5, 10:5:100], ...
        'printN', 10 ...
        );
    opts= vl_argparse(opts, varargin);

    searcherRAW_= @(iQuery, nTop) rawNnSearch(qFeat(:,iQuery), dbFeat, nTop);
    if ismethod(db, 'nnSearchPostprocess')
        searcherRAW= @(iQuery, nTop) db.nnSearchPostprocess(searcherRAW_, iQuery, nTop);
    else
        searcherRAW= searcherRAW_;
    end
    [res, recalls]= recallAtN( searcherRAW, db.numQueries,  @(iQuery, iDb) db.isPosQ(iQuery, iDb), opts.recallNs, opts.printN, opts.nTestSample );

    allRecalls= recalls;
    recalls= mean( allRecalls, 1 )';

end


function denoised = KSVD_WRAP(image_path, sigma)

    Noisy_Image = im2double(imread(image_path));

    redChannel = Noisy_Image(:,:,1); % Red channel
    greenChannel = Noisy_Image(:,:,2); % Green channel
    blueChannel = Noisy_Image(:,:,3); % Blue channel

    redChannel=im2double(redChannel);
    if (length(size(redChannel))>2)
        redChannel = rgb2gray(redChannel);
    end
    if (max(redChannel(:))<2)
        redChannel = redChannel*255;
    end

    greenChannel=im2double(greenChannel);
    if (length(size(greenChannel))>2)
        greenChannel = rgb2gray(greenChannel);
    end
    if (max(greenChannel(:))<2)
        greenChannel = greenChannel*255;
    end

    blueChannel=im2double(blueChannel);
    if (length(size(blueChannel))>2)
        blueChannel = rgb2gray(blueChannel);
    end
    if (max(blueChannel(:))<2)
        blueChannel = blueChannel*255;
    end

    bb=8; % block size
    RR=2; % redundancy factor
    K=RR*bb^2; % number of atoms in the dictionary

    [Denoised_Image_Red,~] = denoiseImageKSVD(redChannel, sigma,K);
    [Denoised_Image_Green,~] = denoiseImageKSVD(greenChannel, sigma,K);
    [Denoised_Image_Blue,~] = denoiseImageKSVD(blueChannel, sigma,K);

    recombinedRGBImage = cat(3, Denoised_Image_Red, Denoised_Image_Green, Denoised_Image_Blue);

    denoised = uint8(255 * mat2gray(recombinedRGBImage));

end


function sF = plus(sF1, sF2)
% overloads sF1 + sF2

if isa(sF2,'S2FunHarmonic')
  sF = sF2 + sF1;
  return
end

sF = S2FunHandle(@(v) sF1.eval(v) + sF2.eval(v));

end


function [ objectIndex ] = GetScenario_waypointCurve(varargin)
% This scenario is designed to present a waypoint following exercise.

fprintf('[SCENARIO]\tGetting the waypoint following exercise.\n');

% DEFAULT CONFIGURATION
defaultConfig = struct('file','scenario.mat',...
                       'agents',[],...
                       'agentVelocity',0,...
                       'noiseFactor',0,...
                       'waypoints',3,...
                       'waypointRadius',0.1,...
                       'plot',0);

% Instanciate the scenario builder
SBinstance = scenarioBuilder();

% PARSE THE USER OVERRIDES USING THE SCENARIO BUILDER
[inputConfig] = SBinstance.configurationParser(defaultConfig,varargin);
% AGENT CONDITIONS
agentIndex = inputConfig.agents;
agentNumber = numel(agentIndex);                    % Declare the number of agents

%% DEFINE THE AGENT CONFIGURATION
% MOVE THROUGH THE AGENTS AND INITIALISE WITH GLOBAL PROPERTIES
fprintf('[SCENARIO]\tAssigning agent definition...\n');
for i=1:agentNumber
    agentIndex{i}.SetGLOBAL('position',[0;0;0] + inputConfig.noiseFactor*randn(3,1));
    agentIndex{i}.SetGLOBAL('velocity',[inputConfig.agentVelocity;0;0] + inputConfig.noiseFactor*randn(3,1));
    agentIndex{i}.SetGLOBAL('quaternion',[1;0;0;0]);
end

%% DEFINE THE WAYPOINT CONFIGURATION
fprintf('[SCENARIO]\tBuilding the new scenario...\n');
waypointDefiningRadius = 20;
angleOffset = -pi/2;                                % Align the first waypoint to be directly infront of the agent
waypointConfig = SBinstance.planarAngle(...
    'objects',agentNumber,...
    'radius',waypointDefiningRadius,...
    'pointA',[waypointDefiningRadius;-waypointDefiningRadius;-1],...
    'pointB',[waypointDefiningRadius;-waypointDefiningRadius;0],...
    'velocities',0,...
    'zeroAngle',angleOffset);

% MOVE THROUGH THE WAYPOINTS AND INITIALISE WITH GLOBAL PROPERTIES
fprintf('[SCENARIO]\tAssigning waypoint definitions:\n');
for index = 1:inputConfig.waypoints
    nameString = sprintf('WP-%s',agentIndex{1}.name);
    waypointIndex{index} = waypoint('radius',inputConfig.waypointRadius,'name',nameString);
    % APPLY GLOBAL STATE VARIABLES
    waypointIndex{index}.SetGLOBAL('position',waypointConfig.positions(:,index));
    waypointIndex{index}.SetGLOBAL('velocity',waypointConfig.velocities(:,index));
    waypointIndex{index}.SetGLOBAL('quaternion',waypointConfig.quaternions(:,index));
    % Assign waypoint to agent with priority
    waypointIndex{index} = waypointIndex{index}.CreateAgentAssociation(agentIndex{1},1/index);  % Create waypoint with association to agent
end

% BUILD THE COLLECTIVE OBJECT INDEX
objectIndex = horzcat(agentIndex,waypointIndex);
% PLOT THE SCENE
if inputConfig.plot
    SBinstance.plotObjectIndex(objectIndex);
end
clearvars -except objectIndex % Clean-up
end


report_this_filefun(mfilename('fullpath'));

% Now lets plot the color-map of the z-value
%
figure

set(gca,'visible','off','FontSize',ZmapGlobal.Data.fontsz.m,'FontWeight','bold',...
    'FontWeight','bold','LineWidth',1.5,...
    'Box','on','SortMethod','childorder')

rect = [0.18,  0.10, 0.7, 0.75];
rect1 = rect;

% set values greater tresh = nan
%
re4 = re3;l = r > tresh; re4(l) = zeros(1,length(find(l)))*nan;

% plot image
%
orient portrait
set(gcf,'PaperPosition', [2. 1 7.0 5.0])

axes('position',rect);hold on
pco1 = pcolor(gx,gy,(re4));
axis([ min(gx) max(gx) min(gy) max(gy)]); axis image; hold on
shading interp;
caxis([0.40 1.00])
if exist('pro', 'var')
    l = pro > 0;
    pro2 = pro;
    pro2(l) = pro2(l)*nan;
    %cs =contour(gx,gy,pro,[ 99 100],'w--');

    % cs =contour(gx,gy,pro,[95 99],'k');
    %[cs, hc] =contour(gx,gy,pro,[ 99 100],'w-');
end % if exist pro
h = jet(64);h = [  h(64:-1:1,:)]; colormap(h)
%end
if fre == 1
    caxis([fix1 fix2])
end

set(gca,'Color',[0.9 0.9 0.9])
%brighten(0.5)
%xlabel('Distance in [km]','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
%ylabel('depth in [km]','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
%set(gca,'XTickLabels',[])
set(gca,'YTick',[ -15 -10 -5 0 ])
set(gca,'YTickLabels',[ 15 10 5 0 ])

if exist('maex', 'var')
    pl = plot(maex,-maey,'*k');
    set(pl,'MarkerSize',6,'LineWidth',2)
end
overlay

set(gca,'visible','on','FontSize',10,'FontWeight','normal',...
    'FontWeight','normal','LineWidth',1.0,...
    'Box','on','TickDir','out')
h1 = gca;hzma = gca;

% Create a colobar
%
%h5 = colorbar('vertical');
%set(h5,'Pos',[0.25 0.25 0.03 0.25],...
%'FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m,'TickDir','out')

% Make the figure visible
%

axes(h1)


% plot the filtered estimates for navigation states, imu errors and imu
% error covariance stds.

kf = readdata(filresfile, 1+18);
if (~isOutNED)
%    save_google_kml(kf(:,2:4), kmlfilename);
end
if(useGPS)
inillh_ant2=options.inillh_ant;

posdata=loadAllGPSData(gpsfile, [kf(1,1), kf(end,1)]);
maxl=size(posdata,1);
if(isOutNED)
    for i=1:maxl
        rovXYZ = posdata(i,2:4);
        posdata(i,2:4)=posdiff_v001(rovXYZ',inillh_ant2);
    end
end
end
nextFig=1;
f(nextFig) = figure;
if (isOutNED)
    %         plot(kf(:,3),kf(:,2),'g.')
    %         hold on
    %         plot(posdata(:,3),posdata(:,2),'r+')
    %         grid
    %         xlabel('East [m]')
    %         ylabel('North[m]')
    plot3(kf(:,1)-kf(1,1),kf(:,3),kf(:,2),'g.')
    hold on
    if(useGPS)
    plot3(posdata(:,1)-kf(1,1),posdata(:,3),posdata(:,2),'r+')
    end
    grid
    axis equal
    xlabel('Time [s]')
    ylabel('East [m]')
    zlabel('North[m]')
else
    plot(kf(:,3),kf(:,2),'g.')
    hold on
    if(useGPS)
    plot(posdata(:,3),posdata(:,2),'r+')
    end
    grid
    axis equal
    xlabel('Longitude [radian]')
    ylabel('Lattitude [radian]')
end
title('green KF trajectory and the red GPS reference for GPS antenna');
saveas(f(nextFig),[resdir,'red truth and track'],'fig');

nextFig=nextFig+1;
f(nextFig) = figure;
plot(kf(:,1)-kf(1,1),kf(:,4),'g.')
hold on
if(useGPS)
plot(posdata(:,1)-kf(1,1),posdata(:,4),'r+');
end
grid
xlabel('Time [s]')
ylabel('Height/m')
title('Height of antenna by KF(green) and reference(red)');
saveas(f(nextFig),[resdir 'red truth and height'],'fig');

isPrintToFile=0;
if(options.useCam)
    campose=load(options.camPoseFile);
    if(~isempty(campose))
        nextFig=nextFig+1;
        f(nextFig) = figure;
        plot(campose(:,2)-campose(1,2),campose(:,6:8),'marker','.')
        grid
        xlabel('Time [s]')
        ylabel('m')
        legend('x','y','z')
        title('Ts2c');
        if isPrintToFile
            print(f(nextFig), '-dtiff', [plotDir 'Ts2c']);
        end

        nextFig=nextFig+1;
        f(nextFig) = figure;
        plot(campose(:,2)-campose(1,2),campose(:, 3:5),'marker','+');
        grid
        xlabel('Time [s]')
        ylabel('degree')
        legend('Roll','Pitch','Yaw/Heading')
        title('Cs2c');
        if isPrintToFile
            print(f(nextFig), '-dtiff', [plotDir 'qs2c']);
        end
        nextFig=nextFig+1;
        f(nextFig) = figure;
        plot(campose(:,2)-campose(1,2),campose(:,12:14),'marker','.')
        grid
        xlabel('Time [s]')
        ylabel('m')
        legend('x','y','z')
        title('Xs02e');
        if isPrintToFile
            print(f(nextFig), '-dtiff', [plotDir 'Xs02e']);
        end

        nextFig=nextFig+1;
        f(nextFig) = figure;
        plot(campose(:,2)-campose(1,2),campose(:, 9:11),'marker','+');
        grid
        xlabel('Time [s]')
        ylabel('degree')
        legend('Roll','Pitch','Yaw/Heading')
        title('qs02e');
        if isPrintToFile
            print(f(nextFig), '-dtiff', [plotDir 'qs02e']);
        end
    end
end
%close all
filresfile=[resdir, 'filresult.bin'];
kf = readdata(filresfile, 1+18);
plotNEDKalmanFilterResult(kf, resdir);
imuresfile=[resdir, 'imuresult.bin'];
err = readdata(imuresfile, 1+24);
ploterr_v001(err,resdir);


function leap = halton_leap_get ( )

%*****************************************************************************80
%
%% HALTON_BASE_GET gets the leap vector of the leaped Halton subsequence.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer LEAP(1:DIM_NUM), the Halton leap vector.
%
  global halton_BASE
  global halton_LEAP
  global halton_DIM_NUM
  global halton_SEED
  global halton_STEP

  dim_num = halton_DIM_NUM;

  if ( ~halham_dim_num_check ( dim_num ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HALTON_LEAP_GET - Fatal error!\n' );
    error ( 'HALTON_LEAP_GET - Fatal error!' );
  end

  if ( length ( halton_LEAP ) ~= dim_num )

    halton_LEAP = [];

    for i = 1 : dim_num
      halton_LEAP(i) = 1;
    end

  end

  leap(1:dim_num) = halton_LEAP(1:dim_num);

  return
end


function prctmus = gp_predprctmu(gp, x, y, varargin)
%GP_PREPRCTMU  Percentiles of the distribution of the location parameter
%
%  Description
%    PRCTMU = GP_PREDPRCTMU(GP, X, Y, XT, OPTIONS)
%    takes a GP structure together with matrix X of training inputs
%    and vector Y of training targets, and evaluates the
%    percentiles of the distribution of the location parameter at
%    test inputs XT.
%
%    PRCTMU = GP_PREDPRCTMU(GP, X, Y, OPTIONS)
%    evaluates the percentiles of the distribution of the location
%    parameter at training inputs X.
%
%    OPTIONS is optional parameter-value pair
%      prct   - percentiles to be computed (default = [5 50 95])
%      nsamp  - determines the number of samples used by GP_RND in case of
%               MCMC or IA (default = 5000).
%      predcf - index vector telling which covariance functions are
%               used for prediction. Default is all (1:gpcfn)
%      tstind - a vector defining, which rows of X belong to which
%               training block in *IC type sparse models. Default is [].
%               See also GP_PRED.
%      z      - optional observed quantity in triplet (x_i,y_i,z_i)
%               Some likelihoods may use this. For example, in case of
%               Poisson likelihood we have z_i=E_i, that is, expected value
%               for ith case.
%      zt     - optional observed quantity in triplet (xt_i,yt_i,zt_i)
%               Some likelihoods may use this. For example, in case of
%               Poisson likelihood we have z_i=E_i, that is, the expected
%               value for the ith case.
%
%  See also
%    GP_PRED, GP_PAK, GP_UNPAK
%
% Copyright (c) 2011 Ville Tolvanen
% Copyright (c) 2011-2012 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'GP_PREDPRCTMU';
  ip.addRequired('gp',@(x) isstruct(x) || iscell(x));
  ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addOptional('xt', [], @(x) isempty(x) || (isreal(x) && all(isfinite(x(:)))))
  ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('zt', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('prct', [5 50 95], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('nsamp', 5000, @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('predcf', [], @(x) isempty(x) || ...
                   isvector(x) && isreal(x) && all(isfinite(x)&x>0))
  ip.addParamValue('tstind', [], @(x) isempty(x) || iscell(x) ||...
                   (isvector(x) && isreal(x) && all(isfinite(x)&x>0)))
  if numel(varargin)==0 || isnumeric(varargin{1})
    % inputParser should handle this, but it doesn't
    ip.parse(gp, x, y, varargin{:});
  else
    ip.parse(gp, x, y, [], varargin{:});
  end
  xt=ip.Results.xt;
  z = ip.Results.z;
  zt = ip.Results.zt;
  prct = ip.Results.prct;
  nsamp = ip.Results.nsamp;
  predcf=ip.Results.predcf;
  tstind=ip.Results.tstind;
  if isempty(xt)
    xt=x;
    if isempty(tstind)
      if iscell(gp)
        gptype=gp{1}.type;
      else
        gptype=gp.type;
      end
      switch gptype
        case {'FULL' 'VAR' 'DTC' 'SOR'}
          tstind = [];
        case {'FIC' 'CS+FIC'}
          tstind = 1:size(x,1);
        case 'PIC'
          if iscell(gp)
            tstind = gp{1}.tr_index;
          else
            tstind = gp.tr_index;
          end
      end
    end
    if isempty(zt)
      zt=z;
    end
  end

  % pass these forward
  options=struct();
  if ~isempty(z);options.z=z;end
  if ~isempty(zt);options.zt=zt;end
  if ~isempty(predcf);options.predcf=predcf;end
  if ~isempty(tstind);options.tstind=tstind;end

  [tn, nin] = size(x);

  if iscell(gp) || numel(gp.jitterSigma2)>1
    % gp_array or MCMC samples
    % the combined latent posterior is not Gaussian
    sampft = gp_rnd(gp, x, y, xt, 'nsamp', nsamp, options);
    if iscell(gp)
      % in the next step we need just one gp from the array
      gp=gp{1};
    end
    if isfield(gp, 'monotonic') && gp.monotonic
        [gp,x,y,z,xt,zt] = gp.fh.setUpDataForMonotonic(gp,x,y,z,xt,zt);
    end
    if isfield(gp.lik.fh,'trcov')
      % Gaussian likelihood
      prctmus = prctile(sampft', prct)';
    else
      prctmus = prctile(gp.lik.fh.invlink(gp.lik, sampft, zt)', prct)';
    end
  else
    % single GP
    if isfield(gp, 'monotonic') && gp.monotonic
        [gp,x,y,z,xt,zt] = gp.fh.setUpDataForMonotonic(gp,x,y,z,xt,zt);
    end
    % the latent posterior is Gaussian
    [Eft, Varft] = gp_pred(gp, x, y, xt, 'tstind', tstind, options);
    prct = prct./100;
    prct = norminv(prct, 0, 1);
    if isfield(gp.lik.fh,'trcov')
      % Gaussian likelihood
      prctmus = bsxfun(@plus, Eft, bsxfun(@times, sqrt(Varft), prct));
    else
      % Non-Gaussian likelihood
      np = length(prct);
      prctmus = zeros(size(Eft,1),np);
      for i=1:np
        prctmus(:,i) = gp.lik.fh.invlink(gp.lik, Eft+prct(i).*sqrt(Varft), zt);
      end
    end
  end

end


function out = isLaue(s)
% check whether s is a Laue group

if ~isempty(s.LaueRef)

  out = s.LaueRef == s;

else

  if s.id > 0
    out = s.id == symmetry.pointGroups(s.id).LaueId;
  else
    out = any(s.rot(:) == rotation.inversion);
  end

  % store Laue group to speed up further checks
  if out
    s.LaueRef = s;
  else
    s.LaueRef = makeLaue(s);
  end

end


%% processing for fluxes
%find the maximal set of metabolites and reactions that are stoichiometrically consistent
if ~isfield(model,'SConsistentMetBool') || ~isfield(model,'SConsistentRxnBool')
    massBalanceCheck=0;
    [~, ~, ~, ~, ~, ~, model, ~] = findStoichConsistentSubset(model, massBalanceCheck, param.printLevel-1);
end

if ~isfield(model,'osenseStr') || isempty(model.osenseStr)
    %default linear objective sense is maximisation
    model.osenseStr = 'max';
end
[~,osense] = getObjectiveSense(model);

if ~isfield(param,'maxUnidirectionalFlux')
    %try to set the maximum unidirectional flux based on the magnitude of the largest bound but dont have it greater than 1e5
    %param.maxUnidirectionalFlux=min(1e5,max(abs(model.ub)));
    param.maxUnidirectionalFlux=inf;
end
if ~isfield(param,'minUnidirectionalFlux')
    if isequal(param.solver,'mosek')
        %try to set the minimum unidirectional flux
        param.minUnidirectionalFlux=0;
    else
        param.minUnidirectionalFlux = 0;
    end
end

if ~isfield(param,'internalNetFluxBounds')
    param.internalNetFluxBounds='original';
end
if isfield(param,'internalBounds')
    error('internalBounds replaced by other parameter options')
end

if param.debug
    solution_optimizeCbModel = optimizeCbModel(model);
    switch solution_optimizeCbModel.stat
        case 0
            message = 'Input model is not feasible according to optimizeCbModel.';
            warning(message)
            solution = solution_optimizeCbModel;
            modelOut = model;
            return
        case 1
            message ='Input model is feasible according to optimizeCbModel.';
            disp(message)
            solution = solution_optimizeCbModel;
            messages = cellstr(message);
    end
end

if 0
    %ignore this for now - TODO
    nMetInRxn=sum(model.S~=0,1);
    if any(nMetInRxn==0)
        nTrivialRxn = nnz(nMetInRxn==0);
        error(['model.S is incorrectly specified as it contains ' int2str(nTrivialRxn) ' zero columns'])
    end

    nonUnitaryRxn = (nMetInRxn~=1)';
    rxnBool = nonUnitaryRxn & ~model.SConsistentRxnBool;
    if any(rxnBool)
        if param.printLevel> 0
            fprintf('%s\n',['Assigned ' int2str(nnz(rxnBool)) ' non-unitary exchange reactions as stoichiometrically consistent.'])
            if param.printLevel> 1
                printConstraints(model, -inf, inf, rxnBool, [], 0);
            end
        end
        %non-unitary exchange reactions are assumed to be stoichiometrically
        %consistent because it is assumed this model was generated using
        %thermoKernel, and all reactions should be forced in some way
        %TODO why is this reaction one:
        %'CYSTS_H2S'        'Cystathionine Beta-Synthase (sulfide-forming)'    -10000    10000    'cyk_L[c] + hcyk_L[c]  <=> HC00250[c] + cyst_L[c] '
        model.SConsistentRxnBool = model.SConsistentRxnBool | rxnBool;
    end
end

lb=model.lb;
ub=model.ub;
switch param.internalNetFluxBounds
    case {'originalNetFluxBounds','original'}
        if param.printLevel>0
            fprintf('%s\n','Using existing internal net flux bounds without modification.')
        end
    case 'directional'
        if param.printLevel>0
            fprintf('%s\n','Using directional internal net flux bounds only.')
        end
        lb(lb<0 & model.SConsistentRxnBool,1)=-param.maxUnidirectionalFlux;
        lb(lb>0 & model.SConsistentRxnBool,1)=0;

        ub(ub>0 & model.SConsistentRxnBool,1)=param.maxUnidirectionalFlux;
        ub(ub<0 & model.SConsistentRxnBool,1)=0;
    case {'maxInternalNetFluxBounds','max'}
        lb(model.SConsistentRxnBool)=-ones(n,1)*param.maxUnidirectionalFlux;
        ub(model.SConsistentRxnBool)= ones(n,1)*param.maxUnidirectionalFlux;

    case 'none'
        if param.printLevel>0
            fprintf('%s\n','Using no internal net flux bounds.')
        end
        lb(model.SConsistentRxnBool,1)=-ones(n,1)*inf;
        ub(model.SConsistentRxnBool,1)= ones(n,1)*inf;

    case 'random'
        lb(model.SConsistentRxnBool,1)=-rand(n,1)*param.maxUnidirectionalFlux;
        ub(model.SConsistentRxnBool,1)= rand(n,1)*param.maxUnidirectionalFlux;

    case 'rangeNt'
        u1=rand(m,1);
        u2=rand(m,1);
        u=u2-u1;
        %reorient S
        N=N*diag(sign(N'*u));
        lb(model.SConsistentRxnBool,1)=N'*u2;
        ub(model.SConsistentRxnBool,1)=N'*u1;

    case 'expRangeNt'
        u1=rand(m,1);
        u2=rand(m,1);
        u=u2-u1;
        %reorient S
        N=N*diag(sign(N'*u));
        lb(model.SConsistentRxnBool,1)=-exp(N'*u2);
        ub(model.SConsistentRxnBool,1)= exp(N'*u1);

    otherwise
        error(['param.internalNetFluxBounds = ' param.internalNetFluxBounds ' is an unrecognised input'])
end
vl = lb(model.SConsistentRxnBool);
vu = ub(model.SConsistentRxnBool);
%exchange reaction bounds (may be overwritten if conc method is chosen)
vel = lb(~model.SConsistentRxnBool);
veu = ub(~model.SConsistentRxnBool);

relaxedUnidirectionalUpperBounds = 1;
%lower bound on forward fluxes
if isfield(model,'vfl')
    vfl = model.vfl;
else
    vfl = max(param.minUnidirectionalFlux,vl);
end
%upper bounds on forward fluxes
if isfield(model,'vfu')
    vfu = model.vfu;
else
    if relaxedUnidirectionalUpperBounds
        vfu = ones(n,1)*param.maxUnidirectionalFlux;
    else
        vfu = vu;
        vfu(vfu<=0) = param.maxUnidirectionalFlux;
    end
end
%lower bounds on reverse fluxes
if isfield(model,'vrl')
    vrl = model.vrl;
else
    vrl = max(param.minUnidirectionalFlux,-vu);
end
%upper bounds on reverse fluxes
if isfield(model,'vru')
    vru = model.vru;
else
    if relaxedUnidirectionalUpperBounds
        vru = ones(n,1)*param.maxUnidirectionalFlux;
    else
        vru = -vl;
        vru(vru<=0) = param.maxUnidirectionalFlux;
    end
end
if any(vfl<0)
    error('lower bound on forward flux cannot be less than zero')
end
if any(vrl<0)
    error('lower bound on reverse flux cannot be less than zero')
end
if any(vfl>vfu)
    error('lower bound on forward flux greater than upper bound')
end
if any(vrl>vru)
    error('lower bound on reverse flux greater than upper bound')
end

if any(vl>0 | vu<0) && ~strcmp(param.internalNetFluxBounds,'original')
    if param.printLevel> 2
        histogram([vl;vu])
        title('internal reaction bounds not directional')
    end
    warning('internal reaction bounds not directional')
end

if ~isfield(model,'c') || isempty(model.c)
    ci = zeros(n,1);
    ce = zeros(k,1);
else
    %osense is only used to changes the sense of the model.c part
    ci = osense*model.c(model.SConsistentRxnBool);
    ce = osense*model.c(~model.SConsistentRxnBool);
end

if ~isfield(model,'cf') || isempty(model.cf)
    model.cf='zero';
end
if ~isfield(model,'cr') || isempty(model.cr)
    model.cr='zero';
end
if ischar(model.cf) || ischar(model.cr)
    switch model.cf
        case 'rand'
            cf=N'*rand(m,1);
            cr=-cf;
        case 'one'
            cf=ones(n,1);
            cr=ones(n,1);
        case 'zero'
            cf=zeros(n,1);
            cr=zeros(n,1);
    end
else
    if length(model.cf)~=length(model.cr)
        error('model.cf and model.cr must have the same dimensions')
    end
    if length(model.cf)==size(model.S,2)
        cf = columnVector(model.cf(model.SConsistentRxnBool));
        cr = columnVector(model.cr(model.SConsistentRxnBool));
    else
        if length(model.cf)==1
            model.cf=ones(n,1)*model.cf;
            model.cr=ones(n,1)*model.cr;
        end
        if length(model.cf)~=nnz(model.SConsistentRxnBool)
            error('cf and cr must have the same dimension as nnz(model.SConsistentRxnBool) x 1')
        else
            cf = columnVector(model.cf);
            cr = columnVector(model.cr);
        end
    end
    if any(~isfinite([cf;cr]))
        error('cf and cr must all be finite')
    end
end

if ~isfield(model,'g') || isempty(model.g)
    if isequal(param.method,'fluxes')
        model.g='one';
    else
        model.g='two';
    end
end
if ischar(model.g)
    switch model.g
        case 'zero'
            g=zeros(n,1);
        case 'rand'
            g=rand(n,1);
        case 'one'
            g=ones(n,1);
        case 'two'
            g=ones(n,1)*2;
        otherwise
            error('unrecognised option for model.g')
    end
else
    if length(model.g)==size(model.S,2)
        g = columnVector(model.g(model.SConsistentRxnBool));
    else
        if length(model.g)==1
            model.g=ones(n,1)*model.g;
        end
        if length(model.g)~=nnz(model.SConsistentRxnBool)
            error('g and cf must have the same dimension as nnz(model.SConsistentRxnBool) x 1')
        else
            g = columnVector(model.g);
        end
    end
    if any(~isfinite(g))
        error('g must all be finite')
    end
    if length(g)~=length(cf)
        error('g and cf must have the same dimensions')
    end
end


function result = merge_results(num_subjects, solver_name)
    filepath = sprintf('bin/yale_faces_%d_subjects_test_%s.mat', num_subjects, solver_name);
    load(filepath);
    num_trials = numel(trials);
    fprintf('Number of subjects: %d, Number of trials: %d, solver: %s\n', num_subjects,  num_trials, solver_name);
    result = struct;
    trial = trials{1};
    result.M = trial.M;
    result.K = trial.K;
    result.D = trial.D;
    result.elapsed_time_vec = zeros(num_trials, 1);
    result.connectivity_vec = zeros(num_trials, 1);
    result.clustering_error_perc_vec = zeros(num_trials, 1);
    result.clustering_acc_perc_vec = zeros(num_trials, 1);
    result.spr_error_vec = zeros(num_trials, 1);
    result.spr_flag_vec = zeros(num_trials, 1);
    result.spr_perc_vec = zeros(num_trials, 1);
    num_points_vec = zeros(num_trials, 1);
    result.trials = trials;
    for i=1:numel(trials)
        trial = trials{i};
        result.elapsed_time_vec(i) = trial.elapsed_time;
        result.connectivity_vec(i) = trial.connectivity;
        result.clustering_error_perc_vec(i) = trial.clustering_error_perc;
        result.clustering_acc_perc_vec(i) = trial.clustering_acc_perc;
        result.spr_error_vec(i) = trial.spr_error;
        result.spr_flag_vec(i) = trial.spr_flag;
        result.spr_perc_vec(i) = trial.spr_perc;
        num_points_vec(i) = trial.S;
    end
    result.num_points = mean(num_points_vec);
    result.elapsed_time = mean(result.elapsed_time_vec);
    result.connectivity = min(result.connectivity_vec);
    result.clustering_error_perc = mean(result.clustering_error_perc_vec);
    result.clustering_acc_perc = mean(result.clustering_acc_perc_vec);
    result.spr_error = mean(result.spr_error_vec);
    result.spr_flag = mean(result.spr_flag_vec);
    result.spr_perc = mean(result.spr_perc_vec);

    fprintf('\n\n');
    fprintf('Mean Points: ');
    fprintf('%.2f ', result.num_points);
    fprintf('\n\n');
    fprintf('Time: ');
    fprintf('%.2f ', result.elapsed_time);
    fprintf('\n\n');
    fprintf('Connectivity: ');
    fprintf('%.2f ', result.connectivity);
    fprintf('\n\n');
    fprintf('Clustering error(%%): ');
    fprintf('%.2f ', result.clustering_error_perc);
    fprintf('\n\n');
    fprintf('Clustering accuracy (%%): ');
    fprintf('%.2f ', result.clustering_acc_perc);
    fprintf('\n\n');
    fprintf('Subspace preserving representation error: ');
    fprintf('%.4f ', result.spr_error);
    fprintf('\n\n');
    fprintf('Subspace preserving representation flag: ');
    fprintf('%.2f ', result.spr_flag);
    fprintf('\n\n');
    fprintf('Subspace preserving representations(%%): ');
    fprintf('%.2f ', result.spr_perc);
    fprintf('\n\n');
end


function G = slnngraph(X, X2, nnparams, varargin)
%SLNNGRAPH Constructs a nearest neighborhood based graph
%
% $ Syntax $
%   - G = slnngraph(X, [], nnparams, ...)
%   - G = slnngraph(X, X2, nnparams, ...)
%
% $ Arguments $
%   - X:        The sample matrix of the (referenced) nodes
%   - X2:       The sample matrix of the (query) nodes
%   - nnparams: The cell array of parameters to slfindnn for determining
%               neighborhoods.
%   - G:        The adjacency matrix of the constructed graph
%
% $ Description $
%   - G = slnngraph(X, [], nnparams, ...) constructs a graph with adjacency
%     matrix representation using specified nearest neighbor strategy.
%     You can further specify the following properties to control the
%     process of adjacency matrix construction.
%       \*
%       \t   The Properties of Graph Matrix construction           \\
%       \h     name         &      description                      \\
%             'valtype'     & The type of the matrix values          \\
%                             - 'logical':  using logical value
%                             - 'numeric':  using numeric value (default)
%             'sparse'      & Whether to construct sparse matrix
%                             (default = true)                      \\
%             'tfunctor'    & The function to transform the distance
%                             values to edge values. (default = [])  \\
%             'sym'         & whether to symmetrizes the graph
%                             (default = false)                       \\
%             'symmethod'   & The method used to symmetrization
%                             (default = [])                          \\
%       \*
%
%   - G = slnngraph(X, X2, nnparams, ...) constructs a bigraph with the
%     source and target set respectively specified.
%
%   - There are three configurations on X and X2:
%       - construct graph with the same set X with all edges connecting
%         the same node excluded. You can use the following syntax:
%           slnngraph(X, [], ...)
%       - construct graph with the same set X with the edges connecting
%         the same node preserved. You can use the following syntax:
%           slnngraph(X, X, ...)
%       - construct graph with the different source and target sets X and
%         X2, You can use the following syntax:
%           slnngraph(X, X2, ...)
%
% $ Remarks $
%   - tfunctor only takes effect for numeric value type.
%
%   - The option sym and symmethod only take effect when X and X2 has the
%     same number of elements.
%
% $ History $
%   - Created by Dahua Lin, on Sep 8th, 2006
%   - Modified by Dahua Lin, on Sep 11st, 2006
%       - revise to conform to the defined neighborhood system
%         representation
%       - fix small bugs
%

%% parse input

if nargin < 3
    raise_lackinput('slnngraph', 3);
end

if ~isnumeric(X) || ndims(X) ~= 2
    error('sltoolbox:invalidarg', ...
        'X should be a 2D numeric matrix');
end

if isempty(X2)
    X2 = [];
else
    if ~isnumeric(X2) || ndims(X2) ~= 2
        error('sltoolbox:invalidarg', ...
            'A non-empty X2 should be a 2D numeric matrix');
    end
end

if ~iscell(nnparams)
    error('stoolbox:invalidarg', ...
        'The parameters for slfindnn should be groupped in a cell array');
end

opts.valtype = 'numeric';
opts.sparse = true;
opts.tfunctor = [];
opts.sym = false;
opts.symmethod = [];
opts = slparseprops(opts, varargin{:});

switch opts.valtype
    case 'logical'
        islogic = true;
    case 'numeric';
        islogic = false;
    otherwise
        error('sltoolbox:invalidarg', ...
            'Invalid value type for graph: %s', opts.valtype);
end

%% Main

n0 = size(X, 2);
if isempty(X2)
    nq = n0;
else
    nq = size(X2, 2);
end
can_sym = (n0 == nq);

% find nearest neighbor
if ~islogic
    [nnidx, dists] = slfindnn(X, X2, nnparams{:});
else
    nnidx = slfindnn(X, X2, nnparams{:});
end

% group edges and vectorizes distances
edges = sladjlist2edgeset(nnidx, 0);
edges = edges(:, [2,1]);  % flip in order to place source to left, target to right
clear nnidx;
if ~islogic
    dists = vertcat(dists{:});
else
    dists = [];
end

% compute edge values
if ~isempty(dists)
    if isempty(opts.tfunctor)
        vals = dists;
    else
        vals = slevalfunctor(opts.tfunctor, dists);
    end
else
    vals = [];
end
clear dists;

% make graph matrix
G = slmakeadjmat(n0, nq, edges, vals, islogic, opts.sparse);

% symmetrize the graph
if opts.sym && can_sym
    G = slsymgraph(G, opts.symmethod);
end


function [sP, isNew] = newSphericalPlot(v,varargin)
% split plot in upper and lower hemisphere
%
% 1: axis given -> no sphericalRegion stored -> compute sphericalRegion -> finish
% 2: axis is hold and has sphericalRegion -> use multiplot
% 3: new multiplot

% case 1: predefined axis
% -----------------------
if check_option(varargin,'parent')

  ax = get_option(varargin,'parent');

  % axis is already a spherical plot
  if isappdata(ax(end),'sphericalPlot') && ishold(ax(end))

    for i = 1:length(ax)
      sP(i) = getappdata(ax(i),'sphericalPlot');
    end
    isNew = false;

  else % set up a new spherical axes if required

    % extract spherical region
    % TODO: it might happen that the spherical region needs two axes
    sR = getPlotRegion(v,varargin{:});

    % extract projection
    proj = getProjection(sR,varargin{:});

    % create a new spherical plot
    sP = sphericalPlot(ax,proj(1),varargin{:});
    isNew = true;

  end
  return;
end

% create a new mtexFigure or get a reference to it
[mtexFig,isNew] = newMtexFigure(varargin{:});

%~check_option(varargin,'add2all') ||

if isNew || ~isappdata(mtexFig.currentAxes,'sphericalPlot')

  % get spherical region
  sR = getPlotRegion(v,varargin{:});

  % extract projection(s)
  % this might return two projections for upper and lower hemisphere
  proj = getProjection(sR,varargin{:});
  holdState = getHoldState(mtexFig.gca);

  for i = 1:numel(proj)

    % create a new axis
    if i>1, mtexFig.nextAxis; end
    hold(mtexFig.gca,holdState);

    % display upper/lower if needed
    if numel(proj)>1
      if ~proj(i).sR.isUpper
        tr = {'TR','lower'};
      elseif ~proj(i).sR.isLower
        tr = {'TR','upper'};
      else
        tr = {};
      end
    else
      tr = {};
    end

    % create a new spherical plot
    sP(i) = sphericalPlot(mtexFig.gca,proj(i),tr{:},varargin{:});         %#ok<AGROW>

  end
  mtexFig.drawNow(varargin{:});
  isNew = true;

elseif check_option(varargin,'add2all') % add to or overide existing axes

  for i = 1:numel(mtexFig.children)

    sP(i) = getappdata(mtexFig.children(i),'sphericalPlot'); %#ok<AGROW>

  end

else

  sP = getappdata(mtexFig.currentAxes,'sphericalPlot');

end

end

% ---------------------------------------------------------
function sR = getPlotRegion(varargin)
% returns spherical region to be plotted

% default values from the vectors to plot
if isa(varargin{1},'vector3d')
  sR = varargin{1}.region(varargin{2:end});
else
  sR = sphericalRegion;
end
sR = getClass(varargin,'sphericalRegion',sR);

% check for simple options
if check_option(varargin,'complete')
  sR = sphericalRegion;
end
if check_option(varargin,'upper')
  sR = sR.restrict2Upper;
elseif check_option(varargin,'lower')
  sR = sR.restrict2Lower;
end

% extract antipodal
sR.antipodal = check_option(varargin,'antipodal') || ...
  (isa(varargin{1},'vector3d') && varargin{1}.antipodal);

% for antipodal symmetry reduce to halfsphere
if sR.antipodal && sR.isUpper && sR.isLower &&...
    ~check_option(varargin,'complete')
  sR = sR.restrict2Upper;
end

end
% ---------------------------------------------------------
function proj = getProjection(sR,varargin)

proj = get_option(varargin,'projection','earea');

if ~isa(proj,'sphericalProjection')

  switch proj
    case 'plain', proj = plainProjection(sR);

    case {'stereo','eangle'}, proj = eangleProjection(sR); % equal angle

    case 'edist', proj = edistProjection(sR); % equal distance

    case {'earea','schmidt'}, proj = eareaProjection(sR); % equal area

    case 'orthographic',  proj = orthographicProjection(sR);

    case 'square',  proj = squareProjection(sR);

    case 'gnonomic', proj = gnonomicProjection(sR);

    otherwise

      error('%s\n%s','Unknown projection specified! Valid projections are:',...
        'plain, stereo, eangle, edist, earea, schmidt, orthographic','square')
  end
end

if ~isa(proj,'plainProjection') && sR.isUpper && sR.isLower
  proj = [proj,proj];
  proj(1).sR = proj(1).sR.restrict2Upper;
  proj(2).sR = proj(2).sR.restrict2Lower;
end

end


function res = imCropBorder(img, pad, varargin)
%IMCROPBORDER Crop the border around a 2D or 3D image
%
%   IMG = imCropBorder(IMG, BORDER)
%   Crop the image by removing BORDER pixels around the image. The size of
%   new image is given by size(IMG)-2*BORDER.
%   The function works for 2D or 3D images, grayscale or color.
%
%   IMG = imCropBorder(IMG, [BORDER1 BORDER2])
%   IMG = imCropBorder(IMG, [BORDER1 BORDER2 BORDER3])
%   Specifies different sizes for borders in first and second directions
%   (and third direction for 3D images)
%
%   IMG = imCropBorder(IMG, [BORDER10 BORDER11 BORDER20 BORDER21])
%   IMG = imCropBorder(IMG, [BORDER10 BORDER11 BORDER20 BORDER21 BORDER30 BORDER31])
%   Specifies different sizes for borders in first and second directions,
%   and in the beginning and at the end in each direction.
%
%
%   Example
%   % Add 20 pixel in each side of rice image
%     img = imread('rice.png');
%     img2 = imAddBorder(img, 20);
%     imshow(img2);
%
%   % add white borders with different sizes
%     img = imread('rice.png');
%     img2 = imAddBorder(img, [10 20 30 40], 255);
%     imshow(img2);
%
%   % add cyan border around a color image
%     img = imread('peppers.png');
%     img2 = imAddBorder(img, [30 20], [0 1 1]);
%     imshow(img2);
%
%   See also
%     imCrop, imAddBorder
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2018-11-05,    using Matlab 7.4.0.287 (R2007a)
% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

%   History

%% Initialisations

% size of input image
dim = size(img);
isColor = size(img, 3) == 3;
isPlanar = length(dim) == 2 || (length(dim) == 3 && isColor);

if isPlanar
    %% Planar case

    % convert pad to have 4 values
    if length(pad) == 1
        pad = [pad pad pad pad];
    elseif length(pad)==2
        pad = [pad(1) pad(1) pad(2) pad(2)];
    end

    dim1 = (pad(1)+1):(size(img,1)-pad(2));
    dim2 = (pad(3)+1):(size(img,2)-pad(4));

    % fillup result image with initial image
    res = img(dim1, dim2, :);

elseif length(dim) - isColor == 3
    %% Case of 3D image

    % convert pad to have 6 values
    if length(pad) == 1
        pad = [pad pad pad pad pad pad];
    elseif length(pad) == 3
        pad = [pad(1) pad(1) pad(2) pad(2) pad(3) pad(3)];
    end

    % compute index of voxels to keep along each dimension
    dim1 = (pad(1)+1):(size(img,1)-pad(2));
    dim2 = (pad(3)+1):(size(img,2)-pad(4));
    dim3 = (pad(5)+1):(size(img,3+isColor)-pad(6));

    % select voxels to keep
    if isColor
        res = img(dim1, dim2, :, dim3);
    else
        res = img(dim1, dim2, dim3);
    end

else
    error('Image dimension not managed');
end


function y = vertcat(varargin)
%VERTCAT (overloaded)

prenargin = nargin;
% Fast exit
if prenargin<2
    y=varargin{1};
    return
end

% Get dimensions
n = zeros(prenargin,1);
m = zeros(prenargin,1);
for i = 1:prenargin
    if isa(varargin{i},'blkvar')
        varargin{i} = sdpvar(varargin{i});
    end
    [n(i),m(i)]=size(varargin{i});
end

% Keep only non-empty
keep_these = find((n.*m)~=0);
if length(keep_these)<length(n)
    varargin = {varargin{keep_these}};
    n = n(keep_these);
    m = m(keep_these);
end;

% All widths should be equal
if any(m~=m(1))
    error('All rows in the bracketed expression must have the same number of columns.');
end

nblocks = size(varargin,2);
isasdpvar = zeros(nblocks,1);
% Get dimensions
for i = 1:nblocks
    isasdpvar(i) = isa(varargin{i},'sdpvar');
end

% Find all free variables used
all_lmi_variables = [];
for i = 1:nblocks
    if isasdpvar(i)
        all_lmi_variables = [all_lmi_variables varargin{i}.lmi_variables];
    end
end
all_lmi_variables = uniquestripped(all_lmi_variables);

% Pick one of the sdpvar objects to build on...
y = varargin{min(find(isasdpvar))};

% Some indexation tricks...
basis_i = [];
basis_j = [];
basis_s = [];
m = m(1);
sumn=sum(n);
shift=[0;cumsum(n)];
for j = 1:nblocks
    if isasdpvar(j)
        if length(all_lmi_variables)==length(varargin{j}.lmi_variables) && all_lmi_variables(1)==varargin{j}.lmi_variables(1) &&  all_lmi_variables(end)==varargin{j}.lmi_variables(end)
            % Avoid call to ismember and find
            in_this = 1:length(all_lmi_variables);
        else
            members = ismembcYALMIP(all_lmi_variables,varargin{j}.lmi_variables);
            in_this = find(members);
        end
        dummy = [1 1+in_this];
        [i2,j2,s2] = find(varargin{j}.basis);
        j2 = dummy(j2);
    else
        [i2,j2,s2] = find(varargin{j}(:));
    end
    if ~isempty(j2)
    col = 1+floor((i2-1)/n(j));
    row = i2-(col-1)*n(j);
    basis_i = [basis_i;row(:)+shift(j)+sumn*(col(:)-1)];
    basis_j = [basis_j;j2(:)];
    basis_s = [basis_s;s2(:)];
    end
end
basis = sparse(basis_i,basis_j,basis_s,sum(n)*m,1+length(all_lmi_variables));

y.dim(1) = sum(n);
y.dim(2) = m;
y.basis = basis;
y.lmi_variables = all_lmi_variables;
% Reset info about conic terms
y.conicinfo = [0 0];
y.extra.opname='';
y.extra.createTime = definecreationtime;


function [varargout]=subTriLoop(varargin)

% function [Fs,Vs,Cs,CV]=subTriLoop(F,V,n,fixBoundaryOpt)
% ------------------------------------------------------------------------
%
%
%
%
% ------------------------------------------------------------------------

%%

switch nargin
    case 2
        F=varargin{1};
        V=varargin{2};
        n=1;
        fixBoundaryOpt=0;
        logicFixedFaces=[];
    case 3
        F=varargin{1};
        V=varargin{2};
        n=varargin{3};
        fixBoundaryOpt=0;
        logicFixedFaces=[];
    case 4
        F=varargin{1};
        V=varargin{2};
        n=varargin{3};
        fixBoundaryOpt=varargin{4};
        logicFixedFaces=[];
    case 5
        F=varargin{1};
        V=varargin{2};
        n=varargin{3};
        fixBoundaryOpt=varargin{4};
        logicFixedFaces=varargin{5};
    case 6
        F=varargin{1};
        V=varargin{2};
        n=varargin{3};
        fixBoundaryOpt=varargin{4};
        logicFixedFaces=varargin{5};
end

%%

if nargout>2 || ~isempty(logicFixedFaces)
    C=(1:1:size(F,1))'; %Face colors or indices
    if nargout==4
        CV=zeros(size(V,1),1); %Vertex labels/colors
    end
end

if n>0
    for qIter=1:1:n

        M=patchConnectivity(F,V,{'ev','ef','vv'});

        edgeVertexMat=M.edge.vertex;
        edgeFaceMat=M.edge.face;
        vertexVertexMat=M.vertex.vertex;

        if ~isempty(logicFixedFaces)
            indFixFaces=find(logicFixedFaces);
            indFixEdges=find(any(ismember(edgeFaceMat,indFixFaces),2));
        else
            indFixEdges=[];
        end

        numPoints = size(V,1);
        numEdges = size(edgeVertexMat,1);

        % Get indices of the three edges associated with each face
        A = sparse(edgeVertexMat(:,1),edgeVertexMat(:,2),(1:numEdges)+numPoints,numPoints,numPoints,numEdges);
        A = max(A,A'); %Copy symmetric

        %Indices for A matrix
        indA_12=F(:,1)+(F(:,2)-1)*numPoints;
        indA_23=F(:,2)+(F(:,3)-1)*numPoints;
        indA_31=F(:,3)+(F(:,1)-1)*numPoints;

        %Get indices for vertex array
        indV_12=full(A(indA_12));
        indV_23=full(A(indA_23));
        indV_31=full(A(indA_31));

        %Create faces array
        Fs=[[F(:,1)  indV_12 indV_31];...
            [F(:,2)  indV_23 indV_12];...
            [F(:,3)  indV_31 indV_23];...
            [indV_12 indV_23 indV_31]];

        indF1=F(edgeFaceMat(:,1),:);
        if size(edgeFaceMat,2)==2
            logicValid=edgeFaceMat(:,2)>0;
            indF2=zeros(numEdges,3);
            indF2(logicValid,:)=F(edgeFaceMat(logicValid,2),:);
        else
            indF2=zeros(numEdges,3);
        end

        indF=[indF1 indF2];

        logicPick1=indF~=edgeVertexMat(:,ones(1,6)) & indF~=edgeVertexMat(:,2*ones(1,6));
        indF(~logicPick1)=0;
        indF=sort(indF,2,'descend');
        indF=indF(:,1:2);

        logicBoundaryEdges=indF(:,2)==0;

        N=sum(vertexVertexMat>0,2);
        beta = (1./N) .* (5/8 - ( 3/8 + (1/4)*(cos(2*pi./N) )).^2);

        %Computed sum of neighbours
        V_sum=zeros(size(V));
        logicValid=vertexVertexMat>0;
        X=nan(size(vertexVertexMat));
        for q=1:1:size(V,2)
            X(logicValid)=V(vertexVertexMat(logicValid),q);
            V_sum(:,q)=sum(X,2,'omitnan');
        end

        %Compute replacement input vertices
        Vv=(1-N.*beta).*V+beta.*V_sum;

        if any(logicBoundaryEdges)
            eb=edgeVertexMat(logicBoundaryEdges,:);
            indBoundaryVertices=unique(eb(:));

            EV=[eb;fliplr(eb)]; %Non-unique edges
            vertexVertexConnectivityBoundary=sparse(EV(:,1),EV(:,2),EV(:,2),size(V,1),size(V,1));
            vertexVertexConnectivityBoundary=sort(vertexVertexConnectivityBoundary,2,'descend');
            [~,J,~] = find(vertexVertexConnectivityBoundary);
            vertexVertexConnectivityBoundary=full(vertexVertexConnectivityBoundary(:,1:max(J)));

            Vv(indBoundaryVertices,:)= 6/8*V(indBoundaryVertices,:) + 1/8*(V(vertexVertexConnectivityBoundary(indBoundaryVertices,1),:)+V(vertexVertexConnectivityBoundary(indBoundaryVertices,2),:));
        end

        %Compute the new edge vertices
        Vn=zeros(numEdges,size(V,2));
        if fixBoundaryOpt==1
            %Use normal mid-edge nodes for boundary edges
            Vn(logicBoundaryEdges,:)=1/2*(V(edgeVertexMat(logicBoundaryEdges,1),:) + V(edgeVertexMat(logicBoundaryEdges,2),:));
            %Replace boundary nodes with original
            indBoundaryVertices=unique(edgeVertexMat(logicBoundaryEdges,:));
            Vv(indBoundaryVertices,:)=V(indBoundaryVertices,:);
        else
            Vn(logicBoundaryEdges,:)=1/2*V(edgeVertexMat(logicBoundaryEdges,1),:) + 1/2*V(edgeVertexMat(logicBoundaryEdges,2),:);
        end
        Vn(~logicBoundaryEdges,:)=3/8*V(edgeVertexMat(~logicBoundaryEdges,1),:) + 3/8*V(edgeVertexMat(~logicBoundaryEdges,2),:) + 1/8*(V(indF(~logicBoundaryEdges,1),:)+V(indF(~logicBoundaryEdges,2),:));

        if ~isempty(indFixEdges)
            %Use normal mid-edge nodes for constrained edges
            Vn(indFixEdges,:)=1/2*(V(edgeVertexMat(indFixEdges,1),:) + V(edgeVertexMat(indFixEdges,2),:));

            %Replace constrained nodes with original
            indConstrainedVertices=unique(edgeVertexMat(indFixEdges,:));
            Vv(indConstrainedVertices,:)=V(indConstrainedVertices,:);
        end

        %Join point sets
        Vs = [Vv; Vn];

        %Color handling
        if nargout>2 || ~isempty(logicFixedFaces)
            %Override face color data
            C=repmat(C,[size(Fs,1)/size(F,1),1]);

            %Update vertex labels
            if nargout==4
                if qIter==1
                    CV=[zeros(size(Vv,1),1); 1*ones(size(Vn,1),1); ];
                else
                    CV=[CV; qIter.*ones(size(Vn,1),1); ];
                end
            end
        end

        %Override face/vertices
        F=Fs;
        V=Vs;
        if ~isempty(logicFixedFaces)
            logicFixedFaces=logicFixedFaces(C);
        end
    end
end

%% Collect output

varargout{1}=F;
varargout{2}=V;
if nargout>2
    varargout{3}=C;
    if nargout==4
        varargout{4}=CV;
    end
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.


function [pvec, pstruct] = tapas_softmax_mu3_wld_transp(r, ptrans)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2018 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

pvec    = NaN(1,length(ptrans));
pstruct = struct;

pvec(1)       = ptrans(1);         % la_wd
pstruct.la_wd = pvec(1);
pvec(2)       = ptrans(2);         % la_ld
pstruct.la_ld = pvec(2);

return;


function field = field_composition(field, N)
% function field = field_composition(field, N)
%
% N compositions of the provided field.
%
% INPUT ARGUMENTS
% field 		- Field to compose
% N 			- Number of compositions to perform
%
% OPTIONAL INPUT ARGUMENTS
% N/A
%
% OUTPUT ARGUMENTS
% field 		- Composed field

% Copyright (c) 2012 Daniel Forsberg
% danne.forsberg@outlook.com
%
% 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/>.

for l = 1 : N
    for k = 1 : length(field)
        newField{k} = deformation(field{k},field,'linear');
        newField{k} = newField{k}+field{k};
    end
    field = newField;
end


function varargout = hat(varargin)
% VL_HAT  Hat operator
%   H = VL_HAT(OM) returns the skew symmetric matrix by taking the "hat"
%   of the 3D vector OM.
%
%   See also: VL_IHAT(), VL_HELP().
[varargout{1:nargout}] = vl_hat(varargin{:});


load -ascii house.dat
houseD=house';
clear house

[N, m] = size(houseD)

class = 1

%rand('state',0); randn('state',0);
%houseD = houseD(:,randperm(m));

Napp = ceil(m*2/3);
Ntest = m-Napp

app  = houseD(:,1:Napp);size(app)
test = houseD(:,Napp+1:end);size(test)

unique(app(class,:))
unique(test(class,:))

ns = max(houseD')
clear houseD


%  INTERNAL FUNCTION: recast the exit flag from fsolve and lsqnonlin
%
%  ::
%
%    flag=exitflag(flag,x1,fx1,TolFun)
%
%  Args:
%
%     - **flag** [scalar]: exit flag from fsolve or lsqnonlin
%     - **x1** [scalar|vector|matrix]: optimum of the function to set to zero
%       (f(x)=0)
%     - **fx1** [scalar]: value or norm of f(x1)
%     - **TolFun** [{sqrt(eps)}|scalar]: scalar or vector of model objects
%
%  Returns:
%     :
%
%     - **flag** [scalar]: recomputed exitflag
%
%


%% dtiInit validation function
%
% Download example data from the RDT
% Set default parameters for rapid processing, excluding eddy current
%
% Run dtiInit and we will set some numbers to check for asserts.
%
% RL/BW Vistasoft Team, 2016

%% Get the example diffusion weighted imaging data from the RDT

rd = RdtClient('vistasoft');
rd.crp('/vistadata/diffusion/dtiInit/raw');
rd.listArtifacts('print',true)

rd.readArtifact('dti','type','bvec','destinationFolder',pwd);
rd.readArtifact('dti','type','bval','destinationFolder',pwd);
rd.readArtifact('dti.nii','type','gz','destinationFolder',pwd);

rd.crp('/vistadata/diffusion/dtiInit');
rd.listArtifacts('print',true)
rd.readArtifact('t1.nii','type','gz','destinationFolder',pwd);

%%

dwRawFileName = fullfile(pwd,'dti.nii.gz');
t1FileName = fullfile(pwd,'t1.nii.gz');
niT1 = niftiRead(t1FileName);

params = dtiInitParams;
params.eddyCorrect    = -1;  % This takes a long time so we turn eddy current and motion correction
params.phaseEncodeDir = 2;   % We have super powers and know this
params.clobber = true;       % Silently over-write files.
params.dwOutMm = niT1.pixdim;% Match the T1 so we can crop together

params.outDir = fullfile(vistaRootPath,'local');         % Stash output
[dt6FileName, outBaseDir] = dtiInit(dwRawFileName,t1FileName,params);

%%
slices = 50:59;
niftiView('dti_aligned_trilin_noMEC.nii.gz','slices',slices);
niftiView('t1.nii.gz','slices',slices);

% Original, before upsampling
niftiView('dti_b0.nii.gz','slices',slices);

% Can we show the white matter mask on top of the dti_aligned?


function ell_demo ( )

%*****************************************************************************80
%
%% ELL_DEMO demonstrates MESH2D on the L-shaped region.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ELL_DEMO:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Use MESH2D to mesh the L-shaped region.\n' );
  fprintf ( 1, '  Use MESH2D_TO_MEDIT to write the mesh to a file.\n' );

  warning off

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Set maximum element size HDATA.HMAX = 0.1\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  hdata = [];
  hdata.hmax = 0.1;

  [ p, t ] = mesh2d ( v, [], hdata );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause
%
%  Now call MESH2D_TO_MEDIT to write the data to a file.
%
  filename = 'ell.mesh';
  mesh2d_to_medit ( p, t, filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Wrote mesh data to MEDIT mesh file "%s"\n', filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ELL_DEMO:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function h = hfun1 ( x, y )

%*****************************************************************************80
%
%% HFUN1 is a size-function for the L-shaped region.
%
%  Discussion:
%
%    The smallest size is at (1.0,1.0), and sizes increase as their distance
%    from that point increases.
%
  h = 0.01 + 0.1 * sqrt ( ( x - 1.0 ).^2  + ( y - 1.0 ).^2 );

  return
end


classdef TestApproxPolyDP
    %TestApproxPolyDP

    methods (Static)
        function test_1
            curve = {[0 0], [1 1], [2 2], [3 3], [4 4]};
            approxCurve = cv.approxPolyDP(curve);
            assert(iscell(approxCurve));
            if ~isempty(approxCurve)
                assert(all(cellfun(@numel, approxCurve) == 2));
            end
        end

        function test_2
            curve = single([0 0; 10 0; 10 10; 5 4]);
            approxCurve = cv.approxPolyDP(curve, 'Epsilon',5, 'Closed',true);
            assert(isnumeric(approxCurve) && isfloat(approxCurve));
            if ~isempty(approxCurve)
                assert(ismatrix(approxCurve) && size(approxCurve,2)==2);
            end
        end

        function test_3
            curve = int32([0 0; 1 1; 2 2; 3 3; 4 4]);
            approxCurve = cv.approxPolyDP(curve);
            assert(isnumeric(approxCurve) && isinteger(approxCurve));
        end

        function test_error_argnum
            try
                cv.approxPolyDP();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


function [pk, final_manip] = SCO_null_space(robot, pathT)
%global robot
global parameters
global hfigures
hfigures.hpaths = figure;
hfigures.hcosts = figure;
hfigures.hee = figure;
hfigures.htheta = figure;
hfigures.hbest_costs = figure;
hfigures.hdtheta = figure;

%N waypoints at each trajectory
%N = parameters.N;
%K particles
K = parameters.K;

%generate K different paths
%starting from arbitrary positions.
for k=1:K
  q0 = uniform(-pi, pi, 1, robot.DOF)';
  [pathq, pathT] = plan_path_moore_penrose(robot, q0);
  %generate particle
  G{k}.pathq = pathq;
  G{k}.pathT = pathT;
  animate_local(robot, pathq)
  figure, plot(pathq'), legend('q1', 'q2', 'q3', 'q4')
end
%pick arbitrarily one...
initial_manip = compute_manip(robot, pathq);
G_inicial = G;

%animate one of the particles (random)
%this may not be the best particle, of course
animate_local(robot, pathq)

i=0;
best_probs = [];
%Main loop
%till convergence of trajectory cost function
while i < 20
    i
    i=i+1;
    G = add_noise_to_particle_set(G, pathT);

    %best particle
    [pk, best_prob] = best_particle(G);
    best_probs = [best_probs, best_prob];
    plot_info(G, pk, best_probs)
end

%plot particle info
best_particle_info(pk)
figure,
plot(best_probs)
title('best PROB at each iteration (sum of probs-weights along trajectory)')
%animate best particle
animate_local(robot, pk.pathq)
final_manip = compute_manip(robot, pk.pathq);
figure, plot(initial_manip), hold
plot(final_manip)
legend('initial_manip', 'final_manip')
title('manip at each time step')

figure, hold
for k=1:K
    plot(compute_manip(robot, G{k}.pathq))
end

% function plot_particles_costs(G)
% K = size(G,2);
% figure, hold
% for k=1:K
%   pk = G{k};
%   plot(pk.P)
% end
% title('Probabilities')

function plot_info(G, best_pk, best_probs)
global hfigures parameters
K = size(G,2);
figure(hfigures.hpaths),
clf
hold on
title('robot paths')
pp=[];
for k=1:K
    pk = G{k};
    plot(pk.pathq')
    pp(k) = sum(pk.P);
end
figure(hfigures.hcosts)
plot(sort(pp))
title('Trajectory sum of probabilities for each particle')

figure(hfigures.hbest_costs), clf, hold on
plot(best_probs)
title('Best trajectory probability at each time step (sum of prob-weights)')

function best_particle_info(pk)
figure,
subplot(3,1,1)
plot(pk.pathq(1,:))
subplot(3,1,2)
plot(pk.pathq(2,:))
subplot(3,1,3)
plot(pk.pathq(3,:))
title('joint values')
figure,
plot(pk.P)
title('Prob at each time step of particle SHOULD BE >0')
figure,
plot(pk.pathq')

title('BEST PARTICLE PATH')

function animate_robot(pk)
global robot
N = size(pk.path,2);
for i=1:N
   qi = pk.path(:,i);
   drawrobot3d(robot, qi)
end

%generate a prior path for the particles
function path = generate_path(robot, q0, qN, N)
deltaq=(qN-q0)/(N-1);
path = [];
for i=0:N-1
    qi = q0 + i*deltaq;
    path = [path qi];
    %drawrobot3d(robot, qi)
end

function mcost = mean_cost(G)
pk = G{1};
K = size(G,2);
N = size(pk.path,2);
cc = [];
for k=1:K
    pk = G{k};
    cc(k) = sum(pk.costs);
end
mcost = mean(cc);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% OBTAIN the best particle according to the defined cost function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [pk, best_prob] = best_particle(G)
%global robot
pk = G{1};
K = size(G,2);
N = size(pk.pathq,2);
probs = [];

for k=1:K
    pk = G{k};
    probs(k) = sum(pk.P);
end
%we must minimize costs
%[val, index]=min(costs);
%alternatively maximize our defined weigths
[val, index]=max(probs);
fprintf('Best particle is index: k=%d\n', index)
pk = G{index};
best_prob = val;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute cost function to encode manipulability.
% Return:
% P: likelihood function
% q: the cost function it self
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [P, cost] = cost_function_manipulability(q)
global robot parameters
%compute nil-space!!
% J = manipulator_jacobian(robot, q);
manip = compute_manip(robot, q);
%define cost: cost is lower as manipulability is higher
cost = 1/(manip+0.01);
lambda = parameters.lambda_manip;
%return likelihood
P = exp(-(1/lambda)*cost);

function V = compute_link_velocity_1(theta, thetad)
V = 1*thetad(1)^2;

function V = compute_link_velocity_2(theta, thetad)
th1 = theta(1);
th2 = theta(2);

J = [-sin(th1)  -sin(th1+th2);
    cos(th1)    cos(th1+th2)];
V = J*[thetad(1) thetad(1)+thetad(2)]';
V = norm(V);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generate a noisy particle set
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function G=generate_initial_particle_set(K, path)
for k=1:K
    G{k} = generate_particle(path);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generate a path for each particle without noise
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function pk = generate_particle(path)
global robot
%each path is in each row for qi
DOF = robot.DOF;
for i = 1:DOF
    pk.path(i,:) = path(i,:);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% add noise to the
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function G=add_noise_to_particle_set(G, pathT)
K = size(G,2);
%for every particle in the set
for k=1:K
    pk = G{k};
    pathT = pk.pathT;
    pk = add_noise_to_particle_null_space(pk, pathT);
    G{k} = pk;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% !! Add noise along the null space!!!
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function pk = add_noise_to_particle_null_space(pk, pathT)
global parameters robot
N = size(pk.pathq, 2);
%each path is in each row for qi
DOF = robot.DOF;

% at each particle add some noise!
%move along the path of the particle
for i = 1:N
    %current joint position
    q1 = pk.pathq(:,i);
    %generate some samples along the nulls space
    qq = sample_from_null_space(q1);
    %populate with the current pose!!!
    qq = [qq q1];
    p = [];
    costs = [];
    %for all the new samples!
    for j=1:size(qq,2)
        %weight the samples,
        %compute cost of obstacles and manipulability
        [Pm, costm] = cost_function_manipulability(qq(:,j));
        if robot.DOF == 4
            [Po] = cost_function_distance_4dof(qq(:,j));
        else
            [Po] = cost_function_distance_7dof(qq(:,j));
        end
        p(j)=Pm*Po;
        %p(j)=Pm;
    end
    %select the best one
    %from the sample performed based on probability
    %max probatility!
    [prob, index] = max(p);
    q = qq(:,index);
    pk.pathq(:,i) = q;
    %and current probability
    pk.P(i) = prob;
end

%obtain samples from the null space
%near to q
function qq=sample_from_null_space(q1)
global robot
global parameters
%sigma_noise = parameters.noise_sigma_null_space;
alpha = parameters.alpha;
time_step = parameters.time_step;

q = q1;
the = 0;
qq=[];
%sigma_noise = 0.3;
%alpha = mvnrnd(0, sigma_noise);
%alpha = 0.3;
ss = +1;
%iterate at each time step: forward movement
while the < abs(alpha)
    %compute null_space
    if robot.DOF==4
        qdnull = null_space_4dof(robot, q);
    else
        qdnull = null_space_7dof(robot, q);
    end
    if norm(qdnull) < 0.01
        break
    end
    qdnull = qdnull/norm(qdnull);
    dq = ss*time_step*qdnull;
    the = the + norm(dq);
    q = q  + dq;
    qq = [qq q];
end

ss = -1;
q = q1;
the = 0;
%backward movement
while the < abs(alpha)
    %compute null_space
        %compute null_space
    if robot.DOF==4
        qdnull = null_space_4dof(robot, q);
    else
        qdnull = null_space_7dof(robot, q);
    end
    if norm(qdnull) < 0.01
        break
    end
    qdnull = qdnull/norm(qdnull);
    dq = ss*time_step*qdnull;
    the = the + norm(dq);
    q = q  + dq;
    qq = [qq q];
end

%
% Uniform value betweenn min_val and max_val
%
function delta = uniform(min_val, max_val, n, m)
delta = (max_val-min_val)*rand(n, m) + min_val;


addpath(genpath('../mutils/My/'));
addpath(genpath('../ptv'));
%%
dat = load('data/dti.mat');
imgs = double(reshape(dat.imgs, [size(dat.imgs, 1), size(dat.imgs, 2), 1, 1, size(dat.imgs,3)]));

opts = [];
opts.check_gradients = 155*0;
opts.pix_resolution = [1,1];
opts.metric = 'nuclear';

opts.grid_spacing = [10, 10];
opts.isoTV = 5e-2;
opts.mean_penalty = 1e-3;

opts.spline_order = 1;
opts.max_iters = 100;
opts.display = 'off';

opts.border_mask = 6;
opts.k_down = 0.7;

tic
[voldef_pl, Tmin_pl,  Kmin_pl] = ptv_register(imgs, [], opts);
toc

%%
savegif('results/dti_register.gif', squeeze([imgs, voldef_pl])*1, 1/20);
%%
dat = load('data/dti2.mat');
imgs = double(abs( reshape(dat.data_to_save(:,:, 2, :, :), 55,116, [])));
imgs = double(reshape(imgs, [size(imgs, 1), size(imgs, 2), 1, 1, size(imgs,3)]));

imgs = imgs ./ max(max(imgs, [], 1), [], 2);

opts = [];
opts.check_gradients = 155*0;
opts.pix_resolution = [1,1];
opts.metric = 'nuclear';

opts.grid_spacing = [6, 6];
opts.isoTV = 5e-3;
opts.mean_penalty = 1e-5*0;

opts.spline_order = 1;
opts.max_iters = 100;
opts.display = 'off';

opts.border_mask = 6;
opts.k_down = 0.7;

tic
[voldef_pl, Tmin_pl,  Kmin_pl] = ptv_register(imgs, [], opts);
toc

%%
savegif('results/dti_register2.gif', squeeze([imgs, voldef_pl])*2, 1/20);


function PlotStates(timeAll, stateAll, P)

clf;

    %break out the data:
        time = timeAll;
        xPos = stateAll(1,:);
        yPos = stateAll(2,:);
        xVel = stateAll(3,:);
        yVel = stateAll(4,:);

    subplot(2,2,1)
        plot(time,xPos,'LineWidth',P.CurveLineWidth);
        xlabel('time (s)','FontSize',P.LabelFontSize)
        ylabel('horizontal position (m)','FontSize',P.LabelFontSize)
        set(gca,'fontsize',P.AxisFontSize);
    subplot(2,2,2)
        plot(time,yPos,'LineWidth',P.CurveLineWidth);
        xlabel('time (s)','FontSize',P.LabelFontSize)
        ylabel('vertical position (m)','FontSize',P.LabelFontSize)
        set(gca,'fontsize',P.AxisFontSize);
    subplot(2,2,3)
        plot(time,xVel,'LineWidth',P.CurveLineWidth);
        xlabel('time (s)','FontSize',P.LabelFontSize)
        ylabel('horizontal speed (m/s)','FontSize',P.LabelFontSize)
        set(gca,'fontsize',P.AxisFontSize);
    subplot(2,2,4)
        plot(time,yVel,'LineWidth',P.CurveLineWidth);
        xlabel('time (s)','FontSize',P.LabelFontSize)
        ylabel('vertical speed (m/s)','FontSize',P.LabelFontSize)
        set(gca,'fontsize',P.AxisFontSize);

end


function varargout = drawLabels3d(varargin)
%DRAWLABELS3D Draw text labels at specified 3D positions.
%
%   drawLabels3d(X, Y, Z, LBL) draw labels LBL at position X and Y.
%   LBL can be either a string array, or a number array. In this case,
%   string are created by using sprintf function, with '%.2f' mask.
%
%   drawLabels3d(POS, LBL) draw labels LBL at position specified by POS,
%   where POS is a N-by-3 int array.
%
%   drawLabels3d(..., NUMBERS, FORMAT) create labels using sprintf function,
%   with the mask given by FORMAT (e. g. '%03d' or '5.3f'), and the
%   corresponding values.
%
%   See also
%     drawLabels
%

%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 31/01/2019.
%

%   HISTORY
%   2019-01-31 write 3D version from drawLabels

%% Parse input arguments

% check if enough inputs are given
if isempty(varargin)
    error('wrong number of arguments in drawLabels3d');
end

% extract handle of axis to draw on
if isscalar(varargin{1}) && ishandle(varargin{1})
    ax = varargin{1};
    varargin(1) = [];
else
    ax = gca;
end

% process input parameters
var = varargin{1};
if size(var, 2) == 1
    % coordinates given as separate arguments
    if length(varargin) < 4
        error('wrong number of arguments in drawLabels');
    end
    px  = var;
    py  = varargin{2};
    pz  = varargin{3};
    lbl = varargin{4};
    varargin(1:4) = [];
else
    % parameters given as a packed array
    if length(varargin) < 2
        error('wrong number of arguments in drawLabels');
    end
    if size(var, 2) < 3
        error('Requires coordinates array to have at least three columns');
    end
    px  = var(:,1);
    py  = var(:,2);
    pz  = var(:,3);
    lbl = varargin{2};
    varargin(1:2) = [];
end

% parse format for displaying numeric values
format = '%.2f';
if ~isempty(varargin)
    format = varargin{1};
end
if size(format, 1) == 1 && size(px, 1) > 1
    format = repmat(format, size(px, 1), 1);
end

%% compute the strings that have to be displayed
labels = cell(length(px), 1);
if isnumeric(lbl)
    for i = 1:length(px)
        labels{i} = sprintf(format(i,:), lbl(i));
    end
elseif ischar(lbl)
    for i = 1:length(px)
        labels{i} = lbl(i,:);
    end
elseif iscell(lbl)
    labels = lbl;
end
labels = char(labels);

%% display the text
h = text(px, py, pz, labels, 'parent', ax);

%% format output
if nargout > 0
    varargout = {h};
end


function r = arange(start, stop, step)
    r = [start:step:stop]';


function [data, headerText] = load_ascii(filename,delimiter,header,nlines,offset)

% This function loads data from a tab delimited or csv ascii file similar
% to dlmread/importdata. If the file has fixed width columns and an offset
% is desired, it uses this information to quickly scan to desired area
% (making it significantly faster in those scenarios). Useful when working
% with very large ascii files that cannot be entirely loaded into memory at
% one time. If a file is loaded in with no offset, load_ascii is comparable
% to dlmread (faster/slower depends on delimiter). Importdata is many times
% slower than both.
%
% Inputs:
%
% filename: absolute or relative file path
% header: number of lines for header (defaults to 0)
% nlines: number of lines to read in (defaults to inf)
% offset: number of lines to skip (defaults to 0) (assumes data is
% written with a fixed field width. If this is not the
% case, offset option will not work.
% delimiter: delimiter in ascii file (defaults to tab)
%
% outputs:
% data: matrix of data in file
% headerText: text of header as specified by header input
%
% Example usage:
% Consider scenario where we have a very large ascii file (fixed width tab
% delimited, 24 header lines). We are only interested in a section of 2e6
% lines, 20e6 past the end of the header.
% %syntax
% %[data,headerText] = load_ascii(filename,delimiter,header,nlines,offset)
% tic;data = load_ascii(filename,'\t',24,2e6,20e6);toc;
% tic;data2 = dlmread(filename,'\t',[20e6+24 0 12e6+23 2]);toc;
% Elapsed time is 3.838641 seconds.
% Elapsed time is 21.537717 seconds.

% initialize some things
if nargin<3, header = 0;end
if nargin<4, nlines = inf;end
if nargin<5, offset = 0;end
if nargin<2, delimiter = '\t';end

fid = fopen(filename);
headerCell = cell(header,1);
for i=1:header
    headerCell{i} = fgets(fid);
end
headerText = char(headerCell);

% First line of Data...
fline = fgets(fid);

% determine number of columns
delKey = double(sprintf(delimiter));
ncols = sum(fline == delKey)+1;

% determine number of bytes per line
bytesPerLine = length(fline);

% skip offset
fseek(fid,bytesPerLine*(offset-1),0);

% read data and keep native format
d = fread(fid,bytesPerLine*nlines,'char=>char');

%convert format to matrix
format = ['%f' delimiter];
if ~strcmp(delimiter,'\t'),format = repmat(['%f' delimiter],1,ncols);format = format(1:end-1);end
data = sscanf(d,format,[ncols,nlines])';

fclose(fid);
end