function [K, sK] = heatXrbfhKernCompute(heatKern, rbfhKern, x1, x2)

% HEATXRBFHKERNCOMPUTE Cross kernel between a HEAT and a RBF kernels.
% FORMAT
% DESC computes cross kernel terms between a HEAT kernel and a RBF kernel
% for the multiple output kernel.
% ARG heatKern : the kernel structure associated with the HEAT kernel.
% ARG rbfKern : the kernel structure associated with the RBFH kernel.
% ARG x1 : inputs for which kernel is to be computed. First column represent
% the time points, while the second column represents the spatial points.
% Entries with Inf indicate missing values.
% RETURN K : block of values from kernel matrix.
% RETURN sK : unscaled kernel matrix
%
% FORMAT
% DESC computes cross kernel terms between a HEAT kernel and a RBF kernel
% for the multiple output kernel.
% ARG heatKern : the kernel structure associated with the HEAT kernel.
% ARG rbfKern : the kernel structure associated with the RBFH kernel.
% ARG x1 : row inputs for which kernel is to be computed. First column
% corresponds to time points and the second column corresponds to spatial
% points. Entries with Inf indicate missing values.
% ARG x2 : column inputs for which kernel is to be computed. First column
% corresponds to time points and the second column corresponds to spatial
% points. Entries with Inf indicate missing values.
% RETURN k : block of values from kernel matrix.
% RETURN sK : unscaled kernel matrix
%
% SEEALSO : multiKernParamInit, multiKernCompute, heatKernParamInit
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 4
    x2 = x1;
end
if size(x1, 2) ~= 2 || size(x2, 2) ~= 2
    error('Input can only have two columns');
end
if (heatKern.inverseWidthTime ~= rbfhKern.inverseWidthTime) || ...
        (heatKern.inverseWidthSpace ~= rbfhKern.inverseWidthSpace)
    error('Kernels cannot be cross combined if they have different inverse widths.')
end

% Split the domain into time domain and spatial domain and account for
% missing values. If there are no missing values the computation of the
% kernel is a pointwise prodruct, otherwise it is a kronecker product.
t1 = x1(x1(:,1)~=Inf,1);
t2 = x2(x2(:,1)~=Inf,1);
s1 = x1(x1(:,2)~=Inf,2);
s2 = x2(x2(:,2)~=Inf,2);
if (length(t1) == length(s1)) && (length(t2) == length(s2))
    ut1 = unique(t1);
    ut2 = unique(t2);
    us1 = unique(s1);
    us2 = unique(s2);
    if (length(ut1)*length(us1) == length(t1)) && ...
            (length(ut2)*length(us2) == length(t2))
        t1 = ut1; s1 = us1; t2 = ut2; s2 = us2;
        isPointwise = false;
        K = zeros(length(t1)*length(s1), length(t2)*length(s2));
    else
        isPointwise = true;
        K = zeros(length(t1), length(t2));
    end
else
    isPointwise = false;
    K = zeros(length(t1)*length(s1), length(t2)*length(s2));
end

% Although this is done in heatKernExpandParam.m, we do it here again as a
% precaution.

heatKern.sim.inverseWidth = heatKern.inverseWidthTime;
rbfhKern.rbf.inverseWidth = rbfhKern.inverseWidthTime;

sigmax = sqrt(2/heatKern.inverseWidthSpace);
lengthX = heatKern.lengthX;
nterms = heatKern.nTerms;
decay = heatKern.decay;
diff = heatKern.diffusion;

% Precompute some terms
w = ((1:nterms)*(pi/lengthX))';
gamma = sqrt(-1)*w;
beta = decay + diff*(w.^2);
cK = 2/lengthX;

if heatKern.includeIC
   error('Not implemented yet')
else
    if isPointwise
        for i=1:nterms
            heatKern.sim.decay = beta(i);
            Kt = simXrbfKernCompute(heatKern.sim, rbfhKern.rbf, t1, t2);
            Ks = srbfhKernCompute(sigmax, lengthX, s1, s2, w, gamma, i);
            K = K + Kt.*Ks;
        end
    else
        for i=1:nterms
            heatKern.sim.decay = beta(i);
            Kt = simXrbfKernCompute(heatKern.sim, rbfhKern.rbf, t1, t2);
            Ks = srbfhKernCompute(sigmax, lengthX, s1, s2, w, gamma, i);
            K = K + kron(Kt,Ks);
        end
    end
    sK = cK*K;
    K = heatKern.sensitivity*sK;
end


function criterion = gbc_angle(q,CS,Dl,Dr,threshold,varargin)
%

d = angle(orientation(q(Dl),CS),orientation(q(Dr),CS));

if length(threshold) == 1

  criterion = d < threshold;

else

  criterion = 0.5 * ((d < threshold(1)) + (d < threshold(2)));

end

% now check whether the have a misorientation heigher or lower than a
% threshold
%m = inv(q(Dl)).*q(Dr);

%criterion = abs(dot(m,quaternion.id)) > cos(threshold/2);

%if any(~criterion)
%  qcs = quaternion(CS.properGroup);
%  criterion(~criterion) = max(abs(dot_outer(m(~criterion),qcs)),[],2) > cos(threshold/2);
%end

% o_Dl = orientation(q(Dl),CS,symmetry);
% o_Dr = orientation(q(Dr),CS,symmetry);
% criterion = dot(o_Dl,o_Dr) > cos(threshold/2);


EbN0=0:5:30;
MSE=zeros(1,length(EbN0));
for i=1:length(EbN0)
    SNR=EbN0(i);
    sim('v_blast_t4_r4_8PSK_crrect_MSE');
    MSE(i)=mse;
    save MSE;
end
semilogy(EbN0,MSE)
xlabel('EbN0(dB)')
ylabel('MSE')
grid on


function geometry_test022 ( )

%*****************************************************************************80
%
%% TEST022 tests DIRECTION_UNIFORM_3D, GET_SEED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST022\n' );
  fprintf ( 1, '  DIRECTION_UNIFORM_3D picks a random direction vector.\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10
    [ vran, seed ] = direction_uniform_3d ( seed );
    fprintf ( 1, '  %10f  %10f  %10f\n', vran(1:dim_num) );
  end

  return
end


function [nu, g] = orderedNoiseUpdateParams(noise, mu, varsigma, y, index)

% ORDEREDNOISEUPDATEPARAMS Update parameters for ordered categorical noise model.

% NOISE

% NOISE

[g, dlnZ_dvs] = orderedNoiseGradVals(noise, mu(index, :), ...
                                            varsigma(index, :), ...
                                            y(index, :));

nu = g.*g - 2*dlnZ_dvs;


function plotCircle3D(center,normal,radius)

theta=0:0.01:2*pi;
v=null(normal);
points=repmat(center',1,size(theta,2))+radius*(v(:,1)*cos(theta)+v(:,2)*sin(theta));
plot3(points(1,:),points(2,:),points(3,:),'r-');

end


%This directory contains functions for statistical analysis.
%
%STAT_NORMAL_CDF - evaluates the normal cumulative distribution function.
%STAT_RMANOVA - repeated-measures ANOVA
%STAT_CALCTCRIT - calculates the critical t-value given significance threshold and degrees of freedom
%STAT_PERCENTILES - returns the percentiles for data samples.


function [U,S,V] = spm_svd(X,U)
% Computationally efficient SVD (that can handle sparse arguments)
% FORMAT [U,S,V] = spm_svd(X,u)
% X    - (m x n) matrix
% u    - threshold (1 > u > 0) for normalized eigenvalues (default = 1e-6)
%      - a value of zero induces u = 64*eps
%
% U    - {m x p} singular vectors
% V    - {m x p} singular variates
% S    - {p x p} singular values
%__________________________________________________________________________
% Copyright (C) 1994-2011 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_svd.m 6110 2014-07-21 09:36:13Z karl $

% default thresholds - preclude singular vectors with small singular values
%--------------------------------------------------------------------------
if nargin < 2, U = 1e-6; end
if U >= 1; U = U - 1e-6; end
if U <= 0; U = 64*eps;   end

% deal with sparse matrices
%--------------------------------------------------------------------------
[M,N] = size(X);
p     = find(any(X,2));
q     = find(any(X,1));
X     = X(p,q);

% SVD
%--------------------------------------------------------------------------
[i, j, s] = find(X);
[m, n]    = size(X);
if any(i - j)

    % off-leading diagonal elements - full SVD
    %----------------------------------------------------------------------
    X     = full(X);
    if m > n

        [v, S, v] = svd(X'*X,0);
        S         = sparse(S);
        s         = diag(S);
        j         = find(s*length(s)/sum(s) > U);
        v         = v(:,j);
        u         = spm_en(X*v);
        S         = sqrt(S(j,j));

    elseif m < n

        [u, S, u] = svd(X*X',0);
        S         = sparse(S);
        s         = diag(S);
        j         = find(s*length(s)/sum(s) > U);
        u         = u(:,j);
        v         = spm_en(X'*u);
        S         = sqrt(S(j,j));

    else

        [u, S, v] = svd(X,0);
        S         = sparse(S);
        s         = diag(S).^2;
        j         = find(s*length(s)/sum(s) > U);
        v         = v(:,j);
        u         = u(:,j);
        S         = S(j,j);
    end

else
    S             = sparse(1:n,1:n,s,m,n);
    u             = speye(m,n);
    v             = speye(m,n);
    [i, j]        = sort(-s);
    S             = S(j,j);
    v             = v(:,j);
    u             = u(:,j);
    s             = diag(S).^2;
    j             = find(s*length(s)/sum(s) > U);
    v             = v(:,j);
    u             = u(:,j);
    S             = S(j,j);

end

% replace in full matrices
%--------------------------------------------------------------------------
j      = length(j);
U      = sparse(M,j);
V      = sparse(N,j);
if j
    U(p,:) = u;
    V(q,:) = v;
end


function w=pde(u)
% PDE  W = PDE(U) is the residual for the convection-diffusion
%      problem in Chapter 3
%
%
global rhsf;
%
% C=20
%
v=20*u.*(dxmf(u)+dymf(u));
%
% uncomment this line for the unpreconditioned problem
%
%w=-lapmf(u)+20*u.*(dxmf(u)+dymf(u))-rhsf;
%
% uncomment this line for the preconditioned problem
%
w=u+fish2d(v)-rhsf;


function test3
%TEST3 test cs_lsolve, cs_ltsolve, cs_usolve, cs_chol
%
% Example:
%   test3
% See also: testall

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

index = UFget ;
[ignore f] = sort (max (index.nrows, index.ncols)) ;
f = f (1:100) ;

clf
% f = f(1)

for i = f
    Prob = UFget (i) ;
    disp (Prob) ;
    A = Prob.A ;
    [m n] = size (A) ;
    if (~isreal (A) | m ~= n)                                               %#ok
        continue
    end

    A = A*A' + 2*n*speye (n) ;
    try
        p = amd (A) ;
    catch
        p = symamd (A) ;
    end
    try
        L0 = chol (A)' ;
    catch
        continue
    end
    b = rand (n,1) ;

    C = A(p,p) ;
    c = condest (C) ;
    fprintf ('condest: %g\n', c) ;

    x1 = L0\b ;
    x2 = cs_lsolve (L0,b) ;
    err = norm (x1-x2,1) ;
    if (err > 1e-12 * c)
        error ('!') ;
    end

    x1 = L0'\b ;
    x2 = cs_ltsolve (L0,b) ;
    err = norm (x1-x2,1) ;
    if (err > 1e-10 * c)
        error ('!') ;
    end

    U = L0' ;

    x1 = U\b ;
    x2 = cs_usolve (U,b) ;
    err = norm (x1-x2,1) ;
    if (err > 1e-10 * c)
        error ('!') ;
    end

    L2 = cs_chol (A) ;
    subplot (2,3,1) ; spy (L0) ;
    subplot (2,3,4) ; spy (L2) ;
    err = norm (L0-L2,1) ;
    if (err > 1e-8 * c)
        error ('!') ;
    end

    L1 = chol (C)' ;
    L2 = cs_chol (C) ;
    subplot (2,3,2) ; spy (L1) ;
    subplot (2,3,5) ; spy (L2) ;
    err = norm (L1-L2,1) ;
    if (err > 1e-8 * c)
        error ('!') ;
    end

    [L3,p] = cs_chol (A) ;
    C = A(p,p) ;
    L4 = chol (C)' ;
    subplot (2,3,3) ; spy (L4) ;
    subplot (2,3,6) ; spy (L3) ;
    err = norm (L4-L3,1) ;
    if (err > 1e-8 * c)
        error ('!') ;
    end

    drawnow

end


function b = power(a,n)
% function B=power(A,N)
%
% DESCRIPTION
%   Element-by-element powers
%
% INPUTS
%   A: polynomial
%   N: matrix of natural numbers
%
% OUTPUTS
%   B: polynomial, result of element-by-element power A.^N.
%
% SYNTAX
%   B = A.^N
%     Element-by-element powers. A and B must have the same dimensions
%     unless one is a scalar. A scalar can operate into anything.
%   B=power(A,N)
%     Function-call form of power.

% 6/7/2002 PJS  Initial Coding
% 12/12/2010 PJS Call matrix.^matrix code for scalar expansion cases

% Check number of inputs
error(nargchk(2,2,nargin));
sza=size(a);
szn=size(n);

if isempty(a) || isempty(n)
    if isempty(a) && all(szn==[1 1])
        % empty.^scalar = empty(sza)
        b=polynomial(zeros(sza));
        return;
    elseif isempty(n) && all(sza==[1 1])
        % scalar.^empty = empty(szn)
        b=polynomial(zeros(szn));
        return;
    elseif all(sza==szn)
        b=polynomial(zeros(sza));
        return;
    else
        error('Matrix dimensions must agree.');
    end

elseif sza==szn
    % Matrix .^ Matrix
    b = zeros(sza);

    % exponent = 0
    idx = find( n==0 );
    b(idx) = 1;
    b = polynomial(b);

    % exponent = 1
    idx = find( n==1 );
    L.type = '()';
    L.subs = {idx};
    b = subsasgn(b,L,subsref(a,L));

    % exponent = 2;
    idx = find( n>1 );
    idx = idx(:)';
    for i1=idx
        L.subs = {i1};
        ai = subsref(a,L);
        Nt = size(ai.coefficient,1);
        if Nt==1
            bi = ai;
            bi.coefficient = bi.coefficient^n(i1);
            bi.degmat = bi.degmat*n(i1);
        else
            bi = mpower(ai,n(i1));
        end
        b = subsasgn(b,L,bi);
    end

    %         % XXX This is pretty slow.
    %         b = a;
    %         L.type = '()';
    %         for i1 = 1:sza(1);
    %             for i2 = 1:sza(2);
    %                 L.subs = {i1,i2};
    %                 aij = subsref(a,L);
    %                 bij = mpower(aij,n(i1,i2));
    %                 b = subsasgn(b,L,bij);
    %             end
    %         end
    %     end
elseif all(szn==[1 1])
    % Matrix.^Scalar:
    n = repmat(n,sza);
    b = power(a,n);

    %     b = polynomial(1);
    %     for i1 = 1:n
    %         b = b.*a;
    %     end
elseif all(sza==[1 1])
    % Scalar .^ Matrix:
    a = repmat(a,szn);
    b = power(a,n);

    %     % XXX This is pretty slow.
    %     b = polynomial(zeros(szn));
    %     L.type = '()';
    %     for i1 = 1:szn(1);
    %         for i2 = 1:szn(2);
    %             L.subs = {i1,i2};
    %             bij = mpower(a,n(i1,i2));
    %             b = subsasgn(b,L,bij);
    %         end
    %     end
else
    error('Matrix dimensions must agree');
end


% Test file for @deltafun/feval.m

function pass = test_feval(pref)

if ( nargin < 1 )
    pref = chebfunpref();
end

seedRNG(1337)

%%
f = bndfun(@sin);
d = deltafun(f, struct('deltaMag', 1, 'deltaLoc', 0));
pass(1) = isinf(feval(d, 0)) && feval(d, 0) > 0;
pass(2) = isinf(feval(-d, 0)) && feval(-d, 0) <0;

%%
f = fun.constructor(@(x) sin(x));
d = deltafun(f, struct('deltaMag', [], 'deltaLoc', []));
x = rand(1, 4);
pass(2) = norm(feval(f, x) - feval(d, x), inf) == 0;

%%
x = rand(1,4);
d = deltafun(f, struct('deltaMag', rand(1, 4), 'deltaLoc', x));
pass(3) = all(isinf(feval(d, x)));

%%
x = chebfun('x');
d = dirac(x-1);
pass(4) = isinf(feval(d, 1));
val = feval(-d, 1);
pass(5) = isinf(val) && val < 0;
pass(6) = isinf(feval(d, 'right'));
pass(7) = feval(d, 'left') == 0;
pass(8) = feval(d, 1, 'left' ) == 0;
pass(9) = feval(d, 0, 'left' ) == 0;
pass(10) = feval(d, 0, 'right' ) == 0;
d = diff(heaviside(x));
pass(11) = isinf(feval(d, 0));
pass(12) = feval(d, 'right') == 0;
pass(13) = feval(d, 'left') == 0;
pass(14) = feval(d, 0, 'left' ) == 0;
pass(15) = feval(d, 0, 'right' ) == 0;

end


% Copyright 2017 Google Inc.
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
%     https://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.

% Demonstration of the image restoration experiments conducted in
% Y. Romano, M. Elad, and P. Milanfar, "The Little Engine that Could:
% Regularization by Denoising (RED)", submitted to SIAM Journal on Imaging
% Sciences, 2016. https://arxiv.org/abs/1611.02862
%
% This example reads a ground-truth image, degrades the image by
% first blurring or downscaling it, followed by an addition of random white
% Gaussian noise. Then it calls to RED in order to restore the image.
% This example compares the input and output PSNR, shows and saves the
% results. The suggested image-adaptive Laplacian-regularization functional
% is minimized using the Fixed-Point, ADMM, and Steepest Descent methods.
% Please refer to the paper for more details.
%
% The following are the degradation models that this example handles:
% 'UniformBlur'  - 9X9 uniform psf with noise-level equal to sqrt(2)
% 'GaussianBlur' - 25X25 Gaussian psf with std 1.6 and noise-level
%                  equal to sqrt(2)
% 'Downscale'    - 7X7 Gaussian psf with std 1.6 and noise-level equal to 5
%
% The denoising engine is TNRD: Yunjin Chen, and Thomas Pock, "Trainable
% Nonlinear Reaction Diffusion: A Flexible Framework for Fast and Effective
% Image Restoration", IEEE TPAMI 2016. The code is available in
% http://www.icg.tugraz.at/Members/Chenyunjin/about-yunjin-chen
% Note: Enable parallel pool to reduce runtime.
%
% The degradation process is similar to the one suggested in NCSR paper:
% Weisheng Dong, Lei Zhang, Guangming Shi, and Xin Li "Nonlocally
% Centralized Sparse Representation for Image Restoration", IEEE-TIP, 2013.
% The code is available in http://www4.comp.polyu.edu.hk/~cslzhang/NCSR.htm
%

clc;
clear;
close all;

% configure the path
% denoising functions
addpath(genpath('./tnrd_denoising/'));
% SD, FP, and ADMM methods
addpath(genpath('./minimizers/'));
% contains the default params
addpath(genpath('./parameters/'));
% contains basic functions
addpath(genpath('./helper_functions/'));
% test images for the debluring and super resolution problems,
% taken from NCSR software package
addpath(genpath('./test_images/'));

% set light_mode = true to run the code in a sub optimal but faster mode
% set light_mode = false to obtain the results reported in the RED paper
light_mode = false;

if light_mode
    fprintf('Running in light mode. ');
    fprintf('Turn off to obatain the results reported in RED paper.\n');
else
    fprintf('Light mode option is off. ');
    fprintf('Reproducing the result in RED paper.\n');
end

%% read the original image

file_name = 'starfish.tif';

fprintf('Reading %s image...', file_name);
orig_im = imread(['./test_images/' file_name]);
orig_im = double(orig_im);

fprintf(' Done.\n');

%% define the degradation model

% choose the secenrio: 'UniformBlur', 'GaussianBlur', or 'Downscale'
degradation_model = 'UniformBlur';

fprintf('Test case: %s degradation model.\n', degradation_model);

switch degradation_model
    case 'UniformBlur'
        % noise level
        input_sigma = sqrt(2);
        % filter size
        psf_sz = 9;
        % create uniform filter
        psf = fspecial('average', psf_sz);
        % use fft to solve a system of linear equations in closed form
        use_fft = true;
        % create a function-handle to blur the image
        ForwardFunc = ...
            @(in_im) imfilter(in_im,psf,'conv','same','circular');
        % the psf is symmetric, i.e., the ForwardFunc and BackwardFunc
        % are the same
        BackwardFunc = ForwardFunc;
        % special initialization (e.g. the output of other method)
        % set to identity mapping
        InitEstFunc = @(in_im) in_im;

    case 'GaussianBlur'
        % noise level
        input_sigma = sqrt(2);
        % filter size
        psf_sz = 25;
        % std of the Gaussian filter
        gaussian_std = 1.6;
        % create gaussian filter
        psf = fspecial('gaussian', psf_sz, gaussian_std);
        % use fft to solve a system of linear equations in closed form
        use_fft = true;
        % create a function handle to blur the image
        ForwardFunc = ...
            @(in_im) imfilter(in_im,psf,'conv','same','circular');
        % the psf is symmetric, i.e., the ForwardFunc and BackwardFunc
        % are the same
        BackwardFunc = ForwardFunc;
        % special initialization (e.g. the output of other method)
        % set to identity mapping
        InitEstFunc = @(in_im) in_im;

    case 'Downscale'
        % noise level
        input_sigma = 5;
        % filter size
        psf_sz = 7;
        % std of the Gaussian filter
        gaussian_std = 1.6;
        % create gaussian filter
        psf = fspecial('gaussian', psf_sz, gaussian_std);
        % scaling factor
        scale = 3;

        % compute the size of the low-res image
        lr_im_sz = [ceil(size(orig_im,1)/scale),...
                    ceil(size(orig_im,2)/scale)];
        % create the degradation operator
        H = CreateBlurAndDecimationOperator(scale,lr_im_sz,psf);
        % downscale
        ForwardFunc = @(in_im) reshape(H*in_im(:),lr_im_sz);
        % upscale
        BackwardFunc = @(in_im) reshape(H'*in_im(:),scale*lr_im_sz);
        % special initialization (e.g. the output of other method)
        % use bicubic upscaler
        InitEstFunc = @(in_im) imresize(in_im,scale,'bicubic');

    otherwise
        error('Degradation model is not defined');
end

%% degrade the original image

switch degradation_model
    case {'UniformBlur', 'GaussianBlur'}
        fprintf('Blurring...');
        % blur each channel using the ForwardFunc
        input_im = zeros( size(orig_im) );
        for ch_id = 1:size(orig_im,3)
            input_im(:,:,ch_id) = ForwardFunc(orig_im(:,:,ch_id));
        end
        % use 'seed' = 0 to be consistent with the experiments in NCSR
        randn('seed', 0);

    case 'Downscale'
        fprintf('Downscaling...');
        % blur the image, similar to the degradation process of NCSR
        input_im = Blur(orig_im, psf);
        % decimate
        input_im = input_im(1:scale:end,1:scale:end,:);
        % use 'state' = 0 to be consistent with the experiments in NCSR
        randn('state', 0);

    otherwise
        error('Degradation model is not defined');
end

% add noise
fprintf(' Adding noise...');
input_im = input_im + input_sigma*randn(size(input_im));

% convert to YCbCr color space if needed
input_luma_im = PrepareImage(input_im);
orig_luma_im = PrepareImage(orig_im);

if strcmp(degradation_model,'Downscale')
    % upscale using bicubic
    input_im = imresize(input_im,scale,'bicubic');
    input_im = input_im(1:size(orig_im,1), 1:size(orig_im,2), :);
end
fprintf(' Done.\n');
psnr_input = ComputePSNR(orig_im, input_im);

%% minimize the Laplacian regularization functional via Fixed Point

fprintf('Restoring using RED: Fixed-Point method\n');

switch degradation_model
    case 'UniformBlur'
        params_fp = GetUniformDeblurFPParams(light_mode, psf, use_fft);
    case 'GaussianBlur'
        params_fp = GetGaussianDeblurFPParams(light_mode, psf, use_fft);
    case 'Downscale'
        assert(exist('use_fft','var') == 0);
        params_fp = GetSuperResFPParams(light_mode);
    otherwise
        error('Degradation model is not defined');
end

[est_fp_im, psnr_fp] = RunFP(input_luma_im,...
                             ForwardFunc,...
                             BackwardFunc,...
                             InitEstFunc,...
                             input_sigma,...
                             params_fp,...
                             orig_luma_im);
out_fp_im = MergeChannels(input_im,est_fp_im);

fprintf('Done.\n');

%% minimize the Laplacian regularization functional via ADMM

fprintf('Restoring using RED: ADMM method\n');

switch degradation_model
    case 'UniformBlur'
        params_admm = GetUniformDeblurADMMParams(light_mode, psf, use_fft);
    case 'GaussianBlur'
        params_admm = GetGaussianDeblurADMMParams(light_mode, psf, use_fft);
    case 'Downscale'
        assert(exist('use_fft','var') == 0);
        params_admm = GetSuperResADMMParams(light_mode);
    otherwise
        error('Degradation model is not defined');
end

[est_admm_im, psnr_admm] = RunADMM(input_luma_im,...
                                   ForwardFunc,...
                                   BackwardFunc,...
                                   InitEstFunc,...
                                   input_sigma,...
                                   params_admm,...
                                   orig_luma_im);
out_admm_im = MergeChannels(input_im,est_admm_im);

fprintf('Done.\n');

%% minimize the laplacian regularization functional via Steepest Descent

fprintf('Restoring using RED: Steepest-Descent method\n');

switch degradation_model
    case 'UniformBlur'
        params_sd = GetUniformDeblurSDParams(light_mode);
    case 'GaussianBlur'
        params_sd = GetGaussianDeblurSDParams(light_mode);
    case 'Downscale'
        params_sd = GetSuperResSDParams(light_mode);
    otherwise
        error('Degradation model is not defined');
end

[est_sd_im, psnr_sd] = RunSD(input_luma_im,...
                             ForwardFunc,...
                             BackwardFunc,...
                             InitEstFunc,...
                             input_sigma,...
                             params_sd,...
                             orig_luma_im);
% convert back to rgb if needed
out_sd_im = MergeChannels(input_im,est_sd_im);

fprintf('Done.\n');

%% display final results

fprintf('Image name %s \n', file_name);
fprintf('Input PSNR = %f \n', psnr_input);
fprintf('RED: Fixed-Point PSNR = %f \n', psnr_fp);
fprintf('RED: ADMM PSNR = %f \n', psnr_admm);
fprintf('RED: Steepest-Decent PSNR = %f \n', psnr_sd);

%% write images

if ~exist('./results/','dir')
    mkdir('./results/');
end

fprintf('Writing the images to ./results...');

imwrite(uint8(input_im),['./results/input_' file_name]);
imwrite(uint8(out_fp_im),['./results/est_fp_' file_name]);
imwrite(uint8(out_admm_im),['./results/est_admm_' file_name]);
imwrite(uint8(out_sd_im),['./results/est_sd_' file_name]);

fprintf(' Done.\n');


function mfunc = minmodB(v,M,h)

% function mfunc = minmodB(v,M,h)
% Purpose: Implement the TVB modified midmod function. v is a vector

mfunc = v(1,:);
ids = find(abs(mfunc) > M*h.^2);

if(size(ids,2)>0)
  mfunc(ids) = minmod(v(:,ids));
end
return


y       = load('data/gas.dat')';
time    = 1960:1/4:1986+3/4;
fprintf(1, '\n');

%% Analysis based on Gaussian model %%
stsm        = ssm_stsm('trend', 'dummy', 4);
[stsm logL] = estimate(y, stsm, 0.01, [], 'fmin', 'bfgs', 'disp', 'off');
fprintf(1, '[Gaussian STSM]\n');
fprintf(1, 'loglikelihood:      %g\n', logL);
fprintf(1, 'Irregular variance: %g\n', stsm.param(1));
fprintf(1, 'Trend variance:     %g\n', stsm.param(2));
fprintf(1, 'Level variance:     %g\n', stsm.param(3));
fprintf(1, 'Seasonal variance:  %g\n\n', stsm.param(4));

[alpha irr] = fastsmo(y, stsm);
ycom        = signal(alpha, stsm);
seas        = ycom(2, :);
figure('Name', 'Gaussian model analysis');
subplot(2, 1, 1), plot(time, seas), title('Seasonal'), xlim([1959 1988]), ylim([-0.9 0.8]);
subplot(2, 1, 2), plot(time, irr), title('Gaussian Irregular'), xlim([1959 1988]), ylim([-0.4 0.4]);
if ispc, set(gcf, 'WindowStyle', 'docked'); end
drawnow;

%% Analysis based on t-model %%
stsmt           = [ssm_t ssm_llt ssm_seasonal('dummy', 4)];
randn('state', [3765023265; 2369472656]);
[stsmt logLt]   = estimate(y, stsmt, [stsm.param(1) 4 stsm.param(2:end)], alpha, 'fmin', 'bfgs', 'disp', 'off');
[alphat irrt]   = fastsmo(y, stsmt);
ycomt           = signal(alphat, stsmt);
lvlt            = ycomt(1, :);
seast           = ycomt(2, :);

fprintf(1, '[t-dist error STSM]\n');
fprintf(1, 'loglikelihood:      %g\n', logLt);
fprintf(1, 't-dist variance:    %g\n', stsmt.param(1));
fprintf(1, 't-dist df:          %g\n', stsmt.param(2));
fprintf(1, 'Trend variance:     %g\n', stsmt.param(3));
fprintf(1, 'Level variance:     %g\n', stsmt.param(4));
fprintf(1, 'Seasonal variance:  %g\n', stsmt.param(5));

figure('Name', 'Component comparison between Gaussian and t-dist. models');
subplot(2, 1, 1), plot(time, seas, 'r', 'DisplayName', 'Gaussian'), hold all, plot(time, seast, 'b', 'DisplayName', 't-distribution'), hold off, title('Seasonal component'), xlim([1959 1988]), ylim([-0.9 0.8]);
subplot(2, 1, 2), plot(time, irr, 'r', 'DisplayName', 'Gaussian'), hold all, plot(time, irrt, 'b', 'DisplayName', 't-distribution'), hold off, title('Irregular component'), xlim([1959 1988]), ylim([-0.4 0.4]);
if ispc, set(gcf, 'WindowStyle', 'docked'); end

fprintf(1, '\n');


% Simulating 2*2 MIMO-LDPC Base-Band Systems
% Copyright (C2010-2015) Yang XIAO, Beijing Jiaotong University, Aug.10, 2010, E-Mail: yxiao@bjtu.edu.cn.
% This program can simulate 2*2 MIMO-LDPC base-band systems.
% The MIMO system's design can refer my following book.
% [1] Yang Xiao, MIMO Multiple Antenna Wireless Communication Systems, Press of Posts and Telecommunications, Beijing, 2009.
% Different from the MIMO scheme of IEEE 802.16e, our MIMO-LDPC system has no space-time coding,
% while it achieved good BER performance.
%---------------------------
clear;
p=87;   %p is the prime number of rank of sub-matrices of parity check matrix;
k2=3;   % k is the row weight of parity check matrix;
j2=6;   % j is the column weight of parity check matrix;
M=k2*p; % M is the number of row of parity check matrix;
N=j2*p;  % N is the number of column of parity check matrix;

NT1=1;
E0=eye(p);
EZ=zeros(p);
a=3; b=5;
R=0.5; % coding rate
frame_num =200;
Npf=2*N*frame_num
dlta=1/Npf;

EZ=zeros(p);
E0=eye(p);
% The design of parity check matrix of QC LDPC without girth_4, see my book.
E11=E0;                   E12=circshift(E0,a*NT1);   E13= circshift(E0,a^2*NT1);   E14= circshift(E0,a^3*NT1);   E15= circshift(E0,a^4*NT1);    E16= circshift(E0,a^5*NT1);
E21=EZ;                   E22=E0;                    E23=circshift(E0,a^2*b*NT1);  E24=circshift(E0,a^3*b*NT1);  E25= circshift(E0,a^4*b*NT1);  E26= circshift(E0,a^5*b*NT1);
E31=circshift(E0,b^2*NT1);E32=EZ;                    E33=E0;                       E34=circshift(E0,b^2*a^3*NT1);E35= circshift(E0,b^2*a^4*NT1);E36= circshift(E0,b^2*a^5*NT1);

h1=[E11 E12 E13 E14 E15 E16;
    E21 E22 E23 E24 E25 E26;
    E31 E32 E33 E34 E35 E36];
 A(:,1:M)=h1(:,1:M);
 B(:,1:M)=h1(:,M+1:N);
%

d=mod(inv_GF2(A)*B,2);
E00=eye(M);
% The generator matrix of QC LDPC, see my book.
G=[d' E00];
H=sparse(h1);

N1=0;
N2=16;
% Flat fading MIMO Channel
h11=0.9;
h12=0.3;
h21=0.4;
h22=0.73;

SNRindB1=N1:1:N2;
for i=1:length(SNRindB1)
error_count1= 0;
error_count2= 0;
    for f_n = 1:frame_num
    SNR=(10^(SNRindB1(i)/10));
  	sigma = 1/sqrt(2*R*SNR);
    x1 = (sign(randn(1,size(G,1)))+1)/2; % random information bits
    x2 = (sign(randn(1,size(G,1)))+1)/2; % random information bits
 y1 = mod(x1*G,2);                   % LDPC Encoding for signal_1 from atenna 1 of the transmitter
 y2 = mod(x2*G,2);                   % LDPC Encoding for signal_2 from atenna 2 of the transmitter
    z1=2*y1-1;                       % BPSK modulation
    z2=2*y2-1;                         % BPSK modulation
    z11=h11*z1+h12*z2+sigma*randn(1,size(G,2));    % AWGN transmission
    z22=h21*z1+h22*z2+sigma*randn(1,size(G,2));
HC=[h11 h12;h21 h22];   % Channel Matrix
HC1=HC^(-1);
u=HC1*[z11;z22];        % ZF spatial decoding
u1=u(1,:);              % recieved signal_1 from atenna 1 of the receiver
u2=u(2,:);              % recieved signal_2 from atenna 1 of the receiver
    f11=1./(1+exp(-2*u1/sigma^2));        % likelihoods
    f01=1-f11;
    [z1_hat, success, k] = ldpc_decode(f01,f11,H);   %  LDPC decoding for signal_1
    x1_hat = z1_hat(size(G,2)+1-size(G,1):size(G,2));
    x1_hat1 = x1_hat';
    f12=1./(1+exp(-2*u2/sigma^2));        % likelihoods
    f02=1-f12;
    [z2_hat, success, k] = ldpc_decode(f02,f12,H);   %  LDPC decoding for signal_2
    x2_hat = z2_hat(size(G,2)+1-size(G,1):size(G,2));
    x2_hat1 = x2_hat';
    error_count2= sum(xor(x1,x1_hat1))+sum(xor(x2,x2_hat1));    % bit error count
    error_count1 = error_count1 + error_count2;
    end
    BER(i)= error_count1/Npf
    if BER(i)<.1*dlta
    BER(i)=.08*dlta;
    end
end
figure(1)
semilogy(SNRindB1,BER);
xlabel('SNR(dB)');
ylabel('BER');
title('2*2 MIMO-LDPC Simulation');
axis([N1 N2 dlta 1]);
%diary off


function x = chi2inv(p,nu)
% Inverse-chi-squared distribution
% x = Gamma(nu/2,1/(2*p)) / Gammma(nu/2)

x = gammainc(1/(2*p),nu/2) / gamma(nu/2);

end


function [x,fval,gp] = gplite_fmin(gp,x0,maxflag)
%GPLITE_FMIN Find global minimum (or maximum) of GP.

if nargin < 2; x0 = 0; end
if nargin < 3 || isempty(maxflag); maxflag = 0; end

MaxBnd = 10;
hpd_frac = 0.5;
D = size(gp.X,2);
N0 = size(x0,1);
Nstarts = max(3,N0);

diam = max(gp.X) - min(gp.X);
LB = min(gp.X) - MaxBnd*diam;
UB = max(gp.X) + MaxBnd*diam;

% First, train GP
if ~isfield(gp,'post') || isempty(gp.post)
    % How many samples for the GP?
    if isfield(gp,'Ns') && ~isempty(gp.Ns); Ns_gp = gp.Ns; else; Ns_gp = 0; end
    options.Nopts = 1;  % Do only one optimization
    gp = gplite_train(...
        [],Ns_gp,gp.X,gp.y,gp.covfun,gp.meanfun,gp.noisefun,[],[],options);
end

% Start from the min (or max) of the training data
if maxflag
    [~,ord] = sort(gp.y,'descend');
else
    [~,ord] = sort(gp.y,'ascend');
end

% Take best for sure
X = gp.X(ord,:);
x0 = [x0; X(1,:)];
X(1,:) = [];

if Nstarts > N0+1
    Nx = size(X,1);
    N_hpd = ceil(Nx*hpd_frac);
    idx = randperm(N_hpd,min(Nstarts-N0,N_hpd));
    x0 = [x0; X(idx,:)];
end

N0 = size(x0,1);
x = zeros(N0,D);
f = zeros(N0,1);
opts = optimoptions('fmincon','GradObj','off','Display','off');
for i = 1:N0
    [x(i,:),f(i)] = fmincon(@(x) optfun(x,gp,maxflag),x0(i,:),[],[],[],[],LB,UB,[],opts);
end

[fval,idx] = min(f);
x = x(idx,:);

if maxflag; fval = -fval; end

end

function [f,df] = optfun(x,gp,maxflag)

if nargout > 1
    [f,df] = gplite_pred(gp,x);
else
    f = gplite_pred(gp,x);
end

if maxflag  % Want to find maximum, swap sign
    f = -f;
    if nargout > 1; df = -df; end
end

end


function [A, B] = gsp_filterbank_bounds(G,W,param)
%GSP_FILTERBANK_BOUNDS Compute approximated frame bounds for a filterbank
%   Usage: [A, B] = gsp_filterbank_bounds(G,W);
%          [A, B] = gsp_filterbank_bounds(G,W,param);
%          [A, B] = gsp_filterbank_bounds([xmin, xmax],W);
%          [A, B] = gsp_filterbank_bounds([xmin, xmax],W,param);
%
%   Input parameters
%       G   : Graph structure or interval to compute the bound
%       W   : Filterbank (cell array of inline function)
%       param: optional parameter
%   Output parameters
%       A   : Filterbank lower bound
%       B   : Filterbank Upper bound
%
%   *param* is a Matlab structure containing the following fields:
%
%   * *param.N* : Number of points for the line search default (default 999)
%   * *param.use_eigenvalues* : Use eigenvalues if possible (default 1). To
%     be used, the eigenvalues have to be computed first using
%     |gsp_compute_fourier_basis|.
%

% Author: Nathanael Perraudin
% Date : 26 March 2014

if nargin < 3
    param = struct;
end

if ~isfield(param,'N'), param.N = 999; end
if ~isfield(param,'use_eigenvalues'),  param.use_eigenvalues = 1; end

if iscell(G)
    NG = numel(G);
    A = cell(NG,1);
    B = cell(NG,1);
    for ii = 1:NG
        [A{ii}, B{ii}] = gsp_filterbank_bounds(G{ii},W{ii},param);
    end
    return
end

if isstruct(G)
    if ~isfield(G,'lmax')
        G = gsp_estimate_lmax(G);
    warning(['GSP_FILTERBANK_BOUNDS: To be more efficient you should run: ',...
        'G = gsp_estimate_lmax(G); before using this proximal operator.']);
    end
    xmax = G.lmax;
    xmin = 0;
else
    xmin = G(1);
    xmax = G(2);
end

if param.use_eigenvalues && isstruct(G) && isfield(G,'e')
    lambda = G.e;
else
    lambda = linspace(xmin,xmax , param.N);
end

Nf = numel(W);

sum_filters = sum(abs(gsp_filter_evaluate(W,lambda).^2),2);
% for ii=1:Nf
%     sum_filters = sum_filters + (W{ii}(lambda)).^2;
% end

A = min(sum_filters);
B = max(sum_filters);

end


function k = linardKernDiagCompute(kern, x)

% LINARDKERNDIAGCOMPUTE Compute diagonal of LINARD kernel.
% FORMAT
% DESC computes the diagonal of the kernel matrix for the automatic relevance determination linear kernel given a design matrix of inputs.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : a vector containing the diagonal of the kernel matrix
% computed at the given points.
%
% SEEALSO : linardKernParamInit, kernDiagCompute, kernCreate, linardKernCompute
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% KERN

scales = sparse(diag(sqrt(kern.inputScales)));
x = x*scales;

k = sum(x.*x, 2)*kern.variance;


%INTLAB interval hessian toolbox
%
%Hessian constructors
%  hessianinit  - Initialization of dependent variables
%  horzcat      - Horizontal concatenation          [ , ]
%  vertcat      - Vertical concatenation            [ ; ]
%  subsasgn     - Subscripted assignment A(i,:) = 1
%  subsref      - Subscripted reference r = A(3,4)
%  hessian      - Double to gradient
%
%Display of hessians and interval hessians (rigorous)
%  display      - Command window display of hessian
%  disp         - Display function for pop-up windows in debugger
%  realimag     - Real and Imaginary part separately
%  infsup       - Display infimum and supremum
%  midrad       - Display midpoint and radius
%  disp_        - Display in "_" notation
%
%Hessian arithmetic operations
%  plus         - Plus                              +
%  uplus        - Unary plus                        +
%  minus        - Minus                             -
%  uminus       - Unary minus                       -
%  mtimes       - Matrix multiply                   *
%  times        - Elementwise multiply              .*
%  mrdivide     - Slash or right division           /
%  mldivide     - Backslash or left division        \
%  rdivide      - Elementwise right division        ./
%  ldivide      - Elementwise left division         .\
%  mpower       - Matrix power                      ^
%  power        - Elementwise power                 .^
%  intersect    - Intersection
%
%Other hessian operations
%  abs          - Absolute value
%  inf          - Infimum
%  sup          - Supremum
%  mid          - Midpoint
%  rad          - Radius
%  diam         - Diameter
%  real         - Real part
%  imag         - Imaginary part
%  trace        - Trace
%  sum          - Sum
%  prod         - Product
%  ctranspose   - Complex conjugate transpose       '
%  transpose    - Transpose                         .'
%
%Utility routines
%  isnan        - True for Not a Number
%  isreal       - Hessian is real
%  isintval     - Hessian is intval
%  isfinite     - Interval is finite
%  isinf        - Interval is infinite
%  isempty      - Hessian is empty in Matlab sense, i.e. []
%  emptyintersect - detect empty intersections
%  issparse     - Hessian has sparse structure
%  find         - find indices of nonzero elements
%  all          - Determine if all array elements are nonzero
%  any          - Determine if any array elements are nonzero
%  logical      - Convert hessian values to logical
%  nnz          - number of nonzero elements
%  end          - determine last index
%
%Structural operations
%  full         - Convert to full hessian
%  sparse       - Convert to sparse hessian
%  band         - Extract band
%  diag         - Extract diagonal
%  tril         - Extract lower triangular
%  triu         - Extract upper triangular
%  bandwidth    - Bandwidth
%  length       - Length
%  size         - Size
%  dim          - Dimension of square matrix
%  reshape      - Reshape
%  repmat       - Duplicate arrays
%
%Hessian trigonometric functions (rigorous, real and complex)
%  sin          - Sine
%  cos          - Cosine
%  tan          - Tangent
%  cot          - Cotangent
%  sec          - Secant
%  csc          - Cosecant
%  asin         - Inverse sine
%  acos         - Inverse cosine
%  atan         - Inverse tangent
%  acot         - Inverse cotangent
%  asec         - Inverse secant
%  acsc         - Inverse cosecant
%  sinh         - Hyperbolic sine
%  cosh         - Hyperbolic cosine
%  tanh         - Hyperbolic tangent
%  coth         - Hyperbolic cotangent
%  asinh        - Inverse hyperbolic sine
%  acosh        - Inverse hyperbolic cosine
%  atanh        - Inverse hyperbolic tangent
%  acoth        - Inverse hyperbolic cotangent
%
%Hessian exponential functions (rigorous, real and complex)
%  exp          - Exponential
%  log          - Natural logarithm
%  log10        - Logarithm to base 10
%  sqr          - Square
%  sqrt         - Square root
%
%Hessian comparison of ".x" part
%  eq           - Equal                             ==
%  ne           - Not equal                         ~=
%  gt           - Greater than                      >
%  ge           - Greater than or equal             >=
%  lt           - Less than                         <
%  le           - Less than or equal                <=
%
%Verification routines and auxiliary
%  typeadj      - Type adjustment
%  typeof       - Type for type adjustment
%
%Initialization of INTLAB hessian package and system variables
%  hessianinit  - Initialization and definition of INTLAB switches, also
%                   initialization of dependent variables
%
%Demonstration, samples
%  demohessian - Some examples for using INTLAB hessian package
%
%
%The hessian package uses forward mode of automatic differentiation. For
%an introduction to forward differentation, see
%  L.B. Rall: Automatic Differentiation: Techniques and Applications,
%    Lecture Notes in Computer Science 120, Springer, 1981.
%

% written  04/04/04     S.M. Rump  INTLAB Version 5
%
% Copyright (c) Siegfried M. Rump, head of the Institute for Reliable Computing,
%               Hamburg University of Technology


function costs = surfprofile(problem, x, d1, d2, t1, t2)
% Plot the cost function as a surface over a 2-dimensional subspace.
%
% function surfprofile(problem, x, d1, d2, t1, t2)
% function costs = surfprofile(problem, x, d1, d2, t1, t2)
%
% Evaluates the cost function at points
%
%   gamma(t1, t2) = exponential_x(t1*d1 + t2*d2)
%
% where the exponential map at x is specified by problem.M.exp (retr is
% used instead if needed). d1 and d2 are two tangent vectors to problem.M
% at the point x. The values assigned to t1 and t2 are as specified in the
% two input vectors t1 and t2.
%
% If the function is called with an output, the plot is not drawn and the
% values of the cost are returned in a matrix of size
% length(t1)*length(t2). To plot a surf, call surf(t1, t2, costs.') (notice
% the transpose).

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Sep. 1, 2014.
% Contributors:
% Change log:
%
%   April 3, 2015 (NB):
%       Works with the new StoreDB class system.

    % Verify that the problem description is sufficient.
    if ~canGetCost(problem)
        error('It seems no cost was provided.');
    end

    if isfield(problem.M, 'exp')
        expo = problem.M.exp;
        str = 'Exp';
    else
        expo = problem.M.retr;
        str = 'Retr';
    end

    storedb = StoreDB();
    linesearch_fun = @(ta, tb) getCost(problem, ...
                         expo(x, problem.M.lincomb(x, ta, d1, tb, d2)), ...
                         storedb);

    costs = zeros(length(t1), length(t2));
    for i = 1 : length(t1)
        for j = 1 : length(t2)
            costs(i, j) = linesearch_fun(t1(i), t2(j));
        end
    end

    if nargout == 0
        surf(t1, t2, costs.');
        xlabel('t1');
        ylabel('t2');
        zlabel(['f(' str '_x(t1*d1+t2*d2))']);
    end

end


% Fig. 5.8   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami

n6=1;
d6=[1 8 32 0];
pzmap(n6,d6)
title('Fig.5.8 The locus crosses the j\omega axis at *')
axis([-10 6 -6 6] )
hold on
r=roots([1 0 32]);
 plot(r,'*')
z=0:.1:.9;
 wn= 1:6;
 sgrid(z, wn)
 hold off


function sp=mtimes(sp1,sp2)
% sympoly/mtimes: Matrix multiplication of sympoly objects or scalars
% usage: sp=sp1*sp2;
%
% arguments:
%  sp,sp1,sp2   - sympoly objects or numeric scalars or a numeric array

% are they of compatible sizes?
s1 = size(sp1);
s2 = size(sp2);
if (length(s1)>2) || (length(s2)>2)
  error 'Matrix multiplication is only defined for vectors and simple arrays.'
end

if (s1(2) == s2(1)) || (numel(sp1)==1) || (numel(sp2)==1)
  % they are compatible

  if (numel(sp1) == 1) && (numel(sp2) == 1)
    % both are scalars. Just use .*
    sp = sp1.*sp2;

  elseif (numel(sp1) == 1) && (numel(sp2) > 1)
    % sp1 is a scalar, but not sp2
    sp = sympoly(sp2);
    for i = 1:numel(sp2)
      sp(i) = sp1.*sp2(i);
    end

  elseif (numel(sp1) > 1) && (numel(sp2) == 1)
    % sp2 is a scalar, but not sp1
    sp = sympoly(sp1);
    for i = 1:numel(sp1)
      sp(i) = sp1(i).*sp2;
    end

  elseif (numel(sp1) > 1) && (numel(sp2) > 1)
    % both are arrays.
    sp = sympoly(zeros(s1(1),s2(2)));

    for i = 1:s1(1)
      for j = 1:s2(2)
        for k = 1:s1(2)
          sp(i,j) = sp(i,j) + sp1(i,k).*sp2(k,j);
        end
      end
    end

  end

else
  error 'sp1 and sp2 are of incompatible sizes for .* operation.'

end


function T = multiply_pots(T1, T2)
% MULTIPLY_POTS Multiply a pair of dpots together pointwise.
% T = multiply_pots(pots)

dom = myunion(T1.domain, T2.domain);
%ns = sparse(1, max(dom)); % causes problems in myreshape on NT
ns = zeros(1, max(dom));
ns(T1.domain) = T1.sizes;
ns(T2.domain) = T2.sizes;
T = dpot(dom, ns(dom));
T = multiply_by_pot(T, T1);
T = multiply_by_pot(T, T2);


function [E,J]=SynthMeasWatsonSHCylSingleRadTortGPD(x, protocol, fibredir, roots)
% Substrate: Impermeable cylinders with one radius in a homogeneous background.
% Orientation distribution: Watson's distribution with SH approximation
% Pulse sequence: Any
% Signal approximation: Gaussian phase distribution.
% Notes: This version estimates the hindered diffusivity from the free diffusivity
% and packing density using Szafer et al's tortuosity model for randomly
% packed cylinders.
%
% [E,J]=SynthMeasWatsonSHCylSingleRadTortGPD(x, protocol, fibredir, roots)
% returns the measurements E according to the model and the Jacobian J of the
% measurements with respect to the parameters.  The Jacobian does not
% include derivates with respect to the fibre direction.
%
% x is the list of model parameters in SI units:
% x(1) is the volume fraction of the intracellular space.
% x(2) is the free diffusivity of the material inside and outside the cylinders.
% x(3) is the radius of the cylinders.
% x(4) is the concentration parameter of the Watson's distribution.
%
% protocol is the object containing the acquisition protocol.
%
% fibredir is a unit vector along the symmetry axis of the Watson's
% distribution.  It must be in Cartesian coordinates [x y z]' with size [3 1].
%
% roots contains solutions to the Bessel function equation from function
% BesselJ_RootsCyl.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

f=x(1);
dPar=x(2);
% This version is for cylinders with regular packing.
%dPerp = dPar/((1 + f^(3/2))^2);
% This one is for randomly packed cylinders
dPerp = dPar*(1-f);
R=[x(3)];
kappa=x(4);

x_full = [f dPar dPerp R kappa];

% Call the model with no isotropic component to get the anisotropic component.
if(nargout == 1)
    E=SynthMeasWatsonSHCylSingleRadGPD(x_full, protocol, fibredir, roots);
else
    [E,J_full]=SynthMeasWatsonSHCylSingleRadGPD(x_full, protocol, fibredir, roots);
    J(:,1) = J_full(:,1) - J_full(:,3)*dPar;
    J(:,2) = J_full(:,2) + J_full(:,3)*(1-f);
    J(:,3) = J_full(:,4);
    J(:,4) = J_full(:,5);
end


function value = dmach ( job )

%*****************************************************************************80
%
%% DMACH computes machine parameters of floating point arithmetic.
%
%  Discussion:
%
%    This routine is for testing only.  It is not required by LINPACK.
%
%    If there is trouble with the automatic computation of these quantities,
%    they can be set by direct assignment statements.
%    We assume the computer has
%
%      B = base of arithmetic
%      T = number of base B digits
%      L = smallest possible exponent
%      u = largest possible exponent
%
%    then
%
%      EPS = B**(1-T)
%      TINY = 100.0D+00 *B**(-L+T)
%      HUGE = 0.01D+00 *B**(U-T)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer JOB:
%    1: requests EPSILON;
%    2: requests TINY;
%    3: requests HUGE.
%
%    Output, real VALUE, the requested value.
%
  s = 1.0;

  while ( 1 )

    tiny = s;
    s = s / 2.0;

    if ( s * 1.0 == 0.0 | s == 0.0 )
      break
    end

  end

  tiny = ( tiny / eps ) * 100.0;
  huge = 1.0 / tiny;

  if ( job == 1 )
    value = eps;
  elseif ( job == 2 )
    value = tiny;
  elseif ( job == 3 )
    value = huge;
  else
    xerbla ( 'DMACH', 1 );
  end

  return
end


function [kip] = ozf2kip(ozf)
% Convert force from ounces force to kip.
% Chad A. Greene 2012
kip = ozf*0.0000625;


function h = int_hist(x, n)
% INT_HIST(x, n) is a histogram of all integer values 1..n in x.
% If n is not given, max(x) is used.

% Hans Olsson's one-liner from matlab faq
h = full(sum(sparse(1:length(x(:)),x(:),1)));
if nargin == 2
  if n > length(h)
    % pad with zeros
    h = [h zeros(1,n-length(h))];
  end
end


function W_down = get_W_down(W)
N = size(W, 2);
W_down = zeros(2, 2 * N^2);
for u2 = 0 : 1
    for y1 = 1 : N
        for y2 = 1 : N
            for u1 = 0 : 1
%                 P = 0.5 * W(mod(u1 + u2, 2) + 1, y1) * W(u2 + 1, y2);
%                 if P == 0
%                     W_down(u2 + 1, 2 * N * (y1 - 1) + 2 * (y2 - 1) + u1 + 1) = realmin;
%                 else
%                     W_down(u2 + 1, 2 * N * (y1 - 1) + 2 * (y2 - 1) + u1 + 1) = P;
%                 end
                W_down(u2 + 1, 2 * N * (y1 - 1) + 2 * (y2 - 1) + u1 + 1) = 0.5 * W(mod(u1 + u2, 2) + 1, y1) * W(u2 + 1, y2);
            end
        end
    end
end
end


function []=PS_calc_ifg_std
% PS_CALC_IFG_STD() calculate std for each ifg
%
%   Andy Hooper, June 2006
%
%   ======================================================
%   09/2006 AH: small baselines added
%   02/2010 AH: More informative info displayed
%   ======================================================

fprintf('\nEstimating noise standard deviation (degrees)...\n')

small_baseline_flag=getparm('small_baseline_flag');

load psver
psname=['ps',num2str(psver)];
phname=['ph',num2str(psver)];
pmname=['pm',num2str(psver)];
bpname=['bp',num2str(psver)];
ifgstdname=['ifgstd',num2str(psver)];

ps=load(psname);
pm=load(pmname);
bp=load(bpname);

if exist([phname,'.mat'],'file')
    phin=load(phname);
    ph=phin.ph;
    clear phin
else
    ph=ps.ph;
end

n_ps=length(ps.xy);
master_ix=sum(ps.master_day>ps.day)+1;

if strcmpi(small_baseline_flag,'y')
    ph_diff=angle(ph.*conj(pm.ph_patch).*exp(-j*(repmat(pm.K_ps,1,ps.n_ifg).*bp.bperp_mat)));
else
    bperp_mat=[bp.bperp_mat(:,1:ps.master_ix-1),zeros(ps.n_ps,1,'single'),bp.bperp_mat(:,ps.master_ix:end)];
    ph_patch=[pm.ph_patch(:,1:master_ix-1),ones(n_ps,1),pm.ph_patch(:,master_ix:end)];
    ph_diff=angle(ph.*conj(ph_patch).*exp(-j*(repmat(pm.K_ps,1,ps.n_ifg).*bperp_mat+repmat(pm.C_ps,1,ps.n_ifg))));
end

ifg_std=[sqrt(sum(ph_diff.^2)/n_ps)*180/pi]';
if strcmpi(small_baseline_flag,'y')
  for i=1:ps.n_ifg
    fprintf('%3d %s_%s %3.2f\n',i,datestr(ps.ifgday(i,1)),datestr(ps.ifgday(i,2)),ifg_std(i))
  end
else
  for i=1:ps.n_ifg
    fprintf('%3d %s %3.2f\n',i,datestr(ps.day(i)),ifg_std(i))
  end
end
fprintf('\n')

save(ifgstdname,'ifg_std');


function legendre_polynomial_test ( )

%*****************************************************************************80
%
%% LEGENDRE_POLYNOMIAL_TEST tests the LEGENDRE_POLYNOMIAL library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 October 2014
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_POLYNOMIAL_TEST:\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Test the LEGENDRE_POLYNOMIAL library.\n' );

  legendre_polynomial_test01 ( );
  legendre_polynomial_test015 ( );
  legendre_polynomial_test016 ( );
  legendre_polynomial_test02 ( );
  legendre_polynomial_test03 ( );
  legendre_polynomial_test04 ( );

  p = 5;
  b = 0.0;
  legendre_polynomial_test05 ( p, b );

  p = 5;
  b = 1.0;
  legendre_polynomial_test05 ( p, b );

  p = 5;
  e = 0;
  legendre_polynomial_test06 ( p, e );

  p = 5;
  e = 1;
  legendre_polynomial_test06 ( p, e );

  legendre_polynomial_test07 ( );
  legendre_polynomial_test08 ( );
  legendre_polynomial_test09 ( );
  legendre_polynomial_test095 ( );

  p = 5;
  legendre_polynomial_test10 ( p );

  legendre_polynomial_plot01 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_POLYNOMIAL_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function [transprob, termprob] = remove_hhmm_end_state(A)
% REMOVE_END_STATE Infer transition and termination probabilities from automaton with an end state
% [transprob, termprob] = remove_end_state(A)
%
% A(i,k,j) = Pr( i->j | Qps=k), where i in 1:Q, j in 1:(Q+1), and Q+1 is the end state
% This implements the equation in footnote 3 of my NIPS 01 paper,
% transprob(i,k,j) = \tilde{A}_k(i,j)
% termprob(k,j) = \tau_k(j)
%
% For the top level, the k index is missing.

Q = size(A,1);
toplevel = (ndims(A)==2);
if toplevel
  Qk = 1;
  A = reshape(A, [Q 1 Q+1]);
else
  Qk = size(A, 2);
end

transprob = A(:, :, 1:Q);
term = A(:,:,Q+1)'; % term(k,j) = P(Qj -> end | k)
termprob = term;
%termprob = zeros(Qk, Q, 2);
%termprob(:,:,2) = term;
%termprob(:,:,1) = 1-term;

for k=1:Qk
  for i=1:Q
    for j=1:Q
      denom = (1-termprob(k,i));
      denom = denom + (denom==0)*eps;
      transprob(i,k,j) = transprob(i,k,j) / denom;
    end
  end
end

if toplevel
  termprob = squeeze(termprob);
  transprob = squeeze(transprob);
end


% test for Fethalah code and Prados code
n = 500;

rep = 'results/anisotropic-feth/';
if not(exist(rep))
    mkdir(rep);
end

%% compute random tensor field
randn('seed', 12345);
U = randn(n,n,2);
sigma = (n/200)*30;
options.bound = 'per';
for it=1:10
    U = perform_vf_normalization( perform_blurring(U, sigma,options) );
end
U = perform_vf_normalization( U );

aniso_list = [1 .5 .2 .1 .05 .01 .001];

p = 5;
x = round( n/(p*2):n/p:n-n/(p*2) );
[Y,X] = meshgrid(x,x);
% start_points = round( start_points*(n-1)+1 );
start_points = cat(1, X(:)', Y(:)');

s = randperm(size(start_points,2));
for i=1:length(aniso_list)

    aniso = aniso_list(i);
    V = cat(3, -U(:,:,2), U(:,:,1)); % orthogonal vector
    T = perform_tensor_recomp(U,V, ones(n),ones(n)*aniso );
    Ti = perform_tensor_recomp(U,V, ones(n),ones(n)*1/aniso );

    options.use_feth_code = 0;
    tic;
    [D,S,Q] = perform_fast_marching(T, start_points, options);
    disp(['Prados: ' num2str(toc)]);

    hx = 1/n; hy = 1/n;
    tic;
    [D1,dUx,dUy, Vor, L] = fm2dAniso([hx;hy], Ti, start_points);
    disp(['Feth: ' num2str(toc)]);

    imageplot(s(Q), 'Prados', 1,2,1);
    imageplot(s(Vor+1), 'Feth', 1,2,2);
    colormap jet(256);
    saveas(gcf, [rep 'voronoi-' num2str(aniso) '.png'], 'png');

    clf;
    plot_tensor_field(T, perform_histogram_equalization(D, 'linear'));
    colormap jet(256);
    saveas(gcf, [rep 'distance-aniso-' num2str(aniso) '-prados.png'], 'png');

    clf;
    plot_tensor_field(T, perform_histogram_equalization(D1, 'linear'));
    colormap jet(256);
    saveas(gcf, [rep 'distance-' num2str(aniso) '-feth.png'], 'png');

end


function res = GRAPPA(kData,kCalib,kSize,lambda, dispp)
% res = GRAPPA(kData,kCalib,kSize,lambda [, disp)
%
% This is a GRAPPA reconstruction algorithm that supports
% arbitrary Cartesian sampling. However, the implementation
% is highly inefficient in Matlab because it uses for loops.
% This implementation is very similar to the GE ARC implementation.
%
% The reconstruction looks at a neighborhood of a point and
% does a calibration according to the neighborhood to synthesize
% the missing point. This is a k-space varying interpolation.
% A sampling configuration is stored in a list, and retrieved
% when needed to accelerate the reconstruction (a bit)
%
% Inputs:
%       kData     -   [Size x, Size y, num coils] 2D multi-coil k-space data to reconstruct from.
%                   Make sure that the missing entries have exact zeros in them.
%       kCalib    -   calibration data (fully sampled k-space)
%       kSize     - size of the 2D GRAPPA kernel [kx, ky]
%       lambda    - Tykhonov regularization for the kernel calibration.
%       dispp      - Figure number to display images as they are
%                   reconstructed
% Outputs:
%       res       - k-space data where missing entries have been filled in.
%
% Example:
%   [x,y] = meshgrid(linspace(0,1,128));
%   % Generate fake Sensitivity maps
%   sMaps = cat(3,x.^2,1-x.^2,y.^2,1-y.^2);
%   % generate 4 coil phantom
%   imgs = repmat(phantom(128),[1,1,4]).*sMaps;
%   DATA = fft2c(imgs);
%   % crop 20x20 window from the center of k-space for calibration
%   kCalib = crop(DATA,[20,20,4]);
%
%   %calibrate a kernel
%   kSize = [5,5];
%   coils = 4;
%
%   % undersample by a factor of 2
%   DATA(1:2:end,2:2:end,:) = 0;
%   DATA(2:2:end,1:2:end,:) = 0;
%
%   %reconstruct:
%   [res] = GRAPPA(DATA,kCalib, kSize, 0.01);
%   figure, imshow(cat(2,sos(imgs), 2*sos(ifft2c(DATA)), sos(ifft2c(res))),[]);
%   title('full,  zero-fill,   result')
%
%
%
% (c) Michael Lustig 2008

if nargin < 5
    dispp = 0;
end

pe = size(kData,2); fe = size(kData,1); coils = size(kData,3); % get sizes

res = kData*0;
%[AtA] = corrMatrix(kCalib,kSize); % build coil correlation matrix
AtA = dat2AtA(kCalib, kSize); % build coil calibrating matrix

for n=1:coils
	disp(sprintf('reconstructing coil %d',n));
	res(:,:,n) = ARC(kData, AtA,kSize, n,lambda); % reconstruct single coil image
    if dispp ~=0
        figure(dispp), imshow3(abs(ifft2c(res)),[]); drawnow
    end

end

function [res] = ARC(kData, AtA, kSize, c,lambda);
[sx,sy,nCoil] = size(kData);

kData = zpad(kData,[sx+kSize(1)-1, sy+kSize(2)-1,nCoil]);

dummyK = zeros(kSize(1),kSize(2),nCoil); dummyK((end+1)/2,(end+1)/2,c) = 1;
idxy = find(dummyK);

res = zeros(sx,sy);

MaxListLen = 100;
LIST = zeros(kSize(1)*kSize(2)*nCoil,MaxListLen);
KEY =  zeros(kSize(1)*kSize(2)*nCoil,MaxListLen);
count = 0;

%H = waitbar(0);
for y = 1:sy
	for x=1:sx
	%	waitbar((x + (y-1)*sx)/sx/sy,H);
		tmp = kData(x:x+kSize(1)-1,y:y+kSize(2)-1,:);
		pat = abs(tmp)>0;
		if pat(idxy) | sum(pat)==0
			res(x,y) = tmp(idxy);
		else
			key = pat(:);
            idx = 0;
			for nn=1:size(KEY,2);
				if sum(key==KEY(:,nn))==length(key)
				   idx = nn;
				   break;
			   	end
			end
			if idx == 0
				count = count + 1;
				kernel = calibrate(AtA,kSize,nCoil,c,lambda,pat);
				KEY(:,mod(count,MaxListLen)+1) = key(:);
				LIST(:,mod(count,MaxListLen)+1) = kernel(:);
				%disp('add another key');size(KEY,2)
			else
				kernel = LIST(:,idx);
			end
			res(x,y) = sum(kernel(:).*tmp(:));
		end

	end
end

%close(H);


clear,clc

addpath(pwd);
cd cvx;
addpath(genpath(pwd));
cd ..;

load('Ns=3.mat');

Ns = 3;

NRF = 3;

SNR_dB = -35:5:5;
SNR = 10.^(SNR_dB./10);
realization = size(H,3);
smax = length(SNR);% enable the parallel

parfor reali = 1:realization
    [ FRF, FBB ] = SDR_AltMin( Fopt(:,:,reali), NRF);
    [ WRF, WBB ] = Receiver( Wopt(:,:,reali), NRF);
    for s = 1:smax
        R(s,reali) = log2(det(eye(Ns) + SNR(s)/Ns * pinv(WRF * WBB) * H(:,:,reali) * FRF * FBB * FBB' * FRF' * H(:,:,reali)' * WRF * WBB));
    end
end
plot(SNR_dB,sum(R,2)/realization,'Marker','diamond','LineWidth',1.5,'Color',[0.87058824300766 0.490196079015732 0]);
grid on
hold on


function dXdt = dynamics_SRB(t,Xt,Ut,Xd,U_ext,p)

%% parameters
mass = p.mass;
J = p.J;       % inertia tensor in body frame {B}
g = 9.81;

%% decompose
% X = [pc dpc vR wb pf]'
pc = reshape(Xt(1:3),[3,1]);
dpc = reshape(Xt(4:6),[3,1]);
R = reshape(Xt(7:15),[3,3]);
wb = reshape(Xt(16:18),[3,1]);
pf34 = reshape(Xt(19:30),[3,4]);
pfd34 = reshape(Xd(19:30),[3,4]);

% r
r34 = pf34 - repmat(pc,[1,4]);

% GRF
f34 = reshape(Ut,[3,4]);

%% dynamics
ddpc = 1/mass * (sum(f34,2) + U_ext) + [0;0;-g];
dR = R * hatMap(wb);

tau_s = zeros(3,1);       % body torque expressed in {S}
for ii = 1:4
    tau_s = tau_s + hatMap(r34(:,ii)) * f34(:,ii);
end
tau_ext = hatMap(R * p.p_ext) * U_ext;
tau_tot = sum(tau_s,2) + tau_ext;
dwb = J \ (R' * tau_tot - hatMap(wb) * J * wb);

dpf = p.Kp_sw * (pfd34(:) - pf34(:));

dXdt = [dpc;ddpc;dR(:);dwb;dpf];

end


%{
helix_example.m
Example of how to use rebin_helix.m for helical cone-beam CT reconstruction.
Copyright 2010-7-21, Gregory Handy and Jeff Fessler, University of Michigan
%}

if 0 % test a particular geometry (case 2795)
	f.down = 4; % down sample a lot to save time
	cg = ct_geom('ge2', 'nt', 32, 'na', 3625, ...
...%		'dfs', inf, ...
		'dfs', 0, ...
		'down', f.down, ...
		'pitch', .53125, 'source_z0', -50.5494160, ...
		'orbit', 3625/984*360, ... % 1326.21951219512195,
		'orbit_start', 109.12139);

	ig = image_geom('nx', 320, 'ny', 320, 'nz', 3*64, ... % 61
		'down', f.down, ...
		'dx', 2.191162, 'dz', 0.625, 'offset_z', 56.0);
%	cg.plot(ig), return
end

if ~isvar('cg'), printm 'cg: cone-beam CT geometry'
	f.down = 4; % down sample a lot to save time
	f.nturn = 12;
	cg = ct_geom('ge2', 'pitch', 0.5, 'source_z0', -100, ...
		'na', 984*f.nturn, 'orbit', 360*f.nturn, 'orbit_start', 17, ...
		'down', f.down);
end

if ~isvar('ig'), printm 'ig: image geometry'
	ig = image_geom('nx', 256, 'ny', 256, 'nz', 160, ...
			'dx', 2, 'dz', 0.625, 'down', f.down);
	im clf, cg.plot(ig);
prompt
end

if ~isvar('ell'), printm 'ell: ellipsoid object'
	z0 = ig.offset_z * ig.dz;
	ell = [ ...
		[0 0 -z0	[0.3 0.1]*ig.fov 0.6*ig.zfov	0 0 1000];
		[80 10 -z0	30 30 10	0 0 1000];
		[-10 -40 75	40 40 40	0 0 1000];
		[-10 80 -20	30 30 30	0 0 1000];
		[0 0 -36	20 20 5	0 0 1000];
		[0 0 -12	20 20 5	0 0 1000];
		[0 0 12	20 20 5	0 0 1000];
		[0 0 36	20 20 5	0 0 1000];
	];
	clear z0
end

if ~isvar('xtrue'), printm 'xtrue: true image volume'
	xtrue = ellipsoid_im(ig, ell, 'oversample', 2);

	clim = [0 2000];
	im plc 2 3
%	im(1, ig.x, ig.y, xtrue, clim), cbar
	im(1, 'mid3', xtrue, clim), cbar
	titlef('x true, z=%g to %g', ig.z(1), ig.z(end))
prompt
end

if ~isvar('proj'), printm 'proj: analytical ellipsoid projection views'
	proj = ellipsoid_proj(cg, ell);

	im(4, 'row', 1, permute(proj, [1 3 2]), 'true helix sinograms'), cbar
prompt
end

if ~isvar('sino'), printm 'sino: rebin helix to fan-beam'
%	f.short = 1;
	f.short = 0; % for rebinned projection views below
	f.itype = 'linear';
%	f.itype = 'nearest';
	[sino, orbits, used] = rebin_helix(cg, ig, proj, ...
		'type', f.itype, 'short', f.short, 'collapse', 0);

	if exist('helix_rebin_mex') == 3 % matlab vs mex
		[sino0, orbits0, used] = rebin_helix(cg, ig, proj, ...
			'use_mex', 0, ...
			'type', f.itype, 'short', f.short, 'collapse', 0);

		equivs(orbits, orbits0)
		equivs(sino0, sino)
		clear sino0 orbits0
	end

	im(5, sino, 'SSRB sinos'), cbar, axis normal
	im(6, used), cbar, axis normal
prompt
end

%% examine the projection views
if false && ~f.short % not possible for short scans!
%	pview = permute(sino, [1 3 2]); % projection views
%	jf_slicer(pview) % awful, understandably because orbits vary!

	% rebin each fan-beam sinogram onto a common orbit_start parallel-beam
	sf = sino_geom('fan', 'dsd', cg.dsd, 'dso', cg.dso, ...
		'ns', cg.ns, 'ds', cg.ds, 'offset_s', cg.offset_s, ...
		'orbit', orbits(1,1), ...
		'na', size(sino, 2)); % # views in fan-beam sinogram, not helix cg.na
	sp = sino_geom('par', 'nb', cg.ns, 'dr', cg.ds/2, ...
		'orbit', 360, 'na', size(sino, 2)); % parallel-beam

	sino2 = zeros(size(sino));
	for iz = 1:ig.nz % each slice
		sf.orbit_start = orbits(iz,2); % sino_geom for this slice!
		sino2(:,:,iz) = rebin_sino(sino(:,:,iz), sf, sp);
	end
%	jf_slicer(sino2)

	pview = permute(sino2, [1 3 2]); % projection views
	jf_slicer(pview) % animate views
return
end

% helical cone-beam reconstruction based on SSRB
if ~isvar('xssrb'), printm 'fbp from ssrb'
	[xssrb, tmp] = fbp_helix_stack(cg, ig, sino, orbits, ...
		'short', f.short, 'window', 'hanning,1.0');
	xssrb = xssrb .* (ig.circ(cg.rmax) > 0); % mask

%	im(2, xssrb, 'SSRB recon', clim), cbar
	im(2, 'mid3', xssrb, 'SSRB recon', clim), cbar
	im(6, tmp, 'after weighting'), cbar, axis normal
	im(3, xssrb - xtrue, 'error', [-500 500]), cbar
prompt
end

if im % profile, for checking HU accuracy
	im subplot 6
	iz = 1:ig.nz;
	ix = ig.nx/2+1; iy = ig.ny/2+1;
	plot(	ig.z, squeeze(xtrue(ix,iy,iz)), 'o-', ...
		ig.z, squeeze(xssrb(ix,iy,iz)), '.--')
	axis([minmax(ig.z)' -200 2100])
	titlef('profile at (ix,iy)=(%g,%g)', ix,iy), xlabel z
end

if 0 % toggle
	fun = @(x) x;
	fun = @(x) jf_mip3(x, 'type', 'mid');
	figure(2)
	im_toggle(fun(xssrb .* ig.circ), fun(xtrue))
return
end

return % below here is comparisons with GH original method

if ~isvar('xssrb_gh'), printm 'fbp from ssrb gh'
	[xssrb_gh, sino_gh] = fbp_helix_gh(cg, ig, proj, 'short', f.short);
	im(3, xssrb_gh, 'SSRB GH recon', clim), cbar
	im(4, sino, 'SSRB GH sinos'), cbar, axis normal
prompt
end

if 0
	figure(2)
%	im_toggle(fun(xssrb .* ig.circ), fun(xtrue), fun(xssrb_gh .* ig.circ), clim)
	im_toggle(fun(xssrb .* ig.circ), fun(xssrb_gh .* ig.circ), clim)
end

if im % profile
	iz = 21;
	iz = 1:ig.nz;
	im(3, xssrb_gh(:,:,iz) - xtrue(:,:,iz), 'GH Error'), cbar

	im subplot 6
	ix = 1:ig.nx; iy = ceil(ig.ny/2); iz = ceil(ig.nz/2);
	iz = round(25/40 * ig.nz);
	plot(ix, xtrue(ix,iy,iz), '-', ix, xssrb_gh(ix,iy,iz), '--')
	axis([1 ig.nx -200 2100])
	titlef('slice %d', iz), xlabel 'ix'
end

% xfdk = easyhelix(cg, ig, li_hat, 'use_mex', has_mex_jf, 'w1cyl', 0);
% xfdk = myFeldkamp(cg, ig, li_hat, 'use_mex', 0);
% im(4, xfdk(:,:,25), 'FDK recon Other'), cbar


function offset = find_offset_multi_source(samples1, samples2)
    [c, lags] = xcorr(samples1, samples2);
    [~, ind] = max(c);
    offset = lags(ind) + 1; % add 1 for Matlab indexing
end


function cut = knapsack_create_cover_cut(a,b,x,alg,gubs)
% Derive cover inequalities cut*[1;x]>=0 for a*x <= b

if (all(a>=0) && sum(a)<=b) || (all(a<=0) && (b>=0))
    % quick exit sum x <= large or sum x >= neg
    cut = [];
    return
end

if nargin < 3
    x = ones(length(a),1);
    alg = 'crowder';
    gubs = [];
elseif nargin < 4
    alg = 'crowder';
    gubs = [];
elseif nargin < 5
    gubs = spalloc(1,length(a),0);
end

if ~all(a)
    nz = find(a);
    a_ = a(nz);
    cut_ = knapsack_create_cover_cut(a_,b,x(nz),alg,gubs(nz));
    if ~isempty(cut_)
        cut = spalloc(1,length(a)+1,0);
        cut(1) = cut_(1);
        cut(1+nz) = cut_(2:end);
    else
        cut = [];
    end
    return
elseif any(a<0)
    % Negative values in a*x <= b
    % models -a*x >= -b i.e. typically something like sum stuff >= bound
    %        -a*(1-y) >= -b
    %        -a*y <= b-sum(a)
    neg = find(a < 0);
    a_ = a;
    a_(neg) = -a(neg);
    x_ = x;
    x_(neg) = 1-x(neg);
    cut = knapsack_create_cover_cut(a_,b-sum(a(neg)),x_,alg,gubs);
    % we know have a cut, cut(1) + cut(2:end)*y >=0
    %                     cut(1) + cut(2:end)(1-x) >= 0
    %                     cut(1) + sum(cut(2:end)) + (-cut(2))*x >= 0
    if ~isempty(cut)
        cut(1) = cut(1)+sum(cut(1 + neg));
        cut(1 + neg) = -cut(1 + neg);
    end
    return
end

switch alg
    case {'crowder',''}
        % Heuristics from H. Crowder, E. Johnson, M. Padberg
        % Solving large-scale 0–1 linear programming programs
        [val,loc] = sort((1-x)./a(:),'ascend');
    case 'gu'
        [val,loc] = sort((x),'descend');
    case 'gu-reverse'
        [val,loc] = sort((x),'ascend');

    otherwise
        error('Unsupport cover cut separation')
end
% Initial cover
C = min(find(cumsum(a(loc))>b));

% Apply Balas lifting
q = knapsack_cover_lift_balas(a,loc(1:C));
% qL = knapsack_cover_lift_letchford(a,b,loc(1:C));
% if q~=qL
%    'HEJ'
% end
% Return row where row*[1;x] hopefully is violated
cut = spalloc(1,length(a)+1,0);
cut(1) = C-1;
cut(2:end)=-q;


function [result,modified_q] = fit_q_to_range(qmin,qmax,q)
modified_q = q;
while modified_q<qmin
    modified_q = modified_q+2*pi;
end
while modified_q>qmax
    modified_q = modified_q-2*pi;
end
if modified_q<qmin
    result = false;
else
    result = true;
end
end

github-repos/MATLAB/xuhuairuogu-V-REP-Simulation-Projects/V-REP-Simulation-Projects-841b944af4ea3a8fb250578d36434515f577f411/admittance control(adapted from an example in the book--A Systematic Approach to Learning Robot Programming with ROS)/iiwa14_kinematics/fit_q_to_range.m


mdl_p560

% at zero pose
t = p560.fkine(qz)
t
q = p560.ikine560(t)
q
p560.fkine(q)
p560.ikine560(t, 'r')
p560.ikine560(t, 'rn')

p560.ikine(t)

% at nominal pose
qn
t = p560.fkine(qn)
t
%q = ikine560(t)
q
p560.fkine(q)
p560.ikine(t, [0, 0.7, 3, 0, 0.7, 0])

% along trajectory
[q,qd,qdd] = jtraj(qz, qr, 20)
fkine(q)

t1 = p560.fkine(qz)
t2 = p560.fkine(qr)
traj = ctraj(t1, t2, 5)
p560.ikine(traj)


% DEMOIL100FGPLVM2 Oil100 data with FGPLVM.

% FGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'oil100';
experimentNo = 2;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('ftc');
options.optimiser = 'graddesc';
latentDim = 2;
d = size(Y, 2);
options.initX = randn(size(Y, 1), latentDim)*1e-3;
model = fgplvmCreate(latentDim, d, Y, options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
modelWriteResult(model, dataSetName, experimentNo);

if exist('printDiagram') & printDiagram
  lvmPrintPlot(model, lbls, dataSetName, experimentNo);
end

% Load the results and display dynamically.
lvmResultsDynamic(model.type, dataSetName, experimentNo, 'vector')

% compute the nearest neighbours errors in latent space.
errors = lvmNearestNeighbour(model, lbls);


function J = NLMF2Dtree(I, Options)
% This function NLMF2Dtree performs Non-Local Means noise filtering of
% 2D grey/color image. This function constructs a KD-tree of the whole
% image to find patches which have the closest intensity difference,
% instead of searching only in the local neighboorhood like in the
% NLMF function.
% Warning, despite the usage of a kd-tree, this function will take more
% then a hour for one image.
%
% Function:
%
%   J = NLMF2Dtree(I, Options);
%
% inputs,
%   I : 2D grey/color or 3D image data, of type Single or Double
%           in range [0..1]
%   Options : Struct with options
%
% outputs,
%   J : The NL-means filtered image or image volume
%
% options,
%   Options.kernelratio : Radius of local Patch (default 3)
%   Options.filterstrength : Strength of the NLMF filtering (default 0.05)
%   Options.number : The number of patches which have the closest distance
%                      to a local patch used for filtering (default 9).
%
% First Compile c-code!!!!, with :
%   mex image2vectors_double.c -v
%
% Note: This function uses the "Matlab Statistics Toolbox"
%

% Process inputs
defaultoptions=struct('kernelratio',3,'filterstrength',0.05,'number',9);
if(~exist('Options','var')), Options=defaultoptions;
else
    tags = fieldnames(defaultoptions);
    for i=1:length(tags), if(~isfield(Options,tags{i})), Options.(tags{i})=defaultoptions.(tags{i}); end, end
    if(length(tags)~=length(fieldnames(Options))),
        warning('NLMF2Dtree:unknownoption','unknown options found');
    end
end

% Detect if this is a color or grey scale image
switch(size(I,3))
    case 1, rgb=false;
    case 3, rgb=true;
    otherwise,error('NLMF2Dtree:inputs','This is not a 2D image');
end

% Pad the image, to allow simple extraction of local patches at the image
% boundary
Ipad = padarray(I,[Options.kernelratio Options.kernelratio],'symmetric');

% Get the local patches
V=image2vectors_double(double(Ipad),Options.kernelratio,2);

% Create a KDtree with all local patches
ns = createns(V','nsmethod','kdtree');

% Find the patches closest by in intensity in relation to the local patch
% itself
[VM,D] = knnsearch(ns,V','k',Options.number+1);

% Calculate the weight of the patches used for a pixel
W=exp(-(D.^2)*Options.filterstrength);
% The first column is the local patch itself, set the weight to maximum
% found in the other patches
W(:,1)=max(W(:,2:end),[],2);
% Make the sum of weights which will be used for one patch, equal to one.
W=W./repmat((sum(W,2)+eps),[1 size(W,2)]);

% Add the weighted intensities, of the center pixels of the in intensity
% closest by patches
if(rgb)
    Ir=I(:,:,1);
    Ig=I(:,:,2);
    Ib=I(:,:,3);
    Jr=sum(Ir(VM).*W,2);
    Jg=sum(Ig(VM).*W,2);
    Jb=sum(Ib(VM).*W,2);
    Jr=reshape(Jr,[size(I,1) size(I,2)]);
    Jg=reshape(Jg,[size(I,1) size(I,2)]);
    Jb=reshape(Jb,[size(I,1) size(I,2)]);
    J(:,:,1)=Jr;
    J(:,:,2)=Jg;
    J(:,:,3)=Jb;
else
    J=sum(I(VM).*W,2);
    J=reshape(J,size(I));
end


function [ x, y, z, w ] = ld1454 ( )

%*****************************************************************************80
%
%% LD1454 computes the 1454 point Lebedev angular grid.
%
%  Modified:
%
%    14 September 2010
%
%  Author:
%
%    Dmitri Laikov
%
%  Reference:
%
%    Vyacheslav Lebedev, Dmitri Laikov,
%    A quadrature formula for the sphere of the 131st
%    algebraic order of accuracy,
%    Russian Academy of Sciences Doklady Mathematics,
%    Volume 59, Number 3, 1999, pages 477-481.
%
%  Parameters:
%
%    Output, real X(N), Y(N), Z(N), W(N), the coordinates
%    and weights of the points.
%
  n = 0;
  x = zeros(1454,1);
  y = zeros(1454,1);
  z = zeros(1454,1);
  w = zeros(1454,1);
  a = 0.0;
  b = 0.0;
  v = 0.7777160743261247E-04;
  [ n, x, y, z, w ] = gen_oh ( 1, n, a, b, v, x, y, z, w );
  v = 0.7557646413004701E-03;
  [ n, x, y, z, w ] = gen_oh ( 3, n, a, b, v, x, y, z, w );
  a = 0.3229290663413854E-01;
  v = 0.2841633806090617E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.8036733271462222E-01;
  v = 0.4374419127053555E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.1354289960531653;
  v = 0.5417174740872172E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.1938963861114426;
  v = 0.6148000891358593E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.2537343715011275;
  v = 0.6664394485800705E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.3135251434752570;
  v = 0.7025039356923220E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.3721558339375338;
  v = 0.7268511789249627E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.4286809575195696;
  v = 0.7422637534208629E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.4822510128282994;
  v = 0.7509545035841214E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.5320679333566263;
  v = 0.7548535057718401E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.6172998195394274;
  v = 0.7554088969774001E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.6510679849127481;
  v = 0.7553147174442808E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.6777315251687360;
  v = 0.7564767653292297E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.6963109410648741;
  v = 0.7587991808518730E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.7058935009831749;
  v = 0.7608261832033027E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.9955546194091857;
  v = 0.4021680447874916E-03;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.9734115901794209;
  v = 0.5804871793945964E-03;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.9275693732388626;
  v = 0.6792151955945159E-03;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.8568022422795103;
  v = 0.7336741211286294E-03;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.7623495553719372;
  v = 0.7581866300989608E-03;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.5707522908892223;
  b = 0.4387028039889501;
  v = 0.7538257859800743E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.5196463388403083;
  b = 0.3858908414762617;
  v = 0.7483517247053123E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.4646337531215351;
  b = 0.3301937372343854;
  v = 0.7371763661112059E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.4063901697557691;
  b = 0.2725423573563777;
  v = 0.7183448895756934E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.3456329466643087;
  b = 0.2139510237495250;
  v = 0.6895815529822191E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.2831395121050332;
  b = 0.1555922309786647;
  v = 0.6480105801792886E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.2197682022925330;
  b = 0.9892878979686097E-01;
  v = 0.5897558896594636E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.1564696098650355;
  b = 0.4598642910675510E-01;
  v = 0.5095708849247346E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.6027356673721295;
  b = 0.3376625140173426;
  v = 0.7536906428909755E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.5496032320255096;
  b = 0.2822301309727988;
  v = 0.7472505965575118E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.4921707755234567;
  b = 0.2248632342592540;
  v = 0.7343017132279698E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.4309422998598483;
  b = 0.1666224723456479;
  v = 0.7130871582177445E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.3664108182313672;
  b = 0.1086964901822169;
  v = 0.6817022032112776E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.2990189057758436;
  b = 0.5251989784120085E-01;
  v = 0.6380941145604121E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.6268724013144998;
  b = 0.2297523657550023;
  v = 0.7550381377920310E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.5707324144834607;
  b = 0.1723080607093800;
  v = 0.7478646640144802E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.5096360901960365;
  b = 0.1140238465390513;
  v = 0.7335918720601220E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.4438729938312456;
  b = 0.5611522095882537E-01;
  v = 0.7110120527658118E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.6419978471082389;
  b = 0.1164174423140873;
  v = 0.7571363978689501E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.5817218061802611;
  b = 0.5797589531445219E-01;
  v = 0.7489908329079234E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );

  return
end


function numUnique=numUniqueElsInArray(vecList,skipEl)
%%NUMUNIQUEELSINARRAY Count the number of unique elements in a vector.
%
%INPUTS: vecList A numElsXnumVecs list of vectors for which the number of
%                unique elements in each vector (not considering across
%                vectors) is desired. NaN values are not counted.
%         skipEl If there is an element that should not count toward being
%                unique (i.e. these are skipped), then it can be given. If
%                no elements should be skipped, then this input can be
%                omitted or an empty matrix can be passed.
%
%OUTPUTS: numUnique A numVecsX1 array, where the number of unique elements
%               of each (column) vector in the matrix vecList is given.
%
%This function sorts the array, which puts makes duplicates consecutive.
%then, it scans through the array, skipping duplicates. The sorting places
%NaN values, if any, at the highest end of the array, past Inf. NaN values
%are just skipped.
%
%EXAMPLE:
% vecList=[17, 24,  1,  8, 15;
%          17,  2,  7, 14, 16;
%           4,  6,  2, 20, 22;
%          10, 12, 19,  2,  3;
%          11, 18, 25,  2,  0];
% numUnique=numUniqueElsInArray(vecList,0)
%One should get
% numUnique=[4;5;5;4;4]
%
%September 2020 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numEls=size(vecList,1);
numVecs=size(vecList,2);

if(nargin>1&&~isempty(skipEl))
    vecList(vecList==skipEl)=NaN;
end

%Sorting the array makes all duplicates consecutive and places all NaN
%values at the end of the array (even after Inf).
vecList=sort(vecList,1,'ascend');

numUnique=zeros(numVecs,1);
for curVec=1:numVecs
    curEl=1;
    while(curEl<=numEls)
        %Advance the index past duplicates.
        while(curEl<numEls&&vecList(curEl,curVec)==vecList(curEl+1,curVec))
            curEl=curEl+1;
        end

        if(isnan(vecList(curEl,curVec)))
            break;
        end
        numUnique(curVec)=numUnique(curVec)+1;
        curEl=curEl+1;
    end
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [beta,r,J,Sigma,mse,errorparam,robustw] = nlinmultifit(x_cell, y_cell, mdl_cell, beta0, options)
%NLINMULTIFIT Nonlinear least-squares regression of multiple data sets
%
%	A wrapper function for NLINFIT which allows simulatenous fitting of
%	multiple data sets with shared fitting parameters. See example below.
%
%	Unlike different solutions (using fminsearch or similar functions)
%	this approach enables simple estimation of model predictions and
%	their confidence intervals, as well as confidence intervals on the
%	fitted parameters (using the built-in NLPREDCI and NLPARCI functions).
%
%	KNOWN ISSUE:
%		In this implementation, different data sets are weighted according
%		to their relative lengths. Special care should be taken when those
%		lengths are considerably different from one another to avoid
%		biased results towards one data set in particular.
%
%	INPUT:
% 		x_cell,y_cell: Cell arrays containing the x,y vectors of the fitted
%					   data sets.
% 		mdl_cell: Cell array containing model functions for each data set.
% 		beta0: Vector containing initial guess of the fitted parameters.
% 		options: Structure containing control parameters for NLINFIT (see
%				 help file on NLINFIT for more details).
%
%	OUTPUT:
%		beta,r,J,Sigma,mse,errorparam,robustw: Direct output from NLINFIT.
%
%	EXAMPLE:
% 		% Generate X vectors for both data sets
% 		x1 = 0:0.1:10;
% 		x2 = 0:0.2:10;
%
% 		% Generate Y data with some noise
% 		y1 = cos(2*pi*0.5*x1).*exp(-x1/5) + 0.1*randn(size(x1));
% 		y2 = 0.5 + 2*exp(-(x2/5)) + 0.1*randn(size(x2));
%
% 		% Define fitting functions and parameters, with identical
%		% exponential decay for both data sets
% 		mdl1 = @(beta,x) cos(2*pi*beta(1)*x).*exp(-x/beta(2));
% 		mdl2 = @(beta,x) beta(4) + beta(3)*exp(-(x/beta(2)));
%
% 		% Prepare input for NLINMULTIFIT and perform fitting
% 		x_cell = {x1, x2};
% 		y_cell = {y1, y2};
% 		mdl_cell = {mdl1, mdl2};
% 		beta0 = [1, 1, 1, 1];
% 		[beta,r,J,Sigma,mse,errorparam,robustw] = ...
%					nlinmultifit(x_cell, y_cell, mdl_cell, beta0);
%
% 		% Calculate model predictions and confidence intervals
% 		[ypred1,delta1] = nlpredci(mdl1,x1,beta,r,'covar',Sigma);
% 		[ypred2,delta2] = nlpredci(mdl2,x2,beta,r,'covar',Sigma);
%
% 		% Calculate parameter confidence intervals
% 		ci = nlparci(beta,r,'Jacobian',J);
%
% 		% Plot results
% 		figure;
% 		hold all;
% 		box on;
% 		scatter(x1,y1);
% 		scatter(x2,y2);
% 		plot(x1,ypred1,'Color','blue');
% 		plot(x1,ypred1+delta1','Color','blue','LineStyle',':');
% 		plot(x1,ypred1-delta1','Color','blue','LineStyle',':');
% 		plot(x2,ypred2,'Color',[0 0.5 0]);
% 		plot(x2,ypred2+delta2','Color',[0 0.5 0],'LineStyle',':');
% 		plot(x2,ypred2-delta2','Color',[0 0.5 0],'LineStyle',':');
%
%	AUTHOR:
%		Chen Avinadav
%		mygiga (at) gmail
%

	num_curves = length(x_cell);
	if length(y_cell) ~= num_curves || length(mdl_cell) ~= num_curves
		error('Invalid input to NLINMULTIFIT');
	end

	x_vec = [];
	y_vec = [];
	mdl_vec = '@(beta,x) [';
	mdl_ind1 = 1;
	mdl_ind2 = 0;
	for ii = 1:num_curves
		if length(x_cell{ii}) ~= length(y_cell{ii})
			error('Invalid input to NLINMULTIFIT');
		end
		if size(x_cell{ii},2) == 1
			x_cell{ii} = x_cell{ii}';
		end
		if size(y_cell{ii},2) == 1
			y_cell{ii} = y_cell{ii}';
		end
		x_vec = [x_vec, x_cell{ii}];
		y_vec = [y_vec, y_cell{ii}];
		mdl_ind2 = mdl_ind2 + length(x_cell{ii});
		mdl_vec = [mdl_vec, sprintf('mdl_cell{%d}(beta,x(%d:%d)), ', ii, mdl_ind1, mdl_ind2)];
		mdl_ind1 = mdl_ind1 + length(x_cell{ii});
	end
	mdl_vec = [mdl_vec(1:end-2), '];'];
	mdl_vec = eval(mdl_vec);

	if nargin == 4
		[beta,r,J,Sigma,mse,errorparam,robustw] = nlinfit(x_vec, y_vec, mdl_vec, beta0);
	else
		[beta,r,J,Sigma,mse,errorparam,robustw] = nlinfit(x_vec, y_vec, mdl_vec, beta0, options);
	end
end


function x = lprec1(c, d, h, g, extmod)
% LPREC1   One-level Laplacian pyramid reconstruction
%
%	x = lprec1(c, d, h, g)
%
% Input:
%   c:      coarse signal at half size
%   d:      detail signal at full size
%   h, g:   two biorthogonal 1-D lowpass filters
%   extmod: [optional] extension mode (default is 'per')
%
% Output:
%   x:      reconstructed signal
%
% Note:     This uses a new reconstruction method by Do and Vetterli,
%           "Framming pyramids", IEEE Trans. on Sig Proc., Sep. 2003.
%
% See also:	LPDEC1

if ~exist('extmod', 'var')
    extmod = 'per';
end

nd = ndims(c);

% First, filter and downsample the detail image
r = d;
for dim = 1:nd
    r = filtdn(r, h, dim, extmod, 0);
end

% Then subtract the result from the coarse signal
p = c - r;

% Even size filter needs to be adjusted to obtain perfect reconstruction
adjust = mod(length(g) + 1, 2);

% Then upsample and filter
for dim = 1:nd
    p = upfilt(p, g, dim, extmod, adjust);
end

% Final combination
x = p + d;


function [tg,theta] = detTG(im,radius,norient)
% function [tg,theta] = detTG(im,radius,norient)
%
% Compute smoothed but not thinned TG fields.

if nargin<2, radius=0.02; end
if nargin<3, norient=8; end

[h,w,unused] = size(im);
idiag = norm([h w]);
if isrgb(im), im=rgb2gray(im); end

% compute texture gradient
no = 6;
ss = 1;
ns = 2;
sc = sqrt(2);
el = 2;
k = 64;
fname = sprintf( ...
    'unitex_%.2g_%.2g_%.2g_%.2g_%.2g_%d.mat',no,ss,ns,sc,el,k);
textonData = load(fname); % defines fb,tex,tsim
tmap = assignTextons(fbRun(textonData.fb,im),textonData.tex);
[tg,theta] = tgmo(tmap,k,idiag*radius,norient,...
                  'smooth','savgol','sigma',idiag*radius);


%#codegen
function [pixel_val, pixel_valid] = aMediantFilter_2D(c_data, c_idx)

smax = 9;
persistent window;
if isempty(window)
    window = zeros(smax, smax);
end

cp = ceil(smax/2); % center pixel;

w3 = -1:1;
w5 = -2:2;
w7 = -3:3;
w9 = -4:4;

r3 = cp + w3;      % 3x3 window
r5 = cp + w5;      % 5x5 window
r7 = cp + w7;      % 7x7 window
r9 = cp + w9;      % 9x9 window

d3x3 = window(r3, r3);
d5x5 = window(r5, r5);
d7x7 = window(r7, r7);
d9x9 = window(r9, r9);

center_pixel = window(cp, cp);

% use 1D filter for 3x3 region
outbuf = get_median_1d(d3x3(:)');
[min3, med3, max3] = getMinMaxMed_1d(outbuf);

% use 2D filter for 5x5 region
outbuf = get_median_2d(d5x5);
[min5, med5, max5] = getMinMaxMed_2d(outbuf);

% use 2D filter for 7x7 region
outbuf = get_median_2d(d7x7);
[min7, med7, max7] = getMinMaxMed_2d(outbuf);

% use 2D filter for 9x9 region
outbuf = get_median_2d(d9x9);
[min9, med9, max9] = getMinMaxMed_2d(outbuf);

pixel_val = get_new_pixel(min3, med3, max3, ...
    min5, med5, max5, ...
    min7, med7, max7, ...
    min9, med9, max9, ...
    center_pixel);

% we need to wait until 9 cycles for the buffer to fill up
% output is not valid every time we start from col1 for 9 cycles.
persistent datavalid
if isempty(datavalid)
    datavalid = false;
end
pixel_valid = datavalid;
datavalid = (c_idx >= smax);

% build the 9x9 buffer
window(:,2:smax) = window(:,1:smax-1);
window(:,1) = c_data;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [min, med, max] = getMinMaxMed_1d(inbuf)

max = inbuf(1);
med = inbuf(ceil(numel(inbuf)/2));
min = inbuf(numel(inbuf));

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [min, med, max] = getMinMaxMed_2d(inbuf)

[nrows, ncols] = size(inbuf);
max = inbuf(1, 1);
med = inbuf(ceil(nrows/2), ceil(ncols/2));
min = inbuf(nrows, ncols);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function new_pixel  = get_new_pixel(...
    min3, med3, max3, ...
    min5, med5, max5, ...
    min7, med7, max7, ...
    min9, med9, max9, ...
    center_data)

if (med3 > min3 && med3 < max3)
    new_pixel = get_center_data(min3, med3, max3,center_data);
elseif (med5 > min5 && med5 < max5)
    new_pixel = get_center_data(min5, med5, max5,center_data);
elseif (med7 > min7 && med7 < max7)
    new_pixel = get_center_data(min7, med7, max7,center_data);
elseif (med9 > min9 && med9 < max9)
    new_pixel = get_center_data(min9, med9, max9,center_data);
else
    new_pixel = center_data;
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [new_data] = get_center_data(min,med,max,center_data)
if center_data <= min || center_data >= max
    new_data = med;
else
    new_data = center_data;
end
end


clear all;
clc

% File folder and name of int16 input signal
int16FileFolder = '/Users/';
int16FileName = 'inputsignal';

fileName = [int16FileFolder int16FileName '.int16'];

nMics = 128; % # mics in array
fs = 44.1e3; %sampling rate

data = readInt16(nMics, fs, 0, fileName);

% Get microphone positions
%Here the microphone positions needs to be loaded. That is mic #1
%corresponds to the first row in the timeSignal matrix, mic #2 to the
%second row and so on
Xm = zeros(1, nMics);
Ym = zeros(1, nMics);
Zm = zeros(1, nMics);
Wm = ones(1, numel(Xm))/numel(Xm);

% Scanning range (multiple frequencies, broadband)
fmin = 3e3;
fmax = 5e3;
c = 340; %speed of sound

t_start = 0.0;
T = 0.035; %time weighting
block = T*fs;

% Scanning parameters
distance = 2.0; %distance to source
maxX = 1; %max scanning extent x
maxY = 1; %max scanning extent y
deltaX = 0; %offset scanning grid
deltaY = 0; %offset scanning grid
anglRes = 1; %angle resolution
filterFrequencies = [fmin, fmax];

tic
[anglex,angley, pow] = sweepPow2(Xm, Ym, Zm, Wm, data(:, (t_start*fs)+1:end), fs, filterFrequencies, distance, maxX, maxY, anglRes, block, deltaX, deltaY);
toc

figure;
imagesc(angley, anglex, db(pow'))


function spec = fwDs_spec(hatRxx, f, d, tauGrid, c)

[nbin,nFrames] = size(hatRxx(:,:,1,1));
ngrid = length(tauGrid);
R11 = hatRxx(:,:,1,1);
R12 = hatRxx(:,:,1,2);
R22 = hatRxx(:,:,2,2);
TR = real(R11 + R22);
SINC = sinc(2*f*d/c);

SNR = zeros(nbin,nFrames,ngrid);
for pkInd=1:ngrid,
    EXP = repmat(exp(-2*1i*pi*tauGrid(pkInd)*f),1,nFrames);
    SNR(:,:,pkInd) = repmat(-(1+SINC)/2,1,nFrames) + repmat((1-SINC)/2,1,nFrames).*(TR + 2*real(R12.*EXP))./(TR - 2*real(R12.*EXP));
end
spec = SNR;

end


function [mu,h] = apgarchcore(parameters, data, ar, ma, x, p, q, y, m, z, v, T)
%{
-----------------------------------------------------------------------
 PURPOSE:
 Estimation of conditional mean and variance of the
 APGARCH: Asymmetric Power GARCH: Ding, Granger and Engle (1993)
-----------------------------------------------------------------------
 USAGE:
 [mu,h] = apgarchcore(parameters, data, p, q, m, T)

 INPUTS:
 parameters:	vector of parameters
 data:         (T x 1) vector of data
 ar:        positive scalar integer representing the order of AR
 am:        positive scalar integer representing the order of MA
 x:         (T x N) vector of factors for the mean process
 p:         positive scalar integer representing the order of ARCH
 q:         positive scalar integer representing the order of GARCH
 y:         (T x N) vector of factors for the volatility process, must be positive!

 OUTPUTS:
 mu:           conditional mean
 h:            conditional variance
-----------------------------------------------------------------------
 Author:
 Alexandros Gabrielsen, a.gabrielsen@city.ac.uk
 Date:     10/2010
 Update 1: 08/2011: ARMA-X Support
-----------------------------------------------------------------------
%}
% Verifying that the vector of parameters is a column vector
[r,c] = size(parameters);
if c>r
    parameters = parameters';
    [r,c] = size(parameters);
end

% Asymmetric  and Nonlinear coefficients
delta = parameters(3+z+p+q+v);
asym=parameters(4+z+p+q:3+z+2*p+q+v);

% Initial parameters
mu = [];
h = [];
mu(1:m,1) = parameters(1);
h(1:m,1) = var(data);

% Dimension of factors
if isscalar(x)
    xy=zeros(size(data,1),1);
else
    xy=x;
end
if isscalar(y)
    yy=zeros(size(data,1),1);
else
    yy=y;
end
% Estimation of Conditional Mean and Variance
for t = (m+1):T;
   mu(t,1) = parameters(1:1+z)'*[1; data(t-(1:ar)); data(t-(1:ma))-mu(t-(1:ma),1); xy(t,:)'*ones((isscalar(x) < 1))];
   h(t,1) = (parameters(2+z:2+z+p+q+v)'*[1; (abs(data(t-(1:p)) - mu(t-(1:p))) - asym(1:p)*(data(t-(1:p)) - mu(t-1))).^delta; (h(t-(1:q))).^delta; yy(t,:)'*ones((isscalar(y) < 1))]).^(1/delta);
end

end


%SF_LINE_P5 1D Fifth order Lagrange shape functions for lines (P5).
%
%   [ VBASE, NLDOF, XLDOF, SFUN ] = SF_LINE_P5( I_EVAL, N_SDIM, N_VERT, I_DOF, XI, AINVJAC, VBASE )
%   Evaluates conforming fifth order P5 Lagrange shape functions on 1D line elements
%   with values defined in the nodes and center. XI are Barycentric coordinates.
%
%       Input       Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       i_eval      scalar:  1             Evaluate function values
%                           >1             Evaluate values of derivatives
%       n_sdim      scalar: 1              Number of space dimensions
%       n_vert      scalar: 2              Number of vertices per cell
%       i_dof       scalar: 1-6            Local basis function to evaluate
%       xi          array [2,1]            Local coordinates of evaluation point
%       aInvJac     [n,3]                  Inverse of transformation Jacobian
%       vBase       [n]                    Preallocated output vector
%                                                                                .
%       Output      Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       vBase       [n]                    Evaluated function values
%       nLDof       [4]                    Number of local degrees of freedom on
%                                          vertices, edges, faces, and cell interiors
%       xLDof       [2,n_ldof]             Local coordinates of local dofs
%       sfun        string                 Function name of called shape function
%
%   See also SF_LINE_P1

% Copyright 2013-2022 Precise Simulation, Ltd.


function Population = EnvironmentalSelection(Population,R,N)
% The environmental selection of RPEA

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Calculate the Tchebychev distances
    Distance = TchebychevDistance(Population.objs,R);

    %% Environmental selection
    RemainP = 1 : length(Population);
    RemainR = 1 : size(R,1);
    while length(RemainP) > length(Population)-N
        if isempty(RemainR)
            RemainR = 1 : size(R,1);
        end
        [temp,imin] = min(Distance(RemainP,RemainR),[],1);
        [~,jmin]    = min(temp);
        imin        = imin(jmin);
        RemainP(imin) = [];
        RemainR(jmin) = [];
    end
    Population = Population(setdiff(1:length(Population),RemainP));
end


function X = my_fftn(x)

X = fft(x);
dd = ndims(x);
for id=2:dd
	ord = 1:dd;
	ord([1 id]) = [id 1];
	x = permute(X, ord);
	X = fft(x);
	X = ipermute(X, ord);
end


x = linspace(-20,2,100);
w = exp(-0.5*x.^2 -0.5*log(2*pi));
f = exp(-normcdfln(x));
g = 1./normcdf(x);
%g = exp((log(1+exp(0.88+x))./1.5).^2);
%g = exp(0.5*log(2*pi) +0.5*x.^2 + log(x));
plot(x, f.*w, x, g.*w)
legend('exp(-normcdfln)','1/normcdf');

if 0
  true = -27.3843074988;
  [abs(log(normcdf(-7))-true) abs(normcdfln(-7)-true)]
end

if 0
  %matnet
  %imports('c:/Documents and Settings/minka/Depots/Infer/Core/bin/Debug/Core.dll')
  h = g;
  for i = 1:length(x)
    h(i) = 1./cl.MMath.NormalCdf(x(i));
  end
  %h = 0.5*(-x + sqrt(x.*x + 8/pi));
  plot(x, f.*w, x, g.*w, x, h.*w)
  legend('exp(-normcdfln)','1/normcdf', '1/normcdf2');

  plot(x, f.*w - h.*w)
end

% evalf(subs(t=1e-4,subs(x=2*(1-t)/t,erfc(x)*exp(x*x))),100);

if 0
% test approximations
a = exp(sqrt(2/pi));
b = 1/log(2/pi*a);
g = log(a-1 + exp(x.*exp(1./(x.^2 + b))));
g = log(a-1 + exp(x));
plot(x, f, x, g)
plot(x, log(exp(f)+1-a)./x)
plot(x, 1./log(log(exp(f)+1-a)./x))
end

% read `/u/tpminka/src/maple/gauss_cdf`;
% f := 1/sqrt(2*Pi)*exp(-1/2*x^2)/gauss_cdf(-x);
% g := exp(sqrt(2/Pi))-1+exp(x);
% plot(f, x=0..50);
% h := log(exp(f*sqrt(Pi/2)-1)+1);
% h := log(exp(f - sqrt(2/Pi))+1);
% h := 1/log(log(exp(f) +1-exp(sqrt(2/Pi)))/x);
% asympt(h,x);


% RENORM_PARENT_2D Renormalization for 2d scattering
%
% Usage
%   Sx_rn = renorm_parent_2d(Sx)
%
% Input
%   Sx (cell): output of 2d scattering
%
% Output
%   Sx_rn (cell): the renormalized 2d scattering
%
% Description
%   This function will renormalize every 2nd order node by its parent
%   (hence his name renorm_PARENT_2d)
%
% See also
%   SCAT, REMORM_PARENT_3D

function Sx_rn = renorm_parent_2d(Sx)

    Sx_rn = Sx;

    % a function handle to find the parent node
    parent = @(p)(find(Sx{2}.meta.j(1,:) == Sx{3}.meta.j(1,p) &...
        Sx{2}.meta.theta(1,:) == Sx{3}.meta.theta(1,p) & ...
        Sx{2}.meta.q(1,:) == Sx{3}.meta.q(1,p)));

    % for each signal in order 2, divide by its ancestor
    for p = 1:numel(Sx{3}.signal)
        Sx_rn{3}.signal{p} = Sx_rn{3}.signal{p}./Sx{2}.signal{parent(p)};
    end

end