%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% sh7.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function f = sh7(x)
% Shekel7 function
function f = sh7(x)
a = [4, 1, 8, 6, 3, 2, 5;
     4, 1, 8, 6, 7, 9, 5;
     4, 1, 8, 6, 3, 2, 3;
     4, 1, 8, 6, 7, 9, 3];
c = [0.1 0.2 0.2 0.4 0.4 0.6 0.3];
if size(x,1) == 1
 x = x';
end
for i=1:7
 b = (x - a(:,i)).^2;
 d(i) = sum(b);
end
f = -sum((c+d).^(-1));


function pce_ode_hermite_test01 ( )

%*****************************************************************************80
%
%% PCE_ODE_HERMITE_TEST01 runs a test problem with PCE_ODE_HERMITE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PCE_ODE_HERMITE_TEST01:\n' );
  fprintf ( 1, '  Call PCE_ODE_HERMITE to compute a polynomial chaos expansion\n' );
  fprintf ( 1, '  for the ODE:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    u'' = - alpha * u,\n' );
  fprintf ( 1, '    u(0) = 1.\n' );

  ti = 0.0;
  tf = 2.0;
  nt = 200;
  ui = 1.0;
  np = 4;
  alpha_mu = 0.0;
  alpha_sigma = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial time         TI = %g\n', ti );
  fprintf ( 1, '  Final time           TF = %g\n', tf );
  fprintf ( 1, '  Number of time steps NT = %d\n', nt );
  fprintf ( 1, '  Initial condition    UI = %g\n', ui );
  fprintf ( 1, '  Expansion degree     NP = %d\n', np );
  fprintf ( 1, '  E(ALPHA)       ALPHA_MU = %g\n', alpha_mu );
  fprintf ( 1, '  STD(ALPHA)  ALPHA_SIGMA = %g\n', alpha_sigma );

  [ t, u ] = pce_ode_hermite ( ti, tf, nt, ui, np, alpha_mu, alpha_sigma );
%
%  Evaluate the exact expected value function.
%
  uex = ui * exp ( t.^2 / 2 );
%
%  Compare the first computed component against the exact expected value.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, ' i  T(i)  E(U(T(i)))    U(T(i),0)\n' );
  fprintf ( 1, '\n' );
  for i = 0 : 10 : nt
    fprintf ( 1, '  %4d  %6.3f  %14.6g  %14.6g  %14.6g\n', ...
      i, t(i+1), uex(i+1), u(i+1,1), abs ( uex(i+1) - u(i+1,1) ) );
  end
%
%  Plot the first computed component against the exact expected value function.
%
  figure ( 1 )
  plot ( t, u(:,1), t, uex )
  grid on
  xlabel ( '<--- T --->' )
  ylabel ( '<--- U --->' )
  title ( 'PCE Expected Value Versus Exact Expected Value' )

  filename = 'expected_value_plot.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Expected value comparison plotted in "%s".\n', filename );

  return
end


function ADEM_lorenz_surprise
% This demo computes the cost-function (negative reward) for a Lorenz
% system; to show cost can be computed easily from value (negative
% surprise or sojourn time). However, value is not a Lyapunov function
% because the flow is not curl-free (i.e., is not irrotational).
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: ADEM_lorenz_surprise.m 4804 2012-07-26 13:14:18Z karl $

% dynamics and parameters
%-------------------------------------------------------------------------
DEMO     = 0;                          % switch for demo
LOR      = 1;                          % Lorenz vs a linear system

% generative model
%==========================================================================                       % switch for demo
G(1).E.s = 1/4;                        % smoothness
G(1).E.n = 6;                          % smoothness
G(1).E.d = 2;                          % smoothness

% dynamics and parameters
%--------------------------------------------------------------------------
if LOR
    fL  = '[v; 0; 0] + [-P(1) P(1) 0; P(3) -1 -x(1); x(2) 0 P(2)]*x/64';
    PL  = [10; -8/3; 32];
    x0  = [1; 1; 24];
    W   = exp(8);
else
    fL  = '[v; 0; 0] + [-P(1) 0 0; 0 -P(2) 0; 0 0 -P(3)]*x/64';
    PL  = [1; 1; 8];
    x0  = [-16; 16; 0];
    W   = diag([32; 32; exp(8)]);
end

% level 1
%--------------------------------------------------------------------------
G(1).x  = x0;
G(1).f  = inline(fL ,'x','v','P');
G(1).g  = inline('x','x','v','P');
G(1).pE = PL;
G(1).V  = exp(8);                           % error precision
G(1).W  = W;                                % error precision

% level 2
%--------------------------------------------------------------------------
G(2).a  = [0;0;0];                          % action variables
G(2).v  = 0;                                % inputs
G(2).V  = exp(16);
G       = spm_DEM_M_set(G);

% space
%--------------------------------------------------------------------------
N        = 66;
if LOR
    x{1} = linspace(-32,32,N);
    x{2} = linspace(-32,32,N);
    x{3} = linspace(  4,64,N);
else
    x{1} = linspace(-8,8,N);
    x{2} = linspace(-8,8,N);
    x{3} = linspace(-1,1,3);
end

% equilibrium density (q0), loss (L) and value (V) functions
%==========================================================================
if DEMO

    % Fokker-Planck operator and equilibrium density
    %----------------------------------------------------------------------
    [M0,q0] = spm_fp(G,x);

    % loss-function and negative surprise (value)
    %----------------------------------------------------------------------
    V    = log(q0);
    L    = spm_unvec(spm_vec(V)'*M0,q0);

    % trim
    %----------------------------------------------------------------------
    q0   = q0(2:end - 1,2:end - 1,2:end - 1);
    L    =  L(2:end - 1,2:end - 1,2:end - 1);
    V    =  V(2:end - 1,2:end - 1,2:end - 1);
    x{1} = x{1}(2:end - 1);
    x{2} = x{2}(2:end - 1);
    x{3} = x{3}(2:end - 1);

    if LOR
        save DEM_lorenz_suprise q0 L V x
    end
else
    load DEM_lorenz_suprise
end

% axes and trajectory
%--------------------------------------------------------------------------
spm_figure('GetWin','DEM');

if LOR, z = 24; else, z = 1; end

i    = 3;
j    = 1:3;
j(i) = [];
T    = 1024;
U.u  = sparse(T,G(1).m);
t    = spm_int_J(G(1).pE,G,U);

% surprise
%--------------------------------------------------------------------------
a    = [x{j(2)}(1) x{j(2)}(end) x{j(1)}(1) x{j(1)}(end)];
subplot(3,2,1)
imagesc(x{j(2)},x{j(1)},V(:,:,z))
hold on, plot(t(:,j(2)),t(:,j(1)),'r'), hold off
axis(a)
axis square xy
title('value','Fontsize',16)

% cost function
%--------------------------------------------------------------------------
subplot(3,2,2)
imagesc(x{j(2)},x{j(1)},L(:,:,z))
hold on, plot(t(:,j(2)),t(:,j(1)),'r'), hold off
axis(a)
axis square xy
title('cost','Fontsize',16)

% equilibrium density
%--------------------------------------------------------------------------
subplot(3,2,3)
imagesc(x{j(2)},x{j(1)},squeeze(max(q0,[],i)))
hold on, plot(t(:,j(2)),t(:,j(1)),'r'), hold off
axis(a)
axis square xy
title('density','Fontsize',16)

% exemplar trajectory
%--------------------------------------------------------------------------
subplot(3,2,4)
plot(t(:,j(2)),t(:,j(1)),'k')
axis(a)
axis square xy
title('trajectory','Fontsize',16)

% evaluate V(t)
%--------------------------------------------------------------------------
for i = 1:T
    [q ii] = min(abs(t(i,1) - x{1}));
    [q ij] = min(abs(t(i,2) - x{2}));
    [q ik] = min(abs(t(i,3) - x{3}));
    v(i)   = V(ii,ij,ik);
end

% and plot
%--------------------------------------------------------------------------
maxV = max(V(:));
subplot(3,1,3)
plot(v,'k'), hold on
plot([1 T],[maxV maxV],'r:'), hold off
title('value','Fontsize',16)
set(gca,'XLim',[1 T]);
box off


function [L,E,err,iter] = trpca_snn(X,alpha,opts)

% Solve the Tensor Robust Principal Component Analysis (TRPCA) based on Sum of Nuclear Norm (SNN) problem by M-ADMM
%
% min_{L,E} \sum_i \alpha_i*||L_{i(i)}||_* + ||E||_1,
% s.t. X = L + E.
%
% ---------------------------------------------
% Input:
%       X       -    d1*d2*...dk tensor
%       alpha   -    k*1 vector, parameters
%       opts    -    Structure value in Matlab. The fields are
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       L       -    d1*d2*...*dk tensor
%       E       -    d1*d2*...*dk tensor
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 24/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

dim = size(X);
k = length(dim);

E = zeros(dim);
Y = cell(k,1);
L = Y;
for i = 1 : k
    Y{i} = E;
    L{i} = E;
end

iter = 0;
for iter = 1 : max_iter
    Lk = L;
    Ek = E;
    % first super block {L_i}
    sumtemp = zeros(dim);
    for i = 1 : k
        L{i} = Fold(prox_nuclear(Unfold(X-E-Y{i}/mu,dim,i), alpha(i)/mu),dim,i);
        sumtemp = sumtemp + L{i} + Y{i}/mu;
    end
    % second super block {E}
    E = prox_l1(X-sumtemp/k,1/(mu*k));

    chg = max(abs(Ek(:)-E(:)));
    err = 0;
    for i = 1 : k
        dY = L{i}+E-X;
        err = err+norm(dY(:))^2;
        Y{i} = Y{i}+mu*dY;
        chg = max([chg, max(abs(dY(:))), max(abs(Lk{i}(:)-L{i}(:)))]);
    end
    err = sqrt(err);

    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', err=' num2str(err)]);
        end
    end
    if chg < tol
        break;
    end
    mu = min(rho*mu,max_mu);
end
L = L{1};


function Mn = powermanifold(M, n)
% Returns a structure describing a power manifold M^n = M x M x ... x M.
%
% function Mn = powermanifold(M, n)
%
% Input: a manifold structure M and an integer n >= 1.
%
% Output: a manifold structure Mn representing M x ... x M (n copies of M)
% with the metric of M extended element-wise. Points and vectors are stored
% as cells of size nx1.
%
% This code is for prototyping uses. The structures returned are often
% inefficient representations of power manifolds owing to their use of
% for-loops, but they should allow to rapidly try out an idea.
%
% Example (an inefficient representation of the oblique manifold (3, 10)):
% Mn = powermanifold(spherefactory(3), 10)
% disp(Mn.name());
% x = Mn.rand()
%
% See also: productmanifold

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%   NB, July 4, 2013: Added support for vec, mat, tangent.
%                     Added support for egrad2rgrad and ehess2rhess.

    assert(n >= 1, 'n must be an integer larger than or equal to 1.');

    Mn.name = @() sprintf('[%s]^%d', M.name(), n);

    Mn.dim = @() n*M.dim();

    Mn.inner = @inner;
    function val = inner(x, u, v)
        val = 0;
        for i = 1 : n
            val = val + M.inner(x{i}, u{i}, v{i});
        end
    end

    Mn.norm = @(x, d) sqrt(Mn.inner(x, d, d));

    Mn.dist = @dist;
    function d = dist(x, y)
        sqd = 0;
        for i = 1 : n
            sqd = sqd + M.dist(x{i}, y{i})^2;
        end
        d = sqrt(sqd);
    end

    Mn.typicaldist = @typicaldist;
    function d = typicaldist()
        sqd = 0;
        for i = 1 : n
            sqd = sqd + M.typicaldist()^2;
        end
        d = sqrt(sqd);
    end

    Mn.proj = @proj;
    function u = proj(x, u)
        for i = 1 : n
            u{i} = M.proj(x{i}, u{i});
        end
    end

    Mn.tangent = @tangent;
    function u = tangent(x, u)
        for i = 1 : n
            u{i} = M.tangent(x{i}, u{i});
        end
    end

    if isfield(M, 'tangent2ambient')
        Mn.tangent2ambient = @tangent2ambient;
    else
        Mn.tangent2ambient = @(x, u) u;
    end
    function u = tangent2ambient(x, u)
        for i = 1 : n
            u{i} = M.tangent2ambient(x{i}, u{i});
        end
    end

    Mn.egrad2rgrad = @egrad2rgrad;
    function g = egrad2rgrad(x, g)
        for i = 1 : n
            g{i} = M.egrad2rgrad(x{i}, g{i});
        end
    end

    Mn.ehess2rhess = @ehess2rhess;
    function h = ehess2rhess(x, eg, eh, h)
        for i = 1 : n
            h{i} = M.ehess2rhess(x{i}, eg{i}, eh{i}, h{i});
        end
    end

    Mn.exp = @expo;
    function x = expo(x, u, t)
        if nargin < 3
            t = 1.0;
        end
        for i = 1 : n
            x{i} = M.exp(x{i}, u{i}, t);
        end
    end

    Mn.retr = @retr;
    function x = retr(x, u, t)
        if nargin < 3
            t = 1.0;
        end
        for i = 1 : n
            x{i} = M.retr(x{i}, u{i}, t);
        end
    end

    if isfield(M, 'log')
        Mn.log = @loga;
    end
    function u = loga(x, y)
        u = cell(n, 1);
        for i = 1 : n
            u{i} = M.log(x{i}, y{i});
        end
    end

    Mn.hash = @hash;
    function str = hash(x)
        str = '';
        for i = 1 : n
            str = [str M.hash(x{i})]; %#ok<AGROW>
        end
        str = ['z' hashmd5(str)];
    end

    Mn.lincomb = @lincomb;
    function x = lincomb(x, a1, u1, a2, u2)
        if nargin == 3
            for i = 1 : n
                x{i} = M.lincomb(x{i}, a1, u1{i});
            end
        elseif nargin == 5
            for i = 1 : n
                x{i} = M.lincomb(x{i}, a1, u1{i}, a2, u2{i});
            end
        else
            error('Bad usage of powermanifold.lincomb');
        end
    end

    Mn.rand = @rand;
    function x = rand()
        x = cell(n, 1);
        for i = 1 : n
            x{i} = M.rand();
        end
    end

    Mn.randvec = @randvec;
    function u = randvec(x)
        u = cell(n, 1);
        for i = 1 : n
            u{i} = M.randvec(x{i});
        end
        u = Mn.lincomb(x, 1/sqrt(n), u);
    end

    Mn.zerovec = @zerovec;
    function u = zerovec(x)
        u = cell(n, 1);
        for i = 1 : n
            u{i} = M.zerovec(x{i});
        end
    end

    if isfield(M, 'transp')
        Mn.transp = @transp;
    end
    function u = transp(x1, x2, u)
        for i = 1 : n
            u{i} = M.transp(x1{i}, x2{i}, u{i});
        end
    end

    if isfield(M, 'pairmean')
        Mn.pairmean = @pairmean;
    end
    function y = pairmean(x1, x2)
        y = cell(n, 1);
        for i = 1 : n
            y{i} = M.pairmean(x1{i}, x2{i});
        end
    end

    % Compute the length of a vectorized tangent vector of M at x, assuming
    % this length is independent of the point x (that should be fine).
    if isfield(M, 'vec')
        rand_x = M.rand();
        zero_u = M.zerovec(rand_x);
        len_vec = length(M.vec(rand_x, zero_u));

        Mn.vec = @vec;

        if isfield(M, 'mat')
            Mn.mat = @mat;
        end

    end

    function u_vec = vec(x, u_mat)
        u_vec = zeros(len_vec, n);
        for i = 1 : n
            u_vec(:, i) = M.vec(x{i}, u_mat{i});
        end
        u_vec = u_vec(:);
    end

    function u_mat = mat(x, u_vec)
        u_mat = cell(n, 1);
        u_vec = reshape(u_vec, len_vec, n);
        for i = 1 : n
            u_mat{i} = M.mat(x{i}, u_vec(:, i));
        end
    end

    if isfield(M, 'vecmatareisometries')
        Mn.vecmatareisometries = M.vecmatareisometries;
    else
        Mn.vecmatareisometries = @() false;
    end

end


function [Ax,Ay,b,K] = convexhullConvex(varargin)
% Two upper bounds from tangents
% y > f(xL) + (x-xL)*df(xL)
% y > f(xU) + (x-xL)*df(xU)
% Upper bound from conneting extreme points
% y < f(xU)(x-xL)/(xU-xL) +  f(xL)(xU-x)/(xU-xL)
% can be wrtitten as
% Ax*x + Ay*y < b

if rem(nargin,3)
    error('The convex hull generator assumes n triplets (x,f,df).')
end
m = nargin/3;
x = [varargin{(1:m)}]';
f = [varargin{(1:m)+m}]';
df = [varargin{(1:m)+2*m}]';

if all(diff(x)>0)
    Ay = [-ones(m,1);1];
    b  = [-f + x.*df; -f(end)*x(1)/(x(end)-x(1)) +  f(1)*x(end)/(x(end)-x(1))];
    Ax  = [df;-f(end)/(x(end)-x(1)) + f(1)/(x(end)-x(1))];

     % Don't use ill-conditioned cuts
    if df(1)<-1000
        Ax(1)=[];
        Ay(1) = [];
        b(1) = [];
    end
    if df(end)>1000
        Ax(end)=[];
        Ay(end) = [];
        b(end) = [];
    end

else
    Ax = [];
    Ay = [];
    b = [];
end
K.f = 0;
K.l = length(b);


%  Figure 10.21      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
%   fig10_21.m is a script to generate Fig 10.21, the
%   frequency response of the LQR symmetric rootlocus compensator for the
%   satellite position control, non-colocated case WITH ESTIMATOR

m=[1, 0.1]; k=[0, 0.091] ; d=[0, 0.0036]; k1=[0, 0.4];
[f,g,h,j] = twomass(m,k,d);
s=[f, g;h, 0];r=[0*g;1]; n=s\r;nx=n(1:4);nu=n(5);
[f1,g,h,j] = twomass(m,k1,d);
a=[f, 0*f;
-h'*h, -f'];
b=[g;0*g];
c=[0*h, g'];
d=[0];

hold off; clf

P=eig(a-b*c*0.1621);
pc=P(real(P<0)==1);
K=place(f,g,pc);
nbar=nu+K*nx;
% eig(f-g*K)
P=eig(a-b*c*3.056e7);
pe=P(real(P<0)==1);
L=place(f',h',pe)';
ac=f-g*K-L*h ;bc=L;cc=K;dc=0;
ac=f-g*K-L*h ;bc=L;cc=K;dc=0;
% [numc denc]=ss2tf(ac,bc,cc,dc)

w=logspace(-1,1);
w(26) = 1; w(25) = .94;w(25)=.9;
[mag ph]=bode(ac,bc,cc,dc,1,w);
subplot(211); loglog(w,mag); grid;
xlabel('\omega (rad/sec)');
ylabel('Magnitude, |D_4(j\omega)|');
title('Fig. 10.21: Bode plot of the optimal compensator D_4(s)');
subplot(212); semilogx(w,ph);grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');

%Bode grid
bodegrid


function [e1,e2,l1,l2] = perform_tensor_eigendecomposition(e1,e2,l1,l2)

% perform_tensor_eigendecomposition - decompose a tensor field
%
% analysis:
%   [e1,e2,l1,l2] = perform_tensor_eigendecomposition(T);
% synthesis:
%   T = perform_tensor_eigendecomposition(e1,e2,l1,l2);
%
%   Copyright (c) 2007 Gabriel Peyre

if nargin==1
    T = e1;
    n = size(T,1);
    if size(T,3)==3 && size(T,4)==1
        T = cat(3, T(:,:,1), T(:,:,3), T(:,:,3), T(:,:,2)); T = reshape(T, [n n 2 2]);
    end
    [e1,e2,l1,l2] = perform_tensor_decomp(T);
elseif nargin==4
    H = perform_tensor_recomp(e1,e2,l1,l2);
    e1 = cat(3, H(:,:,1,1), H(:,:,2,2), H(:,:,2,1) );
else
    error('1 or 4 arguments');
end


function [out,options] = FCLSU_fast2(HIM,M,options)
% Fully Constrained Linear Spectral Unmixing
% Perform a Linear least squares with nonnegativity constraints.
% --------------------------------------------------------------------
% Input:   HIM : input data [nrows x nchannels]
%          M   : set of p endmembers [nchannels x p].
%
% Output:  out : fractions [nrows x p]
%
%
% Copyright (2007) GRNPS group @ University of Extremadura, Spain.
%
% *** Edited version ***

[ns,nb] = size(HIM);
[l,p] = size(M);

Delta = 1/1000; % should be an small value

N = zeros(l+1,p);
N(1:l,1:p) = Delta*M;
N(l+1,:) = ones(1,p);
s = zeros(l+1,1);

OutputImage = zeros(ns,p);

go=(nargin==3);

for i = 1:ns
    s(1:l) = Delta*HIM(i,:)';
    s(l+1) = 1;
    if go==0
        [Abundances,options] = lsqnonneg_fast(N,s);
        go=1;
    else
        Abundances = lsqnonneg_fast(N,s,options);
    end
    OutputImage(i,:) = Abundances;
end

out = OutputImage;


function [lbeta lgamma method] = betaq_up_v2(q, n, P)
% computes the _UPPER_ bound (as good as it can find) for the beta_q function:
%   beta_q = min Q(B), s.t. P(B) >= q
%
%   where
%
%	P ~ Gsn( x_0,	I_n )
%	Q ~ Gsn( 0, (1+A^2) I_n )
%
%	And x_0 something that is ||x_0||^2 = n A^2
%
%	Taking x_0 = [A, ... A] we get
%
%	P[ dP/dQ >= gamma ] = P(  sum (Z_i - 1/A)^2 <= pp  )
%	Q[ dP/dQ >= gamma ] = P(  sum (Z_i - sqrt(1+A^2)/A)^2 <= qq )
%
%	pp = ((1 + A^2) n - gammatil) / A^2
%	qq = ((1 + A^2) n - gammatil) / ((1+A^2) A^2)
%
%	gammatil = [ log gamma - n/2 log (1+A^2) ] 2 (1 + A^2) / log e
%
%
% Idea for the upper bound: take any \gamma s.t. P_\gamma >= q. Then Q_\gamma >= \beta_q
% Or for high gamma: \beta_q <= 1/gamma;

% conversion A->P. Old versions are all in terms of ``amplitude'' A.
A = sqrt(P);

pp0 = ncx2inv(q, n, n/A^2);
iter = 1;
while 1;
	pgam = ncx2cdf(pp0, n, n/A^2);

	if (pgam >= q)
		break
	end

	delta = q - pgam;
	%
	% For q > .5 this is underestimate => we will undershoot.
	%
	pp0 = pp0 + delta/ncx2pdf(pp0, n, n/A^2);
	iter = iter + 1;
end

delta = pgam - q;

gammatil = (1 + A^2) * n - A^2 * pp0;
lgamma = gammatil * log2(exp(1)) / (2  + 2*A^2) + n/2 *log2 (1 + A^2);
qq0 = ((1+A^2) * n - gammatil) / ((1+A^2)*A^2);

%
% The funny property of ncx2cdf is that it returns pretty consistent results up until
% it starts returning 0's. So we believe it until term0. After that we use simple 1/gamma
% upper bound.
%
% TODO: change 1/gamma bound to a large deviation chernoff-type one.
%	04/18/2007: done. (see various comparisons in script_optimize_upper)
%
% 05/02/2007: all upper bounding is deprecated, now we simply use ncx2log() which
%	works for all values of interest.
%
term1 = ncx2cdf(qq0, n, n*(1 + 1/A^2));

if (term1 == 0)
	%
	% This is the old upper bound beta <= 1/gamma
	%
	%lbeta =  -lgamma;

	lbeta = log2(exp(1)) * ncx2log(qq0, n, n*(1 + 1/A^2));

    if (lbeta > -1240)
       	if (lbeta > -lgamma)
    		disp('ERROR: precise value of lbeta is above 1/gamma ');
        	error('betaq_up_v2');
    	end
        msg = 'PREC_LOG';
    else

    %
	% LARGE-DEVIATIONS method: if everything else fails
	%
	pstar = A^2/2;
	sn = qq0/n;

	s = warning('off', 'optim:fminunc:SwitchingMethod');
	opts = optimset('Display', 'off');
	[popt fopt] = ...
		fminunc(@(p)( -1/2 * log(2*abs(p)+1) - abs(p)/(2*abs(p)+1) * (1+ 1/A^2) + abs(p)*sn ), ...
			pstar, opts);
	warning(s);

	lbeta_ld = fopt * log2(exp(1)) * n;
	lbeta_gamma = -lgamma;

	if (lbeta_gamma > lbeta_ld)
		lbeta = lbeta_ld;
		msg = 'LARGE-DEV';
		method = 2;
	else
		lbeta = lbeta_gamma;
		msg = 'GAMMA';
		method = 1;
	end;

    end;

	disp(sprintf('betaq_up(q = %g, n = %d, A = %g): %s: iter = %d, delta = %.1g, lbeta = %.2f', ...
			q, n, A, msg, iter, delta, lbeta));
else
	lbeta = log2(term1);
	method = 0;
	disp(sprintf('betaq_up(q = %g, n = %d, A = %g): PRECISE: iter = %d, delta = %.1g, lbeta = %.2f (loose: %.1f %% worse) ', ...
			q, n, A, iter, delta, lbeta, 100*(-lgamma-lbeta)/(-lbeta)));
end;


function w = anmp (a)

% normalize angle into the range -pi <= a < +pi.

% ported from NOVAS 3.0

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

w = mod(a, 2.0 * pi);

if (abs(w) >= pi)

    w = w - sign(a) * (2.0 * pi);

end


% [y,feature_names,cache] = ComputeChangeWindowFeatures(x,...)
%
% Computes the change window features y for the input one-dimensional
% per-frame time series data x. y(i,t) will correspond to change
% window feature i and frame t, and is the (transformation of) the
% difference between the average of the per-frame data at the end and start
% of the window defined by i and t.
%
% For window feature i, let rho_i be the change window radius, r_i be the
% window radius, and off_i be the window offset. If the transformation type
% for feature i is 'none', the window feature y(i,t) is the difference
% between the average per-frame data at the end of the window (from
% (t+r_i+off_i)-rho_i to (t+r_i+off_i)+rho_i) and at the start of the
% window (from (t-r_i+off_i)-rho_i to (t-r_i+off_i)+rho_i). If the
% transformation type for feature i is 'abs', then we compute the absolute
% value of this difference. If the transformation type for feature
% i is 'flip', then we compute the sign of x(t) times this difference.
%
% Input:
%
% x: 1 x NFRAMES array of per-frame data.
%
% Output:
%
% y: NWINDOWFEATURES x NFRAMES matrix of window data, where y(i,t)
% corresponds to window feature i and frame t. i indexes the radius and
% offset of the window as well as the transformation type.
% feature_names: 1 x NWINDOWFEATURES cell in which feature_names{i}
% describes the ith window feature computed. feature_names{i} is itself a
% cell that can be interpreted as pairs of a string description followed by
% a value, e.g.
% {'stat','change','trans','abs','radius',1,'offset',1,'change_window_radius',0}
% cache: if DOCACHE is set to true, then computations useful in other
% window feature calculations may be saved in the output cache.
%
% Optional inputs:
%
% 'change_window_radii': The 1 X NCHANGEWINDOWRADII change window radii to
% use (this defines the size of the sub-window averaged over at the end and
% start of the original window). default value: 0.
%
% DEFAULT WINDOW LOCATIONS:
% These window locations are used if window locations are not specified on
% a per-feature type basis. Default default values set by
% SetDefaultWindowParameters.
% Inputs interpreted by SetWindowParameters.
% The same window parameter interpretation is done in all
% Compute*WindowFeatures functions.
% 'windows': window offsets and radii to try ([radius1,offset1];...;[radiusn,offsetn])
% if empty, then window_radii and window_offsets are used to specify
% windows. if the window is [r,off], then the window feature at time t
% will be computed from the window at t-r+off to t+r+off. default value: [].
% 'window_radii': if windows is empty, then the cross-product of window_radii
% and window_offsets is used to set windows. if empty, then
% min_window_radius, max_window_radius, and nwindow_radii are used to set
% window_radii. default value: [].
% 'window_offsets': if windows is empty, then the cross-product of  window_radii
% and window_offsets is used to set windows. window_offsets are relative to
% radius, so the window corresponding to radius r_i and offset off_j_rel
% will be [r_i,off_i=r_i*off_j_rel]. default value: [-1,0,1].
% 'min_window_radius': if windows and window_radii are both empty, then
% window_radii is set to nwindow_radii evenly spaced radii between
% min_window_radii and max_window_radii. default value: 0
% 'max_window_radius': see 'min_window_radius'. default value: 20.
% 'nwindow_radii: see 'min_window_radius'. default value: 5.
%
% 'trans_types': default types of transformations to apply for each feature
% type, if not otherwise specified. Options include 'abs','flip', and
% 'none'. 'abs' corresponds to the absolute value, flip corresponds to
% flipping the sign of the feature if the per-frame feature at the frame t
% is negative, and 'none' corresponds to no transformation.
%
% 'sanitycheck': whether to compute all the features a second time in the
% obvious way to make sure that the optimized computations are correct.
% default value: false.
%
% 'docache': whether to cache computations that might be useful, e.g. the
% mean computations are useful when computing the change window features.
% default value: true.
%
% 'cache': input cached computations that might be useful in this
% computation.

function [y,feature_names,cache] = ComputeChangeWindowFeatures(x,varargin)

x = x(:)';
N = numel(x);
y = nan(0,N);
feature_names = {};

%% default parameters

[windows,window_offsets,...
  window_radii,min_window_radius,...
  max_window_radius,nwindow_radii] = ...
  SetDefaultWindowParameters();

% use all transformation types by default
%trans_types = 'all';
trans_types = uint8(15);

% for debugging purposes
SANITY_CHECK = true;

% whether to cache results
DOCACHE = true;

% initialize empty cache
cache = InitializeCache();

% initialize feature_types already computed to empty
%feature_types = {};

% change window radii (width = 2*radius + 1)
change_window_radii = 0;

relativeParams = [];

%% parse parameters

[...
  windows,...
  window_radii,window_offsets,...
  min_window_radius,max_window_radius,nwindow_radii,...
  trans_types,...
  SANITY_CHECK,...
  DOCACHE,...
  cache,...
  change_window_radii,...
  relativeParams,...
  ] = myparse(varargin,...
  'windows',windows,...
  'window_radii',window_radii,'window_offsets',window_offsets,...
  'min_window_radius',min_window_radius,'max_window_radius',max_window_radius,'nwindow_radii',nwindow_radii,...
  'trans_types',trans_types,...
  'sanitycheck',SANITY_CHECK,...
  'docache',DOCACHE,...
  'cache',cache,...
  'change_window_radii',change_window_radii,...
  'relativeParams',relativeParams);
%   'feature_types',feature_types,...

%% whether we've specified to use all trans types by default
%if ischar(trans_types) && strcmpi(trans_types,'all'),
%  trans_types = {'none','abs','flip','relative'};
%end

%% select default windows from various ways of specifying windows

[windows,window_radii,windowi2radiusi,nradii] = ...
  SetWindowParameters(...
  windows,window_offsets,...
  window_radii,min_window_radius,...
  max_window_radius,nwindow_radii);

%% main computation

%if ismember('relative',trans_types)
if bitand(8,trans_types)
  if DOCACHE && ~isempty(cache.relX)
    modX = cache.relX;
  else
    modX = convertToRelative(x,relativeParams);
    cache.relX = modX;
  end
end

for change_r_i = 1:numel(change_window_radii),

  % take the mean across all windows of radius change_r
  change_r = change_window_radii(change_r_i);
  change_w = 2*change_r+1;

  if DOCACHE && ismember(change_r,cache.mean.radii),
    cache_i = find(change_r == cache.mean.radii,1);
    res_mean = cache.mean.data{cache_i};
  else
    % average filter
    res_mean = MeanWindowCore(x,change_w);
    if DOCACHE,
      cache.mean.radii(end+1) = change_r;
      cache.mean.data{end+1} = res_mean;
    end
  end

%  if ismember('relative',trans_types)
  if bitand(8,trans_types)
    if DOCACHE && ismember(change_r,cache.meanRel.radii),
      cache_i = find(change_r == cache.meanRel.radii,1);
      resRel_mean = cache.meanRel.data{cache_i};
    else
      resRel_mean = MeanWindowCore(modX,change_w);
      % store for future computations
      if DOCACHE,
        cache.meanRel.radii(end+1) = change_r;
        cache.meanRel.data{end+1} = resRel_mean;
      end
    end
  end

  % loop over window radii
  for radiusi = 1:nradii,
    r = window_radii(radiusi);
    % don't do for r <= change_r -- the windows will overlap, cancel, ...
    if r <= change_r,
      continue;
    end
    w = 2*r+1;

    % take the difference between the end of the window and the start of
    % the window
    % res(t) corresponds to t+r-change_r
    % so res(t-r+change_r) corresponds to frame t
    res = res_mean(w:end) - res_mean(1:end-w+1);

%    if ismember('relative',trans_types),
    if bitand(8,trans_types),
      resRel = resRel_mean(w:end) - resRel_mean(1:end-w+1);
    end

    % offset
    windowis = find(windowi2radiusi == radiusi);
    for windowi = windowis',

      off = windows(windowi,2);
      % res(t-r+change_r) corresponds to frame t,
      % and we want to grab from [1+off,N+off] which is
      % [1+off-r+change_r,N+off-r+change_r], relative to res
      res1 = padgrab2(res,nan,1,1,1+off-r+change_r,N+off-r+change_r)/r;

%      if ismember('none',trans_types),
      if bitand(1,trans_types),
        y(end+1,:) = res1;
        feature_names{end+1} = {'stat','change','trans','none','radius',r,'offset',off,'change_window_radius',change_r}; %#ok<*AGROW>
      end

%      if ismember('abs',trans_types),
      if bitand(2,trans_types),
          y(end+1,:) = abs(res1);
          feature_names{end+1} = {'stat','change','trans','abs','radius',r,'offset',off,'change_window_radius',change_r};
      end

%      if ismember('flip',trans_types)
      if bitand(4,trans_types)
        res2 = res1;
        res2(x<0) = -res2(x<0);
        y(end+1,:) = res2;
        feature_names{end+1} = {'stat','change','trans','flip','radius',r,'offset',off,'change_window_radius',change_r};
      end

%      if ismember('relative',trans_types),
      if bitand(8,trans_types),
        resRel1 = padgrab2(resRel,nan,1,1,1+off-r+change_r,N+off-r+change_r)/r;
        y(end+1,:) = resRel1;
        feature_names{end+1} = {'stat','change','trans','relative','radius',r,'offset',off,'change_window_radius',change_r}; %#ok<*AGROW>
      end

      if SANITY_CHECK,
        extraStr = sprintf('change_r = %d',change_r);
%        if ismember('none',trans_types),
        if bitand(1,trans_types),
          fastY = res1;
        end
        res_dumb = nan(1,N);
        for n_dumb = 1:N,
          tmp1 = padgrab2(x,nan,1,1,n_dumb+off-r-change_r,n_dumb+off-r+change_r);
          tmp2 = padgrab2(x,nan,1,1,n_dumb+off+r-change_r,n_dumb+off+r+change_r);
          res_dumb(n_dumb) = (nanmean(tmp2) - nanmean(tmp1))/r;
        end
        checkSanity(fastY,res_dumb,r,off,'change','none',extraStr);

%        if ismember('abs',trans_types),
        if bitand(2,trans_types),
          fastY = abs(res1);
          res_dumb = nan(1,N);
          for n_dumb = 1:N,
            tmp1 = padgrab2(x,nan,1,1,n_dumb+off-r-change_r,n_dumb+off-r+change_r);
            tmp2 = padgrab2(x,nan,1,1,n_dumb+off+r-change_r,n_dumb+off+r+change_r);
            res_dumb(n_dumb) = abs(nanmean(tmp2) - nanmean(tmp1))/r;
          end
          checkSanity(fastY,res_dumb,r,off,'change','abs',extraStr);
        end

%        if ismember('flip',trans_types)
        if bitand(4,trans_types)
          res2 = res1;
          res2(x<0) = -res2(x<0);
          fastY = res2;
          res_dumb = nan(1,N);
          for n_dumb = 1:N,
            tmp1 = padgrab2(x,nan,1,1,n_dumb+off-r-change_r,n_dumb+off-r+change_r);
            tmp2 = padgrab2(x,nan,1,1,n_dumb+off+r-change_r,n_dumb+off+r+change_r);
            res_dumb(n_dumb) = (nanmean(tmp2) - nanmean(tmp1))/r;
            if x(n_dumb) < 0,
              res_dumb(n_dumb) = -res_dumb(n_dumb);
            end
          end
          checkSanity(fastY,res_dumb,r,off,'change','flip',extraStr);
        end

      end

    end
  end

end


function [W,E,U] = skeleton_extraction(V,F)
  % SKELETON_EXTRACTION Compute the "skeleton" of a surface mesh following "Skeleton
  % Extraction by Mesh Contraction" [Au et al. 2008]. The final combinatorially
  % simplification is a bit different than the QSlim-based approach in [Au et
  % al. 2008].
  %
  % [W,E,U] = skeletonization(V,F)
  %
  % Inputs:
  %   V  #V by 3 list of mesh vertex positions
  %   F  #F by 3 list of mesh indices
  % Outputs:
  %   W  #W by 3 list of skeleton vertex positions
  %   E  #E by 2 list of skeleton edges into W
  %   U  #V by 3 list of skeleton vertex positions, before thinning
  %

  delta = 1e-3*max(max(V)-min(V))^2;
  U = V;

  viz = false;
  if viz
      tsurf(F,V,'FaceAlpha',0.1,'EdgeColor','k','FaceColor',[0.5 0.5 0.5],'EdgeAlpha',0.5);
      hold on;
      t = tsurf( ...
          bsxfun(@plus,size(F,1)*(0:2),(1:size(F,1))'), ...
          U(F,:),'FaceColor',[0.7 0.8 1.0],'EdgeColor','k','EdgeAlpha',0.5, ...
          'SpecularStrength',0.1,'FaceLighting','phong');
      tr = tsurf([1 1 1],U,'EdgeColor','r','LineWidth',2);
      hold off;
      axis equal;
      view(-62,10);
      camproj('persp');
      light('Position',[-100.0,1.0,-1.0],'Style','infinite');
      set(gca,'Visible','off');
      set(gcf,'Color','w');
      drawnow;
  end

  b = [];

  iter = 1;
  h = avgedge(V,F);

  while true
    L = cotmatrix(U,F);
    M = massmatrix(U,F,'barycentric');
    b = union(find(any(abs(L)>1e+5,2)),b);
    U_prev = U;
    U = min_quad_with_fixed(-(M-delta*L),2*M*U,b,U(b,:));
    %U = U/sqrt(sum(doublearea(U,F))*0.5);
    %U = bsxfun(@minus,U,centroid(U,F));
    %D = max(internalangles(U,F),[],2)>0.9*pi & ...
    %  doublearea(U,F)<1e-5;
    D = all(ismember(F,b),2);
    if viz
        set(t,'Vertices',U(F,:));
        set(tr,'Faces',F(D,:),'Vertices',U);
        xlabel(sprintf('%d',numel(b)));
        drawnow;
    end
    if max(max(abs(U-U_prev))) < 1e-2*h
      break;
    end
    iter =iter + 1;
  end

  W = U;
  A = adjacency_matrix(F) + speye(size(V,1));
  EC = adjacency_edge_cost_matrix(U,F);
  phi = zeros(size(W,1),1);
  marked = false(size(W,1),1);
  E = [];
  % This is very slow for large meshes...
  for wi = 1:size(W,1)
    % this is a lot faster than find(A(ui,:))
    if marked(wi)
      continue;
    end
    pI = A(:,wi)>0;
    I = false(size(V,1),1);
    N = [];
    while true
      pN = find(pI);
      if numel(pN) <= 1
        break;
      end
      Wi = W(pN,:);
      [pC,pS,L] = pca(Wi);
      phi(wi) = L(1)/sum(L);
      if phi(wi) < 0.99
        break;
      end
      I = pI;
      N = pN;
      C = pC;
      S = pS;

      if viz
          tsurf(F,W);
          hold on;
          scatter3(U(I,1),U(I,2),U(I,3),'SizeData',50);
          hold off;
          title(sprintf('%g',phi(wi)));
          axis equal;
          drawnow;
      end

      %pI = (A*I)>0;
      CCI = sum(A(I,:))';
      ECI = minnz(EC(I,:))';
      ECI = ECI./CCI;
      ECI(I) = 0;
      [~,j] = minnz(ECI);
      pI = I;
      pI(j) = 1;

      if isequal(pI,I)
        break;
      end

    end

    if ~isempty(N)
      [~,O] = sort(S(:,1));
      O = N(O);
      A(I,:) = 0;
      A(:,I) = 0;
      %tsurf(F,W);
      %hold on;
      %scatter3(U(I,1),U(I,2),U(I,3),'SizeData',25);
      %scatter3(U(marked,1),U(marked,2),U(marked,3),'SizeData',50);
      %plot_edges(U,E,'b','LineWidth',3);
      %plot_edges(U,[O(1) O(end)],'r','LineWidth',3);
      %hold off;
      %axis equal;
      %drawnow
      marked(N) = true;
      E = [E;O(1) O(end)];
    end

    %if phi<0.4
    %  continue;
    %end
    %Wi = bsxfun(@plus,bsxfun(@times,S(:,1),C(:,1)'),mean(Wi));
    %d = sqrt(sum(bsxfun(@minus,Wi,Wi(1,:)).^2,2));
    %H = max(d);
    %d = d/H;
    %f = 2.*d.^3 - 3.*d.^2 + 1;
    %f = f/sum(f);
    %W(wi,:) = sum(bsxfun(@times,f,Wi));
  end
  %plot_edges(W,E,'LineWidth',2)

  count = ones(size(W,1),1);
  while true
    [W,IM] = remove_unreferenced(W,E);
    count(IM) = count;
    count = count(1:size(W,1));
    E = IM(E);
    %plot_edges(W,E,'LineWidth',2)
    %axis equal;
    %drawnow;
    if size(E,1) == 0
      break;
    end
    A = adjacency_matrix(E)+speye(size(W,1));
    [ncc,C] = conncomp(A);
    if ncc == 1
      break;
    end
    D = pdist2(W,W);
    D(A>0) = inf;
    [D,I] = min(D);
    [~,j] = min(D);
    i = I(j);
    % running average
    W(i,:) = (count(i)*W(i,:)+W(j,:))/(count(i)+1);
    count(i) = count(i) + 1;
    E(E==j) = i;
    E = unique(sort(E,2),'rows');
  end

  %G = F;
  %E = edges(G);
  %
  %A = U(E(:,2),:)-U(E(:,1),:);
  %B = cross(A,U(E(:,1),:),2);
  %UH = [U ones(size(U,1),1)];
  %
  %ne = size(E,1);
  %n = size(U,1);
  %%K = sparse( ...
  %%  repmat(reshape(repmat(1:ne*3,1,3),ne,9),2,1), ...
  %%  bsxfun(@plus,E(:),n*[1 2 0 2 0 1 3 3 3]), ...
  %%  repmat([-A(:,[3 1 2]) A(:,[2 3 1]) -B],2,1), ...
  %%  ne*3,n*4);
  %%Q = K'*K;
  %
  %%K = zeros(3,4,size(E,1));
  %Q = zeros(4,4,size(U,1));
  %%Q = sparse(n*4,n*4);
  %% This can at least be sped up using http://www.alecjacobson.com/weblog/?p=4186
  %for ei = 1:size(E,1)
  %  K = [ ...
  %    0        -A(ei,3)  A(ei,2) -B(ei,1); ...
  %    A(ei,3)  0        -A(ei,1) -B(ei,2); ...
  %    -A(ei,2) A(ei,1)  0        -B(ei,3)];
  %  K2 = K'*K;
  %  Q(:,:,E(ei,1)) = Q(:,:,E(ei,1)) + K2;
  %  Q(:,:,E(ei,2)) = Q(:,:,E(ei,2)) + K2;
  %end
  %% Cost per vertex
  %C = zeros(size(U,1),1);
  %for vi = 1:size(U,1)
  %  C(vi) = UH(vi,:) * (Q(:,:,vi) * (UH(vi,:)'));
  %end
  %
  %
  %A = sparse(repmat((1:ne)',1,2),E,1,ne,n);
  %EL = sqrt(sum(U(E(:,2),:)-U(E(:,1),:),2));
  %% This is a terrible O(n²) implemenation. The energy also doesn't match the [Au
  %% et al. 2008] paper. I don't understand their description or necessarily agree
  %% with the motivation.
  %J = 1:size(U,1);
  %while true
  %  % Edge-vertex incidence matrix
  %  ne = size(E,1);
  %  % Edge lengths summed at vertices
  %  EC = repmat(A*C,2,1) + 0.001*repmat(EL,2,1);
  %  % pick an edge
  %  [~,ei] = min(EC);
  %  e = E(mod(ei-1,ne)+1,:);
  %  if ei>ne
  %    e = fliplr(e);
  %  end
  %  % collapse edge
  %  J(e(2)) = e(1);
  %  J = J(J);
  %  Q(:,:,e(1)) = Q(:,:,e(1)) + Q(:,:,e(2));
  %  C(e(1)) = UH(e(1),:) * (Q(:,:,e(1)) * (UH(e(1),:)'));
  %  G = J(G);
  %  E = J(E);
  %
  %  A(:,e(1)) = A(:,e(1)) + A(:,e(2));
  %  A(:,e(2)) = 0;
  %  ndeg = E(:,1)~=E(:,2);
  %  E = E(ndeg,:);
  %  EL = EL(ndeg,:);
  %  A = A(ndeg,:);
  %  G = G(G(:,1)~=G(:,2)&G(:,2)~=G(:,3)&G(:,3)~=G(:,1),:);
  %  %G = unique(sort(G,2),'rows');
  %  if isempty(G)
  %    break;
  %  end
  %
  %  change = any(E==e(1),2);
  %  Echange = unique(sort(E(change,:),2),'rows');
  %  E = [E(~change,:);Echange];
  %  nc = size(Echange,1);
  %  A = [ ...
  %    A(~change,:); ...
  %    sparse(repmat((1:nc)',1,2),Echange,1,nc,n)];
  %  EL = [EL(~change);sqrt(sum(U(Echange(:,2),:)-U(Echange(:,1),:),2))];
  %
  %  if size(E,1)<50
  %    tsurf(G,U,'EdgeColor','r');
  %    hold on;
  %    tsurf([E E(:,1)],U,'EdgeColor','k');
  %    hold off;
  %    title(sprintf('%d,%d',size(G,1),size(E,1)));
  %    axis equal;
  %    drawnow
  %  end
  %end
  %M = sparse(J,1:n,diag(massmatrix(V,F)),n,n);
  %M(J,:) = M(J,:)*diag(1./sum(M(J,:),2));
  %U = M*V;
  %[U,IM] = remove_unreferenced(U,E);
  %E = IM(E);
  %plot_edges(U,E,'LineWidth',2);
  %hold on;
  %tsurf(F,V,'CData',J,'FaceAlpha',0.2,'EdgeAlpha',0.2);
  %hold off;
end


function Hv = hessvec_fd(v,FUN,x,gx,s)
%HESSVEC_FD   Hessian vector product finite difference approximation.
%
%   HV = HESSVEC_FD(V,FUN,X) computes a forward finite difference
%   approximation of the Hessian vector product, H(X)*V, where V is given
%   and H(X) is the Hessian of the function FUN at the point X. The
%   approximation is given by
%
%              G(X+S*V) - G(X)
%     H(X)*V = ---------------
%                     S
%
%   where G(X) is the gradient of the function FUN at the point X and
%   h is the difference step (default = 1e-8*(1+norm(X)).
%
%   HV = HESSVEC(V,FUN,X,GX) uses GX for the value of G(X), in the
%   case that is has already been computed outside of this method.
%
%   HV = HESSVEC(V,FUN,X,GX,S) uses S as the difference step.
%
%   This method should not be called directly, but only by Poblano Toolbox
%   optimization algorithms.
%
%   The number of calls to FUN is kept track of in the global variable
%   NFEV_HESSVEC_FD to facilitate using this method as a callback function
%   (e.g., from an iterative linear solver such as Matlab's SYMMLQ).
%
%   See also TN.
%
%MATLAB Poblano Toolbox.
%Copyright 2009-2012, Sandia Corporation.

%% Number of calls to FUN
global nfev_hessvec_fd;

%% Check input
if nargin < 3
    error('HESSVEC_FD => at least two input arguments are required (V,FUN,X)');
end

% Compute gradient if not given
if nargin < 4
    [f,gx] = feval(FUN,x);
    nfev_hessvec_fd = nfev_hessvec_fd + 1;
end

% Compute a difference step if not given
if nargin < 5
    s = 1e-8*(1+norm(x));
end

% Compute the gradient at the new point
[f,gxsv] = feval(FUN,x+s*v);
nfev_hessvec_fd = nfev_hessvec_fd + 1;

%% Hessian vector product approximation
Hv = (gxsv-gx)/s;


function partition=new_partitions(sigma,r,x)
%sigma=1;
%r=8.4;
T=r/2/sigma;
N=ceil(r);
%width=sigma*(2*N/r-1);
%x=-width:1/200:width;

partition=zeros(N,length(x));

for k=1:N
  if k==1
    for j=1:length(x)
      if x(j)<=sigma-N/T
        partition(k,j)=0;
      elseif sigma-N/T<x(j) & x(j)<-sigma
        partition(k,j)=rho((-x(j)-sigma)/(N/T-2*sigma));
      elseif -sigma<=x(j) & x(j)<=(1-N)/T+sigma
        partition(k,j)=1;
      elseif (1-N)/T+sigma<x(j) & x(j)<1/T-sigma
	partition(k,j)=rho((x(j)-((1-N)/T+sigma))/(N/T-2*sigma));
%      elseif (1-N)/T+sigma<x(j) & x(j)<(1-N/2)/T
%        partition(k,j)=1-rho((-x(j)+(1-N/2)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(1-N/2)/T
%        partition(k,j)=1/2;
%      elseif (1-N/2)/T<x(j) & x(j)<1/T-sigma
%        partition(k,j)=rho((x(j)-(1-N/2)/T)/(N/T/2-sigma))/2;
      else
        partition(k,j)=0;
      end
    end
  elseif k==N
    for j=1:length(x)
      if x(j)<=sigma-1/T
        partition(k,j)=0;
      elseif sigma-1/T<x(j) & x(j)<(N-1)/T-sigma
        partition(k,j)=1-rho((x(j)-(sigma-1/T))/(N/T-2*sigma));
%      elseif sigma-1/T<x(j) & x(j)<(N/2-1)/T
%        partition(k,j)=rho((-x(j)+(N/2-1)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(N/2-1)/T
%        partition(k,j)=1/2;
%      elseif (N/2-1)/T<x(j) & x(j)<(N-1)/T-sigma
%        partition(k,j)=1-rho((x(j)-(N/2-1)/T)/(N/T/2-sigma))/2;
      elseif (N-1)/T-sigma<=x(j) & x(j)<=sigma
        partition(k,j)=1;
      elseif sigma<x(j) & x(j)<N/T-sigma
        partition(k,j)=rho((x(j)-sigma)/(N/T-2*sigma));
      else
        partition(k,j)=0;
      end
    end
  else
    for j=1:length(x)
      if x(j)<=(-N+k-1)/T+sigma
        partition(k,j)=0;
      elseif (-N+k-1)/T+sigma<x(j) & x(j)<(k-1)/T-sigma
        partition(k,j)=1-rho((x(j)-((-N+k-1)/T+sigma))/(N/T-2*sigma));
%      elseif (-N+k-1)/T+sigma<x(j) & x(j)<(k-1-N/2)/T
%        partition(k,j)=rho((-x(j)+(k-1-N/2)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(k-1-N/2)/T
%        partition(k,j)=1/2;
%      elseif (k-1-N/2)/T<x(j) & x(j)<(k-1)/T-sigma
%        partition(k,j)=1-rho((x(j)-(k-1-N/2)/T)/(N/T/2-sigma))/2;
      elseif (k-1)/T-sigma<=x(j) & x(j)<=(-N+k)/T+sigma
        partition(k,j)=1;
      elseif (-N+k)/T+sigma<x(j) & x(j)<k/T-sigma
        partition(k,j)=rho((x(j)-((-N+k)/T+sigma))/(N/T-2*sigma));
%      elseif (-N+k)/T+sigma<x(j) & x(j)<(k-N/2)/T
%        partition(k,j)=1-rho((-x(j)+(k-N/2)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(k-N/2)/T
%        partition(k,j)=1/2;
%      elseif (k-N/2)/T<x(j) & x(j)<k/T-sigma
%        partition(k,j)=rho((x(j)-(k-N/2)/T)/(N/T/2-sigma))/2;
      else
        partition(k,j)=0;
      end
    end
  end
%  plot(x,partition(k,:))
%  axis([min(x) max(x) 0 1])
%  [k N]
%  pause
end
%plot(x,partition'); %'
%pause

%total=zeros(size(x));
%for j=1:N
%total=total+partition(j,:);
%end
%spy(1-total);


function [H, H_f, H_g] = composeFrames(F,G)

% COMPOSEFRAMES  Compose two 3D frames.
%   H = COMPOSEFRAMES(F,G) composes frames F and G, where frame G is
%   specified in frame F, to get a single frame transform H. Frames are
%   structures with at least the following fields:
%       .t  translation vector
%       .q  orientation quaternion
%
%   The resulting frame H, however, contains the full frame structure.
%
%   [H, H_f, H_g] = COMPOSEFRAMES(...) returns the Jacobians of H.x wrt F.x
%   and G.x.
%
%   See also FRAME, SPLITFRAME, QUATERNION.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargout == 1

    H.t = fromFrame(F,G.t);
    H.q = qProd(F.q,G.q);
    H.x = [H.t;H.q];
    H   = updateFrame(H);

else

    [H.t, T_f, T_tg]  = fromFrame(F,G.t);
    [H.q, Q_qf, Q_qg] = qProd(F.q,G.q);

    H.x = [H.t;H.q];
    H   = updateFrame(H);

    H_f(1:3,:)   = T_f;
    H_f(4:7,4:7) = Q_qf;
    H_g(1:3,1:3) = T_tg;
    H_g(4:7,4:7) = Q_qg;

end

return

%%
syms x y z a b c d X Y Z A B C D real
F.x = [x y z a b c d]';
G.x = [X Y Z A B C D]';
F = updateFrame(F);
G = updateFrame(G);
[H, H_f, H_g] = composeFrames(F,G);
simplify(H_f-jacobian(H.x,F.x))
simplify(H_g-jacobian(H.x,G.x))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


% Extended Kernel Recursive Least Squares algorithm
%
% W. Liu, I.M. Park. Y. Wang, and J.C. Principe, "Extended Kernel Recursive
% Least Squares Algorithm," IEEE Transactions on Signal Processing, vol.
% 57, no. 10, pp. 3801-3814, Oct. 2009,
% http://dx.doi.org/10.1109/TSP.2009.2022007
%
% Remark: implementation of the tracking model, includes a maximum
% dictionary size M
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

classdef exkrls < kernel_adaptive_filter

    properties (GetAccess = 'public', SetAccess = 'private')
        alphaf = .999; % state forgetting factor, "alpha" in publication
        beta = .995; % data forgetting factor
        lambda = 1E-2; % regularization
        q = 1E-3; % trade-off between modeling variation and measurement disturbance
        M = 500; % maximum dictionary size
        kerneltype = 'gauss'; % kernel type
        kernelpar = 1; % kernel parameter
    end

    properties (GetAccess = 'public', SetAccess = 'private')
        dict = []; % dictionary
        rho = [];
        Q = [];
        i = 0; % iteration number;
        alpha = []; % expansion coefficients, "a" in publication
    end

    methods
        function kaf = exkrls(parameters) % constructor
            allpars = {'alphaf','lambda','beta','q','kerneltype','kernelpar','M'};
            if (nargin > 0)
                for j=1:length(allpars)
                    p = allpars{j};
                    if isfield(parameters,p), kaf.(p) = parameters.(p); end
                end
            end
        end

        function y_est = evaluate(kaf,x) % evaluate the algorithm
            if size(kaf.dict,1)>0
                k = kernel(kaf.dict,x,kaf.kerneltype,kaf.kernelpar);
                y_est = k'*kaf.alpha;
            else
                y_est = zeros(size(x,1),1);
            end
        end

        function train(kaf,x,y) % train the algorithm
            kaf.i = kaf.i + 1;
            k = kernel([kaf.dict; x],x,kaf.kerneltype,kaf.kernelpar);
            kt = k(1:end-1);
            ktt = k(end);
            if numel(kt)==0 % initialize
                kaf.alpha = kaf.alphaf*y/(kaf.lambda*kaf.beta+ktt);
                kaf.rho = kaf.lambda*kaf.beta/(kaf.alphaf^2*kaf.beta + kaf.lambda*kaf.q);
                kaf.Q = kaf.alphaf^2/((kaf.beta*kaf.lambda+ktt)*(kaf.alphaf^2+kaf.beta*kaf.lambda*kaf.q));
                kaf.dict = x;
            else
                if (size(kaf.dict,1)<kaf.M) % avoid infinite growth
                    z = kaf.Q*kt;
                    r = kaf.beta^kaf.i*kaf.rho + ktt - kt'*z;
                    err = y - kt'*kaf.alpha;

                    kaf.alpha = kaf.alphaf*[kaf.alpha - z*err/r; err/r]; % grow
                    kaf.dict = [kaf.dict; x];
                    dummy = kaf.alphaf^2 + kaf.beta^kaf.i*kaf.q*kaf.rho;
                    kaf.rho = kaf.rho/dummy;
                    kaf.Q = kaf.alphaf^2/(r*dummy)*...
                        [kaf.Q*r + z*z', -z; -z', 1];
                end
            end
        end
    end
end


function out1 = Sindy_ODE_RHS(in1,in2)
%SINDY_ODE_RHS
%    OUT1 = SINDY_ODE_RHS(IN1,IN2)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    31-Jul-2019 21:06:56

dz1 = in2(:,1);
dz2 = in2(:,2);
dz4 = in2(:,4);
z1 = in1(:,1);
z2 = in1(:,2);
t2 = z2.*-1.0;
t3 = t2+z1;
out1 = dz1.*-3.235001066692433e-2+dz2.*6.00492565527997e-3+sin(z1).*3.796608727034097e1-dz2.^2.*sin(t3).*3.229288227581633e-1-dz4.*cos(t3).*3.229288227581664e-1;


function K = covPERiso(cov, hyp, x, z, i)

% Stationary periodic covariance function for an isotropic stationary covariance
% function k0 such as covMaterniso, covPPiso, covRQiso and covSEiso.
% Isotropic stationary means that the covariance function k0(x,z) depends on the
% data points x,z only through the squared distance
% dxz = (x-z)'*inv(P)*(x-z) where the P matrix is ell^2 times the unit matrix.
% The covariance function is parameterized as:
%
% k(x,z) = k0(u(x),u(z)), u(x) = [sin(pi*x/p); cos(pi*x/p)]
%
% where the period p belongs to covPERiso and hyp0 belong to k0:
%
% hyp = [ log(p)
%         hyp0 ]
%
% The first hyperparameter of k0 is the log lengthscale hyp0(1) = log(ell).
% Note that for k0 = covSEiso and D = 1, a faster alternative is covPeriodic.
%
% Copyright (c) by Hannes Nickisch, 2013-10-15.
%
% See also COVFUNCTIONS.M.

nocov = false;                   % default case when no cov argument is provided
if nargin==0, cov = {@covSEiso}; nocov = true; end                % default case
if isnumeric(cov)       % detect old version where the cov parameter was missing
  % i <- z, z <- x, x <- hyp, hyp <- cov
  if nargin>3, i = z; end
  if nargin>2, z = x; end
  if nargin>1, x = hyp; end
  hyp = cov; cov = {@covSEiso}; nocov = true;
end

if nocov && nargin<2 || ~nocov && nargin<3         % report number of parameters
  K = ['(1+',feval(cov{:}),')']; return
end
if nocov && nargin<3 || ~nocov && nargin<4, z = []; end    % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

[n,D] = size(x);
p = exp(hyp(1));

if nocov && nargin<4 || ~nocov && nargin<5
  [x,z] = u(x,z,p,dg);                      % apply the embedding u:IR^D->IR^2*D
  K = feval(cov{:},hyp(2:end),x,z);
else
  if i==1
    if dg                                                   % compute distance d
      d = zeros([n,1,D]);
    else
      if xeqz                                             % symmetric matrix Kxx
        d = repmat(reshape(x,n,1,D),[1,n, 1])-repmat(reshape(x,1,n, D),[n,1,1]);
      else                                               % cross covariances Kxz
        nz = size(z,1);
        d = repmat(reshape(x,n,1,D),[1,nz,1])-repmat(reshape(z,1,nz,D),[n,1,1]);
      end
    end
    d = 2*pi*d/p; dD2_dlp = -2*sum(sin(d).*d,3);        % derivative dD2/dlog(p)
    [x,z] = u(x,z,p,dg);                    % apply the embedding u:IR^D->IR^2*D
    if dg                                            % compute squared distances
      D2 = zeros(n,1);
    else
      if xeqz, D2 = sq_dist(x'); else D2 = sq_dist(x',z'); end
    end
    % reconstruct derivative w.r.t. D2 from derivative w.r.t. log(ell)
    dK_dD2 = feval(cov{:},hyp(2:end),x,z,1)./(-2*D2); dK_dD2(D2<1e-12) = 0;
    K = dK_dD2.*dD2_dlp;                                      % apply chain rule
  else
    [x,z] = u(x,z,p,dg);                    % apply the embedding u:IR^D->IR^2*D
    K = feval(cov{:},hyp(2:end),x,z,i-1);
  end
end

function [x,z] = u(x,z,p,dg)                % apply the embedding u:IR^D->IR^2*D
  x = 2*pi*x/p; x = [sin(x), cos(x)];
  if numel(z)>0 && ~dg, z = 2*pi*z/p; z = [sin(z), cos(z)]; end


function [error_train, error_val] = ...
    learningCurve(X, y, Xval, yval, lambda)
%LEARNINGCURVE Generates the train and cross validation set errors needed
%to plot a learning curve
%   [error_train, error_val] = ...
%       LEARNINGCURVE(X, y, Xval, yval, lambda) returns the train and
%       cross validation set errors for a learning curve. In particular,
%       it returns two vectors of the same length - error_train and
%       error_val. Then, error_train(i) contains the training error for
%       i examples (and similarly for error_val(i)).
%
%   In this function, you will compute the train and test errors for
%   dataset sizes from 1 up to m. In practice, when working with larger
%   datasets, you might want to do this in larger intervals.
%

% Number of training examples
m = size(X, 1);

% You need to return these values correctly
error_train = zeros(m, 1);
error_val   = zeros(m, 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return training errors in
%               error_train and the cross validation errors in error_val.
%               The vector numex_vec contains the number of training
%               examples to use for each calculation of training error and
%               cross validation error, i.e, error_train(i) and
%               error_val(i) should give you the errors
%               obtained after training on i examples.
%
% Note: You should evaluate the training error on the first i training
%       examples (i.e., X(1:i, :) and y(1:i)).
%
%       For the cross-validation error, you should instead evaluate on
%       the _entire_ cross validation set (Xval and yval).
%
% Note: If you are using your cost function (linearRegCostFunction)
%       to compute the training and cross validation error, you should
%       call the function with the lambda argument set to 0.
%       Do note that you will still need to use lambda when running
%       the training to obtain the theta parameters.
%
% Hint: You can loop over the examples with the following:
%
%       for i = 1:m
%           % Compute train/cross validation errors using training examples
%           % X(1:i, :) and y(1:i), storing the result in
%           % error_train(i) and error_val(i)
%           ....
%
%       end
%

% ---------------------- Sample Solution ----------------------

% linearRegCostFunction(X, y, theta, lambda)

for i = 1:m,
  X_train = X(1:i, :);
  y_train = y(1:i);
  theta = trainLinearReg(X_train, y_train, lambda);
  error_train(i)  = linearRegCostFunction(X_train, y_train, theta, 0);
  error_val(i)    = linearRegCostFunction(Xval, yval, theta, 0);
end

% -------------------------------------------------------------

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

end


function [x, c, funVal, ValueL]=tree_LogisticR(A, y, z, opts)
%
%%
% Function tree_LogisticR
%      Logistic Loss with the
%           tree structured group Lasso Regularization
%
%% Problem
%
%  min  f(x,c) = - sum_i weight_i * log (p_i) + z * sum_j w_j ||x_{G_j}||
%
%  a_i denotes a training sample,
%      and a_i' corresponds to the i-th row of the data matrix A
%
%  y_i (either 1 or -1) is the response
%
%  p_i= 1/ (1+ exp(-y_i (x' * a_i + c) ) ) denotes the probability
%
%  G_j's are nodes with tree structure
%
%  The tree overlapping group information is contained in
%  opts.ind, which is a 3 x nodes matrix, where nodes denotes the number of
%  nodes of the tree.
%  opts.ind(1,:) contains the starting index
%  opts.ind(2,:) contains the ending index
%  opts.ind(3,:) contains the corresponding weight (w_j)
%
%  Note:
%  1) If each element of x is a leaf node of the tree and the weight for
%  this leaf node are the same, we provide an alternative "efficient" input
%  for this kind of node, by creating a "super node" with
%  opts.ind(1,1)=-1; opts.ind(2,1)=-1; and opts.ind(3,1)=the common weight.
%
%  2) If the features are well ordered in that, the features of the left
%  tree is always less than those of the right tree, opts.ind(1,:) and
%  opts.ind(2,:) contain the "real" starting and ending indices. That is to
%  say, x( opts.ind(1,j):opts.ind(2,j) ) denotes x_{G_j}. In this case,
%  the entries in opts.ind(1:2,:) are within 1 and n.
%
%
%  If the features are not well ordered, please use the input opts.G for
%  specifying the index so that
%   x( opts.G ( opts.ind(1,j):opts.ind(2,j) ) ) denotes x_{G_j}.
%  In this case, the entries of opts.G are within 1 and n, and the entries of
%  opts.ind(1:2,:) are within 1 and length(opts.G).
%
% The following example shows how G and ind works:
%
% G={ {1, 2}, {4, 5}, {3, 6}, {7, 8},
%     {1, 2, 3, 6}, {4, 5, 7, 8},
%     {1, 2, 3, 4, 5, 6, 7, 8} }.
%
% ind={ [1, 2, 100]', [3, 4, 100]', [5, 6, 100]', [7, 8, 100]',
%       [9, 12, 100]', [13, 16, 100]', [17, 24, 100]' },
%
% where each node has a weight of 100.
%
%% Input parameters:
%
%  A-         Matrix of size m x n
%                A can be a dense matrix
%                         a sparse matrix
%                         or a DCT matrix
%  y -        Response vector (of size mx1)
%  z -        Regularization parameter (z >=0)
%  opts-      optional inputs (default value: opts=[])
%             !!For tr_LogisticR, we require that opts.ind is specified.!!
%
%% Output parameters:
%  x-         The obtained weight of size n x 1
%  c-         The obtained intercept (scalar)
%  funVal-    Function value during iterations
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
% Last modified on October 3, 2010.
%
%% Related papers
%
% [1] Jun Liu and Jieping Ye, Moreau-Yosida Regularization for
%     Grouped Tree Structure Learning, NIPS 2010
%
%% Related functions:
%
%  sll_opts
%
%%

%% Verify and initialize the parameters
%%
if (nargin <4)
    error('\n Inputs: A, y, z, and opts (.ind) should be specified!\n');
end

[m,n]=size(A);

if (length(y) ~=m)
    error('\n Check the length of y!\n');
end

if (z<0)
    error('\n z should be nonnegative!\n');
end

opts=sll_opts(opts); % run sll_opts to set default values (flags)

%% Detailed initialization
%% Normalization

% Please refer to the function 'sll_opts'
%                 for the definitions of mu, nu and nFlag
%
% If .nFlag =1, the input matrix A is normalized to
%                     A= ( A- repmat(mu, m,1) ) * diag(nu)^{-1}
%
% If .nFlag =2, the input matrix A is normalized to
%                     A= diag(nu)^{-1} * ( A- repmat(mu, m,1) )
%
% Such normalization is done implicitly.
%     This implicit normalization is suggested for the sparse matrix,
%                                    but not for the dense matrix.
%

if (opts.nFlag~=0)
    if (isfield(opts,'mu'))
        mu=opts.mu;
        if(size(mu,2)~=n)
            error('\n Check the input .mu');
        end
    else
        mu=mean(A,1);
    end

    if (opts.nFlag==1)
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=n)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,1)/m).^(0.5); nu=nu';
        end
    else % .nFlag=2
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=m)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,2)/n).^(0.5);
        end
    end

    ind_zero=find(abs(nu)<= 1e-10);    nu(ind_zero)=1;
    % If some values in nu is typically small, it might be that,
    % the entries in a given row or column in A are all close to zero.
    % For numerical stability, we set the corresponding value to 1.
end

if (~issparse(A)) && (opts.nFlag~=0)
    fprintf('\n -----------------------------------------------------');
    fprintf('\n The data is not sparse or not stored in sparse format');
    fprintf('\n The code still works.');
    fprintf('\n But we suggest you to normalize the data directly,');
    fprintf('\n for achieving better efficiency.');
    fprintf('\n -----------------------------------------------------');
end

%% Group & Others

% Initialize ind
if (~isfield(opts,'ind'))
    error('\n In tree_LeastR, the field .ind should be specified');
else
    ind=opts.ind;

    if (size(ind,1)~=3)
        error('\n Check opts.ind');
    end
end

% The parameter 'weight' contains the weight for each training sample.
% See the definition of the problem above.
% The summation of the weights for all the samples equals to 1.
if (isfield(opts,'sWeight'))
    sWeight=opts.sWeight;

    if ( length(sWeight)~=2 || sWeight(1) <=0 || sWeight(2) <= 0)
        error('\n Check opts.sWeight, which contains two positive values');
    end

    % we process the weight, so that the summation of the weights for all
    % the samples is 1.

    p_flag=(y==1);                  % the indices of the postive samples
    m1=sum(p_flag) * sWeight(1);    % the total weight for the positive samples
    m2=sum(~p_flag) * sWeight(2);   % the total weight for the positive samples

    weight(p_flag,1)=sWeight(1)/(m1+m2);
    weight(~p_flag,1)=sWeight(2)/(m1+m2);
else
    weight=ones(m,1)/m;             % if not specified, we apply equal weight
end

GFlag=0;
% if GFlag=1, we shall apply general_altra
if (isfield(opts,'G'))
    GFlag=1;

    G=opts.G;
    if (max(G) >n || max(G) <1)
        error('\n The input G is incorrect. It should be within %d and %d',1,n);
    end
end

%% Starting point initialization

p_flag=(y==1);                  % the indices of the postive samples
m1=sum(weight(p_flag));         % the total weight for the positive samples
m2=1-m1;                        % the total weight for the positive samples

% process the regularization parameter
if (opts.rFlag==0)
    lambda=z;
else % z here is the scaling factor lying in [0,1]
%     if (z<0 || z>1)
%         error('\n opts.rFlag=1, and z should be in [0,1]');
%     end

    computedFlag=0;
    if (isfield(opts,'lambdaMax'))
        if (opts.lambdaMax~=-1)
            lambda=z*opts.lambdaMax;
            computedFlag=1;
        end
    end

    if (~computedFlag)

        % we compute ATb for computing lambda_max, when the input z is a ratio
        b(p_flag,1)=m2;  b(~p_flag,1)=-m1;
        b=b.*weight;

        % compute AT b
        if (opts.nFlag==0)
            ATb =A'*b;
        elseif (opts.nFlag==1)
            ATb= A'*b - sum(b) * mu';  ATb=ATb./nu;
        else
            invNu=b./nu;               ATb=A'*invNu-sum(invNu)*mu';
        end

        % compute lambda_max
        if (GFlag==0)
            lambda_max=findLambdaMax(ATb, n, ind, size(ind,2));
        else
            lambda_max=general_findLambdaMax(ATb, n, G, ind, size(ind,2));
        end

        % As .rFlag=1, we set lambda as a ratio of lambda_max
        lambda=z*lambda_max;

    end
end

% The following is for computing lambdaMax
% we use opts.lambdaMax=-1 to show that we need the computation.
%
% One can use this for setting up opts.lambdaMax
if (isfield(opts,'lambdaMax'))

    if (opts.lambdaMax==-1)

        % we compute ATb for computing lambda_max, when the input z is a ratio
        b(p_flag,1)=m2;  b(~p_flag,1)=-m1;
        b=b.*weight;

        % compute AT b
        if (opts.nFlag==0)
            ATb =A'*b;
        elseif (opts.nFlag==1)
            ATb= A'*b - sum(b) * mu';  ATb=ATb./nu;
        else
            invNu=b./nu;               ATb=A'*invNu-sum(invNu)*mu';
        end

        % compute lambda_max
        if (GFlag==0)
            lambda_max=findLambdaMax(ATb, n, ind, size(ind,2));
        else
            lambda_max=general_findLambdaMax(ATb, n, G, ind, size(ind,2));
        end

        x=lambda_max;
        funVal=lambda_max;
        ValueL=lambda_max;

        return;
    end
end

% initialize a starting point
if opts.init==2
    x=zeros(n,1); c=log(m1/m2);
else
    if isfield(opts,'x0')
        x=opts.x0;
        if (length(x)~=n)
            error('\n Check the input .x0');
        end
    else
        x=zeros(n,1);
    end

    if isfield(opts,'c0')
        c=opts.c0;
    else
        c=log(m1/m2);
    end
end

% compute A x
if (opts.nFlag==0)
    Ax=A* x;
elseif (opts.nFlag==1)
    invNu=x./nu; mu_invNu=mu * invNu;
    Ax=A*invNu -repmat(mu_invNu, m, 1);
else
    Ax=A*x-repmat(mu*x, m, 1);    Ax=Ax./nu;
end

%% The main program

if (opts.mFlag==0 && opts.lFlag==0)

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1/m; % the intial guess of the Lipschitz continuous gradient

    weighty=weight.*y;
    % the product between weight and y

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,1);
    cp=c;         ccp=0;

    %% The Armijo Goldstein line search schemes + accelearted gradient descent

    alphap=0; alpha=1;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    s=x + beta* xxp;  sc=c + beta* ccp;

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute As=A*s
        As=Ax + beta* (Ax-Axp);

        % aa= - diag(y) * (A * s + sc)
        aa=- y.*(As+ sc);

        % fun_s is the logistic loss at the search point
        bb=max(aa,0);
        fun_s= weight' * ( log( exp(-bb) +  exp(aa-bb) ) + bb );

        % compute prob=[p_1;p_2;...;p_m]
        prob=1./( 1+ exp(aa) );

        % b= - diag(y.* weight) * (1 - prob)
        b= -weighty.*(1-prob);

        gc=sum(b); % the gradient of c

        % compute g= AT b, the gradient of x
        if (opts.nFlag==0)
            g=A'*b;
        elseif (opts.nFlag==1)
            g=A'*b - sum(b) * mu';  g=g./nu;
        else
            invNu=b./nu;              g=A'*invNu-sum(invNu)*mu';
        end

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;
        cp=c;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the L1/Lq-norm regularized projection
            v=s-g/L;  c= sc- gc/L;

            % tree overlapping group Lasso projection
            ind_work(1:2,:)=ind(1:2,:);
            ind_work(3,:)=ind(3,:) * (lambda / L);

            if (GFlag==0)
                x=altra(v, n, ind_work, size(ind_work,2));
            else
                x=general_altra(v, n, G, ind_work, size(ind_work,2));
            end

            v=x-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute A x
            if (opts.nFlag==0)
                Ax=A* x;
            elseif (opts.nFlag==1)
                invNu=x./nu; mu_invNu=mu * invNu;
                Ax=A*invNu -repmat(mu_invNu, m, 1);
            else
                Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
            end

            % aa= - diag(y) * (A * x + c)
            aa=- y.*(Ax+ c);

            % fun_x is the logistic loss at the new approximate solution
            bb=max(aa,0);
            fun_x= weight'* ( log( exp(-bb) +  exp(aa-bb) ) + bb );

            r_sum= (v'*v + (c-sc)^2) / 2;
            l_sum=fun_x - fun_s - v'* g - (c-sc)* gc;

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is fun_x <= fun_s + v'* g + c * gc
            %                           + L/2 * (v'*v + (c-sc)^2 )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                %fprintf('\n L=%e, r_sum=%e',L, r_sum);
            end
        end

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        ValueL(iterStep)=L;
        % store values for L

        xxp=x-xp;    ccp=c-cp;
        funVal(iterStep)=fun_x;

        % compute the regularization part
        if (GFlag==0)
            tree_norm=treeNorm(x, n, ind, size(ind,2));
        else
            tree_norm=general_treeNorm(x, n, G, ind, size(ind,2));
        end

        % function value = loss + regularizatioin
        funVal(iterStep)=fun_x + lambda * tree_norm;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp);    norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
else
    error('\n The function does not support opts.mFlag neq 0 & opts.lFlag neq 0!');
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   [T, vx, ax, phi, phid]=directkinematic_5R(robot, q)
%
%   Direct kinematics for the 5R planar parallel robot.
%
%   This script comprises the computation of the position of the end
%   effector T as a function of q. As well, the speed and acceleration
%   of the end effector can be also computed. The speed of the passive
%   joints phid are also computed.
%
%
%   In general, given a value of active joint coordinates q1, q2,
%   there exists two possible solutions. The position of the end effector P(x,y) is returned as two different
%   homogeneous matrices T= [T1, T2] with two possible positions of P in
%   each. If the robot is placed at a singular point P (direct kinematic
%   singularity), then both solutions are coincident. If this is the case,
%   a differential movement
%
%   In addition, if qd (q dot, speed), or qdd (q dot dot, acceleration)
%   of the joint variables, is given, the vx (speed) and ax (acceleration)
%   in cartesian coordinates are also computed.
%
%   Given q1 and q2, in this 5R robot, two possible solutions are feasible.
%   In consequence, vx=[vx1, vx3] where each column stores the speed [vx, vy]
%   for each of the possible solutions
%
%   Please, note that, given a value of active joint coordinates q1, q2,
%   there exists different possible solutions for P()
%
%   Author: Arturo Gil Aparicio, arturo.gil@umh.es
%   Date: 13/09/2013
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

function [T, vx, ax, phi, phid]=directkinematic_5R(robot, q)
%close all

a1=eval(robot.robot1.DH.a);
a2=eval(robot.robot2.DH.a);

%Link lengths
l1=abs(a1(1));
l2=abs(a1(2));
l3=abs(a2(1));
l4=abs(a2(2));
L=robot.L;%2.5;

xA=cos(q(1));
yA=sin(q(1));

xB=cos(q(2))+L;
yB=sin(q(2));

%figure,
plot(0,0,'r.'), %hold
plot(xA, yA,'k.')
plot(xB, yB,'k.')
plot_line([0 0 0], [xA yA 0], 'r', 2)
plot_line([L 0 0], [xB yB 0], 'r', 2)
%plot(L, 0,'*g')

%draw circles around xA, yA and xB yB as a first approximation
x=-0.47:0.001:2;
rr=[];
for i=1:length(x),
    r=solve_poly(x(i), xA, yA, l2);
    rr=[rr r];
end
plot(x, real(rr(1,:)))
plot(x, real(rr(2,:)))

x=0.5:0.001:3.085;
rr=[];
for i=1:length(x),
    r=solve_poly(x(i), xB, yB, l3);
    rr=[rr r];
end
plot(x, real(rr(1,:)), 'b')
plot(x, real(rr(2,:)), 'b')

%compute polynomial in terms of alpha, beta and delta
af=xA-xB;
be=l2^2-l1^2-l3^2+xB^2+yB^2;
de=(yB-yA);

a=(de^2+af^2);
b=af*be-2*xA*de^2-2*yA*af*de;
c=de^2*(xA^2+yA^2)+be^2/4-yA*be*de-l2^2*de^2;

%two different solutions for x
x1=(-b+sqrt(b^2-4*a*c))/(2*a);
x2=(-b-sqrt(b^2-4*a*c))/(2*a);

%find y from polynomial, given x1 and x2
r1=solve_poly(x1, xA, yA, l2);
r2=solve_poly(x2, xA, yA, l2);
r3=solve_poly(x1, xB, yB, l3);
r4=solve_poly(x2, xB, yB, l3);

%find those solutions that comply with both equations
R1=[r1; r2];
R2=[r3; r4];
%to do this, obtain only those roots that are repeated
y=[];
for i=1:length(R1),
   val=is_double_root(R1(i),R2);
   if val==1
       y=[y R1(i)];
   end
end

%Plot error if an unfeasible solution is found
if (~isreal(y))
    disp('ERROR: directkinematic_5R: unfeasible solution');
end

y1=y(1);
y2=y(2);

%return solutions in homogeneous matrices
T1=eye(4);
T2=eye(4);

T1(1,4)=x1;
T1(2,4)=y1;
T2(1,4)=x2;
T2(2,4)=y2;
T=[];
T=[T1 T2];

% avoid warnings for imaginary parts when plottin
% the above message should be triggered when an unfeasible solution is
% found
y1=real(y(1));
y2=real(y(2));
x1=real(x1);
x2=real(x2);

%plot solutions
plot_line([xA yA 0], [x1 y1 0], 'y', 2)
plot_line([xB yB 0], [x1 y1 0], 'y', 2)

plot_line([xA yA 0], [x2 y2 0], 'g', 2)
plot_line([xB yB 0], [x2 y2 0], 'g', 2)

% in this case, q1, q2, qd1, qd2
if length(q)==4
    fprintf('directkinematic_5R:: Computing speeds');
    q1=q(1);
    q2=q(2);
    qd1=q(3);
    qd2=q(4);
    %compute direct kinematic speeds,
    %given joint values q1 and q2 and joint speeds qd1 and qd2,
    %compute the speed
    [vx1, phi1, phid1]=compute_direct_speeds(robot, [q1 q2], [qd1 qd2], T1);
    [vx2, phi2, phid2]=compute_direct_speeds(robot, [q1 q2], [qd1 qd2], T2);
    vx=[vx1, vx2];
    phi=[phi1, phi2]; %position of passive joints
    phid=[phid1, phid2];%speed of passive joints
end

ax=0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Build de coefficients of a polynomial as a function of the
%   points xA, yA and the length of the link L.
%   Solve for second order polynomial:
%   ax^2+bx+c=0;
%
%   as x=(-b +- sqrt(b^2-4*a*c))/(2*a)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function r=solve_poly(x, xAB, yAB, L)
a=1;
b=-2*yAB;
c=x^2-2*x*xAB-L^2+xAB^2+yAB^2;

r =[(-b+sqrt(b^2-4*a*c))/(2*a); (-b-sqrt(b^2-4*a*c))/(2*a)];

function yes_no=is_double_root(R1_i,R2)
thres=0.0001;
[j, k, v]=find(abs(R2-R1_i)<thres);

%in case a match has been found
if length(j)>0
        yes_no=1;
else
        yes_no=0;
end

function plot_line(p0, p1, color, w)
x0 = p0(1);
y0 = p0(2);
z0 = p0(3);
x1 = p1(1);
y1 = p1(2);
z1 = p1(3);
% Draw a line between p0 and p1
plot3([x0;x1],[y0;y1],[z0;z1], color, 'LineWidth',w);

function [vx, phi, phid]=compute_direct_speeds(robot, q, qd, T)
% caution, q1 and q2 are the active coordinates,
% the compute jacobians requires every of the 4 joint values
% in this order q={q1, phi1, q2, phi2}

q1=q(1);
q2=q(2);
qd1=qd(1);
qd2=qd(2);

%yes, compute the inverse kinematic to find the passive joint values that
%correspond to the position specified by T. We choose the only solution
% in which q1 and q2 are equal to the given values
qinv=inversekinematic(robot, T);
for i=1:4,
    qq=qinv(:,i);
    %simple threshold are used to find the most similar solution.
    if(abs(qq(1)-q1)<0.05) && (abs(qq(3)-q2)<0.05)
        % yes, we have used the inversekinematic function to compute
        % the phi1 and phi2 angles
        phi1=qq(2);
        phi2=qq(4);
        [JX, Jq, Jphi]=compute_jacobians_5R(robot, [q1 phi1 q2 phi2]);

        if det([JX Jphi])==0
            fprintf('\n WARNING: directkinematic_5R: direct kinematic singularity detected');
        end
        %Now that the Jacobians are known, compute vx and Phid
        [vxphid]=-inv([JX Jphi])*Jq*[qd1; qd2];
        vx=vxphid(1:2);
        phid=vxphid(3:4);
        phi=[phi1, phi2]';
        return;
    end
end


function [Pa] = inH2O2Pa(inH2O)
% Convert pressure from inches of water column at 4 degrees to pascals
% Chad Greene 2012
Pa = inH2O*249.089;


function [a,n,r,epe,eph,epw] = mg_q1cd(xy,ev)
%mg_q1cd   convection-diffusion matrix generator for GMG
%   [a,n,r,epe,eph,epw] = mg_q1cd(xy,ev)
%   input
%           xy         vertex coordinate vector
%           ev         element mapping matrix
%   output
%           a          discrete diffusion operator
%           n          discrete convection operator
%           r          dummy variable
%           epe        viscosity normalised element peclet numbers
%           eph        flow specific element lengths
%           epw        centroid evaluated wind
%
%   IFISS function: DJS; 5 January 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

%   Analogous to femq1_cd
%
x=xy(:,1); y=xy(:,2);
nvtx=length(x);
nel=length(ev(:,1));
lx=max(x)-min(x); ly=max(y)-min(y);
hx=max(diff(x)); hy=max(diff(y));
%
% initialise global matrices
      a = sparse(nvtx,nvtx);
      n = sparse(nvtx,nvtx);
      r = sparse(nvtx,nvtx);
%
% set up 2x2 Gauss points
      gpt=1.0e0/sqrt(3.0e0);
      s(1) = -gpt;  t(1) = -gpt;
      s(2) =  gpt;  t(2) = -gpt;
      s(3) =  gpt;  t(3) =  gpt;
      s(4) = -gpt;  t(4) =  gpt;
%
% inner loop over elements
        for ivtx = 1:4
        xl_v(:,ivtx) = x(ev(:,ivtx));
        yl_v(:,ivtx) = y(ev(:,ivtx));
		end
        ae = zeros(nel,4,4);
	    ne = zeros(nel,4,4);
% loop over 2x2 Gauss points
         for igpt = 1:4
         sigpt=s(igpt);
         tigpt=t(igpt);
%  evaluate derivatives etc
         [jac,invjac,phi,dphidx,dphidy] = deriv(sigpt,tigpt,xl_v,yl_v);
         [flowx,flowy] = gauss_transprt(sigpt,tigpt,xl_v,yl_v);
		 for j = 1:4
               for i = 1:4
               ae(:,i,j) = ae(:,i,j)  + dphidx(:,i).*dphidx(:,j) .* invjac(:);
               ae(:,i,j) = ae(:,i,j)  + dphidy(:,i).*dphidy(:,j) .* invjac(:);
               ne(:,i,j) = ne(:,i,j) + flowx(:) .* phi(:,i) .* dphidx(:,j);
               ne(:,i,j) = ne(:,i,j) + flowy(:) .* phi(:,i) .* dphidy(:,j);
               end
	    end
% end of Gauss point loop
         end
%
% perform assembly of global matrix  and source vector
      for krow=1:4
	  nrow=ev(:,krow);
          for kcol=1:4
		  ncol=ev(:,kcol);
          a = a + sparse(nrow,ncol,ae(:,krow,kcol),nvtx,nvtx);
          n = n + sparse(nrow,ncol,ne(:,krow,kcol),nvtx,nvtx);
          end
      end
%
%
% computation of element Peclet number (at the centroid)
% rectangle specific calculation here
      hx=abs(xl_v(:,2)-xl_v(:,1)); hy=abs(yl_v(:,3)-yl_v(:,2));
      [flowx,flowy] = gauss_transprt(0,0,xl_v,yl_v);
      flow_l2 = sqrt(flowx(:) .* flowx(:) + flowy(:) .* flowy(:));
        if all(flowx==0), flow_h=hy;
	elseif all(flowy==0), flow_h=hx;
		else
          angle = atan(abs(flowy./flowx));
          flow_h = min([hx./cos(angle),hy./sin(angle)],[],2);
        end
	  eph = flow_h;
      epe = flow_h.*flow_l2/2;
	  epw = flow_l2;
%
return


function log_evidence = ldae_chibms(words, topics, topic_prior, ms_iters)
%LDAE_CHIBMS Approximate evidence for LDA using Murray & Salakhutdinov's Chib-style method
%
% log_evidence = ldae_chibms(words, topics, topic_prior);
%
% Inputs:
%             words 1xNd
%            topics TxV each row is a distribution over a vocabulary of size V
%       topic_prior 1xT parameters of Dirichlet from which document topic vector is drawn
%          ms_iters 1x1 Default: 1000
%
% Outputs:
%     log_evidence  1x1

% Iain Murray, January 2009

BURN_ITERS = 3;

if ~exist('ms_iters', 'var')
    ms_iters = 1000;
end

[T, V] = size(topics);
Nd = length(words);

% Sanity checking input sizes
assert(isvector(topic_prior));
assert(T == length(topic_prior));
assert(isvector(words));

topic_prior = topic_prior(:)';
topic_alpha = sum(topic_prior);
%topic_mean = topic_prior / topic_alpha;

% Assign latents to words in isolation as a simple initialization
Nz = zeros(1, T);
for t = 1:Nd
    pz = topics(:, words(t))' .* topic_prior;
    zz(t) = discreternd(1, pz);
    Nz(zz(t)) = Nz(zz(t)) + 1;
end

% Run some sweeps of Gibbs sampling
for sweeps = 1:BURN_ITERS
    for t = 1:Nd
        Nz(zz(t)) = Nz(zz(t)) - 1;
        pz = topics(:, words(t))' .* (Nz + topic_prior);
        zz(t) = discreternd(1, pz);
        Nz(zz(t)) = Nz(zz(t)) + 1;
    end
end

% Find local optimim to use as z^*, "iterative conditional modes"
% But don't spend forever on this, bail out if necessary
for i = 1:12
    old_zz = zz;
    for t = 1:Nd
        Nz(zz(t)) = Nz(zz(t)) - 1;
        pz = topics(:, words(t))' .* (Nz + topic_prior);
        [dummy, zz(t)] = max(pz);
        Nz(zz(t)) = Nz(zz(t)) + 1;
    end
    if ~isequal(old_zz, zz)
        break;
    end
end

% Run Murray & Salakhutdinov algorithm
zstar = zz;
log_Tvals = zeros(ms_iters, 1);
log_Tprob = @(zto, zfrom, Nzfrom) log_Tprob_base(zto, zfrom, Nzfrom, words, topics, topic_prior);
% draw starting position
ss = ceil(rand() * ms_iters);
% Draw z^(s)
for t = Nd:-1:1
    Nz(zz(t)) = Nz(zz(t)) - 1;
    pz = topics(:, words(t))' .* (Nz + topic_prior);
    zz(t) = discreternd(1, pz);
    Nz(zz(t)) = Nz(zz(t)) + 1;
end
zs = zz;
log_Tvals(ss) = log_Tprob(zstar, zz, Nz);
% Draw forward stuff
for sprime = (ss+1):ms_iters
    for t = 1:Nd
        Nz(zz(t)) = Nz(zz(t)) - 1;
        pz = topics(:, words(t))' .* (Nz + topic_prior);
        zz(t) = discreternd(1, pz);
        Nz(zz(t)) = Nz(zz(t)) + 1;
    end
    log_Tvals(sprime) = log_Tprob(zstar, zz, Nz);
end
% Draw backward stuff
for sprime = (ss-1):-1:1
    for t = Nd:-1:1
        Nz(zz(t)) = Nz(zz(t)) - 1;
        pz = topics(:, words(t))' .* (Nz + topic_prior);
        zz(t) = discreternd(1, pz);
        Nz(zz(t)) = Nz(zz(t)) + 1;
    end
    log_Tvals(sprime) = log_Tprob(zstar, zz, Nz);
end
% Final estimate
Nkstar = histc(zstar, 1:T); Nkstar = Nkstar(:)'; % 1xT
log_pz = sum(gammaln(Nkstar + topic_prior)) + gammaln(topic_alpha) ...
        - sum(gammaln(topic_prior)) - gammaln(Nd + topic_alpha);
log_w_given_z = 0;
for t = 1:Nd
    log_w_given_z = log_w_given_z + log(topics(zstar(t), words(t)));
end
log_joint = log_pz + log_w_given_z;
log_evidence = log_joint - (logsumexp(log_Tvals) - log(ms_iters));

function lp = log_Tprob_base(zto, zfrom, Nz, words, topics, topic_prior)
Nd = length(words);
lp = 0;
for t = 1:Nd
    Nz(zfrom(t)) = Nz(zfrom(t)) - 1;
    pz = topics(:, words(t))' .* (Nz + topic_prior);
    pz = pz/sum(pz);
    lp = lp + log(pz(zto(t)));
    Nz(zto(t)) = Nz(zto(t)) + 1;
end


function imgLogLuv = float2LogLuv(img)
%
%       imgLogLuv=float2LogLuv(img)
%
%
%        Input:
%           -img: a HDR image in RGB
%
%        Output:
%           -imgLogLuv: the HDR image in the 32-bit LogLuv format
%
%     Copyright (C) 2011  Francesco Banterle
%
%     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.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

%is it a three color channels image?
check3Color(img);

%Conversion from RGB to XYZ
imgXYZ = ConvertRGBtoXYZ(img, 0);

imgLogLuv = zeros(size(img));

%Encoding luminance Y
Le = floor(256*(log2(imgXYZ(:,:,2))+64));
imgLogLuv(:,:,1) = ClampImg(Le,0,65535);

%CIE (u,v) chromaticity values
norm = (imgXYZ(:,:,1)+imgXYZ(:,:,2)+imgXYZ(:,:,3));
x = imgXYZ(:,:,1)./ norm;
y = imgXYZ(:,:,2)./ norm;

%Encoding chromaticity
norm_uv = (-2*x+12*y+3);
u_prime = 4*x./norm_uv;
v_prime = 9*y./norm_uv;

Ue = floor(410*u_prime);
imgLogLuv(:,:,2) = ClampImg(Ue,0,255);

Ve = floor(410*v_prime);
imgLogLuv(:,:,3) = ClampImg(Ve,0,255);

end


%%***************************************************
%% min sum_k bk*yk
%% s.t. sum yk*Hk  <= 0
%%      y1 = 1
%% Hk = -hankel(ek) if   1 <= k <= n
%%    = -hankel(0,e(k-n+1)) if n+1 <= k <=2*n-1
%%
%% [blk,At,C,b] = sdphankel(n);
%%***************************************************

   function [blk,At,C,b] = sdphankel(n);

   randn('seed',0);
   tmp = randn(n,n);
   tmp = tmp+tmp';
   X{1} = tmp + norm(tmp,'fro')*speye(n,n);
%%
   for k = 1:n
      ek = zeros(n,1); ek(k) = -1;
      AA{k} = sparse(hankel(ek));
   end
   zz = zeros(n,1);
   for k = n+1:2*n-1
      ek = zeros(n,1); ek(k-n+1) = -1;
      AA{k} = sparse(hankel(zz,ek));
   end
   blk{1,1} = 's'; blk{1,2} = n;
   At = svec(blk,AA,1);
   C{1} = spconvert([n n 0]);
   b = AXfun(blk,At,[],X);
%%
   blk{2,1} = 'u'; blk{2,2} = 1;
   ee = zeros(1,2*n-1); ee(1) = 1;
   At{2,1} = ee;
   C{2,1} = 1;
%%***************************************************


% Compute the per-pixel perceptual energy over each window

function F = WinAvgPE(img, R)

    ff = fft2(img);
    ffc = fftshift(ff);

    [m, n]=size(img);
    [u,v]=freqspace([m,n],'meshgrid');
%     u = u/2*m; v = v/2*n;
%     u = u/m*R; v = v/n*R;
    u = u/2*R; v = v/2*R;
    r=sqrt(u.^2+v.^2);
    fw = MannosSkarision(r);

    affc = abs(ffc(:));
    energ = sqrt(sum(affc.*affc));
    F = abs(ffc.*fw);

    F = sqrt(sum(F(:).*F(:)))/(m*n);
end

%%
function fc = MannosSkarision(r)
    fc = 2.6*(0.0192+0.114*r).*exp(-(0.114*r).^1.1);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Example code for estimating the HRF using the Inverse-Logit Model, a
% Finte Impluse Response Model and the Canonical HRF with 2 derivatives.
% Also the code illustrates our code for detecting model misspecification.
%
% This example illustrates the multi-stimulus version
%
% By Martin Lindquist
% Created: 05/26/10
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Load time course
%

mypath = which('ilogit');
if isempty(mypath), error('Cannot find directory with ilogit.m and other functions. Not on path?'); end
[mydir] = fileparts(mypath)

h = spm_hrf(1);
h = h./max(h);

RunA = zeros(60,10);
RunA(1,:) = 1;
RunA = reshape(RunA,600,1);
RunB = zeros(60,10);
RunB(31,:) = 1;
RunB = reshape(RunB,600,1);

tc = 2*conv(RunA,h) + conv(RunB,h);
tc = tc(1:600);
tc = tc+normrnd(0,1,600,1);

Run =[];
Run{1} = RunA;
Run{2} = RunB;

len = length(tc);

figure; subplot(3,1,1); han = plot(tc);
title('Sample time course'); drawnow

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Settings
%

TR = 1;
T = round(30/TR);
t = 1:T;                        % samples at which to get Logit HRF Estimate
FWHM = 4;                       % FWHM for residual scan
pval = 0.01;
df = 600;
alpha = 0.001;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Create stick function (sample event onsets)
% Variable R contains onset times
% Variable Run contains stick (a.k.a. delta or indicator) function

RA = [1 61 121 181 241 301 361 421 481 541];
RB = [31 91 151 211 271 331 391 451 511 571];

try
    hold on;
    hh = plot_onsets(RA,'k',-3,1);
    hh = plot_onsets(RB,'r',-3,0.5);
    drawnow
catch
    disp('Couldn''t find function to add onset sticks to plot. Skipping.')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using IL-function

% Choose mode (deterministic/stochastic)

mode = 0;   % 0 - deterministic aproach
            % 1 - simulated annealing approach
            % Please note that when using simulated annealing approach you
            % may need to perform some tuning before use.

[h1, fit1, e1, param] = Fit_Logit(tc,Run,t,mode);
[pv sres sres_ns1] = ResidScan(e1, FWHM);
[PowLoss1] = PowerLoss(e1, fit1, (len-7) , tc, TR, Run, alpha);

hold on; han(2) = plot(fit1,'r');

disp('Summary: IL_function');

disp('HRF - Event A');
disp('Amplitude:'); disp(param(1,1));
disp('Time-to-peak:'); disp(param(2,1));
disp('Width:'); disp(param(3,1));

disp('HRF - Event B');
disp('Amplitude:'); disp(param(1,2));
disp('Time-to-peak:'); disp(param(2,2));
disp('Width:'); disp(param(3,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using FIR-model

% Choose mode (FIR/sFIR)

mode = 1;   % 0 - FIR
            % 1 - smooth FIR

[h2, fit2, e2, param] = Fit_sFIR(tc,TR,Run,T,mode);
[pv sres sres_ns2] = ResidScan(e2, FWHM);
[PowLoss2] = PowerLoss(e2, fit2, (len-T) , tc, TR, Run, alpha);

hold on; han(3) = plot(fit2,'g');

disp('Summary: FIR');

disp('HRF - Event A');
disp('Amplitude:'); disp(param(1,1));
disp('Time-to-peak:'); disp(param(2,1));
disp('Width:'); disp(param(3,1));

disp('HRF - Event B');
disp('Amplitude:'); disp(param(1,2));
disp('Time-to-peak:'); disp(param(2,2));
disp('Width:'); disp(param(3,2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using Canonical HRF + 2 derivatives

p=1;

[h3, fit3, e3, param, info] = Fit_Canonical_HRF(tc,TR,Run,T,p);
[pv sres sres_ns3] = ResidScan(e3, FWHM);
[PowLoss3] = PowerLoss(e3, fit3, (len-p) , tc, TR, Run, alpha);

hold on; han(4) = plot(fit3,'m');

legend(han,{'Data' 'IL' 'sFIR' 'DD'})

disp('Summary: Canonical + 2 derivatives');

disp('HRF - Event A');
disp('Amplitude:'); disp(param(1,1));
disp('Time-to-peak:'); disp(param(2,1));
disp('Width:'); disp(param(3,1));

disp('HRF - Event B');
disp('Amplitude:'); disp(param(1,2));
disp('Time-to-peak:'); disp(param(2,2));
disp('Width:'); disp(param(3,2));

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

%figure;

subplot(3,2,5);
han2 = plot(h1(:,1),'r');
hold on; han2(2) = plot(h2(:,1),'g');
hold on; han2(3) = plot(h3(:,1),'m');
legend(han2,{'IL' 'sFIR' 'DD'})
title('Estimated HRF - Event A');

subplot(3,1,2); hold on;
hh = plot_onsets(RA,'k',-3,1);
hh = plot_onsets(RB,'r',-3,0.5);
drawnow

han3 = plot(sres_ns1,'r');
hold on; han3(2) = plot(sres_ns2,'g');
hold on; han3(3) = plot(sres_ns3,'m');
hold on; plot((1:len),zeros(len,1),'--k');
legend(han3,{'IL' 'sFIR' 'DD'})
title('Mis-modeling (time course)');

subplot(3,2,6); hold on;

han4 = plot(h2(:,1),'r');
hold on; han4(2) = plot(h2(:,2),'g');
hold on; han4(3) = plot(h3(:,2),'m');
legend(han4,{'IL' 'sFIR' 'DD'})
title('Estimated HRF - Event A');


function [ n_data, n, x, fx ] = t_polynomial_values ( n_data )

%*****************************************************************************80
%
%% T_POLYNOMIAL_VALUES returns values of the Chebyshev polynomial T(n,x).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%  Parameters:
%
%    Input, integer N_DATA, indicates the index of the previous test data
%    returned, or is 0 if this is the first call.  For repeated calls,
%    set the input value of N_DATA to the output value of N_DATA
%    from the previous call.
%
%    Output, integer N_DATA, the index of the test data.
%
%    Output, integer N, the order of the function.
%
%    Output, real X, the point where the function is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 13;
  fx_vec = [ ...
     1.0000000000E+00,  0.8000000000E+00,  0.2800000000E+00, ...
    -0.3520000000E+00, -0.8432000000E+00, -0.9971200000E+00, ...
    -0.7521920000E+00, -0.2063872000E+00,  0.4219724800E+00, ...
     0.8815431680E+00,  0.9884965888E+00,  0.7000513741E+00, ...
     0.1315856097E+00 ];
  n_vec = [ ...
     0,  1,  2, ...
     3,  4,  5, ...
     6,  7,  8, ...
     9, 10, 11, ...
    12 ];
  x_vec = [ ...
    0.8E+00,  0.8E+00,  0.8E+00, ...
    0.8E+00,  0.8E+00,  0.8E+00, ...
    0.8E+00,  0.8E+00,  0.8E+00, ...
    0.8E+00,  0.8E+00,  0.8E+00, ...
    0.8E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    x = 0.0E+00;
    fx = 0.0E+00;
  else
    n = n_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function [textures] = getNGTDMtextures(NGTDM,countValid)
% -------------------------------------------------------------------------
% function [textures] = getNGTDMtextures(NGTDM,countValid)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes texture features from an input Neighborhood
% Gray-Tone Difference Matrix (NGTDM).
% -------------------------------------------------------------------------
% REFERENCE:
% [1] Amadasun, M., & King, R. (1989). Textural Features Corresponding to
%     Textural Properties. IEEE Transactions on Systems Man and Cybernetics,
%     19(5), 1264–1274.
% -------------------------------------------------------------------------
% INPUTS:
% - NGTDM: Neighborhood Gray-Tone Difference Matrix.
% - countValid: Number of valid voxels used in the NGTDM computation.
%               Required for the computation of texture features in
%               'getNGTDMtextures.m'
%
% ** 'NGTDM' and 'countValid' should be outputs from 'getNGTDM.m' **
% -------------------------------------------------------------------------
% OUTPUTS:
% - textures: Struture specifying the values of different NGTDM texture
%             features as defined below.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: January 2013
% - Revision: May 2015
% -------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package 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 package is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

% PRELIMINARY
nTot = sum(countValid);
countValid = countValid./nTot; % Now representing the probability of gray-level occurences
NL = length(NGTDM);
Ng = sum(countValid~=0);
pValid = find(countValid>0);
nValid = length(pValid);

% COMPUTATION OF TEXTURES
% 1. Coarseness, Ref.[1]
textures.Coarseness = (((countValid')*NGTDM) + eps)^(-1);

% 2. Contrast, Ref.[1]
val = 0;
for i = 1:NL
    for j = 1:NL
        val = val + countValid(i)*countValid(j)*(i-j)^2;
    end
end
textures.Contrast = val*sum(NGTDM)/(Ng*(Ng-1)*nTot);

% 3. Busyness, Ref.[1]
denom = 0;
for i = 1:nValid
    for j = 1:nValid
        denom = denom + abs(pValid(i)*countValid(pValid(i))-pValid(j)*countValid(pValid(j)));
    end
end
textures.Busyness = ((countValid')*NGTDM)/denom;

% 4. Complexity, Ref.[1]
val = 0;
for i = 1:nValid
    for j = 1:nValid
        val = val + (abs(pValid(i)-pValid(j))/(nTot*(countValid(pValid(i)) + countValid(pValid(j)))))*(countValid(pValid(i))*NGTDM(pValid(i)) + countValid(pValid(j))*NGTDM(pValid(j)));
    end
end
textures.Complexity = val;

% 5. Strength, Ref.[1]
val = 0;
for i = 1:nValid
    for j = 1:nValid
        val = val + (countValid(pValid(i))+countValid(pValid(j)))*(pValid(i)-pValid(j))^2;
    end
end
textures.Strength = val/(eps+sum(NGTDM));

end