classdef (Abstract) math_model < mp.element_container & opt_model
%MP.MATH_MODEL  MATPOWER mathematical model abstract base class.
%   ?
%
%   MP.MATH_MODEL provides properties and methods related to the specific
%   problem specification being solved (e.g. power flow, continuation
%   power flow, optimal power flow, etc.) ...
%
%   Properties
%       ? - ?
%
%   Methods
%       ?

%   MATPOWER
%   Copyright (c) 2021-2022, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

    properties
        aux_data    %% struct of auxiliary data relevant to the model,
                    %% e.g. can be passed to model constraint functions
    end

    methods
        function tag = task_tag(obj)
            error('mp.math_model/task_tag: must be implemented in subclass');
        end

        function name = task_name(obj)
            error('mp.math_model/task_name: must be implemented in subclass');
        end

        function tag = form_tag(obj)
            error('mp.math_model/form_tag: must be implemented in subclass');
        end

        function name = form_name(obj)
            error('mp.math_model/form_name: must be implemented in subclass');
        end

        function obj = build(obj, nm, dm, mpopt)
            %% Due to a bug related to inheritance in constructors in
            %% Octave 5.2 and earlier (https://savannah.gnu.org/bugs/?52614),
            %% init_set_types() cannot be called directly in the
            %% MP_IDX_MANAGER constructor, as desired.
            %%
            %% WORKAROUND:  Initialize MP_IDX_MANAGER fields here, if needed,
            %%              after object construction, but before object use.
            if isempty(obj.var)         %% only if not already initialized
                obj.init_set_types();
            end

            %% create element objects for each class with data
            obj.elements = mp.mapped_array();
            for c = obj.element_classes
                mme = c{1}();       %% element constructor
                if dm.online(mme.name)      %% dm element exists
                    obj.elements.add_elements(mme, mme.name);
                end
            end

            obj.add_aux_data(nm, dm, mpopt);
            obj.add_vars(nm, dm, mpopt);
            obj.add_constraints(nm, dm, mpopt);
            obj.add_costs(nm, dm, mpopt);
        end

        function display(obj)
            fprintf('MATH MODEL CLASS : %s\n', class(obj));
            fprintf('    TASK NAME               : %s\n', obj.task_name());
            fprintf('    TASK TAG                : %s\n', obj.task_tag());
            fprintf('    FORMULATION NAME        : %s\n', obj.form_name());
            fprintf('    FORMULATION TAG         : %s\n', obj.form_tag());
            display@opt_model(obj)

            %% elements
            fprintf('\nELEMENTS\n')
            fprintf('========\n')
            fprintf('  name              class\n');
            fprintf(' ----------------  --------------------\n');
            for k = 1:length(obj.elements)
                mme = obj.elements{k};
                fprintf('  %-13s     %s\n', mme.name, class(mme));
            end
        end

        function obj = add_aux_data(obj, nm, dm, mpopt)
            %% create aux_data struct
            obj.aux_data = obj.build_aux_data(nm, dm, mpopt);
        end

        function ad = build_base_aux_data(obj, nm, dm, mpopt)
            %% get model variables
            vvars = nm.model_vvars();
            zvars = nm.model_zvars();
            vars = {vvars{:} zvars{:}};
            vals = cellfun(@(x)nm.params_var(x), vars, 'UniformOutput', false);

            %% get node types
            [ref, pv, pq, by_elm] = nm.node_types(nm, dm);

            %% create aux_data struct
            ad = cell2struct(...
                {vals{:}, {}, ref, pv, pq, ...
                    length(ref), length(pv), length(pq), by_elm}, ...
                {vars{:}, 'var_map', 'ref', 'pv', 'pq', ...
                    'nref', 'npv', 'npq', 'node_type_by_elm'}, 2);
        end

        function obj = add_vars(obj, nm, dm, mpopt)
            obj.add_system_vars(nm, dm, mpopt);

            %% each element adds its OPF variables
            for k = 1:length(obj.elements)
                obj.elements{k}.add_vars(obj, nm, dm, mpopt);
            end
        end

        function obj = add_system_vars(obj, nm, dm, mpopt)
        end

        function obj = add_constraints(obj, nm, dm, mpopt)
            obj.add_system_constraints(nm, dm, mpopt);

            %% each element adds its OPF variables
            for k = 1:length(obj.elements)
                obj.elements{k}.add_constraints(obj, nm, dm, mpopt);
            end
        end

        function obj = add_system_constraints(obj, nm, dm, mpopt)
            %% node balance constraints
            obj.add_node_balance_constraints(nm, dm, mpopt);
        end

        function obj = add_node_balance_constraints(obj, nm, dm, mpopt)
        end

        function obj = add_costs(obj, nm, dm, mpopt)
            obj.add_system_costs(nm, dm, mpopt);

            %% each element adds its OPF variables
            for k = 1:length(obj.elements)
                obj.elements{k}.add_costs(obj, nm, dm, mpopt);
            end
        end

        function obj = add_system_costs(obj, nm, dm, mpopt)
        end

        function opt = solve_opts(obj, nm, dm, mpopt)
            opt = struct();
        end

        function nm_vars = update_nm_vars(obj, mmx, nm)
            %% Returns a struct with the network model variables as fields.
            %% The ad.var_map cell array is used to track mappings
            %% of math model variables back to network model variables.
            %% Each entry is a cell array:
            %%  {nm_var_type, nm_i1, nm_iN, nm_idx, mm_i1, mm_iN, mm_idx}
            %% where
            %%  nm_var_type - network model variable type (e.g. va, vm, zr, zi)
            %%  nm_i1 - starting index for network model variable type
            %%  nm_iN - ending index for network model variable type
            %%  nm_idx - vector of indices for network model variable type
            %%  mm_i1 - starting index for math model variable
            %%  mm_iN - ending index for math model variable
            %%  mm_idx - vector of indices for math model variable
            %% Uses either i1:iN (if i1 is not empty) or idx as the indices,
            %% unless both are empty, in which case it uses ':'

            ad = obj.aux_data;
            vvars = nm.model_vvars();
            zvars = nm.model_zvars();
            vars = {vvars{:} zvars{:}};
            nm_vars = cell2struct( ...
                cellfun(@(x)ad.(x), vars, 'UniformOutput', false), ...
                vars, 2);

            %% copy mm state values to nm state based on ad.var_map
            for k = 1:length(ad.var_map)
                d = ad.var_map{k};

                %% right hand side from mmx
                if ~isempty(d{5})       %% use i1:iN
                    rhs = mmx(d{5}:d{6});
                elseif isempty(d{7})    %% use :
                    rhs = mmx;
                else                    %% use idx
                    rhs = mmx(d{7});
                end

                %% left hand side to nm_vars
                name = d{1};
                if ~isempty(rhs)
                    if ~isempty(d{2})       %% use i1:iN
                        nm_vars.(name)(d{2}:d{3}) = rhs;
                    elseif isempty(d{4})    %% use :
                        nm_vars.(name) = rhs;
                    else                    %% use idx
                        nm_vars.(name)(d{4}) = rhs;
                    end
                end
            end
        end

        function dm = data_model_update(obj, nm, dm, mpopt)
            %% each element updates its data model
            for k = 1:length(obj.elements)
                obj.elements{k}.data_model_update(obj, nm, dm, mpopt);
            end
        end

        function nm = network_model_x_soln(obj, nm)
            %% convert solved state from math model to network model soln
            [nm.soln.v, nm.soln.z, nm.soln.x] = ...
                obj.convert_x_m2n(obj.soln.x, nm);
        end
    end     %% methods
end         %% classdef


function replay_scatter_abund(frames_scatter, frames_abund, fps)
%REPLAY_SCATTER_ABUND Summary of this function goes here
%   Detailed explanation goes here
if nargin < 3
    fps = 5;
end

concat = 0;
if length(frames_scatter) == length(frames_abund)
    scatter1 = frames_scatter(1);
    abund1 = frames_abund(1);
    if isempty(scatter1.colormap) && isempty(abund1.colormap)
        concat = 1;
    end
end

if ~concat
    scatter_fh = figure('name','Replay evolution of the Gaussians');
    movie(scatter_fh,frames_scatter,1,fps);
    abund_fh = figure('name','Replay evolution of the abundances');
    movie(abund_fh,frames_abund,1,fps);
    return;
end

N = length(frames_scatter);
disp(['Replay ',num2str(N), ' frames']);
for i = 1:N
    scatter_data = frames_scatter(i).cdata;
    abund_data = frames_abund(i).cdata;
    new_data = cat(2,scatter_data,abund_data);
    new_frames(i).cdata = new_data;
    new_frames(i).colormap = [];
end

fh = figure('name','Replay evolution of the Gaussians and the abundances');
pos = get(fh,'position');
pos(1) = pos(1) - pos(3)/2;
pos(3) = pos(3) * 2;
set(fh,'position',pos);
movie(fh,new_frames,1,fps);


% eeg_load_difdata_script - Script to load ERP difference waves
% Load data (680 rows x 124 cols)
eegpath = 'D:\data_emse\ptsdpet\link14hz\'; cd(eegpath);
ext = '_link14hz';
groups = {'c' 'p'};
cond = {'tac_oac'};
subs = [1:10];

% Define sample rate & epoch parameters
sample_rate = 2.5;
epoch_start = -200;
epoch_end = 1500;
points = 681;
timeArray = meshgrid(epoch_start:sample_rate:epoch_end,1)';
timeNonZero = find(timeArray);
timeZero = find(timeArray == 0);

for g = 1:length(groups),
    for i=1:length(subs),
        for c=1:length(cond),

            % Load data
            file = sprintf('%s%s%02d%s%s.txt',eegpath,groups{g},subs(i),cond{c},ext);
            data = sprintf(  '%s%02d%s%s',            groups{g},subs(i),cond{c},ext);
            if ~exist(data,'var')
                fprintf('loading %s\n',file);
                load(file);

                % Interpolate the Zero value, only useful for ascii from
                % Neuroscan 3x or 4x AVG files
                volt = eval(data);
                InterpZero = interp1( timeArray(timeNonZero), volt, 0, 'cubic' );
                volt = [volt(1:timeZero-1,:); InterpZero; volt(timeZero:end,:)];
                eval(strcat(data,' = volt;'));

            else
                fprintf('Already loaded %s\n',file);
                % Save translated data file

                %dat = eval(data); dat = dat';
                %file = sprintf('%s%s%02d%s%s.dat',eegpath,groups{g},subs(i),cond{c},ext);
                %eeg_write_ascii(file,dat,'%12.4f');
            end
        end
    end
end

clear g i c file data timeNonZero volt InterpZero


function setup(varargin)
% SETUP  Add the required search paths to MATLAB
run matconvnet/matlab/vl_setupnn ;
addpath matconvnet/examples ;

opts.useGpu = false ;
opts.verbose = false ;
opts.enableImReadJPEG = false ;
opts = vl_argparse(opts, varargin) ;

try
  vl_nnconv(single(1),single(1),[]) ;
catch
  warning('VL_NNCONV() does not seem to be compiled. Trying to compile it now.') ;
  vl_compilenn('enableGpu', opts.useGpu, ...
               'enableImReadJPEG', opts.enableImReadJPEG, ...
               'verbose', opts.verbose) ;
end

if opts.useGpu
  try
    vl_nnconv(gpuArray(single(1)),gpuArray(single(1)),[]) ;
  catch
    vl_compilenn('enableGpu', opts.useGpu, ...
                 'enableImReadJPEG', opts.enableImReadJPEG, ...
                 'verbose', opts.verbose) ;
    warning('GPU support does not seem to be compiled in MatConvNet. Trying to compile it now') ;
  end
end


function gpplot(x, y, e, varargin)
% function gpplot(x, y, e, flipaxis)
%
% x = inputs (e.g., time)
% y = expectated values of the outputs
% e = errors of the outputs

warning('Deprecated, use errorplot')

opts = struct('pseudoinputs', []);
[ opts, errmsg, wrnmsg ] = argschk( opts, varargin{:} );
if ~isempty(errmsg), error( errmsg ), end
if ~isempty(wrnmsg), warning( wrnmsg ), end

% $$$ if nargin < 4
% $$$   flipaxis = false;
% $$$ end
x = x(:);
y = y(:);
%e = sqrt(diag(Cov));
X = [x(1:end); x(end:-1:1)];
Y = [y(1:end)+e; y(end:-1:1)-e(end:-1:1)];
C = [.75,.75,.75];
% $$$ if flipaxis
% $$$   fill(Y,X,C,'EdgeColor',C);
% $$$   hold on
% $$$   plot(y,x,'k');
% $$$ else
fill(X,Y,C,'EdgeColor',C);
hold on
plot(x,y,'k');
if ~isempty(opts.pseudoinputs)
  yl = ylim;
  py = yl(1) + 0.8*(yl(2)-yl(1));
  plot(opts.pseudoinputs, py, 'k+')
end
% $$$ end


classdef CC < handle & matlab.mixin.Copyable

    properties (Access = public)
        value
        gradient
    end

    properties (GetAccess = public, SetAccess = private)
        shapeFunctions
        nSF
    end

    properties (Access = private)
        sizeDesigVar
        valueOld
        gradientOld
    end

    methods (Access = protected, Abstract)
        updateFields(obj)
    end

    methods (Access = public)

        function computeFunctionAndGradient(obj)
            obj.initValueAndGradient();
            for iSF = 1:length(obj.shapeFunctions)
                obj.shapeFunctions{iSF}.updateTargetParameters();
                obj.shapeFunctions{iSF}.computeFunctionAndGradient();
                obj.updateFields(iSF);
            end
        end

        function computeFunction(obj)
            obj.initValueAndGradient();
            for iSF = 1:length(obj.shapeFunctions)
                obj.shapeFunctions{iSF}.updateTargetParameters();
                obj.shapeFunctions{iSF}.computeFunction();
                obj.updateFields(iSF);
            end
        end

        function objClone = clone(obj)
            objClone = copy(obj);
        end

        function restart(obj)
            obj.value    = obj.valueOld;
            obj.gradient = obj.gradientOld;
        end

        function updateOld(obj)
           obj.valueOld    = obj.value;
           obj.gradientOld = obj.gradient;
        end

    end

    methods (Access = protected)

        function obj = init(obj,cParams)
            obj.nSF   = 0;
            obj.sizeDesigVar = size(cParams.designVar.value);
            obj.createShapeFunctions(cParams);
        end

    end

    methods (Access = private)

        function initValueAndGradient(obj)
            obj.value = 0;
            obj.gradient = zeros(obj.sizeDesigVar);
        end

        function createShapeFunctions(obj,cParams)
            nS = cParams.nShapeFuncs;
            for iS = 1:nS
                s = cParams.shapeFuncSettings{iS};
                s.designVariable = cParams.designVar;
                s.shNumber = iS;

                if isprop(cParams,'homogenizedVarComputer')
                    s.homogVarComputer = cParams.homogenizedVarComputer;
                end
                if isprop(cParams,'targetParameters')
                    s.targetParameters = cParams.targetParameters;
                end
                shapeFunction = ShapeFunctional.create(s);
                obj.append(shapeFunction);
            end
        end

        function append(obj,shapeFunction)
            obj.shapeFunctions{obj.nSF+1} = shapeFunction;
            obj.nSF = obj.nSF+1;
        end

    end

end


function out = getChebfunType(~)
%GETCHEBFUNTYPE   Returns SPHEREFUN for a SPINOPSPHERE.
%
% See also SPINOPSPHERE.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

out = @spherefun;

end


function value = i1mach ( i )

%*****************************************************************************80
%
%% I1MACH returns integer machine constants.
%
%  Discussion:
%
%    Input/output unit numbers.
%
%      I1MACH(1) = the standard input unit.
%      I1MACH(2) = the standard output unit.
%      I1MACH(3) = the standard punch unit.
%      I1MACH(4) = the standard error message unit.
%
%    Words.
%
%      I1MACH(5) = the number of bits per integer storage unit.
%      I1MACH(6) = the number of characters per integer storage unit.
%
%    Integers.
%
%    Assume integers are represented in the S digit base A form:
%
%      Sign * (X(S-1)*A^(S-1) + ... + X(1)*A + X(0))
%
%    where 0 <= X(1:S-1) < A.
%
%      I1MACH(7) = A, the base.
%      I1MACH(8) = S, the number of base A digits.
%      I1MACH(9) = A^S-1, the largest integer.
%
%    Floating point numbers
%
%    Assume floating point numbers are represented in the T digit
%    base B form:
%
%      Sign * (B^E) * ((X(1)/B) + ... + (X(T)/B^T) )
%
%    where 0 <= X(I) < B for I=1 to T, 0 < X(1) and EMIN <= E <= EMAX.
%
%      I1MACH(10) = B, the base.
%
%    Single precision
%
%      I1MACH(11) = T, the number of base B digits.
%      I1MACH(12) = EMIN, the smallest exponent E.
%      I1MACH(13) = EMAX, the largest exponent E.
%
%    Double precision
%
%      I1MACH(14) = T, the number of base B digits.
%      I1MACH(15) = EMIN, the smallest exponent E.
%      I1MACH(16) = EMAX, the largest exponent E.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 April 2007
%
%  Author:
%
%    Original FORTRAN77 version by Phyllis Fox, Andrew Hall, Norman Schryer
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Phyllis Fox, Andrew Hall, Norman Schryer,
%    Algorithm 528,
%    Framework for a Portable Library,
%    ACM Transactions on Mathematical Software,
%    Volume 4, Number 2, June 1978, page 176-188.
%
%  Parameters:
%
%    Input, integer I, chooses the parameter to be returned.
%    1 <= I <= 16.
%
%    Output, integer VALUE, the value of the chosen parameter.
%
  if ( i < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I1MACH - Fatal error!\n' );
    fprintf ( 1, '  The input argument I is out of bounds.\n' );
    fprintf ( 1, '  Legal values satisfy 1 <= I <= 16.\n' );
    fprintf ( 1, '  I = %d\n', i );
    value = 0;
    error ( 'I1MACH - Fatal error!' );
  elseif ( i == 1 )
    value = 5;
  elseif ( i == 2 )
    value = 6;
  elseif ( i == 3 )
    value = 7;
  elseif ( i == 4 )
    value = 6;
  elseif ( i == 5 )
    value = 32;
  elseif ( i == 6 )
    value = 4;
  elseif ( i == 7 )
    value = 2;
  elseif ( i == 8 )
    value = 31;
  elseif ( i == 9 )
    value = 2147483647;
  elseif ( i == 10 )
    value = 2;
  elseif ( i == 11 )
    value = 24;
  elseif ( i == 12 )
    value = -125;
  elseif ( i == 13 )
    value = 128;
  elseif ( i == 14 )
    value = 53;
  elseif ( i == 15 )
    value = -1021;
  elseif ( i == 16 )
    value = 1024;
  elseif ( 16 < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I1MACH - Fatal error!\n' );
    fprintf ( 1, '  The input argument I is out of bounds.\n' );
    fprintf ( 1, '  Legal values satisfy 1 <= I <= 16.\n' );
    fprintf ( 1, '  I = %d\n', i );
    value = 0;
    error ( 'I1MACH - Fatal error!' );
  end

  return
end


function [Grad,t]=StdBit(tStart,tEnd,gAmp)

%create a minimum signal pulse starting from tStart
%tStart signal pulse start time
%tEnd   signal pulse end time

[Grad,t]=StdRect(tStart,tEnd,gAmp,3); % signal bit
Grad(1)=0;
Grad(end)=0;

end


function sparse_interp_nd_test ( )

%*****************************************************************************80
%
%% SPARSE_INTERP_ND_TEST tests the SPARSE_INTERP_ND library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 October 2012
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../r8lib' )

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_INTERP_ND_TEST\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Test the SPARSE_INTERP_ND library.\n' );
  fprintf ( 1, '  The R8LIB library is also required.\n' );

  m = 1;
  sparse_max = 9;
  sparse_interp_nd_test01 ( m, sparse_max );

  m = 2;
  sparse_max = 9;
  sparse_interp_nd_test01 ( m, sparse_max );

  m = 3;
  sparse_max = 9;
  sparse_interp_nd_test01 ( m, sparse_max );

  m = 4;
  sparse_max = 7;
  sparse_interp_nd_test01 ( m, sparse_max );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_INTERP_ND_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  rmpath ( '../r8lib' )

  return
end


function bench_plot(times,res,solvers,prob)
%BENCH_PLOT  Plot the results of a benchmark test
%
% bench_plot(times,res,solvers,prob) not intended to be called by a user -
% use optiBench.

%   Copyright (C) 2011 Jonathan Currie (I2C2)

nS = length(solvers);   % Number of Solvers
nP = length(times{1});  % Number of Test problems

tol = 1e-4;

%Determine maximum time for normalizing
maxT = 0;
for i = 1:nS
    s = sum(times{i});
    if(s > maxT), maxT = s; end
end
% maxT = 1;

[~,srtInd] = sort(times{1});

%Generate Plot Data
X = zeros(2*nP+3,nS); Y = X;
str = cell(nS,1);
for i = 1:nS
    acc = res{i}.acc(srtInd); ind = isinf(acc);
    if(any(ind))
        acc(ind) = 1;
    end
    ind = acc < tol;
    fM = sum(ind)/nP;
    fC = [0;cumsum(ind)/nP;fM];
    fT = [0;cumsum(times{i}(srtInd));maxT];
    [X(:,i),Y(:,i)] = stairs(fT,fC);
    str{i} = [upper(solvers{i}) ': ' num2str(fM*100,'%2.2f') '%'];
end

% %Determine maximum time for normalizing
% maxT = 0;
% for i = 1:nS
%     s = sum(times{i});
%     if(s > maxT), maxT = s; end
% end
%
% %Generate Plot Data
% X = zeros(2*nP+3,nS); Y = X;
% str = cell(nS,1);
% for i = 1:nS
%     acc = res{i}.acc; ind = isinf(acc);
%     if(any(ind))
%         acc(ind) = 1;
%     end
%     ind = acc < tol;
%     fM = sum(ind)/nP;
%     fC = [0;cumsum(ind)/nP;fM];
%     fT = [0;cumsum(times{i})/maxT;1];
%     [X(:,i),Y(:,i)] = stairs(fT,fC);
%     str{i} = [upper(solvers{i}) ': ' num2str(fM*100,'%2.2f') '%'];
% end

%Plot
plot(X,Y);
xlabel('Time [s]');
ylabel('Fraction of Problems Solved');
title([upper(prob) ' Benchmark Results for ' num2str(nP) ' Problems']);
axis([0 maxT 0 1.1]);
legend(str,'location','SE');


function obj = Jacob_pnp_func_new(in1,in2,in3,in4,in5)
coef_f0_q_sym1 = in1(:,1);
coef_f0_q_sym2 = in1(:,2);
coef_f1_q_sym1 = in2(:,1);
coef_f0_q_sym3 = in1(:,3);
coef_f1_q_sym2 = in2(:,2);
coef_f2_q_sym1 = in3(:,1);
coef_f0_q_sym4 = in1(:,4);
coef_f1_q_sym3 = in2(:,3);
coef_f2_q_sym2 = in3(:,2);
coef_f3_q_sym1 = in4(:,1);
coef_f0_q_sym5 = in1(:,5);
coef_f1_q_sym4 = in2(:,4);
coef_f2_q_sym3 = in3(:,3);
coef_f3_q_sym2 = in4(:,2);
coef_f0_q_sym6 = in1(:,6);
coef_f1_q_sym5 = in2(:,5);
coef_f2_q_sym4 = in3(:,4);
coef_f3_q_sym3 = in4(:,3);
coef_f0_q_sym7 = in1(:,7);
coef_f1_q_sym6 = in2(:,6);
coef_f2_q_sym5 = in3(:,5);
coef_f3_q_sym4 = in4(:,4);
coef_f0_q_sym8 = in1(:,8);
coef_f1_q_sym7 = in2(:,7);
coef_f2_q_sym6 = in3(:,6);
coef_f3_q_sym5 = in4(:,5);
coef_f0_q_sym9 = in1(:,9);
coef_f1_q_sym8 = in2(:,8);
coef_f2_q_sym7 = in3(:,7);
coef_f3_q_sym6 = in4(:,6);
coef_f1_q_sym9 = in2(:,9);
coef_f2_q_sym8 = in3(:,8);
coef_f3_q_sym7 = in4(:,7);
coef_f2_q_sym9 = in3(:,9);
coef_f3_q_sym8 = in4(:,8);
coef_f3_q_sym9 = in4(:,9);
coef_f0_q_sym10 = in1(:,10);
coef_f0_q_sym11 = in1(:,11);
coef_f0_q_sym20 = in1(:,20);
coef_f1_q_sym10 = in2(:,10);
coef_f0_q_sym12 = in1(:,12);
coef_f0_q_sym21 = in1(:,21);
coef_f1_q_sym11 = in2(:,11);
coef_f1_q_sym20 = in2(:,20);
coef_f2_q_sym10 = in3(:,10);
coef_f0_q_sym13 = in1(:,13);
coef_f0_q_sym22 = in1(:,22);
coef_f1_q_sym12 = in2(:,12);
coef_f1_q_sym21 = in2(:,21);
coef_f2_q_sym11 = in3(:,11);
coef_f2_q_sym20 = in3(:,20);
coef_f3_q_sym10 = in4(:,10);
coef_f0_q_sym14 = in1(:,14);
coef_f0_q_sym23 = in1(:,23);
coef_f1_q_sym13 = in2(:,13);
coef_f1_q_sym22 = in2(:,22);
coef_f2_q_sym12 = in3(:,12);
coef_f2_q_sym21 = in3(:,21);
coef_f3_q_sym11 = in4(:,11);
coef_f3_q_sym20 = in4(:,20);
coef_f0_q_sym15 = in1(:,15);
coef_f0_q_sym24 = in1(:,24);
coef_f1_q_sym14 = in2(:,14);
coef_f1_q_sym23 = in2(:,23);
coef_f2_q_sym13 = in3(:,13);
coef_f2_q_sym22 = in3(:,22);
coef_f3_q_sym12 = in4(:,12);
coef_f3_q_sym21 = in4(:,21);
coef_f0_q_sym16 = in1(:,16);
coef_f1_q_sym15 = in2(:,15);
coef_f1_q_sym24 = in2(:,24);
coef_f2_q_sym14 = in3(:,14);
coef_f2_q_sym23 = in3(:,23);
coef_f3_q_sym13 = in4(:,13);
coef_f3_q_sym22 = in4(:,22);
coef_f0_q_sym17 = in1(:,17);
coef_f1_q_sym16 = in2(:,16);
coef_f2_q_sym15 = in3(:,15);
coef_f2_q_sym24 = in3(:,24);
coef_f3_q_sym14 = in4(:,14);
coef_f3_q_sym23 = in4(:,23);
coef_f0_q_sym18 = in1(:,18);
coef_f1_q_sym17 = in2(:,17);
coef_f2_q_sym16 = in3(:,16);
coef_f3_q_sym15 = in4(:,15);
coef_f3_q_sym24 = in4(:,24);
coef_f0_q_sym19 = in1(:,19);
coef_f1_q_sym18 = in2(:,18);
coef_f2_q_sym17 = in3(:,17);
coef_f3_q_sym16 = in4(:,16);
coef_f1_q_sym19 = in2(:,19);
coef_f2_q_sym18 = in3(:,18);
coef_f3_q_sym17 = in4(:,17);
coef_f2_q_sym19 = in3(:,19);
coef_f3_q_sym18 = in4(:,18);
coef_f3_q_sym19 = in4(:,19);
q0 = in5(1,:);
q1 = in5(2,:);
q2 = in5(3,:);
q3 = in5(4,:);
t114 = q0.^2;
t115 = q1.^2;
t116 = q2.^2;
t117 = q3.^2;
t118 = coef_f0_q_sym11.*q0;
t119 = coef_f0_q_sym24.*q3;
t120 = coef_f0_q_sym1.*q0.*t114;
t121 = coef_f0_q_sym23.*q3.*t117;
t122 = coef_f0_q_sym4.*q3.*t114;
t123 = coef_f0_q_sym10.*q0.*t117;
t124 = coef_f0_q_sym6.*q0.*q1.*q2.*2.0;
t125 = coef_f0_q_sym16.*q1.*q2.*q3.*2.0;
t126 = coef_f0_q_sym18.*q1;
t127 = coef_f0_q_sym22.*q2;
t128 = coef_f0_q_sym12.*q1.*t115;
t129 = coef_f0_q_sym19.*q2.*t116;
t130 = coef_f0_q_sym2.*q1.*t114;
t131 = coef_f0_q_sym5.*q0.*t115;
t132 = coef_f0_q_sym3.*q2.*t114;
t133 = coef_f0_q_sym8.*q0.*t116;
t134 = coef_f0_q_sym7.*q0.*q1.*q3.*2.0;
t135 = coef_f0_q_sym9.*q0.*q2.*q3.*2.0;
obj = reshape([coef_f1_q_sym11.*q0.*-2.0+coef_f0_q_sym11.*q1-coef_f1_q_sym18.*q1-coef_f1_q_sym22.*q2-coef_f1_q_sym24.*q3-coef_f1_q_sym1.*q0.*t114.*4.0+coef_f0_q_sym2.*q0.*t115.*2.0-coef_f1_q_sym5.*q0.*t115.*2.0-coef_f1_q_sym8.*q0.*t116.*2.0-coef_f1_q_sym10.*q0.*t117.*2.0+coef_f0_q_sym1.*q1.*t114.*3.0-coef_f1_q_sym2.*q1.*t114.*3.0+coef_f0_q_sym5.*q1.*t115+coef_f0_q_sym8.*q1.*t116+coef_f0_q_sym10.*q1.*t117-coef_f1_q_sym12.*q1.*t115-coef_f1_q_sym15.*q1.*t116-coef_f1_q_sym17.*q1.*t117-coef_f1_q_sym3.*q2.*t114.*3.0+coef_f0_q_sym6.*q2.*t115-coef_f1_q_sym13.*q2.*t115-coef_f1_q_sym21.*q2.*t117-coef_f1_q_sym19.*q2.*t116-coef_f1_q_sym4.*q3.*t114.*3.0+coef_f0_q_sym7.*q3.*t115-coef_f1_q_sym20.*q3.*t116-coef_f1_q_sym14.*q3.*t115-coef_f1_q_sym23.*q3.*t117+coef_f0_q_sym3.*q0.*q1.*q2.*2.0-coef_f1_q_sym6.*q0.*q1.*q2.*2.0+coef_f0_q_sym4.*q0.*q1.*q3.*2.0-coef_f1_q_sym7.*q0.*q1.*q3.*2.0-coef_f1_q_sym9.*q0.*q2.*q3.*2.0+coef_f0_q_sym9.*q1.*q2.*q3-coef_f1_q_sym16.*q1.*q2.*q3,coef_f2_q_sym11.*q0.*-2.0-coef_f2_q_sym18.*q1+coef_f0_q_sym11.*q2-coef_f2_q_sym22.*q2-coef_f2_q_sym24.*q3-coef_f2_q_sym1.*q0.*t114.*4.0+coef_f0_q_sym3.*q0.*t116.*2.0-coef_f2_q_sym5.*q0.*t115.*2.0-coef_f2_q_sym8.*q0.*t116.*2.0-coef_f2_q_sym10.*q0.*t117.*2.0-coef_f2_q_sym2.*q1.*t114.*3.0+coef_f0_q_sym6.*q1.*t116-coef_f2_q_sym12.*q1.*t115-coef_f2_q_sym15.*q1.*t116-coef_f2_q_sym17.*q1.*t117+coef_f0_q_sym1.*q2.*t114.*3.0-coef_f2_q_sym3.*q2.*t114.*3.0+coef_f0_q_sym5.*q2.*t115+coef_f0_q_sym8.*q2.*t116+coef_f0_q_sym10.*q2.*t117-coef_f2_q_sym13.*q2.*t115-coef_f2_q_sym21.*q2.*t117-coef_f2_q_sym19.*q2.*t116-coef_f2_q_sym4.*q3.*t114.*3.0+coef_f0_q_sym9.*q3.*t116-coef_f2_q_sym20.*q3.*t116-coef_f2_q_sym14.*q3.*t115-coef_f2_q_sym23.*q3.*t117+coef_f0_q_sym2.*q0.*q1.*q2.*2.0-coef_f2_q_sym6.*q0.*q1.*q2.*2.0-coef_f2_q_sym7.*q0.*q1.*q3.*2.0+coef_f0_q_sym4.*q0.*q2.*q3.*2.0-coef_f2_q_sym9.*q0.*q2.*q3.*2.0+coef_f0_q_sym7.*q1.*q2.*q3-coef_f2_q_sym16.*q1.*q2.*q3,coef_f3_q_sym11.*q0.*-2.0-coef_f3_q_sym18.*q1-coef_f3_q_sym22.*q2+coef_f0_q_sym11.*q3-coef_f3_q_sym24.*q3-coef_f3_q_sym1.*q0.*t114.*4.0+coef_f0_q_sym4.*q0.*t117.*2.0-coef_f3_q_sym5.*q0.*t115.*2.0-coef_f3_q_sym8.*q0.*t116.*2.0-coef_f3_q_sym10.*q0.*t117.*2.0-coef_f3_q_sym2.*q1.*t114.*3.0+coef_f0_q_sym7.*q1.*t117-coef_f3_q_sym12.*q1.*t115-coef_f3_q_sym15.*q1.*t116-coef_f3_q_sym17.*q1.*t117-coef_f3_q_sym3.*q2.*t114.*3.0+coef_f0_q_sym9.*q2.*t117-coef_f3_q_sym13.*q2.*t115-coef_f3_q_sym21.*q2.*t117-coef_f3_q_sym19.*q2.*t116+coef_f0_q_sym1.*q3.*t114.*3.0+coef_f0_q_sym5.*q3.*t115-coef_f3_q_sym4.*q3.*t114.*3.0+coef_f0_q_sym8.*q3.*t116+coef_f0_q_sym10.*q3.*t117-coef_f3_q_sym20.*q3.*t116-coef_f3_q_sym14.*q3.*t115-coef_f3_q_sym23.*q3.*t117-coef_f3_q_sym6.*q0.*q1.*q2.*2.0+coef_f0_q_sym2.*q0.*q1.*q3.*2.0-coef_f3_q_sym7.*q0.*q1.*q3.*2.0+coef_f0_q_sym3.*q0.*q2.*q3.*2.0-coef_f3_q_sym9.*q0.*q2.*q3.*2.0+coef_f0_q_sym6.*q1.*q2.*q3-coef_f3_q_sym16.*q1.*q2.*q3,q0.*2.0,t118+t119+t120+t121+t122+t123+t124+t125+t127+t129+t132+t133+t134-coef_f1_q_sym18.*q0+coef_f0_q_sym18.*q1.*2.0-coef_f1_q_sym2.*q0.*t114+coef_f0_q_sym5.*q0.*t115.*3.0-coef_f1_q_sym12.*q0.*t115.*3.0-coef_f1_q_sym15.*q0.*t116-coef_f1_q_sym17.*q0.*t117+coef_f0_q_sym2.*q1.*t114.*2.0-coef_f1_q_sym5.*q1.*t114.*2.0+coef_f0_q_sym12.*q1.*t115.*4.0+coef_f0_q_sym15.*q1.*t116.*2.0+coef_f0_q_sym17.*q1.*t117.*2.0-coef_f1_q_sym6.*q2.*t114+coef_f0_q_sym13.*q2.*t115.*3.0+coef_f0_q_sym21.*q2.*t117-coef_f1_q_sym7.*q3.*t114+coef_f0_q_sym20.*q3.*t116+coef_f0_q_sym14.*q3.*t115.*3.0-coef_f1_q_sym13.*q0.*q1.*q2.*2.0-coef_f1_q_sym14.*q0.*q1.*q3.*2.0+coef_f0_q_sym9.*q0.*q2.*q3-coef_f1_q_sym16.*q0.*q2.*q3,-coef_f2_q_sym18.*q0+coef_f0_q_sym18.*q2-coef_f2_q_sym2.*q0.*t114+coef_f0_q_sym6.*q0.*t116-coef_f2_q_sym12.*q0.*t115.*3.0-coef_f2_q_sym15.*q0.*t116-coef_f2_q_sym17.*q0.*t117-coef_f2_q_sym5.*q1.*t114.*2.0+coef_f0_q_sym13.*q1.*t116.*2.0+coef_f0_q_sym2.*q2.*t114-coef_f2_q_sym6.*q2.*t114+coef_f0_q_sym12.*q2.*t115.*3.0+coef_f0_q_sym15.*q2.*t116+coef_f0_q_sym17.*q2.*t117-coef_f2_q_sym7.*q3.*t114+coef_f0_q_sym16.*q3.*t116+coef_f0_q_sym5.*q0.*q1.*q2.*2.0-coef_f2_q_sym13.*q0.*q1.*q2.*2.0-coef_f2_q_sym14.*q0.*q1.*q3.*2.0+coef_f0_q_sym7.*q0.*q2.*q3-coef_f2_q_sym16.*q0.*q2.*q3+coef_f0_q_sym14.*q1.*q2.*q3.*2.0,-coef_f3_q_sym18.*q0+coef_f0_q_sym18.*q3-coef_f3_q_sym2.*q0.*t114+coef_f0_q_sym7.*q0.*t117-coef_f3_q_sym12.*q0.*t115.*3.0-coef_f3_q_sym15.*q0.*t116-coef_f3_q_sym17.*q0.*t117-coef_f3_q_sym5.*q1.*t114.*2.0+coef_f0_q_sym14.*q1.*t117.*2.0-coef_f3_q_sym6.*q2.*t114+coef_f0_q_sym16.*q2.*t117+coef_f0_q_sym2.*q3.*t114-coef_f3_q_sym7.*q3.*t114+coef_f0_q_sym12.*q3.*t115.*3.0+coef_f0_q_sym15.*q3.*t116+coef_f0_q_sym17.*q3.*t117-coef_f3_q_sym13.*q0.*q1.*q2.*2.0+coef_f0_q_sym5.*q0.*q1.*q3.*2.0-coef_f3_q_sym14.*q0.*q1.*q3.*2.0+coef_f0_q_sym6.*q0.*q2.*q3-coef_f3_q_sym16.*q0.*q2.*q3+coef_f0_q_sym13.*q1.*q2.*q3.*2.0,q1.*2.0,-coef_f1_q_sym22.*q0+coef_f0_q_sym22.*q1-coef_f1_q_sym3.*q0.*t114+coef_f0_q_sym6.*q0.*t115-coef_f1_q_sym13.*q0.*t115-coef_f1_q_sym21.*q0.*t117-coef_f1_q_sym19.*q0.*t116.*3.0+coef_f0_q_sym3.*q1.*t114-coef_f1_q_sym6.*q1.*t114+coef_f0_q_sym13.*q1.*t115+coef_f0_q_sym21.*q1.*t117+coef_f0_q_sym19.*q1.*t116.*3.0-coef_f1_q_sym8.*q2.*t114.*2.0+coef_f0_q_sym15.*q2.*t115.*2.0-coef_f1_q_sym9.*q3.*t114+coef_f0_q_sym16.*q3.*t115+coef_f0_q_sym8.*q0.*q1.*q2.*2.0-coef_f1_q_sym15.*q0.*q1.*q2.*2.0+coef_f0_q_sym9.*q0.*q1.*q3-coef_f1_q_sym16.*q0.*q1.*q3-coef_f1_q_sym20.*q0.*q2.*q3.*2.0+coef_f0_q_sym20.*q1.*q2.*q3.*2.0,t118+t119+t120+t121+t122+t123+t124+t125+t126+t128+t130+t131+t135-coef_f2_q_sym22.*q0+coef_f0_q_sym22.*q2.*2.0-coef_f2_q_sym3.*q0.*t114+coef_f0_q_sym8.*q0.*t116.*3.0-coef_f2_q_sym13.*q0.*t115-coef_f2_q_sym21.*q0.*t117-coef_f2_q_sym19.*q0.*t116.*3.0-coef_f2_q_sym6.*q1.*t114+coef_f0_q_sym15.*q1.*t116.*3.0+coef_f0_q_sym17.*q1.*t117+coef_f0_q_sym3.*q2.*t114.*2.0-coef_f2_q_sym8.*q2.*t114.*2.0+coef_f0_q_sym13.*q2.*t115.*2.0+coef_f0_q_sym21.*q2.*t117.*2.0+coef_f0_q_sym19.*q2.*t116.*4.0-coef_f2_q_sym9.*q3.*t114+coef_f0_q_sym20.*q3.*t116.*3.0+coef_f0_q_sym14.*q3.*t115-coef_f2_q_sym15.*q0.*q1.*q2.*2.0+coef_f0_q_sym7.*q0.*q1.*q3-coef_f2_q_sym16.*q0.*q1.*q3-coef_f2_q_sym20.*q0.*q2.*q3.*2.0,-coef_f3_q_sym22.*q0+coef_f0_q_sym22.*q3-coef_f3_q_sym3.*q0.*t114+coef_f0_q_sym9.*q0.*t117-coef_f3_q_sym13.*q0.*t115-coef_f3_q_sym21.*q0.*t117-coef_f3_q_sym19.*q0.*t116.*3.0-coef_f3_q_sym6.*q1.*t114+coef_f0_q_sym16.*q1.*t117-coef_f3_q_sym8.*q2.*t114.*2.0+coef_f0_q_sym20.*q2.*t117.*2.0+coef_f0_q_sym3.*q3.*t114-coef_f3_q_sym9.*q3.*t114+coef_f0_q_sym13.*q3.*t115+coef_f0_q_sym21.*q3.*t117+coef_f0_q_sym19.*q3.*t116.*3.0-coef_f3_q_sym15.*q0.*q1.*q2.*2.0+coef_f0_q_sym6.*q0.*q1.*q3-coef_f3_q_sym16.*q0.*q1.*q3+coef_f0_q_sym8.*q0.*q2.*q3.*2.0-coef_f3_q_sym20.*q0.*q2.*q3.*2.0+coef_f0_q_sym15.*q1.*q2.*q3.*2.0,q2.*2.0,-coef_f1_q_sym24.*q0+coef_f0_q_sym24.*q1-coef_f1_q_sym4.*q0.*t114+coef_f0_q_sym7.*q0.*t115-coef_f1_q_sym20.*q0.*t116-coef_f1_q_sym14.*q0.*t115-coef_f1_q_sym23.*q0.*t117.*3.0+coef_f0_q_sym4.*q1.*t114-coef_f1_q_sym7.*q1.*t114+coef_f0_q_sym20.*q1.*t116+coef_f0_q_sym14.*q1.*t115+coef_f0_q_sym23.*q1.*t117.*3.0-coef_f1_q_sym9.*q2.*t114+coef_f0_q_sym16.*q2.*t115-coef_f1_q_sym10.*q3.*t114.*2.0+coef_f0_q_sym17.*q3.*t115.*2.0+coef_f0_q_sym9.*q0.*q1.*q2-coef_f1_q_sym16.*q0.*q1.*q2+coef_f0_q_sym10.*q0.*q1.*q3.*2.0-coef_f1_q_sym17.*q0.*q1.*q3.*2.0-coef_f1_q_sym21.*q0.*q2.*q3.*2.0+coef_f0_q_sym21.*q1.*q2.*q3.*2.0,-coef_f2_q_sym24.*q0+coef_f0_q_sym24.*q2-coef_f2_q_sym4.*q0.*t114+coef_f0_q_sym9.*q0.*t116-coef_f2_q_sym20.*q0.*t116-coef_f2_q_sym14.*q0.*t115-coef_f2_q_sym23.*q0.*t117.*3.0-coef_f2_q_sym7.*q1.*t114+coef_f0_q_sym16.*q1.*t116+coef_f0_q_sym4.*q2.*t114-coef_f2_q_sym9.*q2.*t114+coef_f0_q_sym20.*q2.*t116+coef_f0_q_sym14.*q2.*t115+coef_f0_q_sym23.*q2.*t117.*3.0-coef_f2_q_sym10.*q3.*t114.*2.0+coef_f0_q_sym21.*q3.*t116.*2.0+coef_f0_q_sym7.*q0.*q1.*q2-coef_f2_q_sym16.*q0.*q1.*q2-coef_f2_q_sym17.*q0.*q1.*q3.*2.0+coef_f0_q_sym10.*q0.*q2.*q3.*2.0-coef_f2_q_sym21.*q0.*q2.*q3.*2.0+coef_f0_q_sym17.*q1.*q2.*q3.*2.0,t118+t120+t125+t126+t127+t128+t129+t130+t131+t132+t133+t134+t135-coef_f3_q_sym24.*q0+coef_f0_q_sym24.*q3.*2.0-coef_f3_q_sym4.*q0.*t114+coef_f0_q_sym10.*q0.*t117.*3.0-coef_f3_q_sym20.*q0.*t116-coef_f3_q_sym14.*q0.*t115-coef_f3_q_sym23.*q0.*t117.*3.0-coef_f3_q_sym7.*q1.*t114+coef_f0_q_sym15.*q1.*t116+coef_f0_q_sym17.*q1.*t117.*3.0-coef_f3_q_sym9.*q2.*t114+coef_f0_q_sym13.*q2.*t115+coef_f0_q_sym21.*q2.*t117.*3.0+coef_f0_q_sym4.*q3.*t114.*2.0+coef_f0_q_sym20.*q3.*t116.*2.0-coef_f3_q_sym10.*q3.*t114.*2.0+coef_f0_q_sym14.*q3.*t115.*2.0+coef_f0_q_sym23.*q3.*t117.*4.0+coef_f0_q_sym6.*q0.*q1.*q2-coef_f3_q_sym16.*q0.*q1.*q2-coef_f3_q_sym17.*q0.*q1.*q3.*2.0-coef_f3_q_sym21.*q0.*q2.*q3.*2.0,q3.*2.0],[4, 4]);


% EEG_POINT2LAT - convert latency in data points to latency in ms relative
%                   to the time locking. Used in EEGLAB.
% Usage:
%       >> [newlat ] = eeg_point2lat( lat_array, [], srate);
%       >> [newlat ] = eeg_point2lat( lat_array, epoch_array,...
%                                 srate, timelimits, timeunit);
% Inputs:
%   lat_array   - latency array in data points assuming concatenated
%                 data epochs (see EEGLAB event structure)
%   epoch_array - epoch number corresponding to each latency value
%   srate       - data sampling rate in Hz
%   timelimits  - [min max] timelimits in 'timeunit' units (see below)
%   timeunit    - time unit in second. Default is 1 = seconds.
%
% Outputs:
%   newlat      - converted latency values (in 'timeunit' units) for each epoch
%
% Example:
%   tmpevent = EEG.event;
%   eeg_point2lat( [ tmpevent.latency ], [], EEG.srate, [EEG.xmin EEG.xmax]);
%   % returns the latency of all events in second for a continuous
%   % dataset EEG
%
%   eeg_point2lat( [ tmpevent.latency ], [ tmpevent.epoch ],
%                 EEG.srate, [EEG.xmin EEG.xmax]*1000, 1E-3);
%   % returns the latency of all events in millisecond for a dataset
%   % containing data epochs.
%
%
% Author: Arnaud Delorme, CNL / Salk Institute, 2 Mai 2002
%
% See also: EEG_LAT2POINT, EEGLAB, POP_EDITIEVENTVALS, POP_LOADDAT

% Copyright (C) 2 Mai 2002 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function newlat = eeg_point2lat( lat_array, epoch_array, srate, timewin, timeunit);

if nargin <3
    help eeg_point2lat;
    return;
end
if isempty( epoch_array )
    epoch_array = ones( size(lat_array) );
end
if nargin <4
    timewin = 0;
end
if nargin <5
	timeunit = 1;
end

if length(lat_array) ~= length(epoch_array)
	if length(epoch_array)~= 1
		disp('eeg_point2lat: latency and epoch arrays must have the same length'); return;
	else
		epoch_array = ones(1,length(lat_array))*epoch_array;
	end
end
if length(timewin) ~= 2
    disp('eeg_point2lat: timelimits array must have length 2'); return;
end
if iscell(epoch_array)
	epoch_array = [ epoch_array{:} ];
end
if iscell(lat_array)
	lat_array = [ lat_array{:} ];
end

timewin = timewin*timeunit;

if length(timewin) == 2
    pnts = (timewin(2)-timewin(1))*srate+1;
else
    pnts = 0;
end
newlat  = ((lat_array - (epoch_array-1)*pnts-1)/srate+timewin(1))/timeunit;
newlat = round(newlat*1E9)/1E9;


function [A,b,C] = update_spatial_components(Y,C,f,A_,P,options)

% update spatial footprints and background through Basis Pursuit Denoising
% for each pixel i solve the problem
%   [A(i,:),b(i)] = argmin sum(A(i,:))
%       subject to || Y(i,:) - A(i,:)*C + b(i)*f || <= sn(i)*sqrt(T);
% for each pixel the search is limited to a few spatial components

% INPUTS:
% Y:    raw data
% C:    temporal components
% f:    temporal background
% A_:   current estimate of spatial footprints (used for determining search locations only)
% P:    dataset parameters (used for noise values and interpolated entries)

% options    parameter struct (for noise values and other parameters)

% OUTPUTS:
% A:    new estimate of spatial footprints
% b:    new estimate of spatial background
% C:    temporal components (updated only when spatial components are completely removed)

% Written by:
% Eftychios A. Pnevmatikakis, Simons Foundation, 2015

warning('off', 'MATLAB:maxNumCompThreads:Deprecated');
memmaped = isobject(Y);
if memmaped
    sizY = size(Y,'Y');
    d = prod(sizY(1:end-1));
    T = sizY(end);
else
    [d,T] = size(Y);
end
if nargin < 6 || isempty(options); options = []; end
if ~isfield(options,'d1') || isempty(options.d1); d1 = input('What is the total number of rows? \n'); options.d1 = d1; else d1 = options.d1; end          % # of rows
if ~isfield(options,'d2') || isempty(options.d2); d2 = input('What is the total number of columns? \n'); options.d2 = d2; else d2 = options.d2; end          % # of columns
if ~isfield(options,'show_sum'); show_sum = 0; else show_sum = options.show_sum; end            % do some plotting while calculating footprints
if ~isfield(options,'interp'); Y_interp = sparse(d,T); else Y_interp = options.interp; end      % identify missing data
if ~isfield(options,'use_parallel'); use_parallel = ~isempty(which('parpool')); else use_parallel = options.use_parallel; end % use parallel toolbox if present
if ~isfield(options,'search_method'); method = []; else method = options.search_method; end     % search method for determining footprint of spatial components

if nargin < 2 || (isempty(A_) && isempty(C))  % at least either spatial or temporal components should be provided
    error('Not enough input arguments')
else
    if ~isempty(C); K = size(C,1); elseif islogical(A_); K = size(A_,2); else K = size(A_2,2) - options.nb; end
end

if nargin < 5 || isempty(P); P = preprocess_data(Y,1); end  % etsimate noise values if not present
if nargin < 4 || isempty(A_);
    IND = ones(d,size(C,1));
else
    if islogical(A_)     % check if search locations have been provided, otherwise estimate them
        IND = A_;
        if isempty(C)
            INDav = double(IND)/diag(sum(double(IND)));
            px = (sum(IND,2)>0);
            f = mean(Y(~px,:));
            b = max(Y*f',0)/norm(f)^2;
            C = max(INDav'*Y - (INDav'*b)*f,0);
        end
    else
        IND = determine_search_location(A_(:,1:K),method,options);
    end
end

options.sn = P.sn;
if ~memmaped
    Y(P.mis_entries) = NaN; % remove interpolated values
end

Cf = [C;f];

if use_parallel         % solve BPDN problem for each pixel
    Nthr = max(2*maxNumCompThreads,round(d*T/2^24));
    siz_row = [floor(d/Nthr)*ones(Nthr-1,1);d-floor(d/Nthr)*(Nthr-1)];
    indeces = [0;cumsum(siz_row)];
    if ~memmaped
        Ycell = mat2cell(Y,siz_row,T);
    else
        Ycell = cell(Nthr,1);
    end
    INDc =  mat2cell(IND,siz_row,K);
    Acell = cell(Nthr,1);
    Psnc = mat2cell(options.sn(:),siz_row,1);
    Yf = cell(Nthr,1);
    parfor nthr = 1:Nthr
        Acell{nthr} = zeros(siz_row(nthr),size(Cf,1));
        if memmaped
            Ytemp = double(Y.Yr(indeces(nthr)+1:indeces(nthr+1),:));
        else
            Ytemp = Ycell{nthr};
        end
        Yf{nthr} = Ytemp*f';
        for px = 1:siz_row(nthr)
            fn = ~isnan(Ytemp(px,:));       % identify missing data
            ind = find(INDc{nthr}(px,:));
            if ~isempty(ind);
                ind2 = [ind,K+(1:size(f,1))];
                %[~, ~, a, ~] = lars_regression_noise(Ycell{nthr}(px,fn)', Cf(ind2,fn)', 1, Psnc{nthr}(px)^2*T);
                [~, ~, a, ~] = lars_regression_noise(Ytemp(px,fn)', Cf(ind2,fn)', 1, Psnc{nthr}(px)^2*T);
                Acell{nthr}(px,ind2) = a';
            end
        end
    end
    A = cell2mat(Acell);
    Yf = cell2mat(Yf);
else
    A = [zeros(d,K),zeros(d,size(f,1))];
    sA = zeros(d1,d2);
    Yf = Y*f';
    for px = 1:d   % estimate spatial components
        fn = ~isnan(Y(px,:));       % identify missing data
        ind = find(IND(px,:));
        if ~isempty(ind);
            ind2 = [ind,K+(1:size(f,1))];
            [~, ~, a, ~] = lars_regression_noise(Y(px,fn)', Cf(ind2,fn)', 1, options.sn(px)^2*T);
            A(px,ind2) = a';
            sA(px) = sum(a);
        end
        if show_sum
            if mod(px,d1) == 0;
               figure(20); imagesc(sA); axis square;
               title(sprintf('Sum of spatial components (%i out of %i columns done)',round(px/d1),d2)); drawnow;
            end
        end
    end
end

A(isnan(A))=0;
A = sparse(A);
A = threshold_components(A,options);  % post-processing of components

fprintf('Updated spatial components \n');

ff = find(sum(A(:,1:K))==0);           % remove empty components
if ~isempty(ff)
    K = K - length(ff);
    A(:,ff) = [];
    C(ff,:) = [];
end

b = max((Yf - A(:,1:K)*(C(1:K,:)*f'))/(f*f'),0);
A = A(:,1:K);


function net = res_imagenet_init(n, varargin)

opts.batchNormalization = true;
opts.networkType = 'resnet'; % 'plain' | 'resnet'
opts.bottleneck = false; % only used when n is an array
opts.nClasses = 1000;
opts.reLUafterSum = true;
opts = vl_argparse(opts, varargin);
nClasses = opts.nClasses;

net = dagnn.DagNN();

% n -> specific configuration
if numel(n)==4,
  Ns = n;
else
  switch n,
    case 18, Ns = [2 2 2 2]; opts.bottleneck = false;
    case 34, Ns = [3 4 6 3]; opts.bottleneck = false;
    case 50, Ns = [3 4 6 3]; opts.bottleneck = true;
    case 101, Ns = [3 4 23 3]; opts.bottleneck = true;
    case 152, Ns = [3 8 36 3]; opts.bottleneck = true;
    otherwise, error('No configuration found for n=%d', n);
  end
end
if strcmpi(opts.networkType, 'plain') && opts.bottleneck,
  error('plain network cannot be built with bottleneck layers');
end

% Meta parameters
net.meta.inputSize = [224 224 3] ;
net.meta.trainOpts.weightDecay = 0.0001 ;
net.meta.trainOpts.momentum = 0.9;
net.meta.trainOpts.batchSize = 256 ;
if opts.batchNormalization;
  net.meta.trainOpts.learningRate = [0.1*ones(1,30) 0.01*ones(1,30) 0.001*ones(1,50)] ;
else
  net.meta.trainOpts.learningRate = [0.01*ones(1,45) 0.001*ones(1,45) 0.0001*ones(1,75)] ;
end
net.meta.trainOpts.numEpochs = numel(net.meta.trainOpts.learningRate) ;

% First conv layer
switch n
    case {18, 34, 50}
        block = dagnn.Conv('size',  [7 7 3 64], 'hasBias', true, ...
            'stride', 2, 'pad', [3 3 3 3]);
        lName = 'conv0';
net.addLayer(lName, block, 'data', lName, {[lName '_f'], [lName '_b']});
    case {101, 152}
        block = dagnn.Conv('size',  [7 7 3 64], 'hasBias', false, ...
            'stride', 2, 'pad', [3 3 3 3]);
        lName = 'conv0';
    net.addLayer(lName, block, 'data', lName, {[lName '_f']});
end

add_layer_bn(net, 64, lName, 'bn0', 0.1);
block = dagnn.ReLU('leak',0);
net.addLayer('relu0',  block, 'bn0', 'relu0');

%add_block_conv(net, '0', 'image', [7 7 3 64], 2, opts.batchNormalization, true);
block = dagnn.Pooling('poolSize', [3 3], 'method', 'max', 'pad', [0 1 0 1], 'stride', 2);
net.addLayer('pool0', block, 'relu0', 'pool0');

info.lastNumChannel = 64;
info.lastIdx = 0;
info.lastName = 'pool0';

% Four groups of layers
info = add_group(opts.networkType, net, Ns(1), info, 3, 64,  1, opts.bottleneck, opts.batchNormalization, opts.reLUafterSum);
info = add_group(opts.networkType, net, Ns(2), info, 3, 128, 2, opts.bottleneck, opts.batchNormalization, opts.reLUafterSum);
info = add_group(opts.networkType, net, Ns(3), info, 3, 256, 2, opts.bottleneck, opts.batchNormalization, opts.reLUafterSum);
info = add_group(opts.networkType, net, Ns(4), info, 3, 512, 2, opts.bottleneck, opts.batchNormalization, opts.reLUafterSum);

% Prediction & loss layers
block = dagnn.Pooling('poolSize', [7 7], 'method', 'avg', 'pad', 0, 'stride', 1);
if opts.reLUafterSum
    net.addLayer('pool_final', block, sprintf('relu%d',info.lastIdx), 'pool_final');
else
    net.addLayer('pool_final', block, sprintf('sum%d',info.lastIdx), 'pool_final');
end
block = dagnn.Conv('size', [1 1 info.lastNumChannel nClasses], 'hasBias', true, ...
                   'stride', 1, 'pad', 0);
lName = sprintf('fc%d', info.lastIdx+1);
net.addLayer(lName, block, 'pool_final', lName, {[lName '_f'], [lName '_b']});

net.addLayer('softmax', dagnn.SoftMax(), lName, 'softmax');
net.addLayer('loss', dagnn.Loss('loss', 'log'), {'softmax', 'label'}, 'loss');
net.addLayer('error', dagnn.Loss('loss', 'classerror'), {'softmax','label'}, 'error') ;
net.addLayer('error5', dagnn.Loss('loss', 'topkerror', 'opts', {'topK', 5}), ...
  {'softmax','label'}, 'error5') ;

net.initParams();

net.meta.normalization.imageSize = net.meta.inputSize;
net.meta.normalization.border = 256 - net.meta.inputSize(1:2) ;
net.meta.normalization.interpolation = 'bicubic' ;
net.meta.normalization.averageImage = [] ;
net.meta.normalization.keepAspect = true ;
net.meta.augmentation.rgbVariance = zeros(0,3) ;
net.meta.augmentation.transformation = 'stretch' ;

% Add a group of layers containing 2n/3n conv layers
function info = add_group(netType, net, n, info, w, ch, stride, bottleneck, bn, reLUafterSum)
if strcmpi(netType, 'plain'),
  if isfield(info, 'lastName'),
    lName = info.lastName;
    info = rmfield(info, 'lastName');
  else
    lName = sprintf('relu%d', info.lastIdx);
  end
  add_block_conv(net, sprintf('%d', info.lastIdx+1), lName, ...
    [w w info.lastNumChannel ch], stride, bn, true);
  info.lastIdx = info.lastIdx + 1;
  info.lastNumChannel = ch;
  for i=2:2*n,
    add_block_conv(net, sprintf('%d', info.lastIdx+1), sprintf('relu%d', info.lastIdx), ...
      [w w ch ch], 1, bn, true);
    info.lastIdx = info.lastIdx + 1;
  end
elseif strcmpi(netType, 'resnet'),
    info = add_block_res(net, info, [w w info.lastNumChannel ch], stride, bottleneck, bn, 1, reLUafterSum);
    for i=2:n,
        if bottleneck,
            info = add_block_res(net, info, [w w 4*ch ch], 1, bottleneck, bn, 0, reLUafterSum);
        else
            info = add_block_res(net, info, [w w ch ch], 1, bottleneck, bn, 0, reLUafterSum);
        end
    end
end

% Add a smallest residual unit (2/3 conv layers)
function info = add_block_res(net, info, f_size, stride, bottleneck, bn, isFirst, reLUafterSum)

if isfield(info, 'lastName'),
  lName0 = info.lastName;
  info = rmfield(info, 'lastName');
elseif reLUafterSum || info.lastIdx == 0
  lName0 = sprintf('relu%d',info.lastIdx);
else
    lName0 = sprintf('sum%d',info.lastIdx);
end

lName01 = lName0;
if stride > 1 || isFirst,
  if bottleneck,
      ch = 4*f_size(4);
  else
      ch = f_size(4);
  end
  block = dagnn.Conv('size',[1 1 f_size(3) ch], 'hasBias',false,'stride',stride, ...
    'pad', 0);
  lName_tmp = lName0;
  lName0 = [lName_tmp '_down2'];
  net.addLayer(lName0, block, lName_tmp, lName0, [lName0 '_f']);

  pidx = net.getParamIndex([lName0 '_f']);
  net.params(pidx).learningRate = 0;

  add_layer_bn(net, ch, lName0, [lName01 '_d2bn'], 0.1);
  lName0 = [lName01 '_d2bn'];
end

if bottleneck,
  add_block_conv(net, sprintf('%d',info.lastIdx+1), lName01, [1 1 f_size(3) f_size(4)], stride, bn, true);
  info.lastIdx = info.lastIdx + 1;
  info.lastNumChannel = f_size(4);
  add_block_conv(net, sprintf('%d',info.lastIdx+1), sprintf('relu%d',info.lastIdx), ...
    [f_size(1) f_size(2) info.lastNumChannel info.lastNumChannel], 1, bn, true);
  info.lastIdx = info.lastIdx + 1;
  add_block_conv(net, sprintf('%d',info.lastIdx+1), sprintf('relu%d',info.lastIdx), ...
    [1 1 info.lastNumChannel info.lastNumChannel*4], 1, bn, true);
  info.lastIdx = info.lastIdx + 1;
  info.lastNumChannel = info.lastNumChannel*4;
else
  add_block_conv(net, sprintf('%d',info.lastIdx+1), lName01, f_size, stride, bn, true);
  info.lastIdx = info.lastIdx + 1;
  info.lastNumChannel = f_size(4);
  add_block_conv(net, sprintf('%d',info.lastIdx+1), sprintf('relu%d',info.lastIdx), ...
    [f_size(1) f_size(2) info.lastNumChannel info.lastNumChannel], 1, bn, true);
  info.lastIdx = info.lastIdx + 1;
end
if bn,
  lName1 = sprintf('bn%d', info.lastIdx);
else
  lName1 = sprintf('conv%d', info.lastIdx);
end

info.lastIdx = info.lastIdx + 1;
net.addLayer(sprintf('sum%d',info.lastIdx), dagnn.Sum(), {lName0,lName1}, ...
sprintf('sum%d',info.lastIdx));

% relu
if reLUafterSum
    block = dagnn.ReLU('leak', 0);
    net.addLayer(sprintf('relu%d', info.lastIdx), block, sprintf('sum%d', info.lastIdx), ...
    sprintf('relu%d', info.lastIdx));
end

% Add a conv layer (followed by optional batch normalization & relu)
function net = add_block_conv(net, out_suffix, in_name, f_size, stride, bn, relu)
block = dagnn.Conv('size',f_size, 'hasBias',false, 'stride', stride, ...
                   'pad',[ceil(f_size(1)/2-0.5) floor(f_size(1)/2-0.5) ...
                   ]);
lName = ['conv' out_suffix];
net.addLayer(lName, block, in_name, lName, {[lName '_f']});

if bn,
  add_layer_bn(net, f_size(4), lName, strrep(lName,'conv','bn'), 0.1);
  lName = strrep(lName, 'conv', 'bn');
end
if relu,
  block = dagnn.ReLU('leak',0);
  net.addLayer(['relu' out_suffix], block, lName, ['relu' out_suffix]);
end

% Add a batch normalization layer
function net = add_layer_bn(net, n_ch, in_name, out_name, lr)
block = dagnn.BatchNorm('numChannels', n_ch);
net.addLayer(out_name, block, in_name, out_name, ...
  {[out_name '_g'], [out_name '_b'], [out_name '_m']});
pidx = net.getParamIndex({[out_name '_g'], [out_name '_b'], [out_name '_m']});
net.params(pidx(1)).weightDecay = 0;
net.params(pidx(2)).weightDecay = 0;
net.params(pidx(3)).learningRate = lr;
net.params(pidx(3)).trainMethod = 'average';


filename = 'CantileverSquare';%'CantileverSquareSYmmetricMesh';%'CantileverSquareNewFine';%'LshapeTriFine';%'CantileverSquareNew';'ArchTriFine';%'CantileverSquare';%'ArchTriFine';%'CantileverSquare';%'Lshape';%'LshapeTriFine';%'CantileverSquareSmall';%'';%'Lshape';'CantileverSquare';'Lshape';%'CantileverSquare';%'LshapeFine';%'Bridge_quad_coarse';%'Arch_quad_coarse';%'BridgeCool_Quadrilateral_Bilinear_Structured_Coarse';%'Bridge';%'CantileverSquareSmall';
ptype = 'MACRO';
initial_case = 'given';
m1 = 0.0101;
m2 = 0.0101;
%cost = {'compliance'};
cost = {'stressNorm'};
%cost = {'stressNorm','compliance'};
%weights = [0.55,0.45];
weights = 1;
constraint = {'volumeConstraint'};
filterType = 'PDE';
constraint_case = 'EQUALITY';

Vfrac_initial = 0.3;
optimality_initial = 1e-5;
constr_initial = 1e-5;

Vfrac_final = 0.3;
optimality_final = 1e-5;
constr_final = 1e-5;

stressNormExponent_initial = 2;
stressNormExponent_final = 64;

optimizer = 'DualNestedInPrimal';
optimizerUnconstrained = 'PROJECTED GRADIENT';

designVariable = 'MicroParams';
ub = 0.989;
lb = 0.011;
homegenizedVariablesComputer = 'ByVademecum';
%
vademecumFileName = 'SuperEllipseQMax';
%vademecumFileName = 'SuperEllipseQ2';
%vademecumFileName = 'SuperEllipseQOptAnalytic';
%
% designVariable = 'Density';
% homegenizedVariablesComputer = 'ByInterpolation';
% method = 'SIMPALL';
% materialType = 'ISOTROPIC';

kfrac = 2;
nsteps = 20;

plotting = false;
printing = true;
monitoring = true;
monitoring_interval = 1;
maxiter = 500;


function [output] = ft_transform_geometry(transform, input)

% FT_TRANSFORM_GEOMETRY applies a homogeneous coordinate transformation to a
% structure with geometric information, for example a volume conduction model for the
% head, gradiometer of electrode structure containing EEG or MEG sensor positions and
% MEG coil orientations, a head shape or a source model.
%
% Use as
%   [output] = ft_transform_geometry(transform, input)
% where the transform should be a 4x4 homogeneous transformation matrix and the input
% data structure can be any of the FieldTrip data structures that describes
% geometrical data.
%
% The units of the transformation matrix must be the same as the units in which the
% geometric object is expressed.
%
% The type of geometric object constrains the type of allowed transformations.
%
% For sensor arrays:
% If the input is an MEG gradiometer array, only a rigid-body translation plus
% rotation are allowed. If the input is an EEG electrode or fNIRS optodes array,
% global rescaling and individual axis rescaling is also allowed.
%
% For volume conduction models:
% If the input is a volume conductor model of the following type:
%   localspheres model
%   singleshell model with the spherical harmonic coefficients already computed
%   BEM model with system matrix already computed
%   FEM model with volumetric elements
% only a rigid-body translation plus rotation are allowed.
%
% If the input is a volume conductor model of the following type:
%   BEM model with the system matrix not yet computed
%   singleshell model with the spherical harmonic coefficients not yet computed
% rotation, translation, global rescaling and individual axis rescaling is allowed.
%
% If the input is a volume conductor model of the following type:
%   single sphere
%   concentric spheres
% rotation, translation and global rescaling is allowed.
%
% For source models, either defined as a 3D regular grid, a 2D mesh or unstructred
% point cloud, rotation, translation, global rescaling and individual axis rescaling
% is allowed.
%
% For anatomical MRIs and functional volumetric data, rotation, translation, global
% rescaling and individual axis rescaling are allowed.
%
% See also FT_WARP_APPLY, FT_HEADCOORDINATES, FT_SCALINGFACTOR

% Copyright (C) 2011-2022, Jan-Mathijs Schoffelen and 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: ft_transform_geometry.m$

% determine the rotation matrix
rotation = eye(4);
rotation(1:3,1:3) = transform(1:3,1:3);

if any(abs(transform(4,:)-[0 0 0 1])>100*eps)
  ft_error('invalid transformation matrix');
end

% check whether the transformation includes a scaling operation
s = svd(rotation);
if abs(abs(det(rotation))-1)>1e-4
  % the transformation increases or decreases the overall volume, allow for
  % some numerical imprecision
  if any(abs(s./s(1)-1)>1e-3)
    % axes scale differently
    globalrescale = false;
    axesrescale   = true;
  else
    % global rescaling
    globalrescale = true;
    axesrescale   = true;
  end
else
  globalrescale = false;
  axesrescale   = false;
end

% check whether the input data combines well with the requested
% transformation
dtype = ft_datatype(input);
switch dtype
  case 'grad'
    if globalrescale || axesrescale, ft_error('only a rigid body transformation without rescaling is allowed'); end
  case 'mesh'
    if isstruct(input) && length(input)>1
      % there are multiple meshes
      for i=1:length(input)
        output(i) = ft_transform_geometry(transform, input(i));
      end
      return
    end
  otherwise
    % could be a volume conductor model with constrained transformation possibilities
    if ft_headmodeltype(input, 'singleshell') && isfield(input, 'forwpar') && (globalrescale || axesrescale)
      ft_error('only a rigid body transformation without rescaling is allowed');
    end
    if ft_headmodeltype(input, 'bem') && isfield(input, 'mat') && (globalrescale || axesrescale)
      ft_error('only a rigid body transformation without rescaling is allowed');
    end
    if ft_headmodeltype(input, 'localspheres') && isfield(input, 'mat') && (globalrescale || axesrescale)
      ft_error('only a rigid body transformation without rescaling is allowed');
    end
    if (isfield(input, 'tetra') || isfield(input, 'hex')) && (globalrescale || axesrescale)
      ft_error('only a rigid body transformation without rescaling is allowed');
    end
end

% tfields must be rotated, translated and scaled
% rfields must only be rotated
% mfields must be simply multipied
% recfields must be recursed into
tfields   = {'pos' 'pnt' 'o' 'coilpos' 'elecpos' 'optopos' 'chanpos' 'chanposold' 'nas' 'lpa' 'rpa' 'zpoint'}; % apply rotation plus translation
rfields   = {'ori' 'nrm'     'coilori' 'elecori' 'optoori' 'chanori' 'chanoriold' 'mom'                     }; % only apply rotation
mfields   = {'transform'};                % plain matrix multiplication
recfields = {'fid' 'bnd' 'orig' 'dip'};   % recurse into these fields
% the field 'r' is not included here, because it applies to a volume
% conductor model, and scaling is not allowed, so r will not change.

fnames = fieldnames(input);
for k = 1:numel(fnames)
  % name = sprintf('%s.%s', inputname(2), fnames{k});
  if ~isempty(input.(fnames{k}))
    if any(strcmp(fnames{k}, tfields))
      % ft_info('applying transformation to %s', name);
      input.(fnames{k}) = apply(transform, input.(fnames{k}));
    elseif any(strcmp(fnames{k}, rfields))
      % ft_info('applying rotation to %s', name);
      input.(fnames{k}) = apply(rotation, input.(fnames{k}));
    elseif any(strcmp(fnames{k}, mfields))
      % ft_info('applying multiplication to %s', name);
      input.(fnames{k}) = transform*input.(fnames{k});
    elseif any(strcmp(fnames{k}, recfields))
      for j = 1:numel(input.(fnames{k}))
        % ft_info('recursing into %s', name);
        input.(fnames{k})(j) = ft_transform_geometry(transform, input.(fnames{k})(j));
      end
    else
      % do nothing
    end
  end
end
output = input;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION that applies the homogeneous transformation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [new] = apply(transform, old)

[m, n] = size(old);
if m~=3 && n==3
  % each row is one position
  old(:,4) = 1;
  new = old * transform';
  new = new(:,1:3);
elseif m==3 && n~=3
  % each column is one position
  old(4,:) = 1;
  new = transform * old;
  new = new(1:3,:);
else
  % assume that each row is one position
  old(:,4) = 1;
  new = old * transform';
  new = new(:,1:3);
end


% summarize conserved clusters as graph

% P1 = vertcat(Pairs_AllClusAllFish{:,1});
% P2 = vertcat(Pairs_AllClusAllFish{:,2});
% Nodes = unique([P1;P2],'rows');
% numNodes = length(Nodes);
%
% [~,IX1] = ismember(P1,Nodes,'rows');
% [~,IX2] = ismember(P2,Nodes,'rows');
%
% G = graph(IX1,IX2); figure;plot(G)
% bins = conncomp(G);
% %%
% thres_fishcount = round(length(range_fish)/2);
% range_bins = unique(bins);
% fishCountForG = zeros(length(range_bins),1);
% for i = range_bins,
%     IX = find(bins==range_bins(i));
%     fishCountForG(i) = length(unique(Nodes(IX,1)));
% end
%
%
% %% Plot anat
% anat_yx_norm = getappdata(hfig,'anat_yx_norm');
%
% figure;
% hold on;
% image(anat_yx_norm)
% axis equal
% axis ij
% axis off
%
% clrmap = hsv(round(max(bins)*1.1));
%
% for i_node = 1:length(Nodes),
%     i_fish = Nodes(i_node,1);
%     clusIX = Nodes(i_node,2);
%
%     GIX = VAR(i_fish).ClusGroup{3}.gIX;
%     IX = find(GIX == clusIX);
%     CIX_abs = VAR(i_fish).ClusGroup{3}.cIX_abs;
%     cIX_abs = CIX_abs(IX);
%     LoadFishDataWithoutTS(hfig,i_fish);
%     CellXYZ_norm = getappdata(hfig,'CellXYZ_norm');
%     xyz_norm = CellXYZ_norm(cIX_abs,:);
%     %    cmap = repmat(clrmap(bins(i_node),:)',1,length(cIX_abs));
%     scatter(xyz_norm(:,2),xyz_norm(:,1),2,clrmap(bins(i_node),:));%,20,clrmap(IX,:))
% end
%
% %%
% anat_yx_norm = getappdata(hfig,'anat_yx_norm');
%
% figure;
% hold on;
% image(anat_yx_norm)
% axis equal
% axis ij
% axis off
%
% clrmap = hsv(round(max(bins)*1.1));
% range_fish_plot = unique(Nodes(:,1))';
% for i_fish = range_fish_plot,
%     [cIX,gIX,M_xyz_norm] = GetDefaultClustersFromLoad(hfig,i_fish);
%     IX_node = find(Nodes(:,1)==i_fish);
%     U = Nodes(IX_node,2);
%     clr = bins(IX_node);
%     for i = 1:length(U),
%         IX = find(gIX==U(i));
%         scatter(M_xyz_norm(IX,2),M_xyz_norm(IX,1),2,clrmap(clr(i),:));%,20,clrmap(IX,:))
%     end
% end

%% JUST SCREEN NORMALLY

% k_consrv_ratio = 1/3;% or just set int number of fish......
k_consrv = 6; % at least in 5/6 more fish within this set

range_fish = 1:18;
for i_fish = range_fish,

    m = TF_fishrange{i_fish};
    m1 = sum(logical(m),2);
%     m1 = sum(m,2);
    U = find(m1>=k_consrv);
    [cIX_in,gIX_in,~,~,~,absIX] = GetDefaultClustersFromLoad(hfig,i_fish);

    cIX = [];gIX = [];
    for i = 1:length(U),
        IX = find(gIX_in==U(i));
        cIX = [cIX;cIX_in(IX)];
        gIX = [gIX;gIX_in(IX)];
    end

    % save cluster
    name = 'D12screen_6/18fish';
    clusgroupID = 10;
    clusIDoverride = 2; % 1
    SaveCluster_Direct(cIX,gIX,absIX,i_fish,name,clusgroupID,clusIDoverride);
end
%%
% SaveVARwithBackup();
% %%
%
% % fish6_clus47:right abducence
% i_fish = 6;
% ix = 47;
%
% m = TF_fishrange{i_fish};
% fIX = find(m(ix,:));
%


function rpatch = patchResize(opatch,r)

% PATCHRESIZE  Patch resize.
%   PATCHRESIZE(PATCH,R)  resizes the image of PATCH.I by a
%   factor R and crops the result to size(PATCH.I). Works only
%   for square patch images. See PIX2PATCH for information about
%   the structure PATCH.
%
%   See also PIX2PATCH, PATCHSCAN

% (c) 2006 Joan Sola @ LAAS-CNRS

I    = single(opatch.I);

% J = single(imresize2(I,r));
J = imresize2(I,r);

rpatch.I    = J;
rpatch.SI   = sum(sum(J));
rpatch.SII  = sum(sum(J.*J));
rpatch.bias = opatch.bias;

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function dfupdateppdists(dffig)
%DFUPDATEPPDISTS Update distribution list for probability plots

%   $Revision: 1.1.6.5 $  $Date: 2004/01/24 09:36:02 $
%   Copyright 2003-2004 The MathWorks, Inc.

if nargin<1 || isempty(dffig)
   dffig = dfgetset('dffig');
end

% Get handle to control containing the distribution list
hsel = getappdata(dffig,'selectioncontrols');

% Determine plot type, don't continue unless it's a probability plot
h = hsel(3);   % handle of display type control
choice = get(hsel(3),'Value');
ftypes = getappdata(hsel(3),'codenames');
ftype = ftypes{choice};
if ~isequal(ftype, 'probplot');
   return
end

% Look at all plotted data sets and check for negative data
dsdb = getdsdb;
dslist = find(dsdb);
dslist(dslist==dsdb) = [];
nds = length(dslist);
xmin = 1;
for j=1:nds
   ds = dslist(j);
   if ds.plot
      xlimits = xlim(ds);
      if (xlimits(1) < xmin)
         xmin = xlimits(1);
         if (xmin<0)
            break
         end
      end
   end
end

% Get the current selection, and try to re-select it later
h = hsel(5);   % handle of distribution control
dlist = get(h, 'String');
choice = get(h, 'Value');
if choice<=length(dlist)
   cursel = dlist{choice};
else
   cursel = [];
end

% Get the distribution entries only
dlist = getappdata(h, 'allfullnames');
codenames = getappdata(h, 'allcodenames');
ndists = length(dlist);

% With 0 or negative data showing, omit some distributions
if (xmin<=0)
   ok = true(ndists,1);
   allspec = getappdata(h,'alldistspec');

   % ------- Could vectorize the following
   for j=1:ndists
      spec = allspec(j);
      ok(j) = checkdist(spec,xmin);
   end
   % -------

   dlist = dlist(ok);
   codenames = codenames(ok);
   ndists = length(dlist);
end
setappdata(h,'okcodenames',codenames);

% Create a list of fit names
fitdb = getfitdb;
flist = find(fitdb);
flist(flist==fitdb) = [];
nfits = length(flist);

% Weed out fits that cannot be used for probability plotting
for j=nfits:-1:1
   fj = flist(j);
   if ~fj.isgood || isequal(fj.fittype,'smooth')
      flist(j) = [];
   else
      distspec = fj.distspec;
      if isempty(distspec) || ~distspec.iscontinuous ...
                           || ~checkdist(distspec,xmin)
         flist(j) = [];
      end
   end
end
nfits = length(flist);

% Create combined list
newdlist = cell(ndists + nfits, 1);
newdlist(1:ndists) = dlist(:);
savelist = cell(nfits,1);
for j=1:nfits
   fj = flist(j);
   newdlist{ndists+j} = sprintf('Estimated %s (%s)',fj.distspec.name,fj.name);
   savelist{j} = fj.name;
end
setappdata(h, 'fitnames', savelist);

% Re-select the previous selection
set(h, 'String', newdlist);
choice = 1;
if ~isempty(cursel)
   choices = strmatch(cursel,newdlist,'exact');
   if numel(choices) == 1
      choice = choices;
   end
end
set(h, 'Value', choice);

% -----------------------------
function ok = checkdist(spec,xmin)
%Check distribution against minimum data value

lobnd = spec.support(1);
strict = ~spec.closedbound(1);
if strict && lobnd>=xmin
   ok = false;
elseif ~strict && lobnd>xmin
   ok = false;
else
   ok = true;
end


function ssimLabelFeatures(ftrDir,vocabPath,labelDir,imgList,parms)

  load(vocabPath); % Gives the variable V

  for i=1:length(imgList),
    IMGname=imgList{i};
    onlyImgName=extractImgName(IMGname);
    fprintf('Labeling ssim descriptors of %s\n',onlyImgName);
    load([ftrDir onlyImgName '.mat']);
    textonlabels=uint16(MEXfindnearestl2(features,V));
    %save([RESdirs(j,:),IMGname],'textonlabels');

    origImg=ssimReadImg(IMGname,parms);
    [labelImage,cmap]=drawLabels(textonlabels,size(V,2),ftrCoords,salientCoords,uniformCoords,origImg);

    %height=size(labelImage,1);width=size(labelImage,2);
    %labelImage=labelImage( (1+parms.margin):(height-parms.margin),(1+parms.margin):(width-parms.margin));
    save([labelDir onlyImgName '.mat'],'labelImage');

  end;
end

function [labelImage,cmap]=drawLabels(labels,numClusters,dCoords,sCoords,uCoords,origImg)
  numLabels=numClusters;

  if size(labels,2)~=size(dCoords,2)
    fprintf('Something wrong with dimensions of arrays of labels and drawing coordinates\n');keyboard;exit;
  end;

  labelImage=zeros(size(origImg,1),size(origImg,2)); % Intialize with the unlabeled tag

  %for i=1:size(dCoords,2)
    %labelImage(dCoords(2,i),dCoords(1,i))=labels(i);
  %end
  labelImage(sub2ind(size(labelImage),dCoords(2,:),dCoords(1,:)))=labels;

  cmap=colorcube(numLabels);

end

%function [labelImage,cmap]=drawLabels(labels,numClusters,dCoords,sCoords,uCoords,origImg)
  %numLabels=numClusters+2; % Other than texton labels, we have salient,uniform
  %labels=labels+2; % The first 2 labels are for uniform, and salient (in order)
%
  %if size(labels,2)~=size(dCoords,2)
    %fprintf('Something wrong with dimensions of arrays of labels and drawing coordinates\n');keyboard;exit;
  %end;
%
  %labelImage=zeros(size(origImg,1),size(origImg,2)); % Intialize with the unlabeled tag
%
  %%for i=1:size(dCoords,2)
    %%labelImage(dCoords(2,i),dCoords(1,i))=labels(i);
  %%end
  %labelImage(sub2ind(size(labelImage),dCoords(2,:),dCoords(1,:)))=labels;
%
  %for i=1:size(uCoords,2)
    %labelImage(uCoords(2,i),uCoords(1,i))=1;
  %end
%
  %for i=1:size(sCoords,2)
    %labelImage(sCoords(2,i),sCoords(1,i))=2;
  %end
  %
  %cmap=colorcube(numLabels);
  %
%end


function  [oldlb,oldub] = presolve_probebinary(A,b,c,lb,ub,binary_variables);

goon = 1;

AL0A  = (A>0).*A;
AG0A  = (A<0).*A;

At = A';
%used = full(any(A(:,find(changed_bounds)),2));
isbinary  = ismembc(1:length(lb),binary_variables);
isinteger = ismembc(1:length(lb),binary_variables);

goon = all(lb<=ub);

oldub = ub;
oldlb = lb;

for i = 1:length(binary_variables)

    % Probe up
    lb = oldlb;
    ub = oldub;
    if lb(binary_variables(i))~=ub(binary_variables(i))
        lb(binary_variables(i)) = 1;
        bi_dn = AL0A*lb+AG0A*ub;
        if any(bi_dn>b)
            oldlb(binary_variables(i)) = 0;
            oldub(binary_variables(i)) = 0;
        end
    end
    % Probe down
    lb = oldlb;
    ub = oldub;
    if lb(binary_variables(i))~=ub(binary_variables(i))
        ub(binary_variables(i)) = 0;
        bi_dn = AL0A*lb+AG0A*ub;
        if any(bi_dn>b)
            oldlb(binary_variables(i)) = 1;
            oldub(binary_variables(i)) = 1;
        end
    end
end

fix_this = [];
fix_value = [];


% classify function
% TO RUN : 1) MatConvNet must be installed.
%          2) Modify variable 'setup_path'
% Input : 101 x 101 x 3 feature image
% Output : scores [0, 1] of each class (1~8)

function scores = classifyFeature(img, images, net)
    % 1) Input Normalization : change to 50x100x3 and subtract data_mean
    nor_im = single(img(52:101,1:100,:))- images.data_mean;

    % 2) Classify via vl_simplenn
    net.layers{end}.type = 'softmax';
    res = vl_simplenn(net, nor_im) ;

    % show the classification result
    scores = squeeze(gather(res(end).x)) ;

end


function [p1,p2,a,a1,a2,a3,h]=pigeon()
% PIGEON Draw a pigeon
% numandina@gmail.com
%

l=1; % pigeon size
hold on
axis([-5 5 -5 5 -5 5])
view(3)
grid on
rotate3d

% body
[a,b,c]=sphere(25);
aa1=surf(a.*l,b.*l,c.*l);
set(aa1,'edgecolor','none','facecolor',[.5 .5 .5])

% head
[a,b,c]=sphere(25);
aa2=surf(a.*.5.*l,b.*.5.*l+.5.*l,c.*.5.*l+1.*l);
set(aa2,'edgecolor','none','facecolor',[.5 .5 .5])

% eyes
[a,b,c]=sphere(25);
aa2=surf(a.*.1.*l,b.*.1.*l+.875.*l,c.*.1.*l+1.25.*l);
set(aa2,'edgecolor','none','facecolor',[0 0 0])
[a,b,c]=sphere(25);
aa2=surf(a.*.1.*l+.2,b.*.1.*l+.85.*l,c.*.1.*l+1.25.*l);
set(aa2,'edgecolor','none','facecolor',[0 0 0])

% beak
Cone2 = Cone([0 0.5 1].*l ,[0.5 1.5 0.5].*l,[.25 0].*l,25,[.9 .5 .5]);

% legs
l1=line([0 0]-.1,[-.2 -.2].*l,[-.5 -1.5].*l);
set(l1,'linewidth',2,'color',[.9 .5 .5])
l2=line([0 0]+.1,[-.2 -.2].*l,[-.5 -1.5].*l);
set(l2,'linewidth',2,'color',[.9 .5 .5])

a= [.5 0 0;
	.5 0 0;
	1.5 1 0;
	1.5 1 0;
	.5 0 1;
	.5 0 1;
	.5 1 1;
	.5 1 1].*l;

a(:,2)=a(:,2)-.5.*l;
a(:,3)=a(:,3)-.5.*l;
a(:,1)=a(:,1)+0.2.*l;

b= [1 2 6 5;
	2 3 7 6;
	3 4 8 7;
	4 1 5 8;
	1 2 3 4;
	5 6 7 8];

% wings, first position
p1=patch('vertices',a,'faces',b,'edgecolor',[.5 .5 .5],'facecolor',[.9 .9 .9]);
a1=a;
a1(:,1)=-a1(:,1);
p2=patch('vertices',a1,'faces',b,'edgecolor',[.5 .5 .5],'facecolor',[.9 .9 .9]);

% second position of wings
a2= [.5 0 2;
	.5 0 2;
	1.5 1 2;
	1.5 1 2;
	.5 0 1;
	.5 0 1;
	.5 1 1;
	.5 1 1].*l;
a2(:,2)=a2(:,2)-.5.*l;
a2(:,3)=a2(:,3)-.5.*l;
a2(:,1)=a2(:,1)+.2.*l;
a3=a2;
a3(:,1)=-a3(:,1);

% animate wings
while true
	set(p1,'vertices',a2)
	set(p2,'vertices',a3)
	pause(.25)
	set(p1,'vertices',a)
	set(p2,'vertices',a1)
	pause(.25)
end

% get everything's handle
h=[aa1,aa3,Cone2,EndPlate1,EndPlate2,l1,l2];

% edited from a file on the FEX, create a cone
	function Cone2 = Cone(X1,X2,R,n,cyl_color)
		length_cyl=norm(X2-X1);
		t=linspace(0,2*pi,n)';
		x1=[0 length_cyl];
		xx1=repmat(x1,n,1);
		xx2=[R(1)*cos(t) R(2)*cos(t)];
		xx3=[R(1)*sin(t) R(2)*sin(t)];
		Cone2=mesh(xx1,xx2,xx3);
		unit_Vx=[1 0 0];
		angle_X1X2=acos( dot( unit_Vx,(X2-X1) )/( norm(unit_Vx)*norm(X2-X1)) )*180/pi;
		axis_rot=cross([1 0 0],(X2-X1) );
		if angle_X1X2~=0
			rotate(Cone2,axis_rot,angle_X1X2,[0 0 0])
		end
		set(Cone2,'XData',get(Cone2,'XData')+X1(1),'facecolor',cyl_color)
		set(Cone2,'YData',get(Cone2,'YData')+X1(2))
		set(Cone2,'ZData',get(Cone2,'ZData')+X1(3))
		set(Cone2,'EdgeAlpha',0)
	end
end


% dbm - training DBM using Gibbs sampling
% Copyright (C) 2011 KyungHyun Cho, Tapani Raiko, Alexander Ilin
%
% 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 2
% 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, write to the Free Software
% Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
%
function [D] = dbm(D, patches, use_Qpre, Qpre, Qpre_mask);

if nargin < 3
    use_Qpre = 0;
end

actual_lrate = D.learning.lrate;

if D.adaptive_lrate.use == 1
    initial_lrate = D.learning.lrate;
    actual_lrate = initial_lrate;
end

n_samples = size(patches, 1);
if D.structure.layers(1) ~= size(patches, 2)
    error('Data is not properly aligned');
end

minibatch_sz = D.learning.minibatch_sz;
n_minibatches = ceil(n_samples / minibatch_sz);

n_epochs = D.iteration.n_epochs;

cd_k = D.learning.cd_k;
persistent_cd = D.learning.persistent_cd;
momentum = D.learning.momentum;
weight_decay = D.learning.weight_decay;

adaptive_lrate = D.adaptive_lrate.use;
enhanced_grad = D.enhanced_grad.use;

lrate_lb = D.adaptive_lrate.lrate_lb;
lrate_ub = D.adaptive_lrate.lrate_ub;
exp_up = D.adaptive_lrate.exp_up;
exp_down = D.adaptive_lrate.exp_down;
max_iter_up = D.adaptive_lrate.max_iter_up;
max_iter_down = D.adaptive_lrate.max_iter_down;

layers = D.structure.layers;
n_layers = length(layers);

min_recon_error = Inf;
min_recon_error_update_idx = 0;
stopping = 0;

do_normalize = D.grbm.do_normalize;
do_normalize_std = D.grbm.do_normalize_std;
update_sigmas = D.grbm.learn_sigmas;
do_vsample = D.grbm.do_vsample;

if D.data.binary == 0
    if do_normalize == 1
        % make it zero-mean
        patches_mean = mean(patches, 1);
        patches = bsxfun(@minus, patches, patches_mean);
    end

    if do_normalize_std == 1
        % make it unit-variance
        patches_std = std(patches, [], 1);
        patches = bsxfun(@rdivide, patches, patches_std);
    end
end

n_samples = size(patches, 1);

logsigmas = log(D.sigmas.^2);

% upper-bound.. but is there any need for it?
sigmas_ub = D.grbm.sigmas_ub;
logsigmas_ub = log(D.grbm.sigmas_ub);

biases_grad_old = cell(n_layers, 1);
W_grad_old = cell(n_layers, 1);
for l = 1:n_layers
    biases_grad_old{l} = zeros(size(D.biases{l}))';
    if l < n_layers
        W_grad_old{l} = zeros(size(D.W{l}));
    end
end
sigma_grad_old = zeros(size(D.biases{1}))';

n_minibatches = ceil(n_samples / minibatch_sz);
n_updates = 0;

epsilon_sigma = 1e-8;
epsilon_logsigma = log(epsilon_sigma^2);

min_recon_error = Inf;
min_recon_error_update_idx = 0;
stopping = 0;

anneal_counter = 0;
actual_lrate0 = actual_lrate;

if D.debug.do_display == 1
    figure(D.debug.display_fid);
end

try
    use_gpu = gpuDeviceCount;
catch errgpu
    use_gpu = false;
    disp(['Could not use CUDA. Error: ' errgpu.identifier])
end

if use_gpu
    % push
    logsigmas = gpuArray(single(logsigmas));
end

for step=1:n_epochs
    if D.verbose
        fprintf(2, 'Epoch %d/%d: ', step, n_epochs)
    end

    if use_gpu
        % push
        for l = 1:n_layers
            if l < n_layers
                D.W{l} = gpuArray(single(D.W{l}));
            end
            D.biases{l} = gpuArray(single(D.biases{l}));
            if D.centering.use
                D.centering.centers{l} = gpuArray(single(D.centering.centers{l}));
            end
        end

        D.sigmas = gpuArray(single(D.sigmas));
    end

    for mb=1:n_minibatches
        D.iteration.n_updates = D.iteration.n_updates + 1;

        if D.verbose
            tic;
        end

        % p_0
        mb_start = (mb-1) * minibatch_sz + 1;
        mb_end = min(mb * minibatch_sz, n_samples);
        v0 = patches(mb_start:mb_end, :);

        if use_gpu > 0
            v0 = gpuArray(single(v0));
        end

        if D.data.binary
            v0 = binornd(1, v0);
        end
        mb_sz = size(v0,1);

        % just for a bit of speed-up
        if persistent_cd && exist('h1') ~= 0
            fmb_sigma2s = repmat(D.sigmas', [size(h1{1}, 1) 1]);
        else
            fmb_sigma2s = repmat(D.sigmas', [mb_sz 1]);
        end

        if use_Qpre
            % for pretraining
            h0 = cell(n_layers, 1);
            h0{1} = v0;
            for l = 2:n_layers
                if Qpre_mask(l)
                    h0{l} = binornd(1, Qpre{l}(mb_start:mb_end, :));
                else
                    h0{l} = zeros(mb_end - mb_start + 1, layers(l));
                end
                if use_gpu
                    h0{l} = gpuArray(single(h0{l}));
                end
            end

            for l = 2:n_layers
                if Qpre_mask(l)
                    continue;
                end

                h0{l} = h0{l} * 0;
                if l > 1
                    if l == 2 && D.data.binary == 0
                        h0{l} = h0{l} + bsxfun(@rdivide, h0{l-1}, D.sigmas.^2') * D.W{l-1};
                    else
                        h0{l} = h0{l} + h0{l-1} * D.W{l-1};
                    end
                end

                if l < n_layers
                    h0{l} = h0{l} + h0{l+1} * D.W{l}';
                end

                h0{l} = sigmoid(bsxfun(@plus, h0{l}, D.biases{l}'));
            end

            if adaptive_lrate
                if mb == n_minibatches
                    mb_next = 1;
                else
                    mb_next = mb + 1;
                end

                nmb_start = (mb_next-1) * minibatch_sz + 1;
                nmb_end = min(mb_next * minibatch_sz, n_samples);
                if use_gpu
                    v0_next = gpuArray(single(patches(nmb_start:nmb_end, :)));
                else
                    v0_next = single(patches(nmb_start:nmb_end, :));
                end

                if D.data.binary == 0
                    next_mb_sz = size(v0_next,1);

                    if persistent_cd == 0
                        nmb_sigma2s = repmat(D.sigmas, [next_mb_sz 1]);
                    else
                        if next_mb_sz ~= minibatch_sz
                            nmb_sigma2s = repmat(D.sigmas, [next_mb_sz 1]);
                        else
                            nmb_sigma2s = fmb_sigma2s;
                        end
                    end
                end
            end
        else
            % for finetuning
            h0 = dbm_get_hidden(v0, D, 10, 1e-6, D.mf.reg);
            h0{1} = v0;

            if adaptive_lrate
                if mb == n_minibatches
                    mb_next = 1;
                else
                    mb_next = mb + 1;
                end
                if use_gpu
                    v0_next = gpuArray(single(patches((mb_next-1) * minibatch_sz + 1:min(mb_next * minibatch_sz, n_samples), :)));
                else
                    v0_next = single(patches((mb_next-1) * minibatch_sz + 1:min(mb_next * minibatch_sz, n_samples), :));
                end
                if D.data.binary == 0
                    next_mb_sz = size(v0_next,1);

                    if persistent_cd == 0
                        nmb_sigma2s = repmat(D.sigmas, [next_mb_sz 1]);
                    else
                        if next_mb_sz ~= minibatch_sz
                            nmb_sigma2s = repmat(D.sigmas, [next_mb_sz 1]);
                        else
                            nmb_sigma2s = fmb_sigma2s;
                        end
                    end
                end
            end
        end

        % p_1
        if (persistent_cd ~= 0 && exist('h1') == 0)
            h1 = h0;
        end

        if (persistent_cd == 0)
            h1 = h0;

            if use_Qpre
                for l = 2:n_layers
                    if Qpre_mask(l)
                        continue;
                    end

                    h1{l} = binornd(1, h1{l});
                end
            end
        end

        if D.centering.use
            for l = 1:n_layers
                if D.data.binary == 0 && l == 1
                    continue;
                end
                h0{l} = bsxfun(@minus, h0{l}, D.centering.centers{l}');
            end
        end

        % compute reconstruction error
        if D.data.binary == 1
            vr = sigmoid(bsxfun(@plus, h0{2} * D.W{1}',D.biases{1}'));
        else
            vr = bsxfun(@plus, h0{2} * D.W{1}',D.biases{1}');
        end

        rerr = mean(sum((v0 - vr).^2,2));
        if use_gpu > 0
            rerr = gather(rerr);
        end
        D.signals.recon_errors = [D.signals.recon_errors rerr];

        for k=1:cd_k
            for oddeven = [1 0]
                for l = 1:n_layers
                    if mod(l, 2) == oddeven
                        continue;
                    end
                    h1{l} = h1{l} * 0;
                    if D.centering.use
                        if l > 1
                            if l == 2 && D.data.binary == 0
                                h1{l} = h1{l} + bsxfun(@rdivide, h1{l-1}, D.sigmas.^2') * D.W{l-1};
                            else
                                h1{l} = h1{l} + bsxfun(@minus, h1{l-1}, D.centering.centers{l-1}') * D.W{l-1};
                            end
                        end

                        if l < n_layers
                            h1{l} = h1{l} + bsxfun(@minus, h1{l+1}, D.centering.centers{l+1}') * D.W{l}';
                        end
                    else
                        if l > 1
                            if l == 2 && D.data.binary == 0
                                h1{l} = h1{l} + bsxfun(@rdivide, h1{l-1}, D.sigmas.^2') * D.W{l-1};
                            else
                                h1{l} = h1{l} + h1{l-1} * D.W{l-1};
                            end
                        end

                        if l < n_layers
                            h1{l} = h1{l} + h1{l+1} * D.W{l}';
                        end
                    end

                    h1{l} = bsxfun(@plus, h1{l}, D.biases{l}');

                    if l > 1 || D.data.binary == 1
                        h1{l} = sigmoid(h1{l});
                        h1{l} = binornd(1, h1{l});
                    else
                        if do_vsample
                            h1{l} = normrnd(h1{l}, fmb_sigma2s);
                        end
                    end
                end
                if (sum(sum(isnan(h1{1}))) > 0)
                    error('NaN found in the visual fantasy particles.\n It is advisable to adjust learning parameters.');
                end
            end
        end

        if D.centering.use
            for l = 1:n_layers
                if D.data.binary == 0 && l == 1
                    continue;
                end
                h1{l} = bsxfun(@minus, h1{l}, D.centering.centers{l}');
            end
        end

        % get base distribution
        base_vbias = mean(h1{1}, 1);
        base_sigma = std(h1{1}, [], 1)';

        % get gradient
        for l = 1:n_layers
            if D.data.binary == 0 && l == 1
                bias0 = bsxfun(@rdivide, mean(h0{l}, 1), D.sigmas.^2');
                bias1 = bsxfun(@rdivide, mean(h1{l}, 1), D.sigmas.^2');
            else
                bias0 = mean(h0{l}, 1);
                bias1 = mean(h1{l}, 1);
            end

            biases_grad{l} = bias0 - bias1;

            clear bias0 bias1;

            if l < n_layers
                if D.data.binary == 0 && l == 1
                    W0 = bsxfun(@rdivide, (h0{l}' * h0{l+1}) / mb_sz, D.sigmas.^2);
                    W1 = bsxfun(@rdivide, (h1{l}' * h1{l+1}) / size(h1{1},1), D.sigmas.^2);
                else
                    W0 = (h0{l}' * h0{l+1}) / mb_sz;
                    W1 = (h1{l}' * h1{l+1}) / size(h1{1},1);
                end

                W_grad{l} = W0 - W1;

                clear W0 W1;
            end
        end

        if D.data.binary == 0
            sigma0 = mean((bsxfun(@minus, h0{1}, D.biases{1}').^2) - ...
                h0{1} .* (h0{2} * D.W{1}'), 1);
            sigma1 = mean((bsxfun(@minus, h1{1}, D.biases{1}').^2) - ...
                h1{1} .* (h1{2} * D.W{1}'), 1);
            sigma_grad = (sigma0 - sigma1) ./ D.sigmas.^2';

            if D.grbm.use_single_sigma == 1
                mean_sigma_grad = mean(sigma_grad);
                sigma_grad = mean_sigma_grad * ones(size(sigma_grad));
            end

            clear sigma0 sigma1;
        end

        % enhanced grad
        if enhanced_grad == 1
            acts = cell(n_layers, 1);
            for l = 1:n_layers
                acts{l} = (mean(h0{l}, 1) + mean(h1{l}, 1))/2;
            end
%            if D.data.binary == 0
%                acts{1} = acts{1} ./ D.sigmas.^2';
%            end

            for l = 1:n_layers-1
                W_grad{l} = W_grad{l} - biases_grad{l}' * acts{l+1} ...
                    - acts{l}' * biases_grad{l+1};
            end

            for l = 1:n_layers
                if l > 1
                    acts1 = acts{l-1};
                    biases_grad{l} = biases_grad{l} - acts1 * W_grad{l-1};
                end
                if l < n_layers
                    acts2 = acts{l+1};
                    biases_grad{l} = biases_grad{l} - acts2 * W_grad{l}';
                end
            end

            clear acts;
        end

        if D.learning.lrate_anneal > 0 && (step >= D.learning.lrate_anneal * n_epochs)
            anneal_counter = anneal_counter + 1;
            actual_lrate = actual_lrate0 / anneal_counter;
        else
            if adaptive_lrate == 1
                if use_Qpre
                    h0_next = cell(n_layers, 1);
                    h0_next{1} = v0_next;
                    for l = 2:n_layers
                        if Qpre_mask(l)
                            h0_next{l} = Qpre{l}(nmb_start:nmb_end, :);
                        else
                            h0_next{l} = zeros(nmb_end - nmb_start + 1, layers(l));
                        end
                        if use_gpu
                            h0_next{l} = gpuArray(single(h0_next{l}));
                        end
                    end

                    for l = 2:n_layers
                        if Qpre_mask(l)
                            continue;
                        end

                        h0_next{l} = h0_next{l} * 0;
                        if l > 1
                            if l == 2 && D.data.binary == 0
                                h0_next{l} = h0_next{l} + bsxfun(@rdivide, h0_next{l-1}, D.sigmas.^2') * D.W{l-1};
                            else
                                h0_next{l} = h0_next{l} + h0_next{l-1} * D.W{l-1};
                            end
                        end

                        if l < n_layers
                            h0_next{l} = h0_next{l} + h0_next{l+1} * D.W{l}';
                        end

                        h0_next{l} = sigmoid(bsxfun(@plus, h0_next{l}, D.biases{l}'));
                    end
                else
                    h0_next = dbm_get_hidden(v0_next, D, 10, 1e-6, D.mf.reg);
                    h0_next{1} = v0_next;
                end

                if D.centering.use
                    for l = 1:n_layers
                        if D.data.binary == 0 && l == 1
                            continue;
                        end
                        h0_next{l} = bsxfun(@minus, h0_next{l}, D.centering.centers{l}');
                    end
                end

                [cE, cEmin, cEmax, cEs] = dbm_energy(h1, D.W, D.biases, D.data.binary, 1., D.sigmas, base_sigma, base_vbias);

                base_lrate = actual_lrate;
                candidate_lrates;

                costs = zeros(1, length(cand_lrates));
                for s=1:length(cand_lrates)
                    W_test = cell(size(D.W));
                    biases_test = cell(size(D.biases));
                    if use_gpu
                        logsigmas_test = gpuArray(single(zeros(size(logsigmas))));
                    else
                        logsigmas_test = single(zeros(size(logsigmas)));
                    end
                    cand_lrate = cand_lrates(s);

                    for l = 1:n_layers
                        biases_test{l} = D.biases{l} + cand_lrate * (((1 - momentum) * biases_grad{l} + momentum * biases_grad_old{l})' - weight_decay * D.biases{l});
                        if l < n_layers
                            W_test{l} = D.W{l} + cand_lrate * ((1 - momentum) * W_grad{l} + momentum * W_grad_old{l} - weight_decay * D.W{l});
                        end
                    end
                    if D.data.binary == 0
                        if update_sigmas == 1
                            logsigmas_test = logsigmas + cand_lrate * (((1-momentum) * sigma_grad + momentum * sigma_grad_old)' - weight_decay * logsigmas);
                            logsigmas_test = max(epsilon_logsigma, min(logsigmas_ub, logsigmas_test));
                            sigmas_test = sqrt(exp(logsigmas));
                        else
                            sigmas_test = sqrt(exp(logsigmas_test));
                        end
                    else
                        sigmas_test = sqrt(exp(logsigmas_test));
                    end

%                    % FIXME: Should we?
%                    h0_next = dbm_get_hidden_raw(v0_next, D.data.binary, D.structure.layers, ...
%                        W_test, biases_test, sigmas_test, 5, 1e-5);
%                    h0_next{1} = v0_next;
                    [dE, dEmin, dEmax, dEs] = dbm_energy(h0_next, W_test, biases_test, D.data.binary, 1., sigmas_test, base_sigma, base_vbias);
                    [fE, fEmin, fEmax, fEs] = dbm_energy(h1, W_test, biases_test, D.data.binary, 1., sigmas_test, base_sigma, base_vbias);

                    now_cost = sum(-double(gather(dEs)) - logsum(double(gather(-fEs + cEs))) + log(size(h1{1},1)));

                    costs(s) = now_cost;

                    clear W_test biases_test logsigmas_test sigmas_test;
                    %clear h0_next;
                end

                [chosen_cost chosen_index] = max(costs);
                actual_lrate = min(lrate_ub, max(lrate_lb, cand_lrates(chosen_index)));
            else
                actual_lrate = D.learning.lrate / (1 + D.iteration.n_updates / D.learning.lrate0);
            end
            actual_lrate0 = actual_lrate;
        end

        D.signals.lrates = [D.signals.lrates actual_lrate];

%        if D.debug.do_display == 1 && mod(D.iteration.n_updates, D.debug.display_interval) == 0
%            D.debug.display_function (D.debug.display_fid, D, v0, v1, W_grad, vbias_grad, hbias_grad, sigma_grad);
%            drawnow;
%        end
%
        % update
        for l = 1:n_layers
            biases_grad_old{l} = (1 - momentum) * biases_grad{l} + momentum * biases_grad_old{l};
            D.biases{l} = D.biases{l} + actual_lrate * (biases_grad_old{l}' - weight_decay * D.biases{l});
            if l < n_layers
                W_grad_old{l} = (1 - momentum) * W_grad{l} + momentum * W_grad_old{l};
                D.W{l} = D.W{l} + actual_lrate * (W_grad_old{l} - weight_decay * D.W{l});
            end
        end
        if D.data.binary == 0
            if update_sigmas == 1
                sigma_grad_old = (1-momentum) * sigma_grad + momentum * sigma_grad_old;
                logsigmas = logsigmas + actual_lrate * (sigma_grad_old' - weight_decay * logsigmas);
                logsigmas = max(epsilon_logsigma, min(logsigmas_ub, logsigmas));
                D.sigmas = sqrt(exp(logsigmas));
            end
        end

        if D.verbose == 1
            fprintf(2, '%2.3fs.', toc);
        end

        if D.stop.criterion > 0
            if D.stop.criterion == 1
                if min_recon_error > D.signals.recon_errors(end)
                    min_recon_error = D.signals.recon_errors(end);
                    min_recon_error_update_idx = D.iteration.n_updates;
                else
                    if D.iteration.n_updates > min_recon_error_update_idx + D.stop.recon_error.tolerate_count
                        fprintf(2, '\nStopping criterion reached (recon error) %f > %f\n', ...
                            D.signals.recon_errors(end), min_recon_error);
                        stopping = 1;
                        break;
                    end
                end
            else
                error ('Unknown stopping criterion %d', D.stop.criterion);
            end
        end

        if length(D.hook.per_update) > 1
            err = D.hook.per_update{1}(D, D.hook.per_update{2});

            if err == -1
                stopping = 1;
                break;
            end
        end

        if D.centering.use
            for l = 1:n_layers
                if D.data.binary == 0 && l == 1
                    continue;
                end
                h0{l} = bsxfun(@plus, h0{l}, D.centering.centers{l}');
                h1{l} = bsxfun(@plus, h1{l}, D.centering.centers{l}');
            end
        end

        if use_gpu > 0
            clear v0 h0;
            clear v0_next h0_next;
            if persistent_cd == 0
                clear h1;
            end

            clear fmb_sigma2s;
            clear base_sigma base_vbias;
        end

    end

    if use_gpu > 0
        % pull
        for l = 1:n_layers
            if l < n_layers
                D.W{l} = gather(D.W{l});
            end
            D.biases{l} = gather(D.biases{l});
            if D.centering.use
                D.centering.centers{l} = gather(D.centering.centers{l});
            end
        end
        D.sigmas = gather(D.sigmas);
    end

    if length(D.hook.per_epoch) > 1
        err = D.hook.per_epoch{1}(D, D.hook.per_epoch{2});

        if err == -1
            stopping = 1;
        end
    end

    if stopping == 1
        break;
    end

    if D.verbose == 1
        fprintf(2, '\n');
    end

    fprintf(2, 'Epoch %d/%d - recon_error: %f\n', step, n_epochs, ...
        D.signals.recon_errors(end));
end

if use_gpu > 0
    % pull
    for l = 1:n_layers
        if l < n_layers
            D.W{l} = gather(D.W{l});
        end
        D.biases{l} = gather(D.biases{l});
        if D.centering.use
            D.centering.centers{l} = gather(D.centering.centers{l});
        end
    end
    D.sigmas = gather(D.sigmas);

    clear h1 logsigmas;
end