function [alphas,l,rho,d12,ix] = matRad_siddonRayTracer(isocenter, ...
                                    resolution, ...
                                    sourcePoint, ...
                                    targetPoint, ...
                                    cubes)
% siddon ray tracing through 3D cube to calculate the radiological depth
% according to Siddon 1985 Medical Physics
%
% call
%   [alphas,l,rho,d12,vis] = matRad_siddonRayTracer(isocenter, ...
%                               resolution, ...
%                               sourcePoint, ...
%                               targetPoint, ...
%                               cubes)
%
% input
%   isocenter:      isocenter within cube [voxels]
%   resolution:     resolution of the cubes [mm/voxel]
%   sourcePoint:    source point of ray tracing
%   targetPoint:    target point of ray tracing
%   cubes:          cell array of cubes for ray tracing (it is possible to pass
%                   multiple cubes for ray tracing to save computation time)
%
% output (see Siddon 1985 Medical Physics for a detailed description of the
% variales)
%   alphas          relative distance between start and endpoint for the
%                    intersections with the cube
%   l               lengths of intersestions with cubes
%   rho             densities extracted from cubes
%   d12             distance between start and endpoint of ray tracing
%   ix              indices of hit voxels
%
% References
%   [1] http://www.ncbi.nlm.nih.gov/pubmed/4000088
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2015 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Add isocenter to source and target point. Because the algorithm does not
% works with negatives values. This put (resolution.x,resolution.y,resolution.z)
% in the center of first voxel

sourcePoint = sourcePoint + isocenter;
targetPoint = targetPoint + isocenter;

% Save the numbers of planes.
[yNumPlanes, xNumPlanes, zNumPlanes] = size(cubes{1});
xNumPlanes = xNumPlanes + 1;
yNumPlanes = yNumPlanes + 1;
zNumPlanes = zNumPlanes + 1;

% eq 11
% Calculate the distance from source to target point.
d12 = norm(sourcePoint-targetPoint);

% eq 3
% Position of first planes in millimeter. 0.5 because the central position
% of the first voxel is at [resolution.x resolution.y resolution.z]
xPlane_1 = .5*resolution.x;
yPlane_1 = .5*resolution.y;
zPlane_1 = .5*resolution.z;

% Position of last planes in milimiter
xPlane_end = (xNumPlanes - .5)*resolution.x;
yPlane_end = (yNumPlanes - .5)*resolution.y;
zPlane_end = (zNumPlanes - .5)*resolution.z;

% eq 4
% Calculate parametrics values of \alpha_{min} and \alpha_{max} for every
% axis, intersecting the ray with the sides of the CT.
if targetPoint(1) ~= sourcePoint(1)
    aX_1   = (xPlane_1 - sourcePoint(1)) / (targetPoint(1) - sourcePoint(1));
    aX_end = (xPlane_end - sourcePoint(1)) / (targetPoint(1) - sourcePoint(1));
else
    aX_1   = [];
    aX_end = [];
end
if targetPoint(2) ~= sourcePoint(2)
    aY_1   = (yPlane_1 - sourcePoint(2)) / (targetPoint(2) - sourcePoint(2));
    aY_end = (yPlane_end - sourcePoint(2)) / (targetPoint(2) - sourcePoint(2));
else
    aY_1   = [];
    aY_end = [];
end
if targetPoint(3) ~= sourcePoint(3)
    aZ_1   = (zPlane_1 - sourcePoint(3)) / (targetPoint(3) - sourcePoint(3));
    aZ_end = (zPlane_end - sourcePoint(3)) / (targetPoint(3) - sourcePoint(3));
else
    aZ_1   = [];
    aZ_end = [];
end

% eq 5
% Compute the \alpha_{min} and \alpha_{max} in terms of parametric values
% given by equation 4.
alpha_min = max([0 min(aX_1,aX_end) min(aY_1,aY_end) min(aZ_1,aZ_end)]);
alpha_max = min([1 max(aX_1,aX_end) max(aY_1,aY_end) max(aZ_1,aZ_end)]);

% eq 6
% Calculate the range of indeces who gives parametric values for
% intersected planes.
if targetPoint(1) == sourcePoint(1)
    i_min = []; i_max = [];
elseif targetPoint(1) > sourcePoint(1)
    i_min = xNumPlanes - (xPlane_end - alpha_min * (targetPoint(1) - sourcePoint(1)) - sourcePoint(1))/resolution.x;
    i_max = 1          + (sourcePoint(1) + alpha_max * (targetPoint(1) - sourcePoint(1)) - xPlane_1)/resolution.x;
    % rounding
    i_min = ceil(1/1000*(round(1000*i_min)));
    i_max = floor(1/1000*(round(1000*i_max)));
else
    i_min = xNumPlanes - (xPlane_end - alpha_max * (targetPoint(1) - sourcePoint(1)) - sourcePoint(1))/resolution.x;
    i_max = 1          + (sourcePoint(1) + alpha_min * (targetPoint(1) - sourcePoint(1)) - xPlane_1)/resolution.x;
    i_min = ceil(1/1000*(round(1000*i_min)));
    i_max = floor(1/1000*(round(1000*i_max)));
end
if targetPoint(2) == sourcePoint(2)
    j_min = []; j_max = [];
elseif targetPoint(2) > sourcePoint(2)
    j_min = yNumPlanes - (yPlane_end - alpha_min * (targetPoint(2) - sourcePoint(2)) - sourcePoint(2))/resolution.y;
    j_max = 1          + (sourcePoint(2) + alpha_max * (targetPoint(2) - sourcePoint(2)) - yPlane_1)/resolution.y;
    j_min = ceil(1/1000*(round(1000*j_min)));
    j_max = floor(1/1000*(round(1000*j_max)));
else
    j_min = yNumPlanes - (yPlane_end - alpha_max * (targetPoint(2) - sourcePoint(2)) - sourcePoint(2))/resolution.y;
    j_max = 1          + (sourcePoint(2) + alpha_min * (targetPoint(2) - sourcePoint(2)) - yPlane_1)/resolution.y;
    j_min = ceil(1/1000*(round(1000*j_min)));
    j_max = floor(1/1000*(round(1000*j_max)));
end
if targetPoint(3) == sourcePoint(3)
    k_min = []; k_max = [];
elseif targetPoint(3) >= sourcePoint(3)
    k_min = zNumPlanes - (zPlane_end - alpha_min * (targetPoint(3) - sourcePoint(3)) - sourcePoint(3))/resolution.z;
    k_max = 1          + (sourcePoint(3) + alpha_max * (targetPoint(3) - sourcePoint(3)) - zPlane_1)/resolution.z;
    k_min = ceil(1/1000*(round(1000*k_min)));
    k_max = floor(1/1000*(round(1000*k_max)));
else
    k_min = zNumPlanes - (zPlane_end - alpha_max * (targetPoint(3) - sourcePoint(3)) - sourcePoint(3))/resolution.z;
    k_max = 1          + (sourcePoint(3) + alpha_min * (targetPoint(3) - sourcePoint(3)) - zPlane_1)/resolution.z;
    k_min = ceil(1/1000*(round(1000*k_min)));
    k_max = floor(1/1000*(round(1000*k_max)));
end

% eq 7
% For the given range of indices, calculate the paremetrics values who
% represents intersections of the ray with the plane.
if i_min ~= i_max
    if targetPoint(1) > sourcePoint(1)
        alpha_x = (resolution.x*(i_min:1:i_max)-sourcePoint(1)-.5*resolution.x)/(targetPoint(1)-sourcePoint(1));
    else
        alpha_x = (resolution.x*(i_max:-1:i_min)-sourcePoint(1)-.5*resolution.x)/(targetPoint(1)-sourcePoint(1));
    end
else
    alpha_x = [];
end
if j_min ~= j_max
    if targetPoint(2) > sourcePoint(2)
        alpha_y = (resolution.y*(j_min:1:j_max)-sourcePoint(2)-.5*resolution.y)/(targetPoint(2)-sourcePoint(2));
    else
        alpha_y = (resolution.y*(j_max:-1:j_min)-sourcePoint(2)-.5*resolution.y)/(targetPoint(2)-sourcePoint(2));
    end
else
    alpha_y = [];
end
if k_min ~= k_max
    if targetPoint(3) > sourcePoint(3)
        alpha_z = (resolution.z*(k_min:1:k_max)-sourcePoint(3)-.5*resolution.z)/(targetPoint(3)-sourcePoint(3));
    else
        alpha_z = (resolution.z*(k_max:-1:k_min)-sourcePoint(3)-.5*resolution.z)/(targetPoint(3)-sourcePoint(3));
    end
else
    alpha_z = [];
end

% eq 8
% Merge parametrics sets.
alphas = unique([alpha_min alpha_x alpha_y alpha_z alpha_max]);

% eq 10
% Calculate the voxel intersection length.
l = d12*diff(alphas);

% eq 13
% Calculate \alpha_{middle}
alphas_mid = .5*(alphas(1:end-1)+alphas(2:end));

% eq 12
% Calculate the voxel indices: first convert to physical coords
i_mm = sourcePoint(1) + alphas_mid*(targetPoint(1) - sourcePoint(1));
j_mm = sourcePoint(2) + alphas_mid*(targetPoint(2) - sourcePoint(2));
k_mm = sourcePoint(3) + alphas_mid*(targetPoint(3) - sourcePoint(3));
% then convert to voxel index
i = round(i_mm/resolution.x);
j = round(j_mm/resolution.y);
k = round(k_mm/resolution.z);

% Handle numerical instabilities at the borders.
i(i<=0) = 1; j(j<=0) = 1; k(k<=0) = 1;
i(i>xNumPlanes-1) = xNumPlanes-1;
j(j>yNumPlanes-1) = yNumPlanes-1;
k(k>zNumPlanes-1) = zNumPlanes-1;

% Convert to linear indices
ix = j + (i-1)*size(cubes{1},1) + (k-1)*size(cubes{1},1)*size(cubes{1},2);

% obtains the values from cubes
rho = cell(numel(cubes),1);
for i = 1:numel(cubes)
    rho{i} = cubes{i}(ix);
end


//Make some assumptions
// let's assume seq_len = 64 for now
// N d_mod d_ff h dk dv
// 6 512   2048 8 64 64
Constant Seq_Len 128;

Network Transformer {
	//Start encoder
	Layer MH_FC_DimReduce_VKQ_0 { //Batched FC layer, where seq_len is batch, input is d_model x 1
		Type: CONV
		Stride { X: 1, Y: 1 }	// k = 3 * 512, one for each VKQ
		Dimensions { N: Seq_Len, K: 1536, C: 1, R: 1, S: 512, Y:1, X:512 }
		Dataflow {
			SpatialMap(1,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			Cluster(1, P);
			SpatialMap(1,1) C;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
		}
	} //good

	Layer SD_MatMul_QK_00 { //Mat mul, batch is 1
		Type: CONV //MatMul -> M(seql)xK(dv)xN(seql)-> filter = Kx1(m chans), input = KxN
		Stride { X: 1, Y: 1 }
		//N=1, K(conv)=M(matr), C=1, R(conv)=K(matr),S=1,Y(conv)=K(matr), X(conv)=N(matr)
		Dimensions { N: 1, K: Seq_Len, C: 1, R: 64, S: 1, Y:64, X:Seq_Len }
		Dataflow {
			SpatialMap(2,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;

		}
	}

	Layer SD_MatMul_V_00 { //Mat mul, batch is 1
		Type: CONV //MatMul -> M(seql)xK(seql)xN(dv)-> filter = Kx1(m chans), input = KxN
		Stride { X: 1, Y: 1 }
		//N=1, K(conv)=M(matr), C=1, R(conv)=K(matr),S=1,Y(conv)=K(matr), X(conv)=N(matr)
		Dimensions { N: 1, K: Seq_Len, C: 1, R: Seq_Len, S: 1, Y:Seq_Len, X:64 }
		Dataflow {
			SpatialMap(1,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			Cluster(1, P);
			SpatialMap(1,1) C;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
		}
	}

	// done with h parallel sd layers now
	Layer MH_FC_DimRecast_0 { //Batched FC layer, where seq_len is batch, input is d_model x 1
		Type: CONV
		Stride { X: 1, Y: 1 }	// v,k,q have been combined
		Dimensions { N: Seq_Len, K: 512, C: 1, R: 1, S: 512, Y:1, X:512 }
		Dataflow {
			SpatialMap(1,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			Cluster(1, P);
			SpatialMap(1,1) C;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
		}
	} //good

	/// done with h parallel sd layers now
	Layer FF_A_0 { //Batched FC layer, where seq_len is batch, input is d_model x 1
		Type: CONV //2048 output neurons, 512->2048
		Stride { X: 1, Y: 1 }
		Dimensions { N: Seq_Len, K: 2048, C: 1, R: 1, S: 512, Y:1, X:512 }
		Dataflow {
			SpatialMap(1,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			Cluster(1, P);
			SpatialMap(1,1) C;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
		}
	} //good

	/// done with h parallel sd layers now
	Layer FF_B_0 { //Batched FC layer, where seq_len is batch, input is d_model x 1
		Type: CONV //2048 -> 512
		Stride { X: 1, Y: 1 }
		Dimensions { N: Seq_Len, K: 512, C: 1, R: 1, S: 2048, Y:1, X:2048 }
		Dataflow {
			SpatialMap(1,1) K;
			TemporalMap(1,1) C;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			Cluster(1, P);
			SpatialMap(1,1) C;
			TemporalMap(Sz(R),1) Y;
			TemporalMap(Sz(S),1) X;
			TemporalMap(Sz(R),Sz(R)) R;
			TemporalMap(Sz(S),Sz(S)) S;
		}
	} //good

}


function [u,p] = StokesLSCDGS(u,p,f,g,A,B,auxMat,elem,smootherOpt,Ai,Si,SSi,Res,Pro)
%% STOKESLSCDGSTRI Matrix-vecotr form DGS relaxation for Stokes eqns
%
%************************** Algorithm Description *************************
%
%  In matrix form, we need to solve the equations
%
%         Lx =  |A B' | |u| = |f|
%               |B -D | |p| = |0|
%  Distributive matrix will be given by
%
%         M = |I            B'|
%             |0 -inv(BB')BAB'|,
%
%  Therefore, the transformed matrix T will be
%
%         T = L*M = |A  PAB'               |    with P = I-B'inv(BB')B
%                   |B  BB'+D*inv(BB')BAB' |
%
%         tildeT = |Su   0|    with Su and Sp smoother for A and BB'.
%                  |B   Sp|
%
%  The matrix form DGS update can be written as
%
%    |uk+1|   |uk|                 |ru|     with ru = f-Auk -B'pk,
%    |    | = |  | + M*inv(tildeT)*|  |
%    |pk+1|   |pk|                 |rp|     and  rp = 0-Buk
%
%********************** End-of- Algorithm Description *********************
%
%  Created by Ming Wang (with discussion with Long Chen) at Jan, 2012.

%% Set up of smoothers
smoothingstep = smootherOpt.smoothingstep;
smootherSp = upper(smootherOpt.smootherSp);
smootherbarSp = upper(smootherOpt.smootherbarSp);
if isfield(smootherOpt,'smootherbarSpPara')
    smootherbarSpPara = smootherOpt.smootherbarSpPara;
else
    smootherbarSpPara = 1;
end
if strcmp(smootherbarSp,'VCYCLE') || strcmp(smootherSp,'VCYCLE')
    % mg Parameters for barSp
    optionmg.solvermaxit = 1;
%     optionmg.tol = 0.1;
    optionmg.solver = 'VCYCLE';
    optionmg.smoothingstep = 2;
    optionmg.printlevel=0;
    optionmg.setupflag = 0;
end

%% initialize smoother
Bt = auxMat.Bt;
BBt = auxMat.BBt;
BABt = auxMat.BABt;
Su = auxMat.Su;
Sp = auxMat.Sp;
Spt = auxMat.Spt;
DSp = auxMat.DSp;

%% DGS relaxation step
for k = 1: smoothingstep
    % Step 1: relax Momentum eqns
    u = u + Su\(f-Bt*p-A*u);
    % Step 2: relax transformed Continuity eqns
    rp = g - B*u;
    switch(smootherSp)
        case 'SGS'
            dq = Spt\(DSp.*(Sp\rp)); % symmetric Gauss-Seidel iteration
        case 'GS'
            dq = Sp\rp; % Gauss-Seidel iteration
        case 'VCYCLE'
            if exist('elem','var')
                dq = mg(BBt,rp,elem,optionmg,Ai,Si,SSi,Res,Pro); % mg vcycle
            else
                dq = amg(BBt,rp,optionmg); % mg vcycle
            end
    end
    % Step 3: transform the correction back to the original variables
    %   Step 3.1: velocity
    u = u + Bt*dq;
    %   Step 3.2: pressure
    dq = BABt*dq;
%     dq = dq - mean(dq);
    switch(smootherbarSp)
        case 'GS'
            dp = Spt\dq;
        case 'SGS'
            dp = Sp\(DSp.*(Spt\dq));
%             dp = dp + Sp\(DSp.*(Spt\(dq - BBt*dp)));
        case 'VCYCLE'
%            dp = BBt\dq;
            if exist('elem','var')
%                 dq =dq-mean(dq);
                dp = mg(BBt,dq,elem,optionmg,Ai,Si,SSi,Res,Pro); % mg vcycle;
            else
                dp = amg(BBt,dq,optionmg);
            end
    end
    p = p - smootherbarSpPara*dp;

end


function [Y X Z n m p T k1 k3 q1 q2 q3]=maprelim(data_endo,data_exo,const,lags,regimeperiods,names)

% function [Y X Z n m p T k1 k3 q1 q2 q3]=maprelim(data_endo,data_exo,const,lags)
% creates the preliminary values necessary for all the subsequent computations
% inputs:  - matrix 'data_endo': the matrix storing the endogenous time series data used to estimate the model
%          - matrix 'data_exo': the matrix storing the exogenous time series data used to estimate the model
%          - integer 'const': 0-1 value determining whether a constant term should be included in the model
%          - integer 'lags': the number of lags to include in the model
% outputs: - matrix 'Y': the matrix of endogenous variables, defined in (3.5.10)
%          - matrix 'X': the matrix of endogenous regressors, defined in (3.5.10)
%          - matrix 'Z': the matrix of exogenous regressors, defined in (3.5.10)
%          - integer 'n': the number of endogenous variables in the model
%          - integer 'm': the number of exogenous variables in the model
%          - integer 'p': the number of lags in the model
%          - integer 'T': the sample size, i.e. the number of time periods used to estimate the model
%          - integer 'k1': the number of coefficients related to the endogenous variables for each equation in the model
%          - integer 'k3': the number of coefficients related to the exogenous variables for each equation, in the reformulated model (3.5.5)
%          - integer 'q1': the total number of VAR coefficients related to the endogenous variables
%          - integer 'q2': the total number of VAR coefficients related to the exogenous variables
%          - integer 'q3': the total number of VAR coefficients related to the exogenous variables, in the reformulated model (3.5.5)

% first compute p, the number of lags in the model, defined p77
p=lags;

% then compute n, the number of endogenous variables in the model; it is simply the number of columns in the matrix 'data_endo'
n=size(data_endo,2);

% augment the matrix of exogenous with a column of ones to account for the constant
%data_exo=[ones(size(data_endo,1),1) data_exo];
%alternative data exo

if isempty(regimeperiods);
    data_exo=[ones(size(data_endo,1),1)];
else
data_exo=[ones(size(data_endo,1),1) zeros(size(data_endo,1),1)];
data_exo(find(strcmp(names(2:end,1),regimeperiods(1))):find(strcmp(names(2:end,1),regimeperiods(2))),2)=1;
data_exo(find(strcmp(names(2:end,1),regimeperiods(1))):find(strcmp(names(2:end,1),regimeperiods(2))),1)=0;
end

if isempty(regimeperiods);
    data_exo=[ones(size(data_endo,1),1)];
else
data_exo=[ones(size(data_endo,1),1) zeros(size(data_endo,1),1)];
data_exo(find(strcmp(names(2:end,1),regimeperiods(1))):find(strcmp(names(2:end,1),regimeperiods(2))),2)=1;
end
% then compute m, the number of exogenous variables in the model, defined p77
% if data_exo is empty, set m=0
m=size(data_exo,2);

% estimate k1, the number of parameters related to endogenous variables in each equation, defined p77
k1=n*p;

% estimate q1, the total number of parameters related to endogenous variables, defined p77
q1=n*k1;

% estimate q2, the total number of parameters related to exogenous variables, defined p77
q2=n*m;

% estimate k3, the number of parameters related to exogenous variables in each equation of the modified system, defined p78
k3=m*(p+1);

% estimate q3, the total number of parameters related to endogenous variables in the modified system, defined p78
q3=n*k3;

% obtain the matrices Y and X, defined in (3.6.10)
% to do so, use the lagx function on the data matrix
temp=bear.lagx(data_endo,lags);

% to build X, take off the n initial columns of current data
X=temp(:,n+1:end);

% save the n first columns of temp as Y
Y=temp(:,1:n);

% obtain the matrix Z, defined in (3.6.10)
temp=bear.lagx(data_exo,lags);
temp(:,m+1:end)=-temp(:,m+1:end);
Z=temp;

% Define T, the number of periods of the model, as the number of rows of Y
T=size(Y,1);

% obtain eventually y, Xbar and Zbar, as defined in (XXX)
y=Y(:);
Xbar=kron(eye(n),X);
Zbar=kron(eye(n),Z);


function gX = mlpKernGradX(kern, X, X2)

% MLPKERNGRADX Gradient of MLP kernel with respect to input locations.
% FORMAT
% DESC computes the gradident of the multi-layer perceptron
% kernel with respect to the input positions where both the row
% positions and column positions are provided separately.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x1 : row locations against which gradients are being computed.
% ARG x2 : column locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData2 x numInputs x numData1. Where numData1 is
% the number of data points in X1, numData2 is the number of data
% points in X2 and numInputs is the number of input
% dimensions in X.
%
% SEEALSO mlpKernParamInit, kernGradX, mlpKernDiagGradX
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006, 2009

% KERN

gX = zeros(size(X2, 1), size(X2, 2), size(X, 1));
for i = 1:size(X, 1);
  gX(:, :, i) = mlpKernGradXpoint(kern, X(i, :), X2);
end

function gX = mlpKernGradXpoint(kern, x, X2)

% MLPKERNGRADXPOINT Gradient with respect to one point of x.

innerProd = X2*x';
numer = innerProd*kern.weightVariance + kern.biasVariance;
vec1 = sum(x.*x, 2)*kern.weightVariance + kern.biasVariance + 1;
vec2 = sum(X2.*X2, 2)*kern.weightVariance + kern.biasVariance + 1;
denom = sqrt(vec2*vec1');
arg = numer./denom;
gX = zeros(size(X2));
twooverpi = 2/pi;
for j = 1:size(X2, 2)
  gX(:, j)=X2(:, j)./denom - vec2.*x(:, j).*numer./denom.^3;
  gX(:, j) = twooverpi*kern.weightVariance*kern.variance*gX(:, j)./sqrt(1-arg.*arg);
end


function [y,nz] = ompdenoise1(params,msgdelta)
%OMPDENOISE1 OMP denoising of 1-D signals.
%  OMPDENOISE1 denoises a 1-dimensional signal using OMP denoising. The
%  function syntax is identical to OMPDENOISE, but it runs significantly
%  faster on 1-D signals. OMPDENOISE1 requires somewhat more memory than
%  OMPDENOISE (approximately the size of the input signal), so if memory is
%  limited, OMPDENOISE can be used instead.
%
%  See also OMPDENOISE.

%  Ron Rubinstein
%  Computer Science Department
%  Technion, Haifa 32000 Israel
%  ronrubin@cs
%
%  August 2009

% parse input arguments %

x = params.x(:);
D = params.dict;
blocksize = params.blocksize;

% maxval %
if (isfield(params,'maxval'))
  maxval = params.maxval;
else
  maxval = 1;
end

% gain %
if (isfield(params,'gain'))
  gain = params.gain;
else
  gain = 1.15;
end

% maxatoms %
if (isfield(params,'maxatoms'))
  maxatoms = params.maxatoms;
else
  maxatoms = floor(prod(blocksize)/2);
end

% stepsize %
if (isfield(params,'stepsize'))
  stepsize = params.stepsize;
else
  stepsize = 1;
end
if (any(stepsize<1))
  error('Invalid step size.');
end

% noise mode %
if (isfield(params,'noisemode'))
  switch lower(params.noisemode)
    case 'psnr'
      sigma = maxval / 10^(params.psnr/20);
    case 'sigma'
      sigma = params.sigma;
    otherwise
      error('Invalid noise mode specified');
  end
elseif (isfield(params,'sigma'))
  sigma = params.sigma;
elseif (isfield(params,'psnr'))
  sigma = maxval / 10^(params.psnr/20);
else
  error('Noise strength not specified');
end

% lambda %
if (isfield(params,'lambda'))
  lambda = params.lambda;
else
  lambda = maxval/(10*sigma);
end

% msgdelta %
if (nargin <2)
  msgdelta = 5;
end
if (msgdelta<=0)
  msgdelta = -1;
end

epsilon = sqrt(prod(blocksize)) * sigma * gain;   % target error for omp

MEM_LOW = 1;
MEM_NORMAL = 2;
MEM_HIGH = 3;

if (isfield(params,'memusage'))
  switch lower(params.memusage)
    case 'low'
      memusage = MEM_LOW;
    case 'normal'
      memusage = MEM_NORMAL;
    case 'high'
      memusage = MEM_HIGH;
    otherwise
      error('Invalid memory usage mode');
  end
else
  memusage = MEM_NORMAL;
end

% denoise the signal %

if (memusage >= MEM_NORMAL)
  G = D'*D;
end

% process the signal in batches to conserve memory
% choose batchsize so im2col returns a matrix of approximately the same
% size as the signal
batchsize = ceil(length(x)*stepsize/blocksize + blocksize);

y = zeros(size(x));
ids = 1:min(batchsize,length(x));
nz = 0;

tid = timerinit('ompdenoise', length(x));
while (length(ids)>=blocksize)

  % extract the signal blocks
  blocks = im2col(x(ids),[blocksize 1],'sliding');
  blocks = blocks(:,1:stepsize:end);

  % remove DC
  [blocks, dc] = remove_dc(blocks,'columns');

  % denoise the blocks
  if (memusage == MEM_LOW)
    gamma = omp2(D,blocks,[],epsilon,'maxatoms',maxatoms);
  else
    gamma = omp2(D,blocks,G,epsilon,'maxatoms',maxatoms);
  end
  nz = nz + nnz(gamma);
  cleanblocks = add_dc(D*gamma, dc, 'columns');

  y(ids) = y(ids) + col2imsum(cleanblocks, [blocksize 1], [length(ids) 1], [stepsize 1]);
  ids = ids + floor((batchsize-blocksize)/stepsize)*stepsize + stepsize;
  if (ids(end)>length(x))
    ids = ids(ids<=length(x));
  end

  % display status
  if (msgdelta>0 && ~isempty(ids)>0)
    timereta(tid, ids(1), msgdelta);
  end

end

if (msgdelta>0)
  timereta(tid, length(x));
end

cnt = countcover(size(x),[blocksize 1],[stepsize 1]);
y = (y+lambda*x)./(cnt + lambda);
y = reshape(y,size(params.x));


function X = ttm(X,V,varargin)
%TTM Tensor times matrix for ktensor.
%
%   Y = TTM(X,A,N) computes the n-mode product of the ktensor X with a
%   matrix A; i.e., X x_N A.  The integer N specifies the dimension
%   (or mode) of X along which A should be multiplied.  If size(A) =
%   [J,I], then X must have size(X,N) = I.  The result will be a
%   ktensor of the same order and size as X except that size(Y,N) = J.
%
%   Y = TTM(X,{A,B,C,...}) computes the n-mode product of the ktensor
%   X with a sequence of matrices in the cell array.  The n-mode
%   products are computed sequentially along all dimensions (or modes)
%   of X. The cell array contains ndims(X) matrices.
%
%   Y = TTM(X,{A,B,C,...},DIMS) computes the sequence tensor-matrix
%   products along the dimensions specified by DIMS.
%
%   Y = TTM(...,'t') performs the same computations as above except
%   the matrices are transposed.
%
%   Examples
%   X = ktensor({rand(5,2),rand(3,2),rand(4,2),rand(2,2)});
%   A = rand(4,5); B = rand(4,3); C = rand(3,4); D = rand(3,2);
%   Y = ttm(X, A, 1)         %<-- computes X times A in mode-1
%   Y = ttm(X, {A,B,C,D}, 1) %<-- same as above
%   Y = ttm(X, A', 1, 't')   %<-- same as above
%   Y = ttm(X, {A,B,C,D}, [1 2 3 4]) %<-- 4-way multiply
%   Y = ttm(X, {D,C,B,A}, [4 3 2 1]) %<-- same as above
%   Y = ttm(X, {A,B,C,D})            %<-- same as above
%   Y = ttm(X, {A',B',C',D'}, 't')   %<-- same as above
%   Y = ttm(X, {C,D}, [3 4])     %<-- X times C in mode-3 & D in mode-4
%   Y = ttm(X, {A,B,C,D}, [3 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, [1 2 4])   %<-- 3-way multiply
%   Y = ttm(X, {A,B,C,D}, [1 2 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, -3)        %<-- same as above
%   Y = ttm(X, {A,B,C,D}, -3)      %<-- same as above
%
%   See also KTENSOR, KTENSOR/ARRANGE, TENSOR/TTM
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%%%%%%%%%%%%%%%%%%%%%%
%%% ERROR CHECKING %%%
%%%%%%%%%%%%%%%%%%%%%%

% Check the number of arguments
if (nargin < 2)
    error('TTM requires at least two arguments.');
end

% Check for transpose option
isTranspose = false;
if numel(varargin) > 0
  if isnumeric(varargin{1});
    dims = varargin{1};
  end
  isTranspose =  (ischar(varargin{end}) && (varargin{end} == 't'));
end

% Check for dims argument
if ~exist('dims','var')
    dims = [];
end

% Check that 2nd argument is cell array. If not, recall with V as a
% cell array with one element.
if ~iscell(V)
    X = ttm(X,{V},dims,varargin{end});
    return;
end

% Get sorted dims and index for multiplicands
[dims,vidx] = tt_dimscheck(dims,ndims(X),numel(V));

% Determine correct size index
if isTranspose
  j = 1;
else
  j = 2;
end

% Check that each multiplicand is the right size.
for i = 1:numel(dims)
    if (ndims(V) ~= 2) || (size(V{vidx(i)},j) ~= size(X,dims(i)))
disp(size(V{vidx(i)}))
disp(size(X))

        error('Multiplicand is wrong size');
    end
end

% Do the multiplications in the specified modes.
for i = 1:numel(dims)
  if isTranspose
    X.u{dims(i)} = V{vidx(i)}'* X.u{dims(i)};
  else
    X.u{dims(i)} = V{vidx(i)} * X.u{dims(i)};
  end
end


function [ num_int, pint ] = plane_imp_triangle_int_add_3d ( p1, p2, dist1, ...
  dist2, num_int, pint )

%*****************************************************************************80
%
%% PLANE_IMP_TRIANGLE_INT_ADD_3D is a utility for plane/triangle intersections.
%
%  Discussion:
%
%    This routine is called to consider the value of the signed distance
%    from a plane of two nodes of a triangle.  If the two values
%    have opposite signs, then there is a point of intersection between
%    them.  The routine computes this point and adds it to the list.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(3), P2(3), the coordinates of two vertices
%    of a triangle.
%
%    Input, real DIST1, DIST2, the signed distances of the
%    two vertices from a plane.
%
%    Input, integer NUM_INT, the number of intersection points.
%
%    Input, real PINT(3,NUM_INT), the intersection points.
%
%    Output, integer NUM_INT, the updated number of intersection points.
%
%    Output, real PINT(3,NUM_INT), the updated intersection points.
%
  dim_num = 3;

  if ( dist1 == 0.0 )
    num_int = num_int + 1;
    pint(1:dim_num,num_int) = p1(1:dim_num);
  elseif ( dist2 == 0.0 )
    num_int = num_int + 1;
    pint(1:dim_num,num_int) = p2(1:dim_num);
  elseif ( dist1 * dist2 < 0.0 )
    alpha = dist2 / ( dist2 - dist1 );
    num_int = num_int + 1;
    pint(1:dim_num,num_int) = alpha * p1(1:dim_num) + ( 1.0 - alpha ) * p2(1:dim_num);
  end

  return
end


%% FUNCTION Least_Trace
%   Trace-Norm Regularized Learning with Least Squares Loss.
%
%% OBJECTIVE
%   argmin_W { sum_i^t (0.5 * norm (Y{i} - X{i}' * W(:, i))^2)
%            + rho1 \|W\|_*}
%   where \|W\|_* = sum(svd(W, 0)) is the trace norm
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   rho1: trace norm regularization parameter
%
%% OUTPUT
%   W: model: d * t
%   funcVal: function value vector.
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] Ji, S. and Ye, J. An Accelerated Gradient Method for Trace Norm Minimization, ICML 2009
%
%% RELATED FUNCTIONS
%   Logistic_Trace, init_opts

%% Code starts here
function [W, funcVal] = Least_Trace(X, Y, rho1, opts)

if nargin <3
    error('\n Inputs: X, Y, and rho1 should be specified!\n');
end
X = multi_transpose(X);

if nargin <4
    opts = [];
end

% initialize options.
opts=init_opts(opts);

if isfield(opts, 'rho_L2')
    rho_L2 = opts.rho_L2;
else
    rho_L2 = 0;
end

task_num  = length (X);
dimension = size(X{1}, 1);
funcVal = [];

% precomputation.
XY = cell(task_num, 1);
W0_prep = [];
for t_idx = 1: task_num
    XY{t_idx} = X{t_idx}*Y{t_idx};
    W0_prep = cat(2, W0_prep, XY{t_idx});
end

% initialize a starting point
if opts.init==2
    W0 = zeros(dimension, task_num);
elseif opts.init == 0
    W0 = W0_prep;
else
    if isfield(opts,'W0')
        W0=opts.W0;
        if (nnz(size(W0)-[dimension, task_num]))
            error('\n Check the input .W0');
        end
    else
        W0=W0_prep;
    end
end

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

Wz= W0;
Wz_old = W0;

t = 1;
t_old = 0;

iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ws = (1 + alpha) * Wz - alpha * Wz_old;

    % compute function value and gradients of the search point
    gWs  = gradVal_eval(Ws);
    Fs   = funVal_eval  (Ws);

    while true
        [Wzp Wzp_tn] = trace_projection(Ws - gWs/gamma, 2 * rho1 / gamma);
        Fzp = funVal_eval  (Wzp);

        delta_Wzp = Wzp - Ws;
        r_sum = norm(delta_Wzp, 'fro')^2;
        %Fzp_gamma = Fs + trace(delta_Wzp' * gWs) + gamma/2 * norm(delta_Wzp, 'fro')^2;
        Fzp_gamma = Fs + sum(sum(delta_Wzp .* gWs)) + gamma/2 * norm(delta_Wzp, 'fro')^2;

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

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Wz_old = Wz;
    Wz = Wzp;

    %funcVal = cat(1, funcVal, Fzp + rho1 * sum( svd(Wzp, 0) ));
    funcVal = cat(1, funcVal, Fzp + rho1 * Wzp_tn);

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

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

W = Wzp;

% private functions
    function [grad_W] = gradVal_eval(W)
        if opts.pFlag
            grad_W = zeros(size(W));
            parfor t_ii = 1:task_num
                XWi = X{t_ii}' * W(:,t_ii);
                XTXWi = X{t_ii}* XWi;
                grad_W(:, t_ii) = XTXWi - XY{t_ii};
                %grad_W = cat(2, grad_W, X{t_ii}*(X{t_ii}' * W(:,t_ii)-Y{t_ii}) );
            end
        else
            grad_W = [];
            for t_ii = 1:task_num
                XWi = X{t_ii}' * W(:,t_ii);
                XTXWi = X{t_ii}* XWi;
                grad_W = cat(2, grad_W, XTXWi - XY{t_ii});
                %grad_W = cat(2, grad_W, X{t_ii}*(X{t_ii}' * W(:,t_ii)-Y{t_ii}) );
            end
        end
        grad_W = grad_W + rho_L2 * 2 * W;
    end

    function [funcVal] = funVal_eval (W)
        funcVal = 0;
        if opts.pFlag
            parfor i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        else
            for i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        end
        funcVal = funcVal + rho_L2 * norm(W, 'fro')^2;
    end

end


% function x = synsq_cwt_iw(Tx, fs, opt)
%
% Inverse Synchrosqueezing transform of Tx with associated
% frequencies in fs.  This implements Eq. 5 of [1].
%
% 1. E. Brevdo, N.S. Fučkar, G. Thakur, and H-T. Wu, "The
% Synchrosqueezing algorithm: a robust analysis tool for signals
% with time-varying spectrum," 2011.
%
% Input:
%   Tx, fs: See help synsq_cwt_fw
%   opt: options structure (see help synsq_cwt_fw)
%      opt.type: type of wavelet used in synsq_cwt_fw
%
%      other wavelet options (opt.mu, opt.s) should also match
%      those used in synsq_cwt_fw
%
% Output:
%   x: reconstructed signal
%
% Example:
%   [Tx,fs] = synsq_cwt_fw(t, x, 32); % Synchrosqueezing
%   Txf = synsq_filter_pass(Tx, fs, -Inf, 1); % Pass band filter
%   xf = synsq_cwt_iw(Txf, fs);  % Filtered signal reconstruction
%
%---------------------------------------------------------------------------------
%    Synchrosqueezing Toolbox
%    Authors: Eugene Brevdo (http://www.math.princeton.edu/~ebrevdo/)
%---------------------------------------------------------------------------------
function x = synsq_cwt_iw(Tx, fs, opt)
    if nargin<3, opt = struct(); end
    if ~isfield(opt, 'type'), opt.type = 'morlet'; end

    % Find the admissibility coefficient Cpsi
    Css = synsq_adm(opt.type, opt);

    [na, N] = size(Tx);

    % Integration
    % Due to linear discretization of integral in log(fs), this becomes
    % a simple normalized sum.
    x = real(1/Css * ones(1,na) * Tx);
end


function [result]=sv_timeperdiff(mCatalog, fTimePeriod)
% function [result]=sv_timeperdiff(mCatalog, fTimePeriod)
% --------------------------------------------------------------------------------------
% Function to calculate absolute difference of number of events between
% two time periods of an earthquake catalog
% Author: J. Woessner
% woessner@seismo.ifg.ethz.ch
% last update: 08.07.02
%
% Incoming variables:
% mCatalog           : current earthquake catalog
% params.fTimePeriod : Time period in days
%
% Outgoing variable:
% result.dNdiffsumVal      : total difference of number of events in the two time periods
% result.dNdiffsumYearVal  : total difference of number of events in the two time periods normalized to a year
% result.dNdiffsumMonthVal : total difference of number of events in the two time periods normalized to a month
% result.dNdiff            : Difference of seismicity in 0.1 magnitude bins

% Init variable
result=[];

% Create the catalogs for two time periods
fStartTime = min(mCatalog(:,3));
fEndTime = max(mCatalog(:,3));

[result.mFirstCatalog, result.mSecondCatalog, result.fFirstPeriodExact, result.fSecondPeriodExact, result.fFirstPeriod,...
        result.fSecondPeriod] = ex_SplitCatalog(mCatalog, fSplitTime, 0, 100, 0, 100);

[dEv_val dMags dEv_valsum dEv_valsum_rev,  dMags_rev] =fcumulsum(result.mFirstCatalog);
[dEv_val2 dMags2 dEv_valsum2 dEv_valsum_rev2,  dMags_rev2] =fcumulsum(result.mSecondCatalog);

result.dNdiff = dEv_val2-dEv_val;
result.dNdiffYear=result.dNdiff/365;
result.dNdiffMonth = result.dNdiffYear/12;

result.dNdiffsum = cumsum(result.dNdiff');
result.dNdiffsumVal = result.dNdiffsum(length(result.dNdiffsum));
result.dNdiffsumYearVal = result.dNdiffsumVal/365;
result.dNdiffsumMonthVal = result.dNdiffsumYearVal/12;

result.dMags = dMags;
result.dMags = dMags2;
return


function [sys,x0,str,ts]=ADRC_3(t,x,u,flag,h,TD,ESO,NLSEF,b0)

switch flag
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes(h);
    case 2
        sys=mdlUpdate(x,u,h,TD,ESO,b0);
    case 3
        sys=mdlOutputs(x,NLSEF,b0);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,h);
    case {1,9}
        sys=[];
    otherwise
        error(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes(h)
    sizes=simsizes;
    sizes.NumContStates=0;
    sizes.NumDiscStates=7;
    sizes.NumOutputs=2;
    sizes.NumInputs=3;
    sizes.DirFeedthrough=1;
    sizes.NumSampleTimes=1;
    sys=simsizes(sizes);
    x0=[0;0;0;0;0;0;0];
    str=[];
    ts=[h 0];
function sys=mdlUpdate(x,u,h,TD,ESO,b0)
    e1=x(1)-u(1);
    fh=fhan(e1,x(2),TD(1),TD(2));
    sys(1)=x(1)+h*x(2);
    sys(2)=x(2)+h*x(3);
    sys(3)=x(3)+h*fh;
    e2=x(4)-u(2);
    sys(4)=x(4)+h*(x(5)-ESO(1)*e2);
    sys(5)=x(5)+h*(x(6)-ESO(2)*fal(e2,0.5,ESO(5)));
    sys(6)=x(6)+h*(x(7)-ESO(3)*fal(e2,0.25,ESO(5))+b0*u(3));
    sys(7)=x(7)+h*(-ESO(4)*fal(e2,0.125,ESO(5)));

%     fe=fal(e,0.5,Delta);
%     fe1=fal(e,0.25,Delta);
%     sys(1,1)=x(1)+h2*(x(2)-BB(1)*e);
%     sys(2,1)=x(2)+h2*(x(3)-BB(2)*fe+u(1));
%     sys(3,1)=x(3)+h2*(-BB(3)*fe1);
function sys=mdlOutputs(x,NLSEF,b0)
    e3=x(1)-x(4);
    e4=x(2)-x(5);
    e5=x(3)-x(6);
    %sys(1)=NLSEF(1)*e3+NLSEF(2)*e4+NLSEF(3)*e5-x(7)/b0;
    sys(1)=NLSEF(1)*fal(e3,0.5,NLSEF(4))+NLSEF(2)*fal(e4,0.75,NLSEF(4))+NLSEF(3)*fal(e5,1.5,NLSEF(4))-x(7)/b0;
    sys(2)=x(1);
function sys=mdlGetTimeOfNextVarHit(t,h)
    sys=t+h;

function y=fhan(x1,x2,r,h)
d=r*h;
d0=h*d;
y=x1+h*x2;
a0=sqrt(d^2+8*r*abs(y));
if abs(y)>d0
    a=x2+(a0-d)*sign(y)/2;
else
    a=x2+y/h;
end

if abs(a)>d
    y=-r*sign(a);
else
    y=-r*a/d;
end

function f=fal(e,a,d)
    if abs(e)<d
        f=e*d^(a-1);
    else f=(abs(e))^a*sign(e);
    end


function [E,ER] = arap_energy(V,F,U,varargin)
  % ARAP_ENERGY This function is meant as a very human readable groundtruth for
  % evaluating ARAP energies.
  % For faster energy computation see `arap_gradient.m`
  %
  % Inputs:
  %    V  #V by dim list of mesh vertex rest positions
  %    F  #F by simplex list of element indices into V
  %    U  #V by dim list of deformed vertex positions
  %   Optional:
  %     'Energy' followed by 'spokes','spokes-and-rims', or 'elements'
  %       {'elements' or 'spokes-and-rims'} for tets or triangles respectively.
  % Outputs:
  %    E  scalar energy
  %    ER  #R list of "per-rotation" energy contributions
  %
  %
  % Known issues:
  % If E_G is the energy returned by arap_gradient, then
  % 'spokes-and-rims' energy E = 3 E_G
  % 'spokes' energy E = 2 E_G
  % 'elements' energy E = E_G
  %

  switch size(F,2)
  case 4
    energy = 'elements';
  case 3
    energy = 'spokes-and-rims';
  end
  % default values
  % Map of parameter names to variable names
  params_to_variables = containers.Map( {'Energy'}, ...
    {'energy'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  dim = size(V,2);
  C = cotangent(V,F);

  n = size(V,1);
  m = size(F,1);
  switch energy
  case 'spokes'
    nr = size(V,1);
    edge_set = cell(nr,1);
    switch size(F,2)
    case 4
      AC = sparse( ...
        F(:,[2 3 1 4 4 4 3 1 2 1 2 3]),F(:,[3 1 2 1 2 3 2 3 1 4 4 4]), ...
        [C C],n,n);
    case 3
      AC = sparse(F(:,[2 3 1 3 1 2]),F(:,[3 1 2 2 3 1]),[C C],n,n);
    end
    % loop over vertices
    for r = 1:nr
      [Jr,~,Cr] = find(AC(:,r));
      Ir = repmat(r,numel(Jr),1);
      edge_set{r}.E = [Ir Jr];
      edge_set{r}.C = Cr;
    end
  case {'elements','spokes-and-rims'}
    nr = size(F,1);
    switch size(F,2)
    case 3
      I = F(:,[2 3 1]);
      J = F(:,[3 1 2]);
    case 4
      I = F(:,[2 3 1 4 4 4]);
      J = F(:,[3 1 2 1 2 3]);
    end
    edge_set = cell(nr,1);
    % loop over elements
    for r = 1:nr
      edge_set{r}.E = [I(r,:)' J(r,:)'];
      edge_set{r}.C = C(r,:);
    end
    if strcmp(energy,'spokes-and-rims')
      Fedge_set = edge_set;
      nr = size(V,1);
      edge_set = cell(nr,1);
      V2F = sparse( ...
        F,repmat(1:size(F,1),size(F,2),1)',repmat(1:size(F,2),size(F,1),1),n,m);
      % loop over vertices
      for r = 1:nr
        [~,Fr] = find(V2F(r,:));
        edge_set{r}.E = [];
        edge_set{r}.C = [];
        for f = Fr
          edge_set{r}.E = [edge_set{r}.E;Fedge_set{f}.E];
          edge_set{r}.C = [edge_set{r}.C Fedge_set{f}.C];
        end
      end

    end

  end

  ER = zeros(nr,1);
  % loop over rotations
  for r = 1:nr
    % get edges for this rotation
    Er = edge_set{r}.E;
    Cr = edge_set{r}.C;
    S = zeros(dim,dim);
    % loop over edges to build covariance matrix for rotation fitting
    for e = 1:size(Er,1)
      i = Er(e,1);
      j = Er(e,2);
      ev = V(j,:)-V(i,:);
      eu = U(j,:)-U(i,:);
      S = S + Cr(e) * eu'*ev;
    end
    R = fit_rotation(S);
    % Loop over edges to compute energy contribution
    for e = 1:size(Er,1)
      i = Er(e,1);
      j = Er(e,2);
      ev = V(j,:)-V(i,:);
      eu = U(j,:)-U(i,:);
      %ER(r) = ER(r) + 0.5 * Cr(e) * (eu - ev*R)*(eu - ev*R)';
      %ER(r) = ER(r) + 0.5 * Cr(e) * (eu*eu' - 2*ev*R*eu' + ev*R*R'*ev');
      ER(r) = ER(r) + 0.5 * Cr(e) * (eu*eu' - 2*ev*R*eu' + ev*ev');
    end
  end
  E = sum(ER);
end


function [x, infos] = incremental_mu_nmf(V, rank, in_options)
% Incremental non-negative matrix factorization (NMF) with outliers (INCNMF) algorithm.
%
% Inputs:
%       matrix      V
%       rank        rank
%       in_options  options
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       S. S. Bucak, B. Gunsel,
%       "Incremental Subspace Learning via Non-negative Matrix Factorization,"
%       Pattern Recognition, 2009.
%
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai on Feb. 12, 2017
%
% Change log:
%
%       Oct. 27, 2017 (Hiroyuki Kasai): Fixed algorithm.
%
%       May. 20, 2019 (Hiroyuki Kasai): Added initialization module.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.max_inneriter     = 1;
    local_options.online            = 0;
    local_options.tolcostdegrease   = 1e-8;
    local_options.alpha             = 0.5;
    local_options.beta              = 0.5;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W;
    H = init_factors.H;

    % initialize
    method_name = 'Incremental-MU';
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    if options.online
        ht = H(:, end); %why
    else
        % Do nothing
    end
    A = V * H';
    B = H * H';

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, W, H, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main outer loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        % main inner loop
        for t = 1 : options.batch_size : n - 1

            vt = V(:, t:t+options.batch_size -1);

            % Need to be considered more carefully
            if options.online
                % Do nothing (??)
            else
                ht = H(:, t:t+options.batch_size-1);
            end

            for j = 1 : options.max_inneriter

                % Compute new ht (column vector at t)
                ht = ht .* (W' * vt) ./ (W' * (W * ht) + 1e-9);
                ht = ht + (ht<eps) .* eps;

                % Compute new W
                vht = vt * ht';
                hht = ht * ht';
                A_tmp = options.beta * A + options.alpha * vht;
                B_tmp = options.beta * B + options.alpha * hht;
                W = W .* ( A_tmp ./ (W * B_tmp + 1e-9) ); % Add 1e-9 to avoid 0 in the denom.
                W = W + (W<eps) .* eps;

               if j > 1
                    oldobj = newobj;
                end
                newobj = ((sum(sum((vt-W*ht).^2)))/m);

                if options.verbose > 2
                    fprintf('\t[%d-%d-%d] %e\n', epoch, t, j, newobj);
                end

                if j > 1 && (oldobj-newobj)/newobj < options.tolcostdegrease
                    break;
                end

            end

            grad_calc_count = grad_calc_count + m * options.batch_size;

            % update A and B
            A = options.beta * A + options.alpha * vht;
            B = options.beta * B + options.alpha * hht;

            % update ht (columb of H at t)
            H(:,t:t+options.batch_size-1) = ht;

            if options.verbose > 2
                % measure cost
                f_val = nmf_cost(V, W, H, []);
                fprintf('%s: inner [%03d-%03d] %e\n', method_name, epoch, t, f_val);
            end
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, W, H, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = W;
    x.H = H;

end


function [beta_gibbs,F_gibbs,L_gibbs,phi_gibbs,sigma_gibbs,lambda_t_gibbs,sigma_t_gibbs,sbar,favar,It,Bu]=...
    favar_stvol3gibbs(Xbart,Xt,yt,B0,phi0,alpha0,delta0,f0,upsilon0,betahat,sigmahat,gamma,G,I_o,omega,T,n,k,It,Bu,pick,pickf,favar,data_endo,lags)

%% preliminaries
% initialise variables
nfactorvar=favar.nfactorvar;
numpc=favar.numpc;
favarX=favar.X(:,favar.plotX_index);
onestep=favar.onestep;
Sigma=bear.nspd(favar.Sigma);
Ll=favar.L;
favar_X=favar.X;
% load priors
L0=favar.L0*eye(n);
a0=favar.a0;
b0=favar.b0;
% sigmahat=(1/T)*(EPS'*EPS);

% preallocation
Ll_gibbs=zeros(size(Ll(:),1),It-Bu);
R2_gibbs=zeros(size(favarX,2),It-Bu);

if onestep==0 %static factors in this case
    FY=data_endo;
    pbstring='two-step'; %string for the progress bar
    % elseif onestep==1
    %     pbstring='one-step'; %string for the progress bar
end

% preliminary elements for the algorithm
% compute the product G'*I_gamma*G (to speed up computations of deltabar)
GIG=G'*I_o*G;
% compute alphabar
alphabar=T+alpha0;

% initiate the Gibbs sampler
% initiate the counting of iterations
count=1;
pickcount=1;
% initiate the record matrices and cells
beta_gibbs=[];
F_gibbs=[];
L_gibbs=[];
phi_gibbs=[];
sigma_gibbs=[];
lambda_t_gibbs={};
sigma_t_gibbs={};

% step 1: determine initial values for the algorithm

% initial value for beta
beta=betahat;
B=reshape(beta,k,n);
% initial value for f_2,...,f_n
% obtain the triangular factorisation of sigmahat
[Fhat Lambdahat]=bear.triangf(sigmahat);
% obtain the initial value for F
F=Fhat;
% obtain the inverse of Fhat
[invFhat]=bear.invltod(Fhat,n);
% create the cell storing the different vectors of invF
Finv=cell(n,1);
% store the vectors
for ii=2:n
    Finv{ii,1}=invFhat(ii,1:ii-1);
end
% initial values for L
L=zeros(T,1);
% initial values for phi
phi=1;

% step 2: determine the sbar values and Lambda
sbar=diag(Lambdahat);
Lambda=sparse(diag(sbar));
% then determine sigma^(0)
sigma=F*Lambda*F';

% step 3: recover the series of initial values for lambda_1,...,lambda_T and sigma_1,...,sigma_T
lambda_t=repmat(diag(sbar),1,1,T);
sigma_t=repmat(sigmahat,1,1,T);

% create a progress bar
hbar = bear.parfor_progressbar(It,['Progress of the Gibbs sampler (',pbstring,').']);

%% run the Gibbs sampler
while count<=It

    % step 4: draw beta from its conditional posterior
    % first compute the summations required for omegabar and betabar
    summ1=zeros(k,k);
    summ2=zeros(k,n);
    % run the summation
    for jj=1:T
        prodt=Xt{jj,1}'*exp(-L(jj,1));
        summ1=summ1+prodt*Xt{jj,1};
        summ2=summ2+prodt*yt(:,:,jj)';
    end
    % then obtain the inverse of phi0
    invphi0=diag(1./diag(phi0));
    % obtain the inverse of phibar
    invphibar=summ1+invphi0;
    % recover phibar
    C=chol(bear.nspd(invphibar),'Lower')';
    invC=C\speye(k);
    phibar=invC*invC';
    % recover Bbar
    Bbar=phibar*(summ2+invphi0*B0);
    % draw B from its posterior
    B=bear.matrixndraw(Bbar,sigma,phibar,k,n);
    % finally recover beta by vectorising
    beta=B(:);

    % step 5: draw the series f_2,...,f_n from their conditional posteriors
    % recover first the residuals
    for jj=1:T
        epst(:,:,jj)=yt(:,:,jj)-Xbart{jj,1}*beta;
    end
    % then draw the vectors in turn
    for jj=2:n
        % first compute the summations required for upsilonbar and fbar
        summ1=zeros(jj-1,jj-1);
        summ2=zeros(jj-1,1);
        % run the summation
        for kk=1:T
            prodt=epst(1:jj-1,1,kk)*exp(-L(kk,1));
            summ1=summ1+prodt*epst(1:jj-1,1,kk)';
            summ2=summ2+prodt*epst(jj,1,kk)';
        end
        summ1=(1/sbar(jj,1))*summ1;
        summ2=(-1/sbar(jj,1))*summ2;
        % then obtain the inverse of upsilon0
        invupsilon0=diag(1./diag(upsilon0{jj,1}));
        % obtain upsilonbar
        invupsilonbar=summ1+invupsilon0;
        C=chol(bear.nspd(invupsilonbar));
        invC=C\speye(jj-1);
        upsilonbar=full(invC*invC');
        % recover fbar
        fbar=upsilonbar*(summ2+invupsilon0*f0{jj,1});
        % finally draw f_i^(-1)
        Finv{jj,1}=fbar+chol(bear.nspd(upsilonbar),'lower')*randn(jj-1,1);
    end
    % recover the inverse of F
    invF=eye(n);
    for jj=2:n
        invF(jj,1:jj-1)=Finv{jj,1};
    end
    % eventually recover F
    F=bear.invltod(invF,n);
    % update sigma
    sigma=F*Lambda*F';

    % step 6: draw phi from its conditional posterior
    % estimate deltabar
    deltabar=L'*GIG*L+delta0;
    % draw the value phi_i
    phi=bear.igrandn(alphabar/2,deltabar/2);

    % step 7: draw the series lambda_t from their conditional posteriors, t=1,...,T
    % consider periods in turn
    for kk=1:T
        % a candidate value will be drawn from N(lambdabar,phibar)
        % the definitions of lambdabar and phibar varies with the period, thus define them first
        % if the period is the first period
        if kk==1
            lambdabar=(gamma*L(2,1))/(1/omega+gamma^2);
            phibar=phi/(1/omega+gamma^2);
            % if the period is the final period
        elseif kk==T
            lambdabar=gamma*L(T-1,1);
            phibar=phi;
            % if the period is any period in-between
        else
            lambdabar=(gamma/(1+gamma^2))*(L(kk-1,1)+L(kk+1,1));
            phibar=phi/(1+gamma^2);
        end
        % now draw the candidate
        cand=lambdabar+phibar^0.5*randn;
        % compute the acceptance probability
        prob=bear.mhprob3(cand,L(kk,1),sbar,epst(:,1,kk),Finv,n);
        % draw a uniform random number
        draw=rand;
        % keep the candidate if the draw value is lower than the prob
        if draw<=prob
            L(kk,1)=cand;
            % if not, just keep the former value
        end
    end
    % then recover the series of matrices lambda_t and sigma_t
    for kk=1:T
        lambda_t(:,:,kk)=exp(L(kk,1))*diag(sbar);
        sigma_t(:,:,kk)=F*lambda_t(:,:,kk)*F';
    end

    %% Sample Sigma and Ll (static)
    [Sigma,Ll]=bear.favar_SigmaL(Sigma,Ll,nfactorvar,numpc,onestep,n,favar_X,FY,a0,b0,T,lags,L0);

    %% record phase
    % if the burn-in sample phase is not yet over
    if count<=Bu
        % simply add 1 to the iteration count
        count=count+1;
        % on the other hand, if the burn-in sample phase is over
    elseif count>Bu
        % adding one iteration to the count will depend on wether post-burn selection applies
        % if there is no post burn selection
        if pick==0
            % record the results
            beta_gibbs(:,count-Bu)=beta;
            F_gibbs(:,:,count-Bu)=F;
            L_gibbs(:,:,count-Bu)=L;
            phi_gibbs(count-Bu,1)=phi;
            sigma_gibbs(:,count-Bu)=sigma(:);
            % save the factors and loadings (keep the notation in the code consistent, although - except L - they don't change)
            Ll_gibbs(:,count-Bu)=Ll(:);

            % compute R2 (Coefficient of Determination) for plotX variables (keep the notation in the code consistent, although they don't change)
            R2=bear.favar_R2(favarX,FY);
            R2_gibbs(:,count-Bu)=R2(:);
            for jj=1:T
                lambda_t_gibbs{jj,1}(:,:,count-Bu)=lambda_t(:,:,jj);
                sigma_t_gibbs{jj,1}(:,:,count-Bu)=sigma_t(:,:,jj);
            end
            % then add one to the count
            count=count+1;
            % if there is post burn selection, only one draw over 'fpick' draws will be retained
        elseif pick==1
            % if the iteration does not correspond to fpick, don't record the results, don't increase the regular count, but do increase pickcount by 1, and do record the acceptance rate of the Metropolis-Hastings step
            if pickcount~=pickf
                pickcount=pickcount+1;
                % on the other hand, if the iteration does correspond to fpick
            elseif pickcount==pickf
                % do record the results
                beta_gibbs(:,count-Bu)=beta;
                F_gibbs(:,:,count-Bu)=F;
                L_gibbs(:,:,count-Bu)=L;
                phi_gibbs(count-Bu,1)=phi;
                sigma_gibbs(:,count-Bu)=sigma(:);
                % save the factors and loadings (keep the notation in the code consistent, although - except L - they don't change)
                Ll_gibbs(:,count-Bu)=Ll(:);

                % compute R2 (Coefficient of Determination) for plotX variables (keep the notation in the code consistent, although they don't change)
                R2=bear.favar_R2(favarX,FY);
                R2_gibbs(:,count-Bu)=R2(:);
                for jj=1:T
                    lambda_t_gibbs{jj,1}(:,:,count-Bu)=lambda_t(:,:,jj);
                    sigma_t_gibbs{jj,1}(:,:,count-Bu)=sigma_t(:,:,jj);
                end
                % then increase the regular count by 1 and re-initialise pickcount
                count=count+1;
                pickcount=1;
            end
        end
    end

    % update progress by one iteration
    hbar.iterate(1);

end

% in case we have thinning of the draws,
thin=abs(round(favar.thin)); % should be a positive integer
if thin~=1
    beta_gibbs=beta_gibbs(:,thin:thin:end);
    F_gibbs=F_gibbs(:,thin:thin:end);
    L_gibbs=L_gibbs(:,thin:thin:end);
    phi_gibbs=phi_gibbs(:,thin:thin:end);
    sigma_gibbs=sigma_gibbs(:,thin:thin:end);
    Ll_gibbs=Ll_gibbs(:,thin:thin:end);
    R2_gibbs=R2_gibbs(:,thin:thin:end);
    for jj=1:T
        lambda_t_gibbs=lambda_t_gibbs(:,:,thin:thin:end);
        sigma_t_gibbs=sigma_t_gibbs(:,:,thin:thin:end);
    end
    It=(1/thin)*It;
    Bu=(1/thin)*Bu;
end

% save in favar structure
favar.L_gibbs=Ll_gibbs;
favar.R2_gibbs=R2_gibbs;

close(hbar);   %close progress bar


function [stat,pval,statAll,pvalAll]=grangercause(y,constant,lags,het,uncorr,inference)
% Granger causality testing with a variance-covariance matrix estimated under a variety of
% assumptions on the covariance of the errors:
%   * Conditionally Homoskedastic and Uncorrelated
%   * Conditionally Homoskedastic but Correlated
%   * Heteroskedastic but Conditionally Uncorrelated
%   * Heteroskedastic and Correlated
%
% USAGE:
%   [STAT] = grangercause(Y,CONSTANT,LAGS)
%   [STAT,PVAL,STATALL,PVALALL] = grangercause(Y,CONSTANT,LAGS,HET,UNCORR,INFERENCE)
%
% INPUTS:
%   Y             - A T by K matrix of data
%   CONSTANT      - Scalar variable: 1 to include a constant, 0 to exclude
%   LAGS          - Non-negative integer vector representing the VAR orders to include in the model.
%   HET           - [OPTIONAL] A scalar integer indicating the type of covariance estimator
%                      0 - Homoskedastic
%                      1 - Heteroskedastic [DEFAULT]
%   UNCORR        - [OPTIONAL] A scalar integer indicating the assumed structure of the error
%                     covariance matrix
%                      0 - Correlated errors  [DEFAULT]
%                      1 - Uncorrelated errors
%   INFERENCE     - [OPTIONAL] Inference method
%                      1 - Likelihood ratio [DEFAULT]
%                      2 - LM test
%                      3 - Wald test
%
% OUTPUTS:
%   STAT          - K by K matrix of Granger causality statistics computed using the specified
%                     covariance estimator and inference method. STAT(i,j) corresponds to a test that
%                     y(i) is not caused by y(j)
%   PVAL          - K by K matrix of p-values corresponding to STAT
%   STATALL       - K by 1 vector of Granger causality statistics computed using the specified
%                     covariance estimator and inference method. STATALL(i) corresponds to a test that
%                     y(i) is not caused by any y(j), j neq i
%   PVALALL       - K by 1 vector of p-values corresponding to STATALL
%
% COMMENTS:
%   Granger causality tests based on a VAR including any lags.
%
%   y(:,t)' = CONST + P(1) * y(:,y-1) + P(2)*y(:,y-2) + ... + P(1)*y(:,t-K)'
%
%   where P(j) are K by K parameter matrices and CONST is a K by 1 parameter matrix (if CONSTANT==1)
%
% EXAMPLE:
%   Conduct GC testing in a VAR(1) with a constant
%        parameters = grangercause(y,1,1)
%   Conduct GC testing in a VAR(3) with no constant
%        parameters = grangercause(y,0,[1:3])
%   Conduct GC testing in a VAR that includes lags 1 and 3 with a constant
%        parameters = grangercause(y,1,[1 3])
%
% See also VECTORAR, VECTORARVCV

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3.0    Date: 1/1/2007

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<3 || nargin>6
    error('3 to 6 inputs required')
end
if nargin==3
    het=1;
    uncorr=0;
    inference=1;
elseif nargin==4
    uncorr=0;
    inference=1;
elseif nargin==5
    inference=1;
end
% Check Y
if ndims(y)~=2
    error('Y must be T by K')
end
% Check constant
if ~isscalar(constant)
    error('CONSTANT must be either 0 or 1')
end
if ~ismember(constant,[0 1])
    error('CONSTANT must be either 0 or 1')
end
% Check lags
if ndims(lags)~=2
    error('LAGS must be a vector of positive integers containing lags to include')
end
if size(lags,1)>size(lags,2)
    lags = lags';
end
if ~all(lags>0)
    error('LAGS must be a vector of positive integers containing lags to include')
end
if ~all(floor(lags)==lags)
    error('LAGS must be a vector of positive integers containing lags to include')
end
if length(lags)~=length(unique(lags))
    error('LAGS must be a vector of unique elements')
end
lags=sort(lags);
% Check het
if ~isscalar(het)
    error('HET must be either 0 or 1')
end
if ~ismember(het,[0 1])
    error('HET must be either 0 or 1')
end
% Check uncorr
if ~isscalar(uncorr)
    error('UNCORR must be either 0 or 1')
end
if ~ismember(uncorr,[0 1])
    error('UNCORR must be either 0 or 1')
end
% Inference mathod
if ~isscalar(inference)
    error('INFERENCE must be either 0 or 1')
end
if ~ismember(inference,[1 2 3])
    error('INFERENCE must be either 0 or 1')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Inference Methods
% 1. LR MLE : Relatively easy, need to iteratively depete the correct
% columns and reestimate.  Use a LR test
% 2. Wald: Just compute the VCV, then figure out how to select the correct
%       rows, and you are done
% 3. LM: Have to re-estimate under the null and use those errors to compute
%       the scores.  Test is based on scores.
% 4. LR Robust: Just like LM above but using errors estimated under the
%       alternative

% LR MLE
% First generate the unrestricted errors, then iterate across series and
% drop regressors

T=size(y,1);
K=size(y,2);
P=length(lags);
m=max(lags);
ylags=cell(K,1);
for k=1:K
    ytemp = [ones(m,1)*mean(y(:,k));
        y(:,k)];
    [nothing,ylags{k}]=newlagmatrix(ytemp,m,0);
end

X=zeros(T,K+constant);
index=1;
if constant
    X(:,1)=ones(T,1);
    index=index+1;
end
p=length(lags);
for i=1:p
    for k=1:K
        X(:,index)=ylags{k}(:,lags(i));
        index=index+1;
    end
end

% Each column contains the parameters for a single y
paramvec = X\y;
errors = y-X*paramvec;
s2=errors'*errors/T;
e=cell(K,K);
for i=1:K
    for j=1:K
        % i will be the LHS variable, j will be the lags
        tempX=X;
        allcols = 1:size(X,2);
        % Drop columns j, K+j, 2K+j,...P(K-1)*j (+constant if present)
        if constant
            drop = (j:K:size(X,2)-1)+constant;
        else
            drop = (j:K:size(X,2));
        end
        remain = setdiff(allcols,drop);
        beta = tempX(:,remain)\y(:,i);
        % These errors are estiamted under the null and can be used for LR and LM testing
        e{i,j} = y(:,i)-tempX(:,remain)*beta;
    end
end
% Construct the regressions needed to test the "all" hypothesis
eall = cell(K,1);
for i=1:K
    tempX = X;
    % Keep columns i, K+i, 2K+i, P(K-1)*i (+constant if present)
    if constant
        remain = [constant (i:K:(size(X,2)-1))+constant];
    else
        remain = i:K:size(X,2);
    end
    beta = tempX(:,remain)\y(:,i);
    eall{i} = y(:,i)-tempX(:,remain)*beta;
end

Np = p*K+constant;
stat = zeros(K);
statAll = zeros(K,1);
% Homoskedastic Likelihood Ratio
if inference==1 && het==0
    for i=1:K
        for j=1:K
            e2=errors;
            e2(:,i)=e{i,j};
            sR=e2'*e2/T;
            stat(i,j)=(T-P*K^2+P)*(log(det(sR))-log(det(s2)));
        end
    end
    for i=1:K
        e2=errors;
        e2(:,i) = eall{i};
        sR=e2'*e2/T;
        statAll(i)=(T-P*K^2+(K-1)*P)*(log(det(sR))-log(det(s2)));
    end
    % Heteroskedasticity robust LR;  key here is score covariance is computed
    % using errors estimated under the alternative
elseif inference==1 && het==1
    for i=1:K
        for j=1:K
            e2=errors;
            e2(:,i)=e{i,j};
            X2=repmat(X,1,K);
            e2=reshape(repmat(e2,Np,1),T,K*Np);
            s=X2.*e2;
            sbar=mean(s);
            % Estimate S using the errors, not e2
            S=vectorarscorecov(errors,X,het,uncorr,K,T,Np);
            stat(i,j)=(T-P*K^2+P)*sbar*S^(-1)*sbar';
        end
    end
    for i=1:K
        e2=errors;
        e2(:,i)=eall{i};
        X2=repmat(X,1,K);
        e2=reshape(repmat(e2,Np,1),T,K*Np);
        s=X2.*e2;
        sbar=mean(s);
        % Estimate S using the errors, not e2
        S=vectorarscorecov(errors,X,het,uncorr,K,T,Np);
        statAll(i)=(T-P*K^2+(K-1)*P)*sbar*S^(-1)*sbar';
    end
    % All LM tests since function will handle;  the key here is that the errors
    % are computed under the null
elseif inference==2
    for i=1:K
        for j=1:K
            e2=errors;
            e2(:,i)=e{i,j};
            X2=repmat(X,1,K);
            e2=reshape(repmat(e2,Np,1),T,K*Np);
            s=X2.*e2;
            sbar=mean(s);
            e2=errors;
            e2(:,i)=e{i,j};
            S=vectorarscorecov(e2,X,het,uncorr,K,T,Np);
            stat(i,j)=(T-P*K^2+P)*sbar*S^(-1)*sbar';
        end
    end
    for i=1:K
        e2=errors;
        e2(:,i)=eall{i};
        X2=repmat(X,1,K);
        e2=reshape(repmat(e2,Np,1),T,K*Np);
        s=X2.*e2;
        sbar=mean(s);
        e2=errors;
        e2(:,i)=eall{i};
        S=vectorarscorecov(e2,X,het,uncorr,K,T,Np);
        statAll(i)=(T-P*K^2+(K-1)*P)*sbar*S^(-1)*sbar';
    end
    % Wald tests
elseif inference==3
    XpXi = ((X'*X)/T)^(-1);
    Ainv = kron(eye(K),XpXi);
    B = vectorarscorecov(errors,X,het,uncorr,K,T,Np);
    V = Ainv*B*Ainv;
    %Vinv=V^(-1);
    % Now I have to cleverly select the parameters to set to 0 and then
    % compute the wald tests
    % Easier to iterate over the j's in the inside loop
    for i=1:K
        for j=1:K
            temp=zeros(size(paramvec))';
            % Select the columns of the parameters to test
            if constant
                pl = (j:K:size(X,2)-1)+constant;
            else
                pl = (j:K:size(X,2));
            end

            temp(i,pl)=1;
            temp=temp';
            temp=temp(:);
            pl = find(temp);
            p = paramvec;
            p = p(:);
            stat(i,j)=(T-P*K^2+P)*(p(pl)'*V(pl,pl)^(-1)*p(pl));
        end
    end
    for i=1:K
        temp=zeros(size(paramvec))';
        % Select the columns of the parameters to test
        if constant
            remain = [constant (i:K:size(X,2)-1)+constant];
        else
            remain = (i:K:size(X,2)-1);
        end
        pl = setdiff(1:size(X,2),remain);
        temp(i,pl)=1;
        temp=temp';
        temp=temp(:);
        pl = find(temp);
        p = paramvec;
        p = p(:);
        statAll(i)=(T-P*K^2+(K-1)*P)*(p(pl)'*V(pl,pl)^(-1)*p(pl));
    end
end
% All stats have the same dist
pval=1-chi2cdf(stat,length(lags));
pvalAll = 1-chi2cdf(statAll,(K-1)*length(lags));

function S=vectorarscorecov(errors,X,het,uncorr,K,T,Np)
s2=errors'*errors/T;
XpX=X'*X/T;
if ~het && uncorr
    S=kron(diag(diag(s2)),XpX);
elseif ~het && ~uncorr
    S=kron(s2,XpX);
elseif het && uncorr
    X2=repmat(X,1,K);
    e2=reshape(repmat(errors,Np,1),T,K*Np);
    s=X2.*e2;
    s=s-repmat(mean(s),T,1);
    S=zeros(Np*K);
    for i=1:K
        sel=(i-1)*Np+1:i*Np;
        temp = s(:,sel);
        S(sel,sel)=temp'*temp/T;
    end
elseif het && ~uncorr
    X2=repmat(X,1,K);
    e2=reshape(repmat(errors,Np,1),T,K*Np);
    s=X2.*e2;
    s=s-repmat(mean(s),T,1);
    S=s'*s/T;
end


%SerialLink.jacob_dot Derivative of Jacobian
%
% JDQ = R.jacob_dot(Q, QD) is the product (6x1) of the derivative of the
% Jacobian (in the world frame) and the joint rates.
%
% Notes::
% - This term appears in the formulation for operational space control XDD = J(Q)QDD + JDOT(Q)QD
% - Written as per the reference and not very efficient.
%
% References::
% - Fundamentals of Robotics Mechanical Systems (2nd ed)
%   J. Angleles, Springer 2003.
% - A unified approach for motion and force control of robot manipulators: The operational space formulation
%  O Khatib, IEEE Journal on Robotics and Automation, 1987.
%
% See also SerialLink.jacob0, diff2tr, tr2diff.

% Copyright (C) 1993-2017, 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

function Jdot = jacob_dot(robot, q, qd)

	n = robot.n;
    links = robot.links;

    % Using the notation of Angeles:
    %   [Q,a] ~ [R,t] the per link transformation
    %   P ~ R   the cumulative rotation t2r(Tj) in world frame
    %   e       the last column of P, the local frame z axis in world coordinates
    %   w       angular velocity in base frame
    %   ed      deriv of e
    %   r       is distance from final frame
    %   rd      deriv of r
    %   ud      ??

    for i=1:n
        T = links(i).A(q(i));
        Q{i} = t2r(T);
        a{i} = transl(T)';
    end

    P{1} = Q{1};
    e{1} = [0 0 1]';
    for i=2:n
        P{i} = P{i-1}*Q{i};
        e{i} = P{i}(:,3);
    end

    % step 1
    w{1} = qd(1)*e{1};
    for i=1:(n-1)
        w{i+1} = qd(i+1)*[0 0 1]' + Q{i}'*w{i};
    end

    % step 2
    ed{1} = [0 0 0]';
    for i=2:n
        ed{i} = cross(w{i}, e{i});
    end

    % step 3
    rd{n} = cross( w{n}, a{n});
    for i=(n-1):-1:1
        rd{i} = cross(w{i}, a{i}) + Q{i}*rd{i+1};
    end

    r{n} = a{n};
    for i=(n-1):-1:1
        r{i} = a{i} + Q{i}*r{i+1};
    end

    ud{1} = cross(e{1}, rd{1});
    for i=2:n
        ud{i} = cross(ed{i}, r{i}) + cross(e{i}, rd{i});
    end

    % step 4
    %  swap ud and ed
    v{n} = qd(n)*[ud{n}; ed{n}];
    for i=(n-1):-1:1
        Ui = blkdiag(Q{i}, Q{i});
        v{i} = qd(i)*[ud{i}; ed{i}] + Ui*v{i+1};
    end

    Jdot = v{1};


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT 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.
%
% SPHARM-MAT 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 SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function performPCA(confs, objs)

numSbj = length(objs);
groupID = confs.GroupID;

% set parameters
fvecs = [];	% fvecs is accumulator of coefficients from each species.
keep = numSbj-1;	% number of eigenvectors to keep

% do processing of each file
for i = 1:numSbj
	% read in file.
	file = objs{i};
	load(file);
	[path, name, ext] = fileparts(file);
	clear('faces', 'vertices', 'sph_verts');

	[nrows ncols] = size(fvec);
	temp = fvec.';
	temp = reshape(temp, (nrows*3), 1);
	fvecs = [fvecs temp];
end
clear('fvec', 'temp', 'name');

fvecs = fvecs.';
[eigenvecs, scores, eigenvals] = princomp(fvecs);
% trace_eig = trace(eigenvals);
perc_variance_explained = eigenvals/sum(eigenvals);

% drop cells that are to not be kept
eigenvecs = eigenvecs(:,1:keep);
%scores = scores(:,1:keep);
eigenvals = eigenvals(1:keep,1);
perc_variance_explained = perc_variance_explained(1:keep,:);
cum_percent_explained = cumsum(perc_variance_explained);

new_name = [confs.OutDirectory '/' confs.OutputName];
if exist(new_name,'file')
    prompt = {'Enter new filename:'};
    dlg_title = 'New File Name';
    num_lines = 1;
    def = {new_name};
    answer = inputdlg(prompt,dlg_title,num_lines,def);
    new_name = answer{1};
end

save(new_name, 'fvecs', 'eigenvecs', 'eigenvals', 'perc_variance_explained', ...
    'cum_percent_explained', 'scores','groupID');

return;


classdef madgwick < handle
    methods (Static = true)

        function q = imu(q, Gyroscope, Accelerometer, SamplePeriod, Beta)

            % Normalise accelerometer measurement
            acc = Accelerometer;
            if(norm(acc) == 0), return; end   % handle NaN
            acc = acc / norm(acc);  % normalise magnitude

            % Gradient decent algorithm corrective step
            F = (quatrotate(q, [0 0 1]) - acc)';
%             F = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
%                 2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
%                 2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)];

            J = [-2*q(3),	2*q(4),    -2*q(1),	2*q(2)
                2*q(2),     2*q(1),     2*q(4),	2*q(3)
                0,         -4*q(2),    -4*q(3),	0    ];
            step = (J'*F);
            step = step / norm(step);	% normalise step magnitude

            % Compute rate of change of quaternion
            qd = 0.5 * quatmultiply(q, [0 Gyroscope]) - Beta * step';

            % Integrate to yield quaternion
            q = q + qd * SamplePeriod;
            q = q / norm(q); % normalise quaternion
        end

        function q = ahrs(q, Gyroscope, Accelerometer, Magnetometer, SamplePeriod, Beta)

            % Normalise accelerometer measurement
            if(norm(Accelerometer) == 0), return; end	% handle NaN
            acc = Accelerometer / norm(Accelerometer);	% normalise magnitude

            % Normalise magnetometer measurement
            if(norm(Magnetometer) == 0), return; end	% handle NaN
            mag = Magnetometer / norm(Magnetometer);	% normalise magnitude

            % Reference direction of Earth's magnetic feild
            h = quatrotate(quatconj(q), mag);
            h = [0 h];
            b = [0 norm([h(2) h(3)]) 0 h(4)];

            % Gradient decent algorithm corrective step
             F= (quatrotate(q, [0 0 1]) - acc)';
             F = [F;  (quatrotate(q, [b(2) 0 b(4)]) - mag)'];

%             F = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
%                 2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
%                 2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)
%                 2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3)) - Magnetometer(1)
%                 2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4)) - Magnetometer(2)
%                 2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2) - Magnetometer(3)];
            J = [-2*q(3),                 	2*q(4),                    -2*q(1),                         2*q(2)
                2*q(2),                 	2*q(1),                    	2*q(4),                         2*q(3)
                0,                         -4*q(2),                    -4*q(3),                         0
                -2*b(4)*q(3),               2*b(4)*q(4),               -4*b(2)*q(3)-2*b(4)*q(1),       -4*b(2)*q(4)+2*b(4)*q(2)
                -2*b(2)*q(4)+2*b(4)*q(2),	2*b(2)*q(3)+2*b(4)*q(1),	2*b(2)*q(2)+2*b(4)*q(4),       -2*b(2)*q(1)+2*b(4)*q(3)
                2*b(2)*q(3),                2*b(2)*q(4)-4*b(4)*q(2),	2*b(2)*q(1)-4*b(4)*q(3),        2*b(2)*q(2)];
            step = (J'*F);
            step = step / norm(step);	% normalise step magnitude

            % Compute rate of change of quaternion
            qd = 0.5 * quatmultiply(q, [0 Gyroscope]) - Beta * step';

            % Integrate to yield quaternion
            q = q + qd * SamplePeriod;
            q = q / norm(q); % normalise quaternion
        end
    end
end


function Mh = hmxFusion(Mh)
%+========================================================================+
%|                                                                        |
%|         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       : hmxFusion.m                                   |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Fusion and recompress for full, sparse and    |
%|  `---'  |                low-rank leaves                               |
%+========================================================================+

%%% H-Matrix
if (Mh.typ == 0)
    % Check and convert leaf data
    typ = zeros(1,4);
    for i = 1:4
        % Children data
        typ(i) = Mh.chd{i}.typ;
        dat    = Mh.chd{i}.dat;
        tol    = Mh.chd{i}.tol;
        dim    = size(Mh.chd{i});

        % Full or sparse matrix
        if (typ(i) == 2)
            % Non zeros terms
            n = nnz(dat);

            % Sparse
            if issparse(dat)
                % Compress
                if (n == 0)
                    A    = zeros(dim(1),0);
                    B    = zeros(0,dim(2));
                    flag = 1;

                % Full
                elseif (n > 1/4*prod(dim))
                    dat  = full(dat);
                    flag = 2;

                % Sparse
                else
                    flag = 0;
                end

            % Full
            else
                % Compress
                if (n == 0)
                    A    = zeros(dim(1),0);
                    B    = zeros(0,dim(2));
                    flag = 1;

                % Sparse
                elseif (n <= 1/4*prod(dim))
                    dat  = sparse(double(dat));
                    flag = 2;

                % Compress
                else
                    [A,B,flag] = hmxSVD(dat,tol);
                end
            end

            % Update
            if (flag == 1)
                Mh.chd{i}.dat = {A,B};
                Mh.chd{i}.typ = 1;
                typ(i)        = 1;

            elseif (flag == 2)
                Mh.chd{i}.dat = dat;
            end
        end
    end

    % Low-rank fusion (QRSVD)
    if (sum(typ==1) == 4)
        % Rank for each leaf
        rk = zeros(1,4);
        nk = 0;
        for i = 1:4
            rk(i) = size(Mh.chd{i}.dat{1},2);
            nk    = nk + sum(size(Mh.chd{i}))*rk(i);
        end

        % Low rank matrix
        A = zeros(size(Mh,1),sum(rk),class(Mh.pos{1}));
        B = zeros(sum(rk),size(Mh,2),class(Mh.pos{2}));
        j = 0;
        for i = 1:4
            A(Mh.row{i},j+(1:rk(i))) = Mh.chd{i}.dat{1};
            B(j+(1:rk(i)),Mh.col{i}) = Mh.chd{i}.dat{2};
            j = j + rk(i);
        end

        % Recompression
        [A,B] = hmxQRSVD(A,B,Mh.tol);

        % Update
        if (sum(size(Mh))*size(A,2) <= nk)
            Mh.typ = 1;
            Mh.row = cell(1,4);
            Mh.col = cell(1,4);
            Mh.chd = cell(1,4);
            Mh.dat = {A,B};
        end
    end

%%% Unavalaible case
else
    error('hmxFusion.m : unavailable case')
end
end


close all; clear all;

%% Setting of the problem
global s
pde = fonedata; % f = 1;
pde.L = 1;
option.theta = 0.3;
option.estType = 'star';
option.maxIt = 20;
option.maxN = 2e5;
option.solver = 'mg';
option.yrefinement = 10; % ratio of h_y/h_x
option.gNquadorder = 5;
option.tol = 1e-6;
[node,elem] = squaremesh([-1,1,-1,1],0.5);
[node,elem] = delmesh(node,elem,'x>0 & y<0');
bdFlag = setboundary(node,elem,'Dirichlet');

%% s = 0.2
s = 0.2;
afemfracLap(node,elem,pde,bdFlag,option);

%% s = 0.4
s = 0.4;
afemfracLap(node,elem,pde,bdFlag,option);

%% s = 0.6
s = 0.6;
afemfracLap(node,elem,pde,bdFlag,option);

%% s = 0.8
s = 0.8;
afemfracLap(node,elem,pde,bdFlag,option);


function [out,Xt,str,ts] = rfnn_miso_grid(t,Xt,u,flag,itaVector,alphaVector, NumInVars,NumInTerms,x0,T)

% This program is an implementation of the on line RFNN (MISO) system.
% The structure of the network is determined by the user.
% The input space is partitioned using the grid-type method.
% All parameters of the network are estimated by Gradient Descent (GD)
% through error backpropagation.

    ninp = NumInVars;
    nout = 1;
   ninps = ninp+nout+1;  % number of inputs to sfunction [ x y LE ]
   NumRules = NumInTerms^NumInVars;  % Grid-Type Input Space Partitioning.
       ns = 4*NumInVars*NumInTerms + NumRules;
     nds = 3*NumInVars*NumInTerms + NumRules;
     % Learning Rates
     ita1 = itaVector(1); ita2 = itaVector(2);
     ita3 = itaVector(3); ita4 = itaVector(4);
     % Momentum Constants.
     alpha1 = alphaVector(1); alpha2 = alphaVector(2);
     alpha3 = alphaVector(3); alpha4 = alphaVector(4);
%  ----------------------- % initial informations --------------
if abs(flag)==0

    out = [0,ns+nds,1+ns+nds,ninps,0,1,1];    % states, outputs, inputs, ?, df, #ts
    str = [];                                 % API block consistency
     ts = T;                                  % sample time
    Xt = x0;
%  ----------------------- % state derivatives -----------------
elseif abs(flag) == 2

          x = u(1:ninp);
          e = u(ninp+1:ninp+nout);
   learning = u(ninp+nout+1);

if learning == 1

   % Unroll the states:
   off=1;
   off_end=NumInVars*NumInTerms;
   mean2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumInVars*NumInTerms-1;
   sigma2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off+NumInVars*NumInTerms-1;
   Theta2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumRules-1;
   W = reshape(Xt(off:off_end),NumRules,1);

   off=off_end+1;
   off_end=off+NumInVars*NumInTerms-1;
   Out2 = reshape(Xt(off:off_end),NumInVars,NumInTerms);

   % Unroll the differential states:
   off=off_end+1;
   off_end=off + NumInVars*NumInTerms-1;
   dmean2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumInVars*NumInTerms-1;
   dsigma2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumInVars*NumInTerms-1;
   dTheta2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumRules-1;
   dW = reshape(Xt(off:off_end),NumRules,1);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %                                                    FEEDFORWARD OPERATION                                                      %
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % LAYER 2 - INPUT TERM NODES
  Out2_pr = Out2;

 In2 = x*ones(1,NumInTerms) + Out2_pr.*Theta2;
 Out2 = exp(-((In2-mean2)./sigma2).^2);

% LAYER 3 - RULE (PRODUCT) NODES
precond = comb(Out2);
 Out3 = prod(precond,2);

%%%%%%%%%%%% END OF NETWORK FUNCTIONALITY SECTION %%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%			 					                 PARAMETER LEARNING SECTION	                       			                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% BACKWARD PASS. Error Backpropagation
% LAYERS 4-3
delta3 = e*W;

% LAYER 2
Q = zeros(NumInVars,NumInTerms,NumRules);
for i=1:NumInVars
     for j=1:NumInTerms
          for k=1:NumRules
       	       if Out2(i,j)== precond(k,i) && Out2(i,j)~=0
      			  Q(i,j,k) = (Out3(k)/Out2(i,j))*delta3(k);
               end
          end
   	  end
 end

 ThetaE = sum(Q,3);

% LAYER 1 PARAMETER ADJUSTMENT BY GRADIENT DESCENT.
deltamean2  =  2*ThetaE.*Out2.*(In2-mean2)./((sigma2).^2);
deltasigma2 =  2*ThetaE.*Out2.*((In2-mean2).^2)./(sigma2.^3);
deltaTheta2 = -2*ThetaE.*Out2.*(In2-mean2).*Out2_pr./sigma2.^2;

dmean2 = ita2*deltamean2 + alpha2*dmean2;
 mean2  = mean2 + dmean2;

dsigma2 = ita3*deltasigma2 + alpha3*dsigma2;
  sigma2 = sigma2 + dsigma2;

dTheta2 = ita4*deltaTheta2 + alpha4*dTheta2;
 Theta2  = Theta2 + dTheta2;

% LAYER 4 PARAMETER ADJUSTMENT
  deltaW = e*Out3;
  dW = ita1*deltaW + alpha1*dW;
   W = W + dW;

%%%%%%%%%%%   END OF PARAMETER LEARNING PROCESS %%%%%%%%%
% State Vector Storage.
% Xt = [mean2 sigma2 Theta2 W Out2 dmean2 dsigma2 dTheta2 dW];

Xt = [ reshape(mean2,NumInVars*NumInTerms,1);
          reshape(sigma2,NumInVars*NumInTerms,1);
          reshape(Theta2,NumInVars*NumInTerms,1);
          W;
          reshape(Out2,NumInVars*NumInTerms,1);
          reshape(dmean2,NumInVars*NumInTerms,1);
          reshape(dsigma2,NumInVars*NumInTerms,1);
          reshape(dTheta2,NumInVars*NumInTerms,1);
          dW;];
end

out=Xt;

%  ----------------------- % outputs -------------------------
elseif flag == 3

  % Unpack the network's parameters first...
   off=1;
   off_end=NumInVars*NumInTerms;
   mean2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumInVars*NumInTerms-1;
   sigma2=reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off+NumInVars*NumInTerms-1;
   Theta2 =reshape(Xt(off:off_end),NumInVars,NumInTerms);

   off=off_end+1;
   off_end=off + NumRules - 1;
   W = Xt(off:off_end);

   off=off_end+1;
   off_end=off+NumInVars*NumInTerms-1;
   Out2 = reshape(Xt(off:off_end),NumInVars,NumInTerms);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %                                                       FEEDFORWARD OPERATION                                                       %
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % LAYER 1 - INPUT TERM NODES
  Out2_pr = Out2;

  x = u(1:ninp);
 In2 = x*ones(1,NumInTerms) + Out2_pr.*Theta2;
 Out2 = exp(-((In2-mean2)./sigma2).^2);

% LAYER 3 - RULE (PRODUCT) NODES
precond = comb(Out2);
 Out3 = prod(precond,2);

 % LAYER 4
  outact = W.'*Out3;

  % Block Outputs Vector Formation.
   out=[outact;Xt];

else
   out=[];
end


function [energy] = getSpecOrbitEnergyFromSma(sma, gmu)
%getSpecOrbitEnergyFromSma Summary of this function goes here
%   Detailed explanation goes here

    energy = -gmu/(2*sma);
end