function [l,d,perm] = mchol(A,mu)
% Compute a modified LDL factorization of A
% (MEX ME!)

if nargin < 2
    mu = 1e-12;
end

n = size(A,1);
l = eye(n);
d = zeros(n,1);
perm = 1:n;

for i = 1:n
    c(i,i) = A(i,i);
end

% Compute modification parameters
gamma = max(abs(diag(A)));
xi = max(max(abs(setdiag(A,0))));
delta = mu*max(gamma+xi,1);
if n > 1
    beta = sqrt(max([gamma xi/sqrt(n^2-1) mu]));
else
    beta = sqrt(max([gamma mu]));
end

for j = 1:n

    % Find q that results in Best Permutation with j
    [maxVal maxPos] = max(abs(diag(c(j:end,j:end))));
    q = maxPos+j-1;

    % Permute d,c,l,a
    d([j q]) = d([q j]);
    perm([j q]) = perm([q j]);
    c([j q],:) = c([q j],:);
    c(:,[j q]) = c(:,[q j]);
    l([j q],:) = l([q j],:);
    l(:,[j q]) = l(:,[q j]);
    A([j q],:) = A([q j],:);
    A(:,[j q]) = A(:,[q j]);

    for s = 1:j-1
        l(j,s) = c(j,s)/d(s);
    end
    for i = j+1:n
        c(i,j) = A(i,j) - sum(l(j,1:j-1).*c(i,1:j-1));
    end
    theta = 0;
    if j < n && j > 1
        theta = max(abs(c(j+1:n,j)));
    end
    d(j) = max([abs(c(j,j)) (theta/beta)^2 delta]);
    if j < n
        for i = j+1:n
            c(i,i) = c(i,i) - (c(i,j)^2)/d(j);
        end
    end
end


function net = rbfunpak(net, w)
%RBFUNPAK Separates a vector of RBF weights into its components.
%
%	Description
%	NET = RBFUNPAK(NET, W) takes an RBF network data structure NET and  a
%	weight vector W, and returns a network data structure identical to
%	the input network, except that the centres C, the widths WI, the
%	second-layer weight matrix W2 and the second-layer bias vector B2
%	have all been set to the corresponding elements of W.
%
%	See also
%	RBFPAK, RBF
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Check arguments for consistency
errstring = consist(net, 'rbf');
if ~errstring
  error(errstring);
end

if net.nwts ~= length(w)
  error('Invalid length of weight vector')
end

nin 	= net.nin;
nhidden = net.nhidden;
nout 	= net.nout;

mark1 = nin*nhidden;
net.c = reshape(w(1:mark1), nhidden, nin);
if strcmp(net.actfn, 'gaussian')
  mark2 = mark1 + nhidden;
  net.wi = reshape(w(mark1+1:mark2), 1, nhidden);
else
  mark2 = mark1;
  net.wi = [];
end
mark3 = mark2 + nhidden*nout;
net.w2 = reshape(w(mark2+1:mark3), nhidden, nout);
mark4 = mark3 + nout;
net.b2 = reshape(w(mark3+1:mark4), 1, nout);


%%%test for Random MultiGraphs
clear all;
clc;

addpath('./rmg/');
addpath('./util/');

%===== basic parameters =================================
bandNum = 4; % number of band for LPE band selection
w = 7;       % patch size
% LBP feature extraction
r = 1;  nr = 8;
% number of graphs
% for computationa efficiency, number of graphs are set as 4.
% you can set more graphs to obtain better performance
kg = 4;

fprintf('... ... loading data begin ...\n');
load IndianPines_Data.mat;
fprintf('... ... loading data finished !!! \n');

% training number for Inidan_Pines dataset
CTrain = [5 143 83 24 48 73 3 48 2 97 246 59 21 127 39 9];

%===== basic parameters ===============================
no_class = max(gth(:));

% data normalization
fprintf(' ... ... data normalization    ... ...\n');
Data = z./max(z(:));
[ylen, xlen, spec_dim] = size(Data);

% band selection
fprintf(' ... ... band selection        ... ...\n');
X = reshape(Data, ylen*xlen, spec_dim);
Psi = PCA_Train(X', bandNum);
X = X*Psi;
DataTmp = reshape(X, ylen, xlen, size(Psi,2));
clear X Psi;

mapping = getmapping(nr,'u2');
fprintf(' ... ... LBP feature extraction begin ... ...\n');
Feature_P = LBP_feature_global(DataTmp, r, nr, mapping, w, gth);
clear nr r z DataTmp;

lbp_dim = size(Feature_P, 3);
% spatial  data
DataSpat = NewScale(reshape(Feature_P, ylen*xlen, lbp_dim));
% spectral data
DataSpec = NewScale(reshape(Data, ylen*xlen, spec_dim));
% spatial and spectral data combination
DataSpec = DataSpec(:, 1:150);
Data_spec_spat = [DataSpat, DataSpec];
clear DataSpat DataSpec Data Feature_P bandNum;
clear lbp_dim mapping w;

Data = []; Labels = [];

for i = 1: no_class
    pos = find(gth==i);
    Data = [Data; Data_spec_spat(pos, :)];
    Labels = [Labels, length(pos)];
end
clear  Data_spec_spat;

DataTrn = []; DataTst = [];  CTest = [];
k = 0;
for i = 1: no_class
    Data_tmp = Data((k+1):(Labels(i)+k), :);
    k = Labels(i) + k;
    index_i = randperm(Labels(i));
    DataTrn = [DataTrn; Data_tmp(index_i(1:CTrain(i)), :)];
    index_i = find(gth==i);
    CTest(i) = length(index_i);
end

DataTst = Data;

clear k Data_tmp Data index_i;

TrnLab = []; TstLab = [];
for jj = 1: length(CTrain)
   TrnLab = [TrnLab; jj * ones(CTrain(jj),1)];
end
for jj = 1: length(CTest)
   TstLab = [TstLab; jj * ones(CTest(jj),1)];
end

% Scale the data
X=[DataTrn;DataTst];
[N,Dim] = size(X);
clear CTest CTrain gth DataTrn DataTst;

fprintf('... ... Graph number:%d ... ...\n', kg);

%kf=floor(log2(Dim)+1);
% gaofeng revised code 2017/04/08
kf = floor(Dim/4);

label_index = find(TrnLab~=0);
labels = [TrnLab;TstLab];
[G,F]  = MultiGraphs(X,labels,label_index,kg,kf);

[val, predict_res]=max(F,[],2);

[Pr, ConfMat] = GetAccuracy(predict_res(length(label_index)+1:end), ...
                            labels(length(label_index)+1:end));

fprintf(' ... ... Final Accuracy: %f\n', Pr.OA);
fprintf(' ... ... Final Kappa: %f\n', Pr.Kappa);


% This file is part of the project NILM-Eval (https://github.com/beckel/nilm-eval).
% Licence: GPL 2.0 (http://www.gnu.org/licenses/gpl-2.0.html)
% Copyright: ETH Zurich, 2014
% Author: Romano Cicchetti

function [events] = getEvents(total_consumption, threshold)

% extract events

% define parameters and data structures
eventsTotalStep = zeros(length(total_consumption), 1);
eventsMaxStep = zeros(length(total_consumption), 1);
eventsDuration = ones(length(total_consumption), 1);
eventsStartTime = ones(length(total_consumption), 1);
maxEventDuration = 20; % maximum duration of an event in seconds
minEventDuration = 5;
delta_p_prev = 0;      % previous difference between two consecutive power values
p_t_prev = 0;          % previous power value
index = 0;             % index of an event
startTime = 1;         % start time of an event

% for each time step
for i = 1:length(total_consumption)
    p_t = total_consumption(i, 1);
    delta_p = p_t - p_t_prev;

    % a power step between two consecutive power values is ignored if the
    % difference between the two power values is smaller than a threshold
    if (abs(delta_p) > threshold)
        signsEqual = sign(delta_p) == sign(delta_p_prev);
        if (( i - startTime > maxEventDuration || ~signsEqual) && (i - startTime > minEventDuration || startTime == 1))
            % create a new event and assign power step to it
            index = index + 1;
            startTime = i;
            eventsStartTime(index) = startTime;
            delta_p_prev = delta_p;
        end

        % update/initialized maximum power step of the existing/new event
        % if necessary
        if (abs(delta_p) > abs(eventsMaxStep(index)))
            eventsMaxStep(index) = delta_p;
        end
        % update/initialize duration and total power step of the existing/new
        % event
        eventsDuration(index) = i - startTime + 1;
        eventsTotalStep(index) = eventsTotalStep(index) + delta_p;
    end
    p_t_prev = p_t;
end

% cut the event data structures to only include the extracted events
eventsTotalStep = eventsTotalStep(1:index, 1);
eventsMaxStep = eventsMaxStep(1:index, 1);
eventsDuration = eventsDuration(1:index, 1);
eventsStartTime = eventsStartTime(1:index, 1);
events = [eventsTotalStep, eventsMaxStep, eventsDuration, eventsStartTime];
wrong_sign_idx = sign(events(:,1)) ~= sign(events(:,2));
events(wrong_sign_idx,2) = -1*events(wrong_sign_idx,2);
idx_over_10 = abs(eventsTotalStep(:,1)) > 5;
events = events(idx_over_10, :);
end


function roomhyp = sample_roomtype4(n, lines, vp, imgwidth, imgheight)

% 4 ways to create hypotheses
% roomtype2 + 3,top,right
% roomtype2 + 2,top
% roomtype7 + 3,bot,right
% roomtype7 + 2,bot

lc = [lines.lineclass]; % 0,1,2,3
lr = [lines.leftorright]; % 1 if right, -1 if left, 0 if neither
tb = [lines.above_horizon]; % 1 if above, -1 if below, 0 if neither

twotop = find((lc==2) & (tb==1));
twobot = find((lc==2) & (tb==-1));
threetopright = find((lc==3) & (tb==1) & (lr==1));
threebotright = find((lc==3) & (tb==-1) & (lr==1));

linecode{1} = twotop;
linecode{2} = twobot;
linecode{3} = threetopright;
linecode{4} = threebotright;

% recipe: [roomtypepart linecode oldcornerid newcornerid]
recipe(1,:) = [2 3 4 2];
recipe(2,:) = [2 1 4 2];
recipe(3,:) = [7 4 2 4];
recipe(4,:) = [7 2 2 4];

% roomhyp = [];
count = 0;
num_continue_without_progress = 0;
while count < n && num_continue_without_progress < 30
% pick recipe
    cur_recipe = recipe(randsample(size(recipe,1), 1), :);

    % sample partial room
    if cur_recipe(1) == 2
        roomhyppart = sample_roomtype2(1, lines, vp, imgwidth, imgheight);
    elseif cur_recipe(1) == 7
        roomhyppart = sample_roomtype7(1, lines, vp, imgwidth, imgheight);
    end

    if isempty(roomhyppart)
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end

    % sample lines
    l1 = linecode{cur_recipe(2)};
    if length(l1)<1
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end
    ls1 = l1(randsample(length(l1), 1));

    % new corner
    oldpoint = roomhyppart.corner(cur_recipe(3)).pt;
    [newpoint degen] = line_intersect(vp{1},oldpoint,...
        lines(ls1).point1, lines(ls1).point2);
    if degen==1
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end
    % check if inside img
    MARGIN = 5;
    if ~is_in_image(newpoint, imgwidth, imgheight, MARGIN)
        num_continue_without_progress = num_continue_without_progress + 1;
        continue;
    end

    % TODO: check location of new line sample
    %

    % add to hypothesis
    count = count + 1;
    num_continue_without_progress = 0;
    roomhyp(count) = roomhyppart;
    roomhyp(count).corner(cur_recipe(4)).pt = newpoint;
    roomhyp(count).type = 4;

%     global img;
%     disp_vanish(img, lines(ls1), vp);
%     plot(oldpoint(1), oldpoint(2), 'rx', 'MarkerSize',10, 'LineWidth',2);
%     plot(newpoint(1), newpoint(2), 'bx', 'MarkerSize',10, 'LineWidth',2);
%     pause;
%     close;
end

if ~exist('roomhyp','var')
    roomhyp = [];
end


function model= dbnFit(X, numhid, y, varargin)
%fit a DBN to bianry data in X

%INPUTS:
%X              ... data. should be binary, or in [0,1] interpreted as
%               ... probabilities
%numhid         ... list of numbers of hidden units
%y              ... List of discrete labels

%OUTPUTS:
%model          ... A cell array containing models from all RBM's

%varargin may contain options for the RBM's of this DBN, in row one by one
%for example:
%dbnFit(X, [500,400], opt1, opt2) uses opt1 for 500 and opt2 for 400
%dbnFit(X, [500,400], opt1) uses opt1 only for 500, and defaults for 400

numopts=length(varargin);
H=length(numhid);
model=cell(H,1);
cant_clases = length(unique(y));

if H>=2

    %train the first RBM on data
    if(numopts>=1)
        model{1}= rbmBB(X, numhid(1),varargin{1});
    else
        model{1}= rbmBB(X, numhid(1));
    end

%     %extra-debug
%     visualize(model{1}.W);
%     drawnow;

    %train all other RBM's on top of each other
    for ii=2:H
        if(numopts>=ii)
            model{ii}=rbmBB(model{ii-1}.top, numhid(ii), varargin{ii});
        else
            model{ii}=rbmBB(model{ii-1}.top, numhid(ii));
        end

%         %extra-debug
%         visualize(model{ii}.W);
%         drawnow;

    end

%     %the last RBM has access to labels too
%     if(numopts>=H)
%         model{H}= rbmFit(model{H-1}.top, numhid(end), y, varargin{H});
%     else
%         model{H}= rbmFit(model{H-1}.top, numhid(end), y);
%     end
    model{H}.Wc = 0.1*randn(cant_clases, size(model{H}.W,2) );
    model{H}.cc = 0.1*randn(1, cant_clases);
    model{H}.labels = 1:cant_clases;

    %fine-tuning of the DBN

    targets = zeros(length(y), cant_clases);
    for ii = 1:cant_clases
        targets(y == ii, ii) = 1;
    end

    for epoch = 1:200

        %%%%%%%%%%%%%%% PERFORM CONJUGATE GRADIENT WITH 3 LINESEARCHES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        max_iter=3;

        if epoch<6  % First update top-level weights holding other weights fixed.
%             N = size(data,1);
%             XX = [data ones(N,1)];
%             w1probs = 1./(1 + exp(-XX*w1)); w1probs = [w1probs  ones(N,1)];
%             w2probs = 1./(1 + exp(-w1probs*w2)); w2probs = [w2probs ones(N,1)];
%             w3probs = 1./(1 + exp(-w2probs*w3)); %w3probs = [w3probs  ones(N,1)];

            testdata = X;
            for ii=1:length(model)
                testdata= rbmVtoH(model{ii}, testdata);
            end

            VV = colvec([ model{H}.Wc'; model{H}.cc ]);
            Dim = [ numhid(end); cant_clases];
            XX = minimize(VV,'CG_CLASSIFY_INIT',max_iter,Dim,testdata,targets);
            aux = reshape(XX, numhid(end)+1, cant_clases);
            model{H}.Wc = aux(1:end-1,:)';
            model{H}.cc = aux(end,:);

        else

%             VV = [w1(:)' w2(:)' w3(:)' w_class(:)']';
%             Dim = [l1; l2; l3; l4; l5];
            VV = [];
            for ii=1:H
                VV = [VV; colvec([model{ii}.W; model{ii}.b ]) ];
            end
            VV = [VV; colvec([model{ii}.Wc'; model{ii}.cc ])];

            Dim = [ size(X,2); colvec(numhid); cant_clases];

            XX = minimize(VV,'CG_CLASSIFY',max_iter,Dim,X,targets);

            xxx = 0;
            for ii=1:length(model)
                aux = reshape(XX(xxx+1:xxx+(Dim(ii)+1)*Dim(ii+1)),Dim(ii)+1,Dim(ii+1));
                model{ii}.W = aux(1:end-1,:);
                model{ii}.b = aux(end,:);
                xxx = xxx+(Dim(ii)+1)*Dim(ii+1);
            end
            aux = reshape(XX(xxx+1:xxx+(Dim(ii+1)+1)*Dim(ii+2)),Dim(ii+1)+1,Dim(ii+2));
            model{ii}.Wc = aux(1:end-1,:)';
            model{ii}.cc = aux(end,:);

%             w1 = reshape(X(1:(l1+1)*l2),l1+1,l2);
%             xxx = (l1+1)*l2;
%             w2 = reshape(X(xxx+1:xxx+(l2+1)*l3),l2+1,l3);
%             xxx = xxx+(l2+1)*l3;
%             w3 = reshape(X(xxx+1:xxx+(l3+1)*l4),l3+1,l4);
%             xxx = xxx+(l3+1)*l4;
%             w_class = reshape(X(xxx+1:xxx+(l4+1)*l5),l4+1,l5);

        end
        %%%%%%%%%%%%%%% END OF CONJUGATE GRADIENT WITH 3 LINESEARCHES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        prediction = dbnPredict(model, X);
        errors = sum(prediction~=y);
        fprintf(1, 'Errores %d\n', errors);

    end

%     %extra-debug
%     visualize(model{H}.W);
%     drawnow;

else

    %numhid is only a single layer... but we should work anyway
    if (numopts>=1)
        model{1}= rbmFit(X, numhid(1), y, varargin{1});
    else
        model{1}= rbmFit(X, numhid(1), y);
    end
end


function [calscores,w0] = parallel_cal(w,scores,wfuse)
%

if nargin==0
    test_this();
    return;
end

if ~exist('scores','var') || isempty(scores)
    calscores = sprintf(['parallel calibration:',repmat(' %g',1,length(w))],w);
    return;
end

[m,n] = size(scores);

if nargout>1, w0 = init_w0(wfuse); end

calscores = linTrans(w,@(w)map_this(w),@(w)transmap_this(w));

    function w0 = init_w0(wfuse)
        assert(length(wfuse)-1==m);
        scal = wfuse(1:end-1);
        offs = wfuse(end);
        W = [scal*(m+1);((m+1)/m)*offs*ones(m,1)];
        w0 = W(:);
    end

    function y = map_this(w)
        w = reshape(w,m,2);
        y = bsxfun(@times,scores,w(:,1));
        y = bsxfun(@plus,y,w(:,2));
    end

    function w = transmap_this(y)
        y = reshape(y,m,n);
        w = [sum(y.*scores,2),sum(y,2)];
    end

end

function test_this()

    scores = randn(4,10);
    [sys,w0] = parallel_cal([],scores,(1:5)');
    test_MV2DF(sys,w0);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Max manipulability index ALONG A LINE.
% Start by maximum global manipulability at a pose
% Start by maximizing manipulability locally at each time step
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [qq, manips]=path_planning_local_max%(direction)
close all;
global robot

experiment_name = 'min_manip_local.mat';

height1 = 2; %m
height2 = 2.5; %m
x1 = -1;
x2 = 2;

%the orientation needed
phi = pi/2;
p0 = [x1 height1 0]';
pf = [x2 height2 0]';

T0 = build_T(p0, phi);
Tf = build_T(pf, phi);

%this defines the line to follow in direction
deltaV = pf - p0;
deltaV = deltaV/norm(deltaV);
%and total length of the movement
total_length = norm(pf - p0); %m
%the movement step in m
ds=0.1;
mov = 0.0:ds:total_length;

line0 = [p0 pf];
plot(line0(1,:),line0(2,:),'k')

%initial pose and manipulability
qq = [];

q0 = [pi/2 -pi -pi pi/4]';
q = q0;
Ti = T0;
for i=1:length(mov)
    fprintf('Move %d out of %d\n', i, length(mov))
    %update to next point in trajectory
    Ti(1:3,4) = p0 + mov(i)*deltaV;
    %(robot,  Tf, q0, restriction)
    q = inversekinematic_4dofplanar(robot, Ti, q);
    drawrobot3d(robot, q)
    q = max_manipulability_local(robot, q, -1);
    %drawrobot3d(robot, q)
    draw_axes(Ti, 'Xpiece', 'Ypiece', 'Zpiece', 1.2);
    plot3(line0(1,:),line0(2,:),line0(3,:),'k')
    qq = [qq q];
end

animate_local(robot, qq, line0)

manips = compute_manip(robot, qq);
figure,
plot(manips)
title('manipulability index at each movement')
figure,
plot(qq')
title('joint positions')
legend('q_1', 'q_2','q_3','q_4','q_5','q_6','q_7')

save(experiment_name)

% Simple iteration along the null space to increase manipulability
function [q] = max_manipulability_local(robot, q, sign_max)

delta_time = 0.01;
while 1
%    [qd_null] = compute_null_space(robot, q);
[qd_null] = null_space_4dof(robot, q);
    delta_manip = compute_delta_manip(robot, q, qd_null, 0.01);
    % add a sign for maximization/minimization
    % the constant is based on the rate of delta_manip also
    x = sign_max*sign(delta_manip)*qd_null;
    q = q + x*delta_time;
    if abs(delta_manip) < 1e-2
        break
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Computes a gradient so as to let the manipulability be improved
% q is the current joint position
% qd is the instantaneous speed that lies in the null space
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function delta_manip = compute_delta_manip(robot, q, qd, delta)
%compute first manipulability index
m0 = compute_manip(robot, q);
%move differentially along the null space.
q = q + delta*qd;
%compute second manipulability index
m1 = compute_manip(robot, q);
%delta_manip = trace(inv(J*J')*(Jd*J'+J*Jd'));
%return difference
delta_manip=(m1-m0)/delta;

%
% Computes manipulability of poses given in qs
% Caution: only taking into account vx, vy and wz
%
function manips = compute_manip(robot, qs)
manips = [];
for i=1:size(qs,2)
    %compute current manip
    J = manipulator_jacobian(robot, qs(:,i));
    J = [J(1:2,:); J(6,:)];
    manip = sqrt(det(J*J'));
    manips = [manips manip];
end

function T = build_T(p, phi)
T = [cos(phi) -sin(phi) 0 p(1);
     sin(phi) cos(phi) 0 p(2);
     0            0     1  p(3);
     0             0    0   1];

function animate_local(robot, qq, line_work)
for i=1:1:size(qq,2)
     drawrobot3d(robot, qq(:,i))
     plot(line_work(1,:),line_work(2,:))
end


function [P, X] = Get_Parameters()

%This function is used to define all parameters that a user can set. It
%returns a struct P with parameters, grouped into subfields, and a struct X
%with the initial guess at the state that is to be passed to fmincon.

%% Dynamics:
P.dyn.gravity = 9.81;  % (m/s^2)   Acceleration of gravity
P.dyn.length = 0.25;  % (m)   Length of Handle
P.dyn.mass = 0.2;   % (kg)  Hammer Point Mass
P.dyn.coeffRestitution = 0.2;  % () Coefficient of restitution

%% Optimization Algorithm:
% Type: optimset('fmincon') at command line to see all available options
P.opt = optimset(...
    'TolFun', 1e-8,...
    'TolX', 1e-12,...
    'TolCon', 1e-8,...
    'MaxIter',500,...
    'Display', 'iter-detailed',... % [ off | iter | iter-detailed | notify | notify-detailed | final | final-detailed ]
    'Algorithm', 'sqp',... % [ active-set | interior-point | interior-point-convex | levenberg-marquardt | sqp | trust-region-dogleg | trust-region-reflective ]
    'MaxFunEvals', 1e6,...
    'UseParallel', 'never'); % [ always | {never} ]

%% Discritization:
% This is the number of gridpoints along with trajectory
P.nGridPts = 20;

%% Cost Function
P.cost.Method = 0;
  % 0 = Time Integral of Torque^2
  % 1 = Time Integral of Torque*Rate

  % IF P.cost.Method == 1
        P.cost.Count_Negative_Work = 0.2;
            %Negative work is when the system is backdriving the motor.
             % 1 => negative work equal to positive work,
             % 0 => ignore negative work,
             %-1 => full regeneration
        P.cost.Motor_Cost_Smoothing = 1;
            %It turns out that smoothing is very important for convergence.
            %Here I use a modified form hyperbolic tangent smoothing:
            % 0    ==>  Sharp
            % inf  ==>  Smooth
            % 0.1  -->  feasible, non-optimal, hit iteration limit
            % 0.5  -->  close, but with some numerical artifacts
            % 1.0  -->  finds correct solution after 121 iterations
            % 5.0  -->  heavy smoothing - correct solution after 158
            %           iterations. Note that the cost function increases
            %           nearly linearly. This is because the optimization
            %           has found the minimum value of the smoothed cost
            %           function, which is nearly flat at this point.
  % END

%% Initial Guess:
%Initialize the trajectory by guessing a few rough points.
  P.init.durationGuess = 1;
  P.init.angleGuess = (pi/180)*[90, 0, 90];
  P.init.rateGuess = 2*pi*linspace(-1, 2, length(P.init.angleGuess));

  %P.init.torqueGuess = linspace(-1, 1, length(P.init.angleGuess));    %Start with values in toruq
  P.init.torqueGuess = zeros(size(P.init.angleGuess));    %start with empty torque

%% Bounds:
%States
    P.bnd.angle = (pi/180)*[-25,91];   %90 = Table, 0 = Vertical
    P.bnd.rate = 2*pi*[-2,2];
%Motor
    P.bnd.torque = [-1,1];
%Duration
    P.bnd.duration = [0.8,1.2];

%% Display:
% As this optimization runs, you can choose to have it display it's
% progress towards the solution. There are also options to automatically
% save the figures and generate the LaTeX code to put the figures into a
% document.

%Size the text on the figures nicely.
    P.disp.TitleFontSize = 16;
    P.disp.LabelFontSize = 16;
    P.disp.AxisFontSize = 12;
    P.disp.LineWidth = 3;
    P.disp.DefectLineWidth = 4;

%Decide if the plots should be shown as the code runs:
    P.disp.intermediatePlot = true;   %Use to toggle the plotting while running
    %IF P.disp.intermediatePlot, THEN
        P.disp.showIterationNum = ...%Creates intermediate plots at these iterations:
            [0,1,2,3,5,8,13,21,34,55,89,144,233,377,610,987];
        P.disp.saveIntermediatePlots = false;   %Saves each auto-generated plot
        P.disp.createTex = true;   %Creates the skeleton of the LaTeX code to call figures
            P.disp.TexFileName = 'TexFigureCode.txt';
    %END

%% Constraints:
    P.cst.strikeAngle = pi/2;  %Hammer strikes exactly at this angle
    P.cst.strikeRate = max(P.bnd.rate);  %Reaches maximum speed at impact

%% Automatic:
% This section of code consists of things that are automatically done by
% the code, and are not parameters to be adjusted by the user.

% Later on in the code, it is necessary to pass the 'state' (all of the
% decision variables in the optimization problem) as a vector. This format
% is difficuly to work with, so there is a function to convert between a
% vector and a struct. Part of the conversion requires knowledge of the
% structure of the state struct. Here we create the initial guess at the
% state, and then use this as a template to get the information about the
% structure of this struct and store is in P.structDat.
    X = Initial_State(P);
    [~,P.structDat] = Convert_State(X);

% PhysicsIntegration uses persistent variables to keep track of some
% things. Before running the optimization, we need to reset these
% variables. One way of doing this is to clear the function from memory.
% Another way (not used here) is to pass a flag to the function (such as
% calling it with no arguments) and it can then reset the persistent
% variables itself.
    clear PhysicsIntegration;   %Reset all of the persistent variables

% Matlab takes a bit of time to generate a function handle with extra
% arguments (in this case P), so we do it here to save some time. NOTE -
% this means that any future changes to P will not be passed to the
% dynamics function. This is fine here, but could produce problems if the
% code was written differently.
    P.dyn.UserFunc = @(Z,u)HammerDynamics([],Z,u,P);

% This next block of code ensures that the LaTeX code fragmant starts with
% an empty file.
    if P.disp.createTex
        if exist(P.disp.TexFileName,'file')
            fid = fopen(P.disp.TexFileName,'w');
            fclose(fid);
        end
    end

end


function grad = B_tmaxpool(prev_layer, curr_layer, future_layers)
input = prev_layer{1}.a;
[D,T,N] = size(input);
max_idx = curr_layer.idx;

if isfield(curr_layer, 'context')
    context = curr_layer.context;
else
    context = 0;
end
if isfield(curr_layer, 'stride')
    stride = curr_layer.stride;
else
    stride = 0;
end

future_grad = GetFutureGrad(future_layers, curr_layer);

if strcmpi(class(future_grad), 'gpuArray')
    grad = gpuArray.zeros(D,T,N);
else
    grad = zeros(D,T,N);
end

if context==0 || stride==0  % global pooling
    if 0
        for n=1:N
            for d = 1:D
                grad(d,max_idx(d,1,n),n) = future_grad(d,1,n);
            end
        end
    elseif 0
        for n = 1:N
            % for new Matlab version, we can use sub2idx.
            % idx2 = sub2idx(size(grad(:,:,1)), 1:D, max_idx(:,1,n));
            offset = (max_idx(:,1,n)-1)*D + D*T*(n-1);
            idx = offset+ [1:D]';
            grad(idx) = future_grad(:,1,n);
        end
    else
        offset = bsxfun(@plus, (squeeze(max_idx)-1)*D, D*T*(0:(N-1)) );
        offset = reshape(offset, numel(future_grad),1);
        idx = offset+ repmat((1:D)', N,1);
        grad(idx) = future_grad;
    end
else
    if 0
        for i=1:size(max_idx,2)
            offset = (i-1)*stride;
            for n=1:N
                for d = 1:D
                    idx = max_idx(d,i,n) + offset;
                    grad(d,idx,n) = future_grad(d,i,n);
                end
            end
        end
    else
        T2 = size(max_idx,2);
        max_idx = reshape(permute(max_idx, [1 3 2]), D*N,T2);
        future_grad = reshape(permute(future_grad, [1 3 2]), D*N,T2);
        grad = reshape(permute(grad, [1 3 2]), D*N,T);

        if 0
            for i=1:T2
                offset = (i-1)*stride;
                offset2 = (offset+max_idx(:,i)-1)*D*N;
                idx = offset2 + [1:(D*N)]';
                grad(idx) = future_grad(:,i);
            end
        else
            offset = (0:(T2-1))*stride;
            offset2 = bsxfun(@plus, (max_idx-1)*D*N, offset*D*N);
            offset2 = reshape(offset2, numel(offset2),1);
            idx = double(offset2) + repmat([1:(D*N)]', T2,1);   % have to use double precision as sometimes the index can be very big

            if stride<context
                % some idx may be redundant due to the fact that the same
                % element may go to multiple pooled elements.
                idx = reshape(idx,D*N,T2);
                for t=1:T2
                    grad(idx(:,t)) = grad(idx(:,t)) + future_grad(:,t);
                end
            else
                grad(idx) = future_grad;
            end
        end
        grad = permute(reshape(grad, D,N,T), [1 3 2]);
    end
end

end


function L = noiseExpectationLogLikelihood(noise, mu, varsigma, y);

% NOISEEXPECTATIONLOGLIKELIHOOD Return the expectation of the log likelihood.
% FORMAT
% DESC returns the expectation of the log likelihood for a gven noise model.
% ARG noise : the noise structure for which the expectation of the log
% likelihood is required.
% ARG mu : input mean locations for the likelihood.
% ARG varSigma : input variance locations for the likelihood.
% ARG y : target locations for the likelihood.
%
% SEEALSO : noiseParamInit, noiseLogLikelihood
%
% COPYRIGHT : Neil D. Lawrence, 2007

% NOISE

fhandle = str2func([noise.type 'NoiseExpectationLogLikelihood']);
L = fhandle(noise, mu, varsigma, y);


classdef print

properties
end

methods (Static)

    function [ ] = sparse_signal( x )
    %sparse_signal Prints a sparse vector as pairs of indices and values
    N = length(x);
    K = 1;
    for i=1:N
        if x(i)
            fprintf('(%d,%0.4f) ',i, x(i));
            if mod(K, 5) == 0
                % We introduce a new line after every few values
                fprintf('\n');
            end
            K = K+1;
        end
    end
    fprintf('  N=%d, K=%d\n', N, K-1);
    end

    function [ ] = sorted_sparse_signal( x )
        %PRINTSORTEDSPARSEVECTOR Sorts non-zero values in x and prints them.
        % We identify non-zero values of x. We sort them. We print them in
        % the descending magnitude order along with their indices
        values = spx.commons.sparse.sorted_non_zero_elements(x);
        fprintf('Index:\tValue\n');
        fprintf('%4d:\t%f\n', values);
    end

    function vector(x, precision)
        % prints a vector
        if nargin < 2
            precision = 2;
        end
        n = numel(x);
        if (precision == 'e')
            format = '%.4e ';
        else
            format = sprintf('%%.%df ', precision);
        end
        for i=1:n
            fprintf(format, x(i));
            if mod(i, 20) == 0
                fprintf('\n');
            end
        end
        if mod(i, 40) ~= 0
            fprintf('\n');
        end
    end

end
end


% Is In Range function written by NKN(C)-2006
% is c E [a,b]
% True=1;
% False=0;
%
% Example: isinrange(1,2,3)
% ans=0;
function g=isinrange(a,b,c)
bb=max(a,b);
aa=min(a,b);
if (c<=bb) & (c>=aa)
%% disp('true');
g=1;
else
%% disp('false');
g=0;
end


function out=eps(x)
%eps: Floating point relative accuracy (as defined in matlab R14).

one=mp(1);%automatically inherit default precision
two=one+one;
if isempty(x)
    out=one*power(two,-one.precision*one);%automatically inherit data type
else
    out=abs(x)*power(two,-x(1).precision*one);
end


f = @(x) objfun(x);
A = [-2 1;-1 -1;1 0;0 1];
b = [-1;-2;0;0];
E = [];
e = [];

[xstar,ystar] = zoutendijk(f,A,b,E,e)


function dataout=modeDRAEC_BSS(nummics, numrefs, datain)
%
% Perform dr and aec together, then bss.
% nummics:              no. of mic channels
% numrefs:              no. of reference channels
% datain:               input data
% dataout:              output data
%

%% perform stft
addpath('stft2');
config;

M=nummics;
R=numrefs;
N=M;

Xtf=cell(M+R, 1);
for m=1:M+R
    Xtf{m}=stft(datain(:, m), stftshift, fftsize, false);
end
[K, T]=size(Xtf{1});

Ytf=cell(M, 1);
for m=1:M
    Ytf{m}=zeros(K, T);
end

%% params go to config

%% space for dr and aec
% current mic data backup
Micbuffer=zeros(K, M*(DR_DELAY+1));
% ref and delayed mic data
draecfsize=R*AEC_FLEN+M*DR_FLEN;
Refmicdelay=zeros(K, draecfsize);

% mic-ref correlation
Cxr=cell(K, 1);
for k=1:K
    Cxr{k}=zeros(M, draecfsize);
end

% reference auto correlation
Crr=cell(K, 1);
for k=1:K
    Crr{k}=zeros(draecfsize, draecfsize);
end

% reverb and echo path
REPath=cell(K, 1);
for k=1:K
    REPath{k}=zeros(M, draecfsize);
end

%% space for bss
% the weighted correlation matrices
C1=cell(K, 1);
C2=cell(K, 1);
for k=1:K
    C1{k}=STABLE_EPS*eye(M, M);
    C2{k}=STABLE_EPS*eye(M, M);
end

% demixing matrices
Demix=cell(K, 1);
for k=1:K
    Demix{k}=eye(N, M);
end

%% perform iteration
for t=1:T
    %% perform dr and aec together
    % direct nearend and early reverberation
    Early=zeros(K, M);

    %
    % shift in new data
    %
    Micbuffer=circshift(Micbuffer, M, 2);
    for m=1:M
        Micbuffer(:, m)=Xtf{m}(:, t);
    end

    % shift in reference data
    Refmicdelay(:, 1:R*AEC_FLEN)=circshift(Refmicdelay(:, 1:R*AEC_FLEN), R, 2);
    for r=1:R
        Refmicdelay(:, r)=Xtf{M+r}(:, t);
    end

    % delayed mic data
    Refmicdelay(:, R*AEC_FLEN+1:end)=circshift(Refmicdelay(:, R*AEC_FLEN+1:end), M, 2);
    Refmicdelay(:, R*AEC_FLEN+1:R*AEC_FLEN+M)=Micbuffer(:, end-M+1:end);

    for k=1:K
        mic=permute(Micbuffer(k, 1:M), [2,1]);
        ref=permute(Refmicdelay(k, :), [2,1]);

        % calculate late reverberation and echo
        late=REPath{k}*ref;

        % direct nearend and early reverberation
        early=mic-late;
        % output data
        Early(k, :)=early;

        %
        % calculate nonlinearity
        %
        xsq=abs(mic).^2;
        ysq=abs(early).^2;

        phi=sum(ysq(ysq<xsq)) + sum(xsq(ysq>=xsq));
        phi=(1-DRAEC_FORGET)*(phi+VAR_BIAS)^((GAMMA-2)/2);

        % update mic ref correlation
        Cxr{k}=DRAEC_FORGET*Cxr{k}+phi*(mic*ref');

        % update ref auto-correlation
        Crr{k}=DRAEC_FORGET*Crr{k}+phi*(ref*ref');

        % update echo and reverb path
        REPath{k}=Cxr{k}/(Crr{k}+DRAEC_DIAGLOAD*eye(draecfsize, draecfsize));
    end

    %% perform bss
    Bssout=zeros(K, M);

    %
    % calculate nonlinearity
    %
    phi1=0;
    phi2=0;

    for k=1:K
        x=Early(k, :).';
        y=Demix{k}*x;
        % output data
        Bssout(k, :)=y.';

        phi1=phi1+abs(y(1))^2;
        phi2=phi2+abs(y(2))^2;
    end

    phi1=(1-BF_FORGET)*(phi1+VAR_BIAS)^((GAMMA-2)/2);
    phi2=(1-BF_FORGET)*(phi2+VAR_BIAS)^((GAMMA-2)/2);

    % update the demixing matrices
    for k=1:K
        %
        % accumulate the weighted correlation
        %
        x=Early(k, :).';
        C1{k}=BF_FORGET*C1{k}+phi1*(x*x');
        C2{k}=BF_FORGET*C2{k}+phi2*(x*x');

        % solve gev problem
        D=heig2(BF_DIAGLOAD, C2{k}, C1{k});
        Demix{k}=D;
    end

    for m=1:M
        Ytf{m}(:, t)=Bssout(:, m);
    end
end

%% perform istft and output signal
dataout=zeros(dataLength(T, stftshift, fftsize ), N);
for n=1:N
    dataout(:, n)=istft(Ytf{n}, stftshift, false);
end

end


function out = spm_dartel_invnorm(job)
% Warp template to match individuals
% FORMAT spm_dartel_invnorm(job)
% job.flowfields - Filenames of flowfields
% job.images     - Filenames of images to warp
% job.interp     - Interpolation method
% job.K          - 2^K timesteps are used
%
% This function may be useful fo warping labels on to images.
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_dartel_invnorm.m 5668 2013-10-03 18:34:18Z guillaume $

PU    = job.flowfields;
PI    = job.images;
intrp = job.interp;
K     = job.K;

for i=1:numel(PU),
    [pth1,nam1,ext1,num1] = spm_fileparts(PU{i});
    NU = nifti(fullfile(pth1,[nam1,ext1]));
    fprintf('%s: ',nam1);
    y  = spm_dartel_integrate(NU.dat,[0 1], K);
    y1 = double(y(:,:,:,1));
    y2 = double(y(:,:,:,2));
    y3 = double(y(:,:,:,3));
    clear y

    for m=1:numel(PI),
        [pth2,nam2,ext2,num2] = spm_fileparts(PI{m});
        NI = nifti(fullfile(pth2,[nam2 ext2]));

        NO = NI;
        NO.dat.fname = fullfile(pth1,['w' nam2 '_' nam1 ext2]);
        NO.dat.dim = [NU.dat.dim(1:3) NI.dat.dim(4:end)];
        NO.mat  = NU.mat0;
        NO.mat0 = NU.mat0;
        NO.mat_intent  = NU.mat0_intent;
        NO.mat0_intent = NU.mat0_intent;
        NO.descrip = 'Warped';
        create(NO);
        fprintf('%s',nam2); drawnow;

        for j=1:size(NI.dat,4),
            mat = NI.mat;
            if ~isempty(NI.extras) && isstruct(NI.extras) && isfield(NI.extras,'mat'),
                mat1 = NI.extras.mat;
                if size(mat1,3) >= j && sum(sum(mat1(:,:,j).^2)) ~=0,
                    mat = mat1;
                end;
            end;

            M   = mat\NU.mat;
            ty1 = M(1,1)*y1 + M(1,2)*y2 + M(1,3)*y3 + M(1,4);
            ty2 = M(2,1)*y1 + M(2,2)*y2 + M(2,3)*y3 + M(2,4);
            ty3 = M(3,1)*y1 + M(3,2)*y2 + M(3,3)*y3 + M(3,4);
            for k=1:size(NI.dat,5),
                for l=1:size(NI.dat,6),
                    f             = NI.dat(:,:,:,j,k,l);
                    spl_param     = [intrp,intrp,intrp,0,0,0];
                    if intrp>1, f = spm_bsplinc(f,spl_param); end;
                    f             = spm_bsplins(f,ty1,ty2,ty3,spl_param);
                    NO.dat(:,:,:,j,k,l) = f;
                    fprintf('\t%d,%d,%d', j,k,l); drawnow;
                    clear f
                end;
            end;
            clear ty1 ty2 ty3
        end;
        fprintf('\n'); drawnow;
    end;
end;

PU    = job.flowfields;
PI    = job.images;
out.files = cell(numel(PU),numel(PI));
for i=1:numel(PU),
    [pth1,nam1,ext1,num1] = spm_fileparts(PU{i});
    for m=1:numel(PI),
        [pth2,nam2,ext2,num2] = spm_fileparts(PI{m});
        fname = fullfile(pth1,['w' nam2 '_' nam1 ext2]);
        out.files{i,j} = fname;
    end
end


% Compares joint space vs. task space trajectories
% Copyright 2019 The MathWorks, Inc.

%% Setup
clc
createWaypointData;
figure, hold on
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ko:','LineWidth',2);
title('Trajectory Waypoints');
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
grid on
view([45 45]);
% Define IK solver
ik = inverseKinematics('RigidBodyTree',gen3);
ikWeights = [1 1 1 1 1 1];
% Use a small sample time for this example, so the difference between joint
% and task space is clear due to evaluation of IK in task space trajectory.
ts = 0.02;
trajTimes = 0:ts:waypointTimes(end);
% Initialize matrices for plots
qTask = zeros(numJoints,numel(trajTimes)); % Derived joint angles in task space trajectory
posJoint = zeros(3,numel(trajTimes)); % Derived end effector positions in joint space trajectory

%% Create and evaluate a task space trajectory
ikInitGuess = jointAnglesHome';
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;

disp('Running task space trajectory generation and evaluation...')
tic;

% Trajectory generation
[posTask,velTask,accelTask] = trapveltraj(waypoints,numel(trajTimes), ...
    'AccelTime',repmat(waypointAccelTimes,[3 1]), ...
    'EndTime',repmat(diff(waypointTimes),[3 1]));

% Trajectory evaluation
for idx = 1:numel(trajTimes)
    % Solve IK
    tgtPose = trvec2tform(posTask(:,idx)');
    [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
    ikInitGuess = config;
    qTask(:,idx) = config;
end

taskTime = toc;
disp(['Task space trajectory time : ' num2str(taskTime) ' s']);

%% Create and evaluate a joint space trajectory
ikInitGuess = jointAnglesHome';
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;

disp('Running joint space trajectory generation and evaluation...')
tic;

% Solve IK for all waypoints
numWaypoints = size(waypoints,2);
numJoints = numel(gen3.homeConfiguration);
jointWaypoints = zeros(numJoints,numWaypoints);
for idx = 1:numWaypoints
    tgtPose = trvec2tform(waypoints(:,idx)');
    [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
    cfgDiff = config - ikInitGuess;
    jointWaypoints(:,idx) = config';
end

% Trajectory Generation
[qJoint,qdJoint,qddJoint] = trapveltraj(jointWaypoints,numel(trajTimes), ...
    'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ...
    'EndTime',repmat(diff(waypointTimes),[numJoints 1]));

% Trajectory evaluation (only needed to find end effector position)
for idx = 1:numel(trajTimes)
    eeTform = getTransform(gen3,qJoint(:,idx)',eeName);
    posJoint(:,idx) = tform2trvec(eeTform)';
end
jointTime = toc;
disp(['Joint space trajectory time : ' num2str(jointTime) ' s']);

%% Create comparison plots
% Compare trajectories in Cartesian space
close all
figure, hold on
plot3(posTask(1,:),posTask(2,:),posTask(3,:),'b-');
plot3(posJoint(1,:),posJoint(2,:),posJoint(3,:),'r--');
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ko','LineWidth',2);
title('Trajectory Comparison');
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
legend('Task Space Trajectory','Joint Space Trajectory','Waypoints');
grid on
view([45 45]);

% Compare joint angles
% Plot each joint trajectory
for idx = 1:numJoints
    figure, hold on;
    plot(trajTimes,qTask(idx,:),'b-');
    plot(trajTimes,qJoint(idx,:),'r-');
    for wIdx = 1:numWaypoints
       xline(waypointTimes(wIdx),'k--');
    end
    title(['Joint ' num2str(idx) ' Trajectory']);
    xlabel('Time [s]');
    ylabel('Joint Angle [rad]');
    legend('Task Space Trajectory','Joint Space Trajectory');
end


% Aligns the point cloud given a rotation matrix.
%
% Args:
%   points3d - Nx3 point cloud.
%   R - 3x3 rotation matrix.
%
% Returns:
%   points3d - Nx3 aligned point cloud.
%
% Author: Nathan Silberman (silberman@cs.nyu.edu)
function points3d = get_aligned_point_cloud(points3d, R)
  points3d = (R * points3d')';
end


function P = tapas_uniqc_spm_imatrix(M)
% Return the parameters for creating an affine transformation matrix
% copy of spm_imatrix from Version 6906 (SPM12) 20-Oct-16
% FORMAT P = spm_imatrix(M)
% M   - Affine transformation matrix
% P   - Parameters (see spm_matrix for definitions)
%__________________________________________________________________________
%
% See also: spm_matrix.m
%__________________________________________________________________________
% Copyright (C) 1996-2011 Wellcome Trust Centre for Neuroimaging

% John Ashburner & Stefan Kiebel

%-Translations and Zooms
%--------------------------------------------------------------------------
R         = M(1:3,1:3);
C         = chol(R'*R);
P         = [M(1:3,4)' 0 0 0  diag(C)'  0 0 0];
if det(R)<0, P(7)=-P(7); end % Fix for -ve determinants

%-Shears
%--------------------------------------------------------------------------
C         = diag(diag(C))\C;
P(10:12)  = C([4 7 8]);
R0        = spm_matrix([0 0 0  0 0 0 P(7:12)]);
R0        = R0(1:3,1:3);
R1        = R/R0;

%-This just leaves rotations in matrix R1
%--------------------------------------------------------------------------
%[          c5*c6,           c5*s6, s5]
%[-s4*s5*c6-c4*s6, -s4*s5*s6+c4*c6, s4*c5]
%[-c4*s5*c6+s4*s6, -c4*s5*s6-s4*c6, c4*c5]

% There may be slight rounding errors making x>1 or x<-1.
rang      = @(x) min(max(x, -1), 1);

P(5)      = asin(rang(R1(1,3)));
if (abs(P(5))-pi/2)^2 < 1e-9
    P(4)  = 0;
    P(6)  = atan2(-rang(R1(2,1)), rang(-R1(3,1)/R1(1,3)));
else
    c     = cos(P(5));
    P(4)  = atan2(rang(R1(2,3)/c), rang(R1(3,3)/c));
    P(6)  = atan2(rang(R1(1,2)/c), rang(R1(1,1)/c));
end


% Version 1.000
%
% Code provided by Ruslan Salakhutdinov
%
% Permission is granted for anyone to copy, use, modify, or distribute this
% program and accompanying programs and documents for any purpose, provided
% this copyright notice is retained and prominently displayed, along with
% a note saying that the original programs are available from our
% web page.
% The programs and documents are distributed without any warranty, express or
% implied.  As the programs were written for research purposes only, they have
% not been tested to the degree that would be advisable in any important
% application.  All use of these programs is entirely at the user's own risk.

%%%%% Initialize biases of the base-rate model by ML %%%%%%%%%%%%%%%%%%%%%
 [numcases numdims numbatches]=size(batchdata);
 count_int = zeros(numdims,1);
 for batch=1:numbatches
    xx = sum(batchdata(:,:,batch));
    count_int = count_int + xx';
 end

 lp=5;
 p_int = (count_int+lp*numbatches)/(numcases*numbatches+lp*numbatches);
 log_base_rate = log( p_int) - log(1-p_int);


function res = vl_ffdnet_concise(net, x)

global sigmas;
n = numel(net.layers);
res = struct('x', cell(1,n+1));
res(1).x = x ;
cudnn = {'CuDNN'} ;
%cudnn = {'NoCuDNN'} ;

for i=1:n
    l = net.layers{i} ;
    switch l.type
        case 'conv'
            res(i+1).x = vl_nnconv(res(i).x, l.weights{1}, l.weights{2}, ...
                'pad', l.pad, ...
                'stride', l.stride, ...
                'dilate', l.dilate, ...
                l.opts{:}, ...
                cudnn{:}) ;

        case 'concat'
            if size(sigmas,1)~=size(res(i).x,1)
                sigmaMap   = bsxfun(@times,ones(size(res(i).x,1),size(res(i).x,2),1,size(res(i).x,4),'single'),permute(sigmas,[3 4 1 2]));
                res(i+1).x = cat(3,res(i).x,sigmaMap);
            else
                res(i+1).x = cat(3,res(i).x,sigmaMap);
            end

        case 'SubP'
            res(i+1).x = vl_nnSubP(res(i).x, [],'scale',l.scale);

        case 'relu'
            res(i+1).x = max(res(i).x,0) ;
    end
        res(i).x = [] ;
end


function [ a, ipvt, rcond ] = csico ( a, lda, n )

%*****************************************************************************80
%
%% CSICO factors a complex symmetric matrix.
%
%  Discussion:
%
%    The factorization is done by symmetric pivoting.
%
%    The routine also estimates the condition of the matrix.
%
%    If RCOND is not needed, CSIFA is slightly faster.
%
%    To solve A*X = B, follow CSICO by CSISL.
%
%    To compute inverse(A)*C, follow CSICO by CSISL.
%
%    To compute inverse(A), follow CSICO by CSIDI.
%
%    To compute determinant(A), follow CSICO by CSIDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex A(LDA,N), the symmetric matrix to be factored.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, complex A(LDA,N); a block diagonal matrix and the multipliers which
%    were used to obtain it.  The factorization can be written A = U*D*U'
%    where U is a product of permutation and unit upper triangular matrices,
%    U' is the transpose of U, and D is block diagonal with 1 by 1 and
%    2 by 2 blocks.  Only the diagonal and upper triangle are used.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, real RCOND, an estimate of the reciprocal condition of
%    the matrix.  For the system A*X = B, relative perturbations in A and B
%    of size EPSILON may cause relative perturbations in X of size
%    (EPSILON/RCOND).  If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%  Local Parameters:
%
%    Local, complex Z(N), a work vector whose contents are usually
%    unimportant.  If A is close to a singular matrix, then Z is an
%    approximate null vector in the sense that
%      norm(A*Z) = RCOND * norm(A) * norm(Z).
%

%
%  Find norm of A using only upper half.
%
  for j = 1 : n
    z(j) = scasum ( j, a(1:j,j), 1 );
    for i = 1 : j-1
      z(i) = real ( z(i) ) + cabs1 ( a(i,j) );
    end
  end

  anorm = 0.0;
  for j = 1 : n
    anorm = max ( anorm, real ( z(j) ) );
  end
%
%  Factor.
%
  [ a, ipvt, info ] = csifa ( a, lda, n );
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A*Z = Y and A*Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where U*D*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve U*D*W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  k = n;

  while ( 0 < k )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    kp = abs ( ipvt(k) );
    kps = k + 1 - ks;

    if ( kp ~= kps )
      t      = z(kps);
      z(kps) = z(kp);
      z(kp)  = t;
    end

    if ( cabs1 ( z(k) ) ~= 0.0 )
      ek = csign1 ( ek, z(k) );
    end

    z(k) = z(k) + ek;
    z(1:k-ks) = z(1:k-ks) + z(k) * transpose ( a(1:k-ks,k) );

    if ( ks ~= 1 )

      if ( cabs1 ( z(k-1) ) ~= 0.0 )
        ek = csign1 ( ek, z(k-1) );
      end

      z(k-1) = z(k-1) + ek;
      z(1:k-ks) = z(1:k-ks) + z(k-1) * a(1:k-ks,k-1);

    end

    if ( ks ~= 2 )

      if ( cabs1 ( a(k,k) ) < cabs1 ( z(k) ) )
        s = cabs1 ( a(k,k) ) / cabs1 ( z(k) );
        z(1:n) = z(1:n) * s;
        ek = s * ek;
      end

      if ( cabs1 ( a(k,k) ) ~= 0.0 )
        z(k) = z(k) / a(k,k);
      else
        z(k) = 1.0;
      end

    else

      ak = a(k,k) / a(k-1,k);
      akm1 = a(k-1,k-1) / a(k-1,k);
      bk = z(k) / a(k-1,k);
      bkm1 = z(k-1) / a(k-1,k);
      denom = ak * akm1 - 1.0;
      z(k) = ( akm1 * bk - bkm1 ) / denom;
      z(k-1) = ( ak * bkm1 - bk ) / denom;

    end

    k = k - ks;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
%
%  Solve U' * Y = W.
%
  k = 1;

  while ( k <= n )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= 1 )

      z(k) = z(k) + z(1:k-1) * transpose ( a(1:k-1,k) );

      if ( ks == 2 )
        z(k+1) = z(k+1) + z(1:k-1) * transpose ( a(1:k-1,k+1) );
      end

      kp = abs ( ipvt(k) );

      if ( kp ~= k )
        t     = z(k);
        z(k)  = z(kp);
        z(kp) = t;
      end

    end

    k = k + ks;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = 1.0;
%
%  Solve U*D*V = Y.
%
  k = n;

  while ( 0 < k )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= ks )

      kp = abs ( ipvt(k) );
      kps = k + 1 - ks;

      if ( kp ~= kps )
        t      = z(kps);
        z(kps) = z(kp);
        z(kp)  = t;
      end

      z(1:k-ks) = z(1:k-ks) + z(k) * a(1:k-ks,k);

      if ( ks == 2 )
        z(1:k-ks) = z(1:k-ks) + z(k-1) * transpose ( a(1:k-ks,k-1) );
      end

    end

    if ( ks ~= 2 )

      if ( cabs1 ( a(k,k) ) < cabs1 ( z(k) ) )
        s = cabs1 ( a(k,k) ) / cabs1 ( z(k) );
        z(1:n) = z(1:n) * s;
        ynorm = s * ynorm;
      end

      if ( cabs1 ( a(k,k) ) ~= 0.0 )
        z(k) = z(k) / a(k,k);
      else
        z(k) = 1.0;
      end

    else

      ak = a(k,k) / a(k-1,k);
      akm1 = a(k-1,k-1) / a(k-1,k);
      bk = z(k) / a(k-1,k);
      bkm1 = z(k-1) / a(k-1,k);
      denom = ak * akm1 - 1.0;
      z(k) = ( akm1 * bk - bkm1 ) / denom;
      z(k-1) = ( ak * bkm1 - bk ) / denom;

    end

    k = k - ks;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;
%
%  Solve U' * Z = V.
%
  k = 1;

  while ( k <= n )

    if ( ipvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= 1 )

      z(k) = z(k) + z(1:k-1) * transpose ( a(1:k-1,k) );

      if ( ks == 2 )
        z(k+1) = z(k+1) + z(1:k-1) * transpose ( a(1:k-1,k+1) );
      end

      kp = abs ( ipvt(k) );

      if ( kp ~= k )
        t     = z(k);
        z(k)  = z(kp);
        z(kp) = t;
      end

    end

    k = k + ks;

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

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

  return
end


function out=downsample(inp, nsam, ave)
[m,n]=size(inp);
dim=2;
if (m>n)
	dim=1;
end

mx_a=cumsum(double(inp), dim);
if (dim==1)
    out=diff(mx_a(1:nsam:end,:),1,dim)/ave;
else
    out=diff(mx_a(:,1:nsam:end),1,dim)/ave;
end


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcSin(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);

MergeM=get(handles.Matrix_name_edit,'String');
if isfloat(handles.TMatrix(1))
    set(handles.Matrix_name_edit,'String',['sin([' MergeM '])']);
else
    set(handles.Matrix_name_edit,'String',['sin(double([' MergeM ']))']);
end
MU_calc_matrix(handles);

end


classdef MaOPP_binary < PROBLEM
% <many> <binary> <large/none> <expensive/none>
% Many-objective pathfinding problem based on binary encoding
% xmax                    ---  10 --- xmax
% ymax                    ---  10 --- ymax
% obstacleValue           ---   0 --- obstacleValue
% nh                      ---   1 --- nh
% neighbourhood           ---   2 --- neighbourhood
% backtracking            ---   0 --- backtracking
% allowObstaclesOnPath    ---   1 --- allowObstaclesOnPath
% overheadVariablesFactor --- 1.5 --- overheadVariablesFactor

%------------------------------- Reference --------------------------------
% J. Weise and S. Mostaghim, A scalable many-objective pathfinding
% benchmark suite, IEEE Transactions on Evolutionary Computation, 2022,
% 26(1): 188-194.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% The most recent reference fronts and sets can be found at
% https://ci.ovgu.de/Publications/Scalable+Many_Objective+Pathfinding+Benchmark+Suite-p-910.html
% In case of any question, please contact me at
% jens.weise@ovgu.de

    properties(Access = protected)
        x_max;          % size [1,x_max] in x direction
        y_max;          % size [1,y_max] in y direction
        delay;          % expected delay
        nh;             % elevation function {1,2,3,M} -> 1 = 1, 2 = 2, 3 = 3, 4 = M
        neighborhood;   % 2^k, k {2,3}
        backtracking;   % True, False
        obstacle;       %{0,1,2} -> 0 = No, 1 = CH, 2 = LA
        vmax_high;
        vmax_medium;
        vmax_low;
        v_max;
        elevation;
        allowObstaclesOnPath;
        upperBoundsForObjectives;
        overheadVariablesFactor;
        numerOfBits;
    end

    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.x_max, obj.y_max, obstacleValue, obj.nh, obj.neighborhood, obj.backtracking, obj.allowObstaclesOnPath, obj.overheadVariablesFactor] = obj.ParameterSet(10,10,0,1,2,0,1,1);
            if isempty(obj.vmax_high); obj.vmax_high = 130; end
            if isempty(obj.vmax_medium); obj.vmax_medium = 100; end
            if isempty(obj.vmax_low); obj.vmax_low = 50; end
            obj.M = 5;
            obj.setBinaryEncoding();
            obj.v_max     = zeros(obj.x_max, obj.y_max);  % matrix of shape (x_max,y_max), matrix with different v values for each cell
            obj.elevation = zeros(obj.x_max, obj.y_max);  % matrix of shape (x_max,y_max)
            for x = 1 : obj.x_max
                for y = 1 : obj.y_max
                    obj.obstacle(x,y) = obstacleValue;
                    w_x_y = max(sin(x-1), cos(y-1));
                    if w_x_y > 0.9
                        obj.v_max(x, y) = obj.vmax_high;
                    elseif w_x_y < -0.4
                        obj.v_max(x, y) = obj.vmax_low;
                    else
                        obj.v_max(x, y) = obj.vmax_medium;
                    end
                    % velocity matrix obstacles for CH
                    if obj.obstacle(x,y) == 1 && (sign(sin(pi/2 + pi*x)) + sign(sin(pi/2 + pi*y)) - 2* (heaviside(x-obj.x_max+0.5)- heaviside(x-obj.x_max-0.5))* (heaviside(y-obj.y_max+0.5)- heaviside(y-obj.y_max-0.5)) == 2)
                        obj.v_max(x, y) = 0;
                    end
                    % velocity matrix obstacles for LA
                    if obj.obstacle(x,y) == 2 && (x-1-obj.x_max/2)^2 + (y-1-obj.y_max/2)^2 - (0.25*obj.x_max)^2 <0 %Q: using radius of x_max/4 from paper page 4 line 22 A: Yes.
                        obj.v_max(x, y) = 0;
                    end
                    xs = map(x,1,obj.x_max+1,-3,3);
                    ys = map(y,1,obj.y_max+1,-3,3);
                    if obj.nh == 1
                        obj.elevation(x,y) = 5*exp(-(xs-1.5)^2-(ys+1.5)^2);
                    elseif obj.nh == 2
                        obj.elevation(x,y) = 5*exp(-(xs+1.5)^2-(ys+1.5)^2) + 5*exp(-(xs-1.5)^2-(ys-1.5)^2);
                    elseif obj.nh == 3
                        obj.elevation(x,y) = 5*exp(-(xs+1.5)^2-(ys+1.5)^2) + 5*exp(-(xs-1.5)^2-(ys-1.5)^2) + 5*exp(-(xs-1.5)^2-(ys+1.5)^2);
                    elseif obj.nh == 4
                        obj.elevation(x,y) = 3*(1-xs)^2*exp(-(xs^2)-(ys+1)^2)-10*exp(-xs^2-ys^2)*(-xs^3+xs/5-ys^5)-1/3*exp(-(xs+1)^2-ys^2);
                    end
                end
            end
            manhattanSteps = (obj.x_max + obj.y_max - 2);
            obj.upperBoundsForObjectives = [manhattanSteps*1.5, manhattanSteps*1.5, 1.5*5*obj.nh, 1.5*manhattanSteps/50, 0.5*manhattanSteps*pi/2];
        end
        function setBinaryEncoding(obj)
            if obj.neighborhood == 2 && obj.backtracking == 0
                obj.D = 1*(obj.x_max + obj.y_max - 2)*obj.overheadVariablesFactor;
                obj.numerOfBits = 1;
            elseif obj.neighborhood == 2 && obj.backtracking == 1
                obj.D = 2*(obj.x_max + obj.y_max - 2)*obj.overheadVariablesFactor;
                obj.numerOfBits = 2;
            elseif obj.neighborhood == 3 && obj.backtracking == 0
                obj.D = 2*(obj.x_max + obj.y_max - 2)*obj.overheadVariablesFactor;
                obj.numerOfBits = 2;
            elseif obj.neighborhood == 3 && obj.backtracking == 1
                obj.D = 3*(obj.x_max + obj.y_max - 2)*obj.overheadVariablesFactor;
                obj.numerOfBits = 3;
            end
            obj.encoding = 4 + zeros(1,obj.D);
        end
        function [x_coords, y_coords,D] = decodePath(obj,PopDec,x_max,y_max)
            D = size(PopDec,2);
            n = obj.numerOfBits;
            for i = 0 : (D/n)-1
               temp = PopDec(:,[n*i+1:n*i+n]);
               realPopDec(:,i+1) = b2d(temp);
            end
            realPopDec = 1.0*realPopDec/(2^n);
            [x_coords, y_coords, D] = decodePathWithRealValues(obj,realPopDec,x_max,y_max);
            D = size(realPopDec,2);
        end
        function [x_coords, y_coords, D] = decodePathWithRealValues(obj,PopDec,x_max,y_max)
            [N,D] = size(PopDec);
            % convert the array into a sequence of x,y coordinates
            x_coords = zeros(N,D+1);
            y_coords = zeros(N,D+1);
            x_coords(:,1) = 1;
            y_coords(:,1) = 1;

            for i = 1 : N
                x_current = 1;
                y_current = 1;
                for d = 1 : D
                    possibleNeighours      = obj.getPossNeighbours(x_current,y_current,obj.backtracking,obj.neighborhood,obj.allowObstaclesOnPath);
                    noOfPossibleNeighbours = size(possibleNeighours,1);
                    neighbourToTake        = noOfPossibleNeighbours;
                    for a = 1 : noOfPossibleNeighbours
                        if PopDec(i,d) >= (a-1)/noOfPossibleNeighbours && PopDec(i,d) < (a)/noOfPossibleNeighbours
                            neighbourToTake = a;
                            break;
                        end
                    end
                    if neighbourToTake == 0 % Path ends if there are no neighbours available
                        break;
                    end
                    addvals   = possibleNeighours(neighbourToTake,:);
                    xAdd      = addvals(1);
                    yAdd      = addvals(2);
                    x_current = x_current + xAdd;
                    y_current = y_current + yAdd;
                    x_coords(i,d+1) = x_current;
                    y_coords(i,d+1) = y_current;
                    if x_current == obj.x_max && y_current == obj.y_max
                        break;
                    end
                end
            end
        end
        function neighbours = getPossNeighbours(obj,xCurr,yCurr,backtracking,neighborhood,allowObstaclesOnPath)
            e  = [1 0];
            se = [1 1];
            s  = [0 1];
            sw = [-1 1];
            w  = [-1 0];
            nw = [-1 -1];
            n  = [0 -1];
            ne = [1 -1];
            currentCoordinates = [xCurr yCurr];
            if backtracking == 0
                if neighborhood == 2
                    neighbours = [e;s];
                else
                    neighbours=[e;se;s];
                end
            else
                if neighborhood == 2
                    neighbours = [e;s;w;n];
                else
                    neighbours = [e;se;s;sw;w;nw;n;ne];
                end
            end
            % should we remove the outliers?
            % it is closer to the original implementation
            nextCoordinates = neighbours+currentCoordinates;
            out = nextCoordinates(:,1)<1 | nextCoordinates(:,1)>obj.x_max | nextCoordinates(:,2)<1 | nextCoordinates(:,2)>obj.y_max;
            neighbours(out,:) = [];
            if allowObstaclesOnPath == 0
                nextCoordinates = neighbours+currentCoordinates;
                obstancles = getVmaxValues(nextCoordinates(:,2),nextCoordinates(:,1),obj.v_max) == 0;
                neighbours(obstancles,:) = [];
            end
        end
        %% separate the calculations here to be able to compute the feasibility rate as well (with a lot of extra runtime, but whatever..)
        function [objectives,distToTarget,fObstacle] = objectiveValuesPath(obj,PopDec)
            % read out parameters
            N = size(PopDec,1);
            [x_coords, y_coords, D] = obj.decodePath(PopDec,obj.x_max,obj.y_max);
            % for entire population (one column) iterate over nodes (columns)
            f1 = zeros(N,1); % euclidean distance
            f2 = zeros(N,1); % expected delays
            f3 = zeros(N,1); % elevations
            f4 = zeros(N,1); % traveling time
            f5 = zeros(N,1); % smoothness
            fObstacle = zeros(N,1);
            distances = zeros(N,D); % stores distances that were calculated for f1 for later use in f5
            c = [];
            for d = 1 : D
                x1        = x_coords(:,d);
                x2        = x_coords(:,d+1);
                y1        = y_coords(:,d);
                y2        = y_coords(:,d+1);
                hasEnded  = (x2~=0&x1~=0);
                x2(x2==0) = 1;
                y2(y2==0) = 1;
                x1(x1==0) = 1;
                y1(y1==0) = 1;

                vMaxValues1 = getVmaxValues(x1,y1,obj.v_max);
                vMaxValues2 = getVmaxValues(x2,y2,obj.v_max);

                if obj.allowObstaclesOnPath==1
                    obstancles = vMaxValues1==0|vMaxValues2==0;
                    fObstacle  = fObstacle+obstancles;
                else
                    obstancles = zeros(N,1);
                    fObstacle  = fObstacle+obstancles;
                end

                %---Objective 1: euclidean distance
                distances(:,d) = sqrt((x2-x1).^2+(y2-y1).^2);
                res = (distances(:,d)).*hasEnded;
                res(res>sqrt(2)) = 0;
                f1 = f1 + res;

                %---Objective 2: delays
                inds      = vMaxValues1 ~= vMaxValues2;
                del       = inds*2;
                inds      = vMaxValues1 == vMaxValues2;
                loc       = find(inds==1);
                inds      = zeros(N,1);
                inds(loc) = vMaxValues1(loc);
                inds50    = inds == 50;
                del       = del + inds50*3;
                inds100   = inds == 100;
                del       = del + inds100*1;
                inds0     = del == 0;
                del       = del + inds0*0.2;
                res       = del.*hasEnded;
                f2        = f2 + res;

                %---Objective 3: elevations
                h1   = getVmaxValues(x1,y1,obj.elevation);
                h2   = getVmaxValues(x2,y2,obj.elevation);
                inds = h1 < h2;
                res  = (inds.*(h2-h1)).*hasEnded;
                f3   = f3 + res;

                %---Objective 4: traveling time
                res = (2*distances(:,d)./(vMaxValues1+vMaxValues2));
                res(isinf(res)|isnan(res)) = 0;
                if any(isnan(res))
                    errr;
                end
                res = res.*hasEnded;
                f4  = f4 + res;

                %---Objective 5: smoothness
                if d>1
                    x0       = x_coords(:,d-1);
                    y0       = y_coords(:,d-1);
                    x0(x0<1) = 1;
                    y0(y0<1) = 1;

                    test = transpose(dot([(x0-x1).'; (y0-y1).'],[(x1-x2).'; (y1-y2).'],1))./(distances(:,d-1).*distances(:,d));
                    test(isnan(test)) = 0;
                    try
                    	assert(all(test <= 1) && all(test >= -1))
                    catch
                    end
                    res = (acos(transpose(dot([(x0-x1).'; (y0-y1).'],[(x1-x2).'; (y1-y2).'],1))./(distances(:,d-1).*distances(:,d)))).*hasEnded;
                    res(isnan(res) )= 0;
                    f5 = f5 + res;
                end
            end
            objectives = [f1,f2,f3,f4,f5];
            try
                last = [];
                for i = 1 : N
                    last = [last find(x_coords(i,:)~=0,1,'last')];
                end
                last = last';
                lxc  = [];
                for k = 1 : N
                    lxc(k,1) = x_coords(k,last(k));
                    lxc(k,2) = y_coords(k,last(k));
                end
            catch
            end
            target       = [repmat(obj.x_max,N,1) repmat(obj.y_max,N,1)];
            distToTarget = abs(target(:,1)-lxc(:,1))+abs(target(:,2)-lxc(:,2));
        end
        %% Evaluate multiple solutions
        function Population = Evaluation(obj,varargin)
            [objectives,distToTarget,fObstacle] = objectiveValuesPath(obj,varargin{1});
            objectives(:,1)                     = objectives(:,1) + 2*distToTarget + fObstacle;
            Population = SOLUTION(varargin{1},objectives,distToTarget+fObstacle,varargin{2:end});
            obj.FE     = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        % Here, the pre-computed fronts are read. If it is not available,
        % an educated-guess Nadir point is computed. Please check the
        % website above in the header to see, if there are new fronts
        % available.
        function R = GetOptimum(obj,N)
            obs = '';
            if obj.obstacle == 0
                obs = 'NO';
            elseif obj.obstacle == 1
                obs = 'CH';
            elseif obj.obstacle == 2
                obs = 'LA';
            end
            sizeX = obj.x_max;
            sizeY = obj.y_max;
            ele   = '';
            if obj.nh == 1
                ele = '1';
            elseif obj.nh == 2
                ele = '2';
            elseif obj.nh == 3
                ele = '3';
            elseif obj.nh == 4
                ele = 'M';
            end
            k = obj.neighborhood;
            if obj.backtracking
                bt = 'T';
            else
                bt = 'F';
            end
            problem = sprintf('ASLETISMAC_%s_X%i_Y%i_P%s_K%i_B%s',obs,sizeX,sizeY,ele,k,bt);
            try
                CallStack = dbstack('-completenames');
                load(fullfile(fileparts(CallStack(1).file),'PF.mat'),problem);
                order = [2 5 1 3 4];
                eval(['R=',problem,';']);
                R      = R(:,order);
                R(:,5) = deg2rad(R(:,5));
            catch
                R = repmat(obj.upperBoundsForObjectives,N,1);
            end
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end

function out= map(x,in_min,in_max,out_min,out_max)
    out=(x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
end

function vList = getVmaxValues(x1,y1,aMatrix)
    len = size(x1,1);
    vList = zeros(len,1);
    for i = 1:len
        vList(i) = aMatrix(x1(i),y1(i));
    end
end

% Converts binary to decimal (MSB 0)!
function dec = b2d(vector)
    l   = size(vector);
    dec = ones(l(1),1);
    for r = 1 : l(1)
        dTemp = 0;
        for i = 1 : l(2)
            dTemp = dTemp + vector(r,i)*2^(i-1);
        end
        dec(r,1) = dTemp;
    end
end


function [d,a] =  training(a,d)

%if alpha empty in child | alpha full in rss model, train child (first train/retrain).
%if alpha full in child, empty in rss, only train rss.

if isempty(a.child.alpha) | ~isempty(a.alpha) % train algorithm first
   [r a.child]=train(a.child,d);
end

alpha=a.child.alpha;
svs=find(sum(abs(alpha)',1)>1e-5);

if(isfield(struct(a.child),'X'))
   a.Xsv=get(a.child.X,svs);
else
    a.Xsv=get(a.child.Xsv,svs);
end

K=calc(a.child.child,a.Xsv);
newalpha=alpha(svs,:)*0; alpha=alpha(svs,:); origalpha=alpha;
w2=[]; for i=1:size(alpha,2) w2(i)=alpha(:,i)'*K*alpha(:,i); end;
worig=w2; origsvs=svs;
loops=1;
disp('compressing..');
svs=[];

while max(w2)>a.tolerance & loops<a.max_loops
   %w is determined by alpha ONLY

   wx=alpha(:,1)*0;
   for i=1:size(alpha,2)
    f=find(abs(alpha(:,i))>0);
    tmp= w2(i) - (((K(:,f)*alpha(f,i)).^2) ./ diag(K)) ;  % calc all (w.x_i)^2/||x_i||^2 for each w
    if a.bal_w
     wx=wx+ (tmp/w2(i));  % was just tmp, attempt to normalize problems
    else
     wx=wx+tmp;
    end
   end
   if a.dont_revisit
     wx(svs)=Inf;    % ensure do not revisit existing basis function
   end
   [val,I]=min(wx); % find arg max w.x_i
   if ~isempty(intersect(I,svs)) & a.dont_revisit
        break; % seen everything
    end;
   oldsvs=svs;
   if length(oldsvs)<length(union(svs,I))
    svs=[oldsvs;I];
   end

   if loops>0 & (mod(loops,a.backfit)==0  | (loops<a.backfit_at_start) | ...
		 (sum(loops==a.test_on)==1 & a.backfit>0) )
       % find optimal alphas by backfitting

       %replace this by finvupdate
      if ~strcmp(a.optimizer,'iterative')
       %inv=pinv(K(svs,svs)); % only need to compute inverse once
       minv=inv(K(svs,svs)+eye(length(svs))*1e-6);
                             % only need to compute inverse once
       else
                newsv=I;
                 if length(oldsvs)==0
                   R=1/sqrt(K(newsv,newsv)); minv=1/(K(newsv,newsv));
               else
                  newsv=I;
                  [R,minv]=finvupdate(a,R,K(oldsvs,newsv),K(newsv,newsv));
         end
       end

     alpha=origalpha;
     for i=1:size(alpha,2)
      beta=minv*K(svs,:)*origalpha(:,i);
      newalpha(svs,i)=beta;  alpha(svs,i)=alpha(svs,i)-beta;
     end

	if ~isempty(a.dtst) & sum(loops==a.test_on)==1
	                 % run on separate test / validation set
                         %  classification only right now

	  a.alpha=newalpha;a.Xsv=get(a.child.Xsv,origsvs);
	  fin=find(sum(abs(a.alpha)',1)>a.alpha_cutoff);
	  a.alpha=a.alpha(fin,:); a.Xsv=get(a.Xsv,fin);
          a.b0=a.child.b0;

         if a.reoptimize_b % reoptimize b // only for pat. rec. right now
         a.b0=0;
	   r=test(a,d); a.b0=[];
	  for i=1:size(newalpha,2)
	     rr=r.X; rr=rr(:,i); [x s2]=sort(rr); y=r.Y(s2,i);
            xs=cumsum(y(end:-1:1)==1);
            [m1 m2]=max(cumsum(y==-1)+xs(end:-1:1));
            a.b0=[a.b0 -x(m2)];
            %if m2+1<=size(x,1) a.b0=[a.b0 -(x(m2)+x(m2+1))/2]; end;
         end
         end

        [r2]=test(a,a.dtst);
        [m1 c]=max(a.dtst.Y');
        [m1 c1]=max(r2.X');
        err=sum(c1~=c)/length(c);
        a.res=[a.res; loops err sum(sum(abs(newalpha)'>0)>0) ];
	 a.res(:,1)'
	 a.res(:,2)'
        disp(sprintf('svs: %d    err: %f ',sum(sum(abs(newalpha)'>0)>0),err));
     end

   else % only update one alpha
    for i=1:size(alpha,2)
     ai= (K(I,:)*alpha(:,i)) / K(I,I);   %   a_i = (x_i,w) / (x_i,x_i)  , optimal alpha
     alpha(I,i)=alpha(I,i)-ai;          %   w <- w - a_i x_i, project out learnt part of w
     newalpha(I,i)=newalpha(I,i)+ai;    %   wnew <- wnew + a_i x_i, add to new w
    end
   end

    w2=[]; for i=1:size(alpha,2) f=find(abs(alpha(:,i))>0); w2(i)=alpha(f,i)'*K(f,f)*alpha(f,i); end;
    a.w2=w2; % store w2
    %% this line could be sped up

   if mod(loops,25)==0
   txt='iteration %d : max||w_orig-w_new||^2=%1.3f svs=%d';
   disp(sprintf(txt,[loops max(w2) sum(sum(abs(newalpha)',1)>0)]));
   end;
   loops=loops+1;
end

a.alpha=newalpha;

if(isfield(struct(a.child),'X'))
    a.Xsv=get(a.child.X,origsvs);
else
    a.Xsv=get(a.child.Xsv,origsvs);
end

fin=find(sum(abs(a.alpha)',1)>a.alpha_cutoff);
a.alpha=a.alpha(fin,:);
a.Xsv=get(a.Xsv,fin);
a.b0=a.child.b0;

if a.reoptimize_b % reoptimize b // only for pat. rec. right now
   a.b0=0; r=test(a,d); a.b0=[];
   for i=1:size(newalpha,2)
    rr=r.X; rr=rr(:,i); [x s2]=sort(rr); y=r.Y(s2,i);
    xs=cumsum(y(end:-1:1)==1);
    [m1 m2]=max(cumsum(y==-1)+xs(end:-1:1));
    a.b0=[a.b0 -x(m2)];
    %if m2+1<=size(x,1) a.b0=[a.b0 -(x(m2)+x(m2+1))/2]; end;
   end
end

if a.algorithm.do_not_evaluate_training_error==1
  d=set_x(d,get_y(d));
else
  d=test(a,d);
end


%DAS_BF
clear;
close all;
load('Computed_RIRs.mat');

nChannels = size(m_pos,1);
dMic = m_pos(2, 2) - m_pos(1, 2);
c = 340;
delta = pdist2(s_pos(1,:), m_pos(2,:)) - pdist2(s_pos(1,:), m_pos(1,:));
DOA_est = acosd(delta/dMic);

%==================Generate array signals==================================%
speechfilename = {'6319-275224-0008.flac', '6319-275224-0011.flac'};
noisefilename = {'noise1.wav', 'noise2.wav'};

[source1, fs] = audioread(speechfilename{1});
[source2, ~] = audioread(noisefilename{1});
[noise, fs_n] = audioread(noisefilename{1});
noise = resample(noise,fs,fs_n);

n_f = fs * 10; %2 seoconds
source1 = source1(1:n_f);
source2 = source2(1:n_f) ./ 3;
noise = noise(1:n_f);

rir = RIR_sources(:,:,1);
speech1 = fftfilt(rir, source1).*30;

rir = RIR_sources(:,:,2);
speech2 = fftfilt(rir,source2).*30;

arraySignal = speech1 + repmat(noise, 1, 5);
%==========================================================================%

%=============================plot Beamformer==============================%
%h_mvdr = mvdr(d, Phi_u);
theta = linspace(0, 2*pi, 200); % incidence angle range
%plot
figure(1);
f = [10000];
hw = dsb(nChannels, dMic, DOA_est, f);        % delay-and-sum filter
p = plotBeamformer(nChannels, f, dMic, hw, theta);

% f = linspace(0, fs, 256);% freqeuncy vector
% hw = dsb(nChannels, dMic, DOA_est, f);        % delay-and-sum filter
% p = plotBeamformer(nChannels, f, dMic, hw, theta);
%==========================================================================%

%=============================apply Beamformer=============================%
x_das = applyBeamforming(arraySignal.', dMic, fs,  DOA_est, 'DSB', false);
audiowrite('output/x_dsb.wav', x_das, fs);

x_mvdr = applyBeamforming(arraySignal.', dMic, fs,  DOA_est, 'MVDR', false);
audiowrite('output/x_mvdr.wav', x_mvdr, fs);
%==========================================================================%

%====================Quality evaluation====================================%
%Compare
%[y,fs] = audioread('wav/speech1.wav');
%audiowrite('wav/speech1_16k.wav',y,16000);
%[y,fs] = audioread('output/x_dsb.wav');
%audiowrite('output/x_out_16k.wav',y,16000);
%%pesq = pesq('wav/speech1_16k.wav', 'wav/x_out_16k.wav')
%==========================================================================%


function c = tapas_ehgf_binary_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the enhanced Hierarchical Gaussian Filter (eHGF)
% for binary inputs in the absence of perceptual uncertainty.
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
% The binary HGF model has since been augmented with a positive factor kappa1 which
% scales the second level with respect to the first, i.e., the relation between the
% first and second level is
%
% p(x1=1|x2) = s(kappa1*x2), where s(.) is the logistic sigmoid.
%
% By default, kappa1 is fixed to 1, leading exactly to the model introduced in
% Mathys et al. (2011).
%
% This file refers to BINARY inputs (Eqs 1-3 in Mathys et al., (2011));
% for continuous inputs, refer to tapas_ehgf_config.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_ehgf_binary_plotTraj(est)
%
% where est is the stucture returned by tapas_fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mu_0       row vector of initial values of mu (in ascending order of levels)
%         est.p_prc.sa_0       row vector of initial values of sigma (in ascending order of levels)
%         est.p_prc.rho        row vector of rhos (representing drift; in ascending order of levels)
%         est.p_prc.ka         row vector of kappas (in ascending order of levels)
%         est.p_prc.om         row vector of omegas (in ascending order of levels)
%
% Note that the first entry in all of the row vectors will be NaN because, at the first level,
% these parameters are either determined by the second level (mu_0 and sa_0) or undefined (rho,
% kappa, and omega).
%
%         est.traj.mu          mu (rows: trials, columns: levels)
%         est.traj.sa          sigma (rows: trials, columns: levels)
%         est.traj.muhat       prediction of mu (rows: trials, columns: levels)
%         est.traj.sahat       precisions of predictions (rows: trials, columns: levels)
%         est.traj.v           inferred variance of random walk (rows: trials, columns: levels)
%         est.traj.w           weighting factors (rows: trials, columns: levels)
%         est.traj.da          volatility prediction errors  (rows: trials, columns: levels)
%         est.traj.ud          updates with respect to prediction  (rows: trials, columns: levels)
%         est.traj.psi         precision weights on prediction errors  (rows: trials, columns: levels)
%         est.traj.epsi        precision-weighted prediction errors  (rows: trials, columns: levels)
%         est.traj.wt          full weights on prediction errors (at the first level,
%                                  this is the learning rate) (rows: trials, columns: levels)
%
% Note that in the absence of sensory uncertainty (which is the assumption here), the first
% column of mu, corresponding to the first level, will be equal to the inputs. Likewise, the
% first column of sa will be 0 always.
%
% Tips:
% - When analyzing a new dataset, take your inputs u and use
%
%   >> est = tapas_fitModel([], u, 'tapas_ehgf_binary_config', 'tapas_bayes_optimal_binary_config');
%
%   to determine the Bayes optimal perceptual parameters (given your current priors as defined in
%   this file here, so choose them wide and loose to let the inputs influence the result). You can
%   then use the optimal parameters as your new prior means for the perceptual parameters.
%
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2020 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'ehgf_binary';

% Number of levels (minimum: 3)
c.n_levels = 3;

% Input intervals
% If input intervals are irregular, the last column of the input
% matrix u has to contain the interval between inputs k-1 and k
% in the k-th row, and this flag has to be set to true
c.irregular_intervals = false;

% Sufficient statistics of Gaussian parameter priors

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first two levels, this is usually best
% kept fixed to 1 (determines origin on x_i-scale). The
% first level is NaN because it is determined by the second,
% and the second implies neutrality between outcomes when it
% is centered at 0.
c.mu_0mu = [NaN, 0, 1];
c.mu_0sa = [NaN, 0, 0];

c.logsa_0mu = [NaN,   log(0.1), log(1)];
c.logsa_0sa = [NaN,          0,      0];

% Rhos
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
% Fix this to zero to turn off drift.
c.rhomu = [NaN, 0, 0];
c.rhosa = [NaN, 0, 0];

% Kappas
% Format: row vector of length n_levels-1.
% Fixing log(kappa1) to log(1) leads to the original HGF model.
% Higher log(kappas) should be fixed (preferably to log(1)) if the
% observation model does not use mu_i+1 (kappa then determines the
% scaling of x_i+1).
c.logkamu = [log(1), log(1)];
c.logkasa = [     0,      0];

% Omegas
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
c.ommu = [NaN,  -3,   2];
c.omsa = [NaN,   4,   4];

% Gather prior settings in vectors
c.priormus = [
    c.mu_0mu,...
    c.logsa_0mu,...
    c.rhomu,...
    c.logkamu,...
    c.ommu,...
         ];

c.priorsas = [
    c.mu_0sa,...
    c.logsa_0sa,...
    c.rhosa,...
    c.logkasa,...
    c.omsa,...
         ];

% Check whether we have the right number of priors
expectedLength = 3*c.n_levels+2*(c.n_levels-1)+1;
if length([c.priormus, c.priorsas]) ~= 2*expectedLength
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_ehgf_binary;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_ehgf_binary_transp;

end


function DVSet = CorrelationAnalysis(Problem,Population,DV,nCor)
% Detect the group of each distance variable

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

    DVSet = {};
    for v = DV
        RelatedSet = [];
        for d = 1 : length(DVSet)
            for u = DVSet{d}
                drawnow('limitrate');
                sign = false;
                for i = 1 : nCor
                    p    = Population(randi(length(Population)));
                    a2   = unifrnd(Problem.lower(v),Problem.upper(v));
                    b2   = unifrnd(Problem.lower(u),Problem.upper(u));
                    decs = repmat(p.dec,3,1);
                    decs(1,v)     = a2;
                    decs(2,u)     = b2;
                    decs(3,[v,u]) = [a2,b2];
                    F = Problem.Evaluation(decs);
                    delta1 = F(1).obj - p.obj;
                    delta2 = F(3).obj - F(2).obj;
                    if any(delta1.*delta2<0)
                        sign = true;
                        RelatedSet = [RelatedSet,d];
                        break;
                    end
                end
                if sign
                    break;
                end
            end
        end
        if isempty(RelatedSet)
            DVSet = [DVSet,v];
        else
            DVSet = [DVSet,[cell2mat(DVSet(RelatedSet)),v]];
            DVSet(RelatedSet) = [];
        end
    end
end


function [vertices2, faces2] = subdivideMesh(vertices, faces, n)
%SUBDIVIDEMESH Subdivides each face of the mesh
%
%   [V2 F2] = subdivideMesh(V, F, N)
%   Subdivides the mesh specified by (V,F) such that each face F is divided
%   into N^2 smaller faces.
%
%   Example
%     [v f] = createOctahedron;
%     figure; drawMesh(v, f); view(3);
%     [v2 f2] = subdivideMesh(v, f, 4);
%     figure; drawMesh(v2, f2); view(3)
%
%   See also
%     meshes3d, drawMesh
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2013-08-22,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2013 INRA - Cepia Software Platform.

%% Initialisations

if ~isnumeric(faces) || size(faces, 2) ~= 3
    error('Requires a triangular mesh');
end

% compute the edge array
% edgeVertexIndices = computeMeshEdges(faces);
edges = meshEdges(faces);
nEdges = size(edges, 1);

% index of faces around each edge
% edgeFaceIndices = meshEdgeFaces(vertices, edges, faces);

% index of edges around each face
faceEdgeIndices = meshFaceEdges(vertices, edges, faces);

%% Create new vertices on edges

% several interpolated positions
t = linspace(0, 1, n + 1)';
coef2 = t(2:end-1);
coef1 = 1 - t(2:end-1);

% initialise the array of new vertices
vertices2 = vertices;

% keep an array containing index of new vertices for each original edge
edgeNewVertexIndices = zeros(nEdges, n-1);

% create new vertices on each edge
for iEdge = 1:nEdges
    % extract each extremity as a point
    v1 = vertices(edges(iEdge, 1), :);
    v2 = vertices(edges(iEdge, 2), :);

    % compute new points
    newPoints = coef1 * v1 + coef2 * v2;

    % add new vertices, and keep their indices
    edgeNewVertexIndices(iEdge,:) = size(vertices2, 1) + (1:n-1);
    vertices2 = [vertices2 ; newPoints]; %#ok<AGROW>
end

%% Process each face

faces2 = zeros(0, 3);

nFaces = size(faces, 1);
for iFace = 1:nFaces
    % compute index of each corner vertex
    face = faces(iFace, :);
    iv1 = face(1);
    iv2 = face(2);
    iv3 = face(3);

    % compute index of each edge
    faceEdges = faceEdgeIndices{iFace};
    ie1 = faceEdges(1);
    ie2 = faceEdges(2);
    ie3 = faceEdges(3);

    % indices of new vertices on edges
    edge1NewVertexIndices = edgeNewVertexIndices(ie1, :);
    edge2NewVertexIndices = edgeNewVertexIndices(ie2, :);
    edge3NewVertexIndices = edgeNewVertexIndices(ie3, :);

    % keep vertex 1 as reference for edges 1 and 3
    if edges(ie1, 1) ~= iv1
        edge1NewVertexIndices = edge1NewVertexIndices(end:-1:1);
    end
    if edges(ie3, 1) ~= iv1
        edge3NewVertexIndices = edge3NewVertexIndices(end:-1:1);
    end

    % create the first new face, on 'top' of the original face
    topVertexInds = [edge1NewVertexIndices(1) edge3NewVertexIndices(1)];
    newFace = [iv1 topVertexInds];
    faces2 = [faces2; newFace]; %#ok<AGROW>

    % iterate over middle strips
    for iStrip = 2:n-1
        % index of extreme vertices of current row
        ivr1 = edge1NewVertexIndices(iStrip);
        ivr2 = edge3NewVertexIndices(iStrip);

        % extreme vertices as points
        v1 = vertices2(ivr1, :);
        v2 = vertices2(ivr2, :);

        % create additional vertices within the bottom row of the strip
        t = linspace(0, 1, iStrip+1)';
        coef2 = t(2:end-1);
        coef1 = 1 - t(2:end-1);
        newPoints = coef1 * v1 + coef2 * v2;

        % compute indices of new vertices in result array
        newInds = size(vertices2, 1) + (1:iStrip-1);
        botVertexInds = [ivr1 newInds ivr2];

        % add new vertices
        vertices2 = [vertices2 ; newPoints]; %#ok<AGROW>

        % create top faces of current strip
        for k = 1:iStrip-1
            newFace = [topVertexInds(k) botVertexInds(k+1) topVertexInds(k+1)];
            faces2 = [faces2; newFace]; %#ok<AGROW>
        end

        % create bottom faces of current strip
        for k = 1:iStrip
            newFace = [topVertexInds(k) botVertexInds(k) botVertexInds(k+1)];
            faces2 = [faces2; newFace]; %#ok<AGROW>
        end

        % bottom vertices of current strip are top vertices of next strip
        topVertexInds = botVertexInds;
    end

    % for edge 2, keep vertex 2 of the current face as reference
    if edges(ie2, 1) ~= iv2
        edge2NewVertexIndices = edge2NewVertexIndices(end:-1:1);
    end

    % consider new vertices together with extremities
    botVertexInds = [iv2 edge2NewVertexIndices iv3];

    % create top faces for last strip
    for k = 1:n-1
        newFace = [topVertexInds(k) botVertexInds(k+1) topVertexInds(k+1)];
        faces2 = [faces2; newFace]; %#ok<AGROW>
    end

    % create bottom faces for last strip
    for k = 1:n
        newFace = [topVertexInds(k) botVertexInds(k) botVertexInds(k+1)];
        faces2 = [faces2; newFace]; %#ok<AGROW>
    end

end


function [ x, y ] = sswap ( n, x, incx, y, incy )

%*****************************************************************************80
%
%% SSWAP interchanges two vectors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vectors.
%
%    Input, real X(*), one of the vectors to swap.
%
%    Input, integer INCX, the increment between successive entries of X.
%
%    Input, real Y(*), one of the vectors to swap.
%
%    Input, integer INCY, the increment between successive elements of Y.
%
%    Output, real X(*), the swapped vector.
%
%    Output, real Y(*), the swapped vector.
%
  if ( n <= 0 )

  elseif ( incx == 1 & incy == 1 )

    m = mod ( n, 3 );

    for i = 1 : m
      temp = x(i);
      x(i) = y(i);
      y(i) = temp;
    end

    for i = m+1 : 3 : n

      temp = x(i);
      x(i) = y(i);
      y(i) = temp;

      temp = x(i+1);
      x(i+1) = y(i+1);
      y(i+1) = temp;

      temp = x(i+2);
      x(i+2) = y(i+2);
      y(i+2) = temp;

    end

  else

    if ( 0 <= incx )
      ix = 1;
    else
      ix = ( - n + 1 ) * incx + 1;
    end

    if ( 0 <= incy )
      iy = 1;
    else
      iy = ( - n + 1 ) * incy + 1;
    end

    for i = 1 : n
      temp = x(ix);
      x(ix) = y(iy);
      y(iy) = temp;
      ix = ix + incx;
      iy = iy + incy;
    end

  end

  return
end


  function [phantom, params] = rect_im(ig, params, varargin)
%|function [phantom, params] = rect_im(ig, params, options)
%|
%| generate rectangle phantom image from parameters:
%|	[x_center y_center x_width y_width angle_degrees amplitude]
%|
%| in
%|	ig			image_geom() object
%|	params			rect parameters, if empty, use the default
%|				params = rect_im_default_parameters(xfov, yfov)
%|
%| options
%|	'oversample'	int	oversampling factor, for grayscale boundaries
%|	'hu_scale'	float	use 1000 to scale shepp-logan to HU (default: 1)
%|	'fov'		float	default ig.fov
%|
%| out
%|	phantom		[nx ny]	image
%|
%| 2008-08-04, Yong Long, adapted from ellipse_im()
%| Copyright 2006-2-2, Jeff Fessler, University of Michigan

if nargin == 1 && streq(ig, 'test'), rect_im_test, return, end
if nargin < 1, ir_usage, end

arg.oversample = 1;
arg.replace = 0;
arg.hu_scale = 1;
arg.fov = [];
arg.chat = false;
arg = vararg_pair(arg, varargin);
if isempty(arg.fov), arg.fov = ig.fov; end

if ~isvar('params') || isempty(params)
	params = rect_im_default_parameters(arg.fov, arg.fov);
end

params(:,6) = params(:,6) * arg.hu_scale;

do_fast = params(:,5) == 0; % non-rotated ones are faster

if arg.chat
	printm('%d of %d fast', sum(do_fast), numel(do_fast))
end

if any(do_fast)
	phantom = rect_im_fast(params(do_fast,:), ...
		ig.nx, ig.ny, ig.dx, ig.dy, ig.offset_x, ig.offset_y, ...
		arg.replace);
else
	phantom = 0;
end

if any(~do_fast)
	phantom = phantom + rect_im_slow(params(~do_fast,:), ...
		ig.nx, ig.ny, ig.dx, ig.dy, ig.offset_x, ig.offset_y, ...
		arg.oversample, arg.replace);
end

% rect_im_fast()
% fast version for non-rotated rectangles
% using exact integration over each pixel so over-sampling is irrelevant
function phantom = rect_im_fast(params, ...
	nx, ny, dx, dy, offset_x, offset_y, replace)

if size(params,2) ~= 6
	error 'bad rect parameter vector size'
end

phantom = zeros(nx, ny);

wx = (nx-1)/2 + offset_x;
wy = (ny-1)/2 + offset_y;
x1 = ([0:nx-1]' - wx) * dx; % col
y1 = ([0:ny-1] - wy) * dy; % row

fun = @(x1, x2, wx) ... % integrated rect(x/wx) function from x1 to x2
	max(min(x2, wx/2) - max(x1, -wx/2), 0);

%ticker reset
ne = nrow(params);
for ie = 1:ne
%	ticker(mfilename, ie, ne)

	rect = params(ie, :);
	cx = rect(1);	wx = rect(3);
	cy = rect(2);	wy = rect(4);
	theta = deg2rad(rect(5));
	if theta ~= 0, fail 'theta=0 required', end
	x = x1 - cx;
	y = y1 - cy;
	tx = fun(x-abs(dx)/2, x+abs(dx)/2, wx) / abs(dx);
	ty = fun(y-abs(dy)/2, y+abs(dy)/2, wy) / abs(dy);
	tmp = single(tx) * single(ty); % outer product (separable)
	if replace
		phantom(tmp > 0) = rect(6);
	else
		phantom = phantom + rect(6) * tmp;
	end
end

% rect_im_slow()
% slower version that handles rotation too and uses over-sampling
function phantom = rect_im_slow(params, ...
	nx, ny, dx, dy, offset_x, offset_y, over, replace)

if size(params,2) ~= 6
	error 'bad rect parameter vector size'
end

phantom = zeros(nx*over, ny*over);

wx = (nx*over-1)/2 + offset_x*over;
wy = (ny*over-1)/2 + offset_y*over;
xx = ([0:nx*over-1] - wx) * dx / over;
yy = ([0:ny*over-1] - wy) * dy / over;
[xx yy] = ndgrid(xx, yy);

ticker reset
ne = nrow(params);
for ie = 1:ne
	ticker(mfilename, ie, ne)

	rect = params(ie, :);
	cx = rect(1);	wx = rect(3);
	cy = rect(2);	wy = rect(4);
	theta = deg2rad(rect(5));
	x = cos(theta) * (xx-cx) + sin(theta) * (yy-cy);
	y = -sin(theta) * (xx-cx) + cos(theta) * (yy-cy);
	tmp = abs(x / wx) < 1/2 & abs(y / wy) < 1/2;
	if replace
		phantom(tmp > 0) = rect(6);
	else
		phantom = phantom + rect(6) * tmp;
	end
end

phantom = downsample2(phantom, over);

%
% default params for rectangles
% the first four columns are unitless for fov=64
%
function params = rect_im_default_parameters(xfov, yfov)
f = 1/64;
params = [ ...
	0	0	50	50	0	1
	10	-16	25	16	0	-0.5
	-13	15	13	13	1*45	1
...
	-18	0	1	1	0	1
	-12	0	1	1	0	1
	-6	0	1	1	0	1
	0	0	1	1	0	1
	6	0	1	1	0	1
	12	0	1	1	0	1
	18	0	1	1	0	1
];

params(:,[1 3]) = params(:,[1 3]) * xfov / 64;
params(:,[2 4]) = params(:,[2 4]) * yfov / 64;

% rect_im_test()
function rect_im_test
ig = image_geom('nx', 2^8, 'ny', 2^8, 'fov', 100);
pic = rect_im(ig, [[0.5 0 3 20]*ig.dx 0 1], 'oversample', 3);
jf_equal(unique(pic), [0;1])
im(pic)
pic = rect_im(ig, [], 'oversample', 3, 'chat', im);
%unique(pic)
im(ig.x, ig.y, pic, 'default rects'), cbar


function value = r8vec_amax ( n, a )

%*****************************************************************************80
%
%% R8VEC_AMAX returns the maximum absolute value in an R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(N), the array.
%
%    Output, real VALUE, the value of the entry
%    of largest magnitude.
%
  value = max ( abs ( a(1:n) ) );

  return
end


function indx = r8vec_sort_insert_index_a ( n, a )

%*****************************************************************************80
%
%% R8VEC_SORT_INSERT_INDEX_A ascending index sorts an R8VEC using insertion.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Algorithm 1.1,
%    Donald Kreher and Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998, page 11.
%
%  Parameters:
%
%    Input, integer N, the number of items in the vector.
%    N must be positive.
%
%    Input, real A(N), the array to be sorted.
%
%    Output, integer INDX(N), the sorted indices.  The array is sorted
%    when listed from A(INDX(1)) through A(INDX(N)).
%
  if ( n < 1 )
    return
  end

  indx = i4vec_indicator1 ( n );

  for i = 2 : n

    x = a(i);

    j = i - 1;

    while ( 1 <= j )

      if ( a(indx(j)) <= x )
        break
      end

      indx(j+1) = indx(j);
      j = j - 1;

    end

    indx(j+1) = i;

  end

  return
end