% SB2_POSTERIORMODE  Posterior mode-finder for the SPARSEBAYES algorithm
%
% [MU, U, BETA, LIKELIHOODMODE, BADHESS] = ...
%    SB2_POSTERIORMODE(LIKELIHOOD,BASIS,TARGETS,ALPHA,MU,ITSMAX,OPTIONS)
%
% OUTPUT ARGUMENTS:
%
%	MU				Parameter values at the mode
%	U				Cholesky factor of the covariance at the mode
%	BETA			Vector of pseudo-noise variances at the mode
%	LIKELIHOODMODE	Data likelihood at the mode
%	BADHESS			Returns true if Hessian is "bad" (becomes
%					non-positive-definite during maximisation)
%
% INPUT ARGUMENTS:
%
%	LIKELIHOOD	LIKELIHOOD structure
%	BASIS		Current relevant basis matrix
%	TARGETS		N-vector with target output values
%	ALPHA		Current hyperparameters
%	MU			Current weights
%	ITSMAX		Maximum number of iterations to run
%	OPTIONS		Standard OPTIONS structure (only affects diagnostics)
%
% NOTES:
%
% SB2_POSTERIORMODE finds the posterior mode (with respect to the
% weights) of the likelihood function in the non-Gaussian case to
% facilitate subsequent Laplace approximation.
%
% This function is intended for internal use by SPARSEBAYES only (within
% SB2_FullStatistics).
%

%
% Copyright 2009, Vector Anomaly Ltd
%
% This file is part of the SPARSEBAYES library for Matlab (V2.0).
%
% SPARSEBAYES 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.
%
% SPARSEBAYES is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
% FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
% more details.
%
% You should have received a copy of the GNU General Public License along
% with SPARSEBAYES in the accompanying file "licence.txt"; if not, write to
% the Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
% MA 02110-1301 USA
%
% Contact the author: m a i l [at] m i k e t i p p i n g . c o m
%
function [Mu, U, Beta, likelihoodMode, badHess] = ...
    SB2_PosteriorMode(LIKELIHOOD,BASIS,Targets,Alpha,Mu,itsMax,OPTIONS)

% TOLERANCES
%
% Termination criterion for each gradient dimension
%
GRADIENT_MIN	= 1e-6;
%
% Minimum fraction of the full Newton step considered
%
STEP_MIN	= 1/(2^8);
%

[N, M]	= size(BASIS);
A		= diag(Alpha);

% NB: for historical reasons, we work in term of error here (negative
% log-liklihood) and minimise

%
% Get current model output and data error
%
BASIS_Mu		= BASIS*Mu; % Linear output
[dataError, y]	= SB2_DataError(LIKELIHOOD, BASIS_Mu, Targets);
%
% Add on the weight penalty
%
regulariser		= (Alpha'*(Mu.^2))/2;
newTotalError	= dataError + regulariser;
%
badHess		= false;
errorLog	= zeros(itsMax,1);
%
for iteration=1:itsMax
  %
  % Log the error value each iteration
  %
  errorLog(iteration)	= newTotalError;
  SB2_Diagnostic(OPTIONS,4,'PM cycle: %2d\t error: %.6f\n', ...
				 iteration, errorLog(iteration));

  % Construct the gradient
  %
  e	= (Targets-y);
  g	= BASIS'*e - Alpha.*Mu;
  %
  % Compute the likelihood-dependent analogue of the noise precision.
  % NB: Beta now a vector.
  %
  switch LIKELIHOOD.InUse
   case LIKELIHOOD.Bernoulli
    Beta	= y.*(1-y);
   case LIKELIHOOD.Poisson
    Beta	= y;
  end
  % Compute the Hessian
  BASIS_B	= BASIS .* (Beta * ones(1,M));
  H			= (BASIS_B'*BASIS + A);
  % Invert Hessian via Cholesky, watching out for ill-conditioning
  [U, pdErr]	= chol(H);
  % Make sure its positive definite
  if pdErr
	% If you see this, it's *probably* the result of a bad choice of
    % basis. e.g. a kernel with too large a "width"
	%
    SB2_Diagnostic(OPTIONS, 1,...
				   '** Warning ** Ill-conditioned Hessian (%g)\n', ...
				   rcond(H));
    badHess			= true;
    U				= [];
    Beta			= [];
    likelihoodMode	= [];
    return
  end
  %
  % Before progressing, check for termination based on the gradient norm
  %
  if all(abs(g)<GRADIENT_MIN)
    errorLog	= errorLog(1:iteration);
    SB2_Diagnostic(OPTIONS,4,['PM convergence (<%g) after ' ...
					'%d iterations, |g| = %g\n'], ...
				   GRADIENT_MIN,iteration,max(abs(g)));
    break
  end

  % If all OK, compute full Newton step: H^{-1} * g
  %
  DeltaMu	= U \ (U' \ g);
  step		= 1;
  %
  while step>STEP_MIN
	% Follow gradient to get new value of parameters
	%
    Mu_new		= Mu + step*DeltaMu;
    BASIS_Mu	= BASIS*Mu_new;
	%
    % Compute outputs and error at new point
	%
    [dataError,y]	= SB2_DataError(LIKELIHOOD,BASIS_Mu,Targets);
    regulariser		= (Alpha'*(Mu_new.^2))/2;
    newTotalError	= dataError + regulariser;
	%
    % Test that we haven't made things worse
	%
    if newTotalError>=errorLog(iteration)
      % If so, back off!
	  %
      step	= step/2;
      SB2_Diagnostic(OPTIONS, 4,['PM error increase! Backing off to l=' ...
					'%.3f\n'], step);
    else
      Mu	= Mu_new;
      step	= 0;			% this will force exit from the "while" loop
    end
  end
  %
  % If we get here with non-zero "step", it means that the smallest
  % offset from the current point along the "downhill" direction did not
  % lead to a decrease in error. In other words, we must be
  % infinitesimally close to a minimum (which is OK).
  %
  if step
    SB2_Diagnostic(OPTIONS, 4, ...
				   'PM stopping due to back-off limit (|g| = %g)\n', ...
				   max(abs(g)));
    break;
  end
end
%
% Simple computation of return value of log likelihood at mode
%
likelihoodMode	= -dataError;

%%%%%%%%%%%%%%%%%%%%%%
%
% Support function
%
%%%%%%%%%%%%%%%%%%%%%%

function [e,y] = SB2_DataError(LIKELIHOOD,BASIS_Mu,Targets)

switch LIKELIHOOD.InUse
 case LIKELIHOOD.Bernoulli
  y	= SB2_Sigmoid(BASIS_Mu);
  % Handle probability zero cases
  y0	= (y==0);
  y1	= (y==1);
  if any(y0(Targets>0)) || any(y1(Targets<1))
    % Error infinite when model gives zero probability in
    % contradiction to data
    e	= inf;
  else
    % Any y=0 or y=1 cases must now be accompanied by appropriate
    % output=0 or output=1 values, so can be excluded.
    e	= -(Targets(~y0)'*log(y(~y0)) + (1-Targets(~y1))'*log(1-y(~y1)));
  end
  %
 case LIKELIHOOD.Poisson
  y	= exp(BASIS_Mu);
  e	= -sum(Targets.*BASIS_Mu - y);
  %
end


function H = decode_nifti1(blob)
% function H = decode_nifti1(blob)
%
% Decodes a NIFTI-1 header given as raw 348 bytes (uint8) into a Matlab structure
% that matches the C struct defined in nifti1.h, with the only difference that the
% variable length arrays "dim" and "pixdim" are cut off to the right size, e.g., the
% "dim" entry will only contain the relevant elements:
% dim[0..7]={3,64,64,18,x,x,x,x} in C would become dim=[64,64,18] in Matlab.
%
% WARNING: This function currently ignores endianness !!!

% (C) 2010 S.Klanke

if class(blob)~='uint8'
	error 'Bad type for blob'
end
if length(blob)~=348
	error 'Blob must be exactly 348 bytes long'
end

% see nift1.h for information on structure
H = [];

magic = char(blob(345:347));
if blob(348)~=0 | magic~='ni1' & magic~='n+1'
	error 'Not a NIFTI-1 header!';
end

H.sizeof_hdr = typecast(blob(1:4),'int32');
H.data_type  = cstr2matlab(blob(5:14));
H.db_name    = cstr2matlab(blob(15:32));
H.extents    = typecast(blob(33:36),'int32');
H.session_error = typecast(blob(37:38),'int16');
H.regular    = blob(39);
H.dim_info   = blob(40);

dim = typecast(blob(41:56),'int16');
H.dim = dim(2:dim(1)+1);
H.intent_p1   = typecast(blob(57:60),'single');
H.intent_p2   = typecast(blob(61:64),'single');
H.intent_p3   = typecast(blob(65:68),'single');
H.intent_code = typecast(blob(69:70),'int16');
H.datatype    = typecast(blob(71:72),'int16');
H.bitpix      = typecast(blob(73:74),'int16');
H.slice_start = typecast(blob(75:76),'int16');
pixdim = typecast(blob(77:108),'single');
H.qfac        = pixdim(1);
H.pixdim      = pixdim(2:dim(1)+1);
H.vox_offset  = typecast(blob(109:112),'single');
H.scl_scope   = typecast(blob(113:116),'single');
H.scl_inter   = typecast(blob(117:120),'single');
H.slice_end   = typecast(blob(121:122),'int16');
H.slice_code  = blob(123);
H.xyzt_units  = blob(124);
H.cal_max     = typecast(blob(125:128),'single');
H.cal_min     = typecast(blob(129:132),'single');
H.slice_duration = typecast(blob(133:136),'single');
H.toffset     = typecast(blob(137:140),'single');
H.glmax       = typecast(blob(141:144),'int32');
H.glmin       = typecast(blob(145:148),'int32');
H.descrip     = cstr2matlab(blob(149:228));
H.aux_file    = cstr2matlab(blob(229:252));
H.qform_code  = typecast(blob(253:254),'int16');
H.sform_code  = typecast(blob(255:256),'int16');
quats = typecast(blob(257:280),'single');
H.quatern_b = quats(1);
H.quatern_c = quats(2);
H.quatern_d = quats(3);
H.quatern_x = quats(4);
H.quatern_y = quats(5);
H.quatern_z = quats(6);
trafo = typecast(blob(281:328),'single');
H.srow_x = trafo(1:4);
H.srow_y = trafo(5:8);
H.srow_z = trafo(9:12);
%H.S = [H.srow_x; H.srow_y; H.srow_z; 0 0 0 1];
H.intent_name = cstr2matlab(blob(329:344));
H.magic = magic;

function ms = cstr2matlab(cs)
if cs(1)==0
  ms = '';
else
  ind = find(cs==0);
  if isempty(ind)
    ms = char(cs)';
  else
    ms = char(cs(1:ind(1)-1))';
  end
end


% fig2A: correlation to phototaxis stimulus
% for single regressor, detail
% option set to do correlation on trial-averaged traces
% plot corresponding histogram of corr.coeffs,
% and plot functional as well as anat plot

% compare to fig3A_sensory_motor, which for now plots a pair of regressors
% in a double colormap, and batch saves figures.

hfig = figure;
InitializeAppData(hfig);
ResetDisplayParams(hfig);
i_fish = 6;
ClusterIDs = [2,1];
[cIX,gIX,M,stim,behavior,M_0] = LoadSingleFishDefault(i_fish,hfig,ClusterIDs);

%% corr with PT regressor, stim-avr

setappdata(hfig,'isStimAvr',0);
[M_0,M,behavior,stim] = UpdateTimeIndex(hfig);

fishset = getappdata(hfig,'fishset');
[~,names,regressors] = GetStimRegressor(stim,fishset,i_fish);
reg_range = 2;
Reg = regressors(reg_range,:);
R = corr(Reg',M_0');

thres_reg = 0.5;
cIX = (find(R>thres_reg))';
wIX = R(cIX); % weight, i.e. corr.coeff
[~,I] = sort(wIX,'descend');
cIX = cIX(I);
gIX = ones(size(cIX));
wIX = wIX(I)';

UpdateIndices_Manual(hfig,cIX,gIX);

%% histogram of corr.coeff, for all cells
figure('Position',[500,200,500,150]);
hold on;
bins = -1:0.05:1;
[N,~] = histcounts(R,bins);
histogram(R,bins,'FaceColor',[0.4 0.4 0.4]);%,'EdgeColor','none'
plot([thres_reg,thres_reg],[0,max(N)],'r--');
xlim([-1,1]);ylim([0,max(N)]);
set(gca,'ycolor','w')

%% histogram with rainbow colors
clrbins = 0.5:0.05:1;
cmap = hot(length(clrbins)-1);
n_blank = length(bins)-length(clrbins);
cmap = vertcat(ones(n_blank,3),cmap);
figure('Position',[500,200,500,150]);
hold on
DrawRainbowHistogram(N,bins,cmap);
plot([thres_reg,thres_reg],[0,max(N)],'r--');
xlim([-1,1]);ylim([0,max(N)]);
set(gca,'ycolor','w')

%% left plot
figure('Position',[50,500,300,200])%[50,100,800,1000]);
% isCentroid,isPlotLines,isPlotBehavior,isPlotRegWithTS
setappdata(hfig,'isPlotBehavior',0);
setappdata(hfig,'isStimAvr',0);
UpdateTimeIndex(hfig);

% set color
clrmap = [0,0,0];
numK = max(gIX);
assert(size(clrmap,1)==numK);

opts = [];
opts.clrmap = clrmap;
opts.isAxTight = 1;
DrawTimeSeries(hfig,cIX,gIX,opts);

%% set up custom colormap
clrbins = 0.5:0.05:1;
clrmap0 = hot(round(1.5*length(clrbins)));
clrmap = clrmap0(end-length(clrbins):end,:);
gIX = interp1(clrbins,1:length(clrbins),wIX,'nearest');

%% right plot
I = LoadCurrentFishForAnatPlot(hfig,cIX,gIX);
I.clrmap = clrmap;
DrawCellsOnAnat(I);

%%
numTicks = 2;
DrawCustomColorbar(clrmap,clrbins,numTicks);

%% draw average plot (even if regression is done on full trace)
gIX = ones(size(cIX));
setappdata(hfig,'isStimAvr',1);
UpdateTimeIndex(hfig);
figure('Position',[50,500,300,200])%[50,100,800,1000]);
DrawTimeSeries(hfig,cIX,gIX,opts);

%%
[~,tot_image] = DrawCellsOnAnat(I);
ax = axes('Position',[0.8,0.8,0.05,0.15],'Units','normalized');
DrawCustomColorbar(clrmap,clrbins,2,ax);

%%

% %% left-right combined plot
% figure('Position',[50,100,1400,800]);
% % isCentroid,isPlotLines,isPlotBehavior,isPlotRegWithTS
% subplot(121)
% setappdata(hfig,'isPlotBehavior',0);
% setappdata(hfig,'isStimAvr',1);
% UpdateTimeIndex(hfig);
% DrawTimeSeries(hfig,cIX,gIX);
%
% % right plot
% subplot(122)
% I = LoadCurrentFishForAnatPlot(hfig,cIX,gIX);
% DrawCellsOnAnat(I);

%%
% dataDir = GetCurrentDataDir;
% saveDir = fullfile(dataDir,'motorsourceplot');
% if ~exist(saveDir, 'dir'), mkdir(saveDir), end;
% filename = fullfile(saveDir, num2str(i_fish));
% saveas(gcf, filename, 'png');
% close(gcf)

%%
%% motor regressor
%             [~,names,regressors] = GetMotorRegressor(behavior,i_fish);


%% Selections of the dataset, model, and category
% Choices of the dataset. You may select 'ilsvrcanimalpart', 'vocpart', or 'cub200'.
dataset='ilsvrcanimalpart'; %'vocpart'; 'cub200';
% If you select dataset='ilsvrcanimalpart', then you may choose of the following categories.
% 'n01443537','n01503061','n01639765','n01662784','n01674464','n01882714','n01982650','n02084071','n02118333','n02121808','n02129165','n02129604','n02131653','n02324045','n02342885','n02355227','n02374451','n02391049','n02395003','n02398521','n02402425','n02411705','n02419796','n02437136','n02444819','n02454379','n02484322','n02503517','n02509815','n02510455'
% If you select dataset='vocpart', then you may choose of the following categories.
% 'bird','cat','cow','dog','horse','sheep'
% If you select dataset='vocpart', then you need to choose categoryName='cub200'.
categoryName='n02118333';
% Choices of the networks. You may select 'vgg-vd-16', 'alexnet', 'vgg-m', or 'vgg-s'.
model='vgg-vd-16'; % 'alexnet'; 'vgg-m'; 'vgg-s';
dropoutRate=0.8; %0.5; 0.6; 0.7; 0.8; 0.9; %when using a small number of training samples, avoid over-fitting.
% Learn a CNN for multi-class classification or single-class classification
isMultiClassClassification=false; %true;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Network configurations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

setup_matconvnet(); % setup matconvnet

switch(dataset) % setup the dataset
    case 'ilsvrcanimalpart'
        setup_ilsvrcanimalpart();
    case 'vocpart'
        setup_vocpart();
    case 'cub200'
        setup_cub200();
    otherwise
        errors('Cannot find the target dataset.');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Learning an interpretable CNN
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if(isMultiClassClassification)
    if(strcmp(dataset,'vocpart'))
        categoryName={'bird','cat','cow','dog','horse','sheep'};
        lossType='ourloss_logistic'; % 'ourloss_softmaxlog';
        learn_icnn_multiclass(model,categoryName,lossType,dropoutRate);
    end
else
    learn_icnn(model,categoryName,dropoutRate);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Showing results and evaluation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if(isMultiClassClassification)
    if(strcmp(dataset,'vocpart'))
        categoryName={'bird','cat','cow','dog','horse','sheep'};
    end
end
showResults(categoryName,isMultiClassClassification,model,dataset);


%% Lucas-Kanade Tracker
%
% Lucas-Kanade sparse optical flow demo. Uses |cv.goodFeaturesToTrack| for
% track initialization and back-tracking for match verification between
% frames.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/lk_track.py>
%

%% Options
track_len = 10;       % max number of locations of point to remember
detect_interval = 5;  % detect new corners every X iterations

% params for corner detection and flow computation
shi_params = {'MaxCorners',500, 'QualityLevel',0.3, 'MinDistance',7, ...
    'BlockSize',7};
lk_params = {'WinSize',[15 15], 'MaxLevel',2, ...
    'Criteria',struct('type','Count+EPS', 'maxCount',10, 'epsilon',0.03)};

%% Video
% Prepare video source
if true
    vid = 0;
elseif true
    vid = fullfile(mexopencv.root(), 'test', '768x576.avi');
elseif mexopencv.require('vision')
    vid = fullfile(toolboxdir('vision'), 'visiondata', 'visiontraffic.avi');
end
cap = cv.VideoCapture(vid);
assert(cap.isOpened(), 'Failed to initialize capturing');

%% First frame
% Grab first frame
frame = cap.read();
assert(~isempty(frame), 'Failed to read frame');
gray0 = cv.cvtColor(frame, 'RGB2GRAY');

%% Initialize
% stores history of locations for a set of points (cell array of Nx2 matrices)
tracks = {};
clr = [0 255 0];

%%
% Plot
hImg = imshow(frame);
title('Lucas-Kanade tracker')

%% Main loop
counter = 0;  % iterations counter
while ishghandle(hImg)
    % Grab next frame
    frame = cap.read();
    if isempty(frame), break; end
    gray1 = cv.cvtColor(frame, 'RGB2GRAY');

    if ~isempty(tracks)
        % track last position of points, in forward and backward direction
        p0 = cellfun(@(tr) tr(end,:), tracks, 'UniformOutput',false);
        p1 = cv.calcOpticalFlowPyrLK(gray0, gray1, p0, lk_params{:});
        p0r = cv.calcOpticalFlowPyrLK(gray1, gray0, p1, lk_params{:});
        % keep only good matches
        good = cellfun(@(a,b) max(abs(a - b)), p0, p0r) < 1;
        tracks = tracks(good);
        p1 = p1(good);
        if any(good)
            % append new locations to existing tracked points
            tracks = cellfun(@(tr,p) [tr; p], tracks, p1, 'UniformOutput',false);
            % keep only the last 10 locations in each track (fixed size queue)
            idx = cellfun(@(tr) size(tr,1), tracks) > track_len;
            tracks(idx) = cellfun(@(tr) tr(2:end,:), tracks(idx), 'UniformOutput',false);
            % draw latest points and their tracks (comet-like plot)
            frame = cv.circle(frame, p1, 2, 'Thickness','Filled', 'Color',clr);
            frame = cv.polylines(frame, tracks, 'Closed',false, 'Color',clr);
        end
    end

    % display number of tracked points
    frame = cv.putText(frame, ...
        sprintf('track count: %d', numel(tracks)), [20 20], ...
        'FontScale',0.5, 'Color',[255 255 0], 'LineType','AA');

    if rem(counter, detect_interval) == 0 || isempty(tracks)
        % region of interest mask
        if ~isempty(tracks)
            % try to find new points by masking-out last track positions
            mask = 255 * ones(size(gray1), 'uint8');
            p = cellfun(@(tr) tr(end,:), tracks, 'UniformOutput',false);
            mask = cv.circle(mask, p, 5, 'Thickness','Filled', 'Color',0);
            roi_params = {'Mask',mask};
        else
            roi_params = {};
        end
        % detect corners
        p = cv.goodFeaturesToTrack(gray1, roi_params{:}, shi_params{:});
        if ~isempty(p)
            % append new set of points (p is a cell array of 1x2 vectors)
            tracks = [tracks, p];
        end
    end

    % Display result
    set(hImg, 'CData',frame);
    drawnow;

    % Next iteration
    counter = counter + 1;
    gray0 = gray1;
end
cap.release();


function grad = B_splice_single_sentence2(future_grad, context, mask)
[dim,nFr,nSeg] = size(future_grad);
dim = dim/context;
half_ctx = (context-1)/2;

future_grad2 = reshape(future_grad, dim, context, nFr, nSeg);
future_grad2(:,:,end+1,:) = 0;

for i=-half_ctx:half_ctx
    all_idx{i+half_ctx+1} = min(nFr,max(1, (1:nFr) + i));
end
all_idx2 = cell2mat(all_idx');

future_grad2 = all_idx2;
future_grad2(:,end+1) = 0;

grad_idx = zeros(nFr, context);
grad_idx1 = [];
grad_idxend = [];
for i=-half_ctx:half_ctx
    offset = (nFr+1)*(i+half_ctx);
    % curr_future_grad = future_grad( (i+half_ctx)*dim+1 : (i+half_ctx+1)*dim, :,:);
    if i<0
        grad_idx(1:nFr+i, i+half_ctx+1) = (-i+1:nFr) + offset;
        %grad(:,1:end+i,:) = grad(:,1:end+i,:) + curr_future_grad(:, -i+1:end,:);
        grad_idx1 = [grad_idx1 (1:-i)+offset];
    elseif i>0
        grad_idx(i+1:nFr, i+half_ctx+1) = (1:nFr-i) + offset;

        %grad(:,i+1:end,:) = grad(:,i+1:end,:) + curr_future_grad(:, 1:end-i,:);
        %tmp2 = [tmp2 curr_future_grad(:, end-i+1:end,:)];
        grad_idxend = [grad_idxend (nFr-i+1:nFr)+offset];
    else
        grad_idx(1:nFr, i+half_ctx+1) = (1:nFr) + offset;
        %grad = grad + curr_future_grad;
    end
end
grad_idx(grad_idx==0) = nFr*context+1;

% future_grad2 = permute(future_grad2, [1 4 2 3]);
future_grad2 = reshape(future_grad2, dim*nSeg, context*(nFr+1));
grad = future_grad2(:,grad_idx);
grad = reshape(grad, dim, context, nFr);
grad = squeeze(sum(grad,2));
grad = permute(grad, [1 3 2]);

grad = future_grad2(grad_idx');

% grad(:,1,:) = grad(:,1,:) + sum(tmp1,2);
% grad(:,end,:) = grad(:,end,:) + sum(tmp2,2);

end


%% GPS
% for i = 1:32
%     codes_L1CA(:,i) = GNSScodegen(i,'L1CA',0);
%     codes_L2CM(:,i) = GNSScodegen(i,'L2CM',0);
%     codes_L2CL(:,i) = GNSScodegen(i,'L2CL',0);
%     codes_L5I(:,i) = GNSScodegen(i,'L5I',0);
%     codes_L5Q(:,i) = GNSScodegen(i,'L5Q',0);
% end

% save('codes_L1CA.mat','codes_L1CA');
% save('codes_L2CM.mat','codes_L2CM');
% save('codes_L2CL.mat','codes_L2CL');
% save('codes_L5I.mat','codes_L5I');
% save('codes_L5Q.mat','codes_L5Q');

%% Galileo
% for i = 1:50
%     codes_E5aI(:,i) = GNSScodegen(i,'E5aI',0);
%     codes_E5aQ(:,i) = GNSScodegen(i,'E5aQ',0);
%     codes_E5bI(:,i) = GNSScodegen(i,'E5bI',0);
%     codes_E5bQ(:,i) = GNSScodegen(i,'E5bQ',0);
%     codes_E1B(:,i) = GNSScodegen(i,'E1B',0);
%     codes_E1C(:,i) = GNSScodegen(i,'E1C',0);
% end
%
% save('codes_E5aI.mat','codes_E5aI');
% save('codes_E5aQ.mat','codes_E5aQ');
% save('codes_E5bI.mat','codes_E5bI');
% save('codes_E5bQ.mat','codes_E5bQ');
% save('codes_E1B.mat','codes_E1B');
% save('codes_E1C.mat','codes_E1C');

%% BeiDou
% for i = 1:37
%     codes_B1I(:,i) = GNSScodegen(i,'B1I',0);
% end
%
% save('codes_B1I.mat','codes_B1I');


function n = ndims(t)
%NDIMS Return the number of dimensions for a ttensor.
%
%   NDIMS(T) returns the number of dimensions of tensor T.
%
%   See also TTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) 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

n = numel(t.u);


function endpointsAll = gt2endpoints(annolistFilename)

if isstruct(annolistFilename)
    annolist = annolistFilename;
else
    load(annolistFilename);
end
%if (~exist('annolist','var'))
%    annolist = single_person_annolist;
%end
endpointsAll = cell(length(annolist),1);

skel = [0 1; 1 2; 3 4; 4 5; 6 7; 8 9; 10 11; 11 12; 13 14; 14 15];

for imgidx = 1:length(annolist)
    endpoints = nan(10,4);
    points = annolist(imgidx).annorect.annopoints.point;
    for sidx = 1:size(skel,1)
        p1 = util_get_annopoint_by_id(points,skel(sidx,1));
        p2 = util_get_annopoint_by_id(points,skel(sidx,2));
        if (~isempty(p1) && ~isempty(p2))
            endpoints(sidx,:) = [p1.x p1.y p2.x p2.y];
        end
    end
    endpointsAll{imgidx} = endpoints;
end

end


%INPAINT2  The function implements different single-image inpainting algorithms
%
%     dst = cv.inpaint2(src, mask)
%     dst = cv.inpaint2(src, mask, 'OptionName', optionValue, ...)
%
% ## Input
% * __src__ source image, it could be of any type (8/16/32-bit integers or
%   32/64-bit floating-points) and any number of channels from 1 to 4. In case
%   of 3- and 4-channels images the function expect them in CIELab colorspace
%   or similar one, where first color component shows intensity, while second
%   and third shows colors. Nonetheless you can try any colorspaces.
% * __mask__ mask (8-bit 1-channel of same size as `src`), where non-zero
%   pixels indicate valid image area, while zero pixels indicate area to be
%   inpainted.
%
% ## Output
% * __dst__ Output image with the same size and type as `src`.
%
% ## Options
% * __Method__ Inpainting algorithms, one of:
%   * __ShiftMap__ (default) This algorithm searches for dominant
%     correspondences (transformations) of image patches and tries to
%     seamlessly fill-in the area to be inpainted using this transformations.
%
% The function reconstructs the selected image area from known area.
% See the original paper [He2012] for details.
%
% ## References
% [He2012]:
% > Kaiming He, Jian Sun. "Statistics of patch offsets for image completion".
% > In Computer Vision-ECCV 2012, pages 16-29. Springer, 2012.
%
% See also: cv.inpaint
%


function stable_camva(a)
  % STABLE_CAMVA Act like camva without changing zoom (aka dolly zoom)
  %
  % Inputs:
  %   a  angle input to camva
  %
  % Note: This will mess up interactive view orbitting. So set up your
  % view first...
  %
  % See also: camva
  old_a = camva;
  old_t = camtarget;
  old_p = campos;
  camva(a);
  s = tan(a/2*pi/180)/tan(old_a/2*pi/180);
  p = (old_p-old_t)/s+old_t;
  campos(p);
end


filename='Gripping_quad_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'nonadjoint_compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
constraint_case = 'INEQUALITY';
optimizer = 'PROJECTED GRADIENT'; incrementFactor = 1;
filterType = 'P1';

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

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


function bnet = mk_hhmm2(varargin)
% MK_HHMM2 Make a 2 level Hierarchical HMM
% bnet = mk_hhmm2(...)
%
% 2-layer hierarchical HMM  (node numbers in parens)
%
%   Q1(1) ---------> Q1(5)
% /  | \            / |
% |  |  v          /  |
% |  |  F2(3) --- /   |
% |  |  ^         \   |
% |  | /           \  |
% |  v              \ v
% |  Q2(2)--------> Q2 (6)
% |  |
% \  |
%  v v
%   O(4)
%
%
% Optional arguments [default]
%
% discrete_obs - 1 means O is tabular_CPD, 0 means O is gaussian_CPD [0]
% obsCPT       - CPT(o,q1,q2) params for O ['rnd']
% mu           - mu(:,q1,q2) params for O [ [] ]
% Sigma        - Sigma(:,q1,q2) params for O [ [] ]
%
% F2toQ1       - 1 if Q2 is an hhmm_CPD, 0 if F2 -> Q2 arc is absent, so level 2 never resets [1]
% Q1args        - arguments to be passed to the constructors for Q1(t=2) [ {} ]
% Q2args        - arguments to be passed to the constructors for Q2(t=2) [ {} ]
%
% F2 only turns on (wp 0.5) when Q2 enters its final state.
% Q1 (slice 1) is clamped to be uniform.
% Q2 (slice 1) is clamped to always start in state 1.

[os nmodels nstates] = size(mu);

ss = 4;
Q1 = 1; Q2 = 2; F2 = 3; obs = 4;
Qnodes = [Q1 Q2];
names = {'Q1', 'Q2', 'F2', 'obs'};
intra = zeros(ss);
intra(Q1, [Q2 F2 obs]) = 1;
intra(Q2, [F2 obs]) = 1;

inter = zeros(ss);
inter(Q1,Q1) = 1;
inter(F2,Q1) = 1;
if F2toQ2
  inter(F2,Q2)=1;
end
inter(Q2,Q2) = 1;

ns = zeros(1,ss);

ns(Q1) = nmodels;
ns(Q2) = nstates;
ns(F2) = 2;
ns(obs) = os;

dnodes = [Q1 Q2 F2];
if discrete_obs
  dnodes = [dnodes obs];
end
onodes = [obs];

bnet = mk_dbn(intra, inter, ns, 'observed', onodes, 'discrete', dnodes, 'names', names);
eclass = bnet.equiv_class;

% SLICE 1

% We clamp untied nodes in the first slice, since their params can't be estimated
% from just one sequence

% uniform prior on initial model
CPT = normalise(ones(1,nmodels));
bnet.CPD{eclass(Q1,1)} = tabular_CPD(bnet, Q1, 'CPT', CPT, 'adjustable', 0);

% each model always starts in state 1
CPT = zeros(ns(Q1), ns(Q2));
CPT(:, 1) = 1.0;
bnet.CPD{eclass(Q2,1)} = tabular_CPD(bnet, Q2, 'CPT', CPT, 'adjustable', 0);

% Termination probability
CPT = zeros(ns(Q1), ns(Q2), 2);
if 1
  % Each model can only terminate in its final state.
  % 0 params will remain 0 during EM, thus enforcing this constraint.
  CPT(:, :, 1) = 1.0; % all states turn F off ...
  p = 0.5;
  CPT(:, ns(Q2), 2) = p; % except the last one
  CPT(:, ns(Q2), 1) = 1-p;
end
bnet.CPD{eclass(F2,1)}  = tabular_CPD(bnet, F2, 'CPT', CPT);

if discrete_obs
  bnet.CPD{eclass(obs,1)} = tabular_CPD(bnet, obs, obs_args{:});
else
  bnet.CPD{eclass(obs,1)} = gaussian_CPD(bnet, obs, obs_args{:});
end

% SLICE 2

bnet.CPD{eclass(Q1,2)} = hhmm_CPD(bnet, Q1+ss, Qnodes, 1, D, 'args', Q1args);

if F2toQ2
  bnet.CPD{eclass(Q2,2)} = hhmmQD_CPD(bnet, Q2+ss, Qnodes, 2, D, Q2args{:});
else
  bnet.CPD{eclass(Q2,2)} = tabular_CPD(bnet, Q2+ss, Q2args{:});
end


% -------------------------------------------------------------------------------------------------
function [bb] = get_minmax_BB(region)
%get_minmax_BB computes minmax bbox from the rotated one (REGION)
% -------------------------------------------------------------------------------------------------

bb = [min(region(:,1:2:end),[],2),...
    min(region(:,2:2:end),[],2),...
    max(region(:,1:2:end),[],2),...
    max(region(:,2:2:end),[],2)]-1;

end


function [dims,scales,bpp,endian,datatype] = read_avw_hdr(fname)
% [dims,scales,bpp,endian] = READ_AVW_HDR(fname)
%
%  Extracts the 4 dimensions (dims),
%   4 scales (scales) and bytes per pixel (bpp) for voxels
%   contained in the Analyse header file (fname)
%   Also returns endian = 'l' for little-endian or 'b' for big-endian
%
%  See also: READ_AVW, READ_AVW_IMG, SAVE_AVW, SAVE_AVW_HDR, SAVE_AVW_IMG

% remove extension if it exists
if ( (length(findstr(fname,'.hdr'))>0) | ...
        (length(findstr(fname,'.img')>0)) ),
  fname=fname(1:(length(fname)-4));
end
fnhdr=strcat(fname,'.hdr');

% open file in big-endian
endian='b';
fid=fopen(fnhdr,'r','b');
testval = fread(fid,1,'int32');
% check if this gives the correct header size - if not use little-endian
if (testval~=348),
  fclose(fid);
  fid=fopen(fnhdr,'r','l');
  endian='l';
  testval = fread(fid,1,'int32');
  if (testval~=348),
    disp('Can not read this file format');
    return;
  end
end
        % ditch the remaining initial header stuff
  dummy=fread(fid,36,'char');
        % ditch dim[0] = No. dimensions
  dummy=fread(fid,1,'int16');
  dims=fread(fid,4,'int16');
  dummy=fread(fid,3,'int16');
  dummy=fread(fid,14,'char');
  datatype=fread(fid,1,'int16');
  bpp=fread(fid,1,'int16');
  dummy=fread(fid,2,'char');
  dummy=fread(fid,1,'float');
  scales=fread(fid,4,'float');
fclose(fid);
return;


function A = cs_frand (n,nel,s)                                             %#ok
%CS_FRAND generate a random finite-element matrix
% A = cs_frand (n,nel,s) creates an n-by-n sparse matrix consisting of nel
% finite elements, each of which are of size s-by-s with random symmetric
% nonzero pattern, plus the identity matrix.
%
% Example
%   A = cs_frand (100, 100, 3) ;
% See also cs_demo.

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

error ('cs_frand mexFunction not found') ;


function success = write_bspline_coeff_file(bspFileName,bsplineParamsS)
% function success = write_bspline_coeff_file(bspFileName,bsplineParamsS)
%
% APA, 07/17/2012

bsp_img_origin          = bsplineParamsS.bsp_img_origin;
bsp_img_spacing         = bsplineParamsS.bsp_img_spacing;
bsp_img_dim             = bsplineParamsS.bsp_img_dim;
bsp_roi_offset          = bsplineParamsS.bsp_roi_offset;
bsp_roi_dim             = bsplineParamsS.bsp_roi_dim;
bsp_vox_per_rgn         = bsplineParamsS.bsp_vox_per_rgn;
bsp_direction_cosines   = bsplineParamsS.bsp_direction_cosines;
bsp_coefficients        = bsplineParamsS.bsp_coefficients;

fid = fopen(bspFileName,'wb');

fprintf(fid,'MGH_GPUIT_BSP <experimental>\n');
fprintf(fid,['img_origin = ', sprintf('%0.20g ',bsp_img_origin),'\n']);
fprintf(fid,['img_spacing = ', sprintf('%0.20g ',bsp_img_spacing),'\n']);
fprintf(fid,['img_dim = ', sprintf('%0.20g ',bsp_img_dim),'\n']);
fprintf(fid,['roi_offset = ', sprintf('%0.20g ',bsp_roi_offset),'\n']);
fprintf(fid,['roi_dim = ', sprintf('%0.20g ',bsp_roi_dim),'\n']);
fprintf(fid,['vox_per_rgn = ', sprintf('%0.20g ',bsp_vox_per_rgn),'\n']);
fprintf(fid,['direction_cosines = ', sprintf('%0.20g ',bsp_direction_cosines),'\n']);
fprintf(fid,sprintf('%0.20g\n',bsp_coefficients));

%cell2file(fileC,bspFileName);
fclose(fid);

success = 1;


% GAINSANDREFERENCES compute gains matrices, references and other control
%                    related quantities for each state of the state machine.

%% --- Initialization ---

% CoM gains
Gain.KP_CoM = [50    100  5  % state ==  1  TWO FEET BALANCING
               50    100  5  % state ==  2  COM TRANSITION TO LEFT
               50    100  5  % state ==  3  LEFT FOOT BALANCING
               50    100  5  % state ==  4  YOGA LEFT FOOT
               50    100  5  % state ==  5  PREPARING FOR SWITCHING
               50    100  5  % state ==  6  LOOKING FOR CONTACT
               50    100  5  % state ==  7  TRANSITION TO INITIAL POSITION
               50    150  5  % state ==  8  COM TRANSITION TO RIGHT FOOT
               50    100  5  % state ==  9  RIGHT FOOT BALANCING
               50    100  5  % state == 10  YOGA RIGHT FOOT
               50    100  5  % state == 11  PREPARING FOR SWITCHING
               50    100  5  % state == 12  LOOKING FOR CONTACT
               50    100  5];% state == 13  TRANSITION TO INITIAL POSITION

Gain.KD_CoM = 2*sqrt(Gain.KP_CoM)/15;

% Angular momentum gains
Gain.KI_AngularMomentum = 3 ;
Gain.KP_AngularMomentum = 2*sqrt(Gain.KI_AngularMomentum)/5;

% Postural task gains
%                   %  TORSO  %%        LEFT ARM   %%       RIGHT ARM   %%         LEFT LEG           %%         RIGHT LEG           %%
Gain.KP_postural = [10   30   20, 10   10    10    8, 10   10    10    8, 30   30   20    20    100 100, 30   50   30    60    100 100  % state ==  1  TWO FEET BALANCING
                    10   30   20, 10   10    10    8, 10   10    10    8, 30   30   20    20    100 100, 30   50   30    60    100 100  % state ==  2  COM TRANSITION TO LEFT
                    10   30   20, 10   10    10    8, 10   10    10    8, 30   50   30    60    100 100, 30   30   20    20    100 100  % state ==  3  LEFT FOOT BALANCING
                    30   30   30, 10   10    10   10, 10   10    10   10,100  200  100   400    100 100,100   50   30    50    100 100  % state ==  4  YOGA LEFT FOOT
                    30   30   30,  5    5    10   10, 10   10    20   10,200  250   20    20     10  10,220  550  220   200     65 300  % state ==  5  PREPARING FOR SWITCHING
                    30   30   30, 10   10    20   10, 10   10    20   10,100  350   20   200     10 100,220  550  220   200     65 300  % state ==  6  LOOKING FOR CONTACT
                    10   30   20, 10   10    10    8, 10   10    10    8, 30   50   60    30      5   5, 30   30   30    20      5   5  % state ==  7  TRANSITION TO INITIAL POSITION
                    10   30   20, 10   10    10    8, 10   10    10    8, 30   50   60    30    100 100, 30   30   30    20    100 100  % state ==  8  COM TRANSITION TO RIGHT FOOT
                    10   30   20, 10   10    10    8, 10   10    10    8, 30   50   30    60    100 100, 30   30   20    20    100 100  % state ==  9  RIGHT FOOT BALANCING
                    30   30   30, 10   10    10   10, 10   10    10   10,100   50   30    50    100 100,100  200  100   100     10  10  % state == 10  YOGA RIGHT FOOT
                    30   30   30, 10   10    10   10, 10   10    10   10,220  550  220   200     65 300,200  250   20    20     10  10  % state == 11  PREPARING FOR SWITCHING
                    30   30   30, 10   10    10   10, 10   10    10   10,220  550  220   200     65 300,100  350   20   200     10 100  % state == 12  LOOKING FOR CONTACT
                    30   40   30, 10   10    10   10, 10   10    10   10,220  550  220   200     65 300,100  350   20   200     10 100];% state == 13  TRANSITION TO INITIAL POSITION

Gain.KD_postural = 0*sqrt(Gain.KP_postural(1,:))/20;

% symmetric gains
Gain.KP_postural(4,1:3)   = Gain.KP_postural(4,1:3)*3;
Gain.KP_postural(6,18:23) = Gain.KP_postural(6,18:23)*2;
Gain.KP_postural(10,1:3)  = Gain.KP_postural(10,1:3)*3;
Gain.KP_postural(13,1:3)  = Gain.KP_postural(13,1:3)*3;

%% Smoothing times

% Smoothing time gain scheduling
Config.SmoothingTimeGainScheduling = 2;

% Smoothing time CoM references
StateMachine.CoMSmoothingTime      = [1;    %% state ==  1  TWO FEET BALANCING
                                      1;    %% state ==  2  COM TRANSITION TO LEFT FOOT
                                      1;    %% state ==  3  LEFT FOOT BALANCING
                                      0.9;  %% state ==  4  YOGA LEFT FOOT
                                      2;    %% state ==  5  PREPARING FOR SWITCHING
                                      2;    %% state ==  6  LOOKING FOR CONTACT
                                      1;    %% state ==  7  TRANSITION INIT POSITION
                                      1;    %% state ==  8  COM TRANSITION TO RIGHT FOOT
                                      1;    %% state ==  9  RIGHT FOOT BALANCING
                                      0.9;  %% state == 10  YOGA RIGHT FOOT
                                      2;    %% state == 11  PREPARING FOR SWITCHING
                                      2;    %% state == 12  LOOKING FOR CONTACT
                                      5];   %% state == 13  TRANSITION INIT POSITION

% Smoothing time for joints references
StateMachine.jointsSmoothingTime   = [1;    %% state ==  1  TWO FEET BALANCING
                                      1;    %% state ==  2  COM TRANSITION TO LEFT FOOT
                                      1;    %% state ==  3  LEFT FOOT BALANCING
                                      0.9;  %% state ==  4  YOGA LEFT FOOT
                                      2;    %% state ==  5  PREPARING FOR SWITCHING
                                      2;    %% state ==  6  LOOKING FOR CONTACT
                                      1;    %% state ==  7  TRANSITION INIT POSITION
                                      1;    %% state ==  8  COM TRANSITION TO RIGHT FOOT
                                      1;    %% state ==  9  RIGHT FOOT BALANCING
                                      0.9;  %% state == 10  YOGA RIGHT FOOT
                                      2;    %% state == 11  PREPARING FOR SWITCHING
                                      2;    %% state == 12  LOOKING FOR CONTACT
                                      5];   %% state == 13  TRANSITION INIT POSITION

% scale factor smoothing time multiplies the smoothing factor during the
% Yoga (state 4 and 10). The purpose is to reduce the time necessary for
% the reference to converge to the next position, but without changing also
% the valuse stored in Sm.joints_leftYogaRef/Sm.joints_rightYogaRef
StateMachine.scaleFactorSmoothingTime = 0.9;

%% CoM delta

% To be summed to the reference CoM position
StateMachine.CoM_delta  = [% THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE LEFT FOOT
                           0.0,  0.00,  0.0;   %% NOT USED
                           0.0,  0.00,  0.0;   %% state ==  2  COM TRANSITION TO LEFT FOOT
                           0.0,  0.005, 0.0;   %% state ==  3  LEFT FOOT BALANCING
                           0.0,  0.005, 0.0;   %% state ==  4  YOGA LEFT FOOT
                           0.0,  0.005, 0.0;   %% state ==  5  PREPARING FOR SWITCHING
                           0.02,-0.09,  0.0;   %% state ==  6  LOOKING FOR CONTACT
                           0.0,  0.00,  0.0;   %% state ==  7  TWO FEET BALANCING
                           % THIS REFERENCE IS USED AS A DELTA W.R.T. THE POSITION OF THE RIGHT FOOT
                           0.0,  0.00,  0.0;   %% state ==  8  COM TRANSITION TO RIGHT FOOT
                           0.0, -0.005,  0.0;  %% state ==  9  RIGHT FOOT BALANCING
                           0.0, -0.005, 0.0;   %% state == 10  YOGA RIGHT FOOT
                           0.0, -0.015, 0.0;   %% state == 11  PREPARING FOR SWITCHING
                           0.02, 0.02,  0.0;   %% state == 12  LOOKING FOR CONTACT
                           0.0,  0.00,  0.0];  %% NOT USED

%% Joint references
StateMachine.joints_references = [  zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED
                                 [-0.0348,0.0779,0.0429, ...                          %% state == 2  COM TRANSITION TO LEFT
                                  -0.1493,0.8580,0.2437,0.8710, ...                   %
                                  -0.1493,0.8580,0.2437,0.8710, ...                   %
                                  -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ...   %
                                   0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151];     %
                                 [ 0.0864,0.0258,0.0152, ...                          %% state == 3  LEFT FOOT BALANCING
                                   0.1253,0.8135,0.3051,0.7928, ...                   %
                                   0.0563,0.6789,0.3340,0.6214, ...                   %
                                  -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ...   %
                                   0.0005,0.0793,-0.0014,-0.0051,-0.1060,-0.1151];    %
                                   zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED
                                 [-0.0348,0.0779,0.0429, ...                          %% state == 5  PREPARING FOR SWITCHING
                                  -0.1493,0.8580,0.2437,0.8710, ...                   %
                                  -0.1493,0.8580,0.2437,0.8710, ...                   %
                                  -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630, ...   %
                                   0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151];     %
                                 [ 0.0864,0.0258,0.0152, ...                          %% state == 6  LOOKING FOR CONTACT
                                   0.1253,0.8135,0.3051,0.7928, ...                   %
                                   0.0563,0.6789,0.3340,0.6214, ...                   %
                                   0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ...  %
                                  -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277];      %
                                   zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED
                                 [ 0.0864,0.0258,0.0152, ...                          %% state == 8  COM TRANSITION TO RIGHT FOOT
                                   0.1253,0.8135,0.3051,0.7928, ...                   %
                                   0.0563,0.6789,0.3340,0.6214, ...                   %
                                   0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369, ...  %
                                  -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277];      %
                                 [ 0.0864,0.0258,0.0152, ...                          %% state == 9  RIGHT FOOT BALANCING
                                   0.1253,0.8135,0.3051,0.7928, ...                   %
                                   0.0563,0.6789,0.3340,0.6214, ...                   %
                                   0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ...  %
                                  -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630];      %
                                   zeros(1,ROBOT_DOF);                                %% THIS REFERENCE IS IGNORED
                                 [-0.0348,0.0779,0.0429, ...                          %% state == 11  PREPARING FOR SWITCHING
                                  -0.1493,0.8580,0.2437,0.8710, ...                   %
                                  -0.1493,0.8580,0.2437,0.8710, ...                   %
                                   0.0005,0.0793,-0.0014,-0.0051,0.0073,-0.1151, ...  %
                                  -0.0015,-0.1109,-0.0001,0.0003,0.0160,0.1630];      %
                                 [ 0.0864,0.0258,0.0152, ...                          %% state == 12  LOOKING FOR CONTACT
                                   0.1253,0.8135,0.3051,0.7928, ...                   %
                                   0.0563,0.6789,0.3340,0.6214, ...                   %
                                  -0.0026,0.0225,0.0093,-0.0020,0.0027,-0.0277, ...   %
                                   0.0107,-0.0741,-0.0001,-0.0120,0.0252,0.1369];     %
                                   zeros(1,ROBOT_DOF)];                               %% THIS REFERENCE IS IGNORED

% YOGA MOVESET (joint references during state 4 and 10)
q1 =        [-0.0790,0.2279, 0.4519, ...
             -1.1621,0.6663, 0.4919, 0.9947, ...
             -1.0717,1.2904,-0.2447, 1.0948, ...
              0.2092,0.2060, 0.0006,-0.1741,-0.1044, 0.0700, ...
              0.3484,0.4008,-0.0004,-0.3672,-0.1060,-0.0875];

q2 =        [-0.0790,0.1279, 0.4519, ...
             -1.1621,0.6663, 0.4965, 0.9947, ...
             -1.0717,1.2904,-0.2493, 1.0948, ...
              0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ...
              0.3714,0.9599, 1.3253,-1.6594,-0.1060,-0.0614];

q3 =        [-0.0852,-0.3273,0.0821,...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092,0.2060, 0.0006,-0.1741,-0.1044,0.0700, ...
              0.3714,0.9599, 1.3253,-1.6594, 0.5000,-0.0614];

q4 =        [-0.0852,-0.4273,0.0821,...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.3473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614];

q5 =        [-0.0790,-0.2273, 0.4519, ...
             -1.1621,0.6663, 0.4965, 0.9947, ...
             -1.0717,1.2904,-0.2493, 1.0948, ...
              0.2092, 0.4473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614];

q6 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614];

q7 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.3514, 1.3107,1.3253, -1.6217, 0.5000,-0.0614];

q8 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614];

q9 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.3514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614];

q10 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614];

q11 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.8514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614];

q12 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.8514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614];

q13 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.8514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614];

q14 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.8514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614];

q15 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              1.5514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614];

q16 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.2514, 0.0107,1.3253,-0.0189, 0.5000,-0.0614];

q17 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
             -0.3514, 0.3107,1.3253,-0.0189, 0.5000,-0.0614];

q18 =        [-0.0852,-0.4273,0.0821, ...
              0.1391, 1.4585,0.2464, 0.3042, ...
             -0.4181, 1.6800,0.7373, 0.3031, ...
              0.2092, 0.6473,0.0006,-0.1741,-0.1044, 0.0700, ...
              0.3514, 1.3107,1.3253,-0.0189, 0.5000,-0.0614];

StateMachine.joints_leftYogaRef  = [ 0,                                    q1;
                                     1*StateMachine.jointsSmoothingTime(4),q2;
                                     2*StateMachine.jointsSmoothingTime(4),q3;
                                     3*StateMachine.jointsSmoothingTime(4),q4;
                                     4*StateMachine.jointsSmoothingTime(4),q5;
                                     5*StateMachine.jointsSmoothingTime(4),q6;
                                     6*StateMachine.jointsSmoothingTime(4),q7;
                                     7*StateMachine.jointsSmoothingTime(4),q8;
                                     8*StateMachine.jointsSmoothingTime(4),q9;
                                     9*StateMachine.jointsSmoothingTime(4),q10;
                                    10*StateMachine.jointsSmoothingTime(4),q11;
                                    11*StateMachine.jointsSmoothingTime(4),q12;
                                    12*StateMachine.jointsSmoothingTime(4),q13;
                                    13*StateMachine.jointsSmoothingTime(4),q14;
                                    14*StateMachine.jointsSmoothingTime(4),q15;
                                    15*StateMachine.jointsSmoothingTime(4),q16;
                                    16*StateMachine.jointsSmoothingTime(4),q17;
                                    17*StateMachine.jointsSmoothingTime(4),q10;
                                    18*StateMachine.jointsSmoothingTime(4),q11;
                                    19*StateMachine.jointsSmoothingTime(4),q12;
                                    20*StateMachine.jointsSmoothingTime(4),q13;
                                    21*StateMachine.jointsSmoothingTime(4),q14;
                                    22*StateMachine.jointsSmoothingTime(4),q15;
                                    23*StateMachine.jointsSmoothingTime(4),q16;
                                    24*StateMachine.jointsSmoothingTime(4),q17;
                                    25*StateMachine.jointsSmoothingTime(4),q18];

StateMachine.joints_rightYogaRef      = StateMachine.joints_leftYogaRef;
StateMachine.joints_rightYogaRef(:,1) = [0;
                                         1*StateMachine.jointsSmoothingTime(10);
                                         2*StateMachine.jointsSmoothingTime(10);
                                         3*StateMachine.jointsSmoothingTime(10);
                                         4*StateMachine.jointsSmoothingTime(10);
                                         5*StateMachine.jointsSmoothingTime(10);
                                         6*StateMachine.jointsSmoothingTime(10);
                                         7*StateMachine.jointsSmoothingTime(10);
                                         8*StateMachine.jointsSmoothingTime(10);
                                         9*StateMachine.jointsSmoothingTime(10);
                                        10*StateMachine.jointsSmoothingTime(10);
                                        11*StateMachine.jointsSmoothingTime(10);
                                        12*StateMachine.jointsSmoothingTime(10);
                                        13*StateMachine.jointsSmoothingTime(10);
                                        14*StateMachine.jointsSmoothingTime(10);
                                        15*StateMachine.jointsSmoothingTime(10);
                                        16*StateMachine.jointsSmoothingTime(10);
                                        17*StateMachine.jointsSmoothingTime(10);
                                        18*StateMachine.jointsSmoothingTime(10);
                                        19*StateMachine.jointsSmoothingTime(10);
                                        20*StateMachine.jointsSmoothingTime(10);
                                        21*StateMachine.jointsSmoothingTime(10);
                                        22*StateMachine.jointsSmoothingTime(10);
                                        23*StateMachine.jointsSmoothingTime(10);
                                        24*StateMachine.jointsSmoothingTime(10);
                                        25*StateMachine.jointsSmoothingTime(10)];

% if the demo is not "yogaExtended", stop at the 8th move
if ~StateMachine.yogaExtended

    StateMachine.joints_leftYogaRef       = StateMachine.joints_leftYogaRef(1:8,:);
    StateMachine.joints_rightYogaRef      = StateMachine.joints_rightYogaRef(1:8,:);
    StateMachine.joints_leftYogaRef(8,1)  = 15*StateMachine.jointsSmoothingTime(4);
    StateMachine.joints_rightYogaRef(8,1) = 15*StateMachine.jointsSmoothingTime(10);
end

% MIRROR YOGA LEFT MOVESET FOR RIGHT YOGA
for i = 1:size(StateMachine.joints_rightYogaRef,1)

	StateMachine.joints_rightYogaRef(i,2:4)           = [StateMachine.joints_rightYogaRef(i,2) -StateMachine.joints_rightYogaRef(i,3) -StateMachine.joints_rightYogaRef(i,4)];
	rightArm                                          =  StateMachine.joints_rightYogaRef(i,end-15:end-12);
	StateMachine.joints_rightYogaRef(i,end-15:end-12) =  StateMachine.joints_rightYogaRef(i,end-19:end-16);
	StateMachine.joints_rightYogaRef(i,end-19:end-16) =  rightArm;
	rightLeg                                          =  StateMachine.joints_rightYogaRef(i,end-5:end);
	StateMachine.joints_rightYogaRef(i,end-5:end)     =  StateMachine.joints_rightYogaRef(i,end-11:end-6);
	StateMachine.joints_rightYogaRef(i,end-11:end-6)  =  rightLeg;
end

%% References for CoM trajectory (COORDINATOR DEMO ONLY)

% that the robot waits before starting the left-and-right
Config.noOscillationTime       = 0;
Config.directionOfOscillation  = [0;1;0];
Config.amplitudeOfOscillation  = 0.02; % [m]
Config.frequencyOfOscillation  = 0.2;  % [Hz]


function Q1 = mpc(T_meas,T_sp)

s = 'http://byu.apmonitor.com';
c = 'mpc';

% input measurement
apm_meas(s,c,'TC',T_meas);

% input setpoint with deadband +/- DT
DT = 0.1;
apm_option(s,c,'TC.sphi',T_sp+DT);
apm_option(s,c,'TC.splo',T_sp-DT);

% solve MPC
output = apm(s,c,'solve');

% test for successful solution
if (apm_tag(s,c,'apm.appstatus')==1)
    % retrieve the first Q value
    Q1 = apm_tag(s,c,'Q1.Newval');
else
    % display output for debugging
    print(output)
    % not successful, set voltage to zero
    Q1 = 0;
end

end


function [grade,X,SR,improve,AppSet] = LocalSearch1(Problem,X,SR,improve,AppSet)
% Local Search 1 of MTS

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    if ~improve
        SR = SR / 2;
        if all(SR<1e-8)
            SR = (Problem.upper-Problem.lower).*(rand(1,length(SR))/10+0.4);
        end
    end
    improve = false;
    grade   = 0;
    for i = randperm(length(SR))
        old_X  = X;
        dec    = X.dec;
        dec(i) = dec(i) + SR(i)*(rand*2-1);
        X      = Problem.Evaluation(dec);
        [grade,improve,AppSet] = Grading(X,old_X,grade,improve,AppSet,Problem.N);
        if all(old_X.obj<=X.obj)
            dec    = old_X.dec;
            dec(i) = dec(i) - 0.5*SR(i)*(rand*2-1);
            X      = Problem.Evaluation(dec);
            [grade,improve,AppSet] = Grading(X,old_X,grade,improve,AppSet,Problem.N);
            if all(old_X.obj<=X.obj)
                X = old_X;
            end
        end
    end
end


function valid=is_valid_state(x,x_dot,theta,theta_dot)

one_degree=0.0174532;	% 2pi/360 */
six_degrees=0.1047192;
twelve_degrees=0.2094384;
fifty_degrees=0.87266;

valid=1;
if (x < -2.4 | x > 2.4  | theta < -twelve_degrees | theta > twelve_degrees)
    valid=-1;
end


function shiftTSeries(vw,shift,scanList)
%
% function shiftTSeries(view,[shift],[scanList])
%
% Shifts the time series (circular shift), to advance the phase of the response.
% This is useful for approximately compensating for the hemodynamic delay before
% averaging across scans, especially for CW and CCW retinotopies.
%
% scanList: vector of scan numbers to flip. Default: prompt user.
% shift: number of frames to shift. Default: 1.
%
% If you change this function make parallel changes in:
%    flipTSeries
%
% djh, 6/2001
if ieNotDefined('scanList'),  scanList = er_selectScans(vw); end
if ieNotDefined('shift'),     shift = 1;                       end

waitHandle = mrvWaitbar(0,'Shifting tSeries.  Please wait...');

for iScan=1:length(scanList)

    scan = scanList(iScan);
    tSeriesFull = [];
    dimNum = 0;
    for slice=sliceList(vw,scan);
        tSeries = loadtSeries(vw,scan,slice);
        tSeries = circularShift(tSeries,0,-shift);
        dimNum = numel(size(tSeries));
        tSeriesFull = cat(dimNum + 1, tSeriesFull, tSeries); %Combine together
        mrvWaitbar(iScan/length(scanList))
    end

    if dimNum == 3
        tSeriesFull = reshape(tSeriesFull,[1,2,4,3]);
    end %if

    savetSeries(tSeriesFull,vw,scan);
end

close(waitHandle);

if length(scanList)==1
    fprintf('Shifted tSeries for scan %i by %i frames.\n',scanList,shift);
else
    fprintf('Shifted tSeries for scans %s by %i frames.\n',...
        num2str(scanList),shift);
end

return


function [inface, outface]=innersurf(node,face,outface)
%
% outface=innersurf(node,face,outface)
%
% extract the interior triangles (shared by two enclosed compartments) of a complex surface
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%    node:  node coordinates
%    face:  surface triangle list
%    outface: (optional) the exterior triangle list, if not given, will
%           be computed using outersurf().
%
% output:
%    inface: the collection of interior triangles of the surface mesh
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

if(nargin<3)
  outface=outersurf(node,face);
end

tf=ismember(sort(face,2),sort(outface,2),'rows');

inface=face(tf==0,:);


function [inface, outface]=innersurf(node,face,outface)
%
% outface=innersurf(node,face,outface)
%
% extract the interior triangles (shared by two enclosed compartments) of a complex surface
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%    node:  node coordinates
%    face:  surface triangle list
%    outface: (optional) the exterior triangle list, if not given, will
%           be computed using outersurf().
%
% output:
%    inface: the collection of interior triangles of the surface mesh
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

if(nargin<3)
  outface=outersurf(node,face);
end

[I,J]=ismember(sort(face,2),sort(outface,2),'rows');

inface=face(I==0,:);


%% Etract IDE features
clear;clc;
addpath('../caffe/matlab/');
addpath(genpath('utils/'));
% load model and creat network
caffe.set_device(0);
caffe.set_mode_gpu();
netname = 'ResNet_50'; % network: CaffeNet  or ResNet_50

% set your path to the prototxt and model
model =  ['../models/market/' netname '/' netname '_test.prototxt'];
weights = ['../output/market_train/IDE_' netname '.caffemodel'];
net = caffe.Net(model, weights, 'test');

if strcmp(netname, 'CaffeNet')
    im_size = 227;
    feat_dim = 4096;
else
    im_size = 224;
    feat_dim = 2048;
end

% mean data
mean_data = importdata('../caffe/matlab/+caffe/imagenet/ilsvrc_2012_mean.mat');
image_mean = mean_data;
off = floor((size(image_mean,1) - im_size)/2)+1;
image_mean = image_mean(off:off+im_size-1, off:off+im_size-1, :);

ef_path = {'dataset/bounding_box_train/', 'dataset/bounding_box_test/', 'dataset/query/'};
ef_name = {'train', 'test', 'query'};

if ~exist('feat')
    mkdir('feat')
end

% extract features
for i = 1:3
    img_path = ef_path{i};
    img_file = dir([img_path '*.jpg']);
    feat = single(zeros(feat_dim, length(img_file)));

    for n = 1:length(img_file)
        if mod(n, 1000) ==0
            fprintf('%s: %d/%d\n',ef_name{i}, n, length(img_file))
        end
        img_name = [img_path  img_file(n).name];
        im = imread(img_name);
        im = prepare_img( im, image_mean, im_size);
        feat_img = net.forward({im});
        feat(:, n) = single(feat_img{1}(:));
    end

    save(['feat/IDE_'  netname  '_' ef_name{i} '.mat'], 'feat');
    feat = [];
end

caffe.reset_all();


function [altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon,AP,F107,F107A)
%%NRLMSISE00ALT4PRES Given the pressure at a particular latitude and
%                   longitude and time, obtain the approximate ellipsoidal
%                   height using the NRLMSISE-00 atmospheric model. This
%                   function also returns a standard temperature and the
%                   constitutents of the atmosphere, assuming dry air. The
%                   pressure returned is consistent with the function
%                   NRLMSISE00GasTemp, but will not be perfectly
%                   consistent with the function standardAtmosParam.
%
%INPUTS: dayOfYear The integer day of the year in the Gregorian calendar
%                  in universal coordinated time (UTC). Counting starts at
%                  1. the resolution of the model is not sufficient for it
%                  to matter whether 365 or 366 is given at the day if it
%                  isn't/is a leap year.
%   secondOfTheDay The second of the day. This starts at zero. The
%                  resolution of the model is not high enough for leap
%                  seconds to matter, so values above 86400.0 are just
%                  clipped to 86400.0.
%         pressure The measured atmospheirc pressure in Pascals.
%           latLon WGS-84 latitude and longitude of the point where the
%                  pressure is measured.
%               Ap An optional parameter specifying the average daily
%                  geomagnetic index. If omitted, the value 4.0, which is
%                  suitable for models below 90km is used. The index at a
%                  particular time can be obtained from a service of the
%                  International Service of Geomagnetic Indices at
%                  http://www-app3.gfz-potsdam.de/kp_index/
%                  A scalar value of AP indicates standard mode. If a 7X1
%                  vector is passed, then it is assumed that storm mode is
%                  desired. In storm mode, Ap(1) is the daily Ap index.
%                  Ap(2) through Ap(5) are 3 hours Ap indices respectively
%                  for the current time and 3, 6, and 9 hours before the
%                  current time. Ap(6) is the average of 8 3-hour Ap
%                  indices from 12 to 33 hours prior to the current time
%                  and Ap(7) is the average of 8 3-hour Ap indices from 36
%                  to 59 hours prior to the current time.
%             F107 An optional parameter specifying the 10.7cm solar radio
%                  flux for the previous day. If omitted, a value of 150
%                  is used, which should be suitable for models below
%                  90km. Values of the index can be obtained from
%                  http://www.swpc.noaa.gov/ftpdir/indices/quar_DSD.txt
%            F107A An optional parameter specifying the 81 day average of
%                  the 10.7cm radio solar flux. If omitted, a value of 150
%                  is used, which should be suitable for models below 90km.
%
%OUTPUTS: altitude The approximate ellipsoidal altitude associated with
%                  the pressure given on the input in meters.
%         gasTable A cell array of constituent elements of the atmosphere
%                  and their number densities. gasTable{i,1} is a string
%                  listing the name of the ith constituent element of the
%                  atmosphere. This can take the values
%                  'He' Helium
%                  'O'  Elemental Oxygen
%                  'N2' Nitrogen
%                  'O2' Oxygen
%                  'Ar' Argon
%                  'H'  Elemental Hydrogen
%                  'N'  Elemental Nitrogen
%                  'O*' Anomalous Oxygen
%                  gasTable{i,2} is the corresponding number density of
%                  the ith constituent element in units of atoms per cubic
%                  meter.
%                t Temperature variables. t(1) is the exospheric
%                  temperature and t(2) is the temperature at altitude.
%                  The temperature is in units of Kelvin.
%                d A 9X1 vector containing the same information as
%                  gasTable but without labels. The entries in d are
%                  d(1) - HE NUMBER DENSITY
%                  d(2) - O NUMBER DENSITY
%                  d(3) - N2 NUMBER DENSITY
%                  d(4) - O2 NUMBER DENSITY
%                  d(5) - AR NUMBER DENSITY
%                  d(6) - TOTAL MASS DENSITY without d(9)
%                  d(7) - H NUMBER DENSITY
%                  d(8) - N NUMBER DENSITY
%                  d(9) - Anomalous oxygen NUMBER DENSITY(M-3)
%                  where all number densities are in units of atoms per
%                  cubic meter and the units of d(6) are kilograms per
%                  cubic meter. Note that d(6) is computed using a
%                  definition of the atomic mass unit and definitions of
%                  the atomic masses that are not necessarily the most up
%                  to date.
%
%This function is essentially a wrapper for the C-code implementation of
%the  NRLMSISE-00 atmospheric model using appropriate functions to put the
%time and location in the correct format. The model is described in [1].
%
%The algorithm can be compiled for use in Matlab  using the
%CompileCLibraries function.
%
%The algorithm is run in Matlab using the command format
%[altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon);
%or
%[altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon,AP);
%or
%[altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon,AP,F107);
%or
%[altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon,AP,F107,F107A);
%
%EXAMPLE:
% dayOfYear=172;
% secondOfTheDay=29000;
% latLon=[60;-70]*(pi/180);
% F107=150;
% F107A=150;
% AP=4;
% pressure=1;%1 Pascal pressure.
% [altitude,gasTable,t,d]=NRLMSISE00Alt4Pres(dayOfYear,secondOfTheDay,pressure,latLon,AP,F107,F107A)
% %One can verify that the altitude is consistent with the standard model:
% latLonAlt=[latLon;altitude];
% [gasTable1,t1,d1]=NRLMSISE00GasTemp(dayOfYear,secondOfTheDay,latLonAlt,AP,F107,F107A)
% %One will see that t1 and d1 match t and d.
%
%REFERENCES:
%[1] J. M. Picone, A. E. Hedin, D. P. Drob, and A. C. Aikin, "NRLMSISE-00
%    empirical model of the atmosphere: Statistical comparisons and
%    scientific issues," Journal of Geophysical Research: Space Physics,
%    vol. 107, no. A12, Dec. 2002.
%
%June 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

error('This function is only implemented as a mexed C or C++ function. Please run CompileCLibraries.m to compile the function for use.')

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [ output_args ] = performEventFunctionAtDoubleHit( t)
%PERFORMEVENTFUNCTIONATDOUBLEHIT
% this function is used to detect the double touch in the z direction of the
% robot end effector.
global binaryValArray
global averageVal
threshhold=4;
[ f ] = getEEF_Force( t );
zforceVal=f{3}-3.3989;
zforceVal=-zforceVal;
if(zforceVal>0)
else
    zforceVal=0;
end

a=0.7;
averageVal=(averageVal*a+zforceVal*(1-a));

binaryVal=averageVal>threshhold;

binaryValArray(1)=[];
binaryValArray=[binaryValArray;binaryVal];

n=max(max(size(binaryValArray)));
sum=0;
    for i=1:(n-1)
        if((binaryValArray(i+1)-binaryValArray(i))==1)
            sum=sum+1;
        else
        end
    end
sum
    if(sum==2)
        EventFunctionAtDoubleHit();
        binaryValArray=binaryValArray*0;
    end
end


clear; clc; close all;
wkdir = '../'; % The root foler of FM-Bench
addpath([wkdir 'vlfeat-0.9.21/toolbox/']);
vl_setup;

% Datasets = {'TUM', 'KITTI', 'Tanks_and_Temples', 'CPC'};
Datasets = {'KITTI'};

matcher='SIFT-RT'; % SIFT with Ratio Test
estimator='RANSAC';

for s = 1 : length(Datasets)
     dataset = Datasets{s};

    % An example for DoG detector
    FeatureDetection(wkdir, dataset);

    % An example for SIFT descriptor
    FeatureExtraction(wkdir, dataset);

    % An example for exhaustive nearest neighbor matching with ratio test
    FeatureMatching(wkdir, dataset, matcher);

    % An example for RANSAC based FM estimation
    GeometryEstimation(wkdir, dataset, matcher, estimator);

end


function varargout = x
%X   A chebfun of the identity on [-1,1].
%   X = CHEB.X returns a chebfun object for the function @(x)x on [-1,1].
%
%   CHEB.X is shorthand for the expression X = CHEBFUN(@(X) X).

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

x = chebfun(@(x) x);

if ( nargout > 0 )

    % For the syntax x = cheb.x:
    varargout{1} = x;

    if ( nargout > 1 )
        error('CHEB:X:TooManyOutputs',...
            'Too many output arguments. CHEB.X only returns "x".')
    end

else

   % Put 'x' into the workspace:
   assignin('base', 'x', x)

end
end


function solution = createSolution(dim,N)

    solution.lambda = zeros(dim.lambda,   N);
    solution.mu     = zeros(dim.mu,       N);
    solution.u      = zeros(dim.u,        N);
    solution.x      = zeros(dim.x,        N);
    solution.z      = zeros(dim.z,        N);
    solution.LAMBDA = zeros(dim.x, dim.x, N);

end


% EJEMPLO SENCILLO DE INTERFAZ ENTRE ARTE Y COPPELIA.

function irb140_abb_pick_and_place()
    % init basic data. Robots and number of collision objects.
    coppelia = [];
    coppelia.n_collision_objects = 0;
    coppelia.robot.name = 'R1';
    coppelia.robot.n_joints = 6;
    coppelia.robot.end_effector.n_joints = 2;

    coppelia = coppelia_start(coppelia);
    % escribid una funcion que este entre coppelia start y coppelia_stop
    pick_and_place(coppelia)

    coppelia_stop(coppelia);
end

function pick_and_place(coppelia)
%load in arte
global robot
robot = load_robot('ABB', 'IRB140');

    %T1, pre pick point
    T1 = [0  1  0 0.52;
         1  0  0  -0.1;
          0  0 -1 0.45;
          0  0  0 1];
    % pick point
    T2 = [0  1 0 0.52;
          1 0 0  -0.1;
         0 0 -1 0.33;
         0 0 0 1];
    % Release point
    T3 = [-1 0 0 0.1;
         0 1 0  -0.5;
         0 0 -1 0.6;
          0 0 0 1];
    T4 = [-1 0 0 0.1;
         0 1 0  -0.5;
        0 0 -1 0.5;
        0 0 0 1];
    % Podeis ir a una posicion q arbitraria
    q = [0.1 0.1 0.1 0.8 0.8 0.8]';
    set_joint_target_trajectory(coppelia, q)

    %podeis ir a una posicion que sea la cinematica inversa deseada
    qinv = inversekinematic(robot, T1);
    set_joint_target_trajectory(coppelia, qinv(:,1))
    % Esto abre la pinza
    open_gripper(coppelia);
    % podéis esparar un múltiplo del dt de Coppelia (step size de simulación de
    % coppelia, tip. 50 ms. Esperar a que se abra la pinza
    coppelia_wait(coppelia, 10)
    %podeis ir a una posicion que sea la cinematica inversa deseada
    qinv = inversekinematic(robot, T2);
    set_joint_target_trajectory(coppelia, qinv(:,1))

    % cerrar pinza
    close_gripper(coppelia);
    % Esperar a que se cierre
    coppelia_wait(coppelia, 5)
    % subir
    qinv = inversekinematic(robot, T1);
    set_joint_target_trajectory(coppelia, qinv(:,1))

    qinv = inversekinematic(robot, T3);
    set_joint_target_trajectory(coppelia, qinv(:,1))

    qinv = inversekinematic(robot, T4);
    set_joint_target_trajectory(coppelia, qinv(:,1))

    % Esto abre la pinza
    open_gripper(coppelia);
    coppelia_wait(coppelia, 5)

    % VOLVEMOS A la posicion inicial
    set_joint_target_trajectory(coppelia, q)
end


function obj=geom_jitter(obj,varargin)
% geom_jitter Display data as jittered points
%
% Example syntax (default arguments): gramm_object.geom_jitter('width',0.2,'height',0.2)
% In case datapoints are grouped together and are hard to see,
% it's possible to randomly jitter them in an area of width
% 'width' and height 'height' using this function.

p=inputParser;
my_addParameter(p,'width',0.2);
my_addParameter(p,'height',0);
my_addParameter(p,'dodge',0);
my_addParameter(p,'alpha',1);
parse(p,varargin{:});

obj.geom=vertcat(obj.geom,{@(dobj,dd)my_jitter(dobj,dd,p.Results)});
obj.results.geom_jitter_handle={};
end

function  hndl=my_jitter(obj,draw_data,params)

draw_data.x=comb(draw_data.x);
draw_data.y=comb(draw_data.y);

draw_data.x=dodger(draw_data.x,draw_data,params.dodge);

draw_data.x=draw_data.x+rand(size(draw_data.x))*params.width-params.width/2;
draw_data.y=draw_data.y+rand(size(draw_data.y))*params.height-params.height/2;

%We adjust axes limits to accomodate for the jittering
if max(draw_data.x)>obj.plot_lim.maxx(obj.current_row,obj.current_column);
    obj.plot_lim.maxx(obj.current_row,obj.current_column)=max(draw_data.x);
end
if min(draw_data.x)<obj.plot_lim.minx(obj.current_row,obj.current_column);
    obj.plot_lim.minx(obj.current_row,obj.current_column)=min(draw_data.x);
end
if max(draw_data.y)>obj.plot_lim.maxy(obj.current_row,obj.current_column);
    obj.plot_lim.maxy(obj.current_row,obj.current_column)=max(draw_data.y);
end
if min(draw_data.y)<obj.plot_lim.miny(obj.current_row,obj.current_column);
    obj.plot_lim.miny(obj.current_row,obj.current_column)=min(draw_data.y);
end

%hndl=my_point(obj,draw_data);
hndl=line(draw_data.x,draw_data.y,'LineStyle','none','Marker',draw_data.marker,'MarkerEdgeColor','none','markerSize',draw_data.point_size,'MarkerFaceColor',draw_data.color);

set_alpha(hndl,1,params.alpha);

obj.results.geom_jitter_handle{obj.result_ind,1}=hndl;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Max manipulability index ALONG A LINE.
% Use stomp like to optimize along a surface/line
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function experiment2B_K1_N30
close all;
global parameters

%STOMP PARAMETERS
%number of particles
K = 1;

%repeat experiment number of times
parameters.n_repeat = 30;
parameters.experiment_name = 'experiment2B_K1_N30.mat';
parameters.animate = 0;

%repeat the experiment E times
random_manips=[];
Gout = [];
for i=1:parameters.n_repeat
    close all
    [pk, final_manip] = path_planning_SCO_4DOF(K);
    Gout{i}=pk;
    random_manips = [random_manips; final_manip];
    save(parameters.experiment_name)
end

'ended'
parameters.experiment_name


%kmeigen 'Compute eigenvectors and eigenvalues of a square matrix '
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros meigen.pane file
%
% Parameters:
% InputFile: i 'Input Matrix', required: 'Input Matrix'
% OutputFile: o1 'Eigenvectors', optional: 'Eigenvector output matrix'
% OutputFile: o2 'Eigenvalues ', optional: 'Eigenvalue output matrix'
%
% Example: [o1, o2] = kmeigen(i, {'i','';'o1','';'o2',''})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% meigen - Compute Eigenvectors and Eigenvalues of a Square Matrix
%
%  DESCRIPTION
% .I meigen
% computes the eigenvalues and right eigenvectors of a square matrix.
% The input matrix can be nonsymmetric.
% The input matrix is caste to KDCOMPLEX for reading, and all output
% matrices are of type KDCOMPLEX.
%
% The eigenvectors are stored as columns of the eigenvector output matrix.
% The eigenvectors are all normalized to have unit 2-norm and a purely real
% largest component.
%
% The eigenvalues are stored as a diagonal matrix with the i'th diagonal
% being the eigenvalue corresponding the the eigenvector in column i of the
% eigenvector output matrix.
%
% The eigenvalues (and thier associated eigenvectors) appear in decreasing
% order. They are explicitly sorted (while keeping track of the associated
% eigenvectors).
%
% "meigen\fR uses the LAPACK routine ZGEEV to perform the calculations.
%
%
%
%  EXAMPLES
%
%  "SEE ALSO"
% MATRIX:zlapack:zgeev
%
%  RESTRICTIONS
% It has been observed that
% LAPACK generally gets the eigenvalues sorted in decending order, but
% occasionally will return with some of the smaller eigenvalues out of
% order. This is not a bug in "meigen\fR, although a sort may be inserted
% at some future date to make sure that the eigenvalues and thier associated
% eigenvectors are sorted. The 1992 version of the LAPACK USERS' GUIDE doesn't
% specify an ordering for the eigenvalues.
%
%  REFERENCES
% LAPACK Users' Guide, E. Anderson et. al., SIAM 1992. ISBN 0-89871-294-7
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

function varargout = kmeigen(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kmeigen(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'i', '__input';'o1', '__output';'o2', '__output'};
maxval={0,1,1};
minval={0,1,1};
istoggle=[0,1,1];
was_set=istoggle * 0;
paramtype={'InputFile','OutputFile','OutputFile'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=0; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

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


function [text,AIC,BIC]=igarch_display(parameters,ll,vcv,data,p,q,errorType,igarchType,constant)
% Display parameters, tstats, pvals, log-likelihood and AIC/BIC
% from estimates of a IGARCH(P,O,Q) produced using igarch
%
% USAGE:
%   [TEXT] = igarch_display(PARAMETERS,LL,VCV,DATA,P,Q)
%   [TEXT,AIC,BIC] = igarch_display(PARAMETERS,LL,VCV,DATA,P,Q,ERRORTYPE,IGARCHTYE,CONSTANT)
%
% INPUTS:
%   PARAMETERS    - A CONSTANT+p+q-1 column vector of parameters with
%                   [omega alpha(1) ... alpha(p) beta(1) ... beta(q-1) [nu lambda]]'.
%   LL            - The log likelihood at the optimum
%   VCV           - Non-robust standard errors (inverse Hessian)
%   DATA          - A column of mean zero data
%   P             - Positive, scalar integer representing the number of
%                   symmetric innovations
%   Q             - Non-negative, scalar integer representing the number
%                   of lags of conditional variance (0 for ARCH)
%   ERRORTYPE     - [OPTIONAL] The error distribution used, valid types are:
%                     'NORMAL'    - Gaussian Innovations [DEFAULT]
%                     'STUDENTST' - T distributed errors
%                     'GED'       - Generalized Error Distribution
%                     'SKEWT'     - Skewed T distribution
%   IGARCHTYPE     - [OPTIONAL] The type of variance process, either
%                     1 - Model evolves in absolute values
%                     2 - Model evolves in squares [DEFAULT]
%   CONSTANT      - [OPTIONAL] Logical value indicating whether model
%                     should include a constant.  Default is true
%                     (include)
%
% OUTPUTS:
%   TEXT          - Character matrix with the formatted parameters of the model
%   AIC           - Aikake Information Criteria computed from the LL
%   BIC           - Schwartz/Bayesian Information Criteria computed from the LL
%
%  See also IGARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 9/1/2005

%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETER CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 6
        igarchType=2;
        errorType='NORMAL';
        constant = 1;
    case 7
        igarchType=2;
        constant = 1;
    case 8
        constant = 1;
    case 9
        % Nothing
    otherwise
        error('5 to 8 inputs required')
end
if isempty(igarchType)
    igarchType=2;
end
if isempty(errorType)
    errorType='NORMAL';
end
if isempty(constant)
    constant = 1;
end
% parameters, N by 1, real
if any(~isreal(parameters)) || size(parameters,2)~=1
    error('PARAMETERS must be a column vector.')
end
% LL
if ~isscalar(ll) || ~isreal(ll)
    error('LL must be a scalar.')
end
% VCV
if size(vcv,2)~=size(vcv,1) || any(min(eig(vcv))<=0) || size(vcv,1)~=length(parameters)
    error('VCV must be a square positive definite matrix compatible with PARAMETERS.')
end
% data
if any(~isreal(data)) || size(data,2)~=1
    error('DATA must be a T by 1 column vector.')
end
% p
if ~isscalar(p) || p<1 || floor(p)~=p
    error('P must be a postitive scalar integer')
end
% q
if ~isscalar(q) || q<0 || floor(q)~=q
    error('Q must be a positive scalar integer')
end
% errorType
if ~ischar(errorType)
    errorType=[];
end
switch errorType
    case 'NORMAL'
        errorType=1;
        extraP=0;
    case 'STUDENTST'
        errorType=2;
        extraP=1;
    case 'GED'
        errorType=3;
        extraP=1;
    case 'SKEWT'
        errorType=4;
        extraP=2;
    otherwise
        error('ERRORTYPE is not one of the supported distributions')
end
% igarchType
if ~ismember(igarchType,[1 2])
    error('IGARCHTYPE must be either 1 or 2')
end
if length(parameters)~=(constant+p+q-1+extraP)
    error('Size of PARAMETERS is not compatible with input P, O, Q')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETER CHECKING
%%%%%%%%%%%%%%%%%%%%%%%%%%

if igarchType==1
    model_name = ['IAVARCH(' num2str(p) ',' num2str(q) ')'];
else
    model_name = ['IGARCH(' num2str(p) ',' num2str(q) ')'];
end

parameters_text=num2str(parameters,'%4.4f');
stderr=sqrt(diag(vcv));
stderr_text=num2str(stderr,'%4.4f');
tstats=parameters./stderr;
tstats_text=num2str(tstats,'%4.4f');
pvals=2-2*normcdf(abs(tstats));
pvals_text=num2str(pvals,'%4.4f');

% Add in the extra beta
finalBetaPos = constant+p+q;
archParameters = parameters(constant+1:constant+p+q-1);
finalBeta = 1 - sum(archParameters);

n = length(parameters);

parameters_text=[parameters_text(1:finalBetaPos-1,:);
    num2str(finalBeta,'%4.4f');
    parameters_text(finalBetaPos+1:n,:)];
stderr_text=[stderr_text(1:finalBetaPos-1,:);
    repmat('-',1,size(stderr_text,2));
    stderr_text(finalBetaPos+1:n,:)];
tstats_text=[tstats_text(1:finalBetaPos-1,:);
    repmat('-',1,size(tstats_text,2));
    tstats_text(finalBetaPos+1:n,:)];
pvals_text=[pvals_text(1:finalBetaPos-1,:);
    repmat('-',1,size(pvals_text,2));
    pvals_text(finalBetaPos+1:n,:)];

T=length(data);
AIC = -ll/T+2*length(parameters)/T;
BIC = -ll/T+log(T)*length(parameters)/T;

% Format the output
text=[];
text{1,1}= ' ';
text{2,1}= ' ';
text{3,1}=repmat('-',1,50);
text{4,1}=model_name;
text{5,1}=repmat('-',1,50);
text{6,1}=' ';
text{7,1}=['Loglikelihood: ' sprintf('%1.2f',ll)];
text{8,1}=['AIC: ' sprintf('%1.4f',AIC)];
text{9,1}=['BIC: ' sprintf('%1.4f',BIC)];
text{10,1}=' ';

for i=1:size(text,1);
    disp(text{i,1})
end

% Format the parameter table, need to right align everything
K=size(parameters_text,1);
for i=1:K
    N=size(parameters_text,2);
    if any(parameters_text(i,:)==' ')
        numSpace=sum(parameters_text(i,:)==' ');
        parameters_text(i,:)=[repmat(' ',1,numSpace) parameters_text(i,1:N-numSpace)];
    end
    N=size(stderr_text,2);
    if any(stderr_text(i,:)==' ')
        numSpace=sum(stderr_text(i,:)==' ');
        stderr_text(i,:)=[repmat(' ',1,numSpace) stderr_text(i,1:N-numSpace)];
    end
    N=size(tstats_text,2);
    if any(tstats_text(i,:)==' ')
        numSpace=sum(tstats_text(i,:)==' ');
        tstats_text(i,:)=[repmat(' ',1,numSpace) tstats_text(i,1:N-numSpace)];
    end
    N=size(pvals_text,2);
    if any(pvals_text(i,:)==' ')
        numSpace=sum(pvals_text(i,:)==' ');
        pvals_text(i,:)=[repmat(' ',1,numSpace) pvals_text(i,1:N-numSpace)];
    end
end
% Append column labels
labels={' Parameters','   Std. Err.','     T-stat','      P-val'};
cols = {parameters_text,stderr_text,tstats_text,pvals_text};
for i=1:length(labels);
    text1=labels{i};
    text2=cols{i};
    maxcols=max(size(text1,2),size(text2,2));
    if size(text1,2)<maxcols
        labels{i} = [repmat(' ',1,maxcols-size(text1,2)) text1 ];
    end
    if size(text2,2)<maxcols
        cols{i} = [repmat(' ',size(text2,1),maxcols-size(text2,2)) text2 ];
    end
end

% Finally variable names
variable_names=cell(length(parameters),1);
variable_names{1}='omega';
index=2;
for i=1:p
    variable_names{index}=['alpha(' num2str(i) ')'];
    index=index+1;
end
for i=1:q
    variable_names{index}=['beta(' num2str(i) ')'];
    index=index+1;
end
if errorType>1
    variable_names{index}='nu';
    index=index+1;
end
if errorType==4
    variable_names{index}='lambda';
end

maxVarNameLength=max(cellfun('length',variable_names));
for i=1:length(variable_names)
    variable_names{i} = [repmat(' ',1,maxVarNameLength-length(variable_names{i})) variable_names{i}];
end
variable_names=cell2mat(variable_names(:));

% Output the parameters
outmat=strvcat(' ',variable_names); %#ok<*VCAT>
for i=1:4
    outmat=[outmat repmat('  ',size(outmat,1),1) strvcat(labels{i},cols{i})]; %#ok<AGROW>
end

text2=[];
for i=1:length(text)
    text2=strvcat(text2,text{i});
end
text=strvcat(text2,outmat);

disp(outmat)


% Test file for @deltafun/chebcoeffs.m.

function pass = test_chebcoeffs(pref)

if (nargin < 1)
    pref = chebfunpref();
end

a = -4; b = 4;

f = fun.constructor(@(x) sin(x), struct('domain', [a, b]));
mag = .9*(a + (b-a)*rand(3,3));
loc = .9*(a + (b-a)*rand(1,3));

df = deltafun(f, struct('deltaMag', mag, 'deltaLoc', loc));

pass(1) = all( chebcoeffs(f) == chebcoeffs(df) );

end


function [ image ] = DeleteWindowsImage(image,binaryImage)
%UNTITLED3 Summary of this function goes here
%   Detailed explanation goes here
discHeight = size(binaryImage,1);
discWidth = size(binaryImage,2);
Height = size(image,1);
s = Height/discHeight;

for i=1:discHeight
    for j=1:discWidth
        if (binaryImage(i,j)==1)
            image((i-1)*s+1:i*s,(j-1)*s+1:j*s,:) = 0;
        end
    end
end

end


function kern = lfmaKernParamInit(kern)

% LFMAKERNPARAMINIT LFMA kernel parameter initialisation.
% The kernel is designed to interoperate with the multiple output block
% kernel so that f(t) can be inferred given several different
% instantiations of x(t).
%
% The parameters (m, c, delta and k) are constrained positive.
%
% FORMAT
% DESC initialises the latent force model kernel structure with some
% default parameters.
% RETURN kern : the kernel structure with the default parameters placed in.
% ARG kern : the kernel structure which requires initialisation.
%
% SEEALSO : kernCreate, kernParamInit, lfmKernCompute
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

% A wrapper that calls lfmKernParamInit to initialize the parameters of the
% model.

kern = lfmKernParamInit(kern);


function [bestEpoch, bestNet]= pickBestNet(sessionID, N, verbose)
    if nargin<2, N= 5; end
    if nargin<3, verbose= 2; end

    paths= localPaths();
    outFnLatest= sprintf('%s%s_latest.mat', paths.outPrefix, sessionID);
    res= load( outFnLatest, 'obj', 'opts', 'auxData');

    if res.opts.epochTestFrequency~=1
        error('This code assumes epochTestFrequency==1 (it is %d)', res.opts.epochTestFrequency);
    end
    bestEpoch= getBestEpoch(res.obj.val.recall, res.opts.recallNs, N);
    assert(~isempty(bestEpoch));

    if verbose>0
        whichRecallInds= 1:6;
        hline= repmat('=', 1, length(whichRecallInds)*5-1 + 14);

        relja_display('%s Best epoch: %d (out of %d)', sessionID, bestEpoch, size(res.obj.val.recall,2) );

        if isfield(res.obj, 'pretrain')
            offtheshelfValRecs= res.obj.pretrain.val.recall( whichRecallInds );
        end
        bestValRecs= res.obj.val.recall( whichRecallInds, bestEpoch );

        relja_display('%s', hline);
        recallStr= sprintf('%04d ', res.opts.recallNs(whichRecallInds) );
        relja_display('Recall@N      %s', recallStr);
        relja_display('%s', hline);

        if exist('offtheshelfValRecs', 'var')
            offtheshelfStr= sprintf('%.2f ', offtheshelfValRecs);
        end
        trainedStr= sprintf('%.2f ', bestValRecs);

        relja_display('off-the-shelf %s', offtheshelfStr);
        relja_display('our trained   %s', trainedStr);

        if exist('offtheshelfValRecs', 'var')
            relImpStr= sprintf('%.2f ', bestValRecs./offtheshelfValRecs);
            relja_display('trained/shelf %s', relImpStr);
        end

        if verbose>1

            figure; plotResults(res.obj, res.opts, res.auxData);

            figure;
            plot( res.opts.recallNs, res.obj.pretrain.val.recall, 'bx-' );
            hold on;
            plot( res.opts.recallNs, res.obj.val.recall(:, bestEpoch), 'ro-' );
            xlabel('N');
            ylabel('Recall@N');
            xlim([0,50]);
            grid on;
            title( sprintf('%s %s %s %s %s', sessionID, res.opts.netID, res.opts.layerName, res.opts.dbValName, res.opts.method), 'Interpreter', 'none' );
            legend( 'off-the-shelf', 'our trained', 'Location', 'SouthEast');
        end
    end

    if nargout>1
        outFnBest= sprintf('%s%s_ep%06d_latest.mat', paths.outPrefix, sessionID, bestEpoch);
        load(outFnBest, 'net');
        bestNet= net; clear net;
        bestNet.meta.sessionID= sessionID;
        bestNet.meta.epoch= bestEpoch;

        % remove unneeded momentum
        for iLayer= 1:length(bestNet.layers)
            if isfield(bestNet.layers{iLayer}, 'momentum')
                bestNet.layers{iLayer}= rmfield(bestNet.layers{iLayer}, 'momentum');
            elseif isprop(bestNet.layers{iLayer}, 'momentum')
                bestNet.layers{iLayer}.momentum= [];
            end
        end
    end
end