% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function pathS = MC_VGGOU(S0,r,d,T,C,G,M,lambda,a,b,NTime,NSim,NBatches)
    intNt = 20;                 % steps in between orginial grid points
    allsteps = intNt * NTime;   % grid containing all necsseary steps
    dT = T / NTime;             % time step
    time = 0 : dT : T;          % tiem for martingale correction

    pathS = zeros(NSim,NTime+1,NBatches); % output
    lnS = zeros(NSim,NTime+1);      % used in batch
    lnS(:,1) = log(S0*exp(-d*T));   % set S(0) dividend adjusted

    %precompute the constants used for martingale correction
    y0 = 1;
    psiVG = (-1i)*C*log(G*M/(G*M+(M-G)-1));     % char exp                                     %characteristic exponent
    phiGOU = 1i*psiVG*y0/lambda*(1-exp(-lambda*time)) + ...
         lambda*a./(1i*psiVG-lambda*b).* ...
         (b*log(b./(b-1i*psiVG/lambda* ...
         (1-exp(-lambda*time))))-1i*psiVG*time); % char func
    omegaT = -phiGOU;           % martingale correction                                                         %martingale correction value
    omegaT(1) = 0;              % martingale correction in 0

for l = 1:NBatches              % batch loop
    yy = ones(NSim,allsteps+1); % init stochastic clock
    Np = poissrnd(a*lambda/allsteps*T,[NSim,allsteps]);

    for k = 1 : NSim
        for j = 1 : allsteps     % generating OU process
            if Np(k,j) > 0
                Ex = -log(rand(Np(k,j),1))/b; % exponential law
                U = exp(-lambda * T / allsteps * rand(Np(k,j),1));          % Uniforms
                yy(k,j+1) = (1-lambda*T/allsteps)*yy(k,j) ...
                    + sum(Ex .* U);
         else
                yy(k,j+1) = (1-lambda*T/allsteps)*yy(k,j);
            end
        end
    end

    ZZ = T*cumsum(yy,2)/allsteps;       %Integrated Time
    Y = zeros(NSim,NTime+1);
    for m = 2:NTime+1
        Y(:,m) = ZZ(:,(m-1)*intNt);
    end

    Intensity = C * (Y(:,2:end)-Y(:,1:end-1));      % intensity
    DGam = gamrnd(Intensity,1/M) - gamrnd(Intensity,1/G);
    diffomegaT = omegaT(2:end) - omegaT(1:end-1);   % maringale corr

    for m=2:NTime+1             % time loop
        lnS(:,m) = lnS(:,m-1) + (r-d)*dT ...
            + diffomegaT(m-1) + DGam(:,m-1);
    end
    pathS(:,:,l) = exp(lnS);
end


function value = he_double_product_integral ( i, j )

%*****************************************************************************80
%
%% HE_DOUBLE_PRODUCT_INTEGRAL: integral of He(i,x)*He(j,x)*e^(-x^2/2).
%
%  Discussion:
%
%    VALUE = integral ( -oo < x < +oo ) He(i,x)*He(j,x) exp(-x^2/2) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, J, the polynomial indices.
%
%    Output, real VALUE, the value of the integral.
%
  if ( i ~= j )
    value = 0.0;
  else
    value = r8_factorial ( i );
  end

  return
end


clear all; close all; clc;

% pull=[3 5 7 9 11];
% start=1:20;
% q=abs(start-pull(1));
% [x,n2]=min(q);
%
% for j=1:4
%    q=abs(start-pull(j));
%    [x,n2]=min(q)
%    start=start([1:n2-1 n2+1:(20-j)]);
%    start'
% end
%
% break

%%
L=10; x3=-L:0.1:L; n=length(x3)-1; % define domain
x2=x3(1:n); k=(2*pi/(2*L))*[0:n/2-1 -n/2:-1]; % k-vector
ye=exp(-(x2.^2)); ye2=exp((x2.^2)/2); % define Gaussians
for j=0:9      % loop through 10 modes
  yd=real(ifft(((i*k).^j).*fft(ye))); % 2nd derivative
  mode=((-1)^(j))*(((2^j)*factorial(j)*sqrt(pi))^(-0.5))*ye2.*yd;
  y(:,j+1)=(mode).';  % store modes as columns
end

x=x2(n/2+1-40:n/2+1+40);  % keep only -4<x<4
yharm=y(n/2+1-40:n/2+1+40,:);

n=length(x);
f=(exp(-(x-0.5).^2)+3*exp(-2*(x+1.5).^2))';

%%  Willcox alg.

% sensor 1
for jloop=1:81

s=zeros(n,1);
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,1), pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f1=yharm*atild;
Er(1)=norm(f1-f);

%con(jloop)=cond(M2);
con(jloop)=2*sum(diag(M2))-sum(M2(:));

end

%[s1,n1]=min(con)
[s1,n1]=max(con)

% sensor 2
jlook=[1:n1-1 n1+1:81];
for jloop=jlook

s=zeros(n,1);
s(n1)=1;
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,2), pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f2=yharm*atild;
Er(2)=norm(f2-f);

%con2(jloop)=cond(M2);
con2(jloop)=2*sum(diag(M2))-sum(M2(:));

end
%[s2,n2]=min(con2(jlook))
[s2,n2]=max(con2(jlook))

% sensor 3
if n2>n1
   jlook=[1:n1-1 n1+1:n2-1 n2+1:81];
else
   jlook=[1:n2-1 n2+1:n1-1 n1+1:81];
end

jlook
break
for jloop=jlook

s=zeros(n,1);
s(n1)=1; s(n2)=1;
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,3), pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f3=yharm*atild;
Er(3)=norm(f3-f);

%con3(jloop)=cond(M2);
con3(jloop)=2*sum(diag(M2))-sum(M2(:));

end
%[s3,n3]=min(con3(jlook))
[s3,n3]=max(con3(jlook))

% sensor 4
jlook=[1:n1-1 n1+1:n2-1 n2+1:n3-1 n3+1:81];
for jloop=jlook

s=zeros(n,1);
s(n1)=1; s(n2)=1;
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,4),pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f4=yharm*atild;
Er(4)=norm(f4-f);

%con4(jloop)=cond(M2);
con4(jloop)=2*sum(diag(M2))-sum(M2(:));

end
%[s4,n4]=min(con4(jlook))
[s4,n4]=max(con4(jlook))

figure(1)
subplot(4,1,1), bar((con)), axis([1 81 0 1])
subplot(4,1,2), bar((con2)), axis([1 81 0 1])
subplot(4,1,3), bar((con3)), axis([1 81 0 1])
subplot(4,1,4), bar((con4)), axis([1 81 0 1])

break

%%  Willcox:  condition number

% pull=[3 5 7 9 11];
% start=1:20;
% q=abs(start-pull(1));
% [x,n2]=min(q);
%
% for j=1:4
%    q=abs(start-pull(j));
%    [x,n2]=min(q)
%    start=start([1:n2-1 n2+1:(20-j)]);
%    start'
% end
%
% break
clear f1
clear f2
clear ns
ns=[]; jlook=1:81;

count=1;
for jsense=1:20

for jloop=1:(82-jsense)
s=zeros(n,1); s(ns)=1;
s(jlook(jloop))=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
con(jloop)=cond(M2);

end

length(con)
[s1,n1]=min(con)
kond(jsense)=s1;
clear con
ns=[ns n1];

jlook=jlook([1:n1-1 n1+1:(81-count+1)]);
count=count+1;

% reconstruct
s=zeros(n,1);
s(ns)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f1(:,jsense)=yharm*atild;
Errr(jsense)=norm(f1(:,jsense)-f);
scum(:,jsense)=s;

end

figure(4)
subplot(2,1,2), bar(log(Errr+1))
subplot(2,1,1), bar(log(kond))

subplot(2,1,2), set(gca,'Xlim',[0 21],'Xtick',[1 5 10 15 20],'Xticklabel',{'','','','',''},'Ylim',[0 12],'Ytick',[0 4 8 12],'Yticklabel',{'','','',''})
subplot(2,1,1), set(gca,'Xlim',[0 21],'Xtick',[1 5 10 15 20],'Xticklabel',{'','','','',''},'Ylim',[0 42],'Ytick',[0 20 40],'Yticklabel',{'','',''})

%figure(6), bar(s)

figure(7),
titer=[1:20 25]; f1=[f1 f]; titer2=[9:20 25], f2=[f1(:,9:20) f];
subplot(2,2,1), waterfall(x,titer,f1.'), colormap([0 0 0]), view(-150,50)
subplot(2,2,2), waterfall(x,titer2,f2.'), colormap([0 0 0]), view(-150,50)

subplot(2,2,1), set(gca,'Zlim',[-2 8],'Xlim',[-4 4],'Xtick',[-4 0 4],'Xticklabel',{'','',''}, ...
    'Ylim',[0 25],'Ytick',[1 10 20],'Yticklabel',{'','',''},'Ztick',[-2 0 4 8],'Zticklabel',{'','','',''})
subplot(2,2,2), set(gca,'Zlim',[-.2 3],'Xlim',[-4 4],'Xtick',[-4 0 4],'Xticklabel',{'','',''}, ...
    'Ylim',[0 25],'Ytick',[1 10 20],'Yticklabel',{'','',''},'Ztick',[0 1 2 3],'Zticklabel',{'','','',''})

figure(8)
%bar3(scum)
axes('position',[.05 .05 .9 .35])
scum2=[scum(:,[1:11 13:19])];
pcolor(-scum2.'), colormap(hot), axis off

%bar3(scum2)

%caxis([0 1]), axis off

%%  Willcox:  diagonal sum

% pull=[3 5 7 9 11];
% start=1:20;
% q=abs(start-pull(1));
% [x,n2]=min(q);
%
% for j=1:4
%    q=abs(start-pull(j));
%    [x,n2]=min(q)
%    start=start([1:n2-1 n2+1:(20-j)]);
%    start'
% end
%
% break
clear f1
clear f2
clear ns
clear s
ns=[]; jlook=1:81;

count=1;
for jsense=1:20

for jloop=1:(82-jsense)
s=zeros(n,1); s(ns)=1;
s(jlook(jloop))=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%con(jloop)=cond(M2);
con(jloop)=2*sum(diag(M2))-sum(M2(:));

end

%[s1,n1]=min(con)
[s1,n1]=max(con)
kond(jsense)=s1;
clear con
ns=[ns n1];

jlook=jlook([1:n1-1 n1+1:(81-count+1)]);
count=count+1;

% reconstruct
s=zeros(n,1);
s(ns)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f1(:,jsense)=yharm*atild;
Errr(jsense)=norm(f1(:,jsense)-f);
scum(:,jsense)=s;

end

figure(4)
subplot(2,1,2), bar(Errr)
subplot(2,1,1), bar((kond))

subplot(2,1,2), set(gca,'Xlim',[0 21],'Xtick',[1 5 10 15 20],'Xticklabel',{'','','','',''},'Ylim',[0 12],'Ytick',[0 4 8 12],'Yticklabel',{'','','',''})
subplot(2,1,1), set(gca,'Xlim',[0 21],'Xtick',[1 5 10 15 20],'Xticklabel',{'','','','',''},'Ylim',[0 42],'Ytick',[0 20 40],'Yticklabel',{'','',''})

%figure(6), bar(s)

figure(7),
titer=[1:20 25]; f1=[f1 f]; titer2=[9:20 25], f2=[f1(:,9:20) f];
subplot(2,2,1), waterfall(x,titer,f1.'), colormap([0 0 0]), view(-150,50)
subplot(2,2,2), waterfall(x,titer2,f2.'), colormap([0 0 0]), view(-150,50)

subplot(2,2,1), set(gca,'Zlim',[-2 8],'Xlim',[-4 4],'Xtick',[-4 0 4],'Xticklabel',{'','',''}, ...
    'Ylim',[0 25],'Ytick',[1 10 20],'Yticklabel',{'','',''},'Ztick',[-2 0 4 8],'Zticklabel',{'','','',''})
subplot(2,2,2), set(gca,'Zlim',[-.2 3],'Xlim',[-4 4],'Xtick',[-4 0 4],'Xticklabel',{'','',''}, ...
    'Ylim',[0 25],'Ytick',[1 10 20],'Yticklabel',{'','',''},'Ztick',[0 1 2 3],'Zticklabel',{'','','',''})

figure(8)
%bar3(scum)
axes('position',[.05 .05 .9 .35])
scum2=[scum(:,[1:11 13:19])];
pcolor(-scum2.'), colormap(hot), axis off

%bar3(scum2)

%caxis([0 1]), axis off

%% Test Random trials with P% of measurements
% spectrum of mu
break
per=[20 40 60 81];
for thresh=1:4

n2=randsample(n,per(thresh));
s=zeros(n,1); s(n2)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end

[v,d]=eigs(M2);
subplot(2,2,thresh)
plot(real(diag(d)),imag(diag(d)),'ko','Linewidth',[2])
axis([0 2 -1 1])
end


function linpack_d_test31 ( )

%*****************************************************************************80
%
%% TEST31 tests DTRSL.
%
%  Discussion:
%
%    DTRSL solves triangular linear systems.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  lda = n;

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST31\n' );
  fprintf ( 1, '  For a triangular matrix,\n' );
  fprintf ( 1, '  DTRSL solves a linear system.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Lower triangular matrix A.
%
  [ a, seed ] = r8mat_uniform_01 ( n, n, seed );

  for i = 1 : n
    for j = i+1 : n
      a(i,j) = 0.0;
    end
  end

  for i = 1 : n
    x(i,1) = i;
  end

  b(1:n,1) = a(1:n,1:n) * x(1:n,1);

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For a lower triangular matrix A,\n' );
  fprintf ( 1, '  solve A * x = b\n' );

  job = 00;

  [ b, info ] = dtrsl ( a, lda, n, b, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The solution (should be 1,2,3,4,5):\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %6d  %14f\n', i, b(i,1) );
  end

  b(1:n,1) = a(1:n,1:n)' * x(1:n,1);

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For a lower triangular matrix A,\n' );
  fprintf ( 1, '  solve A'' * x = b\n' );

  job = 10;

  [ b, info ] = dtrsl ( a, lda, n, b, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The solution (should be 1,2,3,4,5):\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %6d  %14f\n', i, b(i,1) );
  end
%
%  Upper triangular matrix A.
%
  [ a, seed ] = r8mat_uniform_01 ( n, n, seed );

  for i = 1 : n
    for j = 1 : i - 1
      a(i,j) = 0.0;
    end
  end

  for i = 1 : n
    x(i,1) = i;
  end

  b(1:n,1) = a(1:n,1:n) * x(1:n,1);

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For an upper triangular matrix A,\n' );
  fprintf ( 1, '  solve A * x = b\n' );

  job = 01;

  [ b, info ] = dtrsl ( a, lda, n, b, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The solution (should be 1,2,3,4,5):\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %6d  %14f\n', i, b(i,1) );
  end

  b(1:n,1) = a(1:n,1:n)' * x(1:n,1);

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For an upper triangular matrix A,\n' );
  fprintf ( 1, '  solve A'' * x = b\n' );

  job = 11;

  [ b, info ] = dtrsl ( a, lda, n, b, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The solution (should be 1,2,3,4,5):\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %6d  %14f\n', i, b(i,1) );
  end

  return
end


function [ddCs, rhsCs] = electrodeConcentration(dCs,cs_barrato,T,jflux,param)
% electrodeConcentration describes the ODE of the concentration of lithium ions within the
% electrodes.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

if(param.SolidPhaseDiffusion==1 || param.SolidPhaseDiffusion==2)
    % Cathode
    rhsCs_p  =((-3/param.Rp_p)*jflux(1:param.Np));
    ddCs_p   = dCs(1:param.Np) - rhsCs_p;

    % Anode
    rhsCs_n  = ((-3/param.Rp_n)*jflux(param.Np+1:end));
    ddCs_n   = dCs(param.Np+1:end) - rhsCs_n;
else
    switch param.SolidPhaseDiffusionNumericalScheme
        % Use the FDM method for the solid phase diffusion
        case 1
            [rhsCs_p, rhsCs_n, ddCs_p, ddCs_n] = FDM9orderElectrodeDiffusion(T, cs_barrato, jflux, dCs, param);
        % Use the spectral method for the discretization of the solid phase
        % diffusion
        case 2
            [rhsCs_p, rhsCs_n, ddCs_p, ddCs_n] = spectralMethodElectrodeDiffusion(T, cs_barrato, jflux, dCs, param);
    end
end
rhsCs   = [rhsCs_p;rhsCs_n];
ddCs    = [ddCs_p;ddCs_n];
end


function x=stdtinv(p,v)
% Inverse Cumulative Distribution Function (CDF) of the Standardized T
% distribution; Maps [0,1] to a standardized Students-t with V degrees of freedom
%
% USAGE:
%   X = stdtinv(P,V)
%
% INPUTS:
%   P     - Values to be inverted, P in [0,1]
%   V     - Degree of freedom parameters, either scalar or size(X)
%
% OUTPUTS:
%   X     - Standardized T distributed random variables corresponding to P
%
% COMMENTS:
%   V>2
%
% REFERENCES:
%   [1] Cassella and Berger (1990) 'Statistical Interence'
%
% See also STDTCDF, STDTINV, STDTRND, STDTLOGLIK, TPDF

% Copyright:
% Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 9/1/2005

if nargin~=2
    error('2 inputs required')
end

[err, errtext, sizeOut, v] = iscompatible(1,v,size(p));
if err
    error(errtext)
end

x=tinv(p,v);

stdev=sqrt(v./(v-2));
stdev(v<=2)=NaN;
x=x./stdev;


% The null space of a 3DOF robot when considered redundant.
% if the task m=(vx, vy), then the robot is redundant in lambda=3
%
% Copyright (C) 2016, 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 null_space_3dofplanar
% link lengths
robot = load_robot('example', '3dofplanar');
q = [pi/2 pi/2 pi/2]';

qs = [];
qds = [];
manips = [];
for i=1:300
    [qb, manip] = null_movement(robot, q);
    q = q + 0.1*qb;
    qs = [qs q];
    qds = [qds qb];
    manips = [manips manip];
end

animate(robot, qs(:,1:15:end))
figure, plot(manips), legend('manipulability')
figure, plot(qs'), legend('q_1', 'q_2', 'q_3')
figure, plot(qds'), legend('qd_1', 'qd_2', 'qd_3')
%q = q + 0.01*ns';

function [ns, manip] = null_movement(robot, q)
J = manipulator_jacobian(robot, q);
% consider only vx, vy
J = J(1:2, :);
iJm = moore_penrose(J);
P = (eye(3)-iJm*J)
% project to null space
ns = P*[1 0 0]';
manip = det(J*J')

function iJm = moore_penrose(J)
iJm = J'*inv(J*J');


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

% relationship of linear convolution with circular convolution

% 4-point circular convolution
x1=[1,2,3,4];
x2=[3,2,5,1];
N=length(x1);
for m=0:N-1
p(m+1)=mod(-m,N);
x2s(1+m)=x2(1+p(m+1));
end
x2s
for n=0:N-1
    x2sn=circshift(x2s',n);
    y2(n+1)=x1*x2sn;
end
y2

% 7-point circular convolution
x11=[ x1 0 0 0];
x22=[x2 0 0 0];
N=length(x11);
for m=0:N-1
p(m+1)=mod(-m,N);
end
for m=0:N-1
x22s(1+m)=x22(1+p(m+1));
end
for n=0:N-1
    x22sn=circshift(x22s',n);
    y22(n+1)=x11*x22sn;
end
y22

%linear convolution
y1=conv(x1,x2)


% GP_PREDICT_PSEUDO - Computes the predictive distribution of a Gaussian
%                     process using pseudo inputs.
%
% Usage:
%
%   F_MEAN = GP_PREDICT_PSEUDO(Y, V, K_FP, K_PP, K_PH, K_H)
%   [F_MEAN, F_VAR] = GP_PREDICT_PSEUDO(...)
%
% The marginal of a conditional model
%
%   Y|F ~ N(F, V)
%
% is approximated by
%
%   Y ~ N(0, K_FP*INV(K_PP)*K_FP' + V)
%
% Y : Nx1 vector of observations
% V : NxN diagonal matrix of noise covariance, or a function V(X) which
%     solves V\X
% K_FP : NxM covariance matrix of function values at observation inputs
%        and at pseudo inputs
% K_PP : MxM covariance matrix of function values at pseudo inputs
% K_PH : MxK covariance matrix of function values at pseudo inputs and at
%        inputs to be predicted
% K_H : Kx1 vector of variances of function values at inputs to be
%       predicted
%
% See also GP_LEARN_PSEUDO, GP_PREDICT.

% Last modified 2011-01-27
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function [f_mean, f_var] = gp_predict_pseudo(y, V, K_pf, K_pp, K_ph, k_h)

if isnumeric(V)
  % Get a solver for V\z
  inv_V = get_linsolve_cov(V);
else
  % A function handle which solves V\z
  inv_V = V;
end

% TODO:
%
% - Take K_pf instead of K_fp
%
% - Take V as a vector?

[L_p,p] = chol(K_pp, 'lower');
if p~=0
  figure
  imagesc(K_pp);
  error('Matrix not positive definite');
end

Z_f = linsolve_tril(L_p, K_pf);

Lambda = speye(size(K_pp)) + Z_f*inv_V(Z_f');
inv_Lambda = get_linsolve_cov(Lambda);

f_mean = K_ph' * linsolve_triu(L_p, inv_Lambda(Z_f*inv_V(y)), true);
if nargout >= 2
  Z_h = linsolve_tril(L_p, K_ph);
  f_var = k_h(:) - dot(Z_h, Z_h - inv_Lambda(Z_h),1)';
end

% $$$ Lambda = K_pp + K_fp' * inv_V(K_fp);
% $$$ inv_Lambda = get_linsolve_cov(Lambda);
% $$$ inv_Kpp = get_linsolve_cov(K_pp);
% $$$ f_mean = K_ph' * inv_Lambda(K_fp' * inv_V(y));
% $$$ f_var = k_h(:) - dot(K_ph, inv_Kpp(K_ph) - inv_Lambda(K_ph),1)';


function K = spikernel( k, d1, d2, ind1, ind2, kerparam)

% K = spikernel( k, d1, d2, ind1, ind2, kerparam)
% computes the spikernel between two sequences of spiking activity
%
% kerparam{1} = N; max subsequence lengths (the kernel compares subsequences of lengths 1 to N and returns a sum of kernels)
% kerparam{2} = lam; \lambda parameter from article
% kerparam{3} = mu; \mu parameter from article
% kerparam{4} = p; the q parameter in the article - the parameter that is used in the sum of kernels to weigh them differently
% kerparam{5} = bins; the number of bins used;          we need this to reshape the data correctly
% kerparam{6} = nneurons; the number of neurons;    we need this to reshape the data correctly
%
%   This code uses the spikernel function, defined in:
%   Spikernels: Embedding Spiking Neurons in Inner-Product Spaces
%   Lavi Shpigelman, Yoram Singer, Rony Paz and Eilon Vaadia
%   Advances in Neural Information Processing Systems (NIPS) 15
%   MIT Press, Cambridge, MA, 2003.
%
% for further details see Fspikernel.m

bins = kerparam{5};
nneurons = kerparam{6};

xx = get_x( d1, ind1);
yy = get_x( d2, ind2);
K = zeros( size( yy,1), size( xx,1));

for i = 1:size( xx, 1)
    for j = 1:size( yy,1)
        K( j, i) = Fspikernel( reshape( yy( j, :), bins, nneurons), reshape( xx( i,:), bins, nneurons), kerparam{1}, kerparam{2}, kerparam{3}, kerparam{4});
%        K( j, i) = K( i, j);
%       if bins >= 100 disp(sprintf('we''re at: (%d, %d)', i,j)); end   %<-- if it takes longer, print out, where we are
    end
end


function lik = lik_multinom(varargin)
%LIK_MULTINOM    Create a multinom likelihood structure
%
%  Description
%    LIK = LIK_MULTINOM creates multinom likelihood for multi-class
%    count data. The observed numbers in each class with C classes is given
%    as 1xC vector.
%
%    The likelihood is defined as follows:
%                              __ n                __ C
%      p(y|f^1, ..., f^C, z) = || i=1 [ gamma(N+1) || c=1 p_i^c^(y_i^c)/gamma(y_i^c+1)]
%
%    where p_i^c = exp(f_i^c)/ (sum_c=1^C exp(f_i^c)) is the succes
%    probability for class c, which is a function of the latent variable
%    f_i^c for the corresponding class and N=sum(y) is the number of trials.
%
%  See also
%    GP_SET, LIK_*

% Copyright (c) 2010 Jaakko Riihim�ki, Pasi Jyl�nki
% Copyright (c) 2010 Aki Vehtari
% Copyright (c) 2010 Jarno Vanhatalo

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'LIK_MULTINOM';
  ip.addOptional('lik', [], @isstruct);
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.type = 'Multinom';
    lik.nondiagW = true;
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'Multinom')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_multinom_pak;
    lik.fh.unpak = @lik_multinom_unpak;
    lik.fh.ll = @lik_multinom_ll;
    lik.fh.llg = @lik_multinom_llg;
    lik.fh.llg2 = @lik_multinom_llg2;
    lik.fh.llg3 = @lik_multinom_llg3;
    lik.fh.predy = @lik_multinom_predy;
    lik.fh.invlink = @lik_multinom_invlink;
    lik.fh.recappend = @lik_multinom_recappend;
  end

end

function [w,s] = lik_multinom_pak(lik)
%LIK_MULTINOM_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_MULTINOM_PAK(LIK) takes a likelihood structure LIK and
%    returns an empty verctor W. If Multinom likelihood had
%    parameters this would combine them into a single row vector
%    W (see e.g. lik_negbin). This is a mandatory subfunction used
%    for example in energy and gradient computations.
%
%
%  See also
%    LIK_MULTINOM_UNPAK, GP_PAK

  w = []; s = {};
end

function [lik, w] = lik_multinom_unpak(lik, w)
%LIK_MULTINOM_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    W = LIK_MULTINOM_UNPAK(W, LIK) Doesn't do anything.
%
%    If Multinom likelihood had parameters this would extracts them
%    parameters from the vector W to the LIK structure. This is a
%    mandatory subfunction used for example in energy and gradient
%    computations.
%
%
%  See also
%    LIK_MULTINOM_PAK, GP_UNPAK

  lik=lik;
  w=w;
end

function ll = lik_multinom_ll(lik, y, f, z)
%LIK_MULTINOM_LL  Log likelihood
%
%  Description
%    LL = LIK_MULTINOM_LL(LIK, Y, F) takes a likelihood structure
%    LIK, class counts Y (NxC matrix), and latent values F (NxC
%    matrix). Returns the log likelihood, log p(y|f,z). This
%    subfunction is needed when using Laplace approximation or
%    MCMC for inference with non-Gaussian likelihoods. This
%    subfunction is also used in information criteria
%    (DIC, WAIC) computations.
%
%  See also
%    LIK_MULTINOM_LLG, LIK_MULTINOM_LLG3, LIK_MULTINOM_LLG2, GPLA_E

  f=reshape(f,size(y));
  expf = exp(f);
  p = expf ./ repmat(sum(expf,2),1,size(expf,2));
  N = sum(y,2);

  ll = sum(gammaln(N+1) - sum(gammaln(y+1),2) + sum(y.*log(p),2) );

end

function llg = lik_multinom_llg(lik, y, f, param, z)
%LIK_MULTINOM_LLG    Gradient of the log likelihood
%
%  Description
%    LLG = LIK_MULTINOM_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, class labels Y, and latent values F. Returns
%    the gradient of the log likelihood with respect to PARAM. At
%    the moment PARAM can be 'param' or 'latent'. This subfunction
%    is needed when using Laplace approximation or MCMC for inference
%    with non-Gaussian likelihoods.
%
%  See also
%    LIK_MULTINOM_LL, LIK_MULTINOM_LLG2, LIK_MULTINOM_LLG3, GPLA_E

  f=reshape(f,size(y));
  C = size(y,2);
  expf2 = exp(f);
  N=sum(y, 2);
  pi2 = (N*ones(1,C)).*expf2./(sum(expf2, 2)*ones(1,C));
  pi_vec=pi2(:);
  llg = y(:)-pi_vec;

end

function [pi_vec, pi_mat] = lik_multinom_llg2(lik, y, f, param, z)
%LIK_MULTINOM_LLG2  Second gradients of the log likelihood
%
%  Description
%    LLG2 = LIK_MULTINOM_LLG2(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, class labels Y, and latent values F. Returns
%    the Hessian of the log likelihood with respect to PARAM. At
%    the moment PARAM can be only 'latent'. LLG2 is a vector with
%    diagonal elements of the Hessian matrix (off diagonals are
%    zero). This subfunction is needed when using Laplace
%    approximation or EP for inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_MULTINOM_LL, LIK_MULTINOM_LLG, LIK_MULTINOM_LLG3, GPLA_E

% multinom:
  [n,nout]=size(y);
  N = sum(y,2)*ones(1,nout);
  f=reshape(f,n,nout);

  expf2 = exp(f);
  pi2 = expf2./(sum(expf2, 2)*ones(1,nout));
  pi_vec=pi2(:).*N(:);

  pi_mat=zeros(nout*n, n);
  for i1=1:nout
    pi_mat((1+(i1-1)*n):(nout*n+1):end)=pi2(:,i1).*sqrt(N(:,i1));
  end
  %     D = diag(pi_vec);
  %     llg2 = -D + pi_mat*pi_mat';

end

function [dw_mat] = lik_multinom_llg3(lik, y, f, param, z)
%LIK_MULTINOM_LLG3  Third gradients of the log likelihood
%
%  Description
%    LLG3 = LIK_MULTINOM_LLG3(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, class labels Y, and latent values F and
%    returns the third gradients of the log likelihood with
%    respect to PARAM. At the moment PARAM can be only 'latent'.
%    LLG3 is a vector with third gradients. This subfunction is
%    needed when using Laplace approximation for inference with
%    non-Gaussian likelihoods.
%
%  See also
%    LIK_MULTINOM_LL, LIK_MULTINOM_LLG, LIK_MULTINOM_LLG2, GPLA_E, GPLA_G

  [n,nout] = size(y);
  f2 = reshape(f,n,nout);

  N=sum(y, 2);
  expf2 = exp(f2);
  pi2 = expf2./(sum(expf2, 2)*ones(1,nout));
  pi_vec=pi2(:);

  dw_mat=zeros(nout,nout,nout,n);

  for cc3=1:nout
    for ii1=1:n

      pic=pi_vec(ii1:n:(nout*n));
      for cc1=1:nout
        for cc2=1:nout

          % multinom third derivatives
          cc_sum_tmp=0;
          if cc1==cc2 && cc1==cc3 && cc2==cc3
            cc_sum_tmp=cc_sum_tmp+pic(cc1);
          end
          if cc1==cc2
            cc_sum_tmp=cc_sum_tmp-pic(cc1)*pic(cc3);
          end
          if cc2==cc3
            cc_sum_tmp=cc_sum_tmp-pic(cc1)*pic(cc2);
          end
          if cc1==cc3
            cc_sum_tmp=cc_sum_tmp-pic(cc1)*pic(cc2);
          end
          cc_sum_tmp=cc_sum_tmp+2*pic(cc1)*pic(cc2)*pic(cc3);

          dw_mat(cc1,cc2,cc3,ii1)=cc_sum_tmp.*N(ii1);
        end
      end
    end
  end

end

function [lpy, Ey, Vary] = lik_multinom_predy(lik, Ef, Varf, yt, zt)
%LIK_MULTINOM_PREDY  Returns the predictive mean, variance and density of y
%
%  Description
%    LPY = LIK_MULTINOM_PREDY(LIK, EF, VARF YT)
%    Returns logarithm of the predictive density PY of YT, that is
%        p(yt | y) = \int p(yt | f) p(f|y) df.
%    This requires also the incedence counts YT. This subfunction
%    is needed when computing posterior predictive distributions for
%    future observations.
%
%    [LPY, EY, VARY] = LIK_MULTINOM_PREDY(LIK, EF, VARF, YT) takes a
%    likelihood structure LIK, posterior mean EF and posterior
%    Variance VARF of the latent variable and returns the
%    posterior predictive mean EY and variance VARY of the
%    observations related to the latent variables. This subfunction
%    is needed when computing posterior predictive distributions for
%    future observations.
%

%
%  See also
%    GPLA_PRED, GPEP_PRED, GPMC_PRED

  N=sum(yt,2);
  S=10000;
  [ntest, nout]=size(yt);
  pi=zeros(ntest,nout);
  lpy=zeros(ntest,nout);
  Ey=zeros(ntest,nout);
  Vary=zeros(size(Varf));
  Ef=reshape(Ef(:),ntest,nout);
  [notused,notused,c] =size(Varf);
  if c>1
    mcmc=false;
  else
    mcmc=true;
    Varf=reshape(Varf(:), ntest, nout);
  end
  for i1=1:ntest
    if mcmc
      Sigm_tmp = (Varf(i1,:));
      f_star=bsxfun(@plus, Ef(i1,:), bsxfun(@times, sqrt(Sigm_tmp), ...
        randn(S,nout)));
    else
      Sigm_tmp=(Varf(:,:,i1)'+Varf(:,:,i1))./2;
      f_star=mvnrnd(Ef(i1,:), Sigm_tmp, S);
    end

    tmp = exp(f_star);
    tmp = tmp./(sum(tmp, 2)*ones(1,size(tmp,2)));

    if nargout > 1
        Ey(i1,:) = N(i1).*mean(tmp);
        for z1 = 1:nout;
          for z2 = 1:nout
            for z3=1:S
              Var_tmp(:,:,z3) = (diag(tmp(z3,:)) - tmp(z3,:)'*tmp(z3,:));
            end
            if mcmc
              Vary(i1+(0:nout-1)*ntest,:) = diag(N(i1).*mean(Var_tmp,3));
            else
              Vary(:,:,i1) = N(i1).*mean(Var_tmp,3);
            end
          end
        end
    end
    lpy=[];
    if ~isempty(yt)
      ytmp = repmat(yt(i1,:),S,1);
      lpy(i1,:) = log(mean( mnpdf(ytmp,tmp) ));
    end
  end
  lpy=lpy(:);
  Ey=Ey(:);
end

function p = lik_multinom_invlink(lik, f, z)
%LIK_MULTINOM_INVLINK Returns values of inverse link function
%
%  Description
%    P = LIK_MULTINOM_INVLINK(LIK, F) takes a likelihood structure LIK and
%    latent values F and returns the values of inverse link function P.
%    This subfunction is needed when using function gp_predprctmu.
%
%     See also
%     LIK_MULTINOM_LL, LIK_MULTINOM_PREDY
p = multinominv(f).*z;
end

function reclik = lik_multinom_recappend(reclik, ri, lik)
%RECAPPEND  Append the parameters to the record
%
%  Description
%    RECLIK = LIK_MULTINOM_RECAPPEND(RECLIK, RI, LIK) takes a
%    likelihood record structure RECLIK, record index RI and
%    likelihood structure LIK with the current MCMC samples of
%    the parameters. Returns RECLIK which contains all the old
%    samples and the current samples from LIK. This subfunction
%    is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC

  if nargin == 2
    reclik.type = 'Multinom';
    reclik.nondiagW = true;

    % Set the function handles
    reclik.fh.pak = @lik_multinom_pak;
    reclik.fh.unpak = @lik_multinom_unpak;
    reclik.fh.ll = @lik_multinom_ll;
    reclik.fh.llg = @lik_multinom_llg;
    reclik.fh.llg2 = @lik_multinom_llg2;
    reclik.fh.llg3 = @lik_multinom_llg3;
    reclik.fh.predy = @lik_multinom_predy;
    reclik.fh.invlink = @lik_multinom_invlink;
    reclik.fh.recappend = @lik_multinom_recappend;
  end

end


% [x, Covx, Covx_x, entropy] = rts_smoother_step(x, Covx, x_s, Covx_s, A, Q)

% Last modified 2011-10-19
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@aalto.fi)

function [x, Covx, Covx_x, entropy] = rts_smoother_step(x, Covx, x_s, Covx_s, A, Q)
% Perform the RTS smoothing step

x_p = A*x;
Covx_p = A*Covx*A' + Q;

S = (Covx*A') / Covx_p;
x = x + S*(x_s-x_p);
if nargout >= 2
  Covx = Covx + S*(Covx_s-Covx_p)*S';
end
if nargout >= 3
  Covx_x = S*Covx_s;
end
if nargout >= 4
  % Compute entropy term:
  %
  % INT[ p(x_n, x_(n+1) | Y) log p(x_n | Y, x_(n+1)) dx_n dx_(n+1) ]
  Cov_joint = [Covx    Covx_x
               Covx_x' Covx_s];
  entropy = gaussian_entropy(logdet_cov(Cov_joint), size(Cov_joint,1)) ...
            - gaussian_entropy(logdet_cov(Covx_s), size(Covx_s,1));

end


function output = relaxMtFitFuncLs(x, m, W_B, W_F, T2_B, R1, S0, t_m, t_s, t_r);
% function output = relaxMtFitFuncLs(x, m, W_B, W_F, T2_B, R1, S0, t_m, t_s, t_r);
% inputs:
%       x = [k, f] where k is the cross-relaxation rate constant defined
%           for the transition from free pool (F) to bound pool (B), f is the
%           fraction of bound spins expressed in terms of concentrations as f =
%           [B]/([B]+[F])
%       m: MT(:,ii) ??
%
%       W_B: Effective saturation rate of the bound pool
%            W_B(ii) = pi*(w1rms^2)*lorentzian (delta(ii), T2_B);% eqtn (2)
%       W_F: 1/R1_F .* Effective saturation rate of the free pool
%            W_F = (w1rms./(2*pi*delta)).^2/.055; --> eqtn(3)./R1_F
%            w1rms = 2400; % omega-1 RMS --> ???? where does this number
%            come from???
%       T2_B: T2 relaxation time of Bound pool, units= seconds
%           T2_B = 11e-6; "average-brain" [YY (2004), pg 411, column2, paragraph 1]
%       R1: observed relaxation rate (measured in the independent
%           experiment) [YY(2004), pg 411, column 1, parag 2]
%           nz = T1>0;
%           R1 = zeros(size(T1));
%           R1(nz) = 1./T1(nz);
%       S0: Synthetic reference image computed by equation (6) using PD(protein Density) and
%           R1 maps.
%       t_m: duration of an off-resonance RF pulse
%           t_m = 8e-3; %bese 8e-3
%       t_s: delay time BEFORE an exitation RF pulse
%           t_s = 5e-3; %bese 5e-3
%       t_r: delay time AFTER an exitation RF pulse
%           t_r = 19e-3; %bese 19e-3
%   Output:
%       output: ??? m/S0 - M_z(:,1)./m_norm(1);
%
%    Example:
%       output=relaxMtFitFunc(x, MT(:,ii), W_B, W_F, T2_B, R1(ii),S0(ii), t_m, t_s, t_r)
%

% C : diagonal matrix = diag(cos(alpha),1) corresponding to instant
% rotation of the magnetization Mz_F by an excitation pulse with a flip
% angle alpha
% cos(10*pi/180) = 0.984807753012
C = [ 0.984807753012 0;
            0        1 ];

k = x(1); % the k-parameter is passed through x
f = x(2); % the f-parameter is passed through x

if(f<=0.01 || f>=0.5 || k<=0.1 || k>=5)
   output = Inf;
   return;
end

%T2_B = 11e-6; %bese 11e-6

R1_B = 1; %bese 1
% Compute this term just one to save some cycles
kf = k*(1-f)/f;
R1_F = R1 - k*(R1_B - R1)/(R1_B - R1 + kf); % eqtn (4)
%W_F = R1_F*(w1rms./(2*pi*delta)).^2/.055; %bese .055
W_F = R1_F*W_F;

R = [ (-R1_F - k)     (kf)    ;
           (k)    (-R1_B - kf) ];

A = R1_F*R1_B + R1_F*kf + R1_B*k;
E_s = expm(R*t_s); % relaxation during delays before (t_s) an exitation RF pulse
E_r = expm(R*t_r); % relaxation during delays after (t_r) an exitation RF pulse

I = eye(2); %identity matrix

for ii = 1:length(W_B);
  W = [-W_F(ii), 0;
       0,   -W_B(ii)];
  E_m = expm((R + W)*t_m);% off-resonance saturation by an RF pulse w/ duration t_m
  D = A + (R1_F + k)*W_B(ii) + (R1_B + kf)*W_F(ii) + W_B(ii)*W_F(ii);
  M_eq = [1-f f]';
  M_ss = 1/D*[(1-f)*(A + R1_F*W_B(ii)) f*(A + R1_B*W_F(ii))]';
  term_1 = inv(I - E_s*E_m*E_r*C);
  %if (rcond(term_1)<1e-4), output = +Inf; return; end;
  term_2 = (E_s*E_m*(I-E_r) + I-E_s)*M_eq;
  term_3 = E_s*(I-E_m)*M_ss;
  M_z(ii,:) = term_1*(term_2 + term_3);
end

% Ova mozebi e nepotrebno, zasto m_norm treba da se zameni so S0
E_m = expm((R)*t_m);
D = A;
M_eq = [1-f f]';
M_ss = 1/D*[(1-f)*(A) f*(A)]';
term_1 = inv(I - E_s*E_m*E_r*C);
term_2 = (E_s*E_m*(I - E_r) + I-E_s)*M_eq;
term_3 = E_s*(I-E_m)*M_ss; %na pocetok treba E_s?
m_norm = term_1*(term_2 + term_3);
result = m/S0 - M_z(:,1)./m_norm(1);
output = sum(result.^2);
%output = result;

return;


clear; close all
        % Impunerea primei solutii initiale
sol_init = bvpinit(linspace(0,4,5),[1 0]);
        % Gasirea primei solutii
sol = bvp4c(@fbound,@ffront,sol_init);
        % Evaluarea primei solutii
x = linspace(0,4);
y1 = bvpval(sol,x);
        % Impunerea solutiei initiale a doua
sol_init = bvpinit(linspace(0,4,5),[-1 0]);
        % Gasirea solutiei a doua
sol = bvp4c(@fbound,@ffront,sol_init);
        % Evaluarea solutiei a doua
y2 = bvpval(sol,x);
        % Reprezentarea grafica a celor doua solutii
plot(x,y1(1,:),x,y2(1,:),'Linewidth',1.5);
xlabel('x');
ylabel('y');
grid
legend('Prima solutie','A doua solutie')


function [xPred, PPred, F, B] = ExtendedKalmanFilterX_PredictState(x,P,f,Q,u,b,Qu)
% EKALMANFILTERX_PREDICTSTATE Perform the discrete-time KF state prediction
% step, under the assumption of additive process noise.
%
% Parameters
% ----------
% x: column vector
%   The (xDim x 1) state estimate at the previous time-step.
% P: matrix
%   The (xDim x xDim) state covariance matrix at the previous
%   time-step.
% f: function handle
%   A (non-linear) state transition function.
% Q: matrix
%   The (xDim x xDim) process noise covariance matrix.
% u: column vector, optional
%   An optional (xDim x 1) control input.
%   If omitted, no control input is used.
% b: function handle, optional
%   A (non-linear) control gain function.
%   (Optional, Default = 1 if u provided, 0 otherwise)
% O: matrix, optional
%   An optional (xDim x xDim) control noise covariance
%   matrix. If omitted, Q is assumed to be 0.
%
% Returns
% -------
% xPred: column vector
%   The (xDim x 1) predicted state estimate.
% PPred: matrix
%   The (xDim x xDim) predicted state covariance matrix.
% F: matrix
%   The computed Jacobian transition matrix
% H: matrix
%   The computed (yDim x yDim) Jacobian measurement matrix
% B: matrix, optional
%   The computed Jacobian control gain matrix
%
% October 2017 Lyudmil Vladimirov, University of Liverpool.

    switch(nargin)
        case(4)
            u  = 0;
            b  = 0;
            Qu = 0;
        case(5)
            b  = 1;
            Qu = 0;
        case(6)
            Qu = 0;
    end

    % Prediction for state vector and covariance:
    [xPred,F] = ExtendedKalmanFilterX_computeJac(f,x);    %nonlinear update and linearization at current state
    PPred = F*P*F' + Q;                 %partial update

    % Compute Control Input (if applicable)
    [controlInputWithGain,B] = ExtendedKalmanFilterX_computeJac(b,u);

    % Add control input
    xPred = xPred + controlInputWithGain;
    PPred = PPred + B*Qu*B';
end


function y= DAB(T,P,MA,MB,sigmA,sigmB,epsA,epsB)
%DAB Calculates gas-phase diffusivity using Wilke-Lee equation, p. 17 Text.
%   DAB(T,P,MA,MB,sigmA,sigmB,epsA,epsB)
%   T = absolute temperature in K
%   P = pressure in bar
%   MA, MB = molecular weights
%   sigmA, sigmB, epsA, epsB = Lennard-Jones parameters
%   in Angstroms and Kelvin, respectively
%   DAB = diffusivity in square cm/sec.
sigmAB=(sigmA+sigmB)/2;
epsAB=sqrt(epsA*epsB);
x=T/epsAB;
a=1.06036;b=0.15610;c=0.19300;d=0.47635;
e1=1.03587;f=1.52996;g=1.76474;h=3.89411;
MAB=2*(1/MA+1/MB)^-1;
omega=a/x^b+c/exp(d*x)+e1/exp(f*x)+g/exp(h*x);
y=0.001*(3.03-0.98/sqrt(MAB))*T^1.5/(P*sigmAB^2*omega*sqrt(MAB));


function sigma = massey_berlekamp_M2(n,k,t,S,field)

%http://www.ee.ucla.edu/~matache/rsc/node8.html#SECTION00051000000000000000

%Step 2: Initialize variables
kk = 0;

for i = 1:n
    Kappa(1,i) = -Inf;
end
Kappa(1,1) = 0;

%Kappa

LAMBDA = 0;
Tau = [-inf 0];

done = 0;

%Step 3:
while (done ~= 1)
    %disp('K');

    kk = kk + 1;

    %disp('S(kk)');
    %S(kk)

    %disp('LAMBDA')
    %LAMBDA

    sum = -Inf;
    for i = 1:LAMBDA
        %Kappa(kk,i+1)
        %S(kk-i)
        sum = gfadd(sum,gfmul(Kappa(kk,i+1),S(kk-i),field),field);
    end

    %disp('Delta - sum')
    %sum

    delta(kk) = gfadd(S(kk),sum,field);

    %disp('delta');
    %delta

    %Step 4:
    if (delta(kk) == -Inf)
        for i = 1:n
            Kappa(kk+1,i) = Kappa(kk,i);
        end
    end

    if (delta(kk) ~= -Inf)

        for i = 1:n
            Kappa_i(i) = Kappa(kk-1+1,i);
        end

        Kappa_k = gfadd(Kappa_i,gfconv(delta(kk),Tau,field),field);

        while length(Kappa_k) < n
            Kappa_k = [Kappa_k -Inf];
        end

        for i = 1:length(Kappa_k)
            Kappa(kk+1,i) = Kappa_k(i);
        end

        %Step 7:
        if (2*LAMBDA < kk)
            LAMBDA = kk - LAMBDA;

            for i = 1:n
                Kappa_k(i) = Kappa(kk+1-1,i);
            end

            Tau = gfconv(Kappa_k,gfdiv(0,delta(kk),field),field);
        end
    end

    %Step 8:
    Tau = gfconv([-Inf 0],Tau,field);

    %step 9:
    if kk >= 2*t
        done = 1;
    end

    %Kappa
    %LAMBDA
    %Tau

end

for i = 1:n
    sigma(i) = Kappa(kk+1,i);
end


function F=dwt_sr_fuse(I1,I2,zt,D,overlap,epsilon)
%    DWT-SR
%    Input:
%    I1 - input image A
%    I2 - input image B
%    zt - maximum decomposition level
%    D  - Dictionary for sparse representation
%    overlap - the overlapped pixels between two neighbor patches
%    epsilon - sparse reconstuction error
%    Output:
%    F  - fused image
%
%    The code is edited by Yu Liu, 01-09-2014.

%-------------------------------------------------------------------------%
%                                DWT
%-------------------------------------------------------------------------%
I1=double(I1);
I2=double(I2);
tempA=I1;
tempB=I2;

X=cell(zt,4);
Y=cell(zt,4);
Z=cell(zt,4);
for i=1:zt
    [X{i,1},X{i,2},X{i,3},X{i,4}]=dwt2(tempA,'db1','mode','per');
    tempA=X{i,1};
    [Y{i,1},Y{i,2},Y{i,3},Y{i,4}]=dwt2(tempB,'db1','mode','per');
    tempB=Y{i,1};
end

%-------------------------------------------------------------------------%
%                               low-pass fusion
%-------------------------------------------------------------------------%
Z{zt,1}=sparse_fusion(X{zt,1},Y{zt,1},D,overlap,epsilon);

%-------------------------------------------------------------------------%
%                               high-pass fusion
%-------------------------------------------------------------------------%
for i=zt:-1:1
    for j=2:4
        Z{i,j}=selc(X{i,j},Y{i,j},3);
    end
end

%-------------------------------------------------------------------------%
%                               IDWT
%-------------------------------------------------------------------------%
for i=zt:-1:1
    if i>1
        Z{i-1,1}=idwt2(Z{i,1},Z{i,2},Z{i,3},Z{i,4},'db1','mode','per');
    else
        F=idwt2(Z{i,1},Z{i,2},Z{i,3},Z{i,4},'db1','mode','per');
    end
end


function [C, D] = WatsonSHCoeff(k)
% function [C, D] = WatsonSHCoeff(k)
% Computes the spherical harmonic (SH) coefficients of the Watson's
% distribution with the concentration parameter k (kappa) up to the 12th order
% and the derivatives if requested.
%
% Truncating at the 12th order gives good approximation for kappa up to 64.
%
% INPUTS:
%
% k should be an array of positive numbers, specifying a set of
% concentration parameters for the Watson's distribution.
%
% OUTPUTS:
%
% C will be a 2-D array and each row contains the SH coefficients of the
% orders 0, 2, 4, ..., to 2n for the parameter in the corresponding row in
% k.
%
% Note that the SH coefficients of the odd orders are always zero.
%
% D will be the 1st order derivative of C.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

large = find(k>30);
exact = find(k>0.1);
approx = find(k<=0.1);
% Necessary to make matlab happy when k is a single value
exact = exact(:);
approx = approx(:);
large = large(:);

% The maximum order of SH coefficients (2n)
n = 6;

% Computing the SH coefficients
C = zeros(length(k),n+1);

% 0th order is a constant
C(:,1) = 2*sqrt(pi);

% Precompute the special function values
sk = sqrt(k(exact));
sk2 = sk.*k(exact);
sk3 = sk2.*k(exact);
sk4 = sk3.*k(exact);
sk5 = sk4.*k(exact);
sk6 = sk5.*k(exact);
sk7 = sk6.*k(exact);
k2 = k.^2;
k3 = k2.*k;
k4 = k3.*k;
k5 = k4.*k;
k6 = k5.*k;
k7 = k6.*k;

erfik = NODDI_erfi(sk);
ierfik = 1./erfik;
ek = exp(k(exact));
dawsonk = 0.5*sqrt(pi)*erfik./ek;

% for large enough kappa
C(exact,2) = 3*sk - (3 + 2*k(exact)).*dawsonk;
C(exact,2) = sqrt(5)*C(exact,2).*ek;
C(exact,2) = C(exact,2).*ierfik./k(exact);

C(exact,3) = (105 + 60*k(exact) + 12*k2(exact)).*dawsonk;
C(exact,3) = C(exact,3) -105*sk + 10*sk2;
C(exact,3) = .375*C(exact,3).*ek./k2(exact);
C(exact,3) = C(exact,3).*ierfik;

C(exact,4) = -3465 - 1890*k(exact) - 420*k2(exact) - 40*k3(exact);
C(exact,4) = C(exact,4).*dawsonk;
C(exact,4) = C(exact,4) + 3465*sk - 420*sk2 + 84*sk3;
C(exact,4) = C(exact,4)*sqrt(13*pi)/64./k3(exact);
C(exact,4) = C(exact,4)./dawsonk;

C(exact,5) = 675675 + 360360*k(exact) + 83160*k2(exact) + 10080*k3(exact) + 560*k4(exact);
C(exact,5) = C(exact,5).*dawsonk;
C(exact,5) = C(exact,5) - 675675*sk + 90090*sk2 - 23100*sk3 + 744*sk4;
C(exact,5) = sqrt(17)*C(exact,5).*ek;
C(exact,5) = C(exact,5)/512./k4(exact);
C(exact,5) = C(exact,5).*ierfik;

C(exact,6) = -43648605 - 22972950*k(exact) - 5405400*k2(exact) - 720720*k3(exact) - 55440*k4(exact) - 2016*k5(exact);
C(exact,6) = C(exact,6).*dawsonk;
C(exact,6) = C(exact,6) + 43648605*sk - 6126120*sk2 + 1729728*sk3 - 82368*sk4 + 5104*sk5;
C(exact,6) = sqrt(21*pi)*C(exact,6)/4096./k5(exact);
C(exact,6) = C(exact,6)./dawsonk;

C(exact,7) = 7027425405 + 3666482820*k(exact) + 872972100*k2(exact) + 122522400*k3(exact)  + 10810800*k4(exact) + 576576*k5(exact) + 14784*k6(exact);
C(exact,7) = C(exact,7).*dawsonk;
C(exact,7) = C(exact,7) - 7027425405*sk + 1018467450*sk2 - 302630328*sk3 + 17153136*sk4 - 1553552*sk5 + 25376*sk6;
C(exact,7) = 5*C(exact,7).*ek;
C(exact,7) = C(exact,7)/16384./k6(exact);
C(exact,7) = C(exact,7).*ierfik;

% for very large kappa
if size(large,1) > 0
  lnkd = log(k(large)) - log(30);
  lnkd2 = lnkd.*lnkd;
  lnkd3 = lnkd2.*lnkd;
  lnkd4 = lnkd3.*lnkd;
  lnkd5 = lnkd4.*lnkd;
  lnkd6 = lnkd5.*lnkd;
  C(large,2) = 7.52308 + 0.411538*lnkd - 0.214588*lnkd2 + 0.0784091*lnkd3 - 0.023981*lnkd4 + 0.00731537*lnkd5 - 0.0026467*lnkd6;
  C(large,3) = 8.93718 + 1.62147*lnkd - 0.733421*lnkd2 + 0.191568*lnkd3 - 0.0202906*lnkd4 - 0.00779095*lnkd5 + 0.00574847*lnkd6;
  C(large,4) = 8.87905 + 3.35689*lnkd - 1.15935*lnkd2 + 0.0673053*lnkd3 + 0.121857*lnkd4 - 0.066642*lnkd5 + 0.0180215*lnkd6;
  C(large,5) = 7.84352 + 5.03178*lnkd - 1.0193*lnkd2 - 0.426362*lnkd3 + 0.328816*lnkd4 - 0.0688176*lnkd5 - 0.0229398*lnkd6;
  C(large,6) = 6.30113 + 6.09914*lnkd - 0.16088*lnkd2 - 1.05578*lnkd3 + 0.338069*lnkd4 + 0.0937157*lnkd5 - 0.106935*lnkd6;
  C(large,7) = 4.65678 + 6.30069*lnkd + 1.13754*lnkd2 - 1.38393*lnkd3 - 0.0134758*lnkd4 + 0.331686*lnkd5 - 0.105954*lnkd6;
end

% for small kappa
C(approx,2) = 4/3*k(approx) + 8/63*k2(approx);
C(approx,2) = C(approx,2)*sqrt(pi/5);

C(approx,3) = 8/21*k2(approx) + 32/693*k3(approx);
C(approx,3) = C(approx,3)*(sqrt(pi)*0.2);

C(approx,4) = 16/693*k3(approx) + 32/10395*k4(approx);
C(approx,4) = C(approx,4)*sqrt(pi/13);

C(approx,5) = 32/19305*k4(approx);
C(approx,5) = C(approx,5)*sqrt(pi/17);

C(approx,6) = 64*sqrt(pi/21)*k5(approx)/692835;

C(approx,7) = 128*sqrt(pi)*k6(approx)/152108775;

if nargout == 1
	return;
end

% Computing the derivatives
dawsonk2 = dawsonk.^2;
idawsonk2 = 1./dawsonk2;

D = zeros(length(k),n+1);
D(:,1) = 0.0;

% exact
D(exact,2) = -k(exact) + (2*sk2 -sk).*dawsonk + 2*dawsonk2;
D(exact,2) = (.75*sqrt(5*pi))*D(exact,2)./k2(exact).*idawsonk2;

D(exact,3) = 21*k(exact) - 2*k2(exact);
D(exact,3) = D(exact,3) + (63*sk -44*sk2 + 4*sk3).*dawsonk;
D(exact,3) = D(exact,3) - (84 + 24*k(exact)).*dawsonk2;
D(exact,3) = D(exact,3)*(15*sqrt(pi)/32)./k3(exact).*idawsonk2;

D(exact,4) = -165*k(exact) + 20*k2(exact) - 4*k3(exact);
D(exact,4) = D(exact,4) + (-825*sk + 390*sk2 - 44*sk3 + 8*sk4).*dawsonk;
D(exact,4) = D(exact,4) + (990 + 360*k(exact) + 40*k2(exact)).*dawsonk2;
D(exact,4) = D(exact,4)*(21*sqrt(13*pi)/128)./k4(exact).*idawsonk2;

D(exact,5) = 225225*k(exact) - 30030*k2(exact) + 7700*k3(exact) - 248*k4(exact);
D(exact,5) = D(exact,5) + (1576575*sk - 600600*sk2 + 83160*sk3 - 15648*sk4 + 496*sk5).*dawsonk;
D(exact,5) = D(exact,5) - (1801800 + 720720*k(exact) + 110880*k2(exact) + 6720*k3(exact)).*dawsonk2;
D(exact,5) = D(exact,5)*(3*sqrt(17*pi)/2048)./k5(exact).*idawsonk2;

D(exact,6) = -3968055*k(exact) + 556920*k2(exact) - 157248*k3(exact) + 7488*k4(exact) - 464*k5(exact);
D(exact,6) = D(exact,6) + (-35712495*sk + 11834550*sk2 - 1900090*sk3 + 336960*sk4 - 15440*sk5 + 928*sk6).*dawsonk;
D(exact,6) = D(exact,6) + (39680550 + 16707600*k(exact) + 2948400*k2(exact) + 262080*k3(exact) + 10080*k4(exact)).*dawsonk2;
D(exact,6) = D(exact,6)*(11*sqrt(21*pi)/8192)./k6(exact).*idawsonk2;

D(exact,7) = 540571185*k(exact) - 78343650*k2(exact) + 23279256*k3(exact) - 1319472*k4(exact) + 119504*k5(exact) - 1952*k6(exact);
D(exact,7) = D(exact,7) + (5946283035*sk - 1786235220*sk2 + 319642092*sk3 - 53155872*sk4 + 2997456*sk5 - 240960*sk6 + 3904*sk7).*dawsonk;
D(exact,7) = D(exact,7) - (6486854220 + 2820371400*k(exact) + 537213600*k2(exact) + 56548800*k3(exact) + 3326400*k4(exact) + 88704*k5(exact)).*dawsonk2;
D(exact,7) = D(exact,7)*(65*sqrt(pi)/65536)./k7(exact).*idawsonk2;

% approximation
D(approx,2) = 4/3 + 16/63*k(approx) - 16/315*k2(approx) - 128/6237*k3(approx);
D(approx,2) = D(approx,2)*sqrt(pi/5);

D(approx,3) = 16/105*k(approx) + 32/1155*k2(approx) - 3712/675675*k3(approx) - 5888/2837835*k4(approx);
D(approx,3) = D(approx,3)*sqrt(pi);

D(approx,4) = 16/231*k2(approx) + 128/10395*k3(approx) - 256/106029*k4(approx);
D(approx,4) = D(approx,4)*sqrt(pi/13);

D(approx,5) = 128/19305*k3(approx) + 256/220077*k4(approx);
D(approx,5) = D(approx,5)*sqrt(pi/17);

D(approx,6) = 64/138567*k4(approx);
D(approx,6) = D(approx,6)*sqrt(pi/21);

D(approx,7) = 256/50702925*k5(approx);
D(approx,7) = D(approx,7)*sqrt(pi);


function [res,resL2,qualMeasOut]=SART_TV(proj,geo,angles,niter,varargin)
% SART_TV solves Cone Beam CT image reconstruction using Oriented Subsets
%              Simultaneous Algebraic Reconstruction Technique algorithm
%
%   SART_TV(PROJ,GEO,ALPHA,NITER) solves the reconstruction problem
%   using the projection data PROJ taken over ALPHA angles, corresponding
%   to the geometry described in GEO, using NITER iterations.
%
%   SART_TV(PROJ,GEO,ALPHA,NITER,OPT,VAL,...) uses options and values for solving. The
%   possible options in OPT are:
%
%
%   'lambda':      Sets the value of the hyperparameter. Default is 1
%
%   'lambda_red':  Reduction of lambda. Every iteration
%                  lambda=lambdared*lambda. Default is 0.99
%
%   'Init':        Describes different initialization techniques.
%                  'none'     : Initializes the image to zeros (default)
%                  'FDK'      : Initializes image to FDK reconstruction
%                  'multigrid': Initializes image by solving the problem in
%                               small scale and increasing it when relative
%                               convergence is reached.
%                  'image'    : Initialization using a user specified
%                               image. Not recommended unless you really
%                               know what you are doing.
%   'InitImg'      an image for the 'image' initialization. Avoid.
%
%   'TViter'       number of iterations in the TV step. Default 50
%
%   'TVlambda'     hyperparameter in TV iteration. It gives the ratio of
%                  importance of the image vs the minimum total variation.
%                  default is 15. Lower means more TV denoising.
%
%   'Verbose'      1 or 0. Default is 1. Gives information about the
%                  progress of the algorithm.
%
%   'QualMeas'     Asks the algorithm for a set of quality measurement
%                  parameters. Input should contain a cell array of desired
%                  quality measurement names. Example: {'CC','RMSE','MSSIM'}
%                  These will be computed in each iteration.
% 'OrderStrategy'  Chooses the subset ordering strategy. Options are
%                  'ordered' : uses them in the input order, but divided
%                  'random'  : orders them randomly
%                  'angularDistance': chooses the next subset with the
%                                     biggest angular distance with the ones used.
% 'redundancy_weighting': true or false. Default is true. Applies data
%                         redundancy weighting to projections in the update step
%                         (relevant for offset detector geometry)
%  'groundTruth'  an image as grounf truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------

%% Deal with input parameters
[lambda,res,lamdbared,verbose,QualMeasOpts,TViter,TVlambda,OrderStrategy,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,varargin);
measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    res_prev=gt;
    clear gt
end
if nargout<3 && measurequality
    warning("Image metrics requested but none catched as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);

resL2=zeros(1,niter);
if nargout>1
    computeL2=true;
else
    computeL2=false;
end

blocksize=1;
[alphablocks,orig_index]=order_subsets(angles,blocksize,OrderStrategy);

angles_reorder=cell2mat(alphablocks);
index_angles=cell2mat(orig_index);
% does detector rotation exist?
if ~isfield(geo,'rotDetector')
    geo.rotDetector=[0;0;0];
end
%% Create weighting matrices

% Projection weight, W
W=computeW(geo,angles,gpuids);

% Back-Projection weight, V
V=computeV(geo,angles,alphablocks,orig_index,'gpuids',gpuids);

if redundancy_weights
    % Data redundancy weighting, W_r implemented using Wang weighting
    % reference: https://iopscience.iop.org/article/10.1088/1361-6560/ac16bc

    num_frames = size(proj,3);
    W_r = redundancy_weighting(geo);
    W_r = repmat(W_r,[1,1,num_frames]);
    % disp('Size of redundancy weighting matrix');
    % disp(size(W_r));
    W = W.*W_r; % include redundancy weighting in W
end

%% Iterate
offOrigin=geo.offOrigin;
offDetector=geo.offDetector;
rotDetector=geo.rotDetector;
DSD=geo.DSD;
DSO=geo.DSO;
% TODO : Add options for Stopping criteria
for ii=1:niter
    if (ii==1 && verbose==1);tic;end
    % If quality is going to be measured, then we need to save previous image
    % THIS TAKES MEMORY!
    if measurequality && ~strcmp(QualMeasOpts,'error_norm')
        res_prev = res; % only store if necesary
    end

    for jj=1:size(angles,2)
        if size(offOrigin,2)==size(angles,2)
            geo.offOrigin=offOrigin(:,index_angles(:,jj));
        end
        if size(offDetector,2)==size(angles,2)
            geo.offDetector=offDetector(:,index_angles(:,jj));
        end
        if size(rotDetector,2)==size(angles,2)
            geo.rotDetector=rotDetector(:,index_angles(:,jj));
        end
        if size(DSD,2)==size(angles,2)
            geo.DSD=DSD(jj);
        end
        if size(DSO,2)==size(angles,2)
            geo.DSO=DSO(jj);
        end
        %         proj_err=proj(:,:,jj)-Ax(res,geo,angles(:,jj));     %                                 (b-Ax)
        %         weighted_err=W(:,:,jj).*proj_err;                   %                          W^-1 * (b-Ax)
        %         backprj=Atb(weighted_err,geo,angles(:,jj));         %                     At * W^-1 * (b-Ax)
        %         weigth_backprj=bsxfun(@times,1./V(:,:,jj),backprj); %                 V * At * W^-1 * (b-Ax)
        %         res=res+lambda*weigth_backprj;                      % x= x + lambda * V * At * W^-1 * (b-Ax)
        res=res+lambda* bsxfun(@times,1./V(:,:,jj),Atb(W(:,:,jj).*(proj(:,:,index_angles(:,jj))-Ax(res,geo,angles_reorder(:,jj),'gpuids',gpuids)),geo,angles_reorder(:,jj),'gpuids',gpuids));
        if nonneg
            res=max(res,0);
        end
    end

    % If quality is being measured
    if measurequality
        qualMeasOut(:,ii)=Measure_Quality(res,res_prev,QualMeasOpts);
    end

    lambda=lambda*lamdbared;
    % TV denoising
    res=im3DDenoise(res,'TV',TViter,TVlambda,'gpuids',gpuids);

    if computeL2
        geo.offOrigin=offOrigin;
        geo.offDetector=offDetector;
        geo.DSD=DSD;
        geo.rotDetector=rotDetector;
        resL2(ii)=im3Dnorm(proj(:,:,index_angles)-Ax(res,geo,angles,'gpuids',gpuids),'L2'); % Compute error norm2 of b-Ax
        % If the error is not minimized.
        if  ii~=1 && resL2(ii)>resL2(ii-1)
            if verbose
                disp(['Convergence criteria met, exiting on iteration number:', num2str(ii)]);
            end
            return
        end
    end

    if (ii==1 && verbose==1)
        expected_time=toc*niter;
        disp('SART_TV');
        disp(['Expected duration   :    ',secs2hms(expected_time)]);
        disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
        disp('');
    end
end

end

function initres=init_multigrid(proj,geo,alpha,TViter,TVlambda,gpuids)

finalsize=geo.nVoxel;
% start with 64
geo.nVoxel=[64;64;64];
geo.dVoxel=geo.sVoxel./geo.nVoxel;
if any(finalsize<geo.nVoxel)
    initres=zeros(finalsize');
    return;
end
niter=100;
initres=zeros(geo.nVoxel','single');
while ~isequal(geo.nVoxel,finalsize)

    % solve subsampled grid
    initres=SART_TV(proj,geo,alpha,niter,'Init','image','InitImg',initres,'Verbose',0,'TViter',TViter,'TVlambda',TVlambda,'gpuids',gpuids);

    % Get new dims.
    geo.nVoxel=geo.nVoxel*2;
    geo.nVoxel(geo.nVoxel>finalsize)=finalsize(geo.nVoxel>finalsize);
    geo.dVoxel=geo.sVoxel./geo.nVoxel;
    % Upsample!
    % (hopefully computer has enough memory............)
    [y, x, z]=ndgrid(linspace(1,size(initres,1),geo.nVoxel(1)),...
        linspace(1,size(initres,2),geo.nVoxel(2)),...
        linspace(1,size(initres,3),geo.nVoxel(3)));
    initres=interp3(initres,x,y,z);
    clear x y z
end
end

function [lambda,res,lamdbared,verbose,QualMeasOpts,TViter,TVlambda,OrderStrategy,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,alpha,argin)
opts={'lambda','init','initimg','verbose','lambda_red','qualmeas','tviter','tvlambda','orderstrategy','nonneg','gpuids','redundancy_weighting','groundtruth'};
defaults=ones(length(opts),1);
% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('TIGRE:SART_TV:InvalidInput','Invalid number of inputs')
end
multigrid=false;
% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('TIGRE:SART_TV:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option is not default, then extract value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('TIGRE:SART_TV:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end

    switch opt
        % % % % % % % Verbose
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('TIGRE: Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
            % % % % % % % hyperparameter, LAMBDA
        case 'lambda'
            if default
                lambda=1;
            else
                if length(val)>1 || ~isnumeric(val)
                    error('TIGRE:SART_TV:InvalidInput','Invalid lambda')
                end
                lambda=val;
            end
        case 'lambda_red'
            if default
                lamdbared=0.99;
            else
                if length(val)>1 || ~isnumeric(val)
                    error('TIGRE:SART_TV:InvalidInput','Invalid lambda')
                end
                lamdbared=val;
            end
        case 'init'
            res=[];
            if default || strcmp(val,'none')
                res=zeros(geo.nVoxel','single');
                continue
            end
            if strcmp(val,'FDK')
                res=FDK(proj,geo,alpha);
                continue
            end
            if strcmp(val,'multigrid')
                multigrid=true;
                continue
            end
            if strcmp(val,'image')
                initwithimage=1;
                continue
            end
            if isempty(res)
                error('TIGRE:SART_TV:InvalidInput','Invalid Init option')
            end
            % % % % % % % ERROR
        case 'initimg'
            if default
                continue
            end
            if exist('initwithimage','var')
                if isequal(size(val),geo.nVoxel')
                    res=single(val);
                else
                    error('TIGRE:SART_TV:InvalidInput','Invalid image for initialization');
                end
            end
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('CBCT:SART_TV:InvalidInput','Invalid quality measurement parameters');
                end
            end
        case 'tviter'
            if default
                TViter=50;
            else
                TViter=val;
            end
        case 'tvlambda'
            if default
                TVlambda=50;
            else
                TVlambda=val;
            end
        case 'orderstrategy'
            if default
                OrderStrategy='random';
            else
                OrderStrategy=val;
            end

        case 'nonneg'
            if default
                nonneg=true;
            else
                nonneg=val;
            end
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'redundancy_weighting'
            if default
                redundancy_weights = true;
            else
                redundancy_weights = val;
            end
        case 'groundtruth'
            if default
                gt=nan;
            else
                gt=val;
            end
        otherwise
            error('TIGRE:SART_TV:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option in SART()']);
    end
end
if multigrid
    res=init_multigrid(proj,geo,alpha,TViter,TVlambda,gpuids);
end

end


function [dat, state] = ft_preproc_standardize(dat, begsample, endsample, state)

% FT_PREPROC_STANDARDIZE performs a z-transformation or standardization
% of the data. The standardized data will have a zero-mean and a unit
% standard deviation.
%
% Use as
%   [dat] = ft_preproc_standardize(dat, begsample, endsample)
% where
%   dat        data matrix (Nchans dat Ntime)
%   begsample  index of the begin sample for the mean and stdev estimate
%   endsample  index of the end sample for the mean and stdev estimate
%
% If no begin and end sample are specified, it will be estimated on the
% complete data.
%
% See also PREPROC

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

if nargin<2 || isempty(begsample)
  begsample = 1;
end

if nargin<3 || isempty(endsample)
  endsample = size(dat,2);
end

if nargin<4
  state = [];
end

% preprocessing fails on channels that contain NaN
if any(isnan(dat(:)))
  ft_warning('FieldTrip:dataContainsNaN', 'data contains NaN values');
end

% get the data selection
y = dat(:,begsample:endsample);

% determine the size of the selected data: nChans dat nSamples
[m, n] = size(y);

% compute the sum and sum of squares
s  = sum(y,2);
ss = sum(y.^2,2);

% include the state information from the previous calls
if ~isempty(state)
  s  = s  + state.s;
  ss = ss + state.ss;
  n  = n  + state.n;
end

% compute the mean and standard deviation
my = s ./ n;
sy = sqrt((ss - (s.^2)./n) ./ (n-1));

% standardize the complete input data
dat = (dat - repmat(my, 1, size(dat, 2))) ./ repmat(sy, 1, size(dat, 2));

% remember the state
state.s  = s;  % sum
state.ss = ss; % sum of sqares
state.n  = n;  % number of samples


function [y, iter, lambda, status] = minimize_cubic_newton(H, g, sigma, options)
% Minimize a cubicly regularized quadratic via Newton root finding.
%
% [y, iter, lambda, status] = minimize_cubic_newton(H, g, sigma, options)
%
% Inputs: a symmetric matrix H of size n, a nonzero vector g of length n,
% a positive real sigma and an options structure. The code expects H to
% be tridiagonal, stored as a sparse matrix.
%
% The main output is a vector y of length n, which should minimize
%
%   f(y) = g'*y + (1/2)*y'*H*y + (1/3)*sigma*norm(y)^3.
%
% This is achieved by reducing the problem to a univariate root finding
% problem, where the unknown is a scalar lambda. This root is computed
% using a Newton method.
%
% Other outputs are iter (the number of Newton iterations completed),
% lambda (a real scalar, see below) and status. The latter is 0 if the
% target tolerance was reached, 1 if subsequent iterations induce no
% significant change, and -1 if the algorithm return because it reached
% the maximum number of iterations (see the options structure.)
% Non-negative status values are considered successes.
%
% The options structure must contain the following fields (between
% parentheses are some recommended values):
%   options.verbosity (3): to control how much information this function
%   prints to the command window. Anything below 6 silences the function.
%   options.maxiter_newton (100): maximum number of Newton iterations.
%   options.tol_newton (1e-16): tolerance on the root finding accuracy. See
%   in code for details.
%
% The code is based on Section 6 in
% Cartis, Gould and Toint, "Adaptive cubic regularisation methods for
% unconstrained optimization. Part I: motivation, convergence and numerical
% results", Mathematical Programming, 2011.
% https://link.springer.com/article/10.1007/s10107-009-0286-5
%
% Theorem 3.1 in the referenced paper states y is optimal if and only
% if it there exists a real lambda such that
%
% (H + lambda*I)y = -g,  lambda = sigma*||y||  and  H + lambda*I is psd,
%
% where psd means positive semidefinite. The other way around, if we
% find the corresponding scalar lambda, than we can recover y by
% solving a linear system (though this system might not have a unique
% solution in full generality.) Thus, the general strategy is to search
% for lambda rather than for y.
%
% See also: arc arc_lanczos

% This file is part of Manopt: www.manopt.org.
% Original authors: May 1, 2018,
%    Naman Agarwal, Brian Bullins, Nicolas Boumal and Coralia Cartis.
% Contributors:
% Change log:

    n = size(H, 1);

    % Pick an initial lambda that is cheap to compute and that surely makes
    % the shifted H positive definite.
    lambda = norm(H, 1) + 2;
    H_shifted = H + lambda*speye(n);

    % Compute the smallest eigenvalue of H, as we know the target lambda
    % must be at least as large as the negative of that, so that the
    % shifted H will be positive semidefinite.
    %
    % Since H ought to be sparse and tridiagonal, and since we only need
    % its smallest eigenvalue, this computation could be sped up
    % significantly. It does not appear to be a bottleneck, and eig is
    % simple and reliable, so we keep this for now.
    lambda_min = min(eig(H));
    left_barrier = max(0, -lambda_min);

    % Counter 'iter' holds the number of fully executed Newton iterations.
    iter = 0;
    while true

        if iter >= options.maxiter_newton
            % Iterations exceeded maximum number allowed.
            status = -1;
            return;
        end

        % If lambda has the correct value and the shifted H is positive
        % definite, then this y is a minimizer.
        y = -(H_shifted\g);
        ynorm = norm(y);

        % If the following quantity is zero, we have found a solution.
        phi = 1/ynorm - sigma/lambda;

        % Check if it is close enough to zero to stop.
        if abs(phi) <= options.tol_newton*ynorm
            status = 0;
            return;
        end
        psi = ynorm^2;

        % TODO: clarify this part of the code (see referenced paper).
        % The following is a Newton type of step on the equation
        % sigma/lambda = 1/sqrt(psi(lambda_prev)) ...
        %          - (lambda - lambda_prev)((psi'(lambda_prev))/2(psi)^1.5)
        delta_y = -(H_shifted\y);
        psi_prime = 2*(y'*delta_y);
        p0 = 2*sigma*(psi^(1.5));
        p1 = -2*psi - lambda*psi_prime;
        p2 = psi_prime;
        r = roots([p2 p1 p0]);
        del_lambda = max(r) - lambda;
        iter = iter + 1;

        % If the Newton step would bring us left of the left barrier, jump
        % instead to the midpoint between the left barrier and the current
        % lambda.
        if lambda + del_lambda <= left_barrier
            del_lambda = -.5*(lambda - left_barrier);
        end

        % If the step is so small that it numerically does not make a
        % difference when added to the current lambda, we stop.
        if abs(del_lambda) <= eps(lambda)
            status = 1;
            return;
        end

        % Update lambda
        H_shifted = H_shifted + del_lambda*speye(n);
        lambda = lambda + del_lambda;

        if options.verbosity >= 6
            fprintf(['lambda %.12e, ||y|| %.12e, lambda/sigma %.12e, ' ...
                     'phi %.12e\n\n'], lambda, ynorm, lambda / sigma, phi);
        end

    end

end


function [D, dD1, dD2] = dist_earth(coord1, coord2)
% [D, dD] = dist_earth(coord1, coord2)
% coord1 is 2 x N
% coord2 is 2 x M
% Returns N x M matrix D of mutual distances.

if nargin < 2 || isempty(coord2)
  coord2 = coord1;
end

R = 6371.01; % Spherical Earth radius approximation

n1 = cols(coord1);
n2 = cols(coord2);

D = zeros(n1, n2);

coord1 = pi/180 * coord1;
coord2 = pi/180 * coord2;
if nargout >= 2
  dD1 = zeros([n1, n2, 2]);
end
if nargout >= 3
  error('Hmm.. maybe you shouldn''t use third output? :)');
  dD2 = zeros([n1, n2, 2]);
end

for i=1:n1
  lat1 = coord1(2,i);
  lat2 = coord2(2,:);
  dlon = (coord2(1,:) - coord1(1,i));
  f = sin(lat2).*sin(lat1) + cos(lat2).*cos(lat1).*cos(dlon);
  f(f>=1-eps) = 1-eps; % correction because of numerical errors, f should be [-1,1]
  f(f<=eps-1) = eps-1; % correction because of numerical errors, f should be [-1,1]
  D(i,:) = R * acos(f);
  if ~isreal(D)
    coord1
    f
%    D
    error('Oohps! Complex distance..');
  end
  %dD_df(isinf(dD_df)) = 0;%-R ./ sqrt(1-f.^2);

  if nargout >= 2
    dD_df = -R ./ sqrt(1-f.^2);
    dD1(i,:,1) = pi/180 * dD_df .* cos(lat1) .* cos(lat2) .* sin(dlon);
    dD1(i,:,2) = pi/180 * dD_df .* (cos(lat1).*sin(lat2) - sin(lat1).* ...
                                    cos(lat2).*cos(dlon));
  end
% $$$   if any(isnan(dD1))
% $$$     dD1
% $$$     error('WTF?');
% $$$   end
% $$$   if nargout >= 3
% $$$     dD2(i,:,1) = -dD1(i,:,1);
% $$$     dD2(i,:,2) = dD_df .* (sin(lat1).*cos(lat2) - cos(lat1).*sin(lat2).*cos(dlon));
% $$$   end
end


function e = rmModelSearchFit_oneOvalGaussian(p,Y,Xv,Yv,stim,t)
% rmModelSearchFit_oneOvalGaussian - actual fit function of rmSearchFit
%
% error = rmModelSearchFit_oneOvalGaussian(p,Y,trends,Xgrid,YGrid,stimulusMatrix);
%
% Basic barebones fit of a single time-series. Error is returned in
% percentage: 100% is RSS of unfitted time-series. This way we can quantify
% the improvement of the fit independend of the variation in the raw
% time-series.
%
% 2006/06 SOD: wrote it.
% 2006/12 SOD: modifications for fmincon, this is litterally called >10000
% times so we cut every corner possible.

% make RF (taken from rfGaussian2d)
Xv = Xv - p(1);   % positive x0 moves center right
Yv = Yv - p(2);   % positive y0 moves center up

Xold = Xv;
Yold = Yv;
Xv = Xold .* cos(p(5)) - Yold .* sin(p(5));
Yv = Xold .* sin(p(5)) + Yold .* cos(p(5));

% make gaussian on current grid
RF = exp( -.5 * ((Yv ./ p(3)).^2 + (Xv ./ p(4)).^2));

% make prediction (taken from rfMakePrediction)
X = [stim*RF t];

% fit - inlining pinv
%b = pinv(X)*Y;
[U,S,V] = svd(X,0);
s = diag(S);
tol = numel(X) * eps(max(s));
r = sum(s > tol);
if (r == 0)
    pinvX = zeros(size(X'));
else
    s = diag(ones(r,1)./s(1:r));
    pinvX = V(:,1:r)*s*U(:,1:r)';
end
b = pinvX*Y;

% compute residual sum of squares (e)
% e = norm(Y - X*abs(b));
if b(1)>0,
    e = norm(Y - X*b);
else
    e = norm(Y).*(1+sum(abs(b(1))));
end
return;


% irreg_demo
%
% demo of an irregularly sampled problem
%
% Author Piet M.T. Broersen, September 2008
%
% The three statements :
%           ******************************************************
%           *  [asel bsel sellog]=ARMAsel_mis(ti,xi,ARmax,Tr,w); *
%           *  psdsel=arma2psd(asel,bsel);                       *
%           *  corsel=arma2cor(asel,bsel,50);                    *
%           ******************************************************
%  compute the selected estimated spectrum and autocorrelation function.
%  Sellog gives additional information and plots spectra of all estimated
%  AR models
%
%  ti : observation times [s]
%  xi : observation amplitudes
%  ARmax : higherst candidate AR order
%  Tr : equidistant resampling distance [s]
%  w ; slot width as fraction of Tr [s]
%
%  If the mean sampling frequency f0 Hz is high in comparison with the highest
%  frequency in the desired spectrum, it is advisable to use
%  nearest neighbor resampling and armasel.
%  See the example in simple_irreg_demo.
%  This gives very accurate spectra until f0/20 Hz and
%  about 50 % error due to resampling at f0/2pi.
%  The error becomes very large for higher frequencies and
%  armasel_irreg is advised for spectra higher than f0/2pi Hz.

clear all, close all, clc, echo off

load irreg_data
% row vectors of times ti and amplitudes xi of irregularly sampled observations

N=length(ti)

figure
plot(ti,xi,'p',ti,xi,':r')
title([int2str(N),' irregular benchmark data.'])
xlabel('\rightarrow time axis [s]')
axis tight

disp('Mean time between observations')
T0=(ti(N) - ti(1))/(N-1)

disp(' ')
disp('************************************************************************')
disp('Some warning messages from the OPTIM toolbox cannot be suppressed easily')
disp('It is not necessary to provide gradient information')
disp('Warnings generated by the MATLAB OPTIM routine fminunc can be ignored')
disp('************************************************************************')
disp(' ')
disp('Input values for ARMAsel_irreg')
ARmax=3
w=1/2            %*Tres   slot width, fraction of Tres
Tr=1/2000
disp(' ')
disp('[air bir sellogir] = ARMAsel_irreg(ti,xi,ARmax,Tr,w)')

[air bir sellogir] = ARMAsel_irreg(ti,xi,ARmax,Tr,w)

disp('************************************************************************')
disp(' ')
disp('New input for w')
w=1/8
disp(' ')
disp('[air2 bir2 sellogir2] = ARMAsel_irreg(ti,xi,ARmax,Tr,w)')
[air2 bir2 sellogir2] = ARMAsel_irreg(ti,xi,ARmax,Tr,w)

disp('************************************************************************')
disp(' ')
disp('New input for w and Tr')
w=1/2            %*Tres   slot width, fraction of Tres
Tr2=1/4000
disp(' ')
disp('[air3 bir3 sellogir3] = ARMAsel_irreg(ti,xi,ARmax,Tr,w)')
[air3 bir3 sellogir3] = ARMAsel_irreg(ti,xi,ARmax,Tr2,w)

[h_ir f_ir] = arma2psd(air,bir,1000,Tr);
h_ir2 = arma2psd(air2,bir2,1000,Tr);
[h_ir3 f_ir3] = arma2psd(sellogir3.AR_sel_corrected,1,1000,Tr2);

disp(' ')
disp('  rc AR_sel for w = 1/2 and for w = 1/8, Tr = 0.0005 s')
[dum rc_sel]=ar2arset(sellogir.ar);
[dum rc_sel2]=ar2arset(sellogir2.ar);
[dum rc_sel3]=ar2arset(sellogir3.AR_sel_corrected);
disp(rc_sel)
disp(rc_sel2)
disp('  rc AR_sel-corrected for w = 1/2, Tr = 0.00025 s')
disp(rc_sel3)

figure
loglog(f_ir,h_ir,f_ir,h_ir2,f_ir3,h_ir3)
legend('\it{Tr}\rm = 0.0005 s,  \itw\rm = 1/2 * \it{Tr}\rm s', ...
       '\it{Tr}\rm = 0.0005 s,  \itw\rm = 1/8 * \it{Tr}\rm s', ...
       '\it{Tr}\rm = 0.00025 s, \itw\rm = 1/2 * \it{Tr}\rm s',3)
title(['PSD of ',int2str(N),' irregular benchmark data.'])
xlabel('\rightarrow frequency [Hz]')
ylabel('\rightarrow Logarithm of power spectral density')
axis tight


function [w,b,sv,obj] = linear_primal_svm(lambda,wInit,bInit,D,noneg, maxIteration,opt)
% Solves the following SVM optimization problem in the primal (with quatratic
%   penalization of the training errors). Default solved by Newton.
%
% min_{w,e}  lambda/2 * w'w  + 1/2 sum_i out_i^2
%      s.t.  Y_i * (w .* X_i + b) >= D_i - out_i
%            w(nonneg)>=0
%
% A global variable X containing the training inputs
%   should be defined. X is an n x d matrix (n = number of points).
%   X can be either normal matrix or sparse matrix.
% A global variable Y is the target vector of size nx1. Normal SVM will
%   have +1 and -1 value, but it can be actually aribitury value
% A global variable n is the number of elements that you want to use for training.
% LAMBDA is the regularization parameter ( = 1/C)
% wInit is an optional input for the initial value of [w;b]
% dvec is an optional input, usually it is 1 for standard SVM
% maxIteration is the number of iterations allowd
%
% W is the hyperplane w (vector of length d).
% B is the bias
% The outputs on the training points are either X*W+B
% SV is the support vector index number
% OBJ is the objective function value
% OPT is a structure containing the options (in brackets default values):
%   cg: Do not use Newton, but nonlinear conjugate gradients [0]
%   lin_cg: Compute the Newton step with linear CG
%           [0 unless solving sparse linear SVM]
%   iter_max_Newton: Maximum number of Newton steps [20]
%   prec: Stopping criterion
%   cg_prec and cg_it: stopping criteria for the linear CG.

% Original written by Olivier Chapelle @ http://olivier.chapelle.cc/primal/
% Modified by Jianxiong Xiao to have several advance features @ http://mit.edu/jxiao/

if ~exist('maxIteration','var') || maxIteration==Inf     % Assign the options to their default values
    maxIteration = 10000000;
end

if ~exist('opt','var')       % Assign the options to their default values
    opt = [];
end
if ~isfield(opt,'cg'),                opt.cg = 0;                        end;
if ~isfield(opt,'lin_cg'),            opt.lin_cg = 0;                    end;
if ~isfield(opt,'iter_max_Newton'),   opt.iter_max_Newton = 20;          end; % used to be 20
if ~isfield(opt,'prec'),              opt.prec = 1e-6;                   end;
if ~isfield(opt,'cg_prec'),           opt.cg_prec = 1e-4;                end;
if ~isfield(opt,'cg_it'),             opt.cg_it = 20;                    end;

global X;
global Y;

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

if ~exist('dvec','var') || isempty(D)
    D = ones(numel(Y),1);
end

if isempty(X), error('Global variable X undefined'); end;

if ~exist('bInit','var')
    bInit=0;
end
if ~exist('wInit','var')
    d = size(X,2);
    wInit = zeros(d,1);
end
if issparse(X)
    opt.lin_cg = 1;
end;
if ~opt.cg
    [sol,obj, sv] = primal_svm_linear   (lambda,maxIteration,wInit,bInit,D,noneg,opt);
else
    [sol,obj, sv] = primal_svm_linear_cg(lambda,maxIteration,wInit,bInit,D,noneg,opt);
end;

% The last component of the solution is the bias b.
b = sol(end);
w = sol(1:end-1);
fprintf('\n');

% -------------------------------
% Train a linear SVM using Newton
% -------------------------------
function  [w,obj,sv] = primal_svm_linear(lambda,maxIteration,wInit,bInit,D,noneg,opt)

global X;
global Y;
global n;
d = size(X,2);

w = [wInit; bInit]; % The last component of w is b.
w(noneg) = max(w(noneg),0);
%out = ones(n,1); % Vector containing 1-Y.*(X*w)
out = D(1:n) - Y(1:n).*(X(1:n,:)*w(1:end-1)+w(end));

for iter=1:maxIteration
    if iter > opt.iter_max_Newton;
        warning('PrimalSVM:MaxNumNewton','Maximum number of Newton steps reached. Try larger lambda');
        break;
    end;

    [obj, grad, sv] = obj_fun_linear(w,lambda,out);

    % Compute the Newton direction either exactly or by linear CG
    if opt.lin_cg
        % Advantage of linear CG when using sparse input: the Hessian is never computed explicitly.
        [step, foo, relres] = minres(@hess_vect_mult, -grad, opt.cg_prec,opt.cg_it,[],[],[],sv,lambda);
    else
        Xsv = X(sv,:);
        hess = lambda*diag([ones(d,1); 0]) + [[Xsv'*Xsv sum(Xsv,1)']; [sum(Xsv) length(sv)]];   % Hessian
        step  = - hess \ grad;   % Newton direction
    end;

    % Do an exact line search
    [t,out, sv] = line_search_linear(w,step,out, lambda);

    w = w + t*step;
    w(noneg) = max(w(noneg),0);
    fprintf('Iter = %d, Obj = %f, Nb of sv = %d, Newton decr = %.3f,  Line search = %.3f',iter,obj,length(sv),-step'*grad/2,t);
    if opt.lin_cg
        fprintf(', Lin CG acc = %.4f     \n',relres);
    else
        fprintf('      \n');
    end;

    if -step'*grad < opt.prec * obj
        % Stop when the Newton decrement is small enough
        break;
    end;
end;

% -----------------------------------------------------
% Train a linear SVM using nonlinear conjugate gradient
% -----------------------------------------------------
function  [w, obj, sv] = primal_svm_linear_cg(lambda,maxIteration,wInit,bInit, D,noneg,opt)
global X;
global Y;
global n;
d = size(X,2);

w = [wInit; bInit]; % The last component of w is b.
w(noneg) = max(w(noneg),0);
%out = ones(n,1); % Vector containing 1-Y.*(X*w)
out = D(1:n) - Y(1:n).*(X(1:n,:)*w(1:end-1)+w(end));

%go = [X(1:n,:)'*Y(1:n); sum(Y(1:n))];  % -gradient at w=0, need to be change for w!=0 initialization
[~, grad] = obj_fun_linear(w,lambda,out); go = -grad; % -gradient

s = go; % The first search direction is given by the gradient
for iter=1:maxIteration
    if iter > opt.cg_it * min(n,d)
        warning('PrimalSVM:MaxNumCG','Maximum number of CG iterations reached. Try larger lambda');
        break;
    end;

    % Do an exact line search
    [t,out,sv] = line_search_linear(w,s,out,lambda);
    w = w + t*s;
    w(noneg) = max(w(noneg),0);

    % Compute the new gradient
    [obj, gn, sv] = obj_fun_linear(w,lambda,out); gn=-gn;
    fprintf('Iter = %d, Obj = %f, Norm of grad = %.3f     \n',iter,obj,norm(gn));

    % Stop when the relative decrease in the objective function is small
    if t*s'*go < opt.prec*obj, break; end;

    % Flecher-Reeves update. Change 0 in 1 for Polack-Ribiere
    be = (gn'*gn - 0*gn'*go) / (go'*go);
    s = be*s+gn;
    go = gn;
end;

function [obj, grad, sv] = obj_fun_linear(w,lambda,out)
% Compute the objective function, its gradient and the set of support vectors
% Out is supposed to contain 1-Y.*(X*w)
global X;
global Y;
global n;
out = max(0,out);
wb0 = w; wb0(end) = 0;  % Do not penalize b <= Very important for object detection
obj = sum(out.^2)/2 + lambda*(wb0')*wb0/2; % L2 penalization of the errors
grad = lambda*wb0 - [((out.*Y(1:n))'*X(1:n,:))'; sum(out.*Y(1:n))]; % Gradient
sv = find(out>0);

function [t,out,sv] = line_search_linear(w,d,out,lambda)
% From the current solution w, do a line search in the direction d by
% 1D Newton minimization
global X;
global Y;
global n;
t = 0;
% Precompute some dots products
Xd = X(1:n,:)*d(1:end-1)+d(end);
wd = lambda * w(1:end-1)'*d(1:end-1);
dd = lambda * d(1:end-1)'*d(1:end-1);
while 1
    out2 = out - t*(Y(1:n).*Xd); % The new outputs after a step of length t
    sv = find(out2>0);
    g = wd + t*dd - (out2(sv).*Y(sv))'*Xd(sv); % The gradient (along the line)
    h = dd + Xd(sv)'*Xd(sv); % The second derivative (along the line)
    t = t - g/h; % Take the 1D Newton step. Note that if d was an exact Newton
    % direction, t is 1 after the first iteration.
    if g^2/h < 1e-10, break; end;
    %    fprintf('%f %f\n',t,g^2/h)
end;
out = out2;

function y = hess_vect_mult(w,sv,lambda)
% Compute the Hessian times a given vector x.
% hess = lambda*diag([ones(d-1,1); 0]) + (X(sv,:)'*X(sv,:));
global X;
global n;
y = lambda*w;
y(end) = 0;
z = (X(1:n,:)*w(1:end-1)+w(end));  % Computing X(sv,:)*x takes more time in Matlab :-(
zz = zeros(length(z),1);
zz(sv)=z(sv);
y = y + [(zz'*X(1:n,:))'; sum(zz)];


function jac = p13_jac2 ( option, m, nvar, lambda, u )

%*****************************************************************************80
%
%% P13_JAC2 computes the jacobian by recasting it on a square grid.
%
%  Discussion:
%
%    Actually, to stave off insanity, we only "recast" the variables into
%    a 2D array that corresponds to the spatial ordering of the grid.
%    We leave the jacobian in its original arrangement, which assumes
%    a linear ordering of variables and equations, and we simply
%    compute the equation and variable indices of the jacobian when
%    we are ready to put entries into it.  This approach seems to produce
%    a smaller amount of cosmic grief than the alternatives.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Input, integer M, the number of grid points on a side of the square.
%
%    Input, integer NVAR, the number of variables.
%
%    Input, real LAMBDA, the value of the parameter.
%
%    Input, real U(M,M), the value of the grid function.
%
%    Output, real JAC(NVAR-1,NVAR), the jacobian matrix evaluated
%    at X.  The NVAR-th row is not set by this routine.
%
  jac(1:nvar-1,1:nvar) = 0.0;

  h = 1.0 / ( m + 1 );

  ieqn = 0;

  for i = 1 : m
    for j = 1 : m

      ieqn = ( j - 1 ) * m + i;

      uc = u(i + (j-1)*m);

      if ( i < m )
        un = u(i+1+(j-1)*m);
      else
        un = 0.0;
      end

      if ( 1 < i )
        us = u(i-1+(j-1)*m);
      else
        us = 0.0;
      end

      if ( j < m )
        ue = u(i+j*m);
      else
        ue = 0.0;
      end

      if ( 1 < j )
        uw = u(i+(j-2)*m);
      else
        uw = 0.0;
      end

      fc = p13_gx ( option, uc );
      fn = p13_gx ( option, un );
      fs = p13_gx ( option, us );
      fe = p13_gx ( option, ue );
      fw = p13_gx ( option, uw );

      del5f = fc + h * h * ( - 4.0 * fc + fn + fs + fe + fw ) / 12.0;

      fcp = p13_gp ( option, uc );
      fnp = p13_gp ( option, un );
      fsp = p13_gp ( option, us );
      fep = p13_gp ( option, ue );
      fwp = p13_gp ( option, uw );

      ivar = ( j - 1 ) * m + i;
      jac(ieqn,ivar) = - 20.0 / ( 6.0 * h * h ) ...
        + lambda * ( fcp - 4.0 * h * h * fcp / 12.0 );

      if ( i < m )
        ivar = ( j - 1 ) * m + i + 1;
        jac(ieqn,ivar) = 4.0 / ( 6.0 * h * h ) ...
          + lambda * h * h * fnp / 12.0;
      end

      if ( 1 < i )
        ivar = ( j - 1 ) * m + i - 1;
        jac(ieqn,ivar) = 4.0 / ( 6.0 * h * h ) ...
          + lambda * h * h * fsp / 12.0;
      end

      if ( j < m )
        ivar = j * m + i;
        jac(ieqn,ivar) = 4.0 / ( 6.0 * h * h ) ...
          + lambda * h * h * fep / 12.0;
      end

      if ( 1 < j )
        ivar = ( j - 2 ) * m + i;
        jac(ieqn,ivar) = 4.0 / ( 6.0 * h * h ) ...
          + lambda * h * h * fwp / 12.0;
      end

      if ( 1 < i & 1 < j )
        ivar = ( j - 2 ) * m + i - 1;
        jac(ieqn,ivar) = 1.0 / ( 6.0 * h * h );
      end

      if ( 1 < i & j < m )
        ivar = j * m + i - 1;
        jac(ieqn,ivar) = 1.0 / ( 6.0 * h * h );
      end

      if ( i < m & 1 < j )
        ivar = ( j - 2 ) * m + i + 1;
        jac(ieqn,ivar) = 1.0 / ( 6.0 * h * h );
      end

      if ( i < m & j < m )
        ivar = j * m + i + 1;
        jac(ieqn,ivar) = 1.0 / ( 6.0 * h * h );
      end

      ivar = nvar;
      jac(ieqn,nvar) = del5f;

    end
  end

  return
end


function zz=lpcfq2zz(f,q)
%LPCFQ2ZZ Convert frequencies and q factors to z-plane poles ZZ=(F,Q)
%all input values are in normalized Hz
% roots are at exp(2*pi*f*(-1/(2q) +- j)
% if f has more columns than q, remaining columns are real roots at -f

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: lpcfq2zz.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,pf]=size(f);
if nargin < 2
   pq=0;
else
   pq=size(q,2);
end;
zz=zeros(nf,pf+pq);
if pq
   ii=1:pq;
   zz(:,2*ii-1)=exp(pi*f(:,ii).*(2i-q.^(-1)));
   zz(:,2*ii)=conj(zz(:,2*ii-1));
end
if pf>pq
   ii=1+pq:pf;
   zz(:,ii+pq)= exp(-2*pi*f(:,ii));
end


%Kalman Demo 2 (Mauna Loa Periodic CO2 Readings)
%
%  In this demo we apply the state space inference methods to the
%  well-known time series data consisting of atmospheric CO2
%  concentration readings in parts per million (ppm) by volume from
%  air samples collected at the Mauna Loa observatory, Hawaii (see
%  [1] for details and further references).
%
%  The benefit from the state space formulation is that the
%  computational complexity is linear with respect to the number
%  of data points. Due to efficient matrix solvers in Matlab
%  (favoring the traditional GP solution over sequential looping),
%  the advantages in speed start to show in datasets with thousands
%  of data points.
%
%  The take-home message from this demo is that the GP can be
%  set up exactly as any other GP regression model in GPstuff,
%  and then solved by Kalman filtering methods by specifying the
%  'type' in the GP structure to be 'KALMAN'.
%
%  The methods in this demo are based on the paper:
%
%  [1] Arno Solin and Simo Sarkka (2014). Explicit link between periodic
%      covariance functions and state space models. Accepted for
%      publication in Proceedings of the Seventeenth International
%      Conference on Artifcial Intelligence and Statistics (AISTATS 2014).
%
% Copyright (c) 2014 Arno Solin and Jukka Koskenranta

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

%% Load the data

  % Load the data
  S = which('demo_kalman2.m');
  L = strrep(S,'demo_kalman2.m','demodata/maunaloa_data.txt');

  % Set training data (x = time in years, y = CO2 observations)
  data = load(L);
  y = data(:, 2:13)';  y=y(:);
  x = (data(1,1):1/12:data(end,1)+11/12)';

  % Remove data points with missing information
  x = x(y>0); y = y(y>0);

  % Make data zero mean
  ymean = mean(y); y = y-ymean;

  % Show original data
  figure(1); clf
    plot(x,y+ymean,'-k')
    xlabel('Time (year)');
    ylabel('CO_2 concentration (PPM)')
    title('Kalman Demo 2  (Mauna Loa Periodic CO2 Readings) - Data')

%% Set up GP model

  % Noise variance prior
  ps2 = prior_logunif();

  % The likelihood model
  lik = lik_gaussian('sigma2', 1, 'sigma2_prior', ps2);

  % Covariance function hyperparameter priors
  pl = prior_logunif();
  pm = prior_logunif();

  % A squared exponential covariance function
  % to deal with the smooth long term effects
  gpcf1 = gpcf_sexp('lengthScale', 100, 'magnSigma2', 5000, ...
                    'lengthScale_prior',pl,'magnSigma2_prior',pl);

  % A quasi-periodic covariance function deals with peridic
  % variation in the data. The quasi-periodic covariance function
  % is a product of a periodic covariance function and a squared
  % exponential.
  gpcf2 = gpcf_periodic('magnSigma2',1,'lengthScale',1,'period',1, ...
                        'decay',1,'lengthScale_sexp',100, ...
                        'lengthScale_prior',pl,'magnSigma2_prior',pl, ...
                        'lengthScale_sexp_prior',pl);

  % A Matern52 covariance function deals with short term
  % non-periodic effects that remain otherwise unexplained
  gpcf3 = gpcf_matern52('lengthScale', 10, 'magnSigma2', 10, ...
                        'lengthScale_prior',pl,'magnSigma2_prior',pl);

  % Finally create the GP structure
  gp = gp_set('lik', lik, 'cf', {gpcf1,gpcf2,gpcf3});

  % Set type to KALMAN
  gp = gp_set(gp,'type','KALMAN');

%% Optimize hyperparameters and predict

  % Optimization parameters
  opt=optimset('TolFun',1e-4,'TolX',1e-4,'Display','iter');

  % Find hyperparameters by optimization (BFGS)
  gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

  % Set the test points
  xt = (x(end):1/12:x(end)+10)';

  % Predict values
  [Eft,Varft] =  gp_pred(gp, x, y,'xt',xt);

  % Also predict the latent components separately
  [Eft1, Varft1] = gp_pred(gp, x, y, x, 'predcf', [1 3]);
  [Eft2, Varft2] = gp_pred(gp, x, y, x, 'predcf', [2]);

%% Visualize results

  % Plot
  figure(2); clf; hold on

    % Plot the 95% confidence interval of the predictions
    p=patch([xt; flipud(xt)], ...
       [ymean + Eft + 1.96*sqrt(Varft); ...
        flipud(ymean + Eft - 1.96*sqrt(Varft))],[0.9,0.9,0.9]);
    set(p,'EdgeColor','none')

    % Plot observations
    plot(x,ymean+y,'.k','MarkerSize',5)

    % Labels and legends
    title('Kalman Demo 2 (Mauna Loa Periodic CO2 Readings)')
    xlabel('Time (years)');
    ylabel('CO_2 concentration (PPM)')
    legend('95% confidence region', ...
           'Monthly average measurements',...
           'Location', 'NorthWest');

    % Axis options
    box on; axis tight; set(gca,'Layer','top')

%% Show components separately

  figure(3); clf;
  subplot(211)

    plot(x,Eft1,'-k')

    title('Long-term trend and short-scale variation')
    xlabel('Time (years)');
    ylabel('Effect on CO_2 concentration (PPM)')

    axis tight

  subplot(212)

    plot(x,Eft2,'-k')

    title('Quasi-periodic effect')
    xlabel('Time (years)');
    ylabel('Effect on CO_2 concentration (PPM)')

    axis tight