%GM_PHD_Construct_Update_Components
%Last modified 12th September 2013
%Matlab code by Bryan Clarke b.clarke@acfr.usyd.edu.au

%This file creates the components needed for performing a Kalman filter update on the
%targets using the measurement.
s = sprintf('Step 3: Constructing update components for all targets, new and existing.');
disp(s);

%We need to clear the data structures each iteration
eta = [];
S = [];
K = [];
P_k_k = [];

for j = 1:numTargets_Jk_k_minus_1
    m_j = mk_k_minus_1(:,j);
    eta_j = H2 * m_j;%Observation model. Assume we see position AND velocity of the target.

    P_range = calculateDataRange4(j); %4x4 array

    PHt = Pk_k_minus_1(:,P_range) * H2'; %Taken from Tim Bailey's EKF code. 4x4 array

    %Calculate K via Tim Bailey's method.
    S_j = R2 + H2 * PHt;
    %At this point, Tim Bailey's code makes S_j symmetric. In this case, it leads to the matrix being non-positive definite a lot of the time and chol() crashes.
    %So we won't do that.
    SChol= chol(S_j);

    SCholInv= SChol \ eye(size(SChol)); % triangular matrix, invert via left division
    W1 = PHt * SCholInv;

    K_j = W1 * SCholInv';

    P_j = Pk_k_minus_1(:,P_range) - W1*W1';%4x4 array
    %End Tim Bailey's code.

    eta = [eta, eta_j];
    S = [S, S_j];
    K = [K, K_j];
    P_k_k = [P_k_k, P_j];
end


function [kernel, S] = dat2Kernel(data, kSize)
% kernel = dat2Kernel(data, kSize,thresh)
%
% Function to perform k-space calibration step for ESPIRiT and create
% k-space kernels. Only works for 2D multi-coil images for now.
%
% Inputs:
%       data - calibration data [kx,ky,coils]
%       kSize - size of kernel (for example kSize=[6,6])
%
% Outputs:
%       kernel - k-space kernels matrix (not cropped), which correspond to
%                the basis vectors of overlapping blocks in k-space
%       S      - (Optional parameter) The singular vectors of the
%                 calibration matrix
%
%
% See also:
%           kernelEig
%
% (c) Michael Lustig 2013

[sx,sy,nc] = size(data);
imSize = [sx,sy] ;

tmp = im2row(data,kSize); [tsx,tsy,tsz] = size(tmp);
A = reshape(tmp,tsx,tsy*tsz);

[U,S,V] = svd(A,'econ');

kernel = reshape(V,kSize(1),kSize(2),nc,size(V,2));
S = diag(S);S = S(:);


function r = exp(a)
%EXP          Taylor exponential  exp(a)
%

% written  05/21/09     S.M. Rump
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                   % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  K1 = getappdata(0,'INTLAB_TAYLOR_ORDER') + 1;

  r = a;
  N = size(a.t,2);
  r.t(1,:) = exp(a.t(1,:));
  for j=2:K1
    r.t(j,:) = sum( repmat((1:j-1)',1,N).*r.t(j-1:-1:1,:).*a.t(2:j,:) , 1 ) ./ (j-1);
  end

  if rndold
    setround(rndold)
  end


%
% A variational approach to SPCP (Aravkin et al. 2014)
%
% RPCA | Lag-SPCP-SPG | Lagrangian SPCP solved by Spectral Projected Gradient (Aravkin et al. 2014)
% process_video('RPCA', 'Lag-SPCP-SPG', 'dataset/demo.avi', 'output/demo_Lag-SPCP-SPG.avi');

alg_path_aux = fullfile(lrs_conf.rpca_path,'SPGL1');
addpath(genpath(alg_path_aux));

nFrames     = size(M,2);
lambda      = 1/sqrt(max(size(M,1),size(M,2)));
L0          = repmat(median(M,2), 1, nFrames);
S0          = M - L0;
epsilon     = 5e-3*norm(M,'fro'); % tolerance for fidelity to data

% Lagrangian SPCP solved by Spectral Projected Gradient
opts    = struct('sum',false,'L0',L0,'S0',S0,'max',false,'tol',1e-3);
lambdaL = 0.25; lambdaS = 0.01; % (Aravkin et al. 2014)
[L,S] = solver_RPCA_Lagrangian(M,lambdaL,lambdaS,[],opts);

rmpath(genpath(alg_path_aux));


function padua_test02 ( )

%*****************************************************************************80
%
%% PADUA_TEST02 tests PADUA_POINTS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 June 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PADUA_TEST02\n' );
  fprintf ( 1, '  PADUA_POINTS returns the points of a Padua rule.\n' );

  for l = 0 : 10
    n = padua_order ( l );
    xy = padua_points ( l );
    label = sprintf ( '  Level %d Padua points:', l );
    r8mat_transpose_print ( 2, n, xy, label );
  end

  return
end


function Ath = threshold_components(A,options)

% post processing of spatial components
% for each component perform the following:
%   (i)     perform median filtering
%   (ii)    keep only pixels that contibute up to a level of total energy
%   (iii)   perform morphological closing
%   (iv)    extract largest connected component

% Written by:
% Eftychios A. Pnevmatikakis, Simons Foundation, 2015

    defoptions.nrgthr = 0.9999;              % energy threshold
    defoptions.clos_op = strel('square',3);  % morphological operator for closing
    defoptions.medw = [3,3];                 % size of median filter

    if ~isfield(options,'nrgthr') || isempty(options.nrgthr); options.nrgthr = defoptions.nrgthr; end
    if ~isfield(options,'clos_op') || isempty(options.clos_op); options.clos_op = defoptions.clos_op; end
    if ~isfield(options,'medw') || isempty(options.medw); options.medw = defoptions.medw; end
    if ~isfield(options,'d3') || isempty(options.d3); options.d3 = 1; end

    [d,nr] = size(A);
    Ath = spalloc(d,nr,nnz(A));
    Ath(:,nr-options.nb+1:nr) = A(:,nr-options.nb+1:nr);
    indf = cell(nr,1);
    valf = cell(nr,1);
    parfor i = 1:nr-options.nb
        A_temp = reshape(full(A(:,i)),options.d1,options.d2,options.d3);
        for z = 1:options.d3
            A_temp(:,:,z) = medfilt2(A_temp(:,:,z),options.medw);
        end
        A_temp = A_temp(:);
        [temp,ind] = sort(A_temp(:).^2,'ascend');
        temp =  cumsum(temp);
        ff = find(temp > (1-options.nrgthr)*temp(end),1,'first');
        BW = zeros(options.d1,options.d2,options.d3);
        BW(ind(ff:d)) = 1;
        for z = 1:options.d3
            BW(:,:,z) = imclose(BW(:,:,z),options.clos_op);
        end
        [L,NUM] = bwlabeln(BW,8*(options.d3==1) + 6*(options.d3~=1));
        if NUM > 0
            nrg = zeros(NUM,1);
            for l = 1:NUM
                ff = (L==l);
                nrg(l) = sum(A_temp(ff).^2);
            end
            [~,indm] = max(nrg);
            ff = find(L==indm);
            %Ath(ff,i) = A(ff,i);
            indf{i} = ff;
            valf{i} = A_temp(ff);
        else
            valf{i} = 0;
        end
    end
    for i = 1:nr-options.nb
        Ath(indf{i},i) = valf{i};
    end
end


function test_failed = test_libltfat_rtdgtreal(varargin)
test_failed = 0;
return;
fprintf(' ===============  %s ================ \n',upper(mfilename));

definput.flags.complexity={'double','single'};
[flags]=ltfatarghelper({},definput,varargin);
dataPtr = [flags.complexity, 'Ptr'];

[~,~,enuminfo]=libltfatprotofile;
LTFAT_FIRWIN = enuminfo.LTFAT_FIRWIN;
rdgt_phasetype = enuminfo.rtdgt_phasetype;

glarr =     [500,  512, 1024, 90];
Marr =      [1000, 512, 2048, 101];
aarr =      [100 , 256,  256, 40];
Warr =      [10  ,   2,    1,  3];

f = greasy;

a = 16;
gl = 60;
gdl = gl;
M = 64;
L = dgtlength(numel(f),a,M);
M2 = floor(M/2) + 1;
g = zeros(gl,1);
gd = zeros(gdl,1);
gPtr = libpointer(dataPtr,g);
gdPtr = libpointer(dataPtr,gd);
funname = makelibraryname('firwin',flags.complexity,0);
calllib('libltfat',funname,LTFAT_FIRWIN.LTFAT_HANN,gl,gPtr);

funname = makelibraryname('gabdual_painless',flags.complexity,0);
prd=calllib('libltfat',funname,gPtr,gl,a,M,gdPtr);
if prd
    warning('This is not painless frame');
    g2Ptr = libpointer(dataPtr,fir2long(gPtr.Value,L));
    gdPtr = libpointer(dataPtr,fir2long(gd,L));
    funname = makelibraryname('gabdual_long',flags.complexity,0);
    calllib('libltfat',funname,g2Ptr,L,1,a,M,gdPtr);
    gdl = L;
end

cframes = signal2frames(f,gl,a,M);
% cframes2 = bsxfun(@times,cframes,M*gPtr.Value.*gdPtr.Value);
% f2 = frames2signal(cframes2,gl,a);
% norm(f-postpad(f2,numel(f)))
N = size(cframes,2);

%ctrue = fftreal(ifftshift(bsxfun(@times,cframes,fftshift(gPtr.Value)),1));
ctrue = dgtreal(f,gPtr.Value,a,M,'timeinv');
p = libpointer();
pinv = libpointer();

funname = makelibraryname('rtdgtreal_init',flags.complexity,0);
calllib('libltfat',funname,gPtr,gl,M,rdgt_phasetype.LTFAT_RTDGTPHASE_ZERO,p);
funname = makelibraryname('rtidgtreal_init',flags.complexity,0);
calllib('libltfat',funname,gdPtr,gdl,M,rdgt_phasetype.LTFAT_RTDGTPHASE_ZERO,pinv);

c = zeros(2*M2,N);
f2 = zeros(gdl,N);
fPtr = libpointer(dataPtr,cframes);
cPtr = libpointer(dataPtr,c);
f2Ptr = libpointer(dataPtr,f2);

calllib('libltfat','ltfat_rtdgtreal_execute_d',p,fPtr,N,cPtr);
norm(ctrue - interleaved2complex(cPtr.Value),'fro')
calllib('libltfat','ltfat_rtidgtreal_execute_d',pinv,cPtr,N,f2Ptr);

frec = frames2signal(f2Ptr.Value,gdl,a);
figure(1);plot([f-postpad(frec,numel(f))]);

fprintf('Coef error. %d \n', norm(ctrue-interleaved2complex(cPtr.Value),'fro'));
fprintf('Rec error. %d \n', norm(f-postpad(frec,numel(f))));

figure(2);plotdgtreal(abs(ctrue)-abs(interleaved2complex(cPtr.Value)),a,M,'linabs');
%imagesc(abs(interleaved2complex(cPtr.Value))./abs(ctrue))

calllib('libltfat','ltfat_rtdgtreal_done_d',p);
calllib('libltfat','ltfat_rtidgtreal_done_d',pinv);

%norm(long2fir(gdtrue,gl) - gdPtr.Value)

function cframes = signal2frames(f,gl,a,M)

[Ls,~] = size(f);
L=dgtlength(Ls,a,M);
N = L/a;
f = postpad(f,L);

cframes = zeros(gl,N,1);

idxrange = -floor(gl/2):ceil(gl/2)-1;

for n=0:N-1
    idx = mod(n*a + idxrange,L) + 1;
    cframes(:,n+1) = f(idx);
end

function f = frames2signal(cframes,gl,a)

N = size(cframes,2);
L = N*a;
f = zeros(L,1);

idxrange = -floor(gl/2):ceil(gl/2)-1;
for n=0:N-1
    idx = mod(n*a + idxrange,L) + 1;
    f(idx) = f(idx) + cframes(:,n+1);
end


function [ Z, H, dnorm] = cnmf ( X, k, y, varargin )
% Matrix sizes
% X: m x n
% Z: m x num_of_components
% H: num_of_components x num_of_components

% Process optional arguments
pnames = {'z0' 'h0' 'bUpdateH' 'maxiter' 'nonlinearity_function', 'TolFun'};

% Do SVD initialisation of the init components

if 1
    [z0, h0] = NNDSVD(abs(X), k, 0);
else
    z0 = rand(size(X, 1), k);
    h0 = rand(k, size(X,2));
end

dflts  = {z0, h0, 1, 300, @(x) x, 1e-5};

[z0, h0, bUpdateH, max_iter, g, tolfun] = ...
        internal.stats.parseArgs(pnames,dflts,varargin{:});

% X =>  % p x n
Z = z0; % p x k

A = ind2vec([y; [length(y)+1:size(X, 2)]']')';
H = max(abs(A' * pinv(h0)), eps); % c x k

for i = 1:max_iter
    if bUpdateH
        numer = A' * X' * Z;
        H = H .* (numer ./ (((A' * A) * H * (Z' * Z)) + eps(numer)));
    end

    numer = X * A * H;
    Z = Z .* (numer ./  (Z * (H' * A') * (A * H) + eps(numer)));

    if mod(i, 10) == 0 || mod(i+1, 10) == 0
        s = X - Z * H' * A';
        dnorm = sqrt(sum(s(:).^2));

        if mod(i+1, 10) == 0
            dnorm0 = dnorm;
            continue
        end

%         if mod(i, 100) == 0
            display(sprintf('...CNMF iteration #%d out of %d, error: %f\n', i, max_iter, dnorm));
%         end

%         if exist('dnorm0')
%             assert(dnorm <= dnorm0, sprintf('Rec. error increasing! From %f to %f. (%d)', dnorm0, dnorm, k));
%         end

        % Check for convergence
        if exist('dnorm0') && dnorm0-dnorm <= tolfun*max(1,dnorm0)
            display(sprintf('Stopped at %d: dnorm: %f, dnorm0: %f', i, dnorm, dnorm0));
            break;
        end

    end
end

H = A * H;
H = H(length(y) +1 : end, :)';


close all
clear

%% topology graph
D = [1,-1,0,0,0,0,0,0,0,-1,0,1;
    -1,0,0,0,0,0,1,-1,0,0,0,0;
    0,1,-1,0,0,0,0,0,1,0,0,0;
    0,0,0,0,0,1,-1,0,0,1,-1,0;
    0,0,1,-1,0,0,0,0,0,0,1,-1;
    0,0,0,0,1,-1,0,0,-1,0,0,0;
    0,0,0,1,-1,0,0,1,0,0,0,0];
L = D*D';
H = D';

%% dimensions
[n,m] = size(D);
d = 2;
% r represents P(r)
r = [2,0;
    1,1;
    1,-1;
    0,1;
    0,-1;
    -1,1;
    -1,-1];
% P represents \bar P(r)
P = [r,ones(n,1)];
% edge set
edge = mod(reshape(find(D~=0),2,m),n);
edge(edge==0) = n;

%% target formation
figure
for i=1:m
    plot(r(edge(:,i),1),r(edge(:,i),2),'k','linewidth',2); hold on
end
for i=1:n
    plot(r(i,1),r(i,2),'.','markersize',50);
end
axis([-2 2 -2 2]);

%% trajectory
figure
via = [0,0;
    5,0;
    10,0;
    10,-5;
    10,-10;
    5,-10;
    0,-10];
for j=1:size(via,1)
    if ~mod(j,2)
        if j==4
            T1 = diag([0.1,1]);
        else
            T1 = diag([1,0.1]);
        end
    else
        T1 = eye(2);
    end
    T2 = rot2(-pi/2*floor((j-1)/2));
    ra(:,:,j) = r*T2'*T1'+via(j,:);
    qvia(j,:) = [vec(T1*T2)',via(j,:)];
    for i=1:m
        plot(ra(edge(:,i),1,j),ra(edge(:,i),2,j),'k','linewidth',2); hold on
    end
end
[qr,dqr,ddqr,tr] = mstraj_(qvia,ones(1,6),0.1,0.2);
A = reshape(qr(1,1:4)',[2,2]);
b = qr(1,5:6)';
xr = r*A'+b';
% for j=1:length(tr)
%     dA = reshape(dqr(j,1:4)',[2,2]);
%     db = dqr(j,5:6)';
%     dxr = r*dA'+db';
%     xr = xr+dxr*0.1;
%     plot(xr(1,1),xr(1,2),'bo');
%     plot(xr(2,1),xr(2,2),'go');
%     plot(xr(3,1),xr(3,2),'mo');
%     drawnow
% end

%% calculate stress matrix
E = [];
for i=1:n
    E = [E;P'*H'*diag(H(:,i))];
end
[U,S,V] = svd(P);
% the order in S: from big to small
U1 = U(:,1:d+1);
U2 = U(:,d+2:end);
% in fact z is exactly the same as omega given in Fig. 3
z = null(E);
% check if all(svd(U2'*H'*diag(z)*H*U2)>0), then
Omega = H'*diag(z)*H;

%% initial position
x = rand(n,d)*4-2;
dx = zeros(size(x));
% figure
for i=1:m
    fig_edge(i) = plot(x(edge(:,i),1),x(edge(:,i),2),'k','linewidth',2); hold on
end
for i=1:n
    fig_node(i) = plot(x(i,1),x(i,2),'.','markersize',50);
end
axis([-2 12 -12 2]);

%% simulation
dt = 0.05;
loop = 0;
video_on = true;
for t=tr(1):dt:tr(end)
    loop = loop+1;
    dq = interp1(tr,dqr,t);
    dA = reshape(dq(1:4)',[2,2]);
    db = dq(5:6)';
    dxr = r*dA'+db';
    % test leader
    alpha = 1;
%     dx = dxr-alpha*(x-xr);
    % followers apply control law (11)
    D = H';
    gamma = diag(Omega);
    for i=4:n
        err_sum = [0,0];
        edge_ind = find(D(i,:)~=0);
        for k=edge_ind
            node_ind = find(D(:,k)~=0);
            j = node_ind(node_ind~=i);
            err_sum = err_sum+z(k)*(x(i,:)-x(j,:)-dx(j,:));
        end
        dx(i,:) = -1/gamma(i)*err_sum;
    end
    % first 3 agents are leaders
    dx(1:3,:) = dxr(1:3,:)-alpha*(x(1:3,:)-xr(1:3,:));
    % update states
    x = x+dt*dx;
    xr = xr+dt*dxr;
    pos_data(:,:,loop) = x;
    % update figure
    for i=1:m
        set(fig_edge(i),'xdata',x(edge(:,i),1),'ydata',x(edge(:,i),2));
    end
    for i=1:n
        set(fig_node(i),'xdata',x(i,1),'ydata',x(i,2));
    end
    % video
    if video_on
        frame(loop) = getframe(gcf);
    end
    drawnow
end
% write video
if video_on
    savevideo('affine_maneuver',frame);
end
figure
t_data = tr(1):dt:tr(end);
xpos_data = squeeze(pos_data(:,1,:));
ypos_data = squeeze(pos_data(:,2,:));
plot3(kron(ones(n,1),t_data)',xpos_data',ypos_data');
xlabel('time/s');ylabel('x/m');zlabel('y/m');grid


% Author: Jai Juneja (adapted from SLAM course by Joan Sola)
% Date: 12/02/2013
%
% For a point p_g in the global frame, determine the range-bearing
% measurement y in the robot's local frame. This first requires a
% transformation of the point p_g to the robot's local frame to give p_r.
% This is then converted to a range and bearing vector with getMeasurement.
%
% Inputs:
%   r = [x y alpha]'    :   Robot frame
%   p_g = [pg_x pg_y]'  :   Point in global frame
%
% Outputs:
%   y = [d a]           :   Range-bearing sensor measurement
%   Optional:
%   Y_r                 :   Jacobian of y wrt. r
%   Y_pg                :   Jacobian of y wrt. p_g

function [y, Y_r, Y_pg] = scanPoint(r, p_g)

    if nargout == 1
        p_r = transToLocal(r, p_g);
        y = getMeasurement(p_r);    % Obtain range-bearing measurement y of p_r
    else
        % Compute Jacobians:
        [p_r, PR_r, PR_pg] = transToLocal(r, p_g);
        [y, Y_pr] = getMeasurement(p_r);

        % From the chain rule, we deduce:
        Y_r = Y_pr * PR_r;
        Y_pg = Y_pr * PR_pg;
    end
end


function [mopt, vopt, avals, mvals, vvals] = ...
    scimat_optimal_intersecting_plane(scimat, m0, v0, params)
% SCIMAT_OPTIMAL_INTERSECTING_PLANE  Optimise intersection plane for SCIMAT
% segmentation mask.
%
% [MOPT, VOPT, AVALS, MVALS, VVALS] = ...
%      scimat_optimal_intersecting_plane(SCIMAT, M0, V0, PARAMS)
%
%   This function computes the plane that intersects a SCIMAT segmentation
%   mask in a way that minimizes the segmentation area intersected by the
%   plane. That is, in some sense in finds the plane more orthogonal to the
%   segmented volume.
%
%   (Note that the area is computed on the convex hull of the plane
%   intersection with the volume.)
%
%   SCIMAT is the struct with the volume that we want to intersect (see
%   "help scimat" for details).
%
%   M0 is the rotation centroid. This centroid will not change.
%
%   V0 is a 3-vector that describes the normal vector to the initial
%   intersecting plane. By default, the initial plane is horizontal.
%
%   PARAMS is a struct with optimisation parameters:
%
%   * PARAMS.TYPE is a string. 'local' (default) means that the
%   intersected area will be minimised using multidimensional unconstrained
%   nonlinear minimization (Nelder-Mead, fminsearch). 'global' means that
%   area values will be systematically computed over a range of plane
%   inclination values.
%
%   * PARAMS.RAD can be a scalar or 2-vector:
%
%     scalar:   RAD is the radius of a 2D smoothing disk. The 2D image
%               obtained from intersecting the 3D volume by the plane at
%               each optimization iteration will be smoothed out using the
%               disk before computing the area.
%     2-vector: The smoothing element is a 3D ball. Instead of smoothing
%               a 2D image at each iteration, the whole 3D image volume is
%               smoothed with the ball at the beginning. RAD(1) is the ball
%               radius in the XY-plane. RAD(2) is the ball height.
%
%   * PARAMS.RANGE (global optimisation only): Angular range. The azimuth
%     of V0 will be changed RANGE(1) rad in any direction to compute
%     area values. The elevation of V0 will be changed RANGE(2) rad.
%     (Default RANGE(1) = RANGE(2) = 0.5236 rad = 30º).
%
%   * PARAMS.N (global optimisation only): Number of azimuth (N(1)) or
%   elevation (N(2)) samples. (Default N(1) = N(2) = 61).
%
%   MOPT is the centroid of the optimal intersection.
%
%   VOPT is the normal vector to the optimal plane.
%
%   Note that because M0 and MOPT are both contained in the optimal plane,
%   the duples (M0, VOPT) and (MOPT, VOPT) define the same optimal plane.
%
%   AVALS is a vector with the record of area values from the optimization.
%
%   MVALS is a volume with the coordinates of M at the different tested
%   values. If optimisation is 'local', then MVALS is a matrix where each
%   column is the centroid at each iteration step. If optimisation is
%   'global', then MVALS is a volume where MVALS(T,P,:) is the centroid for
%   the normal vector VVALS(T,P,:).
%
%   VVALS is a volume like MVALS, only for the normal vectors.

% Author(s): Ramon Casero <rcasero@gmail.com>, Vicente Grau
% Copyright © 2010, 2014 University of Oxford
% Version: 0.2.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

%% Checks and initialization

% check arguments
narginchk(2, 4) ;
nargoutchk(0, 5) ;

% defaults
if (nargin < 3 || isempty(v0))
    v0 = [0 0 1];
end
if (nargin < 4 || isempty(params))
    params.type = 'local';
    params.rad = [];
    params.range = [30 30] / 180 * pi;
    params.n = [61 61];
    se = [];
end
if (~isfield(params, 'type'))
    params.type = 'local';
end
if (~isfield(params, 'rad'))
    params.rad = [];
    se = [];
end
if (~isfield(params, 'range'))
    params.range = [30 30] / 180 * pi;
end
if (~isfield(params, 'n'))
    params.n = [61 61];
end

% prevent user entering rotation matrix instead of initial vector by
% mistake
if (size(v0, 2) ~= 1 || size(v0, 1) ~= 3)
    error('V0 must be a column 3-vector')
end

% remove the dummy dimension and convert image data to double
scimat = scimat_squeeze(scimat, true);

% convert radius from real world size into number of pixels
params.rad = round(...
    params.rad ./ [scimat.axis(1:length(params.rad)).spacing]);

% create disk for dilation/erosion if we are going to smooth in 2D
if (length(params.rad) == 1)
    se = strel('disk', params.rad);
end

% 3D smoothing of the segmentation edges
if (length(params.rad) == 2)
    se = strel('ball', params.rad(1), params.rad(2));
    scimat.data = imdilate(scimat.data, se);
    scimat.data = imerode(scimat.data, se);
end

% generate 3D grid of coordinates
[x, y, z] = scimat_ndgrid(scimat);

% % DEBUG: compute intersection of SCIMAT volume with the initial plane
% % (if you want to visualize the image as in Seg3D, you need to do 'axis
% % xy')
% im = scimat_intersect_plane(scimat, m0, v0, x, y, z);

%% Optimisation of the intersection area

% convert Cartesian coordinates into spherical coordinates (length has to
% be one); note: we use phi for azimuth, and theta for elevation, contrary
% to Matlab's naming convention
[phi0, theta0] = cart2sph(v0(1), v0(2), v0(3));

% group spherical coordinates into vector
alpha0 = [phi0, theta0];

% init variables to keep track of the evolution of area values in the
% optimisation
avals = [];
mvals = [];
vvals = [];

% local or global optimisation
if strcmp(params.type, 'local')

    % run optimisation to find minimum area; note that v0 is the only
    % variable optimised, but the rest (scimat, x, y, z, m, rad, se) are
    % available to segmented_area_of_intersection() because the latter is a
    % subfunction
    alpha = fminsearch(@segmented_area_of_intersection, alpha0);

    % convert result from spherical to Carterian coordinates
    [aux1 aux2 aux3] = sph2cart(alpha(1), alpha(2), 1.0);
    vopt = [aux1 aux2 aux3];

    % final centroid of the intersecting plane
    mopt = mvals(:, end);

elseif strcmp(params.type, 'global')

    % interval of azimuth angle values, phi \in [-180º, 180º] or \in
    % [0, 360º]
    phimin = phi0 - abs(params.range(1));
    phimax = phi0 + abs(params.range(1));

    % interval of elevation angle values, theta \in [-90º, 90º]
    thmin = max(theta0 - abs(params.range(2)), -pi/2);
    thmax = min(theta0 + abs(params.range(2)), pi/2);

    % sample angle intervals
    phivals = linspace(phimin, phimax, params.n(1));
    thetavals = linspace(thmin, thmax, params.n(2));

    % create matrices to save outputs; note that for each area value we
    % need to save a 3-vector with the rotation point, and a 3-vector with
    % the normal plane
    avals = zeros(length(thetavals), length(phivals));
    mvals = zeros(length(thetavals), length(phivals), 3);
    vvals = zeros(length(thetavals), length(phivals), 3);

    % compute area for each combination of elevation and azimuth angles
    for T = 1:length(thetavals) % elevation
        for P = 1:length(phivals) % azimuth
            [a, mnew, v] = segmented_area_of_intersection(...
                [phivals(P) thetavals(T)]);

            % put values in output matrices
            avals(T, P) = a;
            mvals(T, P, :) = mnew;
            vvals(T, P, :) = v;

        end
    end

    % find minimum area
    [foo, idx] = min(avals(:));

    % convert linear index to multiple subscripts
    [T, P] = ind2sub(size(avals), idx);

    % output optimal plane
    mopt = squeeze(mvals(T, P, :));
    vopt = squeeze(vvals(T, P, :));

else
    error(['Optimisation type not implemented: ' params.type])
end

    %% Objective function (the function we are trying to minimise)

    % rotate plane, intersect with image, and compute segmented area
    function [a, mnew, v] = segmented_area_of_intersection(alpha)

        % convert spherical to Carterian coordinates
        [aux1 aux2 aux3] = sph2cart(alpha(1), alpha(2), 1.0);
        v = [aux1 aux2 aux3]';

        % vector cannot be zero
        if (norm(v) == 0)
            error('Normal vector to plane cannot be (0,0,0)')
        end

        % this function cannot deal with vertical planes, because of a
        % singularity
        if (v(3) == 0)
            error('Intersecting plane cannot be vertical')
        end

        % compute intersection of plane with volume
        [im, zp, xp, yp] = scimat_intersect_plane(scimat, m0, v, x, y, z);

        % 2D smoothing of the segmentation edges
        if (length(params.rad) == 1)
            im = imdilate(im, se);
            im = imerode(im, se);
        end

%         % DEBUG: plot rotated plane
%         hold off
%         plot3(xp(:), yp(:), zp(:), '.r')

        % find segmented voxels in the 2D cut
        idx = find(im);

        % get coordinates of segmented voxels
        xps = xp(idx);
        yps = yp(idx);
        zps = zp(idx);

%         % DEBUG: visualize intersection projected onto horizontal plane
%         hold off
%         imagesc(xp(:), yp(:), im > 0)
%         hold on
%         % DEBUG: compute and plot convex hull
%         idx2 = convhull(xps, yps);
%         vxs = xps(idx2);
%         vys = yps(idx2);
%         plot(vxs, vys, 'w')
%         xlabel('x (m)')
%         ylabel('y (m)')
%         pause

        % compute a rotation matrix from the Cartesian system to the
        % rotated plane
        rotmat = vec2rotmat(v);

        % we are now seeing the rotated plane projected onto the horizontal
        % plane, i.e. we see the segmentation mask in perspective.
        % In order to see the true area of the segmentation mask, we need
        % to change the system of coordinates so that the rotated plane
        % becames the XY plane

        % first, move segmented points so that centroid is at (0,0,0)...
        xps = xps - m0(1);
        yps = yps - m0(2);
        zps = zps - m0(3);

        % ...second, make the rotated plane horizontal, by inverting the
        % rotation...
        xyzps = [xps(:) yps(:) zps(:)] * rotmat;
        xps = xyzps(:, 1);
        yps = xyzps(:, 2);
        zps = xyzps(:, 3);

        % if everything has gone alright, then the z-coordinate of xyzp
        % should be zero (+numerical errors), because the rotated plane is
        % now the XY plane
        assert(abs(min(zps)) < 1e-10)
        assert(abs(max(zps)) < 1e-10)

%         % DEBUG: visualize segmentation mask in real world coordinates
%         hold off
%         plot(xps + m0(1), yps + m0(2), 'r*')
%         axis ij

        % compute convex hull (reuse idx2): note convex hull coordinates
        % are on projected space
        idx2 = convhull(xps, yps);
        vxs = xps(idx2);
        vys = yps(idx2);

        % compute x-,y-coordinates centroid and area of polygon
        [mnew, a] = polycenter(vxs, vys);
        mnew(3) = 0;

        % the centroid is now on projected coordinates, but we need to put
        % it back on the real world coordinates
        mnew = rotmat * mnew';
        mnew = (mnew' + m0)';

        % for the global algorithm, values are recorded in a different way
        if strcmp(params.type, 'local')
            % kept track of optimisation evolution
            avals = [avals a];
            vvals = [vvals v];
            mvals = [mvals mnew];
        end

    end
end


% LOGDET_KRON_LDLCHOL - Computes the log-determinant of a matrix using its
% Cholesky factor from SuiteSparse.
%
% Y = LOGDET_LDLCHOL(LD1,LD2)
%
% Computes the log-determinant of the matrix X whose Cholesky factor is
% the triangular matrix U. U can be lower or upper triangular.
%
% More generally, X could be a product of two instances of U and/or U'. For
% instance, X=U'*U' or X=U'*U.

% Last modified 2010-11-09
% Copyright (c) Jaakko Luttinen

function y = logdet_ldlchol(LD1,LD2)

N1 = size(LD1,1);
N2 = size(LD2,1);

y = N2*logdet_tri(LD1) + N1*logdet_tri(LD2);


%% Machine Learning Online Class - Exercise 3 | Part 2: Neural Networks

%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  linear exercise. You will need to complete the following functions
%  in this exericse:
%
%     lrCostFunction.m (logistic regression cost function)
%     oneVsAll.m
%     predictOneVsAll.m
%     predict.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%

%% Initialization
clear ; close all; clc

%% Setup the parameters you will use for this exercise
input_layer_size  = 400;  % 20x20 Input Images of Digits
hidden_layer_size = 25;   % 25 hidden units
num_labels = 10;          % 10 labels, from 1 to 10
                          % (note that we have mapped "0" to label 10)

%% =========== Part 1: Loading and Visualizing Data =============
%  We start the exercise by first loading and visualizing the dataset.
%  You will be working with a dataset that contains handwritten digits.
%

% Load Training Data
fprintf('Loading and Visualizing Data ...\n')

load('ex3data1.mat');
m = size(X, 1);

% Randomly select 100 data points to display
sel = randperm(size(X, 1));
sel = sel(1:100);

displayData(X(sel, :));

fprintf('Program paused. Press enter to continue.\n');
pause;

%% ================ Part 2: Loading Pameters ================
% In this part of the exercise, we load some pre-initialized
% neural network parameters.

fprintf('\nLoading Saved Neural Network Parameters ...\n')

% Load the weights into variables Theta1 and Theta2
load('ex3weights.mat');

%% ================= Part 3: Implement Predict =================
%  After training the neural network, we would like to use it to predict
%  the labels. You will now implement the "predict" function to use the
%  neural network to predict the labels of the training set. This lets
%  you compute the training set accuracy.

pred = predict(Theta1, Theta2, X);

fprintf('\nTraining Set Accuracy: %f\n', mean(double(pred == y)) * 100);

fprintf('Program paused. Press enter to continue.\n');
pause;

%  To give you an idea of the network's output, you can also run
%  through the examples one at the a time to see what it is predicting.

%  Randomly permute examples
rp = randperm(m);

for i = 1:m
    % Display
    fprintf('\nDisplaying Example Image\n');
    displayData(X(rp(i), :));

    pred = predict(Theta1, Theta2, X(rp(i),:));
    fprintf('\nNeural Network Prediction: %d (digit %d)\n', pred, mod(pred, 10));

    % Pause with quit option
    s = input('Paused - press enter to continue, q to exit:','s');
    if s == 'q'
      break
    end
end


classdef EIMEGO < ALGORITHM
% <multi> <real/integer> <expensive>
% Expected improvement matrix based efficient global optimization
% InfillCriterionIndex --- 1 --- infill criterion index number

%------------------------------- Reference --------------------------------
% D. Zhan, Y. Cheng, and J. Liu, Expected improvement matrix-based infill
% criteria for expensive multiobjective optimization, IEEE Transactions on
% Evolutionary Computation, 2017, 21(6): 956-975.
%------------------------------- 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".
%--------------------------------------------------------------------------

% This function is written by Dawei Zhan

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            % 1 for Euclidean distance-based EIM criterion (default)
            % 2 for Maximin distance-based EIM criterion
            % 3 for Hypervolume-based EIM criterion
            InfillCriterionIndex = Algorithm.ParameterSet(1);

            %% Generate the initial design points
            % number of design variables
            D = Problem.D;
            % number of objective functions
            M = Problem.M;
            % number of initial design points
            N  = 11*D-1;
            % generate initial design points using Latin Hypercube sampling
            PopDec = repmat(Problem.upper-Problem.lower,N,1).*UniformPoint(N,D,'Latin')+repmat(Problem.lower,N,1);
            % calculate initial design points
            Population   = Problem.Evaluation(PopDec);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                % scale the objective values to 0 and 1
                PopDec = Population.decs;
                PopObj = Population.objs;
                N  = size(PopDec,1);
                PopObjScaled = (PopObj-repmat(min(PopObj),N,1))./(repmat(max(PopObj),N,1)-repmat(min(PopObj),N,1));
                % bulid Kriging models for all the objective functions
                KrigingModel = cell(1,M);
                for i = 1 : M
                    KrigingModel{i}= dacefit(PopDec,PopObjScaled(:,i),'regpoly0','corrgauss',1*ones(1,D),0.001*ones(1,D),1000*ones(1,D));
                end
                % select one candidate with the maximum EIM value using GA
                PopDec     = InfillSamplingEIM(Problem,KrigingModel,PopObjScaled,InfillCriterionIndex);
                Population = [Population,Problem.Evaluation(PopDec)];
            end
        end
    end
end


function f = perform_mesh_smoothing(face,vertex,f,options)

% perform_mesh_smoothing - smooth a function defined on a mesh by averaging
%
%   f = perform_mesh_smoothing(face,vertex,f,options);
%
%   Smooth a function f on a width of options.niter_averaging vertices.
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
naver = getoptions(options, 'niter_averaging', 1);
type = getoptions(options, 'averaging_type', 'combinatorial');

if nargin<3
    f = [];
end
if isempty(f)
    f = vertex;
end
if size(f,1)<size(f,2)
    f = f';
end
[vertex,face] = check_face_vertex(vertex,face);

if size(f,2)>1
    for i=1:size(f,2)
        f(:,i) = perform_mesh_smoothing(face,vertex,f(:,i),options);
    end
    return;
end

n = max(face(:));

% compute normalized averaging matrix
if strcmp(type, 'combinatorial')
    %add diagonal
    W = triangulation2adjacency(face) + speye(n);
    D = spdiags(full(sum(W,2).^(-1)),0,n,n);
    W = D*W;
else
    options.normalize=1;
    W = compute_mesh_weight(vertex,face,type,options);
end

% do averaging to smooth the field
for k=1:naver
    f = W*f;
end


function obj=stat_bin2d(obj,varargin)
% stat_bin2d() Makes 2D bins of X and Y data and displays count
%
% Parameters as 'name',value pairs:
% - 'nbins': Array in the form of [nxbins nybins] to set the
% number of bins in each dimension
% - 'edges': Cell in the form of {[x__edges] [y_edges]} to set
% custom bin edges for each dimension
% - 'geom': Set how results are displayed. 'image' uses a
% heatmap (default), 'contour' uses a contour plot. 'point'
% uses circles of varying size.

p=inputParser;
my_addParameter(p,'nbins',[30 30]);
my_addParameter(p,'edges',{});
my_addParameter(p,'geom','image'); %contour

parse(p,varargin{:});

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

function hndl=my_bin2d(obj,draw_data,params)

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

if isempty(params.edges)
    [N,C] = hist3([shiftdim(x),shiftdim(y)],params.nbins);
else
    [N,C] = hist3([shiftdim(x),shiftdim(y)],'Edges',params.edges);

    obj.plot_lim.minx(obj.current_row,obj.current_column)=params.edges{1}(1);
    obj.plot_lim.maxx(obj.current_row,obj.current_column)=params.edges{1}(end);
    obj.plot_lim.miny(obj.current_row,obj.current_column)=params.edges{2}(1);
    obj.plot_lim.maxy(obj.current_row,obj.current_column)=params.edges{2}(end);

    %Put values on the upper edges as if they were in the last
    %bin
    N(:,end-1)=N(:,end-1)+N(:,end);
    N(end-1,:)=N(end-1,:)+N(end,:);

    %Remove upper edge
    N(:,end)=[];
    N(end,:)=[];

end

obj.results.stat_bin2d{obj.result_ind,1}.edges=C;
obj.results.stat_bin2d{obj.result_ind,1}.counts=N;

switch params.geom
    case 'contour'

        [~,hndl]=contour(C{1},C{2},N',5,'Color',draw_data.color);

    case 'image'

        if ~obj.continuous_color_options.active
            obj.continuous_color_options.active = true;
        end

        Nr=reshape(N',1,numel(N));
        sel=Nr>0;
        %sel=true(size(Nr));

        if isempty(params.edges)
            %Get polygon half widths
            wx=(C{1}(2)-C{1}(1))/2;
            wy=(C{2}(2)-C{2}(1))/2;

            %Generate polygon edges
            [X,Y] = meshgrid(C{1},C{2});

            X=reshape(X,1,numel(X));
            Y=reshape(Y,1,numel(Y));

            patchesx=[X(sel)-wx ; X(sel)-wx ; X(sel)+wx ; X(sel)+wx ];
            patchesy=[Y(sel)-wy ; Y(sel)+wy ; Y(sel)+wy ; Y(sel)-wy ];

        else
            [Xs, Ys]=meshgrid(params.edges{1}(1:end-1),params.edges{2}(1:end-1));
            [Xe, Ye]=meshgrid(params.edges{1}(2:end),params.edges{2}(2:end));

            Xs=reshape(Xs,1,numel(Xs));
            Ys=reshape(Ys,1,numel(Ys));
            Xe=reshape(Xe,1,numel(Xe));
            Ye=reshape(Ye,1,numel(Ye));

            patchesx=[Xs(sel) ; Xs(sel) ; Xe(sel) ; Xe(sel)];
            patchesy=[Ys(sel) ; Ye(sel) ; Ye(sel) ; Ys(sel)];

            %If we have varied-size patches (we use rounding to
            %get away with numerical issues of unique
            if length(unique(round(diff(params.edges{1})*1e10)))>1 || length(unique(round(diff(params.edges{2})*1e10)))>1
                %Correct values by the area of each patch ?
                Nr=Nr./((Xe-Xs).*(Ye-Ys));
                obj.aes_names.color='Count/area';
            else
                obj.aes_names.color='Count';
            end

        end

        %patchesz=[Nr(sel) ; Nr(sel) ; Nr(sel) ; Nr(sel) ];

        %p=patch(patchesx,patchesy,patchesz,Nr(sel));
        hndl=patch(patchesx,patchesy,Nr(sel));
        set(hndl,'edgeColor','none')

        %Store color values
        obj.plot_lim.maxc(obj.current_row,obj.current_column)=max(Nr);
        obj.plot_lim.minc(obj.current_row,obj.current_column)=min(Nr);

    case 'point'
        [X,Y] = meshgrid(C{1},C{2});
        X=reshape(X,1,numel(X));
        Y=reshape(Y,1,numel(Y));
        Nr=reshape(N',1,numel(N));
        sel=Nr>0;
        %hndl=point_patch(X(sel),Y(sel),Nr(sel)*(C{1}(2)-C{1}(1))/30,draw_data.color,20,ratio);
        hndl=scatter(X(sel),Y(sel),Nr(sel)*10,draw_data.marker,'MarkerEdgeColor',draw_data.color,'MarkerFaceColor','none');
end

obj.results.stat_bin2d{obj.result_ind,1}.handle=hndl;

end


% [func, fx] = mcmc_init_hamiltonian(x_init, get_logpdf, get_dlogpdf, ...
% epsilon, L, func_x)
%
% epsilon is the parameter to the exponential distribution for sampling
% the momentum
%
% L is the number of simulation steps
function [func, fx] = mcmc_init_hamiltonian(x_init, get_logpdf, get_dlogpdf, ...
                                            epsilon, L, func_x)

if nargin < 6
  func_x = @func_x_default;
end

x_current = x_init;
[fx_current, dfx_current] = func_x(x_current);
logpdf_current = get_logpdf(fx_current);
dlogpdf_current = get_dlogpdf(dfx_current);

func = @hamiltonian;
fx = fx_current;

  function [x, fx] = hamiltonian(varargin)

  x = x_current;

  p = normrnd(0,1,size(x_current));
  p_current = p;

  lp_current = logpdf_current(varargin{:});
  dlp_current = dlogpdf_current(varargin{:});

% $$$   mycheckgrad(@chkgrad, x, 1e-6)
% $$$
% $$$     function [y,dy] = chkgrad(x)
% $$$     [fx, dfx] = func_x(x);
% $$$     f = get_logpdf(fx);
% $$$     df = get_dlogpdf(dfx);
% $$$     y = f(varargin{:});
% $$$     dy = df(varargin{:});
% $$$     end

  % Random step size
  e = exprnd(epsilon);

  % Make a half step for momentum at the beginning
  p = p + 0.5*e*dlp_current;

  for l=1:L
    x = x + e * p;

    if any(isnan(x))
      break;
    end

    [fx, dfx] = func_x(x);
    dlogpdf = get_dlogpdf(dfx);
    dlp = dlogpdf(varargin{:});
    if l < L
      p = p + e * dlp;
    else
      logpdf = get_logpdf(fx);
      lp = logpdf(varargin{:});
    end
  end

  if all(~isnan(x))

    % Make a half step for momentum at the end
    p = p + 0.5*e*dlp;

    % Negate momentum to make the proposal symmetric
    p = -p;

    if log(rand()) < ( (lp - 0.5*(p'*p)) - ...
                       (lp_current - 0.5*(p_current'*p_current)) )
      % Accept
      disp('Accept hamiltonian')
      x_current = x;
      fx_current = fx;
      dfx_current = dfx;
      logpdf_current = logpdf;
      dlogpdf_current = dlogpdf;
    else
      disp('Reject hamiltonian')
    end

  else
      disp('Reject hamiltonian')
  end

  x = x_current;
  fx = fx_current;
  end

end

function [fx, dfx] = func_x_default(x)
fx = x;
dfx = x;
end


%Normalizes homogenous coordinates such that the last coordinate is 1
%You can use any dimension of the vectors
%
%Author:    Christian Wengert,
%           Institute of Computer Vision
%           Swiss Federale Institute of Technology, Zurich (ETHZ)
%           wengert@vision.ee.ethz.ch
%           www.vision.ee.ethz.ch/~cwengert/
%
%Input:     x       unnormalized homogenous coordinates
%
%Output     y       normalized homogenous coordinates
%
%Syntax:    y = normalizeHomogenousCoordinates(x)

function y = normalizeHomogenousCoordinates(x)

    %get dimension of array
    ni = size(x,1);
    nj = size(x,2);
    %go through
    for j=1:1:nj
        y(:,j) = x(:,j)./x(ni,j);
    end
    y(ni,:) = ones(1,nj);


%% test modulate for all oasis functions.
col = {[0 114 178],[0 158 115], [213 94 0],[230 159 0],...
    [86 180 233], [204 121 167], [64 224 208], [240 228 66]}; % colors
plot_cvx = false;

%% example 7:  constrained-foopsi, AR1
g = 0.95;         % AR coefficient
noise = .3;
T = 3000;
framerate = 30;
firerate = 0.5;
b = 0;              % baseline
N = 1;              % number of trials
seed = 13;          % seed for genrating random variables
[y, true_c, true_s] = gen_data(g, noise, T, framerate, firerate, b, N, seed);

% cvx solution
[c_cvx, s_cvx] = constrained_foopsi(y, g, noise);
% case 1: all parameters are known
[c_oasis, s_oasis] = deconvolveCa(y, 'ar1', g, 'constrained', 'sn', noise);  %#ok<*ASGLU>

figure('name', 'constrained-FOOPSI, AR1, known: g, sn', 'papersize', [15, 4]);
plot_cvx = true;
show_results;
plot_cvx = false;

% case 2: nothing is known, estimate g with auto-correlation method
[c_oasis, s_oasis,options] = deconvolveCa(y, 'ar1', 'constrained');

fprintf('true gamma:        %.3f\n', g);
fprintf('estimated gamma:   %.3f\n', options.pars);

figure('name', 'FOOPSI, AR1, estimated: g, sn', 'papersize', [15, 4]);
show_results;

% case 3: nothing is know, estimate g with auto-correlation method first
% and then update it to minimize the RSS
[c_oasis, s_oasis, options] = deconvolveCa(y, 'ar1', 'constrained', ...
    'optimize_pars');

fprintf('true gamma:        %.3f\n', g);
fprintf('estimated gamma:   %.3f\n', options.pars);

figure('name', 'FOOPSI, AR1, estimated: g, sn, update:g', 'papersize', [15, 4]);
show_results;

% case 4: nothing is know, estimate g with auto-correlation method first
% and then update it to minimize the RSS, the baseline is also unknown
true_b = 0.5;
[c_oasis, s_oasis, options] = deconvolveCa(y+true_b, 'ar1', g,...
    'constrained','optimize_b', 'sn', noise);
fprintf('true gamma:        %.3f\n', g);
fprintf('estimated gamma:   %.3f\n', options.pars);
fprintf('true b:       %.3f\n', true_b);
fprintf('estimated b:       %.3f\n', options.b);
fprintf('tuning parameter:  %.3f\n', options.lambda);

figure('name', 'FOOPSI, AR1, estimated: g, sn, lambda', 'papersize', [15, 4]);
show_results;

% case 5: nothing is know, estimate g with auto-correlation method first
% and then update it to minimize the RSS, the baseline is also unknown
true_b = 0.5;
[c_oasis, s_oasis, options] = deconvolveCa(y+true_b, 'ar1',...
    'constrained','optimize_b', 'optimize_pars');
fprintf('true gamma:        %.3f\n', g);
fprintf('estimated gamma:   %.3f\n', options.pars);
fprintf('estimated b:       %.3f\n', options.b);
fprintf('tuning parameter:  %.3f\n', options.lambda);

figure('name', 'FOOPSI, AR1, estimated: g, sn, lambda, update:g', 'papersize', [15, 4]);
show_results;

%%


function [L,E,EMAP] = crouzeix_raviart_cotmatrix(V,F)
  % CROUZEIX_RAVIART_COTMATRIX Compute the Crouzeix-Raviart cotangent
  % Laplacian matrix where we use test functions define at edge midpoints.
  %
  % See for example "Discrete Quadratic Curvature Energies" [Wardetzky, Bergou,
  % Harmon, Zorin, Grinspun 2007]
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by element-size list of triangle indices
  % Outputs:
  %   L  #E by #E edge-based sparse cotangent matrix
  %   E  #E by 2 list of edges
  %
  % Examples:
  %   % mesh in (V,F)
  %   [Lcr,E,EMAP] = crouzeix_raviart_cotmatrix(V,F);
  %   [Mcr,E,EMAP] = crouzeix_raviart_massmatrix(V,F);
  %   [Ucr,~] = eigs(Lcr,Mcr,5,'sm');
  %   % Convert between edge values and vertex values
  %   E2V = sparse(E(:),repmat(1:size(E,1),1,2)',1,size(V,1),size(E,1));
  %   E2V = bsxfun(@rdivide,E2V,sum(E2V,2));
  %   V2E = sparse(E(:),repmat(1:size(E,1),1,2)',1,size(V,1),size(E,1))';
  %   V2E = bsxfun(@rdivide,V2E,sum(V2E,2));
  %   tsurf(F,[V(:,1:2) E2V*Ucr(:,end-1)])
  %
  %   % Display discontinous solution
  %   FF = reshape(1:numel(F),size(F));
  %   VV = V(F,:);
  %   EMAP = reshape(EMAP,size(F));
  %   A = sparse( ...
  %     [FF FF FF], ...
  %     EMAP(:,[1 2 3 2 3 1 3 1 2]), ...
  %     repmat(-[1 1 1 -1 -1 -1 -1 -1 -1],size(F,1),1), ...
  %     size(VV,1), ...
  %     size(E,1));
  %   tsurf(FF,[VV(:,1:2) A*Ucr(:,end-1)])
  %
  %
  %
  % See also: edge_laplacian, is_boundary_edge, crouzeix_raviart_massmatrix,
  %   cotmatrix
  %

  %% check for non-manifold edges
  %S = statistics(V,F,'Fast',true);
  %if S.num_nonmanifold_edges > 0
  %  error(sprintf('There are %d non-manifold edges',num_nonmanifold_edges));
  %end

  % number of vertices
  n = size(V,1);
  % number of elements
  m = size(F,1);
  % simplex size
  ss = size(F,2);
  switch ss
  case 3
    % Compute cotangents: seems to be 0.5*C
    C = 2*cotangent(V,F);

    allE = [F(:,[2 3]);F(:,[3 1]);F(:,[1 2])];
    % Map each face-edge to a unique edge
    F2E = reshape(1:3*m,m,3);
    % Lij = -2 cot aij
    %
    % o
    % |\
    % | \
    % |  \
    % |   \
    % i    \
    % |     \
    % |      \
    % |αij    \
    % o----j---o
    %
    %
    LI =  F2E(:, [1 2 3 2 3 1        1 2 3 2 3 1]);
    LJ =  F2E(:, [2 3 1 1 2 3        1 2 3 2 3 1]);
    LV = 2*[-C(:,[3 1 2 3 1 2]) C(:,[3 1 2 3 1 2])];

    % Map duplicate edges to first instance
    [E,~,EMAP] = unique(sort(allE,2),'rows');

    assert(all(size(LI)==size(LJ)));
    assert(all(size(LI)==size(LV)));
    L = sparse(EMAP(LI),EMAP(LJ),LV,size(E,1),size(E,1));
  case 4
    % tets
    T = F;
    C = -2*cotangent(V,T);
    allF = [ ...
      T(:,2) T(:,4) T(:,3); ...
      T(:,1) T(:,3) T(:,4); ...
      T(:,1) T(:,4) T(:,2); ...
      T(:,1) T(:,2) T(:,3); ...
      ];
    % Map each element-face to a unique face
    T2F = reshape(1:4*m,m,4);
    % Lij = -2 lij cot αij
    LI = T2F(:,[1 4 4 4 2 3 2 1 2 3 3 1        1 4 4 4 2 3 2 1 2 3 3 1]);
    LJ = T2F(:,[2 1 2 3 3 1 1 4 4 4 2 3        1 4 4 4 2 3 2 1 2 3 3 1]);
    LV = [-C(:,[3 4 5 6 1 2 3 4 5 6 1 2]) C(:,[3 4 5 6 1 2 3 4 5 6 1 2])];
    % Map duplicate facets to first instance
    [F,~,FMAP] = unique(sort(allF,2),'rows');

    assert(all(size(LI)==size(LJ)));
    assert(all(size(LI)==size(LV)));
    L = sparse(FMAP(LI),FMAP(LJ),LV,size(F,1),size(F,1));
    E = F;
    EMAP = FMAP;

  otherwise
    error(['Simplex size ' num2str(ss) ' unsupported']);
  end

end


%% Plotting Rotations
%
%%
%
% The function <quaternion.scatter.html scatter> allows you to visualize a
% rotation in Rodriguez space.

% define 100 random rotations
rot = rotation.rand(100)

% and plot the Rodriguez space
scatter(rot)


% StackExchange Signal Processing Q79314
% https://dsp.stackexchange.com/questions/79314
% Image Segmentation Using Deep Learning
% References:
%   1.
% Remarks:
%   1.  B
% TODO:
% 	1.  C
% Release Notes
% - 1.0.000     27/11/2021
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Simulation Constants

SIG_TYPE_RAND       = 1;
SIG_TYPE_GAUSSIAN   = 2;

%% Simulation Parameters

% Signal
signalType      = SIG_TYPE_GAUSSIAN;
numSamplesSig   = 201; %<! Make sure it is odd
gaussianStd     = 2;

% Support (Recieved Signal)
numSamples  = 300;

% Noise
minNoiseStd = 0;
maxNoiseStd = 1;
numNoisePts = 50;
numRealizations = 200; %<! Per noise level

%% Generate Data

signalRadius = ceil(4 * gaussianStd);
vSignalSupport = linspace(-signalRadius, signalRadius, numSamplesSig);
vSignal = exp(-(vSignalSupport .^ 2) ./ (2 * gaussianStd));
vSignal = single(vSignal(:));

figure(); plot(vSignalSupport, vSignal);

vNoiseLevel = linspace(minNoiseStd, maxNoiseStd, numNoisePts);
vNoiseLevel = single(vNoiseLevel);

signalRadius    = (numSamplesSig - 1) / 2;
numClasses      = numSamples - numSamplesSig + 1;

mData   = zeros(numSamples, numClasses * numNoisePts * numRealizations, 'single');
vLabels = zeros(numClasses * numNoisePts * numRealizations, 1);

vNoise  = zeros(numSamples, 1, 'single');
vX      = zeros(numSamples, 1, 'single');
sigIdx = 1;

for ii = 1:numClasses
    startIdx = ii;
    endIdx = startIdx + numSamplesSig - 1;
    vX(startIdx:endIdx) = vSignal;
    for jj = 1:numNoisePts
        noiseStd = vNoiseLevel(jj);
        for kk = 1:numRealizations
            vNoise(:) = noiseStd * randn(numSamples, 1, 'single');
            mData(:, sigIdx) = vX + vNoise;
            vLabels(sigIdx) = ii;
            sigIdx = sigIdx + 1;
        end
    end
    vX(startIdx:endIdx) = 0;
end

% ii = 20001; figure(); plot(mData(:, ii)); title(vLabels(ii));

% Verify data by generating it without noise.
% Then:
% [vA, vB] = max(mData, [], 1);
% vC = vB - signalRadius;
% isequal(vC(:), vLabels)

%% Save Data

save('Data', 'vSignal', 'mData', 'vLabels', 'subStreamNumber');

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function fe2dx_r_fast_test ( )

%*****************************************************************************80
%
%% FE2DX_R_FAST_TEST tests the FE2DX_R_FAST code.
%
%  Discussion:
%
%    This function sets all parameter values and initial condition information
%    necessary to execute the "fast" version of the fe2dx_r algorithm.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    28 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Reference:
%
%    Marcus R Garvie, John Burkardt, Jeff Morgan,
%    Simple Finite Element Methods for Approximating Predator-Prey Dynamics
%    in Two Dimensions using MATLAB,
%    Submitted to Bulletin of Mathematical Biology, 2014.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FE2DX_R_FAST_TEST:\n' );
  fprintf ( 1, '  Test the FE2DX_R_FAST function\n' );
  fprintf ( 1, '  which applies Robin boundary conditions as it\n' );
  fprintf ( 1, '  approximates a solution to a predator-prey system.\n' );
%
%  Set the parameters.
%
  alpha = 0.4;
  beta = 2.0;
  gamma = 0.6;
  delta = 1.0;
%
%  Use T=150.0 for normal run.
%  Use T=0.50 for a "quick" run that might take 15 minutes of computing.
%
% T = 150.0;
  T = 0.50;
  delt = 1.0 / 384.0;
  k1 = 0.01;
  k2 = 0.01;

  t = tic;
  fe2dx_r_fast ( alpha, beta, gamma, delta, T, delt, @u0f, @v0f, k1, k2 );
  t = toc ( t );

  fprintf ( 1, '  Execution took %10.2g minutes \n', t / 60.0 );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FE2DX_R_FAST_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end

function value = u0f ( x, y )

%*****************************************************************************80
%
%% U0F evaluates the initial condition for U.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    26 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location in the region.
%
%    Output, real VALUE, the initial condition for U at (X,Y).
%
  value = 6.0 / 35.0 - 2.0E-07 * ( x - 0.1 * y - 225.0 ) * ( x - 0.1 * y - 675.0 );

  return
end

function value = v0f ( x, y )

%*****************************************************************************80
%
%% V0F evaluates the initial condition for V.
%
%  Licensing:
%
%    Copyright (C) 2014 Marcus R. Garvie.
%    See 'mycopyright.txt' for details.
%
%  Modified:
%
%    26 April 2014
%
%  Author:
%
%    Marcus R. Garvie.
%
%  Parameters:
%
%    Input, real X, Y, a location in the region.
%
%    Output, real VALUE, the initial condition for V at (X,Y).
%
  value = 116.0 / 245.0 - 3.0E-05 * ( x - 450.0 ) - 1.2E-04 * ( y - 150.0 );

  return
end


function im = fft2c(d)
% Function performs a centered fft2
im = fftshift(fft2(fftshift(d)));


function exact = p01_exact ( )

%*****************************************************************************80
%
%% P01_EXACT returns the exact integral for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the value of the integral.
%
  omega = 1.0;

  exact = sqrt ( pi ) * exp ( - omega * omega );

  return
end


function d=disteusq(x,y,mode,w)
%DISTEUSQ calculate euclidean, squared euclidean or mahanalobis distance D=(X,Y,MODE,W)
%
% Inputs: X,Y         Vector sets to be compared. Each row contains a data vector.
%                     X and Y must have the same number of columns.
%
%         MODE        Character string selecting the following options:
%                         'x'  Calculate the full distance matrix from every row of X to every row of Y
%                         'd'  Calculate only the distance between corresponding rows of X and Y
%                              The default is 'd' if X and Y have the same number of rows otherwise 'x'.
%                         's'  take the square-root of the result to give the euclidean distance.
%
%         W           Optional weighting matrix: the distance calculated is (x-y)*W*(x-y)'
%                     If W is a vector, then the matrix diag(W) is used.
%
% Output: D           If MODE='d' then D is a column vector with the same number of rows as the shorter of X and Y.
%                     If MODE='x' then D is a matrix with the same number of rows as X and the same number of columns as Y'.
%

[nx,p]=size(x); ny=size(y,1);
if nargin<3 | isempty(mode) mode='0'; end
if any(mode=='d') | (mode~='x' & nx==ny)
   nx=min(nx,ny);
   z=x(1:nx,:)-y(1:nx,:);
   if nargin<4
      d=sum(z.*conj(z),2);
   elseif min(size(w))==1
      wv=w(:).';
      d=sum(z.*wv(ones(size(z,1),1),:).*conj(z),2);
   else
      d=sum(z*w.*conj(z),2);
   end
else
   if p>1
      if nargin<4
         z=permute(x(:,:,ones(1,ny)),[1 3 2])-permute(y(:,:,ones(1,nx)),[3 1 2]);
         d=sum(z.*conj(z),3);
      else
         nxy=nx*ny;
         z=reshape(permute(x(:,:,ones(1,ny)),[1 3 2])-permute(y(:,:,ones(1,nx)),[3 1 2]),nxy,p);
         if min(size(w))==1
            wv=w(:).';
            d=reshape(sum(z.*wv(ones(nxy,1),:).*conj(z),2),nx,ny);
         else
            d=reshape(sum(z*w.*conj(z),2),nx,ny);
         end
      end
   else
      z=x(:,ones(1,ny))-y(:,ones(1,nx)).';
      if nargin<4
         d=z.*conj(z);
      else
         d=w*z.*conj(z);
      end
   end
end
if any(mode=='s')
   d=sqrt(d);
end


function q = Qconj(p)

% QCONJ   Quaternion conjugate
%
%   Q = QCONJ(P)returns the quaternion conjugate
%    q = CONJQ(p) returns the quaternion conjugate of the quaternion P. P
%    is a 4-vector representing a quaternion or an array 4*N (column i
%    represents quaternion i) where N is the number of quaternions. If P =
%    p0 + vecp, then P* = p0-vecp. Q has the same size as P.
%
% See also DQCONJ

sp = size(p);
if sp == [1 4], p = p'; sp = size(p); end

% wrong size
if sp(1) ~= 4
    error('DualQuaternion:Qconj:wrongsize',...
        '%d rows in array p. It should be 4.',sp(1));
end

q = p;
q(2:4,:) = - q(2:4,:);


%% Define Feedforward Convolutional Code
SNR_dB = 3
LM = 1600         % Mesg Length excluding pre-determined bits for starting & ending trellis at state 0
TREL_TYPE = 'Feedback'  % {'Feedback', 'Feedforward'}

%% Matlab Generator Polynomial convention
% Build a binary number representation by placing a 1 in each spot where a connection line from the shift register feeds into the adder,
% and a 0 elsewhere. The leftmost spot in the binary number represents the current input, while the rightmost spot represents the
% oldest input that still remains in the shift register.

if strcmp(TREL_TYPE, 'Feedback')
%     CL = 2		% Rate 1/2 Feedback encoder with 2 states
%     GenPoly0_1by2 = 3 % in octal
%     GenPoly1_1by2 = 2
%     FeedBackCoef = [1,1];  % For computing i/p bits needed to terminate trellis at state =0.
%     TREL = poly2trellis(CL, [GenPoly0_1by2, GenPoly1_1by2], GenPoly0_1by2)

%% Rate 1/2 Feedback encoder with 8-states used in 3GPP cellular 3G/4G standard
    CL = 4
    GenPoly0_1by2 = 13 % in octal
    GenPoly1_1by2 = 15
    FeedBackCoef = [1,0,1,1];% For computing i/p bits needed to terminate trellis at state =0.
    TREL = poly2trellis(CL, [GenPoly0_1by2, GenPoly1_1by2], GenPoly0_1by2)

else
    %% Rate 1/3 Feedforward encoder with 4 states
%     CL = 3; % constraint length
%     GenPoly0_third = 4; % in octal
%     GenPoly1_third = 5;
%     GenPoly2_third = 7;
%     TREL = poly2trellis( CL, [GenPoly0_third, GenPoly1_third, GenPoly2_third])

%     CL = 4 % constraint length
%     GenPoly0_1by4 = 13 % in octal
%     GenPoly1_1by4 = 15
%     GenPoly2_1by4 = 15
%     GenPoly3_1by4 = 17
%     TREL = poly2trellis( CL, [GenPoly0_1by4, GenPoly1_1by4, GenPoly2_1by4, GenPoly3_1by4])

end
LM = LM + 2*(CL-1); % space for start & tail bits

%% Verify trellis-structure is OK
[isok, status] = istrellis(TREL)

randn('state', sum(100*clock)); % initialize to random state

%% The encoder is assumed to have both started and ended at the all-zeros state
%% Ensure msg is s.t. trellis starts at "0" state and ends at "0" state.
%%  Generate Random binary stream & encode
if strcmp(TREL_TYPE, 'Feedback')
    msg1(1 : CL-1) = zeros(1, CL-1); % 1st (CL-1) bits must be 0
    msg1(CL : LM -CL+1) = randint(LM -2*CL +2, 1, 2)' ; % Random data
% 	msg1(CL : LM -CL+1) = [0 1];

    % Encode first part of msg, recording final state for later use.
    [cenc_o1, final_state1] = convenc(msg1, TREL);

    % Rest of msg depends on final_state1; it makes trellis terminate at final_state=0.
    bvec_fs = bitget(final_state1, (CL-1) : -1 : 1); % All possible binary-vectors of length EncoderN
    for idx = 1 : (CL-1)
        msg2(idx) = rem( [0, bvec_fs]*FeedBackCoef', 2);
        bvec_fs = [0, bvec_fs(1: (end-1))];
    end
%     msg2(1 : CL -1) = bvec_fs; % Last (CL-1) bits depend on state at time=(LM-CL+1)
    [cenc_o2, final_state] = convenc(msg2, TREL, final_state1);

    msg = [msg1, msg2]; clear msg1 msg2
    [cenc_o] = [cenc_o1, cenc_o2]; clear cenc_o1 cenc_o2
    final_state
else % Feedforward
    msg(1 : CL-1) = zeros(1,CL-1); % 1st (CL-1) bits must be 0
    msg(CL : LM -CL+1) = randint(LM -2*CL +2, 1, 2)' ; % Random data
    msg(LM -CL + 2 : LM) = zeros(CL-1, 1) % Last (CL-1) bits must be 0
    [cenc_o, final_state] = convenc(msg, TREL);
end

if final_state ~= 0
    disp('trellis not terminated properly: check last (CL-1) bits of mesg')
    return
end

%%	Map to BPSK constellation: bit0 -> 1, bit1 -> -1
signal_power = 1;
chan_in = (1 - 2*cenc_o)*sqrt(signal_power);

%%	Generate and add Gaussian-noise mean=0, variance= noise_power
noise_power = signal_power / 10^(SNR_dB/10);
noise = randn(size(chan_in))*sqrt(noise_power);
chan_o = chan_in + noise;
% chan_o = chan_in;

Es = log2(TREL.numOutputSymbols)*signal_power;
No = noise_power;
Lc = 4*Es/No

%%	Soft-Decision decoding, map to decision values
soft_in = chan_o;
% decodeds = vitdec_htm(TREL, soft_in');

[LLR, Alpha, Beta] = LogMAPdecode_htm(TREL, chan_o, Lc);

decd_msg = (1 - sign(LLR))/2;
err_vec = abs(decd_msg - msg);

sprintf('# of Bit-Errors = %d out of %d info-bits ', sum(abs(err_vec)), LM -CL -2)


function B=unimodalcrossproducts(XtX,XtY,Bold)

%UNIMODALCROSSPRODUCTS
% Solves the problem min|Y-XB'| subject to the columns of
% B are unimodal and nonnegative. The algorithm is iterative and
% only one iteration is given, hence the solution is only improving
% the current estimate
%
% I/O B=unimodalcrossproducts(XtX,XtY,Bold)
% Modified from unimodal.m to handle crossproducts in input 1999
% Reference
% Bro and Sidiropoulos, "Journal of Chemometrics", 1998, 12, 223-247.

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% This program is free software; you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation; either version 2 of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more details.
% You should have received a copy of the GNU General Public License along with
% this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
% Street, Fifth Floor, Boston, MA  02110-1301, USA.

B=Bold;
F=size(B,2);
for f=1:F
   xty = XtY(f,:)-XtX(f,[1:f-1 f+1:F])*B(:,[1:f-1 f+1:F])';
   beta=pinv(XtX(f,f))*xty;
   B(:,f)=ulsr(beta',1);
end

function [b,All,MaxML]=ulsr(x,NonNeg);

% ------INPUT------
%
% x          is the vector to be approximated
% NonNeg     If NonNeg is one, nonnegativity is imposed
%
%
%
% ------OUTPUT-----
%
% b 	     is the best ULSR vector
% All 	     is containing in its i'th column the ULSRFIX solution for mode
% 	     location at the i'th element. The ULSR solution given in All
%            is found disregarding the i'th element and hence NOT optimal
% MaxML      is the optimal (leftmost) mode location (i.e. position of maximum)
%
% ___________________________________________________________
%
%
%               Copyright 1997
%
% Nikos Sidiroupolos
% University of Maryland
% Maryland, US
%
%       &
%
% Rasmus Bro
% Royal Veterinary & Agricultural University
% Denmark
%
%
% ___________________________________________________________

% This file uses MONREG.M

x=x(:);
I=length(x);
xmin=min(x);
if xmin<0
  x=x-xmin;
end

% THE SUBSEQUENT
% CALCULATES BEST BY TWO MONOTONIC REGRESSIONS

% B1(1:i,i) contains the monontonic increasing regr. on x(1:i)
[b1,out,B1]=monreg(x);

% BI is the opposite of B1. Hence BI(i:I,i) holds the monotonic
% decreasing regression on x(i:I)
[bI,out,BI]=monreg(flipud(x));
BI=flipud(fliplr(BI));

% Together B1 and BI can be concatenated to give the solution to
% problem ULSR for any modloc position AS long as we do not pay
% attention to the element of x at this position

All=zeros(I,I+2);
All(1:I,3:I+2)=B1;
All(1:I,1:I)=All(1:I,1:I)+BI;
All=All(:,2:I+1);
Allmin=All;
Allmax=All;
% All(:,i) holds the ULSR solution for modloc = i, disregarding x(i),

iii=find(x>=max(All)');
b=All(:,iii(1));
b(iii(1))=x(iii(1));
Bestfit=sum((b-x).^2);
MaxML=iii(1);
for ii=2:length(iii)
  this=All(:,iii(ii));
  this(iii(ii))=x(iii(ii));
  thisfit=sum((this-x).^2);
  if thisfit<Bestfit
    b=this;
    Bestfit=thisfit;
    MaxML=iii(ii);
  end
end

if xmin<0
  b=b+xmin;
end

% Impose nonnegativity
if NonNeg==1
  if any(b<0)
    id=find(b<0);
    % Note that changing the negative values to zero does not affect the
    % solution with respect to nonnegative parameters and position of the
    % maximum.
    b(id)=zeros(size(id))+0;
  end
end

function [b,B,AllBs]=monreg(x);

% Monotonic regression according
% to J. B. Kruskal 64
%
% b     = min|x-b| subject to monotonic increase
% B     = b, but condensed
% AllBs = All monotonic regressions, i.e. AllBs(1:i,i) is the
%         monotonic regression of x(1:i)
%
%
% Copyright 1997
%
% Rasmus Bro
% Royal Veterinary & Agricultural University
% Denmark
% rb@kvl.dk
%

I=length(x);
if size(x,2)==2
   B=x;
else
   B=[x(:) ones(I,1)];
end

   AllBs=zeros(I,I);
   AllBs(1,1)=x(1);
   i=1;
   while i<size(B,1)
      if B(i,1)>B(min(I,i+1),1)
          summ=B(i,2)+B(i+1,2);
          B=[B(1:i-1,:);[(B(i,1)*B(i,2)+B(i+1,1)*B(i+1,2))/(summ) summ];B(i+2:size(B,1),:)];
          OK=1;
          while OK
             if B(i,1)<B(max(1,i-1),1)
                summ=B(i,2)+B(i-1,2);
                B=[B(1:i-2,:);[(B(i,1)*B(i,2)+B(i-1,1)*B(i-1,2))/(summ) summ];B(i+1:size(B,1),:)];
                i=max(1,i-1);
             else
                OK=0;
             end
          end
          bInterim=[];
          for i2=1:i
             bInterim=[bInterim;zeros(B(i2,2),1)+B(i2,1)];
          end
          No=sum(B(1:i,2));
          AllBs(1:No,No)=bInterim;
      else
          i=i+1;
          bInterim=[];
          for i2=1:i
             bInterim=[bInterim;zeros(B(i2,2),1)+B(i2,1)];
          end
          No=sum(B(1:i,2));
          AllBs(1:No,No)=bInterim;
      end
  end

  b=[];
  for i=1:size(B,1)
    b=[b;zeros(B(i,2),1)+B(i,1)];
 end


function [Zin,ip_it] = plain_foopsi(H,D,I_est,eps)

% solves argmin ||X-H||^2 subject to D*X>=0 with an interior point method
% using I_est as the initial value and eps as the initial barrier weight

ln = length(H);
step_back_frac = 0.5;
iter = 0;
if nargin == 2
    I_est = 1e-3*ones(ln,1);
    eps = 1;
end
Zin = I_est(:);

if nargin == 3
    eps = 1;
end
while eps>1e-5
    n = D*Zin;
    nnd = 10;
    E = norm(Zin-H)^2 - eps*sum(log(D*Zin));
    grad = 2*(Zin-H) - eps*D'*(n.^(-1));
    Hs = 2*speye(ln) + eps*D'*spdiags(n.^(-2),0,ln,ln)*D;
    while nnd/2>1
        iter = iter + 1;
        Z_dir = -Hs\grad;
        hit = -n./(D*Z_dir);
        if all(hit<0)
            s = 1;
        else
            s = min(1,.9*min(hit(hit>=0)));
        end
        E_new = E; s = s/step_back_frac;
        x_dg = grad'*Z_dir;
        while E_new > E + 0.25*s*x_dg
            s=s*step_back_frac;
            Z_new = Zin + s*Z_dir;
            n = D*Zin;
            E_new = norm(Z_new-H)^2 - eps*sum(log(D*Z_new));
        end
        %E = E_new;
        Zin = Zin + s*Z_dir;
        nnd = -x_dg;
        E = norm(Zin-H)^2 - eps*sum(log(D*Zin));
        n = D*Zin;
        grad = 2*(Zin-H) - eps*D'*(n.^(-1));
        Hs = 2*speye(ln) + eps*D'*spdiags(n.^(-2),0,ln,ln)*D;
        %disp(nnd)
    end
    eps = eps/10;
end
%fprintf('Interior point method converged after %i iterations \n',iter);
ip_it = iter;


function node_boundary = triangulation_order3_boundary_node ( node_num, ...
  triangle_num, triangle_node )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER3_BOUNDARY_NODE indicates which nodes are on the boundary.
%
%  Discussion:
%
%    This routine is given a triangulation, an abstract list of triples
%    of nodes.  It is assumed that the nodes in each triangle are listed
%    in a counterclockwise order, although the routine should work
%    if the nodes are consistently listed in a clockwise order as well.
%
%    It is assumed that each edge of the triangulation is either
%    * an INTERIOR edge, which is listed twice, once with positive
%      orientation and once with negative orientation, or;
%    * a BOUNDARY edge, which will occur only once.
%
%    This routine should work even if the region has holes - as long
%    as the boundary of the hole comprises more than 3 edges!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(3,TRIANGLE_NUM), the nodes that make up the
%    triangles.  These should be listed in counterclockwise order.
%
%    Output, logical NODE_BOUNDARY(NODE_NUM), is TRUE if the node
%    is on a boundary edge.
%
  m = 2;
  n = 3 * triangle_num;
%
%  Set up the edge array.
%
  edge(1:2,               1:  triangle_num) = triangle_node(1:2,1:triangle_num);
  edge(1:2,  triangle_num+1:2*triangle_num) = triangle_node(2:3,1:triangle_num);
  edge(1,  2*triangle_num+1:3*triangle_num) = triangle_node(3,  1:triangle_num);
  edge(2,  2*triangle_num+1:3*triangle_num) = triangle_node(1,  1:triangle_num);
%
%  In each column, force the smaller entry to appear first.
%
  e1(1:n) = min ( edge(1:2,1:n) );
  e2(1:n) = max ( edge(1:2,1:n) );

  edge(1,1:n) = e1(1:n);
  edge(2,1:n) = e2(1:n);
%
%  Ascending sort the column array.
%
  edge = ( sortrows ( edge' ) )';
%
%  Records which appear twice are internal edges and can be ignored.
%
  node_boundary(1:node_num) = 0;

  j = 0;

  while ( j < 3 * triangle_num )

    j = j + 1;

    if ( j == 3 * triangle_num )
      node_boundary(edge(1:m,j)) = 1;
    elseif ( all ( edge(1:m,j) == edge(1:m,j+1) ) )
      j = j + 1;
    else
      node_boundary(edge(1:m,j)) = 1;
    end

  end

  return
end


%-fanDTasia ToolBox------------------------------------------------------------------
% This Matlab script is part of the fanDTasia ToolBox: a Matlab library for Diffusion
% Weighted MRI (DW-MRI) Processing, Diffusion Tensor (DTI) Estimation, High-order
% Diffusion Tensor Analysis, Diffusion Kurtosis Imaging (DKI) Estimation,
% Tensor ODF estimation, Visualization and more.
%
% A Matlab Tutorial on DW-MRI can be found in:
% http://www.cise.ufl.edu/~abarmpou/lab/fanDTasia/tutorial.php
%
%-CITATION---------------------------------------------------------------------------
% If you use this software please cite the following papers on 4th-order tensors:
% 1) A. Barmpoutis et al. "Diffusion Kurtosis Imaging: Robust Estimation from DW-MRI
%    using Homogeneous Polynomials", In the Proceedings of ISBI, 2011, pp. 262-265.
% 2) A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion
%    Tensors of any order with Symmetric Positive-Definite Constraints",
%    In the Proceedings of ISBI, 2010, pp. 1385-1388.
%
%-DESCRIPTION------------------------------------------------------------------------
% This demo script shows how to compute the Diffusion Kurtosis Coefficients from a given
% Diffusion-Weighted MRI dataset. The method guarantees that the estimated diffusivity as
% well as the Diffusion Tensor are positive semi-definite, using the method
% in Sec. 4.2 of the ISBI'11 paper. Here the given demo dataset consists of 5 voxels,
% 30 gradient directions x 2 b-values from the real brain dataset used in the ISBI'11 paper.
%
%-USE--------------------------------------------------------------------------------
% [D,W]=DEMO_DKI_Estimation_Method2_v2;
%
% D: is a vector with the computed Unique Coefficients of the 2nd-order Diffusion Tensor
% W: is a vector with the computed Unique Coefficients of the 4th-order Kurtosis Tensor
%
%-DISCLAIMER-------------------------------------------------------------------------
% You can use this source code for non commercial research and educational purposes
% only without licensing fees and is provided without guarantee or warrantee expressed
% or implied. You cannot repost this file without prior written permission from the
% authors. If you use this software please cite the following papers:
% 1) A. Barmpoutis et al. "Diffusion Kurtosis Imaging: Robust Estimation from DW-MRI
%    using Homogeneous Polynomials", In the Proceedings of ISBI, 2011.
% 2) A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion
%    Tensors of any order with Symmetric Positive-Definite Constraints",
%    In the Proceedings of ISBI, 2010, pp. 1385-1388.
%
%-AUTHOR-----------------------------------------------------------------------------
% Angelos Barmpoutis, PhD
% Digital Worlds Institute
% University of Florida, Gainesville, FL 32611, USA
% angelbar at ufl dot edu
%------------------------------------------------------------------------------------
function [DKI_D,DKI_W]=DEMO_DKI_Estimation_Method2_v2

%%% DATA OPENING %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%open data files
[S,B]=openFDT('real_data_5voxels.fdt');

%S0 signal, no diffusion weighting
S0=S(:,:,:,1);

%acquisition shell 1, 30 orientations
S_1real=S(:,:,:,[2:31]);
GradientOrientations_1=B([2:31],[1:3]);
BValue_1=B(2,4);

%acquisition shell 2, 30 orientations
S_2real=S(:,:,:,[32:61]);
GradientOrientations_2=B([32:61],[1:3]);
BValue_2=B(32,4);

%%% OPTIONAL: ADD RICIAN NOISE TO THE DATA FOR QUANTITATIVE EVALUATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%

stdv=input('Do you want to add Rician noise to the data for validation? \n If yes, then give the Std. Dev. (e.g: 0.1), otherwise type 0.\n STD.DEV.:');
S_1=S_1real;
for i=1:size(S_1real,4)
    S_1(:,:,:,i)=sqrt((S_1real(:,:,:,i)+stdv*S0.*randn(size(S_1real,1),size(S_1real,2),size(S_1real,3))).^2+(stdv*S0.*randn(size(S_1real,1),size(S_1real,2),size(S_1real,3))).^2);
end
S_2=S_2real;
for i=1:size(S_2real,4)
    S_2(:,:,:,i)=sqrt((S_2real(:,:,:,i)+stdv*S0.*randn(size(S_2real,1),size(S_2real,2),size(S_2real,3))).^2+(stdv*S0.*randn(size(S_2real,1),size(S_2real,2),size(S_2real,3))).^2);
end
%your data can have as many shells and bvalues you want

%%% INITIALIZATION - COMPUTING AUXILIARY DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Construct a constant set of polynomial coefficients C
C_order2=constructSetOf81Polynomials(2)'; %computes C from section 5.1 (ISBI'10)
C_order4=constructSetOf321Polynomials(4)'; %computes C from section 5.1 (ISBI'10)
A_1=D4toDKImatrix(BValue_1);A_1=[A_1(:,[1:6])*C_order2 A_1(:,6+[1:15])];%Computes the matrix A from Table 1 (ISBI'11)
A_2=D4toDKImatrix(BValue_2);A_2=[A_2(:,[1:6])*C_order2 A_2(:,6+[1:15])];%Computes the matrix A from Table 1 (ISBI'11)

%shell 1
G_1_order2=constructMatrixOfMonomials(GradientOrientations_1, 2); %computes G from section 5.1 (ISBI'10)
G_1_order4=constructMatrixOfMonomials(GradientOrientations_1, 4); %computes G from section 5.1 (ISBI'10)
Gbig_1=[-BValue_1*G_1_order2 BValue_1*BValue_1/6*G_1_order4];  %all the monomials for orders 2 and 4.

%shell 2
G_2_order2=constructMatrixOfMonomials(GradientOrientations_2, 2); %computes G from section 5.1 (ISBI'10)
G_2_order4=constructMatrixOfMonomials(GradientOrientations_2, 4); %computes G from section 5.1 (ISBI'10)
Gbig_2=[-BValue_2*G_2_order2 BValue_2*BValue_2/6*G_2_order4];  %all the monomials for orders 2 and 4.

%your data can have as many shells and bvalues you want
Gbig=[Gbig_1; Gbig_2]; %all the monomials for orders 2 and 4 and bvalues b1 and b2.

%%%%%% MAIN LOOP - METHOD: LINEAR FITTING - NO CONSTRAINTS %%%%%%%%%%%%%%%%%%%%%%%%%
for x=1:size(S,1)
    for y=1:size(S,2)
        for z=1:size(S,3)

            logS_1=log(squeeze(S_1(x,y,z,:))/S0(x,y,z));
            logS_2=log(squeeze(S_2(x,y,z,:))/S0(x,y,z));

            %The following 2 steps implement the method in Sec. 4.2 of the ISBI'11 paper.
            %Step 1: Compute a positive-definite 4th-order tensor for each b-value
            D4_1=C_order4*lsqnonneg(-G_1_order4*C_order4, logS_1);%computes a positive-definite tensor for b1
            D4_2=C_order4*lsqnonneg(-G_2_order4*C_order4, logS_2);%computes a positive-definite tensor for b2

            %Step 2: Compute DKI from the positive definite 4th-order tensors.
            x1=zeros(size(C_order2,2),1); %the initialization is the zero vector
            x2=zeros(15,1); %the initialization is the zero vector
            for i=1:100 %we perform a kind of gradient descent for a number of iterations
                x1=lsqnonneg([A_1(:,[1:size(C_order2,2)]);A_2(:,[1:size(C_order2,2)])],[D4_1;D4_2]-[A_1(:,[size(C_order2,2)+1:size(A_1,2)]);A_2(:,[size(C_order2,2)+1:size(A_1,2)])]*x2);
                x2_new=pinv([A_1(:,[size(C_order2,2)+1:size(A_1,2)]);A_2(:,[size(C_order2,2)+1:size(A_1,2)])]) * ([D4_1;D4_2]-[A_1(:,[1:size(C_order2,2)]);A_2(:,[1:size(C_order2,2)])]*x1);
                %sum(abs(x2-x2_new)) %I just put this here for convergence check
                x2=x2_new;
            end
            dki=[C_order2*x1;x2];

            %Optional Validation if user adds noise to the data
            if stdv>0
                logS_1=log(squeeze(S_1real(x,y,z,:))/S0(x,y,z));
                logS_2=log(squeeze(S_2real(x,y,z,:))/S0(x,y,z));
                err(:,x,y,z)=abs(Gbig*dki-[logS_1; logS_2]);
            end

            %Store the data
            DKI_D(:,x,y,z)=dki([1:6]); %The 6 unique coefficients of the diffusion tensor D
            %If you want you can put the result in the form of a 3x3 matrix
            %D=[dki(6) dki(5)/2 dki(4)/2
            %   dki(5)/2 dki(3) dki(2)/2
            %   dki(4)/2 dki(2)/2 dki(1)];

            DKI_W(:,x,y,z)=dki([7:21]); %The 15 unique coefficients of the kurtosis tensor W
            %You can see which coefficient is which you can use the function: printTensor(DKI_W(:,x,y,z),4)
            % or if you want to plot a tensor or a tensor field as spherical functions
            % you have to download the plotTensors.m function developed by Angelos Barmpoutis, Ph.D.
            % and then uncomment the following lines.
            %
            % plotTensors(DKI_D(:,1,1,1),1,[321 1]); %3D ellipsoidal plot of D
            % plotTensors(DKI_W(:,1,1,1),1,[321 1]); %3D ellipsoidal plot of W

            %Optional Calculation of Dapp and Kapp
            Dapp(:,x,y,z) = G_1_order2*dki(1:6);
            Kapp(:,x,y,z) = (G_1_order4*dki(7:21))./((G_1_order2*dki(1:6)).^2);

        end
    end
end

%%%%%% OPTIONAL: PRINT RESULTS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
all_err=[];
all_Dapp=[];
all_Kapp=[];
counter=0;
meanD=zeros(6,1);
meanW=zeros(15,1);
for x=1:size(S,1)
    for y=1:size(S,2)
        for z=1:size(S,3)
            if stdv>0
               all_err=[all_err; err(:,x,y,z)];
            end
            all_Dapp=[all_Dapp; Dapp(:,x,y,z)];
            all_Kapp=[all_Kapp; Kapp(:,x,y,z)];
            meanD=meanD+DKI_D(:,x,y,z);
            meanW=meanW+DKI_W(:,x,y,z);
            counter=counter+1;
        end
    end
end
meanD=meanD/counter;
meanW=meanW/counter;

fprintf(1,'\n-----RESULTS:-----\n\n');
fprintf('Number of fitted voxels: %d\n',counter);
if stdv>0
    fprintf(1,'Fitting Error: %.4f (std. dev. %.4f)\n',mean(all_err),std(all_err));
end
fprintf(1,'Mean Dapp: %.4f (std. dev. %.4f)\n',mean(all_Dapp),std(all_Dapp));
fprintf(1,'Mean Kapp: %.4f (std. dev. %.4f)\n',mean(all_Kapp),std(all_Kapp));
fprintf(1,'\nMean Diffusion Tensor D:\n');
printTensor(meanD,2);
fprintf(1,'\nMean Kurtosis Tensor W:\n');
printTensor(meanW,4);

% If you want to plot a Diffusion or Kurtosis tensor as spherical functions
% you have to download the plotTensors.m function developed by Angelos Barmpoutis, Ph.D.
% and then uncomment the following lines.
%
% subplot(1,2,1)
% plotTensors(meanD,1,[321 1]);
% title('Mean Diffusion Tensor');
% subplot(1,2,2);
% plotTensors(meanW,1,[321 1]);
% title('Mean Kurtosis Tensor');

fprintf(1,'\nIf you use this software please cite the following papers on DKI and DTI estimation:\n');
fprintf(1,'1) A. Barmpoutis et al. "Diffusion Kurtosis Imaging: Robust Estimation from DW-MRI\n');
fprintf(1,'   using Homogeneous Polynomials", In the Proceedings of ISBI, 2011, pp. 262-265.\n');
fprintf(1,'2) A. Barmpoutis and B.C. Vemuri, "A Unified Framework for Estimating Diffusion Tensors\n');
fprintf(1,'   of any order with Symmetric Positive-Definite Constraints",\n');
fprintf(1,'   In the Proceedings of ISBI, 2010, pp. 1385-1388.\n');


%

function [f, df] = sampleEijOpt(x)
% optimization code

% param
vert = x(4:7);
w_res = x(8);

% box case (4 lines), three free parameters
x0 = x(1); y0 = x(2); % camera center
yc = x(3); % corner

% assume vertical lines are fixed
a_aob = (vert(2) - vert (1))/w_res*2*pi;
a_boc = (vert(3) - vert (2))/w_res*2*pi;
a_cod = (vert(4) - vert (3))/w_res*2*pi;
a_doa = (vert(1) + w_res - vert (4))/w_res*2*pi;

% energy
v_ao  = [x0 y0]; v_bo = [x0-1 y0]; v_co = [x0-1 y0 - yc]; v_do = [x0 y0-yc];
n_v_ao = norm(v_ao); n_v_bo = norm(v_bo); n_v_co = norm(v_co); n_v_do = norm(v_do);
b_aob = acos(dot(v_ao, v_bo)/n_v_ao/n_v_bo);
if det([v_ao;v_bo]) < 0
    b_aob = 2*pi - b_aob;
end
b_boc = acos(dot(v_bo, v_co)/n_v_bo/n_v_co);
if det([v_bo;v_co]) < 0
    b_boc = 2*pi - b_boc;
end
b_cod = acos(dot(v_co, v_do)/n_v_co/n_v_do);
if det([v_co;v_do]) < 0
    b_cod = 2*pi - b_cod;
end
b_doa = acos(dot(v_do, v_ao)/n_v_do/n_v_ao);
if det([v_do;v_ao]) < 0
    b_doa = 2*pi - b_doa;
end

f = (b_aob - a_aob)^2 + (b_boc - a_boc)^2 + (b_cod - a_cod)^2 + (b_doa - a_doa)^2;

% gradient
% x0
d_aob_x0 = (2*x0-1)*n_v_ao*n_v_bo + dot(v_ao, v_bo) * (x0*n_v_bo/n_v_ao + (x0-1)*n_v_ao/n_v_bo);
d_aob_x0 = d_aob_x0 * (-1/(sqrt(1-cos(b_aob)*cos(b_aob))+eps))/n_v_ao/n_v_ao/n_v_bo/n_v_bo;
if det([v_ao;v_bo]) < 0
    d_aob_x0 = -d_aob_x0;
end
d_aob_x0 = 2*(b_aob - a_aob) * d_aob_x0;

d_boc_x0 = 2*(x0-1)*n_v_bo*n_v_co + dot(v_bo, v_co) * ((x0-1)*n_v_co/n_v_bo + (x0-1)*n_v_bo/n_v_co);
d_boc_x0 = d_boc_x0 * (-1/(sqrt(1-cos(b_boc)*cos(b_boc))+eps))/n_v_bo/n_v_bo/n_v_co/n_v_co;
if det([v_bo;v_co]) < 0
    d_boc_x0 = -d_boc_x0;
end
d_boc_x0 = 2*(b_boc - a_boc) * d_boc_x0;

d_cod_x0 = (2*x0-1)*n_v_co*n_v_do + dot(v_co, v_do) * ((x0-1)*n_v_do/n_v_co + x0*n_v_co/n_v_do);
d_cod_x0 = d_cod_x0 * (-1/(sqrt(1-cos(b_cod)*cos(b_cod))+eps))/n_v_co/n_v_co/n_v_do/n_v_do;
if det([v_co;v_do]) < 0
    d_cod_x0 = -d_cod_x0;
end
d_cod_x0 = 2*(b_cod - a_cod) * d_cod_x0;

d_doa_x0 = 2*x0*n_v_do*n_v_ao +dot(v_do, v_ao) * (x0*n_v_ao/n_v_do + x0*n_v_do/n_v_ao);
d_doa_x0 = d_doa_x0 * (-1/(sqrt(1-cos(b_doa)*cos(b_doa))+eps))/n_v_do/n_v_do/n_v_ao/n_v_ao;
if det([v_do;v_ao]) < 0
    d_doa_x0 = -d_doa_x0;
end
d_doa_x0 = 2*(b_doa - a_doa) * d_doa_x0;

% y0
d_aob_y0 = 2*y0*n_v_ao*n_v_bo + dot(v_ao, v_bo) * (y0*n_v_bo/n_v_ao + y0*n_v_ao/n_v_bo);
d_aob_y0 = d_aob_y0 * (-1/(sqrt(1-cos(b_aob)*cos(b_aob))+eps))/n_v_ao/n_v_ao/n_v_bo/n_v_bo;
if det([v_ao;v_bo]) < 0
    d_aob_y0 = -d_aob_y0;
end
d_aob_y0 = 2*(b_aob - a_aob) * d_aob_y0;

d_boc_y0 = (2*y0-yc)*n_v_bo*n_v_co + dot(v_bo, v_co) * (y0*n_v_co/n_v_bo + (y0-yc)*n_v_bo/n_v_co );
d_boc_y0 = d_boc_y0/n_v_bo/n_v_bo/n_v_co/n_v_co;
if det([v_bo;v_co]) < 0
    d_boc_y0 = -d_boc_y0;
end
d_boc_y0 = 2*(b_boc - a_boc) * d_boc_y0;

d_cod_y0 = 2*(y0-yc)*n_v_co*n_v_do + dot(v_co, v_do) * ((y0-yc)*n_v_do/n_v_co + (y0-yc)*n_v_co/n_v_do);
d_cod_y0 = d_cod_y0 * (-1/(sqrt(1-cos(b_cod)*cos(b_cod))+eps))/n_v_co/n_v_co/n_v_do/n_v_do;
if det([v_co;v_do]) < 0
    d_cod_y0 = -d_cod_y0;
end
d_cod_y0 = 2*(b_cod - a_cod) * d_cod_y0;

d_doa_y0 = (2*y0-yc)*n_v_do*n_v_ao + dot(v_do, v_ao) * ((y0-yc)*n_v_ao/n_v_do + y0*n_v_do/n_v_ao);
d_doa_y0 = d_doa_y0 * (-1/(sqrt(1-cos(b_doa)*cos(b_doa))+eps))/n_v_do/n_v_do/n_v_ao/n_v_ao;
if det([v_do;v_ao]) < 0
    d_doa_y0 = -d_doa_y0;
end
d_doa_y0 = 2*(b_doa - a_doa) * d_doa_y0;

% yc
d_boc_yc = (-y0)*n_v_bo*n_v_co + dot(v_bo, v_co) * n_v_bo* (yc-y0)/n_v_co;
d_boc_yc = d_boc_yc/n_v_bo/n_v_bo/n_v_co/n_v_co;
if det([v_bo;v_co]) < 0
    d_boc_yc = -d_boc_yc;
end
d_boc_yc = 2*(b_boc - a_boc) * d_boc_yc;

d_cod_yc = 2*(yc-y0)*n_v_co*n_v_do + dot(v_co, v_do) * ((yc-y0)*n_v_do/n_v_co+(yc-y0)*n_v_co/n_v_do);
d_cod_yc = d_cod_yc * (-1/(sqrt(1-cos(b_cod)*cos(b_cod))+eps))/n_v_co/n_v_co/n_v_do/n_v_do;
if det([v_co;v_do]) < 0
    d_cod_yc = -d_cod_yc;
end
d_cod_yc = 2*(b_cod - a_cod) * d_cod_yc;

d_doa_yc = (-y0)*n_v_do*n_v_ao + dot(v_do, v_ao) * n_v_ao * (yc-y0)/n_v_do;
d_doa_yc = d_doa_yc * (-1/(sqrt(1-cos(b_doa)*cos(b_doa))+eps))/n_v_do/n_v_do/n_v_ao/n_v_ao;
if det([v_do;v_ao]) < 0
    d_doa_yc = -d_doa_yc;
end
d_doa_yc = 2*(b_doa - a_doa) * d_doa_yc;

d_x0 = d_aob_x0 + d_boc_x0 + d_cod_x0 + d_doa_x0;
d_y0 = d_aob_y0 + d_boc_y0 + d_cod_y0 + d_doa_y0;
d_yc = d_boc_yc + d_cod_yc + d_doa_yc;

df = [d_x0; d_y0; d_yc; 0; 0; 0; 0; 0];


function Subs = som_ind2sub(msize,inds)

%SOM_IND2SUB Map grid subscripts from linear index.
%
% Subs = som_ind2sub(msize,inds)
%
%  sub = som_ind2sub([10 15],44);
%  sub = som_ind2sub(sMap,44);
%  sub = som_ind2sub(sMap.msize,44);
%  Subs = som_ind2sub([10 15],[44 13 91]');
%
%  Input and output arguments:
%   msize  (struct) map or topology struct
%          (vector) size 1 x m, specifies the map grid size
%   inds   (vector) size n x 1, linear indeces of n map units
%
%   Subs   (matrix) size n x m, the subscripts
%
% See also SOM_SUB2IND.

% Contributed to SOM Toolbox vs2, February 2nd, 2000 by Juha Vesanto
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 2.0beta juuso 300798

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if isstruct(msize),
  if strcmp(msize.type,'som_map'), msize = msize.topol.msize;
  elseif strcmp(msize.type,'som_topol'), msize = msize.msize;
  else error('Invalid first argument.'); end
end

n = length(msize);
k = [1 cumprod(msize(1:end-1))];
inds = inds - 1;
for i = n:-1:1,
  Subs(:,i) = floor(inds/k(i))+1;
  inds = rem(inds,k(i));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [N,Z,M,A,XYZ] = spm_max(X,L)
% Sizes, maxima and locations of local excursion sets
% FORMAT [N Z M A XYZ] = spm_max(X,L)
% X     - values of 3-D field
% L     - locations [x y z]' {in voxels}
%
% N     - size of region {in voxels)
% Z     - Z values of maxima
% M     - location of maxima {in voxels}
% A     - region number
% XYZ   - cell array of voxel locations
%__________________________________________________________________________
%
% spm_max characterizes a point list of voxel values (X) and their
% locations (L) in terms of edge, face and vertex connected subsets,
% returning a maxima- orientated list:  The value of the ith maximum is
% Z(i) and its location is given by M(:,i). A(i) identifies the ith
% maximum with a region. Region A(i) contains N(i) voxels, whose
% coordinates are in a 3-by-N(i) array in XYZ{i}.
%
% See also: spm_bwlabel.m and spm_clusters.m
%__________________________________________________________________________
% Copyright (C) 2003-2011 Wellcome Trust Centre for Neuroimaging

% Jesper Andersson
% $Id: spm_max.m 4384 2011-07-06 17:00:20Z guillaume $

if isempty(L)
    N = []; Z = []; M = []; A = []; XYZ = [];
    return;
end

%-Ensure that L contains exactly integers
%--------------------------------------------------------------------------
L          = round(L);

%-Turn location list to binary 3D volume
%--------------------------------------------------------------------------
dim        = [max(L(1,:)) max(L(2,:)) max(L(3,:))];
vol        = zeros(dim(1),dim(2),dim(3));
index      = sub2ind(dim,L(1,:)',L(2,:)',L(3,:)');
vol(index) = 1;

%-Label each cluster with its own label using an 18 connectivity criterion
% cci = connected components image volume
%--------------------------------------------------------------------------
[cci,num]  = spm_bwlabel(vol,18);

%-Get size (in no. of voxels) for each connected component
% ccs = connected component size
%--------------------------------------------------------------------------
ccs        = histc(cci(:),(0:num) + 0.5);
ccs        = ccs(1:end-1);

%-Get indices into L for voxels that are indeed local maxima (using an 18
% neighbour criterion)
%--------------------------------------------------------------------------
vol(index) = X;
Lindex     = spm_get_lm(vol,L);

M          = L(:,Lindex);
Z          = X(Lindex);
mindex     = sub2ind(dim,L(1,Lindex)',L(2,Lindex)',L(3,Lindex)');
A          = cci(mindex);
N          = ccs(A);

%-Cell array of XYZ locations of voxels in each cluster
%--------------------------------------------------------------------------
if nargout > 4
    xyz(:,index) = sparse(L);
    cci   = sparse(cci(:));
    XYZ = cell(1, max(A));
    for i = 1:max(A)
        XYZ{i} = full(xyz(:,cci == i));
    end
end


function cspy(S,epsilon)
  % cspy Visualize sparsity pattern. coloring positive and negative entries of S
  % in red and blue respectively
  %
  % cspy(S)
  % cspy(S,epsilon)
  %
  % Inputs:
  %   S  m by n (sparse) matrix
  %   epsilon  zero value {0}
  %
  % See also: spy
  %

  if nargin < 2
    epsilon = 0;
  end
  spy(S>epsilon,'r');
  hold on;
  spy(S<-epsilon,'b');
  hold off;
end


function spec_plot2(varargin);
%
% Type: spec_plot2(S_m,freq,time,typ,f_min,f_max,c_min,c_max);
% Type: spec_plot2(S_m,freq,time,typ,f_min,f_max,c_min);
% Type: spec_plot2(S_m,freq,time,typ,f_min,f_max);
% Type: spec_plot2(S_m,freq,time,typ,f_min);
% Type: spec_plot2(S_m,freq,time,typ);
% Type: spec_plot2(S_m,freq,time);
%
% Inputs:
%
% S_m    := Time frequency distribution matrix n x m matrix
% freq   := Frequency vector n x 1 vector
% time   := Time vector m x 1 vector
% typ    := 'lin' or 'log' string for color scale of image plot
% f_min  := Minimum frequency to show, scalar
% f_max  := Maximum frequency to show, scalar
% c_min  := value for low end of color spectrum (see caxis), scalar
% c_max  := value for high end of color spectrum (see caxis), scalar
%
% This version uses pcolor and shading interp instead of imagesc

% Scot McNeill, University of Houston, Fall 2007.
%
msg=nargchk(3,8,nargin);
if ~isempty(msg)
   error(msg)
end
%
S_m=varargin{1};
freq=varargin{2};
time=varargin{3};
%
if (length(varargin) >= 4 & ~isempty(varargin{4}))
   typ=varargin{4};
else
   typ='log';
end
%
if (length(varargin) >= 5 & ~isempty(varargin{5}))
   f_min=varargin{5};
else
   f_min=min(freq);
end
%
if (length(varargin) >=6 & ~isempty(varargin{6}))
   f_max=varargin{6};
else
   f_max=max(freq);
end
%
if (length(varargin) >= 7 & ~isempty(varargin{7}))
   c_min=varargin{7};
else
   c_min=[];
end
%
if (length(varargin) ==8 & ~isempty(varargin{8}))
   c_max=varargin{8};
else
   c_max=[];
end
%
[nr,nc]=size(S_m);
nt=length(time);
nf=length(freq);
if (nr ~= nf & nc ~= nt)
   error('S_m must have dimensions length(freq) x length(time)')
end
%
[i1]=find(freq>=f_min & freq<=f_max);
freq=freq(i1);
S_m=S_m(i1,:);
%
newplot;
if strcmp(typ,'log');
   %imagesc(time,freq,log10(abs(S_m)+eps));
   pcolor(time,freq,log10(abs(S_m)+eps));shading interp;
   axis xy;
   colormap(jet);
   xlabel('Time (seconds)');
   ylabel('Frequency (Hz)');
   title('Spectrogram');
   axis([min(time),max(time),f_min,f_max]);
   [c1,c2]=caxis;
   if isempty(c_min);c_min=c1;end;
   if isempty(c_max);c_max=c2;end;
   caxis([c_min,c_max]);
   h = colorbar;
   set(get(h,'YLabel'),'String','Spectrogram, log_1_0([Units]^2/Hz)');
elseif strcmp(typ,'lin');
   %imagesc(time,freq,abs(S_m));
   pcolor(time,freq,abs(S_m));shading interp;
   axis xy;
   colormap(jet);
   xlabel('Time (seconds)');
   ylabel('Frequency (Hz)');
   title('Spectrogram');
   axis([min(time),max(time),f_min,f_max]);
   [c1,c2]=caxis;
   if isempty(c_min);c_min=c1;end;
   if isempty(c_max);c_max=c2;end;
   caxis([c_min,c_max]);
   h = colorbar;
   set(get(h,'YLabel'),'String','Spectrogram, [Units]^2/Hz');
else
   error(['typ must be ''lin'' or ''log''.']);
end
set(gca,'tickdir','out');
drawnow


function pass = test_contour3( pref )
% Test contour3

if ( nargin == 0 )
    pref = chebfunpref;
end

f = diskfun(@(x,y) cos(cos(4*x).^2 + sin(5*y).^2));
x = -pi:.1:pi;
y = -1:.1:1;
[xx, yy] = meshgrid(x,y);

pass = 1;
try
   contour3(f)
   contour3(f, 5)
   contour3(f, [0.4 0.4])
   contour3(f, 'numpts', 100)
   contour3(f, 'pivots', 'r.-')
   contour3(xx, yy, f)
catch ME
    pass = 0;
end

close all

end