function result = warpAffine3B(in,A,badVal,B,interpMethod)
%
% function result = warpAffine3B(in,A,badVal,B)
%
% in: input volume, 3D array
% A: 3x4 affine transform matrix or a 4x4 matrix with [0 0 0 1]
%    for the last row.
% badVal: if a transformed point is outside of the volume, badVal is used
% B:  number of voxels to put in the border (default =0, no border)
%
% result: output volume, same size as in
%
% 10/99, on - added Border parameter to permit nearest neighbor interpolation at the edges
%

if ( ~exist('badVal') | isempty(badVal) )
  badVal=NaN;
end

if ( ~exist('B') | isempty(B) )
  B = 0;
end

if(ieNotDefined('interpMethod'))
    interpMethod = '*linear';
end

if (size(A,1)>3)
  A=A(1:3,:);
end

% original size
[NyO NxO NzO] = size(in);

% if B~=0, put a border by replicating edge voxels
if B~=0
  % put border at each slice
  for k =1:size(in,3)
    inB(:,:,k) = putborde(in(:,:,k),B,B,3);
 end
 % repeat the first and last slices
 in = cat(3,repmat(inB(:,:,1),[1 1 B]), inB, repmat(inB(:,:,end),[1 1 B]));
end

% coordinates corresponding to the volume with borders
[xB,yB,zB]=meshgrid(1-B:size(in,2)-B,1-B:size(in,1)-B,1-B:size(in,3)-B);

% Compute coordinates corresponding to input volume
% and transformed coordinates for result
[xgrid,ygrid,zgrid]=meshgrid(1:NxO,1:NyO,1:NzO);
coords=[xgrid(:)'; ygrid(:)'; zgrid(:)'];
homogeneousCoords=[coords; ones(1,size(coords,2))];
warpedCoords=A*homogeneousCoords;

% Compute result using interp3
result = interp3(xB,yB,zB,in,warpedCoords(1,:),warpedCoords(2,:),warpedCoords(3,:),interpMethod);
result = reshape(result,[NyO NxO NzO]);

% replace NaNs with badval
if(~isnan(badVal))
  NaNIndices = find(isnan(result));
  result(NaNIndices)=badVal*ones(size(NaNIndices));
end
return;

%%% Debug

slice=[1 2 3; 4 5 6; 7 8 9]';
slice=[1 1 1; 3 3 3; 5 5 5]';
input=ones(3,3,4);
for z=1:4
  input(:,:,z)=slice;
end

A= [1 0 0 .5;
    0 1 0 0;
    0 0 1 0;
    0 0 0 1];

A= [1 0 0 .5;
    0 1 0 .5;
    0 0 1 0];

res=warpAffine3(input,A)
resB=warpAffine3B(input,A,NaN,1)

for z=1:4
  input(:,:,z)=z*ones(3,3);
end
A= [1 0 0 0;
    0 1 0 0;
    0 0 1 .5;
    0 0 0 1];
res=warpAffine3(input,A)
resB=warpAffine3B(input,A,NaN,1)

input=rand(5,5,5);
res=warpAffine3(input,eye(4));
resB=warpAffine3B(input,eye(4),NaN,1);


function e = gsp_assert_test(X1,X2,tol, name)

if X1==0
    if norm(X2(:),'fro')<tol
        fprintf(['Test ',name, ': OK\n']);
        e = 0;
    else
        warning(['Test ',name, ': Error, badness ',...
            num2str(norm(X2(:),'fro')),...
            ', tol ',num2str(tol)]);
        e = 1;
    end
else
    if norm(X1(:)-X2(:),'fro')/norm(X2(:),'fro')<tol
        fprintf(['Test ',name, ': OK\n']);
        e = 0;
    else
        warning(['Test ',name, ': Error, badness ',...
            num2str(norm(X1(:)-X2(:),'fro')/norm(X2(:),'fro')), ...
            ', tol ',num2str(tol)]);
        e = 1;
    end
end

end


function z = rk4_cannon(t,z0,P)
% z = rk4_cannon(dyn,t,z0,P)
%
% This function is used to perform a 4th-order Runge-Kutta
% integration of the cannon dynamical system
%
% INPUTS:
%   t = [1 x nTime] vector of times, created by linspace
%   z0 = [nState (x nSim)] matrix of initial states
%   P = parameter to pass to dynamics
%
% OUTPUTS:
%   z = [nState (x nSim) x nTime ] matrix of trajectories
%
% NOTES:
%   1) See rk4.m for the generic version of this function
%   2) I've hard-coded the dynamics function into this version because it
%      is faster than calling the anonymous function with an embedded
%      parameter.
%

[nState,nSim] = size(z0);
nTime = length(t);

if nSim==1
    z = zeros(nState,nTime);
    z(:,1) = z0;
    for i=1:(nTime-1)
        dt = t(i+1)-t(i);
        k1 = cannonDynamics(t(i),  z(:,i),P);
        k2 = cannonDynamics(t(i)+0.5*dt,  z(:,i) + 0.5*dt*k1,P);
        k3 = cannonDynamics(t(i)+0.5*dt,  z(:,i) + 0.5*dt*k2,P);
        k4 = cannonDynamics(t(i)+dt,  z(:,i) + dt*k3,P);
        z(:,i+1) = z(:,i) + (dt/6)*(k1+2*k2+2*k3+k4);
    end
else
    z = zeros(nState,nSim,nTime);
    z(:,:,1) = z0;
    for i=1:(nTime-1)
        dt = t(i+1)-t(i);
        k1 = cannonDynamics(t(i),  z(:,:,i),P);
        k2 = cannonDynamics(t(i)+0.5*dt,  z(:,:,i) + 0.5*dt*k1,P);
        k3 = cannonDynamics(t(i)+0.5*dt,  z(:,:,i) + 0.5*dt*k2,P);
        k4 = cannonDynamics(t(i)+dt,  z(:,:,i) + dt*k3,P);
        z(:,:,i+1) = z(:,:,i) + (dt/6)*(k1+2*k2+2*k3+k4);
    end
end

end


%% Color Mapping
%
%%
% A central issue when interpreting plots is to have a consistent color
% coding among all plots. In MTEX this can be achieved in two ways. If the
% the minimum and maximum value are known then one can specify the color
% range directly using the options *colorrange* or *contourf*, or the
% command <setColorRange.html setColorRange> is used which allows to set
% the color range afterwards.
%
%% A sample ODFs and Simulated Pole Figure Data
%
% Let us first define some model <SO3Fun.SO3Fun.html ODFs> to be plotted later
% on.

cs = crystalSymmetry('-3m');
odf = fibreODF(Miller(1,1,0,cs),zvector)
pf = calcPoleFigure(odf,[Miller(1,0,0,cs),Miller(1,1,1,cs)],...
  equispacedS2Grid('points',500,'antipodal'));

%% Tight Colorcoding
%
% When <PoleFigure.plot.html plot> is called without any colorcoding option,
% the plots are constructed using the  *tight* option to the range of the data
% independently from the other plots. This means that different pole
% figures may have different color coding and in principle cannot be
% compared to each other.

close all
plot(pf)
mtexColorbar

%% Equal Colorcoding
%
% The *tight* colorcoding can make the reading and comparison of two pole figures
% a bit hard. If you want to have one colorcoding for all plots within one figure use the
% option *colorrange* to *equal*.

plot(pf,'colorRange','equal')
mtexColorbar

%% Setting an Explicite Colorrange
%
% If you want to have a unified colorcoding for several figures you can
% set the colorrange directly in the <SO3Fun.plotPDF.html plot command>

close all
plotPDF(odf,[Miller(1,0,0,cs),Miller(1,1,1,cs)],...
  'colorrange',[0 4],'antipodal');
mtexColorbar

figure
plotPDF(.5*odf+.5*uniformODF(cs),[Miller(1,0,0,cs),Miller(1,1,1,cs)],...
  'colorrange',[0 4],'antipodal');
mtexColorbar

%% Setting the Contour Levels
%
% In the case of contour plots you can also specify the *contour levels*
% directly

close all
plotPDF(odf,[Miller(1,0,0,cs),Miller(1,1,1,cs)],...
  'contourf',0:1:5,'antipodal')
mtexColorbar

%% Modifying the Colorrange After Plotting
%
% The color range of the figures can also be adjusted afterwards using the
% command <mtexFigure.CLim.html CLim>

CLim(gcm,[0.38,3.9])

%% Logarithmic Plots
%
% Sometimes logarithmic scaled plots are of interest. For this case all
% plots in MTEX understand the option *logarithmic*, e.g.

close all;
plotPDF(odf,[Miller(1,0,0,cs),Miller(1,1,1,cs)],'antipodal','logarithmic')
CLim(gcm,[0.01 12]);
mtexColorbar

%% Changing the Colormap
%
% The colormap can be changed by the command mtexColorMap, e.g., in order
% to set a white to black colormap one has the commands

plotPDF(odf,[Miller(1,0,0,cs),Miller(1,1,1,cs)],'antipodal')
mtexColorMap white2black
mtexColorbar

%% Multiple Colormaps
%
% One can even use different colormaps within one figure

% initialize an MTEXFigure
mtexFig = newMtexFigure;

% for three different colormaps
for cm = {'hot', 'cool', 'parula'}

  % generate a new axis
  nextAxis

  % plot some random data in different axis
  plot(vector3d.rand(100),'smooth','grid','grid_res',90*degree,'upper');

  % and apply an individual colormap
  mtexColorMap(mtexFig.gca,char(cm))

  % set the title to be the name of the colormap
  mtexTitle(char(cm))
end

% plot a colorbar for each plot
mtexColorbar


%% sampleCurveEvenly
% Below is a basic demonstration of the features of the |sampleCurveEvenly| function.

%%
clear; close all; clc;

% PLOT SETTINGS
fig_color='w'; fig_colordef='white';
markerSize=15;
lineWidth=2;

%% EXAMPLE USING DEFAULT SETTINGS FOR RESAMPLING A CURVE EVENLY

%Simulating the case of an unevenly sampled loop curve
ns=50;
x=linspace(0,1,ns); %evenly spaced x data
y=sin(x*2*pi); %resulting y data
V=[x(:) y(:)]; %nonlinearity causing uneven point spacing

cPar=[]; %If the control structure is empty these are the settings used:

% cPar.nd=size(V,1); %resample with the same about of points
% cPar.typeOpt='num'; %curve steps are based on the desired number of points
% cPar.interpMethod='pchip'; %use pchip interpolation
% cPar.closeLoopOpt=0; %the curve is not a closed loop

[Vg]=sampleCurveEvenly(V,cPar);

cFigure;
subplot(1,2,1); hold on;
title('Unevenly sampled');
plotV(V,'r.-','MarkerSize',markerSize);
view(2); grid on; axis equal; axis tight;
subplot(1,2,2); hold on;
title('Evenly sampled using default settings');
plotV(Vg,'g.-','MarkerSize',markerSize);
plotV(Vg(1,:),'r.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
plotV(Vg(end,:),'b.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
view(2); grid on; axis equal; axis tight;
drawnow;

%% EXAMPLE USING CUSTOM SETTINGS FOR RESAMPLING A CURVE EVENLY

%Simulating the case of an unevenly sampled loop curve

cPar.nd=size(V,1)*2; %upsample twice
cPar.typeOpt='num'; %curve steps are based on the desired number of points
cPar.interpMethod='linear'; %use linear interpolation
cPar.closeLoopOpt=0; %the curve is not a closed loop

[Vg]=sampleCurveEvenly(V,cPar);

cFigure;
subplot(1,2,1); hold on;
title('Unevenly sampled');
plotV(V,'r.-','MarkerSize',markerSize);
view(2); grid on; axis equal; axis tight;
subplot(1,2,2); hold on;
title('Evenly sampled allong curve');
plotV(Vg,'g.-','MarkerSize',markerSize);
plotV(Vg(1,:),'r.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
plotV(Vg(end,:),'b.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
view(2); grid on; axis equal; axis tight;
drawnow;

%% EXAMPLE USING CUSTOM SETTINGS FOR RESAMPLING A CURVE EVENLY

%Simulating the case of an unevenly sampled loop curve

cPar.nd=0.1; %resmaple based on curve length step size
cPar.typeOpt='dist'; %curve steps are based on the desired number of points
cPar.interpMethod='linear'; %use linear interpolation
cPar.closeLoopOpt=0; %the curve is not a closed loop

[Vg]=sampleCurveEvenly(V,cPar);

cFigure;
subplot(1,2,1); hold on;
title('Unevenly sampled');
plotV(V,'r.-','MarkerSize',markerSize);
view(2); grid on; axis equal; axis tight;
subplot(1,2,2); hold on;
title('Evenly sampled allong curve');
plotV(Vg,'g.-','MarkerSize',markerSize);
plotV(Vg(1,:),'r.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
plotV(Vg(end,:),'b.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
view(2); grid on; axis equal; axis tight;
drawnow;

V(:,2)=V(:,2)*2; %Increasing amplitude increases point spacing and  hence increases number of point used for curve step size resampling

cPar.nd=0.1; %resample based on curve length step size
cPar.typeOpt='dist'; %curve steps are based on the desired number of points
cPar.interpMethod='linear'; %use linear interpolation
cPar.closeLoopOpt=0; %the curve is not a closed loop

[Vg]=sampleCurveEvenly(V,cPar);

cFigure;
subplot(1,2,1); hold on;
title('Unevenly sampled');
plotV(V,'r.-','MarkerSize',markerSize);
view(2); grid on; axis equal; axis tight;
subplot(1,2,2); hold on;
title('Evenly sampled allong curve');
plotV(Vg,'g.-','MarkerSize',markerSize);
plotV(Vg(1,:),'r.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
plotV(Vg(end,:),'b.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
view(2); grid on; axis equal; axis tight;
drawnow;

%% EXAMPLE USING CUSTOM SETTINGS FOR RESAMPLING A CLOSED POLYGON EVENLY

%Simulating the case of an unevenly sampled loop curve
ns=150;
t=sort(linspace(0,2*pi,ns)+pi/10*rand(1,ns));
t=unique(t); %removing double points
t=t(t<2*pi);%Removing 2*pi points since they are the same as the 0 point
r=3+2.*sin(5*t);
[x,y] = pol2cart(t,r);
z=y;
V=[x(:) y(:) z(:)];

cPar.nd=200; %Desired number of pointsupsample twice
cPar.typeOpt='num'; %curve steps are based on the desired number of points
cPar.interpMethod='pchip'; %use pchip interpolation
cPar.closeLoopOpt=1; %the curve is close so the end is considered equal to the start

[Vg]=sampleCurveEvenly(V,cPar);

cFigure;
subplot(1,2,1); hold on;
title('Unevenly sampled');
plot3(V(:,1),V(:,2),V(:,3),'r.-','MarkerSize',markerSize);
drawnow; view(3); grid on; axis equal; axis tight;
subplot(1,2,2); hold on;
title('Evenly sampled allong curve');
plot3(Vg(:,1),Vg(:,2),Vg(:,3),'g.-','MarkerSize',markerSize);
plot3(Vg(1,1),Vg(1,2),Vg(1,3),'r.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
plot3(Vg(end,1),Vg(end,2),Vg(end,3),'b.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
drawnow; view(3); grid on; axis equal; axis tight;

%% EXAMPLE USING CURVE SMOOTHENING

%Adding noise
V=V+0.2.*randn(size(V));

cPar.nd=200;
cPar.typeOpt='num';
cPar.interpMethod=0.5;
cPar.closeLoopOpt=1;

[Vg]=sampleCurveEvenly(V,cPar);

cFigure;
subplot(1,2,1); hold on;
title('Unevenly sampled');
plot3(V(:,1),V(:,2),V(:,3),'r.-','MarkerSize',markerSize);
drawnow; view(3); grid on; axis equal; axis tight;
subplot(1,2,2); hold on;
title('Evenly sampled allong curve and smoothened');
plot3(Vg(:,1),Vg(:,2),Vg(:,3),'g.-','MarkerSize',markerSize,'lineWidth',lineWidth);
plot3(Vg(1,1),Vg(1,2),Vg(1,3),'r.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
plot3(Vg(end,1),Vg(end,2),Vg(end,3),'b.','MarkerSize',2*markerSize,'lineWidth',lineWidth);
drawnow; view(3); grid on; axis equal; axis tight;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function bcdf = beta_cdf (x, a, b)
%  BETACDF Beta cumulative distribution function.
%     P = BETACDF(X,A,B) returns the beta cumulative distribution
%     function with parameters A and B at the values in X.

% Copyright (c) 1995-1997, 2005-2007 Kurt Hornik

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.
if (nargin ~= 3)
  error('must provide 3 parameters')
end

if (~isscalar (a) || ~isscalar(b))
  if ((size(a,1) ~= size(b,1)) || (size(b,1) ~= size(x,1)))
    error ('betainv: x, a and b must be of common size or scalars');
  end
end

[n,nin] = size(x);
bcdf = zeros(n,nin);

k = find ((a < 0) || (b < 0) || isnan (x));
if (any(k))
  bcdf(k) = NaN;
end

k = find((x >= 1) && (a > 0) && (b > 0));
if (any(k))
  bcdf(k) = 1;
end

k = find ((x > 0) && (x < 1) && (a > 0) && (b > 0));
if (any (k))
  if (isscalar(a) && isscalar(b))
    bcdf(k) = betainc(x(k), a, b);
  else
    bcdf(k) = betainc(x(k), a(k), b(k));
  end
end

end


function g = p20_g ( n, x )

%*****************************************************************************80
%
%% P20_G evaluates the gradient for problem 20.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 March 2000
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real G(N), the gradient of the objective function.
%
  g = zeros ( n, 1 );

  for i = 1 : n

    if ( i == 1 )
      g(i) = x(i);
    else
      g(i) = 2.0 * x(i);
    end

    if ( 1 < i )
      g(i) = g(i) - x(i-1);
    end

    if ( i < n )
      g(i) = g(i) - x(i+1);
    end

  end

  g(1) = g(1) - 2.0;

  return
end


% LOGDET_CHOL  -  Computes the log-determinant of a matrix using its
% Cholesky factor.
%
% Y = LOGDET_CHOL(U)
%
% Computes the log-determinant of the matrix X whose Cholesky factor is
% the triangular matrix U. U can be lower or upper triangular.
%
% More generally, X could be a product of two instances of U and/or U'. For
% instance, X=U'*U' or X=U'*U.

% Last modified 2010-06-04
% Copyright (c) Jaakko Luttinen

function y = logdet_chol(U)

y = 2 * logdet_tri(U);


function Nystrom_method
clc;
clear;
close all;
%% the example fuction
datasize=200;
var_noise=0.1;
x=linspace(-10,10,datasize)';
    f=sin(x)./x;
noise=var_noise*randn(datasize,1);
y=f+noise;
x_raw=x;
plot(x_raw,f,'r-','LineWidth',2),hold on;
%% the training data samples
Inset=30;
dataindex=randsample(datasize,Inset);
xI=x(dataindex);
yI=y(dataindex);
plot(xI,yI,'b+'),hold on;
x(dataindex)=[];
y(dataindex)=[];
x=[xI;x];
y=[yI;y];
%% the covariance matrix,kernel;
K_mat=Kernel_func(x,x,0.7,1.2);
K_mm=K_mat(1:Inset,1:Inset);

%%
[vecmat_m,valmat_m]=eig(K_mm);
valmat_m=diag(valmat_m);
[valmat_m,ix]=sort(valmat_m,'descend');
vecmat_m=vecmat_m(:,ix);
valmat_n=datasize/Inset*valmat_m;
prin_val=sum(valmat_n>0);

for i=1:prin_val;
vecmat_n(:,i)=sqrt(Inset/datasize)/valmat_m(i)*K_mat(1:datasize,1:Inset)*...
    vecmat_m(:,i);
valmat_n(i)=datasize/Inset*valmat_m(i);
end

valmat_n=diag(valmat_n(1:prin_val));

new_kernel=inv(valmat_n)+vecmat_n'*1/var_noise*eye(datasize)*vecmat_n;
% rank(new_kernel)
L=chol(new_kernel,'lower');  %using cholesky decomposition to be more stable
%%test
testnum=500;
x_test=linspace(-15,15,testnum)';
K_test_train=Kernel_func(x_test,x,0.7,1.2);
K_test=Kernel_func(x_test,x_test,0.7,1.2);

b_temp=vecmat_n'*1/var_noise*eye(datasize)*y;
f_mean=K_test_train*(1/var_noise*eye(datasize)*y-1/var_noise*eye(datasize)*...
    vecmat_n*(L'\(L\b_temp)));
for i=1:testnum
K_ss=Kernel_func(x_test(i),x_test(i),0.7,1.2);
K_s=Kernel_func(x_test(i),x,0.7,1.2);
v=L\( vecmat_n'*1/var_noise*eye(datasize)*K_s');
  f_var(i)=K_ss-(K_s*1/var_noise*eye(datasize)*K_s'-v'*v);
%  f_var(i)=K_ss-K_s*(1/var_noise*eye(datasize)-1/var_noise*eye(datasize)*vecmat_n...
%      *inv(new_kernel)*vecmat_n'*1/var_noise*eye(datasize))*K_s';
end
f_var=f_var';

% in_pos=find(f_var>0);
% f_var=f_var(in_pos);
% x_test=x_test(in_pos);
% f_mean=f_mean(in_pos);

plot(x_test,f_mean,'b-','LineWidth',2),hold on;
%%plot the variance
x_area=[x_test;flipdim(x_test,1)];
ybound_std=[(f_mean+1.96*sqrt(f_var));flipdim(f_mean-1.96*sqrt(f_var),1)];
fill(x_area,ybound_std,[187 255 0]/255,'EdgeColor','none');
plot(x_raw,f,'r-','LineWidth',2),hold on;
plot(xI,yI,'b+'),hold on;
plot(x_test,f_mean,'b-','LineWidth',2),hold on;
%  alpha(0.6)
legend1=legend('target function','training points','preditive distribution','95% confidence interval');
 set(legend1,'Box','off','Color','none',...
     'Location','NorthEast');
xlabel('30 active training points')
set(gca,'XTick',[-15:3:15])
matlab2tikz( 'Nys_data30.tex' )
end
%%


function P = fdkPreFilter(P, D, Filter)
% This function applies the weighting and filtering steps necessary for FDK
% reconstruction of a conebeam CT sinogram
%
% P: conebeam CT sinogram
% D: rotation radius
% Filter: filter to apply ('Shepp-Logan', 'Cosine', 'Hamming', 'Hann' or
% 'None')
%
% Author: Rene Willemink (Signals and Systems Group, University of Twente)
% Date: 2009-03-11

if nargin<3
    Filter = 'None';
end

% Pre-calculate some things
[U V] = ndgrid(1:size(P,1), 1:size(P,2));
U = U-(size(P,1)-1)/2;
V = V-(size(P,2)-1)/2;

% Calculate filter to use in backprojection (in frequency domain)
% Frequency range of filter
w = (0:(size(P,1)-1))' * 2*pi/size(P,1);
H = w;

% Filter with hamming window
switch Filter
  case 'Shepp-Logan'
    % Sinc window
    wind = [0; sin(w(2:end)/2)./(w(2:end)/2)];
  case 'Cosine'
    wind = cos(w/2);
  case 'Hamming'
    alpha = 0.54;
    wind = alpha + (1-alpha)*cos(w);
  case 'Hann'
    wind = (1 + cos(w))/2;
  case 'None'
    wind = ones(size(w));
end
H = H.*wind;

% Step 1 Weighting
P = P .* repmat(D./sqrt(D^2 + U.^2 + V.^2), [1 1 size(P,3)]);

% Step 2 Filtering
P = ifft(fft(P) .* repmat(H, [1 size(P,2) size(P,3)]), 'symmetric');

% Step 3 Normalizing for the backprojection:
P = double(0.5*1/size(P,3)*P);


function y = cbrt(x)
%CBRT   The real cube root
%
%   CBRT(X) is the real cube root of X (assuming X is real).  X
%   can be any shape.

  y = abs(x).^(1/3);
  y(x < 0) = -y(x < 0);
end


function RHS = convectionTvdRHSSpherical1D(u, phi, FL)
% This function uses the upwind scheme to discretize a 1D
% convection term in the form \grad (u \phi) where u is a face vactor
% It also returns the x and y parts of the matrix of coefficient.
%
% SYNOPSIS:
%   [M, RHS] = convectionTvdTermCylindrical1D(u, phi, FL)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nr = u.domain.dims(1);
% DXp = u.domain.cellsize.x(2:end-1);
dx = 0.5*(u.domain.cellsize.x(1:end-1)+u.domain.cellsize.x(2:end));
% r = u.domain.cellcenters.x;
rf = u.domain.facecenters.x;
RHS = zeros(Nr+2, 1);
psi_p = zeros(Nr+1,1);
psi_m = zeros(Nr+1,1);

% extract the velocity data
% note: size(ux) = [1:m+1, 1:n] and size(uy) = [1:m, 1:n+1]
ux = u.xvalue;

% calculate the upstream to downstream gradient ratios for u>0 (+ ratio)
% P is 3:Nr+2
% W is 2:Nr+1
% WW is 1:Nr
dphi_p = (phi.value(2:Nr+2)-phi.value(1:Nr+1))./dx;
rp = dphi_p(1:end-1)./fsign(dphi_p(2:end));
psi_p(2:Nr+1) = 0.5*FL(rp).*(phi.value(3:Nr+2)-phi.value(2:Nr+1));
psi_p(1) = 0.0; % left boundary will be handled explicitly

% calculate the upstream to downstream gradient ratios for u<0 (- ratio)
% P is 3:Nr+2
% W is 2:Nr+1
% WW is 1:Nr
rm = dphi_p(2:end)./fsign(dphi_p(1:end-1));
psi_m(1:Nr) = 0.5*FL(rm).*(phi.value(1:Nr)-phi.value(2:Nr+1));
psi_m(Nr+1) = 0.0; % right boundary will be handled explicitly

% reassign the east, west, north, and south velocity vectors for the
% code readability
ue = ux(2:Nr+1);		uw = ux(1:Nr);
re = rf(2:Nr+1);     rw = rf(1:Nr);

% find the velocity direction for the upwind scheme
ue_min = min(ue,0);	ue_max = max(ue,0);
uw_min = min(uw,0);	uw_max = max(uw,0);

% calculate the TVD correction term
RHS(2:Nr+1) = -(1./(1/3*(rf(2:Nr+1).^3-rf(1:Nr).^3))).*(re.^2.*(ue_max.*psi_p(2:Nr+1)+ue_min.*psi_m(2:Nr+1))- ...
              rw.^2.*(uw_max.*psi_p(1:Nr)+uw_min.*psi_m(1:Nr)));

end

function phi_out = fsign(phi_in)
% This function checks the value of phi_in and assigns an eps value to the
% elements that are less than or equal to zero, while keeping the signs of
% the nonzero elements
    phi_out = (abs(phi_in)>=eps).*phi_in+eps*(phi_in==0)+eps*(abs(phi_in)<eps).*sign(phi_in);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  BPSO and VPSO source codes version 1.0                           %
%                                                                   %
%  Developed in MATLAB R2011b(7.13)                                 %
%                                                                   %
%  Author and programmer: Seyedali Mirjalili                        %
%                                                                   %
%         e-Mail: ali.mirjalili@gmail.com                           %
%                 seyedali.mirjalili@griffithuni.edu.au             %
%                                                                   %
%       Homepage: http://www.alimirjalili.com                       %
%                                                                   %
%   Main paper: S. Mirjalili and A. Lewis, "S-shaped versus         %
%               V-shaped transfer functions for binary Particle     %
%               Swarm Optimization," Swarm and Evolutionary         %
%               Computation, vol. 9, pp. 1-14, 2013.                %
%                                                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [gBest,gBestScore,ConvergenceCurve]=BPSO(noP,Max_iteration,BPSO_num,CostFunction,noV)

%Initial Parameters for PSO
w=2;              %Inirtia weight
wMax=0.9;         %Max inirtia weight
wMin=0.4;         %Min inirtia weight
c1=2;
c2=2;
Vmax=6;

Velocity=zeros(noP,noV);%Velocity vector
Position=zeros(noP,noV);%Position vector

%////////Cognitive component/////////
pBestScore=zeros(noP);
pBest=zeros(noP,noV);
%////////////////////////////////////

%////////Social component///////////
gBestScore=inf;
gBest=zeros(1,noV);
%///////////////////////////////////

ConvergenceCurve=zeros(1,Max_iteration); %Convergence vector

%Initialization
for i=1:size(Position,1) % For each particle
    for j=1:size(Position,2) % For each variable
        if rand<=0.5
            Position(i,j)=0;
        else
            Position(i,j)=1;
        end
    end
end
for i=1:noP
    pBestScore(i)=inf;
end

for l=1:Max_iteration

    %Calculate cost for each particle
    for i=1:size(Position,1)
        fitness=CostFunction(Position(i,:));

        if(pBestScore(i)>fitness)
            pBestScore(i)=fitness;
            pBest(i,:)=Position(i,:);
        end
        if(gBestScore>fitness)
            gBestScore=fitness;
            gBest=Position(i,:);
        end
    end

    %update the W of PSO
    w=wMax-l*((wMax-wMin)/Max_iteration);
    %Update the Velocity and Position of particles
    for i=1:size(Position,1)
        for j=1:size(Position,2)
            %Equation (1)
            Velocity(i,j)=w*Velocity(i,j)+c1*rand()*(pBest(i,j)-Position(i,j))+c2*rand()*(gBest(j)-Position(i,j));

            if(Velocity(i,j)>Vmax)
                Velocity(i,j)=Vmax;
            end
            if(Velocity(i,j)<-Vmax)
                Velocity(i,j)=-Vmax;
            end

            if BPSO_num==1
                s=1/(1+exp(-2*Velocity(i,j))); %S1 transfer function
            end
            if BPSO_num==2
                s=1/(1+exp(-Velocity(i,j)));   %S2 transfer function
            end
            if BPSO_num==3
                s=1/(1+exp(-Velocity(i,j)/2)); %S3 transfer function
            end
            if BPSO_num==4
               s=1/(1+exp(-Velocity(i,j)/3));  %S4 transfer function
            end

            if BPSO_num<=4 %S-shaped transfer functions
                if rand<s % Equation (4) and (8)
                    Position(i,j)=1;
                else
                    Position(i,j)=0;
                end
            end

            if BPSO_num==5
                s=abs(erf(((sqrt(pi)/2)*Velocity(i,j)))); %V1 transfer function
            end
            if BPSO_num==6
                s=abs(tanh(Velocity(i,j))); %V2 transfer function
            end
            if BPSO_num==7
                s=abs(Velocity(i,j)/sqrt((1+Velocity(i,j)^2))); %V3 transfer function
            end
            if BPSO_num==8
                s=abs((2/pi)*atan((pi/2)*Velocity(i,j))); %V4 transfer function (VPSO)
            end

            if BPSO_num>4 && BPSO_num<=8 %V-shaped transfer functions
                if rand<s %Equation (10)
                    Position(i,j)=~Position(i,j);
                end
            end
        end
    end
    ConvergenceCurve(l)=gBestScore;
end
end


% function XF = fftBandpass(X, FS, FS1, FP1, FP2, FS2)
%
% Bandpass filter for the signal X (time x trials). An acuasal fft
% algorithm is applied (i.e. no phase shift). The filter functions is
% constructed from a Hamming window.
%
% Fs : sampling frequency
%
% The passbands (Fp1 Fp2) and stop bands (Fs1 Fs2) are defined as
%                 -----------
%                /           \
%               /             \
%              /               \
%             /                 \
%   ----------                   -----------------
%           Fs1  Fp1       Fp2  Fs2
%
% If no output arguments are assigned the filter function H(f) and
% impulse response are plotted.
%
% NOTE: for long data traces the filter is very slow.
%
%------------------------------------------------------------------------
% Ole Jensen, Brain Resarch Unit, Low Temperature Laboratory,
% Helsinki University of Technology, 02015 HUT, Finland,
% Report bugs to ojensen@neuro.hut.fi
%------------------------------------------------------------------------

%    Copyright (C) 2000 by Ole Jensen
%    This program is free software; you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation; either version 2 of the License, or
%    (at your option) any later version.
%
%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You can find a copy of the GNU General Public License
%    along with this package (4DToolbox); if not, write to the Free Software
%    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
function xf = fftBandpass(x,Fs,Fs1,Fp1,Fp2,Fs2)
    if size(x,1) == 1
        x = x';
    end
    % Make x even
    Norig = size(x,1);
    if rem(Norig,2)
        x = [x' zeros(size(x,2),1)]';
    end

    % Normalize frequencies
    Ns1 = Fs1/(Fs/2);
    Ns2 = Fs2/(Fs/2);
    Np1 = Fp1/(Fs/2);
    Np2 = Fp2/(Fs/2);

    % Construct the filter function H(f)
    N = size(x,1);
    Nh = N/2;

    B = fir2(N-1,[0 Ns1 Np1 Np2 Ns2 1],[0 0 1 1 0 0]);
    H = abs(fft(B));  % Make zero-phase filter function
    IPR = real(ifft(H));
    if nargout == 0
        subplot(2,1,1)
        f = Fs*(0:Nh-1)/(N);
        plot(f,H(1:Nh));
        xlim([0 2*Fs2])
        ylim([0 1]);
        title('Filter function H(f)')
        xlabel('Frequency (Hz)')
        subplot(2,1,2)
        plot((1:Nh)/Fs,IPR(1:Nh))
        xlim([0 2/Fp1])
        xlabel('Time (sec)')
        ylim([min(IPR) max(IPR)])
        title('Impulse response')
    end

    if size(x,2) > 1
        for k=1:size(x,2)
            xf(:,k) = real(ifft(fft(x(:,k)) .* H'));
        end
        xf = xf(1:Norig,:);
    else
        xf = real(ifft(fft(x') .* H));
        xf = xf(1:Norig);
    end
end


clear all; close all; clc;

% pull=[3 5 7 9 11];
% start=1:20;
% q=abs(start-pull(1));
% [x,n2]=min(q);
%
% for j=1:4
%    q=abs(start-pull(j));
%    [x,n2]=min(q)
%    start=start([1:n2-1 n2+1:(20-j)]);
%    start'
% end
%
% break

%%
L=10; x3=-L:0.1:L; n=length(x3)-1; % define domain
x2=x3(1:n); k=(2*pi/(2*L))*[0:n/2-1 -n/2:-1]; % k-vector
ye=exp(-(x2.^2)); ye2=exp((x2.^2)/2); % define Gaussians
for j=0:9      % loop through 10 modes
  yd=real(ifft(((i*k).^j).*fft(ye))); % 2nd derivative
  mode=((-1)^(j))*(((2^j)*factorial(j)*sqrt(pi))^(-0.5))*ye2.*yd;
  y(:,j+1)=(mode).';  % store modes as columns
end

x=x2(n/2+1-40:n/2+1+40);  % keep only -4<x<4
yharm=y(n/2+1-40:n/2+1+40,:);

%%
n=length(x);
f=(exp(-(x-0.5).^2)+3*exp(-2*(x+1.5).^2))';

for j=1:10  % full reconstruction
  a(j,1)=trapz(x,f.*yharm(:,j));
end
f2=yharm*a;
Efull(1)=log(norm(f2-f)+1);  % reconstruction error

for j=1:10  % matrix M reconstruction
   for jj=1:j
       Area=trapz(x,yharm(:,j).*yharm(:,jj));
       M(j,jj)=Area;
       M(jj,j)=Area;
   end
end
Cfull=cond(M)   % get condition number

%%  Willcox alg.

% 23    52    37    79

% sensor 1
for jloop=1:81

s=zeros(n,1);
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,1), pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f1=yharm*atild;
Er(1)=norm(f1-f);

con1(jloop)=cond(M2);

end

[s1,n1]=min(con1)

% sensor 2
jlook=[1:n1-1 n1+1:81];
for jloop=jlook

s=zeros(n,1);
s(n1)=1;
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,2), pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f2=yharm*atild;
Er(2)=norm(f2-f);

con2(jloop)=cond(M2);

end
[s2,n2]=min(con2(jlook))

% sensor 3
if n2>n1
   jlook=[1:n1-1 n1+1:n2-1 n2+1:81];
else
   jlook=[1:n2-1 n2+1:n1-1 n1+1:81];
end

for jloop=jlook

s=zeros(n,1);
s(n1)=1; s(n2)=1;
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,3), pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f3=yharm*atild;
Er(3)=norm(f3-f);

con3(jloop)=cond(M2);

end
[s3,n3]=min(con3(jlook))

% sensor 4
jlook=[1:n1-1 n1+1:n3-1 n3+1:n2-1 n2+1:81];
for jloop=jlook

s=zeros(n,1);
s(n1)=1; s(n2)=1;
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,4),pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f4=yharm*atild;
Er(4)=norm(f4-f);

con4(jloop)=cond(M2);

end
[s4,n4]=min(con4(jlook))

figure(1)
subplot(4,1,1), bar(log(con1),'Facecolor',[0.7 0.7 0.7]), axis([1 81 0 100])
subplot(4,1,2), bar(log(con2),'Facecolor',[0.7 0.7 0.7]), axis([1 81 0 100])
subplot(4,1,3), bar(log(con3),'Facecolor',[0.7 0.7 0.7]), axis([1 81 0 100])
subplot(4,1,4), bar(log(con4),'Facecolor',[0.7 0.7 0.7]), axis([1 81 0 100])

temp=zeros(n,1), temp(n1)=s1; subplot(4,1,1),hold on, bar(log(temp),'r')
temp=zeros(n,1), temp(n2)=s2; subplot(4,1,2),hold on, bar(log(temp),'r')
temp=zeros(n,1), temp(n3)=s3; subplot(4,1,3),hold on, bar(log(temp),'r')
temp=zeros(n,1), temp(n4)=s4; subplot(4,1,4),hold on, bar(log(temp),'r')

subplot(4,1,1), set(gca,'Xtick',[0 40 80],'Ytick',[0 50 100],'Xticklabel',{'','',''},'Yticklabel',{'','',''})
subplot(4,1,2), set(gca,'Xtick',[0 40 80],'Ytick',[0 50 100],'Xticklabel',{'','',''},'Yticklabel',{'','',''})
subplot(4,1,3), set(gca,'Xtick',[0 40 80],'Ytick',[0 50 100],'Xticklabel',{'','',''},'Yticklabel',{'','',''})
subplot(4,1,4), set(gca,'Xtick',[0 40 80],'Ytick',[0 50 100],'Xticklabel',{'','',''},'Yticklabel',{'','',''})

break

figure(2)
subplot(4,1,1), plot(x,f1,'k',x,f,'r','Linewidth',[2])
subplot(4,1,2), plot(x,f2,'k',x,f,'r','Linewidth',[2])
subplot(4,1,3), plot(x,f3,'k',x,f,'r','Linewidth',[2])
subplot(4,1,4), plot(x,f4,'k',x,f,'r','Linewidth',[2])

%%  Willcox:  condition number

% pull=[3 5 7 9 11];
% start=1:20;
% q=abs(start-pull(1));
% [x,n2]=min(q);
%
% for j=1:4
%    q=abs(start-pull(j));
%    [x,n2]=min(q)
%    start=start([1:n2-1 n2+1:(20-j)]);
%    start'
% end
%
% break
clear f1
clear f2
clear ns
ns=[];            % vector to keep track of measurement locations
jlook=1:81;   % loop over 81 positions

count=1;
for jsense=1:20   %  loop for 20 sensors

for jloop=1:(82-jsense)   % loop from 81 to 61 locations
s=zeros(n,1); s(ns)=1;
s(jlook(jloop))=1;

for j=1:10
   for jj=1:j    % matrix M
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
con(jloop)=cond(M2);  % compute condition number

end

[s1,n1]=min(con);   % find minimum condition number location
kond(jsense)=s1; clear con
ns=[ns n1];  % add sensor location

jlook=jlook([1:n1-1 n1+1:(81-count+1)]);
count=count+1;

% reconstruct
s=zeros(n,1);  s(ns)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f1(:,jsense)=yharm*atild;  % iterative reconstruction
Errr(jsense)=norm(f1(:,jsense)-f);  % iterative error
scum(:,jsense)=s;     % track iterative sensing matrix

end

figure(4)
subplot(2,1,2), bar(log(Errr+1))
subplot(2,1,1), bar(log(kond))

subplot(2,1,2), set(gca,'Xlim',[0 21],'Xtick',[1 5 10 15 20],'Xticklabel',{'','','','',''},'Ylim',[0 12],'Ytick',[0 4 8 12],'Yticklabel',{'','','',''})
subplot(2,1,1), set(gca,'Xlim',[0 21],'Xtick',[1 5 10 15 20],'Xticklabel',{'','','','',''},'Ylim',[0 42],'Ytick',[0 20 40],'Yticklabel',{'','',''})

%figure(6), bar(s)

figure(7),
titer=[1:20 25]; f1=[f1 f]; titer2=[9:20 25], f2=[f1(:,9:20) f];
subplot(2,2,1), waterfall(x,titer,f1.'), colormap([0 0 0]), view(-150,50)
subplot(2,2,2), waterfall(x,titer2,f2.'), colormap([0 0 0]), view(-150,50)

subplot(2,2,1), set(gca,'Zlim',[-2 8],'Xlim',[-4 4],'Xtick',[-4 0 4],'Xticklabel',{'','',''}, ...
    'Ylim',[0 25],'Ytick',[1 10 20],'Yticklabel',{'','',''},'Ztick',[-2 0 4 8],'Zticklabel',{'','','',''})
subplot(2,2,2), set(gca,'Zlim',[-.2 3],'Xlim',[-4 4],'Xtick',[-4 0 4],'Xticklabel',{'','',''}, ...
    'Ylim',[0 25],'Ytick',[1 10 20],'Yticklabel',{'','',''},'Ztick',[0 1 2 3],'Zticklabel',{'','','',''})

figure(8)
%bar3(scum)
axes('position',[.05 .05 .9 .35])
scum2=[scum(:,[1:11 13:19])];
pcolor(-scum2.'), colormap(hot), axis off

%bar3(scum2)

%caxis([0 1]), axis off


function [W,imageEdges] = ICgraph(I,dataW,dataEdgemap);
% [W,imageEdges] = ICgraph(I,dataW,dataEdgemap);
% Input:
% I = gray-level image
% optional parameters:
% dataW.sampleRadius=10;
% dataW.sample_rate=0.3;
% dataW.edgeVariance = 0.1;
%
% dataEdgemap.parametres=[4,3, 21,3];%[number of filter orientations, number of scales, filter size, elongation]
% dataEdgemap.threshold=0.02;
%
% Output:
% W: npixels x npixels similarity matrix based on Intervening Contours
% imageEdges: image showing edges extracted in the image
%
% Timothee Cour, Stella Yu, Jianbo Shi, 2004.

[p,q] = size(I);

if (nargin< 2) | isempty(dataW),
    dataW.sampleRadius=10;
    dataW.sample_rate=0.3;
    dataW.edgeVariance = 0.1;
end

if (nargin<3) | isempty(dataEdgemap),
    dataEdgemap.parametres=[4,3, 21,3];%[number of filter orientations, number of scales, filter size, elongation]
    dataEdgemap.threshold=0.02;
end

edgemap = computeEdges(I,dataEdgemap.parametres,dataEdgemap.threshold);
imageEdges = edgemap.imageEdges;
W = computeW(I,dataW,edgemap.emag,edgemap.ephase);


function [phi,delta,alam,dipdir,ierr] = focal_nd2pl(wanx,wany,wanz,wdx,wdy,wdz)

    % compute strike, dip, rake and dip directions from Cartesian components of the outward normal and slip vectors
    %
    %     usage:
    %     call nd2pl(anx,any,anz,dx,dy,dz,strike,dip,rake,dipdir,ierr)
    %
    %     arguments:
    %     anx,any,anz    components of fault plane outward normal vector in the
    %                    Aki-Richards Cartesian coordinate system (INPUT)
    %     dx,dy,dz       components of slip vector in the Aki-Richards
    %                    Cartesian coordinate system (INPUT)
    %     strike         strike angle in degrees (OUTPUT)
    %     dip            dip angle in degrees (OUTPUT)
    %     rake           rake angle in degrees (OUTPUT)
    %     dipdir         dip direction angle in degrees (OUTPUT)
    %     ierr           error indicator (OUTPUT)
    %
    %     errors:
    %     1              input vectors not perpendicular among each other
    %
    %
    %       call fpsset
    amistr=-360.;
    amastr=360.;
    amidip=0.;
    amadip=90.;
    amirak=-360.;
    amarak=360.;
    amitre=-360.;
    amatre=360.;
    amiplu=0.;
    amaplu=90.;
    orttol=2.;
    ovrtol=0.001;
    tentol=0.0001;
    dtor=0.017453292519943296;
    c360=360.;
    c90=90.;
    c0=0.;
    c1=1.;
    c2=2.;
    c3=3.;

    ierr = 0;
    [ang] = focal_angle(wanx,wany,wanz,wdx,wdy,wdz);
    if (abs(ang - c90) > orttol)
        disp(['ND2PL: input vectors not perpendicular, angle=' num2str(ang)]);
        ierr = 1;
    end
    [anorm,anx,any,anz] = focal_norm(wanx,wany,wanz);
    [dnorm,dx,dy,dz] = focal_norm(wdx,wdy,wdz);
    if (anz > c0)
        [anx,any,anz] = focal_invert(anx,any,anz);
        [dx,dy,dz] = focal_invert(dx,dy,dz);
    end

    if(anz == -c1)
        wdelta = c0;
        wphi = c0;
        walam = atan2(-dy, dx);
    else
        wdelta = acos(-anz);
        wphi = atan2(-anx, any);
        walam = atan2(-dz/sin(wdelta),dx*cos(wphi)+dy*sin(wphi));
    end
    phi = wphi/dtor;
    delta = wdelta/dtor;
    alam = walam/dtor;
    phi = mod(phi+c360, c360);
    dipdir = phi+c90;
    dipdir = mod(dipdir+c360,c360);
end


function varargout = logsumexp(varargin)
%LOGSUMEXP
%
% y = LOGSUMEXP(x)
%
% Computes/declares log of sum of exponentials log(sum(exp(x)))
%
% Implemented as evalutation based nonlinear operator. Hence, the convexity
% of this function is exploited to perform convexity analysis and rigorous
% modelling.

switch class(varargin{1})

    case 'double'
        x = varargin{1};
        varargout{1} = log(sum(exp(x)));

    case 'sdpvar'

        if min(size(varargin{1}))>1
            x = varargin{1};
            y = [];
            for i = 1:size(x,2)
                y = [y yalmip('define',mfilename,x(:,i))];
            end
            varargout{1} = y;
        elseif max(size(varargin{1}))==1
            varargout{1} = varargin{1};
        else
            varargout{1} = yalmip('define',mfilename,varargin{1});
        end

    case 'char'

        X = varargin{3};

        operator = struct('convexity','convex','monotonicity','none','definiteness','none','model','callback');
        operator.bounds = @bounds;
        operator.derivative = @(x) exp(x)./(sum(exp(x)));
        operator.convexhull = @convexhull;

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = X;

    otherwise
        error('SDPVAR/LOG called with CHAR argument?');
end

function [L, U] = bounds(xL,xU)

L = log(sum(exp(xL)));
U = log(sum(exp(xU)));

function [Ax, Ay, b] = convexhull(xL,xU)
Ax = [];
Ay = [];
b = [];


clc;
clearvars;
close all;
%rng default;

N = 1000;
A = spx.dict.simple.gaussian_mtx(N, N);
A = A' * A;
x = randn(N, 1);
b  = A * x;

[x, res, iter ] = cgsolve(A, b, 1e-2, N*5, 0);
spx.io.print.vector(x, 3);

options.tolerance = 1e-2;
options.max_iterations = N*5;
fprintf('\nTrying fast solver: \n\n')
result = spx.fast.cg(A, b, options);
x = result.x;
spx.io.print.vector(x, 3);


function SVP = svp(T)

% T = temperature on Kelvin scale
% SVP = saturation vapor pressure according to Goff & Gratch Equation (1946)
% SVP in Pascal

Ts = 373.15;    % standard temperature at steam point on Kelvin scale (Goff, 1965)
Ps = 101324.6;  % standard atmospheric pressure at steam point (Pascal)

log10SVP = -7.90298*(Ts./T - 1) + 5.02808*log10(Ts./T) -1.3816e-7*(10.^(11.344*(1 - T./Ts)) - 1) + 8.1328e-3*(10.^(3.49149*(1 - Ts./T)) - 1) + log10(Ps);

SVP = 10.^(log10SVP);


function [Hv] = LogisticHv(v,w,X,y)
% v(feature,1) - vector that we will multiply Hessian by
% w(feature,1)
% X(instance,feature)
% y(instance,1)

sig = 1./(1+exp(-y.*(X*w)));
Hv = X.'*(sig.*(1-sig).*(X*v));


function [m3] = cc2m3(cc)
% Convert volume from cubic centimeters to cubic meters.
% Chad Greene 2012
m3 = cc* 0.000001;


function [xPred, PPred, yPred, S, Pxy] = KalmanFilterX_Predict(x,P,F,Q,H,R,u,B,O)
% KALMANFILTERX_PREDICT Perform the discrete-time KF state and measurement
% prediction steps, under the assumption of additive process noise.
%
% Parameters
% ----------
% x: column vector
%   The (xDim x 1) state estimate at the previous time-step.
% P: matrix
%   The (xDim x xDim) state covariance matrix at the previous
%   time-step.
% F: matrix
%   An (xDim x xDim) state transition matrix.
% Q: matrix
%   The (xDim x xDim) process noise covariance matrix.
% H: matrix
%   A (xDim x yDim) measurement matrix.
% R: matrix
%   The (yDim x yDim) measurement noise covariance matrix.
% u: column vector, optional
%   A optional (xDim x 1) control input.
%   If omitted, no control input is used.
% B: matrix, optional
%   An optional (xDim x xDim) control gain matrix.
%   If omitted, B is assumed to be 1.
% O: matrix, optional
%   An optional (xDim x xDim) control noise covariance
%   matrix. If omitted, Q is assumed to be 0.
%
% Returns
% -------
% xPred: column vector
%   The (xDim x 1) predicted state estimate.
% PPred: matrix
%   The (xDim x xDim) predicted state covariance matrix.
% yPred: column vector
%   The (yDim x 1) predicted measurement estimate.
% Pxy: matrix
%   The (xDim x yDim) cross-covariance matrix.
% S: matrix
%   The (yDim x yDim) innovation covariance matrix.
%
%October 2017 Lyudmil Vladimirov, University of Liverpool.

    switch(nargin)
        case(6)
            u = 0;
            B = 0;
            O = 0;
        case(7)
            B = 1;
            O = 0;
        case(8)
            O = 0;
    end

   [xPred, PPred] = KalmanFilterX_PredictState(x,P,F,Q,u,B,O);
   [yPred, S, Pxy] = KalmanFilterX_PredictObs(xPred,PPred,H,R);
end


function A = create_2d_image_upsample_matrix(insz, outsz, varargin)
    % (insz, outsz, {interp_type})
    interp_type = 1;
    if nargin >= 3
        interp_type = varargin{1};
    end
    [m1, m2] = ndgrid(1:outsz(1), 1:outsz(2));
    kk = (insz - 1) ./ (outsz - 1);
    dd = 1 - kk;
    t1 = kk(1) * m1 + dd(1);
    t2 = kk(2) * m2 + dd(2);
    T = cat(3, t1 - m1, t2 - m2);
    if interp_type == 0
        A = create_2d_bilinear_interp_matrix(T, insz);
    elseif interp_type == 1
        A = create_2d_bicubic_interp_matrix(T, insz);
    end
end


function [pf,f]=lpccc2pf(cc,np,nc,c0)
%LPCCC2PF Convert complex cepstrum to power spectrum PF=(CC,NP,NC)
%
%  Inputs: cc(nf,n)     Complex ceptral coefficients excluding c(0), one frame per row
%          np           Size of output spectrum is np+1 [n]
%                       Alternatively, np can be a vector of output frequencies in the range 0 to 0.5
%          nc           Highest cepstral coefficient to use [np or, if np is a vector, n]
%                       Set nc=-1 to use n coefficients
%          c0(nf,1)     Cepstral coefficient cc(0) [0]
%
% Outputs: pf(nf,np+1)  Power spectrum from DC to Nyquist
%          f(1,np+1)    Normalized frequencies (0 to 0.5)
%
% The "complex cepstral coefficients", cc(n), are the inverse discrete-time Fourier transform
% of the log of the complex-valued spectrum. The cc(n) are real-valued and, for n<0, cc(n)=0.
% The "real cepstral coeffcients", rc(n), are the inverse discrete-time Fourier transform
% of the log of the magnitude spectrum; rc(0)=cc(0) and rc(n)=0.5*cc(n) for n~=0.
% For highest speed, choose np to be a power of 2.

%      Copyright (C) Mike Brookes 1998-2014
%      Version: $Id: lpccc2pf.m 5026 2014-08-22 17:47:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nf,mc]=size(cc);
if nargin<2 || ~numel(np)
    if nargout
        np=mc;
    else
        np=128;
    end
end
if nargin>=3 && numel(nc)==1 && nc==-1 nc=mc; end
if nargin<4 || ~numel(c0) c0=zeros(nf,1); end
if numel(np)>1 || np(1)<1
    if nargin<3 || ~numel(nc) nc=mc; end
    f=np(:)';
    if nc==mc
        pf=exp(2*[c0 cc]*cos(2*pi*(0:mc)'*f));
    else
        pf=exp(2*[c0 lpccc2cc(cc,nc)]*cos(2*pi*(0:nc)'*f));
    end
else
    if nargin<3 || ~numel(nc) nc=np; end
    if nc==mc
        pf=exp(2*real(rfft([c0 cc].',2*np).'));
    else
        pf=exp(2*real(rfft([c0 lpccc2cc(cc,nc)].',2*np).'));
    end
    f=linspace(0,0.5,np+1);
end
if ~nargout
    plot(f,db(pf.')/2);
    xlabel('Normalized frequency f/f_s');
    ylabel('Gain (dB)');
end


function [ tet_index, face, step_num ] = tet_mesh_search_delaunay ( ...
  node_num, node_xyz, tet_order, tet_num, tet_node, tet_neighbor, p )

%*****************************************************************************80
%
%% TET_MESH_SEARCH_DELAUNAY searches a Delaunay tet mesh for a point.
%
%  Discussion:
%
%    The algorithm "walks" from one tetrahedron to its neighboring tetrahedron,
%    and so on, until a tetrahedron is found containing point P, or P is found
%    to be outside the convex hull.
%
%    The algorithm computes the barycentric coordinates of the point with
%    respect to the current tetrahedron.  If all 4 quantities are positive,
%    the point is contained in the tetrahedron.  If the I-th coordinate is
%    negative, then P lies on the far side of edge I, which is opposite
%    from vertex I.  This gives a hint as to where to search next.
%
%    For a Delaunay tet mesh, the search is guaranteed to terminate.
%    For other meshes, a cycle may occur.
%
%    Note the surprising fact that, even for a Delaunay tet mesh of
%    a set of nodes, the nearest node to P need not be one of the
%    vertices of the tetrahedron containing P.
%
%    The code can be called for tet meshes of any order, but only
%    the first 4 nodes in each tetrahedron are considered.  Thus, if
%    higher order tetrahedrons are used, and the extra nodes are intended
%    to give the tetrahedron a polygonal shape, these will have no effect,
%    and the results obtained here might be misleading.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 August 2009
%
%  Author:
%
%    John Burkardt.
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XYZ(3,NODE_NUM), the coordinates of
%    the nodes.
%
%    Input, integer TET_ORDER, the order of the tetrahedrons.
%
%    Input, intege TET_NUM, the number of tetrahedrons.
%
%    Input, integer TET_NODE(TET_ORDER,TET_NUM),
%    the nodes that make up each tetrahedron.
%
%    Input, integer TET_NEIGHBOR(4,TET_NUM), the
%    tetrahedron neighbor list.
%
%    Input, real P(3), the coordinates of a point.
%
%    Output, integer TET_INDEX, the index of the tetrahedron
%    where the search ended.  If a cycle occurred, then TET_INDEX = -1.
%
%    Output, integer FACE, indicates the position of the point P in
%    face TET_INDEX:
%    0, the interior or boundary of the tetrahedron;
%    -1, outside the convex hull of the tet mesh, past face 1;
%    -2, outside the convex hull of the tet mesh, past face 2;
%    -3, outside the convex hull of the tet mesh, past face 3.
%    -4, outside the convex hull of the tet mesh, past face 4.
%
%    Output, integer STEP_NUM, the number of steps taken.
%
  persistent tet_index_save;

  if ( length ( tet_index_save ) == 0 )
    tet_index_save = -1;
  end
%
%  If possible, start with the previous successful value of TET_INDEX.
%
  if ( tet_index_save < 1 | tet_num < tet_index_save )
    tet_index = floor ( ( tet_num + 1 ) / 2 );
  else
    tet_index = tet_index_save;
  end

  step_num = -1;
  face = 0;

  while ( 1 )

    step_num = step_num + 1;

    if ( tet_num < step_num )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'TET_MESH_SEARCH_DELAUNAY - Fatal error!\n' );
      fprintf ( 1, '  The algorithm seems to be cycling.\n' );
      tet_index = -1;
      face = -1;
      error ( 'TET_MESH_SEARCH_DELAUNAY - Fatal error!' );
    end

    alpha = tetrahedron_barycentric ( node_xyz(1:3,tet_node(1:4,tet_index)), p );
%
%  If the barycentric coordinates are all positive, then the point
%  is inside the tetrahedron and we're done.
%
    if ( 0.0 <= alpha(1) & 0.0 <= alpha(2) & 0.0 <= alpha(3) & 0.0 <= alpha(4) )
      break
    end
%
%  At least one barycentric coordinate is negative.
%
%  If there is a negative barycentric coordinate for which there exists an
%  opposing tetrahedron neighbor closer to the point, move to that tetrahedron.
%
    if ( alpha(1) < 0.0 & 0 < tet_neighbor(1,tet_index) )
      tet_index = tet_neighbor(1,tet_index);
      continue
    elseif ( alpha(2) < 0.0 & 0 < tet_neighbor(2,tet_index) )
      tet_index = tet_neighbor(2,tet_index);
      continue
    elseif ( alpha(3) < 0.0 & 0 < tet_neighbor(3,tet_index) )
      tet_index = tet_neighbor(3,tet_index);
      continue
    elseif ( alpha(4) < 0.0 & 0 < tet_neighbor(4,tet_index) )
      tet_index = tet_neighbor(4,tet_index);
      continue
    end
%
%  All negative barycentric coordinates correspond to vertices opposite
%  faces on the convex hull.
%
%  Note the face and exit.
%
    if ( alpha(1) < 0.0 )
      face = -1;
      break
    elseif ( alpha(2) < 0.0 )
      face = -2;
      break
    elseif ( alpha(3) < 0.0 )
      face = -3;
      break
    elseif ( alpha(4) < 0.0 )
      face = -4;
      break
    end

  end

  tet_index_save = tet_index;

  return
end


function yp = p33_fun ( neqn, t, y )

%*****************************************************************************80
%
%% P33_FUN evaluates the function for problem P33.
%
%  Discussion:
%
%    2 equation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Wayne Enright, John Pryce,
%    Algorithm 648,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 1, pages 28-34.
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the derivative
%    function.
%
%    Output, real YP(NEQN), the value of the derivative function.
%
  yp = zeros ( neqn, 1 );

  delta = p33_param ( 'GET', 'DELTA' );
  yp(1) = y(2);
  yp(2) = delta * ( 1.0 - y(1).^2 ) * y(2) - y(1);

  return
end


function yp = p20_fun ( neqn, t, y )

%*****************************************************************************80
%
%% P20_FUN evaluates the function for problem P20.
%
%  Discussion:
%
%    4 equations
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Wayne Enright, John Pryce,
%    Algorithm 648,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 1, pages 28-34.
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the derivative
%    function.
%
%    Output, real YP(NEQN), the value of the derivative function.
%
  yp = zeros ( neqn, 1 );

  yp(1) = y(3);
  yp(2) = y(4);
  yp(3) = -y(1) / ( sqrt ( ( y(1).^2 + y(2).^2 ) ) ).^3;
  yp(4) = -y(2) / ( sqrt ( ( y(1).^2 + y(2).^2 ) ) ).^3;

  return
end


function [ iarray, more ] = vec_next ( n, ibase, iarray, more )

%*****************************************************************************80
%
%% VEC_NEXT generates all N-vectors of integers modulo a given base.
%
%  Discussion:
%
%    The items are produced one at a time.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 June 2013
%
%  Parameters:
%
%    Input, integer N, the size of the vectors to be used.
%
%    Input/output, integer IARRAY(N).  On each return from VECNEX,
%    IARRAY will contain entries in the range 0 to IBASE-1.
%
%    Input/output, logical MORE.  Set this variable .FALSE. before
%    the first call.  Normally, MORE will be returned .TRUE. but
%    once all the vectors have been generated, MORE will be
%    reset .FALSE. and you should stop calling the program.
%
%    Input, integer IBASE, the base to be used.  IBASE = 2 will
%    give vectors of 0's and 1's, for instance.
%
  persistent kount
  persistent last

  if ( ~ more )

    kount = 1;
    last = ibase ^ n;
    more = 1;
    iarray(1:n) = 0;

  else

    kount = kount + 1;

    if ( kount == last )
      more = 0;
    end

    iarray(n) = iarray(n) + 1;

    for i = 1 : n

      nn = n - i;

      if ( iarray(nn+1) < ibase )
        return
      end

      iarray(nn+1) = 0;

      if ( nn ~= 0 )
        iarray(nn) = iarray(nn) + 1;
      end

    end

  end

  return
end


function [S,m,psi,chi,del] = ComputeStokesDual(HH,HV,SmoothSize)
%COMPUTESTOKESDUAL Computes Stokes parameters for dual pol complex data

%can be VV/VH as well
[ny,nx] = size(HH);

%Stokes is defines as:
% S(1): Span: HH^2+HV^2
% S(2): Difference: HH^2-HV^2
% S(3): Cov (real): 2*real(HH*HV)
% S(4): Cov (imag): -2*imag(HH*HV)
S = zeros(ny,nx,4);
S(:,:,1) = fastrunmean(abs(HH.*HH)+abs(HV.*HV),[SmoothSize SmoothSize],'mean');
S(:,:,2) = fastrunmean(abs(HH.*HH)-abs(HV.*HV),[SmoothSize SmoothSize],'mean');
S(:,:,3) = fastrunmean(2*real(HH.*conj(HV)),[SmoothSize SmoothSize],'mean');
S(:,:,4) = fastrunmean(-2*imag(HH.*conj(HV)),[SmoothSize SmoothSize],'mean');

%sum of Stokes 2-4 divided by span
m = sqrt(S(:,:,2).^2 + S(:,:,3).^2 + S(:,:,4).^2)./S(:,:,1);

%psi is the orientation angle between 2/3
psi = atan2d(S(:,:,3),S(:,:,2))/2;

chi = asind(S(:,:,4)./(m.*S(:,:,1)))/2;

%del is the phase angle between H & V
del = atan2d(S(:,:,4),S(:,:,3));

end


function [logp, yhat, res] = tapas_logrt_linear_binary(r, infStates, ptrans)
% Calculates the log-probability of log-reaction times y (in units of log-ms) according to the
% linear log-RT model developed with Louise Marshall and Sven Bestmann
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2014-2016 Christoph Mathys, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform parameters to their native space
be0  = ptrans(1);
be1  = ptrans(2);
be2  = ptrans(3);
be3  = ptrans(4);
be4  = ptrans(5);
ze   = exp(ptrans(6));

% Initialize returned log-probabilities, predictions,
% and residuals as NaNs so that NaN is returned for all
% irregualar trials
n = size(infStates,1);
logp = NaN(n,1);
yhat = NaN(n,1);
res  = NaN(n,1);

% Weed irregular trials out from responses and inputs
y = r.y(:,1);
y(r.irr) = [];

u = r.u(:,1);
u(r.irr) = [];

% Extract trajectories of interest from infStates
mu1hat = infStates(:,1,1);
sa1hat = infStates(:,1,2);
mu2    = infStates(:,2,3);
sa2    = infStates(:,2,4);
mu3    = infStates(:,3,3);

% Surprise
% ~~~~~~~~
m1hreg = mu1hat;
m1hreg(r.irr) = [];
poo = m1hreg.^u.*(1-m1hreg).^(1-u); % probability of observed outcome
surp = -log2(poo);

% Bernoulli variance (aka irreducible uncertainty, risk)
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
bernv = sa1hat;
bernv(r.irr) = [];

% Inferential variance (aka informational or estimation uncertainty, ambiguity)
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
inferv = tapas_sgm(mu2, 1).*(1 -tapas_sgm(mu2, 1)).*sa2; % transform down to 1st level
inferv(r.irr) = [];

% Phasic volatility (aka environmental or unexpected uncertainty)
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
pv = tapas_sgm(mu2, 1).*(1-tapas_sgm(mu2, 1)).*exp(mu3); % transform down to 1st level
pv(r.irr) = [];

% Calculate predicted log-reaction time
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
logrt = be0 +be1.*surp +be2.*bernv +be3.*inferv +be4.*pv;

% Calculate log-probabilities for non-irregular trials
% Note: 8*atan(1) == 2*pi (this is used to guard against
% errors resulting from having used pi as a variable).
reg = ~ismember(1:n,r.irr);
logp(reg) = -1/2.*log(8*atan(1).*ze) -(y-logrt).^2./(2.*ze);
yhat(reg) = logrt;
res(reg) = y-logrt;

return;


function noise_filt=VCO_noise_profile_3(Npts,Level_dB,Freq,OverSample,Interp_Select,Verify);
% Npts = length of filter kernel. More provides increased freq resolution
% Freq     = Vector of frequencies in Hz
% Level_dB = Corresponding Vector of noise levels
% OverSample = Sets Fs , must be > 2*max freq spec
% Verify   = true to show plots
% Create a filter kernel (FIR coefficients) that approximates a user spec'd
% power spectrum Level is dB/Hz
% To get dBc, carrier must be 1 Watt.
% Dick Benson  December 2009
% Copyright 2009-2013 The MathWorks, Inc.

L     = round(Npts/2);
f     =(0:(L-1))/L;     % frequency vector normalized to Fs/2
ph    = reshape([0;pi]*ones(1,L/2),L,1)'; % phase curve

Fs = OverSample*Freq(end);

switch Interp_Select
   case 1
     % linear X axis
     Level    = [Level_dB(1),Level_dB,Level_dB(end)];    % pad start and end
     Freq     = [0,          Freq,    Fs/2 ];            % ditto
     % linearly interpolate in the dB realm.
     shape_dB = interp1(Freq,Level,f*Fs/2,'linear');
     shape    =   10.^(shape_dB/20);
   case 2
     % log x axis
     Level    = [Level_dB,Level_dB(end)];    % pad  end
     Freq     = [Freq, Fs/2 ];        % ditto
     % linearly interpolate in the dB realm.
     log_Freq= log10(Freq);

     shape_dB = interp1(log_Freq,Level,log10(f*Fs/2),'linear');
     % remove any NaNs and pad
     bogus_nans = isnan(shape_dB);
     if sum(bogus_nans)==0
        shape    =   10.^(shape_dB/20);
     else
        N=sum(bogus_nans);
        shape    =   10.^([shape_dB(N+1:2*N),shape_dB((N+1):end)]/20);
     end;
   end

  dFspec = min(diff(Freq(2:end)));
  dF     = Fs/Npts;
  if dFspec<dF
     warndlg(['The frequency resolution of the Spec is greater than can be achieved. \n Consider Increasing the number of Points in the FIR Kernel, or removing the low Frequency Specs.',sprintf('F=%10.2f  ',Freq(2:3)), 'etc.'],[ 'Freq REsolution']);
  end;

shape_2 = shape.*f*Fs/2; % multiply by Frequency to create derivative
% Need to create a complex spectral description
% which, when transformed with the ifft, creates a REAL impulse response.
mag=[shape_2,0, fliplr(shape_2(2:end))]; % construct magnitude
phase=[-ph,0,fliplr(ph(2:end))];     % and phase
H=mag.*exp(1i*phase); % complex representation
h=ifft(H);            % Filter Kernel

%  sanity checks ....
if Verify
   hf    =  findobj('Tag','plot_VCO');
   if isempty(hf)
      hf =  figure('Tag','plot_VCO');
   else
      figure(hf);
   end

   subplot(2,1,1)
   plot((h)); title('Filter Impulse Response (no window)');
   xlabel('samples')
end;

% If the filter is used as is, there may be ripples in the frequency
% reponse due to the abrupt transistions at the impulse response endpoints.
% To mitigate this, apply a Hanning window and compare computed response with
% the spec and interpoloted spec.

hout=h.*hann(2*L)';

if Verify
   H2=fft(hout);            % take fft of windowed filter kernel
   subplot(2,1,2)
   % undo derivative by dividing by frequency before displaying
   semilogx(f/2*Fs,20*log10(abs(H2(1:L)./(f.*Fs/2))),f/2*Fs,20*log10(shape),'x',Freq,Level,'o');
   legend('Filter Response','Interpolated Spec','Spec','location','southwest');
   title('Freq Responses of Specification and Attained Noise Profiles.  ');
   xlabel('Freq'); ylabel('dB');
end;

noise_filt=hout;


function y = perform_dct2_transform(x,dir,dim)

% perform_dct2_transform - perform the DCT2 transform
%
%   y = perform_dct2_transform(x);
%
%   Copyright (c) 2007

if nargin<2
    dir=1;
end
if nargin<3
    dim = 1;
    if size(x,1)>1 & size(x,2)>1
        dim = 2;
    end
end

n = size(x,1);
if dim==1
    % perform 1D DCT
    p = prod(size(x))/n;
    if 0
        %% implementation with size x 4 FFT
        y = zeros(4*n,p);
        y(2:2:2*n,:) = x(:,:);
        y(4*n:-2:2*n+1,:) = x(:,:);
        y = fft(y,[],1);
        y = real( y(1:n,:) ) / sqrt(2*n);
        y(1,:) = y(1,:)/sqrt(2);
    else
        %% implementation with size x 2 FFT
        w = exp( -1i*pi/(2*n)*(0:n-1)');
        y = cat(1, x(:,:),zeros(n,p) );
        y = fft(y, [], 1);
        y = y(1:n,:);
        y = sqrt(2/n) * real( y.*repmat( w, [1 p] ) );
        y(1,:) = y(1,:)/sqrt(2);
    end
    y = reshape(y,size(x));
    return;
end

%% apply on 1st dimension
s = size(x); x = x(:,:);
y = perform_dct2_transform(x,dir,1);
y = permute(y,[2 1 3:10]);
y = perform_dct2_transform(y,dir,1);
y = permute(y,[2 1 3:10]);

return;

if size(x,2)==1 || dim==1
    % 1D transform
    y = zeros(4*n,size(x,2),size(x,3),size(x,4));
    y(2:2:2*n,:,:,:) = x; y(4*n:-2:2*n+1,:,:,:) = x;
    y = fft(y,[],1);
    y = real( y(1:n,:,:,:) ) / sqrt(2*n);
    y(:,1,:,:) = y(:,1,:,:)/sqrt(2);
else
    if dir==1
        if  0
        y = x;
        y(1:end/2,:,:,:) = x(1:2:end,:,:,:);
        y(end:-1:end/2+1,:,:,:) = x(2:2:end,:,:,:);
        y = fft(y,[],1);
        y = real( y.*repmat( exp(-1i*pi/(2*n)*(0:n-1)'), [1,size(x,2),size(x,3),size(x,4)]  ) );
        end

        % transform on x
        y = zeros(4*n,size(x,2),size(x,3),size(x,4));
        y(2:2:2*n,:,:,:) = x; y(4*n:-2:2*n+1,:,:,:) = x;
        y = fft(y,[],1);
        y = real( y(1:n,:,:,:) );
        % transform on y
        z = zeros(size(x,1),4*n,size(x,3),size(x,4));
        z(:,2:2:2*n,:,:) = y; z(:,4*n:-2:2*n+1,:,:) = y;
        z = fft(z,[],2);
        y = real( z(:,1:n,:,:) ) / (2*n);
        % y(:,1,:,:) = y(:,1,:,:)/sqrt(2);
        % y(1,:,:,:) = y(1,:,:,:)/sqrt(2);
    else
        error('Not yet implemented (DCT3)');

    end
end


classdef MultiSubspaceSignalGenerator < handle
    %MultiSubspaceSignalGenerator creates sparse signals
    % Usage:
    %   - Create generator with ambient dimension N and sparsity level K
    %   - Design the support for subspaces
    %   - Specify number of signals for each subspace
    %   - Generate signals for each subspace
    properties(SetAccess=private)
        % The ambient signal dimension
        N
        % The sparsity level
        K
        % The resultant vectors
        X
        % The Sparse support for each subspace
        Supports
        % The number of signals in each Subspace
        NumSignalsPerSubspace = []
        % Labels identify the subspace to which a signal belongs
        Labels = []
    end

    methods
        function self = MultiSubspaceSignalGenerator(N, K)
            self.N = N;
            self.K = K;
        end

        function createDisjointSupports(self, numSubspaces)
            n = self.N;
            k  = self.K;
            c = numSubspaces;
            if c*k > n
                error('Total cardinality of subspaces cannot exceed ambient dimensions');
            end
            % Select c*k indices randomly from 1:N
            q = randperm(n, c*k);
            % Arrange the support for each subspace.
            self.Supports = sort(reshape(q, k, c));
            % By default we will create one signal per subspace
            self.NumSignalsPerSubspace = ones(numSubspaces, 1);
        end

        function createOverlappingSupports(self, numSubspaces, overlap)
            % Overlap specifies number of indices which overlap between
            % two consecutive supports.
            n = self.N;
            k  = self.K;
            c = numSubspaces;
            l = c*(k-overlap)+overlap;
            if  l > n
                error('Total cardinality of subspaces cannot exceed ambient dimensions, n=%d, l=%d', n, l);
            end
            % Select l indices randomly from 1:N
            q = randperm(n, l);
            % Arrange the support for each subspace.
            qs = zeros(k,c);
            start = 0;
            for i=1:c
                qs(:, i) = q(start + (1:k));
                start = start + k - overlap;
            end
            self.Supports = sort(qs);
            % By default we will create one signal per subspace
            self.NumSignalsPerSubspace = ones(numSubspaces, 1);
        end

        function setNumSignalsPerSubspace(self, numSignalsPerSubspace)
            c = length(self.NumSignalsPerSubspace);
            data = numSignalsPerSubspace;
            if isscalar(data)
                self.NumSignalsPerSubspace = data *ones(c, 1);
                return;
            end
            % Verify that numSignalsPerSubspace has appropriate size
            if length(data) ~= c
                error('Number of subspaces mismatch');
            end
            % Make sure its a column vector
            if isrow(data)
                data = data';
            end
            % save it
            self.NumSignalsPerSubspace = data;
        end

        function result = uniform(self, a,b)
            % Generates sparse signals from uniform distribution
            if nargin < 3
                a = 0; b =1;
            elseif nargin < 4
                b = -a;
            end
            if b <= a
                error('b must be larger than a');
            end
            % sparsity level
            k = self.K;
            % number of signals per subspace
            signalCounts = self.NumSignalsPerSubspace;
            % total number of signals
            totalSignals = sum(signalCounts);
            % Ambient dimensions
            n = self.N;
            % supports
            qs = self.Supports;
            % Number of subspaces
            c = length(qs);
            % Create space for signals
            self.X = zeros(n, totalSignals);
            self.Labels = zeros(totalSignals, 1);
            % Iterate over subspaces
            start = 0;
            for i=1:c
                % support for this subspace
                q = qs(:, i);
                % number of signals
                s = signalCounts(i);
                % Generate signals
                xx = a + (b-a).*rand(k,s);
                % Assign into combined set
                self.X(q, start + (1:s)) = xx;
                self.Labels(start + (1:s)) = i;
                % update start for next set
                start = start + s;
            end
            % Store the results
            result = self.X;
        end

        function result = biUniform(self, a, b, zeroProbability)
            % Generates sparse vectors where each non-zero values
            % is picked up uniformly from the ranges
            % [-b, -a] and [a, b]
            if nargin < 3
                a = 1; b =2;
            elseif nargin < 4
                b = 2*a;
            end
            if a < 0 || b < 0
                error('a and b both must be +ve');
            end
            if b <= a
                error('b must be larger than a');
            end
            if ~exist('zeroProbability', 'var')
                zeroProbability = 0;
            end
            if zeroProbability > 1 || zeroProbability < 0
                error('zeroProbability must be in [0,1]');
            end
            % sparsity level
            k = self.K;
            % number of signals per subspace
            signalCounts = self.NumSignalsPerSubspace;
            % total number of signals
            totalSignals = sum(signalCounts);
            % Ambient dimensions
            n = self.N;
            % supports
            qs = self.Supports;
            % Number of subspaces
            c = size(qs,2);
            % Create space for signals
            self.X = zeros(n, totalSignals);
            self.Labels = zeros(totalSignals, 1);
            % Iterate over subspaces
            start = 0;
            for i=1:c
                % support for this subspace
                q = qs(:, i);
                % number of signals
                s = signalCounts(i);
                % Generate signals
                % unsigned result
                xx = a + (b-a).*rand(k,s);
                % sign
                sgn = sign(randn(k,s));
                if zeroProbability
                    % We need to introduce some zeros in between
                    sgn = sgn .* binornd(1, 1 - zeroProbability, k, s);
                end
                % Assign into combined set
                self.X(q, start + (1:s)) = sgn .* xx;
                self.Labels(start + (1:s)) = i;
                % update start for next set
                start = start + s;
            end
            % Store the results
            result = self.X;
        end
    end

end


function fresnel_cos_values_test ( )

%*****************************************************************************80
%
%% FRESNEL_COS_VALUES_TEST demonstrates the use of FRESNEL_COS_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FRESNEL_COS_VALUES_TEST:\n' );
  fprintf ( 1, '  FRESNEL_COS_VALUES stores values of\n' );
  fprintf ( 1, '  the Fresnel cosine integral C(X).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           C(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = fresnel_cos_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function R = project2SO3(M)

if size(M,1) ~= 3 || size(M,2) ~= 3
    error('project2SO3 requires a 3x3 matrix as input')
end

[U,S,V] = svd(M);
R = U*V';
if(det(R)<0)
  R = U * diag([1 1 -1]) * V';
end

end


% Liou-Steffen scheme with limiting: Eulerstep in Runge-Kutta time stepping
% Called by Liou_Steffen_MUSCL_scheme

% Theory in Sections 10.3 and 10.8 of:

% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% 	See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% Function called: extrap

  [U1L, U1R] = extrap(rhostar,limtype);   	% Left and right
  [U2L, U2R] = extrap(mstar,limtype);		%extrapolated
  [U3L, U3R] = extrap(totenstar,limtype);	%states
  uL = U2L./U1L; uR = U2R./U1R;
  cL = sqrt(gamma*gam1*(U3L./U1L - 0.5*uL.^2));
  cR = sqrt(gamma*gam1*(U3R./U1R - 0.5*uR.^2));
  pL = U1L.*cL.^2/gamma; pR = U1R.*cR.^2/gamma;
  machL = uL./cL;  machR = uR./cR;
  rhocL = U1L.*cL; rhocR = U1R.*cR;
  enthalpyL = cL.^2/(gamma-1) + 0.5*uL.^2;
  enthalpyR = cR.^2/(gamma-1) + 0.5*uR.^2;

% Preallocations
  machplus = machL; machminus = machL; machhalf = machL;
  presplus = machL; presminus = machL;

  for j = 1:J-1
    if abs(machL(j)) > 1
      machplus(j) = 0.5*(machL(j) + abs(machL(j)));
      presplus(j) = 0.5*pL(j)*(1 + abs(machL(j))/machL(j));
    else
      machplus(j) = 0.25*(machL(j) + 1)^2;
      presplus(j) = 0.5*pL(j)*(1 + machL(j));
    end
    if abs(machR(j)) > 1
      machminus(j) = 0.5*(machR(j) - abs(machR(j)));
      presminus(j) = 0.5*pR(j)*(1 - abs(machR(j))/machR(j));
    else
      machminus(j) = - 0.25*(machR(j) - 1)^2;
      presminus(j) = 0.5*pR(j)*(1 - machR(j));
    end
  end

% Liou-Steffen fluxes
  machhalf = machplus + machminus;
  flux1a = 0.5*(machhalf + abs(machhalf)).*rhocL;
  flux1b = 0.5*(machhalf - abs(machhalf)).*rhocR;
  flux1  = flux1a + flux1b;
  flux2  = flux1a.*uL + flux1b.*uR + presplus + presminus;
  flux3  = flux1a.*enthalpyL + flux1b.*enthalpyR;

% Update of state variables
  rhostar(1) = rholeft; rhostar(J) = rhoright;
  mstar(1) = rholeft*uleft; mstar(J) = rhoright*uright;
  totenstar(1) = totenleft; totenstar(J) = totenright;
  for j = 2:J-1
    rhostar(j)   = rhoold(j)   - rkalpha*lambda*(flux1(j) - flux1(j-1));
    mstar(j)     = mold(j)     - rkalpha*lambda*(flux2(j) - flux2(j-1));
    totenstar(j) = totenold(j) - rkalpha*lambda*(flux3(j) - flux3(j-1));
  end


function cartPoints=pol2Cart(z,systemType,useHalfRange,zTx,zRx,M)
%%POL2CART Convert points from bistatic polar coordinates to 2D Cartesian
%          coordinates. The angle can measured either counterclockwise
%          from the x-axis, which is standard in mathematics, or clockwise
%          from the y axis, which is more common in navigation.
%
%INPUTS: z A 2XN matrix of points in polar coordinates. Each column of the
%          matrix has the format [r;azimuth], with azimuth given in
%          radians. Alternatively, if one just wishes to have unit vectors
%          returned, this can be a 1XN matrix of just azimuthal angles, in
%          which case the useHalfRange, zTx,and zRx inputs are ignored.
% systemType An optional parameter specifying the axis from which the
%          angles are measured. Possible values are:
%          0 (The default if omitted or an empty matrix is passed) The
%            azimuth angle is counterclockwise from the x axis.
%          1 The azimuth angle is measured clockwise from the y axis.
% useHalfRange A boolean value specifying whether the bistatic range value
%           should be divided by two. This normally comes up when operating
%           in monostatic mode, so that the range reported is a one-way
%           range. The default if this parameter is not provided, or an
%           empty matrix is passed, is true.
%       zTx The 2XN [x;y] location vectors of the transmitters in global
%           Cartesian coordinates. If this parameter is omitted or an
%           empty matrix is passed, then the transmitters are assumed to
%           be at the origin. If only a single vector is passed, then the
%           transmitter location is assumed the same for all of the target
%           states being converted. zTx can have more than 2 rows;
%           additional rows are ignored.
%       zRx The 2XN [x;y] location vectors of the receivers in Cartesian
%           coordinates.  If this parameter is omitted or an empty matrix
%           is passed, then the receivers are assumed to be at the origin.
%           If only a single vector is passed, then the receiver location
%           is assumed the same for all of the target states being
%           converted. zRx can have more than 2 rows; additional rows are
%           ignored.
%         M A 2X2XN hypermatrix of the rotation matrices to go from the
%           alignment of the global coordinate system to that at the
%           receiver. If omitted or an empty matrix is passed, then it is
%           assumed that the local coordinate system is aligned with the
%           global and M=eye(2) --the identity matrix is used. If only a
%           single 2X2 matrix is passed, then it is assumed to be the same
%           for all of the N conversions.
%
%OUTPUTS: cartPoints A 2XN or matrix of the points transformed into
%                    Cartesian coordinates. Each columns of cartPoints is
%                    of the format [x;y].
%
%The conversion utilizing bistatic polar measurements in 2D is similar to
%that using bistatic r-u-v measurements in 3D, which is discussed in [1].
%In both instances, one turns the direction components into a unit vector
%and the multiplied it by a one-way monostatic range that has to be
%computed.
%
%REFERENCES:
%[1] David F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%February 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(isempty(z))
    cartPoints=zeros(2,0);
    return
end

N=size(z,2);

if(nargin<6||isempty(M))
    M=repmat(eye(2),[1,1,N]);
elseif(size(M,3)==1)
    M=repmat(M,[1,1,N]);
end

if(nargin<5||isempty(zRx))
    zRx=zeros(2,N);
elseif(size(zRx,2)==1)
    zRx=repmat(zRx,[1,N]);
end

if(nargin<4||isempty(zTx))
    zTx=zeros(2,N);
elseif(size(zTx,2)==1)
    zTx=repmat(zTx,[1,N]);
end

if(nargin<3||isempty(useHalfRange))
    useHalfRange=true;
end

if(nargin<2||isempty(systemType))
    systemType=0;
end

%Extract the components.
if(size(z,1)==1)
    %No ranges, so just get unit vectors.
    cartPoints=zeros(2,N);
    switch(systemType)
        case 0
            cartPoints(1,:)=cos(z);
            cartPoints(2,:)=sin(z);
        case 1
            cartPoints(1,:)=sin(z);
            cartPoints(2,:)=cos(z);
        otherwise
            error('Invalid system type specified.')
    end

    %Rotate the unit vectors into the global coordinate system.
    for k=1:N
        %Convert to global Cartesian coordinates.
        cartPoints(:,k)=M(:,:,k)\cartPoints(:,k);
    end
    return;
else
    rB=z(1,:);
    azimuth=z(2,:);
end

%The bistatic range is used in the conversions below.
if(useHalfRange)
   rB=2*rB;
end

%Get unit vectors from the azimuth.
u=zeros(2,N);
switch(systemType)
    case 0
        u(1,:)=cos(azimuth);
        u(2,:)=sin(azimuth);
    case 1
        u(1,:)=sin(azimuth);
        u(2,:)=cos(azimuth);
    otherwise
        error('Invalid system type specified.')
end

cartPoints=zeros(2,N);
for curPoint=1:N
    if(all(zRx(:,curPoint)==zTx(:,curPoint)))
        %If it is monostatic, set r1 directly. This avoids NaNs with
        %zero-ranges in the monostatic case.
        r1=rB(curPoint)/2;
        %This is the Cartesian location in the local coordinate system of
        %the receiver.
        zL=r1*u(:,curPoint);
        %Convert to global Cartesian coordinates.
        cartPoints(:,curPoint)=M(:,:,curPoint)\zL+zRx(1:2,curPoint);
    else
        %The transmitter location in the receiver's local coordinate
        %system.
        zTxL=M(:,:,curPoint)*(zTx(1:2,curPoint)-zRx(1:2,curPoint));

        r1=(rB(curPoint)^2-norm(zTxL)^2)/(2*(rB(curPoint)-dot(u(:,curPoint),zTxL)));
        %This is the Cartesian location in the local coordinate system of
        %the receiver.
        zL=r1*u(:,curPoint);
        %Convert to global Cartesian coordinates.
        cartPoints(:,curPoint)=M(:,:,curPoint)\zL+zRx(1:2,curPoint);
    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.


%{
    Tests the "analysis" form of basis pursuit de-noising

    min_x ||Wx||_1
s.t.
    || A(x) - b || <= eps

The solvers solve a regularized version, using
    ||Wx||_1 + mu/2*||x-x_0||_2^2

see also test_sBP.m and test_sBPDN.m

%}

% Before running this, please add the TFOCS base directory to your path
myAwgn = @(x,snr) x + ...
        10^( (10*log10(sum(abs(x(:)).^2)/length(x(:))) - snr)/20 )*randn(size(x));

% Try to load the problem from disk
fileName = fullfile('reference_solutions','basispursuit_W_problem1_smoothed_noisy');
randn('state',34324);
rand('state',34324);

N = 512;
M = round(N/2);
K = round(M/5);

A = randn(M,N);
x = zeros(N,1);

% introduce a sparsifying transform "W"
d  = round(4*N);     % redundant
Wf = @(x) dct(x,d);  % zero-padded DCT
downsample  = @(x) x(1:N,:);
Wt = @(y) downsample( idct(y) );    % transpose of W
W  = Wf(eye(N));     % make an explicit matrix for CVX
if exist([fileName,'.mat'],'file')
    load(fileName);
    fprintf('Loaded problem from %s\n', fileName );

else

    % Generate a new problem

    % the signal x consists of several pure tones at random frequencies
    for k = 1:K
        x = x + randn()*sin( rand()*pi*(1:N) + 2*pi*rand() ).';
    end

    b_original = A*x;
    snr = 40;  % SNR in dB
    b   = myAwgn(b_original,snr);
    EPS = norm(b-b_original);
    x_original = x;

    mu = .01*norm(Wf(x),Inf);
    x0 = zeros(N,1);

    % get reference via CVX
    tic
    cvx_begin
        cvx_precision best
        variable xcvx(N,1)
        minimize norm(W*xcvx,1) + mu/2*sum_square(xcvx-x0)
        subject to
            norm(A*xcvx - b ) <= EPS
    cvx_end
    time_IPM = toc;
    x_ref = xcvx;
    obj_ref = norm(W*x_ref,1) + mu/2*sum_square(x_ref-x0);

    save(fileName,'x_ref','b','x_original','mu',...
        'EPS','b_original','obj_ref','x0','time_IPM','snr','d');
    fprintf('Saved data to file %s\n', fileName);

end

[M,N]           = size(A);
norm_x_ref      = norm(x_ref);
norm_x_orig     = norm(x_original);
er_ref          = @(x) norm(x-x_ref)/norm_x_ref;
er_signal       = @(x) norm(x-x_original)/norm_x_orig;
resid           = @(x) norm(A*x-b)/norm(b);  % change if b is noisy

fprintf('\tA is %d x %d\n', M, N );
fprintf('\tl1-norm solution and original signal differ by %.2e (mu = %.2e)\n', ...
    norm(x_ref - x_original)/norm(x_original),mu );

%% Call the TFOCS solver
objF            = @(x)norm(W*x,1) + mu/2*norm(x-x0).^2;
infeasF         = @(x)norm(A*x-b) - EPS;
er              = er_ref;  % error with reference solution (from IPM)
opts            = [];
opts.errFcn     = { @(f,dual,primal) er(primal), ...
                    @(f,dual,primal) obj_ref - f, ...
                    @(f,dual,primal) infeasF(primal) };
opts.maxIts     = 1000;
opts.tol        = 1e-10;
% opts.normA2     = norm(A*A');
% opts.normW2     = norm(W'*W);
z0  = [];   % we don't have a good guess for the dual
tic;
[ x, out, optsOut ] = solver_sBPDN_W( A, W, b, EPS, mu, x0, z0, opts );
time_TFOCS = toc;
fprintf('x is sub-optimal by %.2e, and infeasible by %.2e\n',...
    objF(x) - obj_ref, infeasF(x) );

fprintf('Solution has %d nonzeros.  Error vs. IPM solution is %.2e\n',...
    nnz(x), er(x) );

% Check that we are within allowable bounds
if out.err(end,1) < 1e-4
    disp('Everything is working');
else
    error('Failed the test');
end

%% Doing it "by hand" (for debugging), just for 200 iterations
objF            = @(x)norm(W*x,1) + mu/2*norm(x-x0).^2;
infeasF         = @(x)norm(A*x-b) - EPS;
er              = er_ref;  % error with reference solution (from IPM)
% er              = er_signal;
opts            = [];
opts.errFcn     = { @(f,dual,primal) er(primal), ...
                    @(f,dual,primal) obj_ref - f, ...
                    @(f,dual,primal) infeasF(primal) };
opts.maxIts     = 200;
opts.printEvery = 10;
opts.tol        = 1e-10;
z0      = [];
proxScale = sqrt(norm(W'*W)/norm(A*A'));
scale   = 1;
prox    = { prox_l2( EPS/scale ), proj_linf(proxScale) };
affineF = {A/scale,-b/scale;W/proxScale,0};
[ x, out, optsOut ] = tfocs_SCD( [], affineF, prox, mu, x0, z0, opts );

% Check that we are within allowable bounds
if out.err(end,1) < 1e-2
    disp('Everything is working');
else
    error('Failed the test');
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function [X,obj,err,iter] = groupl1(A,B,G,opts)

% Solve the group l1-minimization problem by ADMM
%
% min_X \sum_{i=1}^n\sum_{g in G} ||(x_i)_g||_2, s.t. AX=B
%
% x_i is the i-th column of X
% ---------------------------------------------
% Input:
%       A       -    d*na matrix
%       B       -    d*nb matrix
%       G       -    a cell indicates a partition of 1:na
%       opts    -    Structure value in Matlab. The fields are
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       X       -    na*nb matrix
%       obj     -    objective function value
%       err     -    residual ||AX-B||_F
%       iter    -    number of iterations
%
% version 1.0 - 18/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

[d,na] = size(A);
[~,nb] = size(B);

X = zeros(na,nb);
Z = X;
Y1 = zeros(d,nb);
Y2 = X;

AtB = A'*B;
I = eye(na);
invAtAI = (A'*A+I)\I;

iter = 0;
for iter = 1 : max_iter
    Xk = X;
    Zk = Z;
    % update X
    for i = 1 : nb
        X(:,i) = prox_gl1(Z(:,i)-Y2(:,i)/mu,G,1/mu);
    end
    % update Z
    Z = invAtAI*(-(A'*Y1-Y2)/mu+AtB+X);
    dY1 = A*Z-B;
    dY2 = X-Z;
    chgX = max(max(abs(Xk-X)));
    chgZ = max(max(abs(Zk-Z)));
    chg = max([chgX chgZ max(abs(dY1(:))) max(abs(dY2(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = compute_obj(X,G);
            err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y1 = Y1 + mu*dY1;
    Y2 = Y2 + mu*dY2;
    mu = min(rho*mu,max_mu);
end
obj = compute_obj(X,G);
err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);

function obj = compute_obj(X,G)
obj = 0;
for i = 1 : size(X,2)
    x = X(:,i);
    for j = 1 : length(G)
        obj = obj + norm(x(G{j}));
    end
end


function b = inCircle(point, circle)
%INCIRCLE test if a point is located inside a given circle.
%
%   B = inCircle(POINT, CIRCLE)
%   return true if point is located inside the circle
%
%   Example:
%   inCircle([1 0], [0 0 1])
%   inCircle([0 0], [0 0 1])
%   returns true, whereas
%   inCircle([1 1], [0 0 1])
%   return false
%
%   See also:
%   circles2d, onCircle

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2004-04-07
% Copyright 2004 INRA - TPV URPOI - BIA IMASTE

% deprecation warning
warning('geom2d:deprecated', ...
    '''inCircle'' is deprecated, use ''isPointInCircle'' instead');

d = sqrt(sum(power(point - circle(:,1:2), 2), 2));
b = d-circle(:,3)<=1e-12;


function pass = test_plus( )
% Test diskfun plus() command

tol = 1000*chebfunpref().cheb2Prefs.chebfun2eps;

f1 = @(x,y) sin(pi*x.*y);  % Strictly even/pi-periodic
f2 = @(x,y) sin(pi*x);  % Strictly odd/anti-periodic
g1 = diskfun(f1);
g2 = diskfun(f2);
gplus = g1 + g2;
fplus = @(t, r) diskfun.pol2cartf(@(x,y) f1(x, y) + ...
    f2(x, y), t, r);
t = pi*(2*rand-1);
r = rand;
pass(1) = abs(feval(gplus, t, r, 'polar') - fplus(t, r)) < tol;

f1 = @(x,y) sin(pi*x.*y) + sin(pi*x);   % Mixed symmetric terms
f2 = @(x,y) sin(pi*(x-.35).*(y-2.3)) + sin(pi*(x-.35)) ;  % Mixed symmetric terms
g1 = diskfun(f1);
g2 = diskfun(f2);
gplus = g1 + g2;
fplus = @(t, r) diskfun.pol2cartf( @(x,y) f1(x, y) + f2(x, y), t, r);
t = pi*(2*rand-1);
r = rand;
pass(2) = abs(feval(gplus, t, r, 'polar') - fplus(t, r)) < tol;

% Check that compression is working:
f = diskfun(@(x,y) x.^2 + y.^2);
r = rank(f);
g = f;
for k = 1:10
    g = g + f;
end
pass(3) = norm(g - 11*f, inf) < vscale(g)*tol;
pass(4) = (rank(g) - r ) == 0;

% Check what happens with cancellation errors:
f = diskfun(@(x,y) sin(x.*y));
g = 2*f;
pass(5) = ( norm(g - f - f, inf) < tol );

% Check for the case where f and g are non-zero at the poles bug
% f+g is zero at the poles.
f = diskfun(@(x,y) x+1);
g = diskfun(@(x,y) cos(y));
h = f - g;
pass(6) = h.nonZeroPoles == 0;

% Check for the case where f non-zero at the poles but g is zero at the
% poles.
f = diskfun(@(x,y) cos(x));
g = diskfun(@(x,y) sin(y));
h = f + g;
pass(7) = h.nonZeroPoles == 1;
h = g + f;
pass(8) = h.nonZeroPoles == 1;

end


function [pdf,cdf] = VBA_binomial(x,n,p)
% PDF and CDF of binomial distribution
% function [pdf,cdf] = VBA_binomial(x,n,p)
% Note: CDF is P(X<x), i.e. it excludes x, whereas PDF is P(X=x)!
% IN:
%   - x: number of successes
%   - n: total number of draws
%   - p: probability of a success
% OUT:
%   - pdf: P(X=x), where X is a binomial random variable
%   - cdf: P(X<x), where X is a binomial random variable

fullpdf = zeros(max(x),1);
for i=0:max(x)
    fullpdf(i+1) = nchoosek(n,i).*(p.^i).*((1-p).^(n-i));
end
pdf = fullpdf(x+1);
fullcdf = cumsum([0;fullpdf]);
cdf = fullcdf(x+1);