function [y,ty]=v_correlogram(x,inc,nw,nlag,m,fs)
%V_CORRELOGRAM calculate correlogram [y,ty]=(x,inc,nw,nlag,m,fs)
% Usage:
%        do_env=1; do_hp=1;                            % flags to control options
%        [b,a,fx,bx,gd]=v_gammabank(25,fs,'',[80 5000]); % determine v_filterbank
%        y=v_filterbank(b,a,s,gd);                       % filter signal s
%        if do_env
%            [bl,al]=butter(3,2*800/fs);
%            y=filter(bl,al,v_teager(y,1),[],1);           % low pass envelope @ 800 Hz
%        end
%        if do_hp
%            y=filter(fir1(round(16e-3*fs),2*64/fs,'high'),1,y,[],1);  % HP filter @ 64 Hz
%        end
%        v_correlogram(y,round(10e-3*fs),round(16e-3*fs),round(12e-3*fs),'',fs);
%
% Inputs:
%        x(*,chan)  is the output of a filterbank (e.g. v_filterbank)
%                   with one column per filter channel
%        inc        frame increment in samples
%        nw         window length in samples [or window function]
%        nlag       number of lags to calculate
%        m          mode string:
%               'h' = Hamming window
%        fs         sample freq (affects only plots)
%
% Outputs:
%        y(lag,chan,frame) is v_correlogram. Lags are 1:nlag samples
%        ty                is time of the window energy centre for each frame
%                            x(n) is at time n
%
% For each channel, the calculated correlation for frame n comprises
%       y(t+1,*,n+1)=(win.*x(n*inc+(1:nw))'*x(n*inc+t+(1:nw))/sqrt(win'*abs(x(n*inc+(1:nw))).^2 * win'*abs(x(n*inc+t+(1:nw))).^2)
% This corresponds to the expression in (1.7) of [1] but incorporating the normalization from (1) of [2].
%
% Future planned mode options:
%       'd' = subtract DC component
%       'n' = unnormalized
%       'v' = variable analysis window proportional to lag
%       'p' = output the peaks only
%
% Refs:
% [1]	D. Wang and G. J. Brown. Fundamentals of computational auditory scene analysis.
%       In D. Wang and G. Brown, editors, Computational Auditory Scene Analysis: Principles,
%       Algorithms, and Applications, chapter 1. Wiley, Oct. 2006. doi: 10.1109/9780470043387.ch1
% [2]	S. Granqvist and B. Hammarberg. The correlogram: a visual display of periodicity. J. Acoust. Soc. Amer., 114: 2934, 2003.

%      Copyright (C) Mike Brookes 2011-2018
%      Version: $Id: v_correlogram.m 10867 2018-09-21 17:35:59Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
memsize=v_voicebox('memsize'); 	% set memory size to use
[nx,nc]=size(x);                % number of sampes and channels
if nargin<6
    fs=1;                       % default sample frequency is 1 Hz
    if nargin<5
        m='h';                  % default analysis window is Hamming
        if nargin<4
            nlag=[];
            if nargin<3
                nw=[];
                if nargin<2
                    inc=[];
                end
            end
        end
    end
end
if ~numel(inc)
    inc=128;                    % default frame hop is 128 samples
end
if ~numel(nw)
    nw=inc;                     % default analysis window length is the fame increment
end
nwin=length(nw);
if nwin>1                       % nw specifies the window function explicitly
    win=nw(:);
else                            % nw gives the window length
    nwin=nw;
    if any(m=='h')
        win=v_windows(3,nwin)'; % Hamming window
    else
        win=ones(nwin,1);       % window function
    end
end
if ~numel(nlag)
    nlag=nwin;
end
nwl=nwin+nlag-1;
nt=pow2(nextpow2(nwl));         % transform length
nf=floor((nx-nwl+inc)/inc);     % number of frames
i1=repmat((1:nwl)',1,nc)+repmat(0:nx:nx*nc-1,nwl,1);
nb=min(nf,max(1,floor(memsize/(16*nwl*nc))));    % chunk size for calculating
nl=ceil(nf/nb);                  % number of chunks
jx0=nf-(nl-1)*nb;                % size of first chunk in frames
wincg=(1:nwin)*win.^2/(win'*win); % determine window centre of energy
fwin=conj(fft(win,nt,1));       % conjugate fft of window
y=zeros(nlag,nc,nf);
% first do partial chunk
jx=jx0;
x2=zeros(nwl,nc*jx);
x2(:)=x(repmat(i1(:),1,jx)+repmat((0:jx-1)*inc,nwl*nc,1));
% the next line was previously: v=ifft(conj(fft(x2(1:nwin,:),nt,1)).*fft(x2,nt,1));
v=ifft(conj(fft(x2(1:nwin,:).*repmat(win(:),1,nc*jx),nt,1)).*fft(x2,nt,1));                 % v(1:nlag,:) contains second half of xcorr(x2) output
w=max(real(ifft(fwin(:,ones(1,nc*jx)).*fft(x2.*conj(x2),nt,1))),0); % v(1:nlag,:) contains second half of xcorr(|x2|^2,win) output
w=sqrt(w(1:nlag,:).*w(ones(nlag,1),:));
if isreal(x)
    y(:,:,1:jx)=reshape(real(v(1:nlag,:))./w,nlag,nc,jx); % note: some values may be NaN is x=0 throughout the window
else
    y(:,:,1:jx)=reshape(conj(v(1:nlag,:))./w,nlag,nc,jx);
end
% now do remaining chunks
x2=zeros(nwl,nc*nb);
for il=2:nl
    ix=jx+1;            % first frame in this chunk
    jx=jx+nb;           % last frame in this chunk
    x2(:)=x(repmat(i1(:),1,nb)+repmat((ix-1:jx-1)*inc,nwl*nc,1));
    % the next line was previously: v=ifft(conj(fft(x2(1:nwin,:),nt,1)).*fft(x2,nt,1));
    v=ifft(conj(fft(x2(1:nwin,:).*repmat(win(:),1,nc*nb),nt,1)).*fft(x2,nt,1));
    w=max(real(ifft(fwin(:,ones(1,nc*nb)).*fft(x2.*conj(x2),nt,1))),0);
    w=sqrt(w(1:nlag,:).*w(ones(nlag,1),:));
    if isreal(x)
        y(:,:,ix:jx)=reshape(real(v(1:nlag,:))./w,nlag,nc,nb);
    else
        y(:,:,ix:jx)=reshape(conj(v(1:nlag,:))./w,nlag,nc,nb);
    end
end
ty=(0:nf-1)'*inc+wincg;       % calculate times of window centres
if ~nargout
    imagesc(ty/fs,(1:nlag)/fs,squeeze(mean(abs(y),2)));
    if nargin<6
        us='samp';
    else
        us='s';
    end
    xlabel(['Time (' v_xticksi us ')']);
    ylabel(['Lag (' v_yticksi us ')']);
    axis 'xy';
    v_colormap('v_thermliny');
    colorbar;
    v_cblabel('Mean Correlation');
    title('Summary Correlogram');
end


function estimatedLabels = GMLVQ_classify(Data, model)

%GMLVQ_classify.m - classifies the given data with the given model
%
% estimatedLabels = GMLVQ_classify(Data, model)
%
% Input and output arguments:
%  trainSet          (struct)
%                    (matrix) matrix with training samples in its rows
%  model    : GMLVQ model with prototypes w their labels c_w and the matrix omega
%
%  estimatedLabels       : the estimated labels
%
%  example for usage:
%  trainSet = [1,2,3;4,5,6;7,8,9];
%  trainLab = [1;1;2];
%  GMLVQ_model=gmlvq_core(trainSet,trainLab); % minimal parameters required
%  estimatedTrainLabels = GMLVQ_classify(trainSet, GMLVQ_model);
%  trainError = mean( trainLab ~= estimatedTrainLabels );
%
% Kerstin Bunte
% kerstin.bunte@googlemail.com
% Mon Nov 05 09:05:52 CEST 2012
%
% Conditions of GNU General Public License, version 2 and BSD License apply.
% See file 'license-gpl2.txt' and 'BSD_license.txt' enclosed in this package.
% Programs are not for use in critical applications!
%

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

if isstruct(Data)
    Data = Data.data;
end

if ~isfield(model, 'w')
    model.c_w = som_label2num(model.labels);
    dist = computeDistance(Data, model.codebook, model);
else
    dist = computeDistance(Data, model.w, model);
end

[~, index] = min(dist,[],2);

estimatedLabels = model.c_w(index);

function distance = computeDistance(X, W, model)
nb_samples = size(X,1);
distance = zeros(nb_samples,length(model.c_w));

% tic;
% for i = 1:size(W,1)
%     delta = X - ones(P,1) * W(i,:);
%     delta = bsxfun(@minus, X, W(i,:));
%     delta(isnan(delta)) = 0;
    % Hadamard product: to skip unnecessary calculation between two different examples
%     distance(1:nb_samples,i) = sum( ((delta*model.omega'*model.omega).*delta) ,2 );
%     wi = W(i,:);
%     distance(1:nb_samples,i) = sum( (( (X-wi(ones(nb_samples,1),:)) *model.omega'*model.omega).*(X-wi(ones(nb_samples,1),:))) ,2 );
% end
% disp(toc);
if isfield(model,'psis')
    if length(model.psis)~=size(W,1)
        classes = unique(model.c_w);
        for i = 1:size(W,1)
            matrixIdx = classes==model.c_w(i);
%             delta = X-ones(nb_samples,1)*W(i,:);
%             distance(1:nb_samples,i) = sum( ((delta* model.psis{matrixIdx}'*model.psis{matrixIdx}).*delta) ,2 );
            distance(1:nb_samples,i) = sum((bsxfun(@minus, X, W(i,:))*model.psis{matrixIdx}').^2, 2);
        end
    else
        for i = 1:size(W,1)
%             delta = X-ones(nb_samples,1)*W(i,:);
%             distance(1:nb_samples,i) = sum( ((delta* model.psis{i}'*model.psis{i}).*delta) ,2 );
            distance(1:nb_samples,i) = sum((bsxfun(@minus, X, W(i,:))*model.psis{i}').^2, 2);
        end
    end
else
    if isfield(model,'lambda')
        for i = 1:size(W,1)
%             delta = X-ones(nb_samples,1)*W(i,:);
%             distance(1:nb_samples,i) = sum( (( delta *model.lambda).*delta) ,2 );
            delta = bsxfun(@minus, X, W(i,:));
            distance(1:nb_samples,i) = sum(bsxfun(@times,delta.^2,model.lambda), 2);
%             distance(1:nb_samples,i) = sum( (( delta *model.lambda).*delta) ,2 );
        end
    end
    if isfield(model,'omega')
        % tic;
        for i = 1:size(W,1)
%             delta = X-ones(nb_samples,1)*W(i,:);
%             distance(1:nb_samples,i) = sum( (( delta *model.omega'*model.omega).*delta) ,2 );
            distance(1:nb_samples,i) = sum((bsxfun(@minus, X, W(i,:))*model.omega').^2, 2);
        end
        % disp(toc);
    end
end


function ins=ins_time_updata(ins)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INS time update
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2020-2025, by Kai Chen, All rights reserved.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ins.Phi=update_trans_mat(ins);

G=zeros(15,15);
G(1:3,1:3)=-ins.Cnb;
G(4:6,4:6)=ins.Cnb;
G(10:12,10:12)=eye(3);
G(13:15,13:15)=eye(3);

Q0=G*ins.Q*G';
P0=ins.P+0.5*Q0;
ins.P=ins.Phi*P0*ins.Phi'+0.5*Q0;

% ins.P=ins.Phi*ins.P*ins.Phi'+ins.Q;

return


function check = triangle_check ( a, b, c )

%*****************************************************************************80
%
%% TRIANGLE_CHECK checks the parameters of the Triangle CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, the parameters of the PDF.
%    A <= B <= C and A < C.
%
%    Output, logical TRIANGLE_CHECK, is true if the parameters are legal.
%
  if ( b < a )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGLE_CHECK - Fatal error!\n' );
    fprintf ( 1, '  B < A.\n' );
    check = 0;
    return
  end

  if ( c < b )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGLE_CHECK - Fatal error!\n' );
    fprintf ( 1, '  C < B.\n' );
    check = 0;
    return
  end

  if ( a == c )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGLE_CHECK - Fatal error!\n' );
    fprintf ( 1, '  A == C.\n' );
    check = 0;
    return
  end

  check = 1;

  return
end


% This is something I always wondered...
% Manuel Diaz, NTU, 2014.01.21

%
A = magic(6);

%
idx = 1:6;
idx_new = repmat(idx,[6,1]);

%
x = linspace(0,1,6);
v = sin(2*pi*x);

% operation
b = A.*v(idx_new);
disp(b);

% conclusion:
% matlab indexes = c++ pointers ;)


filename='Cantileverbeam_Hexahedra_Linear_Structured';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'holes';
cost = {'compliance', 'perimeter'};
weights = [1 0.001];
constraint = {'volumeConstraint'};
optimizer = 'HAMILTON-JACOBI';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'P1';
constraint_case = 'INEQUALITY';

nsteps = 10;
Vfrac_final = 0.1;
Perimeter_target=3.5;
optimality_final =1e-4;
constr_final =1e-4;

BCscale_factor = 0.3;
HJiter0 = 1;
e2 = 100;
N_holes = [12 5 5];
R_holes = 0.4;
phase_holes = [0 0 0];

Vfrac_initial = 0.6;
optimality_initial = 5e-2;
constr_initial = 5e-2;

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

% maxiter = 1;
TOL.nu_plus = 0.3;
TOL.nu_minus = 0.3;

plotting = 1;
printing = 0;
monitoring = 1;
monitoring_interval = 1;


function segment_length = p11_boundary_segment_length ( segment_index, h )

%*****************************************************************************80
%
%% P11_BOUNDARY_SEGMENT_LENGTH returns boundary segment lengths in problem 11.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer SEGMENT_INDEX, the index of one of the boundary segments.
%
%    Input, real H, the suggested spacing between points.
%
%    Output, integer SEGMENT_LENGTH, the number of points in the segment.
%
  if ( h <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P11_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
    fprintf ( 1, '  Nonpositive H = %f\n', h );
    error ( 'P11_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
  end

  if ( segment_index == 1 )

    n = round ( 4.0 / h );
    n = max ( n, 5 );
    segment_length = n + mod ( 4 - mod ( n - 1, 4 ), 4 );

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'P11_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
    fprintf ( 1, '  Illegal SEGMENT_INDEX = %d\n', segment_index );
    error ( 'P11_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );

  end

  return
end


function varargout = meshVertexClustering(vertices, faces, spacing, varargin)
%MESHVERTEXCLUSTERING Simplifies a mesh using vertex clustering.
%
%   [V2, F2] = meshVertexClustering(V, F, SPACING)
%   [V2, F2] = meshVertexClustering(MESH, SPACING)
%   MESH2 = meshVertexClustering(...)
%
%   Simplifies a mesh using vertex clustering. Input mesh is specified
%   either by a pair V, F containing the vertex coordinates and the faces
%   informations, or by a structure with fields 'vertices' and 'faces'.
%
%   The SPACING input defines the size of the grid. It can be either a
%   scalar (uniform grid) or a 1-by-3 row vector.
%
%   The output is specified either in two outputs, or in a structure with
%   fields 'vertices' and 'faces'.
%
%   Example
%     [x, y, z]  = meshgrid(1:100, 1:100, 1:100);
%     img = hypot3(x-51.12, y-52.23, z-53.34);
%     [faces, vertices] = isosurface(img, 45);
%     [v2, f2] = meshVertexClustering(vertices, faces, 10);
%     figure; axis equal; axis([0 100 0 100 0 100]);
%     drawMesh(v2, f2);
%
%   See also
%     reducepatch, smoothMesh

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2019-01-28, using Matlab 9.5.0.944444 (R2018b)
% Copyright 2019-2022 INRA - Cepia Software Platform

%% Initialisation

if isstruct(vertices)
    if nargin > 2
        varargin = [{spacing} varargin(:)];
    end
    spacing = faces;

    mesh = vertices;
    vertices = mesh.vertices;
    faces = mesh.faces;
end

% ensure input mesh is a triangulation
faces = triangulateFaces(faces);

% ensure spacing is a 1-by-3 array
if isscalar(spacing)
    spacing = [spacing spacing spacing];
end

% extract grid origin
origin = [0 0 0];
if ~isempty(varargin)
    origin = varargin{1};
end

%% Apply grid simplification

% identify the vertices belonging to the same grid
[v2, I, J] = unique(round(bsxfun(@rdivide, bsxfun(@minus, vertices, origin), spacing)), 'rows');

%% compute reduced vertex coordinates

% compute coordinates of new vertices
for iVertex = 1:length(I)
    gridVertices = vertices(J == iVertex, :);
    v2(iVertex, :) = mean(gridVertices, 1);
end

%% Compute new faces

% create empty array
faces2 = zeros(0, 3);

% iterate over old faces, and keep only faces whose vertices belong to
% different cell grids
nFaces = size(faces, 1);
for iFace = 1:nFaces
    % current face
    face = faces(iFace, :);

    % equivalent face with new vertices
    face2 = J(face)';

    % some vertices may belong to same cell, so we need to adjust
    % processing
    nInds = length(unique(face2));
    if nInds == 3
        % vertices belong to three different cells -> create a new face

        % keep smaller vertex at first position
        [tmp, indMin] = min(face2); %#ok<ASGLU>
        face2 = circshift(face2, [1-indMin 0]);

        % append the new face to the array
        faces2 = [faces2 ; face2]; %#ok<AGROW>
    end
end

% remove duplicate faces
faces2 = unique(faces2, 'rows');

if nargout == 1
    varargout{1} = struct('vertices', v2, 'faces', faces2);
else
    varargout = {v2, faces2};
end


% LOOK FOR SINGULAR POINTS IN JOINT SPACE

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

function singular_points = find_singular_points(param)

singular_points=[];

q=[0 0 0 0];

teta = eval(param.DH.theta);
d = eval(param.DH.d);
a = eval(param.DH.a);
alfa = eval(param.DH.alpha);

%J = eval(param.J)

q1=-pi:0.05:pi;
q2=-pi:0.05:pi;

for i=1:length(q1),
    fprintf('\nIteration %d of %d', i, length(q1))
   for j=1:length(q2),
        q=[q1(i) q2(j) 0];
        J = eval(param.J);

        val = det(J);
        if abs(val) < 0.0001
            singular_points = [singular_points q'];
        end
   end
end


function [C,phi,S12,S1,S2,t,f,zerosp,confC,phistd,Cerr]=cohgrampt(data1,data2,movingwin,params,fscorr)
% Multi-taper time-frequency coherence - two point processes given as times
%
% Usage:
%
% [C,phi,S12,S1,S2,t,f,zerosp,confC,phistd,Cerr]=cohgrampt(data1,data2,movingwin,params,fscorr)
% Input:
% Note units have to be consistent. Thus, if movingwin is in seconds, Fs
% has to be in Hz. see chronux.m for more information.
%
%       data1  (structure array of spike times with dimension trials; also accepts 1d array of spike times) -- required
%       data2  (structure array of spike times with dimension trials; also accepts 1d array of spike times) -- required
%       movingwin (in the form [window winstep] -- required
%       params: structure with fields tapers, pad, Fs, fpass, err, trialave
%       - optional
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                   (1) A numeric vector [TW K] where TW is the
%                       time-bandwidth product and K is the number of
%                       tapers to be used (less than or equal to
%                       2TW-1).
%                   (2) A numeric vector [W T p] where W is the
%                       bandwidth, T is the duration of the data and p
%                       is an integer such that 2TW-p tapers are used. In
%                       this form there is no default i.e. to specify
%                       the bandwidth, you have to specify T and p as
%                       well. Note that the units of W and T have to be
%                       consistent: if W is in Hz, T must be in seconds
%                       and vice versa. Note that these units must also
%                       be consistent with the units of params.Fs: W can
%                       be in Hz if and only if params.Fs is in Hz.
%                       The default is to use form 1 with TW=3 and K=5
%                    Note that T has to be equal to movingwin(1).
%
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%           Fs   (sampling frequency) - optional. Default 1.
%           fpass    (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%           err  (error calculation [1 p] - Theoretical error bars; [2 p] - Jackknife error bars
%                                   [0 p] or 0 - no error bars) - optional. Default 0.
%           trialave (average over trials when 1, don't average when 0) - optional. Default 0
%       fscorr   (finite size corrections, 0 (don't use finite size corrections) or
%                 1 (use finite size corrections) - optional
%                (available only for spikes). Defaults 0.
% Output:
%       C (magnitude of coherency time x frequencies x trials for trialave=0;
%              time x frequency for trialave=1)
%       phi (phase of coherency time x frequencies x trials for no trial averaging;
%              time x frequency for trialave=1)
%       S12 (cross spectrum - time x frequencies x trials for no trial averaging;
%              time x frequency for trialave=1)
%       S1 (spectrum 1 - time x frequencies x trials for no trial averaging;
%              time x frequency for trialave=1)
%       S2 (spectrum 2 - time x frequencies x trials for no trial averaging;
%              time x frequency for trialave=1)
%       t (time)
%       f (frequencies)
%       zerosp (1 for windows and trials where spikes were absent (in either channel),zero otherwise)
%       confC (confidence level for C at 1-p %) - only for err(1)>=1
%       phistd - theoretical/jackknife (depending on err(1)=1/err(1)=2) standard deviation for phi
%                Note that phi + 2 phistd and phi - 2 phistd will give 95% confidence
%                bands for phi - only for err(1)>=1
%       Cerr  (Jackknife error bars for C - use only for Jackknife - err(1)=2)

if nargin < 3; error('Need data1 and data2 and window parameters'); end;
if nargin < 4; params=[]; end;

if length(params.tapers)==3 & movingwin(1)~=params.tapers(2);
    error('Duration of data in params.tapers is inconsistent with movingwin(1), modify params.tapers(2) to proceed')
end

[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
if nargin < 5 || isempty(fscorr); fscorr=0; end;

if nargout > 10 && err(1)~=2;
    error('Cerr computed only for Jackknife. Correct inputs and run again');
end;
if nargout > 8 && err(1)==0;
    error('Errors computed only if err(1) is not equal to zero');
end;

[N,Ch]=check_consistency(data1,data2);
[mintime1,maxtime1]=minmaxsptimes(data1);
[mintime2,maxtime2]=minmaxsptimes(data2);
mintime=min(mintime1,mintime2);
maxtime=max(maxtime1,maxtime2);

tn=mintime+movingwin(1)/2:movingwin(2):maxtime-movingwin(1)/2;
Nwin=round(Fs*movingwin(1)); % number of samples in window
% Nstep=round(movingwin(2)*Fs); % number of samples to step through
nfft=max(2^(nextpow2(Nwin)+pad),Nwin);
f=getfgrid(Fs,nfft,fpass); Nf=length(f);
params.tapers=dpsschk(tapers,Nwin,Fs); % check tapers
nw=length(tn);
if trialave;
   C=zeros(nw,Nf);
   S12=zeros(nw,Nf);
   S1=zeros(nw,Nf);
   S2=zeros(nw,Nf);
   phi=zeros(nw,Nf);
   Cerr=zeros(2,nw,Nf);
%    phierr=zeros(2,nw,Nf);
   phistd=zeros(nw,Nf);
else
   C=zeros(nw,Nf,Ch);
   S12=zeros(nw,Nf,Ch);
   S1=zeros(nw,Nf,Ch);
   S2=zeros(nw,Nf,Ch);
   phi=zeros(nw,Nf,Ch);
   Cerr=zeros(2,nw,Nf,Ch);
%    phierr=zeros(2,nw,Nf,Ch);
   phistd=zeros(nw,Nf,Ch);
end;
zerosp=zeros(nw,Ch);

for n=1:nw;
   t=linspace(tn(n)-movingwin(1)/2,tn(n)+movingwin(1)/2,Nwin);
   datawin1=extractdatapt(data1,[t(1) t(end)]);datawin2=extractdatapt(data2,[t(1) t(end)]);
   if nargout==11;
     [c,ph,s12,s1,s2,f,zsp,confc,phie,cerr]=coherencypt(datawin1,datawin2,params,fscorr,t);
%      phierr(1,n,:,:)=squeeze(phie(1,:,:));
%      phierr(2,n,:,:)=squeeze(phie(2,:,:));
     phistd(n,:,:)=phie;
     Cerr(1,n,:,:)=squeeze(cerr(1,:,:));
     Cerr(2,n,:,:)=squeeze(cerr(2,:,:));
   elseif nargout==10;
     [c,ph,s12,s1,s2,f,zsp,confc,phie]=coherencypt(datawin1,datawin2,params,fscorr,t);
%      phierr(1,n,:,:)=squeeze(phie(1,:,:));
%      phierr(2,n,:,:)=squeeze(phie(2,:,:));
     phistd(n,:,:)=phie;
   else
     [c,ph,s12,s1,s2,f,zsp]=coherencypt(datawin1,datawin2,params,fscorr,t);
   end;
   C(n,:,:)=c;
   phi(n,:,:)=ph;
   S12(n,:,:)=s12;
   S1(n,:,:)=s1;
   S2(n,:,:)=s2;
   zerosp(n,:)=zsp;
end;
t=tn;
C=squeeze(C); phi=squeeze(phi);S12=squeeze(S12); S1=squeeze(S1); S2=squeeze(S2);zerosp=squeeze(zerosp);
if nargout > 9; confC=confc; end;
if nargout==11;Cerr=squeeze(Cerr);end;
% if nargout==10; phierr=squeeze(phierr);end
if nargout==10; phistd=squeeze(phistd);end


function y = log_sum_exp( x, dim )

%LOG_SUM_EXP    log(sum(exp(x))).
%   LOG_SUM_EXP(X) = LOG(SUM(EXP(X)).
%
%   When used in a CVX model, LOG_SUM_EXP(X) causes CVX's successive
%   approximation method to be invoked, producing results exact to within
%   the tolerance of the solver. This is in contrast to LOGSUMEXP_SDP,
%   which uses a single SDP-representable global approximation.
%
%   If X is a matrix, LOGSUMEXP_SDP(X) will perform its computations
%   along each column of X. If X is an N-D array, LOGSUMEXP_SDP(X)
%   will perform its computations along the first dimension of size
%   other than 1. LOGSUMEXP_SDP(X,DIM) will perform its computations
%   along dimension DIM.
%
%   Disciplined convex programming information:
%       LOG_SUM_EXP(X) is convex and nondecreasing in X; therefore, X
%       must be convex (or affine).

error( nargchk( 1, 2, nargin ) );
if ~isreal( x ),
    error( 'Argument must be real.' );
end
y = exp( x );
if nargin == 1,
    y = sum( y );
else
    y = sum( y, dim );
end
y = log( y );

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [omega,u,eqn,info] = fourCurl3P2(node,elem,bdFlag,pde,option)

%% fourCurl3P2: the fourth order curl problem in 3D
%
% [w,u,eqn,info] = fourCurl3P2(node,elem,bdFlag,pde,option)
% uses the first P2 elements to approximate the velocity u and
% the stream function w.
%
% We solve the following equations:
%  -w + curl curl u = 0
%  curl curl w + u  = f
% u\times n = (curl u )\times n  = 0  on \partial \Omega
%
% please check fourCurl3doc for details.
%
% Lin Zhong, June 2013.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Set up optional input arguments
if ~exist('bdFlag','var'), bdFlag = []; end
if ~exist('option','var'), option = []; end

%% Sort elem to asend ordering
elemunSort = elem;
[elem,bdFlag] = sortelem3(elemunSort,bdFlag);

%% Construct Data Structure
[Dlambda,volume] = gradbasis3(node,elem);
volume = abs(volume);
%---------------------------------------------%
% locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
% locFace = [2 3 4; 1 3 4; 1 2 4; 1 2 3];
locBasesIdx = [1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % phi
               1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % psi
               3 2 4; 3 1 4; 2 1 4; 2 1 3; ...
               4 2 3; 4 1 3; 4 1 2; 3 1 2]; % chi

%---------------------------------------------%
[elem2edge,edge] = dof3edge(elem);
[elem2face,face] = dof3face(elem);
%---------------------------------------------%
NE = size(edge,1);
NF = size(face,1);
NT = size(elem,1);
Ndof = 2*(NE+NF);
%---------------------------------------------%
elem2dof = [elem2edge elem2edge+NE elem2face+2*NE elem2face+2*NE+NF];
face2edge = zeros(NF,3,'int32');
face2edge(elem2face(:,1),:) = elem2edge(:,[4 5 6]);
face2edge(elem2face(:,2),:) = elem2edge(:,[2 3 6]);
face2edge(elem2face(:,3),:) = elem2edge(:,[1 3 5]);
face2edge(elem2face(:,4),:) = elem2edge(:,[1 2 4]);

tic;

%% Assemble Matrix
DiDjcross = zeros(NT,3,4,4);
for i = 1:4
    for j = i+1:4
        DiDjcross(:,:,i,j) = mycross(Dlambda(:,:,i),Dlambda(:,:,j),2);
        DiDjcross(:,:,j,i) = -DiDjcross(:,:,i,j);
    end
end
DiDj = zeros(NT,4,4);
for i = 1:4
    for j = i:4
        DiDj(:,i,j) = dot(Dlambda(:,:,i),Dlambda(:,:,j),2);
        DiDj(:,j,i) = DiDj(:,i,j);
    end
end

ii = zeros(210*NT,1); jj = zeros(210*NT,1);
index = 0;
for i = 1:20
    for j = i:20
        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        i1 = locBasesIdx(i,1); i2 = locBasesIdx(i,2); i3 = locBasesIdx(i,3);
        j1 = locBasesIdx(j,1); j2 = locBasesIdx(j,2); j3 = locBasesIdx(j,3);
        Aij = zeros(NT,1);  Mij = zeros(NT,1);
        %% curl-curl matrix
        if (i<=6) && (j<=6)
            Aij = 4*dot(DiDjcross(:,:,i1,i2),DiDjcross(:,:,j1,j2),2);
        end
        if (i<=6) && (j>12)
            Aij = dot(DiDjcross(:,:,i1,i2),...
            DiDjcross(:,:,j1,j3)-DiDjcross(:,:,j1,j2)+2*DiDjcross(:,:,j2,j3),2)/2;
        end
        if (i>12) && (j>12)
        % curl chi_i =  Dlambda_{i1}mycross phi_{i2,i3} + lambda_{i1}curl phi_{i2,i3}
        % curl chi_j =  Dlambda_{j1}mycross phi_{j2,j3} + lambda_{j1}curl phi_{j2,j3}
        % (Dlambda_{i1}mycross phi_{i2,i3}) dot (Dlambda_{j1}mycross phi_{j2,j3})
            temp11 = ((1+(i2==j2))*dot(DiDjcross(:,:,i1,i3),DiDjcross(:,:,j1,j3),2) ...
                    - (1+(i2==j3))*dot(DiDjcross(:,:,i1,i3),DiDjcross(:,:,j1,j2),2) ...
                    - (1+(i3==j2))*dot(DiDjcross(:,:,i1,i2),DiDjcross(:,:,j1,j3),2) ...
                    + (1+(i3==j3))*dot(DiDjcross(:,:,i1,i2),DiDjcross(:,:,j1,j2),2))/20;
            % lambda_{i1}curl phi_{i2,i3} dot lambda_{j1}curl phi_{j2,j3}
            temp22 = (1+(i1==j1))/5*dot(DiDjcross(:,:,i2,i3),DiDjcross(:,:,j2,j3),2);
            % Dlambda_{i1}mycross phi_{i2,i3} dot lambda_{j1}curl phi_{j2,j3}
            temp12 = dot( (1+(j1==i2))*DiDjcross(:,:,i1,i3) ...
                        - (1+(j1==i3))*DiDjcross(:,:,i1,i2), ...
                                       DiDjcross(:,:,j2,j3),2)/10;
            % Dlambda_{j1}mycross phi_{j2,j3} dot lambda_{i1}curl phi_{i2,i3}
            temp21 = dot( (1+(i1==j2))*DiDjcross(:,:,j1,j3) ...
                        - (1+(i1==j3))*DiDjcross(:,:,j1,j2), ...
                                       DiDjcross(:,:,i2,i3),2)/10;
            Aij = temp11 + temp22 + temp12 + temp21;
        end
        if (i<=6) && (j<=6)
            % block 1: (phi_i,phi_j)
            Mij = 1/20*((1+(i1==j1))*DiDj(:,i2,j2) ...
                      - (1+(i1==j2))*DiDj(:,i2,j1) ...
                      - (1+(i2==j1))*DiDj(:,i1,j2) ...
                      + (1+(i2==j2))*DiDj(:,i1,j1));
        end
        if (i<=6) && (7<=j) && (j<=12)
            % block 2: (psi_j,phi_i)
            Mij = 1/20*( (1+(j1==i1))*DiDj(:,j2,i2) ...
                       - (1+(j1==i2))*DiDj(:,j2,i1) ...
                       + (1+(j2==i1))*DiDj(:,j1,i2) ...
                       - (1+(j2==i2))*DiDj(:,j1,i1));

        end
        if (7<=i) && (i<=12) && (7<=j) && (j<=12)
            % block 3: (psi_j,psi_i)
            Mij = 1/20*((1+(i1==j1))*DiDj(:,i2,j2) ...
                      + (1+(i1==j2))*DiDj(:,i2,j1) ...
                      + (1+(i2==j1))*DiDj(:,i1,j2) ...
                      + (1+(i2==j2))*DiDj(:,i1,j1));
        end
        if (i<=6) && (j>12)
            % block 4: (chi_j,phi_i)
            Mij = intlambda([j1,i1,j2],3)*DiDj(:,i2,j3) ...
                 -intlambda([j1,i1,j3],3)*DiDj(:,i2,j2) ...
                 -intlambda([j1,i2,j2],3)*DiDj(:,i1,j3) ...
                 +intlambda([j1,i2,j3],3)*DiDj(:,i1,j2);
        end
        if (7<=i) && (i<=12) && (j>12)
            % block 5: (chi_j,psi_i)
            Mij = intlambda([j1,i1,j2],3)*DiDj(:,i2,j3) ...
                 -intlambda([j1,i1,j3],3)*DiDj(:,i2,j2) ...
                 +intlambda([j1,i2,j2],3)*DiDj(:,i1,j3) ...
                 -intlambda([j1,i2,j3],3)*DiDj(:,i1,j2);
        end
        if (i>12) && (j>12)
            % block 6: (chi_j,chi_i)
            Mij = intlambda([i1,j1,i2,j2],3)*DiDj(:,i3,j3) ...
                 -intlambda([i1,j1,i2,j3],3)*DiDj(:,i3,j2) ...
                 -intlambda([i1,j1,i3,j2],3)*DiDj(:,i2,j3) ...
                 +intlambda([i1,j1,i3,j3],3)*DiDj(:,i2,j2);
        end
        Aij = Aij.*volume;
        Mij = Mij.*volume;
        sA(index+1:index+NT) = Aij;
        sM(index+1:index+NT) = Mij;
        index = index + NT;
    end
end
clear curlBasis_i curlBasis_j basis_i basis_j
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
A = A + AU + AU';
M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),Ndof,Ndof);
MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),Ndof,Ndof);
M = M + MU + MU';
clear AU MU

% Whole matrix
bigA = [-M A; A M];

%% Righthand side
f = zeros(Ndof,1);
if ~isfield(pde,'f') || (isfield(pde,'f') && isreal(pde.f) && all(pde.f==0))
    pde.f = [];
end
if ~isfield(option,'fquadorder')
    option.fquadorder = 4;   % default order is 4
end
if isfield(pde,'f') && ~isempty(pde.f)
    [lambda,w] = quadpts3(option.fquadorder); % quadrature order is 4
    nQuad = size(lambda,1);
    bt = zeros(NT,20);
    for p = 1:nQuad
        % quadrature points in the x-y-z coordinate
        pxyz = lambda(p,1)*node(elem(:,1),:) ...
             + lambda(p,2)*node(elem(:,2),:) ...
             + lambda(p,3)*node(elem(:,3),:) ...
             + lambda(p,4)*node(elem(:,4),:);
        fp = pde.f(pxyz);
        for k = 1:20
            k1 = locBasesIdx(k,1);
            k2 = locBasesIdx(k,2);
            k3 = locBasesIdx(k,3);
            % evaluate basis at quadrature points
            if k<=6
            % phi_k = lambda_{k1}Dlambda_{k2} - lambda_{k2}Dlambda_{k1};
                basis_k = (lambda(p,k1)*Dlambda(:,:,k2) ...
                          -lambda(p,k2)*Dlambda(:,:,k1));
            elseif k<=12
            % psi_k = lambda_{k1}Dlambda_{k2} + lambda_{k2}Dlambda_{k1};
                basis_k = (lambda(p,k1)*Dlambda(:,:,k2) ...
                          +lambda(p,k2)*Dlambda(:,:,k1));
            else
            % chi_k = lambda_{k1}phi_{k2,k3};
                basis_k = lambda(p,k1)*(lambda(p,k2)*Dlambda(:,:,k3) ...
                                       -lambda(p,k3)*Dlambda(:,:,k2));
            end
            rhs = dot(basis_k,fp,2);
            bt(:,k) = bt(:,k) + w(p)*rhs;
        end
    end
    bt = bt.*repmat(volume,1,20);
    f = accumarray(elem2dof(:),bt(:),[Ndof 1]);
end
clear pxy fp bt rhs basis_k

bigf = [zeros(Ndof,1); f];
assembleTime = toc;

%% Boundary condition
% Find Dirichlet boundary dof: fixedDof
if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N)
    % Dirichlet boundary condition only
    bdFlag = setboundary3(node,elem,'Dirichlet');
end
isBdDof = false(Ndof,1);

if ~isempty(bdFlag)
    %% Dirichlet boundary condition on edge dofs
    % Find boundary faces, edges and nodes
    isBdFace = false(NF,1);
    isBdFace(elem2face(bdFlag(:,1) == 1,1)) = true;
    isBdFace(elem2face(bdFlag(:,2) == 1,2)) = true;
    isBdFace(elem2face(bdFlag(:,3) == 1,3)) = true;
    isBdFace(elem2face(bdFlag(:,4) == 1,4)) = true;
%     boundaryFace = face(isBdFace,:);
    bdFace2edge = face2edge(isBdFace,:);
    isBdEdge = false(NE,1);
    isBdEdge(bdFace2edge(:)) = true;
    edgeBdDof = [find(isBdEdge); NE + find(isBdEdge)];
%     bdEdge = edge(isBdEdge,:);
%     isBdNode(bdEdge) = true;
%     bdNode = find(isBdNode);
    faceBdDof = 2*NE + [find(isBdFace); NF+find(isBdFace)];
%     edgeIdxMap = zeros(NE,1);
%     edgeIdxMap(isBdEdge) = 1:size(bdEdge,1);
%     bdFace2edge = edgeIdxMap(bdFace2edge);
    isBdDof(edgeBdDof) = true;
    isBdDof(faceBdDof) = true;
end

freeDof = ~isBdDof;

if any(freeDof)
    idx = [true(Ndof,1); freeDof];
    bigA_bd= bigA(idx,idx);
    bigf_bd = bigf(idx);
end

%% Solve
t = cputime;
bigu = bigA_bd\bigf_bd;
omega = bigu(1:Ndof);
u = zeros(Ndof,1);
u(freeDof) = bigu(Ndof+1:end);
info.solverTime = cputime -t;
display(info.solverTime);

%% Output information
eqn = struct('elem2edge',elem2edge,'elem2face',elem2face,'face2edge',face2edge,'A',A,'edge',edge,'face',face,'M',M,'f',f);
info.assembleTime = assembleTime;
end


%filename='test2d_micro';
filename = 'MicroQuad';
ptype = 'MICRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'circleInclusion';
cost={'chomog_alphabeta'};
weights=[1];
constraint = {'volumeConstraint'};
constraint_case = 'EQUALITY';
%incrementFactor = 1;
%designVariable = 'Density';
designVariable = 'LevelSet';
filterType = 'P1';
fracRadius = 0.51;
%optimizer = 'IPOPT';
optimizer = 'DualNestedInPrimal';
%optimizer = 'AlternatingPrimalDual';

%optimizerUnconstrained = 'PROJECTED GRADIENT';
%line_search_initiator = 'INCREASING LAST STEP';
%incrementFactor = 1.95;

optimizerUnconstrained = 'SLERP';

nsteps = 1;
Vfrac_final = 0.4;
Perimeter_target=1;
optimality_final = 1*1e-3;
constr_final =1e-12;

Vfrac_initial = 0.8;
optimality_initial = 1*1e-3;
constr_initial = 1e-12;

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
epsilon_isotropy_initial=1e-1;
epsilon_isotropy_final = 1e-3;
micro.alpha =[1 1 0]';
micro.beta =[1 1 0]';

% For all tests
plotting = true;
printing = true;
monitoring = true;
monitoring_interval = 1;
maxiter = 100;


function train_CNN_model(varargin)

setup('useGPU', true);

tic

% Load the data
root_loc = 'D:\Datasets\detection_validation';
location = [root_loc, '/prep_data/'];

faceCheckersLoc_train = dir([location 'face_validator_train_*']);
faceCheckersLoc_test = dir([location 'face_validator_test_*']);

corrs_all = [];
rmses_all = [];

faceCheckers = struct;

% As we will be training a classifier that will act as a regressor,
% binarize it
num_bins = 30;

for i=1:numel(faceCheckersLoc_train)

    load([location faceCheckersLoc_train(i).name]);

    % set a max value to the error
    errors(errors > 3) = 3;
    errors_train = quantizeContinuous(errors, 0, 3, num_bins);
    examples_train = single(examples);
    clear examples

    mean_ex = mean(examples_train);
    std_ex = std(examples_train);
    std_ex = std_ex / 256;
    examples_train = bsxfun(@times, bsxfun(@minus, examples_train, mean_ex), 1./std_ex);
    num_examples_train = size(examples_train,1);

    % Add rows and columns untill we have 60 x 60 image

    % keep adding rows
    while(size(mask,1) < 60)
        mask = cat(1, mask, false(1, size(mask,2)));
        triX = cat(1, triX, -ones(1, size(mask,2)));
    end

    % keep adding cols
    while(size(mask,2) < 60)
        mask = cat(2, mask, false(size(mask,1),1));
        triX = cat(2, triX, -ones(size(triX,1),1));
    end

    examples_r = single(zeros(size(mask, 1), size(mask, 2), num_examples_train));

    img_curr = zeros(size(mask));
    for e=1:num_examples_train

        img_curr(mask) = examples_train(e,:);
        examples_r(:, :, e) = img_curr;

    end

    imdb.images.data = examples_r;
    clear examples_r
    imdb.images.label = errors_train';
    imdb.images.id = 1:numel(errors_train);

    % Split data for training and validation (20%)
    imdb.images.set = ones(1, numel(errors_train));
    imdb.images.set(round(4*end/5):end) = 2;

    % Visualize some of the data
    figure(10) ; clf ; colormap gray ;
    subplot(1,2,1) ;
    pos_samples = squeeze(imdb.images.data(:,:,imdb.images.label==1));
    vl_imarraysc(pos_samples(:,:,1:20)) ;
    axis image off ;
    title('Positive training data') ;

    subplot(1,2,2) ;
    neg_samples = squeeze(imdb.images.data(:,:,imdb.images.label>5));
    vl_imarraysc(neg_samples(:,:,1:20)) ;
    axis image off ;
    title('Negative training data') ;

    net = initializeFaceCNN(num_bins) ;

    trainOpts.batchSize = 100 ;
    trainOpts.numEpochs = 20;
    trainOpts.continue = true ;
    trainOpts.gpus = [1] ;
    trainOpts.learningRate = 0.001 ;
    trainOpts.expDir = ['trained/intermediate/face_validator_' num2str(i)];
    trainOpts.errorFunction = 'regression';
    trainOpts = vl_argparse(trainOpts, varargin);

    % Call training function in MatConvNet
    [net,info] = cnn_train_reg(net, imdb, @getBatch, trainOpts) ;

    % Move the CNN back to the CPU if it was trained on the GPU
    if numel(trainOpts.gpus) > 0
      net = vl_simplenn_move(net, 'cpu') ;
    end

    % Save the result for later use
    net.layers(end) = [] ;
    net.imageMean = mean_ex ;
    net.imageStd = std_ex;

%     save('data/face-experiment/facecnn.mat', '-struct', 'net') ;

    % Evaluate on the test data, also evaluate using correlation and RMSE
    load([location faceCheckersLoc_test(i).name]);

    % set a max value to the error
    errors(errors > 3) = 3;
    errors_test = errors;
    errors_q = quantizeContinuous(errors, 0, 3, num_bins);
    errors_test_q = errors_q;
    examples_test = single(examples);
    examples_test = bsxfun(@times, bsxfun(@minus, examples_test, net.imageMean), 1./net.imageStd);
    num_examples_test = size(examples_test,1);

    % keep adding rows
    while(size(mask,1) < 60)
        mask = cat(1, mask, false(1, size(mask,2)));
        triX = cat(1, triX, -ones(1, size(mask,2)));
    end

    % keep adding cols
    while(size(mask,2) < 60)
        mask = cat(2, mask, false(size(mask,1),1));
        triX = cat(2, triX, -ones(size(triX,1),1));
    end

    examples_r = single(zeros(size(mask, 1), size(mask, 2), num_examples_test));

    img_curr = zeros(size(mask));
    for e=1:num_examples_test

        img_curr(mask) = examples_test(e,:);
        examples_r(:, :, e) = img_curr;

    end
    examples_test = zeros(size(examples_r,1), size(examples_r,2), 1, size(examples_r,3));
    examples_test(:) = single(examples_r(:));

    ids = 1:99:num_examples_test;

    errors_all_test = zeros(num_examples_test,1);
    for k=1:numel(ids)-1
        examples_test_sm = single(examples_test(:,:,:,ids(k):ids(k+1)-1));
        res = vl_simplenn(net, examples_test_sm, [], []);
        res = gather(res(end).x);
        [~,res] = sort(res, 3, 'descend') ;
        res = squeeze(res);
        res = res(1,:);

        errors_test_rec = unQuantizeContinuous(res, 0, 3, num_bins)';
        errors_all_test(ids(k):ids(k+1)-1) = errors_test_rec;
    end
    errors_all_test = errors_all_test(1:ids(end));
    corrs_test = corr(errors_all_test, errors_test(1:numel(errors_all_test)));
    rmse_test = sqrt(mean((errors_all_test-errors_test(1:numel(errors_all_test))).^2));

    corrs_all = cat(1, corrs_all, corrs_test);
    rmses_all = cat(1, rmses_all, rmse_test);

    net.corrs = corrs_test;
    net.rmse = rmse_test;
    save([trainOpts.expDir, '/facecnn.mat'], '-struct', 'net');

    % The CNN
    faceCheckers(i).cnn = net;

    % The orientation
    faceCheckers(i).centres = centres;

    % The info for preprocessing
    faceCheckers(i).mask = mask;
    faceCheckers(i).nPix = nPix;
    faceCheckers(i).minX = minX;
    faceCheckers(i).minY = minY;

    faceCheckers(i).destination = shape(:,1:2);
    tri = load([location faceCheckersLoc_train(i).name], 'triangulation');
    faceCheckers(i).triangulation = tri.triangulation;
    faceCheckers(i).triX = triX;
    faceCheckers(i).mask = mask;
    faceCheckers(i).alphas = alphas;
    faceCheckers(i).betas = betas;

    faceCheckers(i).mean_ex = mean_ex;
    faceCheckers(i).std_ex = std_ex;

    WriteOutFaceCheckersCNNbinary("trained/validator_cnn.txt", faceCheckers);
end

save('trained/faceCheckers.mat', 'faceCheckers', 'corrs_all', 'rmses_all');

end
% --------------------------------------------------------------------
function [im, labels] = getBatch(imdb, batch)
% --------------------------------------------------------------------
    im = imdb.images.data(:,:,batch) ;
    im = reshape(im, 60, 60, 1, []) ;
    labels = imdb.images.label(1,batch) ;
end


function bvalues=scd_scheme_bvalue(scheme)
gyro = 42.57; % kHz/mT
scheme(:,8) = gyro*scheme(:,4).*scheme(:,6); % um-1
bvalues=(2*pi*scheme(:,8)).^2.*(scheme(:,5)-scheme(:,6)/3);
end


clear all
        % Desemnarea variabilei x ca si simbolica
x=sym('x');
		% Incarcarea functiei de integrat
F=1/x
        % Calcularea integralei nedefinite
I1=int(F)
        % Calcularea integralei definite intre limitele 1 si 2
I2=int(F,1,2)


% computes cell and neuropil fluorescence for surround model of neuropil
function [ops, stat, Fcell, FcellNeu] = extractSignalsSurroundNeuropil2(ops, stat)

Nk       = numel(stat); % all ROIs

Ny = numel(ops.yrange);
Nx = numel(ops.xrange);

stat = getNonOverlapROIs(stat, Ny, Nx);

[Ly, Lx] = size(ops.mimg);

% create cell masks and cell exclusion areas
[stat, cellPix, cellMasks] = createCellMasks(stat, Ny, Nx);

% create surround neuropil masks
[ops, neuropMasks] = createNeuropilMasks(ops, stat, cellPix);

% add surround neuropil masks to stat
for k = 1:Nk
    stat(k).ipix_neuropil = find(squeeze(neuropMasks(k,:,:))>0);
end

%%
radius0 = 2; % cells you choose
[stat, centerMasks, surroundMasks] = ...
    createCenterSurroundMasks2(ops, stat, Ny, Nx, 1, radius0);

ivcell = [stat.radius]>=radius0;
Nkv = sum(ivcell);
centerMasks = centerMasks(ivcell, :);
surroundMasks = surroundMasks(ivcell, :);
%%
% convert masks to sparse matrices for fast multiplication
neuropMasks = sparse(double(neuropMasks(:,:)));
cellMasks   = sparse(double(cellMasks(:,:)));
centerMasks = sparse(double(centerMasks(:,:)));
surroundMasks   = sparse(double(surroundMasks(:,:)));

%% get fluorescence and surround neuropil
nimgbatch = 2000;
ix = 0;
fclose all;
fid = fopen(ops.RegFile, 'r');

tic
F = NaN(Nk, sum(ops.Nframes), 'single');
Fneu = NaN(Nk, sum(ops.Nframes), 'single');
cF = NaN(Nkv, sum(ops.Nframes), 'single');
sF = NaN(Nkv, sum(ops.Nframes), 'single');

while 1
    data = fread(fid,  Ly*Lx*nimgbatch, '*int16');
    if isempty(data)
        break;
    end

    data = reshape(data, Ly, Lx, []);
    data = data(ops.yrange, ops.xrange, :);
    NT   = size(data,3);
    data = reshape(data, [], NT);
    data = double(data);

    % process the data
    %data = my_conv2(data, ops.sig, [1 2]);

    % compute cell fluorescence
    % each mask is weighted by lam (SUM TO 1)
    F(:,ix + (1:NT)) = cellMasks * data;

    % compute neuropil fluorescence
    Fneu(:,ix + (1:NT)) = neuropMasks * data;

    % compute neuropil fluorescence
    cF(:,ix + (1:NT)) = centerMasks * data;
    sF(:,ix + (1:NT)) = surroundMasks * data;

    ix = ix + NT;
    if rem(ix, 3*NT)==0
        fprintf('Frame %d done in time %2.2f \n', ix, toc)
    end
end
fclose(fid);

%% get z drift
if getOr(ops, 'getZdrift', 0)
    cFt = my_conv2(cF,.5, 2);
    sFt = my_conv2(sF,.5, 2);

    cFt = ordfilt2(cFt,1, true(1,100),[], 'symmetric');
    sFt = ordfilt2(sFt,1, true(1,100),[], 'symmetric');

    ratCS = log(max(1e-4,cFt )) - log(max(1e-4,sFt));
    ratCS = zscore(ratCS, 1, 2);

    [u, s, v] = svdecon(ratCS);

    ops.zdrift = v(:,1);

    figure(10)
    plot(v(:,1))
    hold all
end
%%
% keyboard;

% get activity stats
[stat, F, Fneu] = getActivityStats(ops, stat, F, Fneu);

%
csumNframes = [0 cumsum(ops.Nframes)];
Fcell       = cell(1, length(ops.Nframes));
FcellNeu    = cell(1, length(ops.Nframes));
for i = 1:length(ops.Nframes)
    Fcell{i}     = F(:, csumNframes(i) + (1:ops.Nframes(i)));
    FcellNeu{i}  = Fneu(:, csumNframes(i) + (1:ops.Nframes(i)));
end


% This file performs the dual channel speech enhancement based on GSC beamformer and postfilter.
% Refer to "Analysis of Two-Channel Generalized Sidelobe Canceller (GSC) With Post-Filtering".

clear
clc

%% Load audio file

audioname = 'carmix';
[x, fs0 ] = audioread([audioname,'.wav']);
fs      = 16e3;
x       = resample(x,fs,fs0);
Ntime   = size(x,1);
Nmic    = size(x,2);
if Nmic ~= 2
    error('The number of channels does not equal 2!')
end

%% Set parameters

NFFT        = 256;
f           = 0:fs/NFFT:fs/2;
full        = 0:fs/NFFT:fs-fs/NFFT;
len         = 17e-2;
fragsize    = 256;
overlap     = 0.5;
c           = 340;
lamdaB0     = 1.54;
omegalow    = 1;
omegahigh   = 3;
gamma0      = 4.6;
alpha       = 0.92;
alphas      = 0.80;
alphadconst = 0.85;
KL          = 9;
KR          = 50;
phi0        = 0.25;
% b           = [1 3 4 3 1] / 12;
b           = hanning(5).';
b           = b/sum(b);
Gmin        = 10^(-20/20); % -20dB
vmax        = 5;
EPIS        = 1E-10;
angle1      = 0;
angle2      = 0.1;
xia         = 0.98;
ximin       = 10^(-15/10);

%% set window

win_1       = hanning(fragsize).';
win_2       = hanning(NFFT).';
win_pre     = sqrt(win_1);
win_post    = sqrt(win_2);
win_pre     = sqrt(win_pre);
win_post    = sqrt(win_post);
scale_fac   = sqrt(NFFT/sum(win_1));
scale_postfac = 1.0 / sqrt(NFFT/sum(win_2));
win_pre     = win_pre * scale_fac;
win_post    = win_post * scale_postfac;

%% Cutframes

% ptout = zeros(Nmic, Nframe, fragsize);
for m = 1:Nmic
    [ptout(m,:,:), Nframe] = cutframe(x(:,m), fragsize, overlap);
end

%% Process

Z       = zeros(Nmic,NFFT/2+1);
pt      = zeros(Nmic,fragsize);
pxx     = zeros(NFFT,Nmic);
q       = ones(Nframe,NFFT/2+1);
p       = zeros(Nframe,NFFT/2+1);
yxx     = zeros(Nframe,NFFT/2+1);
yt      = zeros(Nframe,NFFT);
alphad  = zeros(Nframe,NFFT/2+1);
v       = zeros(Nframe,NFFT/2+1);
gamma   = ones(Nframe,NFFT/2+1);
gammas  = ones(Nframe,NFFT/2+1);
xi      = zeros(Nframe,NFFT/2+1);
phi     = zeros(Nframe,NFFT/2+1);
SY      = zeros(Nframe,NFFT);
MY      = zeros(Nframe,NFFT);
SU      = zeros(Nframe,NFFT);
MU      = zeros(Nframe,NFFT);
lamdad  = zeros(Nframe,NFFT/2+1);
GH1     = zeros(Nframe,NFFT/2+1);
G       = ones(Nframe,NFFT/2+1);
lamdaBY = zeros(Nframe,NFFT/2+1);
lamdaBU = zeros(Nframe,NFFT/2+1);
omega   = zeros(Nframe,NFFT/2+1);
phi_tilt = zeros(1,Nframe);
YMAT    = zeros(Nframe,NFFT);
UMAT    = zeros(Nframe,NFFT);

deltak  = 2*pi*full * len * sin(angle1*pi/180) / c + angle2*pi/180;
W       = 0.5 * conj([exp(1i*deltak/2); exp(-1i*deltak/2)]);
B       = 0.5 * conj([exp(1i*deltak/2); -exp(-1i*deltak/2)]);
coh     = sinc(2*pi*full*len/c);       % diffused noise field
H       = 1i*imag(exp(1i*deltak).*coh) ./ (1-real(exp(1i*deltak).*coh));

% compute OMEGA_HIGH
% sincwl = min(sinc(2*pi*f*len/c), 0.998);
% OMEGA_HIGH = 0.57 * cot(deltak(1:NFFT/2+1)/2).^2 .* (1-sincwl).^2 ./ max((1-sincwl.*cos(deltak(1:NFFT/2+1))).^2, EPIS);
% OMEGA_HIGH = 0.57 * cot(deltak(1:NFFT/2+1)/2).^2;
% OMEGA_HIGH = 0.57 * sin(2*pi*f*len*sin(0)/c).^2 ./ (sin(deltak(1:NFFT/2+1)/2).^2 .* sin(2*pi*f*len*sin(0)/c-deltak(1:NFFT/2+1)/2).^2);

hbar = waitbar(0,'Please wait...');
for n = 1:Nframe
    waitbar(n/Nframe,hbar);

%     % ***************   GSC part   **************************************
%     pt1 = [zeros(1,(NFFT-fragsize)/2), reshape(ptout(1,n,:),1,fragsize), zeros(1,(NFFT-fragsize)/2)];
%     pt2 = [zeros(1,(NFFT-fragsize)/2), reshape(ptout(2,n,:),1,fragsize), zeros(1,(NFFT-fragsize)/2)];
%     pt  = [ pt1 .* win_pre.' ; pt2 .* win_pre.'];
    pt1 = [reshape(ptout(1,n,:),1,fragsize) .* win_pre, zeros(1,NFFT-fragsize)];
    pt2 = [reshape(ptout(2,n,:),1,fragsize) .* win_pre, zeros(1,NFFT-fragsize)];
    pt  = [ pt1 .' pt2.' ];
    pxx = fft(pt,NFFT);
    Z   = pxx.';
    Y   = sum(conj(W) .* Z) - conj(H) .* sum(conj(B) .* Z);
    U   = sum(conj(B) .* Z);

    % **********   Postfilter part   *************************************
    if n == 1
        SY(n,:)     = abs(Y).^2;
        MY(n,:)     = abs(Y).^2;
        lamdad(n,:) = abs(Y(1:NFFT/2+1)).^2;
        SU(n,:)     = abs(U).^2;
        MU(n,:)     = abs(U).^2;
        GH1(n,:)    = ones(1,NFFT/2+1);
        gamma(n,:)  = ones(1,NFFT/2+1);

        param_Y = initialise_parameters(MY(n,:).',fs,'imcra');
        param_U = initialise_parameters(MU(n,:).',fs,'imcra');

        alphad(n,:)     = alphadconst + (1-alphadconst) * p(n,:);
        lamdad(n+1,:)   = alphad(n,:) .* lamdad(n,:) + (1-alphad(n,:)) .* abs(Y(1:NFFT/2+1)).^2;

        xi(n,:) = xia + (1-xia) * max(gamma(n,:)-1,0);     % initialization of a priori SNR
    else
        Ymat    = buildmat(Y,length(b));
        Umat    = buildmat(U,length(b));
        SY(n,:) = alphas * SY(n-1,:) + (1-alphas) * b * abs(Ymat).^2;
        SU(n,:) = alphas * SU(n-1,:) + (1-alphas) * b * abs(Umat).^2;
        param_Y = noise_estimation((abs(Y).^2).','imcra',param_Y);
        param_U = noise_estimation((abs(U).^2).','imcra',param_U);
        MY(n,:) = param_Y.noise_ps.';
        MU(n,:) = param_U.noise_ps.';

        lamdaBY(n,:)    = SY(n,1:NFFT/2+1) ./ max(MY(n,1:NFFT/2+1), EPIS);
        lamdaBU(n,:)    = SU(n,1:NFFT/2+1) ./ max(MU(n,1:NFFT/2+1), EPIS);

%         omega(n,:)      = max(SY(n,1:NFFT/2+1) - MY(n,1:NFFT/2+1), 0) ./ max(SU(n,1:NFFT/2+1) - MU(n,1:NFFT/2+1), 0.001*MY(n,1:NFFT/2+1));
        omega(n,:)      = max(SY(n,1:NFFT/2+1) - MY(n,1:NFFT/2+1), 0) ./ max(SU(n,1:NFFT/2+1) - MU(n,1:NFFT/2+1), EPIS);

        % decide q according to fig.4
        for k = 1:NFFT/2+1
            if lamdaBY(n,k) > lamdaB0
                if lamdaBU(n,k) > lamdaB0
                    % ***** compute phi, using eq.33 ***********
                    if omega(n,k) <= omegalow
                        phi(n,k) = 0;
                    else
                        if omegalow < omega(n,k) && omega(n,k) <= omegahigh      % OMEGA_HIGH(k)       % replace omegahigh with OMEGA_HIGH(k)
                            phi(n,k) = (omega(n,k) - omegalow) / (omegahigh - omegalow);
%                         if omegalow < omega(n,k) <= OMEGA_HIGH(k)     % OMEGA_HIGH(k)       % replace omegahigh with OMEGA_HIGH(k)
%                             phi(n,k) = (omega(n,k) - omegalow) / (OMEGA_HIGH(k) - omegalow);
                        else
                            phi(n,k) = 1;
                        end
                    end
                    % ******************************************
                else
                    phi(n,k) = 1;
                end
            else
                phi(n,k) = 0;
            end
        end

        % compute eq.34
        phi_tilt(n) = sum(phi(n,KL:KR)) / (KR-KL+1);

        gammas(n,:) = abs(Y(1:NFFT/2+1)).^2 ./ MY(n,1:NFFT/2+1);

        % compute q, using eq.35
        if phi_tilt(n) > phi0
            for k = 1:NFFT/2+1
                if (gammas(n,k) <= 1)  || (phi_tilt(n) <= phi0)
                    q(n,k) = 1;
                else
                    q(n,k) = max( (gamma0-gammas(n,k))/(gamma0-1) , 1-phi(n,k) );
                end
            end
        else
            q(n,:) = ones(1,NFFT/2+1);
        end

        q(n,:) = min(q(n,:),1);
        q(n,:) = max(q(n,:),0);

        % compute xi, using eq.37
        gamma(n,:) = abs(Y(1:NFFT/2+1)).^2 ./ max(lamdad(n,1:NFFT/2+1), EPIS);

        xi(n,:) = alpha * GH1(n-1,:).^2 .* gamma(n-1,:) + (1-alpha) * max(gamma(n,:)-1,0);
        xi(n,:) = max(xi(n,:), ximin);
        v(n,:)  = gamma(n,:) .* xi(n,:) ./ (1+xi(n,:));

        % compute GH1, using eq.38
        GH1(n,:)        = ones(1,NFFT/2+1);
        index           = find(v(n,:) > vmax);
        GH1(n,index)    = xi(n,index) ./ (1+xi(n,index));
        index           = find(0<v(n,:) & v(n,:)<=vmax);
        GH1(n,index)    = xi(n,index) .* exp(0.5*expint(v(n,index)))./ (1+xi(n,index));

        % compute p, using eq.36
        p(n,:) = 1 ./ (1 + q(n,:) .* (1+xi(n,:)).*exp(-v(n,:)) ./ (1-q(n,:)+EPIS));
        p(n,:) = min(p(n,:), 1);
        p(n,:) = max(p(n,:), 0);

        % compute eq.39 eq.40
        alphad(n,:) = alphadconst + (1-alphadconst) * p(n,:);
        if n < Nframe
            lamdad(n+1,:) = alphad(n,:) .* lamdad(n,:) + (1-alphad(n,:)) .* abs(Y(1:NFFT/2+1)).^2;
        end

    end

    % apply filter and ifft
    G(n,:)      = (GH1(n,:).^p(n,:)) .* (Gmin.^(1-p(n,:)));
    yxx(n,:)    = G(n,:) .* Y(1:NFFT/2+1);
    yt(n,:)     = real(ifft([yxx(n,:),conj(fliplr(yxx(n,2:end-1)))],NFFT));
    yt(n,:)     = yt(n,:) .* win_post;

end
close(hbar)

%% OLA

yout = ola(yt,fragsize,overlap);

%% .wav output

audiowrite([audioname,'_tc_out_',num2str(fs),'_',num2str(NFFT),'_',num2str(fragsize),'.wav'], yout, fs);


clear all
close all
clc

% csidata;
% csi2batch;
getbatchdata;
gettestbatchdata;
fprintf(1,'finished');
%%
load batchdatalong;
[numcases numdims numbatches]=size(batchdata);
maxepoch=4; %In the Science paper we use maxepoch=50, but it works just fine.
numhid=100; numpen=50; numpen2=30; numopen=30;

fprintf(1,'Pretraining Layer 1 with RBM: %d-%d \n',numdims,numhid);
restart=1;
rbm;
hidrecbiases=hidbiases;
save mnistvh vishid hidrecbiases visbiases;
%%

fprintf(1,'\nPretraining Layer 2 with RBM: %d-%d \n',numhid,numpen);
batchdata=batchposhidprobs;
numhid=numpen;
restart=1;
rbm;
hidpen=vishid; penrecbiases=hidbiases; hidgenbiases=visbiases;
save mnisthp hidpen penrecbiases hidgenbiases;
%%
fprintf(1,'\nPretraining Layer 3 with RBM: %d-%d \n',numpen,numpen2);
batchdata=batchposhidprobs;
numhid=numpen2;
restart=1;
rbm;
hidpen2=vishid; penrecbiases2=hidbiases; hidgenbiases2=visbiases;
save mnisthp2 hidpen2 penrecbiases2 hidgenbiases2;
%%
fprintf(1,'\nPretraining Layer 4 with RBM: %d-%d \n',numpen2,numopen);
batchdata=batchposhidprobs;
numhid=numopen;
restart=1;
rbmhidlinear;
hidtop=vishid; toprecbiases=hidbiases; topgenbiases=visbiases;
save mnistpo hidtop toprecbiases topgenbiases;
% clear all
fprintf(1,'finished');
%%
load batchdata55;
load testbatchdata55;
% load testbatchdata44;
backprop;
% testerror('testbatchdata13');


function [ out ] = laplaceSP(a,rho)

if length(a)==1
    b= 1;
else
    b = a(2);
    a = a(1);
end
F = @(r)(Cp(rho).*besselj(0,rho*r));
out = 2*pi*(F(b) - F(a));

end


function [g]= spm_gx_dem_reach(x,v,P)
% returns the prediction for a two-joint arm
% FORMAT [g]= spm_gx_dem_reach(x,v,P)
%
% x    - hidden states
%   x(1) - joint angle
%   x(2) - joint angle
%   x(3) - angular velocity
%   x(4) - angular velocity
% v    - causal states
%   v(1) - target location (x)
%   v(2) - target location (y)
%   v(3) - force (cue strength)
% P    - parameters
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_gx_dem_reach.m 3893 2010-05-17 18:28:52Z karl $

% evaluate positions
%--------------------------------------------------------------------------
J  = spm_dem_reach_x2J(x);

% stretch (angular) and visual (positional) information (target & arm)
%==========================================================================
g  = [x(1:2); v; J{1} + J{2}];


function pick = nmsMe(boxes, overlap)
% Non-maximum suppression. (FAST VERSION)
% Greedily select high-scoring detections and skip detections
% that are significantly covered by a previously selected
% detection.
% NOTE: This is adapted from Pedro Felzenszwalb's version (nms.m),
% but an inner loop has been eliminated to significantly speed it
% up in the case of a large number of boxes

% modified based on Tomasz Malisiewicz's esvm code

if isempty(boxes)
  pick = [];
  return;
end

x1 = boxes(:,1);
y1 = boxes(:,2);
x2 = boxes(:,3);
y2 = boxes(:,4);
s = boxes(:,end);

area = (x2-x1+1) .* (y2-y1+1);
[~, I] = sort(s);

pick = s*0;
counter = 1;
while ~isempty(I)

  last = length(I);
  i = I(last);
  pick(counter) = i;
  counter = counter + 1;

  xx1 = max(x1(i), x1(I(1:last-1)));
  yy1 = max(y1(i), y1(I(1:last-1)));
  xx2 = min(x2(i), x2(I(1:last-1)));
  yy2 = min(y2(i), y2(I(1:last-1)));

  w = max(0.0, xx2-xx1+1);
  h = max(0.0, yy2-yy1+1);

  inter = w.*h;
  o = inter ./ (area(i) + area(I(1:last-1)) - inter);

  I([last; find(o>overlap)]) = [];
end

pick = pick(1:(counter-1));


function z = gt(x,y)
%GT Greater than for sptensors.
%
%   See also SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Observations for sparse matrix case.
% The result of a > 5 is sparse.
% The result of a > 0 is sparse.
% The result of a > full(a) is sparse.
% The result of a > zeros(a) is sparse.

%% Case 1: One argument is a scalar
if isscalar(y)
    subs1 = x.subs((x.vals > y),:);
    if y < 0
        subs2 = setdiff(allsubs(x),x.subs,'rows');
    else
        subs2 = [];
    end
    z = sptensor([subs1;subs2],true,size(x));
    return;
end

% Call back with the arguments reversed.
if isscalar(x)
    z = lt(y,x);
    return;
end

%% Case 2: Both x and y are tensors of some sort
% Check that the sizes match
if ~isequal(x.size,y.size)
    error('Size mismatch');
end

% Case 2a: Two sparse tensors
if isa(x,'sptensor') && isa(y,'sptensor')
    z = lt(y,x);
    return;
end

% Case 2b: One dense tensor
if isa(y,'tensor')

    % x zero and y < 0
    subs1 = find(y < 0);
    if ~isempty(subs1)
        subs1 = setdiff(subs1,x.subs,'rows');
    end

    % x and y nonzero
    subs2 = x.subs(x.vals > y(x.subs,'extract'),:);

    % assemble
    z = sptensor([subs1;subs2],true,size(x));

    return;

end

%% Otherwise
error('The arguments must be two sptensors or an sptensor and a scalar.');


%MAT_INTO_HDR  The old versions of SPM (any version before SPM5) store
%	an affine matrix of the SPM Reoriented image into a matlab file
%	(.mat extension). The file name of this SPM matlab file is the
%	same as the SPM Reoriented image file (.img/.hdr extension).
%
%	This program will convert the ANALYZE 7.5 SPM Reoriented image
%	file into NIfTI format, and integrate the affine matrix in the
%	SPM matlab file into its header file (.hdr extension).
%
%	WARNING: Before you run this program, please save the header
%	file (.hdr extension) into another file name or into another
%	folder location, because all header files (.hdr extension)
%	will be overwritten after they are converted into NIfTI
%	format.
%
%  Usage: mat_into_hdr(filename);
%
%  filename:	file name(s) with .hdr or .mat file extension, like:
%		'*.hdr', or '*.mat', or a single .hdr or .mat file.
%	e.g.	mat_into_hdr('T1.hdr')
%		mat_into_hdr('*.mat')
%

%  - Jimmy Shen (jimmy@rotman-baycrest.on.ca)
%
%-------------------------------------------------------------------------
function mat_into_hdr(files)

   pn = fileparts(files);
   file_lst = dir(files);
   file_lst = {file_lst.name};
   file1 = file_lst{1};
   [p n e]= fileparts(file1);

   for i=1:length(file_lst)
      [p n e]= fileparts(file_lst{i});
      disp(['working on file ', num2str(i) ,' of ', num2str(length(file_lst)), ': ', n,e]);
      process=1;

      if isequal(e,'.hdr')
         mat=fullfile(pn, [n,'.mat']);
         hdr=fullfile(pn, file_lst{i});

         if ~exist(mat,'file')
            warning(['Cannot find file "',mat  , '". File "', n, e, '" will not be processed.']);
            process=0;
         end
      elseif isequal(e,'.mat')
         hdr=fullfile(pn, [n,'.hdr']);
         mat=fullfile(pn, file_lst{i});

         if ~exist(hdr,'file')
            warning(['Can not find file "',hdr  , '". File "', n, e, '" will not be processed.']);
            process=0;
         end
      else
         warning(['Input file must have .mat or .hdr extension. File "', n, e, '" will not be processed.']);
         process=0;
      end

      if process
         load(mat);
         R=M(1:3,1:3);
         T=M(1:3,4);
         T=R*ones(3,1)+T;
         M(1:3,4)=T;

         [h filetype fileprefix machine]=load_nii_hdr(hdr);
         h.hist.qform_code=0;
         h.hist.sform_code=1;
         h.hist.srow_x=M(1,:);
         h.hist.srow_y=M(2,:);
         h.hist.srow_z=M(3,:);
         h.hist.magic='ni1';

         fid = fopen(hdr,'w',machine);
         save_nii_hdr(h,fid);
         fclose(fid);
      end
   end

   return;				% mat_into_hdr


%SerialLink.PAY Joint forces due to payload
%
% TAU = R.PAY(W, J) returns the generalised joint force/torques due to a
% payload wrench W (1x6) and where the manipulator Jacobian is J (6xN), and
% N is the number of robot joints.
%
% TAU = R.PAY(Q, W, F) as above but the Jacobian is calculated at pose Q
% (1xN) in the frame given by F which is '0' for world frame, 'e' for
% end-effector frame.
%
% Uses the formula TAU = J'W, where W is a wrench vector applied at the end
% effector, W = [Fx Fy Fz Mx My Mz]'.
%
% Trajectory operation::
%
% In the case Q is MxN or J is 6xNxM then TAU is MxN where each row is the
% generalised force/torque at the pose given by corresponding row of Q.
%
% Notes::
% - Wrench vector and Jacobian must be from the same reference frame.
% - Tool transforms are taken into consideration when F = 'e'.
% - Must have a constant wrench - no trajectory support for this yet.
%
% Author::
% Bryan Moutrie
%
% See also SerialLink.paycap, SerialLink.jacob0, SerialLink.jacobe.

% Copyright (C) Bryan Moutrie, 2013-2015
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of
% the License, or (at your option) any later version.
%
% pHRIWARE 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 Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.

function tauP = pay(robot, varargin)

    if length(varargin) == 2
        w = varargin{1};
        J = varargin{2};
        n = size(J,2);
    elseif length(varargin) == 3
        q = varargin{1};
        w = varargin{2};
        f = varargin{3};
        n = robot.n;
        J = zeros(6,n,size(q,1));
        switch f
            case '0'
                for i= 1: size(q,1)
                    J(:,:,i) = robot.jacob0(q(i,:));
                end
            case {'n', 'e'}
                for i= 1: size(q,1)
                    J(:,:,i) = robot.jacobe(q(i,:));
                end
        end
    end

    if ~isequal(size(w),[6 1]), error(pHRIWARE('error', 'inputSize')); end
    tauP = -reshape(J(:,:)'*w,n,[])';

end


% RES = reconWpyr(PYR, INDICES, FILT, EDGES, LEVS, BANDS)
%
% Reconstruct image from its separable orthonormal QMF/wavelet pyramid
% representation, as created by buildWpyr.
%
% PYR is a vector containing the N pyramid subbands, ordered from fine
% to coarse.  INDICES is an Nx2 matrix containing the sizes of
% each subband.  This is compatible with the MatLab Wavelet toolbox.
%
% FILT (optional) can be a string naming a standard filter (see
% namedFilter), or a vector which will be used for (separable)
% convolution.  Default = 'qmf9'.  EDGES specifies edge-handling,
% and defaults to 'reflect1' (see corrDn).
%
% LEVS (optional) should be a vector of levels to include, or the string
% 'all' (default).  1 corresponds to the finest scale.  The lowpass band
% corresponds to wpyrHt(INDICES)+1.
%
% BANDS (optional) should be a vector of bands to include, or the string
% 'all' (default).   1=horizontal, 2=vertical, 3=diagonal.  This is only used
% for pyramids of 2D images.

% Eero Simoncelli, 6/96.

function res = reconWpyr(pyr, ind, filt, edges, levs, bands)

if (nargin < 2)
  error('First two arguments (PYR INDICES) are required');
end

%%------------------------------------------------------------
%% OPTIONAL ARGS:

if (exist('filt') ~= 1)
  filt = 'qmf9';
end

if (exist('edges') ~= 1)
  edges= 'reflect1';
end

if (exist('levs') ~= 1)
  levs = 'all';
end

if (exist('bands') ~= 1)
  bands = 'all';
end

%%------------------------------------------------------------

maxLev = 1+wpyrHt(ind);
if strcmp(levs,'all')
  levs = [1:maxLev]';
else
  if (any(levs > maxLev))
    error(sprintf('Level numbers must be in the range [1, %d].', maxLev));
  end
  levs = levs(:);
end

if strcmp(bands,'all')
  bands = [1:3]';
else
  if (any(bands < 1) | any(bands > 3))
    error('Band numbers must be in the range [1,3].');
  end
  bands = bands(:);
end

if isstr(filt)
  filt = namedFilter(filt);
end

filt = filt(:);
hfilt = modulateFlip(filt);

%% For odd-length filters, stagger the sampling lattices:
if (mod(size(filt,1),2) == 0)
	stag = 2;
else
	stag = 1;
end

%% Compute size of result image: assumes critical sampling (boundaries correct)
res_sz = ind(1,:);
if (res_sz(1) == 1)
  loind = 2;
  res_sz(2) = sum(ind(:,2));
elseif (res_sz(2) == 1)
  loind = 2;
  res_sz(1) = sum(ind(:,1));
else
  loind = 4;
  res_sz = ind(1,:) + ind(2,:);  %%horizontal + vertical bands.
  hres_sz = [ind(1,1), res_sz(2)];
  lres_sz = [ind(2,1), res_sz(2)];
end

%% First, recursively collapse coarser scales:
if any(levs > 1)

  if (size(ind,1) > loind)
    nres = reconWpyr( pyr(1+sum(prod(ind(1:loind-1,:)')):size(pyr,1)), ...
	ind(loind:size(ind,1),:), filt, edges, levs-1, bands);
  else
    nres = pyrBand(pyr, ind, loind); 	% lowpass subband
  end

  if (res_sz(1) == 1)
    res = upConv(nres, filt', edges, [1 2], [1 stag], res_sz);
  elseif (res_sz(2) == 1)
    res = upConv(nres, filt, edges, [2 1], [stag 1], res_sz);
  else
    ires = upConv(nres, filt', edges, [1 2], [1 stag], lres_sz);
    res = upConv(ires, filt, edges, [2 1], [stag 1], res_sz);
  end

else

  res = zeros(res_sz);

end

%% Add  in reconstructed bands from this level:
if any(levs == 1)
  if (res_sz(1) == 1)
    upConv(pyrBand(pyr,ind,1), hfilt', edges, [1 2], [1 2], res_sz, res);
  elseif (res_sz(2) == 1)
    upConv(pyrBand(pyr,ind,1), hfilt, edges, [2 1], [2 1], res_sz, res);
  else
    if any(bands == 1) % horizontal
      ires = upConv(pyrBand(pyr,ind,1),filt',edges,[1 2],[1 stag],hres_sz);
      upConv(ires,hfilt,edges,[2 1],[2 1],res_sz,res);  %destructively modify res
    end
    if any(bands == 2) % vertical
      ires = upConv(pyrBand(pyr,ind,2),hfilt',edges,[1 2],[1 2],lres_sz);
      upConv(ires,filt,edges,[2 1],[stag 1],res_sz,res);  %destructively modify res
    end
    if any(bands == 3) % diagonal
      ires =  upConv(pyrBand(pyr,ind,3),hfilt',edges,[1 2],[1 2],hres_sz);
      upConv(ires,hfilt,edges,[2 1],[2 1],res_sz,res);  %destructively modify res
    end
  end
end


%% Render Random Scenes
%
% This script will generate numerous random scenes and save the results in
% the Output directory. This is useful to sample a large number of scene.
% Note that each scene will be saved as an image with a date stamp *and*
% the random number generator seed used to generate the seed. Passing this
% number to |rng| before running |example_scene| will result in the scene
% being recreated exactly. This allows a user to examine numerous scenes,
% select a scene of interest, recreate it, explore it, and potentially
% export it for whatever purpose. In other words, the seed number is all
% that's necessary to completely regenerate a scene. For example, if an
% image is named |scene_2012-12-13-18-38-28_seed_59150.png|, then the
% following completely recreates the scene.
%
% >> rng(59150);
% >> example_scene;
%
% Tucker McClure
% Copyright 2012, The MathWorks, Inc.

%% Options
% Number of scenes to create.
n_images = 50;

%% Initialize the Figure
fprintf('Generating %d scenes...\n', n_images);
tic();

% Create or clear a figure for the rendering.
figure(1);
clf();
set(1, 'PaperPositionMode',     'auto', ...
       'WindowButtonDownFcn',   [], ...
       'WindowButtonUpFcn',     [], ...
       'WindowButtonMotionFcn', [], ...
       'WindowScrollWheelFcn',  []);

% Choose the next random number generator seed.
rng_seed = mod(floor(1e8*now()), 2^32);
rng(rng_seed);

%%
% Add an Output directory if one doesn't exist.
if ~exist('Output', 'dir')
    mkdir('Output');
end

%% Land and Sea

% Create the terrain height map.
[~, ~, ~, hm, xm, ym] = generate_terrain(7, 513, 0, 0.1);

% Generate appropriate colors.
cm = generate_terrain_colors(hm);

% Flatten the oceans.
hmp = max(hm, 0);

% Draw the land and calculate its normal vectors.
h_land = patch(surf2patch(xm, ym, hmp, cm));
[nx, ny, nz] = surfnorm(hmp);
land_normals = [nx(:), ny(:), nz(:)];

% Set it's material properties for interpolated colors and appropriate
% lighting.
set(h_land, 'VertexNormals',    land_normals, ...
            'DiffuseStrength',  0.8, ...      % Reacts to light direction
            'SpecularStrength', 0, ...        % Not shiny
            'AmbientStrength',  0.3, ...      % Reacts to ambient light
            'BackFaceLighting', 'unlit');     % Don't illuminate reverse

% Add a black backdrop beneath the land. Sometimes a vertex seems to miss
% its position slightly, and sky colors creep through, and that's weird.
patch('Faces',           [1 2 3 4], ...
      'Vertices',        [-1 -1 -0.01; ...
                           1 -1 -0.01; ...
                           1  1 -0.01; ...
                          -1  1 -0.01], ...
      'FaceVertexCData', 0.25*ones(4, 3));

%% Sky

% Draw a random time of day between sunrise and sunset. Get the
% corresponding light color. The sky is always the color of the sun at noon
% (and is then affected by the sun color at the current time).
current_time = 0.24 + 0.52 * rand();
sun_color    = sun_tones(current_time);
sky_color    = sun_tones(0.5);

% Create the sky patch by scaling a sphere. Note that the nothing in the
% scene can be rendered outside of [-100, 100] on any axis due to a
% documented rendering bug. Therefore, the sphere will be scaleld up to
% 99, which is plenty far away since the terrain is limited to [-1, 1].
[xs, ys, zs] = sphere(16);
sky_scale = 99;
sky_patch = surf2patch(sky_scale*xs, sky_scale*ys, sky_scale*zs, ...
                       repmat(reshape(sky_color, [1 1 3]), [17 17 1]));
h_sky = patch(sky_patch);

% Set appropriate lighting options for the sky.
set(h_sky, 'DiffuseStrength',  0.3, ...
           'SpecularStrength', 0, ...
           'AmbientStrength',  1, ...
           'BackFaceLighting', 'unlit');

% Vertex normals are the opposite of what I expect. Reverse them.
set(h_sky,  'VertexNormals', -get(h_sky,  'VertexNormals'));

%% Sun

% Create a light for the sun.
h_light = lightangle(90, 360*(current_time - 0.25));

% Set the light's color.
set(h_light, 'Color',             sun_color);

% Set the ambient color used in the scene.
set(gca(),   'AmbientLightColor', sun_color);

% Use decent lighting.
lighting gouraud;

%% Axes and Global Settings

% Set axes options.
camera_target   = [0 0 mean(hmp(:)) + 0.5*std(hmp(:))];
camera_position = camera_target + [1.15 0 0.5];
set(gca, 'DataAspectRatio', [1 1 1], ...
         'Visible',         'off', ...
         'Projection',      'Perspective', ...
         'Position',        [0 0 1 1], ...
         'CameraTarget',    camera_target, ...
         'CameraViewAngle', 45, ...
         'CameraUpVector',  [0 0 1], ...
         'CameraPosition',  camera_position, ...
         'XLim',            [-100 100], ...
         'YLim',            [-100 100], ...
         'ZLim',            [-100 100]);
camorbit(360*rand(), 0);

% Patches should have no edges and interpolated face colors.
shading interp;

% Force the drawing.
drawnow();

%% Loop, Updating Image Each Time
image_number = 0;
while true

    % Increase image count.
    image_number = image_number + 1;

    % Force a drawing.
    drawnow();

    % Capture the image.
    image_name = sprintf('scene_%s_seed_%d.png', ...
                         datestr(now(), 'yyyy-mm-dd-HH-MM-SS'), ...
                         rng_seed);
    print('-dpng', ['Output' filesep image_name]);

    % If we've generated enough images, stop.
    if image_number == n_images
        break;
    end

    % Choose the next random number generator seed.
    rng_seed = mod(floor(1e8*now()), 2^32);
    rng(rng_seed);

    % Generate new terrain.
    [~, ~, ~, hm, xm, ym] = generate_terrain(7, 513, 0, 0.1);
    cm = generate_terrain_colors(hm);
    hmp = max(hm, 0);
    current_time = 0.24 + 0.52 * rand();
    [nx, ny, nz] = surfnorm(hmp);
    land_normals = [nx(:), ny(:), nz(:)];

    % Update the patches, lights, and camera.
    land_patch = surf2patch(xm, ym, hmp, cm);
    set(h_land, 'Vertices',        land_patch.vertices, ...
                'Faces',           land_patch.faces, ...
                'FaceVertexCData', land_patch.facevertexcdata, ...
                'VertexNormals',   land_normals);
    lightangle(h_light, 90, 360*(current_time - 0.25));
    sun_color = sun_tones(current_time);
    set(h_light, 'Color', sun_color);
    camera_target   = [0 0 mean(hmp(:)) + 0.5*std(hmp(:))];
    camera_position = camera_target + [1.15 0 0.5];
    set(gca(), 'AmbientLightColor', sun_color, ...
               'CameraTarget',      camera_target, ...
               'CameraPosition',    camera_position, ...
               'CameraUpVector',    [0 0 1]);
    camorbit(360*rand(), 0);

end

fprintf('Done.\n');
toc();


function tapas_ehgf_jget_plotTraj(r)
% Plots the estimated trajectories for the HGF perceptual model for
% the JGET project
% Usage example:  est = tapas_fitModel(responses, inputs); tapas_ehgf_plotTraj(est);
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013-2020 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Optional plotting of standard deviations (true or false)
plotsd = true;

% Set up display
scrsz = get(0,'screenSize');
outerpos = [0.2*scrsz(3),0.2*scrsz(4),0.8*scrsz(3),0.8*scrsz(4)];
figure(...
    'OuterPosition', outerpos,...
    'Name', 'HGF trajectories');

% Time axis
if size(r.u,2) > 1 && ~isempty(find(strcmp(fieldnames(r.c_prc),'irregular_intervals'))) && r.c_prc.irregular_intervals
    t = r.u(:,end)';
else
    t = ones(1,size(r.u,1));
end

ts = cumsum(t);
ts = [0, ts];

% Do we know the generative parameters?
if size(r.u,2) > 2
    genpar = true;
    mean   = r.u(:,2);
    sd     = r.u(:,3);
else
    genpar = false;
end

% Number of levels
try
    l = r.c_prc.n_levels;
catch
    l = length(r.p_prc.p)/8;
end

% Upper levels
for j = 1:l-1

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Left subplot (x)                       %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    subplot(l+1,2,2*j-1);

    if plotsd == true
        upperprior = r.p_prc.mux_0(l-j+1) +1.96*sqrt(r.p_prc.sax_0(l-j+1));
        lowerprior = r.p_prc.mux_0(l-j+1) -1.96*sqrt(r.p_prc.sax_0(l-j+1));
        upper = [upperprior; r.traj.mux(:,l-j+1)+1.96*sqrt(r.traj.sax(:,l-j+1))];
        lower = [lowerprior; r.traj.mux(:,l-j+1)-1.96*sqrt(r.traj.sax(:,l-j+1))];

        plot(0, upperprior, 'ob', 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'ob', 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
    plot(ts, [r.p_prc.mux_0(l-j+1); r.traj.mux(:,l-j+1)], 'b', 'LineWidth', 1.5);
    hold all;
    plot(0, r.p_prc.mux_0(l-j+1), 'ob', 'LineWidth', 1.5); % prior
    xlim([0 ts(end)]);
    title(['Posterior expectation of x_' num2str(l-j+1)], 'FontWeight', 'bold');
    ylabel(['\mu x_', num2str(l-j+1)]);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Right subplot (alpha)                  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    subplot(l+1,2,2*j);

    if plotsd == true
        upperprior = r.p_prc.mua_0(l-j+1) +1.96*sqrt(r.p_prc.saa_0(l-j+1));
        lowerprior = r.p_prc.mua_0(l-j+1) -1.96*sqrt(r.p_prc.saa_0(l-j+1));
        upper = [upperprior; r.traj.mua(:,l-j+1)+1.96*sqrt(r.traj.saa(:,l-j+1))];
        lower = [lowerprior; r.traj.mua(:,l-j+1)-1.96*sqrt(r.traj.saa(:,l-j+1))];

        plot(0, upperprior, 'ob', 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'ob', 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
    plot(ts, [r.p_prc.mua_0(l-j+1); r.traj.mua(:,l-j+1)], 'b', 'LineWidth', 1.5);
    hold all;
    plot(0, r.p_prc.mua_0(l-j+1), 'ob', 'LineWidth', 1.5); % prior
    xlim([0 ts(end)]);
    title(['Posterior expectation of \alpha_' num2str(l-j+1)], 'FontWeight', 'bold');
    ylabel(['\mu \alpha_', num2str(l-j+1)]);
end

% Input level
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Left subplot (x)                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l-1);

if plotsd == true
    upperprior = r.p_prc.mux_0(1) +1.96*sqrt(r.p_prc.sax_0(1));
    lowerprior = r.p_prc.mux_0(1) -1.96*sqrt(r.p_prc.sax_0(1));
    upper = [upperprior; r.traj.mux(:,1)+1.96*sqrt(r.traj.sax(:,1))];
    lower = [lowerprior; r.traj.mux(:,1)-1.96*sqrt(r.traj.sax(:,1))];

    plot(0, upperprior, 'or', 'LineWidth', 1);
    hold all;
    plot(0, lowerprior, 'or', 'LineWidth', 1);
    fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
end
plot(ts, [r.p_prc.mux_0(1); r.traj.mux(:,1)], 'r', 'LineWidth', 1.5);
hold all;
plot(0, r.p_prc.mux_0(1), 'or', 'LineWidth', 1.5); % prior
plot(ts(2:end), r.u(:,1), '.', 'Color', [0 0.6 0]); % inputs
if genpar
    plot(ts(2:end), mean, '-', 'Color', 'k', 'LineWidth', 1); % mean of input distribution
    plot(ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
    plot(ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
end
xlim([0 ts(end)]);
title(['Input u (green) and posterior expectation of x_1 (red) for \kappa_x=', ...
       num2str(r.p_prc.kax), ', \omega_x=', num2str(r.p_prc.omx)], 'FontWeight', 'bold');
ylabel('u, \mu x_1');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Right subplot (alpha)                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l);

if plotsd == true
    upperprior = r.p_prc.mua_0(1) +1.96*sqrt(r.p_prc.saa_0(1));
    lowerprior = r.p_prc.mua_0(1) -1.96*sqrt(r.p_prc.saa_0(1));
    upper = [upperprior; r.traj.mua(:,1)+1.96*sqrt(r.traj.saa(:,1))];
    lower = [lowerprior; r.traj.mua(:,1)-1.96*sqrt(r.traj.saa(:,1))];

    transupperprior = sqrt(exp(r.p_prc.kau *upperprior +r.p_prc.omu));
    translowerprior = sqrt(exp(r.p_prc.kau *lowerprior +r.p_prc.omu));
    transupper = sqrt(exp(r.p_prc.kau *upper +r.p_prc.omu));
    translower = sqrt(exp(r.p_prc.kau *lower +r.p_prc.omu));

    plot(0, transupperprior, 'or', 'LineWidth', 1);
    hold all;
    plot(0, translowerprior, 'or', 'LineWidth', 1);
    fill([ts, fliplr(ts)], [(transupper)', fliplr((translower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
end
transmuaprior = sqrt(exp(r.p_prc.kau *r.p_prc.mua_0(1) +r.p_prc.omu));
plot(ts, [transmuaprior; sqrt(exp(r.p_prc.kau *r.traj.mua(:,1) +r.p_prc.omu))], 'r', 'LineWidth', 1.5);
hold all;
plot(0, transmuaprior, 'or', 'LineWidth', 1.5); % prior
if genpar
    plot(ts(2:end), sd, '--', 'Color', 'k', 'LineWidth', 1);
end
xlim([0 ts(end)]);
title(['Belief on noise (red) for \kappa_\alpha=', ...
       num2str(r.p_prc.kaa), ', \omega_\alpha=', num2str(r.p_prc.oma)], 'FontWeight', 'bold');
ylabel('\mu \alpha_1');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Decision model                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l+1);

if plotsd == true
    upper = r.traj.muxhat(:,1)+1.96*sqrt(r.p_obs.ze +r.traj.saxhat(:,1));
    lower = r.traj.muxhat(:,1)-1.96*sqrt(r.p_obs.ze +r.traj.saxhat(:,1));

    fill([ts(2:end), fliplr(ts(2:end))], [(upper)', fliplr((lower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    hold all;
end
plot(ts(2:end), r.traj.muxhat(:,1), 'Color', [153/256 17/256 153/256], 'LineWidth', 1.5);
hold all;
plot(ts(2:end), r.y(:,1), '.', 'Color', [1 0.65 0], 'MarkerSize', 15); % responses
if genpar
    plot(ts(2:end), mean, '-', 'Color', 'k', 'LineWidth', 1); % mean of input distribution
    plot(ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
    plot(ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
end
xlim([1 ts(end)]);
title('Decision model: prediction of decision (purple) and decision (orange)', 'FontWeight', 'bold');
ylabel('y, \^{\mu} x_1');
xlabel({'Trial number', ' '}); % A hack to get the relative subplot sizes right
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Learning rate                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if genpar
subplot(l+1,2,2*l+2);
[AX, H1, H2 ] = plotyy(ts(2:end), mean, ts(2:end), r.traj.lrx(:,1));
hold all;
ylim(AX(1), [min(mean -1.96.*sd-3), max(mean +1.96.*sd+3)]);
plot(AX(1), ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1.1); % 95% interval of input distribution
plot(AX(1), ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1.1); % 95% interval of input distribution
set(H1, 'Color', 'k', 'LineWidth', 1.1);
set(H2, 'Color', [178/256, 34/256, 34/256], 'LineWidth', 1.5);
set(AX(1), 'YColor', 'k');
set(AX(2), 'YColor', 'k');
xlim(AX(1), [1 ts(end)]);
xlim(AX(2), [1 ts(end)]);
title('Learning rate (bordeaux) and input sampling distribution (black)', 'FontWeight', 'bold');
ylabel(AX(1), 'Input');
ylabel(AX(2), '\^{\pi}_u/\pi_x');
xlabel('Trial number');
hold off;
end


%MDL_OFFSET6 A minimalistic 6DOF robot arm with shoulder offset
%
% MDL_OFFSET6 is a script that creates the workspace variable off6 which
% describes the kinematic characteristics of a simple arm manipulator with
% a spherical wrist and a shoulder offset, using standard DH conventions.
%
% Also define the workspace vectors:
%   qz         zero joint angle configuration
%
% Notes::
% - Unlike most other mdl_xxx scripts this one is actually a function that
%   behaves like a script and writes to the global workspace.
%
% See also SerialLink, mdl_simple6, mdl_puma560, mdl_twolink.

% MODEL: generic, 6DOF, standard_DH

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

function r = mdl_offset6()

    % robot length values (metres)
    L1 = 1;
    L2 = 1;
    O1 = 0;
    O2 = 0.2;
    O3 = 0;

    % and build a serial link manipulator

    robot = SerialLink([
        Revolute('alpha', -pi/2, 'a', O1, 'd', 0)
        Revolute('alpha', 0,     'a', L1, 'd', O2)
        Revolute('alpha', pi/2,  'a', L2, 'd', O3)
        Revolute('alpha', pi/2, 'a', 0,  'd', 0)
        Revolute('alpha', -pi/2,  'a', 0,  'd', 0)
        Revolute('alpha', 0,     'a', 0,  'd', 0)
        ], ...
        'name', 'Offset6');

    % place the variables into the global workspace
    if nargout == 1
        r = robot;
    elseif nargout == 0
        assignin('base', 'off6', robot);
        assignin('base', 'qz', [0 0 0 0 0 0]); % zero angles, arm up
    end
end


function tapas_linear_validate_data(y, u)
%%
%
% aponteeduardo@gmail.com
% copyright (C) 2016
%

if ~iscell(y)
    error('tapas:linear:validate_data', 'y should be a cell array');
end

if ~iscell(u)
    error('tapas:linear:validate_data', 'y should be a cell array');
end

if ~ (size(y, 2) == 1)
     error('tapas:linear:validate_data', 'y should be Nx1');
end

if ~ (size(u, 2) == 1)
     error('tapas:linear:validate_data', 'u should be Nx1');
end

if ~ (size(y, 1) == size(u, 1))
    error('tapas:linear:validate_data', 'y and u should have the same size');
end

np = size(y, 1);

[nt, d2] = size(y{1});

if d2 ~= 1
    error('tapas:linear:validate_data', 'y{1} should be Nx1');
end

[nu, nb] = size(u{1});

if nu ~= nt
    error('tapas:linear:validate_data', ...
        'u{1} first dimension should be %d', nt);
end

for i = 1:np
    if ~ (size(y{i}, 2) == 1)
        error('tapas:linear:validate_data', ...
            'y{%d} first dimension should be 1', i);
    end

    if ~ (size(u{i}, 1) == size(y{i}, 1))
         error('tapas:linear:validate_data', ...
            'y{%d} and u{%d} first dimension should be equal', i, i);
    end

    if ~(size(u{i}, 2) == nb)
        error('tapas:linear:validate_data',  ...
            'u{%d} second dimension is inconsistent', i);
    end
end

end


function PRshowx(x, ProbInfo,h)
%PRshowx Show the solution x in IR Tools
%
% PRshowx(x,ProbInfo)
% PRshowx(x,ProbInfo,h)
%
% This function uses the information stored in ProbInfo to display the
% solution x in the correct way for that particular test problem.
%
% An optional third parameters specifies a figure handle.
%
% See also: PRshowb

% Silvia Gazzola, University of Bath
% Per Christian Hansen, Technical University of Denmark
% James G. Nagy, Emory University
% April, 2018.

% This file is part of the IR Tools package and is distributed under the
% 3-Clause BSD License. A separate license file should be provided as part
% of the package.

if nargin < 3
  h = [];
end
if isempty(h)
  h = gcf;
end

switch ProbInfo.xType
    case {'image2D'}
        switch ProbInfo.problemType
            case {'deblurring','tomography'}
                figure(h)
                ImageDisplayGui2D(reshape(x, ProbInfo.xSize), h)
        end
    case {'surf2D'}
        switch ProbInfo.problemType
            case{'nmr'}
                [T2g, T1g] = meshgrid(ProbInfo.T2, ProbInfo.T1);
                figure(h)
                surf(log10(T2g), log10(T1g), reshape(x, ProbInfo.xSize))
                shading interp
                axis tight
            case {'invinterp2'}
                figure(h)
                n = ProbInfo.xSize(1);
                points = linspace(0, 1, n);
                [Xg,Yg] = meshgrid(points);
                plot3(Xg(:), Yg(:), x, '.', 'color', [0 0 0.7])
        end
    case {'fem'}
        trisurf(ProbInfo.elmtab,ProbInfo.elmX,ProbInfo.elmY,x)
        axis([0 1 0 1 -0.1 1])
        shading interp
    otherwise
        error('Can only do image2D, surf2D, and fem')
end


function [ abd, rcond, z, info ] = spbco ( abd, lda, n, m )

%*****************************************************************************80
%
%% SPBCO factors a real symmetric positive definite banded matrix.
%
%  Discussion:
%
%    SPBCO also estimates the condition of the matrix.
%
%    If RCOND is not needed, SPBFA is slightly faster.
%
%    To solve A*X = B, follow SPBCO by SPBSL.
%
%    To compute inverse(A)*C, follow SPBCO by SPBSL.
%
%    To compute determinant(A), follow SPBCO by SPBDI.
%
%  Band storage:
%
%    If A is a symmetric positive definite band matrix, the following
%    program segment will set up the input.
%
%      m = (band width above diagonal)
%      do j = 1, n
%        i1 = max (1, j-m)
%        do i = i1, j
%          k = i-j+m+1
%          abd(k,j) = a(i,j)
%        end do
%      end do
%
%    This uses M + 1 rows of A, except for the M by M upper left triangle,
%    which is ignored.
%
%    For example, if the original matrix is
%
%      11 12 13  0  0  0
%      12 22 23 24  0  0
%      13 23 33 34 35  0
%       0 24 34 44 45 46
%       0  0 35 45 55 56
%       0  0  0 46 56 66
%
%    then N = 6, M = 2  and ABD should contain
%
%       *  * 13 24 35 46
%       * 12 23 34 45 56
%      11 22 33 44 55 66
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2009
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real ABD(LDA,N), the matrix to be factored.  The columns of the
%    upper triangle are stored in the columns of ABD and the diagonals of
%    the upper triangle are stored in the rows of ABD.
%
%    Input, integer LDA, the leading dimension of the array ABD.
%    M+1 <= LDA is required.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer M, the number of diagonals above the main diagonal.
%
%    Output, real ABD(LDA,N), an upper triangular matrix R, stored in band form,
%    so that A = R'*R.  If INFO /= 0, the factorization is not complete.
%
%    Output, real RCOND, an estimate of the reciprocal condition
%    of A.  For the system A*X = B, relative perturbations in A and B of size
%    EPSILON may cause relative perturbations in X of size EPSILON/RCOND.
%    If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%    Output, real Z(N), a work vector whose contents are usually
%    unimportant.  If A is singular to working precision, then Z is an
%    approximate null vector in the sense that
%      norm(A*Z) = RCOND * norm(A) * norm(Z).
%    If INFO /= 0, Z is unchanged.
%
%    Output, integer INFO, error flag.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is not
%    positive definite.
%

%
%  Find the norm of A.
%
  for j = 1 : n

    l = min ( j, m+1 );
    mu = max ( m+2-j, 1 );
    z(j) = sasum ( l, abd(mu:mu+l-1,j), 1 );
    k = j - l;
    for i = mu : m
      k = k + 1;
      z(k) = z(k) + abs ( abd(i,j) );
    end

  end

  anorm = max ( z(1:n) );
%
%  Factor.
%
  [ abd, info ] = spbfa ( abd, lda, n, m );

  if ( info ~= 0 )
    return
  end
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A*Z = Y and A*Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where R'*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve R' * W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  for k = 1 : n

    if ( z(k) ~= 0.0 )
      ek = - abs ( ek ) * r4_sign ( z(k) );
    end

    if ( abd(m+1,k) < abs ( ek - z(k) ) )
      s = abd(m+1,k) / abs ( ek - z(k) );
      z(1:n) = s * z(1:n);
      ek = s * ek;
    end

    wk = ek - z(k);
    wkm = -ek - z(k);
    s = abs ( wk );
    sm = abs ( wkm );
    wk = wk / abd(m+1,k);
    wkm = wkm / abd(m+1,k);
    j2 = min ( k+m, n );
    i = m + 1;

    if ( k+1 <= j2 )

      for j = k+1 : j2
        i = i - 1;
        sm = sm + abs ( z(j) + wkm * abd(i,j) );
        z(j) = z(j) + wk * abd(i,j);
        s = s + abs ( z(j) );
      end

      if ( s < sm )

        t = wkm - wk;
        wk = wkm;
        i = m + 1;

        for j = k+1 : j2
          i = i - 1;
          z(j) = z(j) + t * abd(i,j);
        end

      end

    end

    z(k) = wk;

  end

  z(1:n) = z(1:n) / sasum ( n, z(1:n), 1 );
%
%  Solve R * Y = W.
%
  for k = n : -1 : 1

    if ( abd(m+1,k) < abs ( z(k) ) )
      s = abd(m+1,k) / abs ( z(k) );
      z(1:n) = s * z(1:n);
    end

    z(k) = z(k) / abd(m+1,k);
    lm = min ( k-1, m );
    la = m + 1 - lm;
    lb = k - lm;
    t = -z(k);
    z(lb:lb+lm-1) = z(lb:lb+lm-1) + t * abd(la:la+lm-1,k)';

  end

  z(1:n) = z(1:n) / sasum ( n, z(1:n), 1 );

  ynorm = 1.0;
%
%  Solve R' * V = Y.
%
  for k = 1 : n

    lm = min ( k-1, m );
    la = m + 1 - lm;
    lb = k - lm;

    z(k) = z(k) - sdot ( lm, abd(la:la+lm-1,k), 1, z(lb:lb+lm-1), 1 );

    if ( abd(m+1,k) < abs ( z(k) ) )
      s = abd(m+1,k) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    z(k) = z(k) / abd(m+1,k);

  end

  s = 1.0 / sasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;
%
%  Solve R * Z = W.
%
  for k = n : -1 : 1

    if ( abd(m+1,k) < abs ( z(k) ) )
      s = abd(m+1,k) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    z(k) = z(k) / abd(m+1,k);
    lm = min ( k-1, m );
    la = m + 1 - lm;
    lb = k - lm;
    t = -z(k);
    z(lb:lb+lm-1) = z(lb:lb+lm-1) + t * abd(la:la+lm-1,k)';

  end
%
%  Make ZNORM = 1.0.
%
  s = 1.0 / sasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


clear all; close all;
I=imread('rice.png');
J=I>120;
[width, height]=size(I);
for i=1:width
    for j=1:height
        if (I(i, j)>130)
            K(i, j)=1;
        else
            K(i, j)=0;
        end
    end
end
figure;
subplot(121);  imshow(J);
subplot(122);  imshow(K);