function [data] = ft_freqsimulation(cfg)

% FT_FREQSIMULATION simulates channel-level time-series data . The data is built up
% from different frequencies and can contain a signal in which the different
% frequencies interact (i.e. cross-frequency coherent). Different methods are
% possible to make data with specific properties.
%
% Use as
%   [data] = ft_freqsimulation(cfg)
% which will return a raw data structure that resembles the output of
% FT_PREPROCESSING.
%
% The configuration options can include
%   cfg.method     = The methods are explained in more detail below, but they can be
%                     'superimposed'    simply add the contribution of the different frequencies
%                     'broadband'       create a single broadband signal component
%                     'phalow_amphigh'  phase of low freq correlated with amplitude of high freq
%                     'amplow_amphigh'  amplitude of low freq correlated with amplithude of high freq
%                     'phalow_freqhigh' phase of low freq correlated with frequency of high signal
%                     'asymmetric'      single signal component with asymmetric positive/negative deflections
%   cfg.output     = which channels should be in the output data, can be 'mixed' or 'all' (default = 'all')
%   cfg.randomseed = 'yes' or a number or vector with the seed value (default = 'yes')
%
% The number of trials and the time axes of the trials can be specified by
%   cfg.fsample    = simulated sample frequency (default = 1200)
%   cfg.trllen     = length of simulated trials in seconds (default = 1)
%   cfg.numtrl     = number of simulated trials (default = 1)
%   cfg.baseline   = number (default = 0)
% or by
%   cfg.time       = cell-array with one time axis per trial, which are for example obtained from an existing dataset
%
% For each of the methods default parameters are configured to generate
% example data, including noise. To get full control over the generated
% data you should explicitely set all parameters involved in the method
% of your choise. The interpretation of the following signal components
% depends on the specified method:
%
% cfg.s1.freq     = frequency of signal 1
% cfg.s1.phase    = phase (in rad) relative to cosine of signal 1  (default depends on method)
%                 = number or 'random'
% cfg.s1.ampl     = amplitude of signal 1
% cfg.s2.freq     = frequency of signal 2
% cfg.s2.phase    = phase (in rad) relative to cosine of signal 1  (default depends on method)
%                 = number or 'random'
% cfg.s2.ampl     = amplitude of signal 2
% cfg.s3.freq     = frequency of signal 3
% cfg.s3.phase    = phase (in rad) relative to cosine of signal 1  (default depends on method)
%                 = number or 'random'
% cfg.s3.ampl     = amplitude of signal 3
% cfg.s4.freq     = frequency of signal 4
% cfg.s4.phase    = phase (in rad) relative to cosine of signal 1  (default depends on method)
%                 = number or 'random'
% cfg.s4.ampl     = amplitude of signal 4
%
% cfg.n1.ampl     = root-mean-square amplitude of wide-band signal prior to filtering
% cfg.n1.bpfreq   = [Flow Fhigh]
% cfg.n2.ampl     = root-mean-square amplitude of wide-band signal prior to filtering
% cfg.n2.bpfreq   = [Flow Fhigh]
%
% cfg.asymmetry   = amount of asymmetry (default = 0, which is none)
% cfg.noise.ampl  = amplitude of noise
%
%
% In the method 'superimposed' the signal contains just the sum of the different frequency contributions:
%     s1: first frequency
%     s2: second frequency
%     s3: third frequency
% and the output consists of the following channels:
%     1st channel: mixed signal = s1 + s2 + s3 + noise
%     2nd channel: s1
%     3rd channel: s2
%     4th channel: s3
%     5th channel: noise
%
% In the method 'broadband' the signal contains a the superposition of two
% broadband signal components, which are created by bandpass filtering a
% Gaussian noise signal:
%     n1: first broadband signal
%     n2: second broadband signal
% and the output consists of the following channels:
%     1st channel: mixed signal = n1 + n2 + noise
%     2nd channel: n1
%     3rd channel: n2
%     4th channel: noise
%
% In the method 'phalow_amphigh' the signal is build up of 4 components; s1, s2, s3 and noise:
%     s1: amplitude modulation (AM), frequency of this signal should be lower than s2
%     s2: second frequency, frequncy that becomes amplitude modulated
%     s3: DC shift of s1, should have frequency of 0
% and the output consists of the following channels:
%     1st channel: mixed signal = (s1 + s3)*s2 + noise,
%     2nd channel: s1
%     3rd channel: s2
%     4th channel: s3
%     5th channel: noise
%
% In the method 'amplow_amphigh' the signal is build up of 5 components; s1, s2, s3, s4 and noise.
%     s1: first frequency
%     s2: second frequency
%     s3: DC shift of s1 and s2, should have frequency of 0
%     s4: amplitude modulation (AM), frequency of this signal should be lower than s1 and s2
% and the output consists of the following channels:
%     1st channel: mixed signal = (s4 + s3)*s1 + (s4 + s3)*s2 + noise,
%     2nd channel: s1
%     3rd channel: s2
%     4th channel: s3
%     5th channel: noise
%     6th channel: s4
%     7th channel: mixed part 1: (s4 + s3)*s1
%     8th channel: mixed part 2: (s4 + s3)*s2
%
% In the method 'phalow_freqhigh' a frequency modulated signal is created.
%   signal is build up of 3 components; s1, s2 and noise.
%     s1: represents the base signal that will be modulated
%     s2: signal that will be used for the frequency modulation
% and the output consists of the following channels:
%     1st channel: mixed signal = s1.ampl * cos(ins_pha) + noise
%     2nd channel: s1
%     3rd channel: s2
%     4th channel: noise
%     5th channel: inst_pha_base   instantaneous phase of the high (=base) frequency signal s1
%     6th channel: inst_pha_mod    low frequency phase modulation, this is equal to s2
%     7th channel: inst_pha        instantaneous phase, i.e. inst_pha_base + inst_pha_mod
%
% In the method 'asymmetric' there is only one periodic signal, but that
% signal is more peaked for the positive than for the negative deflections.
% The average of the signal over time is zero.
%     s1: represents the frequency of the base signal
% and the output consists of the following channels:
%     1st channel: mixed signal = asymmetric signal + noise
%     2nd channel: sine wave with base frequency and phase, i.e. s1
%     3rd channel: asymmetric signal
%     4th channel: noise
%
% See also FT_FREQANALYSIS, FT_TIMELOCKSIMULATION, FT_DIPOLESIMULATION,
% FT_CONNECTIVITYSIMULATION

% Copyright (C) 2007-2008, Ingrid Nieuwenhuis & Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% these are used by the ft_preamble/ft_postamble function and scripts
ft_revision = '$Id$';
ft_nargin   = nargin;
ft_nargout  = nargout;

% do the general setup of the function
ft_defaults
ft_preamble init
ft_preamble debug
ft_preamble provenance
ft_preamble randomseed

% the ft_abort variable is set to true or false in ft_preamble_init
if ft_abort
  return
end

% return immediately after distributed execution
if ~isempty(ft_getopt(cfg, 'distribute'))
  return
end

% set defaults
if ~isfield(cfg, 'method'),        cfg.method = 'phalow_amphigh';         end
if ~isfield(cfg, 'output'),        cfg.output = 'all';                    end
if ~isfield(cfg, 'time'),          cfg.time = [];                         end

if isempty(cfg.time)
  cfg.fsample   = ft_getopt(cfg, 'fsample', 1200);
  cfg.trllen    = ft_getopt(cfg, 'trllen', 1);
  cfg.numtrl    = ft_getopt(cfg, 'numtrl', 1);
  cfg.baseline  = ft_getopt(cfg, 'baseline', 0);
else
  cfg.trllen  = [];                         % can be variable
  cfg.fsample = 1/mean(diff(cfg.time{1}));  % determine from time-axis
  cfg.numtrl  = length(cfg.time);
end

if strcmp(cfg.method, 'superimposed')
  if ~isfield(cfg, 's1'),           cfg.s1 = [];                          end
  if ~isfield(cfg.s1, 'freq'),      cfg.s1.freq  = 10;                    end
  if ~isfield(cfg.s1, 'phase'),     cfg.s1.phase = 0;                     end
  if ~isfield(cfg.s1, 'ampl'),      cfg.s1.ampl  = 1;                     end
  if ~isfield(cfg, 's2'),           cfg.s2 = [];                          end
  if ~isfield(cfg.s2, 'freq'),      cfg.s2.freq  = 20;                    end
  if ~isfield(cfg.s2, 'phase'),     cfg.s2.phase = 0;                     end
  if ~isfield(cfg.s2, 'ampl'),      cfg.s2.ampl  = 0;                     end
  if ~isfield(cfg, 's3'),           cfg.s3 = [];                          end
  if ~isfield(cfg.s3, 'freq'),      cfg.s3.freq  = 30;                    end
  if ~isfield(cfg.s3, 'phase'),     cfg.s3.phase = 0;                     end
  if ~isfield(cfg.s3, 'ampl'),      cfg.s3.ampl  = 0;                     end
end

if strcmp(cfg.method, 'broadband')
  if ~isfield(cfg, 'n1'),           cfg.n1 = [];                          end
  if ~isfield(cfg.n1, 'ampl'),      cfg.n1.ampl  = 1;                     end
  if ~isfield(cfg.n1, 'bpfreq'),    cfg.n1.bpfreq  = [30 50];             end
  if ~isfield(cfg, 'n2'),           cfg.n2 = [];                          end
  if ~isfield(cfg.n2, 'ampl'),      cfg.n2.ampl  = 1;                     end
  if ~isfield(cfg.n2, 'bpfreq'),    cfg.n2.bpfreq  = [80 120];            end
end

if strcmp(cfg.method, 'phalow_amphigh')
  if ~isfield(cfg, 's1'),           cfg.s1 = [];                          end
  if ~isfield(cfg.s1, 'freq'),      cfg.s1.freq = 3;                      end
  if ~isfield(cfg.s1, 'phase'),     cfg.s1.phase = -1*pi;                 end
  if ~isfield(cfg.s1, 'ampl'),      cfg.s1.ampl = 1;                      end
  if ~isfield(cfg, 's2'),           cfg.s2 = [];                          end
  if ~isfield(cfg.s2, 'freq'),      cfg.s2.freq = 20;                     end
  if ~isfield(cfg.s2, 'phase'),     cfg.s2.phase = 0;                     end
  if ~isfield(cfg.s2, 'ampl'),      cfg.s2.ampl = 1;                      end
  if ~isfield(cfg, 's3'),           cfg.s3 = [];                          end
  if ~isfield(cfg.s3, 'freq'),      cfg.s3.freq = 0;                      end
  if ~isfield(cfg.s3, 'phase'),     cfg.s3.phase = 0;                     end
  if ~isfield(cfg.s3, 'ampl'),      cfg.s3.ampl = cfg.s1.ampl;            end
end

if strcmp(cfg.method, 'amplow_amphigh')
  if ~isfield(cfg, 's1'),           cfg.s1 = [];                          end
  if ~isfield(cfg.s1, 'freq'),      cfg.s1.freq = 6;                      end
  if ~isfield(cfg.s1, 'phase'),     cfg.s1.phase = 0;                     end
  if ~isfield(cfg.s1, 'ampl'),      cfg.s1.ampl = 1;                      end
  if ~isfield(cfg, 's2'),           cfg.s2 = [];                          end
  if ~isfield(cfg.s2, 'freq'),      cfg.s2.freq = 20;                     end
  if ~isfield(cfg.s2, 'phase'),     cfg.s2.phase = 0;                     end
  if ~isfield(cfg.s2, 'ampl'),      cfg.s2.ampl = 1;                      end
  if ~isfield(cfg, 's4'),           cfg.s4 = [];                          end
  if ~isfield(cfg.s4, 'freq'),      cfg.s4.freq = 1;                      end
  if ~isfield(cfg.s4, 'phase'),     cfg.s4.phase = -1*pi;                 end
  if ~isfield(cfg.s4, 'ampl'),      cfg.s4.ampl = 1;                      end
  if ~isfield(cfg, 's3'),           cfg.s3 = [];                          end
  if ~isfield(cfg.s3, 'freq'),      cfg.s3.freq = 0;                      end
  if ~isfield(cfg.s3, 'phase'),     cfg.s3.phase = 0;                     end
  if ~isfield(cfg.s3, 'ampl'),      cfg.s3.ampl = cfg.s4.ampl;            end
end

if strcmp(cfg.method, 'phalow_freqhigh')
  if ~isfield(cfg, 's1'),           cfg.s1 = [];                          end
  if ~isfield(cfg.s1, 'freq'),      cfg.s1.freq = 20;                     end
  if ~isfield(cfg.s1, 'phase'),     cfg.s1.phase = 0;                     end
  if ~isfield(cfg.s1, 'ampl'),      cfg.s1.ampl = 1;                      end
  if ~isfield(cfg, 's2'),           cfg.s2 = [];                          end
  if ~isfield(cfg.s2, 'freq'),      cfg.s2.freq = 2;                      end
  if ~isfield(cfg.s2, 'phase'),     cfg.s2.phase = -0.5 * pi;             end %then base freq at t=0
  if ~isfield(cfg.s2, 'ampl'),      cfg.s2.ampl = pi;                     end
end

if strcmp(cfg.method, 'asymmetric')
  if ~isfield(cfg, 's1'),           cfg.s1 = [];                          end
  if ~isfield(cfg.s1, 'freq'),      cfg.s1.freq = 6;                      end
  if ~isfield(cfg.s1, 'phase'),     cfg.s1.phase = 0;                     end
  if ~isfield(cfg.s1, 'ampl'),      cfg.s1.ampl = 1;                      end
  if ~isfield(cfg, 'noise'),        cfg.noise = [];                       end
  if ~isfield(cfg.noise, 'ampl'),   cfg.noise.ampl = 0.1;                 end % default should not be too high
end

if ~isfield(cfg, 'noise'),         cfg.noise = [];                        end
if ~isfield(cfg.noise, 'ampl'),    cfg.noise.ampl = 1;                    end

if ~isempty(cfg.time)
  % use the user-supplied time vectors
  timevec = cfg.time;
else
  % give the user some feedback
  ft_debug('using %f as samping frequency', cfg.fsample);
  ft_debug('using %d trials of %f seconds long', cfg.numtrl, cfg.trllen);
  nsample = round(cfg.trllen*cfg.fsample);
  timevec = cell(1, cfg.numtrl);
  for iTr = 1:cfg.numtrl
    timevec{iTr} = (((1:nsample)-1)/cfg.fsample) - cfg.baseline;
  end
end

% give the user some feedback
ft_info('simulating data using %s method', cfg.method);

%%%%%%% SUPERIMPOSED, SIMPLY ADD THE SIGNALS %%%%%%%%%
if strcmp(cfg.method, 'superimposed')

  % make data
  for iTr = 1:length(timevec)
    if ischar(cfg.s1.phase); phase_s1 = rand * 2 * pi; else phase_s1 = cfg.s1.phase; end
    if ischar(cfg.s2.phase); phase_s2 = rand * 2 * pi; else phase_s2 = cfg.s2.phase; end
    if ischar(cfg.s3.phase); phase_s3 = rand * 2 * pi; else phase_s3 = cfg.s3.phase; end

    s1    = cfg.s1.ampl*cos(2*pi*cfg.s1.freq*timevec{iTr} + phase_s1);
    s2    = cfg.s2.ampl*cos(2*pi*cfg.s2.freq*timevec{iTr} + phase_s2);
    s3    = cfg.s3.ampl*cos(2*pi*cfg.s3.freq*timevec{iTr} + phase_s3);
    noise = cfg.noise.ampl*randn(size(timevec{iTr}));
    mix   = s1 + s2 + s3 + noise;

    data.trial{iTr}(1,:) = mix;
    if strcmp(cfg.output, 'all')
      data.trial{iTr}(2,:) = s1;
      data.trial{iTr}(3,:) = s2;
      data.trial{iTr}(4,:) = s3;
      data.trial{iTr}(5,:) = noise;
    end
    data.time{iTr} = timevec{iTr};
  end % for iTr

  data.label{1} = 'mix';
  if strcmp(cfg.output, 'all')
    data.label{2} = 's1';
    data.label{3} = 's2';
    data.label{4} = 's3';
    data.label{5} = 'noise';
  end
  data.fsample = cfg.fsample;

  %%%%%%% SUPERIMPOSED BROADBAND SIGNAL %%%%%%%%%
elseif strcmp(cfg.method, 'broadband')

  % make data
  for iTr = 1:length(timevec)
    n1    = ft_preproc_bandpassfilter(cfg.n1.ampl*randn(size(timevec{iTr})), cfg.fsample, cfg.n1.bpfreq);
    n2    = ft_preproc_bandpassfilter(cfg.n2.ampl*randn(size(timevec{iTr})), cfg.fsample, cfg.n2.bpfreq);
    noise = cfg.noise.ampl*randn(size(timevec{iTr}));
    mix   = n1 + n2 + noise;

    data.trial{iTr}(1,:) = mix;
    if strcmp(cfg.output, 'all')
      data.trial{iTr}(2,:) = n1;
      data.trial{iTr}(3,:) = n2;
      data.trial{iTr}(4,:) = noise;
    end
    data.time{iTr} = timevec{iTr};
  end % for iTr

  data.label{1} = 'mix';
  if strcmp(cfg.output, 'all')
    data.label{2} = 'n1';
    data.label{3} = 'n2';
    data.label{4} = 'noise';
  end
  data.fsample = cfg.fsample;

  %%%%%%% PHASE TO AMPLITUDE CORRELATION %%%%%%%%%
elseif strcmp(cfg.method, 'phalow_amphigh')

  % sanity checks
  if cfg.s2.freq < cfg.s1.freq
    ft_error('with method is phalow_amphigh freq s2 should be higher than freq s1')
  end
  if cfg.s2.freq > cfg.fsample/2
    ft_error('you cannot have a frequency higher than the sample frequency/2')
  end
  if cfg.s3.freq ~= 0 || cfg.s3.phase ~= 0
    ft_warning('for method phalow_amphigh s3 is DC and therefore expect freq and phase to be zero but they are not')
  end
  if cfg.s3.ampl < cfg.s1.ampl
    ft_warning('expect amplitude s3 (=DC) not to be smaller than amplitude s1 (=low frequency)')
  end

  % make data
  for iTr = 1:length(timevec)

    if ischar(cfg.s1.phase); phase_AM   = rand * 2 * pi; else phase_AM   = cfg.s1.phase;  end
    if ischar(cfg.s2.phase); phase_high = rand * 2 * pi; else phase_high = cfg.s2.phase; end
    if ischar(cfg.s3.phase); phase_DC   = rand * 2 * pi; else phase_DC   = cfg.s3.phase;   end
    high  = cfg.s2.ampl*cos(2*pi*cfg.s2.freq*timevec{iTr} + phase_high);
    AM    = cfg.s1.ampl*cos(2*pi*cfg.s1.freq*timevec{iTr} + phase_AM);
    DC    = cfg.s3.ampl*cos(2*pi*0*timevec{iTr} + phase_DC);
    noise = cfg.noise.ampl*randn(size(timevec{iTr}));
    mix   = ((AM + DC) .* high) + noise;

    data.trial{iTr}(1,:) = mix;
    if strcmp(cfg.output, 'all')
      data.trial{iTr}(2,:) = AM;
      data.trial{iTr}(3,:) = high;
      data.trial{iTr}(4,:) = DC;
      data.trial{iTr}(5,:) = noise;
    end
    data.time{iTr} = timevec{iTr};
  end % for iTr

  data.label{1} = 'mix';
  if strcmp(cfg.output, 'all')
    data.label{2} = 's1 (AM)';
    data.label{3} = 's2 (high)';
    data.label{4} = 's3 (DC)';
    data.label{5} = 'noise';
  end
  data.fsample = cfg.fsample;

  %%%%%%% POWER TO POWER CORRELATION %%%%%%%%%
elseif strcmp(cfg.method, 'amplow_amphigh')

  % sanity checks
  if cfg.s2.freq < cfg.s1.freq || cfg.s1.freq < cfg.s4.freq
    ft_error('with method is powlow_powhigh freq s4 < s1 < s2')
  end
  if cfg.s2.freq > cfg.fsample/2
    ft_error('you cannot have a frequency higher than the sample frequency/2')
  end
  if cfg.s3.freq ~= 0 || cfg.s3.phase ~= 0
    ft_warning('for method powlow_powhigh s3 is DC and therefore expect freq and phase to be zero but they are not')
  end
  if cfg.s3.ampl < cfg.s4.ampl
    ft_warning('expect amplitude s3 (=DC) not to be smaller than amplitude s4 (= AM frequency)')
  end

  % make data
  for iTr = 1:length(timevec)

    if ischar(cfg.s1.phase); phase_low  = rand * 2 * pi; else phase_low = cfg.s1.phase;    end
    if ischar(cfg.s2.phase); phase_high = rand * 2 * pi; else phase_high = cfg.s2.phase;   end
    if ischar(cfg.s3.phase); phase_DC   = rand * 2 * pi; else phase_DC = cfg.s3.phase;     end
    if ischar(cfg.s4.phase); phase_AM   = rand * 2 * pi; else phase_AM = cfg.s4.phase; end
    high     = cfg.s2.ampl*cos(2*pi*cfg.s2.freq*timevec{iTr} + phase_high);
    low      = cfg.s1.ampl*cos(2*pi*cfg.s1.freq*timevec{iTr} + phase_low);
    AM       = cfg.s4.ampl*cos(2*pi*cfg.s4.freq*timevec{iTr} + phase_AM);
    DC       = cfg.s3.ampl*cos(2*pi*0*timevec{iTr} + phase_DC);
    noise    = cfg.noise.ampl*randn(size(timevec{iTr}));
    lowmix  = ((AM + DC) .* low);
    highmix = ((AM + DC) .* high);
    mix     = lowmix + highmix + noise;

    data.trial{iTr}(1,:) = mix;
    if strcmp(cfg.output, 'all')
      data.trial{iTr}(2,:) = low;
      data.trial{iTr}(3,:) = high;
      data.trial{iTr}(4,:) = DC;
      data.trial{iTr}(5,:) = noise;
      data.trial{iTr}(6,:) = AM;
      data.trial{iTr}(7,:) = lowmix;
      data.trial{iTr}(8,:) = highmix;
    end
    data.time{iTr} = timevec{iTr};
  end % for iTr

  data.label{1} = 'mix';
  if strcmp(cfg.output, 'all')
    data.label{2} = 's1 (low)';
    data.label{3} = 's2 (high)';
    data.label{4} = 's3 (DC)';
    data.label{5} = 'noise';
    data.label{6} = 's4 (AM)';
    data.label{7} = 'mixlow';
    data.label{8} = 'mixhigh';
  end
  data.fsample = cfg.fsample;

  %%%%%%% PHASE TO FREQUENCY CORRELATION %%%%%%%%%
elseif strcmp(cfg.method, 'phalow_freqhigh')

  % sanity checks
  if cfg.s1.freq > cfg.fsample/2 || cfg.s2.freq > cfg.fsample/2
    ft_error('you cannot have a frequency higher than the sample frequency/2')
  end

  % make data
  for iTr = 1:length(timevec)

    if ischar(cfg.s1.phase); phase_s1 = rand * 2 * pi; else phase_s1 = cfg.s1.phase;    end
    if ischar(cfg.s2.phase); phase_s2 = rand * 2 * pi; else phase_s2= cfg.s2.phase;    end
    s1            = cfg.s1.ampl .* cos(2*pi*cfg.s1.freq * timevec{iTr} + phase_s1); % to be modulated signal
    s2            = cfg.s2.ampl .* cos(2*pi*cfg.s2.freq * timevec{iTr} + phase_s2); % modulation of instantaneous phase
    inst_pha_base = 2*pi*cfg.s1.freq * timevec{iTr} + phase_s1; % unmodulated instantaneous phase s1 (linear)
    inst_pha_mod  = s2;                                    % modulation of instantaneous phase
    inst_pha      = inst_pha_base + inst_pha_mod;
    noise         = cfg.noise.ampl*randn(size(timevec{iTr}));
    mix           = cfg.s1.ampl .* cos(inst_pha) + noise;

    data.trial{iTr}(1,:) = mix;
    if strcmp(cfg.output, 'all')
      data.trial{iTr}(2,:) = s1;
      data.trial{iTr}(3,:) = s2;
      data.trial{iTr}(4,:) = noise;
      data.trial{iTr}(5,:) = inst_pha_base;
      data.trial{iTr}(6,:) = inst_pha_mod;
      data.trial{iTr}(7,:) = inst_pha;
    end
    data.time{iTr} = timevec{iTr};
  end % for iTr

  data.label{1} = 'mix';
  if strcmp(cfg.output, 'all')
    data.label{2} = 's1';
    data.label{3} = 's2';
    data.label{4} = 'noise';
    data.label{5} = 'inst phase base';
    data.label{6} = 'inst phase modulation (=s2)';
    data.label{7} = 'inst phase';
  end
  data.fsample = cfg.fsample;

  %%%%%%% ASYMETRIC POSITIVE AND NEGATIVE PEAKS %%%%%%%%%
elseif strcmp(cfg.method, 'asymmetric')

  % make data
  for iTr = 1:length(timevec)
    if ischar(cfg.s1.phase); phase_s1 = rand * 2 *pi; else phase_s1 = cfg.s1.phase; end

    s1    = cfg.s1.ampl*cos(2*pi*cfg.s1.freq*timevec{iTr} + phase_s1);
    tmp   = cos(2*pi*cfg.s1.freq*timevec{iTr} + phase_s1);  % same signal but with unit amplitude
    tmp   = (tmp+1)/2;                                 % scaled and shifted between 0 and 1
    tmp   = tmp.^(cfg.asymmetry+1);                    % made asymmetric
    tmp   = (tmp - mean(tmp))*2*cfg.s1.ampl;           % rescale
    s2    = tmp;
    noise = cfg.noise.ampl*randn(size(timevec{iTr}));
    mix   = s2 + noise;

    data.trial{iTr}(1,:) = mix;
    if strcmp(cfg.output, 'all')
      data.trial{iTr}(2,:) = s1;
      data.trial{iTr}(3,:) = s2;
      data.trial{iTr}(4,:) = noise;
    end
    data.time{iTr} = timevec{iTr};
  end % for iTr

  data.label{1} = 'mix';
  if strcmp(cfg.output, 'all')
    data.label{2} = 's1';
    data.label{3} = 's2';
    data.label{4} = 'noise';
  end
  data.fsample = cfg.fsample;

else
  ft_error('unknown method specified')
end

% do the general cleanup and bookkeeping at the end of the function
ft_postamble debug
ft_postamble randomseed
ft_postamble provenance data
ft_postamble history    data
ft_postamble savevar    data


function [xu,yu,xl,yl,xc,yc]=GenerateNACASeries4Airfoil(afid,dotcount,xmod)

%--------------------------------------------------------------------------
%GenerateNACASeries4Airfoil
%Version 1.20
%Created by Stepen (zerocross_raptor@yahoo.com)
%Created 20 November 2010
%Last modified 30 November 2011
%--------------------------------------------------------------------------
%GenerateNACASeries4Airfoil generates the airfoil vertexes' coordinate of
%the given NACA Series 4 Airfoil. The math equation used to generates the
%airfoil coordinates is based on Theory of Wing Section Chapter 6 by Abbott
%and Doenhoff.
%--------------------------------------------------------------------------
%Syntax:
%[xu,yu,xl,yl,xc,yc]=GenerateNACASeries4Airfoil(afid,dotcount,xmod)
%Input argument:
%- afid (1 x 4 str) specifies NACA Series 4 Airfoil identifier.
%- dotcount (1 x 1 int) specifies the number of vertexes to be generated on
%  the airfoil's camber/mean line.
%- xmod (str) specifies the mode of airfoil vertex distribution. Enter
%  'Uniform' to create uniform vertex distribution or 'Cosine' to create
%  vertex distribution based on cosine function (More vertex on the leading
%  edge region).
%Output argument:
%- xu (i x 1 num) specifies the x axis location of airfoil's upper surface
%  vertexes in fraction of chord. The airfoil's upper surface vertex are
%  arranged from leading edge (the first element of xu) to the trailing
%  edge (the last element of xu).
%- yu (i x 1 num) specifies the y axis location of airfoil's upper surface
%  vertexes in fraction of chord. The airfoil's upper surface vertex are
%  arranged from leading edge (the first element of yu) to the trailing
%  edge (the last element of yu).
%- xl (i x 1 num) specifies the x axis location of airfoil's lower surface
%  vertexes in fraction of chord. The airfoil's lower surface vertex are
%  arranged from leading edge (the first element of xl) to the trailing
%  edge (the last element of xl).
%- yl (i x 1 num) specifies the y axis location of airfoil's lower surface
%  vertexes in fraction of chord. The airfoil's lower surface vertex are
%  arranged from leading edge (the first element of yl) to the trailing
%  edge (the last element of yl).
%- xc (i x 1 num) specifies the x axis location of airfoil's camber line
%  vertexes in fraction of chord. The airfoil's camber line vertex are
%  arranged from leading edge (the first element of xc) to the trailing
%  edge (the last element of xc).
%- yc (i x 1 num) specifies the y axis location of airfoil's camber line
%  vertexes in fraction of chord. The airfoil's camber line vertex are
%  arranged from leading edge (the first element of yc) to the trailing
%  edge (the last element of yc).
%--------------------------------------------------------------------------

%CodeStart-----------------------------------------------------------------
%Checking input afid
    if ~ischar(afid)
        error('Airfoil identifier must be a string!')
    end
    if numel(afid)~=4
        error('Airfoil identifier must be a 4 digit number!')
    end
    if isempty(str2double(afid))
        error('Airfoil identifier must be a 4 digit number!')
    end
%Checking input dotcount
    if numel(dotcount)~=1
        error('Number of vertex must be scalar!')
    end
    if (mod(dotcount,1~=0))||(dotcount<0)
        error('Number of vertex must be positive integer!')
    end
%Checking input xmod
    if nargin<3
        xmod='Cosine';
    end
    if (~strcmpi(xmod,'Uniform'))&&(~strcmpi(xmod,'Cosine'))
        error('Vertex distribution input must be Uniform or Cosine!')
    end
%Assigning identifier to equation coefficient
    id1=str2double(afid(1));
    id2=str2double(afid(2));
    id3=str2double(afid([3,4]));
    m=id1*(1/100);            %Maximum camber
    p=id2*(10/100);           %Maximum camber location
    t=id3*(1/100);            %Maximum thickness
    if t==0
        warning(['Zero thickness airfoil!',...
                 ' Airfoil will be just a camber line!'])
    end
%Calculating x-axis location of camber line vertexes
    panelcount=dotcount-1;
    if strcmpi(xmod,'Uniform')
        panellength=1/panelcount;
        xc=(0:panellength:1)';
    elseif strcmpi(xmod,'Cosine')
        deltadeg=90/panelcount;
        xc=1-cosd(0:deltadeg:90)';
    end
%Preallocating array for speed
    yc=zeros(dotcount,1);
    gc=zeros(dotcount,1);
    yt=zeros(dotcount,1);
    xu=zeros(dotcount,1);
    xl=zeros(dotcount,1);
    yu=zeros(dotcount,1);
    yl=zeros(dotcount,1);
%Calculating y-axis location of camber line vertexes and camber gradient
    if m~=0
        for i=1:1:dotcount
            if xc(i)<=p
                yc(i)=(m/(p^2))*((2*p*xc(i))-(xc(i)^2));
            elseif xc(i)>p
                yc(i)=(m/((1-p)^2))*(1-(2*p)+(2*p*xc(i))-(xc(i)^2));
            end
            gc(i)=(m/p^2)*(2*p+2*xc(i));
        end
    end
%Converting camber gradient to camber slope and normal
    sc=atand(gc);
%Calculating thickness distribution
    for i=1:1:dotcount
        yt(i)=5*t*((0.29690*(xc(i)^0.5))-...
                   (0.12600*xc(i))-...
                   (0.35160*(xc(i)^2))+...
                   (0.28430*(xc(i)^3))-...
                   (0.10150*(xc(i)^4)));
    end
%Generating airfoil vertexes
    for i=1:1:dotcount
        xu(i)=xc(i)-yt(i)*sind(sc(i));
        yu(i)=yc(i)+yt(i)*cosd(sc(i));
        xl(i)=xc(i)+yt(i)*sind(sc(i));
        yl(i)=yc(i)-yt(i)*cosd(sc(i));
    end
%CodeEnd-------------------------------------------------------------------

end


function [a,b,m,r,bbx,bby,f,g] = stokes_q2p1(xy,xyp,mv)
%stokes_q2p1  vectorized Q2-P1 matrix generator
%   [A,B,Q,G,Bx,By,f,g] = stokes_q2p1(xy,xyp,mv);
%   input
%          xy         Q2 nodal coordinate vector
%          xyp        Q1 nodal coordinate vector
%          mv         Q2 element mapping matrix
%   output
%          A          Q2 vector diffusion matrix
%          B          Q2-Q1 divergence matrix
%          Q          Q1 mass matrix
%          G          Q2 vector mass matrix
%          Bx         Q2 x-derivative matrix
%          By         Q2 y-derivative matrix
%          f          velocity rhs vector
%          g          pressure rhs vector
%
%   Natural boundary conditions apply. Dirichlet conditions
%   must be explicitly enforced by calling function flowbc.
%   IFISS function: DJS; 7 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
nngpt=9;
x=xy(:,1); y=xy(:,2);
xp=xyp(:,1); yp=xyp(:,2);
nvtx=length(x); nu=2*nvtx; np=3*length(xp);
nel=length(mv(:,1));  mp=[[1:3:3*nel]',[2:3:3*nel]',[3:3:3*nel]'];
lx=max(x)-min(x); ly=max(y)-min(y);
hx=max(diff(x)); hy=max(diff(y));
fprintf('setting up Q2-P1 matrices...  ')
%
% initialise global matrices
      a = sparse(nu,nu);
      r = sparse(nu,nu);
    bbx = sparse(nvtx,nvtx);
    bby = sparse(nvtx,nvtx);
     bx = sparse(np,nvtx);
     by = sparse(np,nvtx);
	  b = sparse(np,nu);
      m = sparse(np,np);
      f = zeros(nu,1);
      g = zeros(np,1);
%
%
% Gauss point integration rules
      if (nngpt==4)        % 2x2 Gauss points
      gpt=1.0e0/sqrt(3.0e0);
      s(1) = -gpt; t(1) = -gpt; wt(1)=1;
      s(2) =  gpt; t(2) = -gpt; wt(2)=1;
      s(3) =  gpt; t(3) =  gpt; wt(3)=1;
      s(4) = -gpt; t(4) =  gpt; wt(4)=1;
      elseif (nngpt==9)   % 3x3 Gauss points
      gpt=sqrt(0.6);
      s(1) = -gpt; t(1) = -gpt; wt(1)=25/81;
      s(2) =  gpt; t(2) = -gpt; wt(2)=25/81;
      s(3) =  gpt; t(3) =  gpt; wt(3)=25/81;
      s(4) = -gpt; t(4) =  gpt; wt(4)=25/81;
      s(5) =  0.0; t(5) = -gpt; wt(5)=40/81;
      s(6) =  gpt; t(6) =  0.0; wt(6)=40/81;
      s(7) =  0.0; t(7) =  gpt; wt(7)=40/81;
      s(8) = -gpt; t(8) =  0.0; wt(8)=40/81;
      s(9) =  0.0; t(9) =  0.0; wt(9)=64/81;
      else
	  error('Check Gauss point integration specification')
      end
%
% inner loop over elements
      for ivtx = 1:4
      xl_v(:,ivtx) = x(mv(:,ivtx));
      yl_v(:,ivtx) = y(mv(:,ivtx));
	  end
       ae = zeros(nel,9,9);
       re = zeros(nel,9,9);
     bbxe = zeros(nel,9,9);
     bbye = zeros(nel,9,9);
      bxe = zeros(nel,3,9);
      bye = zeros(nel,3,9);
      mpe = zeros(nel,3,3);
       ge = zeros(nel,3);
%
% loop over Gauss points
         for igpt = 1:nngpt
         sigpt=s(igpt);
         tigpt=t(igpt);
         wght=wt(igpt);
%  evaluate derivatives etc
         [jac,invjac,phi,dphidx,dphidy] = deriv(sigpt,tigpt,xl_v,yl_v);
         [psi,dpsidx,dpsidy] = qderiv(sigpt,tigpt,xl_v,yl_v);
         [chi,dchidx,dchidy] = lderiv(sigpt,tigpt,xl_v,yl_v);
            for j = 1:9
               for i = 1:9
               ae(:,i,j)  = ae(:,i,j)  + wght*dpsidx(:,i).*dpsidx(:,j).*invjac(:);
               ae(:,i,j)  = ae(:,i,j)  + wght*dpsidy(:,i).*dpsidy(:,j).*invjac(:);
               re(:,i,j)  = re(:,i,j)  + wght*psi(:,i).*psi(:,j).*jac(:);
               bbxe(:,i,j) = bbxe(:,i,j) - wght*psi(:,i) .*dpsidx(:,j);
               bbye(:,i,j) = bbye(:,i,j) - wght*psi(:,i) .*dpsidy(:,j);
               end
	       for i=1:3
               bxe(:,i,j) = bxe(:,i,j) - wght*chi(:,i) .* dpsidx(:,j);
               bye(:,i,j) = bye(:,i,j) - wght*chi(:,i) .* dpsidy(:,j);
               end
	    end
	    for j=1:3
	       for i=1:3
               mpe(:,i,j) = mpe(:,i,j) + wght*chi(:,i) .*chi(:,j) .*jac(:);
	       end
	    end
%
% end of Gauss point loop
         end
%
%%  element assembly into global matrices
% component velocity matrices ...
      for krow=1:9
	  nrow=mv(:,krow);
          for kcol=1:9
		  ncol=mv(:,kcol);
          a = a + sparse(nrow,ncol,ae(:,krow,kcol),nu,nu);
		  a = a + sparse(nrow+nvtx,ncol+nvtx,ae(:,krow,kcol),nu,nu);
          r = r + sparse(nrow,ncol,re(:,krow,kcol),nu,nu);
		  r = r + sparse(nrow+nvtx,ncol+nvtx,re(:,krow,kcol),nu,nu);
          bbx = bbx + sparse(nrow,ncol,bbxe(:,krow,kcol),nvtx,nvtx);
          bby = bby + sparse(nrow,ncol,bbye(:,krow,kcol),nvtx,nvtx);
          end
          for kcol=1:3
		  ncol=mp(:,kcol);
          bx = bx + sparse(ncol,nrow,bxe(:,kcol,krow),np,nvtx);
          by = by + sparse(ncol,nrow,bye(:,kcol,krow),np,nvtx);
		  end
       end
%
% vector velocity matrices ...
	   b = [bx,by];
%
% pressure matrices ...
	   for krow=1:3
	   nrow=mp(:,krow);
          for kcol=1:3
		  ncol=mp(:,kcol);
          m = m + sparse(nrow,ncol,mpe(:,krow,kcol),np,np);
		  end
	  end
%
fprintf('done\n')
return


function RHOTPH2O = h2o_rhotp(P,X)
% H2O_RHOTP  Two-phase density of H2O in kg/m^3
% H2O_RHOTP(P,X) Returns the two-phase density
% of H2O at a given pressure and quality.
%  Called function: h2o_rhof(P), h2o_rhog(P)
%  Required Inputs are: P - pressure (kPa), X - quality (fraction)
% ---------------------------------------------------------------
%  The MATLAB function was created by Tibor Balint, December 1998
% TBoreal Research Corporation, Toronto, Ont. Canada
% (tibor@netcom.ca) and also, University of Warwick, UK
% ---------------------------------------------------------------

format long g;  % set the format of the calculations

RF=h2o_rhof(P);
RG=h2o_rhog(P);

INVRHOTP=(X/RG)+((1-X)/RF);
RHOTPH2O=1/INVRHOTP;
return


function [model, varargout] = gmlvq_core(trainSet, trainLab, varargin)
%GMLVQ_core.m - trains the Generalized Matrix LVQ algorithm
%NOTE: minimal requirement version 7.4.0.336 (R2007a)
%
%  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  %% Use the wrapper GMLVQ.M to access the functionality in style of %%
%  %% the SOM Toolbox (i.e. with data structs).                       %%
%  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  example for usage:
%  trainSet = [1,2,3;4,5,6;7,8,9];
%  trainLab = [1;1;2];
%  GMLVQ_model=GMLVQ_train(trainSet,trainLab); % minimal parameters required
%  estimatedTrainLabels = GMLVQ_classify(trainSet, GMLVQ_model);
%  trainError = mean( trainLab ~= estimatedTrainLabels );
%
% input:
%  trainSet : matrix with training samples in its rows
%  trainLab : vector with the labels of the training set
% optional parameters:
%  PrototypesPerClass: (default=1) the number of prototypes per class used. This could
%  be a number or a vector with the number for each class
%  initialPrototypes : (default=[]) a set of prototypes to start with. If not given initialization near the class means
%  initialMatrix     : the matrix omega to start with. If not given random
%  initialization for rectangular matrices and Unity for squared omega
%  dim               : (default=nb of features for training) the maximum rank or projection dimension
%  regularization    : (default=0) values usually between 0 and 1 treat with care.
%  Regularizes the eigenvalue spectrum of omega'*omega to be more homogeneous
%  testSet           : (default=[]) an optional test set used to compute
%  the test error. The last column is expected to be a label vector
%  comparable        : (default=0) a flag which resets the random generator
%  to produce comparable results if set to 1
%  optimization      : (default=fminlbfgs) indicates which optimization is used: sgd or fminlbfgs
% parameter for the stochastic gradient descent sgd
%  nb_epochs             : (default=100) the number of epochs for sgd
%  learningRatePrototypes: (default=[]) the learning rate for the prototypes.
%  Could be the start and end value used for a sigmoidal spectrum or a vector of length nb_epochs
%  learningRateMatrix    : (default=[]) the learning rate for the matrix.
%  Could be the start and end value used for a sigmoidal spectrum or a vector of length nb_epochs
%  MatrixStart           : (default=1) the epoch to start the matrix training
% parameter for the build-in function fminlbfgs
%  threshstop       : (default=0) the training error for early stopping
%  nb_reiterations  : (default=100) the number of optimization reiterations performed
%  useEarlyStopping : (default=1) use early stopping based on threshstop
%  Display          : (default=iter) the optimization output 'iter' or 'off'
%  GradObj          : (default=on) use the gradient information or not
%  HessUpdate       : (default=lbfgs) the update can be 'lbfgs', 'bfgs' or 'steepdesc'
%  TolFun           : (default=1e-6) the tolerance
%  MaxIter          : (default=2500) the maximal number of iterations
%  MaxFunEvals      : (default=1000000) the maximal number of function evaluations
%  TolX             : (default=1e-10) tolerance
%  DiffMinChange    : (default=1e-10) minimal change
%
% output: the GMLVQ model with prototypes w their labels c_w and the matrix omega
%  optional output:
%  initialization : a struct containing the settings
%  trainError     : error in the training set
%  testError      : error in the training set (only computed if 'testSet' is given)
%  costs          : the output of the cost function
%
% Citation information:
% Petra Schneider, Michael Biehl, Barbara Hammer:
% Adaptive Relevance Matrices in Learning Vector Quantization. Neural Computation 21(12): 3532-3561 (2009)
%
% K. Bunte, P. Schneider, B. Hammer, F.-M. Schleif, T. Villmann and M. Biehl,
% Limited Rank Matrix Learning - Discriminative Dimension Reduction and Visualization,
% Neural Networks, vol. 26, nb. 4, pp. 159-173, 2012.
%
% P. Schneider, K. Bunte, B. Hammer and M. Biehl, Regularization in Matrix Relevance Learning,
% IEEE Transactions on Neural Networks, vol. 21, nb. 5, pp. 831-840, 2010.
%
% Kerstin Bunte (modified based on the code of Marc Strickert http://www.mloss.org/software/view/323/ and Petra Schneider)
% uses the Fast Limited Memory Optimizer fminlbfgs.m written by Dirk-Jan Kroon available at the MATLAB central
% kerstin.bunte@googlemail.com
% Fri Nov 09 14:13:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 and BSD License apply.
% See file 'license-gpl2.txt' and 'BSD_license.txt' enclosed in this package.
% Programs are not for use in critical applications!
%

% Contributed to SOM Toolbox vs2, December 3rd, 2012 by Alexander Schulz
% Copyright (c) Kerstin Bunte
% http://www.cis.hut.fi/projects/somtoolbox/

nout = max(nargout,1)-1;
p = inputParser;   % Create an instance of the class.
p.addRequired('trainSet', @isfloat);
p.addRequired('trainLab', @(x) length(x)==size(trainSet,1) & isnumeric(x));

p.addParamValue('PrototypesPerClass', ones(1,length(unique(trainLab))), @(x)(sum(~(x-floor(x)))/length(x)==1 && (length(x)==length(unique(trainLab)) || length(x)==1)));
p.addParamValue('initialPrototypes',[], @(x)(size(x,2)-1==size(trainSet,2) && isfloat(x)));
p.addParamValue('initialMatrix',[], @(x)(size(x,2)==size(trainSet,2) && isfloat(x)));
p.addParamValue('dim',size(trainSet,2), @(x)(~(x-floor(x)) && x<=size(trainSet,2) && x>0));
p.addParamValue('regularization',0, @(x)(isfloat(x) && x>=0));
p.addOptional('testSet', [], @(x)(size(x,2)-1)==size(trainSet,2) & isfloat(x));
p.addOptional('comparable', 0, @(x)(~(x-floor(x))));
p.addOptional('optimization', 'fminlbfgs', @(x)any(strcmpi(x,{'sgd','fminlbfgs'})));
% parameter for the stochastic gradient descent
p.addOptional('nb_epochs', 100, @(x)(~(x-floor(x))));
p.addParamValue('learningRatePrototypes', [], @(x)(isfloat(x) || isa(x,'function_handle'))); % && (length(x)==2 || length(x)==p.Results.epochs)
p.addParamValue('learningRateMatrix', [], @(x)(isfloat(x)  || isa(x,'function_handle')));
p.addOptional('MatrixStart', 1, @(x)(~(x-floor(x))));
% parameter for the build-in function
p.addOptional('threshstop',0,@(x) isfloat(x));
p.addOptional('nb_reiterations',100,@(x)(~(x-floor(x))));
p.addOptional('useEarlyStopping',1,@(x)(~(x-floor(x))));
p.addOptional('Display', 'off', @(x)any(strcmpi(x,{'iter','off'})));
p.addOptional('GradObj', 'on', @(x)any(strcmpi(x,{'on','off'})));
p.addOptional('HessUpdate', 'lbfgs', @(x)any(strcmpi(x,{'lbfgs', 'bfgs', 'steepdesc'})));
p.addOptional('TolFun',1e-6,@(x) isfloat(x));
p.addOptional('MaxIter', 2500, @(x)(~(x-floor(x))));
p.addOptional('MaxFunEvals', 1000000, @(x)(~(x-floor(x))));
p.addOptional('TolX',1e-10,@(x) isfloat(x));
p.addOptional('DiffMinChange',1e-10,@(x)isfloat(x));
p.CaseSensitive = true;
p.FunctionName = 'GMLVQ';
% Parse and validate all input arguments.
p.parse(trainSet, trainLab, varargin{:});

%%% check if results should be comparable
if p.Results.comparable,
    rng('default');
end
%%% set useful variables
nb_samples = size(trainSet,1);
nb_features = size(trainSet,2);
% labels should be a row vector
if size(trainLab,1)~=nb_samples, trainLab = trainLab';end

classes = unique(trainLab);
nb_classes = length(classes);
dim = p.Results.dim;
MatrixStart = p.Results.MatrixStart;
testSet = p.Results.testSet;
% global regularization;
regularization = p.Results.regularization;
if regularization, disp(['Regularize the eigenvalue spectrum of omega''*omega with ',num2str(regularization)]);end

initialization = rmfield(p.Results, 'trainSet');
initialization.trainSet = [num2str(nb_samples),'x',num2str(nb_features),' matrix'];
initialization = rmfield(initialization, 'trainLab');
initialization.trainLab = ['vector of length ',num2str(length(trainLab))];
if ~isempty(testSet)
    initialization = rmfield(initialization, 'testSet');
    initialization.testSet = [num2str(size(testSet,1)),'x',num2str(size(testSet,2)),' matrix'];
end
switch(p.Results.optimization)
case{'sgd'}
    initialization = rmfield(initialization, 'useEarlyStopping');
    initialization = rmfield(initialization, 'Display');
    initialization = rmfield(initialization, 'GradObj');
    initialization = rmfield(initialization, 'HessUpdate');
    initialization = rmfield(initialization, 'TolFun');
    initialization = rmfield(initialization, 'MaxIter');
    initialization = rmfield(initialization, 'MaxFunEvals');
    initialization = rmfield(initialization, 'TolX');
    initialization = rmfield(initialization, 'DiffMinChange');
    initialization = rmfield(initialization, 'nb_reiterations');
case{'fminlbfgs'}
    disp('The fminlbfgs optimization uses some global variables:');
    disp('threshstop earlystopped useEarlyStopping');
    initialization = rmfield(initialization, 'nb_epochs');
    initialization = rmfield(initialization, 'learningRatePrototypes');
    initialization = rmfield(initialization, 'learningRateMatrix');
    initialization = rmfield(initialization, 'MatrixStart');
    nb_reiterations = p.Results.nb_reiterations;
end
% Display all arguments.
%disp 'Settings for GMLVQ:'
%disp(initialization);

%%% check the number of prototypes per class if one integer is given and turn
%%% it into a vector
nb_ppc = p.Results.PrototypesPerClass;
if length(nb_ppc)~=nb_classes,
    nb_ppc = ones(1,nb_classes)*nb_ppc;
end

%%% initialize the prototypes
if isempty(p.Results.initialPrototypes)
    % initialize near the class centers
    w = zeros(sum(nb_ppc),nb_features);
    c_w = zeros(sum(nb_ppc),1);
    actPos = 1;
    for actClass=1:nb_classes
        nb_prot_c = nb_ppc(actClass);
        classMean = mean(trainSet(trainLab==classes(actClass),:));
        % set the prototypes to the class mean and add a random variation between -0.1 and 0.1
        w(actPos:actPos+nb_prot_c-1,:) = classMean(ones(nb_prot_c,1),:)+(rand(nb_prot_c,nb_features)*2-ones(nb_prot_c,nb_features))/10;
        c_w(actPos:actPos+nb_prot_c-1) = classes(actClass);
        actPos = actPos+nb_prot_c;
    end
else
    % initialize with given w
    w = p.Results.initialPrototypes(:,1:end-1);
    c_w = p.Results.initialPrototypes(:,end);
end
%%% initialize the matrix
if isempty(p.Results.initialMatrix)
    if(p.Results.dim==nb_features)
        omega = eye(nb_features);
    else % initialize with random numbers between -1 and 1
        omega = rand(dim,nb_features)*2-ones(dim,nb_features);
    end
else
    omega = p.Results.initialMatrix;
end
% normalize the matrix
omega = omega / sqrt(sum(diag(omega'*omega)));
model = struct('w',w,'c_w',c_w,'omega',omega);
clear w c_w omega;

% GMLVQ_algorithm = struct('update',@GMLVQ_update,'classify',@GMLVQ_classify,'costfun',@GLVQ_costfun);
switch(p.Results.optimization)
case{'sgd'}
    %%% gradient descent variables
    nb_epochs = p.Results.nb_epochs;
    % compute the vector of nb_epochs learning rates alpha for the prototype learning
    if isa(p.Results.learningRatePrototypes,'function_handle')
        % with a given function specified from the user
        alphas = arrayfun(p.Results.learningRatePrototypes, 1:nb_epochs);
    elseif length(p.Results.learningRatePrototypes)>2
        if length(p.Results.learningRatePrototypes)==nb_epochs
            alphas = p.Results.learningRatePrototypes;
        else
            disp('The learning rate vector for the prototypes does not fit the nb of epochs');
            return;
        end
    else
        % or use an decay with a start and a decay value
        if isempty(p.Results.learningRatePrototypes)
            initialization.learningRatePrototypes = [nb_features/100, nb_features/10000];
        end
        alpha_start = initialization.learningRatePrototypes(1);
        alpha_end = initialization.learningRatePrototypes(2);
        alphas = arrayfun(@(x) alpha_start * (alpha_end/alpha_start)^(x/nb_epochs), 1:nb_epochs);
    %     alphas = arrayfun(@(x) alpha_start / (1+(x-1)*alpha_end), 1:nb_epochs);
    end
    % compute the vector of nb_epochs learning rates epsilon for the Matrix learning
    epsilons = zeros(1,nb_epochs);
    if isa(p.Results.learningRateMatrix,'function_handle')
        % with a given function specified from the user
    % 	epsilons = arrayfun(p.Results.learningRateMatrix, 1:nb_epochs);
        epsilons(MatrixStart:nb_epochs) = arrayfun(p.Results.learningRateMatrix, MatrixStart:nb_epochs);
    elseif length(p.Results.learningRateMatrix)>2
        if length(p.Results.learningRateMatrix)==nb_epochs
            epsilons = p.Results.learningRateMatrix;
        else
            disp('The learning rate vector for the Matrix does not fit the nb of epochs');
            return;
        end
    else
        % or use an decay with a start and a decay value
        if isempty(p.Results.learningRateMatrix)
            initialization.learningRateMatrix = [nb_features/1000, nb_features/100000];
        end
        eps_start = initialization.learningRateMatrix(1);
        eps_end = initialization.learningRateMatrix(2);
    %     epsilons = arrayfun(@(x) eps_start * (eps_end/eps_start)^(x/nb_epochs), 1:nb_epochs);
        epsilons(MatrixStart:nb_epochs) = arrayfun(@(x) eps_start * (eps_end/eps_start)^((x-MatrixStart)/(nb_epochs-MatrixStart)), MatrixStart:nb_epochs);
    end

    %%% initialize requested outputs
    trainError = [];
    costs = [];
    testError = [];
    if nout>=2,
        % train error requested
        trainError = ones(1,nb_epochs+1);
        estimatedLabels = GMLVQ_classify(trainSet, model); % error after initialization
        trainError(1) = sum( trainLab ~= estimatedLabels )/nb_samples;
        if nout>=3,
            % test error requested
            if isempty(testSet)
                testError = [];
                disp('The test error is requested, but no labeled test set given. Omitting the computation.');
            else
                testError = ones(1,nb_epochs+1);
                estimatedLabels = GMLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                testError(1) = sum( testSet(:,end) ~= estimatedLabels )/length(estimatedLabels);
            end
            if nout>=4,
                % costs requested
%                 LabelEqPrototype = trainLab*ones(1,numel(model.c_w)) == (model.c_w*ones(1,nb_samples))';
                disp('The computation of the costs is an expensive operation, do it only if you really need it!');
                costs = ones(1,nb_epochs+1);
                costs(1) = GMLVQ_costfun(trainSet, trainLab, model, regularization);
%                 costs(1) = sum(arrayfun(@(idx) GLVQ_costfun(min(dist(idx,model.c_w == trainLab(idx))),...
%                                                             min(dist(idx,model.c_w ~= trainLab(idx))))-regTerm, 1:size(dist,1)));
            end
        end
    end

    %%% optimize with stochastic gradient descent
    for epoch=1:nb_epochs
        if mod(epoch,100)==0, disp(epoch); end
        % generate order to sweep through the trainingset
        order = randperm(nb_samples);
        % perform one sweep through trainingset
        for i=1:nb_samples
            % select one training sample randomly
            xi = trainSet(order(i),:);
            c_xi = trainLab(order(i));

%             dist = ((xi(ones(size(model.w,1),1),:))-model.w)*model.omega'*(model.omega*((xi(ones(size(model.w,1),1),:))-model.w)');
%             dist = diag(dist);
            dist = sum((bsxfun(@minus, xi, model.w)*model.omega').^2, 2);
            % determine the two winning prototypes
            % nearest prototype with the same class
            [sortDist,sortIdx] = sort(dist);
            count = 1;
            J = sortIdx(count);
            while model.c_w(sortIdx(count)) ~= c_xi,
                count = count+1;
                J = sortIdx(count);
            end
            dJ = sortDist(count);
            count = 1;
            K = sortIdx(count);
            while model.c_w(sortIdx(count)) == c_xi,
                count = count+1;
                K = sortIdx(count);
            end
            dK = sortDist(count);
    %         disp([J,K,dJ,dK]);
            wJ = model.w(J,:);
            wK = model.w(K,:);
            % prototype update
            norm_factor = (dJ + dK)^2;
            DJ = (xi-wJ);
            DK = (xi-wK);

            oo = model.omega'*model.omega;

            dwJ = (2*dK/norm_factor)*2*oo*DJ';
            dwK = (2*dJ/norm_factor)*2*oo*DK';
    %         dwJ = (2*dK/norm_factor)*2*model.omega'*model.omega*DJ';
    %         dwK = (2*dJ/norm_factor)*2*model.omega'*model.omega*DK';
            model.w(J,:) = wJ + alphas(epoch) * dwJ';
            model.w(K,:) = wK - alphas(epoch) * dwK';
            % update matrices
            if epsilons(epoch)>0, % epoch >= MatrixStart
                f1 = (2*dK/norm_factor)*2*(model.omega*DJ')*DJ;
                f2 = (2*dJ/norm_factor)*2*(model.omega*DK')*DK;
                % update omega
                if regularization,
                    f3 = (pinv(model.omega))';
                else
                    f3 = 0;
                end
                model.omega = model.omega-epsilons(epoch) * (f1-f2  - regularization * f3);
                % normalization
                model.omega = model.omega / sqrt(sum(diag(oo)));
            end
        end
        if nout>=2,
            % train error requested
            estimatedLabels = GMLVQ_classify(trainSet, model); % error after epoch
            trainError(epoch+1) = sum( trainLab ~= estimatedLabels )/nb_samples;
            if nout>=3,
                % test error requested
                if ~isempty(testSet)
                    estimatedLabels = GMLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                    testError(epoch+1) = sum( testSet(:,end) ~= estimatedLabels )/length(estimatedLabels);
                end
                if nout>=4,
                    % costs requested
                    costs(epoch+1) = GMLVQ_costfun(trainSet, trainLab, model, regularization);
%                     costs(epoch+1) = sum(arrayfun(@(idx) GLVQ_costfun(min(dist(idx,model.c_w == trainLab(idx))),...
%                                                                       min(dist(idx,model.c_w ~= trainLab(idx))))-regTerm, 1:size(dist,1)));
                end
            end
        end
    end
case{'fminlbfgs'}
    %%% optimization options
    options = struct( ...
      'Display',p.Results.Display, ...
      'GradObj',p.Results.GradObj, ...
      'GradConstr',false, ...
      'GoalsExactAchieve',0, ...
      'TolFun',p.Results.TolFun, ...
      'MaxIter',p.Results.MaxIter, ...
      'MaxFunEvals', p.Results.MaxFunEvals, ...
      'TolX',p.Results.TolX, ...
      'DiffMinChange',p.Results.DiffMinChange, ...
      'OutputFcn','LVQ_progresser', ...
      'HessUpdate',p.Results.HessUpdate ...
    );
    clear('progresser'); % memory therein might need reset
    global threshstop earlystopped useEarlyStopping % for LVQ_progresser.m datval labval n_vec
    useEarlyStopping = p.Results.useEarlyStopping; % use early stopping
    earlystopped = false;
    threshstop = p.Results.threshstop; % stop if classification below this threshold for early stopping
%     prototypeLabel = model.c_w;
    nb_prototypes = numel(model.c_w);
%     LabelEqualsPrototype = trainLab*ones(1,nb_prototypes) == (model.c_w*ones(1,nb_samples))';
    LabelEqualsPrototype = bsxfun(@eq,trainLab,model.c_w');
    earlystopped = false; % don't change, assigned in progresser.m
    clear('progresser'); % memory therein might need reset
    newfval = realmax('single');
    % fminlbfgs optimizer courtesy of Dirk-Jan Kroon:
    % http://www.mathworks.de/matlabcentral/fileexchange/23245
    % early stopping to be implemented in progresser function
    variables = zeros(dim+nb_prototypes,size(trainSet,2));
    variables(1:nb_prototypes,:) = model.w;
    variables(nb_prototypes+1:end,:) = model.omega;
    LRprototypes = 1; % learn prototype locations
    LRrelevances = 0; % don't learn metric
%     [variables,fval] = fminlbfgs(@GMLVQ_optfun,variables,options);
    [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);
    if not(isempty(fval))
        newfval = fval;
    end
    LRprototypes = 0; % don't learn prototype locations
    LRrelevances = 1; % learn metric
%     [variables,fval] = fminlbfgs(@GMLVQ_optfun,variables,options);
    [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);

    if not(isempty(fval))
        newfval = fval;
    end
    if not(isempty(fval))
      clear('progresser'); % memory therein might need reset
      LRprototypes = 1;
      for i = 1:nb_reiterations  % depending on data, re-iterations might further improve
%         [variables,fval] = fminlbfgs(@GMLVQ_optfun,variables,options);
        [variables,fval] = fminlbfgs(@(variables) GMLVQ_optfun(variables,trainSet,LabelEqualsPrototype,LRrelevances,LRprototypes,model.c_w,regularization),variables,options);
        if not(isempty(fval))
          if abs(fval - newfval) < 1e-3
            newfval = fval;
            break
          end
          newfval = fval;
        end
        if isempty(fval) || earlystopped
          break
        end
      end
    end
    model.w = variables(1:nb_prototypes,:);
    model.omega = variables(nb_prototypes+1:end,:);
    model.omega = model.omega / sqrt(sum(diag(model.omega'*model.omega)));
    if nout>=2,
        % train error requested
        estimatedLabels = GMLVQ_classify(trainSet, model); % error after initialization
        trainError = mean( trainLab ~= estimatedLabels );
        if nout>=3,
            % test error requested
            if isempty(testSet)
                testError = [];
                disp('The test error is requested, but no labeled test set given. Omitting the computation.');
            else
                estimatedLabels = GMLVQ_classify(testSet(:,1:end-1), model); % error after initialization
                testError = mean( testSet(:,end) ~= estimatedLabels );
            end
            if nout>=4,
                % costs requested
                costs = GMLVQ_costfun(trainSet, trainLab, model, regularization);
%                 dist = computeDistance(trainSet, model.w, model);
%                 if regularization,
%                     regTerm = regularization * log(det(model.omega*model.omega'));
%                 else
%                     regTerm = 0;
%                 end
%                 costs = sum(arrayfun(@(idx) GLVQ_costfun(min(dist(idx,model.c_w == trainLab(idx))),...
%                                                          min(dist(idx,model.c_w ~= trainLab(idx)))), 1:size(dist,1)))-regTerm;
            end
        end
    end
end
%%% output of the training
varargout = cell(nout);
for k=1:nout
	switch(k)
		case(1)
			varargout(k) = {initialization};
		case(2)
			varargout(k) = {trainError};
		case(3)
			varargout(k) = {testError};
		case(4)
            varargout(k) = {costs};
	end
end

function cost = GMLVQ_costfun(trainSet, trainLab, model, regularization)
%GMLVQ_costfun.m - computes the costs for a given training set and GMLVQ
%model with or without regularization
%  example for usage:
%  trainSet = [1,2,3;4,5,6;7,8,9];
%  trainLab = [1;1;2];
%  GMLVQ_model=GMLVQ_train(trainSet,trainLab); % minimal parameters required
%  costs = GMLVQ_costfun(trainSet, trainLab, GMLVQ_model, 0);
%
% input:
%  trainSet : matrix with training samples in its rows
%  trainLab : a vector of training labels
%  model    : GMLVQ model with prototypes w their labels c_w and the matrix omega
%  regularization: the factor>=0 for the regularization
%
% output    : cost function value
%
% Kerstin Bunte (based on the code from Marc Strickert)
% kerstin.bunte@googlemail.com
% Mon Nov 05 09:05:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 apply.
% See file 'license-gpl2.txt' enclosed in this package.
% Programs are not for use in critical applications!
%
nb_samples = length(trainLab);
% labels should be a row vector
if size(trainLab,1)~=nb_samples, trainLab = trainLab';end

% LabelEqPrototype = trainLab*ones(1,numel(model.c_w)) == (model.c_w*ones(1,nb_samples))';
LabelEqPrototype = bsxfun(@eq,trainLab,model.c_w');
dists = computeDistance(trainSet, model.w, model);
if regularization,
    regTerm = regularization * log(det(model.omega*model.omega'));
% if strcmp(p.Results.optimization,'sgd')
else
    regTerm = 0;
end
Dwrong = dists;
Dwrong(LabelEqPrototype) = realmax(class(Dwrong));   % set correct labels impossible
distwrong = min(Dwrong.'); % closest wrong
clear Dwrong;

Dcorrect = dists;
Dcorrect(~LabelEqPrototype) = realmax(class(Dcorrect)); % set wrong labels impossible
distcorrect = min(Dcorrect.'); % closest correct
clear Dcorrect;
clear dists;
distcorrectpluswrong = distcorrect + distwrong;
distcorrectminuswrong = distcorrect - distwrong;
mu = distcorrectminuswrong ./ distcorrectpluswrong;
if regularization,
    regTerm = regularization * log(det(model.omega*model.omega'));
else
    regTerm = 0;
end
cost = sum(mu)-regTerm;

function [f G]  = GMLVQ_optfun(variables,training_data,LabelEqualsPrototype,LRrelevances,LRprototypes,prototypeLabel,regularization)
% [f G] = GMLVQ_optfun(variables)
% function to be optimzed by matrix relevance learning vector quantization
% variables = [prototype matrix;omega matrix]
% global variables are
%   training_data        : data vectors as row vectors, i.e. attributes in columns
%   LabelEqualsPrototype : binary matrix indicating coocurrences of
%   training labels and prototype labels
%   prototypeLabel       : label vector for the prototypes
%   regularization       : the regularization parameter
%   LRrelevances         : learning rate for the relevance matrix
%   LRprototypes         : learning rate for the prototypes
%
% Kerstin Bunte (modified based on the code of Marc Strickert http://www.mloss.org/software/view/323/)
% kerstin.bunte@googlemail.com
% Fri Nov 09 14:13:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 apply.
% See file 'license-gpl2.txt' enclosed in this package.
% Programs are not for use in critical applications!
%
if isempty(LRprototypes) % values between 1e-2,1e-3,... 1e-8 seem pragmatic
    LRprototypes = 1; % no relevance learning by default
end
if isempty(LRrelevances) % values between 1e-2,1e-3,... 1e-8 seem pragmatic
    LRrelevances = 0; % no relevance learning by default
end
[n_data, n_dim] = size(training_data);
nb_prototypes =  numel(prototypeLabel);
omegaT = variables(nb_prototypes+1:end,:)';
n_vec = size(variables,1) - nb_prototypes;

dists = squaredEuclidean(training_data*omegaT, variables(1:nb_prototypes,:)*omegaT);

Dwrong = dists;
Dwrong(LabelEqualsPrototype) = realmax(class(Dwrong));   % set correct labels impossible
[distwrong pidxwrong] = min(Dwrong.'); % closest wrong
clear Dwrong;

Dcorrect = dists;
Dcorrect(~LabelEqualsPrototype) = realmax(class(Dcorrect)); % set wrong labels impossible
[distcorrect pidxcorrect] = min(Dcorrect.'); % closest correct
clear Dcorrect;

distcorrectpluswrong = distcorrect + distwrong;
distcorrectminuswrong = distcorrect - distwrong;
mu = distcorrectminuswrong ./ distcorrectpluswrong;
% callitq = 1./(1 + exp(-squashsigmoid * mu)); % apply sigmoidal

if regularization,
    regTerm = regularization * log(det(omegaT'*omegaT));
else
    regTerm = 0;
end
f = sum(mu)-regTerm;
% f = mean(callitq);

if nargout > 1  % gradient needed not just function eval
    G = zeros(size(variables)); % initially no gradient
    %       callitq = squashsigmoid * callitq .* (1-callitq); % derivative of sigmoid
    %       distcorrectpluswrong = 2 * callitq ./ distcorrectpluswrong.^2; % degeneration?
    distcorrectpluswrong = 4 ./ distcorrectpluswrong.^2; % norm_factor for derivative for every data sample
    if LRrelevances > 0
        Gw = zeros(n_vec,n_dim);
    end
    for k=1:nb_prototypes%(n_vec+1):size(lambda,1) % update all prototypes
        idxc = (k == pidxcorrect);  % Js: idxs where actual prototype is nearest correct
        idxw = (k == pidxwrong);    % Ks: idxs where actual prototype is nearest wrong

        dcd =  distcorrect(idxw) .* distcorrectpluswrong(idxw);
        dwd =    distwrong(idxc) .* distcorrectpluswrong(idxc);
        if LRrelevances > 0
            % part of derivative of distance
            difc = bsxfun(@minus,training_data(idxc,:),variables(k,:)); % DJs
            difw = bsxfun(@minus,training_data(idxw,:),variables(k,:)); % DKs
            % update omega
            Gw = Gw - (bsxfun(@times,difw,dcd.') * omegaT).' * difw + ...
                      (bsxfun(@times,difc,dwd.') * omegaT).' * difc;
            if LRprototypes > 0
                G(k,:) = dcd * difw - dwd * difc;
            end
        else
            if LRprototypes > 0
                G(k,:) = dcd * training_data(idxw,:) - dwd * training_data(idxc,:) + (sum(dwd)-sum(dcd)) * variables(k,:);
            end
        end
    end
if regularization,
    f3 = (pinv(omegaT'))';
else
    f3 = 0;
end
    % some rescalings needed
    if LRrelevances > 0
        G(nb_prototypes+1:nb_prototypes+n_vec,:) = 2/n_data * LRrelevances * Gw - regularization*f3;
    end
    if LRprototypes > 0
        G(1:nb_prototypes,:) = 1./n_data * LRprototypes * G(1:nb_prototypes,:) * omegaT * omegaT.';
    end
    G = G .* (1 + .0001 * (rand(size(G))-.5)); % help break symmetries
end
% if 0,
% w = variables(1:nb_prototypes,:);
% dJs = zeros(1,nb_samples);
% dKs = zeros(1,nb_samples);
% Js = zeros(1,nb_samples);
% Ks = zeros(1,nb_samples);
% norm_factors = zeros(1,nb_samples);
% DJs = zeros(nb_samples,size(training_data,2));
% DKs = zeros(nb_samples,size(training_data,2));
% for i=1:nb_samples
%     % select one training sample randomly
%     xi = training_data(i,:);
%     c_xi = trainLab(i);
%
%     dist = ((xi(ones(size(w,1),1),:))-w)*omegaT*(omegaT'*((xi(ones(size(w,1),1),:))-w)');
%     dist = diag(dist);
%     % determine the two winning prototypes
%     % nearest prototype with the same class
%     [sortDist,sortIdx] = sort(dist);
%     count = 1;
%     J = sortIdx(count);
%     while prototypeLabel(sortIdx(count)) ~= c_xi,
%         count = count+1;
%         J = sortIdx(count);
%     end
%     dJ = sortDist(count);
%     dJs(i) = dJ;
%     Js(i) = J;
%     count = 1;
%     K = sortIdx(count);
%     while prototypeLabel(sortIdx(count)) == c_xi,
%         count = count+1;
%         K = sortIdx(count);
%     end
%     dK = sortDist(count);
%     dKs(i) = dK;
%     Ks(i) = K;
%
%     wJ = w(J,:);
%     wK = w(K,:);
%     % prototype update
%     norm_factors(i) = 4/((dJ + dK)^2);
%     DJ = (xi-wJ);
%     DK = (xi-wK);
%     DJs(i,:) = DJ;
%     DKs(i,:) = DK;
% end
% end

function D = squaredEuclidean(A, B)
% computes the sqared Euclidean distance
%
%   D = squaredEuclidean(X) returns the squared Euclidean distance matrix of data in rows of X
%   D = squaredEuclidean(X, Y) returns the distance matrix with all distances between the points in X and Y.
%
if nargin == 1 % means that one matrix
    D = bsxfun(@plus, sumsquared(A,2), bsxfun(@minus, sumsquared(A,2).', 2*A*A.')); % 2*(Y*Y.')
else
    D = bsxfun(@plus, sumsquared(A,2), bsxfun(@minus, sumsquared(B,2).', 2*A*B.'));
end
D = max(D,0);

function sq  = sumsquared(x,dim)
% sq  = sumsquared(x,dim)
% sum of all squared element of matrix x along dimension dim

persistent isoctave

if isempty(isoctave)
  isoctave = exist('OCTAVE_VERSION','builtin');
end

if nargin == 1
  dim = 1;
end

if isoctave
  sq = sumsq(x,dim);
else
  sq = sum(x.^2, dim);
end


function in = iresponse_proxy(in)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 'iresponse_proxy' computes the impulse response functions to shock
% identified via instrumental variables
% Codes are written based on the Mertens Ravn (2013, AER) source codes,
% modified to
%1) handle different length of VAR and factors
%2) Multiple instruments to explain the same shock

% Filippo Ferroni, 6/1/2015
% Revised, 2/15/2017
% Revised, 3/21/2018
% Revised, 9/11/2019
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Y         = in.vars(in.p+1:end,:);
[T,n]     = size(Y);
[T_m,n_m] = size(in.proxies);

% number of proxies
k  = 1;
%Assuming proxies start at least p periods later
instrument  = in.proxies(1:end,:);

% Identification
%%%%%%%%%%%%%%%%%

% covariance of the reduced form shocks
in.Sigma_m = in.Sigma;

% Instrument on VAR residuals
Phib = [ones(T_m,1) instrument]\in.res(T-T_m-in.T_m_end+1:T-in.T_m_end,:);

% Fitted values of the identified shocks
% here is where thes prior on beta should enter , instead of Phib(:,1)
% m = beta e1 + v
uhat1           =   [ones(T_m,1) instrument]*Phib(:,1);

% regress the fitted values on the other reduced form shocks
b21ib11_TSLS    =   [ones(T_m,1) uhat1]\in.res(T-T_m-in.T_m_end+1:T-in.T_m_end,2:end);
b21ib11_TSLS    =   b21ib11_TSLS(2:end,:)';
b21ib11         =   b21ib11_TSLS;

% Identification of b11 and b12 from the covariance matrix of the VAR
Sig11   = in.Sigma_m(1:k,1:k);
Sig21   = in.Sigma_m(k+1:n,1:k);
Sig22   = in.Sigma_m(k+1:n,k+1:n);
ZZp     = b21ib11*Sig11*b21ib11'-(Sig21*b21ib11'+b21ib11*Sig21')+Sig22;
b12b12p = (Sig21- b21ib11*Sig11)'*(ZZp\(Sig21- b21ib11*Sig11));
b11b11p = Sig11-b12b12p;
b11     = sqrt(b11b11p);
in.b1   = [b11; b21ib11*b11];
in.Phib = Phib;

% Impulse Responses
%%%%%%%%%%%%%%%%%%%%
% initial shock: eps(1,1)=1
irs(in.p+1,:) = in.b1(:,1);

for jj = 2:in.irhor%+max(max(VAR.term_spreads_matur),max(VAR.real_rates_init+VAR.real_rates_matur-1))
    lvars = (irs(in.p+jj-1:-1:jj,:))';
    irs(in.p+jj,:) = lvars(:)'*in.Phi(1:in.p * n,:);
end

in.irs   = irs(in.p+1:in.p+in.irhor,:);
in.uhat1 = uhat1;

if in.compute_F_stat == 1
    % F-test
    %%%%%%%%%%%%%%%%%%%%%%%%%
    XX_m      = [ones(T_m,1) instrument];
    Res_m     = in.res( T-T_m-in.T_m_end+1 : T-in.T_m_end , :)- XX_m * Phib;
    Res_const = in.res( T-T_m-in.T_m_end+1 : T-in.T_m_end,1) - ...
        ones(T_m,1)*(ones(T_m,1)\in.res(T-T_m-in.T_m_end+1 : T-in.T_m_end,1));

    SST_m      = Res_const'*Res_const;
    SSE_m      = Res_m(:,1)'*Res_m(:,1);
    in.F_m     = ((SST_m-SSE_m)/n_m)/ ...
        (SSE_m/(length(in.res(T-T_m-in.T_m_end+1 : T-in.T_m_end,1))-(n_m+1)));
    in.R2_m    = (1-SSE_m/(SST_m));
    in.R2adj_m = in.R2_m-...
        (n_m / ((length(in.res(T-T_m-in.T_m_end+1 : T-in.T_m_end,1)) -(n_m+1))))*(1-in.R2_m);

    % Calculate robust standard errors
    SS_m = zeros(n_m+1,n_m+1);
    for ii = 1 : T_m
        SS_m = SS_m+1/T_m * XX_m(ii,:)'*XX_m(ii,:)*Res_m(ii,1)^2;
    end
    Avarb_m    = inv(1/T_m * XX_m'*XX_m)*SS_m *inv(1/T_m * XX_m'*XX_m);
    RR_m       = [zeros(n_m,1) eye(n_m)];
    WW_m       = T_m*(RR_m*Phib(:,1))' * inv(RR_m*Avarb_m*RR_m') * (RR_m*Phib(:,1));
    in.F_m_rob = WW_m/n_m;
end


function [resCe, rhsCe] = electrolyteDiffusion(ce,dCe,jflux,T,param)
% electrolyteDiffusion evaluates the residual for the electrolyte
% concentration of Li-ions in the electrolyte solution.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

% Diffusion coefficients
% Comment this for benchmark purposes
Deff_p = param.ElectrolyteDiffusionFunction(ce(1:param.Np),T(param.Nal+1:param.Nal+param.Np),param,'p');
Deff_s = param.ElectrolyteDiffusionFunction(ce(param.Np+1:param.Np+param.Ns),T(param.Nal+param.Np+1:param.Nal+param.Np+param.Ns),param,'s');
Deff_n = param.ElectrolyteDiffusionFunction(ce(param.Np+param.Ns+1:end),T(param.Nal+param.Np+param.Ns+1:end-param.Ncu),param,'n');

% Uncomment this for benchmark purposes
% Deff_p = repmat(param.Dp*param.eps_p^param.brugg_p,param.Np,1);
% Deff_s = repmat(param.Ds*param.eps_s^param.brugg_s,param.Ns,1);
% Deff_n = repmat(param.Dn*param.eps_n^param.brugg_n,param.Nn,1);

% Interpolation of the diffusion coefficients
[Deff_p, Deff_s, Deff_n] = interpolateDiffusionCoefficients(Deff_p,Deff_s,Deff_n,param);

%% Positive A Matrix
A_p = - diag(Deff_p);

A_p(2:end,2:end)    = A_p(2:end,2:end) - diag(Deff_p(1:end-1));
A_p(1:end-1,2:end)  = A_p(1:end-1,2:end) + diag(Deff_p(1:end-1));
A_p(2:end,1:end-1)  = A_p(2:end,1:end-1) + diag(Deff_p(1:end-1));
%% Separator A Matrix
A_s = - diag(Deff_s);

A_s(2:end,2:end)    = A_s(2:end,2:end) - diag(Deff_s(1:end-1));
A_s(1:end-1,2:end)  = A_s(1:end-1,2:end) + diag(Deff_s(1:end-1));
A_s(2:end,1:end-1)  = A_s(2:end,1:end-1) + diag(Deff_s(1:end-1));
%% Negative A Matrix
A_n = - diag(Deff_n);

A_n(2:end,2:end)    = A_n(2:end,2:end) - diag(Deff_n(1:end-1));
A_n(1:end-1,2:end)  = A_n(1:end-1,2:end) + diag(Deff_n(1:end-1));
A_n(2:end,1:end-1)  = A_n(2:end,1:end-1) + diag(Deff_n(1:end-1));
% Fix the last elements of the A_n
A_n(end,end-1:end)  = [Deff_n(end-1) -Deff_n(end-1)];
%% A_tot matrix
A_tot = blockDiagonalMatrix(param,A_p,A_s,A_n);

% Divide by the deltax and the length of the positive electrode
A_tot(1:param.Np,1:param.Np)    = A_tot(1:param.Np,1:param.Np)/(param.deltax_p^2*param.len_p^2);
% Reset values on the lines for the interfaces conditions
A_tot(param.Np,:)           = 0;
A_tot(param.Np+1,:)         = 0;

% Divide by the deltax and the length of the separator
A_tot(param.Np+1:param.Np+param.Ns,param.Np+1:param.Np+param.Ns)    = A_tot(param.Np+1:param.Np+param.Ns,param.Np+1:param.Np+param.Ns)/(param.deltax_s^2 * param.len_s^2);
% Reset values on the lines for the interfaces conditions
A_tot(param.Np+param.Ns,:)      = 0;
A_tot(param.Np+param.Ns+1,:)    = 0;

% Divide by the deltax and the length of the negative electrode
A_tot(param.Np+param.Ns+1:end,param.Np+param.Ns+1:end)      = A_tot(param.Np+param.Ns+1:end,param.Np+param.Ns+1:end)/(param.deltax_n^2 * param.len_n^2);

%% Interface between separator and positive electrode (last volume in the positive electrode)

% Compute the common denominator at the interface
den_s   = (param.deltax_p*param.len_p/2 + param.deltax_s*param.len_s/2);
% Last diffusion coefficient of the positive electrode
last_p  = Deff_p(end-1)/(param.deltax_p*param.len_p);
% Diffusion coefficient on the interface
first_s = Deff_p(end)/den_s;
% Fix the values at the boundaries
A_tot(param.Np,param.Np-1:param.Np+1) = [last_p -(last_p+ first_s) first_s]/(param.deltax_p*param.len_p*param.eps_p);

%% Interface between separator and positive electrode (first volume in the separator)

% Compute the common denominator at the interface
den_s       = (param.deltax_p*param.len_p/2 + param.deltax_s*param.len_s/2);
% First diffusion coefficient in the separator
second_s    = Deff_s(1)/(param.deltax_s*param.len_s);
% Diffusion coefficient on the interface
first_s     = Deff_p(end)/den_s;

A_tot(param.Np+1,param.Np:param.Np+2) = [first_s -(first_s+second_s) second_s]/(param.deltax_s*param.len_s*param.eps_s);

%% Interface between separator and negative electrode (last volume in the separator)

% Compute the common denominator at the interface
den_s   = (param.deltax_s*param.len_s/2 + param.deltax_n*param.len_n/2);
% Last diffusion coefficient in the separator
last_s  = Deff_s(end-1)/(param.deltax_s*param.len_s);
% Diffusion coefficient on the interface
first_n = Deff_s(end)/den_s;

A_tot(param.Np+param.Ns,param.Np+param.Ns-1:param.Np+param.Ns+1) = [last_s -(last_s+first_n) first_n]/(param.deltax_s*param.len_s*param.eps_s);

%% Interface between separator and negative electrode (first volume in the negative electrode)

% Compute the common denominator at the interface
den_n       = (param.deltax_s*param.len_s/2 + param.deltax_n*param.len_n/2);
% First diffusion coefficient in the negative electrode
second_n    = Deff_n(1)/(param.deltax_n*param.len_n);
% Diffusion coefficient on the interface
first_n     = Deff_s(end)/den_n;

A_tot(param.Np+param.Ns+1,param.Np+param.Ns:param.Np+param.Ns+2) = [first_n -(first_n+second_n) second_n]/(param.deltax_n*param.len_n*param.eps_n);

%% Useful stuff
a_tot       = [
    repmat(param.a_i(1),param.Np,1);...
    zeros(param.Ns,1);...
    repmat(param.a_i(3),param.Nn,1)...
    ];

jflux_tot   = [
    jflux(1:param.Np);...
    zeros(param.Ns,1);...
    jflux(param.Np+1:end)...
    ];

eps_tot     = [
    repmat(param.eps_p,param.Np,1);...
    repmat(param.eps_s,param.Ns,1);...
    repmat(param.eps_n,param.Nn,1)
    ];

K = 1./eps_tot;
A_eps = diag(K);

% Build porosities matrix
if(~isa(eps_tot,'casadi.MX') && ~isa(eps_tot,'casadi.SX'))
    A_eps = A_eps + diag(K(1:end-1),1);
    A_eps = A_eps + diag(K(1:end-1),-1);
    A_eps = sparse(A_eps);
else
    A_u_l                   = diag(K(1:end-1));
    A_eps(1:end-1,2:end)    = A_eps(1:end-1,2:end)+A_u_l;
    A_eps(2:end,1:end-1)    = A_eps(2:end,1:end-1)+A_u_l;
end

A_eps(param.Np,param.Np-1:param.Np+1) = 1;
A_eps(param.Np+1,param.Np:param.Np+2) = 1;

A_eps(param.Np+param.Ns,param.Np+param.Ns-1:param.Np+param.Ns+1) = 1;
A_eps(param.Np+param.Ns+1,param.Np+param.Ns:param.Np+param.Ns+2) = 1;

G = A_eps.*A_tot;

% Write the RHS of the equation
rhsCe = (G*ce + K.*(1-param.tplus).*a_tot.*jflux_tot);

% Write the residual of the equation
resCe = dCe - rhsCe;
end


function [A,B] = hmxQRSVD(A,B,tol)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxQRSVD.m                                    |
%|    #    |   VERSION    : 0.52                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.01.2019                                    |
%| ( === ) |   SYNOPSIS   : QR factorization and SVD recompression for    |
%|  `---'  |                low-rank matrices                             |
%+========================================================================+

if ~isempty(A)
    % QR Factorisation A = QA * RA;
    [QA,RA] = qr(A,0);

    % QR Factorisation Bt = QB * RB
    [QB,RB] = qr(B.',0);

    % Singular value decomposition U*S*V = RA * RB.'
    try
        [U,S,V] = svd(RA * RB.','econ');
    catch
        return
    end

    % Compression rank
    s = diag(S);
    n = sum( s./s(1) >= tol );

    % Recompression A = QA * U * S
    A = QA * (U(:,1:n) * S(1:n,1:n));

    % Recompression B = V' * QB^t
    B = V(:,1:n)' * QB.';
end
end


%============================================================================
% Copyright (C) 2014, Heikki Hyyti
%
% 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.
%============================================================================

%source: http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=05291740
% A Triaxial Accelerometer Calibration Method Using a Mathematical Model
% by S.P. Won, F. Golnaraghi

function [B, G] = IMUcalibIteration(S_x, S_y, S_z, B, G)
    % gravitation at Helsinki
    g0 = 9.8189;

    % current estimate of accelerations
    A_x = (S_x - B(1)) / G(1);
    A_y = (S_y - B(2)) / G(2);
    A_z = (S_z - B(3)) / G(3);

    % squares
    A_x2 = (A_x.*A_x);
    A_y2 = (A_y.*A_y);
    A_z2 = (A_z.*A_z);

    % error estimate
    E = A_x2 + A_y2 + A_z2 - (g0*g0);

    ACCEL = [A_x2 A_y2 A_z2 A_x A_y A_z];

    CAL = ACCEL \ E;

    % gain change
    G_change2 = 1 ./ (1 - CAL(1:3));
    G_change = sqrt(abs(G_change2));

    % bias change
    B_change = CAL(4:6) .* G .* G_change2 .* 0.5;

    % update estimates
    G = G .* G_change;
    B = B + B_change;
end


function UNew = fluidNeumann3D(varargin);
% fluidNeumann3D: solve fluid registraion in 3D with Neumann
%        boundary conditions
%
%
% author: Nathan D. Cahill
% email: nathan.cahill@rit.edu
% affiliation: Rochester Institute of Technology
% date: January 2014
% licence: GNU GPL v3
%
% Copyright Nathan D. Cahill
% Code available from https://github.com/tomdoel/npReg
%
%

% parse input arguments
[DU,F,mu,lambda,PixSize,M,N,P,RegularizerFactor,HX,HY,HZ] = parse_inputs(varargin{:});

% multiply F by adjoint of Navier-Lame equations
FNew = adjointNL(F/RegularizerFactor,mu,lambda,0,M,N,P);

% compute sine transform of new force field
FS = discreteCosineTransform(FNew,M,N,P);

% construct images of coordinates scaled by pi/(N or M or P)
[a,b,c] = ndgrid(pi*(0:(M-1))/(M-1),pi*(0:(N-1))/(N-1),pi*(0:(P-1))/(P-1));

% construct LHS factor
LHSfactor = mu.*(lambda+2*mu).*(2*cos(a) + 2*cos(b) + 2*cos(c) - 6).^2;

% if gamma is zero, set origin term to 1, as DC term does not matter
LHSfactor(1,1,1) = 1;

% solve for FFT of U
VS = cat(4,FS(:,:,:,1)./LHSfactor,FS(:,:,:,2)./LHSfactor,FS(:,:,:,3)./LHSfactor);

% perform inverse DST
V = discreteCosineTransform(VS,M,N,P);

% now perform Euler integration to construct new displacements
UNew = zeros(M,N,P,3);
UNew(:,:,:,1) = (1 - imfilter(V(:,:,:,1),HX,'replicate','same')).*V(:,:,:,1) - ...
    imfilter(V(:,:,:,2),HY,'replicate','same').*V(:,:,:,2) - ...
    imfilter(V(:,:,:,3),HZ,'replicate','same').*V(:,:,:,3);
UNew(:,:,:,2) = -imfilter(V(:,:,:,1),HY,'replicate','same').*V(:,:,:,1) + ...
    (1 - imfilter(V(:,:,:,2),HY,'replicate','same')).*V(:,:,:,2) - ...
    imfilter(V(:,:,:,3),HY,'replicate','same').*V(:,:,:,3);
UNew(:,:,:,3) = -imfilter(V(:,:,:,1),HZ,'replicate','same').*V(:,:,:,1) - ...
    imfilter(V(:,:,:,2),HZ,'replicate','same').*V(:,:,:,2) + ...
    (1 - imfilter(V(:,:,:,3),HZ,'replicate','same')).*V(:,:,:,3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function FS = discreteCosineTransform(F,M,N,P);
% compute discrete cosine transform of 3-D vector field

% initialize resulting array
FS = F;

% first perform cosine transform down columns
len = 2*M-2; ind = 1:M;
for p=1:P
    for n=1:N
        s = fft(FS(:,n,p,:),len,1);
        FS(:,n,p,:) = real(s(ind,:,:,:));
    end
end
FS = sqrt(2/(M-1))*FS;

% next perform cosine transform across rows
len = 2*N-2; ind = 1:N;
for p=1:P
    for m=1:M
        s = fft(FS(m,:,p,:),len,2);
        FS(m,:,p,:) = real(s(:,ind,:,:));
    end
end
FS = sqrt(2/(N-1))*FS;

% finally perform cosine transform across pages
len = 2*P-2; ind = 1:P;
for n=1:N
    for m=1:M
        s = fft(FS(m,n,:,:),len,3);
        FS(m,n,:,:) = real(s(:,:,ind,:));
    end
end
FS = sqrt(2/(P-1))*FS;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function FNew = adjointNL(F,mu,lambda,gamma,M,N,P);
% multiply vector field F by adjoint Navier-Lame equations

% initialize FNew
FNew = zeros(M,N,P,3);

% construct filter that implements 3-D Laplacian
L = (lambda+2*mu)*cat(3,[0 0 0;0 1 0;0 0 0],[0 1 0;1 -6 1;0 1 0],[0 0 0;0 1 0;0 0 0]);

% we will need to use L to form two different filters
% L1 = -(lambda+2*mu)*L; L1(2,2,2) = gamma + L1(2,2,2);
% L2 = -mu*L; L2(2,2,2) = gamma + L2(2,2,2);

% construct grad div filters
GD11 = (lambda+mu)*cat(3,zeros(3,3),[0 1 0;0 -2 0;0 1 0],zeros(3,3));
GD22 = ipermute(GD11,[2 1 3]);
GD33 = ipermute(GD11,[3 2 1]);
GD23 = zeros(3,3,3);
GD23(2,1,1) = 1; GD23(2,3,3) = 1; GD23(2,1,3) = -1; GD23(2,3,1) = -1;
GD23 = GD23*(lambda+mu)/4;
GD12 = ipermute(GD23,[3 1 2]);
GD13 = ipermute(GD23,[2 3 1]);

% perform filtering
FNew(:,:,:,1) = imfilter(F(:,:,:,1),L-GD11,'replicate') + ...
    imfilter(F(:,:,:,2),-GD12,'replicate') + ...
    imfilter(F(:,:,:,3),-GD13,'replicate');
FNew(:,:,:,2) = imfilter(F(:,:,:,1),-GD12,'replicate') + ...
    imfilter(F(:,:,:,2),L-GD22,'replicate') + ...
    imfilter(F(:,:,:,3),-GD23,'replicate');
FNew(:,:,:,3) = imfilter(F(:,:,:,1),-GD13,'replicate') + ...
    imfilter(F(:,:,:,2),-GD23,'replicate') + ...
    imfilter(F(:,:,:,3),L-GD33,'replicate');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [DU,F,mu,lambda,PixSize,M,N,P,RegularizerFactor,HX,HY,HZ] = parse_inputs(varargin);

% get displacement field and check size
F = varargin{2};
PixSize = varargin{4}(1:3);
M = varargin{5};
N = varargin{6};
P = varargin{7};
mu = varargin{8};
lambda = varargin{9};
RegularizerFactor = varargin{10};
DU = varargin{11};
HX = varargin{12};
HY = varargin{13};
HZ = varargin{14};
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function p = cs_nd (A)
%CS_ND generalized nested dissection ordering.
%   p = cs_nd(A) computes the nested dissection ordering of a matrix.  Small
%   submatrices (order 500 or less) are ordered via cs_amd.  A must be sparse
%   and symmetric (use p = cs_nd(A|A') if it is not symmetric).
%
%   Example:
%       A = delsq (numgrid ('L', 300)) ;    % matrix used in 'bench'
%       p = cs_nd (A) ;
%       cspy (A (p,p)) ;
%
%   See also CS_AMD, CS_SEP, CS_ESEP, CS_NSEP, AMD.

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

n = size (A,1) ;
if (n == 1)
    p = 1 ;
elseif (n < 500)
    p = cs_amd (A) ;                % use cs_amd on small graphs
else
    [s a b] = cs_nsep (A) ;         % find a node separator
    a = a (cs_nd (A (a,a))) ;       % order A(a,a) recursively
    b = b (cs_nd (A (b,b))) ;       % order A(b,b) recursively
    p = [a b s] ;                   % concatenate to obtain the final ordering
end


function [y,dzdx2] = vl_nnscosinesim(x1, x2, varargin)
% VL_NNCOSINESIM Compute the cosine similariy between features
%   Y = VL_NNCOSINESIM(X1, X2) computes the cosine similarity between X1
%   and X2 where X1 has shape H x W x C x N, X2 has shape
%   H x W x C x N and Y is has shape 1 x 1 x 1 x N (producing a scalar
%   per batch element).
%
%   [DZDX1, DZDX2] = VL_NNCOSINESIM(X1, X2, DZDY) computes the
%   derivatives of the block projected onto DZDY. DZDX1 and DZDX2
%   have the same dimensions as X1, X2 and B respectively.
%
%   NOTES: The cosine similarity is defined between vectors A and B of
%   dimension N x 1 to be the scalar given by:
%   cos_sim = a'b / (norm(a,2) * norm(b,2))
%   To operate with tensors, the first three channels (H, W and C) are
%   treated as the vector elements, while the fourth dimension denotes
%   batch indicies.
%
%   VL_NNCOSINELOSS(.., 'option', value, ...) accepts the following options:
%
%   `eps`:: 1e-6
%    A small value to avoid possible division by zero.
%
% Copyright (C) 2018 Samuel Albanie.
% Licensed under The MIT License [see LICENSE.md for details]

  opts.eps = 1e-6 ;
  [~, dzdy] = vl_argparsepos(struct(), varargin) ;

  sz1 = size(x1) ; sz2 = size(x2) ;
  assert(all(sz1 == sz2), 'tensor sizes do not match') ;
  x1 = reshape(x1, [], sz1(4)) ;
  x2 = reshape(x2, [], sz1(4)) ;
  dots = dot(x1, x2, 1) ;
  x1_norms = sqrt(sum(x1 .* x1, 1)) ;
  x2_norms = sqrt(sum(x2 .* x2, 1)) ;

  if isempty(dzdy)
    y = dots ./ max(x1_norms .* x2_norms, opts.eps) ;
    y = reshape(y, 1, 1, 1, sz1(4)) ;
  else
    dzdy = dzdy{1} ; dsize = size(dzdy) ;
    assert(numel(dsize) == 4 & all(dsize(1:3) == ones(1,3)) ...
           & dsize(4) == sz1(4), 'DZDY has an unexpected size') ;
    t1 = bsxfun(@rdivide, x2, max(x1_norms .* x2_norms, opts.eps)) ;
    t2 = bsxfun(@rdivide, bsxfun(@times, x1, dots), ...
                              max(x1_norms .^3 .* x2_norms, opts.eps)) ;
    dzdx1 = t1 - t2 ;

    t1 = bsxfun(@rdivide, x1, max(x2_norms .* x1_norms, opts.eps)) ;
    t2 = bsxfun(@rdivide, bsxfun(@times, x2, dots), ...
                              max(x2_norms .^3 .* x1_norms, opts.eps)) ;
    dzdx2 = t1 - t2 ;

    % reshape to match input and compute projected derivatives
    dzdx1 = reshape(dzdx1, sz1) ;
    dzdx2 = reshape(dzdx2, sz2) ;
    dzdx1 = bsxfun(@times, dzdx1, dzdy) ;
    dzdx2 = bsxfun(@times, dzdx2, dzdy) ;
    y = dzdx1 ;
  end


function k = lfmvKernDiagCompute(lfmKern, t)

% LFMVKERNDIAGCOMPUTE Compute diagonal of LFMV kernel.
% FORMAT
% DESC computes the diagonal of the kernel matrix for velocity- velocity in
% the switching dynamical latent force kernel.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG t : input data matrix in the form of a design matrix.
% RETURN k : a vector containing the diagonal of the kernel matrix
% computed at the given points.
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

% Get length scale out.
sigma2 = 2/lfmKern.inverseWidth;
sigma = sqrt(sigma2);

% Parameters of the kernel
alpha(1) = lfmKern.damper./(2*lfmKern.mass);
omega(1) = sqrt(lfmKern.spring./lfmKern.mass - alpha(1)*alpha(1));

% Precomputations to increase the speed
preExp1 = zeros(length(t),2);
gamma1_p = alpha(1) + j*omega(1);
gamma1_m = alpha(1) - j*omega(1);
preGamma(1) = gamma1_p + gamma1_p;
preGamma(2) = gamma1_p + gamma1_m;
preGamma(3) = gamma1_m + gamma1_p;
preGamma(4) = gamma1_m + gamma1_m;
preConst = 1./preGamma;
preFactors(1) = preConst(2) - preConst(1);
preFactors(2) = preConst(3) - preConst(4);
preFactors(3) = preConst(3) - preConst(1);
preFactors(4) = preConst(2) - preConst(4);
preExp1(:,1) = gamma1_p.*exp(-gamma1_p*t);
preExp1(:,2) = gamma1_m.*exp(-gamma1_m*t);
% Actual computation of the kernel
sk =lfmDiagComputeH3VV(gamma1_p, gamma1_m, sigma2, t, preFactors([1 2]), 1) + ...
    lfmDiagComputeH3VV(gamma1_p, gamma1_m, sigma2, t, preFactors([3 4]), 0) + ...
    lfmDiagComputeH4VV(gamma1_p, gamma1_m, sigma2, t, preGamma([1 2 4 3]), preExp1 ) + ...
    lfmDiagComputeH4VV(gamma1_p, gamma1_m, sigma2, t, preGamma([1 3 4 2]), preExp1 );

if lfmKern.isNormalised
    k0 = lfmKern.sensitivity^2/(8*sqrt(2)*lfmKern.mass^2*omega^2);
else
    k0 = sqrt(pi)*sigma*lfmKern.sensitivity^2/(8*lfmKern.mass^2*omega^2);
end
k = k0*sk;

function h = lfmDiagComputeH3VV(gamma1_p, gamma1_m, sigma2, t, preFactor, mode)

h = preFactor(1)*lfmvvComputeUpsilonDiagVector(gamma1_p, sigma2, t, mode) ...
    + preFactor(2)*lfmvvComputeUpsilonDiagVector(gamma1_m,sigma2, t, mode);

function h = lfmDiagComputeH4VV(gamma1_p, gamma1_m, sigma2, t, ...
    preFactor, preExp)

h =   lfmvpComputeUpsilonVector(gamma1_p,sigma2, t, 0).*( preExp(:,2)/preFactor(2) - preExp(:,1)/preFactor(1)) ...
    + lfmvpComputeUpsilonVector(gamma1_m,sigma2, t, 0).*( preExp(:,1)/preFactor(4) - preExp(:,2)/preFactor(3));


function [transformationMatrix] = ...
    phase_difference_linear_registration3d(moving, movingCertainty, ...
    fixed, fixedCertainty, varargin)
% PHASE_DIFFERENCE_LINEAR_REGISTRATION3D Estimates a transformation matrix using phase-difference
%
% [transformationMatrix] = ...
%     phase_difference_linear_registration3d(moving, movingCertainty, ...
%     fixed, fixedCertainty)
%
% INPUT ARGUMENTS
% moving                    - Moving image
% movingCertainty           - Certainty mask of moving image
% fixed                     - Fixed image
% fixedCertainty            - Certainty mask of fixed image
%
% OPTIONAL INPUT ARGUMENTS
% 'transformationModel'     - Transformation model for estimating the
%                             displacement field
%                             'translation', 'affine' (default)
%
% OUTPUT ARGUMENTS
% transformationMatrix      - Estimated transformation matrix

% Copyright (c) 2012 Daniel Forsberg
% danne.forsberg@outlook.com
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

%% Setup default parameters
% translation, affine, non-rigid
transformationModel = 'affine';

% Overwrites default parameter
for k=1:2:length(varargin)
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

% Initialize transformation matrix
transformationMatrix = eye(4);

% Load quadrature filters
load quadratureFiltersLinearRegistration3D

% Perform quadrature filtering
q21 = imfilter(moving,f1,'same','conv');
q22 = imfilter(moving,f2,'same','conv');
q23 = imfilter(moving,f3,'same','conv');

q11 = imfilter(fixed,f1,'same','conv');
q12 = imfilter(fixed,f2,'same','conv');
q13 = imfilter(fixed,f3,'same','conv');

filterSize = (size(real(f1),1)-1)/2;
q11 = q11(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
q12 = q12(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
q13 = q13(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
q21 = q21(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
q22 = q22(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
q23 = q23(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);

% Compute phase-difference, certainties and phase-gradients
dphi(:,:,:,1) = angle(q11.*conj(q21));
dphi(:,:,:,2) = angle(q12.*conj(q22));
dphi(:,:,:,3) = angle(q13.*conj(q23));

% Estimate certainties
certainty(:,:,:,1) = abs(q11.*q21).*((cos(dphi(:,:,:,1)/2)).^2);
certainty(:,:,:,2) = abs(q12.*q22).*((cos(dphi(:,:,:,2)/2)).^2);
certainty(:,:,:,3) = abs(q13.*q23).*((cos(dphi(:,:,:,3)/2)).^2);

% Add certainty masks
if ~isempty(fixedCertainty)
    fixedCertainty = fixedCertainty(...
        filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
    certainty = bsxfun(@times,certainty,fixedCertainty);
end
if ~isempty(movingCertainty)
    movingCertainty = movingCertainty(...
        filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
    certainty = bsxfun(@times,certainty,movingCertainty);
end

% Estimate gradients of phi
grad_x_dphi_n1 = zeros(size(q11));
grad_x_dphi_n1(:,2:end-1,:) = ...
    angle(q11(:,3:end,:).*conj(q11(:,2:end-1,:)) + ...
    q11(:,2:end-1,:).*conj(q11(:,1:end-2,:)) + ...
    q21(:,3:end,:).*conj(q21(:,2:end-1,:)) + ...
    q21(:,2:end-1,:).*conj(q21(:,1:end-2,:)));

grad_y_dphi_n2 = zeros(size(q11));
grad_y_dphi_n2(2:end-1,:,:) = ...
    angle(q12(3:end,:,:).*conj(q12(2:end-1,:,:)) + ...
    q12(2:end-1,:,:).*conj(q12(1:end-2,:,:)) + ...
    q22(3:end,:,:).*conj(q22(2:end-1,:,:)) + ...
    q22(2:end-1,:,:).*conj(q22(1:end-2,:,:)));

grad_z_dphi_n3 = zeros(size(q11));
grad_z_dphi_n3(:,:,2:end-1) = ...
    angle(q13(:,:,3:end).*conj(q13(:,:,2:end-1)) + ...
    q13(:,:,2:end-1).*conj(q13(:,:,1:end-2)) + ...
    q23(:,:,3:end).*conj(q23(:,:,2:end-1)) + ...
    q23(:,:,2:end-1).*conj(q23(:,:,1:end-2)));

% Save all the phase gradients nicely...
phaseGradient(:,:,:,1) = grad_x_dphi_n1;
phaseGradient(:,:,:,2) = grad_y_dphi_n2;
phaseGradient(:,:,:,3) = grad_z_dphi_n3;

% Build A and h for A p = h
[A, h] = build_A_h_3d(dphi, certainty, phaseGradient, transformationModel);

% Solve the equation system
switch transformationModel
    case 'translation'
        d = A \ h;
        transformationMatrix(1:3,4) = d;
    case {'rigid','affine'}
        p = A \ h;
        transformationMatrix(1:3,1:4) = [1+p(1) p(2) p(3) p(10);...
            p(4) 1+p(5) p(6) p(11);...
            p(7) p(8) 1+p(9) p(12)];
end