function A = sim_genTVMVARStructure(Aproto,Nl,ndisc,p)
% generate time-varying MVAR structure from a prototype structure
%
% Inputs:
%
%       Aproto:   Prototype structure returned by sim_genVARModelFromEq().
%       Nl:       Number of sample points
%       ndisc:    Number of 'startup' points we will discard when generating data
%       p:        MVAR model order
%
% Outputs:
%
%       A:        Cell array of dimension Nl+ndisc+p containing VAR
%                 coefficient matrices for each time point
%
% See Also: sim_genVARModelFromEq()
%
% References:
%
%
% Author: Tim Mullen, May 2011, SCCN/INC, UCSD.
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

    A = repmat({Aproto},1,Nl+ndisc+p);

    if iscell(Aproto)
        % find all the function handles
        numidx = find(cellfun(@(x)isnumeric(x),Aproto));
        funidx = setdiff_bc(1:numel(Aproto),numidx);

        % funidx = find(cellfun(@(x)strcmpi(class(x),'function_handle'),Aproto));

        % construct inline objects
        for fun = 1:length(funidx)
            fx{fun} = inline(Aproto{funidx(fun)});
        end

        % evaluate each function handle
        for t=1:Nl+ndisc+p;

            if ~mod(t,1000)
                fprintf('%d/%d - ',t,Nl+ndisc+p);
            end

            for fun = 1:length(funidx)
                A{t}{funidx(fun)} = double(fx{fun}(t));
            end

            A{t} = cell2mat(A{t});
        end
    end
end


function outpoints = mni2tal(inpoints)
% Converts coordinates from MNI brain to best guess
% for equivalent Talairach coordinates
% FORMAT outpoints = mni2tal(inpoints)
% Where inpoints is N by 3 or 3 by N matrix of coordinates
%  (N being the number of points)
% outpoints is the coordinate matrix with Talairach points
% Matthew Brett 10/8/99

% ensure that SPM is available, needed for spm_matrix
hasspm = ft_hastoolbox('spm8up', 3) || ft_hastoolbox('spm2', 1);

dimdim = find(size(inpoints) == 3);
if isempty(dimdim)
  ft_error('input must be a N by 3 or 3 by N matrix')
end
if dimdim == 2
  inpoints = inpoints';
end

% Transformation matrices, different zooms above/below AC
upT   = spm_matrix([0 0 0 0.05 0 0 0.99 0.97 0.92]);
downT = spm_matrix([0 0 0 0.05 0 0 0.99 0.97 0.84]);

tmp = inpoints(3,:)<0;  % 1 if below AC
inpoints = [inpoints; ones(1, size(inpoints, 2))];
inpoints(:, tmp) = downT * inpoints(:, tmp);
inpoints(:, ~tmp) = upT * inpoints(:, ~tmp);
outpoints = inpoints(1:3, :);
if dimdim == 2
  outpoints = outpoints';
end


filename='CantileverBeam_Quadrilateral_Bilinear_Structured';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'holes';
cost = {'compliance';'perimeter'};
weights = [1 0.1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'P1';
constraints_case = 'INEQUALITY';
line_search_initiator = 'INCREASING LAST STEP';

nsteps = 10;
Vfrac_final = 0.2;
Perimeter_target=1;
optimality_final =1e-3;
constr_final =1e-3;

Vfrac_initial = 0.5;
optimality_initial = 1e-2;
constr_initial = 1e-3;

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


%filename='Cantilever_tetrahedra';
%filename='Cantileverbeam_Tetrahedra_Linear_Structured_Fine';
%filename = 'Cantilever3D';
%filename = 'Cantilever3DLarge';
filename = 'Cantilever3DLargeFineFine';
%filename='Cantilever_quad_fine';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance','perimeter'};
%cost = {'compliance'};
weights = [1, 1];
%weights = 1;
constraint = {'volumeConstraint'};
optimizerUnconstrained = 'SLERP';
incrementFactor = 1;
designVariable = 'LevelSet';

%optimizerUnconstrained = 'PROJECTED GRADIENT';
%designVariable = 'Density';

filterType = 'PDE';
optimizer = 'DualNestedInPrimal';
%optimizer = 'AlternatingPrimalDual';

nsteps = 80;
Vfrac_final = 0.15;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final = 1e-2;

Vfrac_initial      = 0.15;
optimality_initial = 0.5*1e-2;
constr_initial     = 1e-2;

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

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

maxiter = 80;


% Fig. 6.14b   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clear all;
%close all;
clf

num=1;
den=conv([1 0],[1 2 1]);
rlocus(num,den);
title('Fig. 6.14(b)');
grid;


% system process model in local s0 frame with dcm formulation, Cs0e is the
% transformation from the sensor local s0 frame to e-frame
% written by Huai
% rqs02e, r s0 in e, q s0 2e, rvqs0, rs in s0, vs in s0, qs0 2s
% acc, m/s^2, acc by imu in s frame, gyro, rad/s, angular rate by imu
% w.r.t i frame coordinated in s frame, dt, time interval for covariance update
% modelNo, imu model for bias and scale factor errors
function [STM Qd]=sys_local_dcm_v001(rqs02e, rvqs0, acc, gyro, dt, imutype, modelNo)
% the covariance corresponds to states,
% rs in s0, v s in s0, q s02s, gravity in s0, ba, bg, sa, sg, qs2c, Ts in c
% for modelNo=3 and imutype=5, MEMS
% acc bias drift, random walk
% gyro bias drift, random walk
% acc scale factor, random walk
% gyro scale factor, random walk
% for modelNo=1 the acc and gyro turn on bias are removed

%system disturbance coefs
navStates=12;
N=zeros(navStates,6);
N(7:9,4:6)=eye(3); %attitude
Cs02s=quat2dcm_v000(rvqs0(7:10));
N(4:6,1:3)=Cs02s'; %velocity

%system matrix
A=zeros(navStates);
A(1:3, 4:6)=eye(3); % rs in s0
%Velocity
WIE_E=7292115e-11;
wie2s0=quatrot_v000(rqs02e(4:7),[0;0;WIE_E], 1);
% A(4:6,1:3)=-skew(wie2s0)^2; % for low grade IMU, w_ie^e is buried
% $\delta\bar{g}^{s0}(\mathbf{x}_s^{s0})$ is noisy, but buried by low grade
% IMU noise
% A(4:6,4:6)=-2*skew(wie2s0);
A(4:6,7:9)=-Cs02s'*skew(acc);
% A(4:6,10:12)=eye(3); % for low grade IMU, w_ie^e is buried

%Attitude
A(7:9,7:9)=skew(-gyro);
Anav=A;
Nnav=N;
% X(k+1) = ffun[X(k),U(k),V(k)]
% X(k+1) = Ak*X(k)+Gk*Vk

%%%%Imu error model parameters
[Aimu_d, Qimu_d, Cimu, Rimu]=imu_err_model_v001(acc, gyro, dt, imutype, modelNo);
%%%%Combine and discretize nav and imu models
% this discretization can also be accomplished by Loan's matrix exponential
% method, see sys_metric_phipsi_v000.m

Anav_d=eye(navStates)+dt*Anav;  %Use 1st order taylor series to discretize Anav
Qnav=Nnav*Rimu*Nnav';
Qnav_d=dt/2*(Anav_d*Qnav+Qnav*Anav_d');      %Use trapezoidal rule to discretize Rimu

STM=zeros(navStates+size(Aimu_d,1));
STM(1:navStates,1:navStates)=Anav_d;
STM(1:navStates,1+navStates:end)=Nnav*Cimu*dt;
STM(1+navStates:end,1+navStates:end)=Aimu_d;

Qd=zeros(navStates+size(Aimu_d,1));
Qd(1:navStates,1:navStates)=Qnav_d;
Qd(1+navStates:end,1+navStates:end)=Qimu_d;
Qd(1:navStates,1+navStates:end)=Nnav*Cimu*Qimu_d*dt/2;
Qd(1+navStates:end,1:navStates)=Qd(1:navStates,1+navStates:end)';
end


% SP_NURBS: Constructor of the class of a tensor-product spaces of NURBS.
%
%     sp = sp_nurbs (nurbs, msh)
%     sp = sp_nurbs (knots, degree, weights, msh)
%
% INPUTS:
%
%     nurbs:     nurbs structure from the NURBS toolbox (see nrbmak)
%     msh:       msh object that defines the quadrature rule (see msh_cartesian)
%     knots:     open knot vector (cell array of size [1, ndim])
%     degree:    nurbs polynomial degree (vector of size [1, ndim])
%     weights:   weights associated to the basis functions
%
% OUTPUT:
%
%    sp: object of the class sp_scalar, representing the discrete function space. See sp_scalar for details.
%
% Copyright (C) 2015 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

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

function sp = sp_nurbs (varargin)

  if (nargin <= 3)
    nurbs = varargin{1};
    msh   = varargin{2};
    if (nargin == 3)
      transform = varargin{3};
    else
      transform = 'grad-preserving';
    end

    knots   = nurbs.knots;
    degree  = nurbs.order - 1;
    weights = squeeze (nurbs.coefs(4, :, :, :));
  else
    knots   = varargin{1};
    degree  = varargin{2};
    weights = varargin{3};
    msh     = varargin{4};
    if (nargin == 5)
      transform = varargin{5};
    else
      transform = 'grad-preserving';
    end
  end

  sp = sp_scalar (knots, degree, weights, msh, transform);

end


function pot_type = determine_pot_type(model, onodes, nodes)
% DETERMINE_POT_TYPE Determine the type of potential based on the evidence pattern.
% pot_type = determine_pot_type(model, onodes, nodes)
%
% If there are any utility nodes, pot_type = 'u'
% else
% If all hidden nodes are discrete, pot_type = 'd'.
% If all hidden nodes are continuous, pot_type = 'g' (Gaussian).
% If some hidden nodes are discrete, and some cts, pot_type = 'cg' (conditional Gaussian).
%
% nodes defaults to all nodes in graph

nnodes = length(model.node_sizes);
if nargin < 3, nodes = 1:nnodes; end

hnodes = mysetdiff(nodes, onodes);
if isfield(model, 'limid') %~isempty(model.utility_nodes)
  pot_type = 'u';
elseif isempty(myintersect(model.cnodes, hnodes))
  pot_type = 'd';
elseif mysubset(hnodes, model.cnodes)
  pot_type = 'g';
else
  pot_type = 'cg';
end


function c3 = sandia_sgmgg_coef_inc2 ( m, n1, s1, c1, s2 )

%*****************************************************************************80
%
%% SANDIA_SGMGG_COEF_INC2 computes tentative coefficient changes.
%
%  Discussion:
%
%    An active set S1 of N1 sparse grid indices is given, each of
%    size M.
%
%    The coefficient C1 of each sparse grid index is also given.
%
%    A candidate sparse grid index S2 is provided.
%
%    This function determines the N+1 coefficients that would be
%    appropriate if the candidate S2 was added to the active set
%    as the (N+1)-st item.
%
%  Example:
%
%    Input:
%
%      +1 * {0,2}
%      -1 * {0,1}  +1 * {1,1}
%                  -1 * {1,0}  +1 * {2,0}
%
%    Add {3,0}
%
%    Output:
%
%      +1 * {0,2}
%      -1 * {0,1}  +1 * {1,1}
%                  -1 * {1,0}   0 * {2,0}  +1 * {3,0}
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the dimension of the vector.
%
%    Input, integer N1, the number of points in the active set.
%
%    Input, integer S1(M,N1), the indices for the active set.
%
%    Input, integer C1(N1), the coefficients for the active set.
%
%    Input, integer S2(M), the indices for the candidate.
%
%    Output, integer C3(N1+1), the coefficients for the active set
%    plus the candidate.
%

%
%  During the calculation, we may try to update coefficients of inactive
%  index sets.  By the end of the calculation, all these inactive index
%  sets should have accumulated total coefficients of 0 again.  As a check,
%  we temporarily set aside space for these objects, and check, at the end,
%  that the coefficients are zero.
%
  n4 = 0;
  c4 = zeros(n1,1);
  s4 = zeros(m,n1);
%
%  Copy C1.
%
  c3(1:n1) = c1(1:n1);
  c3(n1+1) = 1;
%
%  Consider the effect of the new item S2 on each of the current
%  items in the active set S1.
%
  for j = 1 : n1
%
%  Determine S, the element-wise minimum of the J-th item in S1 versus S2.
%
    k = j;

    for i = 1 : m

      if ( s2(i) < s1(i,j) )
        s(i) = s2(i);
        k = - 1;
      else
        s(i) = s1(i,j);
      end

    end
%
%  If S = S1(*,J), K is J.
%
    if ( k ~= -1 )

      c3(k) = c3(k) - c1(j);
%
%  If S is equal to an element of the active set, we set K to that index.
%
    else

      for j2 = 1 : n1

        k = j2;

        for i2 = 1 : m

          if ( s1(i2,j2) ~= s(i2) )
            k = - 1;
            break
          end

        end

        if ( k ~= - 1 )
          c3(k) = c3(k) - c1(j);
          break
        end

      end

    end
%
%  If S is equal to an element of the inactive set, set K to that index.
%
    if ( k == -1 )

      for j2 = 1 : n4

        k = j2;

        for i2 = 1 : m

          if ( s4(i2,j2) ~= s(i2) )
            k = - 1;
            break
          end

        end

        if ( k ~= - 1 )
          c4(k) = c4(k) - c1(j);
          break
        end

      end

    end
%
%  S is not equal to S1(*,J), or any element of S1, or any element of S4.
%  Add S to the set of elements S4.
%
    if ( k == -1 )
      n4 = n4 + 1;
      k = n4;
      c4(k) = 0;
      s4(1:m,k) = s(1:m);
      c4(k) = c4(k) - c1(j);
    end

  end
%
%  At the end, the C4(1:N4) should all be zero.
%
  if ( any ( c4(1:n4) ~= 0 ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SANDIA_SGMGG_COEF_INC2 - Fatal error!\n' );
    fprintf ( 1, '  Some inactive indices were assigned a nonzero coefficient.\n' );
    i4mat_transpose_print ( m, n4, s4, '  S4:' );
    i4vec_print ( n4, c4, '  C4:' );
    error ( 'SANDIA_SGMGG_COEF_INC2 - Fatal error!' );
  end

  return
end


% RSPFUNC - sub-function used by RSGET
%
% Usage: res = rspfunc(pval, l, rval)
%
% Input:
%   pval - p-value to optimize
%   l    - [l1 l2 l3 l4] l-values for Ramberg-Schmeiser distribution
%   rval - expected r-value
%
% Output:
%   res  - residual
%
% Author: Arnaud Delorme, SCCN, 2003
%
% See also: RSGET
%
% Reference: Ramberg, J.S., Tadikamalla, P.R., Dudewicz E.J., Mykkytka, E.F.
%            A probability distribution and its uses in fitting data.
%            Technimetrics, 1979, 21: 201-214.

% Copyright (C) 2003 Arnaud Delorme, SCCN, 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 rp = rspfunc( pval, l, rval);

    % for fiting rp with fminsearch
    % -----------------------------
    rp   = l(1) + (pval.^l(3) - (1-pval).^l(4))/l(2);
    rp   = abs(rval-rp);


function f = abs( f )
%ABS Absolute value of a BALLFUN.
%   ABS(F) is the absolute value of the BALLFUN F. This function does not work
%   if the function passes through or becomes numerically close to zero.

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

if ( isempty(f) )
    return
end

if ( f.isReal )
    % Positive/negative test.
    % Returns TRUE if there is no sign change.

    tol = chebfun2eps;
    bool = false;                  % Assume false

    % Evaluate on a grid:
    X = sample(f);

    X = X(:);

    if ( all( X >= -tol * f.vscale ))   % If all values are nonnegative
        bool = true;
    elseif ( all( X <= tol * f.vscale))  % If all values are not positive
        bool = true;
    end
    if ( ~bool )
        error('CHEBFUN:BALLFUN:abs:notSmooth', ...
            'Sign change detected. Unable to represent the result.');
    end
end

% Still call the constructor in case we missed a change of sign.
f = compose( f, @abs );
end


function varargout = acsch(varargin)

switch class(varargin{1})

    case 'sdpvar'
        varargout{1} = InstantiateElementWise(mfilename,varargin{:});

    case 'char'

        operator = CreateBasicOperator('callback');
        operator.monotonicity = @(xL,xU)decreasing_except_at(xL,xU,0);
        operator.derivative = @(x)(-1./(abs(x).*sqrt(1 + x.^2)));
        operator.singularity = [0 -inf inf];
        operator.inflection = [-inf -1 0 1];

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error(['SDPVAR/' upper(mfilename) ' called with weird argument']);
end


function plotIntCon(prob,data)
%PLOTINTCON Plot Integer Constraints on the current figure
%   plotIntCon(prob)

%   Copyright (C) 2011 Jonathan Currie (IPL)

%Check we have integer variables in this plot
if(all(prob.int.str(data.idx) == 'C'))
    return; %nothing to do!
end

%Colours
intC = [0 114/255 189/255]; %feasible
intIC = [123/255 142/255 147/255]; %infeasible

xl = round(data.xl); yl = round(data.yl);

hold on;

%Get Problem Components
if(~isempty(prob.rl))
    [A,b,Aeq,beq] = row2gen(prob.A,prob.rl,prob.ru);
else
    A = prob.A; b = prob.b;
    Aeq = prob.Aeq; beq = prob.beq;
end
Q = prob.Q; l = prob.l; qrl = prob.qrl; qru = prob.qru;
if(isempty(prob.lb)), lb = -Inf; else lb = prob.lb; end
if(isempty(prob.ub)), ub = Inf; else ub = prob.ub; end

%Check for 1D problem
if(prob.sizes.ndec == 1)
    if(prob.int.ind(1) == -1)
        X = 0:1; Y = [];
    else
        X = (floor(xl(1))+1):(ceil(xl(2))-1); Y = [];
    end
else
    %Generate Grid and check for binary constraints
    if(prob.int.ind(1) == -1)
        x = 0:1;
    else
        try
            x = (floor(xl(1))+1):(ceil(xl(2))-1);
        catch ME
            optiwarn('opti:plotint',['Could not plot integer constraints due to plotting error: ' ME.message]);
            return;
        end
    end
    if(prob.int.ind(2) == -1)
        y = 0:1;
    else
        try
            y = (floor(yl(1))+1):(ceil(yl(2))-1);
        catch ME
            optiwarn('opti:plotint',['Could not plot integer constraints due to plotting error: ' ME.message]);
            return;
        end
    end
    [X,Y] = meshgrid(x,y);
end

%Check for row nonlinear constraints
if(~isempty(prob.cl))
    prob = nrow2mix(prob,0,false);
end

%Work out feasible integer points
idx = zeros(numel(X),1); Xv = X(:); Yv = Y(:);
for i=1:numel(X)
    if(isempty(Yv))
        xy = Xv(i);
    else
        xy = data.fixval;
        xy(data.idx) = [Xv(i), Yv(i)]';
    end
    if(~isempty(prob.A))
        idx(i) = all(b >= A*xy) && all(xy >= lb) && all(xy <= ub);
    else
        idx(i) = all(xy >= lb) && all(xy <= ub);
    end
    if(~isempty(Aeq))
        idx(i) = idx(i) && (abs(Aeq*xy-beq) < 1e-6);
    end
    if(~isempty(Q))
        for n = 1:prob.sizes.nqc
            if(iscell(Q))
                nQ = Q{n}; nl = l(:,n); nrl = qrl(n); nru = qru(n);
            else
                nQ = Q; nl = l; nrl = qrl; nru = qru;
            end
            if(~isinf(nru))
                qu = all(xy'*nQ*xy + nl'*xy <= nru);
            else
                qu = true;
            end
            if(~isinf(nrl))
                ql = all(xy'*-nQ*xy - nl'*xy <= -nrl);
            else
                ql = true;
            end
            idx(i) = idx(i) && ql && qu;
        end
    end
    if(~isempty(prob.nle))
        vals = prob.nlcon(xy);
        leI = find(prob.nle == -1);
        geI = find(prob.nle == 1);
        eqI = find(prob.nle == 0);
        nlrhs = prob.nlrhs;
        idx(i) = idx(i) && all(vals(leI) <= nlrhs(leI)) && all(vals(geI) >= nlrhs(geI)) && all(abs(vals(eqI)-nlrhs(eqI)) < 1e-6);
    end
end
%Plot points
idx = logical(idx);
if(isempty(Y))
    Y = zeros(size(X));
    for i = 1:length(Y)
        Y(i) = prob.objective(X(i));
    end
end
plot(X(idx),Y(idx),'o','color',intC,'markerface',intC,'markersize',5);
plot(X(~idx),Y(~idx),'o','color',intIC,'markersize',5);

hold off;

end


%compute the xcoordinate of the head in the central coordinate system

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

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
xcentralhead_mm=cell(1,numlarvae);

for i=1:numlarvae
    larva=larvae(i);
    xcentralhead_mm{1,i}=trx(larva).xhead_mm-trx(larva).xcentral_mm;
end

units=parseunits('mm');
data=xcentralhead_mm;


function DrawAllJoints(j)
global uLINK
radius    = 0.02;
len       = 0.06;
joint_col = 0;

if j ~= 0
    if ~isempty(uLINK(j).vertex)
        vert = uLINK(j).R * uLINK(j).vertex;
        for k = 1:3
            vert(k,:) = vert(k,:) + uLINK(j).p(k); % adding x,y,z to all vertex
        end
        DrawPolygon(vert, uLINK(j).face,0);
    end

    hold on

    i = uLINK(j).mother;
    if i ~= 0
        Connect3D(uLINK(i).p,uLINK(j).p,'k',2);
    end
    DrawCylinder(uLINK(j).p, uLINK(j).R * uLINK(j).a, radius,len, joint_col);

    DrawAllJoints(uLINK(j).child);
    DrawAllJoints(uLINK(j).sister);
end


function [ precision, recall, PR_curve ] = evaluateDetectionCNN( objects, nElems )
%EVALUATEODCNN Evaluates the result obtained by the ObjectDetector-CNN
% w.r.t. a given intersection over union (IoU) value.

    min_conf_per = 0:0.02:1;

    %% Get labels and scores for each TP sample
    [ all_TPs, all_FPs, TP_, FP_ ] = getTPs(objects, nElems);

    %% Only continue if we have any detection
    nDetections = TP_+FP_;
    if(nDetections > 0)
        %% Sort detection confidence scores
        sorted_scores = [];
        if(TP_ > 0)
            sorted_scores = [sorted_scores; all_TPs(:,4)];
        end
        if(FP_ > 0)
            sorted_scores = [sorted_scores; all_FPs(:,4)];
        end
        sorted_scores = sort(sorted_scores, 'descend');

        %% Build precision-recall curve
        rec = zeros(1,length(min_conf_per));
        prec = zeros(1,length(min_conf_per));
        count = 1;
        for conf_per = min_conf_per

            % Only evaluate then score is >= score on current percentage position
            pos_min_score = max(round(nDetections*conf_per), 1);
            min_score = sorted_scores(pos_min_score);
            [TP, FP, FN] = measuresByConfidence(objects, min_score, all_TPs, all_FPs, TP_, FP_);

            if(conf_per == 1)
                % precision = 1 - frac_NoObj;
                % recall = detection_rate;

                precision = TP / (TP + FP); % how many are correct from all that it found?
                recall = TP / (TP + FN); % how many did it find from the total?
            end

            prec(count) = TP / (TP + FP);
            rec(count) = TP / (TP + FN);
            count = count+1;
        end
    else
        precision = 0;
        recall = 0;
        rec = zeros(1,length(min_conf_per));
        prec = zeros(1,length(min_conf_per));
    end

    PR_curve.precision = prec;
    PR_curve.recall = rec;
end


function result = cvMcd(data,kmax,resMCD,h)

%CVMCD calculates the robust cross-validated PRESS (predicted residual error sum of squares)
% curve for the MCD method in a fast way.
%
% Input arguments:
%   data   : the full data set
%   kmax   : the maximal number of components to be considered (mostly kmax = p).
%   resMCD : the result of mcdcov(data,'plots',0,'factor',1)
%   h      : the quantile used in MCD.
%
% output:
%   result.press     : vector of length kmax with the press values
%   result.weights   : the weights for all observations
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by Sanne Engelen
% Last Update: 01/07/2004

% Some initialisations:
n = size(data,1);
p = size(data,2);
r = rank(data);
Pk = [];
Lk = [];
teller_if_lus = 0;

if nargin < 4
    alfa = 0.75;
    h=floor(2*floor((n+p+1)/2)-n+2*(n-floor((n+p+1)/2))*alfa);
end

outWeights = weightscvMcd(data,r,kmax,resMCD,h);

w_min = outWeights.w_min;

Hopt = resMCD.Hsubsets.Hopt;
inputH0.H0 = Hopt;
Tfull = mean(data(Hopt,:));
Sfull = cov(data(Hopt,:));

for i = 1:n
    % deciding which index should be removed from H0.
    inputH0.same = 0;
    if isempty(find(inputH0.H0 == i))
        inputH0.j = h;
        if teller_if_lus >= 1
            inputH0.same = 1;
        end
        teller_if_lus = teller_if_lus + 1;
    else
        inputH0.j = find(inputH0.H0 == i);
    end

    % assigning the input variables:
    inputFull.T = Tfull;
    inputFull.S = Sfull;

    if ~inputH0.same
        res = removeObsMcd(data,i,inputH0,inputFull);
    end
    if (isempty(find(inputH0.H0 == i))) & (teller_if_lus == 1)
        resfixed = res;
    end
    if isempty(find(inputH0.H0 == i)) & (teller_if_lus ~= 1)
        res = resfixed;
    end

    P_min_i = res.P_min_i;
    L_min_i = res.L_min_i;
    mu_min_i = res.mu_min_i;

     for k = 1:kmax
        clear Pk Lk;
        Pk = P_min_i(:,1:k);
        Lk = L_min_i(1:k,1:k);
        Xhoedk_min_i(i,(k-1)*p + 1:k*p) = (data(i,:) - mu_min_i)*Pk*Pk' + mu_min_i;

        if k~=r
            odk(i,k) = norm(data(i,:) - Xhoedk_min_i(i,(k-1)*p + 1:k*p));
        else
            odk(i,k) = 0;
        end
    end
end

for k = 1:kmax
    press_min(k) = 1/sum(w_min)*w_min*odk(:,k).^2;
end

result.press = press_min;
result.weights = outWeights;

%----------------------------------------------------------------------------------
function out = weightscvMcd(data,r,kmax,resMCD,h)

% computes the weights used to calculate the robust PRESS values.
%
% input:
%   data   : the whole data
%       r  : the rank of the data
%   kmax   : the maximal number of components to be considered
%   resMCD : the result of mcdcov(data,'plots',0,'factor',1)
%   h      : the number of observations on which the computations are based.
%
% output:
%   out.w_min    : the weights computed by taken the minimum over all k

% Some initialisations:
n = size(data,1);
p = size(data,2);
Pk = [];
Lk = [];
Tik = [];

[P,L] = eig(resMCD.cov);
[L,I] = greatsort(diag(L));
P = P(:,I);

for k = 1:kmax
    Pk = P(:,1:k);
    if h==n
        Lk=L(1:k);
    else
    Lk = chi2inv(h/n,k)/chi2inv(h/n,kmax/2)*L(1:k);% with correction for the factor
    end
    muk = resMCD.center;
    Xhoedk(:,(k-1)*p + 1:k*p) = (data - repmat(muk,n,1))*Pk*Pk' + repmat(muk,n,1);
    Tk = (data - repmat(muk,n,1))*Pk;

    for i =1:n
        % defining the sd for the observation that is left:
        sdk(i,k) = sqrt(libra_mahalanobis(Tk(i,:),zeros(1,k),'cov',diag(Lk)));

        % defining the od for the observation that is left:
        if k~=r
            odk(i,k) = norm(data(i,:) - Xhoedk(i,(k-1)*p + 1:k*p));
        else
            odk(i,k) = 1;
        end
    end

    % defining weights for odk and sdk:
    if k~=r
        [m,s]=unimcd(odk(:,k).^(2/3),h);
        cutoff(k)=sqrt(norminv(0.975,m,s).^3);
        wod(:,k) = (odk(:,k) <= cutoff(k));
    else
        cutoff(k)= 0;
        wod(:,k) = 1;
    end
    wsd(:,k) = (sdk(:,k) <= sqrt(chi2inv(0.975,k)));
end

% determine the weights for every observation:
wk = wsd & wod;

if size(wk,1) == 1 | size(wk,2) == 1
    w_min = wk';
else
    w_min = min(wk');
end

out.w_min = w_min;


function noOffset = removeOffset(readings,refFreq)
%this function removes the offset from frequency readings by calculating
%the mean, subtracting the ref freq by the mean, and then adding the diff
%to the array of readings

offset = refFreq - mean(readings);
noOffset = readings + offset;


function [data] = load_DATA_block( file_pre, output_path, ...
            Nrg_lines_blk, Nrg_cells, AGC_atten_dB, block, UseMATfiles )
%
% This program - reads /loads data for a block
%              - converts to floating point
%              - compansates for the receiver attenuation
% -------------------------------------------------------------------------
% file_pre      : File prefix (path + prefix + block number)
% output_path   : Path where the SAR data MAT files are stored
% Nrg_lines_blk : Number of range lines in a block
% Nrg_cells     : Number of range cells
% AGC_atten_dB  : Nominal attenuation (Rx_AGC_setting) for each line (dB)
% block         : block number
% UseMATfiles   : Data is stored in MAT files
% -------------------------------------------------------------------------
% Created:  Nov 01, 2004  by Kaan Ersahin
% Modified: Nov 29, 2004  by Kaan Ersahin
% Modified: Dec  4, 2004  by Ian Cumming
%           - added single precision option for MATLAB 7
%           - added UseMATfiles option
%           - moved AGC correction to this function
% -------------------------------------------------------------------------

vv        = version;   vers = str2num(vv(1));  % Find MATLAB version

%  Read 4-bit unsigned data as single (MATLAB 7) or double
%  The data is stored as [I Q I Q I Q...] 4-bit words

if UseMATfiles
    % fprintf('\nload_DATA_block:  from MAT file  CDdata%1.0f.mat\n', block )
    disp ' '
    mat_file = strcat( output_path, 'CDdata', num2str(block) )
    load( mat_file )   % The data is already decoded in this case

else   %  Use fread to get all the data and parameters
    % fprintf('\nload_DATA_block:  from binary files\n')

    file_in  = strcat( file_pre, '_data.dat' )
    fid2     = fopen( file_in, 'r' );
    if vers < 7
      data = fread( fid2, [2*Nrg_cells, Nrg_lines_blk], 'ubit4=>double' );
    else
      data = fread( fid2, [2*Nrg_cells, Nrg_lines_blk], 'ubit4=>single' );
    end
    data = data';  %  Arrange the range lines as rows of the matrix

    %  Compensate for packed data format --> convert to signed numbers
    data = 2*( data - 16*(data > 7)) + 1;

    %  Separate the I and Q channels and make a complex array
    data = complex( data(:, 1:2:2*Nrg_cells), data(:, 2:2:2*Nrg_cells) );
    fclose(fid2);
end

% Apply gain correction to the data.  The attenuation varies from 2 to 17
% dB for this CD, so the linear gain factor varies from 1.26 to 7.08.
% If you want to store the decoded data in one byte arrays, use an additional
%  factor of 1.5, so that the maximum abs value is less than 127.
fact = 1.5;
if vers < 7      %  For MATLAB version 6
    linear_gain_factor = double( fact * 10.^(AGC_atten_dB/20) );
    ones_array         = ones(1,Nrg_cells);
    data = (linear_gain_factor * ones_array) .* double(data);
else             %  For MATLAB version 7
    linear_gain_factor = single( fact * 10.^(AGC_atten_dB/20) );
    ones_array         = single(ones(1,Nrg_cells));
    data = (linear_gain_factor * ones_array) .* single(data);
end


function time_skew = offset_to_timeskew(offset, N, fs)
    time_skew = mod(offset, N) / fs;
end


function net = train(net, tutor, varargin)

% TRAIN
%
% Train a max-win multi-class support vector classifier network using the
% specified tutor to train each component two-class network.
%
%    load data/iris x y;
%
%    C      = 100;
%    kernel = rbf(0.5);
%
%    net = train(maxwin, smosvctutor, x, y, C, kernel);

%
% File        : @maxwin/train.m
%
% Date        : Wednesday 13th September 2000
%
% Author      : Dr Gavin C. Cawley
%
% Description : Gateway function used to train a max-win multi-class support
%               vector classifier network using a given tutor.  Part of an
%               object-oriented implementation of Vapnik's Support Vector
%               Machine, as described in [1].
%
% References  : [1] V.N. Vapnik,
%                   "The Nature of Statistical Learning Theory",
%                   Springer-Verlag, New York, ISBN 0-387-94559-8,
%                   1995.
%
% History     : 13/09/2000 - v1.00
%
% Copyright   : (c) Dr Gavin C. Cawley, September 2000
%
%    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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
%

y = varargin{2};

if size(y, 2) == 1

   for i=1:max(y)

      varargin{2} = 2*(y == i) - 1;

      net.net(i) = train(tutor, varargin{:});

   end

else

   for i=1:size(y, 2)

      varargin{2} = y(:,i);

      net.net(i) = train(tutor, varargin{:});

   end

end

% bye bye...


function tc = tc_deconvolvedAmps(tc);
% tc = tc_deconvolvedAmps(tc);
% compute the peak-bsl amplitudes of deconvolved
% time courses (and relamps, when I implement that):

%%%%%% get params
frameWindow = unique(round(tc.params.timeWindow./tc.TR));
prestim = -1 * frameWindow(1);
peakFrames = unique(round(tc.params.peakPeriod./tc.TR));
bslFrames = unique(round(tc.params.bslPeriod./tc.TR));
peakFrames = find(ismember(frameWindow,peakFrames));
bslFrames = find(ismember(frameWindow,bslFrames));

%%%%% norm baseline periods
nConds = size(tc.glm.betas, 2);

if tc.params.normBsl==1
    offset = mean(tc.glm.betas(bslFrames,:), 1);
    tc.glm.betas = tc.glm.betas - repmat(offset, [length(frameWindow) 1]);
end

%%%%% calc amplitudes
for i = 1:nConds
    bsl = tc.glm.betas(bslFrames, i);
    peak = tc.glm.betas(peakFrames, i);
    tc.glm.amps(:,i) = (nanmean(peak) - nanmean(bsl))';
    tc.glm.amp_sems(:,i) = nanmean(tc.glm.sems(:,i))';
end

return


function mesh = genIntfMesh3D(mesh,intf)

%% USAGE: enrich mesh information for interface problem.
% INPUTS:
% mesh --- nt-by-3 matrix contains p and t information
% intf --- interface function in analytical form
%               g(x,y,z) = 0
%
% OUTPUTS:
% pLoc --- mesh node indices.  possible values:
%               pLoc(i) =-1 if g(xi,yi,zi) < 0
%               pLoc(i) = 0 if g(xi,yi,zi) = 0 (on interface)
%               pLoc(i) = 1 if g(xi,yi,zi) > 0
% tLoc --- mesh element indices.  possible values:
%               tLoc(i) = 1 if the element Ti is in region 1 (g(X) < 0)
%               tLoc(i) = 2 if the element Ti is in region 2 (g(X) > 0)
%               tLoc(i) = -ki if Ti is the ki-th interface element
% intType --- interface element type.  possible values:
%             0 for noninterface element,
%             1 for Type 1: one vertex on one side, three vertices on the other
%             2 for Type 2: two vertice on each side of interface)
% eLoc --- mesh edge indices.  possible values:
%               eLoc(i) = 1 if the element Ti is in region 1 (g(X) < 0)
%               eLoc(i) = 2 if the element Ti is in region 2 (g(X) > 0)
%               eLoc(i) = -ki if Ti is the ki-th interface element
% eIntP --- neI-by-3 vector, stores the coordinate of interface
%                    intersection points on each edge.
%
% Last updated by Xu Zhang on 07/09/20

%% 1. generate pLoc
%% basic assumption:
p = mesh.p; t = mesh.t; e = mesh.e; f = mesh.f;
nt = length(t); ne = length(e); nf = length(f);
pLoc = sign(feval(intf,p(:,1),p(:,2),p(:,3))); pLoc(pLoc == 0) = 1;

%% 2. generate tLoc and intType
id1 = sign(feval(intf,p(t(:,1),1),p(t(:,1),2),p(t(:,1),3)));
id2 = sign(feval(intf,p(t(:,2),1),p(t(:,2),2),p(t(:,2),3)));
id3 = sign(feval(intf,p(t(:,3),1),p(t(:,3),2),p(t(:,3),3)));
id4 = sign(feval(intf,p(t(:,4),1),p(t(:,4),2),p(t(:,4),3)));
id = id1+id2+id3+id4;
idm = id1.*id2.*id3.*id4;
tLoc = zeros(nt,1); intType = zeros(nt,1);
r1 = find(id == -4); tLoc(r1) = 1; intType(r1) = 0;
r2 = find(id == 4);  tLoc(r2) = 2; intType(r2) = 0;
r6 = (id == -1); tLoc(r6) = 1; intType(r6) = 0; % one pt is - and others are zero
r7 = (id == 1); tLoc(r7) = 2;  intType(r7) = 0; % one pt is + and others are zero
r8 = (id == -2 & idm == 0); tLoc(r8) = 1; intType(r8) = 0; % two pts are - and others are zero
r9 = (id == 2 & idm == 0);  tLoc(r9) = 2; intType(r8) = 0; % two pts are + and others are zero
r10 = (id == -3 & idm == 0); tLoc(r10) = 1; intType(r10) = 0; % three pts are - and the other s zero
r11 = (id == 3 & idm == 0);  tLoc(r11) = 2; intType(r11) = 0; % three pts are + and the other s zero
r3 = (id == -2 & idm ~= 0); intType(r3) = 1;
r4 = (id == 2 & idm ~= 0);  intType(r4) = 1;
r5 = (id == 0); intType(r5) = 2;
nt1 = length(r1) + sum(r6) + sum(r8) + sum(r10);
nt2 = length(r2) + sum(r7) + sum(r9) + sum(r11);
ntI = nt-nt1-nt2;
tLoc(tLoc == 0) = -(1:ntI);

%% 3. generate eLoc
eLoc = zeros(ne,1);
ide1 = sign(feval(intf,p(e(:,1),1),p(e(:,1),2),p(e(:,1),3))); ide1(ide1 == 0) = 1;
ide2 = sign(feval(intf,p(e(:,2),1),p(e(:,2),2),p(e(:,2),3))); ide2(ide2 == 0) = 1;
ide = ide1+ide2;
re1 = find(ide == -2); eLoc(re1) = 1;
re2 = find(ide == 2);  eLoc(re2) = 2;
ne1 = length(re1); ne2 = length(re2); neI = ne-ne1-ne2;
eLoc(eLoc == 0) = -(1:neI);

%% 4. generate eIntP
p1 = p(e(eLoc<0,1),:);
p2 = p(e(eLoc<0,2),:);
eIntP = IntersectPoint3D(p1, p2, intf);

%% 5. generate fLoc
fLoc = zeros(nf,1);
fID = sum(pLoc(mesh.f),2);
rf1 = find(fID == -3); fLoc(rf1) = 1;
rf2 = find(fID == 3);  fLoc(rf2) = 2;
nf1 = length(rf1); nf2 = length(rf2); nfI = nf-nf1-nf2;
fLoc(fLoc == 0) = -(1:nfI);

%% Form Mesh
mesh.pLoc = pLoc;
mesh.eLoc = eLoc;
mesh.tLoc = tLoc;
mesh.fLoc = fLoc;
mesh.eIntP = eIntP;
mesh.intType = intType;


function [] = chirpDetector(audioFile)

[b a]=butter(3,[25000./150000 75000./150000],'bandpass'); % create filter for chirp range
c=1;

for i = 1:150000:length(y)
   spectrogram(double(y(i:i+150000)),1000,[],[],Fs,'yaxis');
   pause(.1)

   meanPower(c) = mean(filtfilt(b,a,double(y(i:i+150000))));
   stdPower(c) = std(filtfilt(b,a,double(y(i:i+150000))));
   c=c+1;
end

end


N1=2^16; % we are working with 2 seconds frames.

N2=2^12;
db_path='envsounds/solosDbdb/o2_scat/';
T=2^12; %128 ms for fs= 32 KHz
order=2;
Q1=1;
Q2=1;

%set the filters options
filt1_opt.filter_type = {'gabor_1d','morlet_1d'};
filt1_opt.Q = [Q1 Q2];
filt1_opt.J = T_to_J(T,filt1_opt);

% set the scattering options
sc1_opt.M=2;
sc1_opt.use_abs=1; %used in wavelet_1d, preparing for renormalization with conv(abs(x),phi)

%Create the renormalization operator
Wop1=normalize_wavelet_factory_1d(N1,filt1_opt,sc1_opt);

scatt_fun1 = @(x)(scat(x,Wop1));
feature_fun1={@(x)(format_scat(aggregate_scat(scatt_fun1(x),T)))};
[~,meta]=feature_fun1{1}(rand(N1,1));

src=instsnds_src_multi_obj('envsounds/sDbCoVl',N1);

prep_opt.parallel=1;
prep_opt.average=1;
prep_opt.sig_normalize=1;

db1=prepare_inst_db(src,feature_fun1,prep_opt);
db1.features=single(db1.features);

sigmas=threshold_parameters_factory(db1,meta,'median');
save([db_path 'sigmas_sDbCoVl_T12_Q11.mat'],'sigmas');

%%%sigmas=load('sigmas_sDbCoVl_T12_Q11.mat'); sigmas=sigmas.sigmas;
clear db1;

thresholds=[0 1e-5 1e-4 1e-3 1e-2 1e-1 1 2 3];

lgth=length(thresholds);

all_recog_rates={};
all_recog=zeros(size(thresholds));

for l=1:lgth

    sc1_opt.threshold=thresholds(l);
    sc1_opt.sigmas=sigmas;
    sc1_

    %Prepare the normalizing standardizing and thresholding operator
    Wop2=normalize_standardize_threshold_wavelet_factory_1d(N1,filt1_opt,sc1_opt);

    scatt_fun2=@(X)(format_scat(scat(X,Wop2)));

    feature_fun={@(X)(scatt_fun2(X))};

    [~ ,meta]=feature_fun{1}(rand(N1,1));

    db=prepare_inst_db(src,feature_fun,prep_opt);
    db.features=single(db.features);

    %%Remove silent frames;
%     if ~isfield(db.src,'rejects')
%         db.src=filter_src(db.src);
%     end

    db=poor_whiten2(db);

    db=svm_calc_kernel(db,'gaussian','square',1:size(db.features,2));

    partitions = load('sDbCoVl_fparts.mat');

    files_train=partitions.files_train;
    files_test=partitions.files_test;

    [train_set ,test_set]= create_filt_feats_parts(db.src, files_train,files_test,[],[]);

    optt.kernel_type = 'gaussian';

    optt.C=2^16;
    optt.gamma=2^(-8);

    % The data is unbalanced, use weights
    optt.w=1;

    labels={};
    feature_lbls={};
    votes={};

    C=optt.C;
    gamma=optt.gamma;

    optt1 = optt;

    model = svm_train(db,train_set,optt1,db_weights);
    [labels votes feature_lbls] = svm_test(db,model,test_set);

    [recog recog_rates]=classif_recog(labels,test_set,db.src)
    err = 1-recog;

    all_recog_rates{l}=recog_rates;
    all_recog(l)=recog;
end

save('all_recog_rates_sDbCoVl.mat','all_recog_rates','all_recog','thresholds');
sendmail('michelkapoko@gmail.com','take this best cf','check',{'all_recog_rates_sDbCoVl.mat'});


function [X_p] = gmm_project(X, mapping)
[N,B] = size(X);
X_p = (X - repmat(mapping.mean,N,1)) * mapping.M;
end


filename = 'IrrHexagon50x25x50';
ptype = 'MICRO';
method = 'SIMP_P3';
materialType = 'ISOTROPIC';
initial_case = 'circleInclusion';
cost = {'chomog_fraction';'perimeter'};
weights = [1 0.1];
constraint = {'volumeConstraint'};
constraint_case = 'INEQUALITY';
optimizer = 'MMA';

incrementFactor = 1;
designVariable = 'Density';
filterType = 'P1';
fracRadius = 0.5;

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

Vfrac_initial = 0.5;
optimality_initial = 1e-3;
constr_initial = 1e-3;
Perimeter_target = 5;

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

 %Micro
target_parameters.epsilon_isotropy = 1e-3;%%%%%%
epsilon_isotropy_final=target_parameters.epsilon_isotropy;
epsilon_isotropy_initial=1e-1;
micro.alpha =[1 0 0]';%[1 0 0]'
micro.beta =[0 -1 0]';%[0 -1 0]'

% For all tests
plotting = true;
printing = true;
printing_physics = true;
monitoring = true;
maxiter = 3;


function [gr, diams, vols] = imGranulo(img, granuloType, strelShape, strelSizes, varargin)
% Compute granulometry curve of a given image.
%
%   GR = imGranulo(IMG, GRTYPE, STRELSHAPE, SIZES)
%   Computes the granulometry curve for the input image IMG, using the
%   operation GRTYPE, the shape of structuring element given by STRELSHAPE,
%   and the list of structuring element sizes given by SIZES.
%   IMG should be a 2D image (binary or grayscale)
%   GRTYPE can be one of {'opening', 'closing', 'erosion', 'dilation'}.
%   STRELSHAPE can be one of {'square', 'octagon', 'diamond', 'disk',
%     'lineh', 'linev'}.
%   SIZES are given as radius. Diameters of strels are obtained as 2*R+1.
%   The result GR is a 1-by-N array with as many columns as the number of
%     elements provided in SIZES array.
%
%   [GR, DIAMS] = imGranulo(...)
%   Also returns the diameters of the structuring elements for each step of
%   the analysis. DIAMS is 1-by-N array with the same number of columns as
%   the GR array.
%
%   [GR, DIAMS, VOLS] = imGranulo(...)
%   Also returns the array of volume curves, corresponding to the sum of
%   gray levels at each step of the granulometry.
%
%   ... = imGranulo(..., 'verbose', TF)
%   If TF is true, displays the iteration steps on the command window.
%
%
%   Example
%     % Compute granulometric curve by opening on coins image (with two
%     % distinct sizes), using disk structuring element
%     img = imread('coins.png');
%     Rmax = 50;
%     gr = imGranulo(img, 'opening', 'disk', 1:Rmax);
%     % display as a function of strel diameter
%     figure; plot(2*(1:Rmax)+1, gr); xlim([0 2*Rmax]);
%     xlabel('Strel diameter (pixel)'); ylabel('Percentage of Variations');
%     title('Granulometry on "coins" image');
%
%   See also
%     imGranulometry, granuloMeanSize, imGranuloByRegion
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2014-05-05,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2014 INRAE - Cepia Software Platform.

%% Parse input arguments

% check ROI
roi = true(size(img));
if ~isempty(varargin)
    var1 = varargin{1};
    if islogical(var1) && all(size(var1) == size(img))
        roi = var1;
        varargin(1) = [];
    end
end

% check additional input arguments
verbose = false;
while length(varargin) > 1
    switch lower(varargin{1})
        case 'verbose', verbose = varargin{2};
        otherwise
            error(['Can not understand option: ' varargin{1}]);
    end
    varargin(1:2) = [];
end

% create structuring element factory, as a function handle that returns a
% strel object (or an array) from the radius of the strel.
if isa(strelShape, 'functionHandle')
    strelMaker = strelShape;
else
    % create factory from strel name
    switch lower(strelShape)
        case 'square'
            strelMaker = @(r) strel('square', r);
        case {'octagon', 'diamond'}
            strelMaker = @(r) strel(lower(strelShape), r);
        case 'disk'
            % do not use simplification, as it is not suitable for
            % granulometries
            strelMaker = @(r) strel('disk', r, 0);
        case 'lineh'
            strelMaker = @(r) ones(1, 2*r+1);
        case 'linev'
            strelMaker = @(r) ones(2*r+1, 1);
        otherwise
            error(['matImage:' mfilename], ...
                ['Could not process strel type: ' strelShape]);
    end
end

% create funtion handle for morphological operation
if isa(granuloType, 'functionHandle')
    morphoOp = granuloType;
else
    switch lower(granuloType)
        case 'opening'
            morphoOp = @imopen;
        case 'closing'
            morphoOp = @imclose;
        case 'dilation'
            morphoOp = @imdilate;
        case 'erosion'
            morphoOp = @imerode;

        otherwise
            error(['matImage:' mfilename], ...
                ['Could not process granulometry type: ' granuloType]);
    end
end

%% Initialize

% number of structuring element sizes
nSizes = length(strelSizes);

% compute the associated diameter list (for output)
diams = 2 * strelSizes + 1;

% allocate memory for volume curves (sum of gray levels)
vols = zeros(1, nSizes + 1);

% initialize reference volume
vol0 = sum(img(roi));
vols(1) = vol0;

%% Iteration over structuring element sizes

% iterate
for i = 1:nSizes
    % size of strel for current iteration
    radius = strelSizes(i);

    if verbose
        fprintf('iter %2d/%d, radius = %f\n', i, nSizes, radius);
    end

    % create current strel
    se = strelMaker(radius);

    % compute morphological operation
    img2 = morphoOp(img, se);

    % sum of gray levels within each region
    vols(i+1) = sum(img2(roi));
end

% compute granulometry curve
vols2 = (vols - vol0) / (vols(end) - vol0);
gr = 100 * diff(vols2);


prior = GaussianStateX([0; 0.04979784; 0;-5], diag([10, 1, 10, 1]));


% The COBRAToolbox: testFindExtremePathway.m
%
% Purpose:
%     - testFindExtremePathway tests the functionality of findExtremePathway.
%
% Authors:
%     - Sylvain Arreckx March 2017
%     - Laurent Heirendt March 2017
%
% Test problem from
%     "System Modeling in Cellular Biology: From Concepts to Nuts and Bolts",
%     section "Stoichiometric and Constraint Based Modeling", MIT Press

% save the current path
currentDir = pwd;

% initialize the test
fileDir = fileparts(which('testFindExtremePathway'));
cd(fileDir);

model.S = [1, 0, 0, 0,-1,-1,-1, 0, 0, 0
           0, 1, 0, 0, 1, 0, 0,-1,-1, 0
           0, 0, 0, 0, 0, 1, 0, 1, 0,-1
           0, 0, 0, 0, 0, 0, 1, 0, 0,-1
           0, 0, 0,-1, 0, 0, 0, 0, 0, 1
           0, 0,-1, 0, 0, 0, 0, 0, 1, 1];

% define list of solver packages
solverPkgs = {'gurobi', 'glpk'};

obj = [0.5985; 0.4709; 0.6959; 0.6999; 0.6385; 0.0336; 0.0688; 0.3196; 0.5309; 0.6544; 0.4076; 0.8200];

refV = [0.2500; 0; 0.2500; 0; 0.2500; 0; 0; 0; 0.2500; 0];
tol = 1e-14;

for k = 1:length(solverPkgs)

    % print success message
    fprintf('   Testing findExtremePathway using %s ... ', solverPkgs{k});

    % change the solver
    solverOK = changeCobraSolver(solverPkgs{k}, 'LP', 0);

    if solverOK == 1
%
%         % testing findExtremePathway with different arguments
%         if isfield(model, 'revRxns')
%             model = rmfield(model, 'revRxns');
%         end
        %testing missing lb/ub
        try
            v = findExtremePathway(model, obj);
        catch ME
            assert(length(ME.message) > 0)
        end

        model.ub = [-1,  1, -1, -1, -1, -1, -1,  1, -1, -1];
        model.lb = [ 0, -1,  0,  0,  0,  0,  0, -1,  0,  0];
        v = findExtremePathway(model);
        assert(all(abs(model.S * v) < tol))

        v = findExtremePathway(model, obj);
        assert(all(refV == v))
        assert(all(model.S * v == zeros(size(model.S, 1), 1)))

        model2.S = [-1,  0,  1;
                     1, -1,  0;
                     0,  1, -1];

        %solverOK = changeCobraSolver('gurobi', 'LP', 0);

        [x, output] = findExtremePool(model2);

        assert(all(abs(model2.S * x) < tol))

        % delete generated files
        delete('*.ine');
        delete('*.ext');
    end

    % print success message
    fprintf('Done\n');
end

% change the directory
cd(currentDir)


function M = moveLRRVC(F, d)
%------------------------------------------------------------------------------
% Moves gridfunction F in horizontal direction.
% Excess area is filled by Vertex-Centered (VC) Reflection across boundaries.
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: June 23, 2000.
% (c) 1999-2002 Stichting CWI, Amsterdam
%------------------------------------------------------------------------------
M = moveUDRVC(F.', d).';
%------------------------------------------------------------------------------


% The royal roads function. The chromosome length (i.e. len)
% should be a multiple of 8
function fitness=R1(pop)
[popSize len]=size(pop);
fitness=zeros(popSize,1);
for i=1:8:len
    temp=sum(pop(:,i:i+7),2);
    temp=double(temp==8);
    fitness=fitness+temp*8;
end
fitness=fitness';


% this script tests 1st Order Statistics features between CERR and pyradiomics.
%
% RKP, 03/22/2018

firstOrderParamFileName = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing','tests_for_cerr','test_first_order_radiomics_extraction_settings.json');
cerrFileName = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing','data_for_cerr_tests','CERR_plans','head_neck_ex1_20may03.mat.bz2');

planC = loadPlanC(cerrFileName,tempdir);
indexS = planC{end};

paramS = getRadiomicsParamTemplate(firstOrderParamFileName);
strNum = getMatchingIndex(paramS.structuresC{1},{planC{indexS.structures}.structureName});
scanNum = getStructureAssociatedScan(strNum,planC);

%% Calculate features using CERR

firstOrderS = calcGlobalRadiomicsFeatures...
            (scanNum, strNum, paramS, planC);
firstOrderS = firstOrderS.Original.firstOrderS;
cerrFirstOrderV = [firstOrderS.energy, firstOrderS.totalEnergy, firstOrderS.interQuartileRange, ...
    firstOrderS.kurtosis+3, firstOrderS.max, firstOrderS.mean, firstOrderS.meanAbsDev, ...
    firstOrderS.median, firstOrderS.medianAbsDev, firstOrderS.min, ...
    firstOrderS.P10, firstOrderS.P90, firstOrderS.interQuartileRange, ...
    firstOrderS.robustMeanAbsDev, firstOrderS.rms, firstOrderS.skewness, ...
    firstOrderS.std, firstOrderS.var, firstOrderS.entropy];

%% Calculate features using pyradiomics

testM = single(planC{indexS.scan}(scanNum).scanArray) - ...
    single(planC{indexS.scan}(scanNum).scanInfo(1).CTOffset);
mask3M = zeros(size(testM),'logical');
[rasterSegments, planC, isError] = getRasterSegments(strNum,planC);
[maskBoundBox3M, uniqueSlices] = rasterToMask(rasterSegments, scanNum, planC);
mask3M(:,:,uniqueSlices) = maskBoundBox3M;

scanType = 'original';

dx = planC{indexS.scan}(scanNum).scanInfo(1).grid1Units;
dy = planC{indexS.scan}(scanNum).scanInfo(1).grid1Units;
dz = mode(diff([planC{indexS.scan}(scanNum).scanInfo(:).zValue]));
pixelSize = [dx dy dz]*10;

teststruct = PyradWrapper(testM, mask3M, pixelSize, scanType, dirString);

pyradFirstorderNamC = {'Energy', 'TotalEnergy','InterquartileRange','Kurtosis',...
    'Maximum', 'Mean','MeanAbsoluteDeviation','Median','medianAbsDev',...
    'Minimum','10Percentile','90Percentile','InterquartileRange',...
    'RobustMeanAbsoluteDeviation','RootMeanSquared','Skewness',...
    'StandardDeviation','Variance','Entropy'};

pyradFirstorderNamC = strcat(['original', '_firstorder_'],pyradFirstorderNamC);

pyRadFirstOrderV = [];
for i = 1:length(pyradFirstorderNamC)
    if isfield(teststruct,pyradFirstorderNamC{i})
        pyRadFirstOrderV(i) = teststruct.(pyradFirstorderNamC{i});
    else
        pyRadFirstOrderV(i) = NaN;
    end
end

diffFirstOrderV = (cerrFirstOrderV - pyRadFirstOrderV) ./ cerrFirstOrderV * 100


%compute yinflection
function [data,units]=compute_yinflection_mm(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
yinflection_mm=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    sz = size(trx(larva).yspine_mm);
    yinflection_mm{1,i} = trx(larva).yspine_mm(sub2ind(sz,trx(larva).inflectionpointdistance,1:sz(2)));
%
%     yinflection_mm{1,i}=zeros(1,size(trx(larva).yspine_mm,2));
%     for j=1:size(trx(larva).yspine_mm,2)
%     yinflection_mm{1,i}(j)=trx(larva).yspine_mm(trx(larva).inflectionpointdistance(j),j);
%     end
end

units=parseunits('mm');
data=yinflection_mm;


function [ iq_signal ] = read_file_iq( filename )
%read_file_tdoa reads a file with IQ data (e.g. from rtl_sdr)

    disp('read_file_iq');

    % daten vom 1. RX
    disp(['IQ read from data file = ' filename]);
    fileID = fopen(filename);
    a = fread(fileID);
    fclose(fileID);

    inphase1 = a(1:2:end) -128;
    quadrature1 = a(2:2:end) -128;
    disp(['successfully read ' int2str(length(inphase1)) ' samples']);

    % complex representation
    iq_signal = inphase1 + 1i.*quadrature1;

end


% Genetic Algorithm Toolbox.
% Version 1.2 15-Apr-94
% Department of Automatic Control and Systems Engineering
% University of Sheffield, England
%
% Creating populations
%   crtbase	- create a base vector
%   crtbp	- create a binary population
%   crtrp	- create a real-valued population
%
% Fitness assignment
%   ranking	- rank-based fitness assignment
%   scaling	- proportional fitness-scaling
%
% Selection and reinsertion
%   reins	- uniform random and fitness-based reinsertion
%   rws		- roulette wheel selection
%   select	- high-level selection routine
%   sues		- stochastic universal sampling
%
% Mutation operators
%   mut		- discrete mutation
%   mutate	- high-level mutation function
%   mutbga	- real-value mutation
%
% Crossover operators
%   recdis	- discrete recombination
%   recint	- intermediate recombination
%   reclin	- line recombination
%   recmut	- line recombination with mutation features
%   recombin	- high-level recombination function
%   xovdp	- double-point crossover
%   xovdprs	- double-point reduced surrogate crossover
%   xovmp	- general multi-point crossover
%   xovsh	- shuffle crossover
%   xovshrs	- shuffle reduced surrogate crossover
%   xovsp	- single-point crossover
%   xovsprs	- single-point reduced surrogate crossover
%
% Subpopulation support
%   migrate	- exchange individuals between subpopulations
%
%
% Utility functions
%   bs2rv	- binary string to real-value conversion
%   rep		- matrix replication
%
% Demonstration and other functions
%   mpga	- multi-population genetic algorithm demonstration
%   objfun1	- De Jongs first test function (used by sga)
%   objharv	- harvest function (used in mpga)
%   resplot	- result plotting (used in mpga)
%   sga		- simple genetic algorithm demonstration


%----------------------------------------------------------------
% Depth Map Super-Resolution by Deep Multi-Scale Guidance, ECCV16
%                     written by T.-W. HUI
%---------------------------------------------------------------
clear all;
close all;
clc;

%% Settings
dataDir = '/path/to/training_data/';
saveDir = '/path/to/h5/';

chunksz = 128;

scale = 2;
size_dataY = 40;
size_dataD = 20;
size_label = 40;
border_dataY = 1;
border_label = 1;
stride = 22;
data_aug = true;
index_val = [1 20 28 58 64 66 69 73 75 79];

%% Initialization
dataY = zeros(size_dataY, size_dataY, 1, 1);
dataD = zeros(size_dataD, size_dataD, 1, 1);
label = zeros(size_label, size_label, 1, 1);

size_subIm = size_dataD*scale;

pad_dataY = (size_subIm - size_dataY)/2;
pad_label = (size_subIm - size_label)/2;
assert(pad_dataY >= 0, 'pad_dataY cannot be an negative number!');
assert(pad_label >= 0, 'pad_label cannot be an negative number!');

count = 0;

%% Generate training  data
load([dataDir '/RGB']);
load([dataDir '/Df']);

index_train = setdiff(1:numel(Df), index_val);
fprintf('%d images is used for training.\n', numel(index_train));

RGB = RGB(index_train);
Df  = Df(index_train);

h = ones(3,3)/9;

for k = 1 : size(Df,1)

    fprintf('Processing %d of %d, id: %d, ', k, size(Df,1), index_train(k));

    im_Dh = modcrop(Df{k}, scale);
    im_M  = im_Dh > 0;
    im_Dh = normalizeIm(im_Dh, [0 1]);
    im_Dl = imresize(im_Dh, 1/scale, 'bicubic');
    im_Dl_LF = imfilter(im_Dl, h, 'symmetric');
    im_label = im_Dh - imresize(im_Dl_LF, scale, 'bicubic');
    [H, W] = size(im_label);

    I = RGB{k};
    I = rgb2ycbcr(I);
    Y = double(I(:,:,1));
    im_Y = modcrop(Y, scale);
    im_Y = normalize_cleanIm(im_Y);
    im_Y_LF = imfilter(im_Y, h, 'symmetric');
    im_inputY = normalize_cleanIm(im_Y - im_Y_LF);

    im_grad_Dh = imgradient(im_Dh, 'sobel');

    stride_ = stride;

    if index_train(k) >= 75
        stride_ = floor(2.0*stride_);
    elseif index_train(k) >= 65
        stride_ = floor(1.5*stride_);
    end

    for x = 1 : stride_ : H-size_subIm+1
        for y = 1 :stride_ : W-size_subIm+1

            subIm_M = im_M(x : x+size_subIm-1, y : y+size_subIm-1);

            if sum(sum(subIm_M == 0)) == 0

                subim_dataY = im_inputY(x+pad_dataY : x+pad_dataY+size_dataY-1, y+pad_dataY : y+pad_dataY+size_dataY-1);

                subim_dataD = imresize(im_Dh(x : x+size_subIm-1, y : y+size_subIm-1), 1/scale, 'bicubic');
                subim_dataD = subim_dataD - imfilter(subim_dataD, h, 'symmetric');

                subim_label = im_label(x+pad_label : x+pad_label+size_label-1, y+pad_label : y+pad_label+size_label-1);

                count = count + 1;
                dataY(:, :, 1, count) = subim_dataY;
                dataD(:, :, 1, count) = subim_dataD;
                label(:, :, 1, count) = subim_label;

            end

        end
    end

end

fprintf('---------------------------------------\n')

fprintf('Total: %d patches.\n', count);
num_patches = count;

% Data augmntation
if data_aug
    num_patches = num_patches*2;
    dataY_tmp = dataY; clear dataY;
    dataD_tmp = dataD; clear dataD;
    label_tmp = label; clear label;
    dataY = cat(4, dataY_tmp, rot90(dataY_tmp,1));
    dataD = cat(4, dataD_tmp, rot90(dataD_tmp,1));
    label = cat(4, label_tmp, rot90(label_tmp,1));
end

fprintf('Number of patches used: %d\n', num_patches);

% Remove left-most column and bottom row of dataY and label
dataY = dataY(1:end-border_dataY,1:end-border_dataY,:,:);
label = label(1:end-border_label,1:end-border_label,:,:);

% Re-arrange order
order = randperm(num_patches);
dataY = dataY(:,:,:,order);
dataD = dataD(:,:,:,order);
label = label(:,:,:,order);

%% writing to HDF5
created_flag = false;
totalct = 0;

for batchno = 1 : floor(num_patches/chunksz)
    last_read = (batchno - 1)*chunksz;
    batchdata = dataY(:, :, :, last_read+1 : last_read+chunksz);

    startloc = struct('dat', [1,1,1,totalct+1], 'lab', [1,1,1,totalct+1]);
    curr_dat_sz = store2hdf5_v2(savepathY, '-Y', batchdata, [], [], ~created_flag, startloc, chunksz);
    created_flag = true;
    totalct = curr_dat_sz(end);
end

h5disp(savepathY);

created_flag = false;
totalct = 0;

for batchno = 1 : floor(num_patches/chunksz)
    last_read = (batchno - 1)*chunksz;
    batchdata = dataD(:, :, :, last_read+1 : last_read+chunksz);
    batchlabs = label(:, :, :, last_read+1 : last_read+chunksz);

    startloc = struct('dat', [1,1,1,totalct+1], 'lab', [1,1,1,totalct+1]);
    curr_dat_sz = store2hdf5_v2(savepathD, '-D', batchdata, [], batchlabs, ~created_flag, startloc, chunksz);
    created_flag = true;
    totalct = curr_dat_sz(end);
end

h5disp(savepathD);


% OP_U_OTIMES_N_V_OTIMES_N: assemble the matrix A = [A(i,j)], A(i,j) = (epsilon (u \otimes n)_j, (v \otimes n)_i ).
%
%   A = op_u_otimes_n_v_otimes_n (spu, spv, msh, coeff);
%
% INPUT:
%
%   spu:   structure representing the space of trial functions (see sp_vector/sp_eval_boundary_side)
%   spv:   structure representing the space of test functions (see sp_vector/sp_eval_boundary_side)
%   msh:   structure containing the domain partition and the quadrature rule for the boundary,
%           since it must contain the normal vector (see msh_cartesian/msh_eval_boundary_side)
%   coeff: vector-valued function f, evaluated at the quadrature points
%
% OUTPUT:
%
%   A: assembled matrix
%
% Copyright (C) 2015, Rafael, 2017 Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

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

function varargout = op_u_otimes_n_v_otimes_n (spu, spv, msh, mshv, coeff)

  shpu = reshape (spu.shape_functions, spu.ncomp, msh.nqn, spu.nsh_max, msh.nel);
  shpv = reshape (spv.shape_functions, spv.ncomp, msh.nqn, spv.nsh_max, msh.nel);

  rows = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  cols = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  values = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);

  jacdet_weights = msh.jacdet .* msh.quad_weights .* coeff;

  ncounter = 0;
  for iel = 1:msh.nel
    if (all (msh.jacdet(:, iel)))
      shpu_iel = reshape (shpu(:, :, :, iel), spu.ncomp, msh.nqn, spu.nsh_max);
      shpv_iel = reshape (shpv(:, :, :, iel), spv.ncomp, msh.nqn, spv.nsh_max);

      u_otimes_n_iel = zeros (spu.ncomp, spu.ncomp, msh.nqn, spu.nsh_max);
      v_otimes_n_iel = zeros (spv.ncomp, spv.ncomp, msh.nqn, spv.nsh_max);
% I need the normal from both sides
      normalu_iel = msh.normal (:, :, iel);
      normalv_iel = mshv.normal (:, :, iel);
      for ii = 1:spu.ncomp
        for jj = 1:spu.ncomp
          u_otimes_n_iel(ii,jj,:,:) = bsxfun (@times, shpu_iel(ii,:,:), normalu_iel(jj,:));
          v_otimes_n_iel(ii,jj,:,:) = bsxfun (@times, shpv_iel(ii,:,:), normalv_iel(jj,:));
        end
      end
% Should I permute it, before reshaping?
      u_otimes_n_iel = reshape (u_otimes_n_iel, spu.ncomp*spu.ncomp, msh.nqn, 1, spu.nsh_max);
      v_otimes_n_iel = reshape (v_otimes_n_iel, spv.ncomp*spv.ncomp, msh.nqn, spv.nsh_max, 1);

      jacdet_iel = reshape (jacdet_weights(:,iel), [1, msh.nqn, 1, 1]);
      v_oxn_times_jw = bsxfun (@times, jacdet_iel, v_otimes_n_iel);
      tmp1 = sum (bsxfun (@times, v_oxn_times_jw, u_otimes_n_iel), 1);
      elementary_values = reshape (sum (tmp1, 2), spv.nsh_max, spu.nsh_max);

      [rows_loc, cols_loc] = ndgrid (spv.connectivity(:,iel), spu.connectivity(:,iel));
      indices = rows_loc & cols_loc;
      rows(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = rows_loc(indices);
      cols(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = cols_loc(indices);
      values(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = elementary_values(indices);
      ncounter = ncounter + spu.nsh(iel)*spv.nsh(iel);

    else
      warning ('geopdes:jacdet_zero_at_quad_node', 'op_u_otimes_n_v_otimes_n: singular map in element number %d', iel)
    end
  end

  if (nargout == 1 || nargout == 0)
    varargout{1} = sparse (rows(1:ncounter), cols(1:ncounter), ...
                           values(1:ncounter), spv.ndof, spu.ndof);
  elseif (nargout == 3)
    varargout{1} = rows(1:ncounter);
    varargout{2} = cols(1:ncounter);
    varargout{3} = values(1:ncounter);
  else
    error ('op_u_otimes_n_v_otimes_n: wrong number of output arguments')
  end

end


%% housekeeping
close all
home()
%% Choose a model type: see cell "create the structural VAR model" below
model_type=5;

%% Create dataset
do_plot=true;
scale=100;

[db,varlist]=create_dataset(scale,do_plot);

%% Create the structural VAR model
close()

% first we create a template structure
% ------------------------------------
tpl=svar.template();

% we update the fields of the structure
% --------------------------------------
tpl.endogenous=varlist;
tpl.nlags=2;

% create restrictions on parameters as well as markov chains
%------------------------------------------------------------
switch model_type
    case 0
        % constant-parameter model
        [restrictions,tpl]=create_restrictions_and_markov_chains0(tpl);
    case 1
        % Coefficients are switching regimes across all equations
        % (synchronized case)
        [restrictions,tpl]=create_restrictions_and_markov_chains1(tpl);
    case 2
        % Coefficients and variances have different chains, different
        % regimes, and different durations
        [restrictions,tpl]=create_restrictions_and_markov_chains2(tpl);
    case 3
        % Only coefficients in monetary policy equation are changing
        [restrictions,tpl]=create_restrictions_and_markov_chains3(tpl);
    case 4
        % Only variance in monetary policy equation is changing
        [restrictions,tpl]=create_restrictions_and_markov_chains4(tpl);
    case 5
        % Both coefficients and variances in monetary policy equation
        % change with two independent Markov processes
        [restrictions,tpl]=create_restrictions_and_markov_chains5(tpl);
    case 6
        % Only variances in ALL three equations switch
        [restrictions,tpl]=create_restrictions_and_markov_chains6(tpl);
    otherwise
        error('the coded model types are 0, 1, 2, 3, 4 and 6')
end

% finally we create a svar object by pushing the structure into svar
%--------------------------------------------------------------------
m=svar(tpl,'data',db,'estim_linear_restrictions',restrictions);

%% Find posterior mode

mest=estimate(m,'estim_start_date','1960Q1');

%% Printing out A0hat, A1hat, and A2hat in a form compatible with Zha''s original Matlab code

output_in_original_zha_matlab_code(mest,scale)

% uncomment this if you want to capture output
% [A0hat,A1hat,A2hat]=output_in_original_zha_matlab_code(mest,scale)

%% Markov chain Monte Carlo
% Note that because of the linear restrictions, not all parameters are
% estimated. Hence, the effective number of estimated parameters is smaller
% than the number of parameters declared by the user. This is reflected in
% the dimensions of lb,ub,x0 and SIG below. The user does not have to be
% concerned about those.
[objective,lb,ub,x0,SIG]=pull_objective(mest);

ndraws_mcmc         = 1500;  % number of parameter draws through MCMC.
ndraws_burnin       = floor(0.1*ndraws_mcmc); % number of parameter draws to be burned
mcmc_options=struct('burnin',ndraws_burnin,'N',ndraws_mcmc,'thin',1);

Results=mh_sampler(objective,lb,ub,mcmc_options,x0,SIG);

%% Marginal data density
% pick yours: 'bridge','mhm','mueller','swz','is','ris','cj'
tic
log_mdd = mcmc_mdd(Results.pop,lb,ub,...
    struct('log_post_kern',objective,... % function to MINIMIZE !!!
    'algorithm','swz',... % MDD algorithm
    'L',2000 ... % Number of i.i.d. draws of the proposal density function
));
minutes_MDD_Took = toc/60;

laplace_approx=mest.estimation.posterior_maximization.log_marginal_data_density_laplace;
disp(['log(MDD): Laplace Approximation: ',num2str(laplace_approx)])
disp(['log(MDD): Sims-Waggoner-Zha Approximation: ',num2str(log_mdd)])
disp([' Minutes Sims-Waggoner-Zha Approximation took: ',num2str(minutes_MDD_Took)])

%% Impulse responses
myirfs=irf(mest);

do_plot_irfs(myirfs,mest);

%% do smoothed probabilities

do_plot_smoothed_probabilities(mest)

%% Out-of sample forecasts at the mode

mycast=forecast(mest);

do_plot_unconditional_forecasts(mycast,mest)

%% Conditional forecast on ygap
% conditional information
%-------------------------
ygap=scale*(-0.025:0.005:8*0.005).';
cond_db=struct('ygap',ts('2015Q3',ygap));

% options for the exercise
%--------------------------
myoptions=struct('cbands',[10,20,50,80,90],'do_plot',true,'nsteps',20,...
    'param_uncertainty',false,'shock_uncertainty',true,'ndraws',200);

% do it
%-------
tic
[fkst,bands,hdl]=do_conditional_forecasts(mest,db,cond_db,Results.pop,myoptions);
fprintf('\n\n Computing conditional forecasts took %0.4f minutes\n\n',toc/60);
%% Median conditional forecasts
% we take advantage of the fact that in the bands above we specified 50 in
% the bands above
%--------------------------------------------------------------------------
figure('name','Median Conditional Forecasts');
nvars=mest.endogenous.number;
for ivar=1:nvars
    thisname=mest.endogenous.name{ivar};
    subplot(nvars,1,ivar)
    plot(bands.(thisname)('ci_50'),'linewidth',2)
    title(mest.endogenous.tex_name{ivar})
end


function varargout = resize(csize, varargin)
%RESIZE Resize arguments to specified size.
%
%   [XE, YE, ZE, ...] = RESIZE(S, X, Y, Z, ...) resizes each of the input
%   arguments to the size S.  The output arguments will all have the same
%   size.  The resizing is done by truncation or recycling, as
%   necessary, along each dimension.  For instance,
%
%      resize([1 2], [7 8 9])  returns  [7 8]           % truncation
%      resize([1 3], [7 8 9])  returns  [7 8 9]         % no change
%      resize([1 5], [7 8 9])  returns  [7 8 9 7 8]     % recycling
%      resize([1 5], [7 8])    returns  [7 8 7 8 7]     % recycling
%      resize([1 5], 7)        returns  [7 7 7 7 7]     % recycling
%
%   When the size along the dimension to be replicated is one (the last case
%   above), the recycling degenerates to a duplication of elements.
%
%   When expanding an array X to the size S, the following applies to each
%   dimension I:
%
%     If SIZE(X, I) = S(I), then no change is made.
%
%     If SIZE(X, I) > S(I), then X is truncated along the dimension I to the
%     length S(I).
%
%     If SIZE(X, I) < S(I), then X will be expanded along dimension I by
%     recycling, i.e., X will be replicated along dimension I according to
%     an index vector [ 1 2 ... SIZE(X, I) 1 2 ... ] which has length S(I).
%
%   The special case when SIZE(X, I) == 1 and S(I) > 1 is called `scalar
%   expansion', i.e., X is simply duplicated S(I) times along dimension I.
%
%   See also SIZE, REPMAT.

%   Author:      Peter J. Acklam
%   Time-stamp:  2001-05-14 02:31:18 +0200
%   E-mail:      pjacklam@online.no
%   URL:         http://home.online.no/~pjacklam

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % Check number of input arguments.
   %

   nargsin = nargin;
   if nargsin < 2
      error('Not enough input arguments.');
   end

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % Check the size vector.  Apply the same requirements to the size vector
   % for RESIZE as the size vector for RESHAPE.
   %

   % Make sure it has at least two elements.
   vecsiz = size(csize);
   if prod(vecsiz) < 2
      error('Size vector must have at least two elements.');
   end

   % Make sure it is a row vector.  There must be only one non-singleton
   % dimension and that dimension must be the second.
   k = find(vecsiz ~= 1);               % find non-singleton dimensions
   if length(k) ~= 1 | k ~= 2
      error('Size vector must be a row vector with integer elements.');
   end

   % Make sure it contains only real numbers.
   if ~isnumeric(csize) | ~isreal(csize)
      error('Size vector must contain only real non-negative integers.');
   end

   % Make sure the numbers are positive integers.
   csize = double(csize);
   if any(isnan(csize)) | any(csize < 0) | any(csize ~= round(csize))
      error('Size vector must contain only real non-negative integers.');
   end

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % Get number of dimensions in output arguments.
   %

   k = find(csize ~= 1);        % find non-singleton dimensions
   if isempty(k)
      cdims = 2;                % all arrays have at least two dimensions
   else
      cdims = k(end);           % last non-singleton dimension
      cdims = max(cdims, 2);    % must have at least two dimensions
   end
   csize = csize(1:cdims);      % remove any trailing singleton dimensions

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % Prepare output argument list.  The number of arrays to resize should
   % depend on `nargout', not `nargin', to avoid resizing more arrays than
   % those that will be returned
   %

   nargsout = nargout;

   % When called with no output arguments, return one output argument.
   if nargsout == 0
      nargsout = 1;
   end

   if nargsout > length(varargin)
      error('More output arguments than input arguments specified.');
   end

   % Initialize output argument list.
   varargout = cell(1, nargsout);

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % Define other variables that are used in the loop.
   %

   % number of elements in output array
   nelems = prod(csize);

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % Special case: all output arguments are empty.  Still, we must reshape
   % the input argument to the specified size, rather than simply returning
   % `[]', because the input might not be of class `double' and it might not
   % be `0-by-0'.
   %

   if nelems == 0
      for i = 1 : nargsout
         varargout{i} = reshape(varargin{i}([]), csize);
      end
      return
   end

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % General case: resize all input arguments to some non-empty array.
   %

   % All input arrays must be non-empty if the output arrays are to be
   % non-empty.
   if any(cellfun('isempty', varargin))
      error('Can''t resize an empty array to a non-empty array.');
   end

   % Initialize default subscript array.
   subs{5} = ':';
   subs(:) = subs(end);

   for i = 1 : nargsout

      % Size of i'th input argument.
      isize = size(varargin{i});

      % If i'th input argument already has the correct size, just assign
      % directly.
      if isequal(csize, isize)
         varargout{i} = varargin{i};

      % If i'th input argument is a scalar; initialize, fill and reshape.
      elseif all(isize == 1)
         varargout{i}(nelems) = varargin{i};                    % initialize
         varargout{i}(:)      = varargin{i};                    % fill
         varargout{i}         = reshape(varargout{i}, csize);   % reshape

      % The general case: recycle or truncate as necessary.
      else
         % Number of dimensions of i'th input argument.
         idims = length(isize);

         % Initialize list of subscripts.
         subsi = subs;

         % Find the dimensions for which the lengths are different.
         k = find(isize(1:idims) ~= csize(1:idims));

         % Go through all dimensions for which the lengths are different.
         for j = 1 : length(k)

            % If length along j'th dimension is too long - truncate.
            if isize(k(j)) > csize(k(j))
               subsi{k(j)} = 1 : csize(k(j));

            % If length along j'th dimension is too short - recycle.
            elseif isize(k(j)) < csize(k(j))
               subsi{k(j)} = rem(0 : csize(k(j)) - 1, isize(k(j))) + 1;

            % We should never get here.
            else
               error('Internal error');
            end

         end

         % Now resize the i'th input argument.
         varargout{i} = varargin{i}(subsi{:});

      end
   end


function quad = fit_bounding_quad(ptlist, ori, vp)

[pleft pright pbot2 ptop2 pbot3 ptop3 pleft3 pright3] = ...
    getextremepoints(ptlist, vp);

if ori==1
%     quad.pt(1,1:2) = line_intersect(ptop2, vp{2}, ptop3, vp{3}); % top
%     quad.pt(2,1:2) = line_intersect(ptop2, vp{2}, pbot3, vp{3}); % t2b3
%     quad.pt(3,1:2) = line_intersect(pbot2, vp{2}, ptop3, vp{3}); % b2t3
%     quad.pt(4,1:2) = line_intersect(pbot2, vp{2}, pbot3, vp{3}); % bot
    quad.pt(1,1:2) = line_intersect(ptop2, vp{2}, pleft3, vp{3}); % topleft
    quad.pt(2,1:2) = line_intersect(ptop2, vp{2}, pright3, vp{3}); % topright
    quad.pt(3,1:2) = line_intersect(pbot2, vp{2}, pleft3, vp{3}); % botleft
    quad.pt(4,1:2) = line_intersect(pbot2, vp{2}, pright3, vp{3}); % botleft
    if quad.pt(1,1)>quad.pt(2,1) % || quad.pt(3,1)>quad.pt(4,1)
        quad.pt = quad.pt([2 1 4 3],:);
    end
elseif ori==2
    quad.pt(1,1:2) = line_intersect(pleft,  vp{1}, ptop3, vp{3});  % topleft
    quad.pt(2,1:2) = line_intersect(pright, vp{1}, ptop3, vp{3}); % topright
    quad.pt(3,1:2) = line_intersect(pleft,  vp{1}, pbot3, vp{3});  % botleft
    quad.pt(4,1:2) = line_intersect(pright, vp{1}, pbot3, vp{3}); % botright
    if quad.pt(1,2)>quad.pt(3,2) % || quad.pt(2,2)>quad.pt(4,2)
        quad.pt = quad.pt([3 4 1 2],:);
    end
elseif ori==3
    quad.pt(1,1:2) = line_intersect(pleft,  vp{1}, ptop2, vp{2});  % topleft
    quad.pt(2,1:2) = line_intersect(pright, vp{1}, ptop2, vp{2}); % topright
    quad.pt(3,1:2) = line_intersect(pleft,  vp{1}, pbot2, vp{2});  % botleft
    quad.pt(4,1:2) = line_intersect(pright, vp{1}, pbot2, vp{2}); % botright
end

% % % % % %
% % % % % pt = line_intersect(pleft,vp{1},ptop2,vp{2});
% % % % % junc2(1).pt = pt;
% % % % % junc2(1).type = 1;
% % % % % junc2(1).hclass = 2;
% % % % % junc2(1).vclass = 1;
% % % % % junc2(1).regionid = getregionid(pt(1),pt(2),vp);
% % % % %
% % % % % pt = line_intersect(pright,vp{1},ptop2,vp{2});
% % % % % junc2(2).pt = pt;
% % % % % junc2(2).type = 2;
% % % % % junc2(2).hclass = 2;
% % % % % junc2(2).vclass = 1;
% % % % % junc2(2).regionid = getregionid(pt(1),pt(2),vp);
% % % % %
% % % % % pt = line_intersect(pleft,vp{1},ptop3,vp{3});
% % % % % junc2(3).pt = pt;
% % % % % junc2(3).type = 1;
% % % % % junc2(3).hclass = 3;
% % % % % junc2(3).vclass = 1;
% % % % % junc2(3).regionid = getregionid(pt(1),pt(2),vp);
% % % % %
% % % % % pt = line_intersect(pright,vp{1},ptop3,vp{3});
% % % % % junc2(4).pt = pt;
% % % % % junc2(4).type = 2;
% % % % % junc2(4).hclass = 3;
% % % % % junc2(4).vclass = 1;
% % % % % junc2(4).regionid = getregionid(pt(1),pt(2),vp);
% % % % %
% % % % % %
% % % % % pt = line_intersect(pleft,vp{1},pbot2,vp{2});
% % % % % junc2(5).pt = pt;
% % % % % junc2(5).type = 3;
% % % % % junc2(5).hclass = 2;
% % % % % junc2(5).vclass = 1;
% % % % % junc2(5).regionid = getregionid(pt(1),pt(2),vp);
% % % % %
% % % % % pt = line_intersect(pright,vp{1},pbot2,vp{2});
% % % % % junc2(6).pt = pt;
% % % % % junc2(6).type = 4;
% % % % % junc2(6).hclass = 2;
% % % % % junc2(6).vclass = 1;
% % % % % junc2(6).regionid = getregionid(pt(1),pt(2),vp);
% % % % %
% % % % % pt = line_intersect(pleft,vp{1},pbot3,vp{3});
% % % % % junc2(7).pt = pt;
% % % % % junc2(7).type = 3;
% % % % % junc2(7).hclass = 3;
% % % % % junc2(7).vclass = 1;
% % % % % junc2(7).regionid = getregionid(pt(1),pt(2),vp);
% % % % %
% % % % % pt = line_intersect(pright,vp{1},pbot3,vp{3});
% % % % % junc2(8).pt = pt;
% % % % % junc2(8).type = 4;
% % % % % junc2(8).hclass = 3;
% % % % % junc2(8).vclass = 1;
% % % % % junc2(8).regionid = getregionid(pt(1),pt(2),vp);
% % % % %
% % % % % %
% % % % % pt = line_intersect(ptop2,vp{2},ptop3,vp{3});
% % % % % junc2(9).pt = pt;
% % % % % junc2(9).type = 1;
% % % % % junc2(9).hclass = 3;
% % % % % junc2(9).vclass = 2;
% % % % % junc2(9).regionid = getregionid(pt(1),pt(2),vp);
% % % % %
% % % % % pt = line_intersect(pbot2,vp{2},pbot3,vp{3});
% % % % % junc2(10).pt = pt;
% % % % % junc2(10).type = 3;
% % % % % junc2(10).hclass = 3;
% % % % % junc2(10).vclass = 2;
% % % % % junc2(10).regionid = getregionid(pt(1),pt(2),vp);
% % % % %
% % % % % % ***** TODO: junc11,12: type???
% % % % % pt = line_intersect(ptop2,vp{2},ptop3,vp{3});
% % % % % junc2(11).pt = pt;
% % % % % junc2(11).type = 2;
% % % % % junc2(11).hclass = 3;
% % % % % junc2(11).vclass = 2;
% % % % % junc2(11).regionid = getregionid(pt(1),pt(2),vp);
% % % % %
% % % % % pt = line_intersect(pbot2,vp{2},pbot3,vp{3});
% % % % % junc2(12).pt = pt;
% % % % % junc2(12).type = 4;
% % % % % junc2(12).hclass = 3;
% % % % % junc2(12).vclass = 2;
% % % % % junc2(12).regionid = getregionid(pt(1),pt(2),vp);
% % % % %


function [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,slice,n,localglobal,cst)
% gamma index calculation
% according to http://www.ncbi.nlm.nih.gov/pubmed/9608475
%
% call
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,cst)
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,slice,cst)
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,n,cst)
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,localglobal,cst)
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,slice,n,cst)
%               ...
%   [gammaCube,gammaPassRateCell] = matRad_gammaIndex(cube1,cube2,resolution,criteria,slice,n,localglobal,cst)
%
% input
%   cube1:         dose cube as an M x N x O array
%   cube2:         dose cube as an M x N x O array
%   resolution:    resolution of the cubes [mm/voxel]
%   criteria:      [1x2] vector specifying the distance to agreement
%                  criterion; first element is percentage difference,
%                  second element is distance [mm]
%   slice:         (optional) slice in cube1/2 that will be visualized
%   n:             (optional) number of interpolations. there will be 2^n-1
%                  interpolation points. The maximum suggested value is 3.
%   localglobal:   (optional) parameter to choose between 'global' and 'local'
%                  normalization
%   cst:           list of interessing volumes inside the patient
%
% output
%
%   gammaCube:          result of gamma index calculation
%   gammaPassRateCell:  rate of voxels passing the specified gamma criterion
%                  evaluated for every structure listed in 'cst'.
%                  note that only voxels exceeding the dose threshold are
%                  considered.
%
% References
%   [1]  http://www.ncbi.nlm.nih.gov/pubmed/9608475
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2015 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[env, ~] = matRad_getEnvironment();

% set parameters for gamma index calculation
if exist('criteria','var')
    relDoseThreshold = criteria(1); % in [%]
    dist2AgreeMm     = criteria(2); % in [mm]
else
    dist2AgreeMm     = 3; % in [mm]
    relDoseThreshold = 3; % in [%]
end

% set parameters for gamma index calculation
if ~exist('n','var')
    n = 0;
end

% set global or local gamma evaluation
if ~exist('localglobal','var')
    localglobal = 'global';
end

% check if cubes consistent
if ~isequal(size(cube1),size(cube2))
   error('dose cubes must be the same size\n');
end

% define search nighborhood and resolution
if round(n) ~= n
    error('n must be an integer value');
else
    resolution = resolution./(2.^n);
end

neighborhood = 1.5* dist2AgreeMm; % [mm]
searchX = ceil(neighborhood./resolution(1));
searchY = ceil(neighborhood./resolution(2));
searchZ = ceil(neighborhood./resolution(3));

% init cube
% cut all the zeros surrounding the interesting part
cut1 = any(any(cube1,2),3);
cut2 = any(any(cube1,1),3);
cut3 = any(any(cube1,1),2);

% avoids that "zero-slides" are deleted between two interest regions
k = find(cut1); cut1(k(1):k(end)) = 1;
k = find(cut2); cut2(k(1):k(end)) = 1;
k = find(cut2); cut2(k(1):k(end)) = 1;

% copy cubes
cubex1c = cube1;
cubex2c = cube2;

% cut cubes
cubex1c( ~cut1, :, :) = []; % rows
cubex1c( :, ~cut2, :) = []; % columns
cubex1c( :, :, ~cut3) = []; % slices
cubex2c( ~cut1, :, :) = []; % rows
cubex2c( :, ~cut2, :) = []; % columns
cubex2c( :, :, ~cut3) = []; % slices

% pad cubes
cubex2 = zeros([size(cubex2c,1)+2*searchX size(cubex2c,2)+2*searchY size(cubex2c,3)+2*searchZ]);
cubex2((1+searchX):(end-searchX), ...
       (1+searchY):(end-searchY), ...
       (1+searchZ):(end-searchZ)) = cubex2c;

% interpolate if necessary
if n > 0
    switch env
        case 'MATLAB'
            cubex2 = interp3(cubex2,n,'cubic');
        case 'OCTAVE'
            cubex2 = interp3(cubex2,n,'linear');
    end
end

% set up temporary cubes required for calculation
gammaCubeSq = inf*ones(size(cubex1c));

% adjust dose threshold
if strcmp(localglobal,'local')
    doseThreshold = cubex1c .* relDoseThreshold/100;
elseif strcmp(localglobal,'global')
    doseThreshold = relDoseThreshold/100 * max(cube1(:));
end

% search for min
for i = -searchX:searchX
    for j = -searchY:searchY
        for k = -searchZ:searchZ

            delta_sq = ((i*resolution(1))^2 + ...
                        (j*resolution(2))^2 + ...
                        (k*resolution(3))^2) / dist2AgreeMm^2;

            tmpCube = cubex1c - cubex2((1+((2^n)*searchX)+i) : 2^n : (end-((2^n)*searchX)+i), ...
                                       (1+((2^n)*searchY)+j) : 2^n : (end-((2^n)*searchY)+j), ...
                                       (1+((2^n)*searchZ)+k) : 2^n : (end-((2^n)*searchZ)+k));

            tmpCube = tmpCube.^2 ./ doseThreshold.^2 + delta_sq;

            gammaCubeSq = min(gammaCubeSq,tmpCube);

        end
    end

%     display '.';

end

% evaluate gamma cube and set to zero all the voxel that contain an
% infinite value
gammaCubeSqx                 = zeros(size(cube1));
gammaCubeSqx(cut1,cut2,cut3) = gammaCubeSq;
gammaCube                    = sqrt(gammaCubeSqx);

% set values where we did not compute gamma keeping inf in the cube to zero
gammaCube(cube1<=0 & cube2<=0) = 0;

% compute gamma pass rate
doseIx          = cube1 > relDoseThreshold/100*max(cube1(:)) | cube2 > relDoseThreshold/100*max(cube2(:));
numOfPassGamma  = sum(gammaCube(doseIx) < 1);
gammaPassRate   = 100 * numOfPassGamma / sum(doseIx(:));

% compute stats for all segmented sturtures
if exist('cst','var')
    gammaPassRateCell = cell(1,2);
    gammaPassRateCell{1,1} = 'Whole CT';
    gammaPassRateCell{1,2} = gammaPassRate;

    for i = 1:size(cst, 1)
        volume = cst{i,4}{1,1}; % indices of voxels of the interesting volume
        doseIxVol = false(size(doseIx));
        doseIxVol(volume) = doseIx(volume);
        numOfPassGammaVol  = sum(gammaCube(doseIxVol) < 1);
        gammaPassRateVol   = 100 * numOfPassGammaVol / sum(doseIxVol(:));
        if isnan(gammaPassRateVol) % remove organs not receiving any dose (resulting in NaN)
            continue;
        end
        gammaPassRateCell{end+1, 1} = cst{i,2};
        gammaPassRateCell{end, 2} = gammaPassRateVol;

    end
end

% visualize if applicable
if exist('slice','var') && ~isempty(slice)
    figure
    set(gcf,'Color',[1 1 1]);
    imagesc(gammaCube(:,:,slice),[0 2])
    myColormap = matRad_getColormap('gammaIndex');

    colormap(gca,myColormap);
    colorbar

    title({[num2str(gammaPassRate,5) '% of points > ' num2str(relDoseThreshold) ...
            '% pass gamma criterion (' num2str(relDoseThreshold) '% / ' ...
            num2str(dist2AgreeMm) 'mm)']; ['with ' num2str(2^n-1) ' interpolation points']});
end


function vl_demo_alldist
%

numRepetitions  = 3 ;
numDimensions   = 1000 ;
numSamplesRange = [300] ;

settingsRange = {{'alldist2', 'double',  'l2',         }, ...
                 {'alldist',  'double',  'l2', 'nosimd'}, ...
                 {'alldist',  'double',  'l2'          }, ...
                 {'alldist2', 'single',  'l2',         }, ...
                 {'alldist',  'single',  'l2', 'nosimd'}, ...
                 {'alldist',  'single',  'l2'          }, ...
                 {'alldist2', 'double',  'l1',         }, ...
                 {'alldist',  'double',  'l1', 'nosimd'}, ...
                 {'alldist',  'double',  'l1'          }, ...
                 {'alldist2', 'single',  'l1',         }, ...
                 {'alldist',  'single',  'l1', 'nosimd'}, ...
                 {'alldist',  'single',  'l1'          }, ...
                 {'alldist2', 'double',  'chi2',         }, ...
                 {'alldist',  'double',  'chi2', 'nosimd'}, ...
                 {'alldist',  'double',  'chi2'          }, ...
                 {'alldist2', 'single',  'chi2',         }, ...
                 {'alldist',  'single',  'chi2', 'nosimd'}, ...
                 {'alldist',  'single',  'chi2'          }, ...
                 {'alldist2', 'double',  'hell',         }, ...
                 {'alldist',  'double',  'hell', 'nosimd'}, ...
                 {'alldist',  'double',  'hell'          }, ...
                 {'alldist2', 'single',  'hell',         }, ...
                 {'alldist',  'single',  'hell', 'nosimd'}, ...
                 {'alldist',  'single',  'hell'          }, ...
                 {'alldist2', 'double',  'kl2',         }, ...
                 {'alldist',  'double',  'kl2', 'nosimd'}, ...
                 {'alldist',  'double',  'kl2'          }, ...
                 {'alldist2', 'single',  'kl2',         }, ...
                 {'alldist',  'single',  'kl2', 'nosimd'}, ...
                 {'alldist',  'single',  'kl2'          }, ...
                 {'alldist2', 'double',  'kl1',         }, ...
                 {'alldist',  'double',  'kl1', 'nosimd'}, ...
                 {'alldist',  'double',  'kl1'          }, ...
                 {'alldist2', 'single',  'kl1',         }, ...
                 {'alldist',  'single',  'kl1', 'nosimd'}, ...
                 {'alldist',  'single',  'kl1'          }, ...
                 {'alldist2', 'double',  'kchi2',         }, ...
                 {'alldist',  'double',  'kchi2', 'nosimd'}, ...
                 {'alldist',  'double',  'kchi2'          }, ...
                 {'alldist2', 'single',  'kchi2',         }, ...
                 {'alldist',  'single',  'kchi2', 'nosimd'}, ...
                 {'alldist',  'single',  'kchi2'          }, ...
                 {'alldist2', 'double',  'khell',         }, ...
                 {'alldist',  'double',  'khell', 'nosimd'}, ...
                 {'alldist',  'double',  'khell'          }, ...
                 {'alldist2', 'single',  'khell',         }, ...
                 {'alldist',  'single',  'khell', 'nosimd'}, ...
                 {'alldist',  'single',  'khell'          }, ...
                } ;

%settingsRange = settingsRange(end-5:end) ;

styles = {} ;
for marker={'x','+','.','*','o'}
  for color={'r','g','b','k','y'}
    styles{end+1} = {'color', char(color), 'marker', char(marker)} ;
  end
end

for ni=1:length(numSamplesRange)
  for ti=1:length(settingsRange)
    tocs = [] ;
    for ri=1:numRepetitions
      rand('state',ri) ;
      randn('state',ri) ;
      numSamples = numSamplesRange(ni) ;
      settings = settingsRange{ti} ;
      [tocs(end+1), D] = run_experiment(numDimensions, ...
                                        numSamples, ...
                                        settings) ;
    end
    means(ni,ti) = mean(tocs) ;
    stds(ni,ti)  = std(tocs) ;
    if mod(ti-1,3) == 0
      D0 = D ;
    else
      err = max(abs(D(:)-D0(:))) ;
      fprintf('err %f\n', err) ;
      if err > 1, keyboard ; end
    end
  end
end

if 0
  figure(1) ; clf ; hold on ;
  numStyles = length(styles) ;
  for ti=1:length(settingsRange)
    si = mod(ti - 1, numStyles) + 1 ;
    h(ti) = plot(numSamplesRange, means(:,ti), styles{si}{:}) ;
    leg{ti} = sprintf('%s ', settingsRange{ti}{:}) ;
    errorbar(numSamplesRange, means(:,ti), stds(:,ti), 'linestyle', 'none') ;
  end
end

for ti=1:length(settingsRange)
  leg{ti} = sprintf('%s ', settingsRange{ti}{:}) ;
end

figure(1) ; clf ;
barh(means(end,:)) ;
set(gca,'ytick', 1:length(leg), 'yticklabel', leg,'ydir','reverse') ;
xlabel('Time [s]') ;

function [elaps, D] = run_experiment(numDimensions, numSamples, settings)

distType  = 'l2' ;
algType   = 'alldist' ;
classType = 'double' ;
useSimd   = true ;

for si=1:length(settings)
  arg = settings{si} ;
  switch arg
    case {'l1', 'l2', 'chi2', 'hell', 'kl2', 'kl1', 'kchi2', 'khell'}
      distType = arg ;
    case {'alldist', 'alldist2'}
      algType = arg ;
    case {'single', 'double'}
      classType = arg ;
    case 'simd'
      useSimd = true ;
    case 'nosimd'
      useSimd = false ;
    otherwise
      assert(false) ;
  end
end

X = rand(numDimensions, numSamples) ;
X(X < .3) = 0 ;

switch classType
  case 'double'
  case 'single'
    X = single(X) ;
end

vl_simdctrl(double(useSimd)) ;

switch algType
  case 'alldist'
    tic ; D = vl_alldist(X, distType) ; elaps = toc ;
  case 'alldist2'
    tic ; D = vl_alldist2(X, distType) ; elaps = toc ;
end


%
%  Map a sequence of pen points into a Motor Program
%
%  Input
%   S: [ns x 1 nested cell array] of trajectories in motor space [n x 2]
%   I: [binary image]
%   fclassify: [function handle] fclassify([n x 2] trajectory, [scalar]
%                                   scale)
%               this function classifies trajectories are belong to a
%               specific library component
%
%  Output
%   M: MotorProgram object
%
function M = sequence_to_MP(S,I,fclassify,lib)

    assert(iscell(S));
    assert(iscell(S{1}));

    % find the new scale of primitives
    assert(exist('lib','var')>0);
    newscale = lib.newscale;
    ncpt = lib.ncpt;

%     % find the new scale of primitives
%     ps_clustering = defaultps_clustering;
%     newscale = ps_clustering.newscale_ss;
%     ncpt = ps_clustering.ncpt;
%     if exist('lib','var')
%        newscale = lib.newscale;
%        ncpt = lib.ncpt;
%     end

    % get the normalized trajectories, the stroke centers, scales, etc.
    verbose = false;
    [S_norm,~,S_scales] = normalize_dataset(S,newscale,verbose);

    % fit splines to the normalized trajectories, and assign them
    % indices in the library
    PM = defaultps;
    ps_bottomup = defaultps_bottomup;
    ns = length(S);
    S_splines = cell(ns,1);
    for sid=1:ns
        nsub = length(S{sid});
        S_splines{sid} = cell(nsub,1);
        for b=1:nsub
            S_splines{sid}{b} = fclassify(S_norm{sid}{b},S_scales{sid}{b});
        end
    end

    % make motor program structure
    M = MotorProgram(ns);
    M.I = I; % set the image
    M.parameters = PM;
    for sid=1:ns % for each stroke
       nsub = length(S{sid});
       M.S{sid}.ids = []; %zeros(nsub,1);
       M.S{sid}.invscales_token = zeros(nsub,1);
       M.S{sid}.shapes_token = zeros(ncpt,2,nsub);

       for b=1:nsub % for each sub-stroke
          M.S{sid}.ids(b,:) = S_splines{sid}{b}.indx;
          M.S{sid}.invscales_token(b) = 1 ./ S_scales{sid}{b}(1);
          M.S{sid}.shapes_token(:,:,b) = S_splines{sid}{b}.bspline;
       end
       M.S{sid}.invscales_type = M.S{sid}.invscales_token;

       M.S{sid}.pos_token = S{sid}{1}(1,:); % start position in real trajectory

    end

    % initialize the noise parameters
    M.epsilon = ps_bottomup.init_epsilon;
    M.blur_sigma = ps_bottomup.init_blur_sigma;

end


function ncorr_util_colormap(handle_fig)
% This function adjusts the colormap correctly. This was taken from some
% matlab code.
%
% Inputs -----------------------------------------------------------------%
%   handle_fig - handle; handle of the figure;
%
% Outputs ----------------------------------------------------------------%
%   none;

    m = size(get(handle_fig,'colormap'),1);
    n = ceil(m/4);
    u = [(1:1:n)/n ones(1,n-1) (n:-1:1)/n]';
    g = ceil(n/2) - (mod(m,4)==1) + (1:length(u))';
    r = g + n;
    b = g - n;
    g(g>m) = [];
    r(r>m) = [];
    b(b<1) = [];
    J = zeros(m,3);
    J(r,1) = u(1:length(r));
    J(g,2) = u(1:length(g));
    J(b,3) = u(end-length(b)+1:end);

    if verLessThan('matlab','9.1')
        set(handle_fig,'Colormap',J);
    else
        % Get all axes and set their colormap. This is a fix for 2016b+
        % (9.1+) which now sets colormaps on a per-axis basis for imshow()
        % instead of for the entire figure.
        handle_axes = findobj(handle_fig,'type','axes');
        for i = 1:length(handle_axes)
            colormap(handle_axes(i),J);
        end
    end
end