% this is an experimentaal spiral sequence

fov=256e-3; Nx=96; Ny=Nx;  % Define FOV and resolution
sliceThickness=3e-3;             % slice thinckness
Nslices=1;
Oversampling=2; % by looking at the periphery of the spiral I would say it needs to be at least 2
phi=pi/2; % orientation of the readout e.g. for interleaving

% Set system limits
sys = mr.opts('MaxGrad',30,'GradUnit','mT/m',...
    'MaxSlew',120,'SlewUnit','T/m/s',...
    'rfRingdownTime', 30e-6, 'rfDeadtime', 100e-6, 'adcDeadTime', 10e-6);
seq=mr.Sequence(sys);          % Create a new sequence object
warning('OFF', 'mr:restoreShape'); % restore shape is not compatible with spirals and will throw a warning from each plot() or calcKspace() call

% Create fat-sat pulse
% (in Siemens interpreter from January 2019 duration is limited to 8.192 ms, and although product EPI uses 10.24 ms, 8 ms seems to be sufficient)
B0=2.89; % 1.5 2.89 3.0
sat_ppm=-3.45;
sat_freq=sat_ppm*1e-6*B0*sys.gamma;
rf_fs = mr.makeGaussPulse(110*pi/180,'system',sys,'Duration',8e-3,...
    'bandwidth',abs(sat_freq),'freqOffset',sat_freq);
gz_fs = mr.makeTrapezoid('z',sys,'delay',mr.calcDuration(rf_fs),'Area',1/1e-4); % spoil up to 0.1mm

% Create 90 degree slice selection pulse and gradient
[rf, gz] = mr.makeSincPulse(pi/2,'system',sys,'Duration',3e-3,...
    'SliceThickness',sliceThickness,'apodization',0.5,'timeBwProduct',4);

% define k-space parameters
deltak=1/fov;
kRadius = round(Nx/2);
kSamples=round(2*pi*kRadius)*Oversampling;
readoutTime = 4.2e-4;

% calculate a raw Archimedian spiral trajectory
clear ka;
ka(kRadius*kSamples+1)=1i; % init as complex
for c=0:kRadius*kSamples
    r=deltak*c/kSamples;
    a=mod(c,kSamples)*2*pi/kSamples;
    ka(c+1)=r*exp(1i*a);
end
ka=[real(ka); imag(ka)];
% calculate gradients and slew rates
[ga, sa]=mr.traj2grad(ka);

% limit analysis
safety_magrin=0.94; % we need that  otherwise we just about violate the slew rate due to the rounding errors
dt_gcomp=abs(ga)/(sys.maxGrad*safety_magrin)*sys.gradRasterTime;
dt_gabs=abs(ga(1,:)+1i*ga(2,:))/(sys.maxGrad*safety_magrin)*sys.gradRasterTime;
dt_scomp=sqrt(abs(sa)/(sys.maxSlew*safety_magrin))*sys.gradRasterTime;
dt_sabs=sqrt(abs(sa(1,:)+1i*sa(2,:))/(sys.maxSlew*safety_magrin))*sys.gradRasterTime;

figure;plot([dt_gabs; max(dt_gcomp); dt_sabs; max(dt_scomp)]');title('time stepping defined by gradient and slew-rate');

dt_smooth=max([dt_gabs;dt_sabs]);
dt_rough=max([dt_gcomp;dt_scomp]);

% apply the lower limit not to lose the trajectory detail
dt_min=4*sys.gradRasterTime/kSamples; % we want at least 4 points per revolution
dt_smooth0=dt_smooth;
dt_rough0=dt_rough;
dt_smooth(dt_smooth<dt_min)=dt_min;
dt_rough(dt_rough<dt_min)=dt_min;

figure;plot([dt_smooth0; dt_smooth; dt_rough0; dt_rough]');title('combined time stepping');

t_smooth=[0 cumsum(dt_smooth,2)];
t_rough=[0 cumsum(dt_rough,2)];

kopt_smooth=interp1(t_smooth, ka', (0:floor(t_smooth(end)/sys.gradRasterTime))*sys.gradRasterTime)';
kopt_rough=interp1(t_rough, ka', (0:floor(t_rough(end)/sys.gradRasterTime))*sys.gradRasterTime)';

% analyze what we've got
fprintf('duration orig %d us\n', round(1e6*sys.gradRasterTime*length(ka)));
fprintf('duration smooth %d us\n', round(1e6*sys.gradRasterTime*length(kopt_smooth)));
fprintf('duration rough %d us\n', round(1e6*sys.gradRasterTime*length(kopt_rough)));

[gos, sos]=mr.traj2grad(kopt_smooth);
[gor, sor]=mr.traj2grad(kopt_rough);

figure;plot([gos;abs(gos(1,:)+1i*gos(2,:))]');title('gradient with smooth (abs) constraint')
figure;plot([gor;abs(gor(1,:)+1i*gor(2,:))]');title('gradient with rough (component) constraint')

figure;plot([sos;abs(sos(1,:)+1i*sos(2,:))]');title('slew rate with smooth (abs) constraint')
figure;plot([sor;abs(sor(1,:)+1i*sor(2,:))]');title('slew rate with rough (component) constraint')

% Define gradients and ADC events
spiral_grad_shape=gos;
% Create 90 degree slice selection pulse and gradient
[rf, gz] = mr.makeSincPulse(pi/2,'system',sys,'Duration',3e-3,...
    'SliceThickness',sliceThickness,'apodization',0.5,'timeBwProduct',4);
gzReph = mr.makeTrapezoid('z',sys,'Area',-gz.area/2);

% calculate ADC
% round-down dwell time to 10 ns
adcTime = sys.gradRasterTime*size(spiral_grad_shape,2);
% actually it is trickier than that: the (Siemens) interpreter sequence
% per default will try to split the trajectory into segments <=1000 samples
% and every of these segments will have to have duration aligned to the
% gradient raster time
adcSamplesPerSegment=1000; % you may need to play with this number to fill the entire trajectory
adcSamplesDesired=kRadius*kSamples;
adcSegments=round(adcSamplesDesired/adcSamplesPerSegment);
adcSamples=adcSegments*adcSamplesPerSegment;
adcDwell=round(adcTime/adcSamples/100e-9)*100e-9; % on Siemens adcDwell needs to be aligned to 100ns (if my memory serves me right)
adcSegmentDuration=adcSamplesPerSegment*adcDwell; % with the 100 samples above and the 100ns alignment we automatically fullfill the segment alignment requirement
if mod(adcSegmentDuration, sys.gradRasterTime)>eps
    error('ADC segmentation model results in incorrect segment duration');
end
% update segment count
adcSegments=floor(adcTime/adcSegmentDuration);
adcSamples=adcSegments*adcSamplesPerSegment;
adc = mr.makeAdc(adcSamples,'Dwell',adcDwell,'Delay',mr.calcDuration(gzReph));%lims.adcDeadTime);

% extend spiral_grad_shape by repeating the last sample
% this is needed to accomodate for the ADC tuning delay
spiral_grad_shape = [spiral_grad_shape spiral_grad_shape(:,end)];

% readout grad
gx = mr.makeArbitraryGrad('x',spiral_grad_shape(1,:),'Delay',mr.calcDuration(gzReph));
gy = mr.makeArbitraryGrad('y',spiral_grad_shape(2,:),'Delay',mr.calcDuration(gzReph));

% spoilers
gz_spoil=mr.makeTrapezoid('z',sys,'Area',deltak*Nx*4);
gx_spoil=mr.makeExtendedTrapezoid('x','times',[0 mr.calcDuration(gz_spoil)],'amplitudes',[spiral_grad_shape(1,end),0]); %todo: make a really good spoiler
gy_spoil=mr.makeExtendedTrapezoid('y','times',[0 mr.calcDuration(gz_spoil)],'amplitudes',[spiral_grad_shape(2,end),0]); %todo: make a really good spoiler

% because of the ADC alignment requirements the sampling window possibly
% extends past the end of the trajectory (these points will have to be
% discarded in the reconstruction, which is no problem). However, the
% ramp-down parts and the Z-spoiler now have to be added to the readout
% block otherwise there will be a gap inbetween
% gz_spoil.delay=mr.calcDuration(gx);
% gx_spoil.delay=gz_spoil.delay;
% gy_spoil.delay=gz_spoil.delay;
% gx_combined=mr.addGradients([gx,gx_spoil], lims);
% gy_combined=mr.addGradients([gy,gy_spoil], lims);
% gz_combined=mr.addGradients([gzReph,gz_spoil], lims);

% Define sequence blocks
for s=1:Nslices
    seq.addBlock(rf_fs,gz_fs); % fat-sat
    rf.freqOffset=gz.amplitude*sliceThickness*(s-1-(Nslices-1)/2);
    seq.addBlock(rf,gz);
    seq.addBlock(mr.rotate('z',phi,gzReph,gx,gy,adc));
    seq.addBlock(mr.rotate('z',phi,gx_spoil,gy_spoil,gz_spoil));
    %seq.addBlock(gx_combined,gy_combined,gz_combined,adc);
end

% check whether the timing of the sequence is correct
[ok, error_report]=seq.checkTiming;

if (ok)
    fprintf('Timing check passed successfully\n');
else
    fprintf('Timing check failed! Error listing follows:\n');
    fprintf([error_report{:}]);
    fprintf('\n');
end

%
seq.setDefinition('FOV', [fov fov sliceThickness]);
seq.setDefinition('Name', 'spiral');
seq.setDefinition('MaxAdcSegmentLength', adcSamplesPerSegment);

seq.write('spiral.seq');   % Output sequence for scanner

% the sequence is ready, so let's see what we got
seq.plot();             % Plot sequence waveforms

%% k-space trajectory calculation
[ktraj_adc, t_adc, ktraj, t_ktraj, t_excitation, t_refocusing] = seq.calculateKspacePP();

% plot k-spaces
figure; plot(t_ktraj, ktraj'); title('k-space components as functions of time'); % plot the entire k-space trajectory
figure; plot(ktraj(1,:),ktraj(2,:),'b'); % a 2D plot
hold;plot(ktraj_adc(1,:),ktraj_adc(2,:),'r.'); title('2D k-space');

% seq.install('siemens');


function im_ext = bound_extension(im,By,Bx,type);

% im_ext = bound_extension(im,B,type);
%
% Extend an image for avoiding boundary artifacts,
%
%   By, Bx:    widths of the added stripes.
%   type:   'mirror'        Mirror extension
%           'mirror_nr':    Mirror without repeating the last pixel
%           'circular':     fft2-like
%           'zeros'

% Javier Portilla, Universidad de Granada, Jan 2004

[Ny,Nx,Nc] = size(im);

im_ext = zeros(Ny+2*By,Nx+2*Bx,Nc);
im_ext(By+1:Ny+By,Bx+1:Nx+Bx,:) = im;

if strcmp(type,'mirror'),

    im_ext(1:By,:,:) = im_ext(2*By:-1:By+1,:,:);
    im_ext(:,1:Bx,:) = im_ext(:,2*Bx:-1:Bx+1,:);
    im_ext(Ny+1+By:Ny+2*By,:,:) = im_ext(Ny+By:-1:Ny+1,:,:);
    im_ext(:,Nx+1+Bx:Nx+2*Bx,:) = im_ext(:,Nx+Bx:-1:Nx+1,:);
    im_ext(1:By,1:Bx,:) = im_ext(2*By:-1:By+1,2*Bx:-1:Bx+1,:);
    im_ext(Ny+1+By:Ny+2*By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(Ny+By:-1:Ny+1,Nx+Bx:-1:Nx+1,:);
    im_ext(1:By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(2*By:-1:By+1,Nx+Bx:-1:Nx+1,:);
    im_ext(Ny+1+By:Ny+2*By,1:Bx,:) = im_ext(Ny+By:-1:Ny+1,2*Bx:-1:Bx+1,:);

elseif strcmp(type,'mirror_nr'),

    im_ext(1:By,:,:) = im_ext(2*By+1:-1:By+2,:,:);
    im_ext(:,1:Bx,:) = im_ext(:,2*Bx+1:-1:Bx+2,:);
    im_ext(Ny+1+By:Ny+2*By,:,:) = im_ext(Ny+By-1:-1:Ny,:,:);
    im_ext(:,Nx+1+Bx:Nx+2*Bx,:) = im_ext(:,Nx+Bx-1:-1:Nx,:);
    im_ext(1:By,1:Bx,:) = im_ext(2*By+1:-1:By+2,2*Bx+1:-1:Bx+2,:);
    im_ext(Ny+1+By:Ny+2*By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(Ny+By-1:-1:Ny,Nx+Bx-1:-1:Nx,:);
    im_ext(1:By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(2*By+1:-1:By+2,Nx+Bx-1:-1:Nx,:);
    im_ext(Ny+1+By:Ny+2*By,1:Bx,:) = im_ext(Ny+By-1:-1:Ny,2*Bx+1:-1:Bx+2,:);

elseif strcmp(type,'circular'),

    im_ext(1:By,:,:) =  im_ext(Ny+1:Ny+By,:,:);
    im_ext(:,1:Bx,:) = im_ext(:,Nx+1:Nx+Bx,:);
    im_ext(Ny+1+By:Ny+2*By,:,:) = im_ext(By+1:2*By,:,:);
    im_ext(:,Nx+1+Bx:Nx+2*Bx,:) = im_ext(:,Bx+1:2*Bx,:);
    im_ext(1:By,1:Bx,:) = im_ext(Ny+1:Ny+By,Nx+1:Nx+Bx,:);
    im_ext(Ny+1+By:Ny+2*By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(By+1:2*By,Bx+1:2*Bx,:);
    im_ext(1:By,Nx+1+Bx:Nx+2*Bx,:) = im_ext(Ny+1:Ny+By,Bx+1:2*Bx,:);
    im_ext(Ny+1+By:Ny+2*By,1:Bx,:) = im_ext(By+1:2*By,Nx+1:Nx+Bx,:);

end


function hf = r8_hyper_2f1 ( a, b, c, x )

%*****************************************************************************80
%
%% R8_HYPER_2F1 evaluates the hypergeometric function F(A,B,C,X).
%
%  Discussion:
%
%    A minor bug was corrected.  The HW variable, used in several places as
%    the "old" value of a quantity being iteratively improved, was not
%    being initialized.  JVB, 11 February 2008.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Shanjie Zhang, Jianming Jin.
%    MATLAB version by John Burkardt.
%
%    The F77 original version of this routine is copyrighted by
%    Shanjie Zhang and Jianming Jin.  However, they give permission to
%    incorporate this routine into a user program provided that the copyright
%    is acknowledged.
%
%  Reference:
%
%    Shanjie Zhang, Jianming Jin,
%    Computation of Special Functions,
%    Wiley, 1996,
%    ISBN: 0-471-11963-6,
%    LC: QA351.C45
%
%  Parameters:
%
%    Input, real A, B, C, X, the arguments of the function.
%    C must not be equal to a nonpositive integer.
%    X < 1.
%
%    Output, real HF, the value of the function.
%
  el = 0.5772156649015329;

  l0 = ( c == floor ( c ) ) && ( c < 0.0 );
  l1 = ( 1.0 - x < 1.0E-15 ) && ( c - a - b <= 0.0 );
  l2 = ( a == floor ( a ) ) && ( a < 0.0 );
  l3 = ( b == floor ( b ) ) && ( b < 0.0 );
  l4 = ( c - a == floor ( c - a ) ) && ( c - a <= 0.0 );
  l5 = ( c - b == floor ( c - b ) ) && ( c - b <= 0.0 );

  if ( l0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_HYPER_2F1 - Fatal error!\n' );
    fprintf ( 1, '  The hypergeometric series is divergent.\n' );
    fprintf ( 1, '  C is integral and negative.\n' );
    fprintf ( 1, '  C = %f\n', c );
    error ( 'R8_HYPER_F1 - Fatal error!' );
  end

  if ( l1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_HYPER_2F1 - Fatal error!\n' );
    fprintf ( 1, '  The hypergeometric series is divergent.\n' );
    fprintf ( 1, '  1 = X < 0, C - A - B <= 0.\n' );
    fprintf ( 1, '  A = %f\n', a );
    fprintf ( 1, '  B = %f\n', b );
    fprintf ( 1, '  C = %f\n', c );
    fprintf ( 1, '  X = %f\n', x );
    error ( 'R8_HYPER_F1 - Fatal error!' );
  end

  if ( 0.95 < x )
    eps = 1.0E-08;
  else
    eps = 1.0E-15;
  end

  if ( x == 0.0 || a == 0.0 || b == 0.0 )

    hf = 1.0;
    return

  elseif ( 1.0 - x == eps && 0.0 < c - a - b )

    gc = gamma ( c );
    gcab = gamma ( c - a - b );
    gca = gamma ( c - a );
    gcb = gamma ( c - b );
    hf = gc * gcab / ( gca * gcb );
    return

  elseif ( 1.0 + x <= eps && abs ( c - a + b - 1.0 ) <= eps )

    g0 = sqrt ( pi ) * 2.0^( - a );
    g1 = gamma ( c );
    g2 = gamma ( 1.0 + a / 2.0 - b );
    g3 = gamma ( 0.5 + 0.5 * a );
    hf = g0 * g1 / ( g2 * g3 );
    return

  elseif ( l2 || l3 )

    if ( l2 )
      nm = floor ( abs ( a ) );
    end

    if ( l3 )
      nm = floor ( abs ( b ) );
    end

    hf = 1.0;
    r = 1.0;

    for k = 1 : nm
      r = r * ( a + k - 1.0 ) * ( b + k - 1.0 ) ...
        / ( k * ( c + k - 1.0 ) ) * x;
      hf = hf + r;
    end

    return

  elseif ( l4 || l5 )

    if ( l4 )
      nm = floor ( abs ( c - a ) );
    end

    if ( l5 )
      nm = floor ( abs ( c - b ) );
    end

    hf = 1.0;
    r  = 1.0;
    for k = 1 : nm
      r = r * ( c - a + k - 1.0 ) * ( c - b + k - 1.0 ) ...
        / ( k * ( c + k - 1.0 ) ) * x;
      hf = hf + r;
    end
    hf = ( 1.0 - x )^( c - a - b ) * hf;
    return

  end

  aa = a;
  bb = b;
  x1 = x;

  if ( x < 0.0 )
    x = x / ( x - 1.0 );
    if ( a < c && b < a && 0.0 < b )
      a = bb;
      b = aa;
    end
    b = c - b;
  end

  if ( 0.75 <= x )

    gm = 0.0;

    if ( abs ( c - a - b - floor ( c - a - b ) ) < 1.0E-15 )

      m = floor ( c - a - b );
      ga = gamma ( a );
      gb = gamma ( b );
      gc = gamma ( c );
      gam = gamma ( a + m );
      gbm = gamma ( b + m );

      pa = r8_psi ( a );
      pb = r8_psi ( b );

      if ( m ~= 0 )
        gm = 1.0;
      end

      for j = 1 : abs ( m ) - 1
        gm = gm * j;
      end

      rm = 1.0;
      for j = 1 : abs ( m )
        rm = rm * j;
      end

      f0 = 1.0;
      r0 = 1.0;
      r1 = 1.0;
      sp0 = 0.0;
      sp = 0.0;

      if ( 0 <= m )

        c0 = gm * gc / ( gam * gbm );
        c1 = - gc * ( x - 1.0 )^m / ( ga * gb * rm );

        for k = 1 : m - 1
          r0 = r0 * ( a + k - 1.0 ) * ( b + k - 1.0 ) ...
            / ( k * ( k - m ) ) * ( 1.0 - x );
          f0 = f0 + r0;
        end

        for k = 1 : m
          sp0 = sp0 + 1.0 / ( a + k - 1.0 ) ...
            + 1.0 / ( b + k - 1.0 ) - 1.0 / k;
        end

        f1 = pa + pb + sp0 + 2.0 * el + log ( 1.0 - x );
        hw = f1;

        for k = 1 : 250

          sp = sp + ( 1.0 - a ) / ( k * ( a + k - 1.0 ) ) ...
            + ( 1.0 - b ) / ( k * ( b + k - 1.0 ) );

          sm = 0.0;
          for j = 1 : m
            sm = sm + ( 1.0 - a ) ...
              / ( ( j + k ) * ( a + j + k - 1.0 ) ) ...
              + 1.0 / ( b + j + k - 1.0 );
          end

          rp = pa + pb + 2.0 * el + sp + sm + log ( 1.0 - x );

          r1 = r1 * ( a + m + k - 1.0 ) * ( b + m + k - 1.0 ) ...
            / ( k * ( m + k ) ) * ( 1.0 - x );

          f1 = f1 + r1 * rp;

          if ( abs ( f1 - hw ) < abs ( f1 ) * eps )
            break
          end

          hw = f1;

        end

        hf = f0 * c0 + f1 * c1;

      elseif ( m < 0 )

        m = - m;
        c0 = gm * gc / ( ga * gb * ( 1.0 - x )^m );
        c1 = - ( - 1 )^m * gc / ( gam * gbm * rm );

        for k = 1 : m - 1
          r0 = r0 * ( a - m + k - 1.0 ) * ( b - m + k - 1.0 ) ...
            / ( k * ( k - m ) ) * ( 1.0 - x );
          f0 = f0 + r0;
        end

        for k = 1 : m
          sp0 = sp0 + 1.0 / k;
        end

        f1 = pa + pb - sp0 + 2.0 * el + log ( 1.0 - x );
        hw = f1;

        for k = 1 : 250

          sp = sp + ( 1.0 - a ) ...
            / ( k * ( a + k - 1.0 ) ) ...
            + ( 1.0 - b ) / ( k * ( b + k - 1.0 ) );

          sm = 0.0;
          for j = 1 : m
            sm = sm + 1.0 / ( j + k );
          end

          rp = pa + pb + 2.0 * el + sp - sm + log ( 1.0 - x );

          r1 = r1 * ( a + k - 1.0 ) * ( b + k - 1.0 ) ...
            / ( k * ( m + k ) ) * ( 1.0 - x );

          f1 = f1 + r1 * rp;

          if ( abs ( f1 - hw ) < abs ( f1 ) * eps )
            break
          end

          hw = f1;

        end

        hf = f0 * c0 + f1 * c1;

      end

    else

      ga = gamma ( a );
      gb = gamma ( b );
      gc = gamma ( c );
      gca = gamma ( c - a );
      gcb = gamma ( c - b );
      gcab = gamma ( c - a - b );
      gabc = gamma ( a + b - c );
      c0 = gc * gcab / ( gca * gcb );
      c1 = gc * gabc / ( ga * gb ) * ( 1.0 - x )^( c - a - b );
      hf = 0.0;
      hw = hf;
      r0 = c0;
      r1 = c1;

      for k = 1 : 250

        r0 = r0 * ( a + k - 1.0 ) * ( b + k - 1.0 ) ...
          / ( k * ( a + b - c + k ) ) * ( 1.0 - x );

        r1 = r1 * ( c - a + k - 1.0 ) * ( c - b + k - 1.0 ) ...
          / ( k * ( c - a - b + k ) ) * ( 1.0 - x );

        hf = hf + r0 + r1;

        if ( abs ( hf - hw ) < abs ( hf ) * eps )
          break
        end

        hw = hf;

      end

      hf = hf + c0 + c1;

    end

  else

    a0 = 1.0;

    if ( a < c && c < 2.0 * a && b < c && c < 2.0 * b )

      a0 = ( 1.0 - x )^( c - a - b );
      a = c - a;
      b = c - b;

    end

    hf = 1.0;
    hw = hf;
    r = 1.0;

    for k = 1 : 250

      r = r * ( a + k - 1.0 ) * ( b + k - 1.0 ) ...
        / ( k * ( c + k - 1.0 ) ) * x;

      hf = hf + r;

      if ( abs ( hf - hw ) <= abs ( hf ) * eps )
        break
      end

      hw = hf;

    end

    hf = a0 * hf;

  end

  if ( x1 < 0.0 )
    x = x1;
    c0 = 1.0 / ( 1.0 - x )^aa;
    hf = c0 * hf;
  end

  if ( 120 < k )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_HYPER_2F1 - Warning!\n' );
    fprintf ( 1, '  A large number of iterations were needed.\n' );
    fprintf ( 1, '  The accuracy of the results should be checked.\n' );
  end

  return
end


function [out] = melt_3(p1,p2,T,S1,S2,St,dt,varargin)
%melt_3

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

% Flux function
% ------------------
% Description:  Glacier melt provided no snow is stored on the ice layer
% Constraints:  f <= S1/dt
% @(Inputs):    p1   - degree-day factor [mm/oC/d]
%               p2   - temperature threshold for snowmelt [oC]
%               T    - current temperature [oC]
%               S1   - current storage in glacier [mm]
%               S2   - current storage in snowpack [mm]
%               St   - storage in S2 threshold below which glacier melt occurs [mm]
%               dt   - time step size [d]
%               varargin(1) - smoothing variable r (default 0.01)
%               varargin(2) - smoothing variable e (default 5.00)

if size(varargin,2) == 0
    out = min(max(p1*(T-p2),0),S1/dt).*smoothThreshold_storage_logistic(S2,St);
elseif size(varargin,2) == 1
    out = min(max(p1*(T-p2),0),S1/dt).*smoothThreshold_storage_logistic(S2,St,varargin(1));
elseif size(varargin,2) == 2
    out = min(max(p1*(T-p2),0),S1/dt).*smoothThreshold_storage_logistic(S2,St,varargin(1),varargin(2));
end

end


function [nyear,ncoef]=decode_coeff_pointer(np);
%USAGE:  [nyear,ncoef]=decode_coeff_pointer(np);
%
% Decode igrf10syn gh pointer to extract year number and ncoef
% 1<=ncoef<=120 for nyear=1:19
% 1<=ncoef<=195 for nyear=20:24
%
% np=(nyear-1)*120+ncoef for np<=np1_max
% np=np1_max+(nyear-19)*195+ncoef; for nyear=20:24
%
% year=1995+(nyear-1)*5;
%
np1_max=2280;
if np<=np1_max
    nyear=floor(np/120)+1;
    ncoef=np-(nyear-1)*120;
    if ncoef==0
        ncoef=120;
        nyear=nyear+1;
    end
elseif np<=3255
   np2=np-np1_max;
   nyear=floor(np2/195)+1;
   ncoef=np2-(nyear-1)*195;
   if ncoef==0
       ncoef=195;
       nyear=nyear+1;
   end
   nyear=19+nyear;
else
    error('np>3255!')
end
return


function [Fnew, obj, M,k,x,u,n,deg,linears,nonlinears,vecConstraints,isinequality,ulong] = momentmodel(F,obj,k,keepnonlinears)

if nargin < 2
    obj = [];
end
if nargin < 3
    k = [];
end
if nargin < 4
    keepnonlinears = 0;
end

vecConstraints = [];
sdpConstraints = [];
isinequality = [];
binaries = [];
xvars = [];
Fnew = ([]);
for i = 1:length(F)
    if is(F(i),'elementwise')
        X = sdpvar(F(i));
        vecConstraints = [vecConstraints;X(:)];
        isinequality = [isinequality ones(1,prod(size(X)))];
        xvars = [xvars depends(X(:))];
    elseif is(F(i),'equality')
        X = sdpvar(F(i));
        if is(X,'symmetric')
            X = X(find(triu(ones(length(X)))));
        end
        vecConstraints = [vecConstraints;-X(:)];
        isinequality = [isinequality zeros(1,prod(size(X)))];
        xvars = [xvars depends(X(:))];
    elseif is(F(i),'sdp')
        sdpConstraints{end+1} = sdpvar(F(i));
        xvars = [xvars depends(F(i))];
    elseif is(F(i),'binary')
        binaries = [binaries getvariables(F(i))];
    else
        Fnew = Fnew+F(i); % Should only be SOCP constraints
    end
end

% Recover the involved variables
x = recover(unique([depends(obj) xvars]));
n = length(x);

% Check degrees of constraints
deg = [];
for i = 1:length(vecConstraints)
    deg(end+1) = degree(vecConstraints(i));
end
for i = 1:length(sdpConstraints)
    deg(end+1) = degree(sdpConstraints{i});
end
if isempty(deg)
    deg = 0;
end

% Create lowest possible relaxation if k=[]
d = ceil((max(degree(obj),max(deg)))/2);
k_min = d;
if isempty(k)
    k = k_min;
else
    if k<k_min
        error('Higher order relaxation needed')
    end
end

% Generate monomials of order k
u{k} = monolist(x,k);
ulong{k} = monolist(x,2*k);

% Largest moment matrix. NOTE SHIFT M{k+1} = M_k.
M{k+1}=u{k}*u{k}';
% Moment matrices easily generated with this trick
% The matrices will NOT be rank-1 since the products
% generate the relaxed variables

% ... and lower degree localization matrices
M{1} = 1;
for i = 1:1:k-1;
    n_i = round(factorial(n+k-i)/(factorial(n)*factorial(k-i)));
    M{k-i+1} = M{k+1}(1:n_i,1:n_i);
end

% Lasserres relaxation (Lasserre, SIAM J. OPTIM, 11(3) 796-817)
Fmoments = (M{k+1}>=0);
for i = 1:length(vecConstraints)
    if isinequality(i)
        v_k = floor((degree(vecConstraints(i))+1)/2);
        Localizer = vecConstraints(i)*M{k-v_k+1};
        if isa(vecConstraints(i),'double')
            if vecConstraints(i)<0
                error('Problem is trivially infeasible due to negative constant')
            else
                continue
            end
        end
        Fmoments = Fmoments+(Localizer>=0);
    else
        if isa(vecConstraints(i),'double')
            if vecConstraints(i)~=0
                error('Problem is trivially infeasible due to non-zero constant in equality constraints')
            else
                continue
            end
        end
        Localizer = vecConstraints(i)*monolist(x,2*k-degree(vecConstraints(i)));
        Fmoments = Fmoments+(Localizer==0);
    end
end
for i = 1:length(sdpConstraints)
    v_k = floor((degree(sdpConstraints{i})+1)/2);
    Fmoments = Fmoments+(kron(M{k-v_k+1},sdpConstraints{i})>=0);
end

% Add them all
Fnew = Fnew + Fmoments;

% Get all binary and reduce problem
binaries = union(binaries,yalmip('binvariables'));
if ~isempty(binaries)
    if isa(obj,'sdpvar')
        obj = eliminateBinary(obj,binaries);
    end
    for i = 1:length(Fmoments)
        Fnew(i) = eliminateBinary(Fnew(i),binaries);
    end
    for i = 2:1:k+1;
        M{i} = eliminateBinary(M{i},binaries);
    end
end

vars = getvariables(Fnew);
for i = 1:length(M)
    vars = [vars getvariables(M{i})];
end
vars = unique([vars getvariables(obj)]);
[mt,variabletype] = yalmip('monomtable');
nonlinears = vars(find(variabletype(vars)));
newLinear = sdpvar(length(nonlinears),1);

if isa(obj,'sdpvar')
    obj = variablereplace(obj,nonlinears,getvariables(newLinear));
end
for i = 1:length(M)
    if isa(M{i},'sdpvar')
        M{i} = variablereplace(M{i},nonlinears,getvariables(newLinear));
    end
end
linears = getvariables(newLinear);
Fnew = variablereplace(Fnew,nonlinears,getvariables(newLinear));
linears = recover(linears);
nonlinears = recover(nonlinears);

end


function bessel_y1_spherical_values_test ( )

%*****************************************************************************80
%
%% BESSEL_Y1_SPHERICAL_VALUES_TEST tests BESSEL_Y1_SPHERICAL_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSEL_Y1_SPHERICAL_VALUES_TEST:\n' );
  fprintf ( 1, '  BESSEL_Y1_SPHERICAL_VALUES stores values of\n' );
  fprintf ( 1, '  the y1 spherical Bessel function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X            FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

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

    if ( n_data == 0 )
      break
    end

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

  end

  return
end


  function ob = Godwt1(mask, varargin)
%|function ob = Godwt1(mask, varargin)
%| Construct Godwt1 object that computes orthonormal discrete wavelet
%| decomposition of a signal with dimensions [(N)].
%| This is useful for sparsity regularization (aka compressed sensing).
%| (1D or 2D wavelets only)
%|
%| in
%|	'mask'	logical [(Nd)]	image-domain mask, often true(nx,ny)
%|
%| options
%|	'level'	int	decomposition level (default 1)
%|	'wname'	char	wavelet name. default: 'haar'
%|
%| out
%|	ob	[*N np]	fatrix  object, where np = sum(mask(:))
%|
%|
%| Copyright 2012-05-17, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if nargin == 1 && streq(mask, 'test'), Godwt1_test, return, end

arg.mask = mask;
arg.level = 1;
arg.wname = 'haar';
arg.abs = false;
arg = vararg_pair(arg, varargin);

if isempty(arg.mask), fail 'must provide a mask', end

% transform dimension
idim = size(arg.mask);
if idim(end) == 1
	idim = idim(1:end-1);
end

switch numel(idim)
case 1
	forw = @(arg, x) ir_odwt1(x, ...
		'level', arg.level, 'wname', arg.wname, 'abs', arg.abs);
	back = @(arg, y) fatrix2_maskit(arg.mask, ir_odwt1(y, 'adj', 1, ...
		'level', arg.level, 'wname', arg.wname, 'abs', arg.abs));
	does_many = true;
case 2
	forw = @(arg, x) ir_odwt2(x, ...
		'level', arg.level, 'wname', arg.wname, 'abs', arg.abs);
	back = @(arg, y) fatrix2_maskit(arg.mask, ir_odwt2(y, 'adj', 1, ...
		'level', arg.level, 'wname', arg.wname, 'abs', arg.abs));
	does_many = false;
otherwise
        fail('dim %d unsupported', numel(idim))
end

% build fatrix2 object
arg.idim = idim;
ob = fatrix2('idim', idim, 'odim', idim, 'arg', arg, ...
	'abs', @Godwt1_abs, 'meth', {'codes', @Godwt1_codes, '()'}, ...
	'forw', forw, 'back', back, 'does_many', does_many);

% Godwt1_abs()
function ob = Godwt1_abs(ob)
ob.arg.abs = true;

% Godwt1_codes()
function codes = Godwt1_codes(arg)

switch numel(arg.idim)
case 1
	[dummy codes] = ir_odwt1(zeros([arg.idim 1]), ...
		'level', arg.level, 'wname', arg.wname);
case 2
	[dummy codes] = ir_odwt2(zeros(arg.idim), ...
		'level', arg.level, 'wname', arg.wname);
otherwise
        fail('dim %d unsupported', numel(idim))
end

% Godwt1_test()
function Godwt1_test

if 1 % 1d
	mask = true(8*3,1);
	mask(1:3) = false;
	level = 3;
	U = Godwt1(mask, 'level', level);
%	abs(U) % todo

	if im
		im plc 1 2
		im(1, full(U))
		im(2, U.codes)
		drawnow
	end

	fatrix2_tests(U, 'complex', 1) % check complex data
	test_adjoint(U, 'complex', 1, 'tolre', 1e-9);
end

if 1 % 2d
	mask = true(8*3,16);
	mask(1:3) = false;
	level = 3;
	wname = 'haar';
	wname = 'sym2';
	U = Godwt1(mask, 'level', level, 'wname', wname);

	x = ellipse_im(size(mask));
	if im
		im plc 1 2
		im(1, x)
		im(2, U * x)
		drawnow
	end

	fatrix2_tests(U, 'complex', 1) % check complex data
	test_adjoint(U, 'complex', 1, 'tolre', 1e-9, 'big', 1);
end


function [ a, det ] = dpodi ( a, lda, n, job )

%*****************************************************************************80
%
%% DPODI computes the determinant and inverse of a certain matrix.
%
%  Discussion:
%
%    The matrix is real symmetric positive definite.
%    DPODI uses the factors computed by DPOCO, DPOFA or DQRDC.
%
%    A division by zero will occur if the input factor contains
%    a zero on the diagonal and the inverse is requested.
%    It will not occur if the subroutines are called correctly
%    and if DPOCO or DPOFA has set INFO == 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the output A from DPOCO or DPOFA, or the output
%    X from DQRDC.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer N, the order of the matrix A.
%
%    Input, integer JOB, specifies the task.
%    11, both determinant and inverse.
%    01, inverse only.
%    10, determinant only.
%
%    Output, real A(LDA,N), if DPOCO or DPOFA was used to factor A then
%    DPODI produces the upper half of inverse(A).  If DQRDC was used to
%    decompose X then DPODI produces the upper half of inverse(X'*X)
%    where X' is the transpose.  Elements of A below the diagonal are
%    unchanged.  If the units digit of JOB is zero, A is unchanged.
%
%    Output, real DET(2), the determinant of A or of X'*X
%    if requested.
%      determinant = DET(1) * 10.0**DET(2)
%    with 1.0 <= DET(1) < 10.0 or DET(1) == 0.0.
%

%
%  Compute the determinant.
%
  if ( job / 10 ~= 0 )

    det(1) = 1.0;
    det(2) = 0.0;
    s = 10.0;

    for i = 1 : n

      det(1) = a(i,i) * a(i,i) * det(1);

      if ( det(1) == 0.0 )
        break
      end

      while ( det(1) < 1.0 )
        det(1) = s * det(1);
        det(2) = det(2) - 1.0;
      end

      while ( s <= det(1) )
        det(1) = det(1) / s;
        det(2) = det(2) + 1.0;
      end

    end

  end
%
%  Compute inverse(R).
%
  if ( mod ( job, 10 ) ~= 0 )

    for k = 1 : n

      a(k,k) = 1.0 / a(k,k);
      t = -a(k,k);
      a(1:k-1,k) = dscal ( k-1, t, a(1:k-1,k), 1 );

      for j = k+1 : n
        t = a(k,j);
        a(k,j) = 0.0;
        a(1:k,j) = daxpy ( k, t, a(1:k,k), 1, a(1:k,j), 1 );
      end

    end
%
%  Form inverse(R) * (inverse(R))'.
%
    for j = 1 : n
      for k = 1 : j-1
        t = a(k,j);
        a(1:k,k) = daxpy ( k, t, a(1:k,j), 1, a(1:k,k), 1 );
      end
      t = a(j,j);
      a(1:j,j) = dscal ( j, t, a(1:j,j), 1 );
    end

  end

  return
end


dt = 0.02;
sim_t = 20;
x0 = [0;5;0];

params.v = 1; % velocity
params.u_max = 3; % max yaw rate (left)
params.u_min = -3; % min yaw rate (right)

% Obstacle position
params.xo = 5;
params.yo = 4;
% Obstacle radius
params.d = 2;
params.cbf_gamma0 = 1;
% Desired target point
params.xd = 12;
params.yd = 0;

params.clf.rate = 0.5;
params.weight.slack = 10;

params.cbf.rate = 1;

dubins = DubinsCar(params);

odeFun = @dubins.dynamics;
controller = @dubins.ctrlCbfClfQp;
odeSolver = @ode45;

total_k = ceil(sim_t / dt);
x = x0;
t = 0;
% initialize traces.
xs = zeros(total_k, dubins.xdim);
ts = zeros(total_k, 1);
us = zeros(total_k-1, 1);
Vs = zeros(total_k-1, 1);
hs = zeros(total_k-1, 1);
xs(1, :) = x0';
ts(1) = t;
for k = 1:total_k-1
    t
    % Determine control input.
    % dV_hat: analytic Vdot based on model.
    [u, slack, h, V] = controller(x);
    us(k, :) = u';
    hs(k) = h;
    Vs(k) = V;

    % Run one time step propagation.
    [ts_temp, xs_temp] = odeSolver(@(t, s) odeFun(t, s, u), [t t+dt], x);
    x = xs_temp(end, :)';

    xs(k+1, :) = x';
    ts(k+1) = ts_temp(end);
    t = t + dt;
end

plot_results(ts, xs, us, hs, [params.xo;params.yo], params.d)

function plot_results(t, xs, us, hs, p_o, r_o)

figure
subplot(3,1,1)
plot(t, xs(:,1))
xlabel('t')
ylabel('x [m]')

subplot(3,1,2)
plot(t, xs(:,2))
xlabel('t')
ylabel('y [m]')

subplot(3,1,3)
plot(t, xs(:,3))
xlabel('t')
ylabel('theta [rad]')

figure
plot(t(1:end-1), us)
xlabel('t')
ylabel('u [rad/s]')

lim_min = min(min(xs(:, 1)), min(xs(:, 2)));
lim_max = max(max(xs(:, 1)), max(xs(:, 2)));
lim_min = min([lim_min, p_o(1)-r_o, p_o(2)-r_o]);
lim_max = max([lim_max, p_o(1)+r_o, p_o(2)+r_o]);

figure
plot(xs(:, 1), xs(:, 2));
draw_circle(p_o, r_o);

xlim([lim_min, lim_max]);
ylim([lim_min, lim_max]);
xlabel('x [m]')
ylabel('y [m]')

figure
plot(t(1:end-1), hs)
xlabel('t')
ylabel('cbf h(s)');

end

function h = draw_circle(center,r)
hold on
th = 0:pi/50:2*pi;
xunit = r * cos(th) + center(1);
yunit = r * sin(th) + center(2);
h = plot(xunit, yunit);
hold off

end


function amanatidesWooAlgorithm(origin, direction, grid3D, verbose)
% A fast and simple voxel traversal algorithm through a 3D space partition (grid)
% proposed by J. Amanatides and A. Woo (1987).
%
% Input:
%    origin.
%    direction.
%    grid3D: grid dimensions (nx, ny, nz, minBound, maxBound).
% Author:
%    Jesús P. Mena-Chalco.

    if (verbose)
        figure;
        hold on;
        text(origin(1), origin(2), origin(3), 'origin');
        plot3(origin(1), origin(2), origin(3), 'k.', 'MarkerSize', 15);
        quiver3(origin(1), origin(2), origin(3), direction(1), direction(2), direction(3), 30);

        vmin = grid3D.minBound';
        vmax = grid3D.maxBound';
        BoxVertices = [vmax(1) vmin(2) vmin(3); vmax(1) vmax(2) vmin(3); vmin(1) vmax(2) vmin(3); vmin(1) vmax(2) vmax(3); vmin(1) vmin(2) vmax(3); vmax(1) vmin(2) vmax(3); vmin; vmax ];
        BoxFaces = [1 2 3 7; 1 2 8 6; 1 6 5 7; 7 5 4 3; 2 8 4 3; 8 6 5 4];
        h = patch('Vertices',BoxVertices,'Faces',BoxFaces,'FaceColor','yellow');
        set(h, 'FaceAlpha', 0.1);

        view(60,30);
        axis tight;
        xlabel('x');
        ylabel('y');
        zlabel('z');
        grid on;
    end;

    [flag, tmin] = rayBoxIntersection(origin, direction, grid3D.minBound, grid3D.maxBound);

    if (flag==0)
        disp('\n The ray does not intersect the grid');
    else
        if (tmin<0)
            tmin = 0;
        end;

        start   = origin + tmin*direction;
        boxSize = grid3D.maxBound-grid3D.minBound;

        if (verbose)
            plot3(start(1), start(2), start(3), 'r.', 'MarkerSize', 15);
        end;

        x = floor( ((start(1)-grid3D.minBound(1))/boxSize(1))*grid3D.nx )+1;
        y = floor( ((start(2)-grid3D.minBound(2))/boxSize(2))*grid3D.ny )+1;
        z = floor( ((start(3)-grid3D.minBound(3))/boxSize(3))*grid3D.nz )+1;

        if (x==(grid3D.nx+1));  x=x-1;  end;
        if (y==(grid3D.ny+1));  y=y-1;  end;
        if (z==(grid3D.nz+1));  z=z-1;  end;

        if (direction(1)>=0)
            tVoxelX = (x)/grid3D.nx;
            stepX = 1;
        else
            tVoxelX = (x-1)/grid3D.nx;
            stepX = -1;
        end;

        if (direction(2)>=0)
            tVoxelY = (y)/grid3D.ny;
            stepY = 1;
        else
            tVoxelY = (y-1)/grid3D.ny;
            stepY = -1;
        end;

        if (direction(3)>=0)
            tVoxelZ = (z)/grid3D.nz;
            stepZ = 1;
        else
            tVoxelZ = (z-1)/grid3D.nz;
            stepZ = -1;
        end;

        voxelMaxX  = grid3D.minBound(1) + tVoxelX*boxSize(1);
        voxelMaxY  = grid3D.minBound(2) + tVoxelY*boxSize(2);
        voxelMaxZ  = grid3D.minBound(3) + tVoxelZ*boxSize(3);

        tMaxX      = tmin + (voxelMaxX-start(1))/direction(1);
        tMaxY      = tmin + (voxelMaxY-start(2))/direction(2);
        tMaxZ      = tmin + (voxelMaxZ-start(3))/direction(3);

        voxelSizeX = boxSize(1)/grid3D.nx;
        voxelSizeY = boxSize(2)/grid3D.ny;
        voxelSizeZ = boxSize(3)/grid3D.nz;

        tDeltaX    = voxelSizeX/abs(direction(1));
        tDeltaY    = voxelSizeY/abs(direction(2));
        tDeltaZ    = voxelSizeZ/abs(direction(3));

        while ( (x<=grid3D.nx)&&(x>=1) && (y<=grid3D.ny)&&(y>=1) && (z<=grid3D.nz)&&(z>=1) )

            if (verbose)
                fprintf('\nIntersection: voxel = [%d %d %d]', [x y z]);

                t1 = [(x-1)/grid3D.nx, (y-1)/grid3D.ny, (z-1)/grid3D.nz ]';
                t2 = [  (x)/grid3D.nx,  (y)/grid3D.ny,    (z)/grid3D.nz ]';

                vmin = (grid3D.minBound + t1.*boxSize)';
                vmax = (grid3D.minBound + t2.*boxSize)';

                smallBoxVertices = [vmax(1) vmin(2) vmin(3); vmax(1) vmax(2) vmin(3); vmin(1) vmax(2) vmin(3); vmin(1) vmax(2) vmax(3); vmin(1) vmin(2) vmax(3); vmax(1) vmin(2) vmax(3); vmin; vmax ];
                smallBoxFaces    = [1 2 3 7; 1 2 8 6; 1 6 5 7; 7 5 4 3; 2 8 4 3; 8 6 5 4];

                h = patch('Vertices', smallBoxVertices, 'Faces', smallBoxFaces, 'FaceColor', 'blue', 'EdgeColor', 'white');
                set(h,'FaceAlpha',0.2);
            end;

            % ---------------------------------------------------------- %
            % check if voxel [x,y,z] contains any intersection with the ray
            %
            %   if ( intersection )
            %       break;
            %   end;
            % ---------------------------------------------------------- %

            if (tMaxX < tMaxY)
                if (tMaxX < tMaxZ)
                    x = x + stepX;
                    tMaxX = tMaxX + tDeltaX;
                else
                    z = z + stepZ;
                    tMaxZ = tMaxZ + tDeltaZ;
                end;
            else
                if (tMaxY < tMaxZ)
                    y = y + stepY;
                    tMaxY = tMaxY + tDeltaY;
                else
                    z = z + stepZ;
                    tMaxZ = tMaxZ + tDeltaZ;
                end;
            end;
        end;
     end;
end


%#  NLP written by GAMS Convert at 06/20/02 11:29:54
%#
%#  Equation counts
%#     Total       E       G       L       N       X
%#         3       2       1       0       0       0
%#
%#  Variable counts
%#                 x       b       i     s1s     s2s      sc      si
%#     Total    cont  binary integer    sos1    sos2   scont    sint
%#         5       5       0       0       0       0       0       0
%#  FX     0       0       0       0       0       0       0       0
%#
%#  Nonzero counts
%#     Total   const      NL     DLL
%#        12       6       6       0
%#
%#  Reformualtion has removed 1 variable and 1 equation
%
%
%var x1 := 50, >= 50, <= 200;
%var x2 := 37.5, >= 37.5, <= 150;
%var x3 := 45, >= 45, <= 180;
%var x4;
%
%minimize obj: 0.00533*x1^2 + 11.669*x1 + 0.00889*x2^2 + 10.333*x2 + 0.00741*x3^
%              2 + 10.833*x3 + 653.1;
%
%subject to
%
%e2:  - (0.01*(0.0676*x1*x1 + 0.00953*x1*x2 - 0.00507*x1*x3 + 0.00953*x2*x1 +
%    0.0521*x2*x2 + 0.00901*x2*x3 - 0.00507*x3*x1 + 0.00901*x3*x2 + 0.0294*x3*x3
%    ) - 0.000766*x1 - 3.42e-5*x2 + 0.000189*x3) + x4 = 0.040357;
%
%e3:    x1 + x2 + x3 - x4 >= 210;

function test_ipopt

  auxdata = {} ;

  options.lb = [ 50, 37.5, 45, -Inf ] ;  % Lower bound on the variables.
  options.ub = [ 200, 150, 180, Inf ] ;  % Upper bound on the variables.

  % The constraint functions are bounded to zero
  options.cl = [ 0, 0 ]; %  constraints
  options.cu = [ 0, Inf ];

  % Set up the auxiliary data.
  options.auxdata = auxdata ;

  % Set the IPOPT options.
  options.ipopt.jac_d_constant   = 'no';
  options.ipopt.hessian_constant = 'no';
  options.ipopt.mu_strategy      = 'adaptive';
  options.ipopt.max_iter         = 400;
  options.ipopt.tol              = 1e-10;

  % The callback functions.
  funcs.objective         = @objective;
  funcs.constraints       = @constraints;
  funcs.gradient          = @gradient;
  funcs.jacobian          = @jacobian;
  funcs.jacobianstructure = @jacobianstructure;
  if true
    funcs.hessian           = @hessian;
    funcs.hessianstructure  = @hessianstructure;
    options.ipopt.derivative_test = 'second-order';
  else
    options.ipopt.hessian_approximation      = 'limited-memory';
    %options.ipopt.limited_memory_update_type = 'bfgs' ; % {bfgs}, sr1 = 6; % {6}
    %options.ipopt.limited_memory_update_type = 'sr1' ;
    options.ipopt.limited_memory_update_type = 'bfgs' ; % {bfgs}, sr1 = 6; % {6}
  end

  % Run IPOPT.
  x0 = [50, 37.5, 45, 0] ;

  tic
  [x, info] = ipopt_auxdata(x0,funcs,options);
  elapsed = toc ;

  info;

  x

end

%%
% map the indices with the corresponding index in the spase matrix
function f = objective(x,auxdata)
  f = 0.00533*x(1)^2 + 11.669*x(1) + ...
      0.00889*x(2)^2 + 10.333*x(2) + ...
      0.00741*x(3)^2 + 10.833*x(3) + 653.1 ;
end

%%
% map the indices with the corresponding index in the spase matrix
function g = gradient(x,auxdata)
  g = [ 0.01066*x(1) + 11.669, 0.01778*x(2) + 10.333, 0.01482*x(3) + 10.833, 0 ] ;
end

function f = constraints(x,auxdata)
  f = zeros(2,1) ;
  f(1) = - (0.01*(0.0676*x(1)^2 + 0.00953*x(1)*x(2) - 0.00507*x(1)*x(3) + ...
                  0.00953*x(2)*x(1) + 0.0521*x(2)^2 + 0.00901*x(2)*x(3) - ...
                  0.00507*x(3)*x(1) + 0.00901*x(3)*x(2) + 0.0294*x(3)*x(3) ) ...
         - 0.000766*x(1) - 3.42e-5*x(2) + 0.000189*x(3)) + x(4) - 0.040357 ; % = 0
  f(2) = x(1) + x(2) + x(3) - x(4) - 210 ; % >= 0
end

function jac = jacobian(x,auxdata)
  jac = [ -0.001352*x(1) - 0.0001906*x(2) + 0.0001014*x(3) + 0.000766, ...
          -0.0001906*x(1) - 0.001042*x(2) - 0.0001802*x(3) + 0.0000342, ...
           0.0001014*x(1) - 0.0001802*x(2) - 0.000588*x(3) - 0.000189, ...
           1 ; 1, 1, 1, -1 ] ;
  jac = sparse(jac) ;
end

function jac = jacobianstructure(auxdata)
  jac = sparse(ones(2,4)) ;
end

function H = hessian(x, sigma, lambda, auxdata)
  H1 = [ 0.01066 0       0       0 ; ...
         0       0.01778 0       0 ; ...
         0       0       0.01482 0 ; ...
         0       0       0       0 ] ;
  H2 = [ -0.1352e-2,          0,         0, 0 ; ...
         -0.1906e-3, -0.1042e-2,         0, 0 ; ...
          0.0001014, -0.1802e-3, -0.588e-3, 0 ; ...
          0,                  0,         0, 0 ] ;
  H = sparse(sigma*H1 + lambda(1)*H2) ;
end

function H = hessianstructure(auxdata)
  H = sparse([ 1 0 0 0 ; 1 1 0 0 ; 1 1 1 0 ; 1 1 1 1 ]) ;
end


%FEATSELO Branch and bound feature selection
%
%   [W,R] = FEATSELO(A,CRIT,K,T)
%   [W,R] = A*FEATSELO([],CRIT,K,T)
%   [W,R] = A*FEATSELO(CRIT,K,T)
%   [W,R] = FEATSELO(A,CRIT,K,N)
%   [W,R] = A*FEATSELO([],CRIT,K,N)
%   [W,R] = A*FEATSELO(CRIT,K,N)
%
% INPUT
%   A     Input dataset
%   CRIT  String name of the criterion or untrained mapping
%           (optional, def= 'maha-s')
%   K     Numner of features to select (optional, def: K=2)
%   T     Validation set (optional)
%   N     Number of cross-validations (optional)
%
% OUTPUT
%   W     Output feature selection mapping
%   R     Matrix with step-by-step results
%
% DESCRIPTION
% Backward selection of K features by baktracking using the branch
% and bound procedure on the data set A. CRIT sets the criterion
% used by the feature evaluation routine FEATEVAL. If the data set T
% is given, it is used as test set for FEATEVAL. Alternatively a number
% of cross-validations N may be supplied. The resulting W can be used for
% the selecting features of a dataset B by B*W.
% The selected features are stored in W.DATA and can be found by +W.
%
% This procedure finds the optimum feature set if a monotoneous
% criterion is used. The use of a testset does not guarantee that.
%
% REFERENCE
% P. M. Narendra and K. Fukunaga
% A Branch and Bound Algorithm for Feature Subset Selection,
% IEEE Trans. Computer, 26(9), pp. 917-922, September 1977
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, FEATEVAL, FEATSELF, FEATSELB, FEATSELI,
% FEATSEL, FEATSELP, FEATSELM

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: featselo.m,v 1.7 2009/07/01 09:33:23 duin Exp $

function [W,R] = featselo(varargin)

  varargin = shiftargin(varargin,{'char','prmapping'});
  argin = setdefaults(varargin,[],'maha-s',2,[],[]);
  if mapping_task(argin,'definition')
    W = define_mapping(argin,'untrained','B&B FeatSel');
    return
  end

  [A,crit,kmin,T,fid] = deal(argin{:});

	isvaldfile(A,1,2); % at least 1 object per class, 2 classes
	A = testdatasize(A);
	if isdataset(T), iscomdset(A,T); end

	[m,k,c] = getsize(A);
	featlist = getfeatlab(A);
  A = setprior(A,getprior(A));

	if ((kmin < 1) | (kmin >= k))
		error('The desired feature size should be > 0 and < dataset feature size')
	end

	% space for criteria values
	feat = zeros(1,k);

   % Get performance of the individual features:
	if isempty(T)
		for j=1:k
			feat(j) = feateval(A(:,j),crit);
		end
	elseif is_scalar(T)
		for j=1:k
			feat(j) = feateval(A(:,j),crit,T);
		end
	else
		for j=1:k
			feat(j) = feateval(A(:,j),crit,T(:,j));
		end
	end

   % Get the kmin worst(?) individual features according to their
   % individual performance:
	[F,S] = sort(feat);

	%sometimes the above line is bad compared to the following two
	%w = featselb(A,crit,[]);
	%S = fliplr(+w);

	Iopt = [k-kmin+1:k];

	I = [1:k];
	J = [zeros(1,kmin),1:(k-kmin-1),k-kmin+1,k+1];
	level = k;

   % Get the performance of Iopt
	if isdataset(T)
		bound = feateval(A(:,S(Iopt)),crit,T(:,S(Iopt)));
	elseif is_scalar(T)
		bound = feateval(A(:,S(Iopt)),crit,T);
	else
		bound = feateval(A(:,S(Iopt)),crit);
	end

	C = inf;
	prwaitbar(100,'Branch & Bound Feature Selection')
	iter = 0;
	while numel(I) > 0 && J(k+1) == k+1;
		iter = iter+1;
		prwaitbar(100,100-100*exp(-iter/25),['Branch & Bound Feature Selection: ' num2str(iter)]);
		if J(level) == J(level+1) | level <= kmin | C <= bound
			J(level) = level - kmin;
			level = level + 1;
			I = sort([I,J(level)]);
			J(level) = J(level) + 1;
			C = inf;
		else
			I(J(level)) = [];
			level = level - 1;
			if J(level+1) < 3 & level == kmin+1 & 0 % never happens ??
				;
			else
				if isdataset(T)
					C = feateval(A(:,S(I)),crit,T(:,S(I)));
				elseif is_scalar(T)
					C = feateval(A(:,S(I)),crit,T);
				else
					C = feateval(A(:,S(I)),crit);
        end
				if level == kmin & C > bound
					bound = C;
					Iopt = I;
          disp([bound,iter,numel(I)])
        end
			end
		end
  end
	prwaitbar(0);

   % Store the optimal features in the mapping:
	W = featsel(k,S(Iopt));
  W = setmapping_type(W,'trained');
  W = setsize(W,[k length(S(Iopt))]);
	if ~isempty(featlist)
		W = setlabels(W,featlist(S(Iopt),:));
	end
	W = setname(W,'B&B FeatSel');

	R = [];  %DXD I'm still not sure what to return

return


function [r, R_r, R_v, R_a] = rpredict(r, v, a, dt)

% RPREDICT Position prediction.
%   RPREDICT(R,V,DT) performs the time update R = R + V*DT.
%
%   RPREDICT(R,V,A,DT) considers R = R + V*DT + 1/2*A*DT instead.
%
%   [R,R_r,R_v,R_a] = ... returns Jacobian matrices wrt position R,
%   velocity V and acceleration A.
%
%   See also VPREDICT, QPREDICT.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargin == 3
    dt  = a;
    r   = r + v*dt;
    R_r = eye(length(r));
    R_v = dt*R_r;
    R_a = zeros(3);

elseif nargin == 4
    r   = r + v*dt + .5*a*dt^2;
    R_r = eye(length(r));
    R_v = R_r*dt;
    R_a = 0.5*R_r*dt^2;

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [H s phi T] = RSTLS(X1, X2, normalization)

% [H s phi T] = RSTLS(X1, X2, normalization)
%
% DESC:
% computes the RST transformation between the point pairs X1, X2
%
% VERSION:
% 1.0.1
%
% INPUT:
% X1, X2        = point matches (cartesian coordinates)
% normalization = true (default) or false to enable/disable point
%                 normalzation
%
% OUTPUT:
% H             = homography representing the RST transformation
% s             = scaling
% phi           = rotation angle
% T             = translation vector

% AUTHOR:
% Marco Zuliani, email: marco.zuliani@gmail.com
% Copyright (C) 2011 by Marco Zuliani
%
% LICENSE:
% This toolbox is distributed under the terms of the GNU GPL.
% Please refer to the files COPYING.txt for more information.

% HISTORY
% 1.0.0         08/27/08 - intial version
% 1.0.1         06/09/09 - implemented closed form for the LS estimation
%                          routines

if (nargin < 3)
    normalization = true;
end;

N = size(X1, 2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% checks
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (size(X2, 2) ~= N)
    error('RSTLS:inputError', ...
        'The set of input points should have the same cardinality')
end;
if N < 2
    error('RSTLS:inputError', ...
        'At least 2 point correspondences are needed')
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% normalize the input
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (normalization) && (N > 2)
    % fprintf('\nNormalizing...')
    [X1, T1] = normalize_points(X1);
    [X2, T2] = normalize_points(X2);
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% estimation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (N == 2)

    % fast estimation
    Theta = zeros(4,1);

    % $\mbox{\texttt{MM}} \eqdef M_{:,1} = \vct{y}^{(1)}-\vct{y}^{(2)} = \left[\begin{array}{c} y_1^{(1)}-y_1^{(2)} \\ y_2^{(1)}-y_2^{(2)} \end{array}\right]$
    % 2 additions
    MM = X1(:,1) - X1(:,2);
    % $ \mbox{\texttt{detMM}} \eqdef |M|$
    % 1 additions, 2 multiplication
    detMM = MM(1)*MM(1) + MM(2)*MM(2);
    % $ \mbox{\texttt{MMi}} \eqdef \left[ \begin{array}{c} \left[M^{-1}\right]_{1,1} \\ -\left[M^{-1}\right]_{2,1}\end{array}\right]$
    % 2 multiplications
    MMi = MM / detMM;

    % $ \mbox{\texttt{Delta}} \eqdef \vct{T}_{\vct{\theta}} (\vct{y}^{(1)})-\vct{T}_{\vct{\theta}} (\vct{y}^{(2)})$
    % 2 additions
    Delta = X2(:,1) - X2(:,2);

    % $ \mbox{\texttt{Theta(1:2)}} = M^{-1}\left(\vct{T}_{\vct{\theta}} (\vct{y}^{(1)})-\vct{T}_{\vct{\theta}} (\vct{y}^{(2)})\right)$
    % 1 additions, 2 multiplications
    Theta(1) = MMi(1)*Delta(1) + MMi(2)*Delta(2);
    % 1 additions, 2 multiplications
    Theta(2) = MMi(1)*Delta(2) - MMi(2)*Delta(1);
    % $ \mbox{\texttt{Theta(3:4)}} = -S^{(2)}\vct{\theta}_{1:2}+\vct{T}_{\vct{\theta}} (\vct{y}^{(2)})$
    % 2 additions, 2 multiplications

    Theta(3) = X2(1,2) - Theta(1)*X1(1,2) + Theta(2)*X1(2,2);
    % 2 additions, 2 multiplications
    Theta(4) = X2(2,2) - Theta(1)*X1(2,2) - Theta(2)*X1(1,2);

    % total: 11 additions, 12 multiplications
else

    % Closed form LS solution. Using the tutorial notation.

    % Notation semplification:
    % $\vct{p}^{(i)} = \bar{\vct{y}}^{(i)}$ and $\vct{q}^{(i)} = \overline{\vct{T}_{\vct{\theta}} (\vct{y}^{(i)})}$
    % $a = \sum_{i=1}^N\left( (p_1^{(i)})^2 + (p_2^{(i)})^2 \right)$
    a = sum(X1(:).^2);

    % Explicit LS expansion:
    % $  \theta_1 = \frac{1}{a}\sum_{i=1}^N p_1^{(i)} q_1^{(i)} + p_2^{(i)} q_2^{(i)} $
    % $  \theta_2 = \frac{1}{a}\sum_{i=1}^N -p_2^{(i)} q_1^{(i)} + p_1^{(i)} q_2^{(i)} $
    % $  \theta_3 = 0 $
    % $  \theta_4 = 0 $
    Theta(1) = sum( X1(1, :).*X2(1, :) + X1(2, :).*X2(2, :) ) / a;
    Theta(2) = sum( -X1(2, :).*X2(1, :) + X1(1, :).*X2(2, :) ) / a;
    Theta(3) = 0;
    Theta(4) = 0;

    % Traditional LS
    %
    %     A = zeros(2*N, 4);
    %     b = zeros(2*N, 1);
    %
    %     ind = 1:2;
    %     for n = 1:N
    %
    %         A(ind, 1:2) = [X1(1,n) -X1(2,n); X1(2,n) X1(1,n)];
    %         A(ind, 3:4) = eye(2);
    %
    %         b(ind) = X2(1:2, n);
    %
    %         ind = ind + 2;
    %
    %     end;
    %
    %     % solve the linear system in a least square sense
    %     Theta = A\b;

end;

% compute the corresponding homography
H = [Theta(1) -Theta(2) Theta(3); Theta(2) Theta(1) Theta(4); 0 0 1];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% de-normalize the parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (normalization) && (N > 2)
    H = T2\H*T1;
end;
H = H/H(9);

% prepare the output
if nargout > 1

    s       = sqrt(H(1,1)*H(1,1) + H(2,1)*H(2,1));
    phi     = atan2(H(2,1), H(1,1));
    T       = H(1:2, 3);

end;

return


function [M,R] = spm_get_closest_affine(x,y,w1,w2)
% Determine the affine transform mapping x to y
% FORMAT [M,R] = spm_get_closest_affine(X,Y,W1,W2)
% X  - n1*n2*n3*3 array of floats representing coordinates.
% Y  - n1*n2*n3*3 array of floats representing coordinates.
% W1 - n1*n2*n3   array of floats representing weights.
% W2 - n1*n2*n3   array of floats representing weights.
%
% M  - an affine transform
% R  - a rigid-body transform
%
% The code treats X and Y as reshaped versions (n1*n2*n3) x 3,
% and W1 and W2 as column vectors.
%
% It generates XX = [diag(W1)*X W1]'*diag(W2)*[diag(W1)*X W1]
% and          XY = [diag(W1)*X W1]'*diag(W2)*[Y W1]
%
% These can then be used to compute an affine transform (M),
% by M = (XX\XY)'
% A weighted procrustes decomposition is also performed,
% so that a rigid-body transform matrix (R) is returned.
%
% If W1 or W2 are empty or not passed, then they are assumed
% to be all ones.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_get_closest_affine.m 6137 2014-08-19 12:43:11Z john $

XX = zeros(4);
XY = zeros(4);
d  = size(x);
o  = ones(d(1)*d(2),1);
for k=1:size(x,3),
    xk  = reshape(x(:,:,k,:),[d(1)*d(2),3]);
    if (nargin<3 || isempty(w1)) && (nargin<4 || isempty(w2)),
        ox = o;
        oy = o;
    else
        if nargin>=4 && ~isempty(w1) && ~isempty(w2),
            oy = reshape(w2(:,:,k), [d(1)*d(2),1]);
            ox = reshape(w1(:,:,k), [d(1)*d(2),1]).*oy;
        elseif nargin>=3 && ~isempty(w1),
            ox = reshape(w1(:,:,k), [d(1)*d(2),1]);
            oy = ox;
        elseif nargin>=4 && ~isempty(w2),
            ox = reshape(w2(:,:,k), [d(1)*d(2),1]);
        end
        xk(:,1) = xk(:,1).*ox;
        xk(:,2) = xk(:,2).*ox;
        xk(:,3) = xk(:,3).*ox;
    end
    yk  = reshape(y(:,:,k,:),[d(1)*d(2),3]);
    msk = find(all(isfinite(xk),2) & all(isfinite(yk),2));
    X   = [xk(msk,:), ox(msk)];
    Y   = [yk(msk,:), oy(msk)];
    XX  = XX + double(X'*X);
    XY  = XY + double(X'*Y);
end
M = (XX\XY)';

if nargout>1,
    % Procrustes decomposition
    XX1 = XX - XX(:,4)*XX(:,4)'/XX(4,4);
    XY1 = XY - XY(:,4)*XY(4,:) /XY(4,4);
    Z   = (XX1(1:3,1:3)\XY1(1:3,1:3))';
    [U,S,V] = svd(Z);                   % Decompose into rotate, zoom and rotate.
    R   = [U*V' zeros(3,1);0 0 0 1];    % Pure rotation (by taking out the zoom)
    T1  = [eye(4,3) -XY(:,4) /XY(4,4)]; % Initial translation of centre of mass to origin.
    T2  = [eye(4,3) -XY(4,:)'/XY(4,4)]; % Final translation of origin to centre of mass.
    R   = T2 * R * T1;
end


function Out   = RiemannExpMap(P,X)

[U Delta] = eig(P);
G = U*sqrt(Delta);
Y = inv(G)*X*inv(G)';
[V Sigma] = eig(Y);
Out = (G*V)*diag(exp(diag(Sigma)))*(G*V)';


%This Matlab script can be used to reproduce Figure 3.4 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Define the range of BS antennas
Mvalues = [1 10 100];

%Angular standard deviation in the local scattering model (in degrees)
ASD = 10;

%Nominal angle of desired UE
thetaDesired = pi/6;

%Range of nominal angles of the interfering UE
varphiInterfererDegrees = -180:1:180;
varphiInterfererRadians = varphiInterfererDegrees*(pi/180);

%Define the antenna spacing (in number of wavelengths)
antennaSpacing = 1/2; %Half wavelength distance

%Define the effective SNR in (3.13) for the desired UE
SNR1dB = 10;
SNR1 = 10.^(SNR1dB/10);

%Define the effective SNR in (3.13) for the interfering UE
SNR2dB = 0;
SNR2 = 10.^(SNR2dB/10);

%Preallocate matrices for storing the simulation results
correlationcoeff = zeros(length(varphiInterfererRadians),length(thetaDesired),length(Mvalues));

%Compute the spatial correlation matrix of the desired UE
R1 = functionRlocalscattering(max(Mvalues),thetaDesired,ASD,antennaSpacing);

%% Go through all angles of interfering UE
for n = 1:length(varphiInterfererRadians)

    %Output simulation progress
    disp([num2str(n) ' angles out of ' num2str(length(varphiInterfererRadians))]);

    %Compute the spatial correlation matrix of the interfering UE
    R2 = functionRlocalscattering(max(Mvalues),varphiInterfererRadians(n),ASD,antennaSpacing);

    %Go through all number of antennas
    for m = 1:length(Mvalues)

        %Extract correlation matrices of the specified dimension
        R1m = R1(1:Mvalues(m),1:Mvalues(m));
        R2m = R2(1:Mvalues(m),1:Mvalues(m));

        %Compute the denominator in (3.18)
        normalization = sqrt(SNR1*SNR2*abs(trace(R1m*((SNR1*R1m+SNR2*R2m+eye(Mvalues(m)))\R1m)))*abs(trace(R2m*((SNR1*R1m+SNR2*R2m+eye(Mvalues(m)))\R2m))));

        %Compute absolute value of antenna-averaged correlation coefficient in (3.18)
        correlationcoeff(n,m) = sqrt(SNR1*SNR2)*abs(trace(R1m*((SNR1*R1m+SNR2*R2m+eye(Mvalues(m)))\R2m)))/normalization;

    end

end

%% Plot the simulation results
figure;
hold on; box on;

plot(varphiInterfererDegrees,correlationcoeff(:,1),'k-','LineWidth',1);
plot(varphiInterfererDegrees,correlationcoeff(:,2),'r--','LineWidth',1);
plot(varphiInterfererDegrees,correlationcoeff(:,3),'b-.','LineWidth',1);

xlabel('Angle of interfering UE [degree]');
ylabel('Antenna-averaged correlation coefficient');
xlim([-180 180]);
ylim([0 1.1]);

legend('M=1','M=10','M=100','Location','Best');


function [Dps_eff, Dns_eff] = solidPhaseDiffusionCoefficients(T,param)
% solidPhaseDiffusionCoefficients evaluates diffusion coefficients of the solid phase [m^2 /s].
% The user may modify the script to meet specific requirements.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

if(param.TemperatureEnabled>=1)
    Dps_eff     = param.Dps*exp(-param.EaDps/param.R*(1./T(param.Nal+1:param.Nal+param.Np)-1/param.Tref));
else
    Dps_eff     = param.Dps*ones(param.Np,1);
end

if(param.TemperatureEnabled>=1)
    Dns_eff     = param.Dns*exp(-param.EaDns/param.R*(1./T(param.Nal+param.Np+param.Ns+1:param.Nal+param.Np+param.Ns+param.Nn)-1/param.Tref));
else
    Dns_eff     = param.Dns*ones(param.Nn,1);
end

end


classdef L1_ADMM_YZ < handle
% Solver for various L1 minimization problems
% based on the 2011 paper by
% Junfeng Yang and Yin Zhang

properties
    % weight for the quadratic penalty term
    rho
     % verbosity
    verbose
    % Maximum number of ADMM iterations
    max_iterations
    % relative tolerance
    tolerance
    % additional scaling factor for x updates
    gamma
end % properties

properties(SetAccess=private)
    % The sensing operator
    A
    % The signals being recovered
    B
    % The solution representation vectors
    X
    % The dimension of signal space
    M
    % The dimension of representation space
    N
    % Details of execution (intermediate values)
    details
end % properties

% Following properties are meant for implementation only
properties(Access=private)
    X0
    Y0
    Z0
end

methods

function self = L1_ADMM_YZ(A, options)
    if nargin < 1
        error('A must be specified.');
    end
    if isa(A, 'spx.dict.Operator')
        self.A = A;
    elseif ismatrix(A)
        self.A = spx.dict.MatrixOperator(A);
    else
        error('Unsupported operator.');
    end
    [self.M, self.N] = size(self.A);
    if nargin < 2
        options = struct;
    end
    self.init_options();
    self.process_options(options);
end % function

function [X] = solve_bp(self, B, options)
    % Solves the problem min ||x||_1 s.t. Ax = b
    if nargin < 2
        error('B must be specified.');
    end
    if nargin < 3
        options = struct;
    end
    self.process_options(options);
    num_problems = size(B, 2);
    if self.verbose > 0
        fprintf('Solving l1 minimization problem: BP.\n');
    end
    self.init_details(num_problems);
    max_iterations = self.max_iterations;
    A  = self.A;
    X = zeros(self.N, num_problems);
    % import relevant functions from other modules
    import spx.opt.projections.proj_linf_ball;
    % iterate over problems
    for prob=1:num_problems
        tstart = tic;
        b = B(:, prob);
        b_max = norm(b, 'inf');
        terminated = false;
        % Check for zero solution condition
        if b_max < self.tolerance
            % There is no need to proceed further
            % zero solution is best solution
            self.details.terminated(prob) = true;
            self.details.iterations(prob) = 0;
            self.details.elapsed_times(prob) = toc(tstart);
            X(:, prob) = zeros(self.N, 1);
            continue;
        end
        if self.rho > 0
            % we will use user defined quadratic term weight
            rho = self.rho;
        else
            rho = mean(abs(b));
        end
        gamma = self.gamma;
        % scale the problem
        b = b / b_max;
        % Initialize solution
        x = A.adjoint(b);
        z = zeros(self.N, 1);
        y = zeros(self.M, 1);
        % compute current primary residual
        r_primal = A.apply(x) - b;
        for iter=1:max_iterations
            % check if we need to iterate further
            if terminated; break; end;
            % update z
            z_prev = z;
            z = proj_linf_ball(A.adjoint(y) + (x / rho));
            % update y
            y_prev = y;
            Az = A.apply(z);
            y = (Az - r_primal / rho);
            % update dual residual
            Aty = A.adjoint(y);
            r_dual =  Aty - z;
            % update x
            x_prev = x;
            x = x + (gamma * rho)*r_dual;
            % update primary residual
            r_primal = A.apply(x) - b;
            % primal objective
            primal_obj = sum(abs(x));
            % dual objective
            dual_obj = real(b' * y);
            terminated = self.check_termination(x, x_prev, y, z, ...
                r_primal, r_dual, primal_obj, dual_obj, iter, prob);
        end % iteration loop
        self.details.terminated(prob) = terminated;
        self.details.iterations(prob) = iter-1;
        self.details.elapsed_times(prob) = toc(tstart);
        % put the final solution back in result
        X(:, prob) = x * b_max;
    end % problem loop
end % function

function [X] = solve_bpic(self, B, delta, options)
    % Solves the problem min ||x||_1 s.t. ||Ax - b||_2 <= delta
    if nargin < 2
        error('B must be specified.');
    end
    if nargin < 3
        error('delta must be specified.');
    end
    if nargin < 4
        options = struct;
    end
    self.process_options(options);
    num_problems = size(B, 2);
    if self.verbose > 0
        fprintf('Solving constrained l1 minimization problem: BPIC.\n');
    end
    self.init_details(num_problems);
    max_iterations = self.max_iterations;
    A  = self.A;
    X = zeros(self.N, num_problems);
    % import relevant functions from other modules
    import spx.opt.projections.proj_linf_ball;
    import spx.opt.projections.proj_l2_ball;
    % iterate over problems
    for prob=1:num_problems
        tstart = tic;
        b = B(:, prob);
        b_max = norm(b, 'inf');
        terminated = false;
        % Check for zero solution condition
        if norm(b) < delta
            % There is no need to proceed further
            % zero solution is best solution
            self.details.terminated(prob) = true;
            self.details.iterations(prob) = 0;
            self.details.elapsed_times(prob) = toc(tstart);
            X(:, prob) = zeros(self.N, 1);
            continue;
        end
        if self.rho > 0
            % we will use user defined quadratic term weight
            rho = self.rho;
        else
            rho = mean(abs(b));
        end
        gamma = self.gamma;
        % scale the problem
        b = b / b_max;
        % Initialize solution
        x = A.adjoint(b);
        z = zeros(self.N, 1);
        y = zeros(self.M, 1);
        % compute current primary residual
        r_primal = A.apply(x) - b;
        delta_by_rho = delta / rho;
        for iter=1:max_iterations
            % check if we need to iterate further
            if terminated; break; end;
            % update z
            z_prev = z;
            z = proj_linf_ball(A.adjoint(y) + (x / rho));
            % update y
            y_prev = y;
            Az = A.apply(z);
            y = (Az - r_primal / rho);
            y = y - proj_l2_ball(y, delta_by_rho);
            % update dual residual
            Aty = A.adjoint(y);
            r_dual =  Aty - z;
            % update x
            x_prev = x;
            x = x + (gamma * rho)*r_dual;
            % update primary residual
            r_primal = A.apply(x) - b;
            % primal objective
            primal_obj = sum(abs(x));
            % dual objective
            dual_obj = real(b' * y) - delta * norm(y);
            terminated = self.check_termination(x, x_prev, y, z, ...
                r_primal, r_dual, primal_obj, dual_obj, iter, prob);
        end % iteration loop
        self.details.terminated(prob) = terminated;
        self.details.iterations(prob) = iter-1;
        self.details.elapsed_times(prob) = toc(tstart);
        % put the final solution back in result
        X(:, prob) = x * b_max;
    end % problem loop
end % function

function [X] = solve_bpdn_l2(self, B, mu, options)
    % Solves the problem min ||x||_1  + ||Ax - b||_2 / (2 mu)
    if nargin < 2
        error('B must be specified.');
    end
    if nargin < 3
        error('mu must be specified.');
    end
    if nargin < 4
        options = struct;
    end
    self.process_options(options);
    num_problems = size(B, 2);
    if self.verbose > 0
        fprintf('Solving l1-l2 unconstrained minimization problem: BPDN l2.\n');
    end

    self.init_details(num_problems);
    max_iterations = self.max_iterations;
    A  = self.A;
    X = zeros(self.N, num_problems);
    % import relevant functions from other modules
    import spx.opt.projections.proj_linf_ball;
    % iterate over problems
    for prob=1:num_problems
        tstart = tic;
        b = B(:, prob);
        %  Compute A' b
        Atb = A.adjoint(b);
        terminated = false;
        % Check for zero solution condition
        if norm(Atb, 'inf') <= mu
            % There is no need to proceed further
            % zero solution is best solution
            self.details.terminated(prob) = true;
            self.details.iterations(prob) = 0;
            self.details.elapsed_times(prob) = toc(tstart);
            X(:, prob) = zeros(self.N, 1);
            continue;
        end
        if self.rho > 0
            % we will use user defined quadratic term weight
            rho = self.rho;
        else
            rho = mean(abs(b));
        end
        gamma = self.gamma;
        % scale the problem
        b_max = norm(b, 'inf');
        b = b / b_max;
        mu = mu / b_max;
        % Initialize solution
        x = A.adjoint(b);
        z = zeros(self.N, 1);
        y = zeros(self.M, 1);
        % compute current primary residual
        r_primal = A.apply(x) - b;
        for iter=1:max_iterations
            % check if we need to iterate further
            if terminated; break; end;
            % update z
            z_prev = z;
            z = proj_linf_ball(A.adjoint(y) + (x / rho));
            % update y
            y_prev = y;
            Az = A.apply(z);
            y = (rho/(mu +  rho)) * (Az - r_primal / rho);
            % update dual residual
            Aty = A.adjoint(y);
            r_dual =  Aty - z;
            % update x
            x_prev = x;
            x = x + (gamma * rho)*r_dual;
            % update primary residual
            r_primal = A.apply(x) - b;
            % primal objective
            primal_obj = sum(abs(x)) + (1/(2*mu))*(r_primal'*r_primal);
            % dual objective
            dual_obj = real(b' * y) - (mu / 2) * (y' * y);
            terminated = self.check_termination(x, x_prev, y, z, ...
                r_primal, r_dual, primal_obj, dual_obj, iter, prob);
        end % iteration loop
        self.details.terminated(prob) = terminated;
        self.details.iterations(prob) = iter-1;
        self.details.elapsed_times(prob) = toc(tstart);
        % put the final solution back in result
        X(:, prob) = x * b_max;
    end % problem loop
end % function

function [X] = solve_bpdn_l1(self, B, nu, options)
    % Solves the problem min ||x||_1  + ||Ax - b||_1 / (nu)
end % function

end % methods

methods(Access=private)

function init_options(self)
    self.rho = 0;
    self.verbose = 0;
    self.max_iterations = 100;
    % self.eps_abs = 1e-4;
    self.tolerance = 1e-2;
    self.gamma = 1;
end % function

function process_options(self, options)
    % weight for the quadratic penalty term
    if isfield(options, 'rho')
        self.rho = options.rho;
    end
    % verbosity
    if isfield(options, 'verbose')
        self.verbose = options.verbose;
    end
    % Maximum number of ADMM iterations
    if isfield(options, 'max_iterations')
        self.max_iterations = options.max_iterations;
    end
    % gamma for primal variable update
    if isfield(options, 'gamma')
        self.gamma = options.gamma;
    end
    % relative tolerance
    if isfield(options, 'tolerance')
        self.tolerance = options.tolerance;
    end
end % function

function init_xyz(self, num_problems, options)
    if isfield(options, 'X0')
        self.X0 = X0;
    else
        self.X0 = zeros(self.N, num_problems);
    end
    if isfield(options, 'Y0')
        self.Y0 = Y0;
    else
        self.Y0 = zeros(self.N, num_problems);
    end
    if isfield(options, 'Z0')
        self.Z0 = Z0;
    else
        self.Z0 = zeros(self.N, num_problems);
    end
end % function

function init_details(self, num_problems)
    max_iterations = self.max_iterations;
    self.details.iterations = zeros(1, num_problems);
    self.details.terminated = zeros(1, num_problems);
    self.details.elapsed_times = zeros(1, num_problems);
    self.details.x_norms = zeros(max_iterations, num_problems);
    self.details.z_norms = zeros(max_iterations, num_problems);
    self.details.r_primal_norms = zeros(max_iterations, num_problems);
    self.details.r_dual_norms = zeros(max_iterations, num_problems);
    self.details.primal_objectives = zeros(max_iterations, num_problems);
    self.details.dual_objectives = zeros(max_iterations, num_problems);
end % function

function terminate = check_termination(self, x, x_prev, y, z, ...
    r_primal, r_dual, primal_obj, dual_obj, iter, prob)
    terminate = false;
    % norm of primal variable x
    x_norm = norm(x);
    % norm of change in x
    x_diff_norm = norm(x - x_prev);
    % relative change in x
    x_rel_change = x_diff_norm / x_norm;
    % norm of dual variable z
    z_norm = norm(z);
    % primal residual norm
    r_primal_norm  = norm(r_primal);
    % dual residual norm
    r_dual_norm = norm(r_dual);
    % relative change in dual norm
    r_dual_rel_change = r_dual_norm / z_norm;
    % gap in objectives
    gap = abs(primal_obj  - dual_obj);
    relative_gap = gap / abs(primal_obj);

    self.details.x_norms(iter, prob) = x_norm;
    self.details.z_norms(iter, prob) = z_norm;
    self.details.r_primal_norms(iter, prob) = r_primal_norm;
    self.details.r_dual_norms(iter, prob) = r_dual_norm;
    self.details.primal_objectives(iter, prob) = primal_obj;
    self.details.dual_objectives(iter, prob) = dual_obj;

    if self.verbose > 1
        fprintf('Objective primal: %.4f, dual: %.4f\n', primal_obj, dual_obj);
    end

    tolerance = self.tolerance;
    if x_rel_change < tolerance
        terminate = true;
    end
    if x_rel_change >= tolerance*1.1
        % we will continue for a while
        return;
    end
    if relative_gap < tolerance
        terminate = true;
    end
    if r_dual_rel_change < tolerance
        terminate = true;
    end
end % function

end % methods

end % class


classdef MatrixVectorizedInverter_2x2 < MatrixVectorizedInverter_Interface

    methods (Access = public)

        function B = computeInverse(obj,A)
            d(1,1,:) = A(2,2,:);
            d(1,2,:) = -A(1,2,:);
            d(2,1,:) = -A(2,1,:);
            d(2,2,:) = A(1,1,:);

            det = obj.computeDeterminant(A);

            B = zeros(size(A));
            for i = 1:2
                for j = 1:2
                    B(i,j,:) = squeeze(d(i,j,:))./det;
                end
            end
        end

        function detA = computeDeterminant(~,A)
            detA = squeeze(A(1,1,:).*A(2,2,:)-A(1,2,:).*A(2,1,:));
        end

    end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% cam.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function f = cam(x,y)
% Four-hump camel
function f = cam(x,y)
if nargin == 1
  x1 = x(1);
  x2 = x(2);
else
  x1 = x;
  x2 = y;
end
f=(4-2.1.*x1.^2+x1.^4./3).*x1.^2+x1.*x2+(-4+4.*x2.^2).*x2.^2;


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2019.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtBmmAlgebra.m                               |
%|    #    |   VERSION    : 0.61                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 05.09.2019                                    |
%| ( === ) |   SYNOPSIS   : Block matrix algebra                          |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Library path
run('../../addpathGypsilab')

% Dimensions
N = 200;

% Accuracy
tol = 1e-3;

% Particles receptors X
mesh  = mshSphere(N,1);
omega = dom(mesh,3);
phi0  = fem(mesh,'P0');
phi1  = fem(mesh,'P1');

% Wave number or frequency (Hz)
k = 5;

% Green kernel -> exp(1i*k*r)/r
green = '[exp(ikr)/r]';
Gxy   = @(X,Y) femGreenKernel(X,Y,green,k) + ...
    sqrt(N).*(X(:,1)==Y(:,1)).*(X(:,2)==Y(:,2)).*(X(:,3)==Y(:,3))  ;

% Particles charges (multiples)
V0 = (-1+2*rand(length(phi0),2)) + (-1+2i*rand(length(phi0),2));
V1 = (-1+2*rand(length(phi1),2)) + (-1+2i*rand(length(phi1),2));

% Spatial representation of particles
figure
plot(mesh)
axis equal

% All forms
tic
Ah = integral(omega,omega,phi0,Gxy,phi0,tol);
Bv = integral(omega,omega,phi0,green,k,phi1,tol);
Cs = integral(omega,phi1,phi0);
Df = integral(omega,omega,phi1,Gxy,phi1);
toc

%%% Single Builder
disp('~~~~~~~~~~~~~ SINGLE BUILDERS ~~~~~~~~~~~~~')
Mb  = bmm(Ah);
sol = Mb * V0;
ref = Ah * V0;
norm(ref-sol,'inf')/norm(ref,'inf')

Mb  = bmm(Bv);
sol = Mb * V1;
ref = Bv * V1;
norm(ref-sol,'inf')/norm(ref,'inf')

Mb  = bmm(Cs);
sol = Mb * V0;
ref = Cs * V0;
norm(ref-sol,'inf')/norm(ref,'inf')

Mb  = bmm(Df);
sol = Mb * V1;
ref = Df * V1;
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Multi Builder
disp('~~~~~~~~~~~~~ MULTI BUILDERS ~~~~~~~~~~~~~')
Mb  = bmm({Ah,Cs';Cs,Df});
Mr  = [full(Ah) Cs' ; Cs Df];
Vb  = [V0;V1];
sol = Mb * Vb;
ref = Mr * Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

figure
spy(Mb)

disp(' ')

%%% Full conversion
disp('~~~~~~~~~~~~~ FULL CONVERSION ~~~~~~~~~~~~~')
sol = full(Mb) * Vb;
ref = Mr * Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Sparse conversion
disp('~~~~~~~~~~~~~ SPARSE CONVERSION ~~~~~~~~~~~~~')
sol = sparse(Mb) * Vb;
ref = Mr * Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

figure
spy(sparse(Mb))

disp(' ')

%%% Transposition
disp('~~~~~~~~~~~~~ TRANSPOSITION ~~~~~~~~~~~~~')
sol = Vb.' * Mb.';
ref = (Mr * Vb).';
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Transposition
disp('~~~~~~~~~~~~~ CONJUGATE TRANSPOSITION ~~~~~~~~~~~~~')
sol = Vb' * Mb';
ref = (Mr * Vb)';
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Concatenation
disp('~~~~~~~~~~~~~ CONCATENATION ~~~~~~~~~~~~~')
Ab  = Mb;
Bb  = bmm({Bv;Df});
Cb  = bmm({Ah,Bv});
Db  = bmm(Cs');
Mb2 = [Ab , Bb ; Cb , Db];
sol = Mb2 * [Vb;V1];
ref = [Ab*Vb + Bb*V1;Cb*Vb+Db*V1];
norm(ref-sol,'inf')/norm(ref,'inf')

spy(Mb2)

disp(' ')

%%% Scalar product
disp('~~~~~~~~~~~~~ SCALAR PRODUCT ~~~~~~~~~~~~~')
sol = (-(3.*Mb.*2i)) * Vb;
ref = (-3*2i) * (Mr*Vb);
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Addition
disp('~~~~~~~~~~~~~ ADDITION ~~~~~~~~~~~~~')
sol = (Mb+Mb) * Vb;
ref = 2*(Mr*Vb);
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Multiplication
disp('~~~~~~~~~~~~~ MULTIPLICATION ~~~~~~~~~~~~~')
sol = (Mb.'*Mb) * Vb;
ref = Mr.'*(Mr*Vb);
norm(ref-sol,'inf')/norm(ref,'inf')

figure
spy(Mb.'*Mb)

disp(' ')

%%% LU factorization
disp('~~~~~~~~~~~~~ LU FACTORISATION ~~~~~~~~~~~~~')
[Lb,Ub] = lu(Mb);
sol = Lb * (Ub * Vb);
ref = Mb * Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

figure
subplot(1,2,1); spy(Lb)
subplot(1,2,2); spy(Ub)

disp(' ')

%%% Solve LU
disp('~~~~~~~~~~~~~ SOLVE LU SYSTEM ~~~~~~~~~~~~~')
sol = Mb \ Vb;
ref = Mr \ Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

disp(' ')

%%% Inversion
disp('~~~~~~~~~~~~~ INVERSION ~~~~~~~~~~~~~')
Mbm1 = inv(Mb);
sol  = Mbm1 * Vb;
ref  = Mr \ Vb;
norm(ref-sol,'inf')/norm(ref,'inf')

figure
spy(Mbm1)

disp(' ')

disp('~~> Michto gypsilab !')


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: compact version of matrix-free regularization
%
%  S(y) = alpha/2 * norm(B*(y-yRef))^2,
%
% where
%  alpha regularization parameter, weights regularization versus
%        distance in the joint objective function, alpha = 1 here
%  yRef  is a reference configuration, e.g. yRef = x or a
%        pre-registration result
%  B     a discrete partial differential operator either in explicit
%        matrix form or as a structure containing the necessary
%        parameters to compute B*y
% see also regularizer E8_regularization_MB
%==============================================================================

clear, close all, help(mfilename);

% initialize the regularization and create a starting  point
regularizer('reset','regularizer','mfElastic','alpha',1,'mu',1,'lambda',0);
y0 = @(omega,m) randn(size(getStaggeredGrid(omega,m)));

% 2D example, initialize physical domain and number of discretization points
omega = [0,1,0,1]; m = [16,12];   %

% test derivative of 2D implementation
fctn = @(yc) regularizer(yc,omega,m);  checkDerivative(fctn,y0(omega,m));

% 3D example, initialize physical domain and number of discretization points
omega = [0,1,0,1,0,1]; m  = [16,12,8];

% test derivative of 3D implementation
fctn = @(yc) regularizer(yc,omega,m);
checkDerivative(fctn,y0(omega,m));

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


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

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

%% ---------- Assert correct input.

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

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

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

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

else

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

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

end;

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

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

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

R=size(g,2);
% -------- Are we in the Riesz sequence of in the frame case

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

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

if kv.lt(2)==1
    % Rectangular case
    if (info.gl<=M) && (R==1)

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

    else

        % Long window case

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

        gd=comp_gabdual_long(g,a,M)*scale;

    end;

else
    % Non-separable case
    g=fir2long(g,L);

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

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

        gdfull=comp_gabdual_long(mwin,a*kv.lt(2),M)*scale;

        % We need just the first vector
        gd=gdfull(:,1);

    else

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

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

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

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

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

    end;

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

end;

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

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

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