function [W,varW,X,varX] = gprotate2pca(W,varW,X,varX,weights)

[M,D] = size(W);
N = cols(X);

%WW = W'*W + diag(rowsum(varW));

% $$$ % Use ML zero mean (mu should be updated after this function!!) ..
% $$$ dmu = mean(X,2);

% $$$ % Move bias
% $$$ X = X - repmat(dmu,1,n);
% $$$ mu = mu + W*dmu;

Qx = 1;

%warning('DISCARDING VARIANCES!!')

% Whiten w.r.t. X
muX = mean(X,2);
X0 = bsxfun(@minus,X,muX);
XX = X0*X0' + diag(colsum(varX));
% $$$ if fixw
% $$$   [Vx,Dx,tmp] = svd(XX/(n-m)); % USE THIS IF FIXED w ??
% $$$ else
[Vx,Dx,tmp] = svd(XX/N);
% $$$ end
Qx = diag(1./sqrt(diag(Dx))) * Vx';
Qw = Vx*sqrt(Dx);
W = W * Qw;
for i=1:M
  varW(i,:) = diag(Qw'*diag(varW(i,:))*Qw);
end
X = Qx * X;
for j = 1:N
  varX(:,j) = diag(Qx*diag(varX(:,j))*Qx');
end

rotationX = Qx;

% $$$ % Check that XX is really whitened! (because of numerical issues!!)
% $$$ XX = X*X' + sum(CovX,3);
% $$$ if fixw
% $$$   [Vx,Dx,tmp] = svd(XX/(n-m)); % USE THIS IF FIXED w ??
% $$$ else
% $$$   [Vx,Dx,tmp] = svd(XX/n);
% $$$ end
% $$$ if Dx(1) > 1.1
% $$$   % Whiten w.r.t. X AGAIN
% $$$   Qx = diag(1./sqrt(diag(Dx))) * Vx';
% $$$   Qw = Vx*sqrt(Dx);
% $$$   W = W * Qw;
% $$$   for i=1:size(CovW,3)
% $$$     CovW(:,:,i) = Qw'*CovW(:,:,i)*Qw;
% $$$   end
% $$$   X = Qx * X;
% $$$   for j = 1:size(CovX,3)
% $$$     Sv{j} = Qx*Sv{j}*Qx';
% $$$     CovX(:,:,j) = Qx*CovX(:,:,j)*Qx';
% $$$   end
% $$$ end

Qx = 1;
% Diagonalize w.r.t. W
WW = W'*W + diag(rowsum(varW));
% Use weights!!
if nargin >= 5
  WW = W'*diag(weights(:))*W + diag(weights(:)' * varW);
%  WW = W'*diag(weights(:))*W + diag(rowsum(varW));
% $$$   w = sqrt(weights);
% $$$   WW = bsxfun(@times, WW, w(:));
% $$$   WW = bsxfun(@times, WW, w(:)');
end
%WW = W'*W + sum(CovW,3);
[Vw,Dw,tmp] = svd(WW);
%norms_of_W = sqrt(diag(Dw))
%[Dw,I] = sort(diag(Dw), 'descend');
%Vw = Vw(:,I);
Qx = Vw' * Qx;
Qw = Vw;
W = W * Qw;
for i=1:M
  varW(i,:) = diag(Qw'*diag(varW(i,:))*Qw);
end
X = Qx * X;
for j = 1:N
  varX(:,j) = diag(Qx*diag(varX(:,j))*Qx');
end

rotationX = Qx * rotationX;

% Rotate such that, the largest loadings are positive
for d=1:cols(W)
  if max(W(:,d)) < max(-W(:,d))
    W(:,d) = -W(:,d);
    X(d,:) = -X(d,:);
  end
end

% $$$ figure
% $$$ imagesc(abs(rotationX));
% $$$
% $$$ figure
% $$$ plot( sum(W.^2,1) )
% $$$
% $$$ figure
% $$$ plot( diag(Dw) )


close all; clearvars; clc;
N = 256;
Phi = spx.dict.simple.dirac_hadamard_mtx(N);
figure;
imagesc(Phi);
colorbar;
export_fig images/demo_dirac_hadamard_1.png -r120 -nocrop;

mu1 = spx.dict.babel(Phi);
figure;
plot(mu1);
grid on;
export_fig images/demo_dirac_hadamard_babel.png -r120 -nocrop;


function y = BasisQuadPhiX(x1,x2)
y = [x1*x1;
    x1*x2;
    x2*x2;
    x1*x1*x1;
    x1*x1*x2;
    x1*x2*x2;
    x2*x2*x2;
    x1*x1*x1*x1;
    x1*x1*x1*x2;
    x1*x1*x2*x2;
    x1*x2*x2*x2;
    x2*x2*x2*x2;];
end


%IMM_UPDATE  UKF based Interacting Multiple Model (IMM) Filter update step
%
% Syntax:
%   [X_i,P_i,MU,X,P] = IMM_UPDATE(X_p,P_p,c_j,ind,dims,Y,H,R)
%
% In:
%   X_p  - Cell array containing N^j x 1 mean state estimate vector for
%          each model j after prediction step
%   P_p  - Cell array containing N^j x N^j state covariance matrix for
%          each model j after prediction step
%   c_j  - Normalizing factors for mixing probabilities
%   ind  - Indices of state components for each model as a cell array
%   dims - Total number of different state components in the combined system
%   Y    - Dx1 measurement vector.
%   H    - Measurement matrices for each model as a cell array.
%   h    - Measurement mean
%   param - parameters
%   R    - Measurement noise covariances for each model as a cell array.
%
% Out:
%   X_i  - Updated state mean estimate for each model as a cell array
%   P_i  - Updated state covariance estimate for each model as a cell array
%   MU   - Probabilities of each model
%   X    - Combined state mean estimate
%   P    - Combined state covariance estimate
%
% Description:
%   IMM-UKF filter measurement update step. If some of the models have linear
%   measurements standard Kalman filter update step is used for those.
%
% See also:
%   IMM_PREDICT, IMM_SMOOTH, IMM_FILTER

% History:
%   01.11.2007 JH The first official version.
%
% Copyright (C) 2007 Jouni Hartikainen
%
% $Id: imm_update.m 111 2007-11-01 12:09:23Z jmjharti $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function [X_i,P_i,MU,X,P] = uimm_update(X_p,P_p,c_j,ind,dims,Y,H,h,R,param)
    % Number of models
    m = length(X_p);

    % Space for update state mean, covariance and likelihood of measurements
    X_i = cell(1,m);
    P_i = cell(1,m);
    lambda = zeros(1,m);

    % Update for each model
    for i = 1:m
        % Update the state estimates
        if isempty(h) | isempty(h{i})
            [X_i{i}, P_i{i}, K, IM, IS, lambda(i)] = kf_update(X_p{i},P_p{i},Y,H{i},R{i});
        else
            [X_i{i}, P_i{i}, K, IM, IS, lambda(i)] = ukf_update1(X_p{i},P_p{i},Y,h{i},R{i},param{i});
        end
    end

    % Calculate the model probabilities
    MU = zeros(1,m);
    c = sum(lambda.*c_j);
    MU = c_j.*lambda/c;

    % In case lambda's happen to be zero
    if c == 0
        MU = c_j;
    end

    % Output the combined updated state mean and covariance, if wanted.
    if nargout > 3
        % Space for estimates
        X = zeros(dims,1);
        P = zeros(dims,dims);
        % Updated state mean
        for i = 1:m
            X(ind{i}) = X(ind{i}) + MU(i)*X_i{i};
        end
        % Updated state covariance
        for i = 1:m
            P(ind{i},ind{i}) = P(ind{i},ind{i}) + MU(i)*(P_i{i} + (X_i{i}-X(ind{i}))*(X_i{i}-X(ind{i}))');
        end
    end


% BETALPR - Beta Distribution - Log Probability Ratio
% Copyright (c) 1998, Harvard University. Full copyright in the file Copyright
%
%   [ lpr ] = betalpr(p1,p2,alpha,beta)
%
% returns the log of the p(p1) / p(p2) when both
% are distributed Beta(alpha,beta).
%
% See also: METROP, *LPR

function [ lpr ] = betalpr(p1,p2,alpha,beta)
lpr = (alpha-1) * log(p1/p2) + (beta-1) * log((1-p1)/(1-p2)) ;


addpath data/

patch_dim = 64;
num_patches = 10000;
listing = dir('data/denoise/mit_saliency/*.jpg');

num_file = 20;
for m = 1 : num_file
    fprintf('Extracting patch batch: %d / %d\n', m, num_file);
    % extract random patches
    samples = zeros(patch_dim, patch_dim, 3, num_patches);
    labels = zeros(size(samples));
    for i = 1 : num_patches
        if (mod(i,100) == 0)
            fprintf('Extracting patch: %d / %d\n', i, num_patches);
        end

        r_idx = random('unid', size(listing, 1));
        orig_img = imread(strcat('data/denoise/mit_saliency/', listing(r_idx).name));

        orig_img_size = size(orig_img);
        offset = 30;
        r = random('unid', orig_img_size(1) - patch_dim - 2*offset + 1) + offset;
        c = random('unid', orig_img_size(2) - patch_dim - 2*offset + 1) + offset;

        input_patch = im2double(orig_img(r:r+patch_dim-1, c:c+patch_dim-1, :));

        samples(:,:,:,i) = imnoise(input_patch, 'gaussian', 0, 0.01);
        labels(:,:,:,i) = input_patch;
    end
    samples = single(samples);
    labels = single(labels);
    % save it
    filename = strcat('data\denoise\train\patches_', num2str(m));
    save(filename, '-v7.3', 'samples', 'labels');
end


function temp = lpass(E_map)
n = size(E_map,1);
t = E_map;
sn = 33;

vec = linspace(-pi,pi,sn)'*ones(1,sn);
gaus = exp((-vec.^2- (vec').^2)/2)/sqrt(2*pi);

span = [fliplr(fliplr(t).') flipud(t) t.';fliplr(t) t fliplr(t);t.' flipud(t) fliplr(fliplr(t).')];
convdata = conv2(span,gaus);
l = (size(convdata,2)-n)/2;
temp = convdata((l+1):(end-l) , (l+1):(end-l));
temp = temp./sum(sum(temp)).*sum(sum(E_map));


function G = repmat(f, M, N)
%REPMAT   Replicate and tile a CHEBFUN.
%   REPMAT(F, M, N) or REPMAT(F, [M, N]) creates an array-valued CHEBFUN by
%   tiling copies of F. If F is a column CHEBFUN, then REPMAT(F, 1, N) returns
%   an array-valued CHEBFUN with N*SIZE(F, 2) CHEBFUN columns. If F is a row
%   CHEBFUN, REPMAT(F, M, 1) returns an array-valued CHEBFUN with M*size(F, 1).
%
% See also HORZCAT, VERTCAT, CAT.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Parse inputs:
if ( nargin == 2 )
    if ( length(M) ~= 2 )
        error('CHEBFUN:CHEBFUN:repmat:notEnoughInputs', ...
            'Requires REPMAT(F, M, N) or REPMAT(F, [M, N]).')
    end
    N = M(2);
    M = M(1);
end

if ( f(1).isTransposed )
    % REPMAT a row CHEBFUN:
    if ( N ~= 1 )
        error('CHEBFUN:CHEBFUN:repmat:row',...
            'Use REPMAT(F, M, 1) to replicate and tile row CHEBFUN objects.')
    else
        G = repmat({f}, M, 1);
        G = vertcat(G{:});
    end
else
    % REPMAT a column CHEBFUN:
    if ( M ~= 1 )
        error('CHEBFUN:CHEBFUN:repmat:col',...
            'Use REPMAT(F, 1, N) to replicate and tile column CHEBFUN objects.')
    else
        G = repmat({f}, 1, N);
        G = horzcat(G{:});
    end
end


function f = comp_ifwt(c,g,a,J,Ls,ext)
%COMP_IFWT Compute Inverse DWT
%   Usage:  f = comp_ifwt(c,g,J,a,Ls,ext);
%
%   Input parameters:
%         c     : Cell array of length M = J*(filtNo-1)+1. Each element is Lc(m)*W array
%         g     : Synthesis wavelet filters - cell-array of length *filtNo*.
%         J     : Number of filterbank iterations.
%         a     : Upsampling factors - array of length *filtNo*.
%         Ls    : Length of the reconstructed signal.
%         ext   : 'per','zero','odd','even', Type of the forward transform boundary handling.
%
%   Output parameters:
%         f     : Reconstructed data - Ls*W array.
%

% see comp_fwt for explanantion
assert(a(1)==a(2),'First two elements of a are not equal. Such wavelet filterbank is not suported.');

% Impulse responses to a correct format.
filtNo = numel(g);
%gCell = cellfun(@(gEl) conj(flipud(gEl.h(:))),g,'UniformOutput',0);
gCell = cellfun(@(gEl) gEl.h(:),g,'UniformOutput',0);

if strcmp(ext,'per')
   % Initial shift of the filter to compensate for it's delay.
   % "Zero" delay reconstruction is produced.
   % offset = cellfun(@(gEl) gEl.offset,g);
   %offset = cellfun(@(gEl) 1-numel(gEl.h)-gEl.offset,g);
   offset = cellfun(@(gEl) gEl.offset,g);
elseif strcmp(ext,'valid')
   offset = -cellfun(@(gEl) numel(gEl.h)-1,g);
else
   % -1 + 1 = 0 is used for better readability and to be consistent
   % with the shift in comp_fwt.
   % Here we are cheating, because we are making the filters
   % anti-causal to compensate for the delay introduced by causal
   % analysis filters.
   % Instead, we could have used causal filters here and do the
   % delay compensation at the end (cropping f).
   % offset = -cellfun(@(gEl) numel(gEl),gCell) + (a -1) +1;
   offset = -(a-1);
end

Lc = cellfun(@(cEl) size(cEl,1),c);
Lc(end+1) = Ls;
tempca = c(1);
cRunPtr = 2;
for jj=1:J
   tempca=comp_ifilterbank_td([tempca;c(cRunPtr:cRunPtr+filtNo-2)],gCell,a,Lc(cRunPtr+filtNo-1),offset,ext);
   cRunPtr = cRunPtr + filtNo -1;
end
% Save reconstructed data.
f = tempca;

% for ch=1:chans
%   tempca = c(LcStart(1):LcEnd(1),ch);
%   LcRunPtr = filtNo+1;
%   cRunPtr = 2;
%   for jj=1:J
%      tempca = comp_upconv({tempca}, Lc(LcRunPtr),{tmpg{1}},a(1),skip(1),ext,0);
%      for ff=2:filtNo
%         % tempca = tempca + comp_upconv({c{cRunPtr}(:,ch)}, Lc(LcRunPtr),{tmpg},a(ff),skip,doNoExt,0);
%         tempca = tempca + comp_upconv({c(LcStart(cRunPtr):LcEnd(cRunPtr),ch)}, Lc(LcRunPtr),{tmpg{ff}},a(ff),skip(ff),ext,0);
%         cRunPtr = cRunPtr + 1;
%      end
%      LcRunPtr = LcRunPtr + filtNo -1;
%   end
%   f(:,ch) = tempca;
% end


function [ icc, ccc ] = st_to_cc_index ( nst, ist, jst, ncc, n )

%*****************************************************************************80
%
%% ST_TO_CC_INDEX creates CC indices from ST data.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NST, the number of ST elements.
%
%    Input, integer IST(NST), JST(NST), the ST rows and columns.
%
%    Input, integer NCC, the number of CC elements.
%
%    Input, integer N, the number of columns.
%
%    Output, integer ICC(NCC), the CC rows.
%
%    Output, integer CCC(N+1), the compressed CC columns.
%

%
%  Sort the elements.
%
  [ jst2, ist2 ] = i4vec2_sort_a ( nst, jst, ist );
%
%  Get the unique elements.
%
  [ jcc, icc ] = i4vec2_sorted_uniquely ( nst, jst2, ist2, ncc );
%
%  Compress the column index.
%
  ccc = zeros(n+1,1);

  ccc(1) = 1;
  jlo = 1;
  for i = 1 : ncc
    jhi = jcc(i);
    if ( jhi ~= jlo )
      ccc(jlo+1:jhi) = i;
      jlo = jhi;
    end
  end
  jhi = n + 1;
  ccc(jlo+1:jhi) = ncc + 1;

  return
end


function dz = dynamics(z,u,param)
% dz = dynamics(z,u,param)
%
% Computes the first-order form of the dynamics for the combined chain
% integrator and pendulum system
%
% INPUTS:
%   z = [x;v1;v2];
%   u = [u1;u2];
%
% OUTPUTS:
%   dz = dz/dt
%

x = z(1,:);
v1 = z(2,:);
% v2 = z(3,:);   %Unused
u1 = u(1,:);
u2 = u(2,:);

% Pendulum physics
dv1 = pendulum(x,v1,u1,param);

% Integrator chain physics:
dx = v1;
dv2 = u2;

% Combine:
dz = [dx;dv1;dv2];

end


function [M0, T1] = mtv_compute_m0_t1(data, flipAngles, TR, b1Map, roi, fixT1, verbose)

% function function [M0 T1] = mtv_compute_m0_t1 (data, flipAngles, TR [, b1Map, roi, fixT1, verbose])
% -----------------------------------------------------------
% This function performs a weighted-least squares data fit
% on SPGR T1-weighted data set
% INPUTS:
% data: width x length x slices x flipAngles matrix
% flipAngles: a vector of flip angles (in degrees) that corresponds to
% data(:,:,:,4)
% TR: in s
% b1Map: a width x length x slices matrix that contains the relative flip
% angle (i.e. if nominal alpha is 60, and measured alpha is 61, then b1Map
% = 61/60
% roi: binary mask (Matrix)
% fixT1: scalar or Matrix. If 0 --> do not fix T1.
% verbose: logical
%

if ndims(data)<3, data = permute(data(:),[2 3 4 1]); end
dataSize = size(data);
if (nargin < 4) || isempty(b1Map)
    b1Map = ones([dataSize(1:end-1) 1]);
end

if nargin<5 || isempty(roi)
    roi = ones(dataSize(1:end-1));
end

if nargin<6
    fixT1 = 0;
end

if nargin<7
    verbose = 0;
end

if max(size(b1Map) ~= dataSize(1:length(size(b1Map)))), error('B1 size is different from data size'); end

dims = size(data);
T1 = zeros(dims(1:end-1));
M0 = zeros(dims(1:end-1));

warning('off');
%sprintf('%s\n\n\n','loop over voxels...')
for vox=1:dims(1)*dims(2)*dims(3)
    %disp([sprintf('\b\b\b\b\b%3i',floor(vox/(dims(1)*dims(2)*dims(3))*100)) '%'])
    if roi(vox) && b1Map(vox)~=0

    kk=floor((vox-1)/(dims(1)*dims(2))); jj=floor((vox-kk*dims(1)*dims(2)-1)/(dims(1))); ii=vox-kk*dims(1)*dims(2)-jj*dims(1);
    kk=kk+1; jj=jj+1;
    %ii
        if ~fixT1
            %% T1 Mapping Using Variable Flip Angle SPGR Data With
            % Flip Angle Correction - Liberman, et al.
            y = squeeze(squeeze(data(ii,jj,kk, :)))./sin(flipAngles/180*pi*b1Map(vox))';
            x = squeeze(squeeze(data(ii,jj,kk, :)))./tan(flipAngles/180*pi*b1Map(vox))';

            % fit data
            param = polyfit(x,y,1);
%             fitresult = LinearFit(x, y, verbose);
%             param = coeffvalues(fitresult); % slope and intercept of the fitting
            param(isnan(param))=0;

%             ci = confint(fitresult,0.682); % confidence interval of the fitting (returns the slope and intercept of the lines framing the fit) -- corresponding to 2*sigma
%             ci(isnan(ci)) = 0;
%
            % compute PD and T1
            [~,T1(vox)]=getT1(param,TR);
        else
            %% if T1 is known
            T1(vox)=fixT1(min(vox,end));
        end
        %% Get M0
        [FA,iFA]=min(flipAngles);

        M0(vox)=getM0fromT1(T1(vox),TR(1),data(ii,jj,kk,iFA),FA*b1Map(vox));
        % add length of the confidence interval at 68.2% in the fourth
        % dimension

        if ~fixT1
%             [~,t1_min]=getT1(ci(1,:),TR);
%             [~,t1_max]=getT1(ci(2,:),TR);
%             M02(vox)=abs(getM0fromT1(t1_max,TR,data(ii,jj,kk,ialpha),alpha)-getM0fromT1(t1_min,TR,data(ii,jj,kk,ialpha),alpha));
%             T12(vox)=abs(t1_max-t1_min);
        end

    end
end
if ~fixT1
%    T1(:,:,:,2)=T12;
%     M0(:,:,:,2)=M02;
end
%display('...done')

function [fitresult, gof] = LinearFit(x, y, verbose)
%CREATEFIT(X,Y)
%  Create a fit.
%
%  Data for 'untitled fit 1' fit:
%      X Input : x
%      Y Output: y
%  Output:
%      fitresult : a fit object representing the fit.
%      gof : structure with goodness-of fit info.
%
%  See also FIT, CFIT, SFIT.

%  Auto-generated by MATLAB on 22-Jan-2015 15:51:35

% Fit: 'untitled fit 1'.
[xData, yData] = prepareCurveData( x, y );

% Set up fittype and options.
ft = fittype( 'poly1' );

% Fit model to data.
[fitresult, gof] = fit( xData, yData, ft );

% Plot fit with data.
if verbose && max(xData~=0) && max(yData~=0)

    figure(100)
    h = plot( fitresult, xData, yData,'+');
    set(h,'MarkerSize',30)
    legend( h, 'y vs. x', 'untitled fit 1', 'Location', 'NorthEast' );
    p11 = predint(fitresult,x,0.95,'observation','off');
    hold on
    plot(x,p11,'m--'); drawnow;
    hold off
    % Label axes
    xlabel( 'x' );
    ylabel( 'y' );
    grid on
    saveas(gcf,['temp.jpg']);
end

function [M0,T1]=getT1(param,TR)
a=param(1); % slope
b=param(2); % intercept
if a>0
    T1 = -TR/log(a);
else  % due to noise or bad fitting
    T1 = 0.000000000000001;
end
M0 = b/(1-exp(-TR/T1));

function M0=getM0fromT1(T1,TR,S,FA)
% Volz, S., N�th, U., Deichmann, R., 2012. Correction of systematic errors in quantitative proton density mapping. Magn. Reson. Med. 68, 74?85.
% Steady state
ST=(1-exp(-TR./T1))./(1-cos(FA*pi/180).*exp(-TR./T1)).*sin(FA*pi/180);
% M0=RP*PD (RP=Receiver Profile)
M0=S/ST;


function cnnnumgradcheck(net, x, y)
    epsilon = 1e-4;
    er      = 1e-8;
    n = numel(net.layers);
    for j = 1 : numel(net.ffb)
        net_m = net; net_p = net;
        net_p.ffb(j) = net_m.ffb(j) + epsilon;
        net_m.ffb(j) = net_m.ffb(j) - epsilon;
        net_m = cnnff(net_m, x); net_m = cnnbp(net_m, y);
        net_p = cnnff(net_p, x); net_p = cnnbp(net_p, y);
        d = (net_p.L - net_m.L) / (2 * epsilon);
        e = abs(d - net.dffb(j));
        if e > er
            error('numerical gradient checking failed');
        end
    end

    for i = 1 : size(net.ffW, 1)
        for u = 1 : size(net.ffW, 2)
            net_m = net; net_p = net;
            net_p.ffW(i, u) = net_m.ffW(i, u) + epsilon;
            net_m.ffW(i, u) = net_m.ffW(i, u) - epsilon;
            net_m = cnnff(net_m, x); net_m = cnnbp(net_m, y);
            net_p = cnnff(net_p, x); net_p = cnnbp(net_p, y);
            d = (net_p.L - net_m.L) / (2 * epsilon);
            e = abs(d - net.dffW(i, u));
            if e > er
                error('numerical gradient checking failed');
            end
        end
    end

    for l = n : -1 : 2
        if strcmp(net.layers{l}.type, 'c')
            for j = 1 : numel(net.layers{l}.a)
                net_m = net; net_p = net;
                net_p.layers{l}.b{j} = net_m.layers{l}.b{j} + epsilon;
                net_m.layers{l}.b{j} = net_m.layers{l}.b{j} - epsilon;
                net_m = cnnff(net_m, x); net_m = cnnbp(net_m, y);
                net_p = cnnff(net_p, x); net_p = cnnbp(net_p, y);
                d = (net_p.L - net_m.L) / (2 * epsilon);
                e = abs(d - net.layers{l}.db{j});
                if e > er
                    error('numerical gradient checking failed');
                end
                for i = 1 : numel(net.layers{l - 1}.a)
                    for u = 1 : size(net.layers{l}.k{i}{j}, 1)
                        for v = 1 : size(net.layers{l}.k{i}{j}, 2)
                            net_m = net; net_p = net;
                            net_p.layers{l}.k{i}{j}(u, v) = net_p.layers{l}.k{i}{j}(u, v) + epsilon;
                            net_m.layers{l}.k{i}{j}(u, v) = net_m.layers{l}.k{i}{j}(u, v) - epsilon;
                            net_m = cnnff(net_m, x); net_m = cnnbp(net_m, y);
                            net_p = cnnff(net_p, x); net_p = cnnbp(net_p, y);
                            d = (net_p.L - net_m.L) / (2 * epsilon);
                            e = abs(d - net.layers{l}.dk{i}{j}(u, v));
                            if e > er
                                error('numerical gradient checking failed');
                            end
                        end
                    end
                end
            end
        elseif strcmp(net.layers{l}.type, 's')
%            for j = 1 : numel(net.layers{l}.a)
%                net_m = net; net_p = net;
%                net_p.layers{l}.b{j} = net_m.layers{l}.b{j} + epsilon;
%                net_m.layers{l}.b{j} = net_m.layers{l}.b{j} - epsilon;
%                net_m = cnnff(net_m, x); net_m = cnnbp(net_m, y);
%                net_p = cnnff(net_p, x); net_p = cnnbp(net_p, y);
%                d = (net_p.L - net_m.L) / (2 * epsilon);
%                e = abs(d - net.layers{l}.db{j});
%                if e > er
%                    error('numerical gradient checking failed');
%                end
%            end
        end
    end
%    keyboard
end


function model = lleCreate(inputDim, outputDim, Y, options)

% LLECREATE Locally linear embedding model.
% FORMAT
% DESC creates a structure for a locally linear embedding.
% ARG latentDimension : dimension of latent space.
% ARG outputDim : dimension of data.
% ARG Y : the data to be modelled in design matrix format (as many
% rows as there are data points).
% ARG options : options structure as returned by lleOptions.
% RETURN model : model structure containing LLE model.
%
% COPYRIGHT : Neil D. Lawrence, 2008, 2009
%
% SEEALSO : lleOptions, modelCreate

% MLTOOLS

model.type = 'lle';

if size(Y, 2) ~= outputDim
  error(['Input matrix Y does not have dimension ' num2str(d)]);
end
model.isNormalised = options.isNormalised;
model.regulariser = options.regulariser;
model.acyclic = options.acyclic;
model.k = options.numNeighbours;
model.Y = Y;
model.d = outputDim;
model.q = inputDim;
model.N = size(Y, 1);

if isfield(model, 'acyclic') && model.acyclic
  model.indices = findAcyclicNeighbours(model.Y, model.k);
else
  model.indices = findNeighbours(model.Y, model.k);
end
model.W = spalloc(model.N, model.N, model.N*model.k);


% phd_test.m
% ====================================================>
% This is a test script which demonstrates the usage of the "SMC_PHD" class.
%
% SETUP:
%  * Before running the simulation, open "2_crossing_targets.mat" or "3_roaming_targets.mat" datasets, from the "datasets" folder
%  * The datasets have been extracted by simulating the motion of differential robots in a 2D-plane (x,y)
%  * The "gen_obs_cluttered_multi3" function takes as an input the ground truth data, including information about the measurement noise and clutter rate
%     and then produces 1xNk cell array of corrupted and cluttered measurements, Nk being the total number of timesteps

% Load the ground truth data
load('multiple-robot-tracking.mat');

% Plot settings
ShowPlots = 1;              % Set to 0 to hide plots
ShowPrediction = 0;         % Set to 0 to skip showing prediction
ShowUpdate = 1;             % Set to 0 to skip showing update

lambdaV = 0; % Expected number of clutter measurements over entire surveillance region
V = 10^2;     % Volume of surveillance region (10x10 2D-grid)
V_bounds = [0 10 0 10]; % [x_min x_max y_min y_max]
P_D = 1;

% Instantiate a Transitionamic model
transition_model = ConstantVelocityX('NumDims',2,'VelocityErrVariance',0.0001);

% Instantiate a Measurement model
measurement_model = LinearGaussianX('NumMeasDims',2,'NumStateDims',4,'MeasurementErrVariance',0.02,'Mapping',[1 3]);
%measurement_model = RangeBearing2CartesianX('NumStateDims',4,'MeasurementErrVariance',[0.001,0.02],'Mapping',[1 3]);

% Instantiate a clutter model
clutter_model = PoissonRateUniformPositionX('ClutterRate',lambdaV,'Limits',[V_bounds(1:2);V_bounds(3:4)]);

% Instantiate birth model
birth_model = DistributionBasedBirthModelX('Distribution', UniformDistributionX([V_bounds(1:2); ...
                                                                                [-0.1 0.1 ];...
                                                                                V_bounds(3:4);...
                                                                                [-0.1 0.1 ]]),...
                                           'BirthIntensity', 0.0001);

% Instantiate detection model
detection_model = ConstantDetectionProbabilityX('DetectionProbability',P_D);

% Compile the State-Space model
model = StateSpaceModelX(transition_model,...
                       measurement_model,...
                       'Clutter',clutter_model,...
                       'Birth', birth_model,...
                       'Detection', detection_model);

% Extract the ground truth data from the example workspace
NumIter = size(GroundTruth,2);

% Set BirthIntensity
NumTracks = 3;

% Generate DataList
meas_simulator = MultiTargetMeasurementSimulatorX('Model',model);
DataList = meas_simulator.simulate(GroundTruthStateSequence);

% Assign PHD parameter values
config.Model = model;
[priorParticles, priorWeights] = model.Birth.random(50000);
config.StatePrior = ParticleStateX(priorParticles,10*priorWeights);
config.BirthScheme = {'Expansion', 5000};
config.SurvivalProbability = 0.99;

% Instantiate PHD filter
myphd = SMC_PHDFilterX(config);

% Create figure windows
if(ShowPlots)
    img = imread('maze.png');

    % set the range of the axes
    % The image will be stretched to this.
    min_x = 0;
    max_x = 10;
    min_y = 0;
    max_y = 10;

    % make data to plot - just a line.
    x = min_x:max_x;
    y = (6/8)*x;

    figure('units','normalized','outerposition',[0 0 .5 1])
    ax(1) = gca;
    figure('units','normalized','outerposition',[.5 0 .5 1])
    ax(2) = gca;
end

% START OF SIMULATION
% ===================>
for k=1:NumIter
    fprintf('Iteration = %d/%d\n================>\n',k,NumIter);

    % Extract DataList at time k
    tempDataList = DataList(k);

    % Change PHD filter parameters
    myphd.MeasurementList = tempDataList; % New observations

    % Predict PHD filter
    myphd.predict();

    % Update PHD filter
    myphd.update();

    fprintf("Estimated number of targets: %f\n", sum(myphd.StatePosterior.Weights));
    % Plot update step results
    if(ShowPlots && ShowUpdate)
        % Plot data
        cla(ax(1));
         % Flip the image upside down before showing it
        imagesc(ax(1),[min_x max_x], [min_y max_y], flipud(img));

        % NOTE: if your image is RGB, you should use flipdim(img, 1) instead of flipud.
        hold on;
        h2 = plot(ax(1), DataList(k).Vectors(1,:),DataList(k).Vectors(2,:),'k*','MarkerSize', 10);
        for j=1:NumTracks
            states = [GroundTruthTracks(j).Trajectory.Vector];
            h2 = plot(ax(1), states(1,1:k), states(3,1:k),'b.-','LineWidth',1);
            h2 = plot(ax(1), states(1,k), states(3,k),'bo','MarkerSize', 10);
        end
        p_i = myphd.IntensityPerHypothesis(2:end);

        ValidHypothesisInds = find(p_i>0.8)+1;
        for j = 1:length(ValidHypothesisInds)
            hypInd = ValidHypothesisInds(j);
            dist = ParticleDistributionX(myphd.StatePrediction.Particles,myphd.weightsPerHypothesis_(hypInd,:));
            dist.resample(1000);
            plot(ax(1), dist.Particles(1,:), dist.Particles(3,:), '.');
            text(ax(1), dist.Mean(1,:), dist.Mean(3,:), num2str(p_i(hypInd-1)),'FontSize',20,'Color','k');
        end

        % set the y-axis back to normal.
        set(ax(1),'ydir','normal');
        str = sprintf('Robot positions (Update)');
        title(ax(1),str)
        xlabel('X position (m)')
        ylabel('Y position (m)')
        axis(ax(1),V_bounds)

        % Plot PHD
        cla(ax(2), 'reset');
        [bandwidth,density,X,Y]=kde2d(myphd.StatePosterior.Particles([1,3],:)');
        %contour3(X,Y,density,50);
        h = surf(ax(2),X,Y,density);
        shading interp
        colormap(ax(2), jet(3000))
        %set(h, 'edgecolor','none')
        hold on;
%         plot(ax(2), myphd.Particles(1,:), myphd.Particles(3,:), '.')
%         hold on;
        plot(ax(2), myphd.MeasurementList.Vectors(1,:), myphd.MeasurementList.Vectors(2,:), 'y*');
        axis(ax(2), [V_bounds]);
        str = sprintf('PHD intensity (Update)');
        xlabel(ax(2),'X position (m)')
        ylabel(ax(2),'Y position (m)')
        zlabel(ax(2),'Intensity')
        title(ax(2),str)
        pause(0.01)
    end
end


% MPC applied to HIV system using a SINDYc model.
% Function for TESTING

clear all, close all, clc
figpath = '../FIGURES/HIV/'; mkdir(figpath)
datapath = '../DATA/HIV/'; mkdir(datapath)
addpath('../utils');

SystemModel = 'HIV';
ModelName = 'SINDYc';

%% Load Model
Nvar = 5;
InputSignalTypeModel = 'prbs'; % prbs; chirp; noise; sine2; sphs; mixed
load(fullfile(datapath,['EX_',SystemModel,'_SI_SINDYc','_',InputSignalTypeModel,'.mat']))

%% TRUE SYSTEM PARAMETERS
run_HIV_params
yout = poolDataLIST({'x1','x2','x3','x4','x5','u'},Models.SINDYc.Xi,6,3,0);

% ZURAKOWSKI
xi_truth.term{1} = {'1', 'x1', 'x1x2', 'x1x2u'};
xi_truth.coeff{1} = [lambda1, -d, -alpha1, eta*alpha1];
xi_truth.term{2} = {'x2', 'x1x2', 'x2x4', 'x2x5', 'x1x2u'};
xi_truth.coeff{2} = [-a, alpha1, -p1, -p2, -eta*alpha1];
xi_truth.term{3} = {'x3', 'x2x3', 'x1x2x3'};
xi_truth.coeff{3} = [-b2, -c2*q, c2];
xi_truth.term{4} = {'x4', 'x2x4'};
xi_truth.coeff{4} = [-b1, c1];
xi_truth.term{5} = {'x5', 'x2x3'};
xi_truth.coeff{5} = [-h, c2*q];

% Construct true Xi
Xi0 = zeros(size(Models.SINDYc.Xi));
for i = 1:Nvar
    for j = 1:length(xi_truth.term{i})
        idx = find(strcmp(yout,xi_truth.term{i}(j)));
        Xi0(idx,i) = xi_truth.coeff{i}(j);
    end
end

%% Apply Model predictive control to system using SINDYc model
select_model = 'SINDYc';
pest.ahat = Model.Xi(:,1:Nvar); % Use Xi0(:,1:Nvar) to execute true model parameters
pest.polyorder = Model.polyorder;
pest.usesine = Model.usesine;
pest.dt = 1/12; %Model.dt; % Don't need to use the same time step model was trained on

% Parameters MPC
options = optimoptions('fmincon','Algorithm','sqp','Display','none', ...
    'MaxIterations',100);
Nweeks = 50;
Duration = Nweeks*7;             % Run for 'Duration' time units
Ton = 0;                         % Time units when control is turned on
x0n = [10, 0.1, 0.1, 0.1, 0.1]'; % Initial condition
Ts  = pest.dt;                   % Sampling time [1/hr]
Tcontrol = 0;
getMPCparams

% Reference state, which shall be achieved
xref = zeros(1,5);
xref(2) = ((c2*(lambda1-d*q)-b2*alpha1) - sqrt((c2*(lambda1-d*q)-b2*alpha1)^2 - 4*alpha1*c2*q*d*b2))/(2*alpha1*c2*q);
xref(1) = lambda1/(d+alpha1*xref(2));
xref(4) = 0;
xref(5) = (xref(2)*c2*(alpha1*q-a) + b2*alpha1)/(c2*p2*xref(2));
xref(3) = h*xref(5)/(c2*q*xref(2));

% Initialize variables
Nt = (Duration/Ts)+1;
uopt0    = 0;
xhat     = x0n;
uopt     = uopt0.*ones(Nu,1);
xHistory = zeros(Nvar,Nt); xHistory(:,1) = xhat;
uHistory = zeros(1,Nt); uHistory(1)   = uopt(1);
tHistory = zeros(1,Nt); tHistory(1)   = Tcontrol;
rHistory = zeros(Nvar,Nt);

%%
% Start simulation
fprintf('Simulation started.  It might take a while...\n')
tic
for ct = 1:(Duration/Ts)   % For each iteration: take measurements & optimize control input & apply control input

    if mod(ct*Ts,7) == 0 % Update once a week
        % NMPC with full-state feedback
        COSTFUN = @(u) ObjectiveFCN_models(u,xhat,N,Nu,xref,uHistory(:,ct),pest,diag(Q),R,Ru,select_model);
        CONSFUN = @(u) ConstraintFCN_models(u,uHistory(:,ct),xhat,N,LBo,UBo,LBdu,UBdu,pest,select_model);
        uopt = fmincon(COSTFUN,uopt,[],[],[],[],LB,UB,CONSFUN,options);
    end

    % Integrate system
    xhat = rk4u(@HIVsys_ZURAKOWSKI,xhat,uopt(1),Ts/2,2,[],0);
    xHistory(:,ct+1) = xhat;
    uHistory(:,ct+1) = uopt(1);
    tHistory(:,ct+1) = ct*Ts+Tcontrol;
    rHistory(:,ct+1) = xref;

    if mod(ct,1000) == 0
        disp(['PROGRESS: ',num2str(100*ct/(Duration/Ts)),'%'])
    end

end
fprintf('Simulation finished!\n')
toc

%%
clear ph
figure;box on,hold on
ccolors = get(gca,'colororder');
plot(tHistory(1:ct)/7,xref(1)*ones(length(tHistory(1:ct)),1)./max(xHistory(1,1:ct)),'--','Color',ccolors(1,:),'LineWidth',1), hold on
plot(tHistory(1:ct)/7,xref(2)*ones(length(tHistory(1:ct)),1)./max(xHistory(2,1:ct)),'--','Color',ccolors(2,:),'LineWidth',1)
plot(tHistory(1:ct)/7,xref(3)*ones(length(tHistory(1:ct)),1)./max(xHistory(3,1:ct)),'--','Color',ccolors(3,:),'LineWidth',1)
ph(1) = plot(tHistory(1:ct)/7,xHistory(1,1:ct)./max(xHistory(1,1:ct)),'-','Color',ccolors(1,:),'LineWidth',1.5);
ph(2) = plot(tHistory(1:ct)/7,xHistory(2,1:ct)./max(xHistory(2,1:ct)),'-','Color',ccolors(2,:),'LineWidth',1.5);
ph(3) = plot(tHistory(1:ct)/7,xHistory(3,1:ct)./max(xHistory(3,1:ct)),'-','Color',ccolors(3,:),'LineWidth',1.5);
ph(4) = plot(tHistory(1:ct)/7,uHistory(1:ct),'-k','LineWidth',1.5);
ylim([0 1])
legend(ph,'x1','x2','x3','u')
%% Show results
clear ph

figure;hold on, box on,
ccolors = get(gca,'colororder');
plot(tHistory,xref1(1)*ones(length(tHistory),1),'--','Color',ccolors(1,:),'LineWidth',1)
plot(tHistory,xref1(2)*ones(length(tHistory),1),'--','Color',ccolors(2,:),'LineWidth',1)
plot(tHistory,xref1(3)*ones(length(tHistory),1),'--','Color',ccolors(3,:),'LineWidth',1)
ph(1) = plot(tHistory,xHistory(1,:),'-','Color',ccolors(1,:),'LineWidth',1.5);
ph(2) = plot(tHistory,xHistory(2,:),'-','Color',ccolors(2,:),'LineWidth',1.5);
ph(3) = plot(tHistory,xHistory(3,:),'-','Color',ccolors(3,:),'LineWidth',1.5);
ph(4) = plot(tHistory,uHistory,'-k','LineWidth',1.5);
xlabel('Time')
ylabel('xi')
legend(ph,'x1','x2','x3','Control')
axis tight
set(gca,'xtick',[50,100,150,200])
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')

%% Save Results
Results.t = tHistory;
Results.x = xHistory;
Results.u = uHistory;
Results.J = evalObjectiveFCN(uHistory,xHistory,rHistory,diag(Q),R,Ru);


function [cm] = in2cm(in)
% Convert length from inches to centimeters.
% Chad A. Greene
cm = in*2.54;


%ReLU function

x = -10:0.01:10;
y = x;
y(x<0) = 0;
plot(x,y);
xlabel('x');
ylabel('y');
grid on


function transmat = ex_transmat(nt)
%EX_TRANSMAT  Example transition probability matrix definition.

%   MOST
%   Copyright (c) 2015-2016, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MOST.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/most for more info.

transmat = cell(1, nt);
T = [ 0.158655253931457; 0.682689492137086; 0.158655253931457 ];
[transmat{:}] = deal(T * ones(1,3));
transmat{1} = T;


function [ n_data, j1, j2, j3, j4, j5, j6, j7, j8, j9, fx ] = nine_j_values ...
  ( n_data )

%*****************************************************************************80
%
%% NINE_J_VALUES returns some values of the Wigner 9J function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real J1, J2, J3, J4, J5, J6, J7, J8, J9,
%    the arguments of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 9;

  fx_vec = [ ...
     0.0004270039294528318, ...
    -0.001228915451058514, ...
    -0.0001944260688400887, ...
     0.003338419923885592, ...
    -0.0007958936865080434, ...
    -0.004338208690251972, ...
     0.05379143536399187, ...
     0.006211299937499411, ...
     0.03042903097250921 ];
  j1_vec = [ ...
    1.0, ...
    1.5, ...
    2.0, ...
    1.0, ...
    1.5, ...
    2.0, ...
    0.5, ...
    1.0, ...
    1.5  ];
  j2_vec = [ ...
    8.0, ...
    8.0, ...
    8.0, ...
    3.0, ...
    3.0, ...
    3.0, ...
    0.5, ...
    0.5, ...
    0.5  ];
  j3_vec = [ ...
    7.0, ...
    7.0, ...
    7.0, ...
    2.0, ...
    2.0, ...
    2.0, ...
    1.0, ...
    1.0, ...
    1.0 ];
  j4_vec = [ ...
    6.5, ...
    6.5, ...
    6.5, ...
    4.0, ...
    4.0, ...
    4.0, ...
    2.0, ...
    2.0, ...
    2.0 ];
  j5_vec = [ ...
    7.5, ...
    7.5, ...
    7.5, ...
    1.5, ...
    1.5, ...
    1.5, ...
    1.0, ...
    1.0, ...
    1.0 ];
  j6_vec = [ ...
    7.5, ...
    7.5, ...
    7.5, ...
    3.0, ...
    3.0, ...
    3.0, ...
    1.5, ...
    1.5, ...
    1.5 ];
  j7_vec = [ ...
    6.0, ...
    6.0, ...
    6.0, ...
    3.5, ...
    3.5, ...
    3.5, ...
    1.5, ...
    1.5, ...
    1.5 ];
  j8_vec = [ ...
    10.0, ...
    10.0, ...
    10.0, ...
     2.0, ...
     2.0, ...
     2.0, ...
     0.5, ...
     0.5, ...
     0.5 ];
  j9_vec = [ ...
    6.0, ...
    6.0, ...
    6.0, ...
    2.0, ...
    2.0, ...
    2.0, ...
    1.5, ...
    1.5, ...
    1.5 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    j1 = 0.0;
    j2 = 0.0;
    j3 = 0.0;
    j4 = 0.0;
    j5 = 0.0;
    j6 = 0.0;
    j7 = 0.0;
    j8 = 0.0;
    j9 = 0.0;
    fx = 0.0;
  else
    j1 = j1_vec(n_data);
    j2 = j2_vec(n_data);
    j3 = j3_vec(n_data);
    j4 = j4_vec(n_data);
    j5 = j5_vec(n_data);
    j6 = j6_vec(n_data);
    j7 = j7_vec(n_data);
    j8 = j8_vec(n_data);
    j9 = j9_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function b = cppsl ( ap, n, b )

%*****************************************************************************80
%
%% CPPSL solves a complex hermitian positive definite linear system.
%
%  Discussion:
%
%    The matrix is assumed to have been factored by CPPCO or CPPFA.
%
%    A division by zero will occur if the input factor contains
%    a zero on the diagonal.  Technically this indicates
%    singularity but it is usually caused by improper subroutine
%    arguments.  It will not occur if the subroutines are called
%    correctly and INFO == 0.
%
%    To compute inverse(A) * C where C is a matrix with P columns:
%
%      call cppco(ap,n,rcond,z,info)
%
%      if (rcond is too small .or. info /= 0) then
%        error
%      end if
%
%      do j = 1, p
%        call cppsl(ap,n,c(1,j))
%      end do
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex AP(N*(N+1)/2), the output from CPPCO or CPPFA.
%
%    Input, integer N, the order of the matrix.
%
%    Input, complex B(N), the right hand side.
%
%    Output, complex B(N), the solution.
%
  kk = 0;
  for k = 1 : n
    t = conj ( ap(kk+1:kk+k-1) ) * transpose ( b(1:k-1) );
    kk = kk + k;
    b(k) = ( b(k) - t ) / ap(kk);
  end

  for k = n : -1 : 1
    b(k) = b(k) / ap(kk);
    kk = kk - k;
    t = -b(k);
    b(1:k-1) = b(1:k-1) + t * ap(kk+1:kk+k-1);
  end

  return
end


function [nodes, edges] = createTestGraph02(varargin)
%CREATETESTGRAPH02  Test graph for geodesic functions
%
%   [nodes edges] = createTestGraph02;
%
%   Example
%     [nodes edges] = createTestGraph02;
%     figure;
%     axis([0 100 10 90]);
%     axis equal;
%     drawGraph(nodes, edges);
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-05-19,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

nodes = [ ...
   10 40; ...
   10 60; ...
   20 50; ...
   40 50; ...
   50 20; ...
   50 40; ...
   50 60; ...
   50 80; ...
   60 50; ...
   80 50; ...
   90 40; ...
   90 60];

edges = [...
    1  3; ...
    2  3; ...
    3  4; ...
    4  6; ...
    4  7; ...
    5  6; ...
    6  9; ...
    7  8; ...
    7  9; ...
    9 10; ...
   10 11; ...
   10 12];


for i = 60:63
	n = 2^i;
	p = [0.4; 0.6];
	x = sample_hist(p, n);
	assert(sum(x) == n);
	assert(max(abs(x/n - p)) < 1e-4);
end


function toms179_test02 ( )

%*****************************************************************************80
%
%% TOMS179_TEST02 demonstrates the use of MDBETA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TOMS179_TEST02:\n' );
  fprintf ( 1, '  MDBETA estimates the value of the modified Beta function.\n' );
  fprintf ( 1, '  Compare with tabulated values.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X         P         Q         ' );
  fprintf ( 1, 'Beta                      Beta                    Diff\n' );
  fprintf ( 1, '                                    ' );
  fprintf ( 1, '(Tabulated)               (MDBETA)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, p, q, x, fx ] = beta_cdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    [ fx2, ifault ] = mdbeta ( x, p, q );

    fprintf ( 1, '  %8.4f  %8.4f  %8.4f  %24.16e  %24.16e  %10.4e\n', ...
    p, q, x, fx, fx2, abs ( fx - fx2 ) );

  end

  return
end


function [ hScatterObj ] = ScatterSparse( mS, varargin )
% ----------------------------------------------------------------------------------------------- %
% [ hScatterObj ] = ScatterSparse( mS, varargin )
% Visualize sparsity pattern of matrix. Comparable to `spy()` with all the
% controls given by `scatter()`.
% Input:
%   - mS                -   Input Sparse Matrix.
%                           Structure: Matrix (numRows x numCols).
%                           Type: 'Single' / 'Double' (Sprase).
%                           Range: (-inf, inf).
%   - varargin          -   Scatter Plot Parameters.
%                           Set of parameters accepeted by `scatter()`.
%                           Structure: NA.
%                           Type: NA.
%                           Range: NA.
% Output:
%   - hScatterObj       -   Scatter Plot Object.
%                           Structure: Scalar.
%                           Type: Handler / Object.
%                           Range: NA.
% References:
%   1.  A
% Remarks:
%   1.  Supports any functionality by `scatter()` plot.
% TODO:
%   1.
%   Release Notes:
%   -   1.0.000     19/07/2021  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

arguments
    mS (:, :) {mustBeNumeric, mustBeReal, mustBeSparse}
end
arguments (Repeating)
    varargin
end

[vI, vJ, ~] = find(mS);
hScatterObj = scatter(vJ, vI, varargin{:});

end

function [ ] = mustBeSparse( mS )

if(~issparse(mS))
    eid = 'mustBeSparse:mSMustBeSparse';
    msg = 'The input matrix must be sparse.';
    throwAsCaller(MException(eid, msg));
end

end


%  Script file: timings.m
%
%  Purpose:
%    This program calculates the time required to
%    calculate the squares of all integers from 1 to
%    10,000 in four different ways:
%    1.  Using a for loop with an uninitialized output
%        array.
%    2.  Using a for loop with a pre-allocated output
%        array and NO JIT compiler.
%    3.  Using a for loop with a pre-allocated output
%        array and the JIT compiler.
%    4.  Using vectors.
%
%  Record of revisions:
%      Date       Programmer          Description of change
%      ====       ==========          =====================
%    01/29/07    S. J. Chapman        Original code
%
% Define variables:
%   ii, jj       -- Loop index
%   average1     -- Average time for calculation 1
%   average2     -- Average time for calculation 2
%   average3     -- Average time for calculation 3
%   average4     -- Average time for calculation 4
%   maxcount     -- Number of times to loop calculation
%   square       -- Array of squares

% Perform calculation with an uninitialized array
% "square".  This calculation is done only once
% because it is so slow.
maxcount = 1;               % Number of repetitions
tic;                        % Start timer
for jj = 1:maxcount
   clear square             % Clear output array
   for ii = 1:10000
     square(ii) = ii^2;     % Calculate square
   end
end
average1 = (toc)/maxcount;  % Calculate average time

% Perform calculation with a pre-allocated array
% "square", calling an external function to square
% the number.  This calculation is averaged over 10
% loops.
maxcount = 10;              % Number of repetitions
tic;                        % Start timer
for jj = 1:maxcount
   clear square             % Clear output array
   square = zeros(1,10000); % Pre-initialize array
   for ii = 1:10000
     square(ii) = sqr(ii);  % Calculate square
   end
end
average2 = (toc)/maxcount;  % Calculate average time

% Perform calculation with a pre-allocated array
% "square".  This calculation is averaged over 100
% loops.
maxcount = 100;             % Number of repetitions
tic;                        % Start timer
for jj = 1:maxcount
   clear square             % Clear output array
   square = zeros(1,10000); % Pre-initialize array
   for ii = 1:10000
     square(ii) = ii^2;     % Calculate square
   end
end
average3 = (toc)/maxcount;  % Calculate average time

% Perform calculation with vectors.  This calculation
% averaged over 1000 executions.
maxcount = 1000;            % Number of repetitions
tic;                        % Start timer
for jj = 1:maxcount
   clear square             % Clear output array
   ii = 1:10000;            % Set up vector
   square = ii.^2;          % Calculate square
end
average4 = (toc)/maxcount;  % Calculate average time

% Display results
fprintf('Loop / uninitialized array        = %8.4f\n', average1);
fprintf('Loop / initialized array / no JIT = %8.4f\n', average2);
fprintf('Loop / initialized array / JIT    = %8.4f\n', average3);
fprintf('Vectorized                        = %8.4f\n', average4);


function W = randInitializeWeights(L_in, L_out)
    %RANDINITIALIZEWEIGHTS Randomly initialize the weights of a layer with L_in
    %incoming connections and L_out outgoing connections
    %   W = RANDINITIALIZEWEIGHTS(L_in, L_out) randomly initializes the weights
    %   of a layer with L_in incoming connections and L_out outgoing
    %   connections.
    %
    %   Note that W should be set to a matrix of size(L_out, 1 + L_in) as
    %   the column row of W handles the "bias" terms
    %
    EPSILON_INIT = 0.12;
    W = rand(L_out, 1 + L_in) * 2 * EPSILON_INIT - EPSILON_INIT;
end


function [P,S] = computeSMFromWProjective( W, method, isCalibrated )
% Compute SfM from measurements for projective camera
%
% This function should not be called directly: it does not contain
% pre-processing steps (removal of calibration matrix if known) or
% post-procssing steps (affine/metric upgrades) and bundle adjustment
% computeSMFromW should always be used instead.
%
% Essential matrix decomposition
% Reference: HZ2, p259, Result 9.19, and p. 294
% nFrame==2 && isCalibrated
% fast
%
% Normalized 8-point algorithm
% Reference: HZ2, p279 and alg 11.1 p. 282
% nFrame==2 && ~isCalibrated
% fast
%
% Projective Factorization
% Reference: HZ2, p445, Algorithm 18.2
% Sturm and Triggs ECCV 96
% A Factorization Based Algorithm for Multi-Image Projective Structure and
% Motion
% nFrame>2 && method==0
%
% Projective Factorization
% Reference: Iterative Extensions of the Sturm/Triggs Algorithm:
% Convergence and Nonconvergence, from Oliensis, Hartley, PAMI 07
% nFrame>2 && method==Inf
% slower
%
% If there are any missing entries:
% Low-Rank Matrix Fitting Based on Subspace Perturbation Analysis
% with Applications to Structure from Motion
% Hongjun Jia, Aleix M. Martinez, PAMI 08
%
% Returns the structure and camera parameters in an Animation object
%
% USAGE
%   anim = computeSMFromWProjective( W, method )
%
% INPUTS
%  W            - [ 2 x nPoint x nFrame ] 2D projected features
%  method       - [inf] method for performing SFM (see above for details)
%  isCalibrated - flag indicating if the camera is calibrated (only used
%                 for 2 frames)
%
% OUTPUTS
%  P          - [ 3 x 4 x nFrame ] projection matrices
%  S          - [ 3 x nPoint ] structure matrix
%
% EXAMPLE
%
% See also
%
% Vincent's Structure From Motion Toolbox      Version 3.1.1
% Copyright (C) 2008-2011 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

nFrame = size(W,3); nPoint = size(W,2);

P=zeros(3,4,nFrame); P(:,:,1)=eye(3,4);

WIsnan=isnan(W);
hasAnyNan=any(WIsnan(:));

% Normalized 8-point algorithm
% Reference: HZ2, p279 and alg 11.1 p. 282
if nFrame==2
  % Normalize input data
  [x T]=normalizePoint(W(:,:,1),Inf);
  [xp Tp]=normalizePoint(W(:,:,2),Inf);

  A=[xp([1 1],:).*x; xp(1,:); xp([2 2],:).*x;xp(2,:);x;ones(1,nPoint)]';

  [U,S,V]=svd(A,0); F=reshape(V(:,end),[3,3])';
  [U,S,V]=svd(F,0);
  F=U*diag([S(1,1) S(2,2) 0])*V';

  F=[ Tp; 0 0 1 ]'*F*[ T; 0 0 1 ];

  if ~isCalibrated
    P(:,:,2) = convertPF([],F,true);
    S = computeSFromWM( true, W, P, 'method', 0 );
  else
    % Essential matrix decomposition
    % Reference: HZ2, p259, Result 9.19, and p. 294
    [ U disc V ] = svd(F);
    if det(U)<0; U=-U; end; if det(V)<0; V=-V; end

    % Check which of the 4 possibilities gives a point in front of both
    % cameras
    WW = [ 0 -1 0; 1 0 0; 0 0 1 ];
    maxCount=0;
    for i = 1 : 4
      switch i
        case 1,
          P(:,:,2) = [ U*WW*V' U(:,3) ];
        case 2,
          P(:,:,2) = [ U*WW*V' -U(:,3) ];
        case 3,
          P(:,:,2) = [ U*WW'*V' U(:,3) ];
        case 4,
          P(:,:,2) = [ U*WW'*V' -U(:,3) ];
      end

      C = zeros(3,2); v = C;
      for j = 1 : 2
        % camera center (Reference: HZ2, p158-161)
        M = P(:,1:3,j);
        C(:,j)=-M\P(:,4,j);
        % principal axis
        v(:,j) = det(M)*M(3,:)';
      end
      % compute the image of a point
      STmp = computeSFromWM( true, W, P, 'method', 0);
      maxCountTmp = sum((v(:,1)'*bsxfun(@minus,STmp,C(:,1)))>0) + ...
        sum((v(:,2)'*bsxfun(@minus,STmp,C(:,2)))>0);
      if maxCountTmp>maxCount
        P2 = P(:,:,2); maxCount = maxCountTmp; S=STmp;
      end
    end
    P(:,:,2) = P2;
  end
end

% Projective Factorization
% Reference: Iterative Extensions of the Sturm/Triggs Algorithm:
% Convergence and Nonconvergence, from Oliensis, Hartley, PAMI 07
if nFrame>2 && ismember(method,[ 0 Inf ]) && ~hasAnyNan
  % Normalize coordinates
  [ W T ] = normalizePoint(W,-Inf);

  % Initialize
  lam = ones( 1, nPoint, nFrame );
  WSquaredSum = sum(W.^2,1);
  C0Const = sum(WSquaredSum(:));
  for n=1:50
    % Stage 1
    if method==0
      for k = 1 : 10
        % normalize each column and then each row
        lam=bsxfun(@rdivide, lam, sqrt(sum(lam.^2,2)));
        lam=bsxfun(@rdivide, lam, sqrt(sum(lam.^2,3)));
      end
    end

    % get the best rank 4 approximation
    % Wkm1 is for Wk minus 1
    [Wkm1,Wkm1Hat]=projSturmTriggsWkm1Wkm1Hat(lam,W);
    WWkm1Hat = sum(W.*Wkm1Hat,1);

    if method==Inf
      if n==1
        mu = norm(Wkm1(:)-Wkm1Hat(:))^2/norm(Wkm1(:))^4*1.1;
      else
        C0 = mu*C0Const;
        C1 = mu*sum(WWkm1Hat(:));
        C2 = WWkm1Hat.^2./WSquaredSum; C2 = mu*sum(C2(:));
        C3 = mu*sum(Wkm1Hat(:).^2);
      end
    end

    % Stage 2
    % get the optimal lambdas
    lam=WWkm1Hat./WSquaredSum;

    % Stage 3
    if method==Inf && n>1
      a = roots( [ C0, -(C0^2-2*C1), -(2*C0*C3-C2), ...
        -(4*C1*C3-2*C2*C0), C0*C3^2-2*C2*C3, ...
        2*C1*C3^2-C2^2, C2*C3^2 ] );
      a = real(a(abs(imag(a))<1e-10)); a = a(a>=0);

      if length(a)>1
        zkm1=C3*C0/C2;
        if abs(zkm1-1)>eps
          a = a( sign(zkm1-1)==sign(a/sqrt(C3)-1) );
        else a = sqrt(C2/C0)+1;
        end
      end
      lam = ( a + lam )*sqrt(a/(a^2*C0+2*a*C1+C2));
      %        [a,sqrt(a/(a^2*C0+2*a*C1+C2)), mean(lam(:)), std(lam(:))]
    end
  end

  % De-normalize coordinates
  Wk = bsxfun(@times,lam,W);
  [ U S V ] = svd( reshape(permute(Wk,[1,3,2]),3*nFrame,nPoint),...
    'econ' );
  P = U(:,1:4)*S(1:4,1:4); S = V(:,1:4)';

  PTmp = P;
  S = normalizePoint(S,4);
  P = zeros(3,4,nFrame);
  for i=1:nFrame; P(:,:,i) = T(:,:,i)\PTmp(3*i-2:3*i,:); end
end

% Low-Rank Matrix Fitting Based on Subspace Perturbation Analysis
% with Applications to Structure from Motion
% Hongjun Jia, Aleix M. Martinez, PAMI 08
if nFrame>2 && ismember(method,[ 0 Inf ]) && hasAnyNan
  % Normalize coordinates
  [ q TW ] = normalizePoint(W, -Inf);
  HHat=reshape(permute(q,[1,3,2]),3*nFrame,nPoint);
  TInv=1./permute(sum(q.*q,1),[3,2,1]);
  Sk=HHat;
  for k = 1 : 30
    % Sk is HHat (homogeneous W) but scaled by lambda, initialized to 1
    [PStack,S]=lowRankDecomposition(Sk,4);

    % make sure the columns of PStack are orthonormal
    [PStack,disc,disc]=svd(PStack,'econ');

    P = permute(reshape(PStack,3,nFrame,4),[1,3,2]);
    %sumTot=0;
    for j=1:nPoint
      mask=~isnan(TInv(:,j));
      if all(~mask); continue; end
      Cj=permute(sum(bsxfun(@times,q(:,j,mask),P(:,:,mask)),1),[3,2,1]);
      % Solve the maximum of the Rayleigh quotient
      % it is the highest eigenvector of A*v=lam*(1./TjInv)*v
      % or heighest eigen value of TjInv*A*v=lam*v
      A=Cj*Cj';
      [lamjTmp,val]=eigs(bsxfun(@times,TInv(mask,j),A),1,'lm');
      lamj=zeros(1,1,nFrame); lamj(mask)=lamjTmp;
      %sumTot=sumTot+lamjTmp'*Cj*Cj'*lamjTmp/(lamjTmp'*diag(1./ ...
      % TInv(mask,j))*lamjTmp);
      Sk(:,j)=reshape(bsxfun(@times,q(:,j,:),lamj),3*nFrame,1);
    end
    %sumTot;
  end
  [PStack,S]=lowRankDecomposition(Sk,4);

  PTmp = PStack;
  S = normalizePoint(S,4);
  P = zeros(3,4,nFrame);
  for i=1:nFrame; P(:,:,i) = TW(:,:,i)\PTmp(3*i-2:3*i,:); end
end

end

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

function [ Wkm1, Wkm1Hat ]=projSturmTriggsWkm1Wkm1Hat(lam,W)
% perform the computation of Wk and its rank 4 approximation
% in Storm Triggs
nFrame=size(W,3); nPoint=size(W,2);
Wkm1 = bsxfun(@times,lam,W);
[ U S V ] = svd( reshape(permute(Wkm1,[1,3,2]),3*nFrame,nPoint), 'econ' );
Wkm1Hat = permute(reshape(U(:,1:4)*S(1:4,1:4)*V(:,1:4)',3,nFrame,...
  nPoint),[1,3,2]);
end


function [fMc, fBvalue, fBStd, fAvalue, fSigmaLow, fSigmaHi] = calc_McduebBst(magnitudes, varargin)
    % calc_McduebBst  Calculate Mc using the function b-value vs. cut-off-magnitude: Bootstrap approach
    % [fMc, fBvalue, fBStd, fAvalue, fSigmaLow, fSigmaHi] = calc_McduebBst(mCatalog, fBinning, nWindowSize, nMinNumberEvents, nSample)
    %-------------------------------------------------------------------------------------------------------
    % Calculate Mc using the function b-value vs. cut-off-magnitude: Bootstrap approach
    % Decision criterion for b and Mc: b_i-std_Bst(b_i) <= b_ave <= b_i+std_Bst(b_i)

    % Relevant reference: Cao A., Gao, S.S., Temporal variation of seismic b-values
    % beneath northeastern Japan island arc, GRL, 29, 9, 2002
    %
    % Incoming variables:
    % magnitudes         : EQ catalog magnitudes
    % fBinning         : Bin size
    % nWindowSize      : Window size
    % nMinNumberEvents : Minimum number of events
    % nSample          : Number of bootstrap samples
    %
    % Outgoing variables:
    % fMc              : Magnitude of completeness
    % fBvalue          : b-value
    % fBStd            : 2nd moment of b-value-distribution (comparable to standard deviation)
    % fAvalue          :a-value
    % fSigmaLow        : 16-percentile of b-value distribution
    % fSigmaHi         : 84-percentile of b-value distribution
    % Author: J. Woessner
    % updated: 04.06.03

    % Check input
    p=inputParser;
    p.addOptional('fBinning',0.1);
    p.addOptional('nWindowSize',5);
    p.addOptional('nMinNumberEvents',50);
    p.addOptional('nSample',100);
    p.parse(varargin{:});

    PCTL_RANGE = [16, 84];

    fBinning            = p.Results.fBinning;
    nWindowSize         = p.Results.nWindowSize;
    nMinNumberEvents    = p.Results.nMinNumberEvents;
    nSample             = p.Results.nSample;

    % Set fix values
    fMinMag = min(magnitudes);
    fMaxMag = max(magnitudes);

    % Create bootstrap samples using bootstrap matlab toolbox
    deepdim = 1; % for deepdim = 1:size(magnitudes,2)
    mMag_bstsamp = bootrsp(magnitudes,nSample);
    % Calculate b-with magnitude
    magBins = fMinMag:fBinning:fMaxMag;
    nBins = numel(magBins);

    % mBvalue = nan(nBins, 7); % contains [meanb_value, meanb_value_std, meana_value vSigma(1) vSigma(2), fStdBst]
    mean_b_value     = nan(nBins,1);
    mean_b_value_std = nan(nBins,1);
    mean_a_value     = nan(nBins,1);
    mean_mag         = nan(nBins,1);
    sigmas           = nan(nSample,2);
    std_bst          = nan(nBins,1);

    for i = 1 : nBins
        fMag = magBins(i);
        mBvalue_bst = nan(nSample, 4); % resets each pass, contains [b_value, b_value_std, a_value, magvalue]

        magMask = mMag_bstsamp >= fMag - 0.05;

        idxs        = (i-1).* nSample + (1:nSample);
        b_value     = nan(nSample,1);
        b_value_std = nan(nSample,1);
        a_value     = nan(nSample,1);

        mBvalue_bst(idxs,4) = fMag;
        for nSamp=1:nSample
            bvalIsNan = magMask(:, nSamp);
            mCat = mMag_bstsamp(bvalIsNan, nSamp);

            % Check for minimum number of events

            if numel(mCat) >= nMinNumberEvents
                [ b_value(nSamp), b_value_std(nSamp), a_value(nSamp)] =  calc_bmemag(mCat, fBinning);
            end
        end
        mBvalue_bst(idxs, 1:3) = [b_value, b_value_std, a_value];

        % Check for Nan and create output for [16 84]-percentile
        bvalIsNan   = isnan(b_value);
        bval_nonans = b_value(~bvalIsNan);

        if ~isempty(bval_nonans)
            vSigma = prctile(bval_nonans, PCTL_RANGE);
            if length(bval_nonans)==1
                vSigma = vSigma';
            end
            fStdBst = std(bval_nonans, 1, 'omitnan'); % Calculate 2nd moment

            sigmas(i,:)         = vSigma;
            std_bst(i)          = fStdBst;
        end

        mean_b_std_a_mag    = mean(mBvalue_bst,'omitnan');
        mean_b_value(i)     = mean_b_std_a_mag(1);
        mean_b_value_std(i) = mean_b_std_a_mag(2);
        mean_a_value(i)     = mean_b_std_a_mag(3);
        mean_mag(i)         = mean_b_std_a_mag(4);

        % mBvalue(i,:) = [mean(mBvalue_bst,'omitnan'), vSigma(1), vSigma(2), fStdBst]; % contains [meanb_value, meanb_value_std, meana_value, meanmag, vSigma(1), vSigma(2), fStdBst]

    end % END of FOR fMag

    % Use bootstrap percentiles to decide for Mc
    totalSteps = nBins - nWindowSize + 1;

    b_avg       = movmean(mean_b_value, nWindowSize, 'Endpoints','discard');
    keeprows    = sigmas(1:totalSteps, 1) <= b_avg & b_avg <= sigmas(1:totalSteps, 2);

    % b_avg       = b_avg(keeprows); % apply to b_avg before changing it's size

    keeprows(nBins) = false; % OK, because len(keeprows) (aka. totalsteps) is always less than nBins
    fMc         = mean_mag(keeprows);
    fBvalue     = mean_b_value(keeprows);
    fAvalue     = mean_a_value(keeprows);
    fBStd       = mean_b_value_std(keeprows);
    fSigmaLow   = sigmas(keeprows, 1);
    fSigmaHi    = sigmas(keeprows, 2);

end


function lo=v_lpcrf2lo(rf)
%V_LPCRF2LO Convert reflection coefficients to log area ratios LO=(RF)
%the output values are limited to about +-14.5

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

r=max(min(rf,1-1E-6),1E-6-1);
lo=log((1-r)./(1+r));


%%
% Test for power diagrams

%%
% Test for power diagrams

addpath('power_bounded/');

rep = 'results/power-diagrams/';
[~,~] = mkdir(rep);

% number of sites
N = 50;
% x and y coordinate of the sites
sigma = .1;
xy = sigma*randn(N, 2)+1/2;
xy(1,:) = 1/2; % center points

% the bounding box in clockwise order
bb = [0,0; 0,1; 1,1; 1,0];

lambda = [1 5 10 100];
lambda = linspace(1,1.1,5);

for i=1:length(lambda)% weights
    w = ones(N,1);w(1) = lambda(i);
    % get the power diagram
    [V,C] = power_bounded(xy(:,1),xy(:,2), w, bb);
    % draw the resulted power diagram
    plot_power(xy,V,C,bb);
    saveas(gcf, [rep 'power-' num2str(i), '.eps'], 'epsc');
    drawnow;
end


function dl=lpcaa2dl(aa)
%LPCAA2DL LPC: Convert area coefficients to dct of log area DL=(AA)

% note: we do not correct for sinc distortion; perhaps we should multiply by
% k=1:p-1;s=[sqrt(0.5)/p 2*sin(pi*k/(2*p))./(pi*k)];

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

[nf,p2]=size(aa);
dl=rdct(log(aa(:,2:p2-1)./aa(:,p2*ones(1,p2-2))).').';


function [ SimilarityMatrix ] = similarityNeighbor( x, n, range)
%SIMILARITYNEIGHBOR Summary of this function goes here
%   Detailed explanation goes here

    sz = size(x,1);
    SimilarityMatrix = eye(sz);

    i = 1:sz-n;
    SimilarityMatrix(sub2ind([sz, sz], i+n,i)) = 1;
    SimilarityMatrix(sub2ind([sz, sz], i,i+n)) = 1;

    % invalidate the illegal values from the mask (if at least one element is
    % not present in the mask set similarity to 0)
%     if(numel(mask)~=0)
%         invalidInds = sum(mask(:,range),2) < numel(range);
%
%         SimilarityMatrix(invalidInds,:) = 0;
%         SimilarityMatrix(:,invalidInds) = 0;
%     end

    DiagMask = ones(size(x, 1)) - eye(size(x,1));
    SimilarityMatrix = SimilarityMatrix .* DiagMask;
    SimilarityMatrix = SimilarityMatrix + eye(size(x, 1));

end


function x = bernoulli_cdf_inv ( cdf, a )

%*****************************************************************************80
%
%% BERNOULLI_CDF_INV inverts the Bernoulli CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%    0.0D+00 <= CDF <= 1.0.
%
%    Input, real A, the parameter of the PDF.
%    0.0D+00 <= A <= 1.0.
%
%    Output, integer X, the corresponding argument.
%
  if ( cdf < 0.0 | 1.0 < cdf )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BERNOULLI_CDF_INV - Fatal error!\n' );
    fprintf ( 1, '  CDF < 0 or 1 < CDF.\n' );
    error ( 'BERNOULLI_CDF_INV - Fatal error!' );
  end

  if ( cdf <= 1.0 - a )
    x = 0;
  else
    x = 1;
  end

  return
end


function bool = iseven(number)
%ISEVEN true for even integer
%
%   Usage: bool = iseven(number)
%
%   Input parameters:
%       number  - number (or vector/matrix with numbers) to be tested
%
%   Output parameters:
%       bool    - true if the number is even, false else
%
%   See also: isodd

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Computation =====================================================
% Create answer
bool = false( size(number) );
% Look for even numbers, use bitget to overcome a mod() bug, see
% https://bit.ly/1wcNYBI
bool(bitget(number,1)==0) = true;


function [hd_record]=tvhdecomp(beta_gibbs,D_record,strshocks_record,It,Bu,Y,n,m,p,k,T)

% preliminary tasks
% first create the hd_record and temp cells
hd_record=cell(n,n+1);
temp=cell(n,2);

% then initiate the Gibbs algorithm
for ii=1:It-Bu

% recover beta for the current iteration (one column for each period)
beta_iter=[];
for jj=1:T
beta_iter=[beta_iter beta_gibbs{jj,1}(:,ii)];
end

% recover D for the current iteration (one page for each period)
D_iter=repmat(reshape(D_record(:,ii),n,n),[1,1,T]);

% recover the series of period-specific orthogonal IRFs
[IRFcell]=bear.tvhdsim(beta_iter,D_iter,n,m,p,T,k);

% recover the structural disturbances
ETA=[];
for jj=1:n
ETA=[ETA;strshocks_record{jj,1}(ii,:)];
end
ETA=ETA';

% then compute the historical decomposition
   % loop over variables
   for jj=1:n
      % loop over shocks
      for kk=1:n
      % loop over shocks
      vshocks=ETA(:,kk);
         % loop over time periods
         for ll=1:T
         % initiate the vectors of IRFs and shocks
         virf=[];
            % then loop over IRF periods
            for mm=1:ll
            % create the vector of IRF coefficients
            virf=[virf IRFcell{mm,ll}(jj,kk)];
            end
         % compute then the contribution of shock kk for variable jj at period ll
         hd_record{jj,kk}(ii,ll)=virf*flipud(vshocks(1:ll,1));
         end
      end
   end

% then go for next Gibbs iteration
end

% step 6: compute the contributions of deterministic variables
% loop over rows of temp/hd_record
for ii=1:n
% fill the Ytot matrix in temp
% initial condition
temp{ii,1}=hd_record{ii,1};
   % sum over the remaining columns of hd_record
   for jj=2:n
   temp{ii,1}=temp{ii,1}+hd_record{ii,jj};
   end
% fill the Y matrix in temp
temp{ii,2}=repmat(Y(:,ii)',It-Bu,1);
% fill the Yd matrix in hd_record
hd_record{ii,n+1}=temp{ii,2}-temp{ii,1};
% go for next variable
end


clear all

dt = 0.05;
t  = 0:dt:20;

Nsamples = length(t);

Xsaved = zeros(Nsamples, 3);
Zsaved = zeros(Nsamples, 1);

for k=1:Nsamples
  r = GetRadar(dt);

  [pos vel alt] = RadarUKF(r, dt);

  Xsaved(k, :) = [pos vel alt];
  Zsaved(k)    = r;
end

PosSaved = Xsaved(:, 1);
VelSaved = Xsaved(:, 2);
AltSaved = Xsaved(:, 3);

t = 0:dt:Nsamples*dt-dt;

figure
plot(t, PosSaved)

figure
plot(t, VelSaved)

figure
plot(t, AltSaved)


function fh = sumcolumns_fh(m,w)
% This is almost an MV2DF, but it does not return derivatives on numeric
% input, w.
%
%  w -> W = reshape(w,m,[]) -> sum(W,1)'

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

map = @(w) map_this(w,m);
transmap = @(y) transmap_this(y,m);

fh = linTrans([],map,transmap);

if exist('w','var') && ~isempty(w)
    fh = fh(w);
end

end

function w = transmap_this(y,m)
  w = repmat(y(:).',m,1);
end

function s = map_this(w,m)
W = reshape(w,m,[]);
s = sum(W,1);
end

function test_this()
m = 3;
n = 4;
f = sumcolumns_fh(m);
W = randn(m,n);
test_MV2DF(f,W(:));
end


function [z,mz,onrim,Hd_record] = cg_steihaug(grad,hess,Delta,epsilon,maxCG,y,Hd_back,quiet)
% Helper function for trustregion_newton_cg

savemem = true;

grad = grad(:);
onrim = false;
if exist('Hd_back','var') && ~isempty(Hd_back)
    Hd_record = Hd_back;
    backtrack = true;
else
    backtrack = false;
    if savemem
        Hd_record = zeros(length(grad),maxCG,'single');
    else
        Hd_record = zeros(length(grad),maxCG);
    end
end

z = zeros(size(grad));  % z is step, we start at origin
mz = 0;                 % 2nd order prediction for objective change at step z
r = grad;               % 2nd order residual at z:  r = H*z-grad
d = -r;                 % we're going down

residual = sqrt(r'*r)/epsilon;
if residual <= 1
    Hd_record = [];
    fprintf('CG 0: as far as I can see, this should never happen\n');
    fprintf('CG 0: converged with zero step, residual = %g\n',residual);
    return
end

j=0;
while true
    if backtrack && j+1 <= size(Hd_back,2)
        Hd = double(Hd_back(:,j+1));
    else
        Hd = hess(d);
        if j+1 <= size(Hd_record)
            Hd_record(:,j+1) = Hd;
        end
    end
    dHd = d'*Hd;
    if dHd <=0 %region is non-convex in direction d
        a = d'*d;
        b = 2*z'*d;
        c = z'*z-Delta^2;
        discr = sqrt(b^2-4*a*c);
        tau1 = (-b - discr)/(2*a);
        tau2 = (-b + discr)/(2*a);
        model = @(tau) tau*grad'*d + 0.5*tau^2*dHd;
        if model(tau1) < model(tau2)
            tau = tau1;
        else
            tau = tau2;
        end
        z = z + tau*d;
        mz = mz + tau*d'*grad + 0.5*tau^2*dHd;
        onrim = true;
        if ~quiet, fprintf('CG %i: curv=%g, jump to trust region boundary\n',j,dHd); end
        break;
    end

    alpha = r'*r/dHd;
    old_z = z;
    old_mz = mz;
    z = z + alpha*d;
    mz = mz + alpha*d'*grad + 0.5*alpha^2*dHd;

    radius = sqrt(z'*z)/Delta;
    if radius > 1
        a = d'*d;
        b = 2*z'*d;
        c = old_z'*old_z-Delta^2;
        discr = sqrt(b^2-4*a*c);
        tau = (-b + discr)/(2*a);
        z = old_z + tau*d;
        mz = old_mz + tau*d'*grad + 0.5*tau^2*dHd;
        onrim = true;
        if ~quiet, fprintf('CG %i: curv=%g, terminate on trust region boundary, model=%g\n',j,dHd,-mz); end
        break;
    end

    old_r = r;
    r = r + alpha*Hd;
    residual = sqrt(r'*r)/epsilon;
    if residual <= 1
        if ~quiet,
            fprintf('CG %i: curv=%G, converged inside trust region; radius = %g, residual=%g, model=%g\n',j,dHd,radius,residual,-mz);
        end
        break;
    end

    overshot = ~isempty(y) && y+mz <0;
    if overshot
        if ~quiet,
            fprintf('CG %i: curv=%G, overshot inside trust region; radius = %g, residual=%g, model=%g\n',j,dHd,radius,residual,-mz);
        end
        break;
    end

    %stopped = backtrack && j+1 >= maxCG;
    stopped = j+1 >= maxCG;
    if stopped
        if ~quiet,
            fprintf('CG %i: curv=%G, stopped inside trust region; radius = %g, residual=%g, model=%g\n',j,dHd,radius,residual,-mz);
        end
        break;
    end

    beta = (r'*r)/(old_r'*old_r);
    d = -r + beta*d;
    if ~quiet, fprintf('CG %i: curv=%g, radius = %g, residual=%g, model=%g\n',j,dHd,radius,residual,-mz); end
    j = j+1;
end

if j+1<size(Hd_record,2)
    Hd_record = Hd_record(:,1:j+1);
end
return


function [ar, name] = aspectratio(D)
%
% ar = height / width

k = 0;
for i = 1:length(D);
    if isfield(D(i).annotation, 'object')
        Nobjects = length(D(i).annotation.object);

        for n = 1:Nobjects
            [X,Y] = getLMpolygon(D(i).annotation.object(n).polygon);

            k = k+1;
            ar(k) = abs((max(Y)-min(Y))/(0.00001+max(X)-min(X)));
            name{k} = D(i).annotation.object(n).name;
        end
    end
end


function [y, gene, times, scale, rawExp] = gpsimLoadEcoliData

% GPSIMLOADECOLIDATA Load in E. coli data for the represion case.
% FORMAT

% RETURN y : the normalised expression levels.
% RETURN yvar : the variance of the normalised expression levels.
% RETURN gene : the gene names and Affymetrix array tags.
% RETURN times : the times of the expression measurements.
% RETURN scale : the scaling factor applied to normalise.
% RETURN rawExp : the raw gene expresion level.
%
% SEEALSO : demEcoliMap1
%
% COPYRIGHT : Pei Gao, Neil D. Lawrence and Magnus Rattary, 2008

% SHEFFIELDML

if exist('./data/ecoliData.mat') == 2
  load('./data/ecoliData.mat');
else
  expData = importdata('./data/ecoliNormalisedData.txt');
  rawExp.data = expData';
  rawExp.genes = {'dinF', 'dinI', 'lexA', 'recA', 'recN', 'ruvA', 'ruvB', ...
                  'sbmC', 'sulA', 'umuC', 'umuD', 'uvrB', 'yebG', 'yjiW' };

  targetInd = [2 3 6 7 9 11 12 14];

% Perform log-normal transformation.
  yFull = exp(rawExp.data(:, targetInd));  % Logs are normally distributed
                                   % ... recover mean in exp space.

  % Rescale so that average standard deviation of curves is 1.
  scale = sqrt(var(yFull));
  scaleMat = ones(size(yFull, 1), 1)*scale;
  y{1} = yFull./scaleMat;
  times = [0 5 10 20 40 60]';
  gene = rawExp.genes(targetInd);
%  save('./data/ecoliData.mat', 'y', 'gene', 'times', 'scale', 'rawExp');
end