function [K] = ku1v1(x, y, xp, yp, hyp, i)

logsigma = hyp(1);
logthetax = hyp(2);
logthetay = hyp(3);

n_x = size(x,1);
n_y = size(y,1);
n_xp = size(xp,1);
n_yp = size(yp,1);

x = repmat(x,1,n_xp);
y = repmat(y,1,n_yp);
xp = repmat(xp',n_x,1);
yp = repmat(yp',n_y,1);

switch i

case 0

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(x+(-1).*xp).*(y+(-1).*yp);

case 1 % logsigma

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(x+(-1).*xp).*(y+(-1).*yp);

case 2 % logthetax

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*((-1)+(1/2).*exp(1).^((-1).*logthetax).*(x+(-1).*xp).^2).*(x+ ...
  (-1).*xp).*(y+(-1).*yp);

case 3 % logthetay

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(x+(-1).*xp).*((-1)+(1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(y+(-1).*yp);

otherwise

        K = zeros(n_x, n_xp);
end

end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2019.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtFfmHelmholtzBWneu.m                        |
%|    #    |   VERSION    : 0.61                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 05.09.2019                                    |
%| ( === ) |   SYNOPSIS   : Solve neumann scatering problem with          |
%|  `---'  |                Brackage-Werner formulation                   |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
N   = 1e3
tol = 1e-3
typ = 'P1'
gss = 3
X0  = [0 0 -1]

% Spherical mesh
sphere = mshSphere(N,1);
sigma  = dom(sphere,gss);
figure
plot(sphere)
axis equal

% Radiative mesh
square     = mshSquare(5*N,[5 5]);
square.vtx = [square.vtx(:,1) zeros(size(square.vtx,1),1) square.vtx(:,2)];
hold on
plot(square)

% Frequency adjusted to maximum esge size
stp = sphere.stp;
k   = 1/stp(2)
f   = (k*340)/(2*pi)

% Incident wave
PW         = @(X) exp(1i*k*X*X0');
gradxPW{1} = @(X) 1i*k*X0(1) .* PW(X);
gradxPW{2} = @(X) 1i*k*X0(2) .* PW(X);
gradxPW{3} = @(X) 1i*k*X0(3) .* PW(X);

% Incident wave representation
plot(sphere,real(PW(sphere.vtx)))
plot(square,real(PW(square.vtx)))
title('Incident wave')
xlabel('X');   ylabel('Y');   zlabel('Z');
hold off
view(0,10)
% camlight
% material dull
% lighting phong

%%% PREPARE OPERATORS
disp('~~~~~~~~~~~~~ PREPARE OPERATORS ~~~~~~~~~~~~~')

% Green kernel function --> G(x,y) = exp(ik|x-y|)/|x-y|
Gxy      = '[exp(ikr)/r]';
gradyGxy = {'grady[exp(ikr)/r]1','grady[exp(ikr)/r]2','grady[exp(ikr)/r]3'};

% Finite elements
u = fem(sphere,typ);
v = fem(sphere,typ);

% Coupling coeff
beta = 1i*k;

% Finite element mass matrix --> \int_Sx psi(x)' psi(x) dx
Id = integral(sigma,u,v);

% Finite element boundary operator -->
% k^2 * \int_Sx \int_Sy n.psi(x) G(x,y) n.psi(y) dx dy
% - \int_Sx \int_Sy nxgrad(psi(x)) G(x,y) nxgrad(psi(y)) dx dy
tic
Hr = 1/(4*pi) .* (k^2 * regularize(sigma,sigma,ntimes(u),'[1/r]',ntimes(v)) ...
    - regularize(sigma,sigma,nxgrad(u),'[1/r]',nxgrad(v)));
H  = 1/(4*pi) .* (k^2 * integral(sigma,sigma,ntimes(u),Gxy,k,ntimes(v),tol) ...
    - integral(sigma,sigma,nxgrad(u),Gxy,k,nxgrad(v),tol)) + Hr;
toc

% Finite element boundary operator --> \int_Sx \int_Sy psi(x)' dny G(x,y) psi(y) dx dy
tic
Dtr = 1/(4*pi) .* regularize(sigma,sigma,u,'grady[1/r]',ntimes(v)).';
Dt  = 1/(4*pi) .* integral(sigma,sigma,u,gradyGxy,k,ntimes(v),tol).' + Dtr;
toc

% Final operator [1i*k*beta*(-Id/2 + Dt) - H]
tic
LHS  = beta.*(-0.5*Id + Dt) - H;
toc

% Finite element incident wave trace --> \int_Sx psi(x) dnx(pw(x)) dx
RHS = - integral(sigma,ntimes(u),gradxPW);

%%% SOLVE LINEAR PROBLEM
disp('~~~~~~~~~~~~~ SOLVE LINEAR PROBLEM ~~~~~~~~~~~~~')

% Preconditionneur ILU
tic
[L,U] = ilu(beta.*(-0.5*Id + Dtr) - Hr);
toc

% Solve linear system : [H + 1i*k*beta*(Id/2 - Dt)] = -dnP0
tic
mu = mgcr(@(V) LHS*V,RHS,[],tol,100,L,U);
toc

% Jump for derivative
lambda = beta * mu;

%%% INFINITE SOLUTION
disp('~~~~~~~~~~~~~ INFINITE RADIATION ~~~~~~~~~~~~~')

% Plane waves direction
theta = 2*pi/1e3 .* (1:1e3)';
nu    = [sin(theta),zeros(size(theta)),cos(theta)];

% Green kernel function
Ginf      = '[exp(-ikxy)]';
gradxGinf = {'gradx[exp(-ikxy)]1','gradx[exp(-ikxy)]2','gradx[exp(-ikxy)]3'};

% Finite element infinite operators
Sinf = 1/(4*pi) .* integral(nu,sigma,Ginf,k,v,tol);
Dinf = 1/(4*pi) .* integral(nu,sigma,gradxGinf,k,ntimes(v),tol);

% Finite element radiation
sol = Sinf*lambda - Dinf*mu;

% Analytical solution
ref = sphereHelmholtz('inf','neu',1,k,nu);
norm(ref-sol,2)/norm(ref,2)
norm(ref-sol,'inf')/norm(ref,'inf')

% Graphical representation
figure
plot(theta,log(abs(sol)),'b',theta,log(abs(ref)),'--r')

%%% DOMAIN SOLUTION
disp('~~~~~~~~~~~~~ RADIATION ~~~~~~~~~~~~~')

% Finite element mass matrix --> \int_Sx psi(x)' psi(x) dx
Id = integral(sigma,u,v);

% Finite element boundary operator --> \int_Sx \int_Sy psi(x)' G(x,y) psi(y) dx dy
tic
Sbnd = 1/(4*pi) .* (integral(sigma,sigma,u,Gxy,k,v,tol) + ...
    regularize(sigma,sigma,u,'[1/r]',v));
toc

% Finite element boundary operator --> \int_Sx \int_Sy psi(x)' dny G(x,y) psi(y) dx dy
tic
Dbnd = 1/(4*pi) .* (integral(sigma,sigma,u,gradyGxy,k,ntimes(v),tol) + ...
    regularize(sigma,sigma,u,'grady[1/r]',ntimes(v)));
toc

% Boundary solution
Psca = Id\(Sbnd*lambda - (0.5*Id*mu + Dbnd*mu));
Pinc = PW(u.dof);
Pbnd = Pinc + Psca;

% Finite element radiative operator --> \int_Sy G(x,y) psi(y) dy
tic
Sdom = 1/(4*pi) .* (integral(square.vtx,sigma,Gxy,k,v,tol) + ...
    regularize(square.vtx,sigma,'[1/r]',v));
toc

% Finite element radiative operator --> \int_Sx \int_Sy psi(x)' grady(G(x,y)) ny.psi(y) dx dy
tic
Ddom = 1/(4*pi) .* ( integral(square.vtx,sigma,gradyGxy,k,ntimes(v),tol) + ...
    regularize(square.vtx,sigma,'grady[1/r]',ntimes(v)) );
toc

% Domain solution
Psca = Sdom*lambda - Ddom*mu;
Pinc = PW(square.vtx);
Pdom = Pinc + Psca;

% Annulation sphere interieure
r             = sqrt(sum(square.vtx.^2,2));
Pdom(r<=1.01) = Pinc(r<=1.01);

% Graphical representation
figure
plot(sphere,abs(Pbnd))
axis equal;
hold on
plot(square,abs(Pdom))
title('Total field solution')
colorbar
hold off
view(0,10)

%%% ANAYTICAL SOLUTIONS FOR COMPARISONS
% Analytical solution
Pbnd = sphereHelmholtz('dom','neu',1,k,1.001*sphere.vtx) + PW(sphere.vtx);
Pdom = sphereHelmholtz('dom','neu',1,k,square.vtx) + PW(square.vtx);

% Solution representation
figure
plot(sphere,abs(Pbnd))
axis equal;
hold on
plot(square,abs(Pdom))
title('Analytical solution')
colorbar
hold off
view(0,10)

disp('~~> Michto gypsilab !')


function [ loss, real_x ] = loss_with_gradient_single_before( data, net )
train = data.train;
label = data.label;
LL = double(-1:0.02:1);
N = numel(net.layers);

res = struct(...
    'x',cell(1,N+1),...
    'dzdx',cell(1,N+1),...
    'dzdw',cell(1,N+1));
res(1).x = train;

%% forword propagation
for n = 1:N
    l = net.layers{n};
    switch l.type
        case 'X_org'
            res(n+1).x = xorg (res(n).x , l.weights{1} , l.weights{2});
        case 'Convo'
            res(n+1).x = convo(res(n).x , l.weights{1});
        case 'Non_linorg'
          res(n+1).x = zorg( LL ,res(n).x , l.weights{1});
        case 'Multi_org'
            res(n+1).x = betaorg(res(n-1).x , res(n).x ,l.weights{1} );
        case 'X_mid'
            res(n+1).x = xmid( res(n-1).x , res(n).x, res(1).x , l.weights{1} , l.weights{2});
        case 'Non_linmid'
           res(n+1).x = zmid(LL, res(n).x , res(n-2).x , l.weights{1} );
        case 'Multi_mid'
            res(n+1).x = betamid(res(n-3).x , res(n-1).x , res(n).x ,l.weights{1});
        case 'Multi_final'
            res(n+1).x = betafinal(res(n-3).x , res(n-1).x , res(n).x, l.weights{1});
        case 'loss'
              res(n+1).x = rnnloss(res(n).x, label);
        otherwise
            error('No such layers type.');
      end;

end;
    loss = res(end).x;
    loss = double(loss);
    real_x = res(end-1).x;
end


function pixelMeasurements = genFeatureMeasurements(T_wCam_GT, landmarks_w, K, simSetup)
%Will return -1 if the feature cannot be seen

camRes = simSetup.cameraResolution;

pixelMeasurements = zeros(2, size(T_wCam_GT, 3));

% Extract all viewable measurements
for step_i = 1:size(T_wCam_GT,3)

        %Transform and project landmarks into the camera frame
        landmarks_cam = homo2cart(inv(T_wCam_GT(:,:,step_i))*cart2homo(landmarks_w));
        pixels = homo2cart(K*landmarks_cam);

        %Determine which landmarks are viewable
        isInFieldOfView = pixels(1,:) > 0 & pixels(1,:) < camRes(2) & pixels(2,:) > 0 & pixels(2,:) < camRes(1);
        isInFieldOfView = isInFieldOfView & landmarks_cam(3,:) > 0;

        %viewableLandmarkIds = find(viewableLandmarksIdx);
        %viewableLandmarkIds = viewableLandmarkIds(:);
        %viewableLandmarks = landmarks_cam(:, viewableLandmarksIdx);
        if ~isInFieldOfView
            pixels = [-1; -1];
            n_p = [0; 0];
        else
            n_p = simSetup.pixelNoiseStd*randn(2,size(pixels,2)); %image noise
        end

        pixelMeasurements(:, step_i) =  pixels + n_p;

end

end


function As = slsymgraph(A, symmethod)
%SLSYMGRAPH Forces symmetry of the adjacency matrix of a graph
%
% $ Syntax $
%   - As = slsymgraph(A)
%   - As = slsymgraph(A, symmethod)
%
% $ Arguments $
%   - A:            The adjacency matrix of the original graph
%   - symmethod:    The method to symmetrize the graph
%   - As:           The symmetry adjacency matrix
%
% $ Description $
%   - As = slsymgraph(A) makes a symmetry version of the adjacency matrix
%     A using default method.
%
%   - As = slsymgraph(A, symmethod) symmetrizes the adjacency matrix by
%     using the specified method.
%     \*
%     \t    Table. The method to symmetrizes adjacency matrix
%     \h       name       &              description
%             'avgor'     & Force symmetry using the following rule:
%                           if both aij and aji are non-zeros, then
%                           take their average
%                           if only one of aij and aji is non-zero, then
%                           take the non-zero one
%                           if both aij and aji are zeros, then set zero
%                           (for both logical and numeric)
%             'avgand'    & Force symmetry using the following rule:
%                           if both aij and aji are non-zeros, then
%                           take their average
%                           if either one of aij and aji is zero, then
%                           set zero
%                           (for both logical and numeric)
%             'or'        & Use or-rule: d = aij | aji
%                           (for only logical)
%             'and'       & Use and-rule: d = aij & aji
%                           (for only logical)
%             'simavg'    & make simple average: always take (aij+aji)/2
%                           (for only numeric)
%     \*
%     The default method to use is 'avgor'. You can use your own function
%     handle. It should be like the following form:
%       v = f(v1, v2)
%     v1 and v2 are arrays of equal size with corresponding values. For
%     a reasonable function, it should satisfy that:
%       f(v, v) == v && f(v1, v2) = f(v2, v1)
%
% $ Remarks $
%   - A can be full matrix or sparse matrix, and As preserves the same
%     storage form.
%
%   - A should be a square matrix.
%
%   - When 'avgor' method is applied to logical, it is equivalent to 'or'
%     when 'avgand' method is applied to logical, it is equivalent to
%     'and'.
%
% $ History $
%   - Created by Dahua Lin, on Sep 8, 2006
%

%% parse and verify input

if ndims(A) ~= 2 || size(A,1) ~= size(A,2)
    error('sltoolbox:invalidarg', ...
        'The A should be a square 2D matrix');
end
n = size(A, 1);

if nargin < 2 || isempty(symmethod)
    symmethod = 'avgor';
end

if ischar(symmethod)
    switch symmethod
        case 'avgor'
            fcs = @compsym_avgor;
        case 'avgand'
            fcs = @compsym_avgand;
        case 'or'
            fcs = @compsym_or;
            if isnumeric(A)
                error('sltoolbox:rterror', ...
                    'The or method is not applicable to numerical matrix');
            end
        case 'and'
            fcs = @compsym_and;
            if isnumeric(A)
                error('sltoolbox:rterror', ...
                    'The and method is not applicable to numerical matrix');
            end
        otherwise
            error('sltoolbox:invalidarg', ...
                'Invalid method for symmetrization: %s', method);
    end
elseif isa(symmethod, 'function_handle')
    fcs = symmethod;
else
    error('sltoolbox:invalidarg', ...
        'Invalid method for symmetrization.');
end

%% main skeleton

% prepare all indices with aij or aji non-zero

[I0, J0] = find(A);

% single out diagonal ones
is_diag = (I0 == J0);
if any(is_diag)
    inds_diag = sub2ind([n, n], I0(is_diag), J0(is_diag));
else
    inds_diag = [];
end

% process the non-diagonal ones
not_diag = ~is_diag;
clear is_diag;

if any(not_diag)
    % filter indices
    I0 = I0(not_diag);
    J0 = J0(not_diag);
    clear not_diag;

    % merge to down triangular part
    I = I0;
    J = J0;
    idx_ut = find(I0 > J0);
    if ~isempty(idx_ut)
        I(idx_ut) = J0(idx_ut);
        J(idx_ut) = I0(idx_ut);
    end
    clear I0 J0 idx_ut;

    % unique and expand to up triangular part
    inds_dt = sub2ind([n, n], I, J);
    [inds_dt, si] = unique(inds_dt);
    I = I(si);
    J = J(si);
    inds_ut = sub2ind([n, n], J, I);
    clear I J si;
else
    inds_dt = [];
    inds_ut = [];
end

% get original values

if ~isempty(inds_dt)
    v_dt = A(inds_dt);
    v_ut = A(inds_ut);
else
    v_dt = [];
    v_ut = [];
end

% compute the symmetrized value

v = fcs(v_dt, v_ut);
clear v_dt v_ut;

% combine diagonal value and non-diagonal value

if ~isempty(inds_diag)
    v_diag = A(inds_diag);
else
    v_diag = [];
end

s_inds = vertcat(inds_diag, inds_dt, inds_ut);
clear inds_diag inds_dt inds_ut;
s_vals = vertcat(v_diag, v, v);
clear v_diag v;

% create matrix

As = slmakeadjmat(n, n, s_inds, s_vals, islogical(A), issparse(A));

%% symmetry value computation functions

function vd = compsym_avgor(v1, v2)

if isnumeric(v1)
    has_both = v1 & v2;
    only_v1 = v1 & ~v2;
    only_v2 = v2 & ~v1;

    vd = zeros(size(v1));
    vd(has_both) = (v1(has_both) + v2(has_both)) / 2;
    vd(only_v1) = v1(only_v1);
    vd(only_v2) = v2(only_v2);
else
    vd = v1 | v2;
end

function vd = compsym_avgand(v1, v2)

if isnumeric(v1)
    has_both = v1 & v2;

    vd = zeros(size(v1));
    vd(has_both) = (v1(has_both) + v2(has_both)) / 2;
else
    vd = v1 & v2;
end

function vd = compsym_or(v1, v2)

vd = v1 | v2;

function vd = compsym_and(v1, v2)

vd = v1 & v2;


function dx = expmall(J,f,t,EP)

EP(EP==1) = J;
EP(EP==2) = f;

dx = expm(EP*t);


function By = mfBy(yc,para,flag);

if ~exist('flag','var'), flag = 'By';  end;

Omega = para.Omega;
dim   = length(Omega);
m     = para.m;
h     = Omega./m;
a     = sqrt(para.mu);
b     = sqrt(para.mu+para.lambda);

% Bu for elastic-grad
% yc -> [u1,u2,u3]
%
%     dim == 3,                     dim == 2,
%     | d11         |
%     | d21         |
%     | d31         |   | u1 |          | d11     |
%     |     d12     |   |    |          | d21     |   | u1 |
%  y= |     d22     | * | u2 |      y = |     d12 | * |    |
%     |     d32     |   |    |          |     d22 |   | u2 |
%     |         d13 |   | u3 |          | d11 d22 |
%     |         d23 |
%     |         d33 |
%     | d11 d22 d33 |

flag = ['elastic-stg-',flag,'-',int2str(dim)];

switch flag
  case 'elastic-stg-By-2'
    nc = m(1)*m(2);
    nn = (m(1)+1)*(m(2)+1);
    ns1 = (m(1)+1)*m(2);
    ns2 = m(1)*(m(2)+1);

    I11 = 1:nc;                        % d11 y1
    I21 = I11(end)+(1:nn-2*(m(1)+1));  % d21 y1
    I12 = I21(end)+(1:nn-2*(m(2)+1));  % d12 y2
    I22 = I12(end)+(1:nc);             % d22 y2
    I00 = I22(end)+(1:nc);             % div Y

    By = zeros(I00(end),1);

    y1 = reshape(yc(1:ns1),m(1)+1,m(2));
    y2 = reshape(yc(ns1+(1:ns2)),m(1),m(2)+1);

    %% d11 y1
    By(I11) = reshape(y1(2:end,:)-y1(1:end-1,:),length(I11),1)/h(1);

    %% d21 y1
    By(I21) = reshape(y1(:,2:end)-y1(:,1:end-1),length(I21),1)/h(2);

    %% d12 y2
    By(I12) = reshape(y2(2:end,:)-y2(1:end-1,:),length(I12),1)/h(1);

    %% d22 y2
    By(I22) = reshape(y2(:,2:end)-y2(:,1:end-1),length(I22),1)/h(2);

    %% d22 y2
    By(I00) = b*By(I11) + b*By(I22);
    By(1:I22(end)) = a*By(1:I22(end));

  case 'elastic-stg-By-3'

    n0 = m(1)*m(2)*m(3);
    n1 = (m(1)+1)*m(2)*m(3);
    n2 = m(1)*(m(2)+1)*m(3);
    n3 = m(1)*m(2)*(m(3)+1);

    j1 = m(1)*(m(2)+1)*(m(3)+1);
    j2 = (m(1)+1)*m(2)*(m(3)+1);
    j3 = (m(1)+1)*(m(2)+1)*m(3);

    p1 = n0 + j2 + j3;
    p2 = p1 + n0 + j1 + j3;
    p3 = p2 + n0 + j1 + j2;

    ly = 4*n0+2*(j1+j2+j3);

%     mfilename, keyboard
    [y1,y2,y3] = vec2array(yc,m,'stg');

    clear yc

    By = zeros(ly,1);

    % d11 y1 : n1 -> n0
    dummy = y1(2:end,:,:) - y1(1:end-1,:,:);
    By(1:n0) = reshape(dummy,n0,1)/h(1);

    % d21 y1 : n1 -> j3
    dummy = zeros(m(1)+1,m(2)+1,m(3));
    dummy(:,2:end-1,:) = y1(:,2:end,:) - y1(:,1:end-1,:);
    By(n0+(1:j3)) = reshape(dummy,j3,1)/h(2);

    % d31 y1 : n1 -> j2
    dummy = zeros(m(1)+1,m(2),m(3)+1);
    dummy(:,:,2:end-1) = y1(:,:,2:end) - y1(:,:,1:end-1);
    By(n0+j3+(1:j2)) = reshape(dummy,j2,1)/h(3);

    % d12 y2 : n2 -> j3
    dummy = zeros(m(1)+1,m(2)+1,m(3));
    dummy(2:end-1,:,:) = y2(2:end,:,:) - y2(1:end-1,:,:);
    By(p1+(1:j3)) = reshape(dummy,j3,1)/h(1);

    % d22 y2 : n2 -> n0
    dummy = y2(:,2:end,:) - y2(:,1:end-1,:);
    By(p1+j3+(1:n0)) = reshape(dummy,n0,1)/h(2);

    % d32 y2 : n2 -> j1
    dummy = zeros(m(1),m(2)+1,m(3)+1);
    dummy(:,:,2:end-1) = y2(:,:,2:end) - y2(:,:,1:end-1);
    By(p1+j3+n0+(1:j1)) = reshape(dummy,j1,1)/h(3);

    % d13 y3 : n3 -> j2
    dummy = zeros(m(1)+1,m(2),m(3)+1);
    dummy(2:end-1,:,:) = y3(2:end,:,:) - y3(1:end-1,:,:);
    By(p2+(1:j2)) = reshape(dummy,j2,1)/h(1);

    % d23 y3 : n3 -> j1
    dummy = zeros(m(1),m(2)+1,m(3)+1);
    dummy(:,2:end-1,:) = y3(:,2:end,:) - y3(:,1:end-1,:);
    By(p2+j2+(1:j1)) = reshape(dummy,j1,1)/h(2);

    % d33 y3 : n3 -> n0
    dummy = y3(:,:,2:end) - y3(:,:,1:end-1);
    By(p2+j2+j1+(1:n0)) = reshape(dummy,n0,1)/h(3);

    % div u
    By(end-n0+1:end) = By(1:n0)+By(p1+j3+(1:n0)) + By(p2+j2+j1+(1:n0));

    By(1:end-n0)     = a*By(1:end-n0);
    By(end-n0+1:end) = b*By(end-n0+1:end);

  case 'elastic-stg-BTy-2'

    % the input yc is the result of B*y, therefore it can be splitted into 5
    % parts: d11u1, d21u1,d12u2,d22u2,Divy

    nc  = m(1)*m(2);
    nn  = (m(1)+1)*(m(2)+1);
    ns1 = (m(1)+1)*m(2);
    ns2 = m(1)*(m(2)+1);

    By  = zeros(ns1+ns2,1);
    I11 = 1:nc;                        % d11 y1
    I21 = I11(end)+(1:nn-2*(m(1)+1));  % d21 y1
    I12 = I21(end)+(1:nn-2*(m(2)+1));  % d12 y2
    I22 = I12(end)+(1:nc);             % d22 y2
    I00 = I22(end)+(1:nc);             % div Y

    yd = reshape(yc(I11),m);
    zd = reshape([-yd(1,:);yd(1:end-1,:)-yd(2:end,:);yd(end,:)],ns1,1)/h(1);
    By(1:ns1) = a*zd;

    yd = reshape(yc(I21),m(1)+1,m(2)-1);
    zd = reshape([-yd(:,1),yd(:,1:end-1)-yd(:,2:end),yd(:,end)],ns1,1)/h(2);
    By(1:ns1) = By(1:ns1) + a*zd;

    yd = reshape(yc(I00),m);
    zd = reshape([-yd(1,:);yd(1:end-1,:)-yd(2:end,:);yd(end,:)],ns1,1)/h(1);
    By(1:ns1) = By(1:ns1) + b*zd;

    yd = reshape(yc(I12),m(1)-1,m(2)+1);
    zd = reshape([-yd(1,:);yd(1:end-1,:)-yd(2:end,:);yd(end,:)],ns2,1)/h(1);
    By(1+ns1:end) = a*zd;

    yd = reshape(yc(I22),m);
    zd = reshape([-yd(:,1),yd(:,1:end-1)-yd(:,2:end),yd(:,end)],ns2,1)/h(2);
    By(1+ns1:end) = By(1+ns1:end) + a*zd;

    yd = reshape(yc(I00),m);
    zd = reshape([-yd(:,1),yd(:,1:end-1)-yd(:,2:end),yd(:,end)],ns2,1)/h(2);
    By(1+ns1:end) = By(1+ns1:end) + b*zd;

  case 'elastic-stg-BTy-3'

    % the input yc is the result of B*y, therefore it can be splitted into 10
    % parts: d11y1, d21y1,d31y1,d12y2,d22y2,d32y2,d13y3,d23y3,u33y3,Divy

    n0 = m(1)*m(2)*m(3);
    n1 = (m(1)+1)*m(2)*m(3);
    n2 = m(1)*(m(2)+1)*m(3);
    n3 = m(1)*m(2)*(m(3)+1);

    j1 = m(1)*(m(2)+1)*(m(3)+1);
    j2 = (m(1)+1)*m(2)*(m(3)+1);
    j3 = (m(1)+1)*(m(2)+1)*m(3);

    p1 = n0 + j2 + j3;
    p2 = p1 + n0 + j1 + j3;
    p3 = p2 + n0 + j1 + j2;

    By = zeros(n1+n2+n3,1);

    % extract Divy
    Divy  = reshape(yc(end-n0+1:end),m(1),m(2),m(3));

    % ---------------------------------- part 1 of u
    % extract D11y1, boundary condition for tangential derivative
    uu = reshape(yc(1:n0),m(1),m(2),m(3));
    dummy = zeros(m(1)+1,m(2),m(3));
    dummy(1,:,:)   = -uu(1,:,:);
    dummy(end,:,:) =  uu(end,:,:);
    dummy(2:end-1,:,:) = uu(1:end-1,:,:) - uu(2:end,:,:);
    By(1:n1) = a*reshape(dummy,n1,1)/h(1);

    % extract D21y1, boundary condition for normal derivative
    uu = reshape(yc(n0+(1:j3)),m(1)+1,m(2)+1,m(3));
    uu(:,[1,end],:) = 0;
    dummy = uu(:,1:end-1,:) - uu(:,2:end,:);
    By(1:n1) = By(1:n1) + a*reshape(dummy,n1,1)/h(2);

    % extract D31u1, boundary condition for normal derivative
    uu = reshape(yc(n0+j3+(1:j2)),m(1)+1,m(2),m(3)+1);
    uu(:,:,[1,end]) = 0;
    dummy = uu(:,:,1:end-1) - uu(:,:,2:end);
    By(1:n1) = By(1:n1) + a*reshape(dummy,n1,1)/h(3);

    % Divy, boundary condition for tangential derivative
    dummy = zeros(m(1)+1,m(2),m(3));
    dummy(1,:,:)   = -Divy(1,:,:);
    dummy(end,:,:) =  Divy(end,:,:);
    dummy(2:end-1,:,:) = Divy(1:end-1,:,:) - Divy(2:end,:,:);
    By(1:n1) = By(1:n1) + b*reshape(dummy,n1,1)/h(1);

    % ---------------------------------- part 2 of y
    % extract D12y2, boundary condition for normal derivative
    uu = reshape(yc(p1+(1:j3)),m(1)+1,m(2)+1,m(3));
    uu([1,end],:,:) = 0;
    dummy = uu(1:end-1,:,:) - uu(2:end,:,:);
    By(n1+(1:n2)) = a*reshape(dummy,n2,1)/h(1);

    % extract D22y2, boundary condition for tangential derivative
    uu = reshape(yc(p1+j3+(1:n0)),m(1),m(2),m(3));
    dummy = zeros(m(1),m(2)+1,m(3));
    dummy(:,1,:)   = -uu(:,1,:);
    dummy(:,end,:) =  uu(:,end,:);
    dummy(:,2:end-1,:) = uu(:,1:end-1,:) - uu(:,2:end,:);
    By(n1+(1:n2)) = By(n1+(1:n2)) + a*reshape(dummy,n2,1)/h(2);

    % extract D32y2, boundary condition for normal derivative
    uu = reshape(yc(p1+j3+n0+(1:j1)),m(1),m(2)+1,m(3)+1);
    uu(:,:,[1,end]) = 0;
    dummy = uu(:,:,1:end-1) - uu(:,:,2:end);
    By(n1+(1:n2)) = By(n1+(1:n2)) + a*reshape(dummy,n2,1)/h(3);

    % Divy, boundary condition for tangential derivative
    dummy = zeros(m(1),m(2)+1,m(3));
    dummy(:,1,:)   = -Divy(:,1,:);
    dummy(:,end,:) =  Divy(:,end,:);
    dummy(:,2:end-1,:) = Divy(:,1:end-1,:) - Divy(:,2:end,:);
    By(n1+(1:n2)) = By(n1+(1:n2)) + b*reshape(dummy,n2,1)/h(2);

    % ---------------------------------- part 3 of y
    % extract D13y3, boundary condition for normal derivative
    uu = reshape(yc(p2+(1:j2)),m(1)+1,m(2),m(3)+1);
    uu([1,end],:,:) = 0;
    dummy = uu(1:end-1,:,:) - uu(2:end,:,:);
    By(n1+n2+(1:n3)) = a*reshape(dummy,n3,1)/h(1);

    % extract D23y3, boundary condition for normal derivative
    uu = reshape(yc(p2+j2+(1:j1)),m(1),m(2)+1,m(3)+1);
    uu(:,[1,end],:) = 0;
    dummy = uu(:,1:end-1,:) - uu(:,2:end,:);
    By(n1+n2+(1:n3)) = By(n1+n2+(1:n3)) + a*reshape(dummy,n3,1)/h(2);

    % extract D32y2, boundary condition for tangential derivative
    uu = reshape(yc(p2+j2+j1+(1:n0)),m(1),m(2),m(3));
    dummy = zeros(m(1),m(2),m(3)+1);
    dummy(:,:,1)   = -uu(:,:,1);
    dummy(:,:,end) =  uu(:,:,end);
    dummy(:,:,2:end-1) = uu(:,:,1:end-1) - uu(:,:,2:end);
    By(n1+n2+(1:n3)) = By(n1+n2+(1:n3)) + a*reshape(dummy,n3,1)/h(3);

    % Divy, boundary condition for tangential derivative
    dummy = zeros(m(1),m(2),m(3)+1);
    dummy(:,:,1)   = -Divy(:,:,1);
    dummy(:,:,end) =  Divy(:,:,end);
    dummy(:,:,2:end-1) = Divy(:,:,1:end-1) - Divy(:,:,2:end);
    By(n1+n2+(1:n3)) = By(n1+n2+(1:n3)) + b*reshape(dummy,n3,1)/h(3);

  otherwise, jmerror(flag)
end;

% ==============================================================================


function feasible = checkfeasiblefast(p,x,tol)

feasible = 0;
if isempty(x)
    return
end
if ~(all(x < p.ub+tol) && all(x > p.lb-tol))
    return
end

if ~isempty(p.F_struc)
    vecres = p.F_struc*[1;x];

    if any(p.K.f)
        if any(-abs(vecres(1:p.K.f)) < - tol)
            return
        end
    end

    if any(p.K.l)
        if any(vecres(p.K.f+1:p.K.f+p.K.l) <-tol)
            return
        end
    end

    if any(p.K.q)
        top = startofSOCPCone(p.K);
        for i = 1:length(p.K.q)
            n = p.K.q(i);
            X = vecres(top:top+n-1);top = top+n;
            if X(1)-norm(full(X(2:end))) < -tol
                return
            end
        end
    end

    if any(p.K.e)
        top = startofEXPCone(p.K);
        for i = 1:p.K.e           ;
            X = vecres(top:top+2);top = top+3;
            if X(2)==0 && X(3) < -tol
                return
            elseif X(3) - X(2)*exp(X(1)/X(2)) < -tol
                return
            end
        end
    end

    if any(p.K.p)
        top = startofPOWCone(p.K);
        for i = 1:length(p.K.p)
            X = vecres(top:top+p.K.p(i)-1);
            a = X(end);
            top = top+p.K.p(i);
            if X(1)^a*X(2)^(1-a) - norm(X(3:end-1)) < -tol
                return
            end
        end
    end

    if any(p.K.s)
        top = startofSDPCone(p.K);
        for i = 1:length(p.K.s)
            n = p.K.s(i);
            X = reshape(vecres(top:top+n^2-1),n,n);top = top+n^2;
            X = (X+X')/2 + tol*eye(n);
            [~,fail] = chol(X);
            if fail
                return
            end
        end
    end
end
feasible = 1;


% a simple demo of HOG template matching : Jianxiong

% you may need to compile some mex files in matlab: see features_compile.m

clear
close all

image = imread('demo_image.jpg');

feature = feature_pyramid(image);

%% pick a template
template_level = 1;
template_width = 8;
template_height= 6;
template_left  = 41;
tempalte_top   = 31;
% extract a tempalte
template = feature.feat{template_level}(tempalte_top:(tempalte_top+template_height-1),template_left:(template_left+template_width-1),:);
% normalize the tempalte to zero mean
% empirically better for some task, you can also try to disable this
template = template - mean(template(:));

% corresponding image
template_box = ([template_left tempalte_top template_left+size(template,2) tempalte_top+size(template,1)] - 1) * 8 / feature.scale(template_level) + [1 1 0 0];
template_box = round(template_box);
tempalte_image = image(template_box(2):template_box(4),template_box(1):template_box(3),:);

%% template matching
matchMatrix = [];

for level=1:length(feature.feat)
    % convolution
    match{level} = fconvblas(feature.feat{level}, {template}, 1, 1);
    match{level} = match{level}{1};

    % pick the top 5000 results
    [scores,indexes] = sort(match{level}(:),'descend');
    if numel(scores)>5000
        choosen = 1:5000;
        indexes = indexes(choosen);
        scores  = scores(choosen);
    end

    % obtain window pixel locations
    [indexes1 indexes2] = ind2sub(size(match{level}),indexes);
    bbs{level} = ([indexes2 indexes1 indexes2+size(template,2) indexes1+size(template,1)] - 1) * 8 / feature.scale(level) + repmat([1 1 0 0],numel(indexes),1);
    bbs{level} = [bbs{level} scores];

    % non maximal suppression to remove windows too close
    indexes = nmsMe(bbs{level}, 0.5); % pascal voc 0.5 criteria
    bbs{level} = bbs{level}(indexes,:);

    matchMatrix = [matchMatrix; bbs{level}];
end

% overall best match
indexes = nmsMe(matchMatrix, 0.5); % pascal voc 0.5 criteria
matchMatrix = matchMatrix(indexes,:);

%% visualization

visLevel = [];
for level = 1:3:length(feature.feat)
    if ~isempty(bbs{level})
        visLevel = [visLevel, level];
    end
end

subplot(length(visLevel)+1,3, 1);
showHOG(template);
title('the HOG tempalte');
subplot(length(visLevel)+1,3, 2);
imshow(tempalte_image);
title('image of the template');
subplot(length(visLevel)+1,3, 3);
imshow(image);
hold on
for b=1:min(5,size(matchMatrix))
    plot(matchMatrix(b,[1 3 3 1 1]),matchMatrix(b,[2 2 4 4 2]),'-y');
end
title('top 5 detection over all scale levels');

for l=1:length(visLevel)
    level = visLevel(l);
    subplot(length(visLevel)+1,3, 3*l+1);
    showHOG(feature.feat{level});
    title(sprintf('Level %d HOG ',level));

    subplot(length(visLevel)+1,3, 3*l+2);
    imagesc(match{level});
    axis image
    axis off
    title(sprintf('Level %d response ',level));

    subplot(length(visLevel)+1,3, 3*l+3);
    imshow(image);
    hold on
    for b=1:min(5,size(bbs{level},1)) % only visualization the top five results
        plot(bbs{level}(b,[1 3 3 1 1]),bbs{level}(b,[2 2 4 4 2]),'-y');
    end
    title(sprintf('Level %d top 5',level));
end


function view_tensors(D, HA, delta, dim_order)
% VIEW_TENSORS    Renders diffusion tensors in 3D, colour coded by HA
%
%
% Inputs:
%
%   D is the diffusion tensor from fit_DT in 2 dimensions only (i.e. a 3D
%   volume with [x, y, DT])
%
%   HA is the helix angle (in 2D). HA values of NaN are coloured grey.
%
%   delta is the spacing between tensors (default 1.5E-3)
%
%   dim_order can be used (for example) when the tensors are referenced to
%   sagittal slices, whereas you want to display an axial slice. (default
%   [1 2 3])

% Author: Darryl McClymont <darryl.mcclymont@gmail.com>
% Copyright © 2014-2015 University of Oxford
% Version: 0.1.1
%
% 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/>.

% This function is an extension of plotDTI in the fanDTasia toolbox

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

if nargin < 4
    dim_order = [1 2 3]; % dimension
end
if nargin < 3
    delta = 1.5E-3; % spacing between tensors
end

% convert from 6x1 to 3x3 tensor representation
DT_33 = zeros([3 3 size(D,1) size(D,2)]);

DT_33(dim_order(1),dim_order(1),:,:) = D(:,:,1);
DT_33(dim_order(1),dim_order(2),:,:) = D(:,:,2);
DT_33(dim_order(1),dim_order(3),:,:) = D(:,:,3);
DT_33(dim_order(2),dim_order(1),:,:) = D(:,:,2);
DT_33(dim_order(2),dim_order(2),:,:) = D(:,:,4);
DT_33(dim_order(2),dim_order(3),:,:) = D(:,:,5);
DT_33(dim_order(3),dim_order(1),:,:) = D(:,:,3);
DT_33(dim_order(3),dim_order(2),:,:) = D(:,:,5);
DT_33(dim_order(3),dim_order(3),:,:) = D(:,:,6);

sz=size(DT_33);
if length(sz)==2
    nx=1;ny=1;
elseif length(sz)==3
    nx=sz(3);ny=1;
elseif length(sz)==4
    nx=sz(3);ny=sz(4);
end

ha = HA / 90;

hold on;
for i=1:nx
    for j=1:ny

        % don't waste time on nulled voxels
        if DT_33(1,1,i,j) == 0
            continue
        end

        % Jet colourmap
        if isnan(ha(i,j))
            C = [0.8 0.8 0.8];
        else
            C = [red(ha(i,j)), green(ha(i,j)), blue(ha(i,j))];
        end

        % get the eigenvectors of the tensor
        [v,l]=eig(round(DT_33(:,:,i,j)*1E7)/1E7);

        % generate the ellipsoid
        [X,Y,Z]=ellipsoid(0,0,0,l(1,1),l(2,2),l(3,3),10);
        sz=size(X);
        for x=1:sz(1)
            for y=1:sz(2)
                A=[X(x,y) Y(x,y) Z(x,y)]';
                A=v*A;
                X(x,y)=A(1);
                Y(x,y)=A(2);
                Z(x,y)=A(3);
            end
        end
        X=X+(i-1)*delta*2;
        Y=Y+(j-1)*delta*2;

        % display the ellipsoid
        surf(real(X),real(Y),real(Z), 'faceColor', C, 'EdgeAlpha', 0, 'EdgeColor', 'none');

    end
end

axis equal
view([0 90]);
set(gca,'GridLineStyle','none')
set(gca,'XTick',[])
set(gca,'YTick',[])
set(gca,'ZTick',[])

view([-1.2 0.5 0.8])
lighting phong
light('Position',[0 0 1],'Style','infinite','Color',[ 0.8 0.8 0.8]);

end

% these functions are to get the 'jet' colourmap (thanks stackexchange)
function val_out = interpolate( val,  y0,  x0,  y1,  x1 )
    val_out = (val-x0).*(y1-y0)./(x1-x0) + y0;
end

function val_out = base( val )
    if ( val <= -0.75 )
        val_out = 0;
    elseif ( val <= -0.25 )
         val_out = interpolate( val, 0.0, -0.75, 1.0, -0.25 );
    elseif ( val <= 0.25 )
        val_out =  1.0;
    elseif ( val <= 0.75 )
        val_out = interpolate( val, 1.0, 0.25, 0.0, 0.75 );
    else
        val_out = 0.0;
    end
end

function v = red( gray )
    v = base( gray - 0.5 );
end

function v = green( gray )
    v = base( gray );
end

function v = blue( gray )
    v = base( gray + 0.5 );
end


function [surf, labels] = imSurfaceArea(img, varargin)
% Surface area of the regions within a 3D binary or label image.
%
%   S = imSurfaceArea(IMG)
%   Estimates the surface area of the 3D binary structure represented by
%   IMG.
%
%   S = imSurfaceArea(IMG, NDIRS)
%   Specifies the number of directions used for estimating surface area.
%   NDIRS can be either 3 or 13, default is 13.
%
%   S = imSurfaceArea(..., SPACING)
%   Specifies the spatial calibration of the image. SPACING is a 1-by-3 row
%   vector containing the voxel size in the X, Y and Z directions, in that
%   orders.
%
%   S = imSurfaceArea(LBL)
%   [S, L] = imSurface(LBL)
%   When LBL is a label image, returns the surface area of each label in
%   the 3D array, and eventually returns the indices of processed labels.
%
%   S = imSurfaceArea(..., LABELS)
%   In the case of a label image, specifies the labels of the region to
%   analyse.
%
%
%   Example
%     % Create a binary image of a ball
%     [x y z] = meshgrid(1:100, 1:100, 1:100);
%     img = sqrt( (x-50.12).^2 + (y-50.23).^2 + (z-50.34).^2) < 40;
%     % compute surface area of the ball
%     S = imSurfaceArea(img)
%     S =
%         2.0103e+04
%     % compare with theoretical value
%     Sth = 4*pi*40^2;
%     100 * (S - Sth) / Sth
%     ans =
%         -0.0167
%
%     % compute surface area of several regions in a label image
%     img = uint8(zeros(10, 10, 10));
%     img(2:3, 2:3, 2:3) = 1;
%     img(5:8, 2:3, 2:3) = 2;
%     img(5:8, 5:8, 2:3) = 4;
%     img(2:3, 5:8, 2:3) = 3;
%     img(5:8, 5:8, 5:8) = 8;
%     [surfs, labels] = imSurfaceArea(img)
%     surfs =
%        16.4774
%        29.1661
%        29.1661
%        49.2678
%        76.7824
%     labels =
%          1
%          2
%          3
%          4
%          8
%
%
%   See also
%     imVolume, imMeanBreadth, imSurfaceAreaDensity, imJointSurfaceArea
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2010-07-26,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRAE - Cepia Software Platform.

%% Parse input arguments

% check image dimension
if ndims(img) ~= 3
    error('first argument should be a 3D image');
end

% the labels to compute
labels = [];

% default number of directions
nDirs = 13;

% default image calibration
delta = [1 1 1];

% methods to compute direction weights. Can be {'Voronoi'}, 'isotropic'.
directionWeights = 'voronoi';

% Process user input arguments
while ~isempty(varargin)
    var1 = varargin{1};

    if isnumeric(var1)
        % option is either connectivity or resolution
        if isscalar(var1)
            nDirs = var1;
        elseif all(size(var1) == [1 3])
            delta = var1;
        elseif size(var1, 2) == 1
            labels = var1;
        end
        varargin(1) = [];

    elseif ischar(var1)
        if length(varargin) < 2
            error('optional named argument require a second argument as value');
        end
        if strcmpi(var1, 'directionweights')
            directionWeights = varargin{2};
        end
        varargin(1:2) = [];

    else
        error('option should be numeric');
    end

end

%% Process label image

% in case of a label image, return a vector with a set of results
if ~islogical(img)
    % extract labels if necessary (considers 0 as background)
    if isempty(labels)
        labels = imFindLabels(img);
    end

    % allocate result array
    nLabels = length(labels);
    surf = zeros(nLabels, 1);

    % compute bounding box of each region
    boxes = imBoundingBox(img, labels);

    % Compute surface area of each region considered as binary image
    % The computation is performed on a subset of the image for reducing
    % memory footprint.
    for i = 1:nLabels
        label = labels(i);

        % convert bounding box to image extent, in x, y and z directions
        box = boxes(i,:);
        i0 = ceil(box([3 1 5]));
        i1 = floor(box([4 2 6]));

        % crop image of current label
        bin = img(i0(1):i1(1), i0(2):i1(2), i0(3):i1(3)) == label;
        surf(i) = imSurfaceArea(bin, nDirs, delta, 'directionWeights', directionWeights);
    end

    return;
end

%% Process binary images

% in case of binary image, compute only one label...
labels = 1;

% distances between a pixel and its neighbours.
d1  = delta(1); % x
d2  = delta(2); % y
d3  = delta(3); % z

% volume of a voxel (used for computing line densities)
vol = d1 * d2 * d3;

%% Main processing for 3 directions

% number of voxels
nv = sum(img(:));

% number of connected components along the 3 main directions
% (Use Graph-based formula: chi = nVertices - nEdges)
n1 = nv - sum(sum(sum(img(:,1:end-1,:) & img(:,2:end,:)))); % x
n2 = nv - sum(sum(sum(img(1:end-1,:,:) & img(2:end,:,:)))); % y
n3 = nv - sum(sum(sum(img(:,:,1:end-1) & img(:,:,2:end)))); % z

if nDirs == 3
    % compute surface area by averaging over the 3 main directions
    surf = 4/3 * (n1/d1 + n2/d2 + n3/d3) * vol;
    return;
end

%% Additional processing for 13 directions

% Number of connected components along diagonals contained in the three
% main planes
% XY planes
n4 = nv - sum(sum(sum(img(2:end,1:end-1,:)   & img(1:end-1,2:end,:))));
n5 = nv - sum(sum(sum(img(1:end-1,1:end-1,:) & img(2:end,2:end,:))));
% XZ planes
n6 = nv - sum(sum(sum(img(:,2:end,1:end-1)   & img(:,1:end-1,2:end))));
n7 = nv - sum(sum(sum(img(:,1:end-1,1:end-1) & img(:,2:end,2:end))));
% YZ planes
n8 = nv - sum(sum(sum(img(2:end,:,1:end-1)   & img(1:end-1,:,2:end))));
n9 = nv - sum(sum(sum(img(1:end-1,:,1:end-1) & img(2:end,:,2:end))));

%TODO: add the case of 9 directions ?

% Number of connected components along lines corresponding to diagonals of
% the unit cube
n10 = nv - sum(sum(sum(img(1:end-1,1:end-1,1:end-1) & img(2:end,2:end,2:end))));
n11 = nv - sum(sum(sum(img(2:end,1:end-1,1:end-1) & img(1:end-1,2:end,2:end))));
n12 = nv - sum(sum(sum(img(1:end-1,2:end,1:end-1) & img(2:end,1:end-1,2:end))));
n13 = nv - sum(sum(sum(img(2:end,2:end,1:end-1) & img(1:end-1,1:end-1,2:end))));

% space between 2 voxels in each direction
d12  = hypot(d1, d2);
d13  = hypot(d1, d3);
d23  = hypot(d2, d3);
d123 = sqrt(d1^2 + d2^2 + d3^2);

% Compute weights corresponding to surface fraction of spherical caps
% For isotropic case, weights correspond to:
% c1 = 0.04577789120476 * 2;  % Ox
% c2 = 0.04577789120476 * 2;  % Oy
% c3 = 0.04577789120476 * 2;  % Oz
% c4 = 0.03698062787608 * 2;  % Oxy
% c6 = 0.03698062787608 * 2;  % Oxz
% c8 = 0.03698062787608 * 2;  % Oyz
% c10 = 0.03519563978232 * 2;  % Oxyz
if strcmp(directionWeights, 'isotropic')
    c = zeros(13,1);
    c(1) = 0.04577789120476 * 2;  % Ox
    c(2) = 0.04577789120476 * 2;  % Oy
    c(3) = 0.04577789120476 * 2;  % Oz
    c(4:5)  = 0.03698062787608 * 2;  % Oxy
    c(6:7)  = 0.03698062787608 * 2;  % Oxz
    c(8:9)  = 0.03698062787608 * 2;  % Oyz
    c(10:13) = 0.03519563978232 * 2;  % Oxyz

else
    c = computeDirectionWeights3d13(delta);
end

% compute the weighted sum of each direction
% intersection count * direction weight / line density
surf = 4 * vol * (...
    n1*c(1)/d1 + n2*c(2)/d2 + n3*c(3)/d3 + ...
    (n4+n5)*c(4)/d12 + (n6+n7)*c(6)/d13 + (n8+n9)*c(8)/d23 + ...
    (n10 + n11 + n12 + n13)*c(10)/d123 );


function [xo,Ot,nS]=buscarnd(S,x0,ip,nOt,samples,Lb,Ub,problem,tol,mxit,R,red,mem)
%   Unconstrained global optimization using adaptive random search.
%
%   [xo,Ot,nS]=buscarnd(S,x0,ip,nOt,samples,Lb,Ub,problem,tol,mxit,R,red,mem)
%
%   S: objective function
%   x0: initial point
%   ip: (0): no plot (default), (>0) plot figure ip with pause, (<0) plot figure ip
%   nOt: maximum number of optimal points (default = 1)
%   samples: number of samples per stage (default = 3*size(x0(:),1))
%   Lb, Ub: lower and upper bound vectors to plot (default = x0*(1+/-2))
%   problem: (-1): minimum (default), (1): maximum
%   tol: tolerance (default = 1e-4)
%   mxit: maximum number of stages (default = 50*(1+4*~(ip>0)))
%   R: axis vector of the hyperellipse centered in x0 (default = max(0.1*abs(x0+~x0),1))
%   red: factor to reduce the size of the axis vector (0,1) (default = 0.2)
%   mem: number of stored points during the exploration phase [0, min(18,samples-1)]
%   xo: matrix of optimal points (column vectors)
%   Ot: vector of optimal values of S
%   nS: number of objective function evaluations

%   Copyright (c) 2001 by LASIM-DEQUI-UFRGS
%   $Revision: 1.0 $  $Date: 2001/07/10 19:40:05 $
%   Argimiro R. Secchi (arge@enq.ufrgs.br)
%
%   Based on the algorithm of the same author written in C
%   for constrained global optimization in 1989/09/29 and published as:
%   A.R. Secchi and C.A. Perlingeiro, "Busca Aleatoria Adaptativa",
%   in Proc. of XII Congresso Nacional de Matematica Aplicada e
%   Computacional, Sao Jose do Rio Preto, SP, pp. 49-52 (1989).
%
% Modified by Giovani Tonel(giovani.tonel@ufrgs.br) on September 2006

rand('state',sum(100*clock)); 	%rand('state',sum(100*clock)) resets it to
													%		         a different state each time.

if nargin < 2,
   error('buscarnd requires two input arguments');
 end
 if nargin < 3 | isempty(ip),
   ip=0;
 end
 if nargin < 4 | isempty(nOt),
   nOt=1;
 end
 if nargin < 5 | isempty(samples),
   samples=3*size(x0(:),1);
 end
 if nargin < 6 | isempty(Lb),
   Lb=-x0-~x0;
 end
 if nargin < 7 | isempty(Ub),
   Ub=2*x0+~x0;
 end
 if nargin < 8 | isempty(problem),
   problem=-1;
 end
 if nargin < 9 | isempty(tol),
   tol=1e-4;
 end
 if nargin < 10 | isempty(mxit),
   mxit=50*(1+4*~(ip>0));
 end
 if nargin < 11 | isempty(R),
   R=max(0.1*abs(x0+~x0),1);
 end
 if nargin < 12 | isempty(red) | red <= 0 | red >= 1,
   red=0.2;
 end
 if nargin < 13 | isempty(mem),
   mem=min(18,samples-1);
 end
 if mem >= samples,
   mem=samples-1;
 end
                    % initialization
 x0=x0(:);
 R=abs(R(:));
 n=size(x0,1);
 distrib=1;          % distr =  1  --> exploration phase
                     % distr >  1  --> progression phase
 asym1=-1*ones(n,1); % asym1 =  1  --> asymmetric distribution to reduce xo
                     % asym1 = -1  --> asymmetric distribution to increase xo
 asym2=2*ones(n,1);  % asym2 =  2  --> symmetric distribution
                     % asym2 = 1.5 --> asymmetric distribution
 metric=0.5*norm(R);
 a0=0.5*metric;
 a2=0.1*metric;
 a3=10*tol;
 n3=1;
 holdm=floor(1/red+0.5)*n;
 nhold=0;            % counter to keep distribution constant during holdm times

 top=0;
 if mem > 0,
   n4=10*mem;
   idx=[1:n4+1];
   last=n4;
   first=0;
   idx(last+1)=first;
   mem_x=zeros(n,n4);
   mem_S=-ones(1,n4)*inf;
 end

%     Criterion of axis vector reduction
%
%     R_red = R*0.5^(distrib-1)
%
%     or norm(R_red) = norm(R)*0.5^(distrib-1)
%
%     then if the criterion is norm(R_red) < tol = 10^(-asc)
%
%     the distrib is limited by
%
%     distrib = 1 + (asc + log10(norm(R))) / log10(2)

 lim_distrib=floor(0.5*(1+log10(metric/tol)/log10(2.0)));

 if ip & n == 2,
   figure(abs(ip));
   [X1,X2]=meshgrid(Lb(1):(Ub(1)-Lb(1))/20:Ub(1),Lb(2):(Ub(2)-Lb(2))/20:Ub(2));
   [n1,n2]=size(X1);
   f=zeros(n1,n2);
   for i=1:n1,
    for j=1:n2,
      f(i,j)=feval(S,[X1(i,j);X2(i,j)]);
    end
   end
   mxf=max(max(f));
   mnf=min(min(f));
   df=mnf+(mxf-mnf)*(2.^(([0:10]/10).^2)-1);
   [v,h]=contour(X1,X2,f,df); hold on;
%   clabel(v,h);
   h1=plot(x0(1),x0(2),'ro');
   legend(h1,'start point');

   if ip > 0,
     disp('Pause: hit any key to continue...'); pause;
   end
 end

 xOt=[];
 Ot=[];
 xo=x0;
 yo=feval(S,x0)*problem;
 nS=1;
 it=0;
 opt=0;
 x=zeros(n,samples);
 y=zeros(1,samples);

 while it < mxit & opt < nOt,
  l3=0;
  it=it+1;
  for j=1:samples,   % sampling
    a=min(max(rand(n,1),0.05),0.95);
    x(:,j)=xo+(R.*asym1.*(asym2.*a-1).^distrib)/distrib;
    y(j)=feval(S,x(:,j))*problem;
    nS=nS+1;
  end

  l4=mem & distrib == 1;  % sorting the samples
  if samples > 1,
    if ~l4,
      [y(1),i]=max(y);
      x(:,1)=x(:,i);
    else
      [y,i]=sort(-y); y=-y;
      x=x(:,i);
    end
  end

  if l4,                  % memorize samples
    n2=top;
    l1=0;
    for j=1:samples,
      n1=0;
      if opt,
        for i=1:opt
          if norm(xOt(:,i)-x(:,j)) < a2,
            n1=1;
            if j == 1,
              l1=1;
            end
            break;
          end
        end
      end

      if ~n1,
	for i=1:j-1
	   if norm(x(:,j)-x(:,i)) < metric,
	     n1=1;
	     break;
	   end
	end

	if ~n1,
	  k=idx(1);
	  for i=1:top
             if norm(mem_x(:,k)-x(:,j)) < metric,
               n1=1;
               break;
             else
               k=idx(k+1);
             end
          end

	  if ~n1,
            first=idx(first+1);
            if ~first,              % expand memory
              mem_x=[mem_x,zeros(n,n4)];
              mem_S=[mem_S,-ones(1,n4)*inf];
              idx=[idx,[n2+1:n2+n4+1]];
              first=n2;
              idx(last+1)=first;
              last=n2+n4;
              idx(last+1)=0;
            end
            k=first;
            n2=n2+1;
          end

          if ~n1 | y(j) > mem_S(k),
            mem_x(:,k)=x(:,j);
            mem_S(:,k)=y(j);
          end

	  if l1,
            l1=0;
            x(:,1)=x(:,j);
            y(1)=y(j);
          end
        end
      end

      if n2-top == mem,
        break;
      end
    end
    top=n2;
  end
                  % analysis of best sampled point
  a1=norm(x(:,1)-xo)/(0.1+norm(xo))+abs(y(1)-yo)/(0.1+abs(yo));
  l1=y(1) > yo;
  if l1,
    if norm(x(:,1)-xo) > a0,
      nhold=0;
      n3=1;
    end

    z=xo; xo=x(:,1); x(:,1)=z;
    a4=yo; yo=y(1); y(1)=a4;

    if ip & n == 2,
      plot([x(1) xo(1)],[x(2) xo(2)],'r');
      if ip > 0,
        disp('Pause: hit any key to continue...'); pause;
      end
    end
  end

  if a1 < tol & distrib >= lim_distrib,
    l2=1;
    if opt,
      for i=1:opt,
        if norm(xOt(:,i)-xo) < a3,
          l2=0;
          break;
        end
      end
    end

    if l2,
      opt=opt+1;
      Ot=[Ot,yo];
      xOt=[xOt,xo];
      if ip & n == 2,
        h2=plot(xo(1,:),xo(2,:),'r*');
        if opt == 1,
          legend([h1,h2],'start point','optimum');
        end
      end
    end

        % rescue another point from memory
    if ~top,
      if opt,
        xo=xOt;
        yo=Ot;
      end
      break;
    end

    if l2 & ~l1,
      n2=0;
      for j=1:top,
        k=idx(n2+1);
        if norm(mem_x(:,k)-xo) < a2,
          top=top-1;
          idx(last+1)=k;
          last=k;
          idx(n2+1)=idx(k+1);
          idx(k+1)=0;
          if k == first,
            first=n2;
          end
        else
          n2=k;
        end
      end
    end

    if ~top,
      if opt,
        xo=xOt;
        yo=Ot;
      end
      break;
    end

    k=idx(1);
    xo=mem_x(:,k);
    yo=mem_S(k);
    idx(last+1)=k;
    last=k;
    idx(1)=idx(k+1);
    idx(k+1)=1;
    top=top-1;

    if k == first,
      first=1;
    end

    l3=1;

    if ip & n == 2 & opt < nOt,
      plot(xo(1),xo(2),'ro');
    end
  elseif opt & (~l1 | (l1 & ~l4))
        n1=0;
	for i=1:opt,
          if norm(xOt(:,i)-xo) < a2,
            n1=1;
            break;
          end
        end

	if n1,
          if ~top,
            xo=xOt;
            yo=Ot;
            break;
          end

          k=idx(1);
          xo=mem_x(:,k);
          yo=mem_S(k);
          idx(last+1)=k;
          last=k;
          idx(1)=idx(k+1);
          idx(k+1)=1;
          top=top-1;

          if k == first,
            first=1;
          end

	  l3=1;

          if ip & n == 2 & opt < nOt,
            plot(xo(1),xo(2),'ro');
          end
        end
  end
                 % adjust search direction and criterion
  if l3,
    n3=1;
    distrib=1;
    nhold=0;
  elseif (a1 < a2 | ~l1) & (nhold+n3 > holdm),
        n3 = n3+~mod(distrib,3);
        distrib=distrib+2;
	nhold=0;
  else
    nhold=nhold+n3;
  end

  for i=1:n,
    if l3 | (~l3 & x(i,1) == xo(i)),
      asym2(i)=2;
    else
      asym2(i)=1.5;
      if x(i,1) < xo(i),
        asym1(i)=1;
      else
        asym1(i)=-1;
      end
    end
  end
 end

 if it == mxit,
   %disp('Warning Buscarnd: reached maximum number of stages!');
   if opt,
     yo=Ot;
     xo=xOt;
   end
 end

 if opt == nOt,
   yo=Ot;
   xo=xOt;
 end

 if opt > 1,
   [yo,i]=sort(-yo); yo=-yo;
   xo=xo(:,i);
 end

 Ot=yo*problem;


% @author: Maziar Raissi

function params = Fractional_Levy()

clc; close all;

plt = 1;
plt_pred = 0;
save_plt = 1;

addpath ..
addpath ../Utilities
addpath ../Kernels/Fractional_Levy
addpath ../Utilities/export_fig

function CleanupFun()
    rmpath ..
    rmpath ../Utilities
    rmpath ../Kernels/Fractional_Levy
    rmpath ../Utilities/export_fig
end

finishup = onCleanup(@() CleanupFun());

rng('default')

set(0,'defaulttextinterpreter','latex')

%% Load Data
[t_star, x_star, u_star, pos] = gen_data(10^6, 0.01, 5, 100);
% u_star --> 100x5
% t_star --> 5x1
% x_star --> 100x1
N_star = size(x_star,1);
nsteps = size(t_star,1)-1;

%% Setup
N0 = 100;
N1 = 100;
%% Clean Data
i = 4;
dt = t_star(i+1) - t_star(i);

idx0 = randsample(N_star, N0);
x0 = x_star(idx0,:);
u0 = u_star(idx0,i);

idx1 = randsample(N_star,N1);
x1 = x_star(idx1,:);
u1 = u_star(idx1,i+1);

hyp = [log([1.0 0.1]) 1.1 0.0];
model = HPM(x1, u1, x0, u0, dt, hyp);
model = model.train(1500);

hyp = model.hyp;
params = hyp(3);

[pred_n_star, var_n_star] = model.predict(x_star);
var_n_star = abs(diag(var_n_star));

error = norm(pred_n_star - u_star(:,i+1))/norm(u_star(:,i+1));

fprintf(1,'=========================\n');
fprintf(1,'Step: %d, Time = %.2f\n\nNLML = %.2f, Error = %.2e\n\n', i, ...
    t_star(i+1), model.NLML, error);

str = sprintf('%.2f  ', params);
fprintf('Parameters: %s\n\n', str)
fprintf(1,'=========================\n\n');

if plt_pred == 1
    figure();
    plot_prediction_1D(x_star, u_star(:,i+1), pred_n_star, var_n_star, ...
        '$x$', '$u(t,x)$', 'Prediction');

    drawnow;
end

%% Plot Results

if plt == 1
    fig = figure();
    set(fig,'units','normalized','outerposition',[0 0 1 .5])
    subplot(3,2,1:2)
    plot(pos, 'b', 'LineWidth', 2);
    xlabel('Time')
    ylabel('Position')
    axis tight
    set(gca,'FontSize',14);
    set(gcf, 'Color', 'w');

    subplot(3,2,3);
    tit = sprintf('$t = $ %.2f\n%d training data\n', t_star(i), N0);
    bar(x_star, u_star(:,i),'m');
    xlabel('$x$')
    ylabel('$u(t,x)$')
    axis tight
    set(gca,'FontSize',14);
    set(gcf, 'Color', 'w');
    title(tit);

    subplot(3,2,4);
    tit = sprintf('$t = $ %.2f\n%d training data\n', t_star(i+1), N1);
    bar(x_star, u_star(:,i+1),'m');
    xlabel('$x$')
    ylabel('$u(t,x)$')
    axis tight
    set(gca,'FontSize',14);
    set(gcf, 'Color', 'w');
    title(tit);

    subplot(3,2,5:6);

    s = '$\begin{tabular}{|c|c|}';
    s = strcat(s, ' \hline');
    s = strcat(s, ' Correct PDE & $u_t + (-\nabla^{\sqrt{2}}_x) u = 0$ \\');
    s = strcat(s, ' \hline');
    s = strcat(s, ' Identified PDE & $u_t + (-\nabla^{', sprintf('%.3f',params(1)), '}_x) u = 0$ \\');
    s = strcat(s, ' \hline');
    s = strcat(s, ' \end{tabular}$');
    text(0.25,0.8,s,'interpreter','latex','FontSize',18)
    axis off

    if save_plt == 1
        export_fig ../Figures/Fractional_Levy.png -r300
    end

    drawnow();
end

end

function [t_star, x_star, u_star, pos] = gen_data(N,dt,m,n)

    alpha = sqrt(2);
    r = stblrnd(alpha, 0, 1, 0, N, 1);
    pos = cumsum(dt^(1/alpha)*r);

    M = 0.35;

    P = zeros(length(pos)-m,m);
    for i = 1:length(pos)-m
        y = pos(i+1:i+m) - pos(i);
        P(i,:) = y;
    end

    bins = linspace(-M,M,n+1);
    x_star = linspace(M*(1/n-1), M*(1-1/n), n)';
    t_star = dt*(1:1:m)';
    u_star = zeros(n,m);

    f = figure();
    clf
    hold
    for i = 1:m
        h = histogram(P(:,i), bins, 'Normalization', 'pdf');
        u_star(:,i) = h.Values;
    end

    close(f)

end


function [bound,grad] = svr2w2 (sigma,X,Y,ker,p1,scales,slacks,opt,var2)

 % if ker=='weighted_linear' p1=1; else p1=2; end
 %% NOTE: ASSUMES AT MOMENT THAT IT IS A POLYNOMIAL or rbf, if using 'quadprog'-- not compatible w/ svmlight

 % chirag's edits: rbf kernel (non-weighted) implemented;
 % cannot use with 'svmlight' optimizer as of yet..

  l = size(X,1);
  N=size(X,2);
  Xkeep=X;

  scale=max(scales); % number of scaling factors
  if scale>0
    if length(scales)==1
      D=repmat(sigma(1:N),1,l);
    else
      D=repmat(sigma(scales),1,l);
    end
    X= X .* D';
  end

  slack=max(slacks);
  if slacks==0
    ridge=ones(l,1)*1e-11;
  else
    if length(slacks)==1 ridge=ones(l,1)*sigma(scale+1); end;
    if length(slacks)>1 ridge=sigma(scale+slacks); end;
  end
  switch ker
      case 'weighted_linear'
          K = X*X';
      case 'weighted_poly'
          K = X*X';
          K=(K+1).^p1;
      case 'rbf'
          %% create rbf kernel...
          [n1,n2]=size(X);
          kerneloption=ones(1,n2)*p1;
          metric = diag(1./p1.^2);
          ps = X*metric*X';
          [nps,pps]=size(ps);
          normx = sum(X.^2*metric,2);
          normxsup = sum(X.^2*metric,2);
          ps = -2*ps + repmat(normx,1,pps) + repmat(normxsup',nps,1) ;
          K = exp(-ps/2);
          %%
  end

  K= K + diag(ridge) .^2; %(sum(sigma)/N)*eye(l)*ridge;

  H = K .* (Y*Y');

  % construct the lower/upper bound vector
  lower = [zeros(l,1)];
  C=Inf;
  upper = [C*ones(l,1)];
  A = Y';  b = 0;   c = [-ones(l,1)];

  switch opt
   case {'loqo'}
    [a,b0] = loqo(sparse(H),c,sparse(Y'),0,lower,upper,[],1,0);

   case{'quadprog'}
    options = optimset('LargeScale','off');
    options = optimset(options,'MaxIter',400);
    [a,fval,exit,out,lambda] = quadprog(H,c,[],[],Y',0,lower,upper, ...
					[],options);
    b0=lambda.eqlin(1);
   case{'andre'}
    [a,b0] = quadsolve(H,c,Y',0,10000);

   case{'default'}
    [a,b0]=qp_learn(full(K),Y,1e5);

   case{'svmlight'}
    error('not implemented yet for rbf kernels!');
    %if strcmp(get(al.child,'ker'),'linear') ker=1; p1=1; end;
    %if strcmp(get(al.child,'ker'),'poly') ker=2;
    %p1=get(al.child,'kerparam'); p1=p1{1}; end;
     %if strcmp(get(al.child,'ker'),'gaussian') ker=3; p1= ...
     %get(al.child,'kerparam'); p1=(2*p1{1}^2);
     %end;
     ker='linear';p1=1;
     X=full(X);
     [a b0 ind] = svmlight(X,Y,1e5,ridge,0,ker,p1,0);
     alpha=zeros(size(X,1),1); alpha(ind+1)=a;
     a=abs(alpha);
  end

  % radius calculation

  if var2==0
    switch opt
     case {'loqo'}
      b=loqo(sparse(2*K),-diag(K),sparse(ones(1,l)),1,zeros(l,1),[],[],1,0);
     case {'quadprog'}
      b=quadprog(2*K,-diag(K),[],[],ones(1,l),1,zeros(l,1),[],[],options);
     case {'andre'}
       error(['calc radius: not implemented for this optimizer, use  svmlight or quadprog']);
     case {'default'}
      error(['calc radius: not implemented for this optimizer, use  svmlight or quadprog']);
     case {'svmlight'}   %% use andre anyway for r2w2 calculation
      [b] = quadsolve(2*K,-diag(K),ones(1,l),1,10000);
    end
    r=b'*diag(K)-b'*K*b;
    r=abs(r);
  else
    r = mean(diag(K))-mean(mean(K));
  end

  w = a'*H*a; %w = sum(a); %
  bound=r*w;

  XXtemp=X*X';
  if p1>1
      XXtemp=XXtemp+1;
  end

  grad= zeros(scale+slack,1);

  for i=1:scale                       %% calculate scaling factors
    Xc = Xkeep(:,find(i==scales));
    Kc = Xc*Xc';
    switch ker
        case {'weighted_poly', 'weighted_linear'}
            Kgrad= 2 * sigma(i) *  (Kc * p1) .* (XXtemp).^(p1-1);
        case 'rbf'
            x_k=Xc;
            nbx=size(x_k,1);
            deriv=(x_k*ones(1,nbx)-ones(nbx,1)*x_k').^2;
            Kgrad = (deriv.*K)./(p1^2);
    end
    gradw= -(a.*Y)'*Kgrad*(a .* Y);
    if var2==0
      gradr= b'*diag(Kgrad)- b'*Kgrad*b;
    else
      gradr= mean(diag(Kgrad))-mean(mean(Kgrad));
    end
    grad(i)=  (gradw*r + gradr*w);
  end;

  for i=1:slack                       %% calculate slacking factors
    rs=ridge; rs(find(i~=slacks))=0;
    Kgrad=2*diag(rs);
    gradw= -(a.*Y)'*Kgrad*(a .* Y);
    if var2==0
      gradr= b'*diag(Kgrad)- b'*Kgrad*b;
    else
      gradr= mean(diag(Kgrad))-mean(mean(Kgrad));
    end
    grad(i+scale)=  (gradw*r + gradr*w);
  end


% =========================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM%
% 2D Multilevel LDDMM Example using stationary velocity field and
% diffusion regularizer.The example is described in detail in
% Section 4.2 of the paper:
%
% @article{MangRuthotto2017,
%   Title = {A {L}agrangian {G}auss--{N}ewton--{K}rylov solver for mass- and intensity-preserving diffeomorphic image registration},
%   Year = {2017},
%   Journal = {SIAM Journal on Scientific Computing},
%   Author = {A. Mang, L. Ruthotto},
% }
% =========================================================================

close all; clear all; clc;
setup2Ddisc2CData

%% run affine pre-registration
imgModel('reset','imgModel','splineInterMex','regularizer','moments','theta',.1);

alpha = [4e2 0];
parametric = 0;
pad  = .5;
N    = 3;
mV     = @(m) ceil(1*m);
minLevel = 5;
maxLevel = 7;

%% run multilevel LDDMM registration

% 1) setup grid for velocities (padded)
omegaV = omega; omegaV(1:2:end) = omegaV(1:2:end)-pad;  omegaV(2:2:end) = omega(2:2:end)+pad;

% 2) setup regularizer
regularizer('reset','regularizer','mbDiffusionCC','nt',0,'alpha',alpha,'HessianShift',1e-2);

NPIRpara         = optPara('NPIR-GN');
NPIRpara.maxIter = 40;
NPIRpara.scheme  = @GaussNewtonLDDMM;
[vc,~,wc,his] = MLLDDMM(ML,'minLevel',minLevel,'maxLevel',maxLevel,...
    'omegaV',omegaV,'mV',mV,'N',N,'parametric',parametric,'NPIRpara',NPIRpara,'plots',1);

%% show results
yc = getTrafoFromVelocityRK4(vc,getNodalGrid(omega,m),'omega',omegaV,'m',m,'tspan',[1,0],'N',N);
Topt = linearInterMex(dataT,omega,center(yc,m));
Jac = geometry(yc,m,'Jac','omega',omega);
D0  = distance(dataT(:),dataR(:),omega,m);
DOpt = distance(Topt(:),dataR(:),omega,m);

fig = figure(); clf;
fig.Name = sprintf('LDDMM Results: %s',mfilename);

subplot(2,3,1);
viewImage(dataR,omega,m);
title('reference');

subplot(2,3,4);
viewImage(dataT,omega,m);
hold on;
plotGrid(yc,omega,m,'spacing',4)
title('template');

subplot(2,3,2);
viewImage(Topt,omega,m);
title('T(yc)')

subplot(2,3,3);
viewImage(dataT(:)-dataR(:),omega,m);
title('init. residual, SSD=100%');

subplot(2,3,5);
viewImage2Dsc(Jac,omega,m);
title(sprintf('Jac, min=%1.2f max=%1.2f',min(Jac(:)),max(Jac(:))));

subplot(2,3,6);
viewImage(Topt(:)-dataR(:),omega,m);
title(sprintf('opt residual, SSD=%1.2f%%',100*DOpt/D0));


function [key,sdet]=posdef(A)
[nr,nc]=size(A); sdet=[];
for i=1:nr
   sdet=[sdet,det(A(1:i,1:i))];
end
key=1;
if any(sdet<=0), key=0; end


function [k, rbfPart, n2] = sqexpKernCompute(kern, x, x2)

% SQEXPKERNCOMPUTE Compute the SQEXP kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the pre-built compound squared exponential
% kernel given inputs associated with rows and columns.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : the input matrix associated with the rows of the kernel.
% ARG x2 : the input matrix associated with the columns of the kernel.
% RETURN k : the kernel matrix computed at the given points.
%
% FORMAT
% DESC computes the kernel matrix for the pre-built compound squared exponential
% kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : the kernel matrix computed at the given points.
%
% SEEALSO : sqexpKernParamInit, kernCompute, kernCreate, sqexpKernDiagCompute
%
% COPYRIGHT : Neil D. Lawrence, 2004

% KERN

if nargin < 3
  n2 = dist2(x, x);
  wi2 = (.5 .* kern.inverseWidth);
  rbfPart = kern.rbfVariance*exp(-n2*wi2);
  k = rbfPart + kern.whiteVariance*eye(size(x, 1));
else
  n2 = dist2(x, x2);
  wi2 = (.5 .* kern.inverseWidth);
  rbfPart = kern.rbfVariance*exp(-n2*wi2);
  k = rbfPart;
end
k = k + kern.biasVariance;


function [C, S] = QLiftDec2MinMin(X, N)
%-----------------------------------------------------------------------------
% QLiftDec2MinMin
% Multilevel 2-D decomposition by the lifting scheme and using quincunx grids
%
% The MinMin scheme has been proposed by Heijmans and Goutsias, see e.g.
%    H.J.A.M. Heijmans, J. Goutsias,
%    Multiresolution signal decomposition schemes.
%    Part 2: morphological wavelets.
%    CWI Report PNA-R9905, Amsterdam, 1999.
%    http://repository.cwi.nl:8888/cwi_repository/docs/IV/04/04625D.pdf
%
% Calls for: QLmaxlev,
%            storeQ1001, storeR,
%            getcolor01, getcolor10, getcolor00, getcolor11,
%            putcolor01, putcolor10, putcolor00, putcolor11.
% See also: QLiftRec2MaxMin
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: February 3, 2003.
% (c) 1999-2003 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
%Firstly, check input data
%
if  isempty(X)
  error(' QLiftDec2MinMin - empty matrix ');
else
  if mod(N, 2) == 1
    error(' QLiftDec2MinMin - only an even number of levels is accepted ');
  end
  if QLmaxlev(size(X), 'minmin') < N
    error(' QLiftDec2MinMin - too many levels requested ');
  end
  if N < 2
    disp([' QLiftDec2MinMin - WARNING too few levels requested ' ...
          '-> empty decomposition ']);
  end
end
%
%Secondly, start decomposition
%
O = X; % For the sake of efficient use of memory this could be improved upon.
% We descend to coarser grids, integer lev indicates number of scale.
C = []; S = [];

for lev=1:2:N
%
   [nO, mO] = size(O);
   if ( nO < 3 ) || ( mO < 3)
     error(' QLiftDec2MinMin - too many levels ');
   end
   minO = min(min(O));
   maxO = max(max(O));
%  cmin = minO-(maxO-minO);
   cmax = maxO+(maxO-minO);
%
%  The Lifting Scheme proceeds from a rectangular grid
%  towards a quincunx grid.
%
%  Stage: predict
   A00 =getcolor00(O);
   A11 =getcolor11(O);
%  Quincunx grid Q0011 is the union of the values at .00 and .11: "even slots"
%  Quincunx grid Q1001 is the union of the values at .10 and .01: "odd slots"
   Q1001D01 = getcolor01(O) - synA01min(A11, A00, cmax);                % Y1
   Q1001D10 = getcolor10(O) - synA10min(A11, A00, cmax);                % Y1
%  At this point the union (quincunx) of Q1001D01 & Q1001D10
%  contains the DETAILS of O.
%
%  For the inverse transform Q1001D01 and Q1001D10 have to be stored:
   [C, S] = storeQ1001( Q1001D10, Q1001D01, lev, 'd', C, S);
%
%  Stage: update
   Q0011A00 = A00 + ...
        min(zeros(size(A00)), synA00min(Q1001D10, Q1001D01, cmax));     % X1
   clear A00;
   Q0011A11 = A11 + ...
        min(zeros(size(A11)), synA11min(Q1001D10, Q1001D01, cmax));     % X1
   clear A11 Q1001D10 Q1001D01;
%  At this point the union (quincunx) of Q0011A00 & Q0011A11
%  contains the updated APPROXIMATION of O, the DETAILS of O
%  were in the union (quincunx) of Q1001D01 & Q1001D10 (see above).
%
%  The Lifting Scheme proceeds by a subsequent step from quincunx
%  to rectangular grid.
%
%  Q0011 is split into the 11 colour with the "odd slots" and
%  the 00 colour with the "even slots".
%
%  Stage: predict
   DETAIL11 = Q0011A11 - synA11Qmin(Q0011A00, size(Q0011A11), cmax);        % Y2
   clear Q0011A11;
%  Stage: update
   APPROX00 = Q0011A00 + ...
     min(zeros(size(Q0011A00)), synA00Qmin(DETAIL11, size(Q0011A00), cmax));% X2
%
%  DETAIL11 presents the detail gridfunction w.r.t. Q0011
%  APPROX00 now represents the updated version of the approximation of Q0011
%
%  For the inverse transform DETAIL11 has to be stored:
   [C, S] = storeR( DETAIL11, lev+1, 'd', C, S);
   clear Q0011A00 DETAIL11;
%
%  At this point gridfunction DETAIL11 containing the DETAILS has been stored,
%  gridfunction APPROX00 contains the updated APPROXIMATION, on the (down-
%  sampled) rectangular grid and has to be stored as well if at the highest
%  scale.
%  Note that APPROX00 is downsampled onto a rectangular grid with dimensions of
%  half size of the original O.
   if lev+1 >= N
     [C, S] = storeR(APPROX00, lev+1, 'a', C, S);
%    It is obligatory that at least at one scale the Approximation has to be
%    stored or else the scheme cannot be inverted.
     clear APPROX00;
%    In the Lifting Scheme all scales have now been processed!
   else
%    We proceed to the next scale.
     O = APPROX00; clear APPROX00;
   end
end
%-----------------------------------------------------------------------------


function sphere_integrals_test ( )

%*****************************************************************************80
%
%% SPHERE_INTEGRALS_TEST tests the SPHERE_INTEGRALS library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 January 2014
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_INTEGRALS_TEST:\n' )
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the SPHERE_INTEGRALS library.\n' );

  sphere_integrals_test01 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_INTEGRALS_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function LSHparam = trainLSH(LSHparam)

% Input:
%          LSHparam
%              LSHparam.nbits---number of bits (nbits do not need to be a multiple of 8)
% Output:
%             LSHparam:
%                 LSHparam.w---random projection

dim = LSHparam.dim;
nbits = LSHparam.nbits;

W = randn(dim, nbits);

LSHparam.w = W;


classdef meanshift_G_is_Gaussian < dagnn.ElementWise
    properties
        delta=0.1
    end
    properties % (Transient)
        numInputs
        SIZE_=[]
    end

    % [TODO]: current version only supports batchSize=1; need to extend to
    % multiple input images
    methods
        function outputs = forward(obj, inputs, params)
            obj.numInputs = numel(inputs);
            % obj.SIZE_ = inputs{2};
            %outputs{1} = exp((inputs{1}-1)/(obj.delta^2));

            outputs{1} = exp( -0.5*(inputs{1}.^2)/(obj.delta^2));
        end

        function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
            derInputs = cell(1, numel(inputs));
            dzdy = derOutputs{1};
            S = inputs{1};
            %derInputs{1} = dzdy.* ((1/obj.delta^2)* exp((S-1)/(obj.delta^2)));

            derInputs{1} = dzdy.* (-1/(obj.delta^2)*inputs{1}).*exp(-0.5*(inputs{1}.^2)/(obj.delta^2));

            derParams = {} ;
        end

        function obj = meanshift_G_is_Gaussian(varargin)
            obj.load(varargin) ;
        end
    end
end


function [psd, ft] = gsp_jtv_estimate_psd(G, X, param)
%GSP_JTV_ESTIMATE_PSD Estimate the PSD of a time vertex process
%   Usage:  psd = gsp_jtv_estimate_psd(G, X)
%           psd = gsp_jtv_estimate_psd(G, X, param)
%
%   Input parameters:
%         G          : Graph
%         X          : Signal(s) (matrix of NxTxR)
%         param      : Optional parameters
%   Output parameters:
%         psd        : PSD matrix
%         ft         : Filter type
%
%   This function estimates the PSD for vertex time processes. It is a
%   generalization of the Bartlett method.
%
%   Additional parameters
%   ---------------------
%
%   * *param.estimator*  : Method used for the estimation. By default 'TA'
%     - 'TA': Average over the time domain.
%     - 'FC': Fourier convolution a trick used to reduce the computation.
%     - 'VA': Average over the vertex domain
%     - 'TVA': Average over the time and the vertex domain
%     - 'FA' : Average on multiple realization in the joint spectral domain
%   * *param.L*       : Length of the window for the 'GSTFT' method
%   * *param.kernel*  : Kernel for the convolution for 'FC' method.
%
%
%   References: perraudin2016stationary

% Author : Nathanael Perraudin
% Date: 6 January 2016
% Testing: test_gsp_estimate_vertex_time_psd

if nargin<3
    param = struct;
end
if ~isfield(param,'estimator')
    param.estimator = 'FA';
end

% number of realizations
[N, T, R] = size(X);

switch param.estimator

    % This method compute the Fourier transform of the signals and average
    % the modulus
    case 'FA'
        if ~isfield(param, 'L')
            param.L = compute_default_L(X,T,R);
        end
        Xhat = gsp_jft(G,reshape(X,N,param.L,R*T/param.L));
        psd = mean(abs(Xhat).^2,3);
        ft = 'js';

    % This method splits time in windows, and takes an average of the psd
    % over each time window. The size of each window (L) should be
    % equal to the maximum correlation distance in the process
    case 'TA'

        if ~gsp_check_fourier(G)
            error('The Fourier basis is needed the Fourier basis')
            %G = gsp_compute_fourier_basis(G);
        end

        if ~isfield(param, 'L')
            param.L = compute_default_L(X,T,R);
        end
        if ~isfield(param, 'a'),       param.a = round(param.L/2); end
        if ~isfield(param, 'M'),       param.M = G.jtv.T; end
        if ~isfield(param,'win_type'), param.win_type = 'itersine'; end

        if round(T/param.L) - (T/param.L) ~= 0
            warning('Optimally the length of the signal should be divisible by param.L.')
            while round(T/param.L) - (T/param.L) ~= 0
                param.L = param.L + 1;
            end
        end

        % Compute a Gabor window
        w = gabwin(param.win_type, param.a, param.L);

        % estimate the psd as the average over each realization
        psd = zeros(G.N, G.jtv.T);
        for r = 1:R

            % do a windowed JFT
            coeff = gsp_jtwgft(G, w, X(:,:,r), param);

            % compute the psd for each window
            psd_win = abs(coeff(:, 1:end-1, :)).^2;

            % average all windows to obtain an estimate of the psd
            psd_est = squeeze(mean(psd_win, 2));

            % average over realizations
            psd = psd + transpose(psd_est);
        end

        psd = psd / R;

        % normalize energy
        psd = psd * (norm(X(:), 'fro')^2/R / sum(psd(:)));

    % Go to the joint frequency domain and smooth out the PSD with a kernel.
        ft = 'js';

    case 'FC'
        if ~isfield(param, 'kernel')
            param.kernel = exp(-(-20:20).^2/3);
        end
        h = param.kernel;

        if ~gsp_check_fourier(G)
            G = gsp_compute_fourier_basis(G);
        end

        Xhat2 = abs(gsp_jft(G,X)).^2;

        psd = conv2(h', h', mean(Xhat2,3), 'same') / norm(h, 1)^2;
%         psd = conv2(mean(Xhat2,3), param.kernel, 'same') / norm(param.kernel, 1);

        % normalize energy
        psd = psd * (norm(X(:), 'fro')^2/R / sum(psd(:)));
        ft = 'js';

    % For scalability,
    case 'VA'

        if ~isfield(G,'boundary'); param.boundary = 'periodic'; end

        switch param.boundary

            case 'periodic'
                Xhat = fft(X,[],2)/sqrt(T);

            case 'reflecting'
                error('Sorry. Not implemented yet...')

            otherwise
                error('Unknown boundary condition');
        end

        if ~isfield(G,'lmax')
            G = gsp_estimate_lmax(G);

            warning(['GSP_PSD_ESTIMATION: The variable lmax is not ',...
                'available. The function will compute it for you. ',...
                'However, if you apply many time this function, you ',...
                'should precompute it using the function: ',...
                'gsp_estimate_lmax']);
        end

        if ~isfield(param,'Nfilt'),   param.Nfilt = 50; end
        if ~isfield(param,'Nrandom'), param.Nrandom = max(10, R); end
        if ~isfield(param,'g0')
            sigma = sqrt(2*G.lmax/param.Nfilt^2 * (param.Nfilt + 1));
            param.g0 = @(x) exp(-x.^2/sigma^2);
        end

        % Design the frame
        [ g , mu ] = gsp_design_translates(G, param.g0, param.Nfilt);
        %[ g , mu ] = gsp_design_itersine(G,Nfilt );

        % Estimate the energy of the window
        if gsp_check_fourier(G)
            mu_y2 = sum(abs(gsp_filter_evaluate(g,G.e)).^2,1)';
        else
            w = randn(N, param.Nrandom);
            x_filt2    = gsp_vec2mat( gsp_filter(G,g,w,param), param.Nfilt);
            n2_x_filt2 = sum(abs(x_filt2).^2,1);
            mu_y2      = reshape(mean(n2_x_filt2,3),[],1);
        end

        psd = cell(1,T);

        for ii = 1:T
            % Perform the filtering
            x_filt = gsp_vec2mat( ...
                gsp_filter(G, g, squeeze(Xhat(:,ii,:)), param), ...
                param.Nfilt);
            % estimate the points
            n2_x_filt = sum(abs(x_filt).^2,1);
            mu_y = reshape(mean(n2_x_filt,3),[],1);

            % Interpolate to obtain a nice filter.
            psd{ii} = @(s) max(spline(mu,mu_y./mu_y2,s), 0);
        end
        ft = 'js-array';

   case 'TVA'

        if ~isfield(param, 'L')

            threshold = 0.05;

            % compute the average correlation distance
            C_T = zeros(T); Toep = toeplitz(0:(T-1)); Tmax = round(0.5*T);

            for r = 1:R, C_T = C_T + abs(X(:,:,r)'*X(:,:,r)) / R; end
            cor = zeros(Tmax,1);
            for t = 1:Tmax, cor(t) = mean( vec(C_T(Toep == t-1)) ); end
            cor = normalize_data(cor, 0, 1);

            % select the window length conservatively
            param.L = min(4*find(cor>=threshold, 1, 'last' ), T);

            % visualize the correlation
            % figure; plot(0:(Tmax-1), cor, '-o', [1 1]*param.L, [0 1],
            % 'r-'); xlabel('correlation distance');

            % param.L = round(T/4);
        end

       % Make sure that the length of the signal should be divisible by param.L
       if round(T/param.L) - (T/param.L) ~= 0
           % warning('Ideally the length of the signal should be divisible by param.L.')
           while (round(T/param.L) - (T/param.L) ~= 0) || (round(param.L/2) - (param.L/2) ~= 0)
              param.L = param.L + 1;
           end
       end

       if ~isfield(param, 'a'),       param.a = round(param.L/2);  end
       if ~isfield(param, 'M'),       param.M = G.jtv.T;           end
       if ~isfield(param,'win_type'), param.win_type = 'itersine'; end
       if ~isfield(G,'boundary');     param.boundary = 'periodic'; end

       % Compute a Gabor window
       w = gabwin(param.win_type, param.a, param.L);

        switch param.boundary

            case 'periodic'
                S = zeros(N, param.M, ceil(T/param.a), R);
                for r = 1:R
                    % do a discrete gabore transform
                    X_gabore = dgt(transpose(X(:,:,r)), w, param.a, param.M);
                    S(:,:,:,r) = permute( X_gabore, [3,1,2]);
                end
            case 'reflecting'
                error('Sorry. Not implemented yet...')
            otherwise
                error('Unknown boundary condition');
        end

        if ~isfield(G,'lmax')
            G = gsp_estimate_lmax(G);

            warning(['GSP_PSD_ESTIMATION: The variable lmax is not ',...
                'available. The function will compute it for you. ',...
                'However, if you apply many time this function, you ',...
                'should precompute it using the function: ',...
                'gsp_estimate_lmax']);
        end

        if ~isfield(param,'Nfilt'),   param.Nfilt = 50;          end
        if ~isfield(param,'Nrandom'), param.Nrandom = max(10,R); end
        if ~isfield(param,'g0')
            sigma = sqrt(2*G.lmax/param.Nfilt^2 * (param.Nfilt + 1));
            param.g0 = @(x) exp(-x.^2/sigma^2);
        end

        % Design the frame
        [ g , mu ] = gsp_design_translates(G, param.g0, param.Nfilt);
        %[ g , mu ] = gsp_design_itersine(G,Nfilt );

        % Estimate the energy of the window
        if gsp_check_fourier(G)
            mu_y2 = sum(abs(gsp_filter_evaluate(g, G.e)).^2, 1)';
        else
            w = randn(N, param.Nrandom);
            x_filt2 = gsp_vec2mat( gsp_filter_analysis(G,g,w,param), param.Nfilt);
            n2_x_filt2 = sum(abs(x_filt2).^2, 1);
            mu_y2 = reshape(mean(n2_x_filt2,3), [], 1);
        end

        psd = cell(1, param.M);

%         if gsp_check_fourier(G) && param.use_fast
%             warning('This may use a lot of ram!')
%             [N, T, Nt, Ns ] = size(S);
%             Nf = numel(g);
%             ShatG = gsp_gft(G,S);
%             filt_eval = gsp_filter_evaluate(g, G.e);
%             filt_eval = repmat(reshape(filt_eval,[N,1,1,1,Nf]), 1,T,Nt,Ns ,1);
%             % Perform the filtering
%             S_filt = gsp_igft(G, repmat(ShatG,[1,1,1,1,Nf]) .* filt_eval);
%             n2_S_filt = squeeze(mean(mean(sum(abs(S_filt).^2,1),3),4));
%             for ii = 1:param.M
%
%                 % Interpolate to obtain a nice filter.
%                 psd{ii} = @(s) max(spline(mu,reshape(n2_S_filt(ii,:),[],1)./mu_y2,s), 0);
%             end
%         else
        for ii = 1:param.M

            % Perform the filtering
            x_filt = gsp_vec2mat( ...
                gsp_filter(G, g, reshape(S(:,ii,:,:),N,[]), param), ...
                param.Nfilt);

            % estimate the points
            n2_x_filt = sum(abs(x_filt).^2,1);
            mu_y = reshape(mean(n2_x_filt,3),[],1);

            % Interpolate to obtain a nice filter.
            psd{ii} = @(s) max(spline(mu,mu_y./mu_y2,s), 0);
        end
%         end

        ft = 'js-array';

    otherwise
        error('Unknown method')
end

end

function [x] = normalize_data(x, xmin, xmax)
%NORMALIZE_VECTOR Normalize x between xmin and xmax
%   x might be a scalar, vector, or matrix.

%normalize to [0,1]
x = (x - min(min(x))) ./ (max(max(x)) - min(min(x)));

if exist('xmax', 'var') && exist('xmin', 'var')
   x = x .* (xmax - xmin) + xmin;
end

end

function L =  compute_default_L(X,T,R)
% compute the correlation distance
C_T = zeros(T);
for r = 1:R
    C_T = C_T + abs(X(:,:,r)'*X(:,:,r)) / R;
end
Toep = toeplitz(0:(T-1));
Tmax = round(0.5*T);
cor = zeros(Tmax,1);
for t = 1:Tmax
    cor(t) = mean( vec(C_T(Toep == t-1)) );
end
cor = normalize_data(cor, 0, 1);
threshold = 0.05;
L = min(2*find(cor>=threshold, 1, 'last'), T);

%L = round(T/4);
%figure; plot(0:(Tmax-1), cor, '-o', [1 1]*param.L, [0 1], 'r-');

end


function [X,Y,vals,labI]=mp_azim(optn,varargin)
% MP_AZIM Azimuthal projections
%           This function should not be used directly; instead it is
%           is accessed by various high-level functions named M_*.

% Rich Pawlowicz (rich@ocgy.ubc.ca) 2/Apr/1997
%
%         13/5/97 - Added satellite perspective
%          1/6/97 - Another stab at removing some /0 errors.
%         10/8/00 - Rotation for projections?
%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.
%
% Mathematical formulas for the projections and their inverses are taken from
%
%      Snyder, John P., Map Projections used by the US Geological Survey,
%      Geol. Surv. Bull. 1532, 2nd Edition, USGPO, Washington D.C., 1983.
%
% These are azimuthal projections, best suited for circular areas. The
% stereographic is commonly used for polar regions.
%   Stereographic  - conformal
%   Orthographic   - neither conformal nor equal-area, but looks like globe
%                    with viewpoint at infinity.
%   Azimuthal Equal-area  - equal area, but not conformal (by Lambert)
%   Azimuthal Equidistant - distance and direction from center are true
%   Gnomonic       - all great circles are straight lines.
%   Satellite      - a perspective view from a finite distance

global MAP_PROJECTION MAP_VAR_LIST

name={'Stereographic','Orthographic','Azimuthal Equal-area','Azimuthal Equidistant','Gnomonic','Satellite'};

pi180=pi/180;

switch optn

  case 'name'
     X=name;

  case {'usage','set'}
     X=char({['     ''' varargin{1} ''''],...
              '     <,''lon<gitude>'',center_long>',...
              '     <,''lat<itude>'', center_lat>',...
              '     <,''rad<ius>'', ( degrees | [longitude latitude] ) | ''alt<itude>'', alt_frac >',...
              '     <,''rec<tbox>'', ( ''on'' | ''off'' | ''circle'' )>',...
	      '     <,''rot<angle>'', degrees CCW>'});

  case 'get'

     X=char([' Projection: ' MAP_PROJECTION.name '  (function: ' MAP_PROJECTION.routine ')'],...
            [' center longitude: ' num2str(MAP_VAR_LIST.ulong) ],...
            [' center latitude: ' num2str(MAP_VAR_LIST.ulat) ],...
            [' radius/altitude : ' num2str(MAP_VAR_LIST.uradius) ],...
            [' Rectangular border: ' MAP_VAR_LIST.rectbox ],...
	    [' Rotation angle: ' num2str(MAP_VAR_LIST.rotang) ]);

  case 'initialize'

    MAP_VAR_LIST=[];
    MAP_PROJECTION.name=varargin{1};
    MAP_VAR_LIST.ulong=0;
    MAP_VAR_LIST.ulat=60;
    MAP_VAR_LIST.rectbox='circle';
    MAP_VAR_LIST.uradius=90;
    MAP_VAR_LIST.rotang=0;
    MAP_VAR_LIST.ellipsoid='normal';
    k=2;
    while k<length(varargin)
      switch varargin{k}(1:3)
         case 'lon'
           MAP_VAR_LIST.ulong=varargin{k+1}(:)';
         case 'lat'
           MAP_VAR_LIST.ulat=varargin{k+1}(:)';
         case {'rad','alt'}
           MAP_VAR_LIST.uradius=varargin{k+1};
         case 'rec'
           MAP_VAR_LIST.rectbox=varargin{k+1};
	 case 'rot'
	   MAP_VAR_LIST.rotang=varargin{k+1};
         otherwise
           disp(['Unknown option: ' varargin{k}]);
      end
      k=k+2;
    end
    if strcmp(MAP_VAR_LIST.rectbox,'off'), MAP_VAR_LIST.rectbox='circle'; end

    MAP_VAR_LIST.rlong=MAP_VAR_LIST.ulong*pi180;
    MAP_VAR_LIST.rlat=MAP_VAR_LIST.ulat*pi180;

    % Compute the various limits
    % For the perspective viewpoints, we can't go *quite* to the horizon because this causes
    % problems in the clipping later on - but we can get pretty close (within .995) of it.
    % This is a fudge factor that can probably be changed if we increase the number of points
    % in grid lines...

    if length(MAP_VAR_LIST.uradius)==1
      if strcmp(MAP_PROJECTION.name,name{2}) && abs(MAP_VAR_LIST.uradius-90)<.2
        MAP_VAR_LIST.radius=89.8;
      elseif strcmp(MAP_PROJECTION.name,name{5})
        MAP_VAR_LIST.radius=min(80,MAP_VAR_LIST.uradius);
      elseif strcmp(MAP_PROJECTION.name,name{6})
        MAP_VAR_LIST.radius=acos(1/(1+MAP_VAR_LIST.uradius))/pi180*.98; % uradius is the height fraction here
      else
        MAP_VAR_LIST.radius=MAP_VAR_LIST.uradius;
      end
      rradius=MAP_VAR_LIST.radius*pi180;
    else
      % do some sperical trig
      edge=MAP_VAR_LIST.uradius*pi180 - [MAP_VAR_LIST.rlong 0];
      cosc=sin(MAP_VAR_LIST.rlat)*sin(edge(2))+cos(MAP_VAR_LIST.rlat)*cos(edge(2))*cos(edge(1));
      sinc=sqrt( ( cos(edge(2))*sin(edge(1)))^2 + ...
                 (cos(MAP_VAR_LIST.rlat)*sin(edge(2))-sin(MAP_VAR_LIST.rlat)*cos(edge(2))*cos(edge(1)))^2);
      rradius=atan2(sinc,cosc);
      MAP_VAR_LIST.radius=rradius/pi180;
    end
    MAP_VAR_LIST.cosradius=cos(rradius);

    switch MAP_PROJECTION.name
      case name(1)
        MAP_VAR_LIST.rhomax=2*tan(rradius/2);
      case name(2)
        MAP_VAR_LIST.rhomax=sin(rradius);
      case name(3)
        MAP_VAR_LIST.rhomax=2*sin(rradius/2);
      case name(4)
        MAP_VAR_LIST.rhomax=rradius;
      case name(5)
        MAP_VAR_LIST.rhomax=tan(rradius);
      case name(6)
        MAP_VAR_LIST.rhomax=sin(rradius)/(1+(1-cos(rradius))/MAP_VAR_LIST.uradius);
    end

    if strcmp(MAP_VAR_LIST.rectbox,'on')
      if length(MAP_VAR_LIST.uradius)==1
        MAP_VAR_LIST.xlims=[-1 1]/sqrt(2)*MAP_VAR_LIST.rhomax;
        MAP_VAR_LIST.ylims=[-1 1]/sqrt(2)*MAP_VAR_LIST.rhomax;
      else
        [X,Y]=mp_azim('ll2xy',MAP_VAR_LIST.uradius(1),MAP_VAR_LIST.uradius(2),'clip','off');
        MAP_VAR_LIST.xlims=[-abs(X) abs(X)];
        MAP_VAR_LIST.ylims=[-abs(Y) abs(Y)];
      end
    else
      MAP_VAR_LIST.xlims=[-MAP_VAR_LIST.rhomax MAP_VAR_LIST.rhomax];
      MAP_VAR_LIST.ylims=[-MAP_VAR_LIST.rhomax MAP_VAR_LIST.rhomax];
    end

    mu_util('lllimits');

  case 'll2xy'

    long=varargin{1}*pi180-MAP_VAR_LIST.rlong;
    lat=varargin{2}*pi180;
    vals=zeros(size(long));

    pi180=pi/180;
    cosc     =sin(MAP_VAR_LIST.rlat)*sin(lat)+cos(MAP_VAR_LIST.rlat)*(cos(lat).*cos(long));
    sinAzsinc=sin(long).*cos(lat);
    cosAzsinc=cos(MAP_VAR_LIST.rlat)*sin(lat)-sin(MAP_VAR_LIST.rlat)*(cos(lat).*cos(long));
    sinc=sqrt(sinAzsinc.^2+cosAzsinc.^2);

    switch MAP_PROJECTION.name
      case name(1)
        cosc(cosc==-1)=-1+eps;
        rho=2*sinc./(1+cosc);  % = 2*tan(c/2)
      case name(2)
        rho=sinc;   % = sinc
      case name(3)
        cosc(cosc==-1)=-1+eps;
        rho=sqrt(2)*sinc./sqrt(1+cosc);   % = 2*sin(c/2)
      case name(4)
        rho=atan2(sinc,cosc); % = c
      case name(5)
        rho=sinc./cosc; % = tan(c)
      case name(6)
        rho=sinc./(1+(1-cosc)/MAP_VAR_LIST.uradius); %
    end

    sinc(sinc==0)=eps;
    Az=(sinAzsinc+sqrt(-1)*cosAzsinc)./sinc;
    Az(abs(Az)==0)=-1;

    % Clip out-of-range values. We test against cos(c) (where c is the angular
    % distance from map center) rather than directly against rhomax, because
    % in the orthographic map rho->0 for points on the other side of the
    % globe whereas c does not!

    % Also, we clip on rho even if we later clip on X/Y because in some projections (e.g. the
    % orthographic) the X/Y locations wrap back.
    if ~strcmp(varargin{4},'off')
        vals = vals | cosc<=MAP_VAR_LIST.cosradius+eps*10;
        [rho,Az]=mu_util('clip',varargin{4},rho,MAP_VAR_LIST.rhomax,cosc<MAP_VAR_LIST.cosradius,Az);
        Az=Az./abs(Az);
    end

     X=rho.*real(Az*exp(i*pi180*MAP_VAR_LIST.rotang));
     Y=rho.*imag(Az*exp(i*pi180*MAP_VAR_LIST.rotang));

    if strcmp(MAP_VAR_LIST.rectbox,'on')  && ~strcmp(varargin{4},'off')
        vals= vals | X<=MAP_VAR_LIST.xlims(1)+eps*10 | X>=MAP_VAR_LIST.xlims(2)-eps*10 | ...
                     Y<=MAP_VAR_LIST.ylims(1)+eps*10 | Y>=MAP_VAR_LIST.ylims(2)-eps*10;
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(1),X<MAP_VAR_LIST.xlims(1) | isnan(X),Y);
        [X,Y]=mu_util('clip',varargin{4},X,MAP_VAR_LIST.xlims(2),X>MAP_VAR_LIST.xlims(2) | isnan(X),Y);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(1),Y<MAP_VAR_LIST.ylims(1) | isnan(Y),X);
        [Y,X]=mu_util('clip',varargin{4},Y,MAP_VAR_LIST.ylims(2),Y>MAP_VAR_LIST.ylims(2) | isnan(Y),X);
    end

  case 'xy2ll'

    rho=sqrt(varargin{1}.^2+varargin{2}.^2);
    Z=exp(i*(atan2(varargin{2},varargin{1})-MAP_VAR_LIST.rotang*pi180));
    V1=rho.*real(Z);
    V2=rho.*imag(Z);

    ir=rho==0;  % To prevent /0 warnings when rho is 0
    rho(ir)=eps;

    switch MAP_PROJECTION.name
      case name(1)
        c=2*atan(rho/2);
      case name(2)
        c=asin(rho);
        c(abs(rho)>1.0)=NaN;  % points outside the map
      case name(3)
        c=2*asin(rho/2);
         c(abs(rho)>2.0)=NaN;  % points outside the map
     case name(4)
        c=rho;
      case name(5)
        c=atan(rho);
      case name(6)
        arg1=(MAP_VAR_LIST.uradius+1)./sqrt(1+(MAP_VAR_LIST.uradius./rho).^2);
        c=asin(arg1) - atan(rho/MAP_VAR_LIST.uradius);
        c(arg1>1.0)=NaN;
    end
    c(ir)=eps; % we offset this slightly so that the correct limit is achieved in the
               % division below:

%    Y=(asin(cos(c)*sin(MAP_VAR_LIST.rlat) + ...
%            cos(MAP_VAR_LIST.rlat)*sin(c).*varargin{2}./rho))/pi180;
%
%    switch MAP_VAR_LIST.ulat,
%      case 90,
%        X=(MAP_VAR_LIST.rlong+atan2(varargin{1},-varargin{2}))/pi180;
%      case -90,
%        X=(MAP_VAR_LIST.rlong+atan2(varargin{1},varargin{2}))/pi180;
%      otherwise
%        X=(MAP_VAR_LIST.rlong+atan2( varargin{1}.*sin(c), ...
%          cos(MAP_VAR_LIST.rlat)*cos(c).*rho - sin(MAP_VAR_LIST.rlat)*varargin{2}.*sin(c) ) )/pi180;
%     end;

   % Can be problem if the argument is slightly larger than 1 - then the asin
   % returns a complex number.
   arg=cos(c)*sin(MAP_VAR_LIST.rlat) + ...
            cos(MAP_VAR_LIST.rlat)*sin(c).*V2./rho;
    arg=min(max(arg,-1),1);

    Y=(asin(arg))/pi180;

    switch MAP_VAR_LIST.ulat
      case 90
        X=(MAP_VAR_LIST.rlong+atan2(V1,-V2))/pi180;
      case -90
        X=(MAP_VAR_LIST.rlong+atan2(V1,V2))/pi180;
      otherwise
        X=(MAP_VAR_LIST.rlong+atan2( V1.*sin(c), ...
          cos(MAP_VAR_LIST.rlat)*cos(c).*rho - sin(MAP_VAR_LIST.rlat)*V2.*sin(c) ) )/pi180;
    end

  case 'xgrid'

    [X,Y,vals,labI]=mu_util('xgrid',MAP_VAR_LIST.longs,MAP_VAR_LIST.lats,varargin{1},31,varargin{2:3});

  case 'ygrid'

    [X,Y,vals,labI]=mu_util('ygrid',MAP_VAR_LIST.lats,MAP_VAR_LIST.longs,varargin{1},91,varargin{2:3});

  case 'box'

     [X,Y]=mu_util('box',31);

end


classdef REMO < ALGORITHM
% <multi/many> <real> <expensive>
% Expensive multiobjective optimization by relation learning and prediction
% k    ---    6 --- Number of reference solutions
% gmax --- 3000 --- Number of solutions evaluated by surrogate model

%------------------------------- Reference --------------------------------
% H. Hao, A. Zhou, H. Qian, and H. Zhang, Expensive multiobjective
% optimization by relation learning and prediction, IEEE Transactions on
% Evolutionary Computation, 2022.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameterr setting
            [k,gmax] = Algorithm.ParameterSet(6,3000);

            %% Initalize the population by Latin hypercube sampling
            if Problem.D <= 10
                N = 11*Problem.D-1;
            else
                N = 100;
            end
            PopDec     = UniformPoint(N,Problem.D,'Latin');
            Population = Problem.Evaluation(repmat(Problem.upper-Problem.lower,N,1).*PopDec+repmat(Problem.lower,N,1));
            Archive    = Population;

            %% Optimization
            while Algorithm.NotTerminated(Archive)
                % Select reference solutions and preprocess the data
                Ref       = RefSelect(Population,k);
                Input     = Population.decs;
                Catalog   = GetOutput_PBI(Population.objs,Ref.objs);
                [XXs,YYs] = GetRelationPairs(Input,Catalog);
                [TrainIn,TrainOut,TestIn,TestOut] = DataProcess(XXs,YYs);
                xDim = size(TrainIn,2);

                % Train relation model
                [TrainIn_nor,TrainIn_struct] = mapminmax(TrainIn');
                TrainIn_nor     = TrainIn_nor';
                TrainOut_onehot = onehotconv(TrainOut,1);
                net = patternnet([ceil(xDim*1.5),xDim*1,ceil(xDim/2)]);
                net.trainParam.showWindow =0;
                net        = train(net,TrainIn_nor',TrainOut_onehot');
                TestIn_nor = mapminmax('apply',TestIn',TrainIn_struct)';
                TestPre    = onehotconv(net(TestIn_nor')',2);
                p_err      = sum(TestPre ~= TestOut)/size(TestPre,1);
                Smodel.X   = Input;
                Smodel.Y   = Catalog;
                Smodel.mp_struct = TrainIn_struct;
                Smodel.net       = net;
                Smodel.p_err     = p_err;
                Next = RSurrogateAssistedSelection(Problem,Ref,Population.decs,gmax,Smodel);
                if ~isempty(Next)
                    Archive = [Archive,Problem.Evaluation(Next)];
                end
                Population = RefSelect(Archive,Problem.N);
            end
        end
	end
end


% DEMCMU35GPLVM3 Learn a GPLVM on CMU 35 data set.

% FGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

% Get the sequence numbers.
[Y, lbls] = lvmLoadData('cmu35WalkJog');
seq = cumsum(sum(lbls)) - [1:31];

dataSetName = 'cmu35gplvm';
experimentNo = 3;

% load data
[Y, lbls, Ytest, lblstest] = lvmLoadData(dataSetName);
origBias = mean(Y, 1);
origScale = 1./sqrt(var(Y, 1));
Y = Y - repmat(origBias, size(Y, 1), 1);
Ytest = Ytest - repmat(origBias, size(Ytest, 1), 1);
Y = Y.*repmat(origScale, size(Y, 1), 1);
Ytest = Ytest.*repmat(origScale, size(Ytest, 1), 1);

% Set up model
options = fgplvmOptions('fitc');
options.optimiser = 'conjgrad';
options.back = 'mlp';
options.backOptions = mlpOptions(10);
options.numActive = 100;
options.fixInducing = 1;
options.fixIndices = round(linspace(1, size(Y, 1), options.numActive));
latentDim = 5;

d = size(Y, 2);
model = fgplvmCreate(latentDim, d, Y, options);

% Add dynamics model.
optionsDyn = gpOptions('fitc');
optionsDyn.numActive = 100;
optionsDyn.fixInducing = 1;
optionsDyn.kern = kernCreate(model.X, {'rbf', 'white'});
optionsDyn.kern.comp{1}.inverseWidth = 0.2;
% This gives signal to noise of 0.1:5e-3 or 20:1.
optionsDyn.kern.comp{1}.variance = 0.01;
optionsDyn.kern.comp{2}.variance = 0.95;
diff = 1;
learn = 1;
optionsDyn.fixIndices = round(linspace(1, size(Y, 1)-length(seq), options.numActive));
model = fgplvmAddDynamics(model, 'gp', optionsDyn, diff, learn, seq);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
capName = dataSetName;
capName(1) = upper(capName(1));
save(['dem' capName num2str(experimentNo) '.mat'], 'model', 'origScale', 'origBias');


%% 参数设置
%% 基带数据数据产生
%% 信道编码（卷积码、或交织器）
%% qpsk调制
%% 扩频
%% 插入导频
%% 串并转换
%% IFFT
%% 插入保护间隔、循环前缀
%% 并串转换
%% DA
%% 上变频
%% 信道（通过多经瑞利信道、或信号经过AWGN信道）
%% 下变频
%% AD
%% 串并转换
%% 去掉保护间隔、循环前缀
%% FFT
%% 信道估计与插值（均衡）
%% 信道校正
%% 并串转换
%% 解扩
%% QPSK解调
%% （解交织）
%% 信道译码（维特比译码）
%% 计算误码率