function [G1,C,impact,eu,F]=schur_solver(g0,g1,c,psi,pi,continuous,check_exist,check_uniq,n_v)
% Solver based on Schur decomposition (This is based on Sims's gensys, but adjusted
%    to take advantage of special case that applies to heterogeneous agent models).
%
% by SeHyoun Ahn, Dec 2016
%
% REFERENCES:
%    * Sims, Christopher A. "Solving linear rational expectations models."
%         Computational economics 20.1 (2002): 1-20.
%    * Ahn, SeHyoun, Greg Kaplan, Benjamin Moll, Thomas Winberry, and
%         Christian Wolf. "When Inequality Matters for Macro and Macro Matters
%         for Inequality."
%
% PARAMETERS:
%    Check Sims's paper for notation or Our Paper for notation
%
%    XXXXX DO NOT USE DISCRETE TIME VERSION. IT PROBABLY IS WRONG XXXXXX
%    The issue is noted in < https://github.com/gregkaplan/phact/issues/1 >
%
%    continuous = 1 for a continuous time problem (default)
%                0 for a discrete time problem
%
%    check_exist = 1 check for existence of solution (default)
%                 0 existence has been checked before
%    check_uniq = 0 do not check for uniqueness (default)
%                1 check for uniqueness
%    n_v = number of unstable roots
%
% OUPUTS:
%    eu(1) = 1 solution exists
%            0 no stable solution
%           -5 Flag for checking existence is off
%    eu(2) = 1 solution is unique
%            0 solution is not unique
%           -5 Flag for checking uniqueness is off
%
% SYNTAX:
% [G1,C,impact,eu,F]=schur_solver(g0,g1,c,psi,pi,continuous,check_exist,check_uniq,varargin)

%% Set Default Values %%
switch nargin
    case 8
        n_v = -1;
    case 7
        check_uniq = 0;
        n_v = -1;
    case 6
        check_uniq = 0;
        check_exist = 1;
        n_v = -1;
    case 5
        check_uniq = 0;
        check_exist = 1;
        continuous = 1;
        n_v = -1;
    case 1:4
        error('Insufficient Number of Input Variables');
end

realsmall = sqrt(eps)*10;
eu = [-5;-5];
n = size(g1,1);

%% Schur Decomposition
[U,T] = schur(full(g1),'real');
if continuous
    g_eigs = real(ordeig(T));
    nunstab = sum(g_eigs>0);
    if n_v > -1
        [aux,ind] = sort(g_eigs,'descend');
        locs = ones(n,1);
        locs(ind(1:n_v)) = 0;
        [U,T] = ordschur(U,T,locs);
        if nunstab > n_v
            warning('<schur_solver>: There are more than n_v number of positive eigenvalues with smallest values:');
            disp(aux(n_v+1:nunstab));
        elseif nunstab < n_v
            warning('<schur_solver>: There are less than n_v number of positive eigenvalues:');
            disp(aux(nunstab+1:n_v));
        end
        nunstab = n_v;
    else
        [U,T] = ordschur(U,T,'lhp');
    end
else
    g_eigs = abs(ordeig(T));
    nunstab = sum(g_eigs>1);
    if n_v > -1
        [aux,ind] = sort(g_eigs,'descend');
        locs = ones(n,1);
        locs(ind(1:n_v)) = 0;
        [U,T] = ordschur(U,T,locs);
        if nunstab > n_v
            warning('<schur_solver>: There are more than n_v number of positive eigenvalues with smallest values:');
            disp(aux(n_v+1:nunstab));
        elseif nunstab < n_v
            warning('<schur_solver>: There are less than n_v number of positive eigenvalues:');
            disp(aux(nunstab+1:n_v));
        end
        nunstab = n_v;
    else
	[U,T] = ordschru(U,T,'udi');
    end
end

u1 = U(:,1:n-nunstab)';
u2 = U(:,n-nunstab+1:n)';

etawt = u2*pi;
[ueta,deta,veta] = svd(etawt);
md = min(size(deta));
bigev = find(diag(deta(1:md,1:md))>realsmall);
ueta = ueta(:,bigev);
veta = veta(:,bigev);
deta = deta(bigev,bigev);

if check_exist
    zwt = u2*psi;
    [uz,dz,vz] = svd(zwt);
    md = min(size(dz));
    bigev = find(diag(dz(1:md,1:md))>realsmall);
    uz = uz(:,bigev);
    vz = vz(:,bigev);
    dz = dz(bigev,bigev);

    if isempty(bigev)
        eu(1) = 1;
    else
        eu(1) = (norm(uz-ueta*ueta'*uz,'fro') < realsmall*n);
    end

    if (~eu(1) && (n_v == -1))
        warning('<schur_solver>: Solution does not exist');
    end
    impact = real(-pi*veta*(deta\ueta')*uz*dz*vz'+psi);
else
    impact = real(-pi*veta*(deta\ueta')*u2*psi+psi);
end

G1 = U*T*spdiags([ones(n-nunstab,1);zeros(nunstab,1)],0,n,n)*U';
G1 = real(G1);

if check_uniq
    [~,deta1,veta1] = svd(u1*pi);
    md = min(size(deta1));
    bigev = find(diag(deta1(1:md,1:md))>realsmall);
    veta1 = veta1(:,bigev);
    if isempty(veta1)
        eu(2) = 1;
    else
        eu(2) = norm(veta1-veta*veta'*veta1,'fro')<realsmall*n;
    end
end
F = u1(:,1:nunstab)'*inv(u1(:,nunstab+1:end)');
impact = [F*psi(nunstab+1:end,:);psi(nunstab+1:end,:)];
C = c;  	% constant term is not coded yet


function parameters = connfreq_estimation(ns_ps,parameters)

% parameters = struct('n',2,'len',len_val,'ad',0.95,'as',0.8,'ap',0.2,'beta',0.8,'beta1',0.98,'gamma',0.998,'alpha',0.7,...
%             'pk',zeros(len_val,1),'noise_ps',ns_ps,'pxk_old',ns_ps,'pxk',ns_ps,'pnk_old',ns_ps,'pnk',ns_ps);

D = parameters.D;
len = parameters.len;
b = parameters.b;
V = parameters.V;
U = parameters.U;
gamma1 = parameters.gamma1;
gamma2 = parameters.gamma2;
alpha_max = parameters.alpha_max;
beta_min = parameters.beta_min;

alpha_c = parameters.alpha_c;
noise_ps = parameters.noise_ps;
Rmin_old = parameters.Rmin_old;
Pmin = parameters.Pmin;
Pmin_sw = parameters.Pmin_sw;
P = parameters.SmthdP;
u1 = parameters.u1;
j = parameters.j;
stored_min = parameters.stored_min;

P_noise_est = zeros(len,1);

%Spectral smoothing
%by equation(4)
% P_vector = zeros(2*D+1,1);
% P_y = ns_ps;
% for k = 1:len
%     if k>D&k<=(len - D)
%         for i = 1:2*D+1
%             P_vector(i) = ns_ps(k-D+i-1);%.^2;
%         end
%         P_y(k) = b*P_vector;
%     end
% end

P_y=smoothing(ns_ps,b,D);   % spectral smoothing according to Eq. 4

R = sum(P)/sum(ns_ps);
alpha_c_tild = 1/(1+(R-1)^2);
alpha_c = alpha_c*0.7 + 0.3*max(alpha_c_tild, 0.7);
alpha = (alpha_max*alpha_c)./((P./(noise_ps+eps) -1).^2 +1);

%
%Temporal smoothing
power_min = sum(Pmin);
power_noise_ps = sum(noise_ps); %Eq.(8)

%Pold=P; Pminold=Pmin; Pmin_swold=Pmin_sw;
%Decision=zeros(len,1);
% for k = 1:len
%     P(k) = alpha(k)*P(k) + (1-alpha(k))*P_y(k); %Eq.(5)
%     %Speech presence decision
%     if P(k)<Pmin(k)
%         Pmin(k) = P(k);
%     end
%     if P(k)<Pmin_sw(k)
%         Pmin_sw(k) = P(k);
%     end
%     if P(k)>gamma1*Pmin(k)
%         D_1(k) = 1;
%     else
%         D_1(k) = 0;
%     end
%
%     if P(k)>(Pmin(k) + gamma2*power_min/len)
%         D_2(k) = 1;
%     else
%         D_2(k) = 0;
%     end
%     Decision(k) = D_1(k)*D_2(k);
% end

Decision=zeros(len,1);
P=alpha.*P+(1-alpha).*P_y;
%Pmin2=Pminold;
Pmin=min(Pmin,P);
%Pmin_sw2=Pmin_swold;
Pmin_sw=min(Pmin_sw,P);
D_1a=zeros(len,1);
indx=find(P>gamma1*Pmin);
if ~isempty(indx),D_1a(indx)=1; end;
D_2a=zeros(len,1);
indx2=find(P>(Pmin+gamma2*power_min/len));
if ~isempty(indx2), D_2a(indx2)=1; end;
Decision=D_1a.*D_2a;

%Noise periodogram estimation
Rmin_tild = power_noise_ps/(power_min+eps); % Bias factor

if sum(Decision)>0
    Rmin = Rmin_old;
else
    Rmin = beta_min*Rmin_old + (1-beta_min)*Rmin_tild;  %Eq.(18)
end

% for k = 1:len
%     if Decision(k)==1
%         noise_ps(k) = Rmin*Pmin(k);
%     else
%         noise_ps(k) = ns_ps(k);%
%     end
% end

noise_ps=ns_ps;
indd=find(Decision==1);
if ~isempty(indd), noise_ps(indd)=Rmin*Pmin(indd);, end;

%Temporal minimum tracking
%use window to find the minimum
j = j+1;
if j==V
    stored_min(:,u1) = Pmin_sw;
    u1 = u1+1;
    if u1==U+1;
        u1=1;
    end
    Pmin = min(stored_min,[],2);
    Pmin_sw = P;
    j = 0;
end
%

parameters.alpha_c = alpha_c;
parameters.noise_ps = noise_ps;
parameters.Rmin_old = Rmin;
parameters.Pmin = Pmin;
parameters.Pmin_sw = Pmin_sw;
parameters.SmthdP = P;
parameters.u1 = u1;
parameters.j = j;
parameters.alpha = alpha;
parameters.stored_min = stored_min;
parameters.Decision = Decision;

% ----------------------------------------------
function y=smoothing (x,win,N);

len=length(x);
win1=win(1:N+1);
win2=win(N+2:2*N+1);
y1=filter(fliplr(win1),[1],x);

x2=zeros(len,1);
x2(1:len-N)=x(N+1:len);

y2=filter(fliplr(win2),[1],x2);

y=(y1+y2);


function  [m, bool] = maxEntConsVector(SInt, printLevel)
% USAGE:
%
%    [m, bool] = maxEntConsVector(SInt, printLevel)
%
% INPUTS:
%    SInt:          `m` x `n` matrix
%    printLevel:    verbose level
%
% OUTPUTS:
%    m:             primal solution
%    bool:          is 1 if `m > 0`
%

[mlt,nlt]=size(SInt);

d1=1e-4;
d2=1;

%pdco parameters
options = pdcoSet;
% options.FeaTol    = 1e-6; %6 March 2010 medium FeaTol
% options.OptTol    = 1e-6; %6 March 2010 medium OptTol
% options.MaxIter   = 200;
% options.Method    = 1;    % 1=Chol  2=QR (more reliable)
% options.mu0       = 0;  % 0 lets pdco decide.  1 or 10 assumes good scaling
% options.StepTol   = 0.9;
% options.StepSame  = 1;
% options.Print     = printLevel-1;
% options.wait      = 0;

x0=ones(mlt,1);
y0=ones(nlt,1);
z0=ones(mlt,1);
xsize=1;
zsize=1;

alpha=1e-6;
%    -----------------------------------------------------------------------
%    pdco.m: Primal-Dual Barrier Method for Convex Objectives (28 Apr 2012)
%    -----------------------------------------------------------------------
%            [x,y,z,inform,PDitns,CGitns,time] = ...
%       pdco(pdObj,pdMat,b,bl,bu,d1,d2,options,x0,y0,z0,xsize,zsize);
%
%     solves optimization problems of the form
%
%        minimize    phi(x) + 1/2 norm(D1*x)^2 + 1/2 norm(r)^2
%          x,r
%        subject to  A*x + D2*r = b,   bl <= x <= bu,   r unconstrained,

[x,y,z,inform,PDitns,CGitns,time] = ...
    pdco(@(x) pdcoObj(x,alpha),SInt',zeros(nlt,1),zeros(mlt,1),inf*ones(mlt,1),d1,d2,options,x0,y0,z0,xsize,zsize);

m=x;
%boolean indicating metabolites involved in the maximal consistent vector
bool=m>0;
end

function [obj,grad,Hess]=pdcoObj(x,alpha)
obj  = alpha*(x'*(log(x) -x));
grad = alpha*log(x);
Hess = alpha./x;
end


function [p, ptt] = rndtest(X, Y, B)
% Randomized (permutation) paired sample test

% AUTORIGHTS
% -------------------------------------------------------
% Copyright (C) 2009-2012 Ross Girshick
%
% This file is part of the voc-releaseX code
% (http://people.cs.uchicago.edu/~rbg/latent/)
% and is available under the terms of an MIT-like license
% provided in COPYING. Please retain this notice and
% COPYING if you use this file (or a portion of it) in
% your project.
% -------------------------------------------------------

if nargin < 3
  B = 100000;
end

Z0 = X - Y;
t0 = mean(Z0);
T = length(Z0);

t = mean(repmat(Z0, [1 B]) .* ((rand(T,B) < 0.5) * 2 - 1));

p = 1/B * sum(abs(t0) <= abs(t));

% For comparison:
% p-value from matlab's parametric t-test function
[~, ptt] = ttest(X, Y);


function [h, g] = ldpc_h2g(H,q)
% [h, g] = ldpc_h2g(H,q)
% converts tentative binary LDPC matrix H into a new matrix h
% (columns are permuted) and produces the generator matrix g
% H should be a sparse matrix in MATLAB format.
% q - Field base (power of 2) now only 2 4 8 1 32 64 128 and 256
%
% MEX file

%   Copyright (c) 1999 by Igor Kozintsev igor@ifp.uiuc.edu
%   $Revision: 1.1 $  $Date: 1999/08/23 $ - implementation for GFq


function [fc,core]=qtt_tucker_m(tt, sz, tol)
%Compute the QTT-Tucker representation of TT
%   [FC,CORE]=QTT_TUCKER_M(TT, SZ, TOL) Computes the QTT-Tucker
%   representation of TT-tensor. This is a technical function: use
%   corresponding constructors of the QTT-Tucker class
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

d = tt.d;
n = tt.n;
rcr = tt.r;

fc = cell(d,1);

% Initial orthogonalization - 1->d
for i=1:d-1
    cr = tt{i};
    cr = reshape(cr, rcr(i)*n(i), rcr(i+1));
    [cr, rv]=qr(cr, 0);
    cr2 = tt{i+1};
    cr2 = rv*reshape(cr2, rcr(i+1), n(i+1)*rcr(i+2));
    rcr(i+1) = size(cr,2);
    tt{i} = reshape(cr, rcr(i), n(i), rcr(i+1));
    tt{i+1} = reshape(cr2, rcr(i+1), n(i+1), rcr(i+2));
end;

% Split tucker factors from d->1
core = tt;
nextcr = core{d};
rtuck = zeros(d,1);
for i=d:-1:1
    cr = nextcr;
    cr = permute(cr, [2, 1, 3]);
    cr = reshape(cr, n(i), rcr(i)*rcr(i+1));
    [u,s,v]=svd(cr, 'econ');
    s = diag(s);
    nrm = norm(s);
    r = my_chop2(s, tol*nrm); % !!!!!!
    rtuck(i) = r;
    if (i>1)
        fc{i} = u(:,1:r);
        cr = diag(s(1:r))*v(:,1:r)';
        cr = reshape(cr, r, rcr(i), rcr(i+1));
        cr = permute(cr, [1, 3, 2]);
        cr = reshape(cr, r*rcr(i+1), rcr(i));
        [cr,rv] = qr(cr, 0);
        cr2 = core{i-1};
        cr2 = reshape(cr2, rcr(i-1)*n(i-1), rcr(i));
        cr2 = cr2*(rv.');
        rcr(i) = size(cr, 2);
        nextcr = reshape(cr2, rcr(i-1), n(i-1), rcr(i));
        cr = reshape(cr, r, rcr(i+1), rcr(i));
        core{i} = permute(cr, [3, 1, 2]);
    else
        cr = v(:,1:r)';
        cr = reshape(cr, r, rcr(i), rcr(i+1));
        core{i} = permute(cr, [2, 1, 3]);
        fc{i} = u(:,1:r)*diag(s(1:r));
    end;
end;

% Quantics approximation
for i=1:d
    %d0 = log2(n(i));
    fc{i} = tt_tensor(fc{i});
    szc=sz{i}; d0=numel(szc); m0=szc(end); szc=szc(:);
    %fc{i} = tt_reshape(fc{i}, [2*ones(1,d0-1), 2*rtuck(i)], tol);
    fc{i}=tt_reshape(fc{i},[szc(1:end-1); m0*rtuck(i)],tol);
    nocore = fc{i}{d0}; % rqtt, 2*rtuck, 1
    rqtt = size(nocore, 1);
    if (i<d)
        % Play with QRs: fc{i}->cr{i}->cr{i+1}->fc{i+1}
        nocore = reshape(nocore, rqtt*m0, rtuck(i));
        [ocore, nocore]=qr(nocore, 0);
        cr1 = core{i};
        cr1 = permute(cr1, [2, 1, 3]);
        cr1 = reshape(cr1, rtuck(i), rcr(i)*rcr(i+1));
        cr1 = nocore*cr1;
        rtuck(i) = size(cr1, 1);
        fc{i}{d0} = reshape(ocore, rqtt, m0, rtuck(i));

        cr1 = reshape(cr1, rtuck(i)*rcr(i), rcr(i+1));
        [cr1, rv]=qr(cr1, 0);
        cr2 = core{i+1};
        cr2 = reshape(cr2, rcr(i+1), rtuck(i+1)*rcr(i+2));
        cr2 = rv*cr2;
        rcr(i+1) = size(cr1, 2);
        cr1 = reshape(cr1, rtuck(i), rcr(i), rcr(i+1));
        core{i} = permute(cr1, [2,1,3]);
        cr2 = reshape(cr2, rcr(i+1), rtuck(i+1), rcr(i+2));
        cr2 = permute(cr2, [1, 3, 2]);
        cr2 = reshape(cr2, rcr(i+1)*rcr(i+2), rtuck(i+1));
        [cr2, rv]=qr(cr2, 0);
        fc{i+1} = fc{i+1}*(rv.');
        rtuck(i+1) = size(cr2, 2);
        cr2 = reshape(cr2, rcr(i+1), rcr(i+2), rtuck(i+1));
        core{i+1} = permute(cr2, [1, 3, 2]);
    else
        fc{i}{d0} = reshape(nocore, rqtt, m0, rtuck(i));
    end;
end;

end


function BoschettiEnc(img, name, bos_rateE, bos_rateRGB, nBit, tmo_operator)
%
%
%       BoschettiEnc(img, name, bos_rateE, bos_rateRGB, nBit)
%
%
%       Input:
%           -img: input HDR image
%           -name: is output name of the image. If img is empty
%                  an HDR image with filename 'name' is loaded
%           -bos_rateE: JPEG2000 compression rate for the E layer
%           -bos_rateRGB: JPEG2000 compression rate for the RGB layer
%           -nBit: number of bit of the encoding. The maximum is 16.
%           -tmo_operator: an handle to a tone mapping operator
%
%
%     Copyright (C) 2012  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if(~exist('name', 'var'))
    name = 'bosc_enc';
end

if(~exist('bos_rateRGB', 'var'))
    rateRGB = 15;
else
    rateRGB = bos_rateRGB;
end

if(~exist('bos_rateE', 'var'))
    rateE = 15;
else
    rateE = bos_rateE;
end

if(~exist('nBit','var'))
    nBit = 16;
end

if(~exist('tmo_operator','var'))
    tmo_operator = @ReinhardTMO;
end

%Tone mapping
imgTMO = tmo_operator(img);
%LDR Encoding
imgTMO = GammaTMO(imgTMO, 2.2, 0.0, 0);

%Quantization
maxVal = 2^nBit - 1;
imgTMO = round(imgTMO * maxVal) / maxVal;

%Computing E
epsilon = 1e-4;
epi = 1.0 / maxVal;
E = log2(img ./ (imgTMO + epi) + epsilon);
E = mean(E, 3);

%Encoding E
maxE = max(E(:));
minE = min(E(:));
Eq = (E - minE) / (maxE - minE);

%metadata string
metatadata = [num2str(nBit), ' ', num2str(maxE), ' ', num2str(minE)];

if(nBit == 16)
    Eq = uint16(Eq * maxVal);
end

imwrite(Eq,[name,'_bos_E.jp2'], 'Mode', 'lossy', 'CompressionRatio', rateE);

%Decoding E
EqDec = double(imread([name, '_bos_E.jp2'])) / maxVal;
EDec = EqDec * (maxE - minE) + minE;

%Computing RGB
RGB = zeros(size(img));
div = 2.^EDec;
for i=1:size(img, 3)
    RGB(:,:,i) = (img(:,:,i) ./ div);
end

%Encoding RGB
if(nBit == 16)
    RGB = uint16(RGB * maxVal);
end

nameOut = [name,'_bos_RGB.jp2'];
imwrite(RGB, nameOut, 'Mode', 'lossy', 'CompressionRatio', rateRGB, 'Comment', metatadata);
end


%MDL_FANUC10L  Create kinematic model of Fanuc AM120iB/10L robot
%
% MDL_FANUC10L is a script that creates the workspace variable R which
% describes the kinematic characteristics of a Fanuc AM120iB/10L robot
% using standard DH conventions.
%
% Also defines the workspace vector:
%   q0   mastering position.
%
% Notes::
% - SI units of metres are used.
%
% Author::
%  Wynand Swart,
%  Mega Robots CC, P/O Box 8412, Pretoria, 0001, South Africa,
%  wynand.swart@gmail.com
%
% See also mdl_irb140, mdl_m16, mdl_motomanHP6, mdl_puma560, SerialLink.

% MODEL: Fanuc, AM120iB/10L, 6DOF, standard_DH

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for Matlab (RTB).
%
% RTB 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.
%
% RTB 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 RTB.  If not, see <http://www.gnu.org/licenses/>.

%Cell: 073-1555-430
%30 Sep 2007
%Fanuc AM120iB/10L robot

%            theta    d      a    alpha
clear L
L(1) = Link([0        0     0.15  -pi/2   0  ]);
L(2) = Link([0        0     0.77   pi     0  ]);
L(3) = Link([0        0     0.1   -pi/2   0  ]);
L(4) = Link([0       -0.96  0      pi/2   0  ]);
L(5) = Link([0        0     0     -pi/2   0  ]);
L(6) = Link([0       -0.1   0      0      0  ]);
%##########################################################
%Pose 0; At MASTERING position;
%##########################################################
q0 =[0   -pi/2   0   0   0   0];
R=SerialLink(L, 'name', 'Fanuc AM120iB/10L');
%##########################################################


function test_bug2440

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_freqsimulation ft_freqanalysis ft_singleplotTFR

cfg = [];
cfg.method = 'broadband';
cfg.numtrl = 5;
cfg.trllen = 2;

data = ft_freqsimulation(cfg);

cfg = [];
cfg.method = 'mtmconvol';
cfg.taper = 'hanning';
cfg.foi = 8:12;
cfg.t_ftimwin = 4./cfg.foi;
cfg.toi = 0:0.1:2;
cfg.keeptrials = 'yes';
freq = ft_freqanalysis(cfg, data);

freq.logspctrm = log(freq.powspctrm);

% plot
cfg = [];
cfg.parameter = 'logspctrm';
cfg.trials = 2;
ft_singleplotTFR(cfg,freq);

end


function [Population] = EnvironmentalSelection(Population,N)
% Environmental selection

%------------------------------- Copyright --------------------------------
% Copyright 2017-2018 Yiping Liu
% Please contact {yiping0liu@gmail.com} if you have any problem.
%--------------------------------------------------------------------------

    %% Non-dominated sorting
    N = min(N,length(Population));
    [FrontNo,MaxFNo] = NDSort(Population.objs,Population.cons,N);
    Next = FrontNo < MaxFNo;

    %% Calculate the crowding distance of each solution
    CrowdDis = CrowdingDistance(Population.objs,FrontNo);

    %% Select the solutions in the last front based on their crowding distances
    Last     = find(FrontNo==MaxFNo);
    [~,Rank] = sort(CrowdDis(Last),'descend');
    Next(Last(Rank(1:N-sum(Next)))) = true;

    %% Population for next generation
    Population = Population(Next);
end


function innerpro = cinnerprodgeneral(x, y)
% Computes the Euclidean inner product between x and y in the complex case
%
% function innerpro = cinnerprodgeneral(x, y)
%
% The input x and y are numeric data structures which can be defined
% recursively by arrays, structs and cells. Each part of x and y should
% be a struct which contains the fields real and iamg which indicate
% the real and imaginary part of the stored complex numbers. The inner
% product between x and y is defined as sum(real(conj(x(:)).*y(:))).
% The return is the sum of the inner products over each part of x and y.
% In case that x and y are structs with different fields, the inner products
% are computed only for the common fields.
%
% Note: Operations between dlarrays containing complex numbers have been
% introduced in Matlab R2021b or later. This file is only useful for Matlab
% R2021a or earlier. It will be discarded when Matlab R2021b is stable.
%
% See also: innerprodgeneral, manoptADhelp

% This file is part of Manopt: www.manopt.org.
% Original author: Xiaowen Jiang, Aug. 31, 2021.
% Contributors: Nicolas Boumal
% Change log:

    if ~((isstruct(x) && isstruct(y)) || (iscell(x) && iscell(y))...,
            || (isnumeric(x) && isnumeric(y)) || (isstruct(x) && isnumeric(y)))

        up = MException('manopt:autodiff:cinnerprodgeneral' ,...
            'cinnerprodgeneral should only accept structs, cells or arrays.');
        throw(up);

    end
    % recursively compute the inner product
    if isstruct(x) && isstruct(y) && (~isfield(x,'real')) && (~isfield(y,'real'))
        innerpro  = cinnerprodgeneral_struct(x,y);
    elseif iscell(x) && iscell(y)
        innerpro = cinnerprodgeneral_cell(x,y);
    else
        xconj = cconj(x);
        product = cdottimes(xconj,y);
        innerpro = sum(creal(product),'all');
        % slower
        % xcol = cmat2col(x);
        % innerpro = creal(cprod(ctransp(xcol),xcol));
    end

    % struct case
    function innerpro = cinnerprodgeneral_struct(x,y)
        innerpro = 0;
        elemsx = fieldnames(x);
        elemsy = fieldnames(y);
        % find the common fields
        [elems,ix,iy] = intersect(elemsx,elemsy, 'stable');
        nelems = numel(elems);
        for ii = 1:nelems
            if isstruct(x.(elemsx{ix(ii)})) && (~isfield(x.(elemsx{ix(ii)}),'real'))...,
                    && (~isfield(y.(elemsy{iy(ii)}),'real'))
                innerpro = innerpro + cinnerprodgeneral_struct(...,
                    x.(elemsx{ix(ii)}),y.(elemsy{iy(ii)}));
            elseif iscell(x.(elemsx{ix(ii)}))
                innerpro = innerpro + cinnerprodgeneral_cell(...,
                    x.(elemsx{ix(ii)}),y.(elemsy{iy(ii)}));
            else
                xconj = cconj(x.(elemsx{ix(ii)}));
                product = cdottimes(xconj, y.(elemsy{iy(ii)}));
                innerpro = innerpro + sum(creal(product), 'all');
            end
        end
    end

    % cell case
    function innerpro = cinnerprodgeneral_cell(x,y)
        innerpro = 0;
        ncell = length(x);
        for ii = 1:ncell
            if isstruct(x{ii}) && (~isfield(x{ii},'real')) && (~isfield(y{ii},'real'))
                innerpro = innerpro + cinnerprodgeneral_struct(...,
                    x{ii},y{ii});
            elseif iscell(x{ii})
                innerpro = innerpro + cinnerprodgeneral_cell(...,
                    x{ii},y{ii});
            else
                xconj = cconj(x{ii});
                product = cdottimes(xconj, y{ii});
                innerpro = innerpro + sum(creal(product), 'all');
            end
        end
    end

end


function [A, Z, title, key, mtype] = RBfix (filename)
%RBFIX read a possibly corrupted matrix from a R/B file
% (assembled format only).  Usage:
%
% [A Z title key mtype] = RBfix (filename)
%
% The Rutherford/Boeing format stores a sparse matrix in a file in compressed-
% column form, using 3 arrays: Ap, Ai, and Ax.  The row indices of entries in
% A(:,j) are in Ai(p1:p2) and the corresponding numerical values are Ax(p1:p2),
% where p1 = Ap(j) and p2 = Ap(j+1)-1.  The row indices ought to be sorted, and
% no duplicates should appear, but this function ignores that requirement.
% Duplicate entries are summed if they exist, and A is returned with sorted
% columns.  Symmetric matrices are stored with just their lower triangular
% parts in the file.  Normally, it is an error if entries are present in the
% upper triangular part of a matrix that is declared in the file to be
% symmetric.  This function simply ignores those entries.
%
% If CHOLMOD is installed, this function is faster and uses less memory.
%
% Example:
%
%   load west0479
%   RBwrite ('mywest', west0479, [ ], 'My west0479 file', 'west0479') ;
%   [A Z title key mtype] = RBfix ('mywest') ;
%   isequal (A, west0479)
%   title, key, mtype
%
% See also mread, RBread, RBwrite, RBreade, sparse2.

% Optionally uses the CHOLMOD sparse2 mexFunction.

% Copyright 2007, Timothy A. Davis

%-------------------------------------------------------------------------------
% read in the raw contents of the Rutherford/Boeing file
%-------------------------------------------------------------------------------

[mtype Ap Ai Ax title key nrow] = RBraw (filename) ;
mtype = lower (mtype) ;

%-------------------------------------------------------------------------------
% determine dimension, number of entries, and convert numerical entries
%-------------------------------------------------------------------------------

% number of columns
ncol = length (Ap) - 1 ;

% number of entries
nz = length (Ai) ;

% check column pointers
if (any (Ap ~= sort (Ap)) | (Ap (1) ~= 1) | (Ap (ncol+1) - 1 ~= nz))	    %#ok
    error ('invalid column pointers') ;
end

% check row indices
if ((double (max (Ai)) > nrow) | double (min (Ai)) < 1)			    %#ok
    error ('invalid row indices') ;
end

% Ax can be empty, for a p*a matrix
if (~isempty (Ax))
    if (mtype (1) == 'c')
	% Ax is real, with real/imaginary parts interleaved
	if (2 * nz ~= length (Ax))
	    error ('invalid matrix') ;
	end
	Ax = Ax (1:2:end) + (1i * Ax (2:2:end)) ;
    elseif (mtype (1) == 'i')
	Ax = double (Ax) ;
    end
    % numerical values must be of the right size
    if (nz ~= length (Ax))
	error ('invalid matrix') ;
    end
end

%-------------------------------------------------------------------------------
% create the triplet form
%-------------------------------------------------------------------------------

% construct column indices
Aj = zeros (nz,1) ;
for j = 1:ncol
    p1 = Ap (j) ;
    p2 = Ap (j+1) - 1 ;
    Aj (p1:p2) = j ;
end

%-------------------------------------------------------------------------------
% create the sparse matrix form
%-------------------------------------------------------------------------------

if (exist ('sparse2') == 3)						    %#ok
    % Use sparse2 in CHOLMOD.  It's faster, allows integer Ai and Aj, and
    % returns the Z matrix as the 2nd output argument.
    if (isempty (Ax))
	Ax = 1 ;
    end
    % numerical matrix
    [A Z] = sparse2 (Ai, Aj, Ax, nrow, ncol) ;
else
    % stick with MATLAB, without CHOLMOD.  This is slower and takes more memory.
    Ai = double (Ai) ;
    Aj = double (Aj) ;
    if (isempty (Ax))
	% pattern-only matrix
	A = spones (sparse (Ai, Aj, 1, nrow, ncol)) ;
	Z = sparse (nrow, ncol) ;
    else
	% numerical matrix
	A = sparse (Ai, Aj, Ax, nrow, ncol) ;
	% determine the pattern of explicit zero entries
	S = spones (sparse (Ai, Aj, 1, nrow, ncol)) ;
	Z = S - spones (A) ;
    end
end

% check for entries in upper part
if (any (mtype (2) == 'shz') & nnz (triu (A,1) > 0))			    %#ok
    fprintf ('entries in upper triangular part of %s matrix ignored\n', mtype);
end

% add the upper triangular part
if (mtype (2) == 's')
    A = A + tril (A,-1).' ;
    Z = Z + tril (Z,-1)' ;
elseif (mtype (2) == 'h')
    A = A + tril (A,-1)' ;
    Z = Z + tril (Z,-1)' ;
elseif (mtype (2) == 'z')
    A = A - tril (A,-1).' ;
    Z = Z + tril (Z,-1)' ;
end


function [x,r,g,info] = spg_bp(A,b,varargin )
%SPG_BP  Solve the basis pursuit (BP) problem
%
%   SPG_BP is designed to solve the basis pursuit problem
%
%   (BP)  minimize  ||X||_1  subject to  AX = B,
%
%   where A is an M-by-N matrix, B is an M-vector, and SIGMA is a
%   nonnegative scalar.  In all cases below, A can be an explicit M-by-N
%   matrix or matrix-like object for which the operations  A*x  and  A'*y
%   are defined (i.e., matrix-vector multiplication with A and its
%   adjoint.)
%
%   Also, A can be a function handle that points to a function with the
%   signature
%
%   v = A(w,mode)   which returns  v = A *w  if mode == 1;
%                                  v = A'*w  if mode == 2.
%
%   X = SPG_BP(A,B) solves the BP problem.
%
%   X = SPG_BP(A,B,OPTIONS) specifies options that are set using
%   SPGSETPARMS.
%
%   [X,R,G,INFO] = SPG_BP(A,B,OPTIONS) additionally returns the
%   residual R = B - A*X (which should be small), the objective gradient G
%   = A'*R, and an INFO structure.  (See SPGL1 for a description of this
%   last output argument.)
%
%   See also spgl1, spgSetParms, spg_bpdn, spg_lasso.

%   Copyright 2008, Ewout van den Berg and Michael P. Friedlander
%   http://www.cs.ubc.ca/labs/scl/spgl1
%   $Id: spg_bp.m 1074 2008-08-19 05:24:28Z ewout78 $

if ~exist('b','var') || isempty(b)
    error('Second argument cannot be empty.');
end
if ~exist('A','var') || isempty(A)
    error('First argument cannot be empty.');
end

sigma = 0;
tau = 0;
x0  = [];
[x,r,g,info] = spgl1(A,b,tau,sigma,x0,varargin{:});


function tcpWithBoost = seanWalshTCP(paramS,doseBinsC,volHistC)
% Sean Walsh TCP model for prostate
% http://dx.doi.org/10.1118/1.4939260
%
% APA, 07/20/2016
% AI, 09/12/16 Modified for use with outcomeModelsGUI

% Get parameters
alpha = 0.25; %paramS.alpha.val;
beta = 0.1008;% paramS.beta.val;
sigmaAlpha = alpha*11.3/100;
sigmaBeta = beta*12.9/100;

n = paramS.numFractions.val;                      % number of fractions

DILparS = paramS.structures.DIL;
dilVolume = DILparS.dilVolume.val;                % percent
pDIL = DILparS.pDIL.val;                          % DIL density
dDIL = DILparS.dDIL.val;                          % dose per fraction to the DIL

PTVparS = paramS.structures.PTV_1;
hypoxicFraction = PTVparS.hypoxicFraction.val;    % percent
OER = PTVparS.OER.val;                            % Oxygen Enhancement Ratio
pCTV = PTVparS.pCTV.val;                          % Prostate density
ctvVolume = PTVparS.ctvVolume.val;                % 36 cm^3 for intermediate risk
                                                  % 72 cm^3 for high risk pts
numSimulations = PTVparS.numSimulations.val;      % number of (alpha,beta) simulations

%Compute mean dose to prostate
prost = calc_meanDose(doseBinsC{2},volHistC{2},1);
%Dose per fraction to the prostate
dProst = prost/n;

% Compute TCP
%Seed the random generator
rng(now,'twister');

%Generate normally distributed, positive aplha and beta
alphaV = alpha + randn(numSimulations*2, 1) * sigmaAlpha;
betaV = beta + randn(numSimulations*2, 1) * sigmaBeta;
alphaV = alphaV(alphaV > 0);
betaV = betaV(betaV > 0);
alphaV = alphaV(1:numSimulations);
betaV = betaV(1:numSimulations);

%Hypoxic cells alpha,beta
alphaPO2 = alphaV / OER;
betaPO2 = betaV / OER^2;

% Total initial clonogen number
N0 = ((100-dilVolume)*pCTV + dilVolume*pDIL) / 100 * ctvVolume;

%Initial number of clonogens in prostate
Nprost = (100-dilVolume)*pCTV / 100 * ctvVolume;

%Initial number of clonogens in DIL
Ndil = dilVolume*pDIL / 100 * ctvVolume;

%Surviving fraction for Prostate
SProstateV = exp(-alphaV*n*dProst -betaV*n*dProst^2);

%Surviving fraction for hypoxic Prostate
ShypoxicProstateV = exp(-alphaPO2*n*dProst -betaPO2*n*dProst^2);

%Surviving fraction for DIL
SdilV = exp(-alphaV*n*dDIL -betaV*n*dDIL^2);

%Surviving fraction for hypoxic DIL
ShypoxicDilV = exp(-alphaPO2*n*dDIL -betaPO2*n*dDIL^2);

%Total surviving clonogens
NumSurvivingV = Nprost*(1-hypoxicFraction)*SProstateV + ...
    Nprost*hypoxicFraction*ShypoxicProstateV + ...
    Ndil*(1-hypoxicFraction)*SdilV +  ...
    Ndil*hypoxicFraction*ShypoxicDilV;

% TCP
TCPv = exp(-NumSurvivingV);

%Record the TCP for this DIL dose
tcpWithBoost = mean(TCPv);

end


function [ n, x, indx ] = i4vec_index_delete_dupes ( n, x, indx )

%*****************************************************************************80
%
%% I4VEC_INDEX_DELETE_DUPES deletes duplicates from an indexed sorted I4VEC.
%
%  Discussion:
%
%    The output quantities N2, X2, and INDX2 are computed from the
%    input quantities by sorting, and eliminating duplicates.
%
%    The output arrays should be dimensioned of size N, unless the user
%    knows in advance what the value of N2 will be.
%
%    The output arrays may be identified with the input arrays.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the size of the input list.
%
%    Input, integer X(N), the list.
%
%    Input, integer INDX(N), the sort index of the list.
%
%    Output, integer N, the number of unique entries in X.
%
%    Output, integer X(N), a copy of the list which has
%    been sorted, and made unique.
%
%    Output, integer INDX(N), the sort index of the new list.
%
  i = 0;
  n3 = 0;

  while ( 1 )

    i = i + 1;

    if ( n < i )
      break
    end

    if ( 1 < i )
      if ( x(indx(i)) == x3(n3) )
        continue
      end
    end

    n3 = n3 + 1;
    x3(n3) = x(indx(i));

  end
%
%  Copy data into output arrays.
%
  n = n3;
  x(1:n) = x3(1:n3);
  indx = i4vec_indicator1 ( n );

  return
end


function kernkernVardistTest(kernType1, kernType2, numData, numIn)

% KERNKERNVARDISTTEST Description

% VARGPLVM

x = randn(numData, numIn);
x2 = randn(numData/2, numIn);

kern1 = kernCreate(x,kernType1);
kern2 = kernCreate(x,kernType2);

params1 = 0.2*randn(1,kern1.nParams)./sqrt(randn(1,kern1.nParams).^2);
kern1 = kernExpandParam(kern1, params1);

params2 = 0.2*randn(1,kern2.nParams)./sqrt(randn(1,kern2.nParams).^2);
kern2 = kernExpandParam(kern2, params2);

vardist = vardistCreate(x, numIn, 'gaussian');

params = randn(1,(numData*numIn*2));
params(1:numData*numIn) = x(:)';
vardist = vardistExpandParam(vardist, params);

Psi2 = kernkernVardistPsi2Compute(kern1, kern2, vardist, x2);
covGrad = ones(size(Psi2));
[gKern1, gKern2, gVarmeans, gVarcovars, gInd] = kernkernVardistPsi2Gradient(kern1, kern2, vardist, x2, covGrad);
%

epsilon = 1e-6;
paramskern1 = kernExtractParam(kern1);
paramskern2 = kernExtractParam(kern2);
paramsvar = vardistExtractParam(vardist);
%xx2 = x2';
params = [paramskern1 paramskern2 paramsvar x2(:)'];
origParams = params;

fprintf('Kernel hyperparameters\n');
if strcmp(kern1.type,'rbfard2')
fprintf('var: %2.6g max and min inputscale: %2.6g, %2.6g.\n',kern1.variance,max(kern1.inputScales),min(kern1.inputScales));
elseif strcmp(kern1.type,'linard2')
fprintf('max and min inputscale: %2.6g, %2.6g.\n',max(kern1.inputScales),min(kern1.inputScales));
else
  % do nothing
end

fprintf('----- Psi2 term ------- \n');
E = eig(Psi2);
fprintf('max and min eigenvalue of Psi2: %2.6g, %2.6g.\n',max(E),min(E));
origParams = params;
for i = 1:length(params);
  params = origParams;
  params(i) = origParams(i) + epsilon;
  kern1 = kernExpandParam(kern1, params(1:kern1.nParams));
  kern2 = kernExpandParam(kern2, params(kern1.nParams+1:kern1.nParams+kern2.nParams));

  vardist = vardistExpandParam(vardist, params(kern1.nParams+kern2.nParams+1:kern1.nParams+kern2.nParams+vardist.nParams));
  xx2 = params(kern1.nParams+kern2.nParams+vardist.nParams+1:end);
  x2 = reshape(xx2,[numData/2 numIn]);
  Lplus(i) = full(sum(sum(kernkernVardistPsi2Compute(kern1, kern2, vardist, x2))));
  params(i) = origParams(i) - epsilon;

  kern1 = kernExpandParam(kern1, params(1:kern1.nParams));
  kern2 = kernExpandParam(kern2, params(kern1.nParams+1:kern1.nParams+kern2.nParams));
  vardist = vardistExpandParam(vardist, params(kern1.nParams+kern2.nParams+1:kern1.nParams+kern2.nParams+vardist.nParams));
  xx2 = params(kern1.nParams+kern2.nParams+vardist.nParams+1:end);
  x2 = reshape(xx2,[numData/2 numIn]);
  Lminus(i) = full(sum(sum(kernkernVardistPsi2Compute(kern1, kern2, vardist, x2))));
end
params = origParams;
gLDiff = .5*(Lplus - Lminus)/epsilon;
g = [gKern1 gKern2 gVarmeans gVarcovars gInd];
% check firstly the kernel hyperparameters
kerndiff1 = abs(g(1:kern1.nParams) - gLDiff(1:kern1.nParams));
[g(1:kern1.nParams); gLDiff(1:kern1.nParams)]
pause

index = [kern1.nParams+1:kern1.nParams+kern2.nParams];
kerndiff2 = abs(g(kern1.nParams+1:kern1.nParams+kern2.nParams) ...
            - gLDiff(kern1.nParams+1:kern1.nParams+kern2.nParams));
[g(kern1.nParams+1:kern1.nParams+kern2.nParams); gLDiff(kern1.nParams+1:kern1.nParams+kern2.nParams)]
pause

index = [kern1.nParams+kern2.nParams+1:kern1.nParams+kern2.nParams+(vardist.nParams/2)];
varmeansdiff =  abs(g(index) - gLDiff(index));
[g(index); gLDiff(index)]
pause

index = [kern1.nParams+kern2.nParams+(vardist.nParams/2)+1:kern1.nParams+kern2.nParams+vardist.nParams];
varcovarsdiff =  abs(g(index) - gLDiff(index));
[g(index); gLDiff(index)]
pause
index = [(kern1.nParams+kern2.nParams+vardist.nParams+1):size(g,2)];
varinddiff =  abs(g(index) - gLDiff(index));
[g(index); gLDiff(index)]
pause
fprintf('Kernel1 hyps max diff: %2.6g.\n', max(kerndiff1));
fprintf('Kernel2 hyps max diff: %2.6g.\n', max(kerndiff2));
fprintf('Variational means max diff: %2.6g.\n', max(varmeansdiff));
fprintf('Variational covars max diff: %2.6g.\n', max(varcovarsdiff));
fprintf('Inducing inputs max diff: %2.6g.\n', max(varinddiff));


classdef stressTensor < tensor

  methods
    function sT = stressTensor(varargin)

      sT = sT@tensor(varargin{:},'rank',2);
      if ~sT.isSymmetric, warning('Tensor is not symmetric!'); end

    end
  end

  methods (Static = true)

    function sigma = load(varargin)
      T = load@tensor(varargin{:});
      sigma = stressTensor(T);
    end

    function sigma = uniaxial(v)
      % define uniaxial stress tensor
      %
      % Syntax
      %   sigma = stressTensor.uniaxial(v)
      %
      % Input
      %  v - @vector3d loading direction
      %
      % Output
      %  sigma - @stressTensor
      %

      sigma = stressTensor(dyad(v,2));

    end

    function sigma = rand(varargin)
      t = tensor.rand(varargin{:},'rank',2);
      sigma = stressTensor(t.sym);
    end

  end
end


function means = gtmlmean(net, data)
%GTMLMEAN Mean responsibility for data in a GTM.
%
%	Description
%	 MEANS = GTMLMEAN(NET, DATA) takes a GTM structure NET, and computes
%	the means of the responsibility  distributions for each data point in
%	DATA.
%
%	See also
%	GTM, GTMPOST, GTMLMODE
%

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

% Check for consistency
errstring = consist(net, 'gtm', data);
if ~isempty(errstring)
  error(errstring);
end

R = gtmpost(net, data);
means = R*net.X;


%MDL_ONELINK Create model of a simple 1-link mechanism
%
% MDL_ONELINK is a script that creates the workspace variable tl which
% describes the kinematic and dynamic characteristics of a simple planar
% 1-link mechanism.
%
% Also defines the vector:
%   qz   corresponds to the zero joint angle configuration.
%
% Notes::
% - SI units are used.
% - It is a planar mechanism operating in the XY (horizontal) plane and is
%   therefore not affected by gravity.
% - Assume unit length links with all mass (unity) concentrated at the joints.
%
% References::
%  - Based on Fig 3-6 (p73) of Spong and Vidyasagar (1st edition).
%
% See also SerialLink, mdl_twolink, mdl_planar1.

% MODEL: generic, planar, 1DOF, standard_DH

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB 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.
%
% RTB 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 RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

a1 = 1;

onelink = SerialLink([
          Revolute('d', 0, 'a', a1, 'alpha', 0, 'standard')
    ], ...
    'name', 'one link');
qz = [0];
qn = [pi/6];


function [ y, m, d, ierror ] = ymd_check_julian ( y, m, d )

%*****************************************************************************80
%
%% YMD_CHECK_JULIAN checks a Julian YMD date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 December 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, integer M, integer D, the YMD date.
%
%    Output, integer Y, integer M, integer D, the YMD date, which may
%    be corrected if necessary and possible.
%
%    Output, integer IERROR, is 0 if the date is legal.
%

%
%  Check the month.
%
  [ y, m, ierror ] = ym_check_julian ( y, m );

  if ( ierror ~= 0 )
    return
  end
%
%  Check the day.
%
  [ y, m, d ] = day_borrow_julian ( y, m, d );

  [ y, m, d ] = day_carry_julian ( y, m, d );

  return
end


function []=panel5disp(n,N,m,p,k,T,d1,d2,d3,d4,d5,Ymat,Xdot,Units,endo,exo,const,Xi,theta_gibbs,theta_median,theta_std,theta_lbound,theta_ubound,sigma_gibbs,sigma_median,D_estimates,gamma_estimates,alpha0,delta0,startdate,enddate,forecast_record,forecast_estimates,Fcperiods,stringdates3,Fstartdate,Fcenddate,Feval,Fcomp,data_endo_c,data_endo_c_lags,data_exo_c,It,Bu,IRF,IRFt,pref,names)

% recover a point estimate (the median) of the VAR coefficients
betatilde=Xi*theta_median;
Btilde=reshape(betatilde,k,N*n);

% check whether the model is stationary
[stationary,eigmodulus]=bear.checkstable(betatilde,N*n,p,k);

% estimate the in-sample evaluation criteria

% obtain a point estimate thetatilde of the structural factors, which is the median
thetatilde=theta_median;
% compute fitted values
Ytilde=full(Xdot*kron(speye(T),thetatilde))';
% reshape for convenience
Ytilde=reshape(Ytilde,T,n,N);
Ymat=reshape(Ymat,T,n,N);

% loop over units
for ii=1:N
% estimate the residuals for this unit
EPS(:,:,ii)=Ymat(:,:,ii)-Ytilde(:,:,ii);

% Compute then the sum of squared residuals
% compute first the RSS matrix, defined in (1.9.5)
RSS(:,:,ii)=EPS(:,:,ii)'*EPS(:,:,ii);
% retain only the diagonal elements to get the vector of RSSi values
rss(:,:,ii)=diag(RSS(:,:,ii));

% Go on calculating R2
% generate Mbar
Mbar=eye(T)-ones(T,T)/T;
% then compute the TSS matrix, defined in (1.9.8)
TSS(:,:,ii)=Ymat(:,:,ii)'*Mbar*Ymat(:,:,ii);
% generate the R2 matrix in (1.9.9)
R2(:,:,ii)=eye(n)-RSS(:,:,ii)./TSS(:,:,ii);
% retain only the diagonal elements to get the vector of R2 values
r2(:,:,ii)=diag(R2(:,:,ii));

% then calculate the adjusted R2, using (1.9.11)
R2bar(:,:,ii)=eye(n)-((T-1)/(T-k))*(eye(n)-R2(:,:,ii));
% retain only the diagonal elements to get the vector of R2bar values
r2bar(:,:,ii)=diag(R2bar(:,:,ii));

end

% estimate the forecast evaluation criteria
% first note that forecast evaluation can only be conducted if it is activated, and if there is some observable data after the beginning of the forecast
if Feval==1 && Fcomp==1

% generate the elements required for the evaluation
beta_gibbs=Xi*theta_gibbs;
forecast_record=reshape(forecast_record,N*n,1);
forecast_estimates=reshape(forecast_estimates,N*n,1);
data_endo_c=reshape(data_endo_c,Fcperiods,N*n);
data_endo_c_lags=reshape(data_endo_c_lags,p,N*n);

% compute forecast evaluation
[RMSE,MAE,MAPE,Ustat,CRPS_estimates,S1_estimates,S2_estimates]=bear.panelfeval(N*n,p,k,beta_gibbs,sigma_gibbs,forecast_record,forecast_estimates,Fcperiods,data_endo_c,data_endo_c_lags,data_exo_c,const,It,Bu);

end

% start displaying and saving the general results

% preliminary task: create and open the txt file used to save the results

filelocation=fullfile(pref.results_path, [pref.results_sub '.txt']);
fid=fopen(filelocation,'wt');

% print toolbox header

fprintf('%s\n','');
fprintf(fid,'%s\n','');

% print the list of contributors
bear.printcontributors(fid);

% print then estimation results

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

toolboxinfo='BEAR toolbox estimates';
fprintf('%s\n',toolboxinfo);
fprintf(fid,'%s\n',toolboxinfo);

time=clock;
datestring=datestr(time);
dateinfo=['Date: ' datestring(1,1:11) '   Time: ' datestring(1,13:17)];
fprintf('%s\n',dateinfo);
fprintf(fid,'%s\n',dateinfo);

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

VARtypeinfo='Panel VAR: structural factor (static)';
fprintf('%s\n',VARtypeinfo);
fprintf(fid,'%s\n',VARtypeinfo);

if IRFt==1
SVARinfo='structural decomposition: none';
fprintf('%s\n',SVARinfo);
fprintf(fid,'%s\n',SVARinfo);
elseif IRFt==2
SVARinfo='structural decomposition: choleski factorisation';
fprintf('%s\n',SVARinfo);
fprintf(fid,'%s\n',SVARinfo);
elseif IRFt==3
SVARinfo='structural decomposition: triangular factorisation';
fprintf('%s\n',SVARinfo);
fprintf(fid,'%s\n',SVARinfo);
end

temp='units: ';
for ii=1:N
temp=[temp ' ' Units{ii,1} ' '];
end
unitinfo=temp;
fprintf('%s\n',unitinfo);
fprintf(fid,'%s\n',unitinfo);

temp='endogenous variables: ';
for ii=1:n
temp=[temp ' ' endo{ii,1} ' '];
end
endoinfo=temp;
fprintf('%s\n',endoinfo);
fprintf(fid,'%s\n',endoinfo);

temp='exogenous variables: ';
if const==0 && m==0
temp=[temp ' none'];
elseif const==1 && m==1
temp=[temp ' constant '];
elseif const==0 && m>0
   for ii=1:m-1
   temp=[temp ' ' exo{ii,1} ' '];
   end
elseif const==1 && m>1
temp=[temp ' constant '];
   for ii=1:m-1
   temp=[temp ' ' exo{ii,1} ' '];
   end
end
exoinfo=temp;
fprintf('%s\n',exoinfo);
fprintf(fid,'%s\n',exoinfo);

sampledateinfo=['estimation sample: ' startdate '-' enddate];
fprintf('%s\n',sampledateinfo);
fprintf(fid,'%s\n',sampledateinfo);

samplelengthinfo=['sample size (omitting initial conditions): ' num2str(T)];
fprintf('%s\n',samplelengthinfo);
fprintf(fid,'%s\n',samplelengthinfo);

laginfo=['number of lags included in regression: ' num2str(p)];
fprintf('%s\n',laginfo);
fprintf(fid,'%s\n',laginfo);

hyperparam1='hyperparameters:';
fprintf('%s\n',hyperparam1);
fprintf(fid,'%s\n',hyperparam1);

hyperparam2=['IG shape on residual variance (alpha0):       ' num2str(alpha0)];
fprintf('%s\n',hyperparam2);
fprintf(fid,'%s\n',hyperparam2);

hyperparam3=['IG scale on residual variance (delta0):       ' num2str(delta0)];
fprintf('%s\n',hyperparam3);
fprintf(fid,'%s\n',hyperparam3);

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% display factor estimates

factorinfo=['Structural factors:'];
fprintf('%s\n',factorinfo);
fprintf(fid,'%s\n',factorinfo);

fprintf('%s\n','');
fprintf(fid,'%s\n','');

factorheader=fprintf('%35s %15s %15s %15s %15s\n','','Median','St.dev','Low.bound','Upp.bound');
factorheader=fprintf(fid,'%35s %15s %15s %15s %15s\n','','Median','St.dev','Low.bound','Upp.bound');

% common component

fprintf('%s\n','theta1 (common component)');
fprintf(fid,'%s\n','theta1 (common component)');
values=[theta_median(1,1) theta_std(1,1) theta_lbound(1,1) theta_ubound(1,1)];
fprintf('%-35s %15.3f %15.3f %15.3f %15.3f\n','common component',values);
fprintf(fid,'%-35s %15.3f %15.3f %15.3f %15.3f\n','common component',values);
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% unit component

fprintf('%s\n','theta2 (unit-specific component)');
fprintf(fid,'%s\n','theta2 (unit component)');
for ii=1:d2
values=[theta_median(d1+ii,1) theta_std(d1+ii,1) theta_lbound(d1+ii,1) theta_ubound(d1+ii,1)];
fprintf('%-35s %15.3f %15.3f %15.3f %15.3f\n',['unit ' int2str(ii) ' component'],values);
fprintf(fid,'%-35s %15.3f %15.3f %15.3f %15.3f\n',['unit ' int2str(ii) ' component'],values);
end
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% variable component

fprintf('%s\n','theta3 (variable-specific component)');
fprintf(fid,'%s\n','theta3 (endogenous variable component)');
for ii=1:d3
values=[theta_median(d1+d2+ii,1) theta_std(d1+d2+ii,1) theta_lbound(d1+d2+ii,1) theta_ubound(d1+d2+ii,1)];
fprintf('%-35s %15.3f %15.3f %15.3f %15.3f\n',['variable ' int2str(ii) ' component'],values);
fprintf(fid,'%-35s %15.3f %15.3f %15.3f %15.3f\n',['variable ' int2str(ii) ' component'],values);
end
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% lag component (if applicable)

if d4~=0
fprintf('%s\n','theta4 (lag-specific component)');
fprintf(fid,'%s\n','theta4 (lag component)');
for ii=1:d4
values=[theta_median(d1+d2+d3+ii,1) theta_std(d1+d2+d3+ii,1) theta_lbound(d1+d2+d3+ii,1) theta_ubound(d1+d2+d3+ii,1)];
fprintf('%-35s %15.3f %15.3f %15.3f %15.3f\n',['lag ' int2str(ii) ' component'],values);
fprintf(fid,'%-35s %15.3f %15.3f %15.3f %15.3f\n',['lag ' int2str(ii) ' component'],values);
end
fprintf('%s\n','');
fprintf(fid,'%s\n','');
end

% exogenous component (if applicable)

if d5~=0
fprintf('%s\n','theta5 (exogenous variable component)');
fprintf(fid,'%s\n','theta5 (exogenous component)');
% initiate equation count
eqcount=0;
% initiate exogenous count
exocount=0;
for ii=1:d5
   if exocount==m
   exocount=0;
   end
   if exocount==0
   eqcount=eqcount+1;
   end
exocount=exocount+1;
values=[theta_median(d1+d2+d3+d4+ii,1) theta_std(d1+d2+d3+d4+ii,1) theta_lbound(d1+d2+d3+d4+ii,1) theta_ubound(d1+d2+d3+d4+ii,1)];
fprintf('%-35s %15.3f %15.3f %15.3f %15.3f\n',['equation ' int2str(eqcount) ', exogenous ' int2str(exocount) ' component'],values);
fprintf(fid,'%-35s %15.3f %15.3f %15.3f %15.3f\n',['equation ' int2str(eqcount) ', exogenous ' int2str(exocount) ' component'],values);
end
fprintf('%s\n','');
fprintf(fid,'%s\n','');
end

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% display VAR stability results
eigmodulus=reshape(eigmodulus,p,N*n);
stabilityinfo1=['Roots of the characteristic polynomial (modulus):'];
fprintf('%s\n',stabilityinfo1);
fprintf(fid,'%s\n',stabilityinfo1);
for jj=1:p
temp=num2str(eigmodulus(jj,1),'%.3f');
   for kk=2:N*n
   temp=[temp,'  ',num2str(eigmodulus(jj,kk),'%.3f')];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end
if stationary==1;
stabilityinfo2=['No root lies outside the unit circle.'];
stabilityinfo3=['The estimated VAR model satisfies the stability condition'];
fprintf('%s\n',stabilityinfo2);
fprintf(fid,'%s\n',stabilityinfo2);
fprintf('%s\n',stabilityinfo3);
fprintf(fid,'%s\n',stabilityinfo3);
else
stabilityinfo2=['Warning: at leat one root lies on or outside the unit circle.'];
stabilityinfo3=['The estimated VAR model will not be stable'];
end

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% display posterior for sigma
% reshape sigma
sigma_median=reshape(sigma_median,N*n,N*n);
% start displaying
sigmainfo=['sigma (residual covariance matrix): posterior estimates'];
fprintf('%s\n',sigmainfo);
fprintf(fid,'%s\n',sigmainfo);
% calculate the (integer) length of the largest number in sigma, for formatting purpose
width=length(sprintf('%d',floor(max(abs(bear.vec(sigma_median))))));
% add a separator, a potential minus sign, and three digits (total=5) to obtain the total space for each entry in the matrix
width=width+5;
for ii=1:N*n
temp=[];
   for jj=1:N*n
   % convert matrix entry into string
   number=num2str(sigma_median(ii,jj),'% .3f');
      % pad potential missing blanks
      while numel(number)<width
      number=[' ' number];
      end
   number=[number '  '];
   temp=[temp number];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end

fprintf('%s\n','');
fprintf(fid,'%s\n','');

% then display the results for D and gamma, if a structural decomposition was selected

if IRF==1 && IRFt~=1
svarinfo1=['D (structural decomposition matrix): posterior estimates'];
fprintf('%s\n',svarinfo1);
fprintf(fid,'%s\n',svarinfo1);

% recover D
D=reshape(D_estimates,N*n,N*n);
% calculate the (integer) length of the largest number in D, for formatting purpose
width=length(sprintf('%d',floor(max(abs(bear.vec(D))))));
% add a separator, a potential minus sign and three digits (total=5) to obtain the total space for each entry in the matrix
width=width+5;
for ii=1:N*n
temp=[];
   for jj=1:N*n
   % convert matrix entry into string
   number=num2str(D(ii,jj),'% .3f');
      % pad potential missing blanks
      while numel(number)<width
      number=[' ' number];
      end
   number=[number '  '];
   temp=[temp number];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end

fprintf('%s\n','');
fprintf(fid,'%s\n','');

svarinfo2=['gamma (structural disturbances covariance matrix): posterior estimates'];
fprintf('%s\n',svarinfo2);
fprintf(fid,'%s\n',svarinfo2);

% recover gamma
gamma=reshape(gamma_estimates,N*n,N*n);
% calculate the (integer) length of the largest number in D, for formatting purpose
width=length(sprintf('%d',floor(max(abs(bear.vec(D))))));
% add a separator, a potential minus sign and three digits (total=5) to obtain the total space for each entry in the matrix
width=width+5;
for ii=1:N*n
temp=[];
   for jj=1:N*n
   % convert matrix entry into string
   number=num2str(gamma(ii,jj),'% .3f');
      % pad potential missing blanks
      while numel(number)<width
      number=[' ' number];
      end
   number=[number '  '];
   temp=[temp number];
   end
fprintf('%s\n',temp);
fprintf(fid,'%s\n',temp);
end

end

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

% finally display in-sample and forecast evaluation measures
% if forecast evaluation is activated and possible, display the results
if Feval==1 && Fcomp==1
bear.panel5fprint(Units,N,n,endo,rss,r2,r2bar,RMSE,MAE,MAPE,Ustat,CRPS_estimates,S1_estimates,S2_estimates,stringdates3,Fstartdate,Fcenddate,Fcperiods,fid);

% if forecast evaluation is activated but not possible, return a message to signal it
elseif Feval==1 && Fcomp==0

finfo1=['Forecast evaluation cannot be conducted.'];
fprintf('%s\n',finfo1);
fprintf(fid,'%s\n',finfo1);
finfo2=['Forecasts start in ' Fstartdate ', while observable data is available only until ' names{end,1} '.'];
fprintf('%s\n',finfo2);
fprintf(fid,'%s\n',finfo2);
finfo3=['To obtain forecast evaluation, the forecast start date must be anterior to the end of the data set.'];
fprintf('%s\n',finfo3);
fprintf(fid,'%s\n',finfo3);

% if forecast evaluation is not activated altogether, do not do anything

end


function [f,fu,fx] = f_fu_fx_Wrapper(u,x,p,parIdx)
% fu and fx are calculated using finite difference by default

% dx/dt = f(u,x,p)
% parIdx: index of the core (for reentrant purpose)

    [xDim,~] = size(x);
    [uDim,~] = size(u);
    f = f_Wrapper(u,x,p,parIdx);
    fu = zeros(xDim,uDim);
    fx = zeros(xDim,xDim);
    h  = 1e-8;
    % fu
    for i=1:uDim
         ei = zeros(uDim,1);
         ei(i,1) = 1;
         fu(:,i) = (f_Wrapper(u+ei*h,x,p,parIdx) - f)/h;
    end
    % fx
    for i=1:xDim
         ei = zeros(xDim,1);
         ei(i,1) = 1;
         fx(:,i) = (f_Wrapper(u,x+ei*h,p,parIdx) - f)/h;
    end
end


function ll = robThreeDynamicsLogLikelihood(model)

% ROBTHREEDYNAMICSLOGLIKELIHOOD Give the log likelihood of the robot three dynamics part.

% FGPLVM

ll = 0;
for i = 1:size(model.diffX)-1
  covMat = model.lambda*model.diffX(i, :)'*model.diffX(i, :) + ...
      eye(2)*model.sigma2;
  invCovMat = inv(covMat);
  ll = ll -0.5* model.diffX(i+1, :)*invCovMat*model.diffX(i+1, :)';
end


function [HB, NL, level] = HBstructure3(elem,N0,HBmesh)
%% HBSTRUCTURE3 reconstructs a hierachical structure of a 3-D mesh.
%
% HB(:,2:3) are two parent nodes of the node HB(:,1):
%             HB(:,2) --- HB(:,1) --- HB(:,3)
% NL records the range of indices in each level. The indices of nodes in
% the k-th level is given by NL(k)+1:NL(k+1).
%
% See also HBstructure3
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('HBmesh','var'), HBmesh = []; end
N = max(elem(:));
HB = zeros(N,3);
% level = max(min(round(log2(N/N0)),16),2); % at least two level
level = 20;
NL(level+1) = N;
for k = level: -1 : 2
    [elem,HBmesh,newHB] = uniformcoarsen3red(elem,HBmesh);  % try coasen red refinement
    if isempty(newHB) && ~isempty(HBmesh)     % then try coarsen bisection
        [elem,HBmesh,newHB] = uniformcoarsen3(elem,HBmesh);
    end
    if (isempty(newHB)) || (size(elem,1)< 2*N0)
    % no nodes are removed or it reaches the coarsest level
        NL = NL(k:end);
        break;
    end
    NL(k) = NL(k+1) - size(newHB,1);
    HB(NL(k)+1:NL(k+1),1:3) = newHB(:,1:3);
end
level = length(NL)-1;


function [g,L,info] = nonsepgabpars_from_window(g,a,M,lt,L,callfun)
%NONSEPGABPARS_FROM_WINDOW  Compute g and L from window
%   Usage: [g,g.info,L] = gabpars_from_window(f,g,a,M,lt,L);
%
%   Use this function if you know a window and a lattice
%   for the NONSEPDGT. The function will calculate a transform length L and
%   evaluate the window g into numerical form.
%
%   If the transform length is unknown (as it usually is unless explicitly
%   specified by the user), set L to be [] in the input to this function.

if nargin<6
  stacknames=dbstack;
  callfun=stacknames(2).name;
end;

if isempty(L)
  if isnumeric(g)
    L=length(g);
  else
    L=dgtlength(1,a,M,lt);
  end;
else
  Lcheck=dgtlength(L,a,M,lt);
  if Lcheck~=L
    error('%s: Invalid transform size L',upper(mfilename));
  end;
end;

[g,info] = comp_window(g,a,M,L,lt,'NONSEPGABDUAL');

if (info.isfir)
  if info.istight
    g=g/sqrt(2);
  end;
end;


function [I1,I2] = hmxSubdivide(X,dim)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxSubdivide.m                                |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Subdivide particles with median repartition   |
%|  `---'  |                                                              |
%+========================================================================+

% Sort data on largest dimension
[~,I] = sort(X(:,dim));

% Equal repartition
I1 = I(1:floor(end/2));
I2 = I(floor(end/2)+1:end);
end


% example_1 - inversion of a irrational fraction F(s)
clear, close all
[t1,ft1]=INVLAP('tanh(s)/s',0.01,20,1000);
[t2,ft2]=INVLAP('tanh(s)/s',0.01,20,2000,6,280,59);
figure(3)
set(3,'color','white')
subplot(2,1,1)
plot(t1,ft1), grid on, zoom on
xlabel('t [s]'), ylabel('f(t)')
title('rectangular periodic wave')
subplot(2,1,2)
plot(t2,ft2), grid on, zoom on
xlabel('t [s]'), ylabel('f(t)')
title('improved accuracy')


function [S, Serr, per, tau, exctn, lambda] = armode(A, C, th)
%ARMODE	Eigendecomposition of AR model.
%
%  [S,Serr,per,tau,exctn]=ARMODE(A,C,th) computes the
%  eigendecomposition of an AR(p) model that has been fitted using
%  ARFIT. The input arguments of ARMODE are output of ARFIT.
%
%  The columns of the output matrix S contain the estimated eigenmodes
%  of the AR model. The output matrix Serr contains margins of error
%  for the components of the estimated eigenmodes S, such that
%  (S +/- Serr) are approximate 95% confidence intervals for the
%  individual components of the eigenmodes.
%
%  The two-row matrices per and tau contain in their first rows the
%  estimated oscillation period per(1,k) and the estimated damping
%  time tau(1,k) of the eigenmode S(:,k). In their second rows, the
%  matrices per and tau contain margins of error for the periods and
%  damping times, such that
%     ( per(1,k) +/- per(2,k) )   and   ( tau(1,k) +/- tau(2,k) )
%  are approximate 95% confidence intervals for the period and damping
%  time of eigenmode S(:,k).
%
%  For a purely relaxatory eigenmode, the period is infinite (Inf).
%  For an oscillatory eigenmode, the periods are finite.
%
%  The excitation of an eigenmode measures its dynamical importance
%  and is returned as a fraction exctn that is normalized such that
%  the sum of the excitations of all eigenmodes equals one.
%
%  See also ARFIT, ARCONF.

%  Modified 13-Oct-00
%  Author: Tapio Schneider
%	   tapio@gps.caltech.edu

  ccoeff   = .95;                       % confidence coefficient
  m 	   = size(C,1);			% dimension of state space
  p 	   = size(A,2) / m; 		% order of model
  if p <= 0
    error('Order must be greater 0.');
  end

  % Assemble coefficient matrix of equivalent AR(1) model
  A1 	   = [A; eye((p-1)*m) zeros((p-1)*m,m)];

  % Eigenvalues and eigenvectors of coefficient matrix of equivalent
  % AR(1) model
  [BigS,d] = eig(A1);  			% columns of BigS are eigenvectors
  lambda   = diag(d);    		% vector containing eigenvalues
  lambda   = lambda(:); 	        % force lambda to be column vector

  % Warning if the estimated model is unstable
  if any(abs(lambda) > 1)
    warning(sprintf(['The estimated AR model is unstable.\n',...
		     '\t Some excitations may be negative.']))
  end

  % Fix phase of eigenvectors such that the real part and the
  % imaginary part of each vector are orthogonal
  BigS     = adjph(BigS);

  % Return only last m components of each eigenvector
  S 	   = BigS((p-1)*m+1:p*m, :);

  % Compute inverse of BigS for later use
  BigS_inv = inv(BigS);

  % Recover the matrix Uinv that appears in the asymptotic covariance
  % matrix of the least squares estimator (Uinv is output of AR)
  if (size(th,2) == m*p+1)
    % The intercept vector has been fitted by AR; in computing
    % confidence intervals for the eigenmodes, this vector is
    % irrelevant. The first row and first column in Uinv,
    % corresponding to elements of the intercept vector, are not
    % needed.
    Uinv   = th(3:size(th,1), 2:size(th,2));

  elseif (size(th,2) == m*p)
    %  No intercept vector has been fitted
    Uinv   = th(2:size(th,1), :);
  else
    error('Input arguments of ARMODE must be output of ARFIT.')
  end
  % Number of degrees of freedom
  dof 	 = th(1,1);
  % Quantile of t distribution for given confidence coefficient and dof
  t      = tquant(dof, .5+ccoeff/2);

  % Asymptotic covariance matrix of estimator of coefficient matrix A
  Sigma_A  = kron(Uinv, C);

  % Noise covariance matrix of system of relaxators and oscillators
  CovDcpld = BigS_inv(:, 1:m) * C * BigS_inv(:, 1:m)';

  % For each eigenmode j: compute the period per, the damping time
  % tau, and the excitation exctn; also get the margins of error for
  % per and tau
  for j=1:m*p				% eigenmode number
    a		= real(lambda(j)); 	% real part of eigenvalue j
    b 		= imag(lambda(j)); 	% imaginary part of eigenvalue j
    abs_lambda_sq= abs(lambda(j))^2;  	% squared absolute value of eigenvalue j
    tau(1,j) 	= -2/log(abs_lambda_sq);% damping time of eigenmode j

    % Excitation of eigenmode j
    exctn(j) 	= real(CovDcpld(j,j) / (1-abs_lambda_sq));

    % Assemble derivative of eigenvalue with respect to parameters in
    % the coefficient matrix A
    dot_lam 	= zeros(m^2*p, 1);
    for k=1:m
      dot_lam(k:m:k+(m*p-1)*m) = BigS_inv(j,k) .* BigS(1:m*p,j);
    end
    dot_a 	= real(dot_lam); 	% derivative of real part of lambda(j)
    dot_b 	= imag(dot_lam); 	% derivative of imag part of lambda(j)

    % Derivative of the damping time tau w.r.t. parameters in A
    phi 	= tau(1,j)^2 / abs_lambda_sq * (a*dot_a + b*dot_b);
    % Margin of error for damping time tau
    tau(2,j) 	= t * sqrt(phi'*Sigma_A*phi);

    % Period of eigenmode j and margin of error for period. (The
    % if-statements avoid warning messages that may otherwise result
    % from a division by zero)
    if (b == 0 & a >= 0)      % purely real, nonnegative eigenvalue
      per(1,j)	= Inf;
      per(2,j)  = 0;
    elseif (b == 0 & a < 0)   % purely real, negative eigenvalue
      per(1,j)	= 2;
      per(2,j)  = 0;
    else                      % complex eigenvalue
      per(1,j)	= 2*pi/abs(atan2(b,a));

      % Derivative of period with respect to parameters in A
      phi 	= per(1,j)^2 / (2*pi*abs_lambda_sq)*(b*dot_a-a*dot_b);
      % Margin of error for period
      per(2,j) 	= t * sqrt(phi'*Sigma_A*phi);
    end
  end

  % Give the excitation as `relative importance' that sums to one
  exctn 	= exctn/sum(exctn);

  % Compute confidence intervals for eigenmodes
  % -------------------------------------------
  % Shorthands for matrix products
  XX 	        = real(BigS)'*real(BigS);
  YY 	        = imag(BigS)'*imag(BigS);
  XY 	        = real(BigS)'*imag(BigS);

  % Need confidence intervals only for last m rows of BigS
  row1 	        = (p-1)*m+1; % first row for which confidence interval is needed

  mp = m*p;                           	% dimension of equivalent AR(1) model
  for k=1:mp        			% loop over columns of S
    for l=row1:mp  			% loop over rows of S

      % evaluate gradient of S_{lk}
      for ii=1:m
	for jj=1:mp
	  % compute derivative with respect to A(ii,jj)
	  zsum = 0;
	  zkkr = 0; 			% real part of Z_{kk}
	  zkki = 0; 			% imaginary part of Z_{kk}
	  for j=1:mp
	    if (j ~= k) 		% sum up elements that appear in Z_{kk}
	      zjk  = BigS_inv(j,ii)*BigS(jj,k)/(lambda(k)-lambda(j));
	      zjkr = real(zjk);
	      zjki = imag(zjk);
	      zkkr = zkkr+zjki*(XY(k,j)-XY(j,k))-zjkr*(XX(k,j)+YY(k,j));
	      zkki = zkki+zjki*(YY(k,j)-XX(k,j))-zjkr*(XY(k,j)+XY(j,k));
	      zsum = zsum+BigS(l,j)*zjk;
	    end
	  end
	  % now add Z_{kk}
	  zkki = zkki / (XX(k,k)-YY(k,k));
	  grad_S((jj-1)*m+ii) = zsum+BigS(l,k)*(zkkr+i*zkki);
	end
      end
      Serr(l,k) = t * ( sqrt( real(grad_S)*Sigma_A*real(grad_S)') ...
			 + i*sqrt( imag(grad_S)*Sigma_A*imag(grad_S)') );
    end
  end

  % Only return last m*p rows of Serr
  Serr = Serr(row1:m*p, :);


function a = r8cbb_add ( n1, n2, ml, mu, a, i, j, value )

%*****************************************************************************80
%
%% R8CBB_ADD adds a value to an entry of a R8CBB matrix.
%
%  Discussion:
%
%    The R8CBB storage format is for a compressed border banded matrix.
%    Such a matrix has the logical form:
%
%      A1 | A2
%      ---+---
%      A3 | A4
%
%    with A1 a (usually large) N1 by N1 banded matrix, while A2, A3 and A4
%    are dense rectangular matrices of orders N1 by N2, N2 by N1, and N2 by N2,
%    respectively.
%
%    The R8CBB format is the same as the DBB format, except that the banded
%    matrix A1 is stored in compressed band form rather than standard
%    banded form.  In other words, we do not include the extra room
%    set aside for fill in during pivoting.
%
%    A should be defined as a vector.  The user must then store
%    the entries of the four blocks of the matrix into the vector A.
%    Each block is stored by columns.
%
%    A1, the banded portion of the matrix, is stored in
%    the first (ML+MU+1)*N1 entries of A, using the obvious variant
%    of the LINPACK general band format.
%
%    The following formulas should be used to determine how to store
%    the entry corresponding to row I and column J in the original matrix:
%
%    Entries of A1:
%
%      1 <= I <= N1, 1 <= J <= N1, (J-I) <= MU and (I-J) <= ML.
%
%      Store the I, J entry into location
%      (I-J+MU+1)+(J-1)*(ML+MU+1).
%
%    Entries of A2:
%
%      1 <= I <= N1, N1+1 <= J <= N1+N2.
%
%      Store the I, J entry into location
%      (ML+MU+1)*N1+(J-N1-1)*N1+I.
%
%    Entries of A3:
%
%      N1+1 <= I <= N1+N2, 1 <= J <= n1.
%
%      Store the I, J entry into location
%      (ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%
%    Entries of A4:
%
%      N1+1 <= I <= N1+N2, N1+1 <= J <= N1+N2
%
%      Store the I, J entry into location
%      (ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%      (same formula used for A3).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N1, N2, the order of the banded and dense blocks.
%    N1 and N2 must be nonnegative, and at least one must be positive.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%    ML and MU must be nonnegative, and no greater than N1-1.
%
%    Input, real A((ML+MU+1)*N1 + 2*N1*N2 + N2*N2), the R8CBB matrix.
%
%    Input, integer I, J, the indices of the entry to be incremented.
%
%    Input, real VALUE, the value to be added to the (I,J) entry.
%
%    Output, real A((ML+MU+1)*N1 + 2*N1*N2 + N2*N2), the modified R8CBB matrix.
%
  if ( value == 0.0 )
    return;
  end
%
%   Check for I or J out of bounds.
%
  if ( i <= 0 .or. n1+n2 < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8CBB_ADD - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of row index I = %d/n', i );
    error ( 'R8CBB_ADD - Fatal error!' );
  end

  if ( j <= 0 | n1+n2 < j )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8CBB_ADD - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of column index J = %d/n',j );
    error ( 'R8CBB_ADD - Fatal error!' );
  end
%
%  The A1 block of the matrix.
%
%  Check for out of band problems.
%
  if ( i <= n1 & j <= n1 )
    if ( mu < (j-i) | ml < (i-j) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8CBB_ADD - Warning!\n' );
      fprintf ( 1, '  Unable to add to entry (%d,%d).\n', i, j );
      return
    else
      ij = (i-j+mu+1)+(j-1)*(ml+mu+1);
    end
%
%  The A2 block of the matrix:
%
  elseif ( i <= n1 & n1 < j )
    ij = (ml+mu+1)*n1+(j-n1-1)*n1 + i;
%
%  The A3 and A4 blocks of the matrix.
%
  elseif ( n1 < i )
    ij = (ml+mu+1)*n1+n2*n1+(j-1)*n2 + (i-n1);
  end

  a(ij) = a(ij) + value;

  return
end


function month_cal_republican ( y, m )

%*****************************************************************************80
%
%% MONTH_CAL_REPUBLICAN prints a Republican month calendar.
%
%  Format:
%
%    REPUBLICAN CALENDAR
%    Brumaire 3 ER
%
%     1  2  3  4  5  6  7  8  9 10
%    11 12 13 14 15 16 17 18 19 20
%    21 22 23 24 25 26 27 28 29 30
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, the YM date.
%

%
%  Make local copies of the input.
%
  m2 = m;
  y2 = y;
%
%  Check the month and year.
%
  [ y2, m2, ierror ] = ym_check_republican ( y2, m2 );

  if ( ierror ~= 0 )
    return
  end
%
%  Print out a heading.
%
  s1 = month_to_month_name_republican ( m2 );
  s2 = y_to_s_republican ( y2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Republican Calendar\n' );
  fprintf ( 1, '%s %s\n', s1, s2 );
  fprintf ( 1, '\n' );
%
%  Get the days of the week.
%
  for w = 1 : 10
    fprintf ( 1, '%3d', w );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
%
%  Print out a line of day numbers.
%
  iday = 1;

  while ( iday <= month_length_republican ( y2, m2 ) )

    for w = 1 : 10

      if ( month_length_republican ( y2, m2 ) < iday )
        fprintf ( 1, '   ' );
      else
        fprintf ( 1, '%3d', iday );
      end

      iday = iday + 1;

    end

    fprintf ( 1, '\n' );

  end

  return
end


function filename_final=mesh2vrml(X, Y, Z)

% Transforming data into VRML. Remember Z axis is pointing out of the
% screen towards you. X axis to the right and Y Axis is upwards.
xDimension=length(X(1,:));
xSpacing=abs(X(1,2)-X(1,1));
zDimension=length(Y(1,:));
zSpacing=abs(Y(2,1)-Y(1,1));
filename='myshape.wrl';
filename_final='elevation.wrl';
fid = fopen(filename, 'w+','n','UTF-8');
fprintf(fid,'#VRML V2.0 utf8');
fprintf(fid, '\n Transform { \n');
fprintf(fid,' \n translation %8.4f %8.4f %8.4f\n', -(max(max(X))-min(min(X)))/2, 0, -(max(max(Y))-min(min(Y)))/2);
fprintf(fid,' \n  rotation 0 0 1 0\n');
fprintf(fid,' \n  center %8.4f %8.4f %8.4f\n', -(max(max(X))+min(min(X)))/2, -(max(max(Z))+min(min(Z)))/2, 0);
fprintf(fid,' \n  scale 1 1 1\n');
fprintf(fid, '\n children Shape {\n');
fprintf(fid,'\n appearance	Appearance {\n');
fprintf(fid,'\n material	Material {}\n');
fprintf(fid,'\n	}\n');
fprintf(fid, '\n geometry	ElevationGrid { \n');
fprintf(fid, '\n xDimension %d\n', xDimension);
fprintf(fid, '\n xSpacing %d\n', xSpacing);
fprintf(fid, '\n zDimension %d\n', zDimension);
fprintf(fid, '\n zSpacing %d\n', zSpacing);
fprintf(fid,' height [');
for j=1:1:zDimension
    for i=1:1:xDimension
        fprintf(fid,'%f', Z(i,j));
        if (i==xDimension && j==zDimension)
        else
            fprintf(fid,',');
        end
    end
    fprintf(fid,'\n');
end
fprintf(fid,']\n');
fprintf(fid,'\n color NULL\n');
fprintf(fid,'\n colorPerVertex TRUE\n');
fprintf(fid,'\n normal NULL\n');
fprintf(fid,'\n normalPerVertex TRUE\n');
fprintf(fid, '\n texCoord NULL \n');
fprintf(fid, '\n ccw TRUE\n');
fprintf(fid, '\n solid FALSE\n');
fprintf(fid, '\n creaseAngle 0.0\n');
fprintf(fid,'		}\n');
fprintf(fid,'		}\n');
fprintf(fid,'		}\n');
fclose(fid);
copyfile(filename,filename_final, 'f');
delete(filename);

% Visualization code
figure_height=400;
view_width=350;
corner_x=20;
corner_y=10;
clearance=20;
fig=figure('Name', 'Preview', 'Position',[100 450 2*view_width+3*clearance figure_height+clearance]);
% Create the MATLAB GUI with two views of the aircraft
% First create the vrcanvas object for the first view. Specify the location
% and size of the view in pixels
% Create the plot of the translational coordinates of the aircraft
h = axes('Units', 'pixels', 'OuterPosition',[corner_x+view_width+3*clearance corner_y view_width figure_height]);
set(h, 'Position', [corner_x+clearance corner_y view_width figure_height]);
set(h, 'FontSize', 8);
surf(X,Y,Z);
hold on;
grid on;
title('Surface in MATLAB');
xlabel(h, 'x')
ylabel(h, 'y')
zlabel(h, 'z')
set(h, 'Units', 'normalized');
view(h, [90 0]);
world=vrworld(filename_final);
open(world);
c1 = vr.canvas(world, fig,[corner_x++view_width+3*clearance corner_y+clearance view_width-2*clearance  figure_height-2*clearance]);
set(c1, 'Units', 'normalized');


%COMPOSERT  Combines two rotation-and-shift transformations
%
%     S = cv.composeRT(rvec1, tvec1, rvec2, tvec2)
%
% ## Input
% * __rvec1__ First rotation vector, 3x1 float vector.
% * __tvec1__ First translation vector, 3x1 float vector.
% * __rvec2__ Second rotation vector, 3x1 float vector.
% * __tvec2__ Second translation vector, 3x1 float vector.
%
% ## Output
% * __S__ A scalar struct with the following fields:
%   * __rvec3__ Rotation vector of the superposition, 3x1 float vector.
%   * __tvec3__ Translation vector of the superposition, 3x1 float vector.
%   * __dr3dr1__, __dr3dt1__, __dr3dr2__, __dr3dt2__, __dt3dr1__, __dt3dt1__,
%     __dt3dr2__, __dt3dt2__ Derivatives of `rvec3` or `tvec3` with regard to
%     `rvec1`, `tvec1`, `rvec2`, and `tvec2`, respectively. Each derivative is
%     a 3x3 float matrix.
%
% The function computes:
%
%     rvec3 = rodrigues^-1( rodorigues(rvec2) * rodrigues(rvec1) )
%     tvec3 = rodrigues(rvec2) * tvec1 + tvec2
%
% where `rodrigues` denotes a rotation vector to a rotation matrix
% transformation, and `rodrigues^-1` denotes the inverse transformation.
% See cv.Rodrigues for details.
%
% Also, the function can compute the derivatives of the output vectors with
% regards to the input vectors (see cv.matMulDeriv). The function us used
% inside cv.stereoCalibrate but can also be used in your own code where
% Levenberg-Marquardt or another gradient-based solver is used to optimize a
% function that contains a matrix multiplication.
%
% See also: cv.Rodrigues, cv.matMulDeriv, cv.stereoCalibrate
%


function img = flowToColor(flow, varargin)

%  flowToColor(flow, maxFlow) flowToColor color codes flow field, normalize
%  based on specified value,
%
%  flowToColor(flow) flowToColor color codes flow field, normalize
%  based on maximum flow present otherwise

%   According to the c++ source code of Daniel Scharstein
%   Contact: schar@middlebury.edu

%   Author: Deqing Sun, Department of Computer Science, Brown University
%   Contact: dqsun@cs.brown.edu
%   $Date: 2007-10-31 18:33:30 (Wed, 31 Oct 2006) $

% Copyright 2007-2010, Brown University, Providence, RI. USA
%
%
%                         All Rights Reserved
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for any purpose other than its incorporation into a
% commercial product is hereby granted without fee, provided that the
% above copyright notice appear in all copies and that both that
% copyright notice and this permission notice appear in supporting
% documentation, and that the name of the author and Brown University not be used in
% advertising or publicity pertaining to distribution of the software
% without specific, written prior permission.
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
% INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR ANY
% PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR BROWN UNIVERSITY BE LIABLE FOR
% ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
% WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
% ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
% OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

UNKNOWN_FLOW_THRESH = 1e9;
UNKNOWN_FLOW = 1e10;            %

[height widht nBands] = size(flow);

if nBands ~= 2
    error('flowToColor: image must have two bands');
end;

u = flow(:,:,1);
v = flow(:,:,2);

maxu = -999;
maxv = -999;

minu = 999;
minv = 999;
maxrad = -1;

% fix unknown flow
idxUnknown = (abs(u)> UNKNOWN_FLOW_THRESH) | (abs(v)> UNKNOWN_FLOW_THRESH) ;
u(idxUnknown) = 0;
v(idxUnknown) = 0;

maxu = max(maxu, max(u(:)));
minu = min(minu, min(u(:)));

maxv = max(maxv, max(v(:)));
minv = min(minv, min(v(:)));

rad = sqrt(u.^2+v.^2);
maxrad = max(maxrad, max(rad(:)));

fprintf('max flow: %.4f flow range: u = %.3f .. %.3f; v = %.3f .. %.3f\n', maxrad, minu, maxu, minv, maxv);

if isempty(varargin) ==0
    maxFlow = varargin{1};
    if maxFlow > 0
        maxrad = maxFlow;
    end;
end;

u = u/(maxrad+eps);
v = v/(maxrad+eps);

% compute color

img = computeColor(u, v);

% unknown flow
IDX = repmat(idxUnknown, [1 1 3]);
img(IDX) = 0;


function pb = bsdsNonMaxSuppression(pbnew, pbold)

if size(pbold)~=size(pbnew)
    pbold = imresize(pbold, size(pbnew), 'nearest');
end
pb = pbnew.*double(pbold>0);

% [h, w] = size(pbnew);
% norient = size(pball, 3);
% theta = (0:norient-1)/norient*pi;
%
% [h2, w2, tmp] = size(pball);
% if h2~=h || w~=w2
%     pball = imresize(pball, [h w], 'nearest');
% end
%
% % nonmax suppression and max over orientations
% [unused,maxo] = max(pball,[],3);
% pb = zeros(h,w);
% %theta = zeros(h,w);
% r = 2.5;
% for i = 1:norient,
%   mask = (maxo == i);
%   %a = fitparab(pball(:,:,i),r,r,theta(i));
%   %pbi = nonmax(max(0,a),gtheta(i));
%   pbi = nonmax(pbnew,theta(i));
%   pb = max(pb,pbi.*mask);
% end
% pb = max(0,min(1,pb));
%
% % mask out 1-pixel border where nonmax suppression fails
% pb(1,:) = 0;
% pb(end,:) = 0;
% pb(:,1) = 0;
% pb(:,end) = 0;


function [pinit,pflow,names,M,U,Y] = mci_lds_group_data (lds)
% Generate LDS data for a group of subjects
% FORMAT [pinit,pflow,names,M,U,Y] = mci_lds_group_data (lds)
%
% lds        Data structure with fields:
%
% .R         R.pE, R.pC prior over initial conds
% .sd        Standard deviation of observation noise
% .Nsub      Number of subjects
% .Nobs      Number of observations per subject
% .model     'lds_real','forward',etc.
% .flow_par  'fixed' or 'random'
% .init_par  'fixed' or 'random'
%
% pinit      Initial params
% pflow      Flow params
% names      names of parameters
% M          Cell of models
% U          Cell of inputs
% Y          Cell of data
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: mci_lds_group_data.m 6548 2015-09-11 12:39:47Z will $

R=lds.R; sd=lds.sd; Nsub=lds.Nsub;
Nobs=lds.Nobs; model=lds.model;
d=length(R.pE);

% Set Flow Parameters
if strcmp(model,'lds_real')
    r=linspace(0.3,0.9,d);
    T=100;
    q=(1/T)*log(r);
    pflow=log(-q);
else

    Mtmp.name=model;
    Mtmp.sd=sd;
    Mtmp.d=d;
    Mtmp.drop=0.5;
    %Mtmp.t=[1:400]'/4;
    Mtmp.t=[1:25]'/0.25;
    Mtmp.int='sundials';

    Mtmp=mci_lds_struct(Mtmp);
    if strcmp(lds.flow_par,'fixed')
%         PP.self=Mtmp.sd_self*randn(d,1);
%         PP.between=Mtmp.sd_between*randn(Mtmp.Nb,1);
%         pflow=spm_vec(PP);

        Pt = [-0.04,-0.01,-0.005,-0.01,0.01,0.01,0.01]';
        pflow = mci_lds_par2lat (Pt,Mtmp);
    else
        for n=1:Nsub,
            PP.self=Mtmp.sd_self*randn(d,1);
            PP.between=Mtmp.sd_between*randn(Mtmp.Nb,1);
            pflow(:,n)=spm_vec(PP);
        end
    end
end

if strcmp(lds.init_par,'fixed')
    R0 = spm_normrnd(R.pE,R.pC,1);
end
for n=1:Nsub,

    % Sample initial states from prior
    if strcmp(lds.init_par,'random')
        R0 = spm_normrnd(R.pE,R.pC,1);
    end
    pinit(:,n)=R0;
    switch model
        case 'lds_real'
            [M{n},U{n},y] = irlds_init (d,sd,R0,pflow);
        otherwise
            Mtmp.R=R0;
            [M{n},U{n}] = mci_lds_struct (Mtmp);

            if strcmp(lds.flow_par,'fixed')
                y = mci_lds_gen (M{n},U{n},pflow);
            else
                y = mci_lds_gen (M{n},U{n},pflow(:,n));
            end
    end

    if lds.Nobs==M{n}.N
        Y{n}.y=y;
        Y{n}.ind=1:M{n}.N;
    else
        % Thin observations to selected time points
        rind=randperm(M{n}.N);
        ind=rind(1:Nobs);
        Y{n}.y=y(ind,:);
        Y{n}.ind=ind;
    end

end

if strcmp(model,'forward')

    for j=1:d,
        jn=int2str(j);
        names{j}=['a_{',jn,jn,'}'];
    end
    for i=1:d-1,
        jn=int2str(i);
        j1n=int2str(i+1);
        names{i+j}=['a_{',j1n,jn,'}'];
    end

end


function ar=v_lpcim2ar(im)
%V_LPCIM2AR Convert impulse response to AR coefs AR=(IM)

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: v_lpcim2ar.m 10865 2018-09-21 17:22:45Z dmb $
%
%   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,p1]=size(im);
ar=zeros(nf,p1);
wz=[1 zeros(1,p1-1)];
for k=1:nf
  ar(k,:)=wz/toeplitz(wz,im(k,:)/im(k,1));
end


function iou=boxiou(x1,y1,w1,h1,x2,y2,w2,h2)
% compute intersection over union of two bboxes
%

    bisect=boxIntersect(x1,x1+w1,y1+h1,y1,x2,x2+w2,y2+h2,y2);
    iou=0;
    if ~bisect, return; end

    bunion=boxUnion(x1,x1+w1,y1+h1,y1,x2,x2+w2,y2+h2,y2,bisect);

    assert(bunion>0,'something wrong with union computation');
    iou=bisect/bunion;

end


function SO3F = conj(SO3F)
% Construct the complex conjugate function $\overline{f}$ of an SO3Fun $f$.
%
% Syntax
%   SO3F = conj(F)
%
% Input
%  F - @SO3FunHarmonic
%
% Output
%  SO3F - @SO3FunHarmonic
%

bw = SO3F.bandwidth;
for n=0:bw
  ind = deg2dim(n)+1:deg2dim(n+1);
  ind2 = flip(ind);
  SO3F.fhat(ind,:) = SO3F.fhat(ind2,:);
end
SO3F.fhat = conj(SO3F.fhat);

end


function [X,P] = femUnk(fe)
%+========================================================================+
%|                                                                        |
%|              OPENFEM - LIBRARY FOR FINITE ELEMENT METHOD               |
%|           openFem is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal & Francois Alouges (c) 2017-2018.          |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : femUnk.m                                      |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & François Alouges            |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Unknowns and reduction matrix for constrained |
%|  `---'  |                finite elements                               |
%+========================================================================+

% Initialization with dofs
X  = fe.dof;
Nx = size(X,1);
P  = speye(Nx);

% Dirichlet
if ~isempty(fe.dir)
    % Finite element for dirichlet mesh
    fed = fem(fe.dir,fe.typ);

    % Dof indices
    I = find(~ismember(X,fed.dof,'rows'));

    % Reduction matrix
    M = speye(Nx);
    M = M(:,I);

    % Update
    P  = P * M;
    X  = X(I,:);
    Nx = length(I);
end

% Junction
if ~isempty(fe.jct)
    % Number of junction
    Njct = length(fe.jct)/2;

    % Initialization
    feJct = cell(1,Njct);
    Ijct  = cell(1,Njct);

    % Finite element for junction meshes
    for i = 1:Njct
        if isa(fe.jct{2*i-1},'msh') && isnumeric(fe.jct{2*i})
            feJct{i} = fem(fe.jct{2*i-1},fe.typ);
        else
            error('femUnknown.m : unavailable case');
        end
    end

    % Valid dof indices for junction meshes
    for i = 1:Njct
        Xi         = feJct{i}.dof;
        Ijct{i}    = zeros(size(Xi,1),1);
        [~,I,J]    = intersect(Xi,X,'rows','stable');
        Ijct{i}(I) = J;
    end
    Ijct = cell2mat(Ijct);

    % Extract dirichlet condition and multiple indices
    bool = (Ijct(:,Njct) == 0);
    for i = 1:Njct-1
        bool = bool + (Ijct(:,Njct) == Ijct(:,i));
    end
    Ijct = Ijct(~bool,:);

    % Final dof after extraction of the last indices
    I = setdiff((1:Nx)',Ijct(:,end));

    % Linear relation matrix
    M = speye(Nx);
    for  i = 1:Njct
        for j = setdiff(1:Njct,i)
            M = M + sparse(Ijct(:,i),Ijct(:,j),-fe.jct{2*j}/fe.jct{2*i},Nx,Nx);
        end
    end

    % Reduction
    M = M(:,I);

    % Update
    P  = P * M;
    X  = X(I,:);
%     Nx = length(I);
end
end


function title = p06_title ( )

%*****************************************************************************80
%
%% P06_TITLE returns a title for problem 6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Harald Niederreiter, Kevin McCurley,
%    Optimization of functions by quasi-random search methods,
%    Computing,
%    Volume 22, Number 2, 1979, pages 119-123.
%
%  Parameters:
%
%    Output, string TITLE, a title for the problem.
%
  title = 'f(x) = - sin(1/x(1)+1/x(2)+1/x(3)+1/x(4))';

  return
end


function outdegs = findangle(boundvec, startidx)

% This function computes the bifurcation angle

npix = prod(size(boundvec));
if (sum(abs(boundvec))==0 | sum(abs(boundvec))== npix )
    outdegs =[];
    return;
end

% degree assignments
R  = npix/8;
dy = [0:1:R, R*ones(1, 2*R-1), R:-1:-R, -R* ones(1,2*R-1), -R:1:-1];
dx = [R*ones(1, R+1), R-1:-1:-R+1, -R*ones(1, 2*R+1), -R+1:1:R-1, R*ones(1, R)];
degs = atan2(dy, dx)*180/pi;
degs(degs<0) = degs(degs<0) + 360;

% find connected region
[labelmap, numlabel] = bwlabel(boundvec, 8);
if (boundvec(1)==1 & boundvec(end)==1)
    degs(labelmap == labelmap(end)) = degs(labelmap == labelmap(end))-360;
    labelmap(labelmap == labelmap(end)) = labelmap(1);
    numlabel= numlabel-1;
end

% angle assign to each region
for k=1:numlabel
    seeds = sort(degs(labelmap==k));
    m(k) = (seeds(1)+seeds(end))/2;
end
m(m<0) = m(m<0)+360;

for k = 1:numlabel-1
    outdegs(k)  = m(k+1)-m(k);
end
outdegs(numlabel)= m(1)-m(numlabel);
outdegs(outdegs<0) = outdegs(outdegs<0)+360;

% circulate the startidx to the first one
if (nargin == 2)
    while (boundvec(startidx)==0)
        startidx = startidx+1;
        if (startidx>npix)
            startidx = 1;
        end
    end
    outdegs = circshift(outdegs, [0, -labelmap(startidx)+1]);
end


function [data, opt] = RawPixelExtractor(im, tmpl, opt)
sz = opt.FeatureExtractor.tmplsize;

if isfield(opt, 'pixel_im')
    im = opt.pixel_im;
    % disp('lab space image saved.');
else
    if (ndims(im) == 3)
%         if (norm(im(:,:,1) - im(:,:,2)) > 1e-6)
            im = RGB2Lab(im);
%         else
%             im = rgb2gray(im);
%         end
    end
end
data.tmpl = tmpl;

features = zeros(prod(sz)*size(im, 3), size(tmpl, 1));

% pad
minW = min(round(tmpl(:, 1) - tmpl(:, 3) / 2)) - 1;
maxW = max(round(tmpl(:, 1) + tmpl(:, 3) / 2)) + 1;
minH = min(round(tmpl(:, 2) - tmpl(:, 4) / 2)) - 1;
maxH = max(round(tmpl(:, 2) + tmpl(:, 4) / 2)) + 1;
[h, w, c] = size(im);
if (minW < 1)
    im_new = zeros(h, w + abs(minW) + 1, c);
    im_new(:, abs(minW) + 2:end, :) = im;
    im = im_new;
    tmpl(:, 1) = tmpl(:, 1) + abs(minW) + 1;
end
if (maxW > w)
    im_new = zeros(h, size(im, 2) + maxW - w, c);
    im_new(:, 1:size(im, 2), :) = im;
    im = im_new;
end
if (minH < 1)
    im_new = zeros(h + abs(minH) + 1, size(im, 2), c);
    im_new(abs(minH) + 2:end, :, :) = im;
    im = im_new;
    tmpl(:, 2) = tmpl(:, 2) + abs(minH) + 1;
end
if (maxH > h)
    im_new = zeros(size(im, 1) + maxH - h, size(im, 2), c);
    im_new(1:size(im, 1), :, :) = im;
    im = im_new;
end

for i = 1:size(tmpl, 1)
    midW    = tmpl(i, 1);
    midH    = tmpl(i, 2);
    w       = tmpl(i, 3);
    h       = tmpl(i, 4);

    tempIm = im(round(midH-h/2) : round(midH+h/2),...
                round(midW-w/2) : round(midW+w/2), :);
%     tempIm = imresize(tempIm, sz);
    tempIm = mexResize(tempIm, sz, 'auto');
    features(:, i) = tempIm(:);
    if (norm(features(:, i)) > 1e-6)
        features(:, i) = features(:, i) / norm(features(:, i));
    end
end
% features = features - 0.5;

data.feat = features;


function [E_map,w,f,costf,iter,value] = pixelid(data,F_fw,te,varargin)

% in
%   data[n,n,nsets]     source data
%   F_fw = fat water shift (omega)
%   te [nsets]   time
%options
%   niter  # of iterations
%   x       [array or one value]
%   y       [array or one value]
%           x,y are array or one value of object field map
%           If x = 100, y = 200 then we choose just (100,200)th pixel of field map
%           If x = 1:100, y = 200:202 then we choose (1:100,200:202) of
%           field map ( = estimate 100*103 pixels)
%   init  [n,n] Initial guess of field map
%   cost    If cost = 0 : Do not calculate cost function (Default)
%           If cost = 1 : Calculate cost function
%% initialize

n = size(data,1); % length of the image

%defaults
arg.niter = 30;
arg.x = 1:512;
arg.y = 1:512;  % Just choose (100,100)th pixel of field map
arg.init = zeros(n,n);
arg.cost = 0;
arg = vararg_pair(arg, varargin);

c = cos(F_fw*te).';
d = sin(F_fw*te).'; % for Appendix A and B
A1 =  [1 0 c(1) -d(1); 1 0 c(2) -d(2); 1 0 c(3) -d(3); 0 1 d(1) c(1); 0 1 d(2) c(2); 0 1 d(3) c(3)];
A2 = inv(A1'*A1)*A1';  % for Appendix A
map = arg.init; % initial field map
rgmap = arg.init;
A3(:,1) = ones(length(te),1);
A3(:,2) = exp(j*F_fw.*te); % for calculate cost function
if arg.cost == 1
    length_cost = size(arg.x,2)*size(arg.y,2);
    costf = zeros(length_cost*2,arg.niter*2);
end
iteration = 0;

%% pixel independent method

for arr = arg.x
    if (rem(arr,100) == 0)
         disp(['# of iteration of RE = ' num2str(arr)])
    end

    for col = arg.y

        iteration = iteration+1;
        init_guess = arg.init(arr,col);
        [est_map,cost,iter] = regionpixel(data,arr,col,init_guess,te,c,d,A1,A2,A3,arg.niter,arg.cost);
        if arg.cost == 1
                costf((iteration*2-1):iteration*2,:) = cost;
                % cost(2*k-1,:) = field map values(record of estimated field map values in each iteration)
                % cost(2*k,:) = cost values
        end

        % My constraint
        if est_map > 90
            est_map = 90;
        elseif est_map <-80
            est_map = -80;
        end
        map((col-1)*n+arr) = est_map;
    end
end
if arg.cost == 1
    %% plot the cost functions (plot only the first pixel's cost function)
    % just plot costfunction(field map,water^,fat^),water^,fat^ is estimated
    % water, fat based on field map
    figure(5);
    plot(costf(1,(1:arg.niter)*2-1),costf(2,(1:arg.niter)*2-1),'o');title('cost function');
    hold on
    plot(costf(1,iter*2-1),costf(2,iter*2-1),'o','color','red'); % final estimated field map
    hold off

    %% plot the whole cost function (from -150Hz - 150Hz)
    a = linspace(-1200,1200,10000);
    orig_s = reshape(data(arg.x(1),arg.y(1),:),3,1);
    for k = 1:10000
        s =  orig_s.*(exp(-j*2*pi*a(k)*te).'); % shat (m is freq)
        shat = [real(s);imag(s)];
        rowhat = A2*shat;
        water = rowhat(1) + j*rowhat(2);
        fat = rowhat(3) + j*rowhat(4);
        temp(1,k) = norm(orig_s - A3*[water;fat].*(exp(j*2*pi*a(k)*te).'));
    end
    figure(6);
    plot(a,temp,'o');
    w = 0;
    f = 0;
    E_map = map;
    value = est_map;
else
    costf = 0;
    value = 0;
    E_map = zeros(n,n);
    E_map(find(map>0)) = map(find(map>0));
    E_map(find(map<0)) = map(find(map<0));
    E_map = lpass(E_map);
    E_map = E_map*2*pi;
    rgmap = rgmap*2*pi;
   [w f] = restore(data,E_map,F_fw,te);
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
%   - data                 3d-brains, Omega=(0,20)x(0,10)x(0,20), level=3:6, m=[128,64,128]
%   - viewer               imgmontage
%   - interpolation        linearInterMex
%   - distance             SSD
%   - pre-registration     affine3D
%   - regularizer          mfElastic
%   - optimizer            Gauss-Newton
% ===============================================================================

setup3DbrainData;

% extract data of level 4
level = 4; omega = ML{level}.omega; m = ML{level}.m;
imgModel('reset','imgModel','linearInterMex');
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega);
xc = getCellCenteredGrid(omega,m);
Rc = imgModel(R,omega,xc);

% initialize distance measure
distance('reset','distance','SSD');

% initialize the transformation and a starting guess
% trafo('reset','trafo','affine3Dsparse');
trafo('reset','trafo','splineTransformation3Dsparse',...
  'omega',omega,'p',[ 3 4 5],'m',m);
w0 = trafo('w0');

% setup plots and initialize
FAIRplots('reset','mode','PIR-GN','omega',omega,'m',m,'fig',1,'plots',1);
FAIRplots('init',struct('Tc',T,'Rc',R,'omega',omega,'m',m));

%------------------------------------------------------------------------------
% build objective function
% note: T  is data for template image
%       Rc is sampled reference image
%       optional Tikhonov-regularization is disabled by setting m = [], wRef = []
%       beta = 0, M = [], wRef = []:
%       disables additional regularization of Hessian approximation
beta = 0; M = []; wRef = [];
fctn = @(wc) PIRobjFctn(T,Rc,omega,m,beta,M,wRef,xc,wc);
fctn([]);   % report status

%------------------------------------------------------------------------------
%% -- solve the optimization problem on one level
[wc,his] = GaussNewton(fctn,w0,'solver','CG','Plots',@FAIRplots); return;

%------------------------------------------------------------------------------
%% finally: run the MultiLevel Non-Parametric Image Registration
[wc,his] = MLPIR(ML);

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