% Implement the back propagation of an LSTM layer.
% Author: Xiong Xiao, Temasek Labs, NTU, Singapore.
% Last modified: 13 Oct 2015
%
function [grad, grad_W, grad_b] = B_LSTM(input, LSTM_layer, future_layers)
W = LSTM_layer.W;
if strcmpi(class(input), 'gpuArray'); useGPU=1; else useGPU = 0; end
precision = class(gather(input(1)));

[dim, nFr, nSeg] = size(input);
input = permute(input, [1 3 2]);

nCell = LSTM_layer.dim(1);  % number of LSTM cells in the layer

useHidden = 1;
usePastState = 1;
usePastStateAsFeature = LSTM_layer.usePastState;

future_layer_grad = 0;
for fli = 1:length(future_layers)
    future_layer_grad = future_layer_grad + future_layers{fli}.grad;      % this is the gradient of LSTM output from the cost function
end
grad_ht_cost = future_layer_grad;
grad_ht_cost = permute(grad_ht_cost,[1 3 2]);

ft = LSTM_layer.ft;
it = LSTM_layer.it;
ot = LSTM_layer.ot;
Ct_raw = LSTM_layer.Ct_raw;
Ct = LSTM_layer.Ct;
ht = LSTM_layer.a;
ht = permute(ht, [1 3 2]);
Ct0 = LSTM_layer.Ct0;
ht0 = LSTM_layer.ht0;

% allocate memory for the gradients of gates and states
if useGPU == 0
    grad_xt = zeros(dim, nSeg, nFr, precision);
    grad_ft = zeros(nCell, nSeg, nFr, precision);     % forget gates
    grad_it = zeros(nCell, nSeg, nFr, precision);     % input gates
    grad_ot = zeros(nCell, nSeg, nFr, precision);     % output gates
    grad_Ct_raw = zeros(nCell, nSeg, nFr, precision); % candidate cell states
    grad_Ct = zeros(nCell, nSeg, nFr, precision);     % cell states
    grad_Ct_future = zeros(nCell, nSeg, nFr, precision);     % cell states
    grad_ht = zeros(nCell, nSeg, nFr, precision);     % hidde layer output, i.e. the output of the LSTM layer
    grad_ht_future = zeros(nCell, nSeg, nFr, precision);     % hidde layer output, i.e. the output of the LSTM layer
    grad_zt = zeros(nCell*4,nSeg, nFr, precision);
else
    grad_xt = gpuArray.zeros(dim, nSeg, nFr, precision);
    grad_ft = gpuArray.zeros(nCell, nSeg, nFr, precision);     % forget gates
    grad_it = gpuArray.zeros(nCell, nSeg, nFr, precision);     % input gates
    grad_ot = gpuArray.zeros(nCell, nSeg, nFr, precision);     % output gates
    grad_Ct_raw = gpuArray.zeros(nCell, nSeg, nFr, precision); % candidate cell states
    grad_Ct = gpuArray.zeros(nCell, nSeg, nFr, precision);     % cell states
    grad_Ct_future = gpuArray.zeros(nCell, nSeg, nFr, precision);     % cell states
    grad_ht = gpuArray.zeros(nCell, nSeg, nFr, precision);     % hidde layer output, i.e. the output of the LSTM layer
    grad_ht_future = gpuArray.zeros(nCell, nSeg, nFr, precision);     % hidde layer output, i.e. the output of the LSTM layer
    grad_zt = gpuArray.zeros(nCell*4,nSeg, nFr, precision);
end

for t = nFr:-1:1
    % compute the gradient of ht and Ct that requires gradients from
    % future. At frame nFr, the future gradients are initialized to 0.
    Ct_raw_curr = Ct_raw(:,:,t);
    if useHidden
        grad_ht_curr = grad_ht_future(:,:,t) + grad_ht_cost(:,:,t);
    else
        grad_ht_curr = grad_ht_cost(:,:,t);
    end
    tanh_Ct = tanh(Ct(:,:,t));
    grad_Ct(:,:,t) = grad_Ct_future(:,:,t) + grad_ht_curr .* (1-tanh_Ct.*tanh_Ct) .* ot(:,:,t);

    % compute the gradient of gates and candidate states
    if usePastState
        if t==1
            grad_ft(:,:,t) = grad_Ct(:,:,t) .* Ct0;
            grad_Ct_future0 = grad_Ct(:,:,t) .* ft(:,:,t);
        else
            grad_ft(:,:,t) = grad_Ct(:,:,t) .* Ct(:,:,t-1);
            grad_Ct_future(:,:,t-1) = grad_Ct(:,:,t) .* ft(:,:,t);
        end
    end
    grad_Ct_raw(:,:,t) = grad_Ct(:,:,t) .* it(:,:,t);
    grad_it(:,:,t) = grad_Ct(:,:,t) .* Ct_raw_curr;
    grad_ot(:,:,t) = grad_ht_curr .* tanh(Ct(:,:,t));

    % compute the gradient of the gates before the activation function.

    grad_zCt_raw = grad_Ct_raw(:,:,t) .* (1-Ct_raw_curr.*Ct_raw_curr);
    if 1
        gates = [ft(:,:,t); it(:,:,t); ot(:,:,t)];
        grad_gates = [grad_ft(:,:,t); grad_it(:,:,t); grad_ot(:,:,t)];
        grad_zgates = grad_gates .* gates .* (1-gates);
        grad_zt_curr = [grad_zgates(1:nCell,:); grad_zCt_raw; grad_zgates(nCell+1:end,:)];
    else
        grad_zft = grad_ft(:,:,t) .* ft(:,:,t) .* (1-ft(:,:,t));
        grad_zit = grad_it(:,:,t) .* it(:,:,t) .* (1-it(:,:,t));
        grad_zot = grad_ot(:,:,t) .* ot(:,:,t) .* (1-ot(:,:,t));
        grad_zt_curr = [grad_zft; grad_zCt_raw; grad_zit; grad_zot];
    end
    grad_zt(:,:,t) = grad_zt_curr;

    % compute the gradient of the W, b, and past hidden, past state, and x
    grad_yt = W' * grad_zt_curr;
    if usePastStateAsFeature
        grad_xt(:,:,t) = grad_yt(nCell*2+1:end,:);
    else
        grad_xt(:,:,t) = grad_yt(nCell+1:end,:);
    end
    if t==1
%         grad_ht_future0 = grad_yt(nCell+1:nCell*2);
%         grad_Ct_future0 = grad_Ct_future0 + grad_yt(1:nCell);
%         grad_W = grad_W + grad_zt * [Ct0*usePastStateAsFeature; ht0*useHidden; input(:,t)]';
    else
        grad_ht_future(:,:,t-1) = grad_yt(nCell+1:nCell*2,:);
        if usePastStateAsFeature
            grad_Ct_future(:,:,t-1) = grad_Ct_future(:,:,t-1) + grad_yt(1:nCell,:);
        end
%         grad_W = grad_W + grad_zt * [Ct(:,t-1)*usePastStateAsFeature; ht(:,t-1)*useHidden; input(:,t)]';
    end
%     grad_b = grad_b + grad_zt;
    grad_ht(:,:,t) = grad_ht_curr;
end
if usePastStateAsFeature
    tmpMat = [Ct(:,:,1:nFr-1); ht(:,:,1:nFr-1)*useHidden; input(:,:,2:nFr)];
else
    tmpMat = [ht(:,:,1:nFr-1)*useHidden; input(:,:,2:nFr)];
end
tmpMat = reshape(tmpMat, size(tmpMat,1), nSeg*(nFr-1));
tmpMat2 = reshape(grad_zt(:,:,2:nFr), nCell*4, nSeg*(nFr-1));
grad_W = tmpMat2 * tmpMat';
if usePastStateAsFeature
    grad_W = grad_W + grad_zt(:,:,1) * [Ct0; ht0*useHidden; input(:,:,1)]';
else
    grad_W = grad_W + grad_zt(:,:,1) * [ht0*useHidden; input(:,:,1)]';
end

grad_b = sum(sum(grad_zt,3),2);
grad = permute(grad_xt,[1 3 2]);
end


% Display the contents of a pyramid, as returned by functions
% 'laplacian_pyramid' or 'gaussian pyramid'
%
% tom.mertens@gmail.com, August 2007
%

function display_pyramid(pyr)

L = length(pyr);
r = size(pyr{1},1);
c = size(pyr{1},2);
k = size(pyr{1},3);
R = zeros(r,2*c,k);

offset = 1;
for l = 1:L
    I = pyr{l};
    r = size(I,1);
    c = size(I,2);
    R(1:r, offset:offset-1+c, :) = I;
    offset = offset + c;
end

if (min(R(:)) < 1e-5)
    %make negative values displayable
    a = min(R(:));
    b = max(R(:));
    R = (R - a) / (b - a);
end

figure; imshow(R);


function plaza = create_plaza(B, L)
global plazalength;
topgap = 5;
bottomgap = 1;
plaza = zeros(plazalength,B+2);
if mod(B-L,2)==0
for row = 1:plazalength
plaza(row,1) = -888;
plaza(row,2+B) = -888;
end
for col = 2:B/2 - L/2 + 1
for row = 1:(plazalength-1)/2 - topgap * (col-1)
plaza(row,col) = -888;
plaza(row,B+3-col) = -888;
end
end
for col = 2:B/2 - L/2 + 1
for row = (plazalength+3)/2 + bottomgap*(col-1):plazalength
plaza(row,col) = -888;
plaza(row,B+3-col) = -888;
end
end
else
for row = 1:plazalength
plaza(row,1) = -888;
plaza(row,3+B) = -888;
end
for col = 2:(B+1)/2 - L/2 + 1
for row = 1:(plazalength-1)/2 - topgap * (col-1)
plaza(row,col) = -888;
plaza(row,B+4-col) = -888;
end
end
for col = 2:(B+1)/2 - L/2 + 1
for row = (plazalength+3)/2 + bottomgap*(col-1):plazalength
plaza(row,col) = -888;
plaza(row,B+4-col) = -888;
end
end
for row = 1:plazalength
plaza(row,2+B) = -888;
end
end

github-repos/MATLAB/Eurus-Holmes-Mathematical_Modeling/Mathematical_Modeling-cb9dd53af84ffbd455ec62ab89886351e0ec98d9/Cellular automaton/元胞自动机/1月13日课件（元胞自动机）/程序/The Booth Tolls for Thee/create_plaza.m


function [del,obj] = setProj(obj,proj,projtype,insert,bbox)
    % [del,obj] = setProj(obj,proj,projtype,insert)
    % kjr generic function to parse projected space options
    % returns del flag to delete overlapping elements when plotting
    % global models.
    % if insert = 1, then automatically insert the global m_proj variables
    % into the msh obj. insert is 0 by default.

    if nargin < 4
        insert = 0;
    end

    if nargin < 5
        bbox = [];
    end

    % process bounds of mesh (or supply your own)
    if isempty(bbox)
        lon_mi = min(obj.p(:,1)); lon_ma = max(obj.p(:,1));
        lat_mi = min(obj.p(:,2)); lat_ma = max(obj.p(:,2));
    else
        lon_mi = bbox(1,1); lon_ma = bbox(1,2);
        lat_mi = bbox(2,1); lat_ma = bbox(2,2);
    end
    lat_mea = mean(obj.p(:,2)); lon_mea = mean(obj.p(:,1));

    % some defaults
    rad = 100; rot = 15;
    del = 0 ;

    % process the projtype as a varargin type
    I = find(strcmp(projtype,'long'));
    if ~isempty(I)
        if length(projtype{I+1}) == 1
            lon_mea = projtype{I+1};
        else
            lon_mi = projtype{I+1}(1); lon_ma = projtype{I+1}(2);
        end
    end
    I = find(strcmp(projtype,'lat'));
    if ~isempty(I)
        if length(projtype{I+1}) == 1
            lat_mea = projtype{I+1};
        else
            lat_mi = projtype{I+1}(1); lat_ma = projtype{I+1}(2);
        end
    end
    I = find(strcmp(projtype,'rad'));
    if ~isempty(I)
        rad = projtype{I+1};
    end
    I = find(strcmp(projtype,'rot'));
    if ~isempty(I)
        rot = projtype{I+1};
    end
    if proj == 0
        % normal geographic coordinates
        m_proj('equi','lat',[lat_mi lat_ma],'long',[lon_mi lon_ma]) ;
        del = 1;
    else
        if ~ischar(projtype)
            projtype = projtype{1};
        end
        projtype = lower(projtype);
        if ~isempty(regexp(projtype,'ste'))
            % Special treatment of Stereographic projection
            if lat_ma < 0
                % center Antarctica
                m_proj(projtype,'lat',-90,...
                      'long',0.5*(lon_mi+lon_ma),...
                      'radius',min(lat_ma+90,180),'rot',rot);
            else
                % center Arctic
                m_proj(projtype,'lat',90,...
                      'long',0.5*(lon_mi+lon_ma),...
                      'radius',min(90-lat_mi,180),'rot',rot);
            end
            m_proj('get') ;
        elseif  ~isempty(regexp(projtype,'ort')) || ...
                ~isempty(regexp(projtype,'gno')) || ...
                ~isempty(regexp(projtype,'azi')) || ...
                ~isempty(regexp(projtype,'sat'))
            m_proj(projtype,'lat',lat_mea,'long',lon_mea,...
                   'radius',rad,'rot',rot);
            m_proj('get') ;
            del = 1;
        elseif ~isempty(regexp(projtype,'obl'))
            % Oblique Mercator projection
            asp = (lon_ma-lon_mi)/(lat_ma - lat_mi);
            dir = 'hor';
            if asp > 1
                asp = 1./asp; dir = 'ver';
            end
            m_proj(projtype,'lon',[lon_mi lon_ma],...
                            'lat',[lat_mi lat_ma],...
                            'aspect',asp,'dir',dir) ;
            m_proj('get') ;
            del = 1;
        else
            % Cylindrical, Conic or Global type projections
            del = 1;
            m_proj(projtype,'lon',[lon_mi lon_ma],...
                            'lat',[lat_mi lat_ma]) ;
            m_proj('get') ;
        end
    end
    if insert
        global MAP_PROJECTION MAP_COORDS MAP_VAR_LIST
        obj.proj   = MAP_PROJECTION ;
        obj.coord  = MAP_COORDS ;
        obj.mapvar = MAP_VAR_LIST ;
    end
end


function A=posmatr(matriks)
%fungsi ini untuk menghitung pada baris ke berapa suatu matriks mencapai
%nilai maksimum
[m,n,o,p]=size(matriks);
for j=1:p
        A(1,j)=posmax(matriks(:,j));
end
A;


function l = tpl(tab, varargin)
%TPL compute total projection length
%   TPL(IMG) compute the total projection length of structure in image IMG.
%   Projection is computed along the last dimension of image. It
%   correspond to total diameter defined by Serra.
%
%   TPL(IMG, PDIM) specifies dimensions to manage. PDIM= [0 .. 0 1]
%   correspond to the default case. If PDIM contains only one 1, it is a
%   total diameter computation. If PDIM contains 2 ones, it is a total
%   projected surface computation. If PDIM contains only zeros, TPL
%   performs an Euler-Poincare Characteristic computation. If PDIM contains
%   only ones, it performs an area (2D case), volume (3D  case), or
%   Lebesgue measure.
%
%   TPL(IMG, PDIM, CONN) also specifies the neighbourhood configuration to
%   use, which can be 'minimal', or 'maximal'. Default is 'minimal'.
%
%   See also :
%   TPL, MINKOWSKI
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 10/09/2003.
%

%   HISTORY
%   20/04/2004 : 2D : do not call epc, accelerating processing

% remove useless dimensions and convert to binary image
tab = squeeze(tab)~=0;

% input image size
dim = size(tab);

% set default direction for projection : along the last dimension
if length(dim)==2 && (dim(1)==1 || dim(2)==1)
    pdim = 1;                           % dimension 1
else
    pdim = zeros(1, length(dim));       % dimension > 1
    pdim(length(pdim))=1;
end

if ~isempty(varargin)
    pdim = varargin{1};
end

% init
%nc=0;
l = 0;

% dimension 1 -------------------------------------------

if length(dim)==2 && (dim(1)==1 || dim(2)==1)
    if pdim(1)==1
        l = sum(tab);       % total length computation
    else
        l = epc(tab);       % EPC in dimension 1
    end
end

% dimension 2 -------------------------------------------

if length(dim)==2 && dim(1)~=1 && dim(2)~=1
    N1 = dim(1); N2 = dim(2);
    if sum(pdim==[1 0])==2
        % projection along y (first dimension of img)
        l = sum(sum(~tab(:,1:N2-1) & tab(:,2:N2))) + sum(tab(:,1));
    elseif sum(pdim == [0 1])==2
        % projection along x (second dimension of img)
        l = sum(sum(~tab(1:N1-1,:) & tab(2:N1,:))) + sum(tab(1,:));
    elseif sum(pdim == [0 0])==2
        % EPC in dimension 2
        l = epc(tab);
    elseif sum(pdim == [1 1])==2
        % area computation
        l = sum(tab(:));
    end
end

% dimension 3 -------------------------------------------

if length(dim)==3
    N1 = dim(1); N2 = dim(2); N3 = dim(3);

    % three total diameters computations
    if sum(pdim == [1 0 0])==3
        % total diameter in x axis
        n = sum(tab(:));
        n1 = sum(sum(sum(tab(:,1:N2-1,:)&tab(:,2:N2,:))));
        n2 = sum(sum(sum(tab(:,:,1:N3-1)&tab(:,:,2:N3))));
        n12 = sum(sum(sum(tab(:,1:N2-1,1:N3-1) & tab(:,1:N2-1,2:N3) & ...
            tab(:,2:N2,1:N3-1) & tab(:,2:N2,2:N3) )));
        l = n - n1 - n2 + n12;

    elseif sum(pdim == [0 1 0])==3
        % total diameter in y axis
        n = sum(tab(:));
        n1 = sum(sum(sum(tab(1:N1-1,:,:)&tab(2:N1,:,:))));
        n2 = sum(sum(sum(tab(:,:,1:N3-1)&tab(:,:,2:N3))));
        n12 = sum(sum(sum(tab(1:N1-1,:,1:N3-1) & tab(1:N1-1,:,2:N3) & ...
            tab(2:N1,:,1:N3-1) & tab(2:N1,:,2:N3) )));
        l = n - n1 - n2 + n12;

    elseif sum(pdim == [0 0 1])==3
        % total diameter in z axis
        n = sum(tab(:));
        n1 = sum(sum(sum(tab(1:N1-1,:, :)&tab(2:N1,:, :))));
        n2 = sum(sum(sum(tab(:,1:N2-1, :)&tab(:,2:N2, :))));
        n12 = sum(sum(sum(tab(1:N1-1,1:N2-1, :) & tab(1:N1-1,2:N2,:) & ...
            tab(2:N1,1:N2-1,:) & tab(2:N1,2:N2,:) )));
        l = n - n1 - n2 + n12;

        % three total projected area computations
    elseif sum(pdim == [1 1 0])==3
        % projected area on xy plane
        l = sum(sum(sum(~tab(:,:,1:N3-1) & tab(:,:,2:N3)))) + ...
            sum(sum(tab(:, :, 1)));
    elseif sum(pdim == [1 0 1])==3
        % projected area on xz plane
        l = sum(sum(sum(~tab(:,1:N2-1,:) & tab(:,2:N2,:)))) + ...
            sum(sum(tab(:, 1, :)));
    elseif sum(pdim == [0 1 1])==3
        % projected area on yz plane
        l = sum(sum(sum(~tab(1:N1-1,:,:) & tab(2:N1,:,:)))) + ...
            sum(sum(tab(1, :, :)));

    elseif sum(pdim == [0 0 0])==3
        % EPC in dimension 3
        l = epc(tab);

    elseif sum(pdim == [1 1 1])==3
        % volume computation
        l = sum(tab(:));

    end

end


 function [xhat, smap, ref] = ir_mri_coil_combine(ykj, varargin)
%function [xhat, smap, ref] = ir_mri_coil_combine(ykj, varargin)
%|
%| Given multiple MRI surface coil images, estimate their sensitivity maps,
%| relative to the sqrt-sum-of-squares (SSoS) map, and then perform the
%| "best" linear combination using the sensitivity maps.
%| todo: currently ignores noise correlations
%|
%| This routine provides an alternative to the usual "sqrt-sum-of-squares"
%| approach to combining multiple coil data.  It produces a complex-valued
%| output image that has approximately a complex gaussian distribution at
%| each voxel.  However, it does not preserve image phase information
%| because the image phase is included in the sensitivity map phase estimates.
%| So it might be logical to just take the real part of the output image.
%| The output is "approximately" gaussian only because of random errors in
%| the sensitivity maps estimated within.
%|
%| The variance is space-dependent because of coil sensitivity variations.
%| todo: return variance map!
%|
%| Signal model: y_kj = s_kj f_j + noise_kj
%|	s_kj	sensitivity map (relative to SSoS reference image)
%|	f_j	unknown underlying object
%|	k: coil index
%|	j: voxel index
%|
%| in
%|	ykj	[(N) ncoil]	noisy complex images (2D or 3D) for each coil
%|
%| options
%|	bodycoil [(N)]		reference image (optional) (default: SSoS)
%|	thresh0			zero reference image values below this fraction
%|				 of peak value (default: 0.05)
%|	thresh1			fraction of reference peak used for median
%|				 initial value in "background" (default: 0.05)
%|
%| out
%|	xhat	[(N)]		coil combination image
%|	smap	[(N) ncoil]	estimated sensitivity maps
%|
%| Uses the 'chol' (Cholesky) option of mri_sensemap_denoise.m which is
%| fast for medium sized images but may use much memory for large images.
%|
%| Copyright 2014-08-18, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if nargin == 1 && streq(ykj, 'test')
	ir_mri_coil_combine_test
return
end

% defaults
arg.thresh0 = 0.05; % below this set reference image to 0
arg.thresh1 = 0.05; % below this set reference image to median
arg.bodycoil = [];
arg.chol = true;

arg = vararg_pair(arg, varargin);

tmp = size(ykj);
NN = tmp(1:end-1);
ncoil = tmp(end);

% default reference image
if isempty(arg.bodycoil)
	ref = ir_mri_coil_combine_ref(ykj, arg.thresh0);
else
	ref = arg.bodycoil;
end

smap = mri_sensemap_denoise(ykj, 'thresh', arg.thresh1, ...
	'bodycoil', ref, 'chol', 1, 'niter', 1);

coildim = ndims(ykj);
numer = sum(conj(smap) .* ykj, coildim);
denom = sum(abs(smap).^2, coildim);
xhat = div0(numer, denom); % LS solution at each pixel

% ir_mri_coil_combine_ref()
function ref = ir_mri_coil_combine_ref(ykj, thresh0)
coildim = ndims(ykj);
ref = sqrt(sum(abs(ykj).^2, coildim)); % sqrt-sum-of-squares
peak = max(abs(ref(:)));
ref(abs(ref) < peak * thresh0) = 0; % zero low SNR pixels
%ref = ref / peak; % normalize to 1

% ir_mri_coil_combine_test()
% built-in test/example
function ir_mri_coil_combine_test

if 1 % xtrue
	f.dir = [path_find_dir('mri') '/../data/mri/'];
	f.xtrue = [f.dir 'brainweb_t1.jpg'];
	xtrue = single(imread(f.xtrue)');
	xtrue = xtrue(2:end-1,2:end-1); % make it 256^2
	xtrue = downsample2(xtrue, 4); % now 64^2
	[nx, ny] = size(xtrue);
	atrue = 2*pi * (-0.5+([1:nx]'/nx * [1:ny]/ny).^0.5); % smooth phase
	xtrue = xtrue .* exp(1i * atrue); % phase
%	im clf, im('hsv', angle(xtrue), [-pi pi]), cbar, return

	im plc 3 4
	im(1, abs(xtrue), '$|x|$'), cbar
	im(2, 'hsv', angle(xtrue), [-pi pi], '$\angle x$'), cbar
	mask = conv2(abs(xtrue), ones(5), 'same') > 0;
%	im(11, abs(xtrue) + 100*mask), return % check mask
end

if 1 % smap
	ncoil = 4;
	mask4 = repmat(mask, [1 1 ncoil]);
	smap = ir_mri_sensemap_sim('nx', nx, 'ny', ny, 'dx', 192/nx, ...
		'ncoil', ncoil, 'rcoil', 100);
%	smap = smap(:, :, [ncoil 1:(ncoil-1)]); % phase of last coil is cooler
	tmp = sqrt(sum(abs(smap).^2, 3));
	smap = smap / tmp(end/2,end/2); % normalize true smap for simplicity
	im(5, mask4 .* abs(smap), '$|s|$'), cbar
	im(6, 'hsv', mask4 .* angle(smap), [-pi pi], '$\angle s$'), cbar
	im(12, mask .* sqrt(sum(abs(smap).^2, 3)), 's SSoS'), cbar
end

if 1 % y
	ytrue = smap .* repmat(xtrue, [1 1 4]);
	rng(0)
	snr2sigma = @(db, yb) 10^(-db/20) * norm(yb(:)) / sqrt(numel(yb)) / sqrt(2); % for complex noise
	sig = snr2sigma(50, ytrue);
%	sig = 0; % noiseless
	ykj = ytrue + sig * (randn(size(ytrue)) + 1i * randn(size(ytrue)));
	im(7, abs(ykj), '$|y|$'), cbar
	im(8, 'hsv', ir_unwrap(angle(ykj)), [-pi pi], '$\angle y$'), cbar
end

	[xhat shat ref] = ir_mri_coil_combine(ykj);
	ssos = sqrt(sum(abs(ykj).^2, 3));
	xhat = xhat * ir_best_scale(xhat, abs(xtrue));
	ssos = ssos * ir_best_scale(ssos, abs(xtrue));

	pr nrms(xhat(:), col(abs(xtrue)))
	pr nrms(ssos(:), col(abs(xtrue)))
%	pr corrcoef(abs(xtrue(:)), ssos(:))
%	pr corrcoef(abs(xtrue(:)), real(xhat(:)))

%	xhat = reale(xhat);
%	im(3, abs(xhat), '|x hat|'), cbar
%	im(4, angle(xhat), [-pi pi], '< x hat'), cbar
	im(3, real(xhat), 'Re($\hat{x}$)'), cbar
	im(4, imag(xhat), 'Im($\hat{x}$)'), cbar
	im(9, mask4 .* abs(shat), '$|\hat{s}|$'), cbar
	im(10, 'hsv', mask4 .* angle(shat), [-pi pi], '$\angle \hat{s}$'), cbar
	im(11, ssos, 'x SSoS'), cbar


classdef DynamicPressureTermCondition < AbstractEventTerminationCondition
    %AltitudeTermCondition Summary of this class goes here
    %   Detailed explanation goes here

    properties
        dynP(1,1) double = 0; %km
        bodyInfo KSPTOT_BodyInfo
    end

    methods
        function obj = DynamicPressureTermCondition(dynP)
            obj.dynP = dynP;
        end

        function evtTermCondFcnHndl = getEventTermCondFuncHandle(obj)
            evtTermCondFcnHndl = @(t,y) obj.eventTermCond(t,y, obj.dynP, obj.bodyInfo);
        end

        function initTermCondition(obj, initialStateLogEntry)
            obj.bodyInfo = initialStateLogEntry.centralBody;
        end

        function name = getName(obj)
            name = sprintf('Dynamic Pressure (%.3f kPa)', obj.dynP);
        end

        function tf = shouldBeReinitOnRestart(obj)
            tf = true;
        end

        function params = getTermCondUiStruct(obj)
            params = struct();

            params.paramName = 'Dynamic Pressure';
            params.paramUnit = 'kPa';
            params.useParam = 'on';
            params.useStages = 'off';
            params.useTanks = 'off';
            params.useEngines = 'off';
            params.useStopwatches = 'off';

            params.value = obj.dynP;
            params.refStage = LaunchVehicleStage.empty(1,0);
            params.refTank = LaunchVehicleEngine.empty(1,0);
            params.refEngine = LaunchVehicleEngine.empty(1,0);
            params.refStopwatch = LaunchVehicleStopwatch.empty(1,0);
        end

        function optVar = getNewOptVar(obj)
            optVar = DynPressOptimizationVariable(obj);
        end

        function optVar = getExistingOptVar(obj)
            optVar = obj.optVar;
        end

        function tf = usesStage(obj, stage)
            tf = false;
        end

        function tf = usesEngine(obj, engine)
            tf = false;
        end

        function tf = usesTank(obj, tank)
            tf = false;
        end

        function tf = usesEngineToTankConn(obj, engineToTank)
            tf = false;
        end

        function tf = usesStopwatch(obj, stopwatch)
            tf = false;
        end
    end

    methods(Static)
        function termCond = getTermCondForParams(paramValue, stage, tank, engine, stopwatch)
            termCond = DynamicPressureTermCondition(paramValue);
        end
    end

    methods(Static, Access=private)
        function [value,isterminal,direction] = eventTermCond(ut,y, targetDynP, bodyInfo)
            rVectECI = y(1:3);
            vVectECI = y(4:6);

            altitude = norm(rVectECI) - bodyInfo.radius;

            if(altitude <= bodyInfo.atmohgt && altitude >= 0)
                [lat, long, ~, ~, ~, ~, ~, vVectECEF] = getLatLongAltFromInertialVect(ut, rVectECI, bodyInfo, vVectECI);
                density = getAtmoDensityAtAltitude(bodyInfo, altitude, lat, ut, long);
            elseif(altitude <= 0)
                density = 0;
                vVectECEF = [0;0;0];
            else
                density = 0;
                vVectECEF = [0;0;0];
            end

            vVectEcefMag = norm(vVectECEF);
            vVectEcefMagMS = vVectEcefMag*1000;

            dynP = density * (vVectEcefMagMS^2) / 2; %kg/m^3 * m^2 / s^2  = kg/(m*s^2)
            dynP_kPa = dynP/1000;

            value = dynP_kPa - targetDynP;
            isterminal = 1;
            direction = 0;
        end
    end
end


%% retraction of d in T_X(M_r) onto M_r
%   X=U*diag(S)*V'
%   d=U*M*V'+U_p*V'+U*V_p'
%   cf. [Van12]

function [S1,U1,V1]=rtr_fr(d,S,U,V)

n1=size(U,1);
n2=size(V,1);
n3=size(S,1);

sig=0;
if size(S,2)>1, S=diag(S); sig=1; end

eps=1e-3;

% Pu=U*U'; Pv=V*V';

M=U'*d*V;
% U_p=d*V-Pu*d*V; V_p=d'*U-Pv*d'*U;
U_p=d*V-U*M; V_p=d'*U-V*M';

[Q_u,R_u]=qr(U_p,0);
[Q_v,R_v]=qr(V_p,0);
M1=[diag(S)+M, R_v'; R_u, zeros(n3)];
[U1,S1,V1]=svd(M1);
S1=diag(S1); S1=S1(1:n3);
U1=[U,Q_u]*U1(:,1:n3);
V1=[V,Q_v]*V1(:,1:n3);

S1=max(S1,eps);

if sig==1, S1=diag(S1); end


function [mu, varsigma] = vargplvmPosteriorMeanVarPar(model, X, varX);

% VARGPLVMPOSTERIORMEANVARPAR Mean and variances of the posterior at points given by X, parallel version.
% FORMAT
% DESC returns the posterior mean and variance for a given set of
% points.
% ARG model : the model for which the posterior will be computed.
% ARG X : variational mean in the latent space for which posterior is computed.
% ARG varX : variational variances in the latent space for which posterior is computed (assumed zero if not present).
% RETURN mu : the mean of the posterior distribution.
% RETURN sigma : the variances of the posterior distributions.
%
% SEEALSO : vargplvmPosteriorMeanVar, gpPosteriorMeanVar, vargplvmCreate
%
% COPYRIGHT : Michalis K. Titsias and Neil D. Lawrence, 2009, 2011

% VARGPLVM

% do prediction by replacing the variational distribution with a delta function
%model.K_uf = kernCompute(model.kern, model.X_u, model.vardist.means);
%model.A = (1/model.beta)*model.K_uu + model.K_uf*model.K_uf';
%[model.Ainv, U] = pdinv(model.A);
%[mu1, varsigma1] = gpPosteriorMeanVar(model, vardistX.means);

% Find exactly the mean and the variances of the predictive distribution
% (which is not Gaussian, however its moments can be computed in closed-form)

if nargin < 3
  vardistX.covars = repmat(0.0, size(X, 1), size(X, 2));%zeros(size(X, 1), size(X, 2));
else
  vardistX.covars = varX;
end
vardistX.latentDimension = size(X, 2);
vardistX.numData = size(X, 1);
%model.vardist.covars = 0*model.vardist.covars;
vardistX.means = X;
%model = vargplvmUpdateStats(model, model.X_u);

Ainv = model.P1' * model.P1; % size: NxN

if ~isfield(model,'alpha')
    model.alpha = Ainv*model.Psi1'*model.m; % size: 1xD
end
Psi1_star = kernVardistPsi1Compute(model.kern, vardistX, model.X_u);

% mean prediction
mu = Psi1_star*model.alpha; % size: 1xD

if nargout > 1
   %
   % precomputations
   vard = vardistCreate(zeros(1,model.q), model.q, 'gaussian');
   Kinvk = (model.invK_uu - (1/model.beta)*Ainv);
   %
   for i=1:size(vardistX.means,1)
      %
      vard.means = vardistX.means(i,:);
      vard.covars = vardistX.covars(i,:);
      % compute psi0 term
      Psi0_star = kernVardistPsi0Compute(model.kern, vard);
      % compute psi2 term
      Psi2_star = kernVardistPsi2Compute(model.kern, vard, model.X_u);

      vars = Psi0_star - sum(sum(Kinvk.*Psi2_star));

     % parfor j=1:model.d
     %    %[model.alpha(:,j)'*(Psi2_star*model.alpha(:,j)), mu(i,j)^2]
     %    varsigma(i,j) = model.alpha(:,j)'*(Psi2_star*model.alpha(:,j)) - mu(i,j)^2;
     % end
     % varsigma(i,:) = varsigma(i,:) + vars;
      %
   end
   %
   %if isfield(model, 'beta')
   %   varsigma = varsigma + (1/model.beta);
   %end
   %
end

% Rescale the mean
mu = mu.*repmat(model.scale, size(vardistX.means,1), 1);

% Add the bias back in
mu = mu + repmat(model.bias, size(vardistX.means,1), 1);

% rescale the variances
if nargout > 1
    varsigma = varsigma.*repmat(model.scale.*model.scale, size(vardistX.means,1), 1);
end


function [overall_peaks, overall_onsets] = co_ppg_peak_detector(ppg, fs, up)
%CO_PPG_PEAK_DETECTOR detects PPG pulse peaks
%
% This has been adapted by Peter Charlton from code supplied by Christina
% Orphanidou.
%
% Purpose: To detect PPG peaks.
%
% With grateful thanks to Christina Orphanidou and Alexander Darrell.
%

%% Pre-processing
% remove nans
ppg.v(isnan(ppg.v)) = mean(ppg.v(~isnan(ppg.v)));
% filter
ppgfilt.v = filtfilt(up.paramSet.CO_peak_det.PPG_smoother.coeffs, 1, ppg.v);
ppgfilt.t = ppg.t;

%% Setup
% Segmentation into 10s windows
win_length = 10;
overlap = 3;
win_starts = ppgfilt.t(1):win_length:ppgfilt.t(end);
win_ends = win_starts + win_length + overlap;
win_ends(end) = win_ends(end) - overlap;
% setup variables
[overall_peaks, overall_onsets] = deal([]);

%% Perform peak detection on each 10s window
for win_no = 1 : length(win_starts)

    rel_data = struct;

    rel_data.i = find(ppgfilt.t >= win_starts(win_no) & ppgfilt.t <= win_ends(win_no));
    rel_data.t = ppgfilt.t(rel_data.i);
    rel_data.v = ppgfilt.v(rel_data.i);

    %% Calculate thresholds
    thresh1=quantile(rel_data.v,up.paramSet.CO_peak_det.upctl);
    thresh2=quantile(rel_data.v,up.paramSet.CO_peak_det.lpctl);
    thresh3=thresh2+0.3*(thresh1-thresh2);
    thresh4=thresh2+0.7*(thresh1-thresh2);

    %% Find all peaks
    % identify peaks
    diffs_on_left_of_pt = diff(rel_data.v); diffs_on_left_of_pt = diffs_on_left_of_pt(1:(end-1)); diffs_on_left_of_pt = logical(diffs_on_left_of_pt>0);
    diffs_on_right_of_pt = diff(rel_data.v); diffs_on_right_of_pt = diffs_on_right_of_pt(2:end); level_on_right_of_pt = logical(diffs_on_right_of_pt==0); diffs_on_right_of_pt = logical(diffs_on_right_of_pt<0);
    diffs_on_second_right_of_pt = [1; diff(rel_data.v(2:end))]; diffs_on_second_right_of_pt = diffs_on_second_right_of_pt(2:end); diffs_on_second_right_of_pt = logical(diffs_on_second_right_of_pt<0);
    peaks.i = find((diffs_on_left_of_pt & diffs_on_right_of_pt) | ...
        (diffs_on_left_of_pt & level_on_right_of_pt & diffs_on_second_right_of_pt))+1;
    peaks.i = peaks.i';
    % Take maximum value in window if no peaks were found
    if isempty(peaks.i)
        peaks.i = find(max(rel_data.v) == rel_data.v);
    end
    % Extract signal values at peaks
    peaks.t = rel_data.t(peaks.i);
    peaks.v = rel_data.v(peaks.i);

    %% Classify peaks
    % Identify relevant peaks according to amplitude
    upperdiff = abs(peaks.v-thresh1);
    middlehighdiff = abs(peaks.v-thresh4);
    middlelowdiff = abs(peaks.v-thresh3);
    lowerdiff = abs(peaks.v-thresh2);
    upper_pks = find(upperdiff<middlehighdiff & upperdiff<middlelowdiff & upperdiff<lowerdiff);
    PPG_PKS.i = peaks.i(upper_pks);
    PPG_PKS.v = peaks.v(upper_pks);
    PPG_PKS.t = peaks.t(upper_pks);
    % eliminate peaks which are too close together in time
    good_els = find(diff(PPG_PKS.i)>=fs/3)+1;
    PPG_PKS.i = PPG_PKS.i(good_els);
    PPG_PKS.v = PPG_PKS.v(good_els);
    PPG_PKS.t = PPG_PKS.t(good_els);

    %% Find troughs

    PPG_TRS.i = nan(length(PPG_PKS.t)-1,1);
    for s = 1 : (length(PPG_PKS.t)-1)
        start_el = PPG_PKS.i(s);
        [~, additional_el] = min(rel_data.v(PPG_PKS.i(s):PPG_PKS.i(s+1)));
        PPG_TRS.i(s) = start_el - 1 + additional_el;
    end
    PPG_TRS.v = rel_data.v(PPG_TRS.i);
    PPG_TRS.t = rel_data.t(PPG_TRS.i);

    temp_pk_is = rel_data.i(PPG_PKS.i(:));
    temp_on_is = rel_data.i(PPG_TRS.i(:));

    overall_peaks = [overall_peaks; temp_pk_is(:)]; overall_peaks = unique(overall_peaks);
    overall_onsets = [overall_onsets; temp_on_is(:)]; overall_onsets = unique(overall_onsets);

    clear upper lower middlehigh middlelow temp_pk_is temp_on_is PPG_TRS PPG_PKS additional_el s good_els peaks lower_pks upper_pks lowerdiff middlelowdiff middlehighdiff upperdiff ind

end


function C=plotwmdct(coef,varargin)
%PLOTWMDCT  Plot WMDCT coefficients
%   Usage: plotwmdct(coef);
%          plotwmdct(coef,fs);
%          plotwmdct(coef,fs,dynrange);
%
%   `plotwmdct(coef)` plots coefficients from |wmdct|.
%
%   `plotwmdct(coef,fs)` does the same assuming a sampling rate of
%   *fs* Hz of the original signal.
%
%   `plotwmdct(coef,fs,dynrange)` additionally limits the dynamic
%   range.
%
%   `C=plotwmdct(...)` returns the processed image data used in the
%   plotting. Inputting this data directly to `imagesc` or similar
%   functions will create the plot. This is useful for custom
%   post-processing of the image data.
%
%   |plotwmdct| supports all the optional parameters of |tfplot|. Please
%   see the help of |tfplot| for an exhaustive list.
%
%   See also:  wmdct, tfplot, sgram, plotdgt

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: NA
%   REFERENCE: NA

if nargin<1
  error('%s: Too few input parameters.',upper(mfilename));
end;

definput.import={'ltfattranslate','tfplot'};

[flags,kv,fs]=ltfatarghelper({'fs','dynrange'},definput,varargin);

M=size(coef,1);

yr=[.5/M, 1-.5/M];

C = tfplot(coef,M,yr,'argimport',flags,kv);

if nargout<1
    clear C;
end


function displayMobject(mObject,fieldname,note)
%   M-object information display
%   displayMobject(mObject,fieldname,note);

%   Designed and coded by Hideki Kawahara
%   27/Feb./2005
%   Copyright(c) 2005, Hideki Kawahara
%   05/Oct./2005 minor bug fix

fs = mObject.samplingFrequency;
tFrame = mObject.frameUpdateInterval;
switch fieldname
    case 'spectrogram'
        figure
        [nrow,ncolumn]=size(mObject.spectrogram);
        timeSpan = [0 (ncolumn-1)*tFrame];
        dBsgram = 20*log10(mObject.spectrogram);
        maxSgramdB = max(max(dBsgram));
        imagesc(timeSpan, [0 fs/2],max(dBsgram,maxSgramdB-70));
        axis('xy');
        set(gca,'fontsize',14);
        xlabel('time (ms)');
        ylabel('frequency (Hz)');
        title([note ' time span 0 ' num2str(timeSpan(2),10) ' (ms) ' datestr(now)]);
    case 'waveform'
        figure
        x = mObject.waveform;
        timeSpan = (0:length(x)-1)/fs*1000;
        plot(timeSpan,x);grid on;
        axis([timeSpan(1) timeSpan(end) 1.1*[min(x) max(x)]]);
        set(gca,'fontsize',14);
        xlabel('time (ms)');
        title([note ' time span 0 ' num2str(round(timeSpan(end)),8) ' (ms) ' datestr(now)]);
    case {'anchorFrequency', 'anchorTimeLocation'}
        figure
        [nrow,ncolumn]=size(mObject.spectrogram);
        timeSpan = [0 (ncolumn-1)*tFrame];
        dBsgram = 20*log10(mObject.spectrogram);
        maxSgramdB = max(max(dBsgram));
        imagesc(timeSpan, [0 fs/2],max(dBsgram,maxSgramdB-70));
        axis('xy');
        set(gca,'fontsize',14);
        xlabel('time (ms)');
        ylabel('frequency (Hz)');
        title([note ' time span 0 ' num2str(timeSpan(2),10) ' (ms) ' datestr(now)]);
        if length(mObject.anchorTimeLocation)>0
            hold on;
            for ii=1:length(mObject.anchorTimeLocation)
                hh = plot(mObject.anchorTimeLocation(ii)*[1 1],[0 fs/2],'w:');
                set(hh,'linewidth',2);
                if sum(mObject.anchorFrequency(ii,:)>0)>0
                    nFrequency = sum(mObject.anchorFrequency(ii,:)>0);
                    anchorFrequencyVector = mObject.anchorFrequency(ii,mObject.anchorFrequency(ii,:)>0); % 05/Oct./2005 HK
                    for jj=1:nFrequency
                        hh=plot(mObject.anchorTimeLocation(ii),anchorFrequencyVector(jj),'ok');
                        set(hh,'markersize',9);
                        set(hh,'linewidth',2);
                        hh=plot(mObject.anchorTimeLocation(ii),anchorFrequencyVector(jj),'.w');
                        set(hh,'markersize',7);
                        set(hh,'linewidth',4);
                    end;
                end;
            end;
            hold off;
        end;
end;


function test_ft_spike_rate()

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_spike_rate

clear
spikesPerTrial = 1;
nTrials = 100;
shapePar = 2;
scalePar = 3;
data = [];
time       = linspace(0,1,1000);
data.trial(1:nTrials) = {zeros(1,length(time))};
data.time(1:nTrials) = {time};
for iTrial = 1:nTrials
  for iUnit = 1:4
      data.trial{iTrial}(iUnit,:) = rand(1,1000)<iTrial/100;
      if iUnit==4
        data.trial{iTrial}(iUnit,:) = rand(1,1000);
      end
  end
end
data.fsample = 1000;
data.hdr = [];
data.cfg.trl = [];
data.label{1} = 'chan1';
data.label{end+1} = 'chan2';
data.label{end+1} = 'chan3';
data.label{end+1} = 'chan4';

% show that the psth works also with the poisson format
cfg.spikechannel = 1:3;
spike = ft_checkdata(data,'datatype', 'spike', 'feedback', 'yes');

%%
cfgRate = [];
cfgRate.outputunit = 'spikecount';
cfgRate.keeptrials = 'yes';
cfgRate.vartriallen = 'no';
cfgRate.latency = [-10 20];
RateCnt1 = ft_spike_rate(cfgRate,spike);

%%
cfgRate = [];
cfgRate.outputunit = 'spikecount';
cfgRate.keeptrials = 'no';
cfgRate.trials = 1:2:30;
RateCnt1 = ft_spike_rate(cfgRate,spike);

cfgRate.trials = 32:2:60;
RateCnt2 = ft_spike_rate(cfgRate,spike);
cfgRate.trials = 61:2:99;
RateCnt3 = ft_spike_rate(cfgRate,spike);

cfg.stimuli = [0 pi/8 pi/4];
cfg.method = 'orientation';
stat = ft_spike_rate_orituning(cfg,RateCnt1,RateCnt2,RateCnt3);
%%

cfgRate = [];
cfgRate.outputunit = 'spikecount';
cfgRate.keeptrials = 'yes';
cfgRate.trials = 1:5:30;
RateCnt1 = ft_spike_rate(cfgRate,spike);
disp('expect about 10 times the number of spikes as the trial number')
[RateCnt1.cfg.trials(:) RateCnt1.trial]


function test_suite = test_distancePoints
%TESTDISTANCEPOINTS  One-line description here, please.
%   output = test_distancePoints(input)
%
%   Example
%   testDistancePoints
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2009-04-22,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.
% Licensed under the terms of the LGPL, see the file "license.txt"

test_suite = functiontests(localfunctions);

function testSingleSingle(testCase) %#ok<*DEFNU>

pt1 = [10 10];
pt2 = [10 20];
pt3 = [20 20];

testCase.assertEqual(distancePoints(pt1, pt2), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt2, pt3), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt1, pt3), 10*sqrt(2), 'AbsTol', .01);

function testSingleSingleNorm1(testCase)
% test norm 1, equivalent to sum of absolute differences

pt1 = [10 10];
pt2 = [10 20];
pt3 = [20 20];

testCase.assertEqual(distancePoints(pt1, pt2, 1), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt2, pt3, 1), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt1, pt3, 1), 20, 'AbsTol', .01);

function testSingleSingleMaxNorm(testCase)

pt1 = [10 10];
pt2 = [10 20];
pt3 = [20 20];

testCase.assertEqual(distancePoints(pt1, pt2, inf), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt2, pt3, inf), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt1, pt3, inf), 10, 'AbsTol', .01);

function testSingleSingle3d(testCase)

pt1 = [10 10 10];
pt2 = [10 20 10];
pt3 = [20 20 10];
pt4 = [20 20 20];

testCase.assertEqual(distancePoints(pt1, pt2), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt2, pt3), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt1, pt3), 10*sqrt(2), 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt1, pt4), 10*sqrt(3), 'AbsTol', .01);

function testSingleSingle3dNorm1(testCase)
% test norm 1, equivalent to sum of absolute differences

pt1 = [10 10 30];
pt2 = [10 20 30];
pt3 = [20 20 30];
pt4 = [20 20 40];

testCase.assertEqual(distancePoints(pt1, pt2, 1), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt2, pt3, 1), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt1, pt3, 1), 20, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt1, pt4, 1), 30, 'AbsTol', .01);

function testSingleSingleMaxNorm3d(testCase)

pt1 = [10 10 10];
pt2 = [10 20 10];
pt3 = [20 20 10];
pt4 = [20 20 20];

testCase.assertEqual(distancePoints(pt1, pt2, inf), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt2, pt3, inf), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt1, pt3, inf), 10, 'AbsTol', .01);
testCase.assertEqual(distancePoints(pt1, pt4, inf), 10, 'AbsTol', .01);

function testSingleArray(testCase)

pt1 = [10 10];
pt2 = [10 20];
pt3 = [20 20];

testCase.assertEqual(...
    distancePoints(pt1, [pt1; pt2; pt3]), ...
    [0 10 10*sqrt(2)], 'AbsTol', .01);

function testArrayArray(testCase)

pt1 = [10 10];
pt2 = [10 20];
pt3 = [20 20];
pt4 = [20 10];

array1 = [pt1;pt2;pt3];
array2 = [pt1;pt2;pt3;pt4];
res = [...
    0 10 10*sqrt(2) 10;...
    10 0 10 10*sqrt(2);...
    10*sqrt(2) 10 0 10];

testCase.assertEqual(distancePoints(array1, array2), res, 'AbsTol', .01);

function testArrayArrayDiag(testCase)

pt1 = [10 10];
pt2 = [10 20];
pt3 = [20 20];

array = [pt1;pt2;pt3];

testCase.assertEqual(...
    distancePoints(array, array, 'diag'), ...
    [0;0;0], 'AbsTol', .01);

function testArrayArray3dDiag(testCase)

pt1 = [10 10 30];
pt2 = [10 20 30];
pt3 = [10 20 40];

array1 = [pt1;pt2;pt3];
array2 = [pt2;pt3;pt1];

testCase.assertEqual(...
    distancePoints(array1, array2, 'diag'), ...
    [10;10;10*sqrt(2)], 'AbsTol', .01);

function testArrayArray3dNorm1Diag(testCase)

pt1 = [10 10 30];
pt2 = [10 20 30];
pt3 = [10 20 40];

array1 = [pt1;pt2;pt3];
array2 = [pt2;pt3;pt1];

testCase.assertEqual(...
    distancePoints(array1, array2, 1, 'diag'), ...
    [10;10;20], 'AbsTol', .01);

function testArrayArrayDiagMaxNorm(testCase)

pt1 = [10 10];
pt2 = [10 20];
pt3 = [20 20];

array1 = [pt1;pt2;pt3];
array2 = [pt2;pt3;pt1];

testCase.assertEqual(...
    distancePoints(array1, array2, inf, 'diag'), ...
    [10;10;10], 'AbsTol', .01);


function [annotation, img] = LMimscale(annotation, img, scaling, method)
%
% Scales one image (as in imresize) and the associated annotation.
% The scale factor is given by 'scaling'.
%
% When scaling<1, the image is downsampled.
%
% [annotation, img] = LMimscale(annotation, img, scaling);
%
% or, if you are using the LabelMe struct D:
%
% [D(i).annotation, img] = LMimscale(D(i).annotation, img, scaling);
%
% Also, the scaling can be given in absolute coordinates:
%
% [annotation, img] = LMimscale(annotation, img, [NROWS NCOLS]);

% Store the original numerical format of the image and turn it into 'double'.

imgtype = whos('img');
img = single(img);

if nargin < 4
    method = 'bilinear';
end
if length(scaling) > 1
  scalingx = scaling(2)/size(img,2);
  scalingy = scaling(1)/size(img,1);
  if isnan(scalingx)
      scalingx = scalingy;
  end
  if isnan(scalingy)
      scalingy = scalingx;
  end
else
  scalingx = scaling;
  scalingy = scaling;
end

if scaling ~= 1
    if nargout > 1
      % Image resampling:
      %img = imresizefast(img, scaling, method);
      img = imresize(img, scaling, method);
    end

    % Change the size of the polygon coordinates
    if isfield(annotation, 'object')
        Nobjects = length(annotation.object); n=0;
        for i = 1:Nobjects
            [x,y] = getLMpolygon(annotation.object(i).polygon);
            x = round(x*scalingx);
            y = round(y*scalingy);
            annotation.object(i).polygon = setLMpolygon(x,y);

%             Npoints = length(annotation.object(i).polygon.pt);
%             for j = 1:Npoints
%                 % Scale each point:
%                 x=str2num(annotation.object(i).polygon.pt(j).x);
%                 y=str2num(annotation.object(i).polygon.pt(j).y);
%
%                 x = round(x*scalingx);
%                 y = round(y*scalingy);
%
%                 annotation.object(i).polygon.pt(j).x = num2str(x);
%                 annotation.object(i).polygon.pt(j).y = num2str(y);
%             end
        end
    end
end

% add/modify image size field
annotation.imagesize.nrows = size(img,1);
annotation.imagesize.ncols = size(img,2);

if nargout > 1
  % return the image in its original numeric format
  img = feval(imgtype.class, img);
end

function img = imresizefast(img, scaling, method, init)

if nargin<4
    init = 0;
end

if max(scaling) > .5
    img = imresize(img, scaling, method);
else
    c = size(img,3);
    for n = 1:c
        img(:,:,n) = conv2(img(:,:,n), [1 2 1; 2 4 2; 1 2 1]/16, 'same');
    end
    img = img(init+1:2:end, init+1:2:end, :);
    %img = convn(img, [1 2 1]/4, 'same');
    %img = img(:,init+1:2:end,:);
    img = imresizefast(img, 2*scaling, method, 1-init);
end


classdef DepthAnalysisWindow < AnalysisWindow
    % DEPTHANALYSISWINDOW shows the number of events, binned per depth
    properties
        binedges    double; % km
    end
    methods
        function obj=DepthAnalysisWindow(ax,binedges)
            obj@AnalysisWindow(ax);
            obj.binedges=binedges;
        end

        function prepare_axes(obj)
            % prepare the depth profile axes
            obj.ax.Tag          = 'dvEventsWidthDepth';
            obj.ax.YDir         = 'reverse';
            obj.ax.Title.String = 'Depth Profile';
            obj.ax.XLabel.String= 'Number of events';
            obj.ax.YLabel.String= 'Depth';
        end

        function [x,y]=calculate(obj,catalog)
            % calculate the number of events (y) for each binned depth(x)
            x = histcounts(catalog.Depth,obj.binedges);
            y=obj.binedges(1:end-1) + diff(obj.binedges)./2;
        end

    end
end


function [ fea, out ] = ex_planestrain1( varargin )
%EX_PLANESTRAIN1 Plane strain analysis of a pressure vessel.
%
%   [ FEA, OUT ] = EX_PLANESTRAIN1( VARARGIN ) Benchmark example for plane strain
%   approximation of a pressure vessel (annular cross section with symmetry).
%
%   Reference.  B. J. Mac Donald, Practical Stress Analysis with Finite Elements (2nd Ed),
%   case study E on page 327, 2007.
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       E           scalar {207e9}         Modulus of elasticity
%       nu          scalar {0.27}          Poissons ratio
%       sfun        string {sflag1}        Shape function for displacements
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'E',        207e9; ...
  'nu',       0.27; ...
  'sfun',     'sflag1'; ...
  'iplot',    1; ...
  'igrid',    1; ...
  'tol',      0.1; ...
  'fid',      1 };
[got,opt] = parseopt( cOptDef, varargin{:} );
fid       = opt.fid;

% Geometry and grid.
fea.sdim = { 'x' 'y' };   % Coordinate names.
fea.grid = ringgrid( 12, 216, 100e-3, 120e-3 );
fea.grid = delcells( fea.grid, selcells( fea.grid, '(x<=eps) | (y<=eps)') );
if( opt.igrid~=1 )
  fea.grid = quad2tri( fea.grid );
end
n_bdr = max(fea.grid.b(3,:));   % Number of boundaries.

% Problem definition.
fea = addphys( fea, @planestrain );
fea.phys.psn.eqn.coef{1,end} = { opt.nu };
fea.phys.psn.eqn.coef{2,end} = { opt.E  };
fea.phys.psn.sfun            = { opt.sfun opt.sfun };

% Boundary conditions.
bctype = mat2cell( zeros(2,n_bdr), [1 1], ones(1,n_bdr) );
bctype{1,4} = 1;
bctype{2,3} = 1;
fea.phys.psn.bdr.coef{1,5} = bctype;

bccoef = mat2cell( zeros(2,n_bdr), [1 1], ones(1,n_bdr) );
bccoef{1,1} = '-nx*1e4';
bccoef{2,1} = '-ny*1e4';
fea.phys.psn.bdr.coef{1,end} = bccoef;

% Parse and solve problem.
fea       = parsephys( fea );
fea       = parseprob( fea );                          % Check and parse problem struct.
fea.sol.u = solvestat( fea, 'fid', fid, 'icub', 1+str2num(strrep(opt.sfun,'sflag','')) );   % Call to stationary solver.

% Postprocessing.
s_disp = fea.phys.psn.eqn.vars{2,end};
if( opt.iplot>0 )
  figure
  postplot( fea, 'surfexpr', s_disp )
  title( 'Total displacement' )
end

% Error checking.
s_sx     = fea.phys.psn.eqn.vars{5,end};
s_sy     = fea.phys.psn.eqn.vars{6,end};
s_sxy    = fea.phys.psn.eqn.vars{8,end};
s_sp1    = fea.phys.psn.eqn.vars{9,end};
s_sp2    = fea.phys.psn.eqn.vars{10,end};
s_sp3    = fea.phys.psn.eqn.vars{11,end};
v_disp   = evalexpr( s_disp, [100e-3 120e-3-2*sqrt(eps);0 0]+sqrt(eps), fea )';
v_dref   = [2.64e-8 2.41e-8];
[v_sx(1),v_sx(2)] = minmaxsubd( s_sx, fea );
v_sxref  = [-10000 55454];
[v_sy(1),v_sy(2)] = minmaxsubd( s_sy, fea );
v_syref  = [-10000 55454];
[v_sxy(1),v_sxy(2)] = minmaxsubd( s_sxy, fea );
v_sxyref = [-32730 0];
[v_sp1(1),v_sp1(2)] = minmaxsubd( s_sp1, fea );
v_sp1ref = [4.5e4 55454];
[v_sp2(1),v_sp2(2)] = minmaxsubd( s_sp2, fea );
v_sp2ref = [1.227e4 1.227e4];
[v_sp3(1),v_sp3(2)] = minmaxsubd( s_sp3, fea );
v_sp3ref = [-1e4 0];
out.err  = [ abs([v_dref-v_disp])./v_dref ;
             abs([v_sxref-v_sx])./v_sxref ;
             abs([v_syref-v_sy])./v_syref ;
             abs([v_sxyref(1)-v_sxy(1)])./v_sxyref(1) 0 ;
             abs([v_sp1ref(2)-v_sp1(2)])./v_sp1ref(2) 0 ;
             abs([v_sp2ref-v_sp2])./v_sp2ref      ;
             abs([v_sp3ref(1)-v_sp3(1)])./v_sp3ref(1) 0 ];
out.pass = all( out.err(:) <= opt.tol );

if( nargout==0 )
  clear fea out
end


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcFlipV(Temp,Event,handles)
handles=guidata(handles.MU_matrix_display);

if ~isempty(handles.V.Segs)
    choice = questdlg('Segmentation mask is detected, transform operation will reset them, preceed?','Mask Reset', ...
                      'No, go save mask','Yes','No, go save mask');
    if isempty(choice)
        warndlg('Transform is cancelled.');
        return;
    end
    % Handle response
    switch choice
        case 'No, go save mask'
            warndlg('Save your mask before transform.');
            return;
    end

    handles.Mask=handles.Mask*0;
    handles.V.Segs=[];
end

% close 3D slicer
global Figure_handles
if isfield(Figure_handles,'MU_display2')
    slicer_display_handles=guidata(Figure_handles.MU_display2);
    if Figure_handles.MU_display == slicer_display_handles.Parent
        close(Figure_handles.MU_display2);
    end
end

handles.TMatrix=flipdim(handles.TMatrix,1);
handles.Mask=flipdim(handles.Mask,1);

MergeM=get(handles.Matrix_name_edit,'String');
set(handles.Matrix_name_edit,'String',[MergeM '_fpv']);

% update current display matrix
handles=MU_update_image(handles.Matrix_display_axes,{handles.TMatrix,handles.Mask},handles,0);
guidata(handles.MU_matrix_display, handles);

end


function sgmga_weight_test ( dim_num, importance, level_weight, ...
  level_max_min, level_max_max, rule, growth, np, p, tol )

%****************************************************************************80
%
%% SGMGA_WEIGHT_TEST checks the sum of the quadrature weights.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 June 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real IMPORTANCE(DIM_NUM), the anisotropic importance
%    for each dimension.
%
%    Input, real LEVEL_WEIGHT(DIM_NUM), the anisotropic weight
%    for each dimension.
%
%    Input, integer LEVEL_MAX_MIN, LEVEL_MAX_MAX, the minimum and
%    maximum values of LEVEL_MAX.
%
%    Input, integer RULE(DIM_NUM), the rule in each dimension.
%     1, "CC",  Clenshaw Curtis, Closed Fully Nested.
%     2, "F2",  Fejer Type 2, Open Fully Nested.
%     3, "GP",  Gauss Patterson, Open Fully Nested.
%     4, "GL",  Gauss Legendre, Open Weakly Nested.
%     5, "GH",  Gauss Hermite, Open Weakly Nested.
%     6, "GGH", Generalized Gauss Hermite, Open Weakly Nested.
%     7, "LG",  Gauss Laguerre, Open Non Nested.
%     8, "GLG", Generalized Gauss Laguerre, Open Non Nested.
%     9, "GJ",  Gauss Jacobi, Open Non Nested.
%    10, "HGK", Hermite Genz-Keister, Open Fully Nested.
%    11, "UO",  User supplied Open, presumably Non Nested.
%    12, "UC",  User supplied Closed, presumably Non Nested.
%
%    Input, integer GROWTH(DIM_NUM), the growth in each dimension.
%    0, "DF", default growth associated with this quadrature rule;
%    1, "SL", slow linear, L+1;
%    2  "SO", slow linear odd, O=1+2((L+1)/2)
%    3, "ML", moderate linear, 2L+1;
%    4, "SE", slow exponential;
%    5, "ME", moderate exponential;
%    6, "FE", full exponential.
%
%    Input, integer NP(DIM_NUM), the number of parameters used by each rule.
%
%    Input, real P(*), the parameters needed by each rule.
%
%    Input, real TOL, a tolerance for point equality.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SGMGA_WEIGHT_TEST:\n' );
  fprintf ( 1, '  Compute the weights of a sparse grid.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Each sparse grid is of spatial dimension DIM_NUM,\n' );
  fprintf ( 1, '  and is made up of product grids of levels up to LEVEL_MAX.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  IMPORTANCE:    ');
  for dim = 1 : dim_num
    fprintf ( 1, '  %14f', importance(dim) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEVEL_WEIGHT:  ');
  for dim = 1 : dim_num
    fprintf ( 1, '  %14f', level_weight(dim) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dimension      Rule       Growth     Parameters\n' );
  fprintf ( 1, '\n' );

  p_index = 1;

  for dim = 1 : dim_num

    if ( rule(dim) == 1 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  2 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  3 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  4 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  5 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  6 )
      fprintf ( 1, '  %8d  %8d  %11d  %14f\n', dim, rule(dim), growth(dim), p(p_index) );
    elseif ( rule(dim) ==  7 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) ==  8 )
      fprintf ( 1, '  %8d  %8d  %11d  %14f\n', dim, rule(dim), growth(dim), p(p_index) );
    elseif ( rule(dim) ==  9 )
      fprintf ( 1, '  %8d  %8d  %11d  %14f  %14f\n', ...
        dim, rule(dim), growth(dim), p(p_index), p(p_index+1) );
    elseif ( rule(dim) == 10 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) == 11 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    elseif ( rule(dim) == 12 )
      fprintf ( 1, '  %8d  %8d  %11d\n', dim, rule(dim), growth(dim) );
    end

    p_index = p_index + np(dim);

  end

  weight_sum_exact = 1.0;

  p_index = 1;

  for dim = 1 : dim_num

    if ( rule(dim) == 1 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 2 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 3 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 4 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 5 )
      weight_sum_exact = weight_sum_exact * sqrt ( pi );
    elseif ( rule(dim) == 6 )
      alpha = p(p_index);
      weight_sum_exact = weight_sum_exact * gamma ( 0.5 * ( alpha + 1.0 ) );
    elseif ( rule(dim) == 7 )
      weight_sum_exact = weight_sum_exact * 1.0;
    elseif ( rule(dim) == 8 )
      alpha = p(p_index);
      weight_sum_exact = weight_sum_exact * gamma ( alpha + 1.0 );
    elseif ( rule(dim) == 9 )
      alpha = p(p_index);
      beta = p(p_index+1);
      arg1 = - alpha;
      arg2 = 1.0;
      arg3 = beta + 2.0;
      arg4 = - 1.0;
      value1 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );
      arg1 = - beta;
      arg2 = 1.0;
      arg3 = alpha + 2.0;
      arg4 = - 1.0;
      value2 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );
      weight_sum_exact = weight_sum_exact * ( ...
        value1 / ( beta + 1.0 ) + value2 / ( alpha + 1.0 ) );
    elseif ( rule(dim) == 10 )
      weight_sum_exact = weight_sum_exact * sqrt ( pi );
    elseif ( rule(dim) == 11 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SGMGA_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Do not know how to deal with RULE = 11.\n' );
      error ( 'SGMGA_WEIGHT_TEST - Fatal error!' );
    elseif ( rule(dim) == 12 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SGMGA_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Do not know how to deal with RULE = 12.\n' );
      error ( 'SGMGA_WEIGHT_TEST - Fatal error!' );
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SGMGA_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Unexpected value of RULE = %d\n', rule(dim) );
      error ( 'SGMGA_WEIGHT_TEST - Fatal error!' );
    end

    p_index = p_index + np(dim);

  end

  fprintf ( 1, '\n' );
  fprintf ( 1,'  As a simple test, sum these weights.\n' );
  fprintf ( 1, '  They should sum to exactly %f\n', weight_sum_exact );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     Level      Weight sum  Expected sum    Difference\n' );
  fprintf ( 1, '\n' );

  for level_max = level_max_min : level_max_max

    point_total_num = sgmga_size_total ( dim_num, level_weight, level_max, ...
      rule, growth );

    point_num = sgmga_size ( dim_num, level_weight, level_max, rule, growth, ...
      np, p, tol );

    sparse_unique_index = sgmga_unique_index ( dim_num, level_weight, ...
      level_max, rule, growth, np, p, tol, point_num, point_total_num );

    [ sparse_order, sparse_index ] = sgmga_index ( dim_num, level_weight, ...
      level_max, rule, growth, point_num, point_total_num, sparse_unique_index );

    sparse_weight = sgmga_weight ( dim_num, level_weight, level_max, rule, ...
      growth, np, p, point_num, point_total_num, sparse_unique_index );

    weight_sum = sum ( sparse_weight(1:point_num) );

    weight_sum_error = abs ( weight_sum - weight_sum_exact );

    fprintf ( 1, '  %8d  %14f  %14f  %14e\n', ...
      level_max, weight_sum, weight_sum_exact, weight_sum_error );

  end

  return
end


function ylag = lagmatrix( y, lags )
    n = length(y);
    N = max(lags);

    ylag = convmtx(y, N+1);
    ylag = ylag(1:n, lags+1);

end


function [ y, m, d, f, ierror ] = ymdf_check_common ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_CHECK_COMMON checks a Common YMDF date.
%
%  Discussion:
%
%    Certain simple errors in dates will be corrected, such as
%      "31 September 1996"
%    which will become
%      "1 October 1996".
%
%    The routine also knows that in the Common calendar, the dates
%    5 October 1582 through 14 October 1582 are illegal.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, integer M, integer D, real  F, the
%    YMDF date.
%
%    Output, integer Y, integer M, integer D, real F, the
%    YMDF date, which may be corrected if necessary and possible.
%
%    Output, integer IERROR, is 0 if the date is legal.
%
  ierror = 0;

  [ y, m, d, ierror ] = ymd_check_common ( y, m, d );

  if ( ierror ~= 0 )
    return
  end

  [ y, m, d, f ] = frac_borrow_common ( y, m, d, f );

  [ y, m, d, f ] = frac_carry_common ( y, m, d, f );

  return
end


function [fusion,params] = quality_fuser_v1(w,scores,train_vecs,test_vecs,train_ndx,test_ndx,ddim)
%
% Inputs:
%
%    scores: the primary detection scores, for training
%            D-by-T matrix of T scores for D input systems
%
%    train_vecs: K1-by-M matrix, one column-vector for each of M training
%                segments
%
%    test_vecs: K2-by-N matrix, one column-vector for each of N training
%               segemnts
%
%    train_ndx: 1-by-T index where train_ndx(t) is the index into train_vecs
%               for trial t.
%
%    test_ndx: 1-by-T index where test_ndx(t) is the index into test_vecs
%              for trial t.
%    ddim: dimension of subspace for quality distandce calculation,
%          where ddim <= min(K1,K2)

if nargin==0
    test_this();
    return;
end

% Check data dimensions
[K1,M] = size(train_vecs);
[K2,N] = size(test_vecs);
assert(ddim<min(K1,K2));

[D,T] = size(scores);
assert(T == length(train_ndx));
assert(T == length(test_ndx));
assert(max(train_ndx)<=M);
assert(max(test_ndx)<=N);

% Create building blocks

[linfusion,params1] = linear_fuser([],scores);

[train_quality,params2] = sigmoid_logdistance(params1.tail,train_vecs,ddim);
train_distributor = duplicator_fh(train_ndx,size(train_vecs,2));
train_quality = train_distributor(train_quality);

[test_quality,params3] = sigmoid_logdistance(params2.tail,test_vecs,ddim);
test_distributor = duplicator_fh(test_ndx,size(test_vecs,2));
test_quality = test_distributor(test_quality);

params.get_w0 = @(ssat) [params1.get_w0(); params2.get_w0(ssat); params3.get_w0(ssat)];
params.tail = params3.tail;

% Assemble building blocks

%  combine train and test quality
quality = dottimes_of_functions([],train_quality,test_quality);

%  modulate linear fusion with quality
fusion = dottimes_of_functions([],quality,linfusion);

if ~isempty(w)
    fusion = fusion(w);
end

end

function test_this()

D = 2;
N = 5;
T = 3;
Q = 4;
ndx = ceil(T.*rand(1,N));
scores = randn(D,N);
train = randn(Q,T);
test = randn(Q,T);

ddim = 2;
ssat = 0.999;
[fusion,params] = quality_fuser_v1([],scores,train,test,ndx,ndx,ddim);

w0 = params.get_w0(ssat);
test_MV2DF(fusion,w0);

quality_fuser_v1(w0,scores,train,test,ndx,ndx,ddim),

end


%                     benchmarkTransmissionMatrix.m
%
% Code adapted from Sudarshan Nagesh for the reconstruction of images from
% a transmission matrix.
%
% This script will reconstruct images from phaseless measurements that were
% acquired when imaging through a diffusive media.  The quality of the
% reconstruction will be computed by comparing to a known solution, and
% different algorithms are compared.
%   Three datasets are available with resolutions 16x16, 40x40, and 64x64.
% At each resolution, there are 5 sets of measurements, each corresponding
% to a different image.  The user must select which resolution and dataset
% they want to run on.
%   As the script runs, all reconstructed images will be saved inside the
% 'benchmarkResults' folder.
%
% Note: The empirical dataset must be downloaded and installed into the
% 'data' directory for this to work. See the user guide.
%
%
%%I/O
%  Inputs:
%  imageSize            : Size of image to reconstruct.  Must be {16,40,64}
%  datasetSelection     : Choose which of the sets of measurements to use.
%                           Must be in {1,2,3,4,5}.
%  residualConstant     : Only use rows of the transmission matrix that
%                           had residuals less than this cutoff. Must be
%                           between 0 and 1.  Recommend 0.4.
%  algorithms           : A cell-array of structs that describe algorithms.
%
%
%%References
%  Paper Title:   Coherent inverse scattering via transmission matrices:
%  Efficient phase retrieval algorithms and a public dataset.
%  Authors:       Christopher A. Metzler,  Manoj
%  K. Sharma,  Sudarshan Nagesh,  Richard G. Baraniuk,
%                 Oliver Cossairt,  Ashok Veeraraghavan
%
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START----------------------------------

function benchmarkTransmissionMatrix(imageSize, datasetSelection, residualConstant, algorithms)

    %% Load the transmission matrix, and phaseless measurements
    [A,Xt,b] = loadData(residualConstant, datasetSelection, imageSize);

    % This string contains a text-based table with all the results, and is
    % printed after all calculations are done.
    % Results will also be printed incrementally as the benchmark runs.
    resultsTable = '\nAlgorithm  |  Measurement Error \n';

    %% Loop over each algorithm, and perform reconstruction and evaluation
    for k=1:length(algorithms)
    	opts=algorithms{k};
        fprintf('Running Algorithm: %s\n',opts.algorithm);

        % Call a phase retrieval routine, and record the error of the result
        [reconError, measurementError] = reconstructSignal(A,Xt,b,imageSize,datasetSelection,opts);

        % Record the error in the table
        resultsTable = [resultsTable,...
                        pad(opts.algorithm,16),...
                        sprintf('%0.5f',measurementError),...
                        '\n'];
    end

    %% print the table
    fprintf(resultsTable);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                        Utility functions                        %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Load the measurement operator and measurements that correspond to the
% image size and dataset chosen by the user.  This method returns the
% measurement operator 'A' (a dense matrix), a ground truth solution
% 'Xt', and a set of measurements 'Y'.
function [A,Xt,b] = loadData(residualConstant, datasetSelection, imageSize)
    % Create some strings for dealing with different filenames according to the dataset chosen
    switch imageSize
        case {16}
            measurementType = 'AmpSLM_16x16';
        case {40}
            measurementType = 'PhaseSLM_40x40';
        case {64}
            measurementType = 'AmpSLM_64x64';
        otherwise
            error('illegal imageSize: %d. It should be chosen from {16, 40, 64}',imageSize);
    end

    % Make sure the dataset selection is valid
    if imageSize==16 || imageSize==40
        assert(datasetSelection <= 5);
    elseif imageSize==64
        assert(datasetSelection <= 6);
    end

    %% Load the transmission matrix, ground truth image, and measurements
    dataRoot = getFolderPath('data');       % The location of the 'data' folder
    try
        disp(['Loading transmission matrix (A_prVAMP.mat), this may take several minutes']);
        load(strcat(dataRoot,'TransmissionMatrices/Coherent_Data/',measurementType,'/A_prVAMP.mat'));
        disp(['Loading test image (XH_test.mat)']);
        load(strcat(dataRoot,'/TransmissionMatrices/Coherent_Data/',measurementType,'/XH_test.mat'));
        disp(['Loading measurements (YH_squared_test.mat)']);
        load(strcat(dataRoot,'TransmissionMatrices/Coherent_Data/',measurementType,'/YH_squared_test.mat'));
    catch
        error(['You are missing the transmission matrix dataset.  To download it, Go to',...
            'https://rice.app.box.com/v/TransmissionMatrices/,'...
            'and download the "TransmissionMatrices" folder. Unzip the downloaded folder and',...
            'place it in the in "data" folder. See the user guide for details.']);
    end
    % Only use the most accurate rows of the transmission matrix.  This is
    % determined by checking that the residuals found during row calcuation
    % are less than the specificed "residualConstant".
    findPixels = find(residual_vector<residualConstant);
    YH_squared_test = YH_squared_test(:,findPixels);

    A = A(findPixels,:);

    % Unpack the measurement data
    Y = double(YH_squared_test(datasetSelection,:));
    b = sqrt(Y);

    % Unpack the ground-truth image Xt
    % Note:  this image is real-valued
    Xt = double(XH_test(datasetSelection,:))';
    Xt = Xt/max(abs(Xt));
    % reshape original image
    originalImage = imresize(reshape(real(Xt),imageSize,imageSize),4);
    originalImageName = ['TM',num2str(imageSize),'-',num2str(datasetSelection),'-original'];
    imshow(originalImage,[]);  title(originalImageName,'fontsize',16); drawnow;
    % save the image to a folder
    saveBenchmarkImage(originalImage,['TM',num2str(imageSize)],originalImageName);
end

% Reconstruct a signal from a given measurement operator and set of
% measurements.  Compare the results to the ground-truth Xt, and report
% error.
function [reconError, measurementError] = reconstructSignal(A,Xt,Y,imageSize,datasetSelection,opts)
    fprintf('    Reconstructing image...')

    % Convenient variables
    n = imageSize^2;                    % Size of SLM
    imageSizeStr = num2str(imageSize);  % For TM16, this is '16'
    ind = datasetSelection;             % The index of the selected dataset

    % Solve the PR problem
    [X,outs,opts] = solvePhaseRetrieval(A,[],Y(:),n,opts);

    % Compute solution quality by solving a system to get the best affine
    % transform that maps the recovered solution onto the true solution
    Xrec = [X(:), ones(numel(X),1)];
    coeffs = Xrec\Xt(:);
    Xrec = Xrec*coeffs;

    % Reshape images into a square, and save them.
    realIm = imresize(reshape(real(Xrec),imageSize,imageSize),4);
    recoveredImageNameAbs = strcat('TM',imageSizeStr,'-',num2str(ind),'-',opts.algorithm,...
                                num2str(outs.iterationCount),'-real');
    imshow(realIm,[]); title(recoveredImageNameAbs,'fontsize',16); drawnow;
    realIm = realIm-min(realIm(:));
    realIm = realIm/max(realIm(:));
    saveBenchmarkImage(realIm,['TM',imageSizeStr],recoveredImageNameAbs);

    reconError = norm(Xt(:)-Xrec(:))/norm(Xt(:));
    measurementError = norm(abs(A*X)-Y(:))/norm(Y(:));
    %fprintf('    Relative reconstruction error = %s\n',reconError);
    fprintf('    Relative measurement error = %s\n',measurementError);
end

% Find the path for the specified folder by searching the current and
% parent directories
function path = getFolderPath(fname)
    d = dir;
    % check if the folder fname is in current path
    if any(strcmp(fname, {d([d.isdir]).name}))
        path = [fname,'/'];
    else
        % Look for folder fname in parent directory
        d = dir('../');
        if any(strcmp(fname, {d([d.isdir]).name}))
            path = ['../',fname,'/'];
        else
            error(['Unable to find path of folder: ',fname, ...
                '.  Make sure your current directory is the PhasePack root.']);
        end
    end
end

% Save an image to a sub-folder inside 'benchmarkResults'
function saveBenchmarkImage(image,folder,fname)
    bmRoot = getFolderPath('benchmarkResults');
    if ~exist([bmRoot,folder],'dir')
         mkdir([bmRoot,folder]);
    end
    fullName = strcat(bmRoot,folder,'/',fname,'.png');
    fprintf('    Saving reconstructed image: %s\n',fullName);
    try
        imwrite(image,fullName);
    catch
        error(['Unable to save image to file: ', fullName]);
    end
end


function s=maxmod(a,b)
% The maxmod limiter
s=0.5*(sign(a)+sign(b)).*max(abs(a),abs(b));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   KUKA KR 30 3.
%
%   Author:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function robot = parameters()

robot.name= 'KR30_3';

robot.DH.theta= '[  q(1)  q(2)-pi/2     q(3)    q(4)    q(5)   q(6)]';
robot.DH.d='[       0.815     0           0       0.820   0    0.17]';
robot.DH.a='[       0.350   0.850         0.145    0       0      0]';
robot.DH.alpha= '[  -pi/2   0           -pi/2    pi/2    -pi/2   0]';
robot.J=[];

robot.inversekinematic_fn = 'inversekinematic_kuka_kr303(robot, T)';

%number of degrees of freedom
robot.DOF = 6;

%rotational: 0, translational: 1
robot.kind=['R' 'R' 'R' 'R' 'R' 'R'];

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-185) deg2rad(185); %Axis 1, minimum, maximum
                deg2rad(-35) deg2rad(135); %Axis 2, minimum, maximum
                deg2rad(-120) deg2rad(158); %Axis 3
                deg2rad(-350) deg2rad(350); %Axis 4: Unlimited (400� default)
                deg2rad(-119) deg2rad(119); %Axis 5
                deg2rad(-350) deg2rad(350)]; %Axis 6: Unlimited (800� default)

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(140); %Axis 1, rad/s
                deg2rad(126); %Axis 2, rad/s
                deg2rad(140); %Axis 3, rad/s
                deg2rad(260); %Axis 4, rad/s
                deg2rad(245); %Axis 5, rad/s
                deg2rad(322)];%Axis 6, rad/s
% end effectors maximum velocity
robot.linear_velmax = 1.0; %m/s, not specified

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time
%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.path = pwd;

% GRAPHICS
robot.graphical.has_graphics=1;
robot.graphical.color = [255 102 51]./255;
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=0;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis=[-1.5 1.5 -1.5 1.5 0 2];
%read graphics files
robot = read_graphics(robot);

%DYNAMICS
robot.has_dynamics=1;

%consider friction in the computations
robot.dynamics.friction=0;

%link masses (kg)
robot.dynamics.masses=[266 133 99.75 66.5 66.5 33.25];

%COM of each link with respect to own reference system
robot.dynamics.r_com=[0       0          0; %(rx, ry, rz) link 0
                       -0.35	0.252	 0; %(rx, ry, rz) link 1
                       -0.675	   0	 -0.225;  %(rx, ry, rz) link 2
                        0       0       0.150;%(rx, ry, rz) link 4
                        0       0           0;%(rx, ry, rz) link 5
                        0       0         0.1075];%(rx, ry, rz) link 6

%Inertia matrices of each link with respect to its D-H reference system.
% Ixx   Iyy	Izz	Ixy	Iyz	Ixz, for each row
% % % % % robot.dynamics.Inertia=[0      0.35	0   	0	0	0;
% % % % %                         9.89     9.89	16	    0	0	0;
% % % % %                         38.78	38.78	2.56	0	0	0;
% % % % %                         34.2	34.2	0.9	    0	0	0;
% % % % %                         0.214	0.214	0.162	0	0	0;
% % % % %                         0.0356	0.0356	0.07	0	0	0];

robot.dynamics.Inertia=[0      0.35	0   	0	0	0;
    .13     .524	.539	0	0	0;
    .066	.086	.0125	0	0	0;
    1.8e-3	1.3e-3	1.8e-3	0	0	0;
    .3e-3	.4e-3	.3e-3	0	0	0;
    .15e-3	.15e-3	.04e-3	0	0	0];

robot.motors=load_motors([2 4 3 1 3 1]);
%Speed reductor at each joint
robot.motors.G=[600 600 600 100 100 50];


%% housekeeping
close all
clc
clear

%% add a path

addpath('routines')

%% rise the model

m=rise('lwz_switch_model','solve_linear',true);

%% get the parameters

[p,priors]=create_parameters_switch(true);

m=set(m,'parameters',p);

%% bring in the data

data=create_data();

data=pages2struct(data);

vnames=fieldnames(data);

figure('name','Observables')

for ii=1:numel(vnames)

    v=vnames{ii};

    subplot(3,2,ii)

    plot(data.(v))

    title(v)

end

%% Normalization

myNormalization={
    'sig_Eps_phi_hetero_1<=sig_Eps_phi_hetero_2'
    };

%% estimate the model
clc

%do_estimate=false;
%
%if do_estimate

    [ms,filtration]=estimate(m,'data',data,...
        'estim_priors',priors,...
        'kf_presample',3,...
        'kf_init_variance',10,...
        'estim_nonlinear_restrictions',myNormalization);

    % save the mode
    %--------------

    switch_mode=get(ms,'mode');

%    save switch_estimates ms switch_mode filtration
%
%else
%
%    load switch_estimates
%
%end

%% irfs

clc

myirfs=irf(ms);

quick_irfs(ms,myirfs)

%% probability plots

plot_probabilities(ms)

%% probability plots

plot_data_against_probabilities(ms)

%% replicate figure 9
close all

probs=filtration.smoothed_state_probabilities;
% I substract a constant since I don't know the correct level of the data.
% Maybe Dan can help with this?
lp=cumsum(data.DLogQl)-0.55;

plotyy(lp,probs.hetero_2,'linewidth',2)

%% (Approximate) Historical decomposition

h=historical_decomposition(ms);

obslist=get(ms,'obs_list');

dropped={'init'};

figure('name','Observables')

for ii=1:numel(obslist)

    v=obslist{ii};

    subplot(3,2,ii)

    plot_decomp(drop(h.(v),dropped))

    title(v)

    if ii==1

        l=legend(h.(v).varnames-dropped);

    end

end

set(l,'interpreter','none')

%% forecast

%% conditional forecast

%% counterfactual

%% posterior sampling
clc

do_sampling=false;

if do_sampling

    [objective,lb,ub,mu,SIG]=pull_objective(ms,...
        'solve_check_stability',false,'fix_point_TolFun',1e-6);

    sampling_options=struct('MaxTime',3600*24*7,...
        'N',10000,...
        'nchain',2,...
        'thin',5,...
        'adapt_covariance',true);

    results=mh_sampler(objective,lb,ub,sampling_options,mu);

    save switch_sampling results objective

end

do_mdd=true;

if do_mdd

    load switch_sampling

    % Marginal data density
    algorithms={'mhm','swz','mueller','bridge','is','ris','cj','laplace','laplace_mcmc'};
    options=struct('log_post_kern',objective,'L',10000);
    nalgos=numel(algorithms);
    log_mdd=cell(nalgos,3);
    log_mdd(:,1)=algorithms(:);
    tictoc=nan(nalgos,1);

    pop=results{1}.pop;
    for jj=2:numel(results)
        pop=[pop;results{jj}.pop];
    end

    parfor ii=1:nalgos
        opt=options;
        opt.algorithm=algorithms{ii};
        tic
        log_mdd{ii,2} = mcmc_mdd(pop,lb,ub,opt);
        tictoc(ii)=toc;
    end
    log_mdd(:,3)=num2cell(tictoc(:));

    save switch_sampling log_mdd -append

end

% the benchmark DSGE model (2344.0), the DSGE with two volatility regimes
% (2354.1), and the DSGE model with three volatility regimes (2353.2)
%%

pnames={ms.estimation.priors.name};

obj=mcmc(results,pnames)%,start_from,trimming

%% Trace plots

chain_id=1;

plotfunc=@(name)traceplot(obj,name,chain_id);

hfig=utils.plot.multiple(plotfunc,pnames,'trace plots',3,3)

%% Density plots

chain_id=1;

plotfunc=@(name)densplot(obj,name,chain_id,250);

hfig=utils.plot.multiple(plotfunc,pnames,'density plots',3,3)

%% Mean plots

chain_id=[];

plotfunc=@(name)meanplot(obj,name,chain_id);

hfig=utils.plot.multiple(plotfunc,pnames,'Mean plots',3,3);

%% Autocorrelation plots

chain_id=[];

order=10;

plotfunc=@(name)autocorrplot(obj,name,chain_id,order);

hfig=utils.plot.multiple(plotfunc,pnames,'Autocorrelation plots',3,3)

%% Gelman-Rubin plots

plotfunc=@(name)psrf_plot(obj,name);

hfig=utils.plot.multiple(plotfunc,pnames,'Gelman-Rubin (PSRF) plots',3,3)

%% Scatter plots

np=numel(pnames);

scatnames=cell(1,np^2);

iter=0;
for ii=1:np
    for jj=ii+1:np
        iter=iter+1;
        scatnames{iter}={pnames{ii},pnames{jj}};
    end
end
scatnames=scatnames(1:iter);

chain_id=[];

plotfunc=@(name)scatterplot(obj,name{1},name{2},chain_id);%,varargin

hfig=utils.plot.multiple(plotfunc,scatnames,'Scatter plots',7,7)

%% plot priors

[pdata,hdl]=plot_priors(ms,pnames)

%% plot posteriors

plot_posteriors(ms,results.pop,pnames)

%% priors and posteriors

plot_priors_and_posteriors(ms,results.pop,pnames)

%% mode curvature

mode_curvature(ms)


function i4vec_ascends_test ( )

%*****************************************************************************80
%
%% I4VEC_ASCENDS_TEST tests I4VEC_ASCENDS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 4;
  test_num = 6;
%
%  Each ROW of this definition is a COLUMN of the matrix.
%
  x_test = [ ...
    1, 3, 2, 4; ...
    2, 2, 2, 2; ...
    1, 2, 2, 4; ...
    1, 2, 3, 4; ...
    4, 4, 3, 1; ...
    9, 7, 3, 0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_ASCENDS_TEST\n' );
  fprintf ( 1, '  I4VEC_ASCENDS determines if an integer vector ascends.\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    x(1:n) = x_test(1:n,test);

    i4vec_print ( n, x, '  Test vector:' );

    fprintf ( 1, '  I4VEC_ASCENDS =  %d\n', i4vec_ascends ( n, x ) );

  end

  return
end


% Compare the speeds of various inference engines on a coupled HMM

N = 2;
Q = 2;
rand('state', 0);
randn('state', 0);
discrete = 1;
if discrete
  Y = 2; % size of output alphabet
else
  Y = 1;
end
coupled = 1;
[bnet, onodes] = mk_chmm(N, Q, Y, discrete, coupled);
ss = N*2;

T = 3;

engine = {};
tic; engine{end+1} = jtree_dbn_inf_engine(bnet, 'observed', onodes);  toc
%tic; engine{end+1} = jtree_ndxSD_dbn_inf_engine(bnet, onodes);  toc
%tic; engine{end+1} = jtree_ndxB_dbn_inf_engine(bnet, onodes);  toc
engine{end+1} = hmm_inf_engine(bnet, onodes);
%engine{end+1} = dhmm_inf_engine(bnet, onodes);
tic; engine{end+1} = jtree_unrolled_dbn_inf_engine(bnet, T, onodes);  toc

%engine{end+1} = bk_inf_engine(bnet, 'ff', onodes);
%engine{end+1} = loopy_dbn_inf_engine(bnet, onodes);

exact = [1 2 3];

filter = 0;
single = 0;
maximize = 0;

[err, time, engine] = cmp_inference(bnet, onodes, engine, exact, T, filter, single, maximize);
%err = cmp_learning(bnet, onodes, engine, exact, T);


function K = sdlfmXsdlfmKernCompute(sdlfmKern1, sdlfmKern2, t1, t2, covIC, type)

% SDLFMXSDLFMKERNCOMPUTE Compute a cross kernel between two SDLFM kernels.
% FORMAT
% DESC computes cross kernel terms between two switching dynamical
% LFM kernels for the multiple output kernel.
% ARG sdlfmKern1 : the kernel structure associated with the first SDLFM
% kernel.
% ARG sdlfmKern2 : the kernel structure associated with the second SDLFM
% kernel.
% ARG t : inputs for which kernel is to be computed.
% ARG covIC : covariance for the initial conditions
% RETURN K : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between two SDLFM kernels for
% the multiple output kernel.
% ARG sdlfmKern1 : the kernel structure associated with the first SDLFM
% kernel.
% ARG sdlfmKern2 : the kernel structure associated with the second SDLFM
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covIC : covariance for the initial conditions
% RETURN K : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between two SDLFM kernels for
% the multiple output kernel. The SDLFM kernels can correspond to Position
% X Position (default), Velocity X Position, Velocity X Velocity,
% Acceleration X Position, Acceleration X Velocity, Acceleration X
% Acceleration. The type of kernel to be computed is specified in 'type'.
% ARG sdlfmKern1 : the kernel structure associated with the first SDLFM
% kernel.
% ARG sdlfmKern2 : the kernel structure associated with the second SDLFM
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covIC : covariance for the initial conditions
% ARG type : specifies the type of kerne to be computed
% RETURN K : block of values from kernel matrix.
%
% SEEALSO : sdlfmKernParamInit, sdlfmKernCompute, sdlfmKernParamInit
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 6
    type = 'PosPos';
    if nargin < 5
        covIC = t2;
        t2 = t1;
    end
end

if size(t1, 2) > 1 || size(t2, 2) > 1
    error('Input can only have one column');
end

compareInverseWidth = sdlfmKern1.inverseWidth == sdlfmKern2.inverseWidth;
if sum(sum(compareInverseWidth))~=(sdlfmKern1.nIntervals*sdlfmKern1.nlfPerInt)
    error('Kernels cannot be cross combined if they have different inverse widths.')
end
compareSwitchingTimes = sdlfmKern1.switchingTimes == sdlfmKern2.switchingTimes;
if sum(sum(compareSwitchingTimes))~=sdlfmKern1.nIntervals
    error('Kernels cannot be cross combined if they have different switching points.')
end

switch type
    case 'PosPos'
        fhandle = 'sdlfmXsdlfmKernComputeBlock';
    case 'VelPos'
        fhandle = 'sdlfmvXsdlfmKernComputeBlock';
    case 'VelVel'
        fhandle = 'sdlfmvXsdlfmvKernComputeBlock';
    case 'AccelPos'
        fhandle = 'sdlfmaXsdlfmKernComputeBlock';
    case 'AccelVel'
        fhandle = 'sdlfmaXsdlfmvKernComputeBlock';
    case 'AccelAccel'
        fhandle = 'sdlfmaXsdlfmaKernComputeBlock';
end

fhandle = str2func(fhandle);

%Form the basic kernels
lfmKern1 = struct();
lfmKern2 = struct();

% Create structures that will make easy the computation of the kernels

spVector = [cumsum(sdlfmKern1.switchingTimes) t1(end)+50];

dim1 = zeros(1, sdlfmKern1.nIntervals); dim2 = zeros(1, sdlfmKern1.nIntervals);

for i=1:sdlfmKern1.nIntervals
    for j =1:sdlfmKern1.nlfPerInt
        % Create the appropriate set of kernel structures
        lfmKern1(i,j).mass = sdlfmKern1.mass;
        lfmKern1(i,j).spring = sdlfmKern1.spring;
        lfmKern1(i,j).damper = sdlfmKern1.damper;
        lfmKern1(i,j).inverseWidth = sdlfmKern1.inverseWidth(j,i);
        lfmKern1(i,j).sensitivity = sdlfmKern1.sensitivity(j,i);
        lfmKern2(i,j).mass = sdlfmKern2.mass;
        lfmKern2(i,j).spring = sdlfmKern2.spring;
        lfmKern2(i,j).damper = sdlfmKern2.damper;
        lfmKern2(i,j).inverseWidth = sdlfmKern2.inverseWidth(j,i);
        lfmKern2(i,j).sensitivity = sdlfmKern2.sensitivity(j,i);
        lfmKern1(i,j).limit = spVector(i+1) - spVector(i);
        lfmKern2(i,j).limit = spVector(i+1) - spVector(i);
        lfmKern1(i,j).isNormalised = sdlfmKern1.isNormalised;
        lfmKern2(i,j).isNormalised = sdlfmKern2.isNormalised;
    end
    newt1 = t1(t1> spVector(i) & t1<spVector(i+1));
    newt2 = t2(t2> spVector(i) & t2<spVector(i+1));
    dim1(i) = length(newt1);
    dim2(i) = length(newt2);
end

if sum(dim1)~=length(t1) || sum(dim2)~=length(t2)
    error('A problem with the dimensions of the switching intervals occured')
end

% Compute some necessary constants

generalConst = sdlfmKernComputeConstant(sdlfmKern1.nIntervals, ...
    lfmKern1(1), lfmKern2(1), spVector);

kyy = zeros(sdlfmKern1.nIntervals);kyv = zeros(sdlfmKern1.nIntervals);
kvy = zeros(sdlfmKern1.nIntervals);kvv = zeros(sdlfmKern1.nIntervals);
kyy(1,1) = covIC(1,1);
kvy(1,1) = covIC(2,1);
kyv(1,1) = covIC(1,2);
kvv(1,1) = covIC(2,2);

%%% Compute initial conditions for intervals (1-2), (2-2) and (2-1)

tInit1 = [spVector(1) - spVector(1);spVector(2) - spVector(1)];
tInit2 = [spVector(1) - spVector(1);spVector(2) - spVector(1)];

% Pos -- Pos
temp = sdlfmXsdlfmKernComputeBlock(lfmKern1(1), lfmKern2(1), ...
    tInit1, tInit2, kyy(1,1), kyv(1,1), kvy(1,1), kvv(1,1), 1, 1, generalConst);
kyy(2,2) = temp(2,2); kyy(1,2) = temp(1,2); kyy(2,1)  = temp(2,1);
% Vel -- Pos
temp = sdlfmvXsdlfmKernComputeBlock(lfmKern1(1), lfmKern2(1), ...
    tInit1, tInit2, kyy(1,1), kyv(1,1), kvy(1,1), kvv(1,1), 1, 1, generalConst);
kvy(2,2) = temp(2,2); kvy(1,2) = temp(1,2); kvy(2,1)  = temp(2,1);
% Pos -- Vel
temp = sdlfmXsdlfmvKernComputeBlock(lfmKern1(1), lfmKern2(1), ...
    tInit1, tInit2, kyy(1,1), kyv(1,1), kvy(1,1), kvv(1,1), 1, 1, generalConst);
kyv(2,2) = temp(2,2); kyv(1,2) = temp(1,2); kyv(2,1)  = temp(2,1);
% Vel -- Vel
temp = sdlfmvXsdlfmvKernComputeBlock(lfmKern1(1), lfmKern2(1), ...
    tInit1, tInit2, kyy(1,1), kyv(1,1), kvy(1,1), kvv(1,1), 1, 1, generalConst);
kvv(2,2) = temp(2,2); kvv(1,2) = temp(1,2); kvv(2,1)  = temp(2,1);

tempPosPos = cell(sdlfmKern1.nIntervals);
tempVelPos = cell(sdlfmKern1.nIntervals);
tempPosVel = cell(sdlfmKern1.nIntervals);
tempVelVel = cell(sdlfmKern1.nIntervals);

startValOne = 1;
endValOne   = 0;

for i=1:sdlfmKern1.nIntervals
    endValOne = endValOne + dim1(i);
    startValThree = 1;
    endValThree = 0;
    for j=1:sdlfmKern1.nIntervals
        if i>j
            lfmKern1Local = lfmKern1(j,:);
            lfmKern2Local = lfmKern2(j,:);
        else
            lfmKern1Local = lfmKern1(i,:);
            lfmKern2Local = lfmKern2(i,:);
        end
        endValThree = endValThree + dim2(j);
        % POS -- POS (Kernel and initial positions)
        K(startValOne:endValOne, startValThree:endValThree) = fhandle(lfmKern1Local, ...
            lfmKern2Local, t1(startValOne:endValOne) - spVector(i), t2(startValThree:endValThree) - spVector(j), ...
            kyy(i,j), kyv(i,j), kvy(i,j), kvv(i,j), i, j, generalConst);
        % Time vector initial conditions
        tInit1 = [spVector(i) - spVector(i);spVector(i+1) - spVector(i)];
        tInit2 = [spVector(j) - spVector(j);spVector(j+1) - spVector(j)];
        tempPosPos{i,j} = sdlfmXsdlfmKernComputeBlock(lfmKern1Local, ...
            lfmKern2Local, tInit1, tInit2, ...
            kyy(i,j), kyv(i,j), kvy(i,j), kvv(i,j), i, j, generalConst);
        kyy = organizeIC(kyy, tempPosPos, i, j);
        % VEL -- POS
        tempVelPos{i,j} = sdlfmvXsdlfmKernComputeBlock(lfmKern1Local, ...
            lfmKern2Local, tInit1, tInit2 , ...
            kyy(i,j), kyv(i,j), kvy(i,j), kvv(i,j), i, j, generalConst);
        kvy = organizeIC(kvy, tempVelPos, i, j);
        % POS -- VEL
        tempPosVel{i,j} = sdlfmXsdlfmvKernComputeBlock(lfmKern1Local, ...
            lfmKern2Local, tInit1, tInit2, ...
            kyy(i,j), kyv(i,j), kvy(i,j), kvv(i,j), i, j, generalConst);
        kyv = organizeIC(kyv, tempPosVel, i, j);
        % POS -- VEL
        tempVelVel{i,j} = sdlfmvXsdlfmvKernComputeBlock(lfmKern1Local, ...
            lfmKern2Local, tInit1, tInit2, ...
            kyy(i,j), kyv(i,j), kvy(i,j), kvv(i,j), i, j, generalConst);
        kvv = organizeIC(kvv, tempVelVel, i, j);
        startValThree = endValThree + 1;
    end
    startValOne = endValOne + 1;
end

K = real(K);


function check = triangular_check ( a, b )

%*****************************************************************************80
%
%% TRIANGULAR_CHECK checks the parameters of the Triangular CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    A < B.
%
%    Output, logical CHECK, is true if the parameters are legal.
%
  if ( b <= a )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULAR_CHECK - Fatal error!\n' );
    fprintf ( 1, '  B <= A.\n' );
    check = 0;
    return
  end

  check = 1;

  return
end


function R = getHPfilter(freq,numsamps)

% freq is number of frequencies to cut off.
base = 1:2*pi/numsamps:2*pi


function s = SaturationPouli(C, I)
%
%       s = SaturationPouli(C, I)
%
%       This computes the saturation using channel C and I from LCh color
%       space
%
%       input:
%         - C: chroma channel from LCh
%         - I: intensity channel from LCh
%
%       output:
%         - S: saturation channel
%
%     Copyright (C) 2013  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/>.
%
%     The paper describing this technique is:
%     "Color Correction for Tone Reproduction"
% 	  by Tania Pouli1, Alessandro Artusi, Francesco Banterle,
%     Ahmet Oguz Akyuz, Hans-Peter Seidel and Erik Reinhard
%     in the Twenty-first Color and Imaging Conference (CIC21), Albuquerque, Nov. 2013
%
%

D = sqrt(C.^2 + I.^2);
s = C ./ D;

end


% this is a very naiive and non-optimized cardiac cine GRE sequence

seq=mr.Sequence();              % Create a new sequence object
fov=256e-3; Nx=128; Ny=Nx;      % Define FOV and resolution
alpha=5;                        % flip angle
sliceThickness=5e-3;            % slice
%TE=[7.38 9.84]*1e-3;            % give a vector here to have multiple TEs (e.g. for field mapping)
TE=4.92e-3;
TR=9e-3;                        % only a single value for now

% cardiac features
phases = 8;
hearbeats = 15; % odd numbers of heartbeats / segments work better

% more in-depth parameters
rfSpoilingInc=117;              % RF spoiling increment
rf_duration = 2e-3;
adc_duration = 3.2e-3;
pre_duration = 1e-3;

% set system limits
sys = mr.opts('MaxGrad', 28, 'GradUnit', 'mT/m', ...
    'MaxSlew', 150, 'SlewUnit', 'T/m/s', 'rfRingdownTime', 20e-6, ...
    'rfDeadTime', 100e-6, 'adcDeadTime', 10e-6);

% Create fat-sat pulse
% (in Siemens interpreter from January 2019 duration is limited to 8.192 ms, and although product EPI uses 10.24 ms, 8 ms seems to be sufficient)
% B0=2.89; % 1.5 2.89 3.0
% sat_ppm=-3.45;
% sat_freq=sat_ppm*1e-6*B0*lims.gamma;
% rf_fs = mr.makeGaussPulse(110*pi/180,'system',lims,'Duration',8e-3,...
%     'bandwidth',abs(sat_freq),'freqOffset',sat_freq);
% gz_fs = mr.makeTrapezoid('z',sys,'delay',mr.calcDuration(rf_fs),'Area',1/1e-4); % spoil up to 0.1mm

% define the trigger to play out
trig=mr.makeTrigger('physio1','duration', 2000e-6); % duration after
%trig=mr.makeTriggerPulse('osc0','duration', 4100e-6); % possible channels: 'osc0','osc1','ext1'
trig_out=mr.makeDigitalOutputPulse('ext1','duration', 100e-6,'delay',500e-6); % possible channels: 'osc0','osc1','ext1'

% Create alpha-degree slice selection pulse and gradient
[rf, gz] = mr.makeSincPulse(alpha*pi/180,'Duration',rf_duration,...
    'SliceThickness',sliceThickness,'apodization',0.5,'timeBwProduct',4,'system',sys);

% Define other gradients and ADC events
deltak=1/fov;
gx = mr.makeTrapezoid('x','FlatArea',Nx*deltak,'FlatTime',adc_duration,'system',sys);
adc = mr.makeAdc(Nx,'Duration',gx.flatTime,'Delay',gx.riseTime,'system',sys);
gxPre = mr.makeTrapezoid('x','Area',-gx.area/2,'Duration',pre_duration,'system',sys);
gzReph = mr.makeTrapezoid('z','Area',-gz.area/2,'Duration',pre_duration,'system',sys);

lines_per_segment = round(Ny/hearbeats);
Ns=ceil(Ny/lines_per_segment);
Ny=Ns*lines_per_segment; % it can be that because of the rounding above we measure few more k-space lines...
phaseAreas = ((0:Ny-1)-Ny/2)*deltak;
% now reverse the order in every second segment
phaseAreasSeg=reshape(phaseAreas,lines_per_segment,Ns);
phaseAreasSeg(:,2:2:end)=phaseAreasSeg(end:-1:1,2:2:end);
phaseAreas=phaseAreasSeg(:);

% gradient spoiling
gxSpoil=mr.makeTrapezoid('x','Area',2*Nx*deltak,'system',sys);
gzSpoil=mr.makeTrapezoid('z','Area',4/sliceThickness,'system',sys);

% Calculate timing
delayTE=ceil((TE - mr.calcDuration(gxPre) - gz.fallTime - gz.flatTime/2 ...
    - mr.calcDuration(gx)/2)/seq.gradRasterTime)*seq.gradRasterTime;
delayTR=ceil((TR - mr.calcDuration(gxPre) - mr.calcDuration(gz) ...
    - mr.calcDuration(gx) - delayTE)/seq.gradRasterTime)*seq.gradRasterTime;
assert(all(delayTR>=mr.calcDuration(gxSpoil,gzSpoil)));

fprintf('the sequence will acquire %d lines per segment resulting in a temporal resolution of %g ms per phase\n', lines_per_segment, TR*lines_per_segment*1e3);
fprintf('cardiac acquisition window is: %g ms\n', TR*phases*lines_per_segment*1e3);

rf_phase=0;
rf_inc=0;

% Loop over phase encodes and define sequence blocks
for s=1:Ns
    seq.addBlock(trig); % wait for the cardiac trigger
    for p=1:phases
        for l=1:lines_per_segment
            % restore the line counter
            i=(s-1)*lines_per_segment+l;
            %seq.addBlock(rf_fs,gz_fs); % fat-sat
            rf.phaseOffset=rf_phase/180*pi;
            adc.phaseOffset=rf_phase/180*pi;
            rf_inc=mod(rf_inc+rfSpoilingInc, 360.0);
            rf_phase=mod(rf_phase+rf_inc, 360.0);
            %
            seq.addBlock(rf,gz,trig_out);
            gyPre = mr.makeTrapezoid('y','Area',phaseAreas(i),'Duration',pre_duration,'system',sys);
            seq.addBlock(gxPre,gyPre,gzReph);
            if delayTE>0
                seq.addBlock(mr.makeDelay(delayTE));
            end
            seq.addBlock(gx,adc);
            gyPre.amplitude=-gyPre.amplitude;
            seq.addBlock(mr.makeDelay(delayTR),gxSpoil,gyPre,gzSpoil)
        end
    end
end

%% check whether the timing of the sequence is correct
[ok, error_report]=seq.checkTiming;

if (ok)
    fprintf('Timing check passed successfully\n');
else
    fprintf('Timing check failed! Error listing follows:\n');
    fprintf([error_report{:}]);
    fprintf('\n');
end

%% prepare sequence export
seq.setDefinition('FOV', [fov fov sliceThickness]);
seq.setDefinition('Name', 'cine-gre');

seq.write('cine_gre.seq')       % Write to pulseq file

%seq.install('siemens');
return

%% plot sequence and k-space diagrams

seq.plot('timeRange', [0 5*TR]);

% new single-function call for trajectory calculation
[ktraj_adc, ktraj, t_excitation, t_refocusing, t_adc] = seq.calculateKspace();

% plot k-spaces
time_axis=(1:(size(ktraj,2)))*sys.gradRasterTime;
figure; plot(time_axis, ktraj'); % plot the entire k-space trajectory
hold; plot(t_adc,ktraj_adc(1,:),'.'); % and sampling points on the kx-axis
figure; plot(ktraj(1,:),ktraj(2,:),'b'); % a 2D plot
axis('equal'); % enforce aspect ratio for the correct trajectory display
hold;plot(ktraj_adc(1,:),ktraj_adc(2,:),'r.'); % plot the sampling points

%% very optional slow step, but useful for testing during development e.g. for the real TE, TR or for staying within slewrate limits

rep = seq.testReport;
fprintf([rep{:}]);


function d = dot(obj1,obj2)
% inner product between embeddings
%
% Syntax
%   d = dot(e1,e2)
%
% Input
%  e1,e2 - @embedding
%
% Output
%  d - double
%

d = 0;
for i = 1:length(obj1.u)

  id = -(1:obj1.rank(i));
  d = d + EinsteinSum(obj1.u{i},id,obj2.u{i},id);

end

end


function p=baselineParameters()

p=struct();

p.h = 0.8;
p.gz = 0.001;
p.pi_ss = 1.005;
p.beta = 0.9975;
p.kappa1 = 5;
p.kappa2 = 0.5;
p.epsilon = 6;
p.phi = 10;
p.eta = 2;
p.xi = 0.5;
p.rho_r = 0.8;
p.rho_a = 0.9;
p.sig_r = 0.01;
p.sig_a = 0.01;
p.sig_pi = 0.01;
p.sig_z = 0.01;
p.sigma = 2;
p.d = 1;

end