function [A, b, x, ProbInfo] = PRblurgauss(varargin)
% PRblurgauss Image deblurring problem with a Gaussian point spread function
%
% [A, b, x, ProbInfo] = PRblurgauss
% [A, b, x, ProbInfo] = PRblurgauss(n)
% [A, b, x, ProbInfo] = PRblurgauss(n, options)
% [A, b, x, ProbInfo] = PRblurgauss(options)
%
% Generates data for use in image deblurring problems with a Gaussian
% point spread function.
%
% Input:
%  n      -  size of the image. Can be either a scalar n (in this case
%            the size is n x n) or a vector [nrow, ncol] (in this case the
%            size is (nrow x ncol).
%            Default: n = 256.
%  options - Structure containing the following optional fields:
%    trueImage  : test image of size n, of type numeric, 2-D only,
%                 or character string indicating
%                 'pattern1'  : geometrical image
%                 'pattern2'  : geometrical image
%                 'sppattern' : sparse (edges of ) a geometrical image
%                 'ppower'    : random image with patterns of nonzero pixels
%                 'smooth'    : very smooth image
%                 'dot2'      : two small Gaussian shaped dots, e.g., a
%                               binary star
%                 'dotk'      : n/2 small Gaussian shaped dots, e.g., stars
%                               (placement is random, reset using rng(0))
%                 'satellite' : satellite test image
%                 'hst'       : image of the Hubble space telescope
%                 Default: 'hst'.
%                 This image is then stored in the output vector x.
%    BlurLevel  : If choosing one of the built-in PSFs, this sets the
%                 severity of the blur to one of the following:
%                 'mild'
%                 'medium'
%                 'severe'
%                 Default is 'medium'
%    BC         : Specify boundary condition:
%                 'zero'
%                 'periodic'
%                 'reflective' (or 'neumann' or 'reflexive')
%                 Default: 'reflective'
%                 Note that in this case an extended (or padded) test image
%                 is blurred using 'zero' boundary conditions, and then the
%                 central subimage of size n is extracted from the exact and
%                 the blurred image. No inverse crime is committed,
%                 i.e., A*x ~= b.
%    CommitCrime: To get an exact system Ax = b (i.e., commit the inverse
%                 crime), set this to:
%                 'on'
%                 Default is 'off' (do not commit the inverse crime).
%
% Output:
%  A        - blurring matrix, which is a either a psfMatrix object in
%             the case of spatically invariant blur, or a sparse matrix
%             in the case of spatially variant blur
%  b        - blurred vector (i.e., blurred image with stacked columns)
%  x        - image vector, i.e., exact (unknown) image with stacked columns
%  ProbInfo - structure whose fields contain information about problem:
%               problemType : kind of test problem generated
%                             (in this case: 'deblurring')
%               xType       : solution type (in this case 'image2D')
%               bType       : data type (in this case 'image2D')
%               xSize       : size of image x
%               bSize       : size of image b
%               psf         : point spread function
%
% See also: PRblur, PRblurdefocus, PRblurmotion, PRblurrotation,
% PRblurshake, PRblurspeckle, PRdiffusion, PRinvinterp2, PRnmr,
% PRseismic, PRspherical, PRtomo, PRnoise, PRshowb, PRshowx, fspecial

% Silvia Gazzola, University of Bath
% Per Christian Hansen, Technical University of Denmark
% James G. Nagy, Emory University
% April, 2018.

% This file is part of the IR Tools package and is distributed under the
% 3-Clause BSD License. A separate license file should be provided as part
% of the package.

% Set default values for options.
defaultopt = struct('trueImage', 'hst', 'BlurLevel', 'medium', ...
    'BC', 'reflective', 'CommitCrime', 'off');

% If input is 'defaults,' return the default options in X
if nargin == 1 && nargout <= 1 && strcmp(varargin,'defaults')
    A = defaultopt;
    return;
end

% Check for acceptable number of optional input arguments
switch length(varargin)
    case 0
        n = []; options = [];
    case 1
        if isa(varargin{1}, 'double')
            n = varargin{1}; options = [];
        else
            n = []; options = varargin{1};
        end
    case 2
        if isa(varargin{1}, 'double')
            n = varargin{1}; options = varargin{2};
        else
            n = varargin{2}; options = varargin{1};
        end
    otherwise
        error('Too many input parameters')
end

if isempty(options)
    options = defaultopt;
end

options = PRset(defaultopt, options);
options = PRset(options, 'PSF', 'gauss');
[A, b, x, ProbInfo] = PRblur(n, options);


function x = discretesample(p, n,varargin)
% Samples from a discrete distribution
%
%   x = discretesample(p, n)
%       independently draws n samples (with replacement) from the
%       distribution specified by p, where p is a probability array
%       whose elements sum to 1.
%
%       Suppose the sample space comprises K distinct objects, then
%       p should be an array with K elements. In the output, x(i) = k
%       means that the k-th object is drawn at the i-th trial.
%
%   Remarks
%   -------
%       - This function is mainly for efficient sampling in non-uniform
%         distribution, which can be either parametric or non-parametric.
%
%       - The function is implemented based on histc, which has been
%         highly optimized by mathworks. The basic idea is to divide
%         the range [0, 1] into K bins, with the length of each bin
%         proportional to the probability mass. And then, n values are
%         drawn from a uniform distribution in [0, 1], and the bins that
%         these values fall into are picked as results.
%
%       - This function can also be employed for continuous distribution
%         in 1D/2D dimensional space, where the distribution can be
%         effectively discretized.
%
%       - This function can also be useful for sampling from distributions
%         which can be considered as weighted sum of "modes".
%         In this type of applications, you can first randomly choose
%         a mode, and then sample from that mode. The process of choosing
%         a mode according to the weights can be accomplished with this
%         function.
%
%   Examples
%   --------
%       % sample from a uniform distribution for K objects.
%       p = ones(1, K) / K;
%       x = discretesample(p, n);
%
%       % sample from a non-uniform distribution given by user
%       x = discretesample([0.6 0.3 0.1], n);
%
%       % sample from a parametric discrete distribution with
%       % probability mass function given by f.
%       p = f(1:K);
%       x = discretesample(p, n);
%

%   Created by Dahua Lin, On Oct 27, 2008
%

%% parse and verify input arguments

assert(isfloat(p), 'discretesample:invalidarg', ...
  'p should be an array with floating-point value type.');

assert(isnumeric(n) && isscalar(n) && n >= 0 && n == fix(n), ...
  'discretesample:invalidarg', ...
  'n should be a nonnegative integer scalar.');

%% main

if numel(p) == 1 && p <= 1 && nargin == 2

  x = ones(1,n);
  return

elseif numel(p) == 1

  if nargin == 2 % without replacement
    if 4*n > p
      rp = randperm(p);
      x = rp(1:n);

    else
      f = zeros(1,p); % flags
      sumf = 0;
      while sumf < n
        f(ceil(p * rand(1,n-sumf))) = 1; % sample w/replacement
        sumf = sum(f); % count how many unique elements so far
      end
      x = find(f > 0);
      x = x(randperm(n));
    end
  else % with replacement
    x = ceil(p * rand(1,n));
  end

  return
end

% process p if necessary
K = numel(p);
if ~isequal(size(p), [1, K])
    p = reshape(p, [1, K]);
end

% construct the bins
edges = [0, cumsum(p)];
s = edges(end);
if abs(s - 1) > eps, edges = edges * (1 / s); end

% draw bins
rv = rand(1, n);
c = histcounts(rv, edges);

% extract samples
xv = find(c);

if numel(xv) == n  % each value is sampled at most once
  x = xv;
else                % some values are sampled more than once
  xc = c(xv);
  d = zeros(1, n);
  dv = [xv(1), diff(xv)];
  dp = [1, 1 + cumsum(xc(1:end-1))];
  d(dp) = dv;
  x = cumsum(d);
end

% randomly permute the sample's order
x = x(randperm(n));


function [out] = saturation_3(S,Smax,p1,In)
%saturation_3

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Saturation excess from a store with different degrees of
%               saturation (exponential variant)
% Constraints:  -
% @(Inputs):    S    - current storage [mm]
%               Smax - maximum contributing storage [mm]
%               p1   - linear scaling parameter [-]
%               In   - incoming flux [mm/d]

out = (1-(1/(1+exp((S/Smax + 0.5)/p1)))).*In;

end


function X = tdma(A,B,C,D)
%TriDiagonal Matrix Algorithm (TDMA) or Thomas Algorithm
% A_i*X_(i-1) + B_i*X_i + C_i*X_(i+1) = D_i (where A_1 = 0, C_n = 0)
% A,B,C,D are input vectors. X is the solution, also a vector.

% Copyright 2013 The MathWorks, Inc.

Cp = C;
Dp = D;
n = length(A);
X = zeros(n,1);
% Performs Gaussian elimination
Cp(1) = C(1)/B(1);
Dp(1) = D(1)/B(1);
for i = 2:n
Cp(i) = C(i)/(B(i)-Cp(i-1)*A(i));
Dp(i) = (D(i)-Dp(i-1)*A(i))/(B(i)-Cp(i-1)*A(i));
end
% Backward substitution, since X(n) is known first.
X(n) = Dp(n);
for i = n-1:-1:1
X(i) = Dp(i)-Cp(i)*X(i+1);
end


clear, clc
        % Definirea variabilelor simbolice
syms ud uq Rs Ls Rr Lr id iD iq iQ M w
syms diddt diqdt diDdt diQdt
        % Variabilele utilizate care nu corespund exact cu simbolurile utilizate
        % - w - omega
        % - diddt - d(id)/dt  si  diqdt - d(iq)/dt
        % - diDdt - d(iD)/dt  si  diQdt - d(iQ)/dt
        % Incarcarea celor patru ecuatii
eq1=-ud+Rs*id+Ls*diddt+M*diddt+M*diDdt;
eq2=-uq+Rs*iq+Ls*diqdt+M*diqdt+M*diQdt;
eq3=Rr*iD+Lr*diDdt+M*diddt+M*diDdt+w*Lr*iQ+w*M*(iq+iQ);
eq4=Rr*iQ+Lr*diQdt+M*diqdt+M*diQdt-w*Lr*iD-w*M*(id+iD);
        % Rezolvarea sistemului de ecuatii
        % avand ca variabile independente cele 4 derivate ale curentilor
        % Rezultatele obtinute sunt intocmai derivatele curentilor
SOL=solve(eq1,eq2,eq3,eq4,'diddt,diqdt,diDdt,diQdt');
        % Extragerea solutiei din structura returnata
diddt=SOL.diddt;
diqdt=SOL.diqdt;
diDdt=SOL.diDdt;
diQdt=SOL.diQdt;
        % Afisarea solutiei
disp('did/dt = '),pretty(diddt)
disp('diq/dt = '),pretty(diqdt)
disp('diD/dt = '),pretty(diDdt)
disp('diQ/dt = '),pretty(diQdt)
        % Verificarea solutiei prin inlocuirea rezultatelor in sistemul de ecuatii
Verif1=simple(-ud+Rs*id+Ls*diddt+M*diddt+M*diDdt);
Verif2=simple(-uq+Rs*iq+Ls*diqdt+M*diqdt+M*diQdt);
Verif3=simple(Rr*iD+Lr*diDdt+M*diddt+M*diDdt+w*Lr*iQ+w*M*(iq+iQ));
Verif4=simple(Rr*iQ+Lr*diQdt+M*diqdt+M*diQdt-w*Lr*iD-w*M*(id+iD));
        % Formarea unei vectori cu erorile obtinute
        % (daca toate elementele sale sunt nule, solutia este buna)
Eroare=[Verif1,Verif2,Verif3,Verif4]


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [Sc,dS,d2S] = diffusionST(vc,omega,m,varargin)
%
% Matrix-free spatio-temporal diffusion regularization energy for vc, where
% vc is cell-centered
%
% S(v) = 0.5 * \int_{\omega} alpha(1)*v(x)'*A*v(x)+ alpha(2)*v(x)'*B*v(x) dx,
%
% where A is the spatial gradient operator and B the time derivative operator.
%
% Input:
%
%   vc          instationary velocity field (cell-centered)
%   omega       spatial domain
%   m           number of discretization points
%   varargin    optional parameters (see below)
%
% Optional Input:
%
%   tspan       time span (default: [0 1])
%   nt          number of time points for velocity (default: computed from input)
%
%
% Output:
%
%   Sc          current value   (0.5 * hd * vc'* A *vc + 0.5*dt*vc'*B*vc)
%   dS          derivative      (hd * vc'*A )
%   d2S         Hessian, struct A
% ==================================================================================

function [Sc,dS,d2S] = mfDiffusionST(vc,omega,m,varargin)
if nargin == 0
    help(mfilename);
    return;
end

if strcmp(vc,'para')
    Sc = 'cell-centered';       % grid
    dS = 1;                     % matrixFree
    d2S = @spectralPrecondPCG;  % solver
    return;
end

alpha       = [1 1e-3];
tspan       = [0 1];
nt          = [];
for k=1:2:length(varargin) % overwrites default parameter
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

dim = numel(omega)/2;

if isempty(nt) % roughly estimate nt
    nt = round(numel(vc)/(prod(m)*dim))-1;
end

d2S.regularizer = regularizer;
d2S.alpha  = alpha;
d2S.B      = @(omega,m)     getDiffusionMatrixST(omega,tspan,m,nt,alpha);
d2S.d2S    = @(u,omega,m)   diffusionOperatorST(u,omega,tspan,m,nt,alpha);
d2S.diag   = @(omega,m)     diag(omega,tspan,m,nt,alpha);
d2S.solver = @spectralPrecondPCG;
d2S.res    = vc;
dS         = d2S.d2S(vc,omega,m)';
Sc         = .5*dS*vc;

function B = getDiffusionMatrixST(omega,tspan,m,nt,alpha)

h   = (omega(2:2:end)-omega(1:2:end))./m;
hd  = prod(h);
% compute time-stepsize
dt = abs(tspan(2)-tspan(1))/nt;

% build gradient matrix for one transformation
Bx =  getSpaceGradientMatrix(omega,m);
% a = sqrt(alpha(1).*hd.*dt)*ones(nt+1,1);
a = sqrt(alpha(1).*hd.*dt*[1/2;ones(nt-1,1);1/2]);
% apply spatial regularization to all transformations and sum up in
% time
Bx =  kron(sdiag(a(:)),Bx);

% get time regularization matrix
b = sqrt(alpha(2)*hd.*dt);
Bt = b * getTimeDiffusionMatrix(nt,dt,prod(m),length(omega)/2);

% build B
B = [Bx;Bt];

function D = sdiag(v)
	D = diag(sparse(v(:)));

% get diagonal of d2S (interesting in matrix free mode)
function D = diag(omega,tspan,m,nt,alpha)
dim   = numel(omega)/2;
one   = @(i,j) One(omega,m,i,j);
hd    = prod((omega(2:2:end)-omega(1:2:end))./m);
% compute time stepsize
dt = abs(tspan(1)-tspan(2))/nt;

if dim == 2
    Dx = [ one(1,1) + one(1,2);
           one(2,1) + one(2,2) ];
    Dx = kron(hd*dt*ones(nt+1,1),Dx);

    Dt = [1/2;ones(nt-1,1);1/2]/dt^2;

    Dt = kron(hd*dt*Dt , ones(prod(m)*dim,1));

    D  = alpha(1)*Dx + alpha(2)*Dt;

else
    Dx   = [ ...
        one(1,1)+one(1,2)+one(1,3);
        one(2,2)+one(2,1)+one(2,3);
        one(3,3)+one(3,1)+one(3,2)];
    Dx   = kron(hd*dts(:),Dx);

    Dt          = zeros(mTime+1,1);
    Dt(1:end-1) = (1./dt(:));
    Dt(2:end  ) = Dt(2:end) + (1./dt(:));

    Dt = kron(hd*Dt, ones(prod(mSpace)*dim,1));

    D  = alpha(1)*Dx + alpha(2)*Dt;

end;

% helper for computation of diag(d2S)
function o = One(omega,m,i,j)
h = (omega(2:2:end)-omega(1:2:end))./m;
o = ones(m)/h(j)^2;
switch j
    case 1, o(2:end-1,:,:) = 2*o(2:end-1,:,:);
    case 2, o(:,2:end-1,:) = 2*o(:,2:end-1,:);
    case 3, o(:,:,2:end-1) = 2*o(:,:,2:end-1);
end;
o = o(:);

% matrix free implementation of spatio-temporal diffusion operator
function Ay = diffusionOperatorST(vc,omega,tspan,m,nt,alpha)
dim = numel(omega)/2;
h   = (omega(2:2:end)-omega(1:2:end))./m;
hd  = prod(h(1:dim));
dt  = abs(tspan(1)-tspan(2))/nt;
w   = dt*[1/2;ones(nt-1,1);1/2];

switch dim
    case 2
        d1 = @(Y) (Y(2:end,:)-Y(1:end-1,:))/h(1);
        d2 = @(Y) (Y(:,2:end)-Y(:,1:end-1))/h(2);

        d1T = @(Y) reshape([-Y(1,:);Y(1:end-1,:)-Y(2:end,:);Y(end,:)],[],1)/h(1);
        d2T = @(Y) reshape([-Y(:,1),Y(:,1:end-1)-Y(:,2:end),Y(:,end)],[],1)/h(2);

        vc   = reshape(vc,[],nt+1);
        Ay = zeros(prod(m)*dim,nt+1);
        % spatial diffusion
        for k=1:nt+1
            vct = reshape(vc(:,k),[m dim]);
            Ay(:,k) = w(k) * hd*  alpha(1) * ....
                [ (d1T(d1(vct(:,:,1))) + d2T(d2(vct(:,:,1)))); ...
                (d1T(d1(vct(:,:,2))) + d2T(d2(vct(:,:,2))))];
        end
        Ay = Ay(:);

    case 3
        d1 = @(Y) (Y(2:end,:,:)-Y(1:end-1,:,:))/h(1);
        d2 = @(Y) (Y(:,2:end,:)-Y(:,1:end-1,:))/h(2);
        d3 = @(Y) (Y(:,:,2:end)-Y(:,:,1:end-1))/h(3);

        d1T = @(Y) reshape(d1t(Y),[],1)/h(1);
        d2T = @(Y) reshape(d2t(Y),[],1)/h(2);
        d3T = @(Y) reshape(d3t(Y),[],1)/h(3);

        vc   = reshape(vc,[],nt+1);
        Ay = zeros(prod(m)*dim,nt+1);
        % spatial diffusion
        for k=1:nt+1
            vct = reshape(vc(:,k),[m dim]);
            Ay(:,k) = w(k) * hd*  alpha(1) * ....
                [ ...
                (d1T(d1(vct(:,:,:,1))) + d2T(d2(vct(:,:,:,1))) + d3T(d3(vct(:,:,:,1)))); ...
                (d1T(d1(vct(:,:,:,2))) + d2T(d2(vct(:,:,:,2))) + d3T(d3(vct(:,:,:,2)))); ...
                (d1T(d1(vct(:,:,:,3))) + d2T(d2(vct(:,:,:,3))) + d3T(d3(vct(:,:,:,3)))); ...
                ];
        end
        Ay = Ay(:);
    otherwise
        error('%s - dimension %d not supported.',mfilename,dim);
end
% time diffusion
if alpha(2)>0
    Dt  = @(Y) (Y(:,2:end)-Y(:,1:end-1))/dt;
    DtT = @(Y) ([-Y(:,1),Y(:,1:end-1)-Y(:,2:end),Y(:,end)])/dt;
    At  = DtT(Dt(vc));
    Ay  = Ay + (alpha(2)* hd*dt)*At(:);
end

% partial derivative operator for x1 (mf)
function y = d1t(Y)
m = size(Y);
y = zeros(m+[1,0,0]);
y(1,:,:) = -Y(1,:,:);
y(2:end-1,:,:) = Y(1:end-1,:,:)-Y(2:end,:,:);
y(end,:,:) = Y(end,:,:);
% partial derivative operator for x2 (mf)
function y = d2t(Y)
m = size(Y);
y = zeros(m+[0,1,0]);
y(:,1,:) = -Y(:,1,:);
y(:,2:end-1,:) = Y(:,1:end-1,:)-Y(:,2:end,:);
y(:,end,:) = Y(:,end,:);
% partial derivative operator for x3 (mf)
function y = d3t(Y)
m = size(Y); if length(m) == 2, m = [m,1]; end;
y = zeros(m+[0,0,1]);
y(:,:,1) = -Y(:,:,1);
y(:,:,2:end-1) = Y(:,:,1:end-1)-Y(:,:,2:end);
y(:,:,end) = Y(:,:,end);

function A = getSpaceGradientMatrix(omega,m)
dim = length(omega)/2;
h   = (omega(2:2:end)- omega(1:2:end)) ./m ;

I = @(i)  speye(m(i));

% setup regularizer for y
%
% S(y) =  INT |Dy(.,t)|^2 + INT |d/dt y(x,.)|^2 dx
switch dim
    case 2
        % build discrete derivative operators
        D1=spdiags(ones(m(1),1)*[-1 1],0:1,m(1)-1,m(1)); D1=D1/(h(1));
        D2=spdiags(ones(m(2),1)*[-1 1],0:1,m(2)-1,m(2)); D2=D2/(h(2));

        % spatial regularization
        A = [kron(I(2),D1); kron(D2,I(1))];
    case 3
        % build discrete derivative operators
        D1=spdiags(ones(m(1),1)*[-1 1],0:1,m(1)-1,m(1)); D1=D1/(h(1));
        D2=spdiags(ones(m(2),1)*[-1 1],0:1,m(2)-1,m(2)); D2=D2/(h(2));
        D3=spdiags(ones(m(3),1)*[-1 1],0:1,m(3)-1,m(3)); D3=D3/(h(3));
        % build gradient (scalar)
        A = [kron(I(3),kron(I(2),D1)); ...
             kron(I(3),kron(D2,I(1))); ...
             kron(D3,kron(I(2),I(1)))];

end
A = kron(speye(dim), A);                  % nD gradient

function A = getTimeDiffusionMatrix(nt,dt,n,dim)
Dt=spdiags(ones(nt+1,1)*[-1/dt 1/dt],0:1,nt,nt+1);
A = kron(Dt, speye(n*dim)); % 2nd deriveative over time for two components


function xdot = Equation(t,x)
% Set the input values in order to pass onto ode45
%
n = length(x) ;
thita = x(1) ;          % Angular Position of the bead
dthita = x(2) ;         % Angular Velocity of the bead
g = x(3) ;              % Acceleration due to gravity
M = x(4) ;              % Mass of the bead
R = x(5) ;              % Radius of the hoop
V = x(6) ;              % Frictional coefficient of the bead on the hoop
w0 = x(7) ;             % Frequency of rotation of the bead
%
xdot=zeros(n,1);
xdot(1) = dthita;
xdot(2) = -sin(thita)*(g/R-w0^2*cos(thita))-V/M*dthita ;


function[H,P,M1,M2,N1,N2] = countsig(data1,data2,T1,T2,parametric,p,quiet)
% Give the program two spike data sets and one
% or two time intervals and it will decide if
% the counts are significantly different.
% this is either with a non-parametric method
% or with a sqrt transformation followed by a
% t-test
% Usage: [H,P,M1,M2,N1,N2] = countsig(data1,data2,T1,T2,parametric,p,quiet)
%
% Input:
% Note that all times have to be consistent. If data
% is in seconds, so must be sig and t. If data is in
% samples, so must sig and t. The default is seconds.
%
% data1      - structure array of spike times (required)
% data2      - structure array of spike times (required)
% T1         - time interval (default all)
% T2         - time interval (default T1)
% parametric - 0 = non-parametric (Wilcoxon)
%            - 1 = ttest on sqrt of counts
%            - 2 = Poisson assumption
%              (default = 0)
% p          - significance level (0.05)
% quiet      - 1 = no display 0 = display
%
% Output:
%
% H          - 1 if different 0 if not
% P          - prob of result if same
% M1         - mean count for data1
% M2         - mean count for data2
% N1         - counts for data1
% N2         - counts for data2

if nargin < 2;error('I need 2 sets of spike data');end
data1=padNaN(data1); % create a zero padded data matrix from input structural array
data2=padNaN(data2); % create a zero padded data matrix from input structural array
data1=data1'; data2=data2'; % transpose data to get it into a form acceptable to Murray's routine
if nargin < 3,
   T1 = [min(data1(:,1)) max(max(data1))];
end
if nargin < 4,
   T2 = T1;
end
if nargin < 5,
   parametric = 0;
end
if nargin < 6; p = 0.05;end
if nargin < 7; quiet = 0; end

if isempty(T1),
   T1 = [min(data1(:,1)) max(max(data1))];
end
if isempty(T2)
   T2 = T1;
end
if isempty(parametric),
   parametric = 0;
end
if isempty(p)
   p = 0.05;
end
if isempty(quiet),
    quiet = 0;
end

NT1 = length(data1(:,1));
NT2 = length(data2(:,2));

if (NT1 < 4 || NT2 < 4) && parametric ~= 2,
  disp('Low number of trials : switch to Poisson test')
  parametric = 2;
end

if abs((T1(2)-T1(1)) - (T1(2)-T1(1))) > 10.^-6
  error('Time intervals for analysis are different')
end

% get counts...
N1=zeros(1,NT1);
for n=1:NT1
  N1(n) = length(find(data1(n,:) >=  T1(1) & ...
          data1(n,:) <=  T1(2) & ~isnan(data1(n,:))));
end
N2=zeros(1,NT2);
for n=1:NT2
  N2(n) = length(find(data2(n,:) >=  T2(1) & ...
          data2(n,:) <=  T2(2) & ~isnan(data1(n,:))));
end
M1 = mean(N1);
M2 = mean(N2);

% do non parametric test...

if parametric == 0
  [P H] = ranksum(N1,N2,p);
end

% parametric test (with stabilizing transform)...

%  use sqrt transformation from
%  Cox and Lewis to make data more Gaussian
%  the statistical analysis of series of events pg 44

if parametric == 1
  X = sqrt(N1 +0.25);
  Y = sqrt(N2 +0.25);
  [H,P] = ttest2(X,Y,p,0);
end

%  Poisson test.  Use method from Zar
%  pg 580 Ed 3.  Z bahaves as a normal variate under
%  null of same process mean and Poisson processes

if parametric == 2
  X = sum(N1);
  Y = sum(N2);
  Z = abs(X-Y)./sqrt(X+Y);
  P = 2*(1-normcdf(Z));
  if P < p; H = 1;else H = 0;end
end

if quiet == 0
  if H == 1
    disp('Counts are signifcantly different')
  else
    disp('Counts are not signifcantly different')
  end
  disp(['Mean count for data1 = ' num2str(M1)])
  disp(['Mean count for data2 = ' num2str(M2)])
  disp(['P value = ' num2str(P)])
end


function gmm = mixtureOfGaussian(data, K, niter, fullCov, varargin)
% Mixture of K gaussians
% data(ndata, nvariables) - the data
% K - the number of gaussians in the mixture
% niter - the number of iterations
% fullCov - 1 if a full covariance matrix should be used, otherwise
%                   a diagonal matrix will be used
% varargin{1} - weights for the data (optional)
% gmm.mu{K} - the means of the mixtures
% gmm.sigma{K} - the variance matrices
% gmm.priors(K) - the mixture priors

[ndata, ndim] = size(data);

if ndata == 0
    disp('warning no data')
    gmm.mu = {};
    gmm.sigma = {};
    gmm.priors = [];
    return;
end

if length(varargin)==0
    w = ones(ndata, 1);
else
    w = varargin{1}(:);
end

w = w / sum(w) * ndata;

cumw = cumsum(w / sum(w));

% initialize mu by randomly selected data values
r = rand(K, 1);
for k = 1:K
    for i = 1:ndata
        if cumw(i)>=r(k)
            mu{k} = data(i, :);
            break;
        end
    end
end

% initialize all covariances to be covariance of data
%if fullCov
%    sigma(1:K) = {cov(data)};
%else
%    sigma(1:K) = {diag(var(data))};
%end
sigma(1:K) = {diag(ones(1, ndim))};

priors = ones(1, K) / K;

for iter = 1:niter

    % get posteriors: P(k | data) = P(data|k)P(k) / sum_k(P(data|k))
    pC = zeros(ndata, K);
    for k = 1:K
        invSigma = inv(sigma{k});
        for i = 1:ndata
            pC(i, k) = 1 / (sqrt(2*pi)*sqrt(det(sigma{k}))) * ...
                exp(-1/2 * (data(i, :)-mu{k}) * invSigma * (data(i, :)-mu{k})') ...
                * priors(k);
        end
    end

    if 0
    for k = 1:K
        disp(['k: ' num2str(k)])
        disp(['mu: ' num2str(mu{k})])
        disp(['sigma: ' num2str(det(sigma{k}))])
        disp(['prior: ' num2str(priors(k))])
        disp(['min: ' num2str(min(pC(:, k)))])
        disp(['max: ' num2str(max(pC(:, k)))])
    end
    end

    %disp(num2str(priors))

    pC = pC + eps; % avoid divide by zero error

    sumPc = sum(pC, 2);
    for k = 1:K
        pC(:, k) = pC(:, k) ./ sumPc .* w;
    end
    kweight = sum(pC, 1);

    priors = kweight / sum(kweight);

    % mean estimation
    for k = 1:K
        sumkX = pC(:, k)' * data;
        mu{k} = sumkX / kweight(k);
    end

    % covariance matrix estimation
    if fullCov
        for k = 1:K
            diffs = data - (ones(ndata, 1) * mu{k});
            diffs = diffs .* (sqrt(pC(:,k))*ones(1, ndim));
            sigma{k} = (diffs' * diffs) / kweight(k);
        end
    else
        for k = 1:K
            diffs = data - (ones(ndata, 1) * mu{k});
            sigma{k} = diag(sum((diffs.*diffs).*(pC(:, k)*ones(1, ndim)), 1) ./ kweight(k));
        end
    end
end

gmm.mu = mu;
gmm.sigma = sigma;
gmm.priors = priors;


function [ y, m, d, f ] = jed_to_ymdf_ethiopian ( jed )

%*****************************************************************************80
%
%% JED_TO_YMDF_ETHIOPIAN converts a JED to an Ethiopian YMDF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Algorithm F,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999, pages 324-325.
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer Y, M, D, real F, the YMDF date.
%

%
%  Determine the computational date (Y'/M'/D').
%
  j = floor ( jed + 0.5 );
  f = ( jed + 0.5 ) - j;

  j_prime = j + 124;

  y_prime = floor ( ( 4 * j_prime + 3 ) / 1461 );
  t_prime = floor ( mod ( 4 * j_prime + 3, 1461 ) / 4 );
  m_prime = floor ( t_prime / 30 );
  d_prime = mod ( t_prime, 30 );
%
%  Convert the computational date to a calendar date.
%
  d = d_prime + 1;
  m = mod ( m_prime, 13 ) + 1;
  y = y_prime - 4720 + floor ( ( 13 - m ) / 13 );

  return
end


function [y,deriv] = brier_obj(w,T,weights,logit_prior)
% This is an MV2DF. See MV2DF_API_DEFINITION.readme.
%
% Weighted binary classifier cross-entropy objective, based on 'Brier'
% quadratic proper scoring rule. This rule places less emphasis on extreme scores,
% than the logariothmic scoring rule.
%
%  Differentiable inputs:
%   w: is vector of N detection scores (in log-likelihood-ratio format)
%
%  Fixed parameters:
%   T: is vector of N labels: 1 for target and -1 for non-target.
%   weights: is N-vector of objective function weights, one per trial.
%   logit_prior: is logit(prior), this controls the region of interest

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

if isempty(w)
    y = @(w)brier_obj(w,T,weights,logit_prior);
    return;
end

if isa(w,'function_handle')
    outer = brier_obj([],T,weights,logit_prior);
    y = compose_mv(outer,w,[]);
    return;
end

w = w(:);
scores = w.';

arg = bsxfun(@plus,scores,logit_prior).*T;
logp2 = -neglogsigmoid(-arg);
wobj = 0.5*exp(2*logp2).*weights; % 1*N
y = sum(wobj);

if nargout>1
    logp1 = -neglogsigmoid(arg);
    deriv = @(dy) deriv_this(dy,weights(:),T(:),logp1(:),logp2(:));
end

function [g,hess,linear] = deriv_this(dy,weights,T,logp1,logp2)
g0 = -exp(logp1+2*logp2).*weights.*T;
g = dy*g0;
linear = false;
hess = @(d) hessianprod(d,dy,g0,weights,logp1,logp2);

function [h,Jv] = hessianprod(d,dy,g0,weights,logp1,logp2)

ddx = -exp(logp1+2*logp2);
h = dy*(ddx.*(1-3*exp(logp1))).*weights.*d(:);

if nargout>1
    Jv = d.'*g0;
end

function test_this()
N = 30;
T = [ones(1,N/3),-ones(1,N/3),zeros(1,N/3)];
scores = randn(1,N);
weights = [rand(1,2*N/3),zeros(1,N/3)];
f = @(w) brier_obj(w,T,weights,-2.23);
test_MV2DF(f,scores(:));


function [ G ] = gsp_nn_hypergraph( Xin, param )
%GSP_NN_HYPERGRAPH Create a nearest neighbors hypergraph from a point cloud
%   Usage :  G = gsp_nn_hypergraph( Xin );
%            G = gsp_nn_hypergraph( Xin, param );
%
%   Input parameters:
%       Xin         : Input points
%       param       : Structure of optional parameters
%
%   Output parameters:
%       G           : Resulting graph
%
%   Example:::
%
%           P = rand(100,2);
%           G = gsp_nn_hypergraph(P)
%
%   Additional parameters
%   ---------------------
%
%   * *param.use_flann* : [0, 1]              use the FLANN library
%   * *param.center*    : [0, 1]              center the data
%   * *param.rescale*   : [0, 1]              rescale the data (in a 1-ball)
%   * *param.sigma*     : float               the variance of the distance kernel
%   * *param.k*         : int                 number of neighbors for knn
%
%   See also: gsp_nn_graph
%

% Author: Nathanael Perraudin
% Date: 21 October 2015
% Testing: test_rmse

%   * *param.type*      : ['knn', 'radius']   the type of graph (default 'knn')
%   * *param.epsilon*   : float               the radius for the range search
%   * *param.use_l1*    : [0, 1]              use the l1 distance

    if nargin < 2
    % Define parameters
        param = {};
    end

    %Parameters
%     if ~isfield(param, 'type'), param.type = 'knn'; end
    if ~isfield(param, 'use_flann'), param.use_flann = 0; end
    if ~isfield(param, 'center'), param.center = 0; end
    if ~isfield(param, 'rescale'), param.rescale = 1; end
    if ~isfield(param, 'k'), param.k = 10; end

    param.type = 'knn';
%     if ~isfield(param, 'epsilon'), param.epsilon = 0.01; end
%     if ~isfield(param, 'use_l1'), param.use_l1 = 0; end
%     if ~isfield(param, 'target_degree'), param.target_degree = 0; end;
    paramnn = param;
%     paramnn.k = param.k +1;
    [indx, ~, dist] = gsp_nn_distanz(Xin',Xin',paramnn);

%     switch param.type
%         case 'knn'
%             if param.use_l1
%                 if ~isfield(param, 'sigma'), param.sigma = mean(dist); end
%             else
                if ~isfield(param, 'sigma'), param.sigma = mean(dist)^2; end
%             end
%         case 'radius'
%             if param.use_l1
%                 if ~isfield(param, 'sigma'), param.sigma = epsilon/2; end
%             else
%                 if ~isfield(param, 'sigma'), param.sigma = epsilon.^2/2; end
%             end
%         otherwise
%             error('Unknown graph type')
%     end

    w = exp(-dist.^2/param.sigma);
    G.N = size(Xin,1);
    G.Ne = G.N;
    G.W = sparse(G.N,G.Ne);
    G.E = cell(G.Ne,1);
    k = param.k;
    for ii = 1:G.Ne
        edge = indx((1:k)+(ii-1)*k);
        G.E{ii} = edge;
        % Here we use H for HW...
        G.W(edge,ii) = sqrt(sum(w(edge)));
    end
    G.hypergraph = 1;
    G.directed = 0;

    %Fill in the graph structure
    G.coords = Xin;

    G.type = 'Nearest neighboors hypergraph';
    G.lap_type = 'normalized';
    G.sigma = param.sigma;
    G = gsp_graph_default_parameters(G);
end


function vo = ch_qmulv(q, vi)
% 向量通过四元数做3D旋转
%
% Inputs: q - Qb2n
%            vi - 需要旋转的向量
% Output: vout - output vector, such that vout = q*vin*conjugation(q)
%
% See also  q2mat, qconj, qmul.

%     qi = [0; vi];
%     qo = ch_qmul(ch_qmul(q,qi),ch_qconj(q));
%     vo = qo(2:4,1);

    qo1 =              - q(2) * vi(1) - q(3) * vi(2) - q(4) * vi(3);
    qo2 = q(1) * vi(1)                + q(3) * vi(3) - q(4) * vi(2);
    qo3 = q(1) * vi(2)                + q(4) * vi(1) - q(2) * vi(3);
    qo4 = q(1) * vi(3)                + q(2) * vi(2) - q(3) * vi(1);
    vo = vi;
    vo(1) = -qo1 * q(2) + qo2 * q(1) - qo3 * q(4) + qo4 * q(3);
    vo(2) = -qo1 * q(3) + qo3 * q(1) - qo4 * q(2) + qo2 * q(4);
    vo(3) = -qo1 * q(4) + qo4 * q(1) - qo2 * q(3) + qo3 * q(2);


function [pitch, yaw] = ueci2angles(reci, veci, ueci)

% convect eci unit vector to rtn angles

% input

%  reci = eci position vector (kilometers)
%  veci = eci velocity vector (kilometers/second)
%  ueci = eci unit vector

% output

%  pitch = pitch angle (radians)
%          positive above the local horizon
%  yaw   = yaw angle (radians)
%          positive in the direction of the angular momentum vector

% Orbital Mechanics with MATLAB

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

% compute radial frame unit vectors

rmag = norm(reci);

xrdl = reci / rmag;

zrdl = cross(reci, veci);

hmag = norm(zrdl);

zrdl = zrdl / hmag;

yrdl = cross(zrdl, xrdl);

% unit vector in radial-tangential-normal frame

umee(1) = dot(ueci, xrdl);

umee(2) = dot(ueci, yrdl);

umee(3) = dot(ueci, zrdl);

% pitch angle (radians)

pitch = asin(umee(1));

% yaw angle (radians)

yaw = atan2(umee(3), umee(2));


function sF = radon(sF, delta)
% Radon transform of a spherical function
%
% Syntax
%   sF = radon(sF)
%
% Input
%  sF  - @S2FunHarmonic
%
% Output
%  sF - @S2FunHarmonic
%

% Description
% The spherical Radon transform is simply a convolution operator with
% a kernel function with Legendre coefficients given by
%
%         A(n) = (-1)^(n/2) * (n-1)!! / n!!     if n is even
%         A(n) = 0                              if n is odd
%
% where
% (n-1)!! = 1 * 3 * 5 * ... (n-1)
% n!!     = 2 * 4 * 6 * ... n
%

if nargin == 2

  A = sum(legendre0(sF.bandwidth,cos(pi/2+delta)),2);

else
  A = zeros(sF.bandwidth+1,1);
  A(1) = 1;
  for n = 2:2:sF.bandwidth
    A(n+1) = -A(n-1) * (n-1)/n;
  end

end

sF = conv(sF,A);


N = 3;
x = rand(N,2); % each row is a feature vector
m = mean(x,1);
xc = x-repmat(m, N, 1);

C = eye(N) - (1/N)*ones(N,N);
xc2 = C*x;
assert(approxeq(xc, xc2))


% ESTIMATION OF ZERO COUPON YIELDS FROM COUPON BOND PRICES USING THE
% NELSON-SIEGEL AND SVENSSON MODEL
%
% Model = 'NS' (Nelson-Siegel) | 'Svensson'
%
% Optimization Structure
%
% .Method = 'price' | 'ytm'
%  'price' fits (weighted)) cupon bond prices
%  'ytm' fits yields-to-maturity
%
% .Weights  = 'MD' | 'LA' | empty (Weights are irrelevant for the YTM method.)
%  'MD' weights use modified duration of bonds
%  'LA' weights work as the correct linear approaximation of ytm;
%  LA weights equal MD weights divideded by observed cupon bond prices.
%  Note that weights are irrelevant for the YTM method.
%  Further note that short-rates (LIBOR rates) are underweighted, see the
%  NSobjP.m for details and "The Czech Treasury Yield Curve from 1999 to the
%  Present" for a discussion.
%
% .Algorithm = 'lsqnonlin' | 'fminsearch'
%  Supports two standard Matlab optimization routines. The nonlinear least
%  squares, lsqnonlin, which is tha part of Optimization Tbx is recommended.
%
% .Multistart = 'yes' | 'no'
%  The estimation algorothm creates a grid of starting values for optimization, which
%  gives better hope for finding global optima. Recommended.
%
%
% Kamil Kladivko
% email: kladivko@gmail.com
%
% Cite as:
% Kladivko Kamil (2010). The Czech Treasury Yield Curve from 1999 to the Present,
% Czech Journal of Economics and Finance, 60(4): 307-335
% =========================================================================

clc
close all
clear variables

% 1) Set model
Model = 'NS';
Optimization.Method = 'price';
Optimization.Weights = 'LA';
Optimization.Algorithm = 'lsqnonlin';
Optimization.MultiStart = 'yes';

% 2) Read data from EXCEL
% The EXCEL file contains Czech Governemnt bond prices and PRIBOR rates (short rates) from March 2, 2007
% There were 13 Czech Government bonds available on March 2, 2007. Furter, 4 PRIBOR rates are used
% to fix the short-end of the yield curve. See Section 5 of "The Czech Treasury Yield Curve from 1999 to the Present"

[B Btext] = xlsread('bonddata.xls', 'data', 'B6:F18');
[junk Settle] = xlsread('bonddata.xls', 'data', 'C2:C2');
[SR SRtext] = xlsread('bonddata.xls', 'data', 'J6:L9');

% The DATE format in EXCEL depends on the "national settings". Needs to be
% set accordingly!
Bonds.Issue = datenum(Btext(:,1), 'dd.mm.yyyy');
Bonds.Maturity = datenum(Btext(:,2), 'dd.mm.yyyy');
Bonds.Settle = datenum(Settle, 'dd.mm.yyyy');
Bonds.Prices = B(:,2);
% Make sure that bond ID (isseu number) was read as text! If not adjust
% either this code or EXCEL.
Bonds.Emission = Btext(:,5);
Bonds.Coupon = B(:,1)./100;
% Assumes that coupon is paid once a year. The first cupon can be payed
% later than after one year. See czbondcf.m and czbondfuturecf.m for
% details
Bonds.Notional = 100;
Bonds.Basis = 1;
% Bonds.Basis = '1' | '2'
% Bond.Basis is Day-Count basis for calculation coupon cash flows
% 1: 30E/360 (default), 2: Actual/360
% Go to czbondcf.m and czbondfuturecf.m to adjust for your basis

ShortRates.IR = SR(:,1)'./100; % This must be as row vector opposed to the bond data
if ~isempty(ShortRates), ShorRates.IR = log(1+ShortRates.IR); end % Transform SR to continuous compounding
ShortRates.TimeFactor = SR(:,2)';
ShortRates.Emission = SRtext';

% If Short Rates are not to be used then supply "ShotRates = []" into the
% estimation routine below.
%ShortRates = [];

% 3) Nelson-Siegel Parameters Estimation (calls the main estimation routine)
[Params Fval MaxTime2Mat ParamsMS FvalMS] = NSest(Bonds, ShortRates, Model, Optimization);

% 4) Estimation Error;
IncludeSRinError = 'no';
[PriceE YTME] = NSerror(Bonds, ShortRates, Model, Params, IncludeSRinError);

% 5) Plot
Plots1D(Params, Model, YTME, ShortRates);


function [L,S,obj,err,iter] = rpca(X,lambda,opts)

% Solve the Robust Principal Component Analysis minimization problem by M-ADMM
%
% min_{L,S} ||L||_*+lambda*loss(S), s.t. X=L+S
% loss(S) = ||S||_1 or ||S||_{2,1}
%
% ---------------------------------------------
% Input:
%       X       -    d*n matrix
%       lambda  -    >0, parameter
%       opts    -    Structure value in Matlab. The fields are
%           opts.loss       -   'l1' (default): loss(S) = ||S||_1
%                               'l21': loss(S) = ||S||_{2,1}
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       L       -    d*n matrix
%       S       -    d*n matrix
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 19/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

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

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

[d,n] = size(X);

L = zeros(d,n);
S = L;
Y = L;

iter = 0;
for iter = 1 : max_iter
    Lk = L;
    Sk = S;
    % update L
    [L,nuclearnormL] = prox_nuclear(-S+X-Y/mu,1/mu);
    % update S
    if strcmp(loss,'l1')
        S = prox_l1(-L+X-Y/mu,lambda/mu);
    elseif strcmp(loss,'l21')
        S = prox_l21(-L+X-Y/mu,lambda/mu);
    else
        error('not supported loss function');
    end

    dY = L+S-X;
    chgL = max(max(abs(Lk-L)));
    chgS = max(max(abs(Sk-S)));
    chg = max([chgL chgS max(abs(dY(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = nuclearnormL+lambda*comp_loss(S,loss);
            err = norm(dY,'fro');
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y = Y + mu*dY;
    mu = min(rho*mu,max_mu);
end
obj = nuclearnormL+lambda*comp_loss(S,loss);
err = norm(dY,'fro');

function out = comp_loss(E,loss)

switch loss
    case 'l1'
        out = norm(E(:),1);
    case 'l21'
        out = 0;
        for i = 1 : size(E,2)
            out = out + norm(E(:,i));
        end
end


% -----------------------------------------------------------------  %
% Matlab Programs included the Appendix B in the book:               %
%  Xin-She Yang, Engineering Optimization: An Introduction           %
%                with Metaheuristic Applications                     %
%  Published by John Wiley & Sons, USA, July 2010                    %
%  ISBN: 978-0-470-58246-6,   Hardcover, 347 pages                   %
% -----------------------------------------------------------------  %
% Citation detail:                                                   %
% X.-S. Yang, Engineering Optimization: An Introduction with         %
% Metaheuristic Application, Wiley, USA, (2010).                     %
%                                                                    %
% http://www.wiley.com/WileyCDA/WileyTitle/productCd-0470582464.html %
% http://eu.wiley.com/WileyCDA/WileyTitle/productCd-0470582464.html  %
% -----------------------------------------------------------------  %
% ===== ftp://  ===== ftp://   ===== ftp:// =======================  %
% Matlab files ftp site at Wiley                                     %
% ftp://ftp.wiley.com/public/sci_tech_med/engineering_optimization   %
% ----------------------------------------------------------------   %

% Genetic Algorithm (Simple Demo) Matlab/Octave Program              %
% Written by X S Yang (Cambridge University) 2008                    %
% This is a simple demo, there are more efficient packages available %
% both commercial and/or open source. For actual applications,       %
% more sophicated implementation is needed. So search the web to     %
% to find a suitable package concerning genetic algorithms           %

% Usage: B1_ga   or  B1_ga(`x*exp(-x)');
function [bestsol, bestfun, count]=B1_ga(funstr)
global solnew sol pop popnew fitness fitold f range;
if nargin<1,
  % Easom Function with fmax=1 at x=pi
  funstr='-cos(x)*exp(-(x-3.1415926)^2)';
end
range=[-10 10];       % Range/Domain
% Converting to an inline function
f=vectorize(inline(funstr));
% Generating the initil population
rand('state',0');     % Reset the random generator
popsize=20;           % Population size
MaxGen=100;           % Max number of generations
count=0;              % counter
nsite=2;              % number of mutation sites
pc=0.95;              % Crossover probability
pm=0.05;              % Mutation probability
nsbit=16;             % String length (bits)
% Generating initial population
popnew=init_gen(popsize,nsbit);
fitness=zeros(1,popsize);    % fitness array
% Display the shape of the function
x=range(1):0.1:range(2); plot(x,f(x));
% Initialize solution <- initial population
for i=1:popsize,
   solnew(i)=bintodec(popnew(i,:));
end
% Start the evolution loop
for i=1:MaxGen,
   % Record as the history
   fitold=fitness; pop=popnew; sol=solnew;
 for j=1:popsize,
   % Crossover pair
   ii=floor(popsize*rand)+1; jj=floor(popsize*rand)+1;
   % Cross over
   if pc>rand,
      [popnew(ii,:),popnew(jj,:)]=...
                    crossover(pop(ii,:),pop(jj,:));
   % Evaluate the new pairs
   count=count+2;
   evolve(ii); evolve(jj);
   end
   % Mutation at n sites
   if pm>rand,
    kk=floor(popsize*rand)+1;   count=count+1;
    popnew(kk,:)=mutate(pop(kk,:),nsite);
    evolve(kk);
   end
 end  % end for j
   % Record the current best
   bestfun(i)=max(fitness);
   bestsol(i)=mean(sol(bestfun(i)==fitness));
end
% Display results
subplot(2,1,1); plot(bestsol); title('Best estimates');
subplot(2,1,2); plot(bestfun); title('Fitness');
% ------------- All sub functions ----------
% generation of initial population
function pop=init_gen(np,nsbit)
% String length=nsbit+1 with pop(:,1) for the Sign
pop=rand(np,nsbit+1)>0.5;
% Evolving the new generation
function evolve(j)
global solnew popnew fitness fitold pop sol f;
   solnew(j)=bintodec(popnew(j,:));
   fitness(j)=f(solnew(j));
   if fitness(j)>fitold(j),
      pop(j,:)=popnew(j,:);
      sol(j)=solnew(j);
   end
% Convert a binary string into a decimal number
function [dec]=bintodec(bin)
global range;
% Length of the string without sign
nn=length(bin)-1;
num=bin(2:end);   % get the binary
% Sign=+1 if bin(1)=0; Sign=-1 if bin(1)=1.
Sign=1-2*bin(1);
dec=0;
% floating point.decimal place in the binary
dp=floor(log2(max(abs(range))));
for i=1:nn,
   dec=dec+num(i)*2^(dp-i);
end
dec=dec*Sign;
% Crossover operator
function [c,d]=crossover(a,b)
nn=length(a)-1;
% generating random crossover point
cpoint=floor(nn*rand)+1;
c=[a(1:cpoint) b(cpoint+1:end)];
d=[b(1:cpoint) a(cpoint+1:end)];
% Mutatation operator
function anew=mutate(a,nsite)
nn=length(a); anew=a;
for i=1:nsite,
   j=floor(rand*nn)+1;
   anew(j)=mod(a(j)+1,2);
end


function [ n_data, nu, x, fx ] = bessel_ix_values ( n_data )

%*****************************************************************************80
%
%% BESSEL_IX_VALUES returns some values of the Ix Bessel function.
%
%  Discussion:
%
%    This set of data considers the less common case in which the
%    index of the Bessel function In is actually not an integer.
%    We may suggest this case by occasionally replacing the symbol
%    "In" by "Ix".
%
%    The modified Bessel functions In(Z) and Kn(Z) are solutions of
%    the differential equation
%
%      Z^2 W'' + Z * W' - ( Z^2 + N^2 ) * W = 0.
%
%    In Mathematica, the function can be evaluated by:
%
%      BesselI[n,x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real NU, the order of the function.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 28;

  fx_vec = [ ...
    0.3592084175833614E+00,  ...
    0.9376748882454876E+00,  ...
    2.046236863089055E+00,   ...
    3.053093538196718E+00,   ...
    4.614822903407601E+00,   ...
    26.47754749755907E+00,   ...
    2778.784603874571E+00,   ...
    4.327974627242893E+07,   ...
    0.2935253263474798E+00,  ...
    1.099473188633110E+00,   ...
    21.18444226479414E+00,   ...
    2500.906154942118E+00,   ...
    2.866653715931464E+20,   ...
    0.05709890920304825E+00, ...
    0.3970270801393905E+00,  ...
    13.76688213868258E+00,   ...
    2028.512757391936E+00,   ...
    2.753157630035402E+20,   ...
    0.4139416015642352E+00,  ...
    1.340196758982897E+00,   ...
    22.85715510364670E+00,   ...
    2593.006763432002E+00,   ...
    2.886630075077766E+20,   ...
    0.03590910483251082E+00, ...
    0.2931108636266483E+00,  ...
    11.99397010023068E+00,   ...
    1894.575731562383E+00,   ...
    2.716911375760483E+20 ];

  nu_vec = [ ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    0.50E+00, ...
    1.50E+00, ...
    1.50E+00, ...
    1.50E+00, ...
    1.50E+00, ...
    1.50E+00, ...
    2.50E+00, ...
    2.50E+00, ...
    2.50E+00, ...
    2.50E+00, ...
    2.50E+00, ...
    1.25E+00, ...
    1.25E+00, ...
    1.25E+00, ...
    1.25E+00, ...
    1.25E+00, ...
    2.75E+00, ...
    2.75E+00, ...
    2.75E+00, ...
    2.75E+00, ...
    2.75E+00 ];

  x_vec = [ ...
      0.2E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      2.5E+00, ...
      3.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     20.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00, ...
      1.0E+00, ...
      2.0E+00, ...
      5.0E+00, ...
     10.0E+00, ...
     50.0E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    nu = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    nu = nu_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function imgBlend = sc_poisson_blend(imgTrg, imgSrc, holeMask)
% SC_POISSON_BLEND: Blend the source and target image using a simple
% discrete Poisson solver
%
% Input:
%   - imgTrg: original image with hole
%   - imgSrc: synthesized image
%   - holeMask: specify the hole pixels
% Output:
%   - imgBlend: blended image

[imgH, imgW, nCh] = size(imgSrc);

% Initialize the reconstructed image
imgRecon = zeros(imgH, imgW, nCh);

% Independently process each channel
for ch = 1: nCh
    % Source and target image
    imgS = imgSrc(:,:,ch);
    imgT = imgTrg(:,:,ch);

    % prepare discrete Poisson equation
    [A, b] = solvePoisson(holeMask, imgS, imgT);

    % solve Poisson equation
    x = A\b;
    imgRecon(:,:,ch) = reshape(x, [imgH, imgW]);
end

% Combined with the known region in the target
holeMaskC = cat(3, holeMask, holeMask, holeMask);
imgBlend = holeMaskC.*imgRecon + ~holeMaskC.*imgTrg;

end

function [A, b] = solvePoisson(holeMask, imgS, imgT)

% Prepare the linear system of equations for Poisson blending

[imgH, imgW] = size(holeMask);
N = imgH*imgW;

% Number of unknown variables
numUnknownPix = sum(holeMask(:));

maxNumInd = 8*numUnknownPix; % Max number of indices for constructing the sparse matrix
maxNumEqn = 4*numUnknownPix; % Max number of equations (4-neighbor)

% 4-neighbors: dx and dy
dx = [1, 0, -1,  0];    dy = [0, 1,  0, -1];

% Initialize (I, J, S), for sparse matrix A where A(I(k), J(k)) = S(k)
I = zeros(maxNumInd, 1);
J = zeros(maxNumInd, 1);
S = zeros(maxNumInd, 1);

% Initialize b
b = zeros(maxNumEqn, 1);

% Precompute unkonwn pixel position
[pi, pj] = find(holeMask == 1);
pind = sub2ind([imgH, imgW], pi, pj);

% Precompute the 4-neighbor of the unkonwn pixel positions
qi = bsxfun(@plus, pi, dy);
qj = bsxfun(@plus, pj, dx);

% Handling cases at image borders
validN = (qi >= 1) & (qi <= imgH) & (qj >= 1) & (qj <= imgW);
qind = zeros(size(validN));
qind(validN) = sub2ind([imgH, imgW], qi(validN), qj(validN));

c = 1; % index counter for matrix A
e = 1; % equation counter

% Set up the matrix A and the vector b
for k = 1: numUnknownPix
    pind_cur = pind(k);
    for n = 1: 4 % 4-neighbor
        if(validN(k, n)) % if the neighbor pixel q lies in the image
            qind_cur = qind(k, n);
            if(holeMask(qind_cur))
                % A(e, pind_cur) = 1;
                I(c) = e;   J(c) = pind_cur;    S(c) = 1;   c = c + 1;
                % A(e, qind_cur) = 1;
                I(c) = e;   J(c) = qind_cur;    S(c) = -1;  c = c + 1;

                % gradient constraint
                b(e) = imgS(pind_cur) - imgS(qind_cur);
                e = e + 1;
            else
                % A(e, pind_cur) = 1;
                I(c) = e;   J(c) = pind_cur;    S(c) = 1;   c = c + 1;

                % boundary constraint
                b(e) = imgS(pind_cur) - imgS(qind_cur) + imgT(qind_cur);
                e = e + 1;
            end
        end
    end
end

% Clean up unused entries
nEqn = e - 1;
b = b(1:e-1);

nInd = c - 1;
I = I(1:nInd);  J = J(1:nInd);  S = S(1:nInd);

% Construct the sparse matrix A
A = sparse(I, J, S, nEqn, N);

end


function idx=diagElIdx(N,minorDiag)
%%DIAGELIDX For an NXN matrix, determine the linear indices of the elements
%           on the main or minor diagonal of the matrix.
%
%INPUTS:  N The size of the NXN matrix.
% minorDiag Optionally, this can specify whether the main or minor diagonal
%           of the matrix is desired. minorDiag=false (the default if
%           omitted or an empty matrix is passed) chooses the main
%           diagonal, and minorDiag=true chooses the minor diagonal of the
%           matrix.
%
%OUTPUTS: idx A 1XN vector of the indices on the main (or minor) diagonal
%             of the matrix. Minor diagonal elements go from the lower-left
%             corner of the matrix to the upper-right corner.
%
%The major diagonal is also known as the principal diagonal, the primary
%diagonal, the major diagonal and the leading diagonal. The minor diagonal
%of a matrix is also known as the antidiagonal, the counterdiagonal, the
%trailing diagonal, and the secondary diagonal.
%
%EXAMPLE:
% N=8;
% M=magic(N)
% mainDiagEls=M(diagElIdx(N))
% minorDiagEls=M(diagElIdx(N,true))
%One will see that mainDiagEls contains the main diagonal elements of M,
%and minorDiagEls contains the minor diagonal elements, which increase
%sequentialy.
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(minorDiag))
    minorDiag=false;
end

if(minorDiag==false)
    idx=1:(N+1):(N^2);
else
    idx=N:(N-1):(N^2-N+1);
end
end

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


% Add a landmark to the UKF.
% We have to compute the uncertainty of the landmark given the current state
% (and its uncertainty) of the newly observed landmark. To this end, we also
% employ the unscented transform to propagate Q (sensor noise) through the
% current state

function [mu, sigma, map] = add_landmark_to_map(mu, sigma, z, map, Q);

% For computing sigma
global scale;

landmarkId = z.id;

%add landmark to the map
map = [map; landmarkId];
% TODO: Initialize its pose according to the measurement and add it to mu

% Append the measurement to the state vector
mu = [mu; z.range(); z.bearing()];
% Initialize its uncertainty and add it to sigma
sigma = blkdiag(sigma, Q);

% Transform from [range, bearing] to the x/y location of the landmark
% This operation intializes the uncertainty in the position of the landmark
% Sample sigma points
sig_pnts_new = compute_sigma_points(mu, sigma);
% Normalize!
sig_pnts_new(3,:) = normalize_angle(sig_pnts_new(3,:));
% Compute the xy location of the new landmark according to each sigma point
newX = sig_pnts_new(1,:) + sig_pnts_new(end-1,:).*cos(sig_pnts_new(3,:) + sig_pnts_new(end,:));
newY = sig_pnts_new(2,:) + sig_pnts_new(end-1,:).*sin(sig_pnts_new(3,:) + sig_pnts_new(end,:));
% The last 2 components of the sigma points can now be replaced by the xy pose of the landmark
sig_pnts_new(end-1,:) = newX;
sig_pnts_new(end,:) = newY;

% Recover mu and sigma
n = length(mu);
lambda = scale - n;
w0 = lambda/scale;
wm = [w0, repmat(1/(2*scale),1,2*n)];
wc = wm;
% Theta should be recovered by summing up the sines and cosines
cosines = sum(cos(sig_pnts_new(3,:)).*wm);
sines = sum(sin(sig_pnts_new(3,:)).*wm);
% recompute the angle and normalize it
mu_theta = normalize_angle(atan2(sines, cosines));
mu = sum(sig_pnts_new .* repmat(wm, rows(sig_pnts_new), 1), 2);
mu(3) = mu_theta;

diff = sig_pnts_new - repmat(mu,1,size(sig_pnts_new,2));
% Normalize!
diff(3,:) = normalize_angle(diff(3,:));
sigma = (repmat(wc, rows(diff), 1) .* diff) * diff';

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% DISCRETE VARIANCE SWAP/OPTION PRICER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Descritpion: Script to Price Barrier options in Levy Models
%              using the PROJ method
% Author:      Justin Kirkby
% References:  (1) A General Framework for discretely sampled realized
%              variance derivatives in stocahstic volatility models with
%              jumps, EJOR, 2017
%              (2) Efficient Option Pricing By Frame Duality with The Fast
%              Fourier Transform, SIAM J. Financial Math., 2015
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[folder, name, ext] = fileparts(which( mfilename('fullpath')));
cd(folder);
addpath('../RN_CHF')
addpath('../Helper_Functions')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 1) CHOOSE CONTRACT/GENERAL PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K    = 0.01;  %Strike            %NOTE: no error handling in place for extreme values of W (increase grid if strike falls outside)
r    = .05;  %Interest rate
q    = .00;  %dividend yield
T    = 1;    %Time (in years)
M    = 252;  %number of discrete monitoring points

contract = 1;  %1 for for variance swap, 3 for variance call  (other contracts not yet coded)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 2) CHOOSE MODEL PARAMETERS (Levy Models)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

model = 1;   % (CHOOSE) See Models Below (e.g. model 1 is Black Scholes), and choose specific params
params = {};

if model == 1 %BSM (Black Scholes Merton)
    params.sigmaBSM = 0.18;    %CHOOSE

elseif model == 2 %CGMY
    params.C  = 0.02;
    params.G  = 5;
    params.MM = 15;
    params.Y  = 1.2;

elseif model == 3 %NIG
    params.alpha = 15;
    params.beta  = -5;
    params.delta = 0.5;

elseif model == 4 %MJD (Merton Jump Diffusion)
    params.sigma  = 0.12;
    params.lam    = 0.4;
    params.muj    = -0.12;
    params.sigmaj = 0.18;

elseif model == 5 %Kou Double Expo
    params.sigma = 0.15;
    params.lam   = 3;
    params.p_up  = 0.2;
    params.eta1  = 25;
    params.eta2  = 10;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 3) CHOOSE PROJ PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
UseCumulant = 1;  %Set to 1 to use the cumulant base rule (Approach 1) to determine gridwidth, else used fixed witdth (Approach 2)

%---------------------
% APPROACH 1: Cumulant Based approach for grid width
% (see "Robust Option Pricing with Characteritics Functions and the BSpline Order of Density Projection")
%---------------------
if UseCumulant ==1  %With cumulant based rule, choose N and Alpha (N = 2^(P+Pbar) based on second approach)
    logN  = 10;   %Uses N = 2^logN  gridpoint
    L1 = 14;  % determines grid witdth (usually set L1 = 8 to 15 for Levy, or 18 for Heston)
%---------------------
% APPROACH 2: Manual GridWidth approach
%---------------------
else %Manually specify resolution and Pbar
    P     = 7;  % resolution is 2^P
    Pbar  = 3;  % Determines density truncation grid with, 2^Pbar
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% PRICE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Note: rnCHF is the risk netural CHF, c1,c2,c4 are the cumulants
modelInput = getModelInput(model, T/M, r, q, params);

if UseCumulant ==1  % Choose density truncation width based on cumulants
    alpha = getTruncationAlpha(T, L1, modelInput, model);
else    % Manually supply density truncation width above
    logN = P + Pbar;
    alpha = 2^Pbar/2;
end
N = 2^logN;

tic
price = PROJ_DiscreteVariance_Swaps_Options( N,alpha,M,r,T,K,modelInput.rnCHF,contract);
toc

fprintf('price: %.8f\n', price)

if contract == 1  % In swap case, the price is known analytically (for Levy process)
    c2 = modelInput.c2; c1 = modelInput.c1;
    analytical = c2 + c1^2*T/M;
    fprintf('analytical SWAP price: %.8f \n', analytical)
    fprintf('----------------------------\n')
    fprintf('Error: %.2e\n', analytical-price)
end


function k = WalravenValeton_k(Lwa, wv_sigma)
%
%       k = WalravenValeton_k(Lwa, wv_sigma)
%
%
%        Input:
%           -Lwa: world adaptation luminance in cd/m^2
%           -wv_sigma: cd/m^2
%
%        Output:
%           -k: WalravenValeton's k value
%
%     Copyright (C) 2016 Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%     The paper describing this technique is:
%     "A Model of Visual Adaptation for Realistic Image Synthesis"
% 	  by James A. Ferwerda, Sumanta N. Pattanaik, Peter Shirley, Donald P. Greenberg
%     in Proceedings of SIGGRAPH 1996
%

if(~exist('wv_sigma', 'var'))
    wv_sigma = 100; %cd/m^2
end

k = (wv_sigma - Lwa / 4) ./ (wv_sigma + Lwa);

k(k < 0.0) = 0.0;

end


function [huff entropy avglength redundancy]=huffman(alpha,prob)
s=sum(prob(:));
s=roundn(s,-4);
% Calculate length of source and probability
la=length(alpha);
lp=length(prob);
if (la==lp & s==1)
       %Calculate the Entropy
         entropy=prob.*log2(prob);
         entropy=-sum(entropy(:));
         pos=1:lp;
         [prs idx]=sort(prob,'descend');
         npos=pos(idx);
         %Minimum Variance
         idx=find(prs==min(prs(:)));
         tp=npos(idx);
         tp=sort(tp,'descend');
         npos(idx)=tp;
         %
         codebook(1:lp)={''};
         ps=npos;
         np=lp;
         cb=zeros([lp-1 3]);
         cnt=lp+1;
         prb=prs;
         for i=1:lp-1
              fst=ps(np-1);
              sec=ps(np);

                if fst<=lp
                   codebook(fst)=strcat('0',codebook(fst));
                else
                  codebook=encod(fst,cb,codebook,'0',lp);

                end

                if sec<=lp
                    codebook(sec)=strcat('1',codebook(sec));
                else
                   codebook=encod(sec,cb,codebook,'1',lp);
                end
              cb(i,1)=cnt;
              cb(i,2)=fst;
              cb(i,3)=sec;
              if np>2
                  ps=ps(1:np-2);
                  ps(np-1)=cnt;
                  cnt=cnt+1;
                  prbt=prb(1:np-2);
                  prbt(np-1)=prb(np-1)+prb(np);
                  prb=prbt;
                  [prb idx]=sort(prb,'descend');
                  ps=ps(idx);
                  %Minimum Variance
                  idx=find(prb==prbt(np-1));
                  tp=ps(idx);
                  tp=sort(tp,'descend');
                  ps(idx)=tp;
                  %
                  np=np-1;
              end

         end
         for i=1:lp
            huff(i).sym=alpha(i);
            huff(i).prob=prob(i);
            huff(i).code=codebook(i);
         end
           avglength=0;
           for i=1:lp
               avglength=avglength+huff(i).prob*length(cell2mat(huff(i).code));
           end
           redundancy=((avglength-entropy)/entropy)*100;
else
    display('Error in input.....');
    huff=[];
end

function codebook=encod(fs,cb,codebook,str,lp)
idx=find(cb(:,1)==fs);
x=cb(idx,2);
y=cb(idx,3);
 if x<=lp & y<=lp
     codebook(x)=strcat(str,codebook(x));
     codebook(y)=strcat(str,codebook(y));
     return
 elseif x<=lp
     codebook(x)=strcat(str,codebook(x));
     codebook=encod(y,cb,codebook,str,lp);
 elseif y<=lp
     codebook(y)=strcat(str,codebook(y));
     codebook=encod(x,cb,codebook,str,lp);
 else
     codebook=encod(x,cb,codebook,str,lp);
     codebook=encod(y,cb,codebook,str,lp);
 end


function gt=gabtight(varargin)
%GABTIGHT  Canonical tight window of Gabor frame
%   Usage:  gt=gabtight(a,M,L);
%           gt=gabtight(g,a,M);
%           gt=gabtight(g,a,M,L);
%           gd=gabtight(g,a,M,'lt',lt);
%
%   Input parameters:
%         g     : Gabor window.
%         a     : Length of time shift.
%         M     : Number of modulations.
%         L     : Length of window. (optional)
%         lt    : Lattice type (for non-separable lattices).
%   Output parameters:
%         gt    : Canonical tight window, column vector.
%
%   `gabtight(a,M,L)` computes a nice tight window of length *L* for a
%   lattice with parameters *a*, *M*. The window is not an FIR window,
%   meaning that it will only generate a tight system if the system
%   length is equal to *L*.
%
%   `gabtight(g,a,M)` computes the canonical tight window of the Gabor frame
%   with window *g* and parameters *a*, *M*.
%
%   The window *g* may be a vector of numerical values, a text string or a
%   cell array. See the help of |gabwin| for more details.
%
%   If the length of *g* is equal to *M*, then the input window is assumed to
%   be a FIR window. In this case, the canonical dual window also has
%   length of *M*. Otherwise the smallest possible transform length is
%   chosen as the window length.
%
%   `gabtight(g,a,M,L)` returns a window that is tight for a system of
%   length *L*. Unless the input window *g* is a FIR window, the returned
%   tight window will have length *L*.
%
%   `gabtight(g,a,M,'lt',lt)` does the same for a non-separable lattice
%   specified by *lt*. Please see the help of |matrix2latticetype| for a
%   precise description of the parameter *lt*.
%
%   If $a>M$ then an orthonormal window of the Gabor Riesz sequence with
%   window *g* and parameters *a* and *M* will be calculated.
%
%   Examples:
%   ---------
%
%   The following example shows the canonical tight window of the Gaussian
%   window. This is calculated by default by |gabtight| if no window is
%   specified:::
%
%     a=20;
%     M=30;
%     L=300;
%     gt=gabtight(a,M,L);
%
%     % Simple plot in the time-domain
%     figure(1);
%     plot(gt);
%
%     % Frequency domain
%     figure(2);
%     magresp(gt,'dynrange',100);
%
%   See also:  gabdual, gabwin, fir2long, dgt

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: TEST_DGT
%   REFERENCE: OK

%% ------------ decode input parameters ------------

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

if numel(varargin{1})==1
  % First argument is a scalar.

  a=varargin{1};
  M=varargin{2};

  g='gauss';

  varargin=varargin(3:end);
else
  % First argument assumed to be a vector.

  g=varargin{1};
  a=varargin{2};
  M=varargin{3};

  varargin=varargin(4:end);
end;

definput.keyvals.L=[];
definput.keyvals.lt=[0 1];
definput.keyvals.nsalg=0;
[flags,kv,L]=ltfatarghelper({'L'},definput,varargin);

%% ------ step 2: Verify a, M and L
if isempty(L)
    if isnumeric(g)
        % Use the window length
        Ls=length(g);
    else
        % Use the smallest possible length
        Ls=1;
    end;

    % ----- step 2b : Verify a, M and get L from the window length ----------
    L=dgtlength(Ls,a,M,kv.lt);

else

    % ----- step 2a : Verify a, M and get L

    Luser=dgtlength(L,a,M,kv.lt);
    if Luser~=L
        error(['%s: Incorrect transform length L=%i specified. Next valid length ' ...
               'is L=%i. See the help of DGTLENGTH for the requirements.'],...
              upper(mfilename),L,Luser);
    end;

end;

%% ----- step 3 : Determine the window

[g,info]=gabwin(g,a,M,L,kv.lt,'callfun',upper(mfilename));

if L<info.gl
  error('%s: Window is too long.',upper(mfilename));
end;

R=size(g,2);

% -------- Are we in the Riesz sequence of in the frame case

scale=1;
if a>M*R
  % Handle the Riesz basis (dual lattice) case.
  % Swap a and M, and scale differently.
  scale=sqrt(a/M);
  tmp=a;
  a=M;
  M=tmp;
end;

% -------- Compute -------------

if kv.lt(2)==1
    % Rectangular case

    if (info.gl<=M) && (R==1)

        % Diagonal of the frame operator
        d = gabframediag(g,a,M,L);
        gt=g./sqrt(long2fir(d,info.gl));

    else

        % Long window case

        % Just in case, otherwise the call is harmless.
        g=fir2long(g,L);

        gt=comp_gabtight_long(g,a,M)*scale;

    end;

else

    % Just in case, otherwise the call is harmless.
    g=fir2long(g,L);

    if (kv.nsalg==1) || (kv.nsalg==0 && kv.lt(2)<=2)

        mwin=comp_nonsepwin2multi(g,a,M,kv.lt,L);

        gtfull=comp_gabtight_long(mwin,a*kv.lt(2),M)*scale;

        % We need just the first vector
        gt=gtfull(:,1);

    else

        [s0,s1,br] = shearfind(L,a,M,kv.lt);

        if s1 ~= 0
            p1 = comp_pchirp(L,s1);
            g = p1.*g;
        end

        b=L/M;
        Mr = L/br;
        ar = a*b/br;

        if s0 == 0
            gt=comp_gabtight_long(g,ar,Mr);
        else
            p0=comp_pchirp(L,-s0);
            g = p0.*fft(g);
            gt=comp_gabtight_long(g,L/Mr,L/ar)*sqrt(L);
            gt = ifft(conj(p0).*gt);
        end

        if s1 ~= 0
            gt = conj(p1).*gt;
        end

    end;

    if (info.gl<=M) && (R==1)
        gt=long2fir(gt,M);
    end;

end;

% --------- post process result -------

if isreal(g) && (kv.lt(2)<=2)
  % If g is real and the lattice is either rectangular or quinqux, then
  % the output is known to be real.
  gt=real(gt);
end;

if info.wasrow
  gt=gt.';
end;


function varargout = cpower(varargin)
%CPOWER Power of SDPVAR variable with convexity knowledge
%
% CPOWER is recommended if your goal is to obtain
% a convex model, since the function CPOWER is implemented
% as a so called nonlinear operator. (For p/q ==2 you can
% however just as well use the overloaded power)
%
% t = cpower(x,p/q)
%
% For negative p/q, the operator is convex.
% For positive p/q with p>q, the operator is convex.
% For positive p/q with p<q, the operator is concave.
%
% A domain constraint x>0 is automatically added if
% p/q not is an even integer.
%
% Note, the complexity of generating the conic representation
% of these variables are O(2^L) where L typically is the
% smallest integer such that 2^L >= min(p,q)

switch class(varargin{1})

    case 'double'
        varargout{1} = power(varargin{1},varargin{2});
    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.
        X = varargin{1};
        if isreal(X)
            dim = size(X);
            X = reshape(X,prod(dim),1);
            y = [];
            for i = 1:prod(dim)
                y = [y;yalmip('define',mfilename,extsubsref(X,i),varargin{2})];
            end
            y = reshape(y,dim);
            varargout{1} = y;
        else
            error('CPOWER can only be applied to real vectors.');
        end

    case 'char' % YALMIP send 'model' when it wants the epigraph or hypograph
        if isequal(varargin{1},'graph')
            t = varargin{2}; % Second arg is the extended operator variable
            X = varargin{3}; % Third arg and above are the args user used when defining t.
            p = varargin{4};

            if p>0
                [p,q] = rat(abs(p));
                F = pospower(X,t,p,q);
                if p>q
                    convexity = 'convex';
                    monotonicity = 'increasing';
                else
                    convexity = 'concave';
                    monotonicity = 'decreasing';
                end
            else
                [p,q] = rat(abs(p));
                F = negpower(X,t,p,q);
                convexity = 'convex';
                monotonicity = 'decreasing';
            end

            varargout{1} = F;
            varargout{2} = struct('convexity',convexity,'monotonicity',monotonicity,'definiteness','positive','model','graph');
            varargout{3} = X;
        end
    otherwise
end

function F = pospower(x,t,p,q)
if p>q
    l = ceil(log2(abs(p)));
    r = 2^l-p;
    y = [ones(r,1)*x;ones(q,1)*t;ones(2^l-r-q,1)];
    F = detset(x,y);
else
    l = ceil(log2(abs(q)));
    y = [ones(p,1)*x;ones(2^l-q,1)*t;ones(q-p,1)];
    F = detset(t,y);
end

function F = negpower(x,t,p,q)
l = ceil(log2(abs(p+q)));
p = abs(p);
q = abs(q);
y = [ones(2^l-p-q,1);ones(p,1)*x;ones(q,1)*t];
F = detset(1,y);


% We consider a switching Kalman filter of the kind studied
% by Zoubin Ghahramani, i.e., where the switch node determines
% which of the hidden chains we get to observe (data association).
% e.g., for n=2 chains
%
% X1 -> X1
% | X2 -> X2
% \ |
%  v
%  Y
%  ^
%  |
%  S
%
% Y is a gmux (multiplexer) node, where S switches in one of the parents.
% We differ from Zoubin by not connecting the S nodes over time (which
% doesn't make sense for data association).
% Indeed, we assume the S nodes are always observed.
%
%
% We will track 2 objects (points) moving in the plane, as in BNT/Kalman/tracking_demo.
% We will alternate between observing them.

nobj = 2;
N = nobj+2;
Xs = 1:nobj;
S = nobj+1;
Y = nobj+2;

intra = zeros(N,N);
inter = zeros(N,N);
intra([Xs S], Y) =1;
for i=1:nobj
  inter(Xs(i), Xs(i))=1;
end

Xsz = 4; % state space = (x y xdot ydot)
Ysz = 2;
ns = zeros(1,N);
ns(Xs) = Xsz;
ns(Y) = Ysz;
ns(S) = n;

bnet = mk_dbn(intra, inter, ns, 'discrete', S, 'observed', [S Y]);

% For each object, we have
% X(t+1) = F X(t) + noise(Q)
% Y(t) = H X(t) + noise(R)
F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];
H = [1 0 0 0; 0 1 0 0];
Q = 1e-3*eye(Xsz);
%R = 1e-3*eye(Ysz);
R = eye(Ysz);

% We initialise object 1 moving to the right, and object 2 moving to the left
% (Here, we assume nobj=2)
init_state{1} = [10 10 1 0]';
init_state{2} = [10 -10 -1 0]';

for i=1:nobj
  bnet.CPD{Xs(i)} = gaussian_CPD(bnet, Xs(i), 'mean', init_state{i}, 'cov', 1e-4*eye(Xsz));
end
bnet.CPD{S} = root_CPD(bnet, S); % always observed
bnet.CPD{Y} = gmux_CPD(bnet, Y, 'cov', repmat(R, [1 1 nobj]), 'weights', repmat(H, [1 1 nobj]));
% slice 2
eclass = bnet.equiv_class;
for i=1:nobj
  bnet.CPD{eclass(Xs(i), 2)} = gaussian_CPD(bnet, Xs(i)+N, 'mean', zeros(Xsz,1), 'cov', Q, 'weights', F);
end

% Observe objects at random
T = 10;
evidence = cell(N, T);
data_assoc = sample_discrete(normalise(ones(1,nobj)), 1, T);
evidence(S,:) = num2cell(data_assoc);
evidence = sample_dbn(bnet, 'evidence', evidence);

% plot the data
true_state = cell(1,nobj);
for i=1:nobj
  true_state{i} = cell2num(evidence(Xs(i), :)); % true_state{i}(:,t) = [x y xdot ydot]'
end
obs_pos = cell2num(evidence(Y,:));
figure(1)
clf
hold on
styles = {'rx', 'go', 'b+', 'k*'};
for i=1:nobj
  plot(true_state{i}(1,:), true_state{i}(2,:), styles{i});
end
for t=1:T
  text(obs_pos(1,t), obs_pos(2,t), sprintf('%d', t));
end
hold off
relax_axes(0.1)

% Inference
ev = cell(N,T);
ev(bnet.observed,:) = evidence(bnet.observed, :);

engines = {};
engines{end+1} = jtree_dbn_inf_engine(bnet);
%engines{end+1} = scg_unrolled_dbn_inf_engine(bnet, T);
engines{end+1} = pearl_unrolled_dbn_inf_engine(bnet);
E = length(engines);

inferred_state = cell(nobj,E); % inferred_state{i,e}(:,t)
for e=1:E
  engines{e} = enter_evidence(engines{e}, ev);
  for i=1:nobj
    inferred_state{i,e} = zeros(4, T);
    for t=1:T
      m = marginal_nodes(engines{e}, Xs(i), t);
      inferred_state{i,e}(:,t) = m.mu;
    end
  end
end
inferred_state{1,1}
inferred_state{1,2}

% Plot results
figure(2)
clf
hold on
styles = {'rx', 'go', 'b+', 'k*'};
nstyles = length(styles);
c = 1;
for e=1:E
  for i=1:nobj
    plot(inferred_state{i,e}(1,:), inferred_state{i,e}(2,:), styles{mod(c-1,nstyles)+1});
    c = c + 1;
  end
end
for t=1:T
  text(obs_pos(1,t), obs_pos(2,t), sprintf('%d', t));
end
hold off
relax_axes(0.1)


function order = hexagon_unit_set ( rule )

%*****************************************************************************80
%
%% HEXAGON_UNIT_SIZE sizes a quadrature rule inside the unit hexagon in 2D.
%
%  Integration region:
%
%    The definition is given in terms of THETA, the angle in degrees of the
%    vector (X,Y).  The following six conditions apply, respectively,
%    between the bracketing values of THETA of 0, 60, 120, 180, 240,
%    300, and 360.
%
%                              0 <= Y <= - SQRT(3) * X + SQRT(3)
%                              0 <= Y <=                 SQRT(3)/2
%                              0 <= Y <=   SQRT(3) * X + SQRT(3)
%      - SQRT(3) * X - SQRT(3)   <= Y <= 0
%                    - SQRT(3)/2 <= Y <= 0
%        SQRT(3) * X - SQRT(3)   <= Y <= 0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Abramowitz and Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964.
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, integer RULE, the rule desired.
%      1, 1 point,  degree 1;
%      2, 4 points, degree 3;
%      3, 7 points, degree 3;
%      4, 7 points, degree 5;
%
%    Output, integer ORDER, the order of the desired rule.
%    If RULE is not legal, then ORDER is returned as -1.
%
  if ( rule == 1 )

    order = 1;
%
%  Stroud rule H2:3-1.
%
  elseif ( rule == 2 )

    order = 4;
%
%  Stroud rule H2:3-2.
%
  elseif ( rule == 3 )

    order = 7;
%
%  Stroud rule H2:5-1.
%
  elseif ( rule == 4 )

    order = 7;

  else

    order = -1;

  end

  return
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%                  1         , -2<=t<=2
% 	Graph of f(t)= 0         , 2<t<5
%                t*sin(4pi*t), 5<=t<=8

t1=-2:.1:2;
t2=2.1:.1:4.9;
t3=5:.1:8;

f1=ones(size(t1));
f2=zeros(size(t2));
f3=t3.*sin(4*pi*t3);

t=[t1 t2 t3];
f=[f1 f2 f3];

plot(t,f)
title('Multi-part function f(t)')


function  [Z, sigma] =  MCWNNM_ADMM_NL1( Y, NSig, Par )
% This routine solves the following weighted nuclear norm optimization problem with column weights,
%
% min_{X, Z} ||W(Y-X)||_F^2 + ||Z||_w,*  s.t.  X = Z
%
% Inputs:
%        Y        -- 3p^2 x M dimensional noisy matrix, D is the data dimension, and N is the number of image patches.
%        NSig   -- 3p^2 x 1 dimensional vector of weights
%        Par     -- structure of parameters
% Output:
%        Z        -- 3p^2 x M dimensional denoised matrix
%        sigma -- the noise standard deviation

% tol = 1e-8;
if ~isfield(Par, 'maxIter')
    Par.maxIter = 10;
end
if ~isfield(Par, 'rho')
    Par.rho = 1;
end
if ~isfield(Par, 'mu')
    Par.mu = 1;
end
if ~isfield(Par, 'display')
    Par.display = true;
end
%% Initializing optimization variables
% intialize

% Initializing optimization variables
% Intialize the weight matrix W
mNSig = min(NSig);
W = (mNSig+eps) ./ (NSig+eps);

Z = zeros(size(Y));
A = zeros(size(Y));
%% Start main loop
iter = 0;
PatNum       = size(Y,2);
TempC  = Par.Constant * sqrt(PatNum) * mNSig^2;
% TempC  = Par.Constant * sqrt(PatNum);
while iter < Par.maxIter
    iter = iter + 1;

    % update X, fix Z and A
    % min_{X} ||W * Y - W * X||_F^2 + 0.5 * rho * ||X - Z + 1/rho * A||_F^2
    X = diag(1 ./ (W.^2 + 0.5 * Par.rho)) * (diag(W.^2) * Y + 0.5 * Par.rho * Z - 0.5 * A);

    % update Z, fix X and A
    % min_{Z} ||Z||_*,w + 0.5 * rho * ||Z - (X + 1/rho * A)||_F^2
    Temp = X + A/Par.rho;
    [U, SigmaTemp, V] =   svd(full(Temp), 'econ');
    [SigmaZ, svp] = ClosedWNNM(diag(SigmaTemp), 2/Par.rho*TempC, eps);
    Z =  U(:, 1:svp) * diag(SigmaZ) * V(:, 1:svp)';
    %     % check the convergence conditions
    %     stopC = max(max(abs(X - Z)));
    %     if Par.display && (iter==1 || mod(iter,10)==0 || stopC<tol)
    %         disp(['iter ' num2str(iter) ',mu=' num2str(Par.mu,'%2.1e') ...
    %             ',rank=' num2str(rank(Z,1e-4*norm(Z,2))) ',stopALM=' num2str(stopC,'%2.3e')]);
    %     end
    %     if stopC < tol
    %         break;
    %     else
    % update the multiplier A, fix Z and X
    A = A + Par.rho * (X - Z);
    Par.rho = min(1e4, Par.mu * Par.rho);
    %     end
end
sigma = sqrt( mean( reshape(mean((Y - Z).^2, 2)', [Par.ps2 Par.ch])) )';
return;


function boundary = p05_boundary_nearest ( m, n, point )

%*****************************************************************************80
%
%% P05_BOUNDARY_NEAREST returns a nearest boundary point in problem 05.
%
%  Discussion:
%
%    The given input point need not be inside the region.
%
%    In some cases, more than one boundary point may be "nearest",
%    but only one will be returned.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real POINT(M,N), the coordinates of the points.
%
%    Output, real BOUNDARY(M,N), points on the boundary
%    that are nearest to each point.
%
  center1 = [  0.0, 0.0 ];
  center2 = [ -0.4, 0.0 ];
  r1 = 1.0;
  r2 = 0.55;

  for j = 1 : n

    dist_min = r8_huge ( );
%
%  Distance to the semicircle #1.
%
    if ( center1(2) <= point(2,j) )

      norm = sqrt ( ( point(1,j) - center1(1) ).^2 ...
                  + ( point(2,j) - center1(2) ).^2 );

      if ( 0.0 == norm )

        pn(1) = center1(1) + r1 * sqrt ( 0.5 );
        pn(2) = center1(2) + r1 * sqrt ( 0.5 );

      else

        pn(1) = center1(1) + ( point(1,j) - center1(1) ) / norm;
        pn(2) = center1(2) + ( point(2,j) - center1(2) ) / norm;

      end

    elseif ( point(1,j) <= center1(1) )

      pn(1) =  center1(1) - r1;
      pn(2) =  center1(2);

    elseif ( center1(1) <= point(1,j) )

      pn(1) =  center1(1) + r1;
      pn(2) =  center1(2);

    end

    dist = sqrt ( ( point(1,j) - pn(1) ).^2 + ( point(2,j) - pn(2) ).^2 );

    if ( dist < dist_min )
      dist_min = dist;
      boundary(1:2,j) = pn(1:2)';
    end
%
%  Distance to semicircle #2.
%
    if ( center2(2) <= point(2,j) )

      norm = sqrt ( ( point(1,j) - center2(1) ).^2 ...
                  + ( point(2,j) - center2(2) ).^2 );

      if ( 0.0 == norm )

        pn(1) = center2(1) + r2 * sqrt ( 0.5 );
        pn(2) = center2(2) + r2 * sqrt ( 0.5 );

      else

        pn(1) = center2(1) + ( point(1,j) - center2(1) ) / norm;
        pn(2) = center2(2) + ( point(2,j) - center2(2) ) / norm;

      end

    elseif ( point(1,j) <= center2(1) )

      pn(1) =  center2(1) - r2;
      pn(2) =  center2(2);

    elseif ( center2(1) <= point(1,j) )

      pn(1) =  center2(1) + r2;
      pn(2) =  center2(2);

    end

    dist = sqrt ( ( point(1,j) - pn(1) ).^2 + ( point(2,j) - pn(2) ).^2 );

    if ( dist < dist_min )
      dist_min = dist;
      boundary(1:2,j) = pn(1:2)';
    end
%
%  Distance to line segment #1: (P1,P2).
%
    p1(1:2) = [ center2(1) - r1, center2(1) ];
    p2(1:2) = [ center2(1) - r2, center2(2) ];

    [ pn, dist, t ] = segment_point_near_2d ( p1, p2, point(1:2,j)' );

    if ( dist < dist_min )
      dist_min = dist;
      boundary(1:2,j) = pn(1:2)';
    end
%
%  Distance to line segment #2: (Q1,Q2).
%
    q1(1:2) = [ center2(1) + r2, center2(2) ];
    q2(1:2) = [ center1(1) + r1, center1(2) ];

    [ pn, dist, t ] = segment_point_near_2d ( q1, q2, point(1:2,j)' );

    if ( dist < dist_min )
      dist_min = dist;
      boundary(1:2,j) = pn(1:2)';
    end

  end

  return
end


function inside = polygon_contains_point_2d ( n, v, p )

%*****************************************************************************80
%
%% POLYGON_CONTAINS_POINT_2D finds if a point is inside a simple polygon in 2D.
%
%  Discussion:
%
%    A simple polygon is one whose boundary never crosses itself.
%    The polygon does not need to be convex.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    M Shimrat,
%    Position of Point Relative to Polygon,
%    ACM Algorithm 112,
%    Communications of the ACM,
%    Volume 5, Number 8, page 434, August 1962.
%
%  Parameters:
%
%    Input, integer N, the number of nodes or vertices in the polygon.
%    N must be at least 3.
%
%    Input, real V(2,N), the coordinates of the vertices of the polygon.
%
%    Input, real P(2,1), the coordinates of the point to be tested.
%
%    Output, logical INSIDE, is TRUE if the point is inside the polygon.
%
  inside = 0;

  for i = 1 : n

    x1 = v(1,i);
    y1 = v(2,i);

    if ( i < n )
      x2 = v(1,i+1);
      y2 = v(2,i+1);
    else
      x2 = v(1,1);
      y2 = v(2,1);
    end

    if ( ( y1 < p(2,1) & p(2,1) <= y2 ) | ( p(2,1) <= y1 & y2 < p(2,1) ) )
      if ( ( p(1,1) - x1 ) - ( p(2,1) - y1 ) * ( x2 - x1 ) / ( y2 - y1 ) < 0.0 )
        inside = ~inside;
      end
    end

  end

  return
end


function coef = sgmga_vcn_coef_naive ( dim_num, level_weight, x_max, x, q_max )

%*****************************************************************************80
%
%% SGMGA_VCN_COEF_NAIVE returns the "next" constrained vector's coefficient.
%
%  Discussion:
%
%    This function uses a naive approach to the computation, resulting in
%    a set of 2^DIM_NUM tasks.  Hence it is not suitable for cases where
%    DIM_NUM is moderately large.  The function SGMGA_VCN_COEF carries out
%    a more complicated but more efficient algorithm for the same computation.
%
%    We are given a nonnegative vector X of dimension DIM_NUM which satisfies:
%
%      sum ( 1 <= I <= DIM_NUM ) LEVEL_WEIGHT(I) * X(I) <= Q_MAX
%
%    This routine computes the appropriate coefficient for X in the
%    anisotropic sparse grid scheme.
%
%    The coefficient is calculated as follows:
%
%      Let B be a binary vector of length N, and let ||B|| represent
%      the sum of the entries of B.
%
%      Coef = sum ( all B such that X+B satisfies constraints ) (-1)^||B||
%
%    Since X+0 satisfies the constraint, there is always at least one
%    summand.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 May 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    An Anisotropic Sparse Grid Stochastic Collocation Method for Partial
%    Differential Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2411-2442.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the vector.
%
%    Input, real LEVEL_WEIGHT(DIM_NUM), the anisotropic weights.
%
%    Input, integer X_MAX(DIM_NUM), the maximum
%    values allowed in each component.
%
%    Input, integer X(DIM_NUM), a point which satisifies the constraints.
%
%    Input, real Q_MAX, the upper limit on the sum.
%
%    Output, real COEF, the combinatorial coefficient.
%

%
%  Force column vectors.
%
  level_weight = level_weight ( : );
  x = x ( : );

  b = zeros ( dim_num, 1 );
  coef = 1.0;

  while ( 1 )
%
%  Generate the next binary perturbation.
%
    b = binary_vector_next ( dim_num, b );
    b_sum = sum ( b(1:dim_num) );
%
%  We're done if we've got back to 0.
%
    if ( b_sum == 0 )
      break
    end
%
%  Perturb the vector.
%
    x2(1:dim_num,1) = x(1:dim_num,1) + b(1:dim_num,1);
%
%  Does it satisfy the XMAX constraint?
%  (THIS CHECK IS SURPRISINGLY NECESSARY, PARTLY BECAUSE OF ZERO WEIGHT).
%
    if ( any ( x_max(1:dim_num) < x2(1:dim_num) ) )
      continue
    end
%
%  Does it satisfy the Q_MAX constraint?
%
    q = level_weight(1:dim_num)' * x2(1:dim_num);

    if ( q <= q_max )
      coef = coef + r8_mop ( b_sum );
    end

  end

  return
end


function [ w, f ] = jed_to_weekday ( jed )

%*****************************************************************************80
%
%% JED_TO_WEEKDAY computes the day of the week from a JED.
%
%  Discussion:
%
%    BC 4713/01/01 => JED = 0.0 was noon on a Monday.
%
%    jedmod = mod ( 0.0D+00, 7.0D+00 ) = 0.0D+00
%    j = mod ( nint ( 0 ), 7 ) = 0
%    f = ( 0.0D+00 + 0.5D+00 ) - real ( j ) = 0.5D+00
%    w = i4_wrap ( 0 + 2, 1, 7 ) = 2 = MONDAY
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 April 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999.
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer W, the day of the week of the date.
%    The days are numbered from Sunday through Saturday, 1 through 7.
%
%    Output, real F, the fractional part of the day.
%
  jedmod = mod ( jed, 7.0 );

  j = mod ( floor ( jedmod ), 7 );

  f = ( jedmod + 0.5 ) - j;

  w = i4_wrap ( j + 2, 1, 7 );

  return
end


% Question No: 7

% Given an image. Implement the split and merge procedure for segmenting
% the image with different values for minimum dimensions of the quadtree
% regions.

function quadtree(x)
f=imread(x);
q=2^nextpow2(max(size(f)));
[m n]=size(f);
f=padarray(f,[q-m,q-n],'post');
mindim=2;
s=qtdecomp(f,@split,mindim,@predicate);
lmax=full(max(s(:)));
g=zeros(size(f));
marker=zeros(size(f));
for k=1:lmax
    [vals,r,c]=qtgetblk(f,s,k);
    if ~isempty(vals)
        for i=1:length(r)
            xlow=r(i);ylow=c(i);
            xhigh=xlow+k-1;
            yhigh=ylow+k-1;
            region=f(xlow:xhigh,ylow:yhigh);
            flag=feval(@predicate,region);
            if flag
                 g(xlow:xhigh,ylow:yhigh)=1;
                 marker(xlow,ylow)=1;
            end
        end
    end
end
g=bwlabel(imreconstruct(marker,g));
g=g(1:m,1:n);
f=f(1:m,1:n);
figure, imshow(f),title('Original Image');
figure, imshow(g),title('Segmented Image');
end

function v=split(b,mindim,fun)
k=size(b,3);
v(1:k)=false;
for i=1:k
    quadrgn=b(:,:,i);
    if size(quadrgn,1)<=mindim
        v(i)=false;
        continue;
    end
    flag=feval(fun,quadrgn);
    if flag
        v(i)=true;
    end
end
end

function flag=predicate(region)
sd=std2(region);
m=mean2(region);
flag=(sd>5)&(m>0)&(m<200);
end


function [R,t,E] = psth(data,sig,plt,T,err,t)
%  function to plot trial averaged rate smoothed by
%  a Gaussian kernel - visual check on stationarity
%  Usage: [R,t,E] = psth(data,sig,plt,T,err,t)
%
%  Inputs:
% Note that all times have to be consistent. If data
% is in seconds, so must be sig and t. If data is in
% samples, so must sig and t. The default is seconds.
% data            structural array of spike times
% sig             std dev of Gaussian (default 50ms)
%                 (minus indicates adaptive with
%                  approx width equal to mod sig)
% plt = 'n'|'r' etc      (default 'r')
% T is the time interval (default all)
% err - 0 = none
%       1 = Poisson
%       2 = Bootstrap over trials (default)
% (both are based on 2* std err rather than 95%)
% t   = times to evaluate psth at
%
% The adaptive estimate works by first estimating
% the psth with a fixed kernel width (-sig) it
% then alters the kernel width so that the number
% of spikes falling under the kernel is the same on
% average but is time dependent.  Reagions rich
% in data therefore have their kernel width reduced
%
% Outputs:
%
% R = rate
% t = times
% E = errors (standard error)

if nargin <1 ; error('I need data!');end
[data]=padNaN(data); % create a zero padded data matrix from input structural array
sz=size(data);
% transposes data so that the longer dimension is assumed to be time
% Procedure used to bring input data into form compatible with that required
% for Murray's routine
if sz(1)>sz(2); data=data';end;
if nargin < 2; sig = 0.05;end
if nargin < 3; plt = 'r';end
if nargin < 5; err = 2; end

if isempty(sig); sig = 0.05;end
if isempty(plt); plt = 'r';end
if isempty(err); err = 2; end

adapt = 0;
if sig < 0; adapt = 1; sig = -sig; end

%  to avoid end effects increase time interval by the width
%  of the kernel (otherwise rate is too low near edges)

if nargin < 4;
  T(1) = min(data(:,1));
  T(2) = max(max(data));
else
  T(1) = T(1)-4*sig;
  T(2) = T(2)+4*sig;
end

% calculate NT and ND and filter out times of interest

NT = length(data(:,1));
if NT < 4 && err == 2
  disp('Using Poisson errorbars as number of trials is too small for bootstrap')
  err = 1;
end

m = 1;
D = zeros(size(data));
ND=zeros(1,NT);
for n=1:NT
  indx = find(~isnan(data(n,:)) & data(n,:)>=T(1) & data(n,:)<=T(2));
  ND(n) = length(indx);
  D(n,1:ND(n)) = data(n,indx);
  m = m + ND(n);
end
N_tot = m;
N_max = max(ND);
D = D(:,1:N_max);

% if the kernel density is such that there are on average
% one or less spikes under the kernel then the units are probably wrong

L = N_tot/(NT*(T(2)-T(1)));
if 2*L*NT*sig < 1 || L < 0.1
  disp('Spikes very low density: are the units right? is the kernel width sensible?')
  disp(['Total events: ' num2str(fix(100*N_tot)/100) ' sig: ' ...
        num2str(fix(1000*sig)) 'ms T: ' num2str(fix(100*T)/100) ' events/sig: ' ...
        num2str(fix(100*N_tot*sig/(T(2)-T(1)))/100)])
end

%    Smear each spike out
%    std dev is sqrt(rate*(integral over kernal^2)/trials)
%    for Gaussian integral over Kernal^2 is 1/(2*sig*srqt(pi))

if nargin < 6
  N_pts =  fix(5*(T(2)-T(1))/sig);
  t = linspace(T(1),T(2),N_pts);
else
  N_pts = length(t);
end

RR = zeros(NT,N_pts);
f = 1/(2*sig^2);
for n=1:NT
  for m=1:ND(n)
    RR(n,:) = RR(n,:) + exp(-f*(t-D(n,m)).^2);
  end
end
RR = RR*(1/sqrt(2*pi*sig^2));
if NT > 1; R = mean(RR); else R = RR;end

if err == 1
  E = sqrt(R/(2*NT*sig*sqrt(pi)));
elseif err == 2
  Nboot = 10;
  mE = 0;
  sE = 0;
  for b=1:Nboot
    indx = floor(NT*rand(1,NT)) + 1;
    mtmp = mean(RR(indx,:));
    mE = mE + mtmp;
    sE = sE + mtmp.^2;
  end
  E = sqrt((sE/Nboot - mE.^2/Nboot^2));
end

% if adaptive warp sig so that on average the number of spikes
% under the kernel is the same but regions where there is
% more data have a smaller kernel

if adapt
  sigt = mean(R)*sig./R;
  RR = zeros(NT,N_pts);
  f = 1./(2*sigt.^2);
  for n=1:NT
    for m=1:ND(n)
      RR(n,:) = RR(n,:) + exp(-f.*(t-D(n,m)).^2);
    end
    RR(n,:) = RR(n,:).*(1./sqrt(2*pi*sigt.^2));
  end
  if NT > 1; R = mean(RR); else R = RR;end

  if err == 1
    E = sqrt(R./(2*NT*sigt*sqrt(pi)));
  elseif err == 2
    Nboot = 10;
    mE = 0;
    sE = 0;
    for b=1:Nboot
      indx = floor(NT*rand(1,NT)) + 1;
      mtmp = mean(RR(indx,:));
      mE = mE + mtmp;
      sE = sE + mtmp.^2;
    end
    E = sqrt((sE/Nboot - mE.^2/Nboot^2));
  end
end

if plt == 'n';return;end
plot(t,R,plt)
hold on
if err > 0
  plot(t,R+2*E,'g')
  plot(t,R-2*E,'g')
end
%axis([T(1)+(4*sig) T(2)-(4*sig) 0 1.25*max(R)])
axis([T(1)+(4*sig) T(2)-(4*sig) 0 max(R+2*E)+10])
xlabel('time (s)')
ylabel('rate (Hz)')
title(['Trial averaged rate : Gaussian Kernel :'  ...
	    ' sigma = ' num2str(1000*sig) 'ms'])
hold off