function r = esvm_apply_M(x, boxes, M)
% Apply boosting "co-occurrence" matrix M to the boxes
% function r = esvm_apply_M(x, boxes, M)
%
% Apply the multiplexer matrix M which boosts the scores of a
% window based on its friends and their scores embedded in the
% context feature vector x
%
% Copyright (C) 2011-12 by Tomasz Malisiewicz
% All rights reserved.
%
% This file is part of the Exemplar-SVM library and is made
% available under the terms of the MIT license (see COPYING file).
% Project homepage: https://github.com/quantombone/exemplarsvm

if prod(size(x))==0
  r = zeros(1,0);
  return;
end
exids = boxes(:,6);

%Because the co-occurrence matrix treats exemplar flips as separate
%exemplars, we need to check if an exemplar has been flipped at
%update its exemplar index
exids(boxes(:,7)==1) = exids(boxes(:,7)==1) + size(x,1)/2;
r = zeros(1,size(boxes,1));

for i = 1:size(boxes,1)
  r(i) = (M.w{exids(i)}'*x(:,i) + sum(x(:,i)))-M.b{exids(i)};
end


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

nsteps = 1;
Vfrac_final = 1;
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 [f1,W,beta,grt] = par_fun_B(theta,W_old,K,M,Pt,omega,Hd,Hr,G)
    Theta=diag(theta');
    H=Hd+Hr*Theta*G;
    W_span=W_old;
    [ ~,grt,f0 ] = update_SINR( H,W_old,K,omega );
    [ beta ] =upadte_beta( H,W_old,K,grt);
    while(1)
        [ A,L ] = Proxlinear_beam_para( H,K,M,beta );
        [ W ] =Proxlinear_beam_v2( W_span,H,K,M,grt,Pt,beta,omega,A,L );
        W_span=W;%+step*(W-W_last);
        [ ~,grt,f1 ] = update_SINR( H,W,K,omega );
        [ beta ] =upadte_beta( H,W,K,grt);
        if abs(f0-f1)<1e-3
            break
        end
        f0=f1;
    end
end


function [model, newHeight, newWidth] = vargplvmReduceVidModel(model, height, width, varargin)

% VARGPLVMREDUCEVIDMODEL Take a model computed on a video dataset and return a model which is computed on
% the same video but with lower resolution
% DESC Take a model computed on a video dataset and return a model which is computed on
% the same video but with lower resolution.
% SEEALSO: vargplvmReduceVideo.m
% VARGPLVM

% varargin may include "factor1" and "factor2" optionally

[model.m, newHeight, newWidth] = vargplvmReduceVideo(model.m, height, width, varargin{:});
[model.scale, newHeight, newWidth] = vargplvmReduceVideo(model.scale, height, width, varargin{:});
[model.bias, newHeight, newWidth] = vargplvmReduceVideo(model.bias, height, width, varargin{:});
model.d = newHeight * newWidth;

if ~isempty(model.y)
    [model.y, newHeight, newWidth] = vargplvmReduceVideo(model.y, height, width, varargin{:});
end

model.TrYY = sum(sum(model.m .* model.m));
model.P = model.P1 * (model.Psi1' * model.m);
model.B = model.P1' * model.P;

params = vargplvmExtractParam(model);
model = vargplvmExpandParam(model, params);


function [vs, output, v0] = computeConfidenceInterval2(v0, expdata, model, max_score)

majorIterationLimit = 2000;  %max number of iterations
minorIterationLimit = 1e7;   % essentially infinity
diffInterval = 1e-5;         %gradient step size.
feasibilityTolerance = max_score/20; % how close you need to be to the max score.

v0 = fitC13Data(v0,expdata,model);

if ~isfield(model, 'N')
   model.N = null(model.S);
end

x0 = model.N\v0; % back substitute

% safety check:
if (max(abs(model.S*v0))> 1e-6)
    display('v0 not quite in null space');
    pause;
end
if(max(abs(model.N*x0 - v0)) > 1e-6)
    display('null basis is weird');
    pause;
end

nalpha = size(model.N, 2);
x_L = -1000*ones(nalpha,1);
x_U = 1000*ones(nalpha,1);
Name = 't2';
[A, b_L, b_U] = defineLinearConstraints(model);

numpoints = size(x0,2);
scores = zeros(numpoints,1);

% compute scores for all points.
tProb.user.expdata = expdata;
tProb.user.model = model;
for i = 1:numpoints
    scores(i) = errorComputation2(x0(:,i),tProb);
end
valid_index = scores < max_score + feasibilityTolerance;
fprintf('found %d valid points\n', sum(valid_index));

x0_valid = x0(:,valid_index);
x0_invalid = x0;
scores_valid = scores(valid_index);

c = []; c_L = []; c_U = []; HessPattern = [];
f = 'errorComputation2';
g = 'errorComputation2_grad'; H = [];
%c = 'errorComputation2';
%c_L = 0;
%c_U = max_score;

dc = []; d2c = []; ConsPattern = [];
pSepFunc = [];
x_min = []; x_max = []; f_opt = []; x_opt = [];
Solver = 'snopt';

% pre-compute unnecesary directions.
% if checkedbefore(i) ~= 0 then direction i is redundant
% checkedbefore(i) < 0 means that the sign of all computations should be
% switched.
checkedbefore = zeros(length(model.lb),1);
for i = 2:length(model.lb)
    d = objective_coefficient(i, model);
    for j = 1:i-1
        dj = objective_coefficient(j, model);
        if max(abs(dj - d))< 1e-4
            checkedbefore(i) = j;
            break;
        elseif max(abs(dj + d))< 1e-4
            checkedbefore(i) = -j;
            break;
        end
    end
end

% initialize variables;
numpoints = size(x0, 2);

outputminv = 222*ones(length(model.lb), numpoints);
outputminexitflag = -222*ones(length(model.lb), numpoints);
outputminfinalscore = -222*ones(length(model.lb), numpoints);
outputmaxv = -222*ones(length(model.lb), numpoints);
outputmaxexitflag = -222*ones(length(model.lb), numpoints);
outputmaxfinalscore = -222*ones(length(model.lb), numpoints);
outputminstruct = cell(length(model.lb), numpoints);
outputmaxstruct = cell(length(model.lb), numpoints);

%iterate through directions
parfor i = 1:length(model.lb)
    if checkedbefore(i) ~= 0
        continue;
    end
    for j = -1:2:1 % max and min
        d = objective_coefficient(i,model)*j;
        lbprob = lpAssign(d, A, b_L, b_U, x_L, x_U, [], 'linear_bound', [],[],[],[],[],[],[]);
        lbresult = tomRun('cplex', lbprob, 0);
        fLowBnd = lbresult.f_k;
        fprintf('reaction %d of %d, direction %d, lowerbound %f\n', i,length(model.lb), j, fLowBnd);

        % short circuit if x0 already close to a bound.
        obj1 = d'*x0_valid;
        if(any(abs(obj1-fLowBnd)<.0001))
            display('short circuiting');
            [nil, index1] = min(obj1);
            if (j > 0)
                outputminv(i,:) = j*fLowBnd; % multiply by j to correct sign.
                outputminexitflag(i,:) = 111;
                outputminfinalscore(i,:) = scores_valid(index1);
            else
                outputmaxv(i,:) = j*fLowBnd; % multiply by j to correct sign.
                outputmaxexitflag(i,:) = 111;
                outputmaxfinalscore(i,:) = scores_valid(index1);
            end
        else % gotta actually do the computation.
            all_ds = d'*x0_invalid;
            [nil, index2] = sort(all_ds);

            % initialize temp variables to make parfor work.
            v = j*222*ones(1,numpoints);
            exitflag = -222*ones(1,numpoints);
            finalscore = 222*ones(1,numpoints);
            ostruct = cell(1,numpoints);

            for k = 1:length(index2)
                if exist('ttt.txt', 'file')
                    fprintf('quitting due to file found\n');
                    continue;
                end
                xinitial = x0_invalid(:,index2(k));
%                 Prob  = lpconAssign(d, x_L, x_U, Name, xinitial,...
%                                       A, b_L, b_U,...
%                                       c, dc, d2c, ConsPattern, c_L, c_U,...
%                                       fLowBnd, x_min, x_max, f_opt, x_opt);
                Prob = conAssign(f, g, H, HessPattern, x_L, x_U, Name, xinitial, ...
                                pSepFunc, fLowBnd, ...
                                A, b_L, b_U, c, dc, d2c, ConsPattern, c_L, c_U, ...
                                x_min, x_max, f_opt, x_opt);
                %Prob.NumDiff = 2; % central diff
                %Prob.optParam.CentralDiff = 1e-5;
                %pause;
                Prob.user.expdata = expdata;
                Prob.user.model = model;
                Prob.user.objective = d;
                Prob.user.max_error = max_score;
                Prob.user.diff_interval = diffInterval;
                Prob.user.multiplier = 10;

                Prob.optParam.IterPrint = 0;
                %Prob.optParam.cTol = .1*feasibilityTolerance;

                Prob.PriLevOpt = 0;
                Prob.SOL.PrintFile = strcat('temp/snoptp', num2str(i), 'x', num2str(j), 'x', num2str(k),'.txt');
                Prob.SOL.SummFile = strcat('temp/snopts', num2str(i), 'x', num2str(j), 'x', num2str(k),'.txt');
                Prob.SOL.optPar(35) = majorIterationLimit; %This is major iteration count.
                Prob.SOL.optPar(30) = minorIterationLimit; %total iteration limit;
                %Prob.SOL.optPar(11) = feasibilityTolerance; % feasibility tolerance

                Result = tomRun(Solver, Prob, 5);
                tscore = errorComputation2(Result.x_k, tProb);
                tbest = Result.f_k;

                fprintf('reaction %d (%d), x %d; x=%f (%f); score=%f (%f)\n', i,length(model.lb),j, tbest,fLowBnd, tscore, max_score)

                v(k) = j*tbest;
                exitflag(k) = Result.Inform;
                finalscore(k) = tscore;
                ostruct{k} = Result;
            end
            if (j > 0) %minimizing
                outputminv(i,:) = v; % multiply by j to correct sign.
                outputminexitflag(i,:) = exitflag;
                outputminfinalscore(i,:) = finalscore;
                outputminstruct(i,:) = ostruct;
            else
                outputmaxv(i,:) = v; % multiply by j to correct sign.
                outputmaxexitflag(i,:) = exitflag;
                outputmaxfinalscore(i,:) = finalscore;
                outputmaxstruct(i,:) = ostruct;
            end
        end
    end
end

output.minv = outputminv;
output.maxv = outputmaxv;
output.minexitflag = outputminexitflag;
output.maxexitflag = outputmaxexitflag;
output.minfinalscore = outputminfinalscore;
output.maxfinalscore = outputmaxfinalscore;
output.minstruct = outputminstruct;
output.maxstruct = outputmaxstruct;

for i = 1:length(model.lb)
    if checkedbefore(i) > 0 %short circuit if seen before.
        output.minv(i,:) = output.minv(checkedbefore(i),:);
        output.maxv(i,:) = output.maxv(checkedbefore(i),:);
        output.minexitflag(i,:) = output.minexitflag(checkedbefore(i),:);
        output.maxexitflag(i,:) = output.maxexitflag(checkedbefore(i),:);
        output.minfinalscore(i,:) = output.minfinalscore(checkedbefore(i),:);
        output.maxfinalscore(i,:) = output.maxfinalscore(checkedbefore(i),:);
        output.minstruct(i,:) = output.minstruct(checkedbefore(i),:);
        output.maxstruct(i,:) = output.maxstruct(checkedbefore(i),:);
    elseif checkedbefore(i) < 0
        output.minv(i,:) = -output.maxv(-checkedbefore(i),:);
        output.maxv(i,:) = -output.minv(-checkedbefore(i),:);
        output.minexitflag(i,:) = output.maxexitflag(-checkedbefore(i),:);
        output.maxexitflag(i,:) = output.minexitflag(-checkedbefore(i),:);
        output.minfinalscore(i,:) = output.maxfinalscore(-checkedbefore(i),:);
        output.maxfinalscore(i,:) = output.minfinalscore(-checkedbefore(i),:);
        output.minstruct(i,:) = output.maxstruct(-checkedbefore(i),:);
        output.maxstruct(i,:) = output.minstruct(-checkedbefore(i),:);
    end
end

vs = zeros(length(model.lb), 2);
for i = 1:length(model.lb)
    validindex = output.minfinalscore(i,:) < max_score + feasibilityTolerance;
    if any(validindex)
        vs(i,1) = min(output.minv(i,validindex));
    else
        vs(i,1) = 222;
    end
    validindex = output.maxfinalscore(i,:) < max_score + feasibilityTolerance;
    if any(validindex)
        vs(i,2) = max(output.maxv(i,validindex));
    else
        vs(i,2) = -222;
    end
end

return;

% function that returns the proper objective coefficient for each reaction
% takes into account the reversibility of reactinos etc.
function [d] =  objective_coefficient(i, model)
d = zeros(length(model.lb),1);
d(i) = 1;
if (model.match(i))
    d(model.match(i)) = -1;
end
d = (d'*model.N)'; % transform to null space;
return


filename='Bridge_quad_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP'; incrementFactor = 1;
filterType = 'P1';

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

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


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

% modified based on Tomasz Malisiewicz's esvm code

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

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

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

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

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

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

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

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

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

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


function [result] = oneMatrix(size, isGPU, type)
%%%
%
% Initialize zero matrix with GPU supprot
%
% Thang Luong @ 2015, <lmthang@stanford.edu>
%
%%%
  if isGPU
    result = ones(size, type, 'gpuArray');
  else
    result = ones(size, type);
  end
end


function L = ivmLikelihoods(model, x, y);

% IVMLOGLIKELIHOODS Return the likelihood for each point for the IVM.
% FORMAT
% DESC computes the likelihood of each given point under the given
% IVM model structure.
% ARG model : the IVM for which the likelihoods are to be computed.
% ARG x : the input points where the likelihoods are to be
% computed.
% ARG y : the target points where the likelihoods are to be
% computed.
%
% SEEALSO : noiseLikelihood, ivmPosteriorMeanVar
%
% COPYRIGHT : Neil D. Lawrence, 2005

if nargin < 3
  % This implies evaluate for the traing data.
  mu = model.mu;
  varsigma = model.varSigma;
  y = model.y;
else
  [mu, varsigma] = ivmPosteriorMeanVar(model, x);
end

L = noiseLikelihood(model.noise, mu, varsigma, y);


function Cube = CreateNewGame(hAxes,Cube)
% to create new game
n = 20; % number of random rotations
Axe = unidrnd(3,1,n); % axes
Side = unidrnd(2,1,n)*2-3; % sides
Direction = unidrnd(2,1,n)*2-3; % directions
for k=1:n, % random rotations
  Cube = RotateLayer(hAxes,Cube,Axe(k),Side(k),Direction(k),0);
end
PlotCube(hAxes,Cube);
return


filename='BridgeCool_Quadrilateral_Bilinear_Structured';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'holes';
cost = {'compliance'};
weights = [1];
constraint = {'volume'};
optimizer = 'HAMILTON-JACOBI'; incrementFactor = 1;
filterType = 'P1';
line_search_initiator = 'STANDARD';

HJiter0 = 1;
e2 = 100;
N_holes = [24 11 11];
R_holes = 0.4;
phase_holes = [0 0 0];
nsteps = 5;

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

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


function [local_ids, labels, scores2, THRESH]=paste_segments(chosenpercateg,scores, region_meta_info,MAX_AVG_N_SEGM, NMS_MAX_SEGMS, THRESH)
n_segms_per_img = inf;

NMS_MAX_OVER = 0; % spatial non-maximum supression (1 means nms is not performed)

SIMP_BIAS_STEP = 0;%0.02; % background threshold increase for each additional segment above 1

% max number of segments per image on average (used to set the background threshold)
% we set this value to the average number of objects in the
% training set. Of course, that is just a coincidence. ;-)
%MAX_AVG_N_SEGM = 2.2;
for i=1:numel(scores)
    scores{i}=scores{i}(region_meta_info.gt{i}==0,:);
    for j=1:20
        scores{i}(~ismember([1:size(scores{i},1)], chosenpercateg{j}{i}),j)=-inf;
    end
end

chosen=chosenpercateg{1};
for k=2:20
    for i=1:numel(chosen)
        chosen{i}=[chosen{i}(:); chosenpercateg{k}{i}(:)];
    end
end
for i=1:numel(chosen)
    chosen{i}=unique(chosen{i});
end
while(n_segms_per_img > MAX_AVG_N_SEGM)

    [local_ids, labels, scores2] = nms_inference_simplicity_bias(chosen, scores, NMS_MAX_OVER, NMS_MAX_SEGMS, SIMP_BIAS_STEP, THRESH);

    n_segms_per_img = numel(cell2mat(labels')) / numel(labels)

    THRESH = THRESH
    THRESH = THRESH+0.01;
end
THRESH=THRESH-0.01;


classdef TestComputeRecallPrecisionCurve
    %TestComputeRecallPrecisionCurve

    methods (Static)
        function test_1
            H = [0.8 -0.04 50; -0.05 0.9 50; 1e-5 1e-4 1];
            img1 = imread(fullfile(mexopencv.root(),'test','fruits.jpg'));
            img2 = cv.warpPerspective(img1, H);

            obj = cv.ORB();
            [~, feat1] = obj.detectAndCompute(img1);
            [~, feat2] = obj.detectAndCompute(img2);
            matcher = cv.DescriptorMatcher('BruteForce-Hamming');
            matches1to2 = matcher.radiusMatch(feat1, feat2, 100);

            %HACK: fake correct matches (see cv::evaluateGenericDescriptorMatcher)
            % Ideally we would look into the thresold overlap mask matrix
            correctMatches1to2Mask = cell(size(matches1to2));
            for i=1:numel(matches1to2)
                [~,ord] = sort([matches1to2{i}.distance]);
                mask = false(size(ord));
                mask(ord(1:fix(end/2))) = true;
                correctMatches1to2Mask{i} = mask;
            end

            recallPrecisionCurve = cv.computeRecallPrecisionCurve(...
                matches1to2, correctMatches1to2Mask);
            validateattributes(recallPrecisionCurve, {'numeric'}, ...
                {'2d', 'size',[NaN 2]});
            %plot(recallPrecisionCurve(:,1), recallPrecisionCurve(:,2))
        end

        function test_error_argnum
            try
                cv.computeRecallPrecisionCurve();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end

%{
function nearestPointIndex = getNearestPoint(recallPrecisionCurve, l_precision)
    if (0 <= l_precision && l_precision <= 1)
        [~,nearestPointIndex] = min(abs(recallPrecisionCurve(:,1) - l_precision));
    else
        nearestPointIndex = -1;
end

function recall = getRecall(recallPrecisionCurve, l_precision)
    nearestPointIndex = getNearestPoint(recallPrecisionCurve, l_precision);
    if nearestPointIndex > 0
        recall = recallPrecisionCurve(nearestPointIndex,2);
    else
        recall = -1.0;
    end
end
%}


filename='Chair_hexahedra_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.3;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

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


function [map,lo,hi] = cubehelix(N,start,rots,sat,gamma,irange,domain)
% Generate an RGB colormap of Dave Green's Cubehelix colorscheme. With range and domain control.
%
% (c) 2017 Stephen Cobeldick
%
% Returns a colormap with colors defined by Dave Green's Cubehelix colorscheme.
% The colormap nodes are selected along a tapered helix in the RGB color cube,
% with a continuous increase in perceived intensity. Black-and-white printing
% using postscript results in a monotonically increasing grayscale colorscheme.
%
% This function offers two extra controls over the Cubehelix colorscheme:
%  <irange> specifies the intensity levels of the colormap's endnodes (lightness).
%  <domain> subsamples a part of the helix, so the endnodes are color (not gray).
% These options are both explained in the section below 'Range and Domain'.
%
%%% Syntax:
%  map = cubehelix;
%  map = cubehelix(N);
%  map = cubehelix(N,start,rots,sat,gamma);
%  map = cubehelix(N,start,rots,sat,gamma,irange);
%  map = cubehelix(N,start,rots,sat,gamma,irange,domain);
%  map = cubehelix(N,[start,rots,sat,gamma],...)
%  map = cubehelix([],...)
% [map,lo,hi] = cubehelix(...)
%
% Cubehelix is defined here: http://astron-soc.in/bulletin/11June/289392011.pdf
% For more information and examples: http://www.mrao.cam.ac.uk/~dag/CUBEHELIX/
%
% Note: The original specification (the links above) misnamed the saturation
% option as "hue". In this function the saturation option is named "sat".
%
% See also BREWERMAP RGBPLOT COLORMAP COLORBAR SURF CONTOURF IMAGE CONTOURCMAP JET LBMAP
%
%% Range and Domain %%
%
% Using the default <irange> and <domain> vectors ([0,1]) creates colormaps
% exactly the same as Dave Green's original algorithm: from black to white.
%
% The option <irange> sets the intensity level of the colormap's endnodes:
%  cubehelix(3, [0.5,-1.5,1,1], [0.2,0.8]) % irange=[0.2,0.8]
%  ans = 0.2          0.2          0.2     % <- gray, not black
%        0.62751      0.47498      0.28642
%        0.8          0.8          0.8     % <- gray, not white
%
% The option <domain> sets the sampling window for the Cubehelix, such
% that the tapered-helix does not taper all the way to unsaturated (gray).
% This allows the colormap to end with colors rather than gray shades:
%  cubehelix(3, [0.5,-1.5,1,1], [0.2,0.8], [0.3,0.7]) % domain=[0.3,0.7]
%  ans = 0.020144     0.29948      0.15693 % <- color, not gray shade
%        0.62751      0.47498      0.28642
%        0.91366      0.71351      0.95395 % <- color, not gray shade
%
% The function "colormap_view" demonstrates the effects of these options.
%
%% Examples %%
%
%%% New colors for the COLORMAP example:
% load spine
% image(X)
% colormap(cubehelix)
%
%%% New colors for the SURF example:
% [X,Y,Z] = peaks(30);
% surfc(X,Y,Z)
% colormap(cubehelix([],0.7,-0.7,2,1,[0.2,0.8],[0.4,0.8]))
% axis([-3,3,-3,3,-10,5])
%
%% Input and Output Arguments %%
%
%%% Inputs (*=default):
%  N     = NumericScalar, an integer to define the colormap length.
%        = *[], uses the length of the current figure's colormap.
%  start = NumericScalar, *0.5, the helix's start color (modulus 3): R=1, G=2, B=3.
%  rots  = NumericScalar, *-1.5, the number of R->G->B rotations over the scheme length.
%  sat   = NumericScalar, *1,    controls how saturated the colors are.
%  gamma = NumericScalar, *1,    can be used to emphasize low or high intensity values.
%  irange = NumericVector, *[0,1], range of brightness levels of the scheme's endnodes. Size 1x2.
%  domain = NumericVector, *[0,1], domain of the Cubehelix calculation (endnode positions). Size 1x2.
%
%%% Outputs:
%  map = NumericMatrix, a colormap of RGB values between 0 and 1. Size Nx3
%  lo  = LogicalMatrix, true where <map> values<0 were clipped to 0. Size Nx3
%  hi  = LogicalMatrix, true where <map> values>1 were clipped to 1. Size Nx3
%
% [map,lo,hi] = cubehelix(N, start,rots,sat,gamma, irange, domain)
% OR
% [map,lo,hi] = cubehelix(N, [start,rots,sat,gamma], irange, domain)

%% Input Wrangling %%
%
if nargin==0 || (isnumeric(N)&&isempty(N))
	N = size(get(gcf,'colormap'),1);
else
	assert(isnumeric(N)&&isscalar(N),'First input <N> must be a scalar numeric.')
	assert(isreal(N)&&isfinite(N)&&fix(N)==N,'First input <N> must be real and whole: %g+%gi',N,imag(N))
	N = double(N);
end
%
if N==0
	map = ones(0,3);
	lo = false(0,3);
	hi = false(0,3);
	return
end
%
iss = @(x)isnumeric(x)&&isreal(x)&&isscalar(x)&&isfinite(x);
isn = @(x,n)isnumeric(x)&&isreal(x)&&numel(x)==n&&all(isfinite(x(:)));
%
% Parameters:
if nargin<2
	% Default parameter values.
	start = 0.5;
	rots  = -1.5;
	sat   = 1.5;
	gamma = .65;
elseif nargin<5
	% Parameters are in a vector.
	if nargin>2
		irange = rots;
	end
	if nargin>3
		domain = sat;
	end
	assert(isn(start,4)&&isvector(start),'Second input can be a 1x4 real numeric of parameter values.')
	start = double(start);
	gamma = start(4); sat = start(3); rots = start(2); start = start(1);
else
	% Parameters as individual scalar values.
	assert(iss(start),'Input <start> must be a real scalar numeric.')
	assert(iss(rots), 'Input <rots> must be a real scalar numeric.')
	assert(iss(sat),  'Input <sat> must be a real scalar numeric.')
	assert(iss(gamma),'Input <gamma> must be a real scalar numeric.')
	start=double(start); rots=double(rots); sat=double(sat); gamma=double(gamma);
end
%
% Range:
if any(nargin==[0,1,2,5])
	irange = [0,1];
else
	assert(isn(irange,2),'Input <irange> must be a 1x2 real numeric.')
	irange = double(irange);
end
%
% Domain:
if any(nargin==[0,1,2,3,5,6])
	domain = [0,1];
else
	assert(isn(domain,2),'Input <domain> must be a 1x2 real numeric.')
	domain = double(domain);
end
%
%% Core Function %%
%
vec = linspace(domain(1),domain(2),abs(N)).';
ang = 2*pi * (start/3+1+rots*vec);
csm = [cos(ang),sin(ang)].';
fra = vec.^gamma;
amp = sat .* fra .* (1-fra)/2;
%
tmp = linspace(0,1,abs(N)).'.^gamma;
tmp = irange(1)*(1-tmp) + irange(2)*(tmp);
%
cof = [-0.14861,1.78277;-0.29227,-0.90649;1.97294,0];
%
vec = sign(N)*(1:abs(N)) - min(0,N-1);
for m = abs(N):-1:1
	n = vec(m);
	map(m,:) = tmp(n) + amp(n) * (cof*csm(:,n));
end
%
lo = map<0;
hi = map>1;
map = max(0,min(1,map));
%
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%cubehelix
% Copyright (c) 2017 Stephen Cobeldick
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
% http://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and limitations under the License.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%license


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% SPHARM-MAT 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 SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

disp('Ex0301: Perform Surface Meshes (CALD)')

SpharmMatDir = '../';
CodeDir = [SpharmMatDir 'code'];
addpath(CodeDir);

DataFolder = 'data';

%% (1) Input data directory
InDataDir = [SpharmMatDir DataFolder '/Ex0301/mesh01_obj'];

%% (2) List input file names
inFiles = dir([InDataDir '/*_obj.mat']); inNames={};
for i=1:length(inFiles)
    inNames{end+1} = [InDataDir '/' inFiles(i).name];
end

%% (3) Display input objects (optional)
    %Available values for Space- 'object';'param';'both'
dispConfs.Space = 'object';
    % Available values for Mesh- 'orig';'quad32';'quad64';'quad128';'quad256';'quad512';'icosa1';'icosa2'; ...
    %    'icosa3';'icosa4';'icosa5';'icosa6'
dispConfs.Mesh = 'orig';
    % Available values for Shape- 'solid';'mesh';'both'
dispConfs.Shade = 'both';
    % Available values for Overlay- 'none';'adc_paramap'
dispConfs.Overlay = 'none';
    % Available values for Export- 'screen';'png';'both'
dispConfs.Export = 'png';
dispConfs.Degree = [];
dispConfs.Template = '';

SpharmMatUtilDisplayObjs(dispConfs, inNames, CodeDir);

%% (4) Perform Surface Meshes (CALD)
confs.MeshGridSize = 50;
confs.MaxSPHARMDegree = 6;
confs.Tolerance = 2;
confs.Smoothing = 2;
confs.Iteration = 100;
confs.LocalIteration = 10;
    % Available values for t_major- 'x';'y'
confs.t_major = 'x';
    % Available values for SelectDiagonal- 'ShortDiag';'LongDiag'
confs.SelectDiagonal = 'ShortDiag';

confs.OutDirectory = [SpharmMatDir DataFolder '/Ex0301/mesh02_smo'];

if ~exist(confs.OutDirectory,'dir')
    mkdir(confs.OutDirectory);
end

outNames = SpharmMatParameterization(confs, inNames, 'ParamCALD');

%% (5) Display output objects (optional)
    %Available values for Space- 'object';'param';'both'
dispConfs.Space = 'both';
    % Available values for Mesh- 'orig';'quad32';'quad64';'quad128';'quad256';'quad512';'icosa1';'icosa2'; ...
    %    'icosa3';'icosa4';'icosa5';'icosa6'
dispConfs.Mesh = 'orig';
    % Available values for Shape- 'solid';'mesh';'both'
dispConfs.Shade = 'both';
    % Available values for Overlay- 'none';'adc_paramap'
dispConfs.Overlay = 'adc_paramap';
    % Available values for Export- 'screen';'png';'both'
dispConfs.Export = 'png';
dispConfs.Degree = [];
dispConfs.Template = '';

SpharmMatUtilDisplayObjs(dispConfs, outNames, CodeDir);

inFiles = dir([confs.OutDirectory '/initParamCALD/*.mat']); inNames={};
for i=1:length(inFiles)
    inNames{end+1} = [confs.OutDirectory '/initParamCALD/' inFiles(i).name];
end
SpharmMatUtilDisplayObjs(dispConfs, inNames, CodeDir);

rmpath(CodeDir);

clear;


% Generate samples from the HHMM with the true params.

seed = 1;
rand('state', seed);
randn('state', seed);

discrete_obs = 0;

bnet = mk_square_hhmm(discrete_obs, 1);
Q1 = 1; Q2 = 2; Q3 = 3; F3 = 4; F2 = 5; Onode = 6;
Qnodes = [Q1 Q2 Q3]; Fnodes = [F2 F3];

for seqi=1:1
  evidence = sample_dbn(bnet, 'stop_test', 'is_F2_true_D3');
  clf
  plot_square_hhmm(evidence);
  %pretty_print_hhmm_parse(evidence, Qnodes, Fnodes, Onode, []);
  fprintf('sequence %d has length %d; press key to continue\n', seqi, size(evidence,2))
  pause
end


function [x,fval,exitflag,info,Opt] = opti_fminunc(fun,x0,opts)
%OPTI_FMINUNC Solve an UNO using an OPTI UNO Solver (Matlab Overload)
%
%   [x,fval,exitflag,info] = opti_fminunc(fun,x0) solves the unconstrained
%   nonlinear optimization min f(x) where fun is the nonlinear function to
%   be minimized [fun(x)], starting at x0.
%
%   [x,fval,exitflag,info] = opti_fminunc(fun,...,opts) allows the
%   user to specify optiset options. This includes specifying a solver via
%   the 'solver' field of optiset.
%
%   [x,...,info,Opt] = opti_fminunc(fun,...) returns the internally
%   built OPTI object.

%   Copyright (C) 2011 Jonathan Currie (I2C2)

% Handle missing arguments
if nargin < 3, opts = optiset; end
if nargin < 2, error('You must supply at least 2 arguments to opti_fminunc'); end

%Sort out Fun + Grad
[f,g] = detGrad(fun,x0);

%Build OPTI Object
Opt = opti('fun',f,'grad',g,'x0',x0,'options',opts);

%Solve
[x,fval,exitflag,info] = solve(Opt);


function x = p39_start ( n )

%*****************************************************************************80
%
%% P39_START returns a starting point for optimization for problem 39.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables X.
%
%    Output, real X(N), a starting point for the optimization.
%
  x = [ 0.6, 1.3 ]';

  return
end


function calpak_test51535 ( )

%*****************************************************************************80
%
%% CALPAK_TEST51535 tests YEAR_IS_EMBOLISMIC_EG_LUNAR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST51535\n' );
  fprintf ( 1, '  For the Egyptian Lunar calendar:\n' );
  fprintf ( 1, '  YEAR_IS_EMBOLISMIC_EG_LUNAR determines if a year is\n' );
  fprintf ( 1, '  an embolismic year.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Year  Embolismic?\n' );
  fprintf ( 1, '\n' );

  for y = 1 : 25
    s = y_to_s_eg_lunar ( y );
    fprintf ( 1, '  %s  %d\n', s, year_is_embolismic_eg_lunar ( y ) );
  end

  return
end


function newSegmentation = grow_region(this, startingSegmentation, threshold, nIter)
% Iterative region growing using starting segmentation and threshold.
%
%   Y = MrImage()
%   Y.grow_region(startingSegmentation, threshold, nIter)
%
% This is a method of class MrImage.
%
% IN
%   startingSegmentation    MrImage-object containing the starting
%                           segmentation, e.g. obtained via binarizing
%                           this
%   threshold               Threshold above which voxel connected to the
%                           object are considered part of the object.
%   nIter                   Number of maximum iterations. Maximum path
%                           length that can be grown.
%
% OUT
%
% EXAMPLE
%   Y.grow_region(Y.binarize(500), 300, 20);
%
%   See also MrImage

% Author:   Saskia Bollmann
% Created:  2020-03-30
% Copyright (C) 2020 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public License (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

% initialize
trueLocations = startingSegmentation.copyobj();

for n = 1:nIter

    % create searchLocations
    searchLocations = trueLocations.imdilate(strel('disk', 1));
    % exclude already found locations
    searchLocations = searchLocations - trueLocations;
    % apply in this
    searchVoxel = this .* searchLocations;
    % threshold
    newLocations = searchVoxel.binarize(threshold);
    % check new found locations
    disp([num2str(sum(newLocations.data(:))), ' new voxel(s) found.']);
    if ~any(newLocations.data(:))
        break
    end
    % add to true locations
    trueLocations = trueLocations + newLocations;

end
newSegmentation = trueLocations;

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_IRB7600_500_230(robot, T)
%   Solves the inverse kinematic problem for the ABB IRB7600_500_2300 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_IRB7600_500_2300 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   >>abb=load_robot('ABB', 'IRB7600_500_2300');
%   >>q = [0 0 0 0 0 0];
%   >>T = directkinematic(abb, q);
%   %Call the inversekinematic for this robot
%   >>qinv = inversekinematic(abb, T);
%
%   check that all of them are feasible solutions!
%   and every Ti equals T
%
%   for i=1:8,
%        Ti = directkinematic(abb, qinv(:,i))
%   end
%
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry at the reference for this robot
L1=d(1);
L2=a(2);
L3=d(4);
A2=a(3);
L6=d(6);

A1 = a(1);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%Arrange solutions, there are 8 possible solutions so far.
% if q1 is a solution, q1* = q1 + pi is also a solution.
% For each (q1, q1*) there are two possible solutions
% for q2 and q3 (namely, elbow up and elbow up solutions)
% So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
% there exist two more possible solutions for the last three joints, generally
% called wrist up and wrist down solutions. For this reason,
%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    % use solve_spherical_wrist2 for the particular orientation
    % of the systems in this ABB robot
    % use either the geometric or algebraic method.
    % the function solve_spherical_wrist2 is used due to the relative
    % orientation of the last three DH reference systems.

    %use either one algebraic method or the geometric
    %qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1, 'geometric'); %wrist up
    qtemp = solve_spherical_wrist2(robot, q(:,i), T, 1,'algebraic'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    %qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'geometric'); %wrist down
    qtemp = solve_spherical_wrist2(robot, q(:,i), T, -1, 'algebraic'); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);
A2=a(3);

%compute L4
L4=sqrt(A2^2+L3^2);
%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = (acos((L2^2+r^2-L4^2)/(2*r*L2)));

if ~isreal(gamma)
    disp('WARNING:inversekinematic_irb7600_500_230: the point is not reachable for this configuration, imaginary solutions');
%gamma = real(gamma);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=a(2);
L3=d(4);
A2=a(3);

%See geometry of the robot
L4=sqrt(A2^2+L3^2);
%the angle phi is fixed
phi= acos((A2^2-L3^2+L4^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = (acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

if ~isreal(eta)
   disp('WARNING:inversekinematic_irb7600_500_230: the point is not reachable for this configuration, imaginary solutions');
    %eta = real(eta);
end

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - eta;
q3(2) = pi - phi + eta;


function [  net,res,opts ] = sgd2(  net,res,opts )
% Stochastic gradient descent using second-order information.
%   Ye, C., Yang, Y., Fermuller, C., & Aloimonos, Y. (2017).
%   On the Importance of Consistency in Training Deep Neural Networks. arXiv preprint arXiv:1708.00631.

    if ~isfield(opts.parameters,'weightDecay')
        opts.parameters.weightDecay=1e-4;
    end
    if ~isfield(opts.parameters,'lambda_sgd2')
        opts.parameters.lambda_sgd2=1e0;
    end
    if ~isfield(opts.parameters,'large_matrix_inversion')
        opts.parameters.large_matrix_inversion=0;
    end
    if ~isfield(opts.parameters,'max_inv_size')
        opts.parameters.max_inv_size=500;
    end
    if ~isfield(opts.parameters,'clip')
        opts.parameters.clip=0;
    end
    if ~isfield(opts.parameters,'decorr_bias')
        opts.parameters.decorr_bias=1;
    end
    if ~isfield(net,'iterations')||(isfield(opts,'reset_mom')&&opts.reset_mom==1)
        net.iterations=0;
    end

    net.iterations=net.iterations+1;

    mom_factor=(1-opts.parameters.mom.^net.iterations);
    max_inv_size=opts.parameters.max_inv_size;
    lambda=opts.parameters.lambda_sgd2;

    for layer=1:numel(net.layers)
        if isfield(net.layers{layer},'weights')&&~isempty(net.layers{layer}.weights)
            if ~isfield(net.layers{layer},'momentum')||(isfield(opts,'reset_mom')&&opts.reset_mom==1)
                net.layers{layer}.momentum{1}=zeros(size(net.layers{layer}.weights{1}),'like',net.layers{layer}.weights{1});
                net.layers{layer}.momentum{2}=zeros(size(net.layers{layer}.weights{2}),'like',net.layers{layer}.weights{2});
            end

            dzdw=res(layer).dzdw;
            dzdb=res(layer).dzdb;

            if length(net.layers{layer}.weights)==2
                x=res(layer).x;
                batch_dim=length(size(x));%This assumes the batch size must be >1
                if batch_dim==4%2d cnn
                    x=permute(x,[3,1,2,4]);x=reshape(x,size(x,1),[]);
                    dzdw=permute(dzdw,[1,2,4,3]);new_size=size(dzdw);dzdw=reshape(dzdw,prod(new_size(1:3)),new_size(4));
                    K=size(dzdw,1)/numel(dzdb);dzdb=repelem(dzdb(:),K,1);
                end
                if batch_dim==3%1d cnn
                    x=permute(x,[2,1,3]);x=reshape(x,size(x,1),[]);
                    dzdw=permute(dzdw,[1,3,2]);new_size=size(dzdw);dzdw=reshape(dzdw,prod(new_size(1:2)),new_size(3));
                    K=size(dzdw,1)/numel(dzdb);dzdb=repelem(dzdb(:),K,1);
                end
                subsample=1;batch_size=size(x,2);
                if batch_size>1e4,subsample=ceil(min(50,batch_size/1e4));end
                if subsample>1,x=x(:,1:subsample:end);end
                if opts.parameters.decorr_bias==1
                    %insert bias
                    x=[ones(1,size(x,2),'like',x);x];
                    dzdw=[dzdb,dzdw];
                end
                if size(dzdw,2)<=max_inv_size %small scale inversion
                    dzdw=dzdw/(x*x'./size(x,2)+lambda*eye(size(x,1),'like',x));
                elseif opts.parameters.large_matrix_inversion %divide large scale into smaller scale
                    order=randperm(size(dzdw,2));
                    for i=1:max_inv_size:length(order) %could have been parallelized
                       block_size=min(max_inv_size,length(order)-i+1);
                       idx=order(i:i+block_size-1);x_tmp=x(idx,:);
                       dzdw(:,idx)=dzdw(:,idx)/(x_tmp*x_tmp'./size(x_tmp,2)+lambda*eye(size(x_tmp,1),'like',x));
                    end
                end
                if opts.parameters.decorr_bias==1
                    dzdb=dzdw(:,1);dzdw(:,1)=[];
                end
                if batch_dim==4,dzdw=reshape(dzdw,new_size);dzdw=permute(dzdw,[1,2,4,3]);end
                if batch_dim==3,dzdw=reshape(dzdw,new_size);dzdw=permute(dzdw,[1,3,2]);end
                if batch_dim>2%for cnn:
                    %dzdb is decorrelated with dzdw, take average to smooth the results.
                    dzdb=reshape(mean(reshape(dzdb(:),K,[]),1),size(res(layer).dzdb));
                end
            end

            if opts.parameters.clip>0
                mask=abs(res(layer).dzdw)>opts.parameters.clip;
                res(layer).dzdw(mask)=sign(res(layer).dzdw(mask)).*opts.parameters.clip;%%this type of processing seems to be very helpful
                mask=abs(res(layer).dzdb)>opts.parameters.clip;
                res(layer).dzdb(mask)=sign(res(layer).dzdb(mask)).*opts.parameters.clip;
            end

            %sgd updates
            net.layers{layer}.momentum{1}=opts.parameters.mom.*net.layers{layer}.momentum{1}-(1-opts.parameters.mom).*dzdw - opts.parameters.weightDecay * net.layers{layer}.weights{1};
            net.layers{layer}.weights{1}=net.layers{layer}.weights{1}+opts.parameters.lr*net.layers{layer}.momentum{1}./mom_factor;

            net.layers{layer}.momentum{2}=opts.parameters.mom.*net.layers{layer}.momentum{2}-(1-opts.parameters.mom).*dzdb;
            net.layers{layer}.weights{2}=net.layers{layer}.weights{2}+opts.parameters.lr*net.layers{layer}.momentum{2}./mom_factor;

        end
    end

    if ~isfield(opts,'reset_mom')||opts.reset_mom==1
        opts.reset_mom=0;
    end
end


% Author: Adapted by Jai Juneja from a function created by Ajmal Saeed
% Mian. Modified to include the following functionality:
%   - Calculation of estimated variance
%   - Initial guess input to reduce number of iterations for convergence
%   and increase likelihood of convergence
% Date Modified: 16/03/2013
%
% Original code includes the following copyright information:
% This code is written by Ajmal Saeed Mian {ajmal@csse.uwa.edu.au}
% Computer Science, The University of Western Australia. The code
% may be used, modified and distributed for research purposes with
% acknowledgement of the author and inclusion of this copyright information.
function [R, t, corr, icp_var, data2] = doICP(data1, data2, res, u, tri)
% [R, t, corr, error, data2] = icp2(data1, data2, res, u, tri)
%
% This is an implementation of the Iterative Closest Point (ICP) algorithm.
% The function takes two data sets and registers data2 with data1. It is
% assumed that data1 and data2 are in approximation registration. The code
% iterates till no more correspondences can be found.
%
% This is a modified version (12 April, 2005). It is more accurate and has
% less chances of getting stuck in a local minimum as opposed to my earlier
% version icp.m
%
% Arguments: data1 - 3 x n matrix of the x, y and z coordinates of data set 1
%            data2 - 3 x m matrix of the x, y and z coordinates of data set 2
%            res   - the tolerance distance for establishing closest point
%                     correspondences. Normally set equal to the resolution
%                     of data1
%            u     - 2 x 1 matrix in local polar co-ordinates with initial
%                    guess of rototranslation
%            tri   - optional argument. obtained by tri = delaunayn(data1');
%
% Returns: R - 3 x 3 accumulative rotation matrix used to register data2
%          t - 3 x 1 accumulative translation vector used to register data2
%          corr - p x 3 matrix of the index no.s of the corresponding points of
%                 data1 and data2 and their corresponding Euclidean distance
%          error - the mean error between the corresponding points of data1
%                  and data2 (normalized with res)
%          data2 - 3 x m matrix of the registered data2
%
%

maxIter = 30;
c1 = 0;
c2 = 1;
R = eye(2);
t = zeros(2,1);

% Determine R1 and t1 from odometry input u:

R1 = [cos(u(2)) -sin(u(2)); sin(u(2)) cos(u(2))];
t1 = [u(1); 0];

R_init = R1;
t_init = t1;

if nargin < 5
    tri = delaunayn(data1');
end

n = 0;
while c2 ~= c1
    c1 = c2;
    [corr, D] = dsearchn(data1', tri, data2');
    corr(:,2:3) = [[1 : length(corr)]' D];
    corr(find(D>2*res),:) = [];

    corr = -sortrows(-corr,3);
    corr = sortrows(corr,1);
    [B, Bi, Bj] = unique(corr(:,1));
    corr = corr(Bi,:);

    data2 = R1*data2;
    data2 = [data2(1,:)+t1(1); data2(2,:)+t1(2)];
    R = R1*R;
    t = R1*t + t1;
    c2 = length(corr);
    n = n + 1;
    if n > maxIter
        break;
    end
    % If there is no correlation revert to odometry data
    if isempty(corr)
        R = R_init;
        t = t_init;
        break;
    end
    [R1, t1] = reg(data1, data2, corr);
end

e1 = 1000001;
e2 = 1000000;
n = 0;
noChangeCount = 0;
while noChangeCount < 10
    e1 = e2;
    [corr, D] = dsearchn(data1', tri, data2');
    corr(:,2:3) = [[1 : length(corr)]' D];
    corr(D>2*res,:) = [];

    corr = -sortrows(-corr,3);
    corr = sortrows(corr,1);
    [B, Bi, Bj] = unique(corr(:,1));
    corr = corr(Bi,:);

    % If there is no correlation revert to odometry data
    if isempty(corr)
        R = R_init;
        t = t_init;
        break;
    end
    [R1 t1] = reg(data1, data2, corr);
    data2 = R1*data2;
    data2 = [data2(1,:)+t1(1); data2(2,:)+t1(2)];
    R = R1*R;
    t = R1*t + t1;
    e2 = sum(corr(:,3))/(length(corr)*res);

    n = n + 1;
    if n > maxIter
        break;
    end
    if abs(e2-e1)<res/10000
        noChangeCount = noChangeCount + 1;
    end
end
error = min(e1,e2);     % Minimum error
min_error2 = sum(corr(:,3))^2;  % Least squares minimum error
icp_var = min_error2/(2*length(corr)-3);    % Estimated variance

%-----------------------------------------------------------------
function [R1, t1] = reg(data1, data2, corr)
n = length(corr);
M = data1(:,corr(:,1));
mm = mean(M,2);
S = data2(:,corr(:,2));
ms = mean(S,2);
Sshifted = [S(1,:)-ms(1); S(2,:)-ms(2)];
Mshifted = [M(1,:)-mm(1); M(2,:)-mm(2)];
K = Sshifted*Mshifted';
K = K/n;
[U A V] = svd(K);
R1 = V*U';
if det(R1)<0
    B = eye(2);
    B(2,2) = det(V*U');
    R1 = V*B*U';
end
t1 = mm - R1*ms;


function successFlag=unfoldMeshFromGUI(meshFileName,grayFileName,flatFileName,startCoords,scaleFactor,perimDist,statusHandle,busyHandle,showFigures,saveExtra,truePerimDist);
% function successFlag=unfoldMeshFromGUI(meshFileName,grayFileName,flatFileName,startCoords,scaleFactor,perimDist,statusHandle,busyHandle,showFigures,saveExtra,truePerimDist);
% To be called from unfoldMeshAlpha.m
% This is pre-release code 0.1
%
% ARW 021401
% Unfolds a legally-triangulated mesh using floater/tuele's sparse linear matrix method. Then maps L1 grey nodes to the flattened
% mesh and squeezes remaining grey layers to L1 to generate a flat.mat file for use in mrLoadRet

NPERIMS=1;
SAVE_INTERMEDIATE=0;

if (length(startCoords)~=3)
	error ('Error: you must enter 3 start coords');
end

startTime=now;

% READ IN THE ORIGNIAL MRGRAY MESH
statusStringAdd(statusHandle,['Loading: ',meshFileName]);

mesh=mrReadMrM(meshFileName,0); %
statusStringAdd(statusHandle,'Finding faces');

mesh.faceIndexList=findFaces(mesh,busyHandle);
% The entries in mesh.vertices aren't unique. That is, the same
% vertex can be represented by two different numbers.  This happens
% because strips always contain shared vertices.
%
% This next series of functions finds the unique vertices and
% and builds hash tables for vert->uniqueVerts and uniqueVerts->vert.
% Using these tables, we can adjust the list of colors and faces
% to represent each vertex by a unique index.

[mesh.uniqueVertices,mesh.vertsToUnique,mesh.UniqueToVerts]= unique(mesh.vertices,'rows'); % Get rid of identical points
mesh.uniqueVertices=mesh.uniqueVertices./(repmat(scaleFactor,length(mesh.uniqueVertices),1));
mesh.uniqueNormal=mesh.normal(mesh.vertsToUnique);

mesh.uniqueCols=mesh.rgba(mesh.vertsToUnique,:); % try to assign the colors that came in from mrGray

mesh.uniqueFaceIndexList=findUniqueFaceIndexList(mesh); % this gets rid of faces with multiple duplicate vertices and different permutations of the same face

% Now we find the connection matrix:
% a sparse matrix of nxn points where M(i,j) is 1 if i and j are connected

statusStringAdd(statusHandle,'Finding connection matrix.');

[mesh.connectionMatrix]=findConnectionMatrix(mesh);
messageString=sprintf('%d connections found',length(mesh.connectionMatrix));
statusStringAdd(statusHandle,messageString);

statusStringAdd(statusHandle,'Checking group perimeter.');

% Check to make sure that this is a clean surface: no edge points yet.
edgeList=findGroupPerimeter(mesh,1:length(mesh.uniqueVertices));
if (~isempty(edgeList))
	error('Error - initial mesh is not closed!');
	break;
else
	fprintf('\nInitial mesh is closed');
end

statusStringAdd(statusHandle,'Finding closest mesh point.');

% **********************************
% THIS IS ****** CHECKPOINT 1 ******
% **********************************

[startNode,snDist]=assignToNearest(mesh.uniqueVertices,startCoords);

messageString=sprintf('Start node %d selected at %d voxel units from input coords.',startNode,sqrt(snDist));
statusStringAdd(statusHandle,messageString);

if (sqrt(snDist)>15)
	beep;
	messageString=sprintf('** Warning: mesh node far from start coord. Expect trouble.');
	statusStringAdd(statusHandle,messageString)
end

% Have replaced mrManDist with dll dijkstra
statusStringAdd(statusHandle,'Finding distances from start node');

D=find3DNeighbourDists(mesh,scaleFactor);

% find distances to all the nodes
% When we go to the newer mrGray, dimdist can
% be recovered from the mesh.parameters field, we think.
% Probably mesh.parameters.voxsize.  See mrReadMrM.m

mesh.dist=dijkstra(D,startNode);

% Now we want to generate a perimeter by thresholding these distances
% First find a good distance to take for the perimeter

mesh.perimDist=perimDist;
messageString=sprintf('Perimeter minimum distance=%d',perimDist);
statusStringAdd(statusHandle,messageString);

statusStringAdd(statusHandle,'Using threshold to find perimeter(s).');

% Find perims with simple thold
insideNodes=find(mesh.dist<=perimDist);
insideNodes=insideNodes(:);

% Eliminate 'hanging nodes' : nodes that are not part of a face:
%insideNodes=removeHangingNodes(mesh,insideNodes);

statusStringAdd(statusHandle,'Internal nodes found. Defining perimeter.');

% Find the perimeter points of this group (there can be more than perimeter at this point)
numBadNodes=9999999;
 [perimeterEdges,eulerCondition]=findGroupPerimeter(mesh,insideNodes);

while (numBadNodes>0)
    [perimeterEdges,eulerCondition]=findGroupPerimeter(mesh,insideNodes);
	length(perimeterEdges)
	length(unique(perimeterEdges,'rows'))
    fprintf('\nEuler number=%d',eulerCondition);

    badPerimNodes=findBadPerimNodes(mesh,perimeterEdges);
    numBadNodes=length(badPerimNodes)
    disp(badPerimNodes);

    if(numBadNodes)
        [insideNodes]=setdiff(insideNodes,badPerimNodes);
		%insideNodes=removeHangingNodes(mesh,insideNodes);
    end
end

% The euler number will tell you whether you've got a mesh with no holes in it.

messageString=sprintf('Euler number for this set=%d',eulerCondition);
statusStringAdd(statusHandle,messageString);
uniquePerimPoints=unique(perimeterEdges(:));
messageString=sprintf('%d unique perimeter points found.',length(uniquePerimPoints));
statusStringAdd(statusHandle,messageString);
[orderedUniquePerimeterPoints,biggest]=orderMeshPerimeterPointsAll(mesh,perimeterEdges);
nPerims=size(orderedUniquePerimeterPoints);
fprintf('\n%d different perimeters found',nPerims);
orderedUniquePerimeterPoints=orderedUniquePerimeterPoints{biggest}.points;

%insideNodes=[insideNodes(:),setdiff(perimeterEdges(:),perimeterEdges2(:))];
%perimeterEdges=perimeterEdges2;

% internal points (the ones we want)
unfoldMesh.connectionMatrix=mesh.connectionMatrix(insideNodes,insideNodes);
unfoldMesh.uniqueVertices=mesh.uniqueVertices(insideNodes,:);
unfoldMesh.dist=mesh.dist(insideNodes);

% Convert the edges to feed into orderMeshPerimeterPoints
fullEdgePointList=perimeterEdges(:);

[numEdges,x]=size(perimeterEdges);

newEdges=zeros((numEdges*2),1);
statusStringAdd(statusHandle,'Finding sub-mesh edges.');

for t=1:(numEdges*2)
	if ((~mod(t,100)) & busyHandle)
		updateBusybar(busyHandle,t);
	end
	newEdges(t)=find(insideNodes==fullEdgePointList(t));
end

newEdges=reshape(newEdges,numEdges,2);
statusStringAdd(statusHandle,'Finding sub-mesh perim.');

% Find the perimeter points.
unfoldMesh.orderedUniquePerimeterPoints=zeros(length(orderedUniquePerimeterPoints),1);

for t=1:length(orderedUniquePerimeterPoints)
	f1=find(insideNodes==orderedUniquePerimeterPoints(t));%
	unfoldMesh.orderedUniquePerimeterPoints(t)=f1;%orderMeshPerimeterPoints(newEdges);

end

% Unfolding bit...
% Now we'd like to unfold this.
% Need the following things...
% Connection matrix N (nxn) - almost the same as the connection matrix except that rows=rows/sum(rows)
% and all points on the perimeter have been removed.
% X0 - a px2 matrix containing the 2D locations of the perimeter points.
% P - The perimeter connection matrix: (nxp) 'whose (i,j)th entry is 1/mi when perimeter node j is connected
% to sample node i.

% Find the N and P connection matrices
statusStringAdd(statusHandle,'Finding sub-mesh con. mat.');
[N,P,unfoldMesh.internalNodes]=findNPConnection(unfoldMesh);
[unfoldMesh.distSQ]=find3DNeighbourDists(unfoldMesh);   % Here we find the 3D distance from each point to its neighbours.

% Assign the initial perimeter points - they're going to go in a circle for now...
numPerimPoints=length(unfoldMesh.orderedUniquePerimeterPoints);

perimeterDists=mesh.dist(orderedUniquePerimeterPoints); % Distance of each perimeter point from the start node

statusStringAdd(statusHandle,'Assigning perimeter points');

% We'd like to place the perimeter points down in an intelligent manner. We can place them at the correct distance from the start node
% and at the correct distance from each other. I think.
% We already know their distance from the start node, now we'd like to get their distances
% from each other.

% Should be able to extract this from unfoldMesh.distSQ

%  start at the first perimeter point in unfoldMesh.orderedUniquePerimeterPoints
% Find the distance between this and the next point, etc etc...
nPerimPoints=length(unfoldMesh.orderedUniquePerimeterPoints);
interPerimDists=zeros(nPerimPoints,1);

for thisPerimPoint=1:nPerimPoints
	nextIndex=mod((thisPerimPoint+1),nPerimPoints);
	if(nextIndex==0)
		nextIndex=1;
	end

	interPerimDists(thisPerimPoint)=unfoldMesh.distSQ(unfoldMesh.orderedUniquePerimeterPoints(thisPerimPoint),unfoldMesh.orderedUniquePerimeterPoints(nextIndex));
end

interPerimDists=sqrt(interPerimDists);

unfoldMesh.X_zero=assignPerimeterPositions(perimeterDists); % Can set distances around circle to match actual distances from the center.
% Angular positions will come next.

statusStringAdd(statusHandle,'Solving position equation (slow)');
X=(speye(size(N)) - N) \ (sparse(P * unfoldMesh.X_zero));

unfoldMesh.N=N;
unfoldMesh.P=P;
unfoldMesh.X=X;

% Find out the differences between
dist2DSQ=find2DNeighbourDists(unfoldMesh);
d1=sparse(unfoldMesh.distSQ)-sparse(dist2DSQ);
d1=abs(d1);

goodness=sum(sum(d1.^2));

messageString=sprintf('Current error per node: %d',full(sqrt(goodness))/length(insideNodes));
statusStringAdd(statusHandle,messageString);

% Show the mesh
if (showFigures)

	statusStringAdd(statusHandle,'Displaying unfold');
	figure(50);

	hold off;
	gplot(unfoldMesh.N,unfoldMesh.X);

	axis equal;
	axis off;
	zoom on

end

% Finally - the mapping of grey to mesh points takes place using the entire mesh.
% Therefore, we need to generate X for the mesh as well as the unfold mesh'

unfoldToOrigPerimeter=insideNodes(unfoldMesh.orderedUniquePerimeterPoints);
unfoldToOrigInside=insideNodes(unfoldMesh.internalNodes);

mesh.X=zeros(length(mesh.uniqueVertices),2);
mesh.X(unfoldToOrigPerimeter,:)=unfoldMesh.X_zero;
mesh.X(unfoldToOrigInside,:)=unfoldMesh.X;

hasCoords=[unfoldToOrigPerimeter(:);unfoldToOrigInside(:)];
coords=mesh.X(hasCoords,:);%mesh.X(unfoldToOrigInside,:);
dists=mesh.dist(hasCoords);

% use griddata to image the distance map

	mesh.distMap=makeDistanceImage(coords,dists,128);
	ZI=mesh.distMap;

	if (showFigures)
		figure(51);

		imagesc(mesh.distMap);
		axis image;

		colormap hot;
		title('Manifold distance map');
		colorbar;
	end

mesh.strain=zeros(length(mesh.uniqueVertices),1);
strain=d1.^2;
mesh.strain(unfoldToOrigPerimeter)=sum(strain(unfoldMesh.orderedUniquePerimeterPoints,:)');
mesh.strain(unfoldToOrigInside)=sum(strain(unfoldMesh.internalNodes,:)');
% Record which nodes in the big mesh are in the unfold
mesh.insideNodes=insideNodes;
if (SAVE_INTERMEDIATE)
	statusStringAdd(statusHandle,'Saving intermediate data.');
	save ('meshOutTemp.mat','mesh');
end

% (At this point, switch to testGray....)

% *********************************************************************************
% ************************************************************************************
% **********************************************************************************

statusStringAdd(statusHandle,'Reading grey graph...');

[gNodes, gEdges, gvSize] = readGrayGraph_progress(grayFileName,0);

% Get the indices for all the gnodes (all 3 layers)
l1NodeIndices=find(gNodes(6,:)==1);
l2NodeIndices=find(gNodes(6,:)==2);
l3NodeIndices=find(gNodes(6,:)==3);
l4NodeIndices=find(gNodes(6,:)==4);

% Extract the layer 1 nodes
l1gNodes=gNodes(:,l1NodeIndices);
l1mesh.vertices=l1gNodes(1:3,:);
l1mesh.indices=l1NodeIndices;

% How many gNodes are there?
nGnodes=length(gNodes);
% How many gEdges are there?
nGedges=length(gEdges);

% We want to make a grey node connection matrix - which grey nodes are connected to which other gnodes?
sp=sparse(nGnodes,nGnodes);
i=zeros(nGnodes*30,1); % no more that 30 conenctions per gNode on average!
j=i;

offset=1;

statusStringAdd(statusHandle,'Finding grey connection matrix (slow)');

for t=1:nGnodes % for each gNode...
	if ((~mod(t,1000)) & busyHandle)
		updateBusybar(busyHandle,t);
	end
	% Find its edges (the nodes of the things that it's connected to...)
	thisOffset=gNodes(5,t);
	thisNumEdges=gNodes(4,t);
	theseEdges=gEdges(thisOffset:(thisOffset-1+thisNumEdges));

	% add these to i,j - eventually we'll call sp=sparse(i,j,s,nGnodes,nGnodes)
	% i contains the y coords, j contains the x coords
	endPoint=offset+thisNumEdges-1;

	i(offset:endPoint)=ones(1,thisNumEdges)*t;
	j(offset:endPoint)=theseEdges;

	offset=endPoint+1;

	% % This takes about 18 secs on gwyrdd (nGnodes=32000)
	% and 2.1 seconds on Gwyn :)
end

i=i(1:offset-1);
j=j(1:offset-1);
s=ones(size(i));
sp=sparse(i,j,s,nGnodes,nGnodes);

clear i;
clear j;
clear s;

% We can assign layer 1 grey nodes to the white matter mesh using assignToNearest.dll (see assignToNearest.c)
% Can't do this for higher levels of grey matter 'cos they might get mis-assigned. (Also, potential problem near
% very crinkly edges. - Could we accidentally assign a l1 grey matter node to the wrong WM point?)

% So for higher grey matter points, we have to restrict the possible sub node search space by assigning them >only< to
% points they are connected to. Note that a single layer2 grey node may be connected to several l1 nodes

% The gray is defined over the entire mesh but we only want to deal with gray points over the
% unfolded part. The strategy should be....
% 1) do assignToNearest for each mesh point to find the nearest connected l1 node
% 2) Use the set of l1 nodes found in 1) to build up a list of other connected gray nodes
% 3) Proceed as before...

statusStringAdd(statusHandle,'Mapping L1 to mesh.');
% Find 3D coords of all the l1 gnodes
l1GNodeCoords=l1gNodes(1:3,:)';

% Find 3D coords of all the mesh points (not just the unfolded ones) We have to do this in order
% to shift the two sets to a common mean
meshCoords=mesh.uniqueVertices;

% These coordinate sets seem to have very similar shapes (as we'd expect) but offset to each other. Remove the means to
% center them (but there must be a better way...);
% Mesh coords are in mm, gnode coords are in voxels:
%scaleFactor=[240/256 240/256 1.2]; % This has to be sent in eventually.... IMPORTANT!

l1GNodeCoordsN=l1GNodeCoords;

% There are roughly half as many l1gnodes as there are unique vertices in the mesh.

% Now restrict the mesh coords to just those points in the unfold
meshCoordsN=meshCoords(mesh.insideNodes,:);

% And now restrict the set of l1 gray nodes so that only those that are relatively near the
% mesh are included in the search - this is done first as a simple bounds check
boundedL1NodeIndices=boundsCheck3D(min(meshCoordsN)-3,max(meshCoordsN)+3,l1GNodeCoordsN);
boundedL1GNodes=l1GNodeCoordsN(boundedL1NodeIndices,:);
boundedL1NodeIndices=l1NodeIndices(boundedL1NodeIndices); % This is now a list of indices into the full gNode array

statusStringAdd(statusHandle,'Finding nearest Ll gray points to mesh (very slow)');

% Now we >could< restrict further at this point by running
% [meshToBoundedL1Indices,sqrDist]=assignToNearest(boundedL1GNodes,meshCoordsN);
% to define a subset of l1gNodes closest to the mesh. However, this seems to bugger things up - probably becasue the
% l1gNodes are sampled more densely than the mesh in places.

% What we do instead is run
[boundedL1toMeshIndices,sqrDist]=assignToNearest(meshCoordsN,boundedL1GNodes);
% This returns a list of indices into the meshCoordsN array that links a single 3D mesh point to each l1Gnode)

% and then eliminate any l1gNodes that are more than a set distance away from the mesh - here 3.2mm

% *************************
closeEnough=find(sqrDist<2);
% *************************

boundedL1toMeshNodes=boundedL1GNodes(closeEnough,:); % remember, assignToNearest returns the squared distance
boundedL1toMeshIndices=boundedL1toMeshIndices(closeEnough);

% For each member of the bounded l1Gnodes, this tells us the index of the full mesh point that it maps to.
fullBoundedL1toMeshIndices=insideNodes(boundedL1toMeshIndices);

restL1NodeIndices=boundedL1NodeIndices(closeEnough);
statusStringAdd(statusHandle,'Setting L1 glocs');
% We can start setting gLocs
l1Glocs3d=boundedL1toMeshNodes;
l1Glocs2d=mesh.X(fullBoundedL1toMeshIndices,:);

%l1Curvature=mesh.uniqueNormal(fullBoundedL1toMeshIndices);

l1ConMat=sp(restL1NodeIndices,restL1NodeIndices);

% Prune the l1gray nodes here ? Eliminate connections that are too big (roughly >2 or so.).
% PRUNE
% PRUNE

if (showFigures);
	statusStringAdd(statusHandle,'Displaying L1 gray mesh.');
	figure(8);
	hold off;
	gplot(l1ConMat,l1Glocs2d);
	title('L1 gray mesh');
	zoom on
end

% Now we (only!) have to find l2tol1Indices, l3tol2Indices and l4tol3Indices. This is faster since for each point, we restrict its
% potential nearest neighbours to points that it is connected to in the previous layer.
% We also restrict the l2 nodes to just those that are connected to the restricted l1 nodes and the l3 nodes to those connected to the
% l2 nodes.

% Use the full connection matrix to find which l2 Gnodes are connected to the restricted l1Gnodes
statusStringAdd(statusHandle,'Mapping higher levels (2,3,4)');
L1L2sp=sp(:, restL1NodeIndices);

% We assume here that, almost by definition, the only things that l1 nodes connect to are l2 nodes and other l1 nodes
[restL2NodeIndices,dummy]=find(L1L2sp);
restL2NodeIndices=unique(restL2NodeIndices);
restL2NodeIndices=intersect(restL2NodeIndices,l2NodeIndices); % Only take potential l2 node indices

% repeat this for the l3 nodes
L2L3sp=sp(:,restL2NodeIndices);
[restL3NodeIndices,dummy]=find(L2L3sp);
restL3NodeIndices=unique(restL3NodeIndices);
restL3NodeIndices=intersect(restL3NodeIndices,l3NodeIndices);

% repeat again for the l4 nodes
L3L4sp=sp(:,restL3NodeIndices);
[restL4NodeIndices,dummy]=find(L3L4sp);
restL4NodeIndices=unique(restL4NodeIndices);
restL4NodeIndices=intersect(restL4NodeIndices,l4NodeIndices);

% For each l2 node, find the l1 nodes it's connected to, then find the
% 3D coords of the l2 node and all the connected l1 nodes and send them
% in to assignToNearest.

l2ToL1Indices=findNearestConnected(gNodes', restL2NodeIndices,restL1NodeIndices,sp);
l3ToL2Indices=findNearestConnected(gNodes', restL3NodeIndices,restL2NodeIndices,sp);
l4ToL3Indices=findNearestConnected(gNodes', restL4NodeIndices,restL3NodeIndices,sp);

statusStringAdd(statusHandle,'Setting upper levels glocs');
% Set the l2, l3 and l4 glocs3d
l2Glocs3d=gNodes(1:3,restL2NodeIndices)';
l3Glocs3d=gNodes(1:3,restL3NodeIndices)';
l4Glocs3d=gNodes(1:3,restL4NodeIndices)';

% And the gLocs2
l2Glocs2d=l1Glocs2d(l2ToL1Indices,:);
l3Glocs2d=l2Glocs2d(l3ToL2Indices,:);
l4Glocs2d=l3Glocs2d(l4ToL3Indices,:);

% and the curvature
l2Curvature=l1Curvature(l2ToL1Indices);
l3Curvature=l2Curvature(l3ToL2Indices);
l4Curvature=l3Curvature(l4ToL3Indices);

if (showFigures)
	statusStringAdd(statusHandle,'Displaying 2D glocs');
	figure(12);
	hold off;
	subplot(4,1,1);
	plot(l1Glocs2d(:,1),l1Glocs2d(:,2),'.');
	title('L1 2d glocs');
	subplot(4,1,2);
	plot(l2Glocs2d(:,1),l2Glocs2d(:,2),'g.');
	title('L2 2d glocs');
	subplot(4,1,3);
	plot(l3Glocs2d(:,1),l3Glocs2d(:,2),'r.');
	title('L3 2d glocs');
	subplot(4,1,4);
    plot(l4Glocs2d(:,1),l4Glocs2d(:,2),'k.');
	title('L4 2d glocs');

end
% old skool flat.mat structure looks like
%   curvature      47263x1         378104  double array
%   gLocs2d        47263x2         756208  double array
%   gLocs3d        47263x3        1134312  double array
%   gLocs3dfloat       0x0              0  double array
%   startPoint         1x1              8  double array
%   unfList            1x47263     378104  double array
%   xSampGray          1x701         5608  double array

% But all we really need are.....
statusStringAdd(statusHandle,'Creating flat.mat structure');
gLocs2d=[l1Glocs2d;l2Glocs2d;l3Glocs2d;l4Glocs2d];
gLocs3d=[l1Glocs3d;l2Glocs3d;l3Glocs3d;l4Glocs3d];
curvature=[l1Curvature;l2Curvature;l3Curvature;l4Curvature];

% Curvature goes from 1 to 64
curvature=normalize(curvature)*63+1;
endTime=now;

% TODO , Get this file name from a GUI
statusStringAdd(statusHandle,'Saving:');
statusStringAdd(statusHandle,flatFileName);

messageString=sprintf('Unfold started at %s\nFinished at %s',datestr(startTime),dateStr(endTime));
statusStringAdd(statusHandle,messageString);
if (saveExtra)
	statusString=char(get(statusHandle,'UserData'));

	info.perimDist=perimDist;

	infoStr.startTime=datestr(startTime);
	infoStr.endTime=datestr(endTime);

	infoStr.perimType=truePerimDist;
	infoStr.meshFile=meshFileName;
	infoStr.grayFile=grayFileName;

	save (flatFileName,'gLocs2d','gLocs3d','curvature','statusString','infoStr','ZI');
else
	save (flatFileName,'gLocs2d','gLocs3d','curvature');
end
statusStringAdd(statusHandle,'Done.');

successFlag=1;


function pot = convert_to_pot(CPD, pot_type, domain, evidence)
% CONVERT_TO_POT Convert a gmux CPD to a Gaussian potential
% pot = convert_to_pot(CPD, pot_type, domain, evidence)

switch pot_type
 case {'d', 'u', 'cg', 'scg'},
  error(['can''t convert gmux to potential of type ' pot_type])

 case {'c','g'},
  % We create a large weight matrix with zeros in all blocks corresponding
  % to the non-chosen parents, since they are effectively disconnected.
  % The chosen parent is determined by the value, m,  of the discrete parent.
  % Thus the potential is as large as the whole family.
  ps = domain(1:end-1);
  dps = ps(CPD.dps); % CPD.dps is an index, not a node number (because of param tying)
  cps = ps(CPD.cps);
  m = evidence{dps};
  if isempty(m)
    error('gmux node must have observed discrete parent')
  end
  bs = CPD.sizes(CPD.cps);
  b = block(m, bs);
  sum_cpsz = sum(CPD.sizes(CPD.cps));
  selfsz = CPD.sizes(end);
  W = zeros(selfsz, sum_cpsz);
  W(:,b) = CPD.weights(:,:,m);

  ns = zeros(1, max(domain));
  ns(domain) = CPD.sizes;
  self = domain(end);
  cdom = [cps(:)' self];
  pot = linear_gaussian_to_cpot(CPD.mean(:,m), CPD.cov(:,:,m), W, domain, ns, cdom, evidence);

 otherwise,
  error(['unrecognized pot_type' pot_type])
end


function check = r8sp_check ( m, n, nz_num, row, col )

%*****************************************************************************80
%
%% R8SP_CHECK checks that a R8SP matrix data structure is properly sorted.
%
%  Discussion:
%
%    This routine assumes that the data structure has been sorted,
%    so that the entries of ROW are ascending sorted, and that the
%    entries of COL are ascending sorted, within the group of entries
%    that have a common value of ROW.
%
%    The R8SP storage format stores the row, column and value of each nonzero
%    entry of a sparse matrix.
%
%    The R8SP format is used by CSPARSE ("sparse triplet"), DLAP/SLAP
%    ("nonsymmetric SLAP triad"), by MATLAB, and by SPARSEKIT ("COO" format).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of
%    the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero elements in
%    the matrix.
%
%    Input, integer ROW(NZ_NUM), COL(NZ_NUM), the row and
%    column indices of the nonzero elements.
%
%    Output, logical CHECK, is TRUE if the matrix is properly defined.
%
  check = 1;
%
%  Check 1 <= ROW(*) <= M.
%
  for k = 1 : nz_num

    if ( row(k) < 1 | m < row(k) )
      check = 0;
      return
    end

  end
%
%  Check 1 <= COL(*) <= N.
%
  for k = 1 : nz_num

    if ( col(k) < 1 | n < col(k) )
      check = 0;
      return
    end

  end
%
%  Check that ROW(K) <= ROW(K+1).
%
  for k = 1 : nz_num - 1

    if ( row(k+1) < row(k) )
      check = 0;
      return
    end

  end
%
%  Check that, if ROW(K) == ROW(K+1), that COL(K) < COL(K+1).
%
  for k = 1 : nz_num - 1

    if ( row(k) == row(k+1) )
      if ( col(k+1) <= col(k) )
        check = 0;
        return
      end
    end

  end

  return
end


function [ state ] =movePTPArcYZ_AC(t,theta,c,vel)
%% This function is used for moving the endeffector on an arc in the YZ plane, for the KUKA iiwa 7 R 800.

%% Syntax:
% [ state ] =movePTPArcYZ_AC(t,theta,c,vel)

%% About:
% This function is used to move the end-effector on an arc in the YZ plane,

%% Arreguments:
% t: is the TCP/IP connection
% theta: is the arc angle, in radians
% c: the YZ coordinates of the center of the circle, it is 1x2 vector.
% vel : is a double, defines the motion velocity mm/sec.

% Copyright, Mohammad SAFEEA, 9th of May 2017

    k=[1;0;0];
    pos=getEEFPos( t );
    c=colVec(c);
    c1=[pos{1};c(1);c(2)];
    state=movePTPArc_AC(t,theta,c1,k,vel);

end

function [ y ] = colVec( v)
% Convert a vector to a column vector:
    if(size(v,2)==1)
        y=v;
    else
        y=v';
    end
end


% Digital Video Stabilization and Rolling Shutter Correction using Gyroscopes
% Copyright (C) 2011 Alexandre Karpenko
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

function y = lininterp(t1, x, t2)

if (t2(1) < t1(1))
    t1 = [t2(1); t1];
    x = [x(1,:); x];
end
if (t2(end) > t1(end))
    t1 = [t1; t2(end)];
    x = [x; x(end,:)];
end

y = interp1(t1, x, t2);


function [ panoSegment ] = gbPanoSegment( img, sigma, k, minSz )
%GBPANOSEGMENT Summary of this function goes here
%   Detailed explanation goes here
% global config

[height, width, ~] = size(img);
img_smooth = smooth(img, sigma);

%% pixel segmentation, finish
% edges = zeros(width*height*4,3);
% [gridX, gridY] = meshgrid(1:width,1:height);
% num = 0;
%
% x = gridX(:);
% y = gridY(:);
%
% % xyz = uv2xyzN(coords2uv([x y], 2048, 1024));
% % angleNorm = 2*pi/width;
%
% vector = [1 0; 0 1; 1 1; 1 -1];
% inda = sub2ind([height width], y, x);
%
% for vid = 1:size(vector,1)
%     xv = x+vector(vid,1);
%     yv = y+vector(vid,2);
%     valid = xv>=1 & xv<=width & yv>=1 & yv<=height;
%     indav = inda(valid);
%     indbv = sub2ind([height width], yv(valid), xv(valid));
%     diff = (img_smooth(indav)-img_smooth(indbv)).^2 ...
%          + (img_smooth(indav+width*height)-img_smooth(indbv+width*height)).^2 ...
%          + (img_smooth(indav+2*width*height)-img_smooth(indbv+2*width*height)).^2;
% %     edges(num+1:num+sum(valid),:) = [x(valid)-1 + (y(valid)-1)*width ...
% %                                      xv(valid)-1 + (yv(valid)-1)*width ...
% %                                      sqrt(diff)];
% %     angleWeight = sqrt(1-dot(xyz(indav,:), xyz(indbv,:), 2).^2)/angleNorm;
%     angleWeight = ones(length(indav),1);
%     edges(num+1:num+sum(valid),:) = [indav indbv sqrt(diff).*angleWeight];
%     num = num + sum(valid);
% end
%
% % connect left to right
% XY(1).xyLHS = [ones(height,1) (1:height)'];
% XY(1).xyRHS = [width*ones(height,1) (1:height)'];
% XY(2).xyLHS = [ones(height-1,1) (1:height-1)'];
% XY(2).xyRHS = [width*ones(height-1,1) (2:height)'];
% XY(3).xyLHS = [ones(height-1,1) (2:height)'];
% XY(3).xyRHS = [width*ones(height-1,1) (1:height-1)'];
% %
% % combos = combntns(1:width,2);
% % XY(4).xyLHS = [combos(:,1) ones(size(combos,1),1)];
% % XY(4).xyRHS = [combos(:,2) ones(size(combos,1),1)];
% % XY(5).xyLHS = [combos(:,1) height*ones(size(combos,1),1)];
% % XY(5).xyRHS = [combos(:,2) height*ones(size(combos,1),1)];
% %
% for sid = 1:length(XY)
%     xyLHS = XY(sid).xyLHS;
%     xyRHS = XY(sid).xyRHS;
%     indav = sub2ind([height width], xyLHS(:,2), xyLHS(:,1));
%     indbv = sub2ind([height width], xyRHS(:,2), xyRHS(:,1));
%     diff = (img_smooth(indav)-img_smooth(indbv)).^2 ...
%              + (img_smooth(indav+width*height)-img_smooth(indbv+width*height)).^2 ...
%              + (img_smooth(indav+2*width*height)-img_smooth(indbv+2*width*height)).^2;
%     addNum = size(xyLHS,1);
% %     edges(num+1:num+addNum,:) = [xyLHS(:,1)-1+(xyLHS(:,2)-1)*width ...
% %                                  xyRHS(:,1)-1+(xyRHS(:,2)-1)*width ...
% %                                  sqrt(diff)];
% %     angleWeight = sqrt(1-dot(xyz(indav,:), xyz(indbv,:), 2).^2)/angleNorm;
%     angleWeight = ones(length(indav),1);
%     edges(num+1:num+addNum,:) = [indav indbv sqrt(diff).*angleWeight];
%     num = num + addNum;
% end
%
% edges = edges';
% segment = segmentGraphMex_edge(width*height, num, edges, k, minSz);
% L = unique(segment);
% temp = zeros(height, width);
% for i = 1:length(L)
%     temp(segment==L(i)) = i;
% end
% panoSegment = temp;
%
% % segment = segmentGraphMex(width, height, num, edges, k, minSz);
% % L = unique(segment);
% %
% % temp = zeros(size(segment));
% % for i = 1:length(L)
% %     temp(segment==L(i)) = i;
% % end

%% uniformly sample vectors on sphere and segment, test later
% global coor;
% global tri;
% load('./rectangleDetector/segmentation/uniformvector_lvl8.mat');
[coor, tri] = getUniformVector(8);
% load('./region_based_hypothesis/SketchTokens-master/models/forest/modelSmall.mat');
% st = stDetect( img, model );
% E = stToEdges( st, 1 );
[ E ] = getSketchTokenEdgemap( img );

% h = [0 -1 0; -1 4 -1; 0 -1 0];
% E = filter2(h,rgb2gray(img));
[EE, Ix, Iy] = dt2(double(E), 0.1, 0, 0.1, 0 );
% EE = zeros(1024, 2048);
% EE(747:846,1641:1697) = 1;

% [coor,tri] = icosahedron2sphere(level);
xySubs = uv2coords(xyz2uvN(coor), width, height);
xyinds = sub2ind([height width], xySubs(:,2), xySubs(:,1));
offset = width*height;

edges = [tri(:,1) tri(:,2); tri(:,2) tri(:,3); tri(:,3) tri(:,1)];
invert = edges(:,2)<edges(:,1);
edges(invert,:) = edges(invert,[2 1]);

uniEdges = unique(edges, 'rows');
weight = (img_smooth(xyinds(uniEdges(:,1)))-img_smooth(xyinds(uniEdges(:,2)))).^2 ...
       + (img_smooth(xyinds(uniEdges(:,1))+offset)-img_smooth(xyinds(uniEdges(:,2))+offset)).^2 ...
       + (img_smooth(xyinds(uniEdges(:,1))+2*offset)-img_smooth(xyinds(uniEdges(:,2))+2*offset)).^2;
gdweight = (EE(xyinds(uniEdges(:,1)))+EE(xyinds(uniEdges(:,2))))/2;
panoEdge = [uniEdges(:,1)'; uniEdges(:,2)'; sqrt(weight)'+10*double(gdweight)'];

maxID = size(coor,1);
num = size(uniEdges,1);

edgeLabel = segmentGraphMex_edge(maxID, num, panoEdge, k, minSz);

L = unique(edgeLabel);
temp = zeros(size(edgeLabel));
% center = zeros(length(L),2);
[gridX, gridY] = meshgrid(1:width, 1:height);
for i = 1:length(L)
%     fprintf('%d\n',i);
    temp(edgeLabel==L(i)) = i;
%     center(i,:) = [mean(gridX(edgeLabel==L(i))) mean(gridY(edgeLabel==L(i)))];
end

pixelvector = uv2xyzN(coords2uv([gridX(:) gridY(:)], width, height));

k = 1;
% [nnidx, dists] = annsearch( coor', pixelvector', k);
[nnidx, dists] = knnsearch( coor, pixelvector);

panoSegment = reshape(temp(nnidx), height, width);
% [ colors, pc ] = pcsel( center, 6 );
% C = [1 0 0;0 1 0;0 0 1;1 1 0;1 0 1;0 1 1];
% figure; imshow(label2rgb(panoSegment, C(colors,:)));

% panoSegment = zeros(height, width);
% for i = 1:length(L)
%     panoSegment(nnidx
% end

end


function Us = get_PCproj(S1, row, col, wPCA, maskMaxChans)

[nT, nChan] = size(S1);
dt = -21 + [1:size(wPCA,1)];
inds = repmat(row', numel(dt), 1) + repmat(dt', 1, numel(row));

clips = reshape(S1(inds, :), numel(dt), numel(row), nChan);

mask = repmat([1:nChan], [numel(row) 1]) - repmat(col, 1, nChan);
Mask(1,:,:) = abs(mask)<maskMaxChans;

clips = bsxfun(@times, clips , Mask);

Us = wPCA' * reshape(clips, numel(dt), []);
Us = reshape(Us, size(wPCA,2), numel(row), nChan);

Us = permute(Us, [3 2 1]);


function statplot(c)

   % Private method. See ../plot for details.

   % Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
   % $Date$
   % $Revision$

   Rmean  = c.stat(:,1);
   Rrmshi = c.stat(:,2);
   Rrmslo = c.stat(:,3);
   T      = c.stat(:,4);
   Trms   = c.stat(:,5);

   % PLOT RESULTS
   figure('Color','w','Position',[50 50 1000 600]);
   set(gcf,'DefaultLineLineWidth',1);
   set(gcf,'DefaultAxesFontSize',14);

   subplot(2,1,1);
   hold on; grid on; box on;
   ylabel('Time (s)');
   title('Best fit delay time and 1\sigma error');
   xfill = [ [1:length(T)]' ; [length(T):-1:1]' ];
   yfill = [T+Trms ; flipud(T-Trms)];
   fill(xfill,yfill,[.9 .9 .9]);
   plot(c.lags','b.','MarkerSize',5);
   plot(T,'ko-','LineWidth',2);
   xlim([1 length(T)]);
   maxval = 1.05*max(max(abs(c.lags)));
   ylim([-1*maxval maxval]);

   subplot(2,1,2);
   hold on; grid on; box on;
   xfill = [ [1:length(T)]' ; [length(T):-1:1]' ];
   yfill = [Rrmshi ; flipud(Rrmslo)];
   fill(xfill,yfill,[.9 .9 .9]);
   plot(Rmean,'ko-','LineWidth',2);
   xlabel('Trace no.');
   ylabel('Mean R');
   title('Mean max correlation and 1\sigma error');
   xlim([1 length(T)]);
   ylim([0 1]);

   % PREP PRINT OUT
   set(gcf, 'paperorientation', 'landscape');
   set(gcf, 'paperposition', [1 3 9 4] );
   print(gcf, '-depsc2', 'FIG_stat.ps');
end


function [dataout] = ft_denoise_dssp(cfg, datain)

% FT_DENOISE_DSSP implements a dual signal subspace projection algorithm
% to suppress interference outside a predefined source region of
% interest. It is based on: Sekihara et al. J. Neural Eng. 2016 13(3), and
% Sekihara et al. J. Neural Eng. 2018 15(3).
%
% Use as
%   dataout = ft_denoise_dssp(cfg, datain)
% where cfg is a configuration structure that contains
%   cfg.channel          = Nx1 cell-array with selection of channels (default = 'all'), see FT_CHANNELSELECTION for details
%   cfg.trials           = 'all' or a selection given as a 1xN vector (default = 'all')
%   cfg.pertrial         = 'no', or 'yes', compute the temporal projection per trial (default = 'no')
%   cfg.sourcemodel      = structure, source model with precomputed leadfields, see FT_PREPARE_LEADFIELD
%   cfg.demean           = 'yes', or 'no', demean the data per epoch (default = 'yes')
%   cfg.dssp             = structure with parameters that determine the behavior of the algorithm
%   cfg.dssp.n_space     = 'all', or scalar. Number of dimensions for the
%                          initial spatial projection.
%   cfg.dssp.n_in        = 'all', or scalar. Number of dimensions of the
%                          subspace describing the field inside the ROI.
%   cfg.dssp.n_out       = 'all', or scalar. Number of dimensions of the
%                          subspace describing the field outside the ROI.
%   cfg.dssp.n_intersect = scalar (default = 0.9). Number of dimensions (if
%                          value is an integer>=1), or threshold for the
%                          included eigenvalues (if value<1), determining
%                          the dimensionality of the intersection.
%
% See also FT_DENOISE_PCA, FT_DENOISE_SYNTHETIC, FT_DENOISE_TSR

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

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

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

% the ft_abort variable is set to true or false in ft_preamble_init
if ft_abort
  % do not continue function execution in case the outputfile is present and the user indicated to keep it
  return
end

% check the input data
datain = ft_checkdata(datain, 'datatype', {'raw'}); % FIXME how about timelock and freq?

% ensure the external cellfunction toolbox is on the path
ft_hastoolbox('cellfunction', 1);

% check if the input cfg is valid for this function
cfg = ft_checkconfig(cfg, 'forbidden',  {'channels', 'trial'}); % prevent accidental typos, see issue 1729

% set the defaults
cfg.trials            = ft_getopt(cfg, 'trials',  'all', 1);
cfg.channel           = ft_getopt(cfg, 'channel', 'all');
cfg.pertrial          = ft_getopt(cfg, 'pertrial', 'no');
cfg.sourcemodel       = ft_getopt(cfg, 'sourcemodel');
cfg.demean            = ft_getopt(cfg, 'demean', 'yes');
cfg.dssp              = ft_getopt(cfg, 'dssp');         % sub-structure to hold the parameters
cfg.dssp.n_space      = ft_getopt(cfg.dssp, 'n_space', 'interactive'); % number of spatial components to retain from the Gram matrix
cfg.dssp.n_in         = ft_getopt(cfg.dssp, 'n_in',    'interactive'); % dimensionality of the Bin subspace to be used for the computation of the intersection
cfg.dssp.n_out        = ft_getopt(cfg.dssp, 'n_out',   'interactive'); % dimensionality of the Bout subspace to be used for the computation of the intersection
cfg.dssp.n_intersect  = ft_getopt(cfg.dssp, 'n_intersect', 'interactive'); % dimensionality of the intersection
cfg.output            = ft_getopt(cfg, 'output', 'original');

pertrial = istrue(cfg.pertrial);

% select channels and trials of interest, by default this will select all channels and trials
tmpcfg = keepfields(cfg, {'trials', 'channel', 'tolerance', 'showcallinfo', 'trackcallinfo', 'trackusage', 'trackdatainfo', 'trackmeminfo', 'tracktimeinfo', 'checksize'});
datain = ft_selectdata(tmpcfg, datain);
% restore the provenance information
[cfg, datain] = rollback_provenance(cfg, datain);

if istrue(cfg.demean)
  ft_info('demeaning the time series');
  tmpcfg = [];
  tmpcfg.demean = 'yes';
  datain = ft_preprocessing(tmpcfg, datain);
  % restore the provenance information
  [cfg, datain] = rollback_provenance(cfg, datain);
end

% match the input data's channels with the labels in the leadfield
sourcemodel = cfg.sourcemodel;
if ~isfield(sourcemodel, 'leadfield')
  ft_error('cfg.sourcemodel needs to contain leadfields');
end
[indx1, indx2] = match_str(datain.label, sourcemodel.label);
if ~isequal(indx1(:),(1:numel(datain.label))')
  ft_error('unsupported mismatch between data channels and leadfields');
end
if islogical(sourcemodel.inside)
  inside = find(sourcemodel.inside);
else
  inside = sourcemodel.inside;
end
for k = inside(:)'
  sourcemodel.leadfield{k} = sourcemodel.leadfield{k}(indx2,:);
end

% compute the Gram-matrix of the supplied forward model
lf = cat(2, sourcemodel.leadfield{:});
G  = lf*lf';

%dat     = cat(2,datain.trial{:});
[Bclean, subspace] = dssp(datain.trial, G, cfg.dssp.n_in, cfg.dssp.n_out, cfg.dssp.n_space, cfg.dssp.n_intersect, pertrial);
% datAe   = datain.trial*cellfun(@transpose, Ae, 'UniformOutput', false); % the projection is a right multiplication
% with a matrix (eye(size(Ae,1))-Ae*Ae'), since Ae*Ae' can become quite
% sizeable, it's computed slightly differently here.

% put some diagnostic information in the output cfg.
cfg.dssp.subspace = subspace;

% replace the input cfg values
cfg.dssp.n_space = subspace.S(1).n;
cfg.dssp.n_in    = subspace.Sin(1).n;
cfg.dssp.n_out   = subspace.Sout(1).n;
cfg.dssp.n_intersect = subspace.T(1).n;

% compute the cleaned data and put in a cell-array
switch cfg.output
  case 'original'
    trial = Bclean;
  case 'complement'
    trial = datain.trial-Bclean;
  otherwise
    ft_error(sprintf('cfg.output = ''%s'' is not implemented',cfg.output));
end

% create the output argument
dataout       = keepfields(datain, {'label', 'time', 'fsample', 'trialinfo', 'sampleinfo', 'grad', 'elec', 'opto'}); % grad can be kept and does not need to be balanced, since the cleaned data is a mixture over time, not space.
dataout.trial = trial;

% do the general cleanup and bookkeeping at the end of the function
ft_postamble debug
ft_postamble previous   datain
ft_postamble provenance dataout
ft_postamble history    dataout
ft_postamble savevar    dataout

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions for the computation of the projection matrix
% kindly provided by Kensuke, and adjusted a bit by Jan-Mathijs
function [Bclean, subspace] = dssp(B, G, Nin, Nout, Nspace, Nintersect, pertrial)

% Nc: number of sensors
% Nt: number of time points
% inputs
% B(Nc,Nt):  interference overlapped sensor data
% G(Nc,Nc): Gram matrix of voxel lead field
% Nout and Nin: dimensions of the two row spaces
% recom_Nspace: recommended value for the dimension of the pseudo-signal subspace
% outputs
% Bclean(Nc,Nt): cleaned sensor data
% Nintersect: dimension of the intersection
% Nspace: dimension of the pseudo-signal subspace
%  ------------------------------------------------------------
%  programmed by K. Sekihara,  Signal Analysis Inc.
%  All right reserved by Signal Analysis Inc.
% -------------------------------------------------------------
%
% The code below is modified by Jan-Mathijs, no functional changes
% merely cosmetics, added the possibility to run the temporal subspace per
% trial

% eigen decomposition of the Gram matrix, matrix describing the spatial components of the defined 'in' compartment
fprintf('Computing the spatial subspace projection\n');
fprintf('Eigenvalue decomposition of the Gram matrix\n');
[Uspace,S] = eig(G);
Sspace     = abs(diag(S));

[Sspace, iorder] = sort(-Sspace);
Sspace           = -Sspace;
Uspace(:,:)      = Uspace(:,iorder);
Nspace           = getN(Nspace, Sspace, 'spatial');

% spatial subspace projector
Us   = Uspace(:,1:Nspace);

%USUS = Us*Us';
% % Bin and Bout creation
%Bin  =                  USUS  * B;
%Bout = (eye(size(USUS))-USUS) * B;

% computationally more efficient than the above
fprintf('Applying the spatial subspace projector\n');
Bin  = Us*(Us'*B);
Bout = B - Bin;

fprintf('Computing the subspace projector based on signal correlations\n');
[Ae, subspace] = CSP01(Bin, Bout, Nin, Nout, Nintersect, pertrial);

% add the first spatial subspace projection information as well
subspace.S.U = Uspace;
subspace.S.S = Sspace;
subspace.S.n = Nspace;
subspace.trial = Ae;

fprintf('Applying the subspace projector\n');
%Bclean    = B - (B*Ae)*Ae'; % avoid computation of Ae*Ae'
Bclean = B - (B*cellfun(@transpose, Ae, 'UniformOutput', false))*Ae;

function [Ae, subspace] = CSP01(Bin, Bout, Nin, Nout, Nintersect, pertrial)
%
% interference rejection by removing the common temporal subspace of the two subspaces
% K. Sekihara,  March 28, 2012
% Golub and Van Loan, Matrix computations, The Johns Hopkins University Press, 1996
%
%  Nc: number of channels
%  Nt: number of time points
% inputs
%  Bout(1:Nc,1:Nt): interference data
%  Bin(1:Nc,1:Nt): signal plus interference data
%  Nout: dimension of the interference subspace
%  Nin: dimension of the signal plus interference subspace
%  Nintersect: dimension of the intersection of the two subspaces
% outputs
% Ae = matrix from which the projector onto the intersection can
%      be obtained:
% subspace: struct containing information about the different subspace
%      projections
%  ------------------------------------------------------------
%  programmed by K. Sekihara,  Signal Analysis Inc.
%  All right reserved by Signal Analysis Inc.
% -------------------------------------------------------------
%

if ~pertrial
  % compute the projection across trials
  trllist = 1:numel(Bout);
else
  % compute the projection per trial
  trllist = (1:numel(Bout))';
end

Ae = cell(size(Bin));
for k = 1:size(trllist,1)
  indx = trllist(k,:); % this is either a scalar, or a vector
  [Uout,Sout,Vout] = svd(cat(2, Bout{indx}),'econ');
  [Uin, Sin, Vin]  = svd(cat(2, Bin{indx}), 'econ');
  Sout = diag(Sout);
  Sin  = diag(Sin);

  Nout = getN(Nout, Sout, 'outside');
  Nin  = getN(Nin,  Sin,  'inside');

  % compute unit-norm orthogonal time courses
  Qout = diag(1./Sout(1:Nout))*Uout(:,1:Nout)'*Bout(indx); % keep it in cell representation
  Qin  = diag(1./Sin(1:Nin)  )* Uin(:,1:Nin)' *Bin(indx);
  C    = Qin * cellfun(@transpose, Qout, 'UniformOutput', false);
  C    = sum(cat(3, C{:}), 3);

  % store the subspace information that is used in the next step
  subspace.Sin(k).U  = Uin;
  subspace.Sin(k).S  = Sin;
  subspace.Sin(k).n  = Nin;
  subspace.Sout(k).U = Uout;
  subspace.Sout(k).S = Sout;
  subspace.Sout(k).n = Nout;

  % covariance matrix of unit-norm 'components' -> how does this relate to
  % multivariate decomp? This is I guess equivalent mathematically
  [U,S] = svd(C);
  S     = diag(S);
  Nintersect = getN(Nintersect, S, 'intersection');

  Ae(indx) = U(:, 1:Nintersect)'*Qin;

  % keep the subspace information
  subspace.T(k).U = U;
  subspace.T(k).S = S;
  subspace.T(k).C = C; clear C;
  subspace.T(k).n = Nintersect;
end

function N = getN(N, S, name)

ttext = sprintf('enter the dimension for the %s field: ', name);
if isempty(N)
  N  = input(ttext);
elseif ischar(N) && isequal(N, 'interactive') && ~any(strcmp(name, {'outside' 'intersection'}))
  figure, plot(log10(S),'-o'); drawnow
  N = input(ttext);
elseif ischar(N) && isequal(N, 'interactive') && any(strcmp(name, {'outside' 'intersection'}))
  figure, plot(S, '-o'); drawnow
  N = input(ttext);
elseif ischar(N) && isequal(N, 'all')
  N = find(S./S(1)>1e5*eps, 1, 'last');
elseif isnumeric(N) && N<1
  N = find(S<=N, 1, 'last');
end
fprintf('Using %d dimensions for the %s field\n', N, name);


function [z, logQ] = sampleStateSeq_RGS( logSoftEv, eta_ii, F_ii, ii, TargetPsi )
% Draw new assignments for the discrete hidden state sequence
%  for each time series object, under *restricted settings*.
% Uses a fast message backward, sample forwards algorithm.

ks = find( F_ii > 0 );
T  = size(logSoftEv,2);
K  = length(ks);

if K == 1
    z = ks(1)*ones( 1, Tii );
    continue;
end

pi_init = 1/K*ones(1,K);
pi      = eta_ii;

normC = max( logSoftEv, [], 1);
logSoftEv = bsxfun( @minus, logSoftEv, normC );
Lik = exp( logSoftEv );

SEED = randomseed();
randomseed( SEED+1 );
[z, logQ] = SampleHMMStateSeqWithQsC( pi, Lik, pi_init, SEED(1) );
z = ks(z);

end % main function