function [c, v, n] = ft_connectivity_psi(input, varargin)

% FT_CONNECTIVITY_PSI computes the phase slope index from a data-matrix
% containing the cross-spectral density. It implements the method described
% in: Nolte et al., Robustly estimating the flow direction of information
% in complex physical systems. Physical Review Letters, 2008; 100; 234101.
%
% Use as
%   [c, v, n] = ft_connectivity_psi(input, ...)
%
% The input data input should be organized as
%   Repetitions x Channel x Channel (x Frequency) (x Time)
% or
%   Repetitions x Channelcombination (x Frequency) (x Time)
%
% The first dimension should be singleton if the input already contains an
% average.
%
% Additional optional input arguments come as key-value pairs:
%   nbin			=	scalar, half-bandwidth parameter: the number of frequency bins
%								across which to integrate
%   hasjack		= 0 or 1, specifying whether the repetitions represent
%               leave-one-out samples (allowing for a variance estimate)
%   feedback	= 'none', 'text', 'textbar' type of feedback showing progress of
%               computation
%   dimord		= string, specifying how the input matrix should be interpreted
%   powindx   =
%   normalize =
%
% The output p contains the phase slope index, v is a variance estimate
% which only can be computed if the data contains leave-one-out samples,
% and n is the number of repetitions in the input data. If the phase slope
% index is positive, then the first chan (1st dim) becomes more lagged (or
% less leading) with higher frequency, indicating that it is causally
% driven by the second channel (2nd dim)
%
% See also FT_CONNECTIVITYANALYSIS

% Copyright (C) 2009-2010 Donders Institute, Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% FIXME: interpretation of the slope

hasjack   = ft_getopt(varargin, 'hasjack', 0);
feedback  = ft_getopt(varargin, 'feedback', 'none');
dimord    = ft_getopt(varargin, 'dimord');
powindx   = ft_getopt(varargin, 'powindx');
normalize = ft_getopt(varargin, 'normalize', 'no');
nbin      = ft_getopt(varargin, 'nbin');

if isempty(dimord)
  ft_error('input parameters should contain a dimord');
end

if (length(strfind(dimord, 'chan'))~=2 || contains(dimord, 'pos')>0) && ~isempty(powindx)
  %crossterms are not described with chan_chan_therest, but are linearly indexed

  siz = size(input);

  outsum = zeros(siz(2:end));
  outssq = zeros(siz(2:end));
  pvec   = [2 setdiff(1:numel(siz),2)];

  ft_progress('init', feedback, 'computing metric...');
  %first compute coherency and then phaseslopeindex
  for j = 1:siz(1)
    ft_progress(j/siz(1), 'computing metric for replicate %d from %d\n', j, siz(1));
    c      = reshape(input(j,:,:,:,:), siz(2:end));
    p1     = abs(reshape(input(j,powindx(:,1),:,:,:), siz(2:end)));
    p2     = abs(reshape(input(j,powindx(:,2),:,:,:), siz(2:end)));

    p      = ipermute(phaseslope(permute(c./sqrt(p1.*p2), pvec), nbin, normalize), pvec);

    outsum = outsum + p;
    outssq = outssq + p.^2;
  end
  ft_progress('close');

elseif length(strfind(dimord, 'chan'))==2 || length(strfind(dimord, 'pos'))==2
  %crossterms are described by chan_chan_therest

  siz = size(input);

  outsum = zeros(siz(2:end));
  outssq = zeros(siz(2:end));
  pvec   = [3 setdiff(1:numel(siz),3)];

  ft_progress('init', feedback, 'computing metric...');
  for j = 1:siz(1)
    ft_progress(j/siz(1), 'computing metric for replicate %d from %d\n', j, siz(1));
    p1  = zeros([siz(2) 1 siz(4:end)]);
    p2  = zeros([1 siz(3) siz(4:end)]);
    for k = 1:siz(2)
      p1(k,1,:,:,:,:) = input(j,k,k,:,:,:,:);
      p2(1,k,:,:,:,:) = input(j,k,k,:,:,:,:);
    end
    c      = reshape(input(j,:,:,:,:,:,:), siz(2:end));
    p1     = p1(:,ones(1,siz(3)),:,:,:,:);
    p2     = p2(ones(1,siz(2)),:,:,:,:,:);
    p      = ipermute(phaseslope(permute(c./sqrt(p1.*p2), pvec), nbin, normalize), pvec);
    p(isnan(p)) = 0;
    outsum = outsum + p;
    outssq = outssq + p.^2;
  end
  ft_progress('close');

end

n = siz(1);
c = outsum./n;

if n>1
  n = shiftdim(sum(~isnan(input),1),1);
  if hasjack
    bias = (n-1).^2;
  else
    bias = 1;
  end
  v = bias.*(outssq - (outsum.^2)./n)./(n - 1);
else
  v = [];
end

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

function [y] = phaseslope(x, n, norm)

m   = size(x, 1); %total number of frequency bins
y   = zeros(size(x));
x(1:end-1,:,:,:,:) = conj(x(1:end-1,:,:,:,:)).*x(2:end,:,:,:,:);

if strcmp(norm, 'yes')
  coh = zeros(size(x));
  coh(1:end-1,:,:,:,:) = (abs(x(1:end-1,:,:,:,:)) .* abs(x(2:end,:,:,:,:))) + 1;
  %FIXME why the +1? get the coherence
  for k = 1:m
    begindx = max(1,k-n);
    endindx = min(m,k+n);
    y(k,:,:,:,:) = imag(nansum(x(begindx:endindx,:,:,:,:)./coh(begindx:endindx,:,:,:,:),1));
  end
else
  for k = 1:m
    begindx = max(1,k-n);
    endindx = min(m,k+n);
    y(k,:,:,:,:) = imag(nansum(x(begindx:endindx,:,:,:,:),1));
  end
end


function [ RGBpersp, XYZpersp ] = ht_Points2Persp( RGB, XYZ, P, H, W )
%RGB, XYZ: 3*(#Number of Points)
%P = K * [R, t]
%H, W: size of output perspective

%outputs
RGBpersp = nan(H, W, 3);
XYZpersp = nan(H, W, 3);

%XYZ->image coordinate
UV = P * [XYZ; ones(1, size(XYZ, 2))];
UV_norm = UV(3, :);
UV = bsxfun(@rdivide, UV(1:2, :), UV_norm);

% keyboard;

front_idx = UV_norm > 0;
UV = UV(:, front_idx);
RGB = RGB(:, front_idx);
XYZ = XYZ(:, front_idx);
UV_norm = UV_norm(front_idx);

visible_idx = UV(1, :) >= 0 & UV(2, :) >= 0 & UV(1, :) < W & UV(2, :) < H;
UV = UV(:, visible_idx);
RGB = RGB(:, visible_idx);
XYZ = XYZ(:, visible_idx);
UV_norm = UV_norm(visible_idx);

% UV= 1 + uint16(UV);
% UV = UV(:, UV_norm>0);
% UV_norm = UV_norm(UV_norm>0);
% RGB = RGB(:, UV_norm>0);
% XYZ = XYZ(:, UV_norm>0);
%
% UV = UV(:, UV(1, :) <=W & UV(2, :) <= H);
% UV_norm = UV_norm(UV(1, :) <=W & UV(2, :) <= H);
% RGB = RGB(:, UV(1, :) <=W & UV(2, :) <= H);
% XYZ = XYZ(:, UV(1, :) <=W & UV(2, :) <= H);

% keyboard;

%rendering
UV = 1 + uint16(floor(UV));
check_norm = ones(H, W) * max(UV_norm);
for pp = 1:1:size(UV, 2)
    if UV_norm(pp) <= check_norm(UV(2, pp), UV(1, pp))
        RGBpersp(UV(2, pp), UV(1, pp), 1) = RGB(1, pp);
        RGBpersp(UV(2, pp), UV(1, pp), 2) = RGB(2, pp);
        RGBpersp(UV(2, pp), UV(1, pp), 3) = RGB(3, pp);
        XYZpersp(UV(2, pp), UV(1, pp), 1) = XYZ(1, pp);
        XYZpersp(UV(2, pp), UV(1, pp), 2) = XYZ(2, pp);
        XYZpersp(UV(2, pp), UV(1, pp), 3) = XYZ(3, pp);
        check_norm(UV(2, pp), UV(1, pp)) = UV_norm(pp);
    end
end

RGBpersp = uint8(RGBpersp);

end


load camera_results;

string_global = 'global n_ima';
for kk = 1:n_ima,
   string_global = [string_global ' x_' num2str(kk) ' X_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk)];
end;
eval(string_global);

%----------------- global optimization: ---------------------

load projector_data; % load the projector corners (previously saved)
load projector_results;

solution_projector = solution;

load camera_results;

solution_camera = solution;

param_cam = solution_camera([1:10 16:end]);
param_proj = solution_projector([1:10 16:end]);

param = [param_cam;param_proj];

% Restart the minimization from here (if need be):
load camera_results;
load calib_cam_proj_optim2;

options = [1 1e-4 1e-4 1e-6  0 0 0 0 0 0 0 0 0 12000 0 1e-8 0.1 0];

%if 0, % use the full distortion model:

%   fprintf(1,'Take the complete distortion model\n');

   % test the global error function:
%   e_global = error_cam_proj(param);

%   param_init = param;

%   param = leastsq('error_cam_proj',param,options);

%else

   % Use a limitd distortion model (no 6th order)
   fprintf(1,'Take the 6th order distortion coefficient out\n');

   param = param([1:9 11:11+6*n_ima-1  11+6*n_ima:11+6*n_ima+9-1  11+6*n_ima+9+1:end]);

   % test the global error function:
   e_global2 = error_cam_proj2(param);

   param_init = param;

   param = leastsq('error_cam_proj2',param,options);

   param = [param(1:9);0;param(10:10+6*n_ima-1);param(10+6*n_ima:10+6*n_ima+9-1);0;param(10+6*n_ima+9:end)];

%end;

% Extract the parameters:

cam_proj_extract_param;

% Relative prosition of camera wrt world:
omc = omc_1;
Rc = Rc_1;
Tc = Tc_1;

% relative position of projector wrt world:
Rp = R*Rc;
omp = rodrigues(Rp);
Tp = T + R*Tc;

eval(['save calib_cam_proj_optim3  R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp param param_init']);

no_image = 0;
% Image size: (may or may not be available)
nx = 640;
ny = 480;

comp_error_calib;

% Save the optimal camera parameters:
saving_calib;
copyfile('Calib_Results.mat','camera_results_optim3.mat');
delete('Calib_Results.mat');

% Save the optimal camera parameters:
fc = fp;
cc = cp;
alpha_c = alpha_p;
kc = kp;

n_ima = 1;
X_1 = X_proj;
x_1 = x_proj;
omc_1 = om;
Tc_1 = T;
Rc_1 = R;

% Image size: (may or may not be available)
nx = 1024;
ny = 768;

% No calibration image is available (only the corner coordinates)
no_image = 1;

comp_error_calib;

saving_calib;
copyfile('Calib_Results.mat','projector_results_optim3.mat');
delete('Calib_Results.mat');


function plot_q4_mesh ( node_num, element_num, node_xy, element_node, ...
  node_show, element_show, output_filename )

%*****************************************************************************80
%
%% TPLOT_Q4_MESH plots a Q4 mesh.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the nodes.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_NODE(4,ELEMENT_NUM), the nodes
%    that form the elements.
%
%    Input, logical NODE_SHOW:
%    0, do not show the nodes.
%    1, show the nodes.
%    2, show the nodes, and label them.
%
%    Input, logical ELEMENT_SHOW,
%    0, do not show the elements.
%    1, show the elements.
%    2, show the elements, and label them.
%
%    Input, string OUTPUT_FILENAME, the name of the output file.
%
  element_order = 4;

  x_ps_max = 576;
  x_ps_max_clip = 594;
  x_ps_min = 36;
  x_ps_min_clip = 18;
  y_ps_max = 666;
  y_ps_max_clip = 684;
  y_ps_min = 126;
  y_ps_min_clip = 108;

  date_time = timestring ( );
%
%  We need to do some figuring here, so that we can determine
%  the range of the data, and hence the height and width
%  of the piece of paper.
%
  x_max = max ( node_xy(1,1:node_num) );
  x_min = min ( node_xy(1,1:node_num) );
  x_scale = x_max - x_min;

  x_max = x_max + 0.05 * x_scale;
  x_min = x_min - 0.05 * x_scale;
  x_scale = x_max - x_min;

  y_max = max ( node_xy(2,1:node_num) );
  y_min = min ( node_xy(2,1:node_num) );
  y_scale = y_max - y_min;

  y_max = y_max + 0.05 * y_scale;
  y_min = y_min - 0.05 * y_scale;
  y_scale = y_max - y_min;

  if ( x_scale < y_scale )

    delta = round ( ( x_ps_max - x_ps_min ) ...
      * ( y_scale - x_scale ) / ( 2.0 * y_scale ) );

    x_ps_max = x_ps_max - delta;
    x_ps_min = x_ps_min + delta;

    x_ps_max_clip = x_ps_max_clip - delta;
    x_ps_min_clip = x_ps_min_clip + delta;

    x_scale = y_scale;

  elseif ( y_scale < x_scale )

    delta = round ( ( y_ps_max - y_ps_min ) ...
      * ( x_scale - y_scale ) / ( 2.0 * x_scale ) );

    y_ps_max      = y_ps_max - delta;
    y_ps_min      = y_ps_min + delta;

    y_ps_max_clip = y_ps_max_clip - delta;
    y_ps_min_clip = y_ps_min_clip + delta;

    y_scale = x_scale;

  end
%
%  Plot the mesh.
%
  file_unit = fopen ( output_filename, 'wt' );

  if ( file_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PLOT_Q4_MESH - Fatal error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'PLOT_Q4_MESH - Fatal error!' );
  end

  fprintf ( file_unit, '%%!PS-Adobe-3.0 EPSF-3.0\n' );
  fprintf ( file_unit, '%%%%Creator: plot_q4_mesh.m\n' );
  fprintf ( file_unit, '%%%%Title: %s\n', output_filename );
  fprintf ( file_unit, '%%%%CreationDate: %s\n', date_time );
  fprintf ( file_unit, '%%%%Pages: 1\n' );
  fprintf ( file_unit, '%%%%BoundingBox:  %d  %d  %d  %d\n', ...
    x_ps_min, y_ps_min, x_ps_max, y_ps_max );
  fprintf ( file_unit, '%%%%Document-Fonts: Times-Roman\n' );
  fprintf ( file_unit, '%%%%LanguageLevel: 1\n' );
  fprintf ( file_unit, '%%%%EndComments\n' );
  fprintf ( file_unit, '%%%%BeginProlog\n' );
  fprintf ( file_unit, '/inch {72 mul} def\n' );
  fprintf ( file_unit, '%%%%EndProlog\n' );
  fprintf ( file_unit, '%%%%Page: 1 1\n' );
  fprintf ( file_unit, 'save\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the RGB color to very light gray.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '0.900  0.900  0.900 setrgbcolor\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Draw a gray border around the page.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'newpath\n' );
  fprintf ( file_unit, '  %d  %d  moveto\n', x_ps_min, y_ps_min );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max, y_ps_min );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max, y_ps_max );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min, y_ps_max );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min, y_ps_min );
  fprintf ( file_unit, 'stroke\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the RGB color to black.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '0.000  0.000  0.000 setrgbcolor\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the font and its size.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '/Times-Roman findfont\n' );
  fprintf ( file_unit, '0.50 inch scalefont\n' );
  fprintf ( file_unit, 'setfont\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Print a title.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%210  702  moveto\n' );
  fprintf ( file_unit, '%%(Triangulation)  show\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Define a clipping polygon.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'newpath\n' );
  fprintf ( file_unit, '  %d  %d  moveto\n', x_ps_min_clip, y_ps_min_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max_clip, y_ps_min_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max_clip, y_ps_max_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min_clip, y_ps_max_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min_clip, y_ps_min_clip );
  fprintf ( file_unit, 'clip newpath\n' );
%
%  Draw the nodes.
%
  if ( node_num <= 200 )
    circle_size = 5;
  elseif ( node_num <= 500 )
    circle_size = 4;
  elseif ( node_num <= 1000 )
    circle_size = 3;
  elseif ( node_num <= 5000 )
    circle_size = 2;
  else
    circle_size = 1;
  end

  if ( 1 <= node_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Draw filled dots at the nodes.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to blue.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.000  0.150  0.750 setrgbcolor\n' );
    fprintf ( file_unit, '%%\n' );

    for node = 1 : node_num

      x_ps = floor ( ...
        ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
        + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
        / ( x_max                   - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - node_xy(2,node)         ) * y_ps_min   ...
        + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
        / ( y_max                   - y_min ) );

      fprintf ( file_unit, ...
        '  newpath  %d  %d  %d 0 360 arc closepath fill\n', ...
        x_ps, y_ps, circle_size );

    end

  end
%
%  Label the nodes.
%
  if ( 2 <= node_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Label the nodes.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to darker blue.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.000  0.250  0.850 setrgbcolor\n' );
    fprintf ( file_unit, '/Times-Roman findfont\n' );
    fprintf ( file_unit, '0.20 inch scalefont\n' );
    fprintf ( file_unit, 'setfont\n' );
    fprintf ( file_unit, '%%\n' );

    for node = 1 : node_num

      x_ps = floor ( ...
        ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
        + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
        / ( x_max                   - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - node_xy(2,node)         ) * y_ps_min   ...
        + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
        / ( y_max                   - y_min ) );

      fprintf ( file_unit, '  %d  %d  moveto (%d) show\n', ...
        x_ps, y_ps+5, node );

    end

  end
%
%  Draw the elements.
%
  if ( 1 <= element_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to red.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.900  0.200  0.100 setrgbcolor\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Draw the elements.\n' );
    fprintf ( file_unit, '%%\n' );

    for element = 1 : element_num

      fprintf ( file_unit, 'newpath\n' );

      for i = 1 : element_order + 1

        e = i4_wrap ( i, 1, element_order )

        node = element_node(e,element);

        x_ps = floor ( ...
          ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
          + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
          / ( x_max                   - x_min ) );

        y_ps = floor ( ...
          ( ( y_max - node_xy(2,node)         ) * y_ps_min ...
          + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
          / ( y_max                   - y_min ) );

        if ( i == 1 )
          fprintf ( file_unit, '  %d  %d  moveto\n', x_ps, y_ps );
        else
          fprintf ( file_unit, '  %d  %d  lineto\n', x_ps, y_ps );
        end

      end

      fprintf ( file_unit, 'stroke\n' );

    end

  end
%
%  Label the elements.
%
  if ( 2 <= element_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Label the elements.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to darker red.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.950  0.250  0.150 setrgbcolor\n' );
    fprintf ( file_unit, '/Times-Roman findfont\n' );
    fprintf ( file_unit, '0.20 inch scalefont\n' );
    fprintf ( file_unit, 'setfont\n' );
    fprintf ( file_unit, '%%\n' );

    for element = 1 : element_num

      ave_x = 0.0;
      ave_y = 0.0;

      for i = 1 : element_order

        node = element_node(i,element);
        ave_x = ave_x + node_xy(1,node);
        ave_y = ave_y + node_xy(2,node);
      end

      ave_x = ave_x / element_order;
      ave_y = ave_y / element_order;

      x_ps = floor ( ...
        ( ( x_max - ave_x         ) * x_ps_min ...
        + (         ave_x - x_min ) * x_ps_max ) ...
        / ( x_max         - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - ave_y         ) * y_ps_min ...
        + (         ave_y - y_min ) * y_ps_max ) ...
        / ( y_max         - y_min ) );

      fprintf ( file_unit, '  %d  %d  moveto (%d) show\n', ...
        x_ps, y_ps, element );

    end
  end

  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'restore  showpage\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  End of page.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%%%Trailer\n' );
  fprintf ( file_unit, '%%%%EOF\n' );

  fclose ( file_unit );

  return
end


function r = sup(a)
%SUP          right bound of point matrix (only for completeness)
%
%   r = sup(a)
%

% written  10/16/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  r = a;


function [flowx,flowy] = specific_wind(x,y,nel)
%constant_wind   Reference problem 3.3 convective wind
%   [flowx,flowy] = specific_wind(x,y,nel);
%   input
%          x          x coordinate vector
%          y          y coordinate vector
%          nel        number of elements
%   specifies constant wind at angle theta to vertical
%   IFISS function: DJS; 5 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      theta= -pi/6;
      flowx =  sin(theta)*ones(nel,1);
      flowy =  cos(theta)*ones(nel,1);
      return


function [x, cost, info, options] = trustregions(problem, x, options)
% Riemannian trust-regions solver for optimization on manifolds.
%
% function [x, cost, info, options] = trustregions(problem)
% function [x, cost, info, options] = trustregions(problem, x0)
% function [x, cost, info, options] = trustregions(problem, x0, options)
% function [x, cost, info, options] = trustregions(problem, [], options)
%
% This is the Riemannian Trust-Region solver (with tCG inner solve), named
% RTR. This solver will attempt to minimize the cost function described in
% the problem structure. It requires the availability of the cost function
% and of its gradient. It will issue calls for the Hessian. If no Hessian
% nor approximate Hessian is provided, a standard approximation of the
% Hessian based on the gradient will be computed. If a preconditioner for
% the Hessian is provided, it will be used.
%
% For a description of the algorithm and theorems offering convergence
% guarantees, see the references below. Documentation for this solver is
% available online at:
%
% http://www.manopt.org/solver_documentation_trustregions.html
%
%
% The initial iterate is x0 if it is provided. Otherwise, a random point on
% the manifold is picked. To specify options whilst not specifying an
% initial iterate, give x0 as [] (the empty matrix).
%
% The two outputs 'x' and 'cost' are the last reached point on the manifold
% and its cost. Notice that x is not necessarily the best reached point,
% because this solver is not forced to be a descent method. In particular,
% very close to convergence, it is sometimes preferable to accept very
% slight increases in the cost value (on the order of the machine epsilon)
% in the process of reaching fine convergence. In practice, this is not a
% limiting factor, as normally one does not need fine enough convergence
% that this becomes an issue.
%
% The output 'info' is a struct-array which contains information about the
% iterations:
%   iter (integer)
%       The (outer) iteration number, or number of steps considered
%       (whether accepted or rejected). The initial guess is 0.
%	cost (double)
%       The corresponding cost value.
%	gradnorm (double)
%       The (Riemannian) norm of the gradient.
%	numinner (integer)
%       The number of inner iterations executed to compute this iterate.
%       Inner iterations are truncated-CG steps. Each one requires a
%       Hessian (or approximate Hessian) evaluation.
%	time (double)
%       The total elapsed time in seconds to reach the corresponding cost.
%	rho (double)
%       The performance ratio for the iterate.
%	rhonum, rhoden (double)
%       Regularized numerator and denominator of the performance ratio:
%       rho = rhonum/rhoden. See options.rho_regularization.
%	accepted (boolean)
%       Whether the proposed iterate was accepted or not.
%	stepsize (double)
%       The (Riemannian) norm of the vector returned by the inner solver
%       tCG and which is retracted to obtain the proposed next iterate. If
%       accepted = true for the corresponding iterate, this is the size of
%       the step from the previous to the new iterate. If accepted is
%       false, the step was not executed and this is the size of the
%       rejected step.
%	Delta (double)
%       The trust-region radius at the outer iteration.
%	cauchy (boolean)
%       Whether the Cauchy point was used or not (if useRand is true).
%   And possibly additional information logged by options.statsfun.
% For example, type [info.gradnorm] to obtain a vector of the successive
% gradient norms reached at each (outer) iteration.
%
% The options structure is used to overwrite the default values. All
% options have a default value and are hence optional. To force an option
% value, pass an options structure with a field options.optionname, where
% optionname is one of the following and the default value is indicated
% between parentheses:
%
%   tolgradnorm (1e-6)
%       The algorithm terminates if the norm of the gradient drops below
%       this. For well-scaled problems, a rule of thumb is that you can
%       expect to reduce the gradient norm by 8 orders of magnitude
%       (sqrt(eps)) compared to the gradient norm at a "typical" point (a
%       rough initial iterate for example). Further decrease is sometimes
%       possible, but inexact floating point arithmetic will eventually
%       limit the final accuracy. If tolgradnorm is set too low, the
%       algorithm may end up iterating forever (or at least until another
%       stopping criterion triggers).
%   maxiter (1000)
%       The algorithm terminates if maxiter (outer) iterations were executed.
%   maxtime (Inf)
%       The algorithm terminates if maxtime seconds elapsed.
%	miniter (3)
%       Minimum number of outer iterations (used only if useRand is true).
%	mininner (1)
%       Minimum number of inner iterations (for tCG).
%	maxinner (problem.M.dim() : the manifold's dimension)
%       Maximum number of inner iterations (for tCG).
%	Delta_bar (problem.M.typicaldist() or sqrt(problem.M.dim()))
%       Maximum trust-region radius. If you specify this parameter but not
%       Delta0, then Delta0 will be set to 1/8 times this parameter.
%   Delta0 (Delta_bar/8)
%       Initial trust-region radius. If you observe a long plateau at the
%       beginning of the convergence plot (gradient norm VS iteration), it
%       may pay off to try to tune this parameter to shorten the plateau.
%       You should not set this parameter without setting Delta_bar.
%	useRand (false)
%       Set to true if the trust-region solve is to be initiated with a
%       random tangent vector. If set to true, no preconditioner will be
%       used. This option is set to true in some scenarios to escape saddle
%       points, but is otherwise seldom activated.
%	kappa (0.1)
%       Inner kappa convergence tolerance.
%	theta (1.0)
%       Inner theta convergence tolerance.
%	rho_prime (0.1)
%       Accept/reject ratio : if rho is at least rho_prime, the outer
%       iteration is accepted. Otherwise, it is rejected. In case it is
%       rejected, the trust-region radius will have been decreased.
%       To ensure this, rho_prime must be strictly smaller than 1/4.
%   rho_regularization (1e3)
%       Close to convergence, evaluating the performance ratio rho is
%       numerically challenging. Meanwhile, close to convergence, the
%       quadratic model should be a good fit and the steps should be
%       accepted. Regularization lets rho go to 1 as the model decrease and
%       the actual decrease go to zero. Set this option to zero to disable
%       regularization (not recommended). See in-code for the specifics.
%   statsfun (none)
%       Function handle to a function that will be called after each
%       iteration to provide the opportunity to log additional statistics.
%       They will be returned in the info struct. See the generic Manopt
%       documentation about solvers for further information. statsfun is
%       called with the point x that was reached last, after the
%       accept/reject decision. See comment below.
%   stopfun (none)
%       Function handle to a function that will be called at each iteration
%       to provide the opportunity to specify additional stopping criteria.
%       See the generic Manopt documentation about solvers for further
%       information.
%   verbosity (2)
%       Integer number used to tune the amount of output the algorithm
%       generates during execution (mostly as text in the command window).
%       The higher, the more output. 0 means silent. 3 and above includes a
%       display of the options structure at the beginning of the execution.
%   debug (false)
%       Set to true to allow the algorithm to perform additional
%       computations for debugging purposes. If a debugging test fails, you
%       will be informed of it, usually via the command window. Be aware
%       that these additional computations appear in the algorithm timings
%       too.
%   storedepth (20)
%       Maximum number of different points x of the manifold for which a
%       store structure will be kept in memory in the storedb. If the
%       caching features of Manopt are not used, this is irrelevant. If
%       memory usage is an issue, you may try to lower this number.
%       Profiling may then help to investigate if a performance hit was
%       incured as a result.
%
% Notice that statsfun is called with the point x that was reached last,
% after the accept/reject decision. Hence: if the step was accepted, we get
% that new x, with a store which only saw the call for the cost and for the
% gradient. If the step was rejected, we get the same x as previously, with
% the store structure containing everything that was computed at that point
% (possibly including previous rejects at that same point). Hence, statsfun
% should not be used in conjunction with the store to count operations for
% example. Instead, you could use a global variable and increment that
% variable directly from the cost related functions. It is however possible
% to use statsfun with the store to compute, for example, alternate merit
% functions on the point x.
%
% See also: steepestdescent conjugategradient manopt/examples

% This file is part of Manopt: www.manopt.org.
% This code is an adaptation to Manopt of the original GenRTR code:
% RTR - Riemannian Trust-Region
% (c) 2004-2007, P.-A. Absil, C. G. Baker, K. A. Gallivan
% Florida State University
% School of Computational Science
% (http://www.math.fsu.edu/~cbaker/GenRTR/?page=download)
% See accompanying license file.
% The adaptation was executed by Nicolas Boumal.
%
% Change log:
%
%   NB April 3, 2013:
%       tCG now returns the Hessian along the returned direction eta, so
%       that we do not compute that Hessian redundantly: some savings at
%       each iteration. Similarly, if the useRand flag is on, we spare an
%       extra Hessian computation at each outer iteration too, owing to
%       some modifications in the Cauchy point section of the code specific
%       to useRand = true.
%
%   NB Aug. 22, 2013:
%       This function is now Octave compatible. The transition called for
%       two changes which would otherwise not be advisable. (1) tic/toc is
%       now used as is, as opposed to the safer way:
%       t = tic(); elapsed = toc(t);
%       And (2), the (formerly inner) function savestats was moved outside
%       the main function to not be nested anymore. This is arguably less
%       elegant, but Octave does not (and likely will not) support nested
%       functions.
%
%   NB Dec. 2, 2013:
%       The in-code documentation was largely revised and expanded.
%
%   NB Dec. 2, 2013:
%       The former heuristic which triggered when rhonum was very small and
%       forced rho = 1 has been replaced by a smoother heuristic which
%       consists in regularizing rhonum and rhoden before computing their
%       ratio. It is tunable via options.rho_regularization. Furthermore,
%       the solver now detects if tCG did not obtain a model decrease
%       (which is theoretically impossible but may happen because of
%       numerical errors and/or because of a nonlinear/nonsymmetric Hessian
%       operator, which is the case for finite difference approximations).
%       When such an anomaly is detected, the step is rejected and the
%       trust region radius is decreased.
%
%   NB Dec. 3, 2013:
%       The stepsize is now registered at each iteration, at a small
%       additional cost. The defaults for Delta_bar and Delta0 are better
%       defined. Setting Delta_bar in the options will automatically set
%       Delta0 accordingly. In Manopt 1.0.4, the defaults for these options
%       were not treated appropriately because of an incorrect use of the
%       isfield() built-in function.

% Verify that the problem description is sufficient for the solver.
if ~canGetCost(problem)
    warning('manopt:getCost', ...
            'No cost provided. The algorithm will likely abort.');
end
if ~canGetGradient(problem)
    warning('manopt:getGradient', ...
            'No gradient provided. The algorithm will likely abort.');
end
if ~canGetHessian(problem)
    warning('manopt:getHessian:approx', ...
            'No Hessian provided. Using an approximation instead.');
end

% Define some strings for display
tcg_stop_reason = {'negative curvature',...
                   'exceeded trust region',...
                   'reached target residual-kappa',...
                   'reached target residual-theta',...
                   'dimension exceeded',...
                   'model increased'};

% Set local defaults here
localdefaults.verbosity = 2;
localdefaults.maxtime = inf;
localdefaults.miniter = 3;
localdefaults.maxiter = 1000;
localdefaults.mininner = 1;
localdefaults.maxinner = problem.M.dim();
localdefaults.tolgradnorm = 1e-6;
localdefaults.kappa = 0.1;
localdefaults.theta = 1.0;
localdefaults.rho_prime = 0.1;
localdefaults.useRand = false;
localdefaults.rho_regularization = 1e3;

% Merge global and local defaults, then merge w/ user options, if any.
localdefaults = mergeOptions(getGlobalDefaults(), localdefaults);
if ~exist('options', 'var') || isempty(options)
    options = struct();
end
options = mergeOptions(localdefaults, options);

% Set default Delta_bar and Delta0 separately to deal with additional
% logic: if Delta_bar is provided but not Delta0, let Delta0 automatically
% be some fraction of the provided Delta_bar.
if ~isfield(options, 'Delta_bar')
    if isfield(problem.M, 'typicaldist')
        options.Delta_bar = problem.M.typicaldist();
    else
        options.Delta_bar = sqrt(problem.M.dim());
    end
end
if ~isfield(options,'Delta0')
    options.Delta0 = options.Delta_bar / 8;
end

% Check some option values
assert(options.rho_prime < 1/4, ...
        'options.rho_prime must be strictly smaller than 1/4.');
assert(options.Delta_bar > 0, ...
        'options.Delta_bar must be positive.');
assert(options.Delta0 > 0 && options.Delta0 < options.Delta_bar, ...
        'options.Delta0 must be positive and smaller than Delta_bar.');

% It is sometimes useful to check what the actual option values are.
if options.verbosity >= 3
    disp(options);
end

% Create a store database
storedb = struct();

tic();

% If no initial point x is given by the user, generate one at random.
if ~exist('x', 'var') || isempty(x)
    x = problem.M.rand();
end

%% Initializations

% k counts the outer (TR) iterations. The semantic is that k counts the
% number of iterations fully executed so far.
k = 0;

% initialize solution and companion measures: f(x), fgrad(x)
[fx fgradx storedb] = getCostGrad(problem, x, storedb);
norm_grad = problem.M.norm(x, fgradx);

% initialize trust-region radius
Delta = options.Delta0;

% Save stats in a struct array info, and preallocate
% (see http://people.csail.mit.edu/jskelly/blog/?x=entry:entry091030-033941)
if ~exist('used_cauchy', 'var')
    used_cauchy = [];
end
stats = savestats(problem, x, storedb, options, k, fx, norm_grad, Delta);
info(1) = stats;
info(min(10000, options.maxiter+1)).iter = [];

% ** Display:
if options.verbosity == 2
   fprintf(['%3s %3s      %5s                %5s     ',...
            'f: %e   |grad|: %e\n'],...
           '   ','   ','     ','     ', fx, norm_grad);
elseif options.verbosity > 2
   fprintf('************************************************************************\n');
   fprintf('%3s %3s    k: %5s     num_inner: %5s     %s\n',...
           '','','______','______','');
   fprintf('       f(x) : %e       |grad| : %e\n', fx, norm_grad);
   fprintf('      Delta : %f\n', Delta);
end

% **********************
% ** Start of TR loop **
% **********************
while true

	% Start clock for this outer iteration
    tic();

    % Run standard stopping criterion checks
    [stop reason] = stoppingcriterion(problem, x, options, info, k+1);

    % If the stopping criterion that triggered is the tolerance on the
    % gradient norm but we are using randomization, make sure we make at
    % least miniter iterations to give randomization a chance at escaping
    % saddle points.
    if stop == 2 && options.useRand && k < options.miniter
        stop = 0;
    end

    if stop
        if options.verbosity >= 1
            fprintf([reason '\n']);
        end
        break;
    end

    if options.verbosity > 2 || options.debug > 0
        fprintf('************************************************************************\n');
    end

    % *************************
    % ** Begin TR Subproblem **
    % *************************

    % Determine eta0
    if ~options.useRand
        % Pick the zero vector
        eta = problem.M.zerovec(x);
    else
        % Random vector in T_x M (this has to be very small)
        eta = problem.M.lincomb(x, 1e-6, problem.M.randvec(x));
        % Must be inside trust-region
        while problem.M.norm(x, eta) > Delta
            eta = problem.M.lincomb(x, sqrt(sqrt(eps)), eta);
        end
    end

    % solve TR subproblem
    [eta Heta numit stop_inner storedb] = ...
                     tCG(problem, x, fgradx, eta, Delta, options, storedb);
    srstr = tcg_stop_reason{stop_inner};

    % This is only computed for logging purposes, because it may be useful
    % for some user-defined stopping criteria. If this is not cheap for
    % specific application (compared to evaluating the cost), we should
    % reconsider this.
    norm_eta = problem.M.norm(x, eta);

    if options.debug > 0
        testangle = problem.M.inner(x, eta, fgradx) / (norm_eta*norm_grad);
    end

    % If using randomized approach, compare result with the Cauchy point.
    % Convergence proofs assume that we achieve at least the reduction of
    % the Cauchy point. After this if-block, either all eta-related
    % quantities have been changed consistently, or none of them have
    % changed.
    if options.useRand
        used_cauchy = false;
        % Check the curvature,
        [Hg storedb] = getHessian(problem, x, fgradx, storedb);
        g_Hg = problem.M.inner(x, fgradx, Hg);
        if g_Hg <= 0
            tau_c = 1;
        else
            tau_c = min( norm_grad^3/(Delta*g_Hg) , 1);
        end
        % and generate the Cauchy point.
        eta_c  = problem.M.lincomb(x, -tau_c * Delta / norm_grad, fgradx);
        Heta_c = problem.M.lincomb(x, -tau_c * Delta / norm_grad, Hg);

        % Now that we have computed the Cauchy point in addition to the
        % returned eta, we might as well keep the best of them.
        mdle  = fx + problem.M.inner(x, fgradx, eta) ...
                   + .5*problem.M.inner(x, Heta,   eta);
        mdlec = fx + problem.M.inner(x, fgradx, eta_c) ...
                   + .5*problem.M.inner(x, Heta_c, eta_c);
        if mdle > mdlec
            eta = eta_c;
            Heta = Heta_c; % added April 11, 2012
            used_cauchy = true;
        end
    end

	% Compute the retraction of the proposal
	x_prop  = problem.M.retr(x, eta);

	% Compute the function value of the proposal
	[fx_prop storedb] = getCost(problem, x_prop, storedb);

	% Will we accept the proposed solution or not?
    % Check the performance of the quadratic model against the actual cost.
    rhonum = fx - fx_prop;
    rhoden = -problem.M.inner(x, fgradx, eta) ...
             -.5*problem.M.inner(x, eta, Heta);

    % Heuristic -- added Dec. 2, 2013 (NB) to replace the former heuristic.
    % This heuristic is documented in the book by Conn Gould and Toint on
    % trust-region methods, section 17.4.2.
    % rhonum measures the difference between two numbers. Close to
    % convergence, these two numbers are very close to each other, so
    % that computing their difference is numerically challenging: there may
    % be a significant loss in accuracy. Since the acceptance or rejection
    % of the step is conditioned on the ratio between rhonum and rhoden,
    % large errors in rhonum result in a large error in rho, hence in
    % erratic acceptance / rejection. Meanwhile, close to convergence,
    % steps are usually trustworthy and we should transition to a Newton-
    % like method, with rho=1 consistently. The heuristic thus shifts both
    % rhonum and rhoden by a small amount such that far from convergence,
    % the shift is irrelevant and close to convergence, the ratio rho goes
    % to 1, effectively promoting acceptance of the step.
    % The rationale is that close to convergence, both rhonum and rhoden
    % are quadratic in the distance between x and x_prop. Thus, when this
    % distance is on the order of sqrt(eps), the value of rhonum and rhoden
    % is on the order of eps, which is indistinguishable from the numerical
    % error, resulting in badly estimated rho's.
    % For abs(fx) < 1, this heuristic is invariant under offsets of f but
    % not under scaling of f. For abs(fx) > 1, the opposite holds. This
    % should not alarm us, as this heuristic only triggers at the very last
    % iterations if very fine convergence is demanded.
    rho_reg = max(1, abs(fx)) * eps * options.rho_regularization;
    rhonum = rhonum + rho_reg;
    rhoden = rhoden + rho_reg;

    if options.debug > 0
        fprintf('DBG:     rhonum : %e\n', rhonum);
        fprintf('DBG:     rhoden : %e\n', rhoden);
    end

    % This is always true if a linear, symmetric operator is used for the
    % Hessian (approximation) and if we had infinite numerical precision.
    % In practice, nonlinear approximations of the Hessian such as the
    % built-in finite difference approximation and finite numerical
    % accuracy can cause the model to increase. In such scenarios, we
    % decide to force a rejection of the step and a reduction of the
    % trust-region radius. We test the sign of the regularized rhoden since
    % the regularization is supposed to capture the accuracy to which
    % rhoden is computed: if rhoden were negative before regularization but
    % not after, that should not be (and is not) detected as a failure.
    model_decreased = (rhoden >= 0);

    if ~model_decreased
        srstr = [srstr ', model did not decrease']; %#ok<AGROW>
    end

    rho = rhonum / rhoden;

    if options.debug > 0
        m = @(x, eta) ...
          getCost(problem, x, storedb) + ...
          getDirectionalDerivative(problem, x, eta, storedb) + ...
          .5*problem.M.inner(x, getHessian(problem, x, eta, storedb), eta);
        zerovec = problem.M.zerovec(x);
        actrho = (fx - fx_prop) / (m(x, zerovec) - m(x, eta));
        fprintf('DBG:   new f(x) : %e\n', fx_prop);
        fprintf('DBG: actual rho : %e\n', actrho);
        fprintf('DBG:   used rho : %e\n', rho);
    end

    % Choose the new TR radius based on the model performance
    trstr = '   ';
    % If the actual decrease is smaller than 1/4 of the predicted decrease,
    % then reduce the TR radius.
    if rho < 1/4 || ~model_decreased
        trstr = 'TR-';
        Delta = Delta/4;
    % If the actual decrease is at least 3/4 of the precicted decrease and
    % the tCG (inner solve) hit the TR boundary, increase the TR radius.
    elseif rho > 3/4 && (stop_inner == 1 || stop_inner == 2)
        trstr = 'TR+';
        Delta = min(2*Delta, options.Delta_bar);
    end
    % Otherwise, keep the TR radius constant.

    % Choose to accept or reject the proposed step based on the model
    % performance.
    if model_decreased && rho > options.rho_prime
        accept = true;
        accstr = 'acc';
        x = x_prop;
        fx = fx_prop;
        [fgradx storedb] = getGradient(problem, x, storedb);
        norm_grad = problem.M.norm(x, fgradx);
    else
        accept = false;
        accstr = 'REJ';
    end

    % Make sure we don't use too much memory for the store database
    storedb = purgeStoredb(storedb, options.storedepth);

    % k is the number of iterations we have accomplished.
    k = k + 1;

    % Log statistics for freshly executed iteration.
    % Everything after this in the loop is not accounted for in the timing.
    stats = savestats(problem, x, storedb, options, k, fx, norm_grad, ...
                      Delta, info, rho, rhonum, rhoden, accept, numit, ...
                      norm_eta, used_cauchy);
    info(k+1) = stats; %#ok<AGROW>

    % ** Display:
    if options.verbosity == 2,
        fprintf(['%3s %3s   k: %5d     num_inner: %5d     ', ...
        'f: %e   |grad|: %e   %s\n'], ...
        accstr,trstr,k,numit,fx,norm_grad,srstr);
    elseif options.verbosity > 2,
        if options.useRand && used_cauchy,
            fprintf('USED CAUCHY POINT\n');
        end
		fprintf('%3s %3s    k: %5d     num_inner: %5d     %s\n', ...
				accstr, trstr, k, numit, srstr);
		fprintf('       f(x) : %e     |grad| : %e\n',fx,norm_grad);
		if options.debug > 0
			fprintf('      Delta : %f          |eta| : %e\n',Delta,norm_eta);
		end
		fprintf('        rho : %e\n',rho);
    end
    if options.debug > 0,
        fprintf('DBG: cos ang(eta,gradf): %d\n',testangle);
        if rho == 0
            fprintf('DBG: rho = 0, this will likely hinder further convergence.\n');
        end
    end

end  % of TR loop (counter: k)

% Restrict info struct-array to useful part
info = info(1:k+1);

if (options.verbosity > 2) || (options.debug > 0),
   fprintf('************************************************************************\n');
end
if (options.verbosity > 0) || (options.debug > 0)
    fprintf('Total time is %f [s] (excludes statsfun)\n', info(end).time);
end

% Return the best cost reached
cost = fx;

end

% Routine in charge of collecting the current iteration stats
function stats = savestats(problem, x, storedb, options, k, fx, ...
                           norm_grad, Delta, info, rho, rhonum, ...
                           rhoden, accept, numit, norm_eta, used_cauchy)
    stats.iter = k;
    stats.cost = fx;
    stats.gradnorm = norm_grad;
    stats.Delta = Delta;
    if k == 0
        stats.time = toc();
        stats.rho = inf;
        stats.rhonum = NaN;
        stats.rhoden = NaN;
        stats.accepted = true;
        stats.numinner = NaN;
        stats.stepsize = NaN;
        if options.useRand
            stats.cauchy = false;
        end
    else
        stats.time = info(k).time + toc();
        stats.rho = rho;
        stats.rhonum = rhonum;
        stats.rhoden = rhoden;
        stats.accepted = accept;
        stats.numinner = numit;
        stats.stepsize = norm_eta;
        if options.useRand,
          stats.cauchy = used_cauchy;
        end
    end

    % See comment about statsfun above: the x and store passed to statsfun
    % are that of the most recently accepted point after the iteration
    % fully executed.
    stats = applyStatsfun(problem, x, storedb, options, stats);

end


vI = cat(3, 85, 80, 217) / 255;

vPhotoshopValues = [30; 98; 51; 73; 5; 53];

vCoeffValues = (vPhotoshopValues - 50) ./ 50;
vO = ApplyBlackWhiteFilter(vI, vCoeffValues);
vO = max(min(vO, 1), 0);

round(255 * vI(:))
round(255 * vO(:))


function [frames, descrs] = vl_phow(im, varargin)
% VL_PHOW  Extract PHOW features
%   [FRAMES, DESCRS] = VL_PHOW(IM) extracts PHOW features [1] from the
%   image IM. PHOW is simply dense SIFT applied at several resolutions. This function is a commodity interface to VL_DSIFT() and
%   VL_IMSMOOTH().
%
%   DESCRS has the same format of VL_SIFT() and VL_DSIFT(). FRAMES(1:2,:)
%   are the x,y coordinates of the center of each descriptor, FRAMES(3,:)
%   is the contrast of the descriptor, as returned by VL_DSIFT() (for
%   colour variant, contranst is computed on the intensity channel).
%   FRAMES(4,:) is the size of the bin of the descriptor.
%
%   By default,
%   VL_PHOW() computes the gray-scale variant of the descriptor.  The
%   COLOR option can be used to compute the color variant instead.
%
%   Verbose:: false
%     Set to true to turn on verbose output.
%
%   Sizes:: [4 6 8 10]
%     Scales at which the dense SIFT features are extracted. Each
%     value is used as bin size for the VL_DSIFT() function.
%
%   Fast:: true
%     Set to false to turn off the fast SIFT features computation by
%     VL_DSIFT().
%
%   Step:: 2
%     Step (in pixels) of the grid at which the dense SIFT features
%     are extracted.
%
%   Color:: 'gray'
%     Choose between 'gray' (PHOW-gray), 'rgb', 'hsv', and 'opponent'
%     (PHOW-color).
%
%   ContrastThreshold:: 0.005
%     Contrast threshold below which SIFT features are mapped to
%     zero. The input image is scaled to have intensity range in [0,1]
%     (rather than [0,255]) and this value is compared to the
%     descriptor norm as returned by VL_DSIFT().
%
%   WindowSize:: 1.5
%     Size of the Gaussian window in units of spatial bins.
%
%   Magnif:: 6
%     The image is smoothed by a Gaussian kernel of standard deviation
%     SIZE / MAGNIF. Note that, in the standard SIFT descriptor, the
%     magnification value is 3; here the default one is 6 as it seems
%     to perform better in applications.
%
%   FloatDescriptors:: false
%     If set to TRUE, the descriptors are returned in floating point
%     format.
%
%   See also: VL_DSIFT(), VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

% -------------------------------------------------------------------
%                                                 Parse the arguments
% -------------------------------------------------------------------

  opts.verbose = false ;
  opts.fast = true ;
  opts.sizes = [4 6 8 10] ;
  opts.step = 2 ;
  opts.color = 'gray' ;
  opts.floatdescriptors = false ;
  opts.magnif = 6 ;
  opts.windowsize = 1.5 ;
  opts.contrastthreshold = 0.005 ;
  opts = vl_argparse(opts,varargin) ;

  dsiftOpts = {'norm', 'windowsize', opts.windowsize} ;
  if opts.verbose, dsiftOpts{end+1} = 'verbose' ; end
  if opts.fast, dsiftOpts{end+1} = 'fast' ; end
  if opts.floatdescriptors, dsiftOpts{end+1} = 'floatdescriptors' ; end
  dsiftOpts(end+(1:2)) = {'step', opts.step} ;

% -------------------------------------------------------------------
%                                                Extract the features
% -------------------------------------------------------------------

  % standarize the image
  imageSize = [size(im,2) ; size(im,1)] ;
  if strcmp(lower(opts.color), 'gray')
    numChannels = 1 ;
    if size(im,3) > 1, im = rgb2gray(im) ; end
  else
    numChannels = 3 ;
    if size(im,3) == 1, im = cat(3, im, im, im) ; end
    switch lower(opts.color)
      case 'rgb'
      case 'opponent'
        % Note that the mean differs from the standard definition of opponent
        % space and is the regular intesity (for compatibility with
        % the contrast thresholding).
        %
        % Note also that the mean is added pack to the other two
        % components with a small multipliers for monochromatic
        % regions.
        mu = 0.3*im(:,:,1) + 0.59*im(:,:,2) + 0.11*im(:,:,3) ;
        alpha = 0.01 ;
        im = cat(3, mu, ...
                 (im(:,:,1) - im(:,:,2))/sqrt(2) + alpha*mu, ...
                 (im(:,:,1) + im(:,:,2) - 2*im(:,:,3))/sqrt(6) + alpha*mu) ;
      case 'hsv'
        im = rgb2hsv(im) ;
      otherwise
        opts.color = 'hsv' ;
        warning('Color space not recongized, defaulting to HSV color space.') ;
    end
  end

  if opts.verbose
    fprintf('%s: color space: %s\n', mfilename, opts.color) ;
    fprintf('%s: image size: %d x %d\n', mfilename, imageSize(1), imageSize(2)) ;
    fprintf('%s: sizes: [%s]\n', mfilename, sprintf(' %d', opts.sizes)) ;
  end

  for si = 1:length(opts.sizes)

    % Recall from VL_DSIFT() that the first descriptor for scale SIZE has
    % center located at XC = XMIN + 3/2 SIZE (the Y coordinate is
    % similar). It is convenient to align the descriptors at different
    % scales so that they have the same geometric centers. For the
    % maximum size we pick XMIN = 1 and we get centers starting from
    % XC = 1 + 3/2 MAX(OPTS.SIZES). For any other scale we pick XMIN so
    % that XMIN + 3/2 SIZE = 1 + 3/2 MAX(OPTS.SIZES).
    %
    % In pracrice, the offset must be integer ('bounds'), so the
    % alignment works properly only if all OPTS.SZES are even or odd.

    off = floor(1 + 3/2 * (max(opts.sizes) - opts.sizes(si))) ;

    % smooth the image to the appropriate scale based on the size
    % of the SIFT bins
    sigma = opts.sizes(si) / opts.magnif ;
    ims = vl_imsmooth(im, sigma) ;

    % extract dense SIFT features from all channels
    for k = 1:numChannels
      [f{k}, d{k}] = vl_dsift(...
        ims(:,:,k), ...
        dsiftOpts{:},  ...
        'size', opts.sizes(si), ...
        'bounds', [off off +inf +inf]) ;
    end

    % remove low contrast descriptors
    % note that for color descriptors the V component is
    % thresholded
    switch lower(opts.color)
      case {'gray', 'opponent'}
        contrast = f{1}(3,:) ;
      case 'rgb'
        contrast = mean([f{1}(3,:) ; f{2}(3,:) ; f{3}(3,:)],1) ;
      otherwise % hsv
        contrast = f{3}(3,:) ;
    end
    for k = 1:numChannels
      d{k}(:, contrast < opts.contrastthreshold) = 0 ;
    end

    % save only x,y, and the scale
    frames{si} = [f{1}(1:3, :) ; opts.sizes(si) * ones(1,size(f{1},2))] ;
    descrs{si} = cat(1, d{:}) ;
  end
  descrs = cell2mat(descrs) ;
  frames = cell2mat(frames) ;
end


%DEMOIMAGEGRAPHS  One-line description here, please.
%
%   output = demoImageGraphs(input)
%
%   Example
%   demoImageGraphs
%
%   See also
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% INRAE - BIA Research Unit - BIBS Platform (Nantes)
% Created: 2022-04-01,    using Matlab 9.12.0.1884302 (R2022a)
% Copyright 2022 INRAE.

% create a simple 2D binary image

lx = -10:10;
ly = -10:10;
[x, y] = meshgrid(lx, ly);
img = hypot(x, y) < 8.5;

figure;
imshow(img, 'InitialMagnification', 1000);
print(gcf, 'sample_image.png', '-dpng');

% create adjacency graph

[v, e] = imageGraph(img);
figure; imshow(ones(size(img)), 'InitialMagnification', 1000);
hold on; drawGraph(v, e);
print(gcf, 'imageGraph_2d.png', '-dpng');

% create boundary graph

[v, e] = imageBoundaryGraph(img);
figure; imshow(ones(size(img)), 'InitialMagnification', 1000);
hold on; drawGraphEdges(v, e, 'Color', 'b', 'LineWidth', 2);
print(gcf, 'imageBoundaryGraph_2d.png', '-dpng');

% save into figure
print(gcf, 'image_graphs_2d.png', '-dpng');


function varargout = parseMeshData(varargin)
%PARSEMESHDATA Conversion of data representation for meshes.
%
%   MESH = parseMeshData(VERTICES, FACES)
%   MESH = parseMeshData(VERTICES, EDGES, FACES)
%   Returns the mesh info into a single structure with fields "vertices",
%   "edges" and "faces".
%
%   [VERTICES, FACES] = parseMeshData(MESH)
%   Returns the mesh info into two output variables containing coordinates
%   of vertices (as a Nv_by_3 array) and the list of vertex indices for
%   each face (either as a Nf-by-3 or Nf-by-4 int array, or as a cell
%   array).
%
%   [VERTICES, EDGES, FACES] = parseMeshData(MESH)
%   Also returns the vertex indices of each edge, as a Ne-by-2 array of
%   vertex indices.
%
%   See also
%   meshes3d, formatMeshOutput
%

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

% initialize edges to empty variable
edges = [];

% Process input arguments
switch nargin
    case 1
        % input is a data structure
        mesh = varargin{1};
        vertices = mesh.vertices;
        faces = mesh.faces;
        if isfield(mesh, 'edges')
            edges = mesh.edges;
        end

    case 2
        % input are vertices and faces
        vertices = varargin{1};
        faces = varargin{2};

    case  3
        % input are vertices, edges and faces
        vertices = varargin{1};
        edges = varargin{2};
        faces = varargin{3};

    otherwise
        error('Wrong number of arguments');
end

% returns either a struct or several variables
varargout = formatMeshOutput(nargout, vertices, edges, faces);


function [ y, symm ] = cvx_s_sparse( m, n, symm, i, j )

%CVX_S_SPARSE Matrices with a fixed sparsity pattern.

if nargin < 5,
    error( 'Sparsity structure missing.' );
elseif ~isnumeric( i ) || ~isnumeric( j ),
    error( 'Sparsity arguments must be vectors of nonnegative integers.' );
elseif any( i <= 0 ) || any( j <= 0 ) || any( i ~= floor( i ) ) || any( j ~= floor( j ) ),
    error( 'Sparsity arguments must be vectors nonnegative integers.' );
elseif numel( i ) ~= 1 && numel( j ) ~= 1 && numel( i ) ~= numel( j ),
    error( 'Sparsity arguments have incompatible size.' );
elseif any( i > m ) || any( j > n ),
    error( 'One or more indices are out of range.' );
elseif symm && m ~= n,
    error( 'Symmetric structure requires a square matrix.' );
end
i = i(:);
j = j(:);
nz = max( numel(i), numel(j) );
if symm,
    t = max(i,j);
    j = min(i,j);
    i = t;
end
[ c, cndxs ] = sort( i + ( j - 1 ) * m );
tt = [true;c(2:end)~=c(1:end-1)];
c = c(tt);
r = 1 : length(c);
if symm,
    if numel(i) > 1,
        i = i(cndxs(tt));
    end
    if numel(j) > 1,
        j = j(cndxs(tt));
    end
    tt = i ~= j;
    r = [ r , r(tt) ];
    c = [ c ; j(tt) + ( i(tt) - 1 ) * m ];
end
y = min( sparse( r, c, 1, nz, m * n ), 1 );
symm = false;

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


%% Image Warping
% A demo program shows how perspective transformation applied on an image.
% Based on a sample code from
% <http://study.marearts.com/2015/03/image-warping-using-opencv.html MareArts blog>.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.3.1/samples/cpp/warpPerspective_demo.cpp>
%

function varargout = warp_perspective_demo_gui(im)
    % load source image and set initial ROI corners (4 points clockwise)
    roi = [];
    if nargin < 1
        if true
            im = fullfile(mexopencv.root(), 'test', 'books_right.jpg');
            roi = [360 109; 532 138; 460 417; 317 338];
        else
            im = fullfile(mexopencv.root(), 'test', 'box_in_scene.png');
            roi = [243 188; 328 151; 388 294; 319 351];
        end
        img = cv.imread(im, 'Color',true);
    elseif ischar(im)
        img = cv.imread(im, 'Color',true);
    else
        img = im;
    end
    if isempty(roi)
        roi = bsxfun(@rdivide, [size(img,2) size(img,1)], ...
            [1.7 4.2; 1.15 3.32; 1.33 1.1; 1.93 1.36]);
    end

    % create the UI and hook event handlers
    h = buildGUI(img, roi);
    if nargout > 0, varargout{1} = h; end
    opts = {'Interruptible','off', 'BusyAction','cancel'};
    set(h.fig, 'CloseRequestFcn',@(~,~) delete(h.fig), ...
        'WindowKeyPressFcn',@onType, opts{:});
    set(h.fig(1), 'WindowButtonDownFcn',@onMouseDown, opts{:});

    %% Callback Functions

    function redraw()
        %REDRAW  Apply transformation using current ROI and display results

        % reapply transformation
        out1 = drawROI(img, roi);
        [out2, sz] = warpROI(img, roi);

        % show results and ajust second plot to fit image
        set(h.img(1), 'CData',out1);
        set(h.img(2), 'CData',out2, 'XData',[1 sz(1)], 'YData',[1 sz(2)]);
        set(h.ax(2), 'XLim',[0 sz(1)]+0.5, 'YLim',[0 sz(2)]+0.5);
        pos = get(h.fig(2), 'Position');
        set(h.fig(2), 'Position',[pos(1:2) sz]);
        drawnow;
    end

    function onType(~,e)
        %ONTYPE  Event handler for key press on figure

        switch e.Key
            case 'h'
                helpdlg({
                    'Use your mouse to select a point and move it'
                    'to see transformation changes.'
                    ''
                    'Hot keys:'
                    'h - this help dialog'
                    'q - quit the program'
                    'r - change order of points to rotate transformation'
                    'i - change order of points to invert transformation '
                });
            case {'q', 'escape'}
                close(h.fig);
            case 'r'
                roi = circshift(roi, -1);
                redraw();
            case 'i'
                roi = roi([2 1 4 3],:);
                redraw();
        end
    end

    function onMouseDown(~,~)
        %ONMOUSEDOWN  Event handler for mouse down on figure

        % hit-test for closest ROI corner
        pt = getCurrentPoint(h.ax(1));
        d = sum(abs(bsxfun(@minus, roi, pt)), 2);
        [mn, pt_idx] = min(d);
        if mn < 20
            % attach event handlers, and change mouse pointer
            set(h.fig(1), 'Pointer','cross', ...
                'WindowButtonMotionFcn',{@onMouseMove, pt_idx}, ...
                'WindowButtonUpFcn',@onMouseUp);
        end
    end

    function onMouseMove(~,~,idx)
        %ONMOUSEMOVE  Event handler for mouse move on figure

        % move specified ROI corner
        pt = getCurrentPoint(h.ax(1));
        roi(idx,:) = pt;
        redraw();
    end

    function onMouseUp(~,~)
        %ONMOUSEUP  Event handler for mouse up on figure

        % detach event handlers, and restore mouse pointer
        set(h.fig(1), 'Pointer','arrow', ...
            'WindowButtonMotionFcn','', ...
            'WindowButtonUpFcn','');
    end
end

%% Helper Functions

function out = drawROI(img, pts)
    %DRAWROI  Show ROI corners with labels

    labels = {'TL', 'TR', 'BR', 'BL'};
    out = cv.polylines(img, pts, 'Closed',true, 'Color',[0 0 255], 'Thickness',2);
    out = cv.circle(out, pts, 5, 'Color',[0 255 0], 'Thickness',3);
    out = cv.putText(out, labels, pts, ...
        'FontScale',0.8, 'Color',[255 0 0], 'Thickness',2);
end

function [out, dsz] = warpROI(img, srcPts)
    %WARPROI  Compute warped image from ROI

    % map ROI corners to corresponding destination rectangle
    len = diff(srcPts([1:end 1],:), 1, 1);  % [TR-TL; BR-TR; BL-BR; TL-BL]
    len = cellfun(@norm, num2cell(len, 2)); % lengths of sides
    w = max(len(1), len(3));
    h = max(len(2), len(4));
    dstPts = [0 0; w 0; w h; 0 h];

    % compute homography between points and apply persepective transformation
    dsz = round([w h]);
    H = cv.findHomography(srcPts, dstPts);
    out = cv.warpPerspective(img, H, 'DSize',dsz);
end

function p = getCurrentPoint(ax)
    %GETCURRENTPOINT  Retrieve current mouse location

    p = get(ax, 'CurrentPoint');
    p = p(1,1:2) - 1;
end

function h = buildGUI(img, roi)
    %BUILDGUI  Creates the UI

    % apply initial perspective transformation
    out1 = drawROI(img, roi);
    out2 = warpROI(img, roi);
    sz1 = size(out1);
    sz2 = size(out2);

    % properties
    fprops = {'Menubar','none', 'Resize','on'};
    aprops = {'Units','normalized', 'Position',[0 0 1 1]};
    h = struct();

    % show input image + ROI corners
    h.fig(1) = figure('Name','Image', ...
        'Position',[100 200 sz1(2) sz1(1)], fprops{:});
    h.ax(1) = axes('Parent',h.fig(1), aprops{:});
    if mexopencv.isOctave()
        h.img(1) = imshow(out1);
    else
        h.img(1) = imshow(out1, 'Parent',h.ax(1));
    end

    % show warped image
    h.fig(2) = figure('Name','Warped', ...
        'Position',[200+sz1(2) 200 sz2(2) sz2(1)], fprops{:});
    h.ax(2) = axes('Parent',h.fig(2), aprops{:});
    if mexopencv.isOctave()
        h.img(2) = imshow(out2);
    else
        h.img(2) = imshow(out2, 'Parent',h.ax(2));
    end
end


function Data = normalize_data ( Data, options )

n = length(Data);

%% noramlizing the first image
shape          = normalize_first_shape( Data(1) , options );
Data(1).shape  = shape;

%% using the first to noramlizing others.
for i = 2 : n
    [shape] = normalize_rest_shape( Data(1), Data(i), options );
    Data(i).shape  = shape;
end

end


% given 3D point set of face, draw structure of face in 3D space, or
% given 2D reprojection point set of face, draw it in corresponding image
function drawFace(KeyPointSet, showKpsIdx)
%   KeyPointSet - 66x2 or 66x3

    dim = size(KeyPointSet, 2);

    % 9 sets of keypoints(outlines) to be draw using line function
    sets = {0:16; 17:21; 22:26; 27:30; 31:35;... % face, eyebrows(r,l), nose(t,b)
        [36:41 36]; [42:47 42]; [48:59 48]; [48, 60:62, 54, 63:65, 48]}; % eyes(r,l), mouth(o,i)
    % notice: 0 indexed here

    % draw connections
    for i = 1:size(sets,1)
        indices = sets{i} + 1; % convert to 1-indexed
        if dim == 3
            line(KeyPointSet(indices, 1), KeyPointSet(indices, 2), KeyPointSet(indices, 3)...
                , 'Color', [0 150 230]/255, 'LineWidth', 3);
        elseif dim == 2
            line(KeyPointSet(indices, 1), KeyPointSet(indices, 2), 'Color', [0 150 230]/255, 'LineWidth', 2);
        end
    end

    hold on

    % draw kps
    if dim == 3
        scatter3(KeyPointSet(:,1), KeyPointSet(:,2), KeyPointSet(:,3), 500, 'r.');
        if showKpsIdx
            text(KeyPointSet(:,1), KeyPointSet(:,2), KeyPointSet(:,3), num2str((0 : 65)'));
        end
    elseif dim == 2
        scatter(KeyPointSet(:,1), KeyPointSet(:,2), 200, 'r.');
        if showKpsIdx
            text(KeyPointSet(:,1), KeyPointSet(:,2), num2str((0 : 65)'));
        end
    end
end


function datapt = lvd_AeroTasks(stateLogEntry, subTask, dummy)
%lvd_AeroTasks Summary of this function goes here
%   Detailed explanation goes here
    arguments
        stateLogEntry(1,1) LaunchVehicleStateLogEntry
        subTask(1,:) char
        dummy
    end

    ut = stateLogEntry.time;
    rVect = stateLogEntry.position;
    vVect = stateLogEntry.velocity;
    bodyInfo = stateLogEntry.centralBody;
    mass = stateLogEntry.getTotalVehicleMass();
    aero = stateLogEntry.aero;
    attState = stateLogEntry.attitude;
    celBodyData = bodyInfo.celBodyData;

    maStateLogEntry = [ut, rVect(:)', vVect(:)', bodyInfo.id, mass, 0, 0, 0, -1];
    altitude = ma_GALongLatAltTasks(maStateLogEntry, 'alt', celBodyData);
    vVectEcefMag = ma_GALongLatAltTasks(maStateLogEntry, 'bodyFixedVNorm', celBodyData);
    [lat, long, ~, ~, ~, ~, ~, ~] = getLatLongAltFromInertialVect(ut, rVect, bodyInfo, vVect);
    [density, pressureKPA, ~] = getAtmoDensityAtAltitude(bodyInfo, altitude, lat, ut, long);
    [~,angOfAttack,angOfSideslip,totalAoA] = attState.getAeroAngles(ut, rVect, vVect, bodyInfo);

    switch subTask
        case 'dragCoeff'
            datapt = stateLogEntry.aero.getDragCoeff(ut, rVect, vVect, bodyInfo, mass, altitude, pressureKPA, density, vVectEcefMag, totalAoA, angOfAttack, angOfSideslip);
        case 'dragForce'
            dragForceModel = DragForceModel();
            dragForceVect = dragForceModel.getForce(ut, rVect, vVect, mass, bodyInfo, aero, [], [], [], [], [], [], [], [], [], [], attState);
            dragForceMag = norm(dragForceVect); %mT*km/s^2
            datapt = dragForceMag*1000; %kN
    end
end


function out = calcGrainVotes(job,varargin)
% compute votes from grain boundaries
%
% Syntax
%
%   % compute votes from all p2c and c2c boundaries
%   job.calcGBVotes('threshold', 2*degree)
%
%   % compute votes only from p2c boundaries -> growth algorithm
%   job.calcGBVotes('p2c', 'threshold', 3*degree, 'tol', 1.5*degree)
%
% Input
%  job - @parentGrainReconstructor
%
% Output
%  job.votes - table of votes
%
% Options
%  p2c  - consider only parent / child grain boundaries
%  c2c  - consider only child / child grain boundaries
%  threshold - threshold fitting angle between job.p2c and the boundary OR
%  tolerance - range over which the probability increases from 0 to 1 (default 1.5)
%  numFit - number of fits to be computed
%

% noOpt = ~check_option(varargin,{'p2c','c2c'});

if nargin > 1 && isnumeric(varargin{1})
  id = varargin{1};
else
  id = job.grains.id;
end

% the original child orientation
ori = job.grainsPrior('id',id).meanOrientation;

% all variants
oriV = variants(job.p2c, ori);
numV = size(oriV,2);

% fit with neighboring grains
A = job.grains('id',id).neighbors('matrix','maxId',max(job.grains.id));
A = A(id,:);
[grainInd,nId] = find(A);
grainInd = grainInd(:); nId = nId(:);

% some neighbors coorespond to parent and some to child grains
nInd = job.grains.id2ind(nId);
isParent = job.grains.phaseId(nInd) == job.parentPhaseId;
oriP = job.grains(nInd(isParent)).meanOrientation;

isChild = job.grains.phaseId(nInd) == job.childPhaseId;
oriChildV = variants(job.p2c,job.grains(nInd(isChild)).meanOrientation);

% compute fits to all neighbors
fit = nan(length(nId),numV);
for iV = 1:numV

  % parent - parent fit
  fit(isParent,iV) = angle(oriV(grainInd(isParent),iV), oriP);

  % parent - child fit
  %fit(isChild,iV) = min(angle(oriV(grainInd(isChild),iV), oriChildV),[],2);

end

% accumulate votes, i.e. compute a probability for each grain / parentId
% combination
votes = accumVotes(repmat(grainInd,1,numV), repmat(1:numV,length(grainInd),1), fit,...
  max(grainInd), varargin{:});

% output
if nargin > 1 && isnumeric(varargin{1})
  out = votes;
else
  job.votes = votes;
  out = job;
end

end


function gr_test07 ( node_num, node_coordinates, edge_num, edge_nodes );

%*****************************************************************************80
%
%% GR_TEST07 tests GR_INCIDENCE_MATRIX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 January 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GR_TEST07:\n' );
  fprintf ( 1, '  GR_INCIDENCE_MATRIX computes the incidence matrix.\n' );

  incidence_matrix = gr_incidence_matrix ( node_num, node_coordinates, ...
    edge_num, edge_nodes );

  i4mat_print ( edge_num, node_num, incidence_matrix, ...
    '  The incidence matrix:' );

  return
end


function r8mat_print ( m, n, a, title )

%*****************************************************************************80
%
%% R8MAT_PRINT prints an R8MAT.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, real A(M,N), the matrix.
%
%    Input, string TITLE, a title to be printed.
%
  r8mat_print_some ( m, n, a, 1, 1, m, n, title );

  return
end


classdef BankAngleConstraint < AbstractConstraint
    %BankAngleConstraint Summary of this class goes here
    %   Detailed explanation goes here

    properties
        normFact = 1;
        event LaunchVehicleEvent
        eventNode(1,1) ConstraintStateComparisonNodeEnum = ConstraintStateComparisonNodeEnum.FinalState;

        lb(1,1) double = 0;
        ub(1,1) double = 0;

        evalType(1,1) ConstraintEvalTypeEnum = ConstraintEvalTypeEnum.FixedBounds;
        stateCompType(1,1) ConstraintStateComparisonTypeEnum = ConstraintStateComparisonTypeEnum.Equals;
        stateCompEvent LaunchVehicleEvent
        stateCompNode(1,1) ConstraintStateComparisonNodeEnum = ConstraintStateComparisonNodeEnum.FinalState;
    end

    methods
        function obj = BankAngleConstraint(event, lb, ub)
            obj.event = event;
            obj.lb = lb;
            obj.ub = ub;

             obj.id = rand();
        end

        function [lb, ub] = getBounds(obj)
            lb = obj.lb;
            ub = obj.ub;
        end

        function [c, ceq, value, lwrBnd, uprBnd, type, eventNum, valueStateComp] = evalConstraint(obj, stateLog, celBodyData)
            type = obj.getConstraintType();

            switch obj.eventNode
                case ConstraintStateComparisonNodeEnum.FinalState
                    stateLogEntry = stateLog.getLastStateLogForEvent(obj.event);

                case ConstraintStateComparisonNodeEnum.InitialState
                    stateLogEntry = stateLog.getFirstStateLogForEvent(obj.event);

                otherwise
                    error('Unknown event node.');
            end

            ut = stateLogEntry.time;
            rVect = stateLogEntry.position;
            vVect = stateLogEntry.velocity;
            bodyInfo = stateLogEntry.centralBody;

            [bankAng,~,~] = stateLogEntry.attitude.getAeroAngles(ut, rVect, vVect, bodyInfo);
            value = rad2deg(bankAng);

            if(obj.evalType == ConstraintEvalTypeEnum.StateComparison)
                switch obj.stateCompNode
                    case ConstraintStateComparisonNodeEnum.FinalState
                        stateLogEntryStateComp = stateLog.getLastStateLogForEvent(obj.stateCompEvent).deepCopy();

                    case ConstraintStateComparisonNodeEnum.InitialState
                        stateLogEntryStateComp = stateLog.getFirstStateLogForEvent(obj.stateCompEvent).deepCopy();

                    otherwise
                        error('Unknown event node.');
                end

                cartElem = stateLogEntryStateComp.getCartesianElementSetRepresentation();
                cartElem = cartElem.convertToFrame(stateLogEntry.centralBody.getBodyCenteredInertialFrame());
                stateLogEntryStateComp.setCartesianElementSet(cartElem);

                ut = stateLogEntryStateComp.time;
                rVect = stateLogEntryStateComp.position;
                vVect = stateLogEntryStateComp.velocity;
                bodyInfo = stateLogEntryStateComp.centralBody;

                [bankAng,~,~] = stateLogEntryStateComp.attitude.getAeroAngles(ut, rVect, vVect, bodyInfo);
                valueStateComp = rad2deg(bankAng);
            else
                valueStateComp = NaN;
            end

            [c, ceq] = obj.computeCAndCeqValues(value, valueStateComp);

            lwrBnd = obj.lb;
            uprBnd = obj.ub;

            eventNum = obj.event.getEventNum();
        end

        function sF = getScaleFactor(obj)
            sF = obj.normFact;
        end

        function setScaleFactor(obj, sF)
            obj.normFact = sF;
        end

        function tf = usesStage(obj, stage)
            tf = false;
        end

        function tf = usesEngine(obj, engine)
            tf = false;
        end

        function tf = usesTank(obj, tank)
            tf = false;
        end

        function tf = usesEngineToTankConn(obj, engineToTank)
            tf = false;
        end

        function tf = usesEvent(obj, event)
            tf = obj.event == event;
            if(obj.evalType == ConstraintEvalTypeEnum.StateComparison)
                tf = tf || obj.stateCompEvent == event;
            end
        end

        function tf = usesStopwatch(obj, stopwatch)
            tf = false;
        end

        function tf = usesExtremum(obj, extremum)
            tf = false;
        end

        function tf = canUseSparseOutput(obj)
            tf = true;
        end

        function event = getConstraintEvent(obj)
            event = obj.event;
        end

        function type = getConstraintType(obj)
            type = 'Bank Angle';
        end

%         function name = getName(obj)
%             name = sprintf('%s - Event %i', obj.getConstraintType(), obj.event.getEventNum());
%         end

        function [unit, lbLim, ubLim, usesLbUb, usesCelBody, usesRefSc] = getConstraintStaticDetails(obj)
            unit = 'deg';
            lbLim = -360;
            ubLim = 360;
            usesLbUb = true;
            usesCelBody = false;
            usesRefSc = false;
        end

        function addConstraintTf = openEditConstraintUI(obj, lvdData)
%             addConstraintTf = lvd_EditGenericMAConstraintGUI(obj, lvdData);

            output = AppDesignerGUIOutput({false});
            lvd_EditGenericMAConstraintGUI_App(obj, lvdData, output);
            addConstraintTf = output.output{1};
        end
    end

    methods(Static)
        function constraint = getDefaultConstraint(~, ~)
            constraint = BankAngleConstraint(LaunchVehicleEvent.empty(1,0),0,0);
        end
    end
end


function [V,openVariables] = branchVolume(p,openVariables)

if nargin == 1
    d = p.ub(p.branch_variables)-p.lb(p.branch_variables);
    openVariables = find(d~=0);
    d = d(openVariables);
    if any(d<0)
        % This is infeasible!
        V = 0;
    else
        V = geomean(d);
    end
else
    if ~p.feasible
        V = 0;
    else
        d = p.ub(p.branch_variables)-p.lb(p.branch_variables);
        d = d(openVariables);
        if any(d<0)
            % This has been propagated to infeasibility
            V = 0;
        else
            V = geomean(d);
        end
    end
end


% -*- INTERNAL UNDOCUMENTED FUNCTION -*-
function [geometry, msh, space, sp_mul, eigv, eigf, gnum, dofs_ornt, gnum_mul] = ...
              mp_solve_maxwell_eig_mixed1_3d (problem_data, method_data)

warning ('geopdes:obsolete','Function MP_SOLVE_MAXWELL_EIG_MIXED1_3D is obsolete. Using MP_SOLVE_MAXWELL_EIG_MIXED1 instead')

[geometry, msh, space, sp_mul, eigv, eigf, gnum, dofs_ornt, gnum_mul] = mp_solve_maxwell_eig_mixed1 (problem_data, method_data);


% run_simExampleBasisSet.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% This script is run simply by editing the input parameters and then
% clicking "Run".
%
% DESCRIPTION:
% Script to generate simulated basis spectra for all metabolites of interest
% in the human brain.  The script will generate an LCModel format .RAW file
% for each metabolite basis spectrum, which can then be passed into
% LCModel's "makebasis" function to generate a complete LCModel basis set.
%
% INPUTS:
% To run this script, edit the following parameters as desired and then click
% run:
%  lb       = linewidth (Hz)
%  np       = Spectral points
%  sw       = Spectral width (Hz)
%  Bo       = Magnetic Field Strength (Tesla)
%  te1      = First PRESS echo time, or SPECIAL echo time (ms)
%  te2      = Second PRESS echo time (if applicable) (ms).
%  seq      = Pulse sequence ('se'= SPECIAL, 'p'=press, 'st'=steam);
%  ref      = Add reference peak at 0ppm (used in LCModel, y or n);
%
% OUTPUTS:
% H2O       = Simulated water spectrum
% Ala       = Simulated alanine spectrum
% Asp       = Simulated aspartate spectrum
% PCh       = Simulated phosphocholine spectrum
% Cr        = Simulated creatine spectrum
% PCr       = Simulated phosphochreatine spectrum
% GABA      = Simulated GABA spectrum
% Gln       = Simulated glutamine spectrum
% Glu       = Simulated glutamate spectrum
% GSH       = Simulated glutathione spectrum
% Gly       = Simulated glycine spectrum
% Ins       = Simulated myo-inositol spectrum
% Lac       = Simulated Lactate spectrum
% NAA       = Simulated NAA spectrum
% Scyllo    = Simulated scyllo-inositol spectrum
% Tau       = Simulated taurine spectrum
% Asc       = Simulated ascorbate spectrum
% bHB       = Simulated beta-hydroxybutyrate spectrum
% bHG       = Simulated 2-hydroxyglutyrate spectrum
% Glc       = Simulated glucose spectrum
% NAAG      = Simulated N-acetylaspartylglutamate spectrum
% GPC       = Simulated glycerophosphocholine spectrum
% PE        = Simulated phosphoethanolamine spectrum
% Ser       = Simulated serine spectrum
% EtOH      = Simulated ethanol spectrum
%
%
% ************INPUT PARAMETERS**********************************
 lb=2;         %linewidth (Hz)
 np=8192;      %Spectral points
 sw=4000;      %Spectral width (Hz)
 Bo=7;         %Magnetic Field Strength (Tesla)
 te1=10;       %First PRESS echo time, or SPECIAL echo time (ms)
 te2=125;      %Second PRESS echo time (if applicable) (ms).
 seq='p'       %Pulse sequence ('se'= SPECIAL, 'p'=press, 'st'=steam, 'l'=laser);
 ref='n'       %Add reference peak at 0ppm (used in LCModel, y or n);
% *************END OF INPUT PARAMETERS**************************

    [RF,H2O]=sim_lcmrawbasis(np,sw,Bo,lb,'H2O',te1,te2,ref,'y',seq);
    [RF,Ala]=sim_lcmrawbasis(np,sw,Bo,lb,'Ala',te1,te2,ref,'y',seq);
    [RF,Asp]=sim_lcmrawbasis(np,sw,Bo,lb,'Asp',te1,te2,ref,'y',seq);
    [RF,PCh]=sim_lcmrawbasis(np,sw,Bo,lb,'PCh',te1,te2,ref,'y',seq);
    [RF,Cr]=sim_lcmrawbasis(np,sw,Bo,lb,'Cr',te1,te2,ref,'y',seq);
    [RF,PCr]=sim_lcmrawbasis(np,sw,Bo,lb,'PCr',te1,te2,ref,'y',seq);
    [RF,GABA]=sim_lcmrawbasis(np,sw,Bo,lb,'GABA',te1,te2,ref,'y',seq);
    [RF,Gln]=sim_lcmrawbasis(np,sw,Bo,lb,'Gln',te1,te2,ref,'y',seq);
    [RF,Glu]=sim_lcmrawbasis(np,sw,Bo,lb,'Glu',te1,te2,ref,'y',seq);
    [RF,GSH]=sim_lcmrawbasis(np,sw,Bo,lb,'GSH',te1,te2,ref,'y',seq);
    [RF,Gly]=sim_lcmrawbasis(np,sw,Bo,lb,'Gly',te1,te2,ref,'y',seq);
    [RF,Ins]=sim_lcmrawbasis(np,sw,Bo,lb,'Ins',te1,te2,ref,'y',seq);
    [RF,Lac]=sim_lcmrawbasis(np,sw,Bo,lb,'Lac',te1,te2,ref,'y',seq);
    [RF,NAA]=sim_lcmrawbasis(np,sw,Bo,lb,'NAA',te1,te2,ref,'y',seq);
    [RF,Scyllo]=sim_lcmrawbasis(np,sw,Bo,lb,'Scyllo',te1,te2,ref,'y',seq);
    [RF,Tau]=sim_lcmrawbasis(np,sw,Bo,lb,'Tau',te1,te2,ref,'y',seq);
    [RF,Asc]=sim_lcmrawbasis(np,sw,Bo,lb,'Asc',te1,te2,ref,'y',seq);
    [RF,bHB]=sim_lcmrawbasis(np,sw,Bo,lb,'bHB',te1,te2,ref,'y',seq);
    [RF,bHG]=sim_lcmrawbasis(np,sw,Bo,lb,'bHG',te1,te2,ref,'y',seq);
    [RF,Glc]=sim_lcmrawbasis(np,sw,Bo,lb,'Glc',te1,te2,ref,'y',seq);
    [RF,NAAG]=sim_lcmrawbasis(np,sw,Bo,lb,'NAAG',te1,te2,ref,'y',seq);
    [RF,GPC]=sim_lcmrawbasis(np,sw,Bo,lb,'GPC',te1,te2,ref,'y',seq);
    [RF,PE]=sim_lcmrawbasis(np,sw,Bo,lb,'PE',te1,te2,ref,'y',seq);
    [RF,Ser]=sim_lcmrawbasis(np,sw,Bo,lb,'Ser',te1,te2,ref,'y',seq);
    [RF,EtOH]=sim_lcmrawbasis(np,sw,Bo,lb,'EtOH',te1,te2,ref,'y',seq);

%LEGEND:
%   'Ala'    = Alanine
%   'Asp'    = Aspartate
%   'PCh'    = PhosphoCholine
%   'Cr'     = Creatine
%   'PCr'    = PhosphoCreatine
%   'GABA'   = Gamma-aminobutyric acid
%   'Gln'    = Glutamine
%   'Glu'    = Glutamate
%   'GSH'    = Glutathione
%   'Gly'    = Glycine
%   'Ins'    = Myo-inositol
%   'Lac'    = Lactate
%   'NAA'    = N-acetyl aspartate
%   'Scyllo' = Scyllo-inositol
%   'Tau'    = Taurine
%   'Asc'    = Ascorbate (Vitamin C)
%   'bHB'    = beta-Hydroxybutyrate
%   'bHG'    = beta-Hydroxyglutarate
%   'Glc'    = Glucose
%   'NAAG'   = N-acetyl aspartyl glutamate
%   'GPC'    = Glycero-phosphocholine
%   'PE'     = Phosphoryl ethanolamine
%   'Ser'    = Serine
%   'EtOH'   = Ethanol


function [ cluass,cluNRG ] = bz_GradDescCluster( simMat, varargin )
%[cluass] = bz_GradDescCluster(simMat) clusters recording sites given a pairwise
%similarity matrix using gradient descent to find minimize within-cluster
%interaction energy. (i.e. maximize within-cluster coherence, see Berenyi
%et al 2014 for details).
%
%INPUT
%   simMat          undirected similarity matrix (coherence, correlation, or other)
%
%   (optional parameters)
%   'numsteps'      number of steps (default: 5e5)
%   'numinit'       number of initial clusters, should be larger than the
%                   number of clusters you expect (default: 20)
%   'stopthresh'    stability threshold to stop descending. probability of
%                   selected site changing cluster identity, P(switch)
%                   (default: 0.001)
%   'stopwin'       window of time for which the P(switch) must stay below
%                   stopthresh (default: 10000 steps)
%   'showplot'      true or false, to show the update plot as it goes
%   'brainmap'      cell array with two cells: {1} = linear indices, {2} = size map; 2 dim vector x and y dimensions of map

%OUTPUT
%   cluass      cluster assignments for each
%   cluNRG      negative energy (i.e. mean within-cluster coherence) for each cluster
%
%From Berenyi et al. (2014). Large-scale, high-density (up to 512
%   channels) recording of local circuits in behaving animals. Journal of
%   Neurophysiology, 111(5), 1132?1149. http://doi.org/10.1152/jn.00785.2013
%
%Code implementation by DLevenstein 2016. Updates DL and RS May 2017.
%
% TO DO
% - make brainmap general to any 2D ephys/imaging data
% - develop method for stopping num iterations
% - add consensus clustering option?

%% Parse the input parameters
parms = inputParser;
addParameter(parms,'numsteps',500000,@isnumeric);
addParameter(parms,'numinit',20,@isnumeric);
addParameter(parms,'showplot',true,@islogical);
addParameter(parms,'brainmap',{},@iscell);
addParameter(parms,'stopthresh',0.001,@(x) x>0 && x<=1)
addParameter(parms,'stopwin',10000,@isnumeric)

parse(parms,varargin{:})
numsteps = parms.Results.numsteps;
numinit = parms.Results.numinit;
SHOWPLOT = parms.Results.showplot;
LinearInds = parms.Results.brainmap{1};
SizeVid = parms.Results.brainmap{2};
stopthresh = parms.Results.stopthresh;
stopwin = parms.Results.stopwin;

%% Initialize and Run
numsites = size(simMat,1);
rng('shuffle') %Shuffle the random number generator seed... just in case
cluass = randi(numinit,numsites,1); %Start from random initial assignments

if SHOWPLOT; figure; switchtracker = zeros(numsteps,1); end
for ss = 1:numsteps
    %The time ticker...
    if mod(ss,1000)==1
        display(['Step: ',num2str(ss),' of ',num2str(numsteps)])
        [~,clusort] = sort(cluass);

        % Visuals
        if SHOWPLOT
            % plot changing clusters over time
            subplot(2,2,1)
                imagesc(simMat(clusort,clusort))
            %plot switching behavior
            subplot(2,2,2)
                plot(log10(smooth(switchtracker(1:ss),1000,'moving')))
                hold on
                plot(get(gca,'xlim'),log10(stopthresh).*[1 1],'r--')
                xlabel('Step #');ylabel('P(switch)')
                ylim([log10(stopthresh)-1 0])
                LogScale('y',10)
                hold off
            % plot map
            if ~isempty(LinearInds)
                map = ExpandVid(cluass, LinearInds, SizeVid);
                subplot(2,2,3)
                imagesc(map)
            end;
            %plot number of clusters
            subplot(2,2,4)
                plot(ss,length(unique(cluass)),'ko') %number clusters
                hold on
                xlabel('Step #');ylabel('# Clusters')
                drawnow
        end;
    end;

    % Fx meat
    [cluass,switchtracker(ss)] = DescOneStep(cluass,simMat);

    %Decide to exit the loop - if you've been consistent for a certain window
    if mean(switchtracker(max(ss-stopwin,1):ss)) <= stopthresh
        disp('Clustering has descended to the bottom of the (local) pit!')
        break
    end
end

%Calculate -Energy (mean similarity) for each final cluster
finalclus = unique(cluass);
numfinalclus = length(finalclus);
for ff = 1:numfinalclus
    %N = sum(cluass == finalclus(ff));
    clusmat = simMat(cluass == finalclus(ff),cluass == finalclus(ff));
    cluspairs = triu(clusmat,1);
    E(ff) = mean(cluspairs(:));
end
cluNRG = [finalclus,E'];

%% FUNCTION: OneStep
    function [newcluass,diditswitch] = DescOneStep(oldcluass,simMat)
        clus = unique(oldcluass);
        numclus = length(clus);

        %Pick a random site and calculate the energy for it's current cluster
        sitepick = randi(length(oldcluass),1);
        clu_A = oldcluass(sitepick); %Cluster the current site is in
        othersites = setdiff(1:length(oldcluass),sitepick);
        N_A = sum(oldcluass == clu_A)-1; %ALl OTHER elements of current cluster
        E_A = -(1./N_A).*sum(simMat(sitepick,oldcluass(othersites)==clu_A));

        %If only one site in cluster, energy is 0
        if N_A == 0
            E_A = 0;
        end

        %Calculate energy gap for switching site to other clusters
        energygap = zeros(size(clus));
        for cc = 1:numclus
            clu_B = clus(cc);
            if clu_B==clu_A
                continue
            end
            N_B = sum(oldcluass == clu_B);
            E_B = -(1./N_B).*(sum(simMat(sitepick,oldcluass==clu_B)));
            energygap(cc) = E_A - E_B;
        end

        %Switch site to cluster with largest energy gap
        newcluass = oldcluass;
        newcluass(sitepick) = clus(energygap==max(energygap));

        %Keep Track of switching - 1 if the channel stays in its cluster
        diditswitch = clus(energygap==max(energygap))~=clu_A;
    end
end

%% FUNCTION: ExpandVid

function map = ExpandVid(cluass, LinearInds, SizeVid)

    % Intialize
    map = single(nan(SizeVid(1)*SizeVid(2),1));

    % Put tmpR values int initialized 2D matrix
    map(LinearInds) = cluass;

    % Reshape into video
    map = reshape(map,SizeVid(1),SizeVid(2));

end


%IMATCH Template matching
%
% XM = IMATCH(IM1, IM2, U, V, H, S) is the position of the matching subimage of
% IM1 (template) within the image IM2.  The template in IM1 is centred at (U,V)
% and its half-width is H.
%
% The template is searched for within IM2 inside a rectangular region, centred
% at (U,V) and whose size is a function of S.  If S is a scalar the search
% region is [-S, S, -S, S] relative to (U,V).  More generally S is a 4-vector
% S=[umin, umax, vmin, vmax] relative to (U,V).
%
% The return value is XM=[DU,DV,CC] where (DU,DV) are the u- and v-offsets
% relative to (U,V) and CC is the similarity score for the best match in the
% search region.
%
% [XM,SCORE] = IMATCH(IM1, IM2, U, V, H, S) as above but also returns a matrix
% of matching score values for each template position tested. The rows
% correspond to horizontal positions of the template, and columns the vertical
% position.  The centre element corresponds to (U,V).
%
% Example::
%  Consider a sequence of images im(:,:,N) and we find corner points in the
%  k'th image
%        corners = icorner(im(:,:,k), 'nfeat', 20);
%  Now, for each corner we look for the 11x11 patch of surrounding pixels
%  in the next image, by searching within a 21x21 region
%        for corner=corners
%           xm = imatch(im(:,:,k), im(:,:,k+1), 5, 10);
%           if xm(3) > 0.8
%               fprintf('feature (%f,%f) moved by (%f,%f) pixels)\n', ...
%                   corner.u, corner.v, xm(1), xm(2) );
%           end
%        end
%
% Notes::
% - Useful for tracking a template in an image sequence where IM1 and IM2
%   are consecutive images in a template and (U,V) is the coordinate of
%   a corner point in IM1.
% - Is a MEX file.
% - IM1 and IM2 must be the same size.
% - ZNCC (zero-mean normalized cross correlation) matching is used as the
%   similarity measure. A perfect match score is 1.0 but anything above 0.8
%   is typically considered to be a good match.
%
% See also ISIMILARITY.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.

if ~exist('imatch', 'file')
    error('you need to build the MEX version of imatch, see vision/mex/README');
end


function tc = tc_visualizeGlm_sparklines(tc);
%
% tc = tc_visualizeGlm_sparklines(tc);
%
% Visualize results of a general linear model on a time
% course, using the sparkline method. (See Edward Tufte,
% "Beautiful Evidence".)
%
% This is a test to see whether this would work.
%
% ras. 01/2007.
if notDefined('tc')
    tc = get(gcf, 'UserData');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% apply a GLM if needed
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~isfield(tc, 'glm')
    tc = tc_applyGlm(tc);
end
X = tc.glm.designMatrix;
Y = tc.wholeTc(:);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% clean up existing objects in figure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
otherAxes = findobj('Type', 'axes','Parent', tc.ui.fig);
delete(otherAxes);
otherUiControls = findobj('Type', 'uicontrol', 'Parent', tc.ui.fig);
delete(otherUiControls);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% params
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nConds = sum(tc.trials.condNums>0);
nRows = nConds + 1;
rowSize = min(.15, 1/(nRows+1)); % normalized height of rows
t = [1:length(tc.wholeTc)] .* tc.TR; % time poitns for wholeTc

lo = min(tc.wholeTc); % min and max time series values
hi = max(tc.wholeTc);

if isnumeric(tc.params.glmHRF)
    opts = {sprintf('Mean trial for conditions \n %s',num2str(tc.params.snrConds)), ...
        'Boynton gamma function', 'SPM difference-of-gammas' ...
        'Dale & Buckner ''97'};
    hrfName = opts{tc.params.glmHRF};
else
    hrfName = tc.params.glmHRF; hrfName(hrfName=='_') = ' ';
end
title({'HRF function used: ' hrfName}, 'FontWeight', 'bold')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot the data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for row = 1:nRows
   subplot('Position', [.3 1-row*rowSize .65 rowSize]);

   if row==1        % plot time series data
       plot(t, tc.wholeTc, 'k');
   else             % plot predictor
       plot(t, tc.glm.designMatrix(:,row-1) .* tc.glm.betas(:,row-1), ...
            'Color', tc.trials.condColors{row});
   end
   axis([t(1) t(end) lo hi]);
   axis off

   % label condition
   if row==1
       text(-5, [lo + .3*(hi-lo)], 'Data');
   else
       text(-5, [lo + .3*(hi-lo)], tc.trials.condNames{row});
   end
end

return


function [ vX ] = EstimateSignalSamples( vX, vH, vY, convShape, paramLambda, hSumLogProb )
% ----------------------------------------------------------------------------------------------- %
% [ mK ] = CreateConvMtx1D( vK, numElements, convShape )
% Generates a Convolution Matrix for 1D Kernel (The Vector vK) with
% support for different convolution shapes (Full / Same / Valid). The
% matrix is build such that for a signal 'vS' with 'numElements = size(vS
% ,1)' the following are equiavlent: 'mK * vS' and conv(vS, vK,
% convShapeString);
% Input:
%   - vK                -   Input 1D Convolution Kernel.
%                           Structure: Vector.
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - numElements       -   Number of Elements.
%                           Number of elements of the vector to be
%                           convolved with the matrix. Basically set the
%                           number of columns of the Convolution Matrix.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, 3, ...}.
%   - convShape         -   Convolution Shape.
%                           The shape of the convolution which the output
%                           convolution matrix should represent. The
%                           options should match MATLAB's conv2() function
%                           - Full / Same / Valid.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, 3}.
% Output:
%   - mK                -   Convolution Matrix.
%                           The output convolution matrix. The product of
%                           'mK' and a vector 'vS' ('mK * vS') is the
%                           convolution between 'vK' and 'vS' with the
%                           corresponding convolution shape.
%                           Structure: Matrix (Sparse).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References:
%   1.  MATLAB's 'convmtx()' - https://www.mathworks.com/help/signal/ref/convmtx.html.
% Remarks:
%   1.  The output matrix is sparse data type in order to make the
%       multiplication by vectors to more efficient.
%   2.  In caes the same convolution is applied on many vectors, stacking
%       them into a matrix (Each signal as a vector) and applying
%       convolution on each column by matrix multiplication might be more
%       efficient than applying classic convolution per column.
% TODO:
%   1.
%   Release Notes:
%   -   1.0.000     20/01/2019  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

CONVOLUTION_SHAPE_FULL         = 1;
CONVOLUTION_SHAPE_SAME         = 2;
CONVOLUTION_SHAPE_VALID        = 3;

switch(convShape)
    case(CONVOLUTION_SHAPE_FULL)
        convShapeString = 'full';
    case(CONVOLUTION_SHAPE_SAME)
        convShapeString = 'same';
    case(CONVOLUTION_SHAPE_VALID)
        convShapeString = 'valid';
end

vXX = vX;

hObjFun = @(vX) 0.5 * sum((conv(vX, vH, convShapeString) - vY) .^ 2) - (paramLambda * hSumLogProb(vX));

vX = fminunc(hObjFun, vX);

end