function [ abd, info ] = zpbfa ( abd, lda, n, m )

%*****************************************************************************80
%
%% ZPBFA factors a complex hermitian positive definite band matrix.
%
%  Discussion:
%
%    ZPBFA is usually called by ZPBCO, but it can be called
%    directly with a saving in time if RCOND is not needed.
%
%  Band storage:
%
%    If A is a hermitian positive definite band matrix,
%    the following program segment will set up the input.
%
%      m = (band width above diagonal)
%      do j = 1, n
%        i1 = max ( 1, j-m )
%        do i = i1, j
%          k = i-j+m+1
%          abd(k,j) = a(i,j)
%        end do
%      end do
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 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 ABD(LDA,N); the matrix to be factored.
%    The columns of the upper triangle are stored in the columns of ABD
%    and the diagonals of the upper triangle are stored in the rows of ABD.
%
%    Input, integer LDA, the leading dimension of ABD.
%    LDA must be at least M+1.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer M, the number of diagonals above the main diagonal.
%    0 <= M < N.
%
%    Output, integer INFO.
%    0, for normal return.
%    K, if the leading minor of order K is not positive definite.
%
%    Output, complex ABD(LDA,N); an upper triangular matrix R, stored in
%    band form, so that A = hermitian(R)*R.
%
  info = 0;

  for j = 1 : n

    s = 0.0;
    ik = m + 1;
    jk = max ( j - m, 1 );
    mu = max ( m + 2 - j, 1 );

    for k = mu : m
      t = abd(k,j) - abd(ik:ik+k-mu-1,jk)' * abd(mu:mu+k-mu-1,j);
      t = t / abd(m+1,jk);
      abd(k,j) = t;
      s = s + real ( t * conj ( t ) );
      ik = ik - 1;
      jk = jk + 1;
    end

    s = real ( abd(m+1,j) ) - s;

    if ( s <= 0.0 | imag ( abd(m+1,j) ) ~= 0.0 )
      info = j;
      break
    end

    abd(m+1,j) = sqrt ( s );

  end

  return
end


function [AF,BF]=gabframebounds(g,a,M,varargin)
%GABFRAMEBOUNDS  Calculate frame bounds of Gabor frame
%   Usage:  fcond=gabframebounds(g,a,M);
%           [A,B]=gabframebounds(g,a,M);
%           [A,B]=gabframebounds(g,a,M,L);
%           [A,B]=gabframebounds(g,a,M,'lt',lt);
%
%   Input parameters:
%           g     : The window function.
%           a     : Length of time shift.
%           M     : Number of channels.
%           L     : Length of transform to consider.
%           lt    : Lattice type (for non-separable lattices).
%   Output parameters:
%           fcond : Frame condition number (B/A)
%           A,B   : Frame bounds.
%
%   `gabframebounds(g,a,M)` calculates the ratio $B/A$ of the frame bounds
%   of the Gabor system with window *g*, and parameters *a*, *M*.
%
%   `[A,B]=gabframebounds(...)` returns the frame bounds *A* and *B*
%   instead of just the ratio.
%
%   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.
%
%   `gabframebounds(g,a,M,L)` will cut or zero-extend the window to length
%   *L*.
%
%   `gabframebounds(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*.
%
%   See also: gabrieszbounds, gabwin

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

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

definput.keyvals.L=[];
definput.keyvals.lt=[0 1];
[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;

%% ----- actual computation ------------

g=fir2long(g,L);
R=size(g,2);

if kv.lt(2)==1
    % Rectangular case
    % Get the factorization of the window.
    gf=comp_wfac(g,a,M);

    % Compute all eigenvalues.
    lambdas=comp_gfeigs(gf,L,a,M);
    s=size(lambdas,1);

else

    % Convert to multi-window
    mwin=comp_nonsepwin2multi(g,a,M,kv.lt,L);

    % Get the factorization of the window.
    gf=comp_wfac(mwin,a*kv.lt(2),M);

    % Compute all eigenvalues.
    lambdas=comp_gfeigs(gf,L,a*kv.lt(2),M);
    s=size(lambdas,1);

end;

% Min and max eigenvalue.
if a>M*R
    % This can is not a frame, so A is identically 0.
    AF=0;
else
    AF=lambdas(1);
end;

BF=lambdas(s);

if nargout<2
    % Avoid the potential warning about division by zero.
    if AF==0
        AF=Inf;
    else
        AF=BF/AF;
    end;
end;


function [mbar] = dynpcm22mbar(dynpcm2)
% Convert pressure from dynes per square centimeter to millibar
% Chad Greene 2012
mbar = dynpcm2*0.00100000;


%% Brightness and contrast adjustments
%
% In this demo we show how to perform the operation
% $g(i,j) = \alpha \cdot f(i,j) + \beta$.
%
% Sources:
%
% * <https://docs.opencv.org/3.3.0/d3/dc1/tutorial_basic_linear_transform.html>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/tutorial_code/ImgProc/BasicLinearTransforms.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/tutorial_code/HighGUI/BasicLinearTransformsTrackbar.cpp>
% * <https://github.com/opencv/opencv/blob/3.3.0/samples/cpp/tutorial_code/ImgProc/changing_contrast_brightness_image/changing_contrast_brightness_image.cpp>
%

%% Theory
%
% A general image processing operator is a function that takes one or more
% input images and produces an output image. Image transforms can be seen as:
%
% * Point operators (pixel transforms)
% * Neighborhood (area-based) operators
%
% In pixel transforms, each output pixel's value depends on only the
% corresponding input pixel value (plus, potentially, some globally collected
% information or parameters). Examples of such operators include
% *brightness and contrast adjustments* as well as color correction and
% transformations.
%
% For brightness and contrast adjustments, two commonly used point processes
% are *multiplication* and *addition* with a constant:
%
% $$g(x) = \alpha f(x) + \beta$$
%
% The parameters $\alpha > 0$ and $\beta$ are often called the *gain* and
% *bias* parameters; sometimes these parameters are said to control *contrast*
% and *brightness* respectively. You can think of $f(x)$ as the source image
% pixels and $g(x)$ as the output image pixels. Then, more conveniently we can
% write the expression as:
%
% $$g(i,j) = \alpha \cdot f(i,j) + \beta$$
%
% where $i$ and $j$ indicates that the pixel is located in the *i-th* row and
% *j-th* column.
%
% In the implementation below, instead of using a for-loop to access each
% pixel, we simply use the function |cv.convertTo| which effectively performs
% *new_image = saturate(a*image + beta)*. This is more optimized than
% accessing each pixel and works a lot faster. Also notice that the operation
% can give values out of range or not integers (if $\alpha$ is float), in
% which case |cv.convertTo| makes sure the values are valid.
%

%% Example
%
% We will put into practice what we have learned to correct an underexposed
% image by adjusting the brightness and the contrast of the image. We will
% also see another technique to correct the brightness of an image called
% gamma correction.
%
%% Brightness and contrast adjustments
%
% Increasing/decreasing the $\beta$ value will add/subtract a constant value
% to every pixel. Pixel values outside of the |[0 ; 255]| range will be
% saturated (i.e. a pixel value higher/lesser than 255/0 will be clamp to
% 255/0).
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_hist_beta.png>>
%
% _In light gray, histogram of the original image, in dark gray when
% |brightness = 80| in Gimp_
%
% The histogram represents for each color level the number of pixels with that
% color level. A dark image will have many pixels with low color value and
% thus the histogram will present a peak in his left part. When adding a
% constant bias, the histogram is shifted to the right as we have added a
% constant bias to all the pixels.
%
% The $\alpha$ parameter will modify how the levels spread. If $\alpha < 1$,
% the color levels will be compressed and the result will be an image with
% less contrast.
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_hist_alpha.png>>
%
% _In light gray, histogram of the original image, in dark gray when
% |contrast < 0| in Gimp_
%
% Note that these histograms have been obtained using the Brightness-Contrast
% tool in the Gimp software. The brightness tool should be identical to the
% $\beta$ bias parameters but the contrast tool seems to differ to the
% $\alpha$ gain where the output range seems to be centered with Gimp (as you
% can notice in the previous histogram).
%
% It can occur that playing with the $\beta$ bias will improve the brightness
% but in the same time the image will appear with a slight veil as the
% contrast will be reduced. The $\alpha$ gain can be used to diminue this
% effect but due to the saturation, we will lose some details in the original
% bright regions.
%
%% Gamma correction
%
% <https://en.wikipedia.org/wiki/Gamma_correction Gamma correction> can be
% used to correct the brightness of an image by using a non linear
% transformation between the input values and the mapped output values:
%
% $$O = \left( \frac{I}{255} \right)^{\gamma} \times 255$$
%
% As this relation is non linear, the effect will not be the same for all the
% pixels and will depend to their original value.
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_gamma.png>>
%
% When $\gamma < 1$, the original dark regions will be brighter and the
% histogram will be shifted to the right whereas it will be the opposite with
% $ gamma > 1$.
%
% For the gamma correction, a look-up table can be used to improve the
% performance of the computation as only 256 values needs to be calculated
% once.
%
% Let's an example of how to correct an underexposed image.
%
% The following image has been corrected with: $\alpha = 1.3$ and $\beta = 40$.
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_linear_transform_correction.jpg>>
%
% The overall brightness has been improved but you can notice that the clouds
% are now greatly saturated due to the numerical saturation of the
% implementation used
% (<https://en.wikipedia.org/wiki/Clipping_(photography) highlight clipping>
% in photography).
%
% The following image has been corrected with: $\gamma = 0.4$.
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_gamma_correction.jpg>>
%
% The gamma correction should tend to add less saturation effect as the
% mapping is non linear and there is no numerical saturation possible as in
% the previous method.
%
% <<https://docs.opencv.org/3.3.0/Basic_Linear_Transform_Tutorial_histogram_compare.png>>
%
% * Left: histogram after alpha, beta correction
% * Center: histogram of the original image
% * Right: histogram after the gamma correction
%
% The previous figure compares the histograms for the three images (the
% y-ranges are not the same between the three histograms). You can notice that
% most of the pixel values are in the lower part of the histogram for the
% original image. After $\alpha$, $\beta$ correction, we can observe a big
% peak at 255 due to the saturation as well as a shift in the right. After
% gamma correction, the histogram is shifted to the right but the pixels in
% the dark regions are more shifted (see the gamma curves figure) than those
% in the bright regions.
%
% In this tutorial, you have seen two simple methods to adjust the contrast
% and the brightness of an image. They are basic techniques and are not
% intended to be used as a replacement of a raster graphics editor!
%
%% Additional resources
%
% * <https://learnopengl.com/#!Advanced-Lighting/Gamma-Correction
%   Gamma correction in graphics rendering>
% * <http://web.archive.org/web/20170106081601/http://www.graphics.cornell.edu/~westin/gamma/gamma.html
%   Gamma correction and images displayed on CRT monitors>
% * <http://www.cambridgeincolour.com/tutorials/digital-exposure-techniques.htm
%   Digital exposure techniques>
%

%% Code

function varargout = linear_transform_demo_gui(im)
    % load source image
    if nargin < 1
        im = fullfile(mexopencv.root(),'test','lena.jpg');
        img = cv.imread(im);
    elseif ischar(im)
        img = cv.imread(im, 'Color',true);
    else
        img = im;
    end

    % create the UI
    h = buildGUI(img);
    if nargout > 0, varargout{1} = h; end
end

function onLinearTransform(~,~,h)
    %ONLINEARTRANSFORM  Event handler for UI controls

    % retrieve current values from UI controls
    a = get(h.slid(1), 'Value');
    b = round(get(h.slid(2), 'Value'));
    set(h.txt(1), 'String',sprintf('Contrast: %.2f',a));
    set(h.txt(2), 'String',sprintf('Brightness: %2d',b));

    % linear transformation
    out = cv.convertTo(h.src, 'Alpha',a, 'Beta',b);

    % show result
    out = cv.putText(out, 'Brightness/Contrast Adjustment', [10 20], ...
        'FontScale',0.5, 'Color',[0 255 0], 'LineType','AA');
    set(h.img(1), 'CData',out);
    drawnow;
end

function onGammaCorrection(~,~,h)
    %ONGAMMACORRECTION  Event handler for UI controls

    % retrieve current values from UI controls
    g = get(h.slid(3), 'Value');
    set(h.txt(3), 'String',sprintf('Gamma: %.2f',g));

    % gamma correction
    lookUpTable = uint8((((0:255)/255) .^ g) * 255);
    if true
        out = cv.LUT(h.src, lookUpTable);
    elseif mexopencv.require('images')
        out = intlut(h.src, lookUpTable);
    else
        out = lookUpTable(double(h.src) + 1);
    end

    % show result
    out = cv.putText(out, 'Gamma Correction', [10 20], ...
        'FontScale',0.5, 'Color',[0 255 0], 'LineType','AA');
    set(h.img(2), 'CData',out);
    drawnow;
end

function h = buildGUI(img)
    %BUILDGUI  Creates the UI

    % parameters
    a = 1.0;  % alpha gain (contrast)
    b = 0;    % beta bias (brightness)
    g = 1.0;  % gamma correction
    sz = size(img);
    sz(2) = max(sz(2), 300);  % minimum figure width

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.src = img;
    h.fig = figure('Name','Linear Transform Demo', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2)*2 sz(1)+80-1]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax(1) = axes('Parent',h.fig, 'Units','pixels', 'Position',[1 80 sz(2) sz(1)]);
    h.ax(2) = axes('Parent',h.fig, 'Units','pixels', 'Position',[sz(2)+1 80 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img(1) = imshow(img, 'Parent',h.ax(1));
        h.img(2) = imshow(img, 'Parent',h.ax(2));
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax(1)); h.img(1) = imshow(img);
        axes(h.ax(2)); h.img(2) = imshow(img);
    end
    h.txt(1) = uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[5 5 130 20], 'String',sprintf('Contrast: %.2f',a));
    h.txt(2) = uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[5 30 130 20], 'String',sprintf('Brightness: %2d',b));
    h.txt(3) = uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[5 55 130 20], 'String',sprintf('Gamma: %.2f',g));
    h.slid(1) = uicontrol('Parent',h.fig, 'Style','slider', 'Value',a, ...
        'Min',0.1, 'Max',3, 'SliderStep',[0.01 0.2]./(3-0.1), ...
        'Position',[135 5 sz(2)-135-5 20]);
    h.slid(2) = uicontrol('Parent',h.fig, 'Style','slider', 'Value',b, ...
        'Min',-100, 'Max',100, 'SliderStep',[1 20]./(100+100), ...
        'Position',[135 30 sz(2)-135-5 20]);
    h.slid(3) = uicontrol('Parent',h.fig, 'Style','slider', 'Value',g, ...
        'Min',0.1, 'Max',3, 'SliderStep',[0.01 0.2]./(3-0.1), ...
        'Position',[135 55 sz(2)-135-5 20]);

    % hook event handlers, and trigger default start
    opts = {'Interruptible','off', 'BusyAction','cancel'};
    set(h.slid(1:2), 'Callback',{@onLinearTransform,h}, opts{:});
    set(h.slid(3), 'Callback',{@onGammaCorrection,h}, opts{:});
    onLinearTransform([],[],h);
    onGammaCorrection([],[],h);
end


function angdataw=angtimewarp(evLatency, newLatency, angdata)
% ANGTIMEWARP - Given two event marker vectors, computes a
%                 warping of the input angular time series so that its
%                 evlatencies match newlatencies. Values of the warped
%                 timeserie that falls between two frames in the original
%                 timeserie will be linearly interpolated under the
%                 assumption that phase change is minimal between two
%                 successive time points.
% Usage:
%   >> warpAngs = angtimewarp(evlatency, newlatency, angData)
%
% Necessary inputs:
%   evlatency  - [vector] time markers on the original time-series, in
%                frames. Markers must be ordered by increasing
%                latency. If you want to warp the entire time series,
%                make sure frame 1 and the last frame are in the vector.
%   newlatency - [vector] desired time marker latencies. The original
%                time series will be warped so that its time markers (see
%                evlatency) match the ones in newlatency. newlatency
%                frames must be sorted by ascending latencies in frames.
%                Both vectors have to be the same length.
%   angData    - [vector] original angular time series (in radians).
%                Angles should be between -pi and pi.
%
% Optional outputs:
%   warpAngs   - [vector] warped angular time-course, with values between
%                -pi and pi
%
% Example:
%   >> angs = 2*pi*rand(1,10)-pi;
%   >> warpangs = angtimewarp([1 5 10], [1 6 10], angs)
%
% Authors: Jean Hausser, SCCN/INC/UCSD, 2006
%
% See also: TIMEWARP, PHASECOHER, ERPIMAGE, NEWTIMEF

  if min(sort(evLatency) == evLatency) == 0
    error('evlatency should be sorted');
    return;
  end
  if min(sort(newLatency) == newLatency) == 0
    error('newlatency should be sorted');
    return;
  end
  if length(evLatency) ~= length(newLatency)
    error('evlatency and newlatency must have the same length.');
    return;
  end
  if length(evLatency) < 2 || length(newLatency) < 2
    error(['There should be at least two events in evlatency and ' ...
          'newlatency, that is "begin" and "end"' ]);
    return;
  end
  if evLatency(1) ~= 1
    disp(['Assuming old and new time series beginnings are ' ...
          'synchronized']);
    disp(['Make sure you defined an end event for both old and new time ' ...
          'series !']);
    evLatency(end+1)=1;
    newLatency(end+1)=1;
    evLatency = sort(evLatency);
    newLatency = sort(newLatency);
  end

  t = 1:max(evLatency);

  for k=1:length(evLatency)-1
    for i=evLatency(k):evLatency(k+1)-1
      tp(i) = (t(i)-evLatency(k)) * ...
              (newLatency(k+1) - newLatency(k))/...
              (evLatency(k+1) - evLatency(k)) + ...
              newLatency(k);
    end
  end

  %Check what's going on at tp(max(newLatency)), should equal t(max(evLatency))
  tp(max(evLatency)) = max(newLatency);
  ts = tp-min(newLatency)+1;

  angdataw = zeros(1, max(newLatency)-min(newLatency)+1);

  k = 0;
  for i=1:length(angdataw)
    while i > ts(k+1)
      k = k+1;
    end

    if k == 0

      angdataw(1) = angdata(1);
    else

      %Linear interp
      angdataw(i) = angdata(k)*(1 - (i-ts(k))/(ts(k+1)-ts(k))) + ...
                    angdata(k+1)*(1 - (ts(k+1)-i)/(ts(k+1)-ts(k)));

%       %Correction because angles have a ring structure
%       theta1 = [angdata(k) angdata(k+1) angdataw(i)];
%       theta2 = theta1 - min(angdata(k), angdata(k+1));
%       theta2max = max(theta2(1), theta2(2));
%       if ~ ( (theta2max <= pi & theta2(3) <= theta2max) | ...
%              (theta2max >= pi & theta2(3) >= theta2max) | ...
%              theta2(3) == theta2(1) | theta2(3) == theta2(2) )
%         angdataw(i) = angdataw(i) + pi;
%       end
%       if angdataw(i) > pi %Make sure we're still on [-pi, pi]
%         angdataw(i) = angdataw(i) - 2*pi;
%       end
    end
  end
  angdataw = wrap2pi(angdataw);

  function a = wrap2pi(a, a_center )
% function a = wrap(a,a_center)
%
% Wraps angles to a range of 2*pi.
% Inverse of Matlab's "unwrap", and better than wrapToPi ( which has
% redundant [-pi,pi])
% Optional input "a_center" defines the center angle.  Default is 0, giving
% angles from (-pi,pi], chosen to match angle(complex(-1,0)).  Maximum
% possible value is pi.

% T.Hilmer, UH
% 2010.10.18 version 2
%   removed code from version 1. Have not bug-checked second input
%   "a_center"

if nargin < 2, a_center = 0; end

% new way
a = mod(a,2*pi); % [0 2pi)

% shift
j = a > pi - a_center;
a(j) = a(j) - 2*pi;
j = a < a_center - pi;
a(j) = a(j) + 2*pi;


% VL_DEMO_KMEANS_ANN_SPEED   Compares Lloyd's, Elkan, and ANN k-means

numCenters = 200 ;
numTrials = 3 ;
maxNumIterations = 10 ;
initialization = 'randsel' ;
%initialization = 'plusplus';
distance = 'l2' ;

%% Create an example dataset

dimension = 32 ;
numData = 50000 ;

X = randn(dimension,numData);

%% Run various k-means algorithms on the data
algorithms = {'Lloyd','Elkan','ANN 1/4','ANN 1/10','ANN 1/50' } ;
options = {{'Algorithm', 'Lloyd'}, ...
           {'Algorithm', 'Elkan'}, ...
           {'Algorithm', 'ANN', 'MaxNumComparisons', ceil(numCenters / 4)}, ...
           {'Algorithm', 'ANN', 'MaxNumComparisons', ceil(numCenters / 10)}, ...
           {'Algorithm', 'ANN', 'MaxNumComparisons', ceil(numCenters / 50)}} ;
numCpus = [1 0] ;

clear time energy ;
for n = 1:2
  for a = 1:numel(algorithms)
    for t = 1:numTrials
      vl_threads(numCpus(n)) ;
      start = tic ;
      [C, A, E] = vl_kmeans(X, ...
                            numCenters, 'Verbose', ...
                            'Distance', distance, ...
                            'MaxNumIterations', maxNumIterations, ...
                            options{a}{:}) ;
      if vl_isoctave()
        time(t,a,n) = (tic() - start) / 1e6 ;
      else
        time(t,a,n) = toc(start) ;
      end
      energy(t,a,n) = E ;
    end
  end
end

% average over tirals
time = squeeze(mean(time,1)) ;
energy = squeeze(mean(energy,1)) ;

figure(1) ; clf ;
for n=1:2
  if n == 1
    str = 'Serial' ;
  else
    str = 'Parallel' ;
  end

  subplot(3,2,(n-1)+1) ;
  bar(time(:,n)) ;
  set(gca,'XTickLabel',algorithms);
  set(gca,'FontSize',8),
  xlabel('Algorithm');
  ylabel('Time [s]');
  title(str) ;

  subplot(3,2,(n-1)+3) ;
  bar(energy(:,n));
  set(gca,'XTickLabel',algorithms);
  set(gca,'FontSize',8),
  xlabel('Algorithm');
  ylabel('Energy');
  title(str) ;

  subplot(3,2,(n-1)+5) ;
  bar(time(1,1)./time(:,n)) ;
  set(gca,'XTickLabel',algorithms);
  set(gca,'FontSize',8),
  xlabel('Algorithm');
  ylabel('Speedup');
  title(str) ;
end

vl_demo_print('kmeans_speed',1);


function partition_count_values_test ( )

%*****************************************************************************80
%
%% PARTITION_COUNT_VALUES_TEST demonstrates the use of PARTITION_COUNT_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2005
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PARTITION_COUNT_VALUES_TEST:\n' );
  fprintf ( 1, '  PARTITION_COUNT_VALUES returns values of \n' );
  fprintf ( 1, '  the integer partition count function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N         P(N)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, fn ] = partition_count_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %4d  %10d\n', n, fn );

  end

  return
end


classdef MOEADM2M_F3 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing MOEA/D-M2M

%------------------------------- Reference --------------------------------
% H. Liu, F. Gu, and Q. Zhang, Decomposition of a multiobjective
% optimization problem into a number of simple multiobjective subproblems,
% IEEE Transactions on Evolutionary Computation, 2014, 18(3): 450-455.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            t = X(:,2:end) - repmat(sin(pi/2*X(:,1)),1,size(X,2)-1);
            g = 10*sin(pi/2*X(:,1)).*sum(abs(t)./(1+exp(5*abs(t))),2);
            PopObj(:,1) = (1+g).*cos(pi/2*X(:,1));
            PopObj(:,2) = (1+g).*sin(pi/2*X(:,1));
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = sqrt(1-R(:,1).^2);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function gX = ardKernGradX(kern, X, X2)

% ARDKERNGRADX Gradient of ARD kernel with respect to a point x.
% FORMAT
% DESC computes the gradient of the pre-built RBF and linear ARD
% kernel with respect to the input positions.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x : locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData x numInputs x numData. Where numData is
% the number of data points and numInputs is the number of input
% dimensions in X.
%
% FORMAT
% DESC computes the gradident of the pre-built RBF and linear ARD
% kernel with respect to the input positions where both the row
% positions and column positions are provided separately.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x1 : row locations against which gradients are being computed.
% ARG x2 : column locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData2 x numInputs x numData1. Where numData1 is
% the number of data points in X1, numData2 is the number of data
% points in X2 and numInputs is the number of input
% dimensions in X.
%
% SEEALSO ardKernParamInit, kernGradX, ardKernDiagGradX
%
% COPYRIGHT : Neil D. Lawrence, 2004

% KERN

gX = zeros(size(X2, 1), size(X2, 2), size(X, 1));
for i = 1:size(X, 1);
  gX(:, :, i) = ardKernGradXpoint(kern, X(i, :), X2);
end

function gX = ardKernGradXpoint(kern, x, X2)

% ARDKERNGRADXPOINT Gradient with respect to one point of x.

scales = sparse(diag(kern.inputScales));

gX = kern.linearVariance.*X2*scales;

scales = sqrt(scales);

n2 = dist2(X2*scales, x*scales);
wi2 = (.5 .* kern.inverseWidth);
rbfPart = kern.rbfVariance*exp(-n2*wi2);
for i = 1:size(x, 2)
  gX(:, i) = gX(:, i) + kern.inverseWidth*kern.inputScales(i)*(X2(:, i) - x(i)).*rbfPart;
end


%DEMO_SVI_CLASSIFIC  Classification problem demonstration for SVI GP
%
%  Description
%    The demonstration program is based on synthetic two class data
%    used by B.D. Ripley (Pattern Recognition and Neural Networks,
%    1996}. The data consists of 2-dimensional vectors that are
%    divided into two classes, labeled 0 or 1. Each class has a
%    bimodal distribution generated from equal mixtures of Gaussian
%    distributions with identical covariance matrices. A Bayesian
%    approach is used to find the decision line and predict the
%    classes of new data points. The result can be compared to the
%    ones from the DEMO_CLASSIFIC.
%
%    The probability of y being one is assumed to be
%
%      p(y=1|f) = normcdf(f)
%
%    The latent values f are given a zero mean Gaussian process
%    prior. This implies that at the observed input locations
%    latent values have prior
%
%      f ~ N(0, K),
%
%    where K is the covariance matrix, whose elements are given as
%    K_ij = k(x_i, x_j | th). The function k(x_i, x_j | th) is
%    covariance function and th its parameters.
%
%    Here we demonstarte use of stochastic variational inference
%    methods to find the posterior of the latent values and parameters.
%    With these we can make predictions on the class probability of
%    future observations. See Hensman et. al. (2013) for the
%    detailed treatment.
%
%  See also
%    DEMO_SVI_REGRESSION, DEMO_CLASSIFIC
%
%
%  References:
%    Hensman, J., Fusi, N. and Lawrence, N. D. (2013). Gaussian
%    processes for big data. arXiv preprint arXiv:1309.6835.

% Copyright (c) 2014 Tuomas Sivula

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% This demonstration is based on the dataset used in the book Pattern
% Recognition and Neural Networks by B.D. Ripley (1996), Cambridge
% University Press.

% Training data
S = which('demo_classific');
L = strrep(S,'demo_classific.m','demodata/synth.tr');
x=load(L);
y=x(:,end);
y = 2.*y-1;
x(:,end)=[];
[n, nin] = size(x);

% Test data
xt1=repmat(linspace(min(x(:,1)),max(x(:,1)),20)',1,20);
xt2=repmat(linspace(min(x(:,2)),max(x(:,2)),20)',1,20)';
xt=[xt1(:) xt2(:)];

% Create likelihood function
lik = lik_probit();
%lik = lik_logit();

% Create covariance functions
gpcf = gpcf_sexp('lengthScale', [0.9 0.9], 'magnSigma2', 10);
% Set the prior for the parameters of covariance functions
pl = prior_t();
pm = prior_sqrtt('s2',0.5);
gpcf = gpcf_sexp(gpcf, 'lengthScale_prior', pl,'magnSigma2_prior', pm);

% Create the GP structure (type is by default FULL)
fprintf('SVI GP classification model with probit likelihood\n')
gp = gp_set('lik', lik, 'cf', gpcf, ...
  'latent_method', 'SVI', 'jitterSigma2', 1e-6);

% Select 20 inducing inputs by clustering 10 from both training class
% inputs
fprintf(['Select 20 inducing inputs by clustering 10 points ', ...
  'from both training classes ...'])
Sw = warning('off','stats:kmeans:EmptyCluster');
[~,X_u1] = kmeans(x(y==1,:), 10,'Start','uniform',...
    'EmptyAction','singleton');
[~,X_u2] = kmeans(x(y==-1,:), 10,'Start','uniform',...
    'EmptyAction','singleton');
warning(Sw);
X_u = [X_u1 ; X_u2];
fprintf(' done\n')

% Optimise
maxi = 1000; % The maximum number of iteration rounds
gp = svigp(gp,x,y,'X_u',X_u,'maxiter',maxi,'mu2',1e-7);
% Make predictions
[Eft, Varft, lpyt, Eyt, Varyt] = ...
    gpsvi_pred(gp, x, y, xt, 'yt', ones(size(xt,1),1) );

% Visualise predictive probability p(ystar = 1) with grayscale
figure, hold on;
n_pred=size(xt,1);
h1=pcolor(reshape(xt(:,1),20,20),reshape(xt(:,2),20,20),reshape(exp(lpyt),20,20));
set(h1, 'edgealpha', 0), set(h1, 'facecolor', 'interp')
colormap(repmat(linspace(1,0,64)', 1, 3).*repmat(ones(1,3), 64,1))
axis([-inf inf -inf inf]), %axis off
plot(x(y==-1,1),x(y==-1,2),'o', 'markersize', 8, 'linewidth', 2);
plot(x(y==1,1),x(y==1,2),'rx', 'markersize', 8, 'linewidth', 2);
plot(gp.X_u(:,1),gp.X_u(:,2),'gs', 'markersize', 8, 'linewidth', 1);
set(gcf, 'color', 'w'), title('predictive probability, training cases and inducing inputs with SVIGP', 'fontsize', 14)


% op_alignISIS.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% [out,fs,phs]=op_alignISIS(in,tmax,initPars);
%
% DESCRIPTION:
% Apply spectral registration to align ISIS subspectra prior to
% subtraction.  This is intended to be used prior to averaging, so that the
% alignment can be performed independently for each average.
%
% INPUTS:
% in        = Input data structure.
% tmax      = Maximum time (s) in time domain to use for alignment.
% initPars	= (Optional) Initial fit parameters [freq(Hz), phase(degrees)]. Default=[0,0];
%
% OUTPUTS:
% out       = Output following alignment of ISIS subspectra.
% fs        = Vector of frequency shifts (in Hz) used for alignment.
% phs       = Vector of phase shifts (in degrees) used for alignment.

function [out,fs,phs]=op_alignISIS(in,tmax,initPars)

if ~in.flags.addedrcvrs
    error('ERROR:  I think it only makes sense to do this after you have combined the channels using op_addrcvrs.  ABORTING!!');
end

if in.dims.subSpecs==0
    error('ERROR:  Must have multiple subspectra.  ABORTING!!');
end

if nargin<3
    parsGuess=[0,0];
else
    parsGuess=initPars;
end

if in.dims.averages
    fs=zeros(in.sz(in.dims.averages),1);
    phs=zeros(in.sz(in.dims.averages),1);
else
    fs=0;
    phs=0;
end
fids=zeros(in.sz(in.dims.t),1);

disp('aligning all averages to the Average ISIS subtracted spectrum');
if in.dims.averages
    base0=op_median(op_combinesubspecs(in,'diff'));
else
    base0=op_combinesubspecs(in,'diff');
end
base=[real(base0.fids( base0.t>=0 & base0.t<tmax ));imag(base0.fids( base0.t>=0 & base0.t<tmax ))];
begin=1;
if in.dims.averages
    for n=begin:in.sz(in.dims.averages)
        %disp(['fitting subspec number ' num2str(m) ' and average number ' num2str(n)]);
        parsFit=nlinfit(squeeze(in.fids(in.t>=0 & in.t<tmax,n,:)),base,@op_freqPhaseShiftComplexNest,parsGuess);
        A=op_freqPhaseShiftNest(parsFit,in.fids(:,n,:));
        size(A);
        size(fids);
        fids(:,n,1)=A(:,1);
        fids(:,n,2)=A(:,2);
        fs(n)=parsFit(1);
        phs(n)=parsFit(2);
        %plot(in.ppm,fftshift(ifft(fids(:,1,m))),in.ppm,fftshift(ifft(fids(:,n,m))));
    end
else
    n=1;
    %disp(['fitting subspec number ' num2str(m) ' and average number ' num2str(n)]);
    parsFit=nlinfit(squeeze(in.fids(in.t>=0 & in.t<tmax,:)),base,@op_freqPhaseShiftComplexNest,parsGuess);
    A=op_freqPhaseShiftNest(parsFit,in.fids(:,:));
    size(A);
    size(fids);
    fids(:,1)=A(:,1);
    fids(:,2)=A(:,2);
    fs(n)=parsFit(1);
    phs(n)=parsFit(2);
    %plot(in.ppm,fftshift(ifft(fids(:,1,m))),in.ppm,fftshift(ifft(fids(:,n,m))));
end

%re-calculate Specs using fft
specs=fftshift(ifft(fids,[],in.dims.t),in.dims.t);

%FILLING IN DATA STRUCTURE
out=in;
out.fids=fids;
out.specs=specs;

%FILLING IN THE FLAGS
out.flags=in.flags;
out.flags.writtentostruct=1;
out.flags.freqcorrected=1;

    function y=op_freqPhaseShiftComplexNest(pars,input)
        f=pars(1);     %Frequency Shift [Hz]
        p=pars(2);     %Phase Shift [deg]

        dwelltime=in.dwelltime;
        t=0:dwelltime:(length(input)-1)*dwelltime;
        fid=input(:,2);

        shifted=addphase(fid.*exp(-1i*t'*f*2*pi),p);
        subtracted=input(:,1)+shifted;
        subtracted=subtracted/2;

        y=[real(subtracted);imag(subtracted)];
        %y=real(fid.*exp(-1i*t'*f*2*pi));

    end

    function y=op_freqPhaseShiftNest(pars,input)
        f=pars(1);     %Frequency Shift [Hz]
        p=pars(2);     %Phase Shift [deg]

        dwelltime=in.dwelltime;
        t=0:dwelltime:(length(input)-1)*dwelltime;
        fid=input(:,2);

        shifted=addphase(fid.*exp(-1i*t'*f*2*pi),p);
        y(:,1)=input(:,1);
        y(:,2)=shifted;
        %y=real(fid.*exp(-1i*t'*f*2*pi));

    end

end


function s = signum (x)
%SIGNUM compute and display the sign of a column vector x
% Example
%   s = signum(x)
% See also: testall

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

s = ones (length (x),1) ;
s (find (x < 0)) = -1 ;     %#ok
disp ('s =') ;
disp (s) ;


function v = wrap(u,N)
% WRAP Wrap a vector of indices around a torus.
% v = wrap(u,N)
%
% e.g., wrap([-1 0 1 2 3 4], 3)   =   2 3 1 2 3 1

v = mod(u-1,N)+1;


function [out] = interflow_5(p1,S)
%interflow_5

% 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:  Linear interflow
% Constraints:  -
% @(Inputs):    p1   - time coefficient [d-1]
%               S    - current storage [mm]

out = p1.*S;

end


% GSPBOX - Embeddings
%
%  Availlable embeddings
%    gsp_lle                       -  Local Linear Embedding
%    gsp_laplacian_eigenmaps       -  Laplacian Eigenmaps
%    gsp_isomap                    -  Isomap
%    gsp_eigenspace_estimation     -  Fast Eigenspace Approximation using Random Signals (FEARS)
%
%  Utils
%    gsp_weight2distance           -  Distance matrix from weight matrix
%    gsp_compute_coordinates       -  Compute new coordinates for a graph
%
%  For help, bug reports, suggestions etc. please send email to
%  gspbox 'dash' support 'at' groupes 'dot' epfl 'dot' ch
%


function y=scInv(z, LinCol)
%scInv : Inverts a given impedance (admitance) by mirroring it about the origin
%
%  SYNOPSIS:
%     This function inverts the given impedance (admittance) by mirroring it about
%     the origin of the smith chart.
%
%     See also scDraw, scMove, scConCirc, scMatchCirc
%
%  SYNTAX:
%     [y] = scInv(z)
%
%  INPUT ARGUMENTS:
%     z     : Impedance (or admittance)
%
%  OUTPUT ARGUMENT:
%     y : Inverted admittance (or impedance)
%
%  EXAMPLE:
%        y = scInv([2 3])
%        y =
%        0.1538   -0.2308
%
%
%     Mohammad Ashfaq - (31-05-2000)
%     Mohammad Ashfaq - (13-04-2006) Modified (example included)
%

 if nargin < 1
    error('scInv.m: One input argument required...You may give LinCol as second argument as well');
 end

 if nargin == 1
    LinCol = 'm';
 end

 if (size(z)==[1, 1])
    if conj(z) == z
       r = z;
       x = 0;
    else
       r = real (z);
       x = imag (z);
    end
 elseif max(size(z)) == 2 && min(size(z)) == 1
    r = z(1);
    x = z(2);
 else
    error('scInv.m: Input must either be a complex number or a 1x2 vector  [r x]');
 end

 if (r+j*x)~=0
    y1   = 1/(r+j*x);
    y(1) = real(y1);
    y(2) = imag(y1);

 else
    y = inf;
 end


% LTFAT - Simple auditory processing
%
%  Peter L. Søndergaard, 2011 - 2018
%
%  Plots
%     SEMIAUDPLOT      - 2D plot on auditory scale.
%
%  Auditory scales
%     AUDTOFREQ        - Auditory unit to frequency conversion.
%     FREQTOAUD        - Frequency to auditory unit conversion.
%     AUDSPACE         - Auditory unit spaced vector
%     AUDSPACEBW       - Auditory unit spaced vector by equal bandwidth.
%     ERBTOFREQ        - Erb scale to frequency conversion.
%     FREQTOERB        - Frequency to erb scale conversion.
%     ERBSPACE         - Equidistant points on the erb scale.
%     ERBSPACEBW       - Equidistant points by equal bandwidth.
%     AUDFILTBW        - Bandwidth of audiory filters.
%
%  Range compression
%     RANGECOMPRESS    - Compress range of signal (mu-law etc).
%     RANGEEXPAND      - Expand range of signal.
%
%  Auditory filters
%     GAMMATONEFIR     - Gammatone FIR approximation.
%
%  For help, bug reports, suggestions etc. please visit
%  http://github.com/ltfat/ltfat/issues


function jed = ymdf_to_jed_jelali ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_JED_JELALI converts a Jelali YMDF date to a JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, real JED, the corresponding Julian Ephemeris Date.
%
  jed_epoch = epoch_to_jed_jelali ( );

  jed = jed_epoch + ( d - 1 ) + 30 * ( m - 1 ) + 365 * ( y - 1 ) ...
    + floor ( ( y - 1 ) / 4 ) + f;

  return
end


% PURPOSE: demo of chowlin()
%          Temporal disaggregation with indicators.
% 			  Chow-Lin method
%---------------------------------------------------
% USAGE: chowlin_d
%---------------------------------------------------

close all; clear all; clc;

% Low-frequency data: Spain's Exports of Goods. 1995 prices

Y=[  20499
     23477
     25058
     27708
     31584
     31898
     30233
     32235
     34049
     36035
     39795
     44299
     47426
     52339
     62949
     69885
     77174
     90133
     96496
    102776
    113026
    115573 ];

% High-frequency data: Spain's Registered exports of goods deflated by
%                      unit value index.

x=[   5162
      5054
      4049
      5196
      4972
      5606
      5844
      6196
      6526
      5671
      5631
      6510
      6575
      6797
      5973
      6796
      8404
      8260
      7058
      7403
      7934
      7762
      7087
      8659
      7471
      8082
      6700
      8117
      8271
      8336
      7698
      8372
      9120
      8911
      8035
      8613
      9725
      9529
      7774
      9295
     10357
     10372
      9056
     10812
     11989
     11839
      9686
     11736
     12878
     12211
     10278
     12321
     13267
     12973
     11268
     15008
     16565
     15641
     13684
     17254
     18613
     17774
     14966
     18543
     19287
     19399
     17299
     21065
     20687
     23215
     21382
     24935
     24256
     25558
     21680
     24951
     25284
     26149
     23344
     27754
     28271
     29835
     26148
     30917
     30494
     30486
     26153
     29930 ];

% ---------------------------------------------
% Inputs for td library

% Type of aggregation
ta=1;
% Frequency conversion
sc=4;
% Method of estimation
type=1;
% Intercept
opC = -1;
% Interval of rho for grid search
% rl = [-.33 .80];
rl = 0.57;
% rl = [];
% Name of ASCII file for output
file_sal='td.sal';
% Calling the function: output is loaded in a structure called res
res=chowlin(Y,x,ta,sc,type,opC,rl);
% Calling printing function
tdprint(res,file_sal);
edit td.sal;
% Calling graph function
tdplot(res);


function [warp_pts center]= getLKcorner(warp_p, sz)

template_nx = sz(2);
template_ny = sz(1);

tmplt_pts= [1, 1;
        1, template_ny;
        template_nx, template_ny;
        template_nx, 1]';

if size(warp_p,1)==2
    M = [warp_p; 0 0 1];
    M(1,1) = M(1,1) + 1;
    M(2,2) = M(2,2) + 1;
else
    M = warp_p;
%     M(1,3) = M(1,3) + 1;
%     M(2,3) = M(2,3) + 1;
end

warp_pts = M * [tmplt_pts; ones(1, size(tmplt_pts,2))];

c = [(1+template_nx)/2; (1+template_ny)/2; 1];

center = M * c;

warp_pts = warp_pts(1:2,:);

center = center(1:2)';


%%*********************************************************************
%% This is a test example in using interface
%% to solve the following FAP problem
%% max <((k-1)/2k)*L(G,W)-1/2*Diag(We),X>
%% s.t. diag(X)==1 ; X positive semidefinite
%%      X(i,j) == -1/(k-1)  for (i,j) in U_set
%%      X(i,j) >= -1/(k-1)  for (i,j) in Edge_set\U_set
%% SDPNAL+:
%% Copyright (c) 2017 by
%% Yancheng Yuan, Kim-Chuan Toh, Defeng Sun and Xinyuan Zhao
%%*********************************************************************

%% read FAP data: U_set, Edge_set
 clear all;
 fname = 'fap08';
 if exist(fname)
    fid = fopen(fname,'r');
 elseif exist([fname,'.dat']);
    fid = fopen([fname,'.dat'],'r');
 else
    error('** Problem not found. \n');
 end
 [tmpr,count] = fscanf(fid,'%c');
 datavec = sscanf(tmpr,'%f'); clear tmpr;
 n = datavec(1);
 numedges = datavec(2);
 kpara = datavec(3);
 datavec = datavec(4:length(datavec));
 len = length(datavec);
 if (len ~= 3*numedges)
    error(' fapread: numedges and data do not match.');
 end
 I = datavec(1:3:len);
 J = datavec(2:3:len);
 w = datavec(3:3:len);
 idxU = find(w==1000);
 IU = I(idxU); JU = J(idxU); wU = w(idxU);
 idxE = find(w~=1000);
 IE = I(idxE); JE = J(idxE); wE = w(idxE);
 fclose(fid);
 %% model the problem using the interface
 GE = spconvert([IE JE wE; n n 0]);
 GE = GE + GE';
 GU = spconvert([IU JU wU; n n 0]);
 GU = GU + GU';
 LG = diag(GE*ones(n,1))-GE;
 C =  -0.5*diag(GE*ones(n,1)) + ((kpara-1)/(2*kpara))*LG;
 %%*********************************************************************
 model = ccp_model('Example_FAP');
     X = var_sdp(n, n);
     model.add_variable(X);
     model.maximize(inprod(C,X));
     model.add_affine_constraint(map_diag(X) == ones(n,1));
     const = -1/(kpara-1);
     model.add_affine_constraint(X(IU,JU)==const);
     model.add_affine_constraint(X(IE,JE) >= const);
 model.solve;
 %%*********************************************************************


%% TOMLAB PROPT Parameter Estimation Problems
% http://tomdyn.com/parameter_estimation_dynamic_systems.html
clc
clear

%% http://tomdyn.com/examples/catalyticCracking.html
clc
ode = @(t,z,p) [-(p(1)+p(3))*z(1).^2
                p(1)*z(1).^2-p(2)*z(2)];
z0 = [1;0]; %ic

% Various constants and expressions
y1meas = [1.0;0.8105;0.6208;0.5258;0.4345;0.3903;...
    0.3342;0.3034;0.2735;0.2405;0.2283;0.2071;0.1669;...
    0.153;0.1339;0.1265;0.12;0.099;0.087;0.077;0.069];
y2meas = [0;0.2;0.2886;0.301;0.3215;0.3123;0.2716;...
    0.2551;0.2258;0.1959;0.1789;0.1457;0.1198;0.0909...
    ;0.0719;0.0561;0.046;0.028;0.019;0.014;0.010];
tmeas = [0;0.025;0.05;0.075;0.1;0.125;...
    0.15;0.175;0.2;0.225;0.25;0.3;0.35;0.4;...
    0.45;0.5;0.55;0.65;0.75;0.85;0.95];

%Build OPTI Object
theta0 = [10;8;1]; %inital parameter guess
opts = optiset('display','iter');
Opt = opti('ode',ode,'data',tmeas,[y1meas' y2meas'],'theta0',theta0,'z0',z0,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% http://tomdyn.com/examples/isometrizationAlpha.html
clc
ode = @(t,z,p) [-(p(1)+p(2))*z(1)
                p(1)*z(1)
                p(2)*z(1)-(p(3)+p(4))*z(3)+p(5)*z(5)
                p(3)*z(3)
                p(4)*z(3)-p(5)*z(5)];
z0 = [100;0;0;0;0]; %ic

y1meas = [88.35; 76.4; 65.1; 50.4; 37.5; 25.9; 14.0; 4.5];
y2meas = [7.3; 15.6; 23.1; 32.9; 42.7; 49.1; 57.4; 63.1];
y3meas = [2.3; 4.5; 5.3; 6.0; 6.0; 5.9; 5.1; 3.8];
y4meas = [0.4; 0.7; 1.1; 1.5; 1.9; 2.2; 2.6; 2.9];
y5meas = [1.75; 2.8; 5.8; 9.3; 12.0; 17.0; 21.0; 25.7];
tmeas  = [1230; 3060; 4920; 7800; 10680; 15030; 22620; 36420];

%Build OPTI Object
theta0 = [0;0;0;0;0]; %inital parameter guess
opts = optiset('display','iter','dynamicOpts',optidynset('sensitivity','cs','initialT',0));
Opt = opti('ode',ode,'data',tmeas,[y1meas' y2meas' y3meas' y4meas' y5meas'],'theta0',theta0,'z0',z0,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% http://tomdyn.com/examples/marinePopulation.html
clc
ode = @(t,z,p) [[0; p(9:end)].*[0; z(1:7)] - (p(1:8)+[p(9:end);0]).*z];

ymeas = [20000 17000 10000 15000 12000 9000 7000 3000
    12445 15411 13040 13338 13484 8426 6615 4022
     7705 13074 14623 11976 12453 9272 6891 5020
     4664  8579 12434 12603 11738 9710 6821 5722
     2977  7053 11219 11340 13665 8534 6242 5695
     1769  5054 10065 11232 12112 9600 6647 7034
      943  3907  9473 10334 11115 8826 6842 7348
      581  2624  7421 10297 12427 8747 7199 7684
      355  1744  5369  7748 10057 8698 6542 7410
      223  1272  4713  6869  9564 8766 6810 6961
      137   821  3451  6050  8671 8291 6827 7525
       87   577  2649  5454  8430 7411 6423 8388
       49   337  2058  4115  7435 7627 6268 7189
       32   228  1440  3790  6474 6658 5859 7467
       17   168  1178  3087  6524 5880 5562 7144
       11    99   919  2596  5360 5762 4480 7256
        7    65   647  1873  4556 5058 4944 7538
        4    44   509  1571  4009 4527 4233 6649
        2    27   345  1227  3677 4229 3805 6378
        1    20   231   934  3197 3695 3159 6454
        1    12   198   707  2562 3163 3232 5566];
tmeas  = 0:0.5:10;

z0 = ymeas(1,:); %ic

%Build OPTI Object
theta0 = [zeros(8,1);zeros(7,1)]; %inital parameter guess
opts = optiset('display','iter','dynamicOpts',optidynset('sensitivity','cs'));
Opt = opti('ode',ode,'data',tmeas,ymeas,'theta0',theta0,'z0',z0,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% http://tomdyn.com/examples/methanolToHydrocarbons.html
clc
ode = @(t,z,p) [-(2*p(2)-(p(1)*z(2))./((p(2)+p(5))*z(1)+z(2))+p(3)+p(4)).*z(1)
                (p(1)*z(1).*(p(2)*z(1)-z(2)))./((p(2)+p(5))*z(1)+z(2))+p(3)*z(1)
                (p(1)*z(1).*(z(2)+p(5)*z(1)))./((p(2)+p(5))*z(1)+z(2))+p(4)*z(1)];

y1meas = [0.7085;0.5971;0.5537;0.3684;0.1712;...
    0.1198;0.0747;0.0529;0.0415;0.0261;0.0208;...
    0.0085;0.0053;0.0019;0.0018];
y2meas = [0.1621;0.1855;0.1989;0.2845;0.3491;...
    0.3098;0.3576;0.3347;0.3388;0.3557;0.3483;...
    0.3836;0.3611;0.3609;0.3485];
y3meas = [0.0811;0.0965;0.1198;0.1535;0.2097;...
    0.2628;0.2467;0.2884;0.2757;0.3167;0.2954;...
    0.295;0.2937;0.2831;0.2846];
tmeas = [0.05;0.065;0.08;0.123;0.233;0.273;...
    0.354;0.397;0.418;0.502;0.553;...
    0.681;0.75;0.916;0.937];

z0 = [1;0;0]; %ic
lb = ones(5,1)*sqrt(eps);
ub = 10*ones(5,1);

%Build OPTI Object
theta0 = [1;1;1;1;1]; %inital parameter guess
opts = optiset('display','iter','solver','nl2sol','dynamicOpts',optidynset('initialT',0,'sensitivity','cs'));
Opt = opti('ode',ode,'bounds',lb,ub,'data',tmeas,[y1meas y2meas y3meas],'theta0',theta0,'z0',z0,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)

%% http://tomdyn.com/examples/parameterEstimation.html
clc
ode = @(t,z,p) [p(1)*z(2); 1-2*z(2)-z(1)];

y1meas = [0.264;0.594;0.801;0.959];
tmeas = [1;2;3;5];

z0 = [NaN;NaN]; %ic

lb = -1.5*ones(3,1); lb(1)=1; %not actually interested in p1, but needed for OPTI
ub = 1.5*ones(3,1); ub(1)=1;

%Build OPTI Object
theta0 = [1;0;0]; %inital parameter guess
opts = optiset('display','iter','solver','nl2sol','dynamicOpts',optidynset('stateIndex',1,'initialT',0,'sensitivity','cs'));
Opt = opti('ode',ode,'bounds',lb,ub,'data',tmeas,y1meas,'theta0',theta0,'z0',z0,'options',opts)

[x,f,e,i] = solve(Opt)
plot(Opt)


function [c] = mstep_update_c(P, S_bar, V, E_z, E_zz, RO, Tr)
%MSTEP_UPDATE_C Summary of this function goes here
%   Detailed explanation goes here

%equation 48 of PAMI paper

[K, T] = size(E_z);
J = size(S_bar, 2);
c = zeros(T,1);
Pc = P - Tr(:)*ones(1,J);

parfor t=1:T

    zz_hat_t = [1 E_z(:,t)'; E_z(:,t) E_zz((t-1)*K+1:t*K,:)];
    z_hat_t = [1;E_z(:,t)];
    num = 0;
    den = 0;
    for j=1:J
        M_jt = [1 0 0 ; 0 1 0]*RO{t}*[S_bar(:,j) reshape(V(:,j), 3, K)];
        num = num + z_hat_t'*M_jt'*[Pc(t,j); Pc(t+T,j)];
        den = den+trace(M_jt*zz_hat_t*M_jt');
    end
    c(t,1) = num/den;
end

end


function [px,py,pz,tx,ty,tz,bx,by,bz,ierr] = nd2pt(wanx,wany,wanz,wdx,wdy,wdz)

%% c     compute Cartesian component of P, T and B axes from outward normal
%% c     and slip vectors
%% c
%% c     usage:
%% c     call nd2pt(anx,any,anz,dx,dy,dz,px,py,pz,tx,ty,tz,bx,by,bz,ierr)
%% c
%% c     arguments:
%% c     anx,any,anz    components of fault plane outward normal vector in the
%% c                    Aki-Richards Cartesian coordinate system (INPUT)
%% c     dx,dy,dz       components of slip vector in the Aki-Richards
%% c                    Cartesian coordinate system (INPUT)
%% c     px,py,pz       components of downward P (maximum dilatation) axis versor
%% c                    in the Aki-Richards Cartesian coordinate system (OUTPUT)
%% c     tx,ty,tz       components of downward T (maximum tension) axis versor
%% c                    in the Aki-Richards Cartesian coordinate system (OUTPUT)
%% c     bx,by,bz       components of downward B (neutral) axis versor in the
%% c                    Aki-Richards Cartesian coordinate system (OUTPUT)
%% c     ierr           error indicator (OUTPUT)
%% c
%% c     errors:
%% c     1              input vectors not perpendicular among each other

%% c
%% c      implicit none
%% c-------------------------------------------------------------------------------
%%       integer io
%%       real amistr,amastr,amidip,amadip,amirak,amarak,amitre,amatre
%%      1,amiplu,amaplu,orttol,ovrtol,tentol,dtor,c360,c90,c0,c1,c2,c3
%%       common /fpscom/amistr,amastr,amidip,amadip,amirak,amarak,amitre
%%      1,amatre,amiplu,amaplu,orttol,ovrtol,tentol,dtor,c360,c90,c0,c1,c2
%%      2,c3,io
%% c-------------------------------------------------------------------------------
%%       real wanx,wany,wanz,amn,anx,any,anz,wdx,wdy,wdz,amd,dx,dy,dz
%%      1,ang,px,py,pz,tx,ty,tz,bx,by,bz,amp
%%       integer ierr
%% c

%%       call fpsset
         amistr=-360.;
         amastr=360.;
         amidip=0.;
         amadip=90.;
         amirak=-360.;
         amarak=360.;
         amitre=-360.;
         amatre=360.;
         amiplu=0.;
         amaplu=90.;
         orttol=2.;
         ovrtol=0.001;
         tentol=0.0001;
         dtor=0.017453292519943296;
         c360=360.;
         c90=90.;
         c0=0.;
         c1=1.;
         c2=2.;
         c3=3.;
         io=6;

      ierr=0;
      [amn,anx,any,anz] = focal_norm(wanx,wany,wanz);
      [amd,dx,dy,dz] = focal_norm(wdx,wdy,wdz);
      [ang] = focal_angle(anx,any,anz,dx,dy,dz);
      if (abs(ang-c90) > orttol)
        disp(['ND2PT: input vectors not perpendicular, angle=' num2str(ang)]);
        ierr=1;
      end
      px=anx-dx;
      py=any-dy;
      pz=anz-dz;
      [amp,px,py,pz] = focal_norm(px,py,pz);
      if (pz < c0)
        [px,py,pz] = focal_invert(px,py,pz);
      end
      tx=anx+dx;
      ty=any+dy;
      tz=anz+dz;
      [amp,tx,ty,tz] = focal_norm(tx,ty,tz);
      if (tz < c0)
        [tx,ty,tz] = focal_invert(tx,ty,tz);
      end
      [bx,by,bz] = focal_vecpro(px,py,pz,tx,ty,tz);
      if(bz < c0)
        [bx,by,bz] = focal_invert(bx,by,bz);
      end


function [f] = elec_sphere_fit_optim(r, X, Y, Z, xo, yo, zo)

% elec_sphere_fit_optim - Optimization for elec_sphere_fit.m
%
% Called from elec_sphere_fit.m
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:55 $

% Licence:  GNU GPL, no implied or express warranties
% History:  02/2002, Darren.Weber_at_radiology.ucsf.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% with center (Xo,Yo,Zo) and radius r, the equation of a sphere is:
%
% r^2 = (x-xo)^2  +  (y-yo)^2  +  (z-zo)^2
%
% This function below creates a scalar value to
% return to the fminsearch function in elec_sphere_fit.

S = (X-xo).^2  +  (Y-yo).^2  +  (Z-zo).^2  -  r^2;

f = sum( S.^2 );


function semilogx(varargin)
%SEMILOGX     Semilogarithmic plot X vs. Y for real interval vectors X or Y
%
%Call
%
%  semilogx(Y)   or   semilogx(X,Y)   or   semilogx(X,Y,c)
%
%The x-axis is in logarithmic scale.
%
%For Y being a real interval vector, the curves (x,Y.inf) and (x,Y.sup) are plotted.
%For X being a real interval vector, the boxes X(i) x Y(i) are plotted.
%
%The area between curves or boxes is filled with color c; default is 'r' for red.
%

% written  11/15/07     S.M. Rump  (inspired by R. Malti)
%

  plot(varargin{:})
  set(gca,'xscale', 'log')


function frequency = convertPhaseToFrequency(phase,fs,sps)
factor =  (fs/sps)/(2*pi);
frequency = filter(ones(200,1)/200, 1, ... % Moving average
    diff(unwrap(phase(:)))*factor);
end


function rho45=rho45(h)
sldata;
limitpoints;
slopes;
rho2=rho1*(T2/T1)^-(1+(g/(m12*R)));
rho3=rho2*exp(-g*(h3-h2)/(R*T2));
rho4=rho3*(T4/T3)^-(1+(g/(m34*R)));
rho45=rho4*(T45(h)/T4)^-(1+(g/(m45*R)));


function [NABF]=analysis_nabf(f,I1,I2)
% function [QABF,LABF,NABF,NABF1]=objective_fusion_perform_fn(f,I1,I2)
%
%%% objective_fusion_perform_fn: Computes the Objective Fusion Performance Parameters proposed by Petrovic
%%% and modified Fusion Artifacts (NABF) measure proposed by B. K. Shreyamsha Kumar
%%%
%%% Inputs:
%%% xrcw -> fused image
%%% x -> source images, x{1}, x{2}
%%%
%%% Outputs:
%%% QABF -> Total information transferred from source images to fused image measure proposed by Petrovic
%%% LABF -> Total loss of information measure proposed by Petrovic
%%% NABF1 -> Fusion Artifacts measure proposed by Petrovic
%%% NABF -> Modified Fusion Artifacts measure proposed by B. K. Shreyamsha Kumar
%%%
%%% Author : B. K. SHREYAMSHA KUMAR
%%% Created on 28-10-2011.
%%% Updated on 08-11-2011.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Petrovic Metrics %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% Parameters for Petrovic Metrics Computation.
Td=2;
wt_min=0.001;
P=1;
Lg=1.5;
Nrg=0.9999;
kg=19;
sigmag=0.5;
Nra=0.9995;
ka=22;
sigmaa=0.5;

xrcw = (f);
x1 = (I1);
x2 = (I2);

%%% Edge Strength & Orientation.
[gvA,ghA]=sobel_fn(x1);
gA=sqrt(ghA.^2+gvA.^2);

[gvB,ghB]=sobel_fn(x2);
gB=sqrt(ghB.^2+gvB.^2);

[gvF,ghF]=sobel_fn(xrcw);
gF=sqrt(ghF.^2+gvF.^2);

%%% Relative Edge Strength & Orientation.
[p,q]=size(xrcw);
for ii=1:p
   for jj=1:q
      if(gA(ii,jj)==0 | gF(ii,jj)==0)
         gAF(ii,jj)=0;
      elseif(gA(ii,jj)>gF(ii,jj))
         gAF(ii,jj)=gF(ii,jj)/gA(ii,jj);
      else
         gAF(ii,jj)=gA(ii,jj)/gF(ii,jj);
      end
      if(gB(ii,jj)==0 | gF(ii,jj)==0)
         gBF(ii,jj)=0;
      elseif(gB(ii,jj)>gF(ii,jj))
         gBF(ii,jj)=gF(ii,jj)/gB(ii,jj);
      else
         gBF(ii,jj)=gB(ii,jj)/gF(ii,jj);
      end
      if(gvA(ii,jj)==0 & ghA(ii,jj)==0)
         aA(ii,jj)=0;
      else
         aA(ii,jj)=atan(gvA(ii,jj)/ghA(ii,jj));
      end
      if(gvB(ii,jj)==0 & ghB(ii,jj)==0)
         aB(ii,jj)=0;
      else
         aB(ii,jj)=atan(gvB(ii,jj)/ghB(ii,jj));
      end
      if(gvF(ii,jj)==0 & ghF(ii,jj)==0)
         aF(ii,jj)=0;
      else
         aF(ii,jj)=atan(gvF(ii,jj)/ghF(ii,jj));
      end
   end
end
aAF=abs(abs(aA-aF)-pi/2)*2/pi;
aBF=abs(abs(aB-aF)-pi/2)*2/pi;

%%% Edge Preservation Coefficient.
QgAF=Nrg./(1+exp(-kg*(gAF-sigmag)));
QaAF=Nra./(1+exp(-ka*(aAF-sigmaa)));
QAF=sqrt(QgAF.*QaAF);
QgBF=Nrg./(1+exp(-kg*(gBF-sigmag)));
QaBF=Nra./(1+exp(-ka*(aBF-sigmaa)));
QBF=sqrt(QgBF.*QaBF);

%%% Total Fusion Performance (QABF).
wtA=wt_min*ones(p,q);
wtB=wt_min*ones(p,q);
cA=ones(p,q); cB=ones(p,q);
for ii=1:p
   for jj=1:q
      if(gA(ii,jj)>=Td)
         wtA(ii,jj)=cA(ii,jj)*gA(ii,jj)^Lg;
      end
      if(gB(ii,jj)>=Td)
         wtB(ii,jj)=cB(ii,jj)*gB(ii,jj)^Lg;
      end
   end
end
wt_sum=sum(sum(wtA+wtB));
QAF_wtsum=sum(sum(QAF.*wtA))/wt_sum;  %% Information Contributions of A.
QBF_wtsum=sum(sum(QBF.*wtB))/wt_sum;  %% Information Contributions of B.
QABF=QAF_wtsum+QBF_wtsum;   %% QABF=sum(sum(QAF.*wtA+QBF.*wtB))/wt_sum -> Total Fusion Performance.

%%% Fusion Gain (QdeltaABF).
Qdelta=abs(QAF-QBF);
QCinfo=(QAF+QBF-Qdelta)/2;
QdeltaAF=QAF-QCinfo;
QdeltaBF=QBF-QCinfo;
QdeltaAF_wtsum=sum(sum(QdeltaAF.*wtA))/wt_sum;
QdeltaBF_wtsum=sum(sum(QdeltaBF.*wtB))/wt_sum;
QdeltaABF=QdeltaAF_wtsum+QdeltaBF_wtsum;   %% Total Fusion Gain.
QCinfo_wtsum=sum(sum(QCinfo.*(wtA+wtB)))/wt_sum;
QABF11=QdeltaABF+QCinfo_wtsum;              %% Total Fusion Performance.

%%% Fusion Loss (LABF).
rr=zeros(p,q);
for ii=1:p
   for jj=1:q
      if(gF(ii,jj)<=gA(ii,jj) | gF(ii,jj)<=gB(ii,jj))
         rr(ii,jj)=1;
      else
         rr(ii,jj)=0;
      end
   end
end
LABF=sum(sum(rr.*((1-QAF).*wtA+(1-QBF).*wtB)))/wt_sum;

%%% Fusion Artifacts (NABF) by Petrovic.
for ii=1:p
   for jj=1:q
      if(gF(ii,jj)>gA(ii,jj) & gF(ii,jj)>gB(ii,jj))
         na1(ii,jj)=2-QAF(ii,jj)-QBF(ii,jj);
      else
         na1(ii,jj)=0;
      end
   end
end
NABF1=sum(sum(na1.*(wtA+wtB)))/wt_sum;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Fusion Artifacts (NABF) changed by B. K. Shreyamsha Kumar .
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for ii=1:p
   for jj=1:q
      if(gF(ii,jj)>gA(ii,jj) & gF(ii,jj)>gB(ii,jj))
         na(ii,jj)=1;
      else
         na(ii,jj)=0;
      end
   end
end
NABF=sum(sum(na.*((1-QAF).*wtA+(1-QBF).*wtB)))/wt_sum;

QABF+LABF+NABF1;
QABF+LABF+NABF;


function LBP_Im = LBP(Input_Im, R)

% %=======================================================================
% %=======================================================================
% This function computes the LBP transformation of the input image
% Input_Im.
% The parameters of the LBP operator are (P = 8, R),
% where P - the number of sampling points in the region with the radius R.
% Radius R is the input parameter of the function.
% Possible values for R = 1, 2, 3, etc.
% If input image is COLOR, then the grayscale transformation is performed.
% %=======================================================================
% %=======================================================================

if size(Input_Im, 3) == 3
    Input_Im = rgb2gray(Input_Im);
end;
L = 2*R + 1; %% The size of the LBP label
C = round(L/2);
Input_Im = uint8(Input_Im);
row_max = size(Input_Im,1)-L+1;
col_max = size(Input_Im,2)-L+1;
LBP_Im = zeros(row_max, col_max);
for i = 1:row_max
    for j = 1:col_max
        A = Input_Im(i:i+L-1, j:j+L-1);
        A = A+1-A(C,C);
        A(A>0) = 1;
        LBP_Im(i,j) = A(C,L) + A(L,L)*2 + A(L,C)*4 + A(L,1)*8 + A(C,1)*16 + A(1,1)*32 + A(1,C)*64 + A(1,L)*128;
    end;
end;


function d=meansqtf(b,a)
%AVEPSPEC calculates the mean square transfer function for a filter D=(B,A)
%
% Inputs: B,A         Numerator and denominator filter coefficients.
%
% Output: D           The mean square transfer function of the filter B/A. This equals
%                     the average otuput power when the filter is fed with unit variance
%                     white noise.
%
%                     D may be obtained approximately by:
%                         N=1024; D=sum(filter(B,A,[1 zeros(1,N)]).^2)

% Since the power spectrum is the fourier transform of the autocorrelation, we can calculate
% the average value of pb/pa by taking the 0'th order term of the convolution of the autocorrelation
% functions associated with b and 1/a. Since b is an FIR filter, this convolution is
% a finite sum even though the autocorrelation function of 1/a is infinite in extent.

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: meansqtf.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if length(a)==1
   d=(b(:)')*b(:);
else
   m=lpcar2ra(b(:)');
   m(1)=m(1)*0.5;
   d=2*lpcar2rr(a(:)',length(m)-1)*m';
end


function [X, R, S] = snapshot_gen_sto(design, doas, wavelength, t, ncov, scov)
%SNAPSHOT_GEN_STO Generates snapshots for the stochastic model.
%Syntax:
%   X = STO_SNAPSHOT_GEN(design, doas, wavelength[, t, ncov, scov]);
%   [X, R] = STO_SNAPSHOT_GEN(design, doas, wavelength[, t, ncov, scov]);
%   [X, R, S] = STO_SNAPSHOT_GEN(design, doas, wavelength[, t, ncov, scov]);
%Inputs:
%   design - Array design.
%   doas - DOA vector. For 2D DOAs, each column represents a DOA pair.
%   wavelength - Wavelength.
%   t - Number of snapshots.
%   ncov - Covariance matrix of the additive complex circular-symmetric
%          Gaussian noise. Can be a scalar, vector (for uncorrelated noise
%          with different powers), or a matrix.
%   scov - Covariance matrix of the source signals. Can be a scalar, vector
%          (for uncorrelated sources with different powers), or a matrix.
%Outputs:
%   X - Snapshots, where each columns is a single snapshot.
%   R - Sample covariance matrix (averaged by the number of snapshots).
%   S - A source_count x snapshot_count matrix consists of source signal
%       vectors.
if nargin <= 5
    scov = 1;
end
if nargin <= 4
    ncov = 1;
end
if nargin <= 3
    t = 1;
end
A = steering_matrix(design, wavelength, doas);
[m, k] = size(A);
S_internal = gen_ccsg(k, t, scov);
X = A * S_internal + gen_ccsg(m, t, ncov);
if nargout >= 2
    R = (X*X')/t;
    if nargout == 3
        S = S_internal;
    end
end
end

function X = gen_ccsg(m, n, cov)
X0 = randn(m, n) + 1j * randn(m, n);
if isscalar(cov)
    X = sqrt(cov/2) * X0;
elseif isvector(cov)
    X = bsxfun(@times, X0, cov(:));
else
    C = sqrtm(cov/2);
    X = C*X0;
end
end


function D = sampling_dep(f, depidx, domain, gridsize)
%SAMPLING_DEP Sampling a (multivariate) function
%	D = SAMPLING_DEP(f, depidx, domain, gridsize)
%
%	f        - function of a real^P vector (f depends on a subset of its input)
%	depidx   - index of the dependent elements
%	domain   - [min1 max1;... minP maxP] intervals for each element
%	gridsize - number of sampling grid points for each dependent element
%
%	D        - P-dimensional array of the sampled data
%
%	This function is used internally by the tptoool toolbox
%
%	eg.:   sampling_dep(@(x) x(2)+x(3), [2 3] [-1 1; 0 3], [7 5])
%
%	See also SAMPLING_LPV

% internal function (used by sampling_lpv)

%DIMENSIONS
P = length(gridsize);
p = zeros(max(depidx), 1);

%SAMPLING
if P > 0
	% allocation of sample array
	siz = prod(gridsize);
	D = zeros(siz,1);
	a = domain(:,1);
	b = domain(:,2);
	step = (b - a) ./ (gridsize - 1);

	% sampling
	z = ones(P,1);
	z(1) = 0;
	for k = 1:siz
		% next grid point index
		z = nexti(gridsize, z);
		% argument
		p(depidx) = a + step .* (z - 1);
		% sampling
		D(k) = f(p);
	end

	if P > 1
		% reshape to proper size
		D = reshape(D, gridsize');
	end
else
	D = f(p);
end

%SUBFUNCTION: Next grid point index
function n = nexti(gridsize, n)
i = 1;
n(i) = n(i) + 1;
while n(i) > gridsize(i)
	n(i) = 1;
	i = i + 1;
	n(i) = n(i) + 1;
end


function [GAmp,GTime]=GySpiral(p)

global VCtl;
global VObj;
global VVar;

tStart=p.tStart;
dt = p.dt;

% 2D spiral encoding
FOV = VCtl.FOVFreq; % choose FOVFreq as FOV
Res = VCtl.ResFreq; % choose ResFreq as effective resolution

KMax  = Res/ (2*FOV);
ThetaMax = KMax / VCtl.S_Lamda;
Beta = (VObj.Gyro/(2*pi))*(VCtl.S_SlewRate / VCtl.S_Lamda);
a2 = (9*Beta/4)^(1/3);
CLamda = VCtl.S_SlewRate/VCtl.S_SlewRate0;
Ts = ((3*VObj.Gyro*VCtl.S_Gradient)/(4*pi*VCtl.S_Lamda*a2^2))^3;

ThetaTs = (0.5 * Beta * Ts^2)/(CLamda + (Beta/(2 * a2)) * Ts^(4/3));
if ThetaTs < ThetaMax
    t1 = 0:dt:Ts;
    Theta1 = (0.5 * Beta * t1.^2)./(CLamda + (Beta/(2 * a2)) * t1.^(4/3));

    Tacq = t1(end) + ((pi*VCtl.S_Lamda)/(VObj.Gyro * VCtl.S_Gradient))*(ThetaMax^2 - Theta1(end)^2);
    t2 = t1(end):dt:Tacq;
    Theta2 = sqrt(Theta1(end)^2 + (VObj.Gyro / (pi * VCtl.S_Lamda))*VCtl.S_Gradient*(t2 - t1(end)));

    Theta = [Theta1(1:end-1) Theta2];
    t     = [t1(1:end-1)     t2];

else
    Tacq = ((2*pi*FOV)/(3*VCtl.S_ShotNum))*sqrt(pi/(VObj.Gyro*VCtl.S_SlewRate*VCtl.RFreq^3));
    t1 = 0:dt:Tacq;
    Theta1 = (0.5 * Beta * t1.^2)./(CLamda + (Beta/(2 * a2)) * t1.^(4/3));

    Theta = Theta1;
    t     = t1;
end

DTheta = [0 diff(Theta)./diff(t)];

GAmp = ((2*pi)/(VObj.Gyro))*VCtl.S_Lamda*DTheta.*(sin(Theta + (VVar.PhaseCount-1)*(2*pi/VCtl.S_ShotNum)) ...
                                         + Theta.*cos(Theta + (VVar.PhaseCount-1)*(2*pi/VCtl.S_ShotNum)));
GTime = tStart + VCtl.TEAnchorTime + t;

GTime = [GTime GTime(end) + max(VCtl.MinUpdRate,abs(GAmp(end)/VCtl.S_SlewRate))];
GAmp = [GAmp 0];

[GTime,m,n]=unique(GTime);
GAmp=GAmp(m);

end


function X = Adj_DetailCurveCoeff(C,is_real);
% Adj_DetailCurveCoeff: Adjoint of DetailCurveCoeff
%  Usage:
%    X = Adj_DetailCurveCoeff(C);
%  Inputs:
%    C    matrix of curvelet coefficients at scale 2^j
%  Outputs:
%    X    matrix of Fourier samples; jth dyadic subband
%  See Also
%   DetailCurveCoeff, Adj_SeparateAngles, Adj_Curvelet02Xform
%
% By Emmanuel Candes, 2003-2004

  C = ClockwisetoWENS(C);
  nn = size(C);
  R = zeros(nn);
  deep = log2(nn(2));

  for j = 1:size(R,1),
    for m = 1:size(R,2),
      W = squeeze(C(j,m,:,:));
      W = fft2_mid0(W)/sqrt(prod(size(W)));
      R(j,m,:,:) = W;
    end
  end

  for w=1:size(R,2)
    tmp = squeeze(R(2,w,:,:));
    R(2,w,:,:) = tmp([2:end,1], [2:end,1]);
  end
  for w=1:size(R,2)
    tmp = squeeze(R(4,w,:,:));
    R(4,w,:,:) = tmp([2:end,1], [2:end,1]);
  end

  X = Adj_SeparateAngles(Adj_SqueezeAngularFT(R),deep,is_real);


function R = AngleAxis2RotationMatrix(angle_axis)

theta2 = dot(angle_axis,angle_axis);
if (theta2 > 0.0)
    % We want to be careful to only evaluate the square root if the
    % norm of the angle_axis vector is greater than zero. Otherwise
    % we get a division by zero.

    theta = sqrt(theta2);
    wx = angle_axis(1) / theta;
    wy = angle_axis(2) / theta;
    wz = angle_axis(3) / theta;

    costheta = cos(theta);
    sintheta = sin(theta);

    R(1+0, 1+0) =     costheta   + wx*wx*(1 -    costheta);
    R(1+1, 1+0) =  wz*sintheta   + wx*wy*(1 -    costheta);
    R(1+2, 1+0) = -wy*sintheta   + wx*wz*(1 -    costheta);
    R(1+0, 1+1) =  wx*wy*(1 - costheta)     - wz*sintheta;
    R(1+1, 1+1) =     costheta   + wy*wy*(1 -    costheta);
    R(1+2, 1+1) =  wx*sintheta   + wy*wz*(1 -    costheta);
    R(1+0, 1+2) =  wy*sintheta   + wx*wz*(1 -    costheta);
    R(1+1, 1+2) = -wx*sintheta   + wy*wz*(1 -    costheta);
    R(1+2, 1+2) =     costheta   + wz*wz*(1 -    costheta);
else
    % At zero, we switch to using the first order Taylor expansion.
    R(1+0, 1+0) =  1;
    R(1+1, 1+0) = -angle_axis(3);
    R(1+2, 1+0) =  angle_axis(2);
    R(1+0, 1+1) =  angle_axis(3);
    R(1+1, 1+1) =  1;
    R(1+2, 1+1) = -angle_axis(1);
    R(1+0, 1+2) = -angle_axis(2);
    R(1+1, 1+2) =  angle_axis(1);
    R(1+2, 1+2) = 1;
end


function [otf] = psf2otf_Dy(outSize)

psf = single(zeros(outSize));
psf(1, 1) = -1;
psf(end, 1) = 1;
otf = fft2(psf);


function  [bdLat,bdLng]= gcj2bd(gcjLat, gcjLng)

    bdLat = gcjLat;
    bdLng = gcjLng;

    x_pi = pi * 3000.0 / 180.0;
    inChina = ~outOfChina(gcjLat, gcjLng);
    if ~any(inChina),return;end

    x = gcjLng(inChina);
    y = gcjLat(inChina);
    z = hypot(x, y) + 0.00002 * sin(y * x_pi);
    theta = atan2(y, x) + 0.000003 * cos(x * x_pi);
    bdLng(inChina) = z.* cos(theta) + 0.0065;
    bdLat(inChina) = z.* sin(theta) + 0.006;

end


function basis_11_t6_test ( )

%*****************************************************************************80
%
%% BASIS_11_T6_TEST verifies BASIS_11_T6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  node_num = 6;

  t = [ ...
    2.0, 0.0; ...
    4.0, 3.0; ...
    0.0, 4.0; ...
    3.0, 1.5; ...
    2.0, 3.5; ...
    1.0, 2.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BASIS_11_T6_TEST:\n' );
  fprintf ( 1, '  Verify basis functions for element T6.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', node_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Physical Nodes:\n' );
  fprintf ( 1, '\n' );
  for j = 1 : node_num
    fprintf ( 1, '  %8d  %7f  %7f\n', j, t(1:2,j) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The basis function values at basis nodes\n' );
  fprintf ( 1, '  should form the identity matrix.\n' );
  fprintf ( 1, '\n' );

  for i = 1 : node_num
    for j = 1 : node_num
      [ phi(i,j), dphidx(i,j), dphidy(i,j) ] = basis_11_t6 ( t, i, t(1:2,j) );
    end
  end

  for i = 1 : node_num
    for ( j = 1 : node_num )
      fprintf ( 1, '  %7f', phi(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The X and Y derivatives should sum to 0.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      dPhidX sum    dPhidY sum\n' );
  fprintf ( 1, '\n' );
  for j = 1 : node_num
    sum_x = sum ( dphidx(1:node_num,j) );
    sum_y = sum ( dphidy(1:node_num,j) );
    fprintf ( 1, '  %14f  %14f\n', sum_x, sum_y );
  end

  return
end


function nz_num = wathen_st_size ( nx, ny )

%*****************************************************************************80
%
%% WATHEN_ST_SIZE: Size of Wathen matrix stored in sparse triplet format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 June 2014
%
%  Author:
%
%    John Burkardt.
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
%    Andrew Wathen,
%    Realistic eigenvalue bounds for the Galerkin mass matrix,
%    IMA Journal of Numerical Analysis,
%    Volume 7, Number 4, October 1987, pages 449-457.
%
%  Parameters:
%
%    Input, integer NX, NY, values which determine the size of the matrix.
%
%    Output, integer NZ_NUM, the number of items of data used to describe
%    the matrix.
%
  nz_num = nx * ny * 64;

  return
end


function stroud_test085 ( )

%*****************************************************************************80
%
%% TEST085 tests CIRCLE_ANNULUS_AREA_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  ntest = 3;

  radius1_test = [ 0.0, 1.0, 1.0 ];
  radius2_test = [ 1.0, 2.0, 3.0 ];
  xc_test = [ 0.0, 1.0, 3.0 ];
  yc_test = [ 0.0, 0.0, 4.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST085\n' );
  fprintf ( 1, '  CIRCLE_ANNULUS_AREA_2D computes the area of a \n' );
  fprintf ( 1, '    circular annulus.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     XC        YC          Radius1    Radius2    Area\n' );
  fprintf ( 1, '\n' );

  for i = 1 : ntest

    xc = xc_test(i);
    yc = yc_test(i);

    radius1 = radius1_test(i);
    radius2 = radius2_test(i);

    area = circle_annulus_area_2d ( radius1, radius2 );

    fprintf ( 1, '  %9f  %9f  %9f  %9f  %9f\n', xc, yc, radius1, radius2, area );

  end

  return
end


function log_L = Kalman_Estimation(y, psi, matur, dt, a0, P0, N, nobs, locked_parameters)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Extracting initial parameter values from initial psi
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k = psi(1,1);
sigmax = psi(2,1);
lambdax = psi(3,1);
mu = psi(4,1);
sigmae = psi(5,1);
rnmu = psi(6,1);
pxe = psi(7,1);

if sum(locked_parameters) == 0
    k = psi(1,1);
    sigmax = psi(2,1);
    lambdax = psi(3,1);
    mu = psi(4,1);
    sigmae = psi(5,1);
    rnmu = psi(6,1);
    pxe = psi(7,1);

    s = zeros(1, size(psi,1)-7);
    for i = 1:size(s,2)
        s(1, i) = psi(i+7,1);
    end
end

if sum(locked_parameters) ~= 0
    s = zeros(1, size(psi,1)-7+size(locked_parameters,1));
    j = 1;
    for i = 1:size(s,2)
        if all(abs(i-(locked_parameters))) == 1
             s(1, i) = psi(7+j,1);
             j = j+1;
        end
    end
end

% m = Number of state variables (number of rows in a0)
m = size(a0,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% THE TRANSITION EQUATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% S&S NOTATION: x(t)=c+G*x(t-1)+w(t)        w~N(0,W)    Equation (14)
% NEW NOTATION: a(t)=c+T*a(t-1)+R(t)*n(t)   n~N(0,Q)

% c is a {m x 1} Vector
% T is a {m x m} Matrix
c=[0;mu*dt];
T=[exp(-k*dt),0;0,1];

% Defining Q = var[n(t)] and R
xx=(1-exp(-2*k*dt))*(sigmax)^2/(2*k);
xy=(1-exp(-k*dt))*pxe*sigmax*sigmae/k;
yx=(1-exp(-k*dt))*pxe*sigmax*sigmae/k;
yy=(sigmae)^2*dt;
Q=[xx,xy;yx,yy];
R=eye(size(Q,1));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% THE MEASUREMENT EQUATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% S&S NOTATION: y(t)=d(t)+F(t)'x(t)+v(t)    v~N(0,V) Equation (15)
% NEW NOTATION: y(t)=d(t)+Z(t)a(t)+e(t)     e~N(0,H)

% d is a {N x 1} Vector
% Z is a {N x m} Matrix
    for i=1:N
        p1=(1-exp(-2*k*matur(i)))*(sigmax)^2/(2*k);
        p2=(sigmae)^2*matur(i);
        p3=2*(1-exp(-k*matur(i)))*pxe*sigmax*sigmae/k;
        d(i,1)=rnmu*matur(i)-(1-exp(-k*matur(i)))*lambdax/k+.5*(p1+p2+p3);
        Z(i,1)=exp(-k*matur(i));
        Z(i,2)=1;
    end

% Measurment errors Var-Cov Matrix: Cov[e(t)]=H
H=diag(s);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RUNNING THE KALMAN FILTER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Creating placeholder vectors/matrices for variables to be stored in
global save_vt save_att save_dFtt_1 save_vFv save_vtt save_Ptt_1 save_Ftt_1 save_Ptt

save_ytt_1 = zeros(nobs,N);
save_vtt = zeros(nobs,N);
save_vt    = zeros(nobs,N);
save_att_1 = zeros(nobs,m);
save_att   = zeros(nobs,m);
save_Ptt_1 = zeros(nobs,m*m);
save_Ptt   = zeros(nobs,m*m);
save_Ftt_1 = zeros(nobs,N*N);
save_dFtt_1 = zeros(nobs,1);
save_vFv    = zeros(nobs,1);
%save_log_Lt   = zeros(nobs,1);

Ptt = P0;
att = a0;

% Running the kalman filter for t = 1,...,nobs
    for t = 1:nobs
        Ptt_1   = T*Ptt*T'+R*Q*R';
        Ftt_1   = Z*Ptt_1*Z'+H;
        dFtt_1  = det(Ftt_1);

        %Ptt_1_test = [Ptt_1(1,1) 0; 0 Ptt_1(2,2)];
        %Ftt_1_test   = Z*Ptt_1_test*Z'+H;
        %dFtt_1_test  = det(Ftt_1_test);

        att_1   = T*att + c;
        yt      = y(t,:)';
        ytt_1   = Z*att_1+d;
        vt      = yt-ytt_1;

        att = att_1 + Ptt_1*Z'*inv(Ftt_1)*(vt);
        Ptt = Ptt_1 - Ptt_1*Z'*inv(Ftt_1)*Z*Ptt_1;

        ytt = Z*att+d;
        vtt  = yt-ytt;

        % save_ytt_1(t,:) = ytt_1';
        save_vtt(t,:) = vtt';
        save_vt(t,:)    = (vt)';
        % save_att_1(t,:) = att_1';
        save_att(t,:)   = att';
        save_Ptt_1(t,:) = [Ptt_1(1,1), Ptt_1(1,2), Ptt_1(2,1), Ptt_1(2,2)];
        save_Ptt(t,:)   = [Ptt(1,1), Ptt(1,2), Ptt(2,1), Ptt(2,2)];
        % save_Ftt_1(t,:) = [Ftt_1(1,1), Ftt_1(1,2), Ftt_1(1,3), Ftt_1(1,4), Ftt_1(1,5), Ftt_1(2,1), Ftt_1(2,2), Ftt_1(2,3), Ftt_1(2,4), Ftt_1(2,5), Ftt_1(3,1), Ftt_1(3,2), Ftt_1(3,3), Ftt_1(3,4), Ftt_1(3,5), Ftt_1(4,1), Ftt_1(4,2), Ftt_1(4,3), Ftt_1(4,4), Ftt_1(5,5), Ftt_1(5,1), Ftt_1(5,2), Ftt_1(5,3), Ftt_1(5,4), Ftt_1(5,5)];

        %save_dFtt_1(t,:)= dFtt_1_test;
        %save_vFv(t,:)   = vt'*inv(Ftt_1_test)*vt;
        save_dFtt_1(t,:)= dFtt_1;
        save_vFv(t,:)   = vt'*inv(Ftt_1)*vt;

    end

logL = -(N*nobs/2)*log(2*pi)-0.5*sum(log(save_dFtt_1))-0.5*sum(save_vFv);
% logL = -(N*nobs/2)*log(2*pi)-0.5*sum(save_vFv);
% logL = sum(diag(save_vt'*save_vt));
log_L = -logL;


function class = gmm_classification(xtest, mu_train, sigma_train, c_train)
    % Classify new points in the 'xtest' set using GMM matching
    % Each column in xtest represents a point to be classified.
    % Each column in xtrain contains a training sample.
    % ytrain - column vector.

%     global NUM_OF_GAUSSIANS;
%     global NUM_OF_ITERATIONS;
%     [mu_train, sigma_train, c_train] = ...
%         gmm_training(xtrain, ytrain, NUM_OF_GAUSSIANS, NUM_OF_ITERATIONS);

    NUM_OF_TEST_SAMPLES = size(xtest, 2);
    class = zeros(NUM_OF_TEST_SAMPLES, 1);
    for k = 1:NUM_OF_TEST_SAMPLES
        [class(k), score] = GMM.gmm_classify_sample(mu_train, sigma_train, ...
            c_train, xtest(:, k));
    end;
end


function [out] = soilmoisture_1(S1,S1max,S2,S2max)
%soilmoisture_1

% 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:  Water rebalance to equal relative storage (2 stores)
% Constraints:  -
% @(Inputs):    S1    - current storage in S1 [mm]
%               S1max - maximum storage in S1 [mm]
%               S2    - current storage in S2 [mm]
%               S2max - maximum storage in S2 [mm]

out = ((S2.*S1max-S1.*S2max)/(S1max+S2max)).*smoothThreshold_storage_logistic(S1./S1max,S2./S2max);

end


function J = J_pnp_loss(image_pt, world_pt, K, R, t)
numPoints = size(image_pt, 1);
J = 0;
for i = 1 : numPoints
    world_point = [world_pt(i, :), 1]; % homogeneous coordinates
    image_point = image_pt(i, :);

    cameraMatrix = [R; t.'] * K;
    projectedPoint = world_point * cameraMatrix;

    if(~isnumeric(R))
        image_point = image_point .* projectedPoint(3);
        projectedPoint = projectedPoint(1 : 2);
    else
        projectedPoint = projectedPoint(1 : 2) / projectedPoint(3);
    end
    d = image_point - projectedPoint;
    J = J + d * d';
end
J = J ./ numPoints;
end


function out = ST_FitPolynomial(y,k)
% ST_FitPolynomial   Goodness of a polynomial fit to a time series
%
% Usually kind of a stupid thing to do with a time series, but it's sometimes
% somehow informative for time series with large trends.
%
%---INPUTS:
% y, the input time series.
% k, the order of the polynomial to fit to y.
%
%---OUTPUT:
% RMS error of the fit.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

doPlot = 0; % Plot stuff to screen

if nargin < 2 || isempty(k)
    k = 1; % Linear by default
end

N = length(y); % Length of the time series (number of samples)
t = (1:N)'; % Get a range for the time axis for time series y

% ------------------------------------------------------------------------------
% Fit a polynomial to the time series
% ------------------------------------------------------------------------------
% Supress the (valid!) warning from stupidly fitting a polynomial to a time series...
warning('off','MATLAB:polyfit:RepeatedPointsOrRescale');
cf = polyfit(t,y,k);
warning('on','MATLAB:polyfit:RepeatedPointsOrRescale');

f = polyval(cf,t);
out = mean((y-f).^2); % mean RMS ERROR OF FIT

% ------------------------------------------------------------------------------
% Plot
% ------------------------------------------------------------------------------
if doPlot
    n = 10;
    errs = zeros(n,1);
    x = 1:length(y);
    for i = 1:n
        cf = polyfit(x,y',i);
        f = polyval(cf,x);
        errs(i) = sum((y'-f).^2);
    end
    f = figure('color','w'); hold on;
    plot(f,'k')
    plot(errs);
end

end


function c=comp_atrousfilterbank_td(f,g,a,offset)
%COMP_ATROUSFILTERBANK_TD   Uniform filterbank by conv2
%   Usage:  c=comp_atrousfilterbank_fft(f,g,a,skip);
%
%   Input parameters:
%         f   : Input data - L*W array.
%         g   : Filterbank filters - filtLen*M array.
%         a   : Filter upsampling factor - scalar.
%         offset: Delay of the filters - scalar or array of length M.
%
%   Output parameters:
%         c  : L*M*W array of coefficients
%

%input data length
L=size(f,1);
%input channel number
W=size(f,2);
%filter number
M=size(g,2);
g = comp_ups(g,a,1);

%length of filters
filtLen = size(g,1);
skip = -offset;
% Allow filter delay only in the filter support range
if(all(skip>=filtLen) || all(skip<0))
  error('%s: The filter zero index position outside of the filter support.', upper(mfilename));
end

if(numel(skip)==1)
    skip = skip*ones(M,1);
end

% Output memory allocation
c=zeros(L,M,W,assert_classname(f,g));

% Explicitly extend the input. length(fext) = length(f) + 2*(filtLen-1)
fext = comp_extBoundary(f,filtLen-1,'per','dim',1);
% CONV2 does 2-D linear convolution. 'valid' option crops tails
% length(fextconv2) = length(f) + (filtLen-1)
% length(c(:,m,:)) = N
% W channels done simultaneously by conv2
for m=1:M
  c(:,m,:) = comp_downs(conv2(fext,g(:,m),'valid'),1,skip(m),L);
end;


 function MyPerecptronExample
% Rosenblatt's Perecptron
% References :
%      Neural Networks for Pattern Recognition By C. Bishop
%      An Introduction to Support Vector Machines By Cristianini
%      Introduction to Machine Learning By Alpayd?n
%      Neural Networks. A Comprehensive Foundation By Haykin
% Code by By Ibraheem Al-Dhamari 2010

clc
%============================================
% Generate 2 dimensions linear separable data
%===========================================

% You may change the size of the data from here or input your own data
 %   note that the drawing is for two dimensions only, hence you need to
 %   modify the code for different data.
mydata = rand(500,2);
% Separate the data into two classes
acceptindex = abs(mydata(:,1)-mydata(:,2))>0.012;
acceptindex = abs(mydata(:,1)-mydata(:,2))>0.012;
mydata = mydata(acceptindex,:); % data
myclasses = mydata(:,1)>mydata(:,2); % labels
[m n]=size(mydata);
%training data
 x=mydata(1:400,:);   y=myclasses(1:400);
% test data
xt=mydata(401:m,:); yt=myclasses(401:m);
%=====================================
% Train the perceptron
%=====================================
[w,b,pass] = PerecptronTrn(x,y);
Iterations=pass

%=====================================
% Test
%=====================================
e=PerecptronTst(xt,yt,w,b);
disp(['Test_Errors=' num2str(e) '     Test Data Size= ' num2str(m-400)])

%=====================================
% Draw the result (sparating hyperplane)
%=====================================
 l=y==0;
 hold on
 plot(x(l,1),x(l,2),'k.' );
 plot(x(~l,1),x(~l,2),'g.');
 [l,p]=size(x);
 plot([0,1],[0,1],'r-')
 axis([0 1 0 1]), axis square, grid on
 drawnow


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Test different Hawkes process simulation methods.
% Given synthetic data generated via different simulation methods, we
% estimate the parameters via MLE, and plot the relative estimation errors
% w.r.t. the number of training sequences.
%
% Provider:
% Hongteng Xu @ Georgia Tech
% June 14, 2017
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear

options.N = 50; % the number of sequences
options.Nmax = 100; % the maximum number of events per sequence
options.Tmax = 100; % the maximum size of time window
options.tstep = 0.2;% the step length for computing sup intensity
options.M = 50; % the number of steps
options.GenerationNum = 5; % the number of generations
D = 4; % the dimension of Hawkes processes
nTest = 5;
nSeg = 5;
nNum = options.N/nSeg;

disp('Fast simulation of Hawkes processes with exponential kernel')
para1.mu = rand(D,1)/D;
para1.A = rand(D, D);
para1.A = 0.25 * para1.A./max(abs(eig(para1.A)));
para1.A = reshape(para1.A, [D, 1, D]);
para1.w = 1;
Seqs1 = SimulationFast_Thinning_ExpHP(para1, options);

disp('Thinning-based simulation of Hawkes processes with exponential kernel')
para2 = para1;
para2.kernel = 'exp';
para2.landmark = 0;
Seqs2 = Simulation_Thinning_HP(para2, options);

disp('Approximate simulation of Hawkes processes via branching process')
para3 = para2;
Seqs3 = Simulation_Branch_HP(para3, options);

disp('Learning Hawkes processes from synthetic data')
alg.LowRank = 0;
alg.Sparse = 0;
alg.GroupSparse = 0;
alg.outer = 4;
alg.rho = 0.1;
alg.inner = 4;
alg.thres = 1e-5;
alg.Tmax = [];
alg.storeErr = 0;
alg.storeLL = 0;

disp('Evaluation of quality of synthetic data.')

Err = zeros(3,nSeg,nTest);
for n = 1:nTest
    for i = 1:nSeg
        % initialize
        model.A = rand(D,1,D)./(D^2);
        model.mu = rand(D,1)./D;
        model.kernel = 'exp';
        model.w = 1;
        model.landmark = 0;

        model1 = model;
        model2 = model;
        model3 = model;
        model1 = Learning_MLE_Basis( Seqs1(1:i*nNum), model1, alg );
        model2 = Learning_MLE_Basis( Seqs2(1:i*nNum), model2, alg );
        model3 = Learning_MLE_Basis( Seqs3(1:i*nNum), model3, alg );

        Err(1,i,n) = norm([model1.mu; model1.A(:)] - [para1.mu; para1.A(:)])/...
            norm([para1.mu; para1.A(:)]);
        Err(2,i,n) = norm([model2.mu; model2.A(:)] - [para2.mu; para2.A(:)])/...
            norm([para2.mu; para2.A(:)]);
        Err(3,i,n) = norm([model3.mu; model3.A(:)] - [para3.mu; para3.A(:)])/...
            norm([para3.mu; para3.A(:)]);
    end
end

Error = mean(Err,3);
Std = std(Err, 0, 3);
figure
hold on
for i = 1:3
    errorbar(nNum:nNum:options.N, Error(i,:), Std(i,:), 'o-');
end
hold off
axis tight
xlabel('The number of training sequences');
ylabel('Relative estimation error')
legend('FastThinning', 'Thinning', 'Branching')
title('Learning results based on different simulation methods')


% EX_STOKES_TWISTED_PIPE: solve the Stokes problem on a twisted pipe using a B-spline discretization.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_twisted_pipe.mat';

% Type of boundary conditions for each side of the domain
problem_data.nmnn_sides   = [1 2];
problem_data.drchlt_sides = [3 4 5 6];

% Physical parameters
problem_data.viscosity = @(x, y, z) ones (size (x));

% Force term
fx = @(x, y, z) ones(size(x));
fy = @(x, y, z) zeros(size(x));
fz = @(x, y, z) zeros(size(x));
problem_data.f  = @(x, y, z) cat(1, reshape (fx (x,y,z), [1, size(x)]), reshape (fy (x,y,z), [1, size(x)]), reshape (fz (x,y,z), [1, size(x)]));

% Boundary terms
problem_data.h  = @(x, y, z, iside) zeros ([3, size(x)]); %Dirichlet boundary condition
problem_data.g  = @(x, y, z, iside) zeros ([3, size(x)]); %Neumann boundary condition

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.element_name = 'TH';
method_data.degree       = [2 2 2]; % Degree of the splines (pressure space)
method_data.regularity   = [1 1 1]; % Regularity of the splines
method_data.nsub         = [2 2 2]; % Number of subdivisions
method_data.nquad        = [4 4 4]; % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER
[geometry, msh, space_v, vel, space_p, press] = solve_stokes (problem_data, method_data);

% 4) POST-PROCESSING
% 4.1) EXPORT TO PARAVIEW

output_file = 'TwistedPipe_BSP_Deg2_Reg1_Sub2';

fprintf ('The result is saved in the files %s \n and %s \n \n', ...
           [output_file '_vel'], [output_file '_press']);
vtk_pts = {linspace(0, 1, 10), linspace(0, 1, 10), linspace(0, 1, 10)};
sp_to_vtk (press, space_p, geometry, vtk_pts, [output_file '_press'], 'press')
sp_to_vtk (vel,   space_v, geometry, vtk_pts, [output_file '_vel'  ], {'velocity', 'divergence'}, {'value', 'divergence'})

%!test
%! problem_data.geo_name = 'geo_twisted_pipe.mat';
%! problem_data.nmnn_sides   = [1 2];
%! problem_data.drchlt_sides = [3 4 5 6];
%! problem_data.viscosity = @(x, y, z) ones (size (x));
%! fx = @(x, y, z) ones(size(x));
%! fy = @(x, y, z) zeros(size(x));
%! fz = @(x, y, z) zeros(size(x));
%! problem_data.f  = @(x, y, z) cat(1, reshape (fx (x,y,z), [1, size(x)]), reshape (fy (x,y,z), [1, size(x)]), reshape (fz (x,y,z), [1, size(x)]));
%! problem_data.h  = @(x, y, z, iside) zeros ([3, size(x)]); %Dirichlet boundary condition
%! problem_data.g  = @(x, y, z, iside) zeros ([3, size(x)]); %Neumann boundary condition
%! method_data.element_name = 'TH';
%! method_data.degree       = [2 2 2]; % Degree of the splines (pressure space)
%! method_data.regularity   = [1 1 1]; % Regularity of the splines
%! method_data.nsub         = [2 2 2]; % Number of subdivisions
%! method_data.nquad        = [4 4 4]; % Points for the Gaussian quadrature rule
%! [geometry, msh, space_v, vel, space_p, press] = solve_stokes (problem_data, method_data);
%! assert (msh.nel, 192)
%! assert (space_p.ndof, 576)
%! assert (space_v.ndof, 8400)