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

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

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

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

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

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

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

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

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

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

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

% Set BirthIntensity
NumTracks = 3;

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

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

% Instantiate PHD filter
myphd = SMC_PHDFilterX(config);

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

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

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

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

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

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

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

    % Predict PHD filter
    myphd.predict();

    % Update PHD filter
    myphd.update();

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

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

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

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

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


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

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

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

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function b = cppsl ( ap, n, b )

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

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

  return
end


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

end

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

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


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

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

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

    PCTL_RANGE = [16, 84];

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

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

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

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

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

        magMask = mMag_bstsamp >= fMag - 0.05;

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

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

            % Check for minimum number of events

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

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

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

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

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

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

    end % END of FOR fMag

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

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

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

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

end


function x = bernoulli_cdf_inv ( cdf, a )

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

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

  return
end


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

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

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

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

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

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

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

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

% then go for next Gibbs iteration
end

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


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

savemem = true;

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

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

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

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

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

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

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

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

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

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

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


function [indpeak twfPer indtrough] = peakfinder(twf,fmin,fmax)
% [indpeak twfPer indtrough] = peakfinder(twf,<fmin>,<fmax>)
%
% Finds local peaks in a near-periodic waveform. This fails if the
% peaks are closer than 1/2 period. twfPer is the period of twf in
% samples. If the 1st or last peak are less than 90% of the mean of
% the rest of the peaks, they are exluded.
%
% fmin,fmax are constraints on the fundamental frequency and are
% given in units of items per time point (NOT IN Hz!).
%
% $Id: peakfinder.m,v 1.9 2010/05/18 19:31:44 greve Exp $

indpeak = [];

if(nargin < 1 | nargin > 3)
  fprintf('[indpeak twfPer indtrough] = peakfinder(twf,<fmin>,<fmax>)\n');
  return;
end

Ntp = length(twf);
nn = 1:Ntp;

% detrend - necessary?
X = fast_polytrendmtx(1,Ntp,1,3);
%twf = twf - X*(inv(X'*X)*(X'*twf));

% Get major period of waveform
[fftaxis, deltafreq] = fast_fftaxis(Ntp,1);
nfft = length(fftaxis);
nnfft = 1:nfft;
twffft = abs(fft(twf-mean(twf)));
if(exist('fmin','var'))
  indok = find(fftaxis >= fmin & fftaxis <= fmax);
else
  indok = [1:length(fftaxis)];
end
[tmp k] = max(twffft(indok));
twfFreq = fftaxis(indok(k));
twfPer = 1/twfFreq;
twfPerSamp      = round(twfPer);
twfHalfPerSamp  = round(twfPerSamp/2);
twfQuartPerSamp = round(twfPerSamp/4);

%plot(fftaxis,twffft(nnfft))
%keyboard

% Assume global peak is a local peak
[tmp k0] = max(twf);
%fprintf('global peak at %d %f\n',k0,k0/25);

% Look ahead, starting at global peak
indpeak = k0;
kprev = k0;
while(1)
  % Find next max by searhing over time starting at 1/2
  % period beyond the previous max and ending one period
  % later. This fails if the peaks are closer than 1/2
  % period.
  kstart = kprev + twfHalfPerSamp;
  k = kstart + [0:twfPerSamp-1];
  indok = find(k < Ntp);
  if(length(indok) < twfQuartPerSamp) break; end
  k = k(indok);
  [tmp mmax] = max(twf(k));
  kmax = kstart + mmax - 1;
  indpeak = [indpeak kmax];
  kprev = kmax;
end

% Look behind (reverse and look ahead)
twfrev = flipud(twf(:));
k0rev = Ntp - k0 + 1; % DONT = max(twfrev);
indpeakrev = []; % dont include k0rev here
kprev = k0rev;
while(1)
  kstart = kprev + twfHalfPerSamp;
  k = kstart + [0:twfPerSamp-1];
  indok = find(k < Ntp);
  if(length(indok) < twfQuartPerSamp) break; end
  k = k(indok);
  [tmp mmax] = max(twfrev(k));
  kmax = kstart + mmax - 1;
  indpeakrev = [indpeakrev kmax];
  kprev = kmax;
end

% Convert reversed indices to forard indices
indpeakrevfor = Ntp - indpeakrev + 1;
indpeak = sort([indpeak indpeakrevfor]);
npeaks = length(indpeak);

% Decide whether to eliminate the first peak
% Compute mean of closest 3 peaks
peakfirst = twf(indpeak(1));
indpm = [2:min(4,npeaks)];
peakmean = mean(twf(indpeak(indpm)));
% Must be greater than 0.7 times this mean
if(peakfirst < .7*peakmean)  indpeak = indpeak(2:end); end

% Decide whether to eliminate the last peak
% Compute mean of closest 3 peaks
peaklast  = twf(indpeak(end));
indpm = [max(npeaks-3,1),max(npeaks-1,1)];
peakmean = mean(twf(indpeak(indpm)));
% Must be greater than 0.7 times this mean
if(peaklast  < .7*peakmean)  indpeak = indpeak(1:end-1); end

% Make sure they are unique (why not always?)
indpeak = unique(indpeak);

npeaks = length(indpeak);
indtrough = zeros(size(indpeak));
for nthpeak = 1:npeaks
  i1 = indpeak(nthpeak);
  if(nthpeak < npeaks)
    i2 = indpeak(nthpeak+1);
  else
    i2 = Ntp;
  end
  if(i2 > i1 + 1) i1 = i1 + 1; end
  [mmin imin] = min(twf(i1:i2));
  indtrough(nthpeak) = imin+i1-1;
end

if(0)
nn = 1:Ntp;
plot(nn,twf,nn(indpeak),twf(indpeak),'*',nn(indtrough),twf(indtrough),'o');
fprintf('Period %f\n',twfPerSamp);
keyboard
end

return;


function sr=comp_gabreassign(s,tgrad,fgrad,a);
%COMP_GABREASSIGN  Reassign time-frequency distribution.
%   Usage:  sr = comp_gabreassign(s,tgrad,fgrad,a);
%
%   `comp_gabreassign(s,tgrad,fgrad,a)` will reassign the values of the positive
%   time-frequency distribution *s* using the instantaneous time and frequency
%   *fgrad* and *ifdummy*. The lattice is determined by the time shift *a* and
%   the number of channels deduced from the size of *s*.
%
%   See also: gabreassign
%
%   References: aufl95

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

[M,N,W]=size(s);
L=N*a;
b=L/M;

freqpos=fftindex(M);
tgrad=bsxfun(@plus,tgrad/b,freqpos);

timepos=fftindex(N);
fgrad=bsxfun(@plus,fgrad/a,timepos.');

tgrad=round(tgrad);
fgrad=round(fgrad);

tgrad=mod(tgrad,M);
fgrad=mod(fgrad,N);

sr=zeros(M,N,W,assert_classname(s,tgrad,fgrad));

fgrad=fgrad+1;
tgrad=tgrad+1;

for w=1:W
    for ii=1:M
        for jj=1:N
            sr(tgrad(ii,jj),fgrad(ii,jj),w) = sr(tgrad(ii,jj),fgrad(ii,jj),w)+s(ii,jj,w);
        end;
    end;
end;


function y = perform_curvelet_transform(x,options)

% perform_curvelet_transform - a wrapper to curvlab
%
%     M = perform_curvelet_transform(MW,options);
%
%   Forward and backward curvelet transform
%   You must provide options.n (width of the image).
%
%   Visit www.curvelab.org for the full code.

options.null = 0;
finest = getoptions(options, 'finest',1)  ; % =1(curv)  =2(wav)
nbscales = getoptions(options, 'nbscales', 5); % log2(size(M,1))-2;
nbangles_coarse = getoptions(options, 'nbangles_coarse', 16);
is_real = getoptions(options, 'is_real', 0);
n = getoptions(options, 'n', 1,1);

if not(iscell(x))
    % fwd transform
    y = fdct_wrapping(x, is_real, finest, nbscales, nbangles_coarse);
else
    y = real( ifdct_wrapping(x, is_real, n,n ) );
end

%%

function C = fdct_wrapping(x, is_real, finest, nbscales, nbangles_coarse)

% fdct_wrapping.m - Fast Discrete Curvelet Transform via wedge wrapping - Version 1.0
%
% Inputs
%   x           M-by-N matrix
%
% Optional Inputs
%   is_real     Type of the transform
%                   0: complex-valued curvelets
%                   1: real-valued curvelets
%               [default set to 0]
%   finest      Chooses one of two possibilities for the coefficients at the
%               finest level:
%                   1: curvelets
%                   2: wavelets
%               [default set to 2]
%   nbscales    number of scales including the coarsest wavelet level
%               [default set to ceil(log2(min(M,N)) - 3)]
%   nbangles_coarse
%               number of angles at the 2nd coarsest level, minimum 8,
%               must be a multiple of 4. [default set to 16]
%
% Outputs
%   C           Cell array of curvelet coefficients.
%               C{j}{l}(k1,k2) is the coefficient at
%                   - scale j: integer, from finest to coarsest scale,
%                   - angle l: integer, starts at the top-left corner and
%                   increases clockwise,
%                   - position k1,k2: both integers, size varies with j
%                   and l.
%               If is_real is 1, there are two types of curvelets,
%               'cosine' and 'sine'. For a given scale j, the 'cosine'
%               coefficients are stored in the first two quadrants (low
%               values of l), the 'sine' coefficients in the last two
%               quadrants (high values of l).
%
% See also ifdct_wrapping.m, fdct_wrapping_param.m
%
% By Laurent Demanet, 2004

X = fftshift(fft2(ifftshift(x)))/sqrt(prod(size(x)));
[N1,N2] = size(X);
if nargin < 2, is_real = 0; end;
if nargin < 3, finest = 2; end;
if nargin < 4, nbscales = ceil(log2(min(N1,N2)) - 3); end;
if nargin < 5, nbangles_coarse = 16; end;

% Initialization: data structure
nbangles = [1, nbangles_coarse .* 2.^(ceil((nbscales-(nbscales:-1:2))/2))];
if finest == 2, nbangles(nbscales) = 1; end;
C = cell(1,nbscales);
for j = 1:nbscales
    C{j} = cell(1,nbangles(j));
end;

% Loop: pyramidal scale decomposition
M1 = N1/3;
M2 = N2/3;
if finest == 1,

    % Initialization: smooth periodic extension of high frequencies
    bigN1 = 2*floor(2*M1)+1;
    bigN2 = 2*floor(2*M2)+1;
    equiv_index_1 = 1+mod(floor(N1/2)-floor(2*M1)+(1:bigN1)-1,N1);
    equiv_index_2 = 1+mod(floor(N2/2)-floor(2*M2)+(1:bigN2)-1,N2);
    X = X(equiv_index_1,equiv_index_2);
        % Invariant: equiv_index_1(floor(2*M1)+1) == (N1 + 2 - mod(N1,2))/2
        % is the center in frequency. Same for M2, N2.
    window_length_1 = floor(2*M1) - floor(M1) - 1 - (mod(N1,3)==0);
    window_length_2 = floor(2*M2) - floor(M2) - 1 - (mod(N2,3)==0);
        % Invariant: floor(M1) + floor(2*M1) == N1 - (mod(M1,3)~=0)
        % Same for M2, N2.
    coord_1 = 0:(1/window_length_1):1;
    coord_2 = 0:(1/window_length_2):1;
    [wl_1,wr_1] = fdct_wrapping_window(coord_1);
    [wl_2,wr_2] = fdct_wrapping_window(coord_2);
    lowpass_1 = [wl_1, ones(1,2*floor(M1)+1), wr_1];
    if mod(N1,3)==0, lowpass_1 = [0, lowpass_1, 0]; end;
    lowpass_2 = [wl_2, ones(1,2*floor(M2)+1), wr_2];
    if mod(N2,3)==0, lowpass_2 = [0, lowpass_2, 0]; end;
    lowpass = lowpass_1'*lowpass_2;
    Xlow = X .* lowpass;

    scales = nbscales:-1:2;

else

    M1 = M1/2;
    M2 = M2/2;
    window_length_1 = floor(2*M1) - floor(M1) - 1;
    window_length_2 = floor(2*M2) - floor(M2) - 1;
    coord_1 = 0:(1/window_length_1):1;
    coord_2 = 0:(1/window_length_2):1;
    [wl_1,wr_1] = fdct_wrapping_window(coord_1);
    [wl_2,wr_2] = fdct_wrapping_window(coord_2);
    lowpass_1 = [wl_1, ones(1,2*floor(M1)+1), wr_1];
    lowpass_2 = [wl_2, ones(1,2*floor(M2)+1), wr_2];
    lowpass = lowpass_1'*lowpass_2;
    hipass = sqrt(1 - lowpass.^2);
    Xlow_index_1 = ((-floor(2*M1)):floor(2*M1)) + ceil((N1+1)/2);
    Xlow_index_2 = ((-floor(2*M2)):floor(2*M2)) + ceil((N2+1)/2);
    Xlow = X(Xlow_index_1, Xlow_index_2) .* lowpass;
    Xhi = X;
    Xhi(Xlow_index_1, Xlow_index_2) = Xhi(Xlow_index_1, Xlow_index_2) .* hipass;
    C{nbscales}{1} = fftshift(ifft2(ifftshift(Xhi)))*sqrt(prod(size(Xhi)));
    if is_real, C{nbscales}{1} = real(C{nbscales}{1}); end;

    scales = (nbscales-1):-1:2;

end;
for j = scales,

    M1 = M1/2;
    M2 = M2/2;
    window_length_1 = floor(2*M1) - floor(M1) - 1;
    window_length_2 = floor(2*M2) - floor(M2) - 1;
    coord_1 = 0:(1/window_length_1):1;
    coord_2 = 0:(1/window_length_2):1;
    [wl_1,wr_1] = fdct_wrapping_window(coord_1);
    [wl_2,wr_2] = fdct_wrapping_window(coord_2);
    lowpass_1 = [wl_1, ones(1,2*floor(M1)+1), wr_1];
    lowpass_2 = [wl_2, ones(1,2*floor(M2)+1), wr_2];
    lowpass = lowpass_1'*lowpass_2;
    hipass = sqrt(1 - lowpass.^2);
    Xhi = Xlow;                 % size is 2*floor(4*M1)+1 - by - 2*floor(4*M2)+1
    Xlow_index_1 = ((-floor(2*M1)):floor(2*M1)) + floor(4*M1) + 1;
    Xlow_index_2 = ((-floor(2*M2)):floor(2*M2)) + floor(4*M2) + 1;
    Xlow = Xlow(Xlow_index_1, Xlow_index_2);
    Xhi(Xlow_index_1, Xlow_index_2) = Xlow .* hipass;
    Xlow = Xlow .* lowpass;     % size is 2*floor(2*M1)+1 - by - 2*floor(2*M2)+1

    % Loop: angular decomposition
    l = 0;
    nbquadrants = 2 + 2*(~is_real);
    nbangles_perquad = nbangles(j)/4;
    for quadrant = 1:nbquadrants
        M_horiz = M2 * (mod(quadrant,2)==1) + M1 * (mod(quadrant,2)==0);
        M_vert = M1 * (mod(quadrant,2)==1) + M2 * (mod(quadrant,2)==0);
        if mod(nbangles_perquad,2),
            wedge_ticks_left = round((0:(1/(2*nbangles_perquad)):.5)*2*floor(4*M_horiz) + 1);
            wedge_ticks_right = 2*floor(4*M_horiz) + 2 - wedge_ticks_left;
            wedge_ticks = [wedge_ticks_left, wedge_ticks_right(end:-1:1)];
        else
            wedge_ticks_left = round((0:(1/(2*nbangles_perquad)):.5)*2*floor(4*M_horiz) + 1);
            wedge_ticks_right = 2*floor(4*M_horiz) + 2 - wedge_ticks_left;
            wedge_ticks = [wedge_ticks_left, wedge_ticks_right((end-1):-1:1)];
        end;
        wedge_endpoints = wedge_ticks(2:2:(end-1));         % integers
        wedge_midpoints = (wedge_endpoints(1:(end-1)) + wedge_endpoints(2:end))/2;
                % integers or half-integers

        % Left corner wedge
        l = l+1;
        first_wedge_endpoint_vert = round(2*floor(4*M_vert)/(2*nbangles_perquad) + 1);
        length_corner_wedge = floor(4*M_vert) - floor(M_vert) + ceil(first_wedge_endpoint_vert/4);
        Y_corner = 1:length_corner_wedge;
        [XX,YY] = meshgrid(1:(2*floor(4*M_horiz)+1),Y_corner);
        width_wedge = wedge_endpoints(2) + wedge_endpoints(1) - 1;
        slope_wedge = (floor(4*M_horiz) + 1 - wedge_endpoints(1))/floor(4*M_vert);
        left_line = round(2 - wedge_endpoints(1) + slope_wedge*(Y_corner - 1));
                                                            % integers
        [wrapped_data, wrapped_XX, wrapped_YY] = deal(zeros(length_corner_wedge,width_wedge));
        first_row = floor(4*M_vert)+2-ceil((length_corner_wedge+1)/2)+...
            mod(length_corner_wedge+1,2)*(quadrant-2 == mod(quadrant-2,2));
        first_col = floor(4*M_horiz)+2-ceil((width_wedge+1)/2)+...
            mod(width_wedge+1,2)*(quadrant-3 == mod(quadrant-3,2));
                % Coordinates of the top-left corner of the wedge wrapped
                % around the origin. Some subtleties when the wedge is
                % even-sized because of the forthcoming 90 degrees rotation
        for row = Y_corner
            cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
            admissible_cols = round(1/2*(cols+1+abs(cols-1)));
            new_row = 1 + mod(row - first_row, length_corner_wedge);
            wrapped_data(new_row,:) = Xhi(row,admissible_cols) .* (cols > 0);
            wrapped_XX(new_row,:) = XX(row,admissible_cols);
            wrapped_YY(new_row,:) = YY(row,admissible_cols);
        end;
        slope_wedge_right = (floor(4*M_horiz)+1 - wedge_midpoints(1))/floor(4*M_vert);
        mid_line_right = wedge_midpoints(1) + slope_wedge_right*(wrapped_YY - 1);
                % not integers in general
        coord_right = 1/2 + floor(4*M_vert)/(wedge_endpoints(2) - wedge_endpoints(1)) * ...
            (wrapped_XX - mid_line_right)./(floor(4*M_vert)+1 - wrapped_YY);
        C2 = 1/(1/(2*(floor(4*M_horiz))/(wedge_endpoints(1) - 1) - 1) + 1/(2*(floor(4*M_vert))/(first_wedge_endpoint_vert - 1) - 1));
        C1 = C2 / (2*(floor(4*M_vert))/(first_wedge_endpoint_vert - 1) - 1);
        wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) + (wrapped_YY-1)/floor(4*M_vert) == 2) = ...
            wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) + (wrapped_YY-1)/floor(4*M_vert) == 2) + 1;
        coord_corner = C1 + C2 * ((wrapped_XX - 1)/(floor(4*M_horiz)) - (wrapped_YY - 1)/(floor(4*M_vert))) ./ ...
            (2-((wrapped_XX - 1)/(floor(4*M_horiz)) + (wrapped_YY - 1)/(floor(4*M_vert))));
        wl_left = fdct_wrapping_window(coord_corner);
        [wl_right,wr_right] = fdct_wrapping_window(coord_right);
        wrapped_data = wrapped_data .* (wl_left .* wr_right);

        switch is_real
            case 0
                wrapped_data = rot90(wrapped_data,-(quadrant-1));
                C{j}{l} = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
            case 1
                wrapped_data = rot90(wrapped_data,-(quadrant-1));
                x = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
                C{j}{l} = sqrt(2)*real(x);
                C{j}{l+nbangles(j)/2} = sqrt(2)*imag(x);
        end;

        % Regular wedges
        length_wedge = floor(4*M_vert) - floor(M_vert);
        Y = 1:length_wedge;
        first_row = floor(4*M_vert)+2-ceil((length_wedge+1)/2)+...
            mod(length_wedge+1,2)*(quadrant-2 == mod(quadrant-2,2));
        for subl = 2:(nbangles_perquad-1);
            l = l+1;
            width_wedge = wedge_endpoints(subl+1) - wedge_endpoints(subl-1) + 1;
            slope_wedge = ((floor(4*M_horiz)+1) - wedge_endpoints(subl))/floor(4*M_vert);
            left_line = round(wedge_endpoints(subl-1) + slope_wedge*(Y - 1));
            [wrapped_data, wrapped_XX, wrapped_YY] = deal(zeros(length_wedge,width_wedge));
            first_col = floor(4*M_horiz)+2-ceil((width_wedge+1)/2)+...
                mod(width_wedge+1,2)*(quadrant-3 == mod(quadrant-3,2));
            for row = Y
                cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
                new_row = 1 + mod(row - first_row, length_wedge);
                wrapped_data(new_row,:) = Xhi(row,cols);
                wrapped_XX(new_row,:) = XX(row,cols);
                wrapped_YY(new_row,:) = YY(row,cols);
            end;
            slope_wedge_left = ((floor(4*M_horiz)+1) - wedge_midpoints(subl-1))/floor(4*M_vert);
            mid_line_left = wedge_midpoints(subl-1) + slope_wedge_left*(wrapped_YY - 1);
            coord_left = 1/2 + floor(4*M_vert)/(wedge_endpoints(subl) - wedge_endpoints(subl-1)) * ...
                (wrapped_XX - mid_line_left)./(floor(4*M_vert)+1 - wrapped_YY);
            slope_wedge_right = ((floor(4*M_horiz)+1) - wedge_midpoints(subl))/floor(4*M_vert);
            mid_line_right = wedge_midpoints(subl) + slope_wedge_right*(wrapped_YY - 1);
            coord_right = 1/2 + floor(4*M_vert)/(wedge_endpoints(subl+1) - wedge_endpoints(subl)) * ...
                (wrapped_XX - mid_line_right)./(floor(4*M_vert)+1 - wrapped_YY);
            wl_left = fdct_wrapping_window(coord_left);
            [wl_right,wr_right] = fdct_wrapping_window(coord_right);
            wrapped_data = wrapped_data .* (wl_left .* wr_right);
            switch is_real
                case 0
                    wrapped_data = rot90(wrapped_data,-(quadrant-1));
                    C{j}{l} = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
                case 1
                    wrapped_data = rot90(wrapped_data,-(quadrant-1));
                    x = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
                    C{j}{l} = sqrt(2)*real(x);
                    C{j}{l+nbangles(j)/2} = sqrt(2)*imag(x);
            end;
        end;

        % Right corner wedge
        l = l+1;
        width_wedge = 4*floor(4*M_horiz) + 3 - wedge_endpoints(end) - wedge_endpoints(end-1);
        slope_wedge = ((floor(4*M_horiz)+1) - wedge_endpoints(end))/floor(4*M_vert);
        left_line = round(wedge_endpoints(end-1) + slope_wedge*(Y_corner - 1));
        [wrapped_data, wrapped_XX, wrapped_YY] = deal(zeros(length_corner_wedge,width_wedge));
        first_row = floor(4*M_vert)+2-ceil((length_corner_wedge+1)/2)+...
            mod(length_corner_wedge+1,2)*(quadrant-2 == mod(quadrant-2,2));
        first_col = floor(4*M_horiz)+2-ceil((width_wedge+1)/2)+...
            mod(width_wedge+1,2)*(quadrant-3 == mod(quadrant-3,2));
        for row = Y_corner
            cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
            admissible_cols = round(1/2*(cols+2*floor(4*M_horiz)+1-abs(cols-(2*floor(4*M_horiz)+1))));
            new_row = 1 + mod(row - first_row, length_corner_wedge);
            wrapped_data(new_row,:) = Xhi(row,admissible_cols) .* (cols <= (2*floor(4*M_horiz)+1));
            wrapped_XX(new_row,:) = XX(row,admissible_cols);
            wrapped_YY(new_row,:) = YY(row,admissible_cols);
        end;
        slope_wedge_left = ((floor(4*M_horiz)+1) - wedge_midpoints(end))/floor(4*M_vert);
        mid_line_left = wedge_midpoints(end) + slope_wedge_left*(wrapped_YY - 1);
        coord_left = 1/2 + floor(4*M_vert)/(wedge_endpoints(end) - wedge_endpoints(end-1)) * ...
            (wrapped_XX - mid_line_left)./(floor(4*M_vert) + 1 - wrapped_YY);
        C2 = -1/(2*(floor(4*M_horiz))/(wedge_endpoints(end) - 1) - 1 + 1/(2*(floor(4*M_vert))/(first_wedge_endpoint_vert - 1) - 1));
        C1 = -C2 * (2*(floor(4*M_horiz))/(wedge_endpoints(end) - 1) - 1);
        wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) == (wrapped_YY - 1)/floor(4*M_vert)) = ...
            wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) == (wrapped_YY - 1)/floor(4*M_vert)) - 1;
        coord_corner = C1 + C2 * (2-((wrapped_XX - 1)/(floor(4*M_horiz)) + (wrapped_YY - 1)/(floor(4*M_vert)))) ./ ...
            ((wrapped_XX - 1)/(floor(4*M_horiz)) - (wrapped_YY - 1)/(floor(4*M_vert)));
        wl_left = fdct_wrapping_window(coord_left);
        [wl_right,wr_right] = fdct_wrapping_window(coord_corner);

        wrapped_data = wrapped_data .* (wl_left .* wr_right);
        switch is_real
            case 0
                wrapped_data = rot90(wrapped_data,-(quadrant-1));
                C{j}{l} = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
            case 1
                wrapped_data = rot90(wrapped_data,-(quadrant-1));
                x = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
                C{j}{l} = sqrt(2)*real(x);
                C{j}{l+nbangles(j)/2} = sqrt(2)*imag(x);
        end;

        if quadrant < nbquadrants, Xhi = rot90(Xhi); end;
    end;
end;

% Coarsest wavelet level
C{1}{1} = fftshift(ifft2(ifftshift(Xlow)))*sqrt(prod(size(Xlow)));
if is_real == 1,
    C{1}{1} = real(C{1}{1});
end;

function x = ifdct_wrapping(C, is_real, M, N)

% ifdct_wrapping.m - Inverse Fast Discrete Curvelet Transform via wedge wrapping - Version 1.0
% This is in fact the adjoint, also the pseudo-inverse
%
% Inputs
%   C           Cell array containing curvelet coefficients (see
%               description in fdct_wrapping.m)
%   is_real     As used in fdct_wrapping.m
%   M, N        Size of the image to be recovered (not necessary if finest
%               = 2)
%
% Outputs
%   x           M-by-N matrix
%
% See also fdct_wrapping.m
%
% By Laurent Demanet, 2004

% Initialization
nbscales = length(C);
nbangles_coarse = length(C{2});
nbangles = [1, nbangles_coarse .* 2.^(ceil((nbscales-(nbscales:-1:2))/2))];
if length(C{end}) == 1, finest = 2; else finest = 1; end;
if finest == 2, nbangles(nbscales) = 1; end;
if nargin < 2, is_real = 0; end;
if nargin < 4,
    if finest == 1, error('Syntax: IFCT_wrapping(C,M,N) where the matrix to be recovered is M-by-N'); end;
    [N1,N2] = size(C{end}{1});
else
    N1 = M;
    N2 = N;
end;

M1 = N1/3;
M2 = N2/3;

if finest == 1;

    bigN1 = 2*floor(2*M1)+1;
    bigN2 = 2*floor(2*M2)+1;
    X = zeros(bigN1,bigN2);

    % Initialization: preparing the lowpass filter at finest scale
    window_length_1 = floor(2*M1) - floor(M1) - 1 - (mod(N1,3)==0);
    window_length_2 = floor(2*M2) - floor(M2) - 1 - (mod(N2,3)==0);
    coord_1 = 0:(1/window_length_1):1;
    coord_2 = 0:(1/window_length_2):1;
    [wl_1,wr_1] = fdct_wrapping_window(coord_1);
    [wl_2,wr_2] = fdct_wrapping_window(coord_2);
    lowpass_1 = [wl_1, ones(1,2*floor(M1)+1), wr_1];
    if mod(N1,3)==0, lowpass_1 = [0, lowpass_1, 0]; end;
    lowpass_2 = [wl_2, ones(1,2*floor(M2)+1), wr_2];
    if mod(N2,3)==0, lowpass_2 = [0, lowpass_2, 0]; end;
    lowpass = lowpass_1'*lowpass_2;

    scales = nbscales:-1:2;

else

    M1 = M1/2;
    M2 = M2/2;

    bigN1 = 2*floor(2*M1)+1;
    bigN2 = 2*floor(2*M2)+1;
    X = zeros(bigN1,bigN2);

    window_length_1 = floor(2*M1) - floor(M1) - 1;
    window_length_2 = floor(2*M2) - floor(M2) - 1;
    coord_1 = 0:(1/window_length_1):1;
    coord_2 = 0:(1/window_length_2):1;
    [wl_1,wr_1] = fdct_wrapping_window(coord_1);
    [wl_2,wr_2] = fdct_wrapping_window(coord_2);
    lowpass_1 = [wl_1, ones(1,2*floor(M1)+1), wr_1];
    lowpass_2 = [wl_2, ones(1,2*floor(M2)+1), wr_2];
    lowpass = lowpass_1'*lowpass_2;
    hipass_finest = sqrt(1 - lowpass.^2);

    scales = (nbscales-1):-1:2;

end;

% Loop: pyramidal reconstruction

Xj_topleft_1 = 1;
Xj_topleft_2 = 1;
for j = scales,

    M1 = M1/2;
    M2 = M2/2;
    window_length_1 = floor(2*M1) - floor(M1) - 1;
    window_length_2 = floor(2*M2) - floor(M2) - 1;
    coord_1 = 0:(1/window_length_1):1;
    coord_2 = 0:(1/window_length_2):1;
    [wl_1,wr_1] = fdct_wrapping_window(coord_1);
    [wl_2,wr_2] = fdct_wrapping_window(coord_2);
    lowpass_1 = [wl_1, ones(1,2*floor(M1)+1), wr_1];
    lowpass_2 = [wl_2, ones(1,2*floor(M2)+1), wr_2];
    lowpass_next = lowpass_1'*lowpass_2;
    hipass = sqrt(1 - lowpass_next.^2);
    Xj = zeros(2*floor(4*M1)+1,2*floor(4*M2)+1);

    % Loop: angles
    l = 0;
    nbquadrants = 2 + 2*(~is_real);
    nbangles_perquad = nbangles(j)/4;
    for quadrant = 1:nbquadrants

        M_horiz = M2 * (mod(quadrant,2)==1) + M1 * (mod(quadrant,2)==0);
        M_vert = M1 * (mod(quadrant,2)==1) + M2 * (mod(quadrant,2)==0);
        if mod(nbangles_perquad,2),
            wedge_ticks_left = round((0:(1/(2*nbangles_perquad)):.5)*2*floor(4*M_horiz) + 1);
            wedge_ticks_right = 2*floor(4*M_horiz) + 2 - wedge_ticks_left;
            wedge_ticks = [wedge_ticks_left, wedge_ticks_right(end:-1:1)];
        else
            wedge_ticks_left = round((0:(1/(2*nbangles_perquad)):.5)*2*floor(4*M_horiz) + 1);
            wedge_ticks_right = 2*floor(4*M_horiz) + 2 - wedge_ticks_left;
            wedge_ticks = [wedge_ticks_left, wedge_ticks_right((end-1):-1:1)];
        end;
        wedge_endpoints = wedge_ticks(2:2:(end-1));         % integers
        wedge_midpoints = (wedge_endpoints(1:(end-1)) + wedge_endpoints(2:end))/2;

        % Left corner wedge

        l = l+1;
        first_wedge_endpoint_vert = round(2*floor(4*M_vert)/(2*nbangles_perquad) + 1);
        length_corner_wedge = floor(4*M_vert) - floor(M_vert) + ceil(first_wedge_endpoint_vert/4);
        Y_corner = 1:length_corner_wedge;
        [XX,YY] = meshgrid(1:(2*floor(4*M_horiz)+1),Y_corner);
        width_wedge = wedge_endpoints(2) + wedge_endpoints(1) - 1;
        slope_wedge = (floor(4*M_horiz) + 1 - wedge_endpoints(1))/floor(4*M_vert);
        left_line = round(2 - wedge_endpoints(1) + slope_wedge*(Y_corner - 1));
        [wrapped_XX, wrapped_YY] = deal(zeros(length_corner_wedge,width_wedge));
        first_row = floor(4*M_vert)+2-ceil((length_corner_wedge+1)/2)+...
            mod(length_corner_wedge+1,2)*(quadrant-2 == mod(quadrant-2,2));
        first_col = floor(4*M_horiz)+2-ceil((width_wedge+1)/2)+...
            mod(width_wedge+1,2)*(quadrant-3 == mod(quadrant-3,2));

        for row = Y_corner
            cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
            new_row = 1 + mod(row - first_row, length_corner_wedge);
            admissible_cols = round(1/2*(cols+1+abs(cols-1)));
            wrapped_XX(new_row,:) = XX(row,admissible_cols);
            wrapped_YY(new_row,:) = YY(row,admissible_cols);
        end;

        slope_wedge_right = (floor(4*M_horiz)+1 - wedge_midpoints(1))/floor(4*M_vert);
        mid_line_right = wedge_midpoints(1) + slope_wedge_right*(wrapped_YY - 1);
                                                            % not integers
                                                            % in general
        coord_right = 1/2 + floor(4*M_vert)/(wedge_endpoints(2) - wedge_endpoints(1)) * ...
            (wrapped_XX - mid_line_right)./(floor(4*M_vert)+1 - wrapped_YY);
        C2 = 1/(1/(2*(floor(4*M_horiz))/(wedge_endpoints(1) - 1) - 1) + 1/(2*(floor(4*M_vert))/(first_wedge_endpoint_vert - 1) - 1));
        C1 = C2 / (2*(floor(4*M_vert))/(first_wedge_endpoint_vert - 1) - 1);
        wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) + (wrapped_YY-1)/floor(4*M_vert) == 2) = ...
            wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) + (wrapped_YY-1)/floor(4*M_vert) == 2) + 1;
        coord_corner = C1 + C2 * ((wrapped_XX - 1)/(floor(4*M_horiz)) - (wrapped_YY - 1)/(floor(4*M_vert))) ./ ...
            (2-((wrapped_XX - 1)/(floor(4*M_horiz)) + (wrapped_YY - 1)/(floor(4*M_vert))));
        wl_left = fdct_wrapping_window(coord_corner);
        [wl_right,wr_right] = fdct_wrapping_window(coord_right);
        switch is_real
         case 0
          wrapped_data = fftshift(fft2(ifftshift(C{j}{l})))/sqrt(prod(size(C{j}{l})));
          wrapped_data = rot90(wrapped_data,(quadrant-1));
         case 1
          x = C{j}{l} + sqrt(-1)*C{j}{l+nbangles(j)/2};
          wrapped_data = fftshift(fft2(ifftshift(x)))/sqrt(prod(size(x)))/sqrt(2);
          wrapped_data = rot90(wrapped_data,(quadrant-1));
        end;
        wrapped_data = wrapped_data .* (wl_left .* wr_right);

        % Unwrapping data
        for row = Y_corner
            cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
            admissible_cols = round(1/2*(cols+1+abs(cols-1)));
            new_row = 1 + mod(row - first_row, length_corner_wedge);
            Xj(row,admissible_cols) = Xj(row,admissible_cols) + wrapped_data(new_row,:);
                                % We use the following property: in an assignment
                                % A(B) = C where B and C are vectors, if
                                % some value x repeats in B, then the
                                % last occurrence of x is the one
                                % corresponding to the eventual assignment.
        end;

        % Regular wedges
        length_wedge = floor(4*M_vert) - floor(M_vert);
        Y = 1:length_wedge;
        first_row = floor(4*M_vert)+2-ceil((length_wedge+1)/2)+...
            mod(length_wedge+1,2)*(quadrant-2 == mod(quadrant-2,2));
        for subl = 2:(nbangles_perquad-1);
            l = l+1;
            width_wedge = wedge_endpoints(subl+1) - wedge_endpoints(subl-1) + 1;
            slope_wedge = ((floor(4*M_horiz)+1) - wedge_endpoints(subl))/floor(4*M_vert);
            left_line = round(wedge_endpoints(subl-1) + slope_wedge*(Y - 1));
            [wrapped_XX, wrapped_YY] = deal(zeros(length_wedge,width_wedge));
            first_col = floor(4*M_horiz)+2-ceil((width_wedge+1)/2)+...
                mod(width_wedge+1,2)*(quadrant-3 == mod(quadrant-3,2));
            for row = Y
                cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
                new_row = 1 + mod(row - first_row, length_wedge);
                wrapped_XX(new_row,:) = XX(row,cols);
                wrapped_YY(new_row,:) = YY(row,cols);
            end;
            slope_wedge_left = ((floor(4*M_horiz)+1) - wedge_midpoints(subl-1))/floor(4*M_vert);
            mid_line_left = wedge_midpoints(subl-1) + slope_wedge_left*(wrapped_YY - 1);
            coord_left = 1/2 + floor(4*M_vert)/(wedge_endpoints(subl) - wedge_endpoints(subl-1)) * ...
                (wrapped_XX - mid_line_left)./(floor(4*M_vert)+1 - wrapped_YY);
            slope_wedge_right = ((floor(4*M_horiz)+1) - wedge_midpoints(subl))/floor(4*M_vert);
            mid_line_right = wedge_midpoints(subl) + slope_wedge_right*(wrapped_YY - 1);
            coord_right = 1/2 + floor(4*M_vert)/(wedge_endpoints(subl+1) - wedge_endpoints(subl)) * ...
                (wrapped_XX - mid_line_right)./(floor(4*M_vert)+1 - wrapped_YY);
            wl_left = fdct_wrapping_window(coord_left);
            [wl_right,wr_right] = fdct_wrapping_window(coord_right);
            switch is_real
             case 0
              wrapped_data = fftshift(fft2(ifftshift(C{j}{l})))/sqrt(prod(size(C{j}{l})));
              wrapped_data = rot90(wrapped_data,(quadrant-1));
             case 1
              x = C{j}{l} + sqrt(-1)*C{j}{l+nbangles(j)/2};
              wrapped_data = fftshift(fft2(ifftshift(x)))/sqrt(prod(size(x)))/sqrt(2);
              wrapped_data = rot90(wrapped_data,(quadrant-1));
            end;
            wrapped_data = wrapped_data .* (wl_left .* wr_right);

            % Unwrapping data
            for row = Y
                cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
                new_row = 1 + mod(row - first_row, length_wedge);
                Xj(row,cols) = Xj(row,cols) + wrapped_data(new_row,:);
            end;

        end;    % for subl

        % Right corner wedge
        l = l+1;
        width_wedge = 4*floor(4*M_horiz) + 3 - wedge_endpoints(end) - wedge_endpoints(end-1);
        slope_wedge = ((floor(4*M_horiz)+1) - wedge_endpoints(end))/floor(4*M_vert);
        left_line = round(wedge_endpoints(end-1) + slope_wedge*(Y_corner - 1));
        [wrapped_XX, wrapped_YY] = deal(zeros(length_corner_wedge,width_wedge));
        first_row = floor(4*M_vert)+2-ceil((length_corner_wedge+1)/2)+...
            mod(length_corner_wedge+1,2)*(quadrant-2 == mod(quadrant-2,2));
        first_col = floor(4*M_horiz)+2-ceil((width_wedge+1)/2)+...
            mod(width_wedge+1,2)*(quadrant-3 == mod(quadrant-3,2));

        for row = Y_corner
            cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
            admissible_cols = round(1/2*(cols+2*floor(4*M_horiz)+1-abs(cols-(2*floor(4*M_horiz)+1))));
            new_row = 1 + mod(row - first_row, length_corner_wedge);
            wrapped_XX(new_row,:) = XX(row,admissible_cols);
            wrapped_YY(new_row,:) = YY(row,admissible_cols);
        end;
        YY = Y_corner'*ones(1,width_wedge);
        slope_wedge_left = ((floor(4*M_horiz)+1) - wedge_midpoints(end))/floor(4*M_vert);
        mid_line_left = wedge_midpoints(end) + slope_wedge_left*(wrapped_YY - 1);
        coord_left = 1/2 + floor(4*M_vert)/(wedge_endpoints(end) - wedge_endpoints(end-1)) * ...
            (wrapped_XX - mid_line_left)./(floor(4*M_vert)+1 - wrapped_YY);
        C2 = -1/(2*(floor(4*M_horiz))/(wedge_endpoints(end) - 1) - 1 + 1/(2*(floor(4*M_vert))/(first_wedge_endpoint_vert - 1) - 1));
        C1 = -C2 * (2*(floor(4*M_horiz))/(wedge_endpoints(end) - 1) - 1);
        wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) == (wrapped_YY-1)/floor(4*M_vert)) = ...
            wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) == (wrapped_YY-1)/floor(4*M_vert)) - 1;
        coord_corner = C1 + C2 * (2-((wrapped_XX - 1)/(floor(4*M_horiz)) + (wrapped_YY - 1)/(floor(4*M_vert)))) ./ ...
            ((wrapped_XX - 1)/(floor(4*M_horiz)) - (wrapped_YY - 1)/(floor(4*M_vert)));
        wl_left = fdct_wrapping_window(coord_left);
        [wl_right,wr_right] = fdct_wrapping_window(coord_corner);
        switch is_real
         case 0
          wrapped_data = fftshift(fft2(ifftshift(C{j}{l})))/sqrt(prod(size(C{j}{l})));
          wrapped_data = rot90(wrapped_data,(quadrant-1));
         case 1
          x = C{j}{l} + sqrt(-1)*C{j}{l+nbangles(j)/2};
          wrapped_data = fftshift(fft2(ifftshift(x)))/sqrt(prod(size(x)))/sqrt(2);
          wrapped_data = rot90(wrapped_data,(quadrant-1));
        end;
        wrapped_data = wrapped_data .* (wl_left .* wr_right);

         % Unwrapping data
        for row = Y_corner
            cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
            admissible_cols = round(1/2*(cols+2*floor(4*M_horiz)+1-abs(cols-(2*floor(4*M_horiz)+1))));
            new_row = 1 + mod(row - first_row, length_corner_wedge);
            Xj(row,fliplr(admissible_cols)) = Xj(row,fliplr(admissible_cols)) + wrapped_data(new_row,end:-1:1);
                                % We use the following property: in an assignment
                                % A(B) = C where B and C are vectors, if
                                % some value x repeats in B, then the
                                % last occurrence of x is the one
                                % corresponding to the eventual assignment.
        end;

        Xj = rot90(Xj);

    end;    % for quadrant

    Xj = Xj .* lowpass;
    Xj_index1 = ((-floor(2*M1)):floor(2*M1)) + floor(4*M1) + 1;
    Xj_index2 = ((-floor(2*M2)):floor(2*M2)) + floor(4*M2) + 1;
    Xj(Xj_index1, Xj_index2) = Xj(Xj_index1, Xj_index2) .* hipass;

    loc_1 = Xj_topleft_1 + (0:(2*floor(4*M1)));
    loc_2 = Xj_topleft_2 + (0:(2*floor(4*M2)));
    X(loc_1,loc_2) = X(loc_1,loc_2) + Xj;

    % Preparing for loop reentry or exit

    Xj_topleft_1 = Xj_topleft_1 + floor(4*M1) - floor(2*M1);
    Xj_topleft_2 = Xj_topleft_2 + floor(4*M2) - floor(2*M2);

    lowpass = lowpass_next;

end;    % for j

if is_real
    Y = X;
    X = rot90(X,2);
    X = X + conj(Y);
end

% Coarsest wavelet level
M1 = M1/2;
M2 = M2/2;
Xj = fftshift(fft2(ifftshift(C{1}{1})))/sqrt(prod(size(C{1}{1})));
loc_1 = Xj_topleft_1 + (0:(2*floor(4*M1)));
loc_2 = Xj_topleft_2 + (0:(2*floor(4*M2)));
X(loc_1,loc_2) = X(loc_1,loc_2) + Xj .* lowpass;

% Finest level
M1 = N1/3;
M2 = N2/3;
if finest == 1,

    % Folding back onto N1-by-N2 matrix
    shift_1 = floor(2*M1)-floor(N1/2);
    shift_2 = floor(2*M2)-floor(N2/2);
    Y = X(:,(1:N2)+shift_2);
    Y(:,N2-shift_2+(1:shift_2)) = Y(:,N2-shift_2+(1:shift_2)) + X(:,1:shift_2);
    Y(:,1:shift_2) = Y(:,1:shift_2) + X(:,N2+shift_2+(1:shift_2));
    X = Y((1:N1)+shift_1,:);
    X(N1-shift_1+(1:shift_1),:) = X(N1-shift_1+(1:shift_1),:) + Y(1:shift_1,:);
    X(1:shift_1,:) = X(1:shift_1,:) + Y(N1+shift_1+(1:shift_1),:);

else

    % Extension to a N1-by-N2 matrix
    Y = fftshift(fft2(ifftshift(C{nbscales}{1})))/sqrt(prod(size(C{nbscales}{1})));
    X_topleft_1 = ceil((N1+1)/2) - floor(M1);
    X_topleft_2 = ceil((N2+1)/2) - floor(M2);
    loc_1 = X_topleft_1 + (0:(2*floor(M1)));
    loc_2 = X_topleft_2 + (0:(2*floor(M2)));
    Y(loc_1,loc_2) = Y(loc_1,loc_2) .* hipass_finest + X;
    X = Y;

end;

x = fftshift(ifft2(ifftshift(X)))*sqrt(prod(size(X)));
if is_real, x = real(x); end;

function [wl,wr] = fdct_wrapping_window(x)

% fdct_wrapping_window.m - Creates the two halves of a C^inf compactly supported window
%
% Inputs
%   x       vector or matrix of abscissae, the relevant ones from 0 to 1
%
% Outputs
%   wl,wr   vector or matrix containing samples of the left, resp. right
%           half of the window
%
% Used at least in fdct_wrapping.m and ifdct_wrapping.m
%
% By Laurent Demanet, 2004

wr = zeros(size(x));
wl = zeros(size(x));
x(abs(x) < 2^-52) = 0;
wr((x > 0) & (x < 1)) = exp(1-1./(1-exp(1-1./x((x > 0) & (x < 1)))));
wr(x <= 0) = 1;
wl((x > 0) & (x < 1)) = exp(1-1./(1-exp(1-1./(1-x((x > 0) & (x < 1))))));
wl(x >= 1) = 1;
normalization = sqrt(wl.^2 + wr.^2);
wr = wr ./ normalization;
wl = wl ./ normalization;


function [ r, z, c, s ] = zchex ( r, ldr, p, k, l, z, ldz, nz, job )

%*****************************************************************************80
%
%% ZCHEX updates a Cholesky factorization.
%
%  Discussion:
%
%    ZCHEX updates a Cholesky factorization
%
%      A = hermitian(R) * R
%
%    of a positive definite matrix A of order P under diagonal
%    permutations of the form
%
%      E' * A * E
%
%    where E is a permutation matrix.  Specifically, given
%    an upper triangular matrix R and a permutation matrix
%    E (which is specified by K, L, and JOB), ZCHEX determines
%    a unitary matrix U such that
%
%      U * R * E = RR,
%
%    where RR is upper triangular.  At the user's option, the
%    transformation U will be multiplied into the array Z.
%
%    If A = hermitian(X)*X, so that R is the triangular part of the
%    QR factorization of X, then RR is the triangular part of the
%    QR factorization of X * E, that is, X with its columns permuted.
%
%    For a less terse description of what ZCHEX does and how
%    it may be applied, see the LINPACK guide.
%
%    The matrix Q is determined as the product U(L-K)*...*U(1)
%    of plane rotations of the form
%
%      (    C(I)       S(I) )
%      (                    ) ,
%      ( -conj(S(i))  C(I) )
%
%    where C(I) is real, the rows these rotations operate on
%    are described below.
%
%    There are two types of permutations, which are determined
%    by the value of job.
%
%    JOB = 1, right circular shift:
%    The columns are rearranged in the following order.
%
%      1, ..., K-1, L, K, K+1, ..., L-1, L+1, ..., P.
%
%    U is the product of L-K rotations U(I), where U(I)
%    acts in the (L-I,L-I+1)-plane.
%
%    JOB = 2, left circular shift:
%    The columns are rearranged in the following order
%
%      1, ..., K-1, K+1, K+2, ..., L, L, L+1, ..., P.
%
%    U is the product of L-K rotations U(I), where U(I)
%    acts in the (K+I-1,K+I)-plane.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%
%  Parameters:
%
%    Input, complex R(LDR,P); the upper triangular factor that is to be updated.
%    Elements below the diagonal are not referenced.
%
%    Input, integer LDR, the leading dimension of R, which is at least P.
%
%    Input, integer P, the order of the matrix.
%
%    Input, integer K, the first column to be permuted.
%
%    Input, integer L, the last column to be permuted.
%    L must be strictly greater than K.
%
%    Input, complex Z(LDZ,NZ), an array of NZ P-vectors into
%    which the transformation U is multiplied.
%
%    Input, integer LDZ, the leading dimension of Z, which must
%    be at least P.
%
%    Input, integer NZ, the number of columns of the matrix Z.
%
%    Input, integer JOB, determines the type of permutation.
%    1, right circular shift.
%    2, left circular shift.
%
%    Output, complex R(LDR,P), the updated factor.
%
%    Output, complex Z(LDZ,NZ), the updated matrix.  Z is not referenced
%    if NZ = 0.
%
%    Output, real C(P), the cosines of the transforming rotations.
%
%    Output, complex S(P), the sines of the transforming rotations.
%
  if ( job == 1 )
%
%  Right circular shift.
%
%  Reorder the columns.
%
    for i = 1 : l
      ii = l - i + 1;
      s(i) = r(ii,l);
    end

    for jj = k : l - 1
      j = l - 1 - jj + k;
      r(1:j,j+1) = r(1:j,j);
      r(j+1,j+1) = 0.0;
    end

    for i = 1 : k - 1
      ii = l - i + 1;
      r(i,k) = s(ii);
    end
%
%  Calculate the rotations.
%
    t = s(1);
    for i = 1 : l - k
      [ c(i), s(i), s(i+1) ] = zrotg ( s(i+1), t );
      t = s(i+1);
    end

    r(k,k) = t;
    for j = k+1 : p
      il = max ( 1, l - j + 1 );
      for ii = il : l - k
        i = l - ii;
        t = c(ii) * r(i,j) + s(ii) * r(i+1,j);
        r(i+1,j) = c(ii) * r(i+1,j) - conj ( s(ii) ) * r(i,j);
        r(i,j) = t;
      end
    end
%
%  If required, apply the transformations to Z.
%
    for j = 1 : nz
      for ii = 1 : l - k
        i = l - ii;
        t = c(ii) * z(i,j) + s(ii) * z(i+1,j);
        z(i+1,j) = c(ii) * z(i+1,j) - conj ( s(ii) ) * z(i,j);
        z(i,j) = t;
      end
    end

  else
%
%  Left circular shift.
%
%  Reorder the columns.
%
    for i = 1 : k
      ii = l - k + i;
      s(ii) = r(i,k);
    end

    for j = k : l - 1
      r(1:j,j) = r(1:j,j+1);
      jj = j - k + 1;
      s(jj) = r(j+1,j+1);
    end

    for i = 1 : k
      ii = l - k + i;
      r(i,l) = s(ii);
    end

    r(k+1:l,l) = 0.0;
%
%  Reduction loop.
%
    for j = k : p
%
%  Apply the rotations.
%
      if ( j ~= k )
        iu = min ( j - 1, l - 1 );
        for i = k : iu
          ii = i - k + 1;
          t = c(ii) * r(i,j) + s(ii) * r(i+1,j);
          r(i+1,j) = c(ii) * r(i+1,j) - conj ( s(ii) ) * r(i,j);
          r(i,j) = t;
        end
      end

      if ( j < l )
        jj = j - k + 1;
        t = s(jj);
        [ c(jj), s(jj), r(j,j) ] = zrotg ( r(j,j), t );
      end

    end
%
%  Apply the rotations to Z.
%
    for j = 1 : nz
      for i = k : l - 1
        ii = i - k + 1;
        t = c(ii) * z(i,j) + s(ii) * z(i+1,j);
        z(i+1,j) = c(ii) * z(i+1,j) - conj ( s(ii) ) * z(i,j);
        z(i,j) = t;
      end
    end

  end

  return
end


function y = log_sum_exp( varargin )

%LOG_SUM_EXP    log(sum(exp(x))).
%   LOG_SUM_EXP(X) = LOG(SUM(EXP(X)).
%
%   When used in a CVX model, LOG_SUM_EXP(X) causes CVX's successive
%   approximation method to be invoked, producing results exact to within
%   the tolerance of the solver. This is in contrast to LOGSUMEXP_SDP,
%   which uses a single SDP-representable global approximation.
%
%   If X is a matrix, LOGSUMEXP_SDP(X) will perform its computations
%   along each column of X. If X is an N-D array, LOGSUMEXP_SDP(X)
%   will perform its computations along the first dimension of size
%   other than 1. LOGSUMEXP_SDP(X,DIM) will perform its computations
%   along dimension DIM.
%
%   Disciplined convex programming information:
%       LOG_SUM_EXP(X) is convex and nondecreasing in X; therefore, X
%       must be convex (or affine).

persistent P
if isempty( P ),
    P.map = cvx_remap( { 'real' ; 'convex' } );
    P.funcs = { @lse_1, @lse_2 };
    P.zero = -Inf;
    P.reduce = true;
    P.constant = 1;
    P.reverse = false;
    P.name = 'log_sum_exp';
    P.dimarg = 2;
end
cvx_expert_check( 'log_sum_exp', varargin{1} );
y = cvx_reduce_op( P, varargin{:} );

function x = lse_1( x )
xmid = 0.5 * ( max( x, [], 1 ) + min( x, [], 1 ) );
x = log( sum( exp( bsxfun( @minus, x, xmid ) ), 1 ) ) + xmid;

function y = lse_2( x ) %#ok
[nx,nv] = size(x); %#ok
cvx_begin
    variable w( nx, nv )
    epigraph variable y( 1, nv )
    exp( x - repmat( y, [ nx, 1 ] ) ) <= w; %#ok
    sum( w, 1 ) == 1; %#ok
cvx_end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


classdef TTeMPS_op_laplace
% TTeMPS_op_laplace
%
%   A MATLAB class for representing and manipulating
%   Laplace-like operators in the TT/MPS operator format,
%
%   Laplace-like operators are of the form
%         L \otimes I \otimes I \otimes I
%       + I \otimes L \otimes I \otimes I
%       + ...
%       + I \otimes I \otimes I \otimes L
%
%   with e.g. L being the discrete 1D-Laplacian matrix.

%   TTeMPS Toolbox.
%   Michael Steinlechner, 2013-2016
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt

properties( SetAccess = public, GetAccess = public )

    L0
    U           % core tensors as 4D doubles
    rank
    order
    size_row
    size_col
    V_L
    E_L

end

% Set methods for Cores
methods

    function A = set.U( A, U_);

        A.U = U_;
        A = update_properties( A );

    end

    function A = update_properties( A );

        A.rank = [1,  2*ones(1, length(A.U)-1), 1];  % the TT rank is always two for such Laplace-like tensors
        size_col_ = cellfun( @(y) size(y,1), A.U);
        A.size_col = size_col_ ./ (A.rank(1:end-1).*A.rank(2:end));
        A.size_row = cellfun( @(y) size(y,2), A.U);
        A.order = length( A.size_row );

    end

end

methods( Access = public )

    function A = TTeMPS_op_laplace(varargin)
    %LAPLACE Construct a tensor in TT/MPS format and return TTeMPS_op_laplace object.
    %
    %   A = TTEMPS_OP_LAPLACE( L, D ) creates the D-dimensional Laplace-like
    %       TT/MPS operator using the supplied matrix L.
    %
    %
        if nargin == 1
            % debug constructor
            A.U = varargin{1};
            A = update_properties( A );
            A.L0 = [];
            A.V_L = [];
            A.E_L = [];
        % Default constructor
        elseif (nargin == 2)

            % only one matrix passed
            L = varargin{1};
            A.L0 = L;
            d = varargin{2};

            A.V_L = [];
            A.E_L = [];

            [m,n] = size( L );
            E = speye( m, n );
            a_1 = sparse( 1, 1, 1, 2, 1 );
            a_mid = sparse( 2, 1, 1, 4, 1 );
            a_end = sparse( 2, 1, 1, 2, 1 );
            b_1 = sparse( 2, 1, 1, 2, 1 );
            b_mid = sparse( [1;4], [1;1], [1;1], 4, 1 );
            b_end = sparse( 1, 1, 1, 2, 1 );

            A.U = cell( 1, d );
            A.U{1} = kron( L, a_1 ) + kron( E, b_1 );
            A_mid = kron( L, a_mid ) + kron( E, b_mid );
            for i=2:d-1
                A.U{i} = A_mid;
            end
            A.U{d} = kron( L, a_end ) + kron( E, b_end );

            A = update_properties( A );

        else
            error('Invalid number of arguments.')
        end
    end

    % Other public functions
    y = apply( A, x, idx );
    A = mtimes( B, A );
    res = contract( A, x, y, idx );

    disp( A, name );
    display( A );

    B = TTeMPS_op_laplace_to_TTeMPS_op( A );
    B = TTeMPS_op_laplace_to_TT_matrix( A );
    expB = constr_precond_inner( A, X, mu );

    function A = initialize_precond( A )
        [A.V_L, A.E_L] = eig(full(A.L0));
        A.E_L = diag( A.E_L );
    end
end

end


function [V, policy, iter, cpu_time] = mdp_policy_iteration(P, R, discount, policy0, max_iter, eval_type)

% max_iter=2000;
% eval_type=1;

% mdp_policy_iteration  Resolution of discounted MDP
%                       with policy iteration algorithm
% Arguments ---------------------------------------------------------------
% Let S = number of states, A = number of actions
%   P(SxSxA) = transition matrix
%              P could be an array with 3 dimensions or
%              a cell array (1xA), each cell containing a matrix (SxS) possibly sparse
%   R(SxSxA) or (SxA) = reward matrix
%              R could be an array with 3 dimensions (SxSxA) or
%              a cell array (1xA), each cell containing a sparse matrix (SxS) or
%              a 2D array(SxA) possibly sparse
%   discount = discount rate, in ]0, 1[
%   policy0(S) = starting policy, optional
%   max_iter = maximum number of iteration to be done, upper than 0,
%              optional (default 1000)
%   eval_type = type of function used to evaluate policy:
%              0 for mdp_eval_policy_matrix, else mdp_eval_policy_iterative
%              optional (default 0)
% Evaluation --------------------------------------------------------------
%   V(S)   = value function
%   policy(S) = optimal policy
%   iter     = number of done iterations
%   cpu_time = used CPU time
%--------------------------------------------------------------------------
% In verbose mode, at each iteration, displays the number
% of differents actions between policy n-1 and n

% MDPtoolbox: Markov Decision Processes Toolbox
% Copyright (C) 2009  INRA
% Redistribution and use in source and binary forms, with or without modification,
% are permitted provided that the following conditions are met:
%    * Redistributions of source code must retain the above copyright notice,
%      this list of conditions and the following disclaimer.
%    * Redistributions in binary form must reproduce the above copyright notice,
%      this list of conditions and the following disclaimer in the documentation
%      and/or other materials provided with the distribution.
%    * Neither the name of the <ORGANIZATION> nor the names of its contributors
%      may be used to endorse or promote products derived from this software
%      without specific prior written permission.
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
% ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
% IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
% INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
% BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
% DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
% OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
% OF THE POSSIBILITY OF SUCH DAMAGE.

cpu_time = cputime;

global mdp_VERBOSE;

% check of arguments
if iscell(P); S = size(P{1},1); else S = size(P,1); end;
if iscell(P); A = length(P); else A = size(P,3); end

if discount <= 0 || discount >= 1
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: Discount rate must be in ]0; 1[')
    disp('--------------------------------------------------------')
elseif nargin > 3 && (size(policy0,1)~=S || any(mod(policy0,1)) || any(policy0<1)|| any(policy0>A) )
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: policy0 must a (Sx1) vector with integer from 1 to A')
    disp('--------------------------------------------------------')
elseif nargin > 4 && max_iter <= 0
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: The maximum number of iteration must be upper than 0')
    disp('--------------------------------------------------------')
else

    PR = mdp_computePR(P,R);

    % initialization of optional arguments
    if nargin < 6; eval_type = 0; end;
    if nargin < 5; max_iter = 1000; end;
    if nargin < 4;
        % initialization of policy:
        % the one wich maximizes the expected immediate reward
        [nil, policy0] = mdp_bellman_operator(P,PR,discount,zeros(S,1));
    end;

%     if mdp_VERBOSE; disp('  Iteration  Number_of_different_actions'); end;
        disp('  Iteration  Number_of_different_actions');

    iter = 0;
    policy = policy0;
    is_done = false;
    while ~is_done
        iter = iter + 1;
        if  (eval_type==0)
            V = mdp_eval_policy_matrix(P,PR,discount,policy);
        else
            V = mdp_eval_policy_iterative(P,PR,discount,policy);
        end;
        [nil, policy_next] = mdp_bellman_operator(P,PR,discount,V);

	n_different = sum(policy_next ~= policy);
%         if mdp_VERBOSE; disp(['       ' num2str(iter) '                 '  num2str(n_different)]); end;
        disp(['       ' num2str(iter) '                 '  num2str(n_different)]);

        if all(policy_next==policy) || iter == max_iter || (iter > 20 && n_different <=5)
            is_done = true;
        else
            policy = policy_next;
        end;
    end;

end;

cpu_time = cputime - cpu_time;


function c = rdivide(a,b)
% function C=rdivide(A,B)
%
% DESCRIPTION
%   Element-by-element right division
%
% INPUTS
%   A: polynomial
%   B: matrix of constants
%
% OUTPUTS
%   C: polynomial, the result of division.
%
% SYNTAX
%   C= A./B
%     A and B must have the same dimensions unless one
%     is a scalar.  Scalars can be divided with anything.
%   C = rdivide(A,B)
%     Function-call form

% 10/22/2002: PJS  Initial Coding

% Promote a to polynomial
a = polynomial(a);
sza = size(a);

% Promote b to polynomial
b = polynomial(b);
szb = size(b);

if isempty(a) || isempty(b)

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

elseif all(sza==szb) || all(sza==[1 1]) || ...
        all(szb==[1 1])

    % Matrix./Matrix
    degidx = find(b.degmat);
    if ~isempty(degidx)
        error('B must be a constant.');
    end
    b = combine(b);
    bcoef = reshape(b.coefficient,b.matdim);
    c = times(a,1./bcoef);
else

    error('Matrix dimensions must agree');
end


classdef cluster

methods(Static)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Following functions are meant for assisting in
% setting up experiments.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    function labels = labels_from_cluster_sizes(cluster_sizes)
        % total number of points
        S = sum(cluster_sizes);
        % Number of points in array
        K = numel(cluster_sizes);
        labels = zeros(1, S);
        i = 0;
        for k=1:K
            Sk  = cluster_sizes(k);
            for s=1:Sk
                i = i+1;
                labels(i) = k;
            end
        end
        return;
    end

    function [cluster_sizes, labels] = cluster_sizes_from_labels(labels)
        tbl = tabulate(labels);
        labels = tbl(:, 1)';
        cluster_sizes =  tbl(:, 2)';
    end

    function [start_indices, end_indices] = start_end_indices(cluster_sizes)
        % Returns start and end indices from cluster sizes
        start_indices = cumsum(cluster_sizes) + 1;
        start_indices = [1 start_indices(1:end-1)];
        end_indices = start_indices + cluster_sizes -1;
    end

    function result = clustering_error(estimated_labels, true_labels, num_clusters)
        % finds out an appropriate mapping between
        % true labels and estimated labels and
        % calculates the corresponding clustering error

        % make sure that both are row vectors
        if ~isrow(estimated_labels)
            estimated_labels = estimated_labels';
        end
        if ~isrow(true_labels)
            true_labels = true_labels';
        end
        if size(estimated_labels) ~= size(true_labels)
            error('Both estimated and true label arrays should have same size.');
        end
        % Let us consider all possible mappings
        possible_mappings = perms(1:num_clusters);
        % number of possible mappings
        num_mappings = size(possible_mappings,1 );
        % number of labels
        num_labels = size(estimated_labels, 2);
        % number of misclustered points for each mapping
        missed_points = zeros(num_mappings, 1);
        for j=1:num_mappings
            %cur_mapping = possible_mappings(j, :)
            mapped_labels = possible_mappings(j, true_labels);
            missed_points(j) = sum(estimated_labels ~= mapped_labels);
        end
        % missed_points
        % we choose the mapping with minimum mismatch
        [miss, index] = min(missed_points, [], 1);
        % final clustering error value
        result.num_labels = num_labels;
        result.num_missed_points = miss;
        result.error  = miss / num_labels;
        result.error_perc = result.error * 100;
        % corresponding mapping
        result.mapping = possible_mappings(index, :);
        result.mapped_labels = result.mapping(true_labels);
        result.misses = estimated_labels ~= result.mapped_labels;
    end

    function result = clustering_error_hungarian_mapping(estimated_labels, true_labels, num_clusters)
        % make sure that both are row vectors
        if ~isrow(estimated_labels)
            estimated_labels = estimated_labels';
        end
        if ~isrow(true_labels)
            true_labels = true_labels';
        end
        if size(estimated_labels) ~= size(true_labels)
            error('Both estimated and true label arrays should have same size.');
        end
        % number of labels
        num_labels = size(estimated_labels, 2);
        mapped_labels = bestMap(estimated_labels, true_labels)';
        miss = sum(estimated_labels ~= mapped_labels);
        % final clustering error value
        result.num_labels = num_labels;
        result.num_missed_points = miss;
        result.error  = miss / num_labels;
        result.error_perc = result.error * 100;
        result.mapped_labels = mapped_labels;
        result.misses = estimated_labels ~= result.mapped_labels;
    end

end

end


function [Cext,val] = extendLSAPEinstance(C,val)
% Extend a cost matrix for error-correcting matching to its square version
%
%  [Cext,val] = extendCostMtx(C,val)
%
%  inputs:
%  C (n+1)x(m+1) cost matrix of an error-correcting bipartite graph
%  val the value used to fill 2nd and 3rd block of Cext (optional)
%  val = max(max(C)) + 1 if not given
%
%  outputs:
%  Cext (n+m)x(m+n) extended cost matrix
%  valcpt (optional) is the value val used to fill 2nd and 3rd block of
%  Cext
%
%         | c(1,1)   ...  c(1,m-1)  | c(1,m) val   ...      val   |
%         |   .              .      |  val  c(2,m) val ...   .    |
%         |   .              .      |   .                   val   |
%         | c(n-1,1) ... c(n-1,m-1) |  val     ...   val c(n-1,m) |
%  Cext =  -------------------------------------------------------
%         | c(n,1) val  ...    val  |                             |
%         |  val c(n,2) val ..  .   |           0_{m,n}           |
%         |   .                val  |                             |
%         |  val  ...  val c(n,m-1) |                             |
%
%
%   author: Sebastien Bougleux
%   institution: Normandie Univ, UNICAEN, ENSICAEN, CNRS, GREYC
% -----------------------------------------------------------
% This file is part of LSAPE.
% LSAPE is free software: you can redistribute it and/or modify
% it under the terms of the CeCILL-C License. See README file
% for more details.
% -----------------------------------------------------------

    if ~exist('val','var') || nargin == 1
        val = max(max(C)) + 1;
    end

    D = val .* cast(ones(size(C,1)-1),class(C));
    E = val .* cast(ones(size(C,2)-1),class(C));
    D(logical(eye(size(D)))) = C(1:end-1,end);
    E(logical(eye(size(E)))) = C(end,1:end-1);
    Cext = [[C(1:end-1,1:end-1),D];[E,zeros(size(C,2)-1,size(C,1)-1)]];

end


%%********************************************************************
%% blkbarrier: calculate
%% [-v(p)*logdet(X{p}),   v(p)*logdet(Z{p}) + n*v(p)*(1-log(v(p)))]
%% [-v(p)*log(gam(X{p})), v(p)*log(gam(Z{p})) + v(p)]
%% [-v(p)*log(X{p}),      v(p)*log(Z{p}) + n*v(p)*(1-log(v(p)))]
%%********************************************************************

  function objadd = blkbarrier(blk,X,Z,Xchol,Zchol,v);

   objadd = zeros(1,2); tmp = zeros(1,2);
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      vp = v{p};
      idx = find(vp > 0);
      if ~isempty(idx)
         vpsub = vp(idx);
         if size(vpsub,1) < size(vpsub,2); vpsub = vpsub'; end
         if strcmp(pblk{1},'s')
            ss = [0, cumsum(pblk{2})];
            logdetX = 2*log(diag(Xchol{p}));
            logdetZ = 2*log(diag(Zchol{p}));
            logdetXsub = zeros(length(idx),1);
            logdetZsub = zeros(length(idx),1);
            for k = 1:length(idx)
               idxtmp = [ss(idx(k))+1:ss(idx(k)+1)];
               logdetXsub(k) = sum(logdetX(idxtmp));
               logdetZsub(k) = sum(logdetZ(idxtmp));
            end
            tmp(1) = -sum(vpsub.*logdetXsub);
            tmp(2) = sum(vpsub.*logdetZsub + (pblk{2}(idx)').*vpsub.*(1-log(vpsub)));
         elseif strcmp(pblk{1},'q')
            gamX = sqrt(qops(pblk,X{p},X{p},2));
            gamZ = sqrt(qops(pblk,Z{p},Z{p},2));
            tmp(1) = -sum(vpsub.*log(gamX(idx)));
            tmp(2) = sum(vpsub.*log(gamZ(idx)) + vpsub);
         elseif strcmp(pblk{1},'l')
            logX = log(X{p}); logZ = log(Z{p});
            tmp(1) = -sum(vpsub.*logX(idx));
            tmp(2) = sum(vpsub.*logZ(idx) + vpsub.*(1-log(vpsub)));
         end
         objadd = objadd + tmp;
      end
   end
%%********************************************************************


function [ class, clsize, critvl, ntrans, ifault ] = swap ( varval, class, ...
  clsize, in, ik, iv, critvl, ntrans, ifault )

%*****************************************************************************80
%
%% SWAP interchanges objects between different classes to improve a criterion.
%
%  Discussion:
%
%    This routine is given a classification of objects, including the
%    number of objects in each class, and the current value of some criterion
%    which is desired to be minimized.
%
%    The routine calculates the change in criterion for all possible swaps,
%    that is, operations in which two objects in different classes exchange
%    places. Each swap that would result in a lowering of the criterion is
%    executed, and the related quantities are updated.
%
%    When no more advantageous swaps can be found, the routine returns.
%
%    The routine relies on a user-supplied routine, CRSWAP, to report the
%    expected change in the criterion for a given swap, and to carry
%    out that transfer if requested.
%
%    The variables CLASS and CRITVL have been added to the argument list
%    of CRSWAP.
%
%    Also, the order of the two classes "L" and "M" was interchanged in
%    the call to CRSWAP.  The original order was counterintuitive.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2008
%
%  Author:
%
%    Original FORTRAN77 version by Banfield, Bassill.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Colin Banfield, LC Bassill,
%    Algorithm AS 113:
%    A transfer for non-hierarchichal classification,
%    Applied Statistics,
%    Volume 26, Number 2, 1977, pages 206-210.
%
%  Parameters:
%
%    Input, real VARVAL(IN,IV), the data values.  There are
%    IN objects, each having spatial dimension IV.
%
%    Input, integer CLASS(IN), the initial classification of
%    each object.
%
%    Input, integer CLSIZE(IK), the initial number of objects
%    in each class.
%
%    Input, integer IN, the number of objects.
%
%    Input, integer IK, the number of classes.
%
%    Input, integer IV, the number of spatial dimensions,
%    or variates, of the objects.
%
%    Input, real CRITVL, the initial value of the criterion.
%
%    Output, integer CLASS(IN), the classification of
%    each object.
%
%    Output, integer CLSIZE(IK), the number of objects
%    in each class.
%
%    Output, real CRITVL, the current value of the criterion.
%
%    Output, integer NTRANS, the number of transfers executed.
%
%    Output, integer IFAULT, error indicator.
%    0, no error detected.
%    1, the number of classes was less than 2.
%    2, the number of objects was less than the number of classes.
%
  eps = 1.0E-38;
  ntrans = 0;

  if ( ik <= 1 )
    ifault = 1
    return
  end

  if ( in <= ik )
    ifault = 2;
    return
  end

  ifault = 0;
  icount = 0;
  itop = ( in * ( in - 1 ) ) / 2;

  i = 1;

  while ( 1 )

    i = i + 1;

    if ( itop <= icount )
      break
    end

    if ( in < i )
      i = 1;
      continue
    end

    l = class(i);
    k = l;
    it = i - 1;
%
%  Test the swap of object I from class M to L,
%  and object J from class L to M.
%
    for j = 1 : it

      icount = icount + 1;
      m = class(j);

      if ( l ~= j )

        if ( clsize(l) ~= 1 | clsize(m) ~= 1 )

          iswitch = 1;
          inc = crswap ( varval, class, clsize, in, ik, iv, critvl, ...
            i, j, l, m, iswitch );

          if ( inc < - eps )

            critvl = critvl + inc;
            icount = 0;

            iswitch = 2;
            crswap ( varval, class, clsize, in, ik, iv, critvl, ...
              i, j, l, m, iswitch );

            ntrans = ntrans + 1;
            class(i) = m;
            class(j) = l;
            l = m;

          end

        end
      end

    end

  end

  return
end


function jrt=jacprojRT(j, i, rt, xyz, r0, a)
% symbolic projection function Jacobian
% code automatically generated with maple

  qr0=r0(j*4+1:(j+1)*4);

  t1 = (rt(1) ^ 2);
  t2 = (rt(2) ^ 2);
  t3 = (rt(3) ^ 2);
  t5 = sqrt((1 - t1 - t2 - t3));
  t6 = 0.1e1 / t5;
  t7 = t6 * qr0(2);
  t9 = -t7 * rt(1) + qr0(1);
  t11 = t6 * qr0(3);
  t13 = -t11 * rt(1) - qr0(4);
  t15 = t6 * qr0(4);
  t17 = -t15 * rt(1) + qr0(3);
  t19 = -t9 * xyz(1) - t13 * xyz(2) - t17 * xyz(3);
  t24 = -t5 * qr0(2) - qr0(1) * rt(1) - rt(2) * qr0(4) + rt(3) * qr0(3);
  t31 = t5 * qr0(3) + qr0(1) * rt(2) + rt(3) * qr0(2) - rt(1) * qr0(4);
  t37 = t5 * qr0(4) + qr0(1) * rt(3) + rt(1) * qr0(3) - rt(2) * qr0(2);
  t39 = t24 * xyz(1) - t31 * xyz(2) - t37 * xyz(3);
  t41 = t6 * qr0(1);
  t43 = -t41 * rt(1) - qr0(2);
  t48 = t5 * qr0(1) - rt(1) * qr0(2) - rt(2) * qr0(3) - rt(3) * qr0(4);
  t52 = t48 * xyz(1) + t31 * xyz(3) - t37 * xyz(2);
  t57 = t43 * xyz(1) + t13 * xyz(3) - t17 * xyz(2);
  t62 = t43 * xyz(2) + t17 * xyz(1) - t9 * xyz(3);
  t67 = t48 * xyz(2) + t37 * xyz(1) + t24 * xyz(3);
  t72 = t43 * xyz(3) + t9 * xyz(2) - t13 * xyz(1);
  t77 = t48 * xyz(3) - t24 * xyz(2) - t31 * xyz(1);
  t89 = -t19 * t31 - t39 * t13 + t43 * t67 + t48 * t62 + t72 * t24 - t77 * t9 + t57 * t37 + t52 * t17;
  t99 = -t19 * t37 - t39 * t17 + t43 * t77 + t48 * t72 - t57 * t31 - t52 * t13 - t62 * t24 + t67 * t9;
  t106 = -t39 * t37 + t48 * t77 - t52 * t31 - t67 * t24 + rt(6);
  t107 = 0.1e1 / t106;
  t119 = -t39 * t31 + t48 * t67 + t77 * t24 + t52 * t37 + rt(5);
  t123 = t106 ^ 2;
  t124 = 0.1e1 / t123;
  t125 = (a(1) * (t39 * t24 + t48 * t52 - t67 * t37 + t77 * t31 + rt(4)) + a(2) * t119 + a(3) * t106) * t124;
  t129 = -t7 * rt(2) + qr0(4);
  t132 = -t11 * rt(2) + qr0(1);
  t135 = -t15 * rt(2) - qr0(2);
  t137 = -t129 * xyz(1) - t132 * xyz(2) - t135 * xyz(3);
  t141 = -t41 * rt(2) - qr0(3);
  t146 = t141 * xyz(1) + t132 * xyz(3) - t135 * xyz(2);
  t151 = t141 * xyz(2) + t135 * xyz(1) - t129 * xyz(3);
  t157 = t141 * xyz(3) + t129 * xyz(2) - t132 * xyz(1);
  t170 = -t137 * t31 - t39 * t132 + t141 * t67 + t48 * t151 + t157 * t24 - t77 * t129 + t146 * t37 + t52 * t135;
  t180 = -t137 * t37 - t39 * t135 + t141 * t77 + t48 * t157 - t146 * t31 - t52 * t132 - t151 * t24 + t67 * t129;
  t187 = -t7 * rt(3) - qr0(3);
  t190 = -t11 * rt(3) + qr0(2);
  t193 = -t15 * rt(3) + qr0(1);
  t195 = -t187 * xyz(1) - t190 * xyz(2) - t193 * xyz(3);
  t199 = -t41 * rt(3) - qr0(4);
  t204 = t199 * xyz(1) + t190 * xyz(3) - t193 * xyz(2);
  t209 = t199 * xyz(2) + t193 * xyz(1) - t187 * xyz(3);
  t215 = t199 * xyz(3) + t187 * xyz(2) - t190 * xyz(1);
  t228 = -t195 * t31 - t39 * t190 + t199 * t67 + t48 * t209 + t215 * t24 - t77 * t187 + t204 * t37 + t52 * t193;
  t238 = -t195 * t37 - t39 * t193 + t199 * t77 + t48 * t215 - t204 * t31 - t52 * t190 - t209 * t24 + t67 * t187;
  t255 = (a(4) * t119 + a(5) * t106) * t124;
  jrt(1) = (a(1) * (t19 * t24 - t39 * t9 + t43 * t52 + t48 * t57 - t62 * t37 - t67 * t17 + t72 * t31 + t77 * t13) + a(2) * t89 + a(3) * t99) * t107 - t125 * t99;
  jrt(2) = (a(1) * (t137 * t24 - t39 * t129 + t141 * t52 + t48 * t146 - t151 * t37 - t67 * t135 + t157 * t31 + t77 * t132) + a(2) * t170 + a(3) * t180) * t107 - t125 * t180;
  jrt(3) = (a(1) * (t195 * t24 - t39 * t187 + t199 * t52 + t48 * t204 - t209 * t37 - t67 * t193 + t215 * t31 + t77 * t190) + a(2) * t228 + a(3) * t238) * t107 - t125 * t238;
  jrt(4) = a(1) * t107;
  jrt(5) = a(2) * t107;
  jrt(6) = a(3) * t107 - t125;
  jrt(7) = (a(4) * t89 + a(5) * t99) * t107 - t255 * t99;
  jrt(8) = (a(4) * t170 + a(5) * t180) * t107 - t255 * t180;
  jrt(9) = (a(4) * t228 + a(5) * t238) * t107 - t255 * t238;
  jrt(10) = 0.0e0;
  jrt(11) = a(4) * t107;
  jrt(12) = a(5) * t107 - t255;