function ex1bvp
%EX1BVP  Example 1 of the BVP tutorial.
%   This is the example for MUSN in U. Ascher, R. Mattheij, and R. Russell,
%   Numerical Solution of Boundary Value Problems for Ordinary Differential
%   Equations, SIAM, Philadelphia, PA, 1995.  MUSN is a multiple shooting
%   code for nonlinear BVPs.  The problem is
%
%      u' =  0.5*u*(w - u)/v
%      v' = -0.5*(w - u)
%      w' = (0.9 - 1000*(w - y) - 0.5*w*(w - u))/z
%      z' =  0.5*(w - u)
%      y' = -100*(y - w)
%
%   The interval is [0 1] and the boundary conditions are
%
%      u(0) = v(0) = w(0) = 1,  z(0) = -10,  w(1) = y(1)
%
%   The example uses a guess for the solution coded here in EX1INIT.
%   The results of a run of the FORTRAN code MUSN are here compared to
%   the curves produced by BVP4C.

% Copyright 2004, The MathWorks, Inc.

solinit = bvpinit(linspace(0,1,5),@ex1init);
options = bvpset('Stats','on','RelTol',1e-5);

sol = bvp4c(@ex1ode,@ex1bc,solinit,options);

% The solution at the mesh points
x = sol.x;
y = sol.y;

% Solution obtained using MUSN:
amrx = [ 0. .1 .2 .3 .4 .5 .6 .7 .8 .9 1.]';
amry = [1.00000e+00   1.00000e+00   1.00000e+00  -1.00000e+01   9.67963e-01
        1.00701e+00   9.93036e-01   1.27014e+00  -9.99304e+00   1.24622e+00
        1.02560e+00   9.75042e-01   1.47051e+00  -9.97504e+00   1.45280e+00
        1.05313e+00   9.49550e-01   1.61931e+00  -9.94955e+00   1.60610e+00
        1.08796e+00   9.19155e-01   1.73140e+00  -9.91915e+00   1.72137e+00
        1.12900e+00   8.85737e-01   1.81775e+00  -9.88574e+00   1.80994e+00
        1.17554e+00   8.50676e-01   1.88576e+00  -9.85068e+00   1.87957e+00
        1.22696e+00   8.15025e-01   1.93990e+00  -9.81503e+00   1.93498e+00
        1.28262e+00   7.79653e-01   1.98190e+00  -9.77965e+00   1.97819e+00
        1.34161e+00   7.45374e-01   2.01050e+00  -9.74537e+00   2.00827e+00
        1.40232e+00   7.13102e-01   2.02032e+00  -9.71310e+00   2.02032e+00];

% Shift up the fourth component for the plot.
amry(:,4) = amry(:,4) + 10;
y(4,:) = y(4,:) + 10;

figure
plot(x,y',amrx,amry,'*')
axis([0 1 -0.5 2.5])
title('Example problem for MUSN')
ylabel('bvp4c and MUSN (*) solutions')
xlabel('x')

% --------------------------------------------------------------------------

function dydx = ex1ode(x,y)
%EX1ODE  ODE function for Example 1 of the BVP tutorial.
%   The components of y correspond to the original variables
%   as  y(1) = u, y(2) = v, y(3) = w, y(4) = z, y(5) = y.
dydx =  [ 0.5*y(1)*(y(3) - y(1))/y(2)
         -0.5*(y(3) - y(1))
         (0.9 - 1000*(y(3) - y(5)) - 0.5*y(3)*(y(3) - y(1)))/y(4)
          0.5*(y(3) - y(1))
          100*(y(3) - y(5)) ];

%-------------------------------------------------------------------------

function res = ex1bc(ya,yb)
%EX1BC  Boundary conditions for Example 1 of the BVP tutorial.
%   RES = EX1BC(YA,YB) returns a column vector RES of the
%   residual in the boundary conditions resulting from the
%   approximations YA and YB to the solution at the ends of
%   the interval [a b]. The BVP is solved when RES = 0.
%   The components of y correspond to the original variables
%   as  y(1) = u, y(2) = v, y(3) = w, y(4) = z, y(5) = y.
res = [ ya(1) - 1
        ya(2) - 1
        ya(3) - 1
        ya(4) + 10
        yb(3) - yb(5)];

%-------------------------------------------------------------------------

function v = ex1init(x)
%EX1INIT  Guess for the solution of Example 1 of the BVP tutorial.
v = [        1
             1
     -4.5*x^2+8.91*x+1
            -10
     -4.5*x^2+9*x+0.91 ];


function laplacian_test01 ( )

%*****************************************************************************80
%
%% LAPLACIAN_TEST01 tests L1DD and similar routines.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 October 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LAPLACIAN_TEST01\n' );
  fprintf ( 1, '  A full-storage matrix is returned by:\n' );
  fprintf ( 1, '  L1DD: Dirichlet/Dirichlet BC;\n' );
  fprintf ( 1, '  L1DN: Dirichlet/Neumann BC;\n' );
  fprintf ( 1, '  L1ND: Neumann/Dirichlet BC;\n' );
  fprintf ( 1, '  L1NN: Neumann/Neumann BC;\n' );
  fprintf ( 1, '  L1PP: Periodic BC;\n' );

  n = 5;

  for test = 1 : 2

    if ( test == 1 )
      h = 1.0;
    else
      h = 1.0 / ( n + 1 );
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Using spacing H = %g\n', h );

    l = l1dd ( n, h );
    r8mat_print ( n, n, l, '  L1DD:' );

    l = l1dn ( n, h );
    r8mat_print ( n, n, l, '  L1DN:' );

    l = l1nd ( n, h );
    r8mat_print ( n, n, l, '  L1ND:' );

    l = l1nn ( n, h );
    r8mat_print ( n, n, l, '  L1NN:' );

    l = l1pp ( n, h );
    r8mat_print ( n, n, l, '  L1PP:' );

  end

  return
end


function [cost,grad] = softICACost(theta, x, params)

% unpack weight matrix
W = reshape(theta, params.numFeatures, params.n);

% % project weights to norm ball (prevents degenerate bases)
Wold = W;
W = l2rowscaled(W, 1);

% Forward Prop
h = W*x;
r = W'*h;

% Sparsity Cost
K = sqrt(params.epsilon + h.^2);
sparsity_cost = params.lambda * sum(sum(K));
K = 1./K;

% Reconstruction Loss and Back Prop
diff = (r - x);
reconstruction_cost = 0.5 * sum(sum(diff.^2));
outderv = diff;

% compute the cost comprised of: 1) sparsity and 2) reconstruction
cost = sparsity_cost + reconstruction_cost;

% Backprop Output Layer
W2grad = outderv * h';

% Baclprop Hidden Layer
outderv = W * outderv;
outderv = outderv + params.lambda * (h .* K);

W1grad = outderv * x';
Wgrad = W1grad + W2grad';

% % unproject gradient for minFunc
grad = l2rowscaledg(Wold, W, Wgrad, 1);
grad = grad(:);


function [scales,weights,covar]=realized_quantile_variance_scale(samplesperbin,quantiles,simulations,symmetric)
% Computes the scales needed for estimating the integrated variance using Realized Quantile
% Variance.  Also computes the weights of the optimal combination and non-scaled covariance from
% which the weights are derived.
%
% USAGE:
%   [SCALES,WEIGHTS,COVAR]=realized_quantile_variance_scale(SAMPLESPERBIN,QUANTILES,SIMULATIONS)
%
% INPUTS:
%   SAMPLESPERBIN    - Number of returns to use in each bin when computing the quantiles. NOTE: The
%                        number of returns produced by filtering according to SAMPLINGTYPE and
%                        SAMPLINGINTERVAL must be an integer multiple of SAMPLESPERBIN.
%   QUANTILES        - k by 1 vector of quantile values to use when computing RQ, must satisfy 0.5<QUANTILES<=1.
%                        Quantiles must produce values such that QUANTILES*SAMPLESPERBIN is an
%                        integer.  The simplest method to accomplish this is to specify QUANTILES as
%                        the ratio of the index number of the return to SAMPLESPERBIN (e.g.
%                        QUANTILES = [13 15 19]/20)
%   SIMULATIONS       - [OPTIONAL] Scalar integer.  Should usually be at least 1,000,000.  Default
%                         value is 10,000,000.
%   SYMMETRIC         - [OPTIONAL] Logical value indicating whether the symmetric estimator is being
%                         used. Default is false.
%
% OUTPUTS:
%   SCALES            - k by 1 vector of scales for standardized quantile based estimates of variance
%   WEIGHTS           - k by 1 vector of weights that produce the optimal combination (lowest variance)
%   COVAR             - k by k non-scaled covariance matrix of the quantile realized variance - one
%                         for each quantile used.  Forms the actual (scaled) covariance when
%                         multiplied by the integrated quarticity.
%
% COMMENTS:
%   Uses Monte Carlo integration with 10,000,000 simulations.  Trade offs between accuracy and time
%   can be made by altering SIMULATIONS.
%
%  See also REALIZED_QUANTILE_VARIANCE
%

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

if nargin<4
    symmetric = false;
end
if nargin ==2
    simulations = 10000000;
end

% Save the state and set it to a specific value so that the simulated results will be reproducable
state0=randn('state');
randn('state',datenum('MAR-26-1974'))

k = length(quantiles);

if symmetric
    indices = round(samplesperbin*quantiles);
else
    indicesHigh = round( samplesperbin*quantiles);
    indicesLow = round(samplesperbin*(1-quantiles)+1 );
end

% Need to block eventually incase someone tries to compute this with a
% large number of samplesperbin
rows = min(floor(2^13/samplesperbin),simulations);
iter = ceil(simulations/rows);
remaining = simulations;

scales = zeros(1,k);
opMatrix  = zeros(k);
% The MC integration
for i=1:iter
    numThisIter = min(remaining,rows);
    remaining = remaining - rows;

    x=randn(numThisIter,samplesperbin);
    if symmetric
        x=sort(x.^2,2);
        x2=x(:,indices);
    else
        x=sort(x,2);
        x2=x(:,indicesHigh).^2 + x(:,indicesLow).^2;
    end
    scales = scales + sum(x2)/simulations;
    opMatrix = opMatrix + x2'*x2/simulations;
end

% Compute the covariance
covar = opMatrix - scales'*scales;
covar = samplesperbin*diag(1./scales)*covar*diag(1./scales);
% And the weights according to the GMVP
weights=covar^(-1)*ones(k,1)/(ones(1,k)*covar^(-1)*ones(k,1));

% Reset the state
randn('state',state0);


function [initState, transmat, mu, Sigma] = gausshmm_train_observed(obsData, hiddenData, ...
						  nstates, varargin)
% GAUSSHMM_TRAIN_OBSERVED  Estimate params of HMM with Gaussian output from fully observed sequences
% [initState, transmat, mu, Sigma] = gausshmm_train_observed(obsData, hiddenData, nstates,...)
%
% INPUT
% If all sequences have the same length
% obsData(:,t,ex)
% hiddenData(ex,t)  - must be ROW vector if only one sequence
% If sequences have different lengths, we use cell arrays
% obsData{ex}(:,t)
% hiddenData{ex}(t)
%
% Optional argumnets
% dirichletPriorWeight - for smoothing transition matrix counts
%
% Optional parameters from mixgauss_Mstep:
% 'cov_type' - 'full', 'diag' or 'spherical' ['full']
% 'tied_cov' - 1 (Sigma) or 0 (Sigma_i) [0]
% 'clamped_cov' - pass in clamped value, or [] if unclamped [ [] ]
% 'clamped_mean' - pass in clamped value, or [] if unclamped [ [] ]
% 'cov_prior' - Lambda_i, added to YY(:,:,i) [0.01*eye(d,d,Q)]
%
% Output
% mu(:,q)
% Sigma(:,:,q)

[dirichletPriorWeight, other] = process_options(...
    varargin, 'dirichletPriorWeight', 0);

[transmat, initState] = transmat_train_observed(hiddenData, nstates, ...
						'dirichletPriorWeight', dirichletPriorWeight);

% convert to obsData(:,t*nex)
if ~iscell(obsData)
  [D T Nex] = size(obsData);
  obsData = reshape(obsData, D, T*Nex);
else
  obsData = cat(2, obsData{:});
  hiddenData = cat(2,hiddenData{:});
end
[mu, Sigma] = condgaussTrainObserved(obsData, hiddenData(:), nstates, varargin{:});


% This is a demo for segmentation using local gaussian distribution (LGD)
% fitting energy
%
% Reference: <Li Wang, Lei He, Arabinda Mishra, Chunming Li.
% Active Contours Driven by Local Gaussian Distribution Fitting Energy.
% Signal Processing, 89(12), 2009,p. 2435-2447>
%
% Please DO NOT distribute this code to anybody.
% Copyright (c) by Li Wang
%
% Author:       Li Wang
% E-mail:       li_wang@med.unc.edu
% URL:          http://www.unc.edu/~liwa/
%
% 2010-01-02 PM

clc;clear all;close all;

Img=imread('3.bmp');
Img = double(Img(:,:,1));

NumIter = 1000; %iterations
timestep=0.1; %time step
mu=0.1/timestep;% level set regularization term, please refer to "Chunming Li and et al. Level Set Evolution Without Re-initialization: A New Variational Formulation, CVPR 2005"
sigma = 3;%size of kernel
epsilon = 1;
c0 = 2; % the constant value
lambda1=1.03;%outer weight, please refer to "Chunming Li and et al,  Minimization of Region-Scalable Fitting Energy for Image Segmentation, IEEE Trans. Image Processing, vol. 17 (10), pp. 1940-1949, 2008"
lambda2=1.0;%inner weight
%if lambda1>lambda2; tend to inflate
%if lambda1<lambda2; tend to deflate
nu = 0.0005*255*255;%length term
alf = 20;%data term weight

figure,imagesc(uint8(Img),[0 255]),colormap(gray),axis off;axis equal
[Height Wide] = size(Img);
[xx yy] = meshgrid(1:Wide,1:Height);
phi = (sqrt(((xx - 64).^2 + (yy - 76).^2 )) - 15);
phi = sign(phi).*c0;

Ksigma=fspecial('gaussian',round(2*sigma)*2 + 1,sigma); %  kernel
ONE=ones(size(Img));
KONE = imfilter(ONE,Ksigma,'replicate');
KI = imfilter(Img,Ksigma,'replicate');
KI2 = imfilter(Img.^2,Ksigma,'replicate');

figure,imagesc(uint8(Img),[0 255]),colormap(gray),axis off;axis equal,
hold on,[c,h] = contour(phi,[0 0],'r','linewidth',1); hold off
pause(0.5)

tic
for iter = 1:NumIter
    phi =evolution_LGD(Img,phi,epsilon,Ksigma,KONE,KI,KI2,mu,nu,lambda1,lambda2,timestep,alf);

    if(mod(iter,50) == 0)
        figure(2),
        imagesc(uint8(Img),[0 255]),colormap(gray),axis off;axis equal,title(num2str(iter))
        hold on,[c,h] = contour(phi,[0 0],'r','linewidth',1); hold off
        pause(0.02);
    end

end
toc


%quad_diff   solve Poisson problem in quadrilateral domain
%   IFISS scriptfile: DJS; 4 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
clear
%% define geometry
pde=1; domain=8;
global viscosity
viscosity=1;
quad_domain
load quad_grid
%
%% set up matrices
qmethod=default('Q1/Q2 approximation 1/2? (default Q1)',1);
if qmethod ==2,
   [x,y,xy] = q2grid(x,y,xy,mv,bound);
   [A,M,f] = femq2_diff(xy,mv);
else
   [ev,ebound] = q1grid(xy,mv,bound,mbound);
   [A,M,f] = femq1_diff(xy,ev);
end
%
%% boundary conditions
[Agal,fgal] = nonzerobc(A,f,xy,bound);
%
%% save resulting system
fprintf('system saved in quad_diff.mat ...\n')
gohome
cd datafiles
save quad_diff qmethod Agal M  fgal  xy x y
%% compute solution
fprintf('solving linear system ...  ')
x_gal=Agal\fgal;
fprintf('done\n')
save quad_diff x_gal  -append
% plot solution
if qmethod > 0
   solplot(x_gal,xy,x,y,10);
   title(['Q',int2str(qmethod),' solution'])
   drawnow
end


function [tdata, validIdx] = translate_movie(data, dv, ops)

%% Parameters
[ly, lx, nFrames] = size(data);
if nargin < 3
  ops = [];
end
subpixel = getOr(ops, {'subPixel' 'SubPixel'}, 1);
if isfinite(subpixel)
  dv = round(subpixel*dv)./subpixel;
end
useGPU = getOr(ops, 'useGPU', false);

dv = permute(dv, [2 3 1]);
fy = ifftshift((-fix(ly/2):ceil(ly/2) - 1)/ly)';% freq along first dimension
fx = ifftshift((-fix(lx/2):ceil(lx/2) - 1)/lx); % freq along second dimension

if useGPU
  batchSize = 32;
  fx = gpuArray(fx);
  fy = gpuArray(fy);
  dv = gpuArray(dv);
else
  batchSize = 3;
end
tdata = zeros([ly, lx, nFrames], 'like', data);
%% Work through data in batches
nBatches = ceil(nFrames/batchSize);
for bi = 1:nBatches
  fi = (bi - 1)*batchSize + 1:min(bi*batchSize, nFrames);
  if useGPU
    batchData = gpuArray(single(data(:,:,fi)));
  else
    batchData = single(data(:,:,fi));
  end
  phaseShift = bsxfun(@times,...
    exp(-1j*2*pi*bsxfun(@times, fy, dv(1,:,fi))),... y rotation
    exp(-1j*2*pi*bsxfun(@times, fx, dv(2,:,fi)))); % x rotation
  tdata(:,:,fi) = gather(real(ifft2(fft2(batchData).*phaseShift)));
end
if nargout > 1
  dvMax = max(0, ceil(max(gather(dv), [], 3)));
  dvMin = min(0, floor(min(gather(dv), [], 3)));
  validX = (1 + dvMax(2)):(lx + dvMin(2));
  validY = (1 + dvMax(1)):(ly + dvMin(1));
  validIdx = {validY validX};
end

end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
% ##2
%==============================================================================
%
% Tutorial for FAIR: regulariztion
%
% given a certain force field f on a 2D domain, this tutorial
% computes and visualizes the elastic displacement u, such that
%
%  B'*B u = f
%
% where B is the discrete elastic operator
% see also getElasticMatrixStg
%==============================================================================

clear, close all, help(mfilename);

                    % 2D example
omega  = [0,1,0,1]; % physical domain
m      = [16,12];   % number of discretization points
mu     = 1;         % Lame constants, control elasticity properties
lambda = 0;         % Youngs modulus and Poisson ratio
n      = prod(m);   % number of cells, and staggered dimensions
ns     = [(m(1)+1)*m(2),m(1)*(m(2)+1)];

% generate cell centered, staggered and nodal grids
% note: computation is staggered, others used for visualization
xc = getCellCenteredGrid(omega,m);
xn = getNodalGrid(omega,m);

% build elasticity operator on a staggered grid and visualize
B = getElasticMatrixStg(omega,m,mu,lambda);
FAIRfigure(1); clf; subplot(2,2,1); spy(B); title('B elastic on staggered grid')

% create a force field o cell centered grid and visualize
fc  = [0*xc(1:n);-10*sin(pi*xc(1:n))];
% computations on staggered grid
fs = grid2grid(fc,m,'centered','staggered');

% compute the displacement from (B'*B)*us = fs
% note B is derivative operator and has null space (constants)

% remove constant parts: E'*(f-E*w) = 0
E = [ones(ns(1),1)*[1,0];ones(ns(2),1)*[0,1]];
fs = fs - E*((E'*E)\(E'*fs));

% solve for displacements
warning off % matrix is singular, but MATLAB doesn't care
us = (B'*B)\fs;
warning on

% visualization on cell centered and nodal grids
uc = grid2grid(us,m,'staggered','centered');
un = grid2grid(us,m,'staggered','nodal');

% shortcuts for visualization,
vecNorm   = @(v) sqrt(sum(reshape(v,[],2).^2,2));
viewField = @(v) viewImage2Dsc(vecNorm(v),omega,m);
PG        = @(x) plotGrid(x,omega,m);

% visualize force field
FAIRfigure(1); subplot(2,2,3);
viewField(fc); hold on; colormap(gray); colorbar; PG(xn);
qh = quiver(xc(1:n),xc(n+1:end),fc(1:n),fc(n+1:end),1);
set(qh,'color','r','linewidth',1)
title('a force field')

% visualize displacement field
FAIRfigure(1); subplot(2,2,4);
viewField(uc); hold on; colormap(gray); colorbar; PG(xn);
qh = quiver(xc(1:n),xc(n+1:end),uc(1:n),uc(n+1:end),1);
set(qh,'color','g','linewidth',1)
title('the displacement field')

% visuaize the displaced grid
FAIRfigure(1); subplot(2,2,2);
xh = PG(xn); hold on; axis equal image; yh = PG(xn+un);
set(xh,'linewidth',2); set(xh,'linewidth',2,'color','g');
title('initial and displaced grids')

%==============================================================================


function [h_1d, h_2d] = create_blur_kernel(Kx,opt)

FIX_HSIZE = 1; % hsize = 11
HSIZE = 15;
eps = 0.01;

if FIX_HSIZE
    V = [Kx(1:floor(HSIZE/2)+1); Kx(opt.N-floor(HSIZE/2)+1:opt.N)];
    V = circshift(V, floor(HSIZE/2));
else
    for i = 1:length(Kx)
        if i>2 && Kx(i) < eps
            halflen = i-2;
            break
        end
    end

    len = 2*halflen + 1;
    V = zeros(1, len);
    for i = 1:halflen
        V(i) = Kx(opt.N - halflen + i);
    end
    for i = halflen+1:len
        V(i) = Kx(i-halflen);
    end
end

V = V / sum(V);

h_1d = V';
h_2d = h_1d'*h_1d;


function [oAllanDev] = calculateADEV(tau,sPeriod,readings)
%Overlapping Allan (ADEV) function that uses phase error or time error values
%input argument is readings already grouped as tau values. tau is the
%desired gate or tau time needed for calculating overlap.
%sPeriod is used to determine the overlap

N = numel(readings); %get the reading count
n = tau / sPeriod; %calculate averaging factor
n = floor(n); %make sure this is an integer
const = 1/(2*(N - (2*n))*tau^2); %calculate the const mult 1/(2*(N - (2*n))*tau^2)
%sum from i=1 to N-2n (Xi+2m - 2Xi+m + Xi)^2
sum = 0; %variable to store summation calculation

%loop for performing summation
for i = 1:(N-(2*n))
   sum = sum + (readings(i+(2*n)) - (2*readings(i+n)) + readings(i))^2; %previos sum + (yi+1 - yi)^2
end

oAllanDev = sqrt(const*sum); %square root for Allan Dev


  function [cost grad] = mri_r2_fit_costgrad_pd(pd, data)
%|function [cost grad] = mri_r2_fit_costgrad_pd(pd, data)
%| gradient wrt "proton density" (pd) image
%| for R2=1/T2 estimation from images

if nargin == 1 && streq(pd, 'test'), mri_r2_fit_costgrad_pd_test, return, end
if nargin < 2, ir_usage, end

yi = data.yi; % [(nd) nt] images
r2 = data.r2; % [(nd)] R2=1/T2 maps
te = data.te; % echo times

[yb yi] = mri_r2_fit_mean(pd, r2, yi, te);
cost = sum(abs(yi(:) - yb(:)).^2) / 2;

grad = mri_r2_fit_grad_pd(pd, r2, yi - yb, te);

if isfield(data, 'R')
	R = data.R;
	cost = cost + R.penal(R, pd);
	grad = grad + R.cgrad(R, pd);
end
grad = reshape(grad, size(pd));

% mri_r2_fit_mean()
function [yb yi] = mri_r2_fit_mean(pd, r2, yi, te)

nt = numel(te);
yi = reshapee(yi, [], nt); % [*nd nt]
yb = zeros(size(yi)); % [*nd nt]
for it=1:nt
	yb(:,it) = pd(:) .* exp(-te(it) * r2(:));
end

% mri_r2_fit_grad_pd()
function grad = mri_r2_fit_grad_pd(pd, r2, resid, te)

nt = numel(te);

grad = 0;
for it=1:nt
	tmp = exp(-te(it) * r2(:));
	grad = grad - tmp .* resid(:,it);
end

function mri_r2_fit_costgrad_pd_test

r2 = 0.01;
te = [0:3] * 20;
pd = 2;
yb = pd .* exp(-te * r2);
rng(0)
yi = yb + 0.1 * randn(size(yb));

%R = Reg1(true(1), 'beta', 3, 'order', 0, 'type_penal', 'mat');

data.yi = yi;
data.r2 = r2;
data.te = te;
%data.R = R;

pdlist = linspace(0, 4, 101);
cost = zeros(size(pdlist));
for ii=1:numel(pdlist)
	cost(ii) = mri_r2_fit_costgrad_pd(pdlist(ii), data);
end

p0 = 1.5;
[cost0 grad0] = mri_r2_fit_costgrad_pd(p0, data);

if im
	clf
	plot(pdlist, cost, '-', pdlist, cost0 + grad0*(pdlist-p0), '--')
end


function result=rsimpls(x,y,varargin)

%RSIMPLS is a 'Robust method for Partial Least Squares Regression based on the
% SIMPLS algorithm'. It can be applied to both low and high-dimensional predictor variables x
% and to one or multiple response variables y. It is resistant to outliers in the data.
% The RSIMPLS algorithm is built on two main stages. First, a matrix of scores is derived
% based on a robust covariance criterion (see robpca.m),
% and secondly a robust regression is performed based on the results from ROBPCA.
%
% The RSIMPLS method is described in:
%    Hubert, M., and Vanden Branden, K. (2003),
%    "Robust Methods for Partial Least Squares Regression",
%    Journal of Chemometrics, 17, 537-549.
%
% To select the number of components in the regression model, a robust RMSECV (root mean squared
% error of cross validation) curve is drawn, based on a fast algorithm for
% cross-validation. This approach is described in:
%
%    Engelen, S., Hubert, M. (2005),
%    "Fast model selection for robust calibration methods",
%    Analytica Chimica Acta, 544, 219-228.
%
% Required input arguments:
%            x : Data matrix of the explanatory variables
%                (n observations in rows, p variables in columns)
%            y : Data matrix of the response variables
%                (n observations in rows, q variables in columns)
%
% Optional input arguments:
%            k : Number of components to be used.
%                (default = min(rank([x,y]),kmax)). If k is not specified,
%                it can be selected using the option 'rmsecv'.
%         kmax : Maximal number of components to be used (default = 9).
%                If k is provided, kmax does not need to be specified, unless k is larger
%                than 9.
%        alpha : (1-alpha) measures the fraction of outliers the algorithm should
%                resist. Any value between 0.5 and 1 may be specified. (default = 0.75)
%            h : (n-h+1) measures the number of observations the algorithm should
%                resist. Any value between n/2 and n may be specified. (default = 0.75*n)
%                Alpha and h may not both be specified.
%       rmsecv : If equal to zero and k is not specified, a robust R-squared curve
%                is plotted and an optimal k value can be chosen. (default)
%                If equal to one and k is not specified, a robust component selection-curve is plotted and
%                an optimal k value can be chosen. This curve computes a combination of
%                the cross-validated root mean squared error and the robust residual
%                sum of squares for k=1 to kmax. Rmsecv and k may not both
%                be specified.
%        rmsep : If equal to one, the robust RMSEP-value (root mean squared error of
%                prediction) for the model with k components (default = 0). This value
%                is automatically given if rmsecv = 1.
%        plots : If equal to one, a menu is shown which allows to draw several plots,
%                such as a robust score outlier map and a regression
%                outlier map are drawn. (default)
%                If the input argument 'classic' is equal to one, the classical
%                diagnostic plots are drawn as well.
%                If 'plots' is equal to zero, all plots are suppressed.
%                See also makeplot.m
%        labsd : The 'labsd' observations with largest score distance are
%                labeled on the outlier map (default = 3)
%        labod : The 'labod' observations with largest orthogonal distance are
%                labeled on the outlier map (default = 3)
%      labresd : The 'labresd' observations with largest residual distance are
%                labeled on the outlier map (default = 3)
%      classic : If equal to one, the classical SIMPLS analysis will be performed as well
%                (see also csimpls.m). (default = 0)
%
% Options for advanced users:
%           kr : Total number of components used by the ROBPCA method.
%                We advise to use kr=k+q. (default)
%        kmaxr : Maximal number of components used by the ROBPCA method.
%                default = min(kmax+q,rank([x,y]))
%  plotsrobpca : If equal to one, a robust score outlier map from ROBPCA is drawn.
%                If the input argument 'classic' is equal to one, the classical
%                outlier map is drawn as well.
%                If 'plotsrobpca' is equal to zero, all plots are suppressed. (default)
%           st : Indicates the current stage of the algorithm for cross-validation (RMSECV)
%                default = 0 -> performs all the stages of the algorithm and computes all the
%                parameters for the full model.
%                st=1, robpca is still performed, but when
%                st=2, the algorithm proceeds based on the previous knownledge of the output from robpca.
%          out : Is an empty structure, but is constructed while performing the cross-validation.
%                (see rrmse.m)
%
% I/O: result=rsimpls(x,y,'k',k,'kmax',10,'alpha',0.75,'h',h,'rmsecv',0,'rmsep',0,...
%       'plots',1,'labsd',3,'labod',3,'labresd',3,'classic',1,'kr',kr,...
%       'kmaxr',kmaxr,'plotsrobpca',0,'st',0,'out',[]);
%  The user should only give the input arguments that have to change their default value.
%  The name of the input arguments needs to be followed by their value.
%  The order of the input arguments is of no importance.
%
% Examples:
%   rsimpls(x,y,'k',5,'plots',1);
%   rsimpls(x,y,'classic',1,'rmsecv',1);
%
% The output of RSIMPLS is a structure containing:
%
%   result.slope     : Robust slope estimate
%   result.int       : Robust intercept estimate
%   result.fitted    : Robust fitted values
%   result.res       : Robust residuals
%   result.cov       : Estimated variance-covariance matrix of the residuals
%   result.T         : Robust scores
%   result.weights.r : Robust simpls weights
%   result.weights.p : Robust simpls weights
%   result.Tcenter   : Robust center of the scores
%   result.Tcov      : Robust covariance matrix of the scores
%   result.rsquared  : Robust R-squared value for the optimal k
%   result.rcs       : Robust Component Selection Criterion:
%                      This is a matrix with kmax columns. The first row contains the approximate
%                      R-squared values (for k=1,...,kmax) and the second row the square root of the weighted
%                      residuals sum of squares. This is equal to the RCS-value with
%                      gamma = 0 (see Engelen and Hubert (2005) for the definition).
%                      If the input argument rmsecv = 1, the third row contains the RCS-value for
%                      gamma = 0.5 and the fourth for gamma = 1. The last one is equal to the robust
%                      cross-validated RMSE values.
%                      Note that all the entries in this matrix depend on the choice of kmax.
%   result.rmsep     : Robust RMSEP value
%   result.k         : Number of components used in the regression
%   result.h         : The quantile h used throughout the algorithm
%   result.sd        : Robust score distances
%   result.od        : Robust orthogonal distances
%   result.resd      : Residual distances (when there are several response variables).
%                      If univariate regression is performed, it contains the standardized residuals.
%   result.cutoff    : Cutoff values for the score (result.cutoff.sd), orthogonal
%                      (result.cutoff.od) and residual distances (result.cutoff.resd).
%                      We use 0.975 quantiles of the chi-squared distribution.
%   result.flag      : The observations whose score distance is larger than
%                      'result.cutoff.sd' receive a flag 'result.flag.sd' equal
%                      to zero (good leverage points). Otherwise 'result.flag.sd'
%                      is equal to one.
%                      The components 'result.flag.od' and 'result.flag.resd' are
%                      defined analogously, and determine the orthogonal outliers,
%                      resp. the bad leverage points/vertical outliers.
%                      The observations with 'result.flag.od' and 'result.flag.resd'
%                      equal to zero, can be considered as calibration outliers and receive
%                      'result.flag.all' equal to zero. The regular observations and the good leverage
%                      points have 'result.flag.all' equal to one.
%   result.class     : 'RSIMPLS'
%   result.classic   : If the inputargument 'classic' is equal to 1, this structure
%                      contains results of the classical SIMPLS analysis. (see also csimpls.m)
%   results.robpca   : The results of robpca on [X,Y].
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by Karlien Vanden Branden
% Version date: 07/04/2004
% Last update: 04/08/2006

%
%initialization with defaults
%
if rem(nargin,2)~=0
    error('Number of input arguments must be even!');
end
[n,p1]=size(x);
[n2,q1]=size(y);
z=[x,y];
rx=rank(x);
rz=rank(z);
if n~=n2
    error('The response variables and the predictor variables have a different number of observations.')
end
niter=100;counter=1;mcd=0;alfa=0.75;
kmax=min([9,rx,floor(n/2),p1]);
kmaxr=min([kmax+q1,rz]);
h=floor(2*floor((n+kmaxr+1)/2)-n+2*(n-floor((n+kmaxr+1)/2))*alfa);
labsd=3;labod=3;labresd=3;
plotsrobpca=0;plots=1;k=0;
kr=k+q1;
st=0;rmsecv=0;
out=[];classic=0;rmsep=0;rmsep_value=nan;rmsecv_value = nan;rsquared_value = nan;rss_value = nan;
default=struct('alpha',alfa,'h',h,'labsd',labsd,'labod',labod,'labresd',labresd,'k',k,'kr',kr,...
    'plotsrobpca',plotsrobpca,'plots',plots,'kmax',kmax,'st',st,...
    'out',out,'rmsecv',rmsecv,'classic',classic,'rmsep',rmsep,...
    'kmaxr',kmaxr,'rmsep_value',rmsep_value,'rmsecv_value',rmsecv_value,'rsquared_value',...
    rsquared_value,'rss_value',rss_value);
list=fieldnames(default);
options=default;
IN=length(list);
i=1;
%
if nargin>2
    %
    %placing inputfields in array of strings
    %
    for j=1:nargin-3
        if rem(j,2)~=0
            chklist{i}=varargin{j};
            i=i+1;
        end
    end
    dummy=sum(strcmp(chklist,'h')+2*strcmp(chklist,'alpha'));
    switch dummy
        case 0 %Take on default values
            options.alpha=alfa;%0.75;
            options.h=h;
        case 3
            error('Both input arguments alpha and h are provided. Only one is required.')
    end
    %
    %Checking which default parameters have to be changed
    % and keep them in the structure 'options'.
    %
    while counter<=IN
        index=strmatch(list(counter,:),chklist,'exact');%contains the users input one by one
        if ~isempty(index) %in case of similarity
            for j=1:nargin-3 %searching the index of the accompanying field
                if rem(j,2)~=0 %fieldnames are placed on odd index
                    if strcmp(chklist{index},varargin{j})
                        I=j;
                    end
                end
            end
            options=setfield(options,chklist{index},varargin{I+1});
            index=[];
        end
        counter=counter+1;
    end
    options.h=floor(options.h);
    options.kmax=floor(options.kmax);
    options.k=floor(options.k);
    options.kmaxr=floor(options.kmaxr);
    options.kr=floor(options.kr);
    kmax=min([options.kmax,floor(n/2),rz,p1]);
    kmaxr=max([min([options.kmaxr,rz]),kmax+q1]);
    k=min(options.k,kmax);
    while k<0
        k=input(['The number of components can not be negative.\n'...
        'How many principal components would you like to retain?\n']);
    end
    if any(strcmp(chklist,'kr'))
        kr=floor(max([options.kr,k+q1]));
    else
        kr=k+q1;
    end
    if dummy==1 %checking inputvariable h
        if options.h-floor(options.h)~=0
            mess=sprintf('Attention (rsimpls.m): h must be an integer. \n');
            disp(mess)
        end
        if kr==0
            if options.h<floor((n+kmaxr+1)/2)
                options.h=floor((n+kmaxr+1)/2);
                mess=sprintf(['Attention (rsimpls.m): h should be larger than (n+kmaxr+1)/2.\n',...
                        'It is set to its minimum value',num2str(options.h)]);
                disp(mess)
            end
            options.alpha=options.h/n;
        else
            if options.h<floor((n+kr+1)/2)
                options.h=floor((n+kr+1)/2);
                mess=sprintf(['Attention (rsimpls.m): h should be larger than (n+kr+1)/2.\n',...
                        'It is set to its minimum value',num2str(options.h)]);
                disp(mess)
            end
            options.alpha=options.h/n;
        end
        if options.h>n
            options.alpha=0.75;
            if kr==0
                options.h=floor(2*floor((n+kmaxr+1)/2)-n+2*(n-floor((n+kmaxr+1)/2))*options.alpha);
            else
                options.h=floor(2*floor((n+kr+1)/2)-n+2*(n-floor((n+kr+1)/2))*options.alpha);
            end
            mess=sprintf(['Attention (rsimpls.m): h should be smaller than n. \n',...
                    'It is set to its default value ',num2str(options.h)]);
            disp(mess)
        end
    elseif dummy==2
        if options.alpha < 0.5
            options.alpha=0.5;
            mess=sprintf(['Attention (rsimpls.m): Alpha should be larger than 0.5. \n',...
                    'It is set to 0.5.']);
            disp(mess)
        end
        if options.alpha > 1
            options.alpha=0.75;
            mess=sprintf(['Attention (rsimpls.m): Alpha should be smaller than 1.\n',...
                    'It is set to 0.75.']);
            disp(mess)
        end
        if kr==0
            options.h=floor(2*floor((n+kmaxr+1)/2)-n+2*(n-floor((n+kmaxr+1)/2))*options.alpha);
        else
            options.h=floor(2*floor((n+kr+1)/2)-n+2*(n-floor((n+kr+1)/2))*options.alpha);
        end
    end
    h=options.h;alfa=options.alpha;labsd=max(0,min(floor(options.labsd),n));
    dummyh = strcmp(chklist,'h');
    dummykmax = strcmp(chklist,'kmax');
    if all(dummyh == 0) && any(dummykmax)
        h = floor(2*floor((n+kmax+1)/2)-n+2*(n-floor((n+kmax+1)/2))*alfa);
    end
    labod=max(0,min(floor(options.labod),n));labresd=max(0,min(floor(options.labresd),n));
    plotsrobpca=options.plotsrobpca;plots=options.plots;
    st=options.st;
    out=options.out;
    rmsecv=options.rmsecv;
    classic=options.classic;
    rmsep=options.rmsep;
    rmsep_value = options.rmsep_value;
    rmsecv_value = options.rmsecv_value;
    rmsecv = options.rmsecv;
    rsquared_value = options.rsquared_value;
    rss_value = options.rss_value;
end

if q1==1 && k>=(h-2)
    mess=sprintf(['Attention (rsimpls.m): The number of components, k = ',num2str(k),...
            '\n is larger than our recommended maximum value of k = ',num2str(h-2)-1,'.']);
    disp(mess)
elseif q1>1 && k>=((h/q1)-(q1/2)-0.5)
    mess=sprintf(['Attention (rsimpls.m): The number of components, k = ',num2str(k),...
            '\n is larger than our recommended maximum value of k = ',num2str(floor((h/q1)-(q1/2)-1.5)),'.']);
    disp(mess)
end

%
%MAIN PART
%
% selection of number of components
if k == 0
    if rmsecv == 0
        [R2,final]=rsquared(x,y,kmax,'RSIMPLS',options.h);
        rss = final.rss;
        k = final.k;
        result=rsimpls(x,y,'k',k,'kr',k+q1,'h',h,'rmsecv',0,'rmsep',options.rmsep,'plots',0,'classic',classic,'rsquared_value',R2,'rss_value',rss);
    else
        out=rrmse(x,y,h,kmax,'RSIMPLS',1);
        R2 = out.R2;
        rss = out.rss;
        k=out.k;
        rmsecv_value = out.rmsecv;
        pred = rrmse(x,y,h,kmax,'RSIMPLS',0,k,out.weight,out.res);
        result=rsimpls(x,y,'k',k,'kr',k+q1,'h',h,'rmsecv',0,'rmsep',0,'plots',0,'classic',classic,'rmsep_value',pred.rmsep,'rmsecv_value',rmsecv_value,'rsquared_value',R2,'rss_value',rss);
    end
else
    if rmsecv
        error(['Both RMSECV and k were given.', ...
            'Please rerun your analysis with one of these inputs.  (see help file)'])
    end
%First stage: Obtain the scores T by first performing ROBPCA on z:
    if st<=1
        out.robpca=robpca(z,'k',kr,'h',h,'plots',plotsrobpca,'kmax',kmaxr,'classic',classic,'mcd',mcd);
        out.h=h;
        out.centerz=out.robpca.M;
        out.sigmaxy=out.robpca.P(1:p1,:)*diag(out.robpca.L)*out.robpca.P(p1+1:p1+q1,:)';
        out.sigmax=out.robpca.P(1:p1,:)*diag(out.robpca.L)*out.robpca.P(1:p1,:)';
        out.xcentr=x-repmat(out.centerz(1:p1),n,1);
        out.ycentr=y-repmat(out.centerz(p1+1:p1+q1),n,1);
        out.weights2=out.robpca.flag.all;
    end
    if st
        i=k;
    else
        i=1;
    end
    while i<=k
        out.sigmayx=out.sigmaxy';
        if q1>p1
            [RR,LL]=eig(out.sigmaxy*out.sigmayx);
            [LL,I]=greatsort(diag(LL));
            rr=RR(:,I(1));
            qq=out.sigmayx*rr;
            qq=qq/norm(qq);
        else
            [QQ,LL]=eig(out.sigmayx*out.sigmaxy);
            [LL,I]=greatsort(diag(LL));
            qq=QQ(:,I(1));
            rr=out.sigmaxy*qq;
            rr=rr/norm(rr);
        end
        tt=out.xcentr*rr;
        uu=out.ycentr*qq;
        pp=out.sigmax*rr/(rr'*out.sigmax*rr);
        vv=pp;
        if i>1
            vv=vv-out.v*(out.v'*pp);
        end
        if vv'*vv==0
            error('The number of components is too large')
        end
        vv=vv./norm(vv);
        out.sigmaxy=out.sigmaxy-vv*(vv'*out.sigmaxy);
        out.v(:,i)=vv;
        out.q(:,i)=qq;
        out.t(:,i)=tt;
        out.u(:,i)=uu;
        out.p(:,i)=pp;
        out.r(:,i)=rr;
        i=i+1;
    end

    %Second Stage : Robust ROBPCA-regression
    robpcareg=robpcaregres(out.t,y,out.weights2);
    breg=robpcareg.coeffs(1:k,:);
    Yhat=out.t*breg+repmat(robpcareg.coeffs(k+1,:),n,1);
    b=out.r*breg;
    int=robpcareg.coeffs(k+1,:)-out.centerz(1:p1)*out.r*breg;

    if rmsep==1
        rmse=rrmse(x,y,h,kmax,'RSIMPLS',0,k);
        out.rmsep=rmse.rmsep;
    end

    % testing several output parameters
    if ~isnan(rmsep_value)
        out.rmsep = rmsep_value;
        options.rmsep = 1;
    end

    if ~isnan(rsquared_value)
        out.rcs = rsquared_value;
    end
    if ~isnan(rss_value)
        out.rcs = [out.rcs;sqrt(rss_value)];
    end
    if ~isnan(rmsecv_value)
        gammahalf = 0.5*sqrt(rss_value) + 0.5*rmsecv_value;
        out.rcs = [out.rcs;gammahalf;rmsecv_value];
        options.rmsecv = 1;
    end
    if any(isnan(rsquared_value)) && any(isnan(rss_value)) && any(isnan(rmsecv_value))
        out.rcs = 0;
    end

   %The output:
   out.T=out.t;
   out.weights.p=out.p;
   out.weights.r=out.r;
   out.kr=kr;
   out.h=h;
   out.alpha=alfa;
   out.slope=b;
   out.int=int;
   out.yhat=x*b+repmat(int,n,1);
   out.x=x;
   out.y=y;
   out.res=y-out.yhat;
   out.class='RSIMPLS';
   out.k=k;
   out.cov=robpcareg.cov;

   if ~st
       %calculation of robust distances
       %Score distance
       out.Tcov=robpcareg.sigma(1:k,1:k);
       out.Tcenter=robpcareg.center(1:k);
       out.sd=sqrt(mahalanobis(out.t,out.Tcenter,'cov',out.Tcov))';
       out.cutoff.sd=sqrt(chi2inv(0.975,k));
       %robust residual distance
       if q1==1
           out.resd=out.res/sqrt(out.cov);
       else
           out.resd=sqrt(mahalanobis(out.res,zeros(1,q1),'cov',out.cov))';
       end
       %robust orthogonal distances
       xtilde=out.t*out.p';
       Cdiff=out.xcentr-xtilde;
       for i=1:n
           out.od(i,1)=norm(Cdiff(i,:));
       end
       r=rank(x);
       if k~=r
           [m,s]=unimcd(out.od.^(2/3),out.h);
           out.cutoff.od = sqrt(norminv(0.975,m,s)^3);
       else
           out.cutoff.od=0;
       end
       out.cutoff.resd=sqrt(chi2inv(0.975,q1));

       %Computing flags
       out.flag.od=out.od<=out.cutoff.od;
       out.flag.resd=abs(out.resd)<=out.cutoff.resd;
       out.flag.all=(out.flag.od & out.flag.resd);

       %Multivariate Rsquared
       Yw=y(out.flag.all==1,:);
       cYw=mcenter(Yw);
       res=out.res(out.flag.all==1,:);
       out.rsquared=1-(det(res'*res)/det(cYw'*cYw));

       %Assigning output
       if options.rmsep~=1 && options.rmsecv~=1
           out.rmsep=0;
       end

       if classic
           resultclassic=csimpls(x,y,'k',k,'plots',0);
       else
           resultclassic=0;
       end
       result=struct('slope',{out.slope}, 'int',{out.int},'fitted',{out.yhat},'res',{out.res}, 'cov',{out.cov},...
           'T',{out.T}, 'weights', {out.weights},'Tcenter',{out.Tcenter},'Tcov',{out.Tcov},'rsquared',{out.rsquared},'rcs',{out.rcs},'rmsep',{out.rmsep},...
           'k',{out.k},'alpha',{out.alpha},'h',{out.h},'sd', {out.sd},'od',{out.od},...
           'resd',{out.resd},'cutoff',{out.cutoff},'flag',{out.flag},'class',{out.class},'classic',{resultclassic},'robpca',{out.robpca});
       if result.rcs==0
           result=rmfield(result,'rcs');
       end
       if result.rmsep==0
           result=rmfield(result,'rmsep');
       end
   else
       result=out;
   end
end

% Plots
try
    if plots && options.classic
        makeplot(result,'classic',1,'labsd',labsd,'labod',labod,'labresd',labresd)
    elseif plots
        makeplot(result,'labsd',labsd,'labod',labod,'labresd',labresd)
    end
catch %output must be given even if plots are interrupted
end


%MDL_PLANAR2 Create model of a simple planar 2-link mechanism
%
% MDL_PLANAR2 is a script that creates the workspace variable p2 which
% describes the kinematic characteristics of a simple planar 2-link
% mechanism.
%
% Also defines the vector:
%   qz   corresponds to the zero joint angle configuration.
%
% Also defines the vector:
%   qz   corresponds to the zero joint angle configuration.
%
% Notes::
% - Moves in the XY plane.
% - No dynamics in this model.
%
% See also mdl_twolink, mdl_planar1, mdl_planar3, SerialLink.

% MODEL: generic, planar, 2DOF, symbolic, standard_DH

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

syms a1 a2 real;

p2 = SerialLink([
    Revolute('d', 0, 'a', a1, 'alpha', 0, 'standard')
    Revolute('d', 0, 'a', a2, 'alpha', 0, 'standard')
    ], ...
    'name', 'two link');
qz = [0 0];


function f = bilinear_kernel(k, num_input, num_output)
% -------------------------------------------------------------------------
%   Description:
%       create bilinear interpolation kernel for the convt (deconv) layer
%
%   Input:
%       - k             : kernel size k x k
%       - num_input     : number of input channels
%       - num_output    : number of output channels
%
%   Output:
%       - f             : bilinear filter
%
%   Citation:
%       Deep Laplacian Pyramid Networks for Fast and Accurate Super-Resolution
%       Wei-Sheng Lai, Jia-Bin Huang, Narendra Ahuja, and Ming-Hsuan Yang
%       IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2017
%
%   Contact:
%       Wei-Sheng Lai
%       wlai24@ucmerced.edu
%       University of California, Merced
% -------------------------------------------------------------------------

    radius = ceil(k / 2);

    if rem(k, 2) == 1
        center = radius;
    else
        center = radius + 0.5;
    end

    C = 1:k;
    f = (ones(1, k) - abs(C - center) ./ radius)' ...
      * (ones(1, k) - abs(C - center) ./ radius);

    f = repmat(f, 1, 1, num_input, num_output);

end


function y = l2lossForward( x,r )

delta = x - r;

y = sum(delta(:).^2);

y = y / (size(x,1)*size(x,2));

end


function [M,E,EMAP] = crouzeix_raviart_massmatrix(V,F)
  % CROUZEIX_RAVIART_MASSMATRIX Compute the Crouzeix-Raviart mass matrix where
  % M(e,e) is just the sum of 1/3 the areas of the triangles on either side of
  % an edge e. For tets, edges are now faccets.
  %
  % See for example "Discrete Quadratic Curvature Energies" [Wardetzky, Bergou,
  % Harmon, Zorin, Grinspun 2007]
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by element-size list of triangle indices
  % Outputs:
  %   M  #E by #E edge-based diagonal mass matrix
  %   E  #E by 2 list of edges
  %
  % See also: edge_laplacian, is_boundary_edge, crouzeix_raviart_cotmatrix,
  %   massmatrix
  %

  switch size(F,2)
  case 3
    allE = [F(:,[2 3]);F(:,[3 1]);F(:,[1 2])];
    % Map duplicate edges to first instance
    [E,~,EMAP] = unique(sort(allE,2),'rows');
    TA = doublearea(V,F)/2;
    M = sparse(EMAP,EMAP,repmat(TA/3,3,1),size(E,1), size(E,1));
  case 4
    T = F;
    allF = [ ...
      T(:,2) T(:,4) T(:,3); ...
      T(:,1) T(:,3) T(:,4); ...
      T(:,1) T(:,4) T(:,2); ...
      T(:,1) T(:,2) T(:,3); ...
      ];
    vol = volume(V,T);
    [F,~,FMAP] = unique(sort(allF,2),'rows');
    M = sparse(FMAP,FMAP,repmat(vol/4,4,1),size(F,1), size(F,1));
    E = F;
    EMAP = FMAP;
  end
end


function J=calcPolarConvJacob(zPolar,systemType,useHalfRange,lTx,lRx,M)
%%CALCPOLARCONVJACOB Calculate the Jacobian for a monostatic or bistatic
%            range and polar angle measurement in 2D with respect to
%            Cartesian position. Atmospheric effects are ignored. This
%            type of Jacobian is useful when performing tracking using
%            Cartesian-converted measurements where the clutter density is
%            specified in the measurement coordinate system, not the
%            converted measurement coordinate system.
%
%INPUTS: zPolar A 2X1 point in polar coordinates in the format
%          [range;azimuth], where the angle is given in radians and the
%          range can be bistatic.
% systemType An optional parameter specifying the axis from which the
%          azimuth angle is measured. It is assumed that the azimuth
%          angle is given in radians. Possible values are
%          0 (The default if omitted) 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
%          is false.
%      lTx The 2X1 [x;y] location vector of the transmitter in Cartesian
%          coordinates. If this parameter is omitted or an empty matrix is
%          passed, then the transmitter is assumed to be at the origin.
%      lRx The 2X1 [x;y] location vector of the receiver in Cartesian
%          coordinates. If this parameter is omitted or an empty matrix is
%          passed, then the receiver is assumed to be at the origin.
%        M A 2X2 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,2)
%          --the identity matrix is used.
%
%OUTPUTS: J The 2X2 Jacobian matrix. Each row is a components of range, and
%           azimuth (in that order by row) with derivatives taken with
%           respect to [x,y] by column.
%
%This function converts the measurement into Cartesian coordinates and then
%calls rangeGradient and polAngGradient.
%
%February 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<6||isempty(M))
   M=eye(2,2);
end

if(nargin<5||isempty(lRx))
   lRx=zeros(2,1);
end

if(nargin<4||isempty(lTx))
   lTx=zeros(2,1);
end

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

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

x=pol2Cart(zPolar,systemType,useHalfRange,lTx,lRx,M);

J=zeros(2,2);
J(1,:)=rangeGradient(x,useHalfRange,lTx,lRx);
J(2,:)=polAngGradient(x,systemType,lRx);

end

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


function y = beta_pdf(x,a,b)
%BETA_PDF    Beta probability density function (pdf).
%
%   Y = BETA_PDF(X,A,B) Returns the Beta pdf with
%   parameters A and B, at the values in X.
%
%   The size of Y is the common size of the input arguments. A
%   scalar input functions as a constant matrix of the same size as
%   the other inputs.
%
%   Default value for A and B is 1.

% Copyright (c) 2005 Aki Vehtari

% 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,
  a = 1;
end

if nargin < 2;
  b = 1;
end

if nargin < 1,
  error('Requires at least one input argument.');
end

y=exp((a-1).*log(x) +(b-1).*log(1-x) -betaln(a,b));


function result = lasso_cv(y, x, k, options)
% -------------------------------------------------------------------------
% function result = lasso_cv(y, x, k, options)
% -------------------------------------------------------------------------
% PURPOSE: fits the parameters of a linear model by using the lasso and
%          k-folds cross validation
% -------------------------------------------------------------------------
% INPUTS:
% y:         the values of the dependent variable
% x:         the values of the explanatory variables - constants will be
%            ignored
% k:         determine number of cross validation folds
% n_div:
% alignment:
% cv_assignment: allows that the division within groups to be defined by
%                the user. cv_assignment is a nx1 vector where n is the
%                number of available observations. The second columns
%                defines which observations belongs to each group.
%                If this
%                parameter is used, k is ignored and set to be equal to the
%                number of unique elements of cv_assignment;
% -------------------------------------------------------------------------
% OUTPUTS:
% a structure containing:
% all the fields contained in the strucutre returned by the LASSO function
% (see the lasso function help)  plus the following fields:
% result.CV.intercept    : intercept for the model chosen by CV
% result.CV.betas        : coefficients for the model selected by CV
% result.CV.fit          : Fitted values for the cross-validated s
% result.CV.residuals    : Residuals for the cross-validated s
% result.CV.SSR          : Sum of squared residuals for the cross-validated
%                          estimate
% result.CV.MSE          : minimum cross-validated MSE
% result.CV.cv_assignment: a matrix determining which observations where
%                          assigned to which CV groups
% result.CV.k            : number of CV folds
% -------------------------------------------------------------------------
% WARNING:
% unless cv_assignment is provided, this function invokes ASSIGN_CV.
% ASSIGN_CV calls randperm affecting the state of the random number gen.
% -------------------------------------------------------------------------
% Author: Guilherme V. Rocha
%         Department of Statistics
%         University of California, Berkeley
%         gvrocha@stat.berkeley.edu, gvrocha@gmail.com
% 2006/09
% -------------------------------------------------------------------------
% See also: LASSO, ASSIGN_CV, RANDPERM

if(nargin < 4)
  options = [];
end;

if(~isfield(options, 'alignment'))
  options.alignment = 'normalized_penalty';
end;
if(~isfield(options, 'cv_assignment'))
  assign_cv_flag = 1;
else
  assign_cv_flag = 0;
  cv_assignment  = options.cv_assignment;
  k              = length(unique(cv_assignment));
end;
if(~isfield(options, 'num_div'))
  options.num_div =100;
end;
n_div = options.num_div;
alignment = options.alignment;

n_obs           = size(y, 1);
max_index       = -Inf;
if(assign_cv_flag)
  cv_assignment = assign_cv(n_obs, k);
end;

for i = 1:k
  fit_indexes   = find(cv_assignment~=i);
  y_fit         = y(fit_indexes);
  x_fit         = x(fit_indexes,:);
  res(i)        = lasso_rocha(y_fit, x_fit, options);
  switch lower(alignment)
    case 'normalized_penalty'
      max_index = max(max_index, max(res(i).npenalty));
    case 'penalty'
      max_index = max(max_index, max(res(i).penalty));
    case 'lambda'
      max_index = max(max_index, max(res(i).lambda));
  end;
end;

index_slices = 0:max_index/n_div:max_index;

for i = 1:k
  interpol    = lasso_coefficients(res(i), index_slices, alignment);
  cv_indexes  = find(cv_assignment==i);
  y_cv        = y(cv_indexes);
  x_cv        = x(cv_indexes,:);

  y_pred      = lasso_predict(res(i), x_cv, index_slices, options);
  residuals   = repmat(y_cv, 1, size(y_pred, 2)) - y_pred;
  if(size(residuals, 1)>1)
    MSE(i,:)    = mean(residuals.^2);
  else
    MSE(i,:)    = residuals.^2;
  end;
end;

[min_MSE, best_cv_MSE] = min(mean(MSE));
best_index             = index_slices(best_cv_MSE);
result                 = lasso_rocha(y, x, options);

result.CV                = lasso_coefficients(result, best_index, alignment);
result.CV.cv_index       = best_cv_MSE;
result.CV.cv_assignment  = cv_assignment;
result.CV.k              = k;
result.CV.min_MSE        = min_MSE;
result.CV.MSEs           = MSE;
result.CV.path           = lasso_coefficients(result, index_slices, alignment);
result.options           = options;
rm_fields                = {'ny', 'nx', 'xtx', 'xty', 'beta', 'intercept', 'penalty', 'xtrs'};
result.CV.fold_results   = rmfield(res, intersect(fieldnames(res), rm_fields));


function B = dtimatrix(bvalues,bvectors)
% B = dtimatrix(bvalues,bvectors)
%
% Constructs a DTI design matrix from the bvalues and bvectors.
% bvalues is a vector of length N
% bvectors is a matrix either Nx3 or 3xN
%
% B will be N by 7
% The 7th is the mean (all ones)
% The tensor will be constructed using the following regressors
%     1 2 3
%     2 4 5
%     3 5 6
%
% $Id: dtimatrix.m,v 1.2 2011/03/02 00:04:12 nicks Exp $

%
% dtimatrix.m
%
% Original Author: Doug Greve
% CVS Revision Info:
%    $Author: nicks $
%    $Date: 2011/03/02 00:04:12 $
%    $Revision: 1.2 $
%
% Copyright © 2011 The General Hospital Corporation (Boston, MA) "MGH"
%
% Terms and conditions for use, reproduction, distribution and contribution
% are found in the 'FreeSurfer Software License Agreement' contained
% in the file 'LICENSE' found in the FreeSurfer distribution, and here:
%
% https://surfer.nmr.mgh.harvard.edu/fswiki/FreeSurferSoftwareLicense
%
% Reporting: freesurfer@nmr.mgh.harvard.edu
%

if(nargin ~= 2)
  fprintf('B = dtimatrix(bvalues,bvectors)\n');
  return;
end

if(size(bvectors,2) ~= 3) bvectors = bvectors'; end
if(size(bvectors,2) ~= 3)
  fprintf('ERROR: bvectors must be Nx3 or 3xN\n');
  return;
end
Nb = size(bvectors,1);

if(Nb ~= length(bvalues))
  fprintf('ERROR: dimension mismatch between bvectors and bvalues\n');
  return;
end

bvalues = bvalues(:);

B = zeros(Nb,7);
B(:,1) =     bvalues .* bvectors(:,1).*bvectors(:,1);
B(:,2) = 2 * bvalues .* bvectors(:,1).*bvectors(:,2);
B(:,3) = 2 * bvalues .* bvectors(:,1).*bvectors(:,3);
B(:,4) =     bvalues .* bvectors(:,2).*bvectors(:,2);
B(:,5) = 2 * bvalues .* bvectors(:,2).*bvectors(:,3);
B(:,6) =     bvalues .* bvectors(:,3).*bvectors(:,3);
B(:,7) = 1;

return;


function [curve, params, errors, T] = fit_poly_to_fragment(fragment, order)
%
%[curve, params, errors] = fit_poly_to_fragment(fragment, order)
%
% Fit a polynomial curve of specified order to an edge fragment.  A
% fragment is simply an Nx2 vector of (x,y) coordinates.
%
% Can also return fit error and the polynomial parameters.
%

% Ensure there are enough points:
if(isempty(fragment))
    error('Supplied fragment contains no points!');
end

N = size(fragment,1);

% Reduce the order if not enough points are provided to fit the requested
% order polynomial (e.g. we need N=4 points to fit a cubic, if only N=3
% points are provided, this will reduce the order to 2, thereby enabling
% successful fitting of a quadratic)
while( N < (order+1) )
    order = order-1;
end

t = linspace(0,1,N)';

fit_normals = false;
% if(nargin==2)
%     order = normal_angles;
%     fit_normals = false;
% end

T = ones(N,1);
for(i_order = 1:order)
    T = [t.^i_order T];
end

weights = ones(N, 1);
weights(1) = N/4;
weights(end) = N/4;
if(~fit_normals)
    % Fit the polynomial so that it simply tries to pass through the
    % fragment's vertex coordinates.
    params = (T .* repmat(weights, [1, order+1])) \ (fragment .* repmat(weights, [1, 2]));

%     % Constrained least squares to get the start and end points to exactly
%     % match up to the input coordinates:
%     for(i=1:2)
%         params(:,i) = lsqlin(T(2:end-1,:), fragment(2:end-1,i), ...
%             T([1 end],:), fragment([1 end],i),T([1 end],:), fragment([1 end],i));
%     end
else
    error(['Um, actually, trying to constrain the slopes this way will NOT ' ...
           'work. It is not linear because we do not know the length of the ' ...
           'normal vectors, only the direction.']);

%     params = T \ fragment;
%     L = L_helper(params, t, order);
%
%     % Fit the polynomial so that it tries to pass through the coordinates of the
%     % input fragment AND tries to have slope matching the orientations of
%     % each vertex in the fragment.
%     T_orient = [ones(N,1) zeros(N,1)];
%     for(i_order = 1:(order-1))
%         T_orient = [(i_order+1)*t.^i_order T_orient];
%     end
%
%     for(i=1:100)
%         TT = [blkdiag(T,T); blkdiag(-T_orient, T_orient)];
%
%         params = TT \ [fragment(:); L.*sin(normal_angles); L.*cos(normal_angles)];
%         params = reshape(params, [order+1 2]);
%
%         L = L_helper(params, t, order);
%     end

end

curve = T*params;
if(nargout>=3)
    errors = abs(curve - fragment);
end

return;

function L = L_helper(params, t, order)

dx = params(end-1,1);
dy = params(end-1,2);
for(i_order = 2:order)
    dx = dx + i_order*params(end-i_order,1)*t.^(i_order-1);
    dy = dy + i_order*params(end-i_order,2)*t.^(i_order-1);
end

L = sqrt(dx.^2 + dy.^2);
return;


function [ fxx, fxy, fyy ] = f03_f2 ( n, x, y )

%*****************************************************************************80
%
%% F03_F2 returns second derivatives of function 3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N,1), Y(N,1), the evalution points.
%
%    Output, real FXX(N,1), FXY(N,1), FYY(N,1), second derivatives.
%
  t1(1:n,1) = 5.4 * y(1:n,1);
  t2(1:n,1) = 1.0 + ( 3.0 * x(1:n,1) - 1.0 ).^2;

  fxx(1:n,1) = 3.0 * ( 1.25 + cos ( t1(1:n,1) ) ) .* ( 3.0 * t2(1:n,1) - 4.0 ) ...
    ./ ( t2(1:n,1).^3 );
  fxy(1:n,1) = 5.4 * ( 3.0 * x(1:n,1) - 1.0 ) .* sin ( t1(1:n,1) ) ...
    ./ ( t2(1:n,1) .* t2(1:n,1) );
  fyy(1:n,1) = - 4.86 * cos ( t1(1:n,1) ) ./ t2(1:n,1);

  return
end


function [pc,r]=circumcenter(p,t)

%   Copyright (C) 2004-2006 Per-Olof Persson. See COPYRIGHT.TXT for details.

nt=size(t,1);
pc=zeros(nt,2);
r=zeros(nt,1);

for it=1:nt
  ct=t(it,:);
  dp1=p(ct(2),:)-p(ct(1),:);
  dp2=p(ct(3),:)-p(ct(1),:);

  mid1=(p(ct(2),:)+p(ct(1),:))/2;
  mid2=(p(ct(3),:)+p(ct(1),:))/2;

  s=[-dp1(2),dp2(2);dp1(1),-dp2(1)]\[-mid1+mid2]';

  cpc=mid1+s(1)*[-dp1(2),dp1(1)];
  cr=norm(p(ct(1),:)-cpc);

  pc(it,:)=cpc;
  r(it,1)=cr;
end


function ncc_set_test ( )

%*****************************************************************************80
%
%% NCC_SET_TEST tests NCC_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'NCC_SET_TEST\n' );
  fprintf ( 1, '  NCC_SET sets up a Newton-Cotes Closed quadrature rule;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Index       X             W\n' );
  fprintf ( 1, '\n' );

  for n = 1 : 10

    [ x, w ] = ncc_set ( n );

    fprintf ( 1, '\n' );

    for i = 1 : n
      fprintf ( 1, '  %2d  %12g  %12g\n', i, x(i), w(i) );
    end

  end

  return
end


function beale_test ( )

%*****************************************************************************80
%
%% BEALE_TEST works with the Beale function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BEALE_TEST:\n' );
  fprintf ( 1, '  Test COMPASS_SEARCH with the Beale function.\n' );
  m = 2;
  delta_tol = 0.00001;
  delta = 0.1;
  k_max = 20000;

  x = [ 1.0, 1.0 ];
  r8vec_print ( m, x, '  Initial point X0:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', beale ( m, x ) );

  [ x, fx, k ] = compass_search ( @beale, m, x, delta_tol, delta, k_max );
  r8vec_print ( m, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g, number of steps = %d\n', fx, k );
%
%  Repeat with more difficult start.
%
  x = [ 1.0, 4.0 ];
  r8vec_print ( m, x, '  Initial point X0:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', beale ( m, x ) );

  [ x, fx, k ] = compass_search ( @beale, m, x, delta_tol, delta, k_max );
  r8vec_print ( m, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g, number of steps = %d\n', fx, k );
%
%  Demonstrate correct minimizer.
%
  x = [ 3.0, 0.5 ];
  r8vec_print ( m, x, '  Correct minimizer X*:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %g\n', beale ( m, x ) );

  return
end


function flag = isodd(n)
%ISODD returns true if the input is odd.

flag = mod(n,2);

% Copyright 2002 - 2009 The MathWorks, Inc.


function[num,a,b]=blocknum(x,delta)
%BLOCKNUM  Numbers the contiguous blocks of an array.
%
%   Suppose X is a column vector which contains blocks of identical
%   values, e.g. X=[0 0 0 1 1 3 2 2 2]';
%
%   N=BLOCKNUM(X) gives each contiguous block of identical values a
%   unique number, in order beginning with 1.  Each elements of N
%   specifies the number of the block to which the corresponding
%   element of X belongs.
%
%   In the above example, N=[1 1 1 2 2 3 4 4 4]';
%
%   [N,A,B]=BLOCKNUM(X) also returns arrays A and B which are indices
%   into the first and last point, respectively, of each block.  In
%   the above example, A=[1 4 6 7]' and B=[3 5 6 9]';
%
%   [...]=BLOCKNUM(X,D) defines the junction between two blocks as
%   locations where ABS(DIFF(X))>D.  Thus D=1 is a 'rate of change'
%   definition, and X=[1 2 3 5 6 10 16 17 18]'; will yield the same
%   result for N as in the previous example.
%
%   See also BLOCKLEN.
%
%   Usage: num=blocknum(x);
%          [num,a,b]=blocknum(x);
%   _________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2000--2014 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(x,'--t')
  blocknum_test;return;
end

if nargin==1
  delta=0;
end

dx=diff(x);
index=find(abs(dx)>delta);
num=zeros(size(x));
if ~isempty(index)
  num(index+1)=1;
end
num(1)=1;
num=cumsum(num);

if nargout>=2
   a=[1;find(diff(num)~=0)+1];
end
if nargout>=3
  b=[find(diff(num)~=0);length(num)];
end

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

function[]=blocknum_test
x= [0 0 0 1 1 3 2 2 2]';
y= [1 1 1 2 2 3 4 4 4]';

reporttest('BLOCKNUM with D==0',all(y==blocknum(x)))

x=[1 2 3 5 6 10 16 17 18]';
reporttest('BLOCKNUM with D==1',all(y==blocknum(x,1)))


function [u,x]=initialvalues(uflux,a,b,init_f,N);
%
%   Approximates the function init_f on the interval [a b] by a piecewise
%   constant function taking values in the set {uflux}.
%   The output is the location of the discontinuities x and u such that
%   the discontinuity between u(i-1) and u(i) is located at x(i).
%
if nargin<5,
  N=1/(uflux(2)-uflux(1));
end;
x=linspace(a,b,N);
xf=0.5*(x(2:N)+x(1:N-1));
u1=feval(init_f,xf);
A=ones(size(u1))'*uflux;
B=ones(size(uflux))'*u1;
[m ind]=min(abs(A'-B));
h=uflux(ind);
x=x(2:N-1);
d=diff(h);
ind=d~=0;
[i s]=find(ind);
u=[h(s) h(N-1)];
x=x(s);


function Y = sl2dpca_apply(Mm, PL, PR, data, matsiz, n)
%SL2DPCA_APPLY Applies 2D PCA onto a set of matrices to extract features
%
% $ Syntax $
%   - Y = sl2dpca_apply(Mm, PL, PR, data, matsiz, n)
%
% $ Description $
%   - Mm:           the mean matrix
%   - PL:           the left projection matrix
%   - PR:           the right projection matrix
%   - data:         the matrix samples or the cell array of filenames
%   - matsiz:       the original matrix size
%   - n:            the number of samples
%   - Y:            the extracted 2D features
%
% $ Description $
%   - Y = sl2dpca_apply(data, Mm, PL, PR) extracts 2D features for
%     the matrices given in data, in either a 3D array or a set of
%     array filenames. Suppose the original matrix size is d1 x d2,
%     PL be d1 x k1, PR be d2 x k2, then the feature matrix would be
%     of size k1 x k2. Y is a k1 x k2 x n array.
%
% $ History $
%   - Created by Dahua Lin, on Jul 31st, 2006
%

%% Parse and verify input arguments

if nargin < 6
    raise_lackinput('sl2dpca_apply', 6);
end

matsiz = matsiz(:)';
if length(matsiz) ~= 2
    error('sltoolbox:invalidarg', ...
        'matsiz should be a 2-elem vector');
end

if ~isequal(size(Mm), matsiz)
    error('sltoolbox:sizmismatch', ...
        'the sample size does not match the model');
end
d1 = matsiz(1);
d2 = matsiz(2);

if size(PL, 1) ~= d1 || size(PR, 1) ~= d2
    error('sltoolbox:sizmismatch', ...
        'the size of projection matrices are illegal');
end

%% Compute

if isnumeric(data)

    if size(data, 3) ~= n
        error('sltoolbox:sizmismatch', ...
            'The number of samples is not as specified');
    end

    Y = computeY(data, Mm, PL, PR);

elseif iscell(data)

    Y = zeros(size(PL, 2), size(PR, 2), n);

    nfiles = length(data);
    cf = 0;
    for i = 1 : nfiles
        curdata = slreadarray(data{i});
        curn = size(curdata, 3);
        Y(:,:,cf+1:cf+curn) = computeY(curdata, Mm, PL, PR);
        cf = cf + curn;
    end

else
    error('sltoolbox:invalidarg', ...
        'data should be a numeric array or a cell array of filenames');

end

%% Core compute function

function Y = computeY(X, Mm, PL, PR)

n = size(X, 3);
Y = zeros(size(PL, 2), size(PR, 2), n);
PLT = PL';

for i = 1 : n
    Y(:,:,i) = PLT * (X(:,:,i) - Mm) * PR;
end


function [h,g,a,info]=wfilt_remez(L,K,B)
%WFILT_REMEZ Filters designed using Remez exchange algorithm
%   Usage: [h,g,a]=wfilt_remez(L,K,B)
%
%   Input parameters:
%         L     : Length of the filters.
%         K     : Degree of flatness (regularity) at $z=-1$.
%         B     : Normalized transition bandwidth.
%
%   `[h,g,a]=wfilt_remez(L,K,B)` calculates a set of wavelet filters.
%   Regularity, frequency selectivity, and length of the filters can be
%   controlled by *K*, *B* and *L* parameters respectivelly.
%
%   The filter desigh algorithm is based on a Remez algorithm and a
%   factorization of the complex cepstrum of the polynomial.
%
%   Examples:
%   ---------
%   :::
%
%     wfiltinfo('remez50:2:0.1');
%
%   References: rioul94remez

% Original copyright goes to:
% Copyright (C) 1994, 1995, 1996, by Universidad de Vigo
% Author: Jose Martin Garcia
% e-mail: Uvi_Wave@tsc.uvigo.es

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

complainif_notposint(L,'L',mfilename);
complainif_notposint(L,'K',mfilename);

if B>0.2
    error(['%s: Bandwidth of the transition band should not be',...
           ' bigger than 0.2.'],upper(mfilename));
end

poly=remezwav(L,K,B);
rh=fc_cceps(poly);

g{1} = flipud(rh(:));
g{2} = -(-1).^(1:length(rh)).'.*flipud(g{1});

% Default offset
d = [0,0];
  % Do a filter alignment according to "center of gravity"
  d(1) = -floor(sum((1:L)'.*abs(g{1}).^2)/sum(abs(g{1}).^2));
  d(2) = -floor(sum((1:L)'.*abs(g{2}).^2)/sum(abs(g{2}).^2));
  if rem(d(1)-d(2),2)==1
      % Shift d(2) just a bit
      d(2) = d(2) + 1;
  end

g = cellfun(@(gEl,dEl) struct('h',gEl,'offset',dEl),g,num2cell(d),...
            'UniformOutput',0);
h = g;

a= [2;2];
info.istight = 1;

function [p,r]=remezwav(L,K,B)

%REMEZWAV    P=REMEZWAV(L,K,B) gives impulse response of maximally
%	     frequency selective P(z), product filter of paraunitary
%	     filter bank solution H(z) of length L satisfying K flatness
%	     constraints (wavelet filter), with normalized transition
%	     bandwidth B (optional argument if K==L/2).
%
%	     [P,R]=REMEZWAV(L,K,B) also gives the roots of P(z) which can
%	     be used to determine H(z).
%
%	     See also: REMEZFLT, FC_CCEPS.
%
%	     References: O. Rioul and P. Duhamel, "A Remez Exchange Algorithm
%			 for Orthonormal Wavelets", IEEE Trans. Circuits and
%			 Systems - II: Analog and Digital Signal Processing,
%			 41(8), August 1994
%
%       Author: Olivier Rioul, Nov. 1, 1992 (taken from the
%		above reference)
%  Modified by: Jose Martin Garcia
%       e-mail: Uvi_Wave@tsc.uvigo.es
%--------------------------------------------------------

computeroots=(nargout>1);

%%%%%%%%%%%%%%%%%%%%%%%%%% STEP 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%
if rem(L,2), error('L must be even'); end
if rem(L/2-K,2), K=K+1; end
N=L/2-K;
%%%%%%%%%%%%%%%%%%%%%%%%%% STEP 2  %%%%%%%%%%%%%%%%%%%%%%%%%%
% Daubechies solution
% PK(z)=z^(-2K-1))+AK(z^2)
if K==0, AK=0;
else
   binom=pascal(2*K,1);
   AK=binom(2*K,1:K)./(2*K-1:-2:1);
   AK=[AK AK(K:-1:1)];
   AK=AK/sum(AK);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%% STEP 2' %%%%%%%%%%%%%%%%%%%%%%%%%%%
% Daubechies factor
% PK(z)=((1+z^(-1))/2)^2*K QK(z)
if computeroots && K>0
   QK=binom(2*K,1:K);
   QK=QK.*abs(QK);
   QK=cumsum(QK);
   QK=QK./abs(binom(2*K-1,1:K));
   QK=[QK QK(K-1:-1:1)];
   QK=QK/sum(QK)*2;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%% STEP 3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
% output Daubechies solution PK(z)
if K==L/2
   p=zeros(1,2*L-1);
   p(1:2:2*L-1)=AK; p(L)=1;
   if computeroots
      r=[roots(QK); -ones(L,1)];
   end
   return
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% STEP 4 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Daubechies polinomial
% PK(x)=1+x*DK(x^2)
if K==0, DK=0;
else
   binom=pascal(K,1);
   binom=binom(K,:);
   DK=binom./(1:2:2*K-1);
   DK=fliplr(DK)/sum(DK);
end

wp=(1/2-B)*pi;  % cut-off frequency
gridens=16*(N+1);  % grid density
found=0;  % boolean for Remez loop

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% STEP I %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial estimate of yk
a=min(4,K)/10;
yk=linspace(0,1-a,N+1);
yk=(yk.^2).*(3+a-(2+a)*yk);
yk=1-(1-yk)*(1-cos(wp)^2);
ykold=yk;

iter=0;
while 1  % REMEZ LOOP
iter=iter+1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% STEP II %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute delta
Wyk=sqrt(yk).*((1-yk).^K);
Dyk=(1-sqrt(yk).*polyval(DK,yk))./Wyk;
for k=1:N+1
   dy=yk-yk(k); dy(k)=[];
   dy=dy(1:N/2).*dy(N:-1:N/2+1);
   Lk(k)=prod(dy);
end
invW(1:2:N+1)=2./Wyk(1:2:N+1);
delta=sum(Dyk./Lk)/sum(invW./Lk);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% STEP III %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute R(y) on fine grid
Ryk=Dyk-delta.*invW; Ryk(N+1)=[];
Lk=(yk(1:N)-yk(N+1))./Lk(1:N);
y=linspace(cos(wp)^2,1-K*1e-7,gridens);
yy=ones(N,1)*y-yk(1:N)'*ones(1,gridens);
% yy contain y-yk on each line
ind=find(yy==0);  % avoid division by 0
if ~isempty(ind)
   yy(ind)=1e-30*ones(size(ind));
end
yy=1./yy;
Ry=((Ryk.*Lk)*yy)./(Lk*yy);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% STEP IV %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% find next yk
Ey=1-delta-sqrt(y).*(polyval(DK,y)+((1-y).^K).*Ry);
k=find(abs(diff(sign(diff(Ey))))==2)+1;
% N extrema
if length(k)>N
% may happen if L and K are large
   k=k(1:N);
end
yk=[yk(1) y(k)];
% N+1 extrema including wp
if K==0, yk=[yk 1]; end
% extrema at y==1 added
if all(yk==ykold), break; end
ykold=yk;

end  % REMEZ LOOP

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  STEP A %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute impulse response
w=(0:2*N-2)*pi/(2*N-1);
y=cos(w).^2;
yy=ones(N,1)*y-yk(1:N)'*ones(1,2*N-1);
ind=find(yy==0);
if ~isempty(ind)
   yy(ind)=1e-30*ones(size(ind));
end
yy=1./yy;
Ry=((Ryk.*Lk)*yy)./(Lk*yy);
Ry(2:2:2*N-2)=-Ry(2:2:2*N-2);
r=Ry*cos(w'*(2*(0:N-1)+1));
% partial real IDFT done
r=r/(2*N-1);
r=[r r(N-1:-1:1)];
p1=[r 0]+[0 r];
pp=p1;  % save p1 for later use
for k=1:2*K
   p1=[p1 0]-[0 p1];
end
if rem(K,2), p1=-p1; end
p1=p1/2^(2*K+1);
p1(N+1:N+2*K)=p1(N+1:N+2*K)+AK;
% add Daubechies response:
p(1:2:2*L-1)=p1; p(L)=1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% STEP A' %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute roots
if computeroots
   Q(1:2:2*length(pp)-1)=pp;
   for k=1:2*K
     Q=[Q 0]-[0 Q];
   end
   if rem(K,2), Q=-Q; end
   Q=Q/2;
   if K>0  % add Daubechies factor QK
      Q(2*N+1:L-1)=Q(2*N+1:L-1)+QK;
   else
      Q(L)=1;
   end
   r=[roots(Q); -ones(2*K,1)];
end

function  h=fc_cceps(poly,ro)

%FC_CCEPS    Performs a factorization using complex cepstrum.
%
%	     H = FC_CCEPS (POLY,RO) provides H that is the spectral
%	     factor of a FIR transfer function POLY(z) with non-negative
%	     frequency response. This methode let us obtain lowpass
%	     filters of a bank structure without finding the POLY zeros.
%	     The filter obtained is minimum phase (all zeros are inside
%	     unit circle).
%
%	     RO is a parameter used to move zeros out of unit circle.
%	     It is optional and the default value is RO=1.02.
%
%	     See also: INVCCEPS, MYCCEPS, REMEZWAV.
%
%	     References: P.P Vaidyanathan, "Multirate Systems and Filter
%			 Banks", pp. 849-857, Prentice-Hall, 1993

%--------------------------------------------------------
% Copyright (C) 1994, 1995, 1996, by Universidad de Vigo
%
%
% Uvi_Wave 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, or (at your option) any
% later version.
%
% Uvi_Wave 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 Uvi_Wave; see the file COPYING.  If not, write to the Free
% Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
%
%       Author: Jose Martin Garcia
%       e-mail: Uvi_Wave@tsc.uvigo.es
%--------------------------------------------------------

if nargin < 2
	ro=1.02;
end

L=4096;   % number points of fft.

N=(length(poly)-1)/2;

%% Moving zeros out of unit circle
roo=(ro).^[0:2*N];
g=poly./roo;

%% Calculate complex cepstrum of secuence g
ghat=mycceps(g,L);

%% Fold the anticausal part of ghat, add it to the causal part and divide by 2
gcausal=ghat(1 : L/2);
gaux1=ghat(L/2+1 : L);
gaux2=gaux1(L/2 :-1: 1);
gantic=[0 gaux2(1 : L/2-1)];

xhat=0.5*(gcausal+gantic);

%% Calculate cepstral inversion
h=invcceps(xhat,N+1);

%% Low-pass filter has energie sqrt(2)
h=h*sqrt(2)/sum(h);

function  x=invcceps(xhat,L)

%INVCCEPS    Complex cepstrum Inversion
%
%	     X= INVCCEPS (CX,L) recovers X from its complex cepstrum sequence
%	     CX. X has to be real, causal, and stable (X(z) has no zeros
%	     outside unit circle) and x(0)>0. L is the length of the
%	     recovered secuence.
%
%	     See also: MYCCEPS, FC_CCEPS, REMEZWAV.
%
%	     References: P.P Vaidyanathan, "Multirate Systems and Filter
%			 Banks", pp. 849-857, Prentice-Hall, 1993

%--------------------------------------------------------
% Copyright (C) 1994, 1995, 1996, by Universidad de Vigo
%
%
% Uvi_Wave 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, or (at your option) any
% later version.
%
% Uvi_Wave 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 Uvi_Wave; see the file COPYING.  If not, write to the Free
% Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
%
%       Author: Jose Martin Garcia
%       e-mail: Uvi_Wave@tsc.uvigo.es
%--------------------------------------------------------

x=zeros(1,L);

%% First point of x
x(1)=exp(xhat(1));

%% Recursion to obtain the other point of x
for muestra=1:L-1
   for k=1:muestra
	x(muestra+1)=x(muestra+1)+k/muestra*xhat(k+1)*x(muestra-k+1);
   end
end

function xhat=mycceps(x,L)

%MYCCEPS     Complex Cepstrum
%
%	     CX = MYCCEPS (X,L) calculates complex cepstrum of the
%	     real sequence X. L is the number of points of the fft
%	     used. L is optional and its default value is 1024 points.
%
%	     See also: FC_CEPS, INVCCEPS, REMEZWAV.

%--------------------------------------------------------
% Copyright (C) 1994, 1995, 1996, by Universidad de Vigo
%
%
% Uvi_Wave 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, or (at your option) any
% later version.
%
% Uvi_Wave 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 Uvi_Wave; see the file COPYING.  If not, write to the Free
% Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
%
%       Author: Jose Martin Garcia
%       e-mail: Uvi_Wave@tsc.uvigo.es
%--------------------------------------------------------

if nargin < 2
   L=1024;
end

H = fft(x,L);

%% H must not be zero
ind=find(abs(H)==0);
if length(ind) > 0
   H(ind)=H(ind)+1e-25;
end

logH = log(abs(H))+sqrt(-1)*rcunwrap(angle(H));

xhat = real(ifft(logH));

function y = rcunwrap(x)
%RCUNWRAP Phase unwrap utility used by CCEPS.
%	RCUNWRAP(X) unwraps the phase and removes phase corresponding
%	to integer lag.  See also: UNWRAP, CCEPS.

%	Author(s): L. Shure, 1988
%		   L. Shure and help from PL, 3-30-92, revised
%	Copyright (c) 1984-94 by The MathWorks, Inc.
%       $Revision: 1.4 $  $Date: 1994/01/25 17:59:42 $

n = max(size(x));
y = unwrap(x);
nh = fix((n+1)/2);
y(:) = y(:)' - pi*round(y(nh+1)/pi)*(0:(n-1))/nh;


function p = predictOneVsAll(all_theta, X)
%PREDICT Predict whether the label is 0 or 1 using learned logistic
%regression parameters all_theta
%   p = PREDICT(all_theta, X) computes the predictions for X using a
%   threshold at 0.5 (i.e., if sigmoid(all_theta'*x) >= 0.5, predict 1)

m = size(X, 1);
num_labels = size(all_theta, 1);

% You need to return the following variables correctly
p = zeros(size(X, 1), 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters (one-vs-all).
%               You should set p to a vector of predictions (from 1 to
%               num_labels).
%
% Hint: This code can be done all vectorized using the max function.
%       In particular, the max function can also return the index of the
%       max element, for more information see 'help max'. If your examples
%       are in rows, then, you can use max(A, [], 2) to obtain the max
%       for each row.
%

% train each row for all classes and keep the label with the highest probability
htheta = X * all_theta';
[temp, p] = max(htheta, [], 2);

% =========================================================================

end


function [R, scale]=arqr(v, p, mcor)
%ARQR	QR factorization for least squares estimation of AR model.
%
%  [R, SCALE]=ARQR(v,p,mcor) computes the QR factorization needed in
%  the least squares estimation of parameters of an AR(p) model. If
%  the input flag mcor equals one, a vector of intercept terms is
%  being fitted. If mcor equals zero, the process v is assumed to have
%  mean zero. The output argument R is the upper triangular matrix
%  appearing in the QR factorization of the AR model, and SCALE is a
%  vector of scaling factors used to regularize the QR factorization.
%
%  ARQR is called by ARFIT.
%
%  See also ARFIT.

%  Modified 29-Dec-99
%  Author: Tapio Schneider
%          tapio@gps.caltech.edu

  % n: number of time steps; m: dimension of state vectors
  [n,m] = size(v);

  ne    = n-p;                  % number of block equations of size m
  np    = m*p+mcor;             % number of parameter vectors of size m

  % If the intercept vector w is to be fitted, least squares (LS)
  % estimation proceeds by solving the normal equations for the linear
  % regression model
  %
  %                  v(k,:)' = Aaug*u(k,:)' + noise(C)
  %
  % with Aaug=[w A] and `predictors'
  %
  %              u(k,:) = [1 v(k-1,:) ...  v(k-p,:)].
  %
  % If the process mean is taken to be zero, the augmented coefficient
  % matrix is Aaug=A, and the regression model
  %
  %                u(k,:) = [v(k-1,:) ...  v(k-p,:)]
  %
  % is fitted.
  % The number np is the dimension of the `predictors' u(k).

  % Assemble the data matrix K (of which a QR factorization will be computed)
  K = zeros(ne,np+m);                 % initialize K
  if (mcor == 1)
    % first column of K consists of ones for estimation of intercept vector w
    K(:,1) = ones(ne,1);
  end

  % Assemble `predictors' u in K
  for j=1:p
    K(:, mcor+m*(j-1)+1:mcor+m*j) = [v(p-j+1:n-j, :)];
  end
  % Add `observations' v (left hand side of regression model) to K
  K(:,np+1:np+m) = [v(p+1:n, :)];

  % Compute regularized QR factorization of K: The regularization
  % parameter delta is chosen according to Higham's (1996) Theorem
  % 10.7 on the stability of a Cholesky factorization. Replace the
  % regularization parameter delta below by a parameter that depends
  % on the observational error if the observational error dominates
  % the rounding error (cf. Neumaier, A. and T. Schneider, 2001:
  % "Estimation of parameters and eigenmodes of multivariate
  % autoregressive models", ACM Trans. Math. Softw., 27, 27--57.).
  q     = np + m;             % number of columns of K
  delta = (q^2 + q + 1)*eps;  % Higham's choice for a Cholesky factorization
  scale = sqrt(delta)*sqrt(sum(K.^2));
  R     = triu(qr([K; diag(scale)]));


function msm_to_mm_test03 ( )

%*****************************************************************************80
%
%% MSM_TO_MM_TEST03 tests MSM_TO_MM_ARRAY_COMPLEX_SKEWSYMMETRIC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MSM_TO_MM_TEST03\n' );
  fprintf ( 1, '  Convert an MSM to MM array complex skew-symmetric format.\n' );

  output_filename = 'msm_to_mm_test03.mm';
  a = c8mat_indicator ( 4, 4 );
  for j = 1 : 4
    a(j,j) = 0.0;
    for i = j + 1 : 4
      a(i,j) = - a(j,i);
    end
  end
%
%  Have MSM_TO_MM write the matrix to a file.
%
  msm_to_mm ( output_filename, a, 'array', 'complex', 'skew-symmetric' );

  return
end


function c = tapas_gaussian_obs_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the Gaussian noise observation model for continuous responses
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The Gaussian noise observation model assumes that responses have a Gaussian distribution around
% the inferred mean of the relevant state. The only parameter of the model is the noise variance
% (NOT standard deviation) zeta.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, 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/>.

% Config structure
c = struct;

% Model name
c.model = 'tapas_gaussian_obs';

% Sufficient statistics of Gaussian parameter priors
%
% Zeta
c.logzemu = log(0.005);
c.logzesa = 0.1;

% Gather prior settings in vectors
c.priormus = [
    c.logzemu,...
         ];

c.priorsas = [
    c.logzesa,...
         ];

% Model filehandle
c.obs_fun = @tapas_gaussian_obs;

% Handle to function that transforms observation parameters to their native space
% from the space they are estimated in
c.transp_obs_fun = @tapas_gaussian_obs_transp;

return;


function dUpsilonS = lfmaaGradientSigmaUpsilonMatrix(gamma, sigma2, ...
    t1, t2, mode)

% LFMAAGRADIENTSIGMAUPSILONMATRIX Gradient of upsilon matrix aa wrt sigma
% FORMAT
% DESC computes the gradient wrt sigma of a portion of the LFMAA kernel.
% ARG gamma : Gamma value for system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode : operation mode, according to the derivative (mode 0,
% derivative wrt t1, mode 1 derivative wrt t2)
% RETURN upsilon : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2010
%
% SEEALSO : lfmaaComputeUpsilonMatrix.m

% KERN

sigma = sqrt(sigma2);
gridt1 = repmat(t1, 1, length(t2));
gridt2 = repmat(t2', length(t1), 1);
timeGrid = gridt1 - gridt2;

dUpsilon = lfmapGradientSigmaUpsilonMatrix(gamma, sigma2, t1, t2, 1-mode);

if mode == 0
    dUpsilonS = (gamma^2)*dUpsilon - (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2).* ...
        ((2/sigma2- (2*timeGrid/sigma2).^2).*((gamma + 2*timeGrid/sigma2).* ...
        (1/sigma - 2*(timeGrid.^2)/sigma^3) + 4*timeGrid/sigma^3) ...
        - (gamma + 2*timeGrid/sigma2).*(-4/sigma^3 + 16*timeGrid.^2/sigma^5)) ...
        - (16/(sqrt(pi)*sigma^6))*timeGrid.*exp(-(timeGrid.^2)./sigma2).* ...
        (5 - 2*timeGrid.^2/sigma2);
else
    dUpsilonS = (gamma^2)*dUpsilon - (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2).* ...
        ((2/sigma2- (2*timeGrid/sigma2).^2).*((gamma + 2*timeGrid/sigma2).* ...
        (1/sigma - 2*(timeGrid.^2)/sigma^3) + 4*timeGrid/sigma^3) ...
        - (gamma + 2*timeGrid/sigma2).*(-4/sigma^3 + 16*timeGrid.^2/sigma^5)) ...
        - (16/(sqrt(pi)*sigma^6))*timeGrid.*exp(-(timeGrid.^2)./sigma2).* ...
        (5 - 2*timeGrid.^2/sigma2) - (2*gamma^2/(sqrt(pi)*sigma))*exp(-gamma*t1)* ...
        (((gamma-2*t2/sigma2).*(1/sigma - 2*t2.^2/sigma^3) - 4*t2/sigma^3).*exp(-t2.^2/sigma2)).';
end


%% housekeeping
clear
clc
close all

%% create data

tmp=importdata('ht2014data.txt');

vnames={'real_pce','cpixfe','ffr','m2','fsi'};

rawdb=ts('1988M12',tmp.data(:,2:end),vnames);

rawdb=pages2struct(rawdb);

%% plot the data

figure('name','raw data');
for ii=1:numel(vnames)
    v=vnames{ii};
    subplot(3,2,ii)
    plot(rawdb.(v))
    title(v)
end

%% transformed data
%
% we use levels of the federal funds rate and the stress index and growth
% rates of real personal consumption expenditures (PCE), money and prices.
db=struct();
db.R=rawdb.ffr;
db.S=rawdb.fsi;
db.C=100*log(rawdb.real_pce/rawdb.real_pce{-1});
db.M=100*log(rawdb.m2/rawdb.m2{-1});
db.P=100*log(rawdb.cpixfe/rawdb.cpixfe{-1});

descr=struct('C','Consumption growth',...
    'P','Inflation',...
    'R','Feds Funds rate',...
    'M','Money growth',...
    'S','Financial stress index');

varlist=fieldnames(descr);

figure('name','Transformed data');
for ii=1:numel(varlist)
    v=varlist{ii};
    subplot(3,2,ii)
    plot(db.(v))
    title(descr.(v))
    axis tight
end
%% Model 1: SVAR models with cholesky identification
clc

models={'1v1c','2v1c','3v1c','1v2c','2v2c','3v2c',...
    '3vS2c','3vSC2c','3vSCP2c','3vSRM2c','3vRM2c','3vRMC2c'};

nlags=2;

exog={};

constant=true;

panel=[];

priors=struct();
priors.var=svar.prior_template();
priors.var.type='sz';

date_range={db.C.start,db.C.finish};

sv0=svar.empty(1,0);

tic

for ii=1:numel(models)

    fprintf(1,' -------------------- %s -------------------- \n',models{ii});

    [markov_chains,switch_prior,restrictions]=build_model(models{ii},varlist);

    sv0(1,ii)=svar(varlist,exog,nlags,constant,panel,markov_chains);

    priors.nonvar=switch_prior;

    sv0(1,ii)=estimate(sv0(1,ii),db,date_range,priors,restrictions);

end

toc

% 2601.732325 seconds

%% estimates

for ii=1:numel(models)
    clc, close all

    fprintf(1,' -------------------- %s -------------------- \n',models{ii});

    pmode=posterior_mode(sv0(ii))

    pause
    % Printing estimates

    print_structural_form(sv0(ii))

    pause
    % Printing solution

    print_solution(sv0(ii))
    pause

    % historical probabilities
    plot_probabilities(sv0(ii))
    pause

    % plots probabilities against data
    close all
    plot_data_against_probabilities(sv0(ii),'regime')
    pause

end

%% A model with endogenous switching

clc

[mctvp,switch_prior,restrictions]=build_model('2v1c',varlist);

mctvp.endogenous_probabilities={
    'syncvol_tp_1_2=1/(1+exp(a12-b12*S))'
    'syncvol_tp_2_1=a21'
    };

mctvp.probability_parameters={'a12','b12','a21'};

mctvp.controlled_parameters={'s(3)'};%'s(1)','s(2)','s(4)',

switch_prior=rmfield(switch_prior,{'syncvol_tp_1_2','syncvol_tp_2_1'});

switch_prior.a12={0.1,0,5,'normal'};

switch_prior.b12={0.1,2,5,'gamma'};

switch_prior.a21={0.2,0.2,0.2,'beta'};

svtvp=svar(varlist,exog,nlags,constant,panel,mctvp);

priors.nonvar=switch_prior;

svtvp=estimate(svtvp,db,date_range,priors,restrictions);


function L = image_laplacian(varargin)
  % IMAGE_LAPLACIAN Compute the image laplacian for a given image in the manner
  % of "Colorization using Optimization" by [Levin et al. 2004]. This is a sort
  % of amalgamation of the literal description in the paper but the knowledge
  % that a Laplacian (rather than a bi-Laplacian) is being used.
  %
  % L = IMAGE_LAPLACIAN(im)
  % L = IMAGE_LAPLACIAN(im,'ParameterName',ParameterValue)
  %
  % Inputs:
  %   im  h by w by (3|1) image (expects double)
  %   Optional:
  %     'Omega' followed by an omega value
  % Outputs:
  %   L w*h by w*h sparse laplacian matrix
  %
  % See also: cotmatrix, levin, lischinski
  %

  im = varargin{1};

  % width and height
  h = size(im,1);
  w = size(im,2);
  %   image size
  %  w     h    c  harmonic biharmonic
  %  185   138  1  0.05     0.1
  %  185   138  3           0.1
  %  93    69   3           0.1
  omega = 0.1;
  % number of vertices/pixels
  n = h*w;

  if ~strcmp(class(im),'double')
    warning('Casting input to double: `im = im2double(im)`');
    im = im2double(im);
  end

  ii = 2;
  while(ii <= nargin)
    switch varargin{ii}
    case 'Omega'
      ii = ii + 1;
      assert(ii<=nargin)
      omega = varargin{ii};
    otherwise
      error('Unsupported parameter');
    end
    ii = ii+1;
  end

  % runs across height fastest
  I = (1:n)';
  I = reshape(I,h,w);
  Ileft = I(1:h,1:(w-1));
  Iright = I(1:h,2:w);
  Ibottom = I(1:(h-1),1:w);
  Itop= I(2:h,1:w);
  % determine neighborhood via FD stencil
  E = [ ...
    Ileft(:) Iright(:);...
    Iright(:) Ileft(:);...
    Ibottom(:) Itop(:);...
    Itop(:) Ibottom(:);...
    ];
  assert(size(E,1) == ((h-1)*w + (w-1)*h)*2);
  r = E(:,1);
  s = E(:,2);
  imlong = reshape(im,size(im,1)*size(im,2),size(im,3));

  wrs = exp(-sum((imlong(r,:)-imlong(s,:)).^2,2)./(2*omega.^2));

  L = sparse(r,s,wrs,n,n);
  % We've made L(r,s) = wrs, now we need L(r,r) = ∑L(r,s) and L(r,s) = -wrs
  L = diag(sum(L,2))-L;
end


% plot of sampling location on warped grid

rep = 'results/';

if ~exist(rep)
    mkdir(rep);
end

save_warped = 0;
save_dich = 1;

s = 50;
c = 0.45;
n = 16;
t = linspace(0,1,n);
[Y,X] = meshgrid(t,t);

x = X(:); y = Y(:);

xw = x;
yw = y + c * x.^2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot of the warped locations
clf;
hold on;
scatter(x,y, s, 'filled');
plot(t, 1-c*t.^2);
hold off;

clf;
scatter(x,y, s, 'filled');
axis equal;
axis off;
if save_warped
saveas(gcf, [rep 'samples.eps'], 'epsc');
end

clf;
scatter(xw,yw, 25, 'filled');
axis equal; axis off;
if save_warped
saveas(gcf, [rep 'samples_warped.eps'], 'epsc');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot of the grouping process
posw = [xw, yw]';
pos = [x, y]';
options.part_type = '1axis';
for i=0:5
    options.depthmax = i;
    [part,B,G] = dichotomic_grouping([yw, xw]',options);
    options.point_size = 80;
    clf;
    plot_dichotomic_partition(posw, part, options);
    axis([0 1.5 0 1.5]); axis equal; axis off;
    if save_dich
        saveas(gcf, [rep '_dich_' num2str(i) '.eps'], 'epsc');
    end
    clf;
    plot_dichotomic_partition(pos, part, options);
    axis([0 1.5 0 1.5]); axis equal; axis off;
    if save_dich
        saveas(gcf, [rep '_dich_' num2str(i) 'w.eps'], 'epsc');
    end
end


function [out, param] = pid_loop(y_c, y, dy, kp, ki, kd, limit, Ts, tau, param)

integrator      = param.int;
differentiator  = param.diff;
error_prev      = param.error_prev;
y_c_prev        = param.y_c_prev;

% Update error
error           = y_c - y;

% Update integrator
if isfinite(dy)
    integrator  = integrator + Ts*(error + dy/2);
else
    integrator  = integrator + (Ts/2)*(error + error_prev);
end

% Update differentiator
if isfinite(1/kd)
    if isfinite(dy)
        dy_c = y_c - y_c_prev;          % Compute command diff
        derror = dy_c - dy;             % Compute error diff
        y_c_prev = y_c;                 % Update the command for next step
    else
        derror = error - error_prev;    % Compute error diff
        error_prev = error;             % Update the error for next step
    end
    differentiator = (2*tau-Ts)/(2*tau+Ts)*differentiator...
                        + 2/(2*tau+Ts)*(derror);
end

% Update output before considering saturation
out_unsat = kp*error + ki*integrator + kd*differentiator;
% Check saturation
out = saturate(out_unsat, limit);

% Implement integrator anti-windup
    if ki~=0
        integrator = integrator + Ts/ki * (out - out_unsat);
    end

param.int           = integrator;
param.diff          = differentiator;
param.error_prev    = error_prev;
param.y_c_prev      = y_c_prev;

end

function out = saturate(out_unsat, limit)

    if out_unsat > limit
        out = limit;
    elseif out_unsat < -limit
        out = -limit;
    else
        out = out_unsat;
    end

end


function tests = test_spx_norms
  tests = functiontests(localfunctions);
end

function test_lp_norms(testCase)
    x = [1 1 1
         2 2 2
        3 3 3];
    verifyEqual(testCase, [6 6 6], spx.norm.norms_l1_cw(x));
    verifyEqual(testCase, [3 6 9]', spx.norm.norms_l1_rw(x));
    verifyEqual(testCase, sqrt([14 14 14]), spx.norm.norms_l2_cw(x));
    verifyEqual(testCase, sqrt([3 12 27]'), spx.norm.norms_l2_rw(x));
    verifyEqual(testCase, [3 3 3], spx.norm.norms_linf_cw(x));
    verifyEqual(testCase, [1 2 3]', spx.norm.norms_linf_rw(x));
    verifyEqual(testCase, 18, spx.norm.cr_l1_l1(x));
    verifyEqual(testCase, 18, spx.norm.rc_l1_l1(x));
    verifyEqual(testCase, sqrt(108), spx.norm.cr_l1_l2(x));
    verifyEqual(testCase, sqrt(126), spx.norm.rc_l1_l2(x), 'AbsTol', 1e-6);
    verifyEqual(testCase, 6, spx.norm.cr_l1_linf(x));
    verifyEqual(testCase, 9, spx.norm.rc_l1_linf(x));
    verifyEqual(testCase, 3*sqrt(14),spx.norm.cr_l2_l1(x));
    verifyEqual(testCase, sqrt(3) + sqrt(12) + sqrt(27), spx.norm.rc_l2_l1(x));
    verifyEqual(testCase, sqrt(42), spx.norm.cr_l2_l2(x), 'AbsTol', 1e-6);
    verifyEqual(testCase, sqrt(42), spx.norm.rc_l2_l2(x), 'AbsTol', 1e-6);
    verifyEqual(testCase, sqrt(14), spx.norm.cr_l2_linf(x));
    verifyEqual(testCase, sqrt(27), spx.norm.rc_l2_linf(x));
    verifyEqual(testCase, 9, spx.norm.cr_linf_l1(x));
    verifyEqual(testCase, 6, spx.norm.rc_linf_l1(x));
    verifyEqual(testCase, sqrt(27), spx.norm.cr_linf_l2(x), 'AbsTol', 1e-6);
    verifyEqual(testCase, sqrt(14), spx.norm.rc_linf_l2(x), 'AbsTol', 1e-6);
    verifyEqual(testCase, 3, spx.norm.cr_linf_linf(x));
    verifyEqual(testCase, 3, spx.norm.rc_linf_linf(x));
end