% MSH_EVALUATE_ELEMENT_LIST: evaluate the parameterization in a given list of elements.
%
%     msh_elems = msh_evaluate_element_list (msh, elements)
%
% INPUTS:
%
%    msh:          mesh object (see msh_cartesian)
%    element_list: numbering of the elements where the evaluations are performed.
%
% OUTPUT:
%
%     msh_elems: structure containing the quadrature rule in the given elements of the physical domain, which contains the following fields
%
%     FIELD_NAME         (SIZE)                  DESCRIPTION
%     npatch             (scalar)                number of patches
%     ndim               (scalar)                dimension of the parametric space
%     rdim               (scalar)                dimension of the physical space
%     nel                (scalar)                number of elements in the list
%     elem_list          (1 x nel)               numbering of the elements in the list
%     nqn                (scalar)                number of quadrature points per element (must be the same for every patch)
%     nqn_dir            (1 x ndim)              number of quadrature points in each direction (must be the same for every patch)
%     nel_per_patch      (1 x npatch)            number of selected elements on each patch
%     elem_list_of_patch (1 x npatch cell-array) selected elements on the patch, with local numbering
%     nel_dir_of_patch   (1 x npatch cell-array) the total number of elements in each direction, for each patch
%     quad_weights, geo_map, geo_map_jac, deo_map_der2, jacdet, element_size (see msh_evaluate_col for details)
%
% The function only works if the number of quadrature points is the same for all the patches and all directions.
%
% Copyright (C) 2015 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function msh_col = msh_evaluate_element_list (msh, elem_list, varargin)

  elem_list = elem_list(:)';

  msh_col.npatch = msh.npatch;
  msh_col.ndim = msh.ndim;
  msh_col.rdim = msh.rdim;

  msh_col.nel  = numel (elem_list);
  msh_col.elem_list = elem_list;

  if (isempty (elem_list)), return, end

  fields = {'quad_weights', 'geo_map', 'geo_map_jac', 'geo_map_der2', 'jacdet', 'element_size'};
  cat_position = [2 3 4 5 2 2];
  for ii = 1:numel (fields)
    msh_col.(fields{ii}) = [];
  end

  Nelem = cumsum ([0, msh.nel_per_patch]);

  indices = cell (1, msh.npatch);
  for iptc = 1:msh.npatch
    [~,indices{iptc},~] = intersect ((Nelem(iptc)+1):Nelem(iptc+1), elem_list);
  end
  active_patches = find (~cellfun (@isempty, indices));

  for iptc = active_patches
    msh_patch = msh_evaluate_element_list (msh.msh_patch{iptc}, indices{iptc});

    if (msh_patch.nqn_dir ~= msh.msh_patch{active_patches(1)}.nqn_dir)
      error ('msh_evaluate_element_list: for multipatch geometries, the number of quadrature points should be the same on each patch')
    end

    for ii = 1:numel (fields)
      if (isfield (msh_patch, fields{ii}))
        msh_col.(fields{ii}) = cat (cat_position(ii), msh_col.(fields{ii}), msh_patch.(fields{ii}));
      end
    end
  end

  msh_col.nel_per_patch = cellfun (@numel, indices);
  msh_col.elem_list_of_patch = indices;
  msh_col.nel_dir_of_patch = cell (1, msh.npatch);
  for iptc = 1:msh.npatch
    msh_col.nel_dir_of_patch{iptc} = msh.msh_patch{iptc}.nel_dir;
  end
  msh_col.nqn = msh.msh_patch{active_patches(1)}.nqn;
  msh_col.nqn_dir = msh.msh_patch{active_patches(1)}.nqn_dir;

end


function [V,F,UV,C,N] = readOFF( filename )
  % READOFF reads an OFF file with vertex/face information
  %
  % [V,F,UV,C,N] = readOFF( filename )
  %
  % Input:
  %  filename  path to .off file
  % Outputs:
  %  V  #V by 3 list of vertices
  %  F  #F by 3 list of triangle indices
  %  UV  #V by 2 list of texture coordinates
  %  C  #V by 3 list of colors
  %  N  #V by 3 list of normals
  %
  % See also: load_mesh, readOBJfast, readOBJ

% (C) 2007 Denis Kovacs, NYU
%-------------------------------------------------------------------------

  V = [];
  F = [];
  UV = [];
  C = [];
  N = [];

  fp = fopen( filename, 'r' );
  OFFheader = upper(fscanf( fp, '%s\n', 1 ));
  if OFFheader(end-2:end) ~= 'OFF'
    warning('no OFF file!')
    fclose(fp);
    return;
  end
  OFFdim = 3;
  OFF_N = 0; OFF_C=0; OFF_ST=0;

  if find(OFFheader=='N') OFFdim = OFFdim+3; OFF_N=1; end
  if find(OFFheader=='C') OFFdim = OFFdim+3; OFF_C=1; end
  if find(OFFheader=='S') OFFdim = OFFdim+2; OFF_ST=1; end

  % eat any comments before
  line = eat_comments(fp,'#');

  d = sscanf( line, '%d', 3);
  nV = d(1); nF = d(2); nE = d(3);

  %disp(sprintf('  - Reading %d vertices', nV));

  switch OFFdim
      case  3; OFFV = textscan( fp, '%f %f %f', nV);
      case  5; OFFV = textscan( fp, '%f %f %f %f %f', nV);
      case  6; OFFV = textscan( fp, '%f %f %f %f %f %f', nV);
      case  7; OFFV = textscan( fp, '%f %f %f %f %f %f %f', nV);
      case  8; OFFV = textscan( fp, '%f %f %f %f %f %f %f %f', nV);
      case  9; OFFV = textscan( fp, '%f %f %f %f %f %f %f %f %f', nV);
      case 10; OFFV = textscan( fp, '%f %f %f %f %f %f %f %f %f %f', nV);
      case 11; OFFV = textscan( fp, '%f %f %f %f %f %f %f %f %f %f %f', nV);
      otherwise; fclose(fp); error('Unsupported number of vertex entries');
  end

  try
     OFFV = cell2mat(OFFV);
  end

  OFFdim = 1;
  V = OFFV(:,OFFdim:(OFFdim+2)); OFFdim = OFFdim + 3;
  if (OFF_N) N = OFFV(:,OFFdim:(OFFdim+2)); OFFdim = OFFdim + 3; end
  if (OFF_C) C = OFFV(:,OFFdim:(OFFdim+2)); OFFdim = OFFdim + 3; end
  if (OFF_ST) UV = OFFV(:,OFFdim:(OFFdim+1)); OFFdim = OFFdim + 2; end

  if (nF ~= 0)
    %disp(sprintf('  - Reading %d faces', nF));
    temp = textscan( fp, '%d %d %d %d %d %d %d %d %d %d %d', nF );
    sz = temp{1}(1);
    if all(sz == cell2mat(temp(1)))
      F = double (cell2mat( temp(2:sz+1 ))) +1;
    else
      warning('Trivially triangulating high degree facets');
      F = zeros(sum(temp{1}-2),3);
      fi = 1;
      for f = 1:size(temp{1},1)
        for j = 3:temp{1}(f);
          F(fi,:) = [temp{2}(f) temp{j}(f) temp{j+1}(f)]+1;
          fi = fi+1;
        end
      end
    end
  else
    F = [];
  end

  fclose(fp);

  %disp('  - done.');
end


% IAIF Glottal Inverse Filtering
%   [g,dg,a,ag] = iaif(x,fs,p_vt,p_gl,d,hpfilt)
%
% Description
%   This function estimates vocal tract linear prediction coefficients and
%   the glottal volume velocity waveform from a speech signal frame using
%   Iterative Adaptive Inverse Filtering (IAIF) method.
%
% Inputs
%   x      : Speech signal frame [samples]
%   fs     : Sampling frequency [Hz]
%   p_vt   : Order of LPC analysis for vocal tract
%   p_gl   : Order of LPC analysis for glottal source
%   d      : Leaky integration coefficient (e.g. 0.99)
%   hpfilt : High-pass filter flag (0: do not apply, 1...N: apply N times)
%
% Outputs
%   g      : Glottal volume velocity waveform
%   dg     : Glottal volume velocity derivative waveform
%   a      : LPC coefficients of vocal tract
%   ag     : LPC coefficients of source spectrum
%
% Notes
%   This function does not perform pitch synchronous analysis. This ensures
%   the robustness of the method regardless of the GCI estimation
%   performance.
%
% Example
%   Simplest, type g = iaif(x,fs) to estimate the glottal flow of a speech
%   frame.
%
% References
%  [1] P. Alku, "Glottal wave analysis with pitch synchronous iterative
%      adaptive inverse filtering", Speech Communication, vol. 11, no. 2-3,
%      pp. 109–118, 1992.
%
% Copyright (c) 2013 Aalto University
%
% License
%  This file is under the LGPL license,  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. This file 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.
%
% This function is part of the Common Speech Processing Repository
% http://covarep.github.io/covarep/
%
% Octave Compatible
%
% Author
%  Tuomo Raitio tuomo.raitio@aalto.fi
%
% $Id <info set by the versioning system> $

function [g,dg,a,ag,hpfilter_out] = iaif(x,fs,p_vt,p_gl,d,hpfilt,hpfilter_in)

% Set default parameters
if nargin < 7
    hpfilter_in = [];
    if nargin < 6
        hpfilt = 1;
        if nargin < 5
            d = 0.99;
            if nargin < 4
                p_gl = 2*round(fs/4000);
                if nargin < 3
                    p_vt = 2*round(fs/2000)+4;
                    if nargin < 2
                        disp('Error: Not enough input arguments.');
                    end
                end
            end
        end
    end
end
preflt = p_vt+1;

% Ensure column vector form
x = x(:);

% High-pass filter speech in order to remove possible low frequency
% fluctuations (Linear-phase FIR, Fc = 70 Hz)
hpfilter_out = [];
if hpfilt > 0
    Fstop = 40;                 % Stopband Frequency
    Fpass = 70;                 % Passband Frequency
    Nfir = round(300/16000*fs); % FIR numerator order
    if mod(Nfir,2) == 1
        Nfir = Nfir + 1;
    end

    % it is very very expensive to calculate the firls filter! However, as
    % long as the fs does not change, the firls filter does not change.
    % Therefore, the computed filter is returned and can be passed to this
    % function later on to avoid the calculated of the (same) filter.
    if ~isempty(hpfilter_in)
        B = hpfilter_in;
    else
        B = hpfilter_fir(Fstop,Fpass,fs,Nfir);
    end
    hpfilter_out = B;

    for i = 1:hpfilt
        x = filter(B,1,[x ; zeros(round(length(B)/2)-1,1)]);
        x = x(round(length(B)/2):end);
    end
end

% Estimate the combined effect of the glottal flow and the lip radiation
% (Hg1) and cancel it out through inverse filtering. Note that before
% filtering, a mean-normalized pre-frame ramp is appended in order to
% diminish ripple in the beginning of the frame. The ramp is removed after
% filtering.
if length(x)>p_vt
    win = hanning(length(x));
    signal = [linspace(-x(1),x(1),preflt)' ; x];
    idx = preflt+1:numel(signal);

    Hg1 = lpc(x.*win,1);
    y = filter(Hg1,1,signal);
    y = y(idx);

    % Estimate the effect of the vocal tract (Hvt1) and cancel it out through
    % inverse filtering. The effect of the lip radiation is canceled through
    % intergration. Signal g1 is the first estimate of the glottal flow.
    Hvt1 = lpc(y.*win,p_vt);
    g1 = filter(Hvt1,1,signal);
    g1 = filter(1,[1 -d],g1);
    g1 = g1(idx);

    % Re-estimate the effect of the glottal flow (Hg2). Cancel the contribution
    % of the glottis and the lip radiation through inverse filtering and
    % integration, respectively.
    Hg2 = lpc(g1.*win,p_gl);
    y = filter(Hg2,1,signal);
    y = filter(1,[1 -d],y);
    y = y(idx);

    % Estimate the model for the vocal tract (Hvt2) and cancel it out through
    % inverse filtering. The final estimate of the glottal flow is obtained
    % through canceling the effect of the lip radiation.
    Hvt2 = lpc(y.*win,p_vt);
    dg = filter(Hvt2,1,signal);
    g = filter(1,[1 -d],dg);
    g = g(preflt+1:end);
    dg = dg(idx);

    % Set vocal tract model to 'a' and glottal source spectral model to 'ag'
    a = Hvt2;
    ag = Hg2;
else g=[];
    dg=[];
    a=[];
    ag=[];
    disp('IAIF - frame not analysed!!')
end

function B = hpfilter_fir(Fstop,Fpass,fs,N)
% FIR least-squares Highpass filter design using the FIRLS function
%
% Tuomo Raitio
% 20.8.2013

% Calculate the coefficients using the FIRLS function.
B  = firls(N, [0 Fstop Fpass fs/2]/(fs/2), [0 0 1 1], [1 1]);


%kMulWave 'multiplies each element with exp(-ikx) '
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros MulWave.pane file
%
% Parameters:
% InputFile: i 'Input ', required: 'First Input data object'
% InputFile: OrderFactors 'Order Factors', optional: 'a matrix which supplies float value order factors (positive or negative)'
% Integer: nx 'OrdersX', default: 2: 'number of orders along x (0 .. +- OrdersX)'
% Integer: ny 'OrdersY', default: 0: 'number of orders along y (0 .. +- OrdersY)'
% Toggle: auto 'Determine Grid automatically', default: 0: 'if selected, the grid values are ignored and the grid position, direction and shift is determined from the first order in X and Y'
% Integer: aorderx 'UseOrder X #', default: 1: 'which order to use for shift determination'
% Integer: aordery 'Y', default: 1: 'which y order to use for shift determination'
% Double: angle 'Angle', default: 0: 'angle, with which the pattern is oriented to the X-Axis'
% Double: periodx 'Grid Spacing X', default: 4: 'number of pixels from grid to grid element'
% Double: periody 'Grid Spacing Y', default: 4: 'number of pixels from grid to grid element'
% Toggle: autom 'Determine Gridshift automatically', default: 0: 'The grid zero position will be determined automatically'
% Double: mx 'Grid shift X [rad]', default: 0: 'exp(-i val) will be applied to first order peak in X'
% Double: my 'Y [rad]', default: 0: 'exp(-i val) will be applied to first order peak in Y'
% Double: of 'Order Enhancement Factor', default: 1: 'Will determine the enhancement of higher orders in the sum'
% Double: zw 'Zero Order Weight', default: 1: 'if 1.0, the zero order will not have and ignore radius, if ==0.0 the zero order will be ignored in the center like all other orders'
% Double: ir 'Ignore Zero Radius', default: 0: 'The reconstructed zero peak in each order is ignored by weigthing with a certain gaussian radius'
% Double: ar 'Apodize Radius', default: 0: 'relative max. frequency to apodize to by a cos window (linear edge), see Gabor 1946. If this is zero, no apodization will be performed and the orders will be plainly added'
% Double: ary 'Apo Radius Y', default: 0.9: 'If given an anisotropic apodization will be performed'
% Double: kxmax 'Order Kmax X', default: 0.5: 'This defines the border of each reconstructed peak, as a relative frequency'
% Double: kymax 'Order Kmax Y', default: 0.5: 'This defines the border of each reconstructed peak, as a relative frequency'
% OutputFile: o 'Output', required: 'Resulting output data object'
% OutputFile: sum 'Sum', required: 'The sum of all elements (reconstructed Fourier-components) will be stored here'
% OutputFile: pat 'Patterns', optional: 'If given, patterns will be generated for each class of distances and saved (each class along depth), moved patterns along elements.'
% Integer: PStepsX 'Pattern Steps X', default: 3: 'Number of steps along X for pattern movement'
% Integer: PStepsY 'Y', default: 3: 'Number of steps along Y for pattern movement'
%
% Example: [o, sum, pat] = kMulWave({i, OrderFactors}, {'i','';'OrderFactors','';'nx',2;'ny',0;'auto',0;'aorderx',1;'aordery',1;'angle',0;'periodx',4;'periody',4;'autom',0;'mx',0;'my',0;'of',1;'zw',1;'ir',0;'ar',0;'ary',0.9;'kxmax',0.5;'kymax',0.5;'o','';'sum','';'pat','';'PStepsX',3;'PStepsY',3})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% MulWave - multiplies each element with exp(-ikx)
%
%  DESCRIPTION
%
%
%
%  EXAMPLES
%
%  "SEE ALSO"
%
%  RESTRICTIONS
%
%  REFERENCES
%
%  COPYRIGHT
% Copyright (C) 1996-2003, Rainer Heintzmann,  All rights reserved.
%

function varargout = kMulWave(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kMulWave(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'i', '__input';'OrderFactors', '__input';'nx', 2;'ny', 0;'auto', 0;'aorderx', 1;'aordery', 1;'angle', 0;'periodx', 4;'periody', 4;'autom', 0;'mx', 0;'my', 0;'of', 1;'zw', 1;'ir', 0;'ar', 0;'ary', 0.9;'kxmax', 0.5;'kymax', 0.5;'o', '__output';'sum', '__output';'pat', '__output';'PStepsX', 3;'PStepsY', 3};
maxval={0,1,1,1,0,1,1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,1,1};
minval={0,1,1,1,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1};
istoggle=[0,1,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,1,0,0];
was_set=istoggle * 0;
paramtype={'InputFile','InputFile','Integer','Integer','Toggle','Integer','Integer','Double','Double','Double','Toggle','Double','Double','Double','Double','Double','Double','Double','Double','Double','OutputFile','OutputFile','OutputFile','Integer','Integer'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=2; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(1,1);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
[varargout{:}]=callKhoros([w 'mulwave"  -k'],Inputs,narglist);


clf; hold on
K = 10;
color = .7*[1 1 1];
dx = .9;
binedges = (0:5*K)+.5;
for ki = 1:K
    Data = poissrnd(2*ki,1,200);
    xpos = ki;
    p = boxyviolin(Data, binedges, xpos,dx, color)
    Data = randn(1,200)+3*K;
    xpos = ki;
    p = boxyviolin(Data, binedges, xpos,dx, [1 .8 .8])
end
shg


%
%   This .m file creates a rectangular grid and calculates the
%   cumulative number curve at each grid point. The grid is
%   saved in the file "cumugrid.mat".
%                        Operates on catalogue "a"
%
% define size of the area
%
% ________________________________________________________________________
%  Please use the left mouse button or the cursor to select the lower
%  left corner of the area of investigation. Please use the left
%  mouse again to select the upper right corner. The calculation might take
%  some time. This time can be reduced by using a smaller area and/or
%  a larger grid-spacing! The amount of calculation done will be displayed
%  in percent of the total time.
%
%_________________________________________________________________________

report_this_filefun(mfilename('fullpath'));

selgp

itotal = length(newgri(:,1));
if length(gx) < 2  ||  length(gy) < 2
    errordlg('Selection too small! (not a matrix)');
    return
end

close(gpf)
zmap_message_center.clear_message();;think
%  make grid, calculate start- endtime etc.  ...
%
t0b = min(a.Date)  ;
n = a.Count;
teb = a(n,3) ;
tdiff = round((teb - t0b)*365/par1);
cumu = zeros(length(t0b:par1/365:teb)+2);
ncu = length(cumu);
cumuall = zeros(ncu,length(newgri(:,1)));
loc = zeros(3,length(newgri(:,1)));

% loop over  all points
%
i2 = 0.;
i1 = 0.;
allcount = 0.;
%
% loop for all grid points
wai = waitbar(0,' Please Wait ...  ');
set(wai,'NumberTitle','off','Name','Makegrid - Percent completed');;
drawnow

x = newgri(:,1);y = newgri(:,2);
% calculate distance from center point and sort wrt distance
%
lea = a.Count;
lex = length(x(:,1));
al  = a.Longitude * ones(1,lex);
ala = a.Latitude * ones(1,lex);
alt = a.Date * ones(1,lex);
al  = (al' - x * ones(1,lea)) * cos(pi/180*mean(y)*111);
ala  = (ala' - y * ones(1,lea)) * 111;
l = sqrt(al.^2 + ala.^2);
[s,is] = sort(l);
l2 = is <= ni;
alt = reshape(alt(l2),ni,lea);
cumuall = [histogram(alt,t0b:par1/365:teb) ];

%cumuall(:,allcount) = [cumu';  x; l(ni,:)];
loc= [x ; y; s(:,ni)];

waitbar(allcount/itotal)

%
% save data
%
%  set(txt1,'String', 'Saving data...')
close(wai)
drawnow
%save cumugrid.mat cumuall par1 ni dx dy gx gy tdiff t0b teb loc

catSave3 =...
    [ 'zmap_message_center.set_info(''Save Grid'',''  '');think;',...
    ' [file1,path1] = uiputfile(fullfile(hodi, ''eq_data'', ''*.mat''), ''Grid Datafile'');',...
    ' sapa2 = [''save '' path1 file1 '' x y tmpgri newgri xvect yvect ll cumuall par1 ni dx dy gx gy tdiff t0b teb loc a main faults mainfault coastline''];',...
    ' if length(file1) > 1, eval(sapa2),end , done'];

eval(catSave3)

watchoff
zmapmenu
return

figure_w_normalized_uicontrolunits(mess)
clf
set(gca,'visible','off')

te = text(0.01,0.95,'The cumulative number array \newlinehas been saved in \newlinefile cumugrid.mat \newlinePlease rename it \newlineto protect if from overwriting.');
set(te,'FontSize',ZmapGlobal.Data.fontsz.m,'FontWeight','bold')

uicontrol('Units','normal','Position',...
    [.7 .10 .2 .12],'String','Options ', 'Callback','zmapmenu')

uicontrol('Units','normal','Position',...
    [.3 .10 .2 .12],'String','Back', 'Callback','welcome')


function varargout = sound_field_mono_wfs(X,Y,Z,xs,src,f,conf)
%SOUND_FIELD_MONO_WFS sound field for WFS
%
%   Usage: [P,x,y,z,x0] = sound_field_mono_wfs(X,Y,Z,xs,src,f,conf)
%
%   Input parameters:
%       X           - x-axis / m; single value or [xmin,xmax] or nD-array
%       Y           - y-axis / m; single value or [ymin,ymax] or nD-array
%       Z           - z-axis / m; single value or [zmin,zmax] or nD-array
%       xs          - position of virtual source / m
%       src         - source type of the virtual source
%                         'pw' - plane wave (xs is the direction of the
%                                plane wave in this case)
%                         'ps' - point source
%                         'fs' - focused source
%       f           - monochromatic frequency / Hz
%       conf        - configuration struct (see SFS_config)
%
%   Output parameters:
%       P           - simulated sound field
%       x           - corresponding x values / m
%       y           - corresponding y values / m
%       z           - corresponding z values / m
%       x0          - active secondary sources / m
%
%   SOUND_FIELD_MONO_WFS(X,Y,Z,xs,src,f,conf) simulates a monochromatic sound
%   field for the given source type (src) synthesized with wave field synthesis
%   for the frequency f.
%
%   To plot the result use:
%   plot_sound_field(P,X,Y,Z,x0,conf);
%   or simple call the function without output argument:
%   sound_field_mono_wfs(X,Y,Z,xs,src,f,conf)
%
%   See also: plot_sound_field, sound_field_imp_wfs, driving_function_mono_wfs

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 7;
nargmax = 7;
narginchk(nargmin,nargmax);
isargxs(xs);
isargpositivescalar(f);
isargchar(src);
isargstruct(conf);

%% ===== Configuration ==================================================
if strcmp('2D',conf.dimension)
    greens_function = 'ls';
else
    greens_function = 'ps';
end

%% ===== Computation ====================================================
% Get the position of the loudspeakers and its activity
x0 = secondary_source_positions(conf);
x0 = secondary_source_selection(x0,xs,src);
x0 = secondary_source_tapering(x0,conf);
% Driving function
D = driving_function_mono_wfs(x0,xs,src,f,conf);
% Wave field
[varargout{1:min(nargout,4)}] = ...
    sound_field_mono(X,Y,Z,x0,greens_function,D,f,conf);
% Return secondary sources if desired
if nargout==5, varargout{5}=x0; end


function [patches, pdm, clmParams, early_term_params] = Load_CECLM_general()
%LOAD_PATCH_EXPERTS Summary of this function goes here
%   Detailed explanation goes here

    % Load the patch experts/local detectors
    [patches] = Load_CECLM_Patch_Experts( '../models/cen/', 'cen_patches_*_general.mat');

    % the default PDM to use
    pdmLoc = ['../models/pdm/pdm_68_aligned_wild.mat'];

    load(pdmLoc);

    pdm = struct;
    pdm.M = double(M);
    pdm.E = double(E);
    pdm.V = double(V);

    clmParams = struct;

    clmParams.window_size = [25,25; 23,23; 21,21; 21,21];
    clmParams.numPatchIters = size(clmParams.window_size,1);

    clmParams.regFactor = 0.9*[35, 27, 20, 20];
    clmParams.sigmaMeanShift = 1.5*[1.25, 1.375, 1.5, 1.5];
    clmParams.tikhonov_factor = [2.5, 5, 7.5, 7.5];

    clmParams.startScale = 1;
    clmParams.num_RLMS_iter = 10;
    clmParams.fTol = 0.01;
    clmParams.useMultiScale = true;
    clmParams.use_multi_modal = 1;
    clmParams.multi_modal_types  = patches(1).multi_modal_types;
    clmParams.numPatchIters = 4;

    % As the orientations are not equally reliable reweigh them
    load('../models/cen/cen_general_mapping.mat');
end


function denoised = WNNM_WRAP(image_path, sigma)

    N_Img = double(rgb2gray(imread(image_path)));

    Par   = ParSet(sigma);
    Denoised_Image = WNNM_DeNoising(N_Img, Par);

    recombinedRGBImage = cat(3, Denoised_Image, Denoised_Image, Denoised_Image);

    denoised = uint8(recombinedRGBImage);

end


function ykplus1 = sparseGalerkinControl_Discrete(t,y,u,p)

if isfield(p,'SelectVars')==0
    p.SelectVars = 1:length(y);
end
polyorder = p.polyorder;
usesine = p.usesine;
ahat = p.ahat;
yPool = poolData([y(p.SelectVars)' u],length(p.SelectVars)+1,polyorder,usesine);
ykplus1 = (yPool*ahat)';


function [varargout] = stdDevDose(planC, varargin)
%Calculate the std dev of the dose (non-variance corrected).
%Stand alone metric :       stdDevDose(planC, structNum, doseNum);
%Request m object   :       stdDevDose(planC, 'getnewmetric');
%Evaluate m object  :       stdDevDose(planC, m, 'evaluate');
%LM:  6 Oct 06, JOD: new metric.
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

indexS = planC{end};
optS = planC{indexS.CERROptions};

if(nargin > 1)
    call = varargin{end};
else
    warning('Incorrect Usage, try: stdDevDose(planC, structNum, doseNum);');
    return;
end

switch upper(call)
    case 'GETNEWMETRIC'
        m.name = 'stdDevDose';
        m.valueV = [];
        m.description = 'Returns the standard deviation (non-variance corrected) dose in specified structure';
        m.functionName = @stdDevDose;
        m.note = '';
        m.params(1).name = 'Structure';
        m.params(1).type = 'DropDown';
        m.params(1).list = {planC{indexS.structures}.structureName};
        m.params(1).value = 1;
        m.params(2).name = 'IsTarget?';
        m.params(2).type = 'DropDown';
        m.params(2).list ={'Target', 'Not Target'};
        m.params(2).value = 2;
        m.units = [];
        m.range = [0 inf];
        m.doseSets = [1];
        varargout = {planC,m};
        return;

    case 'EVALUATE'
        m = varargin{1};
        structName = planC{indexS.structures}(m.params(1).value).structureName;
        m.note = structName;
        m.valueV = [];

        maxD = -inf;
        for i=1:length(m.doseSets)
            [planC, doseBinsV, volsHistV] = getDVHMatrix(planC, m.params(1).value, m.doseSets(i));
            ans = calc_stdDevDose(doseBinsV, volsHistV);
            m.valueV = [m.valueV ans];
            maxD = max(maxD, max(planC{indexS.dose}(m.doseSets(i)).doseArray(:)));
        end
        if m.params(2).value == 1
            %Is target, high dose is best.
            m.range = [0 max(m.valueV)];
        elseif m.params(2).value == 2
            m.range = [maxD min(m.valueV)];
        end
        m.units = getDoseUnitsStr(i,planC);
        varargout = {planC,m};
        return;

    otherwise  %No specific call has been made, assume user just wants raw metric.
        if(nargin == 3)
            structNum = varargin{1};
            doseNum = varargin{2};
            [planC, doseBinsV, volsHistV] = getDVHMatrix(planC, structNum, doseNum);
            varargout = {calc_stdDevDose(doseBinsV, volsHistV)};
        else
            error('Not enough parameters, try: stdDevDose(planC, structNum, doseNum);')
            return;
        end
end


function c = unifytracelengths(c)
   %unifytracelengths   make all traces the same length (mode length)
   %   C = unifytracelengths(C) if traces are of unequal lengths, then they
   %   are either trimmed or zero-padded to the same size.  The final length
   %   will be the mode length of the original set of traces.
   %
   %   See also mode

   if isempty(c.traces)
      return
   end

   lengths = [c.traces.nsamples];
   ideal = mode(lengths);

   tooShort = lengths < ideal;
   tooLong = lengths > ideal;

   for i = 1:numel(lengths)
      if tooShort(i)
         c.traces(i).data(ideal) = 0;
      elseif tooLong(i)
         c.traces(i).data = c.traces(i).data(1:ideal);
      end
   end
end


%SerialLink.perturb Perturb robot parameters
%
% RP = R.perturb(P) is a new robot object in which the dynamic parameters (link
% mass and inertia) have been perturbed.  The perturbation is multiplicative so
% that values are multiplied by random numbers in the interval (1-P) to (1+P).
% The name string of the perturbed robot is prefixed by 'P/'.
%
% Useful for investigating the robustness of various model-based control
% schemes. For example to vary parameters in the range +/- 10 percent is:
%    r2 = p560.perturb(0.1);
%
% See also SerialLink.rne.

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

function  r2 = perturb(r, p)

	if nargin == 1
		p = 0.1;	% 10 percent disturb by default
	end

    r2 = SerialLink(r);

    links = r2.links;
	for i=1:r.n
		s = (2*rand-1)*p + 1;
		links(i).m = links(i).m * s;

		s = (2*rand-1)*p + 1;
		links(i).I = links(i).I * s;
	end

	r2.name = ['P/' r.name];


function [varargout]=pillowHex(E,V,C,shrinkFactor)

%Defive core element
[E_core,V_core]=scalePatch(E,V,shrinkFactor);

Vp=[V;V_core]; %Collect nodes
E_core=E_core+size(V,1); %Fix node indices in E_core

%% FORMAT FOR FACES
indTop=1:1:4;
indBottom=5:1:8;
indFront=[2 3 7 6];
indBack=[5 8 4 1];
indSide1=[1 2 6 5];
indSide2=[3 4 8 7];

%% THE TOP ELEMENT
%   top-of-core top-of-outer
E1=[E_core(:,indTop) E(:,indTop)];

%% THE BOTTOM ELEMENT
%   bottom-of-core bottom-of-outer
E2=[E_core(:,indBottom) E(:,indBottom)];

%% THE FRONT ELEMENT
%   bottom=front of E2 top=front of E1
E3=[E2(:,indFront) E1(:,indFront)];

%% THE BACK ELEMENT
%   bottom=back of E2 top=back of E1
E4=[E2(:,indBack) E1(:,indBack)];

%% THE SIDE1 ELEMENT
%   bottom=side1 of E2 top=side1 of E1
E5=[E2(:,indSide1) E1(:,indSide1)];

%% THE SIDE2 ELEMENT
%   bottom=side2 of E2 top=side2 of E1
E6=[E2(:,indSide2) E1(:,indSide2)];

%% Gather element sets
Ep=[E_core; E1; E2; E3; E4; E5; E6];
Cp=repmat(C,[7,1]);

%% Collect output
varargout{1}=Ep;
varargout{2}=Vp;
varargout{3}=Cp;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


% fmri_data.subs

% This is different from fmri_data.apply_parcellation, which returns
% averages or weighted averages per image for a set of images in a data
% object.  apply_parcellation can be used to return local pattern responses
% for any parcellation, but it does not return vectors with the local
% weights themselves.

% expand into atlas subregions
% Take an image object (one image) and an integer-valued parcellation/atlas whose values
% indicate k regions, and expand the image object into a set of k images with
% the original values only for voxels in the kth region, and zero
% elsewhere. Useful for breaking a pattern mask into local atlas-defined subregions.

% define target image obj to sample to : nps

nps = replace_empty(nps); % just in case
target_obj = nps;  % this will be expanded to set of images

orig_image_vector = target_obj.dat(:, 1);

parcellation_file = which('BN_Atlas_274_noCb_uint16.nii');
parcel_obj = fmri_data(parcellation_file);

parcel_obj = resample_space(parcel_obj, nps);

names_file = fullfile(fileparts(parcellation_file), 'cluster_names.mat');
clear names
load(names_file, 'names');
names;      % returns names var. make sure: break if not

% Find which names match
%wh = ~cellfun(@isempty, strfind(names, string_to_find));

% Index values to look for
% indx = find(wh)';

%% Do expansion
% -----------------------------------------------------------------------
for indx = 1:length(names)

    output_obj = select_voxels_by_value(parcel_obj, indx);

    target_subregion_vector = orig_image_vector; % will become image values only in region, 0 elsewhere
    target_subregion_vector(output_obj.removed_voxels) = 0;

    target_obj.dat(:, indx) = target_subregion_vector;

end

% Add image for everything not already covered
% *******

% Remove empty images, attach names to object

empty_imgs = sum(abs(target_obj.dat)) < 10*eps;
target_obj.dat(:, empty_imgs) = [];

names(empty_imgs) = [];
target_obj.additional_info = names;

%% Get region object, r
% May have different length (n regions) because of contiguity requirement
% -----------------------------------------------------------------------

dat_wh = get_wh_image(target_obj, 1);
r = region(dat_wh);

region_names = repmat(names(1), 1, length(r));

for i = 2:size(target_obj.dat, 2)

    dat_wh = get_wh_image(target_obj, i);

    % threshold at 20% probability, get rid of stray voxels
    %dat_wh = threshold(dat_wh, [.2 Inf], 'raw-between', 'k', 3);

    r_wh = region(dat_wh);

    for j = 1:length(r_wh)
        [r_wh(j).title, r_wh(j).shorttitle] = deal(names{i});
    end

    r = [r r_wh];

    region_names = [region_names repmat(names(i), 1, length(r_wh))];

end
%%

% similarity_output = canlab_pattern_similarity(dat, pattern_weights, varargin)


classdef ConstantDetectionProbabilityX < DetectionModelX
% ConstantDetectionProbabilityX class
%
% Summary of ConstantDetectionProbabilityX
% This is a class implementation of a detection model, described by a Poisson
% distributed false alarm rate and a Uniform spatial distribution.
%
% ConstantDetectionProbabilityX Properties:
%   + DetectionProbability - The detection probability.
%
% ConstantDetectionProbabilityX Methods:
%   + pdf       - Function to evaluate the instensity p(b_t) at a given point in
%                 the state space.
%
% (+) denotes puplic properties/methods
%
%  February 2018 Lyudmil Vladimirov, University of Liverpool

    properties
        DetectionProbability
    end

    methods (Access = protected)
        function initialise_(this, config)
            if(isfield(config,'DetectionProbability'))
                this.DetectionProbability = config.DetectionProbability;
            end
        end
    end

    methods
        function this = ConstantDetectionProbabilityX(varargin)
        % ConstantDetectionProbabilityX Constructor method
        %
        % Parameters
        % ----------
        % DetectionProbability: scalar
        %   The detection probability over the search space.
        %
        % See also pdf.

            % Call SuperClass method
            this@DetectionModelX();

            % First check to see if a structure was received
            if(nargin==1)
                if(isstruct(varargin{1}))
                    config = varargin{1};
                    this.initialise_(config);
                    return;
                end
            end

            % Otherwise, fall back to input parser
            parser = inputParser;
            parser.KeepUnmatched = true;
            parser.parse(varargin{:});
            config = parser.Unmatched;
            this.initialise_(config);

        end

        function int = pdf(this, varargin)
        % pdf Evaluates the detection intensity at given points xk of the state space
        %
        % Parameters
        % ----------
        % xk: matrix
        %   A (NumStateDims x Ns) matrix, whose columns correspond to Ns individual state vectors.
        %
        % Returns
        % -------
        % prob: matrix
        %   A (1 x Ns) matrix, where :math:`prob(i,j)=p(D_k^i|x_k^j)

            Ns = 1;
            if(nargin>1)
                xk = varargin{1};
                Ns = size(xk,2);
            end
            int = this.DetectionProbability*ones(1,Ns);
            x = [-3422.85261310741,-3191.58441229017,-2993.55608122595,-3228.08558459284, -3422.85261310741];
            y = [1.472966334316646e+03,1.123217741935625e+03,1.243952901078573e+03,1.579667558371468e+03, 1.472966334316646e+03];
            a = find(inpolygon(xk(1,:),xk(3,:),x,y));
            if numel(a)>0
                int(a) = 0.1;
            end
        end
    end
end


function density = p14_density ( m, n, point )

%*****************************************************************************80
%
%% P14_DENSITY returns the density for problem 14.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real POINT(M,N), the coordinates of the points.
%
%    Output, real DENSITY(N), the mesh density at each point.
%
  density = ones ( 1, n );

  return
end


addpath('../toolbox/');
addpath('../image_blur/');
addpath('../blur_functions//');
addpath('../convolutional_wasserstein/');
addpath('../../data/images/shapes/'); % low-res images

rep = '../results/shapes/';
if not(exist(rep))
    mkdir(rep);
end

%% Read data

if not(exist('imresize'))
    imresize = @(x,s)image_resize(x,s);
end

targetSize = 199;

shape_list = [2 3 4];
nImages = length(shape_list);

data = zeros(targetSize*targetSize,nImages);
for i=1:nImages
    im = imread(sprintf('shape%dfilled.png', shape_list(i)));
    im = im2double(rgb2gray(im));

    padding = abs(size(im,2)-size(im,1));
    pad1 = floor(padding/2);
    pad2 = padding-pad1;

    if size(im,1) < size(im,2)
        im = [zeros(pad1,size(im,2)) ; im ; zeros(pad2,size(im,2))];
    elseif size(im,2) < size(im,1)
        im = [zeros(size(im,1),pad1) im zeros(size(im,1),pad2)];
    end

    im = 1-imresize(im,[targetSize targetSize]);
    im(im<0) = 0;

    im = im > .01;

    im = im + 1e-3;
    im = im / sum(im(:));

    % figure;imagesc(-im);axis equal;axis off;colormap gray;

    data(:,i) = im(:);
end

n = size(data,1);
areaWeights = ones(n,1)/n;
data = data*n;

entropies = -sum(bsxfun(@times,data.*log(data),areaWeights),1);
maxEntropy = max(entropies);

%% Set up blur

filterSize = 1.5; %was 5
imSize = [targetSize targetSize];

if exist('imfilter')
    % using image toolbox
    h = fspecial('gaussian',[1 max(imSize)],filterSize);% hsize sigma
    h = h / sum(h);
    imBlur = @(x) imfilter(imfilter(x,h,'replicate'),h','replicate');
else
    blur = load_filtering('imgaussian', targetSize);
    imBlur = @(x)blur(x,filterSize);
end

imagesc(imBlur(reshape(data(:,1),imSize)));

blurColumn = @(x) reshape(imBlur(reshape(x,imSize)),[],1);
blurAll2 = @(x) cell2mat(cellfun(blurColumn, num2cell(x,1), 'UniformOutput', false));
blurAll = @(x) blurAll2(blurAll2(x));

% blurColumn = @(x) reshape(fastBlur(reshape(x,[targetSize,targetSize]),filterSize),[],1);
% blurAll = @(x) cell2mat(cellfun(blurColumn, num2cell(x,1), 'UniformOutput', false));

imagesc(reshape(blurAll(data(:,1)),imSize));
axis equal;
axis off;

%% Compute barycenter

entropyLimit = Inf;

steps = linspace(0,1,5);% linspace(-.5,1.5,9);

averages = cell(length(steps),length(steps));
barycenters = cell(length(steps),length(steps));

options.niter = 400; %not 300
options.verb = 2;
options.tol = 1e-15;
resh = @(x)reshape(x, imSize);
options.disp = @(x)imageplot(-resh(x));

W = { ...
    [0, 0, 1] ...
    [1, 0, 3] [0, 1, 3] ...
    [1,0,1] [1,1,2] [0,1,1] ...
    [3,0,1] [2,1,1] [1,2,1] [0,3,1] ...
    [1,0,0] [3,1,0] [1,1,0] [1,3,0] [0,1,0] ...
    };

B = {};
for i=1:length(W)
    alpha = W{i}; alpha = alpha/sum(alpha);
    B{i} = convolutionalBarycenter(data,alpha,areaWeights,blurAll,blurAll,entropyLimit,options);
    u = rescale(-resh(B{i}));
    imwrite(u, [rep 'shape-barycenter-' num2str(i) '.png'], 'png');
    imwrite(double(u>.5), [rep 'tresh-barycenter-' num2str(i) '.png'], 'png');
end


function [D,x0,xv,idx] = driving_function_mono_localwfs_vss(x0,xs,src,f,conf)
%DRIVING_FUNCTION_MONO_LOCALWFS_VSS driving signal for local WFS using virtual
%secondary sources
%
%   Usage: [D,xv,x0,idx] = driving_function_mono_localwfs_vss(x0,xs,src,f,conf)
%
%   Input parameters:
%       x0          - position and direction of the secondary source / m [nx7]
%       xs          - position of virtual source or direction of plane
%                     wave / m [1x3]
%       src         - source type of the virtual source
%                         'pw' - plane wave (xs is the direction of the
%                                plane wave in this case)
%                         'ps' - point source
%                         'ls' - line source
%                         'fs' - focused source
%
%       f           - frequency of the monochromatic source / Hz
%       conf        - configuration struct (see SFS_config)
%
%   Output parameters:
%       D           - driving function signal [nx1]
%       x0          - position, direction, and weights of the real secondary
%                     sources / m [nx7]
%       xv          - position, direction, and weights of the virtual secondary
%                     sources / m [mx7]
%       idx         - index of the selected sources from the original x0
%                     matrix [mx1]
%
%   See also: plot_sound_field, sound_field_mono_wfs
%
%   References:
%       Spors (2010) - "Local Sound Field Synthesis by Virtual Secondary
%       Sources", 40th Conference of the Audio Engineering Society, Paper 6-3,
%       http://www.aes.org/e-lib/browse.cfm?elib=15561

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 5;
nargmax = 5;
narginchk(nargmin,nargmax);
isargsecondarysource(x0);
isargxs(xs);
isargpositivescalar(f);
isargchar(src);
isargstruct(conf);

%% ===== Configuration ==================================================
virtualconf = conf;
virtualconf.usetapwin = conf.localwfs_vss.usetapwin;
virtualconf.tapwinlen = conf.localwfs_vss.tapwinlen;
virtualconf.secondary_sources.size = conf.localwfs_vss.size;
virtualconf.secondary_sources.center = conf.localwfs_vss.center;
virtualconf.secondary_sources.geometry = conf.localwfs_vss.geometry;
virtualconf.secondary_sources.number = conf.localwfs_vss.number;
virtualconf.wfs = conf.localwfs_vss.wfs;
virtualconf.nfchoa = conf.localwfs_vss.nfchoa;
virtualconf.driving_functions = conf.localwfs_vss.driving_functions;
method = conf.localwfs_vss.method;

%% ===== Computation ====================================================

% Determine driving functions of virtual array with different sfs methods
switch method
case 'wfs'
    % === Wave Field Synthesis ===
    % Create virtual source array
    xv = virtual_secondary_source_positions(x0,xs,src,conf);
    % Secondary_source_selection
    xv = secondary_source_selection(xv, xs, src);
    % Optional tapering
    xv = secondary_source_tapering(xv,virtualconf);
    % Driving functions for virtual source array
    Dv = driving_function_mono_wfs(xv,xs,src,f,virtualconf);
case 'nfchoa'
    % === Near-Field-Compensated Higher Order Ambisonics ===
    % Create virtual source array
    xv = secondary_source_positions(virtualconf);
    % Driving functions for virtual source array
    Dv = driving_function_mono_nfchoa(xv,xs,src,f,virtualconf);
otherwise
    error('%s: %s is not a supported method for localsfs!',upper(mfilename),method);
end

% Select secondary sources
[x0,idx] = secondary_source_selection(x0,xv(:,1:6),'vss');
% Driving functions for real source array
D = driving_function_mono_wfs_vss(x0,xv,'fs',Dv,f,conf);


function optimParam = tuneParamForModel(model,varargin)
% Optimizes cplex parameters to make model resolution faster.
% Particularly interetsing for large-scale MILP models and repeated runs of
% optimisation.
% While, it won't optimize memory space nor model constraints for numerical
% infeasibilities, tuneParam will provide the optimal set of solver
% parameters for feasbile models. It requires IBM ILOG cplex (for now).
%
% USAGE:
%
%    optimalParameters = tuneParam(model,contFunctName,1000,1000,0);
%
% INPUT:
%         model:         A COBRA model struct.
%         contFunctName: Parameters structure containing the name and value.
%                        A set of routine parameters will be added by the solver
%                        but won't be reported.
%         timelimit:     default is 10000 second
%         nrepeat:       number of row/column permutation of the original
%                        problem, reports robust results.
%                        sets the CPX_PARAM_TUNINGREPEAT parameter
%                        High values of nrepeat would require consequent
%                        memory and swap.
%         printLevel:    0/1/2/3
%
% OUTPUT:
%         optimParam: structure of optimal parameter values directly usable as
%                     contFunctName argument in solveCobraLP function
%
% NOTE:
%       This is just a wrapper function that calls the tuneParam function
%       using a Cobra model structure converted to a LP problem.
%
% .. Author: Thomas Pfau Dec 2017

optimParam = tuneParam(buildLPproblemFromModel(model),varargin{:});


% nbins = 8;
% psize = 10;
% npatches = 8;
% optflowwinsig = 3;
% optflowsig = 2;
% optreliability = 1e-4;
% patchsz = psize*npatches;

expdir = '/groups/branson/home/kabram/bransonlab/data/katie/20170308_133311/20170308_133311_08';
moviefilestr = 'movie.avi';
frames = 2831:2900;
vidoutfile = sprintf('%s_From%d_To%d_HOF_%s.avi',expname,fly,t0,t1,datestr(now,'yyyymmdd'));
makeVideo = true;

%%
params = getParamsKatie;
npatchesx = params.npatchesx;
npatchesy = params.npatchesy;
psize = params.psize;
nbins = params.nbins;
scale = params.scale;

optflowwinsig = params.optflowwinsig ;
optflowsig = params.optflowsig ;
optreliability = params.optreliability ;

maxflow = 5;
fly_thres = 90;
flow_thres = params.flow_thres;

% Antennal grooming for andy

[~,expname] = fileparts(expdir);
moviefile = fullfile(expdir,moviefilestr);

[readframe,nframes] = get_readframe_fcn(moviefile);
for i = 1:numel(frames)
  curf = readframe(frames(i));
  nextf = readframe(frames(i)+1);

  curpatch = curf;
  curpatch2 = nextf;

  im1(:,:,i) = curpatch;
  im2(:,:,i) = curpatch2;
end

[nr,nc,~] = size(im1);
%
% figure out the histogram bins
tmptheta = (0:179)*pi/180;
res = nan(nbins,numel(tmptheta));
m = single(ones(10,10));
o = single(zeros(10,10));
for i = 1:numel(tmptheta),
%   fprintf('i = %d, theta = %f\n',i,tmptheta(i));
  o(:) = single(tmptheta(i));
  rescurr = gradientHist(m,o,1,nbins,1);
  res(:,i) = rescurr(1,1,:);
end

bincenters = nan(1,nbins);
for i = 1:nbins,
  bincenters(i) = tmptheta(argmax(res(i,:)));
end

% this seems to be what the centers correspond to
bincenters = linspace(0,pi,nbins+1);
bincenters = bincenters(1:nbins);

% mayank divides by 2
bincenters2 = bincenters*2;
dt = mean(diff(bincenters2));
binedges2 = [bincenters2(1)-dt/2,(bincenters2(1:end-1)+bincenters2(2:end))/2,bincenters2(end)+dt/2];

%% make a video

tr = 191;
hfig = 100;
t0 = frames(1);
t1 = frames(end);
colorpos = [1,0,0];
colorneg = [0,.3,1];

maxv2 = 0;
colors = hsv(nbins);
sz = size(im1);
bwimg = zeros(sz(1),sz(2));
ctr = [ceil( (sz(1)+1)/2),ceil( (sz(2)+1)/2)];
bwimg(ctr(1),ctr(2))=1;
dimg = bwdist(bwimg,'euclidean');
[xx,yy]= meshgrid(1:sz(2),1:sz(1));
aimg = atan2(-(yy-ctr(1)),-(xx-ctr(2)));

for t = t0:20:t1,

  im1curr = gaussSmooth(im1(:,:,t-t0+1),1,'same');
  im2curr = gaussSmooth(im2(:,:,t-t0+1),1,'same');
  uv = estimate_flow_interface(im1curr,im2curr,'hs-brightness',{'max_warping_iters',2});
  Vx = uv(:,:,1); Vy = uv(:,:,2);

  M = sqrt(Vx.^2 + Vy.^2);
  O = mod(atan2(Vy,Vx)/2,pi);
  O = min(O,pi-1e-6);
  H = gradientHist(single(M),single(O),psize,nbins,1);
  maxv2 = max(maxv2,max(H(:)));
end

%%

maxv2 = 4;

if makeVideo
  vid = VideoWriter(vidoutfile);
  open(vid);
end

for t = t0:t1

  im1curr = gaussSmooth(im1(:,:,t-t0+1),1,'same');
  im2curr = gaussSmooth(im2(:,:,t-t0+1),1,'same');

  imsz = size(im1curr);
  pairimg = zeros(2*imsz(1),imsz(2),3);
  ttimg = [];
  tt(:,:,1) = im2curr;
  tt(:,:,2) = im1curr;
  tt(:,:,3) = im2curr;
  pairimg(1:imsz(1),:,:) = tt;

  uv = estimate_flow_interface(im1curr,im2curr,'hs-brightness',...
    {'max_warping_iters',2 });
  uvorig = uv;
  pairimg(imsz(1)+(1:imsz(1)),1:imsz(2),:) = flowToColor(uv,maxflow);

  pairimg = uint8(pairimg);
  pairimg = imresize(pairimg,scale);
  Vx = uvorig(:,:,1); Vy = uvorig(:,:,2);
  M = sqrt(Vx.^2 + Vy.^2);
  O = mod(atan2(Vy,Vx)/2,pi);
  O = min(O,pi-1e-6);
  Horig = gradientHist(single(M),single(O),psize,nbins,1);

  if t == t0,
    figure(hfig);
    clf;
    hax = axes('Position',[0,0,1,1]);
    set(hfig,'Units','pixels','Position',get(0,'ScreenSize'));

    him = imshow(pairimg);
    axis image;
    truesize;
    colormap gray;
    hold on;
    axis off;
    htext = text(1,nr-1,sprintf('%.2f s',(t-1)/200),'Color','k','HorizontalAlignment','left','VerticalAlignment','bottom','FontSize',18);

  else
    delete(horig(ishandle(horig)));
    set(him,'CData',pairimg);
    set(htext,'String',sprintf('%.2f s',(t-1)/200));
  end

  horig = plotHofFeatures(Horig,hax,maxv2,colors,binedges2,psize*scale,[0 0]);

  drawnow;
  if makeVideo
    fr = getframe(hax);
    writeVideo(vid,fr);
  elseif t0<t1
    pause;
  end
end

if makeVideo,  close(vid); end


function gX = multiKernGradX(kern, x, x2)

% MULTIKERNGRADX Gradient of MULTI kernel with respect to a point x.
% FORMAT
% DESC computes the gradient of the multiple output block
% kernel with respect to the input positions.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x : locations against which gradients are being computed.
% RETURN gX : the returned gradients. The gradients are returned in
% a matrix which is numData x numInputs x numData. Where numData is
% the number of data points and numInputs is the number of input
% dimensions in X.
%
% FORMAT
% DESC computes the gradident of the multiple output block
% kernel with respect to the input positions where both the row
% positions and column positions are provided separately.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x : row locations against which gradients are being computed.
% ARG x2 : column locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData2 x numInputs x numData1. Where numData1 is
% the number of data points in X1, numData2 is the number of data
% points in X2 and numInputs is the number of input
% dimensions in X.
%
% SEEALSO : multiKernParamInit, kernGradX, multiKernDiagGradX
%
% COPYRIGHT : Mauricio A. Alvarez and Neil D. Lawrence, 2008

% KERN

if iscell(x)
    if nargin > 3 && ~iscell(x2)
        error('Time course information is not matched in Cell format!');
    end
    % Collate arguments.
    for i=1:kern.numBlocks
        arg{i}{1} = x{i};
        if nargin > 2
            arg{i}{2} = x2{i};
        else
            arg{i}{2} = arg{i}{1};
        end
    end
    for i = 1:kern.numBlocks
        if nargin > 2
            gX = multiKernGradientBlockX(kern, arg{i}{:}, i, i);
        else
            gX = multiKernGradientBlockX(kern, arg{i}{1}, i, i);
        end
        for j = 1:i-1
            if ~isempty(kern.block{i}.cross{j})
                gX = multiKernGradientBlockX(kern, arg{i}{1}, arg{j}{2}, i, j);
            end
        end
    end
end


function [week,sow]=time2gpst(t)

gpst0=[1980,1, 6,0,0,0]; %gps time reference

t0=epoch2time(gpst0);

sec=t.time-t0.time;
week=fix(sec/7/86400);
sow=sec-week*7*86400+t.sec;

return


% /*M-FILE SCRIPT SwarmBat_demo_SO_std MMM SGALAB */ %
% /*==================================================================================================
%  Swarm Optimisation and Algorithm Laboratory Toolbox for MATLAB
%
%  Copyright 2012 The SxLAB Family - Yi Chen - leo.chen.yi@live.co.uk
% ====================================================================================================
% File description:
%
%Appendix comments:
%
%Usage:
%
%===================================================================================================
%  See Also:
%
%===================================================================================================
%===================================================================================================
%Revision -
%Date          Name     Description of Change  email
%16-May-2011   Chen Yi  Initial version        leo.chen.yi@live.co.uk
%26-Jul-2011   Chen Yi  Add eachtimeplot       leo.chen.yi@live.co.uk
%HISTORY$
%==================================================================================================*/
%==================================================================================================*/

% SwarmBat_demo_SO_std Begin

% fresh
clear ;
close ('all');
warning off
% to delete old output_*.txt
!del SwarmBat_O_*.*
% set working path

% begin to count time during calculating
home ;
tic % timer start >>

% data preparation

% open data files

%%%input data files
fid1  = fopen('SwarmBat_I_min_confines.txt' , 'r' );
fid2  = fopen('SwarmBat_I_max_confines.txt' , 'r' );
%fid3  = fopen('SwarmBat_I_soundspeed.txt' , 'r' );
fid4  = fopen('SwarmBat_I_distpara.txt' , 'r' );
fid5  = fopen('SwarmBat_I_population.txt' , 'r' );
fid6  = fopen('SwarmBat_I_randomstep.txt' , 'r' );
fid7  = fopen('SwarmBat_I_max_generation.txt' , 'r' );
fid8  = fopen('SwarmBat_I_loundness_min.txt' , 'r' );
fid9  = fopen('SwarmBat_I_PErate_min.txt','r');
%          fid10 = fopen('INPUT_deta_fitness_max.txt','r');
%          fid11 = fopen('INPUT_max_probability_crossover.txt','r');
%          fid12 = fopen('INPUT_probability_crossover_step.txt','r');
%          fid13 = fopen('INPUT_max_no_change_fitness_generation.txt','r');
%          fid22 = fopen('INPUT_tournament_size.txt','r');
fid10  = fopen('SwarmBat_I_loundness_alpha.txt' , 'r' );
fid11  = fopen('SwarmBat_I_PErate_gamma.txt','r');

fid20= fopen('SwarmBat_I_fHz.txt','r');

% set total test number
fid1000= fopen('SwarmBat_I_testnumber.txt','r');

%output data files
fid120 = fopen('SwarmBat_O_best_fitness.txt','a+');
fid130 = fopen('SwarmBat_O_maxfitness.txt','a+');
fid140 = fopen('SwarmBat_O_minfitness.txt','a+');
fid150 = fopen('SwarmBat_O_meanfitness.txt','a+');
fid160 = fopen('SwarmBat_O_best_solutions.txt','a+');
%          fid19 = fopen('OUTPUT_best_coding_space.txt','w+');
%          fid20 = fopen('OUTPUT_now_generation.txt','w+');
%          fid21 = fopen('OUTPUT_now_probability_crossover.txt','w+');

% begin to load data from file

% read data from these files
disp('/*==================================================================================================*/')
disp('/*  Swarm Optimisation and Algorithm Laboratory Toolbox 1.0.0.1 */ ')
disp('');
disp('    30-Mar-2012 Chen Yi leo.chen.yi@live.co.uk Glasgow ')
disp('/*==================================================================================================*/')
disp('>>>>')
disp(' Begin to Evaluate...Waiting Please ...');

min_confines = fscanf( fid1 , '%f' ); min_confines = min_confines' ;

max_confines = fscanf( fid2 , '%f' ); max_confines = max_confines';
%
%       probability_crossover = fscanf( fid3 , '%f' ); probability_mutation = fscanf(fid4,'%f');
%
population = fscanf( fid5 , '%f' );
%
random_step = fscanf( fid6 , '%f' );
%
max_generation = fscanf( fid7 , '%f' );
%
%soundspeed = fscanf( fid3 , '%f' );
%
beta = fscanf( fid4 , '%f' ); % random distribution parameters
%
loudness_min = fscanf( fid8 , '%f' );
%
PErate_min = fscanf( fid9,'%f' );
%
alpha = fscanf(fid10,'%f');
%
gamma = fscanf(fid11,'%f');
%

%
%       now_probability_crossover = probability_crossover;
%
fHz = fscanf(fid20,'%f'); % Hz = [fmin, fmax] is the frequency range
% %

testnumber = fscanf(fid1000,'%f');

disp(' the total test number is');
disp(testnumber);

% Step into SGALAB()
%
eachtimeplot       = 0;
statisticalplot    = [1,1,1,1,1];
%statisticalplot(1) - plot MIN,MEAN,MAX or not, 0-NO, 1-YES
%statisticalplot(2) - plot mAP or not,          0-NO, 1-YES
%statisticalplot(3) - plot mSTD or not,         0-NO, 1-YES
%statisticalplot(4) - plot mmAP or not,         0-NO, 1-YES
%statisticalplot(5) - plot mmSTD or not,        0-NO, 1-YES

options = { ...
    fHz,                  % 1, Hz range
    random_step,          % 2, bat random walk step
    [],                   % 3, sound speed, 340m/s in default
    beta,                 % 4, distribution parameters
    eachtimeplot,         % 5, plot each test, 1 - plot each time, 0 - no plots
    statisticalplot,      % 6, statistical plots, 1 - yes ,  0 - no plots
    'Roulettewheel',       % 7, selection method
    loudness_min,         % 8, loudness min
    PErate_min,           % 9, Pulse Emission rate min
    alpha,                % 10, alpha for loudness
    gamma   };            % 11, gamma for pulse emission rate

SwarmBat_Procedure_h = timebar('SwarmsLAB::SwarmFish','Total Progress...');

for idx = 1 : 1 : testnumber
    % Output

    timebar( SwarmBat_Procedure_h , idx/testnumber );

    disp('Test NO.'); disp(idx);
    disp('');

    [   fitness_data ,...
        best_decimal_space ,...
        error_status ]= SwarmBat__entry_SO_std...
        ( options,...
        min_confines ,...
        max_confines ,...
        population ,...
        max_generation,...
        [testnumber,idx] );

    disp('');

    if ( error_status ~= 0 )  return ;  end

    %write data to output files

    %   OUTPUT_bestfitness.txt
    fprintf( fid120 , '%f\n', fitness_data(1));

    %   OUTPUT_maxfitness.txt
    fprintf( fid130 , '%f\n', fitness_data(2));

    %   OUTPUT_minfitness.txt
    fprintf( fid140,  '%f\n', fitness_data(3));

    %   OUTPUT_meanfitness.txt
    fprintf(  fid150,  '%f\n', fitness_data(4));

    %   OUTPUT_best_result_space.txt
    fprintf(  fid160,  '%f\n', best_decimal_space );

    % %   OUTPUT_best_coding_space
    % fprintf(  fid19 , '%f\n' , best_coding_space );

    % %   OUTPUT_now_generation.txt
    % fprintf(  fid20, '%f\n' , now_generation );
    %
    % %   OUTPUT_now_probability_crossover.txt
    % fprintf(  fid21, '%f\n' , now_probability_crossover );

end

%close files
close( SwarmBat_Procedure_h );
status = fclose( 'all' );

disp('End SwarmFish Evaluating');
disp('');

disp(' More detail result in text files with " Swarm_O_*.txt " ' )
disp('----------------------------------------------------------------------------------------')
result_files = list_current_dir_files ('SwarmBat_O_*.*')

disp('----------------------------------------------------------------------------------------')

% timer end
toc
% SwarmBat_demo_SO_std End


function [data,units] = compute_min_wing_area(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);

  data{i} = min(trx(fly).wing_areal_mm,trx(fly).wing_arear_mm);

end
units = parseunits('mm^2');


function [g1, g2, g3] = sdlfmaXsdlfmvKernGradientBlock(lfmKern1, lfmKern2, ...
    t1, t2, kyy, kyv, kvy, kvv, i, j, generalConst, generalConstGrad, ...
    covGrad)

% SDLFMAXSDLFMVKERNGRADIENTBLOCK Gradients of the parameters in block i,j
% FORMAT
% DESC computes the kernel parameters gradients for the SDLFM kernel
% function in the block specified at indeces i,j. It assumes the
% computation for functions that system 1 describes an acceleration and
% system 2 describes a velocity.
% ARG lfmKern1 : structure containing parameters for the system 1
% ARG lfmKern2 : structure containing parameters for the system 2
% ARG t1 : times at which the system 1 is evaluated
% ARG t2 : times at which the system 2 is evaluated
% ARG kyy : covariance for the initial conditions between position 1 and
% position 2 at block i,j
% ARG kyv : covariance for the initial conditions between position 1 and
% velocity 2 at block i,j
% ARG kvy : covariance for the initial conditions between velocity 1 and
% position 2 at block i,j
% ARG kvv : covariance for the initial conditions between velocity 1 and
% velocity 2 at block i,j
% ARG i : interval to be evaluated for system 1
% ARG j : interval to be evaluated for system 2
% ARG generalConstant : constants evaluated with sdlfmKernComputeConstant.m
% ARG generalConstGrad : derivatives of the constants computed with
% sdlfmKernGradientConstant.m
% ARG covGrad : partial derivatives of the objective function wrt portion
% of the kernel matrix in block i,j
% RETURN g1 : gradients of parameters for the system 1
% RETURN g2 : gradients of parameters for the system 2
% RETURN g3 : gradients of switching points
%
% COPYRIGHT : Mauricio A. Alvarez, 2010.

% KERN

if nargin<11
    j = i;
    generalConst = [];
end

g3 = [];

% Compute derivatives of the mean terms with respect to the parameters

[g1Mean, g2Mean, gsp1Mean, gsp2Mean] = sdlfmKernGradientMean(lfmKern1(1), ...
    lfmKern2(1), t1, t2, kyy, kyv, kvy, kvv, covGrad, {'sdlfma','sdlfmv'}, ...
    {'sdlfmj', 'sdlfma'});

if i==j
    [g1, g2, g3t] = sdlfmaXsdlfmKernGradientBlockIEJ(lfmKern1, lfmKern2, t1, ...
        t2, covGrad, g1Mean, g2Mean, gsp1Mean, gsp2Mean, {'lfma', 'lfmv'}, ...
        {'lfmj', 'lfmv'}, {'lfma', 'lfma'});
    g3(i) = g3t;
else
    if i>j
        [g1, g2, g3] = sdlfmXsdlfmvKernGradientBlockIGJ(lfmKern1, lfmKern2, ...
            t1, t2, i, j, generalConst, generalConstGrad, covGrad, g1Mean, ...
            g2Mean, gsp1Mean, gsp2Mean, 'sdlfma', 'sdlfmj', 'lfmvXlfm', ...
            'lfmvXlfmv', {'lfmvXlfmv', 'lfmaXlfm'}, {'lfmaXlfmv', 'lfmaXlfmv'});
    else
        [g1, g2, g3] = sdlfmaXsdlfmKernGradientBlockILJ(lfmKern1, lfmKern2, ...
            t1, t2, i, j, generalConst, generalConstGrad, covGrad, g1Mean, ...
            g2Mean, gsp1Mean, gsp2Mean, 'sdlfmv', 'sdlfma', 'lfmaXlfm', ...
            'lfmaXlfmv',{'lfmjXlfm', 'lfmaXlfmv'}, {'lfmjXlfmv', 'lfmaXlfma'});
    end
end


function spm_dem_search_movie(DEM)
% creates a movie of visual search in extrinsic and intrinsic coordinates
% FORMAT spm_dem_search_movie(DEM)
%
% DEM - {DEM} structures from visual search simulations
%
% hidden causes and states
%==========================================================================
% x    - hidden states:
%   o(1) - oculomotor angle
%   o(2) - oculomotor angle
%   x(1) - relative amplitude of visual hypothesis 1
%   x(2) - relative amplitude of visual hypothesis 2
%   x(3) - ...
%
% v    - hidden causes
%
% g    - sensations:
%   g(1) - oculomotor angle (proprioception - x)
%   g(2) - oculomotor angle (proprioception - y)
%   g(3) - retinal input - channel 1
%   g(4) - retinal input - channel 2
%   g(5) - ...
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dem_search_movie.m 6901 2016-10-08 13:21:41Z karl $

% Preliminaries
%--------------------------------------------------------------------------
clf, global STIM
if ~iscell(DEM), DEM = {DEM};           end
if ~isfield(STIM,'W'), STIM.W = 1/6;    end
if ~isfield(STIM,'P'), STIM.P = [0;0];  end
if ~isfield(STIM,'U'), STIM.U = STIM.V; end
if ~isfield(STIM,'S'), STIM.S = STIM.H; end

N   = length(DEM);
S   = spm_read_vols(STIM.U);

% Stimulus
%==========================================================================
Dx  = STIM.P(1)*16 + STIM.U.dim(1)/2;
Dy  = STIM.P(2)*16 + STIM.U.dim(2)/2;

dim = size(STIM.R);
vox = STIM.U.dim(1);
dx  = vox/dim(1)*STIM.W;
di  = dx*([1 dim(1)] - dim(1)/2) + Dx;
dj  = dx*([1 dim(2)] - dim(2)/2) + Dy;
di  = [di;di]; di = di(:);
dj  = [dj;dj]';dj = dj(:);
ax  = [(Dy - vox) (Dy + vox) (Dx - vox) (Dx + vox)]/2;

a     = [];
for i = 1:N

    % i-th saccade - position
    %----------------------------------------------------------------------
    pU = DEM{i}.pU.x{1}(1:2,:)*16;
    qU = DEM{i}.qU.x{1}(1:2,:)*16;
    T  = length(pU);
    a  = [a DEM{i}.qU.a{2}];

    % eye movements in extrinsic coordinates
    %======================================================================
    subplot(2,2,1)
    for t = 1:T
        image((S + 1)*32), axis image, hold on
        plot(qU(2,t) + Dy,qU(1,t) + Dx,'.g','Markersize',8)
        plot(pU(2,t) + Dy,pU(1,t) + Dx,'.r','Markersize',16)
        plot(pU(2,t) + dj,pU(1,t) + di,'+' ,'Markersize',16)

        % show location of image ccentre
        %------------------------------------------------------------------
        plot(Dy,Dx,'+r','Markersize',32);  % axis(ax);

        drawnow, hold off

        % save
        %------------------------------------------------------------------
        Me(t + T*(i - 1)) = getframe(gca);

    end

    % sensory input
    %======================================================================
    subplot(2,2,2)
    for t = 1:T

        o   = DEM{i}.pU.x{1}(:,t);
        s   = ADEM_sample_image(STIM.U,o,STIM.R);
        image(s*64), axis image, drawnow

        % save
        %----------------------------------------------------------------------
        Mi(t + T*(i - 1)) = getframe(gca);

    end

    % i-th saccade - percept
    %----------------------------------------------------------------------
    qU = DEM{i}.qU.x{1}(3:end,:);

    % percept
    %======================================================================
    subplot(2,2,4)
    for t = 1:T

        % hypotheses
        %------------------------------------------------------------------
        h     = spm_softmax(qU(:,t),2);
        H     = 1 + h'*log(h)/log(length(h));

        % retinotopic predictions
        %------------------------------------------------------------------
        s     = 0;
        for j = 1:length(h)
            s = s + h(j)*spm_read_vols(STIM.S{j});
        end
        image(s*H*64), axis image, drawnow

        % save
        %------------------------------------------------------------------
        Mq(t + T*(i - 1)) = getframe(gca);

    end

end

% set ButtonDownFcn
%--------------------------------------------------------------------------
subplot(2,2,3)
plot(a')
title('Action (EOG)','FontSize',16)
xlabel('time')
axis square

subplot(2,2,1)
set(gca,'Userdata',{Me,16})
set(gca,'ButtonDownFcn','spm_DEM_ButtonDownFcn')
title('saccades (click axis for movie)','FontSize',16)

subplot(2,2,2)
set(gca,'Userdata',{Mi,16})
set(gca,'ButtonDownFcn','spm_DEM_ButtonDownFcn')
title('samples (click axis for movie)','FontSize',16)

subplot(2,2,4)
set(gca,'Userdata',{Mq,16})
set(gca,'ButtonDownFcn','spm_DEM_ButtonDownFcn')
title('percept (click axis for movie)','FontSize',16)


function c8_uniform_01_test ( )

%*****************************************************************************80
%
%% C8_UNIFORM_01_TEST tests C8_UNIFORM_01.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 February 2015
%
%  Author:
%
%    John Burkardt
%
  seed = 123456678;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'C8_UNIFORM_01_TEST\n' );
  fprintf ( 1, '  C8_UNIFORM_01 returns a uniformly random "unit" C8.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       C1=C8_UNIFORM_01\n' );
  fprintf ( 1, '     ---------------------\n' );
  fprintf ( 1, '\n' );

  for test = 1 : 10

    [ c1, seed ] = c8_uniform_01 ( seed );

    fprintf ( 1, '  (%12f  %12f)\n', real ( c1 ), imag ( c1 ));

  end

  return
end


function [grid3d, ge, osc] = read_grid(filenamebase)

chkarg(istypesizeof(filenamebase, 'char', [1 0]), '"filenamebase" should be string.');

inputfile = ['./', filenamebase, '.h5'];

lambda = h5read(inputfile, '/lambda');
L0 = h5read(inputfile, '/L0');
Npml = h5read(inputfile, '/Npml').';
ge = h5read(inputfile, '/ge');
ge = GT.elems(ge+1);
bc = BC.elems(h5read(inputfile, '/bc').' + 1);
bc = bc(:, Sign.n).';

lprim = cell(1, Axis.count);
for w = Axis.elems
	lprim{w} = h5read(inputfile, ['/', char(w), '_prim']).';
end

unit = PhysUnit(L0);
osc = Oscillation(lambda, unit);
grid3d = Grid3d(unit, lprim, Npml, bc);


function [y,z]=amen_sum(X, c, tol, varargin)
%Approximate the linear combination of TT tensors via the AMEn iteration
%   [y,z]=amen_sum(X, v, tol, varargin)
%   Attempts to approximate the y(j) = sum_i X{i}*c(i,j)
%   with accuracy TOL using the AMEn+ALS iteration.
%
%   Options are provided in form
%   'PropertyName1',PropertyValue1,'PropertyName2',PropertyValue2 and so
%   on. The parameters are set to default (in brackets in the following)
%   The list of option names and default values are:
%       o y0 - initial approximation to y [rand rank-2]
%       o nswp - maximal number of sweeps [20]
%       o verb - verbosity level, 0-silent, 1-sweep info, 2-block info [1]
%       o kickrank - compression rank of the error,
%         i.e. enrichment size [3]
%       o init_qr - perform QR of the input (save some time in ts, etc) [true]
%       o renorm - Orthog. and truncation methods: direct (svd,qr) or gram
%         (apply svd to the gram matrix, faster for m>>n) [direct]
%       o fkick - Perform solution enrichment during forward sweeps [false]
%         (rather questionable yet; false makes error higher, but "better
%         structured": it does not explode in e.g. subsequent matvecs)
%       o z0 - initial approximation to the error Ax-y [rand rank-kickrank]
%       o can - whether the input is in CP format [false]
%       o multrank - shall we try to grow ranks multiplicatively by adding
%       the previous iterand [false]
%
%
%********
%   For description of adaptive ALS please see
%   Sergey V. Dolgov, Dmitry V. Savostyanov,
%   Alternating minimal energy methods for linear systems in higher dimensions.
%   Part I: SPD systems, http://arxiv.org/abs/1301.6068,
%   Part II: Faster algorithm and application to nonsymmetric systems, http://arxiv.org/abs/1304.1222
%
%   Use {sergey.v.dolgov, dmitry.savostyanov}@gmail.com for feedback
%********
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

% Special case: amen_sum called from another amen package
if (~isempty(varargin))
    v1 = varargin{1};
    if (isa(v1, 'cell'))
        varargin=v1;
    end;
end;

% Default valuse
nswp = 20;
kickrank = 4;
kickrank2 = 0;
verb = 1;
y = [];
z = [];
init_qr = true;
renorm = 'direct';
% renorm = 'gram';
fkick = false;
rmax = Inf;
multrank = false;

can = false; % Whether the input is the canonical format

% Read parameters from input sequence
for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'nswp'
            nswp=varargin{i+1};
        case 'y0'
            y=varargin{i+1};
        case 'z0'
            z=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};
        case 'kickrank'
            kickrank=varargin{i+1};
        case 'kickrank2'
            kickrank2=varargin{i+1};
        case 'init_qr'
            init_qr = varargin{i+1};
        case 'renorm'
            renorm = varargin{i+1};
        case 'fkick'
            fkick = varargin{i+1};
        case 'rmax'
            rmax = varargin{i+1};
        case 'can'
            can = varargin{i+1};
        case 'multrank'
            multrank = varargin{i+1};

        otherwise
            warning('Unrecognized option: %s\n',varargin{i});
    end
end

% Convert input to canonical or TT cell array
N = size(c,1);
M = size(c,2);
Xin = X;

if (can)
    N = 1;
    % X itself is already given in the required format
    d = numel(X);
    R = size(X{1}, 2);
    nrms = ones(d,1);
    n = zeros(d,1);
    for i=1:d
        n(i) = size(X{i}, 1);
    end;
    vectype = 1;
else
    R = 1;
    for j=1:N
        if (isa(Xin{j}, 'tt_tensor'))
            if (j==1)
                d = Xin{j}.d;
                n = Xin{j}.n;
                X = cell(d,N);

                % A storage for norms.
                % Z may be kept normalized, but for y we will have y_real = y*prod(nrms).
                % The same for all x. But note that we keep only the _scale_ for all x vecs.
                nrms = ones(d,1);
            end;
            X(:,j) = core2cell(Xin{j});
            vectype = 1; % tt_tensor
        else
            if (j==1)
                d = numel(Xin{j});
                n = zeros(d,1);
                for i=1:d
                    n(i) = size(Xin{j}{i}, 2);
                end;
                X = cell(d,N);
            end;
            X(:,j) = Xin{j};
            vectype = 0; % cell
        end;
    end;
end;

% Initial guess
if (isempty(y))
    init_qr = false;
    [y,ry] = gen_rand(n,d,2);
else
    if (isa(y, 'tt_tensor'))
        y = core2cell(y);
    end;
    ry = ones(d+1,1);
    for i=1:d
        ry(i+1) = size(y{i}, 3);
    end;
end;

% Enrichment vector
if (kickrank+kickrank2>0)
    if (isempty(z))
        [z,rz] = gen_rand(n,d,kickrank+kickrank2);
        init_qr_z = false;
    else
        init_qr_z = true;
        if (isa(z, 'tt_tensor'))
            z = core2cell(z);
        end;
        rz = ones(d+1,1);
        for i=1:d
            rz(i+1) = size(z{i}, 3);
        end;
    end;

    phizx = cell(d+1,N);
    for j=1:N
        phizx{1,j}=ones(1,R); phizx{d+1,j}=ones(R,1);
    end;
    phizy = cell(d+1,1); phizy{1}=1; phizy{d+1}=1;
end;

phiyx = cell(d+1,N);
for j=1:N
    phiyx{1,j}=ones(1,R); phiyx{d+1,j}=ones(R,1);
end;

% Initial ort
for i=1:d-1
    if (init_qr)
        cr = reshape(y{i}, ry(i)*n(i), ry(i+1));
        if (strcmp(renorm, 'gram'))&&(ry(i)*n(i)>5*ry(i+1))
            [cr,~,R]=svdgram(cr);
        else
            [cr,R]=qr(cr, 0);
        end;
        nrmr = norm(R, 'fro');
        if (nrmr>0)
            R = R/nrmr;
        end;
        cr2 = reshape(y{i+1}, ry(i+1), n(i+1)*ry(i+2));
        cr2 = R*cr2;
        ry(i+1) = size(cr, 2);
        y{i} = reshape(cr, ry(i), n(i), ry(i+1));
        y{i+1} = reshape(cr2, ry(i+1), n(i+1), ry(i+2));
    end;
    % We will extract the norms of Y'X
    [phiyx(i+1,:),nrms(i)] = compute_next_Phi(phiyx(i,:), y{i}, X(i,:), 'lr', can);

    if (kickrank+kickrank2>0)
        if (init_qr_z)
            cr = reshape(z{i}, rz(i)*n(i), rz(i+1));
            if (strcmp(renorm, 'gram'))&&(rz(i)*n(i)>5*rz(i+1))
                [cr,~,R]=svdgram(cr);
            else
                [cr,R]=qr(cr, 0);
            end;
            nrmr = norm(R, 'fro');
            if (nrmr>0)
                R = R/nrmr;
            end;
            cr2 = reshape(z{i+1}, rz(i+1), n(i+1)*rz(i+2));
            cr2 = R*cr2;
            rz(i+1) = size(cr, 2);
            z{i} = reshape(cr, rz(i), n(i), rz(i+1));
            z{i+1} = reshape(cr2, rz(i+1), n(i+1), rz(i+2));
        end;

        % But we have to renorm Z'X/|Y'X| to keep Z in the same
        % scale. Hope it will not make |Z'X| too small
        phizx(i+1,:) = compute_next_Phi(phizx(i,:), z{i}, X(i,:), 'lr', can, nrms(i));
        % Z'Y is actually an orthogonal matrix and does not require renorm
        phizy(i+1) = compute_next_Phi(phizy(i), z{i}, y(i), 'lr');
    end;
end;

i = d;
dir = -1;
swp = 1;
max_dx = 0;
y{d} = reshape(y{d}, ry(d)*n(d), ry(d+1));
y{d} = y{d}(:,1:min(ry(d+1), M));
y{d} = [y{d}, zeros(ry(d)*n(d), M-ry(d+1))];
y{d} = reshape(y{d}, ry(d), n(d), 1, M);
ry(d+1) = 1;

while (swp<=nswp)
    % Project the sum
    cry = proj_sum(phiyx(i,:), X(i,:), phiyx(i+1,:), c);
    % It will survive in the terminal block. All we need.
    nrms(i) = norm(cry, 'fro');
    % The main goal is to keep y{i} of norm 1
    if (nrms(i)>0)
        cry = cry/nrms(i);
    else
        nrms(i)=1;
    end;

    % Check stopping criteria
    y{i} = reshape(y{i}, ry(i)*n(i)*ry(i+1), M);
    dx = norm(cry-y{i}, 'fro')/norm(cry, 'fro');
    max_dx = max(max_dx, dx);

    % Truncation and enrichment
    if ((dir>0)&&(i<d))
        cry = reshape(cry, ry(i)*n(i), ry(i+1)*M);
        if (M==1)&&(multrank)
            % Try to accelerate the rank growth
            cry = [cry, reshape(y{i}, ry(i)*n(i), ry(i+1))];
        end;
        if (strcmp(renorm, 'gram'))
            [u,~,v]=svdgram(cry, tol/sqrt(d));
            v = v.';
            if (size(u,2)>rmax)
                u = u(:,1:rmax);
                v = v(:,1:rmax);
            end;
            r = size(u,2);
        else
            [u,s,v]=svd(cry, 'econ');
            s = diag(s);
            r = my_chop2(s, tol*norm(s)/sqrt(d));
            r = min(r,rmax);
            u = u(:,1:r);
            v = conj(v(:,1:r))*diag(s(1:r));
        end;
        if (M==1)&&(multrank)
            % Remove auxiliary previous solution
            v = reshape(v, ry(i+1), 2, r);
            v = v(:,1,:);
            v = reshape(v, ry(i+1), r);
        end;

        % Prepare enrichment, if needed
        if (kickrank+kickrank2>0)
            cry = u*v.';
            cry = reshape(cry, ry(i)*n(i)*ry(i+1), M);
            % For updating z
            crys = cry.';
            crys = reshape(crys, M*ry(i)*n(i), ry(i+1));
            crys = crys*phizy{i+1};
            crys = reshape(crys, M, ry(i)*n(i)*rz(i+1));
            crys = crys.';
            cryz = reshape(crys, ry(i), n(i)*rz(i+1)*M);
            cryz = phizy{i}*cryz;
            cryz = reshape(cryz, rz(i)*n(i)*rz(i+1), M);

            crz = proj_sum(phizx(i,:), X(i,:), phizx(i+1,:), c);
            crz = crz/nrms(i) - cryz;
            nrmz = norm(crz, 'fro');
            crz = reshape(crz, rz(i)*n(i), rz(i+1)*M);
            [crz,~,~]=svd(crz, 'econ');
            crz = crz(:, 1:min(size(crz,2), kickrank));
            if (kickrank2>0)
                crz = [crz, randn(rz(i)*n(i), kickrank2)];
                [crz,~]=qr(crz,0);
            end;
            % For adding into solution
            if (fkick)
                crs = proj_sum(phiyx(i,:), X(i,:), phizx(i+1,:), c);
                crs = crs/nrms(i) - crys;
                crs = reshape(crs, ry(i)*n(i), rz(i+1)*M);
                [crs,~,~]=svd(crs, 'econ');
                crs = crs(:, 1:min(size(crs,2), kickrank));
                u = [u,crs];
                if (strcmp(renorm, 'gram'))&&(ry(i)*n(i)>5*(ry(i+1)+rz(i+1)))
                    [u,~,R]=svdgram(u);
                else
                    [u,R]=qr(u, 0);
                end;
                v = [v, zeros(ry(i+1)*M, size(crs,2))];
                v = v*R.';
                r = size(u, 2);
            end;
        end;
        y{i} = reshape(u, ry(i), n(i), r);

        cr2 = reshape(y{i+1}, ry(i+1), n(i+1)*ry(i+2));
        v = reshape(v, ry(i+1), M*r);
        cr2 = v.'*cr2;
        cr2 = reshape(cr2, M, r*n(i+1)*ry(i+2));
        y{i+1} = reshape(cr2.', r, n(i+1), ry(i+2), M);

        ry(i+1) = r;

        [phiyx(i+1,:), nrms(i)] = compute_next_Phi(phiyx(i,:), y{i}, X(i,:), 'lr', can);

        if (kickrank+kickrank2>0)
            rz(i+1) = size(crz, 2);
            z{i} = reshape(crz, rz(i), n(i), rz(i+1));
            % z{i+1} will be recomputed from scratch in the next step
            phizx(i+1,:) = compute_next_Phi(phizx(i,:), z{i}, X(i,:), 'lr', can, nrms(i));
            phizy(i+1) = compute_next_Phi(phizy(i), z{i}, y(i), 'lr');
        end;
    elseif ((dir<0)&&(i>1))
        cry = reshape(cry.', M*ry(i), n(i)*ry(i+1));
        if (M==1)&&(multrank)
            % Try to accelerate the rank growth
            cry = [cry; reshape(y{i}, ry(i), n(i)*ry(i+1))];
        end;
        [u,s,v]=svd(cry, 'econ');
        s = diag(s);
        r = my_chop2(s, tol*norm(s)/sqrt(d));
        r = min(r,rmax);
        u = u(:,1:r)*diag(s(1:r));
        v = conj(v(:,1:r));

        if (M==1)&&(multrank)
            % Remove auxiliary previous solution
            u = reshape(u, ry(i), 2, r);
            u = u(:,1,:);
            u = reshape(u, ry(i), r);
        end;

        % Prepare enrichment, if needed
        if (kickrank+kickrank2>0)
            cry = u*v.';
            cry = reshape(cry, M, ry(i)*n(i)*ry(i+1));
            % For updating z
            crys = cry.';
            crys = reshape(crys, ry(i), n(i)*ry(i+1)*M);
            crys = phizy{i}*crys;
            crys = reshape(crys, rz(i)*n(i)*ry(i+1), M);
            cryz = reshape(crys.', M*rz(i)*n(i), ry(i+1));
            cryz = cryz*phizy{i+1};
            cryz = reshape(cryz, M, rz(i)*n(i)*rz(i+1));
            cryz = cryz.';
            crz = proj_sum(phizx(i,:), X(i,:), phizx(i+1,:), c);
            crz = crz/nrms(i) - cryz;
            nrmz = norm(crz, 'fro');
            crz = reshape(crz.', M*rz(i), n(i)*rz(i+1));
            [~,~,crz]=svd(crz, 'econ');
            crz = crz(:, 1:min(size(crz,2), kickrank));
            if (kickrank2>0)
                crz = [crz, randn(n(i)*rz(i+1), kickrank2)];
                [crz,~]=qr(crz,0);
            end;
            crz = crz';
            % To add into solution
            crs = proj_sum(phizx(i,:), X(i,:), phiyx(i+1,:), c);
            crs = crs/nrms(i) - crys;
            crs = reshape(crs.', M*rz(i), n(i)*ry(i+1));
            [~,~,crs]=svd(crs, 'econ');
            crs = crs(:, 1:min(size(crs,2), kickrank));
            v = [v,conj(crs)];
            if (strcmp(renorm, 'gram'))&&(ry(i+1)*n(i)>5*(ry(i)+rz(i)))
                [v,~,R]=svdgram(v);
            else
                [v,R]=qr(v, 0);
            end;
            u = [u, zeros(M*ry(i), size(crs,2))];
            u = u*R.';
            r = size(v, 2);
        end;
        y{i} = reshape(v.', r, n(i), ry(i+1));

        cr2 = reshape(y{i-1}, ry(i-1)*n(i-1), ry(i));
        u = reshape(u, M, ry(i)*r);
        u = u.';
        u = reshape(u, ry(i), r*M);
        cr2 = cr2*u;
        cr2 = reshape(cr2, ry(i-1), n(i-1), r, M);
        y{i-1} = cr2;

        ry(i) = r;

        [phiyx(i,:), nrms(i)] = compute_next_Phi(phiyx(i+1,:), y{i}, X(i,:), 'rl', can);

        if (kickrank+kickrank2>0)
            rz(i) = size(crz, 1);
            z{i} = reshape(crz, rz(i), n(i), rz(i+1));
            % z{i+1} will be recomputed from scratch in the next step
            phizx(i,:) = compute_next_Phi(phizx(i+1,:), z{i}, X(i,:), 'rl', can, nrms(i));
            phizy(i) = compute_next_Phi(phizy(i+1), z{i}, y(i), 'rl');
        end;
    else
        y{i} = reshape(y{i}, ry(i), n(i), ry(i+1), M);
    end;

    if (verb>1)
        fprintf('amen-sum: swp=[%d,%d], dx=%3.3e, r=%d, |y|=%3.3e, |z|=%3.3e\n', swp, i, dx, r, norm(cry, 'fro'), nrmz);
    end;

    % Stopping or reversing
    if ((dir>0)&&(i==d))||((dir<0)&&(i==1))
        if (verb>0)
            fprintf('amen-sum: swp=%d{%d}, max_dx=%3.3e, max_r=%d\n', swp, (1-dir)/2, max_dx, max(ry));
        end;
        if ((max_dx<tol)||(swp==nswp))&&(dir>0)
            break;
        end;
        max_dx = 0;
        if (dir>0); swp = swp+1; end;
        dir = -dir;
    else
        i = i+dir;
    end;
end;

y{d} = reshape(cry, ry(d), n(d), M);

% Distribute norms equally...
nrms = exp(sum(log(nrms))/d);
% ... and plug them into y
for i=1:d
    y{i} = y{i}*nrms;
end;

if (vectype==1)
    y = cell2core(tt_tensor, y);
    z = cell2core(tt_tensor, z);
end;

end

function [Phi,nrm] = compute_next_Phi(Phi_prev, x, Y, direction, can, extnrm)
% Performs the recurrent Phi (or Psi) matrix computation
% Phi = Phi_prev * (x'y).
% If direction is 'lr', computes Psi
% if direction is 'rl', computes Phi
% A can be empty, then only x'y is computed.

% Phi1:  rx1, ry1
% Phi2:  ry2, rx2

if (nargin<5)||(isempty(can))
    can = false;
end;
if (nargin<6)
    extnrm = [];
end;

rx1 = size(x,1); n = size(x,2); rx2 = size(x,3);
N = numel(Y);
if (can) % We are working with the canonical format
    Phi = cell(1,1);
    R = size(Y{1},2);
    % Phi1: rx1, R
    % Phi2: R, rx2

    if (strcmp(direction, 'lr'))
        %lr: Phi1
        x = reshape(x, rx1, n*rx2);
        Phi{1} = x'*Phi_prev{1};
        Phi{1} = reshape(Phi{1}, n*rx2, R);
        Y = repmat(Y{1}, rx2, 1); % equalize the sizes for Hadamard product
        Phi{1} = Phi{1}.*Y;
        Phi{1} = reshape(Phi{1}, n, rx2*R);
        Phi{1} = sum(Phi{1}, 1);
        Phi{1} = reshape(Phi{1}, rx2, R);
        if (nargout>1)
            % Extract the scale to prevent overload
            nrm = norm(Phi{1}, 'fro');
            if (nrm>0)
                Phi{1} = Phi{1}/nrm;
            else
                nrm=1;
            end;
        elseif (~isempty(extnrm))
            % Override the normalization by the external one
            Phi{1} = Phi{1}/extnrm;
        end;
    else
        %rl: Phi2
        x = reshape(x, rx1*n, rx2);
        Phi{1} = Phi_prev{1}*x';
        Phi{1} = reshape(Phi{1}, R, rx1*n);
        Y = Y{1}.';
        Y = repmat(Y,rx1,1); % equalize the sizes for Hadamard product
        Y = reshape(Y, R, rx1*n);
        Phi{1} = Phi{1}.*Y;
        Phi{1} = reshape(Phi{1}, R*rx1, n);
        Phi{1} = sum(Phi{1}, 2);
        Phi{1} = reshape(Phi{1}, R, rx1);
        if (nargout>1)
            % Extract the scale to prevent overload
            nrm = norm(Phi{1}, 'fro');
            if (nrm>0)
                Phi{1} = Phi{1}/nrm;
            else
                nrm=1;
            end;
        elseif (~isempty(extnrm))
            % Override the normalization by the external one
            Phi{1} = Phi{1}/extnrm;
        end;
    end;
else % a set of TT-tensors
    Phi = cell(1,N);
    if (strcmp(direction, 'lr'))
        %lr: Phi1
        x = reshape(x, rx1, n*rx2);
        if (nargout>1)
            nrm = 0;
        end;
        for i=1:N
            y = Y{i};
            ry1 = size(y,1); ry2 = size(y,3);
            Phi{i} = x'*Phi_prev{i};
            Phi{i} = reshape(Phi{i}, n, rx2*ry1);
            Phi{i} = Phi{i}.';
            Phi{i} = reshape(Phi{i}, rx2, ry1*n);
            y = reshape(y, ry1*n, ry2);
            Phi{i} = Phi{i}*y;
            Phi{i} = reshape(Phi{i}, rx2, ry2);
            if (nargout>1)
                nrm = max(nrm, norm(Phi{i}, 'fro'));
            end;
        end;
        if (nargout>1)
            % Extract the scale to prevent overload
            if (nrm>0)
                for i=1:N
                    Phi{i} = Phi{i}/nrm;
                end;
            else
                nrm=1;
            end;
        elseif (~isempty(extnrm))
            % Override the normalization
            for i=1:N
                Phi{i} = Phi{i}/extnrm;
            end;
        end;
    else
        %rl: Phi2
        x = reshape(x, rx1, n*rx2);
        if (nargout>1)
            nrm = 0;
        end;
        for i=1:N
            y = Y{i};
            ry1 = size(y,1); ry2 = size(y,3);
            y = reshape(y, ry1*n, ry2);
            Phi{i} = y*Phi_prev{i};
            Phi{i} = reshape(Phi{i}, ry1, n*rx2);
            Phi{i} = Phi{i}*x';
            Phi{i} = reshape(Phi{i}, ry1, rx1);
            if (nargout>1)
                nrm = max(nrm, norm(Phi{i}, 'fro'));
            end;
        end;
        if (nargout>1)
            % Extract the scale to prevent overload
            if (nrm>0)
                for i=1:N
                    Phi{i} = Phi{i}/nrm;
                end;
            else
                nrm=1;
            end;
        elseif (~isempty(extnrm))
            % Override the normalization
            for i=1:N
                Phi{i} = Phi{i}/extnrm;
            end;
        end;
    end;
end;

end

function [x,r]=gen_rand(n,d,r)
% Generate an orthogonal random vector
if (numel(r)==1)
    r = [1; r*ones(d-1,1); 1];
end;
x = cell(d,1);
for i=1:d
    cr = randn(r(i)*n(i), r(i+1));
    [cr,~]=qr(cr,0);
    r(i+1) = size(cr,2);
    x{i} = reshape(cr, r(i), n(i), r(i+1));
end;
end

function [y]=proj_sum(Phi1, X, Phi2, c)
N = size(c,1);
M = size(c,2);

ry1 = size(Phi1{1},1);
ry2 = size(Phi2{1},2);

if (numel(X)==1) % Canonical format
    X = X{1};
    n = size(X,1);
    Phi1 = repmat(Phi1{1}, n, 1); % size ry1*n, R
    X = reshape(X,  1, n*N);
    X = repmat(X, ry1, 1);
    X = reshape(X, ry1*n, N);
    y = X.*Phi1;
    c = repmat(c, ry2, 1); % size N*ry2, M
    c = reshape(c, N, ry2*M);
    Phi2 = repmat(Phi2{1}, 1, M); % size N, ry2*M
    Phi2 = Phi2.*c;
    y = y*Phi2;
    y = reshape(y, ry1*n*ry2, M);
    if (issparse(y))
        y = full(y);
    end;
else
    n = size(X{1}, 2);
    y = zeros(ry1*n*ry2, M);
    for j=1:N
        rx1 = size(X{j},1); rx2 = size(X{j},3);
        cry = reshape(X{j}, rx1, n*rx2);
        cry = Phi1{j}*cry;
        cry = reshape(cry, ry1*n, rx2);
        cry = cry*Phi2{j};
        cry = reshape(cry, ry1*n*ry2, 1);
        cry = cry*c(j,:);
        y = y+cry;
    end;
end;
end