%% Configurable Simulink Model for DC-DC Converters
%

%% DC-DC Converters
% There are three kinds of switching mode DC-DC converters, buck, boost and
% buck-boost. The buck mode is used to reduce output voltage, whilst the
% boost mode can increase the output voltage. In the buck-boost mode, the
% output voltage can be maintained either higher or lower than the source
% but in the opposite polarity. The simplest forms of these converters are
% schematically represented in Figure 1.

%%
%
%
%
%
% Figure 1. Buck (left), Boost (middle) and Buck-Boost (right) converters.

%%
% These converters consist of the same components, an inductor, $L$, a
% capacitor, $C$ and a switch, which has two states  $u=1$ and $u=0$. All
% converters connect to a DC power source with a voltage (unregulated),
% $V_\mathrm{in}$ and provide a regulated voltage, $v_\mathrm{o}$ to the
% load resistor, $R$ by controlling the state of the switch. In some
% situations, the load also could be inductive, for example a DC motor, or
% approximately, a current load, for example in a cascade configuration.
% For simplicity, here, only current and resistive loads are to be
% considered.
%

%% Principles
% The working principles of the these DC-DC converters can be explained as
% follows. In the buck mode, when the switch is on position 1, the DC
% source supplies power to the circuit which results an output voltage
% across the resistor. When the switch changes its position to 0, the
% energy stored in the inductor and capacitor will discharge through the
% resistor. Appropriately controlling the switching position can maintain
% the output voltage at a desired level lower than the source.
%
% In the boost mode, when the switch is on position 1, the circuit is
% separated into two parts: on the left, the source is charging the
% inductor, meanwhile the capacitor on the right maintains the output
% voltage using previously stored energy. When the switch changes its
% position to 0, both the DC source and energy stored in the inductor will
% supply power to the circuit on the right, hence boost the output voltage.
% Again, the output voltage can be maintain at desired level by controlling
% the switching sequence.
%
% Finally, for the buck-boost mode, switch positions 1 and 0 represents
% charging and discharging modes of the inductor. Appropriately controlling
% the switching sequence can result in output voltage higher or lower than
% the DC source. Since the inductor cannot change the direction of current,
% the output voltage is opposite to the DC source.

%% Model under ideal assumptions
% Under ideal assumptions: ideal switch, ideal capacitor and ideal
% inductor, these converters can be described using ordinary
% differentiation equations as follows:
%
%% Buck converter:
% $C{dv_{c} \over dt} = i_{L} - v_{c}/R - i_{o}$
%
% $L{di_{L} \over dt} = uv_{in} - v_{c}$
%
% where, $i_\mathrm{o}$ is the load current.
%
%% Boost converter:
% $C{dv_\mathrm{c}\over dt}=(1-u)i_\mathrm{L}-v_\mathrm{c}/R-i_\mathrm{o}$
%
% $L{di_\mathrm{L}\over dt}=v_\mathrm{in} - (1-u) v_\mathrm{c}$
%
%% Buck-boost converter:
% $C{dv_\mathrm{c}\over dt}=(1-u)i_\mathrm{L}-v_\mathrm{c}/R-i_\mathrm{o}$
%
% $L{di_\mathrm{L}\over dt}=uv_\mathrm{in} - (1-u) v_\mathrm{c}$
%
% Introduce the following state, time and load normalization:
%
% $x_1={v_\mathrm{c}\over v_\mathrm{in}}$,
%
% $x_2={i_\mathrm{L}\over v_\mathrm{in}}\sqrt{{L}\over{C}}$,
%
% $\tau = {t \over \sqrt{LC}}$,
%
% $\gamma = {\sqrt{LC}\over R}$,
%
% $d = {i_\mathrm o \over v_\mathrm{in}}\sqrt{{L}\over {C}}$
%
% Then the normalized state equations of three converters are as follows:
%
%% Normalized buck model
% $\dot{x_1} = -\gamma x_1 + x_2 - d$
%
% $\dot{x_2}= -x_1 + u $
%
% where, with an abuse of notation, `.' represents the derivation with
% respect to the normalized time, $\tau$.
%
%% Normalized boost model
% $\dot{x_1} = -\gamma x_1 + (1-u)x_2 - d$
%
% $\dot{x_2} = -(1-u)x_1 + 1$
%
%% Normalized buck-boost model
% $\dot{x_1} = -\gamma x_1 + (1-u)x_2 -d$
%
% $\dot{x_2} = -(1-u)x_1 + u$
%

%% Model with body resistors
% In more general cases, a body resistor of the inductor, $R_\mathrm{L}$
% and an equivalent series resistor (ESR) of the capacitor, $R_\mathrm{c}$
% can be added to the above models.
%
%% Buck model with $R_\mathrm{L}$ and $R_\mathrm{c}$
% Since,
%
% $C{dv_\mathrm{c}\over dt} = i_\mathrm{L} - v_\mathrm{o}/R - i_\mathrm{o}$
%
% $v_\mathrm{o} = v_\mathrm{c} + R_\mathrm{c}C{dv_\mathrm{c}\over dt}$
%
% $L{di_\mathrm{L}\over dt} = uv_\mathrm{in} - v_\mathrm{o} -
% R_\mathrm{L}i_\mathrm{L}$
%
% Inserting the second equation into the first leads to:
%
% $C{dv_\mathrm{c}\over dt} = i_\mathrm{L} - v_\mathrm{c}/R -
% {R_\mathrm{c}\over R}C{dv_\mathrm{c}\over dt}- i_\mathrm{o}$
%
% $\left(1+{R_\mathrm{c}\over R}\right)C{dv_\mathrm{c}\over
% dt}=i_\mathrm{L} - v_\mathrm{c}/R - i_\mathrm{o}$
%
% Hence,
%
% $v_\mathrm{o}={Rv_\mathrm{c}\over R+R_\mathrm{c}}+{RR_\mathrm{c}\over
% R+R_\mathrm{c}}(i_\mathrm{L}-i_\mathrm{o})$
%
% and the overall model is
%
% $C{dv_\mathrm{c}\over dt}={R\over R+R_\mathrm{c}}\left(i_\mathrm{L} - {v_\mathrm{c}\over R} - i_\mathrm{o}\right)$
%
% $L{di_\mathrm{L}\over dt} = uv_\mathrm{in} - {Rv_\mathrm{c}\over
% R+R_\mathrm{c}} - \left(R_\mathrm{L}+{RR_\mathrm{c}\over R+R_\mathrm{c}}\right) i_\mathrm{L} + {RR_\mathrm{c}i_\mathrm{o}\over R+R_\mathrm{c}}$
%
% $v_\mathrm{o}={Rv_\mathrm{c}\over R+R_\mathrm{c}}+{RR_\mathrm{c}\over
% R+R_\mathrm{c}}(i_\mathrm{L}-i_\mathrm{o})$
%
%% Boost model with $R_\mathrm{L}$ and $R_\mathrm{c}$
% $C{dv_\mathrm{c}\over dt} = (1-u)i_\mathrm{L} - v_\mathrm{o}/R - i_\mathrm{o}$
%
% $L{di_\mathrm{L}\over dt} = v_\mathrm{in} - (1-u) v_\mathrm{o} - R_\mathrm{L}i_\mathrm{L}$
%
% $v_\mathrm{o} = {Rv_\mathrm{c}\over R+R_\mathrm{c}}+{RR_\mathrm{c}\over
% R+R_\mathrm{c}}((1-u)i_\mathrm{L}-i_\mathrm{o})$
%
%% Buck-boost model with $R_\mathrm{L}$ and $R_\mathrm{c}$
% $C{dv_\mathrm{c}\over dt} = (1-u)i_\mathrm{L} - v_\mathrm{o}/R-i_\mathrm{o}$
%
% $L{di_\mathrm{L}\over dt}=uv_\mathrm{in}-(1-u) v_\mathrm{o}-R_\mathrm{L}i_\mathrm{L}$
%
% $v_\mathrm{o}={Rv_\mathrm{c}\over R+R_\mathrm{c}}+{RR_\mathrm{c}\over R+R_\mathrm{c}}((1-u)i_\mathrm{L}-i_\mathrm{o})$
%
%% Simulink Model
% These three modes of DC-DC converters have been uniformly implemented in
% the MATLAB/Simulink as show in Figure~\ref{fig:simulinkmodel}.
%
% <<DCDCModelInside.png>>
%
% Figure 2. A uniform Simulink model of DC-DC converters.
%
% The input-output connections of the model is shown in Figure 3.
%
% <<DCDCModel.png>>
%
% Figure 3. Input and output connections of the DC-DC converter model.
%
% The first input to the model is the switch signal eight 1 or 0. The
% second one defines the DC source voltage and internal resistance. The
% third input is used to define the output current. The model has two
% outputs, the output voltage and the inductor current, which are the
% states of the system.
%
% The model can be configure with a number of parameters as shown in
% Figure~\ref{fig:simulinkmodelparameters}. These parameters are: the
% capacitance, $C$, inductance, $L$, the internal resistance of the
% capacitor and the inductor, $R_C$ and $R_L$ respectively. Three converter
% modes can be selected through the pull-down menu. One can also define
% either zero or non-zero value to the initial capacitor voltage by
% selecting or de-selecting the ``zero capacitor voltage'' option. Finally,
% the option ``Positive Inductor Current'' defines whether the condition
% $i_L\ge 0$ should be enforced or not.
%
% <<DCDCModelParameters.png>>
%
% Figure 4. Parameters of the DC-DC converter model.
%


function plotFFTSeriesAngle(view,scan)
%
% plotFFTSeriesAngle(view,[scan])
%
% Plots the FFT of the tSeries for the current scan, averaging across
% all pixels (in all slices) in the current ROI.

if ~exist('scan','var')
    scan = getCurScan(view);
end
scan = getCurScan(view);
nCycles = numCycles(view,scan);
maxCycles = round(numFrames(view,scan)/3); % number of frequencies to plot

% Compute the mean tSeries
if view.selectedROI
  ROIcoords = getCurROIcoords(view);
else
  myErrorDlg('No current ROI');
end
tSeries = meanTSeries(view,scan,ROIcoords);

% Calulate the FFT;
absFFT=2*abs(fft(tSeries)) / length(tSeries);
angleFFT=angle(fft(tSeries));

% Header
ROIname = view.ROIs(view.selectedROI).name;
headerStr = ['Mean tSeries, ROI ',ROIname,', scan ',num2str(scan)];
set(gcf,'Name',headerStr);

% plot it
x= [1:maxCycles];
y1 =[absFFT(2:maxCycles+1)];
y2 =[angleFFT(2:maxCycles+1)];

subplot(2,1,1);
plot(x(1:nCycles-1),y1(1:nCycles-1),'b','LineWidth',2)
hold on
plot(x(nCycles-1:nCycles+1),y1(nCycles-1:nCycles+1),'r','LineWidth',2)
plot(x(nCycles+1:maxCycles),y1(nCycles+1:maxCycles),'b','LineWidth',2)
plot(x,y1,'bo','LineWidth',2);
hold off

% Ticks
fontSize = 14;
xtick=nCycles:nCycles:(maxCycles+1);
set(gca,'xtick',xtick);
set(gca,'FontSize',fontSize)
xlabel('Cycles per scan','FontSize',fontSize)
ylabel('Percent modulation','FontSize',fontSize)
grid on

% plot the phase as well
subplot(2,1,2);

plot(x(1:nCycles-1),y2(1:nCycles-1),'b','LineWidth',2)
hold on
plot(x(nCycles-1:nCycles+1),y2(nCycles-1:nCycles+1),'r','LineWidth',2)
plot(x(nCycles+1:maxCycles),y2(nCycles+1:maxCycles),'b','LineWidth',2)
plot(x,y2,'bo','LineWidth',2);
hold off
% Ticks
fontSize = 14;
xtick=nCycles:nCycles:(maxCycles+1);
set(gca,'xtick',xtick);
set(gca,'FontSize',fontSize)
xlabel('Cycles per scan','FontSize',fontSize)
ylabel('Phase','FontSize',fontSize)
grid on

% Save the data in gca('UserData')
data.x = x(1:maxCycles);
data.y1  =  y1(1:maxCycles);
data.y2 = 	y2(1:maxCycles);

set(gca,'UserData',data);


function [y, dsdx, dsdp] = VBA_sigmoid(x, varargin)
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [y, dsdx, dsdp] = VBA_sigmoid(x, [name, value, ...])
% Apply a sigmoid transformation to x
%
% By default, the canonical sigmoid is used:
%
%   y = 1 / (1 + exp(- x))
%
% However, this function can be parametrized using name/value pairs of
% arguments to get, using all options:
%
%   y = offset + scale * 1 / (1 + exp(- slope * (x - center))
%
% IN:
%   - x: values to be transformed
%   - optional key/value pairs or structure (or both) that parametrize the
%     sigmoid transformation:
%       * 'slope'     : inverse-temperature parameter (default = 1)
%       * 'center'    : absissa of the inflexion point (default = 0)
%       * 'scale'     : multiplicative gain of the transformation (default = 1)
%       * 'offset'    : additive gain (default = 0)
%       * 'lapseRate' : shotcut for offset = lapseRate and scale = 1 - 2 * lapseRate
%                       this option is incompatible with offset or scale
%   - other optional key/value pairs or structure (or both)
%       * 'inverse' : reversed transformation, ie. x = VBA_sigmoid(y, opt)
%                       (default = false)
%       * 'finite'  : boundaries that enforce precision to be finite and
%                     derivative to stay numerically non zero (default = 1e-9)
%                     Set to 0 to deactivate
%       * 'derivatives' : cell array of parameter names wrt which dsdp must be
%                         computed (in given order).
%
%
%   Note that if the 'inverse' flag is set to true, the derivatives will
%   not be computed and dsdx = dsdp = [] will be returned instead.
%
% OUT:
%   - y: transformed values, with the same dimension as x.
%   - dsdx: derivative of the transformation wrt. x, taken at each point x.
%     Has the same dimension as x.
%   - dsdp: derivative of the transformation wrt. the parameters specified
%     as arguments. Derivatives are for parameters taken in alphabetical
%     order and aggregated along the first dimension. For example, calling
%     VBA_sigmoid([x1; x2], 'slope', 2, 'center', 3) will return dsdp as:
%     [ d_s/d_center(x1) d_s/d_center(x2) ;
%       d_s/d_slope(x1)  d_s/d_slope(x2)  ]
%     If x is multidimensional, size(dsdp) = [nb_params, size(x,1), size(x,2), ...]
%
% /////////////////////////////////////////////////////////////////////////

%% Globals
% =========================================================================
% truncature for finite sigmoid
epsilon = 1e-9;

%% Shrtcut
% =========================================================================
% quick version!
if nargin == 1 && nargout == 1
    y = epsilon + (1 - 2 * epsilon) ./ (1 + exp (- x));
    return
end

%% Parse arguments
% =========================================================================

% define inputParser
% -------------------------------------------------------------------------
persistent parser;

if isempty (parser)
    parser = getParser ();
end

% parse arguments
% -------------------------------------------------------------------------

parser.parse (varargin{:});
params = parser.Results ;

% apply shortcuts if needed
% -------------------------------------------------------------------------

if ~ ismember ('lapseRate', parser.UsingDefaults)
    % lapseRate is mutually ex
    if any (~ ismember ({'offset', 'scale'}, parser.UsingDefaults))
        error('*** VBA_sigmoid: you can not specify lapseRate and offset or scale at the same time');
    end
    params.offset = params.lapseRate;
    params.scale = 1 - 2 * params.lapseRate;
end

%% Compute transformation
% =========================================================================

%% Inversed case
if params.inverse
    % check that values are valid
    if ~ VBA_isInRange(x, [0 1])
        error('*** VBA_sigmoid: inverse sigmoid inputs must be between 0 and 1');
    end

    % inverse sigmoid transformation
    lx = params.scale * (x - params.offset) .^-1  - 1;
    y = params.center - params.slope ^-1 * log (lx) ;

    % skip derivatives, they are generally not used in inverse case
    dsdx = [];
    dsdp = [];

%% Normal case
else
    % evaluate sigmoid
    % ---------------------------------------------------------------------
    sx = 1 ./ (1 + exp(- params.slope * (x - params.center)));
    y = params.offset + params.scale * sx;

    % ensure finite precision ('finite' flag)
    % ---------------------------------------------------------------------
    if params.finite > 0
        minY = params.offset + epsilon;
        y = max(y,minY);
        maxY = params.offset + params.scale - epsilon;
        y = min(y,maxY);
    end

    % compute derivatives with respect to value
    % ---------------------------------------------------------------------

    % skip if not not required
    if nargout < 2
        return
    end

    % actual computation
    dsdx = params.slope * (y - params.offset) .* (1 - (y - params.offset) ./ params.scale);

    % compute derivatives with respect to parameters
    % ---------------------------------------------------------------------

    % skip if not not required
    if nargout < 3
        return
    end

    % concatenate derivatives for all parameters
    dims = size(x);

    dsdp = cat (2, ...
        - VBA_vec (dsdx), ... % d_center
        1 - 2 * VBA_vec (sx), ... d_lapseRate
        ones(numel(sx),1), ... d_offset
        VBA_vec (sx), ... d_scale
        ((VBA_vec (x) - VBA_vec(params.center)) / params.slope) .* VBA_vec (dsdx) ... d_slope
        );

    % keep only those passed s parameter
    derivables = {'center','lapseRate','offset','scale','slope'};
    if isempty(params.derivatives)
         params.derivatives = setdiff(derivables, parser.UsingDefaults);
    end
    dIdx = cellfun(@(l) find(strcmp (derivables, l)), params.derivatives);

    dsdp = dsdp(:,dIdx);

    % set derived parameter as first dimension
    if all(dims == 1)
        dsdp = dsdp';
    else
        dims(dims==1) = [];
        dsdp = reshape(dsdp', [size(dsdp,2) dims]);
    end

end

end

function parser = getParser()

parser = inputParser;
parser.PartialMatching = false;
parser.KeepUnmatched = true;

% define parameters
% -------------------------------------------------------------------------

% flags
parser.addParameter ('inverse', false, @islogical);
parser.addParameter ('finite', true, @(z) VBA_isInRange(z, [0 1e-2]));
parser.addParameter ('derivatives', {}, @iscellstr);

% x transfomations
parser.addParameter ('slope', 1, @isnumeric);
parser.addParameter ('center', 0, @isnumeric);

% sig transformation
parser.addParameter ('offset', 0, @isnumeric);
parser.addParameter ('scale', 1, @(z) VBA_isInRange(z, [eps Inf]));

% shortcut for lapse rate model
parser.addParameter ('lapseRate', 0, @(z) VBA_isInRange(z, [0 0.5]));
end


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

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

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

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

    for i = 1 : n

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

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

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

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

    end

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

    for k = 1 : n

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

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

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

  end

  return
end


function wimg = warpimg(img, p, sz)
% function wimg = warpimg(img, p, sz)
%
%    img(h,w)
%    p(6,n) : mat format
%    sz(th,tw)
%

%% Copyright (C) 2005 Jongwoo Lim and David Ross.
%% All rights reserved.

if (nargin < 3)
    sz = size(img);
end
if (size(p,1) == 1)
    p = p(:);
end
w = sz(2);  h = sz(1);  n = size(p,2);
%[x,y] = meshgrid(1:w, 1:h);
[x,y] = meshgrid([1:w]-w/2+0.5, [1:h]-h/2);
pos = reshape(cat(2, ones(h*w,1),x(:),y(:)) ...
              * [p(1,:) p(2,:); p(3:4,:) p(5:6,:)], [h,w,n,2]);
cn=size(img,3);
wimg=zeros([sz cn]);
for i=1:cn
    wimg(:,:,i) = squeeze(interp2(img(:,:,i), pos(:,:,:,1), pos(:,:,:,2)));
end
wimg(find(isnan(wimg))) = 0;


function avg = AvgFilter(x)
%
%
persistent prevAvg k
persistent firstRun

if isempty(firstRun)
  k = 1;
  prevAvg = 0;

  firstRun = 1;
end

alpha = (k - 1) / k;
avg   = alpha*prevAvg + (1 - alpha)*x;

prevAvg = avg;
k       = k + 1;


function [a,b] = fdct_usfft_pos2idx(NX,NY,SX,SY,s,w,x,y)

%fdct_usfft_pos2idx.m - For a fixed scale and fixed direction, returns
%		the curvelet which is closest to a certain point on the image
%
% Inputs
%   NX,NY,SX,SY     Values returned by fdct_usfft_param
%   s               scale index
%   w               wedge (angular) index
%   x,y             position in image
%
% Outputs
%   a,b             Index of the curvelet at scale s and angle w which is nearest to (x,y)
%

  nx = NX{s}{w};  ny = NY{s}{w};
  bx = SX{s}{w}(1,1);  by = SY{s}{w}(1,1);
  sx = [SX{s}{w}(2,1)-bx, SY{s}{w}(2,1)-by];
  sy = [SX{s}{w}(1,2)-bx, SY{s}{w}(1,2)-by];
  tmp = 1 + [x-bx,y-by] / [sx; sy];
  a = round(tmp(1));  b = round(tmp(2));
  a = mod(a,nx);  b = mod(b,ny);
  if(a<1)    a = a+nx;  end
  if(a>nx)    a = a-nx;  end
  if(b<1)    b = b+ny;  end
  if(b>ny)    b = b-ny;  end


%  Figure 10.05      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% fig10_05.m is a script to generate Fig. 10.5
% root locus, for the PD design for the satellite
clf;
% parameters of two-mass spring model
m=[1, 0.1]; k0=[0, 0.091] ; d0=[0, 0.0036]; k1=[0, 0.4];

% call two mass-spring model function
[f,g,h,j] = twomass(m,k0,d0);
nc1=0.25*[2, 1];
dc1=[1/40, 1];

% convert controller to state-space
[ac,bc,cc,dc]=tf2ss(nc1, dc1);

% series of controller and plant
[aol,bol,col,dol]= series(ac, bc,cc,dc,f,g,h,j);
[acl]=aol-bol*col;

hold off ; clf

rlocus(aol,bol,col,dol)
grid;
v =[-2.0000,    2.0000,   -1.5000,    1.5000];
axis(v);
title('Fig. 10.5 Root locus for the PD design of the satellite')
%


function [Bdraw,statOK]=sampleB(yData,Psi,SV_H,A,p,priorValues)
% This function uses the method proposed by Carriero Clark and Marcellino
% to draw from the conditional posterior distribution of B.
%sampleB(yData,PsiDraw_prop,HvarsDraw_prop,Adraw_prop,p,priorValues)
%Psi = PsiDraw_prop; %local mean
%SV_H = HvarsDraw_prop; %draw for the time varrying diagonal elements of the VCV
%A = Adraw_prop;     %the time invariant component of the VCV

%% Initialize
[T,M] = size(yData);
Tp=T-p;

% expand prior data
varsH=priorValues.vars;
lambda=priorValues.lambda; %overall tightness
theta=priorValues.theta;   %extra shrinkage for off diagonal elements

%% set up prior variance and mean of B
% Prior mean of B
priorMeanBeta=zeros(M*p,1); %means are equal to zero

% Prior variance of B
priorVarVectors=zeros(M*p,p);

% lag shrinkage
pVector=zeros(p,1);
for j=1:p
    pVector(j)=1/(j^2);
end

varsH=reshape(varsH,M,1);

for i=1:M
    % scale by variance estimates
    vector_iTemp=(varsH(i)*ones(M,1))./varsH;
    vector_iTemp=vector_iTemp*lambda*theta;
    vector_iTemp(i)=vector_iTemp(i)/theta;

    % calculate the prior variances
    vector_i=kron(pVector,vector_iTemp);
    priorVarVectors(:,i)=vector_i;
end

%% Now sample B row-by-row
Ainv=A\eye(M);

Y_Psi=yData-Psi;
Y_Psi(1:p,:)=yData(1:p,:)-ones(p,1)*mean(yData(1:p,:));
X_Psi = lagmatrix(Y_Psi,1:p);
X_Psi = X_Psi(p+1:end,:);
Y_Psi=Y_Psi(p+1:end,:);

OK=0; % to control for unstable draws

rejections=0;
statOK=1;
% start drawing B
while OK==0

    % initialize
    scaledEpsilon=zeros(Tp,M);
    Bdraw=zeros(p*M,M);

    % prepare data for column 1
    y1=Y_Psi(:,1);
    y1Scaled=y1./(SV_H(p+1:T,1).^0.5);
    hScaling1=(SV_H(p+1:T,1).^0.5)*ones(1,p*M);
    X1Scaled=X_Psi./hScaling1;

    % calculate posterior distribution
    postVarBinv=diag(priorVarVectors(:,1).^(-1))+X1Scaled'*X1Scaled;
    postVarB=postVarBinv\eye(M*p);
    postMeanB=postVarB*(diag(priorVarVectors(:,1).^(-1))*priorMeanBeta+X1Scaled'*y1Scaled);

    % obtain cholvar
    [cholPostVarBi,testPD]=chol(postVarB);
    if testPD>0
        cholPostVarBi= cholred(postVarB);
        disp('NPD!')
    end
    cholPostVarBi=cholPostVarBi'; % transpose to obtain lower triangular matrix

    % sample the column of Bdraw
    Bi_draw=postMeanB+cholPostVarBi*randn(p*M,1);
    Bdraw(:,1)=Bi_draw;

    % prepare residuals
    resids_1=y1Scaled-X1Scaled*Bi_draw;
    scaledEpsilon(:,1)=resids_1;

    for i=2:M

        % prepare data for column i
        a_vector=Ainv(i,:)';
        yi=Y_Psi(:,i)-scaledEpsilon*a_vector;
        yiScaled=yi./(SV_H(p+1:T,i).^0.5);

        hScaling_i=(SV_H(p+1:T,i).^0.5)*ones(1,p*M);
        XiScaled=X_Psi./hScaling_i;

        % calculate posterior distribution
        postVarBinv=diag(priorVarVectors(:,i).^(-1))+XiScaled'*XiScaled;
        postVarB=postVarBinv\eye(M*p);
        postMeanB=postVarB*(diag(priorVarVectors(:,i).^(-1))*priorMeanBeta+XiScaled'*yiScaled);

        % obtain cholvar
        [cholPostVarBi,testPD]=chol(postVarB);
        if testPD>0
            cholPostVarBi= cholred(postVarB);
            disp('NPD!')
        end
        cholPostVarBi=cholPostVarBi'; % transpose to obtain lower triangular matrix

        % sample the column of Bdraw
        Bi_draw=postMeanB+cholPostVarBi*randn(p*M,1);
        Bdraw(:,i)=Bi_draw;

        % prepare residuals
        resids_i=yiScaled-XiScaled*Bi_draw;
        scaledEpsilon(:,i)=resids_i;
    end

    % check stability
    [ max_vTemp ] = determineEV(Bdraw,M,p);
    max_v=abs(max_vTemp);

    if max_v <0.999
        OK=1; % accept the draw
    else
        rejections=rejections+1;
        %disp([max_v rejections])
        if rejections>1000
%             disp('Redo This round')
            statOK=0;
            OK=1;
        end

    end

end

end


function [f, tElapsed] = Complex_DIIVINE_feature(im)
% 82 features are extracted:
% Magnitude 1:16
% Relative Mag: 17:28
%Phase: 29:52
%CW-ssim: 53:82
% Magnitude: 1st scale -> 14 features (MGGD); 2nd scale -> 2 features (MGGD);
%        finest scale -> 12 features (GGD, relative magnitude, combined orientations)
% Fhase: 24 features (first 2 scales, relative phase, two orientations)
% Scale Correlation: 30 features (1&2, 2&3, 1&3, hpr & 1,2)

num_scale = 3;
num_ori = 6;
if(size(im,3)~=1)
    im = (double(rgb2gray(im)));
else
    im = double(im);
end

% complex steerable pyramid decomposition
[pyr, pind] = buildSCFpyr(im, num_scale, num_ori-1);
% f = [];
tStart_1 = tic;
[band_dnt ind] = complex_divisive_normalized(pyr,pind,num_scale,num_ori,1,1,3,3);

% Magnitude features
shifts = [0 1; 1 0; 1 1; -1 1];
para_diff = [];
para = [];
b_s = [];

for i = 1:num_ori
    b = abs(band_dnt{i});
    b_s = [b_s; b(:)];
    [alpha, beta] = MGGD_ParaEstimate(b(:));
    para = [para; log(alpha) beta];

    b_diff = b + circshift(b, shifts(3,:)) - circshift(b, shifts(1,:)) - circshift(b, shifts(2,:));
    [sigma, gam] = gaussian_para_esti(b_diff(:));
    para_diff = [para_diff; sigma gam];
end
[alpha_s, beta_s] = MGGD_ParaEstimate(b_s);
para = [para; log(alpha_s) beta_s];

for s_n = 2:num_scale-1
    b_s = [];
    for ori_n = 1:num_ori
        b_ori = abs(band_dnt{(s_n-1)*num_ori+ori_n});
        b_s = [b_s; b_ori(:)];
    end
    [alpha_s, beta_s] = MGGD_ParaEstimate(b_s);
    para = [para; log(alpha_s) beta_s];
end
tElapsed_1 = toc(tStart_1);

f = [para(:); para_diff(:)];

% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
% horizontal and vertical relative phase
tStart_2 = tic;
ph_rela = RelativePhase_HV(pyr, pind, num_ori);
cell_len = length(ph_rela);
wrap_cauchy = [];
for k = 1:cell_len
    re_phase = ph_rela{k};
    [mu, row, loop_num] = WrapCauchyEstimate(re_phase);
    wrap_cauchy = [wrap_cauchy; row];
end
tElapsed_2 = toc(tStart_2);
f = [f; wrap_cauchy];

% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
% CW-SSIM
tStart_3 = tic;
K = 0;
cw_ssim_pyr = Scale_CW_SSIM(pyr, pind, num_scale, num_ori, K);
cw_ssim = cw_ssim_pyr';
cw_ssim = cw_ssim(:);
tElapsed_3 = toc(tStart_3);

f = [f; cw_ssim];

tElapsed = [tElapsed_1 tElapsed_2 tElapsed_3];

% sub-function
function phase_relative = RelativePhase_HV(pyr, pind, NumOri)
m = size(pind,1);
shifts = [0 1; 1 0];
for bnum = 2:m-1*NumOri-1
    band = pyrBand(pyr,pind,bnum);
    phase = angle(band);
    for j = 1:size(shifts, 1)
        phase_shift_hori = shift(phase,shifts(j,:));
        phase_rela_hori = phase_shift_hori - phase;
        phase_rela_hori = phase_rela_hori(:,2:end);
        ind_1_hori = phase_rela_hori < -pi;
        ind_2_hori = phase_rela_hori > pi;
        ind_3_hori = (phase_rela_hori >= -pi)&(phase_rela_hori <= pi);
        ph_rela_hori = ind_1_hori .*(phase_rela_hori+2*pi) + ind_2_hori .*(phase_rela_hori-2*pi) + ind_3_hori .*phase_rela_hori;
        phase_relative(bnum - 1, j) = {[ph_rela_hori]};
    end
end
phase_relative = phase_relative(:);

function cw_ssim_pyr = Scale_CW_SSIM(pyr, pind, num_scale, num_ori, K)
m = size(pind,1);
for bnum = 1:m
    band = pyrBand(pyr,pind,bnum);
    suband(bnum) = {[band]};
end
for scale = 1:num_scale-1
    s1_ind = m - num_ori*scale;
    s2_ind = m - num_ori*(scale+1);

    for ori = 1:num_ori
        band1 = suband{1,(s1_ind + ori - 1)};
        band2 = suband{1,(s2_ind + ori - 1)};
        cw_ssim_pyr(scale,ori) = cwssim_index_new(imresize(band1,size(band2)), band2, K);
    end
end

scale = 1;
s1_ind = m - num_ori*scale;
s2_ind = m - num_ori*(scale+2);

for ori = 1:num_ori
    band1 = suband{1,(s1_ind + ori - 1)};
    band2 = suband{1,(s2_ind + ori - 1)};
    cw_ssim_ori(scale, ori) = cwssim_index_new(imresize(band1,size(band2)), band2, K);
end

st = 2; % only calculate HPR band and 1:num_scale+1-st scale subband
hp_band = suband{1,1};
for scale = st:num_scale
    s_ind = m - num_ori*scale;
    for ori = 1:num_ori
        bp_band =  suband{1,(s_ind + ori - 1)};
        cw_ssim_pyr(scale+num_scale-st,ori) = cwssim_index_new(imresize(bp_band,size(hp_band)), hp_band, K);
    end
end
cw_ssim_pyr = [cw_ssim_pyr; cw_ssim_ori];

function [mu, row, k] = WrapCauchyEstimate(AngleMatrix)
u1 = 0.3; u2 = 0.3;
e = .00001;
k = 0;
while (k < 1000)
    mu1 = u1;
    mu2 = u2;
    w = 1./(1 - mu1*cos(AngleMatrix) - mu2*sin(AngleMatrix));
    num1 = w.*cos(AngleMatrix);
    num2 = w.*sin(AngleMatrix);
    u1 = sum(num1(:))/sum(w(:));
    u2 = sum(num2(:))/sum(w(:));
    k = k + 1;
    if (abs(u1-mu1)< e)&&(abs(u2-mu2)< e)
        break;
    end
end
if k == 1000
    error('Cauchy data do not converge');
   %mu = 0; row = 0; k =0;
end
mu = atan(u2/u1);
row = (1 - sqrt(1 - u1^2 - u2^2))/sqrt(u1^2 + u2^2);


function [] = test()
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
randn('state',1212412414424234324);
rand('state',1212412414424234324);
dim_ambient = 1000;
dim_r = 10;
M = randn(dim_ambient,dim_r);
N = randn(dim_r,dim_ambient);
D0 = M*N/dim_r;

E0 = sign(randn(dim_ambient,dim_ambient));
inds = rand(dim_ambient)<0.7;
E0(inds) = 0;

D = D0 + E0;

D_hat = as_rpca(D,0.05,5,1.3);

error = max(max(abs(D0 - D_hat)))./max(max(abs(D0)));
disp(['recover error=' num2str(error)]);

D_hat = as_rpca(D,0.05,10,1.3);

error = max(max(abs(D0 - D_hat)))./max(max(abs(D0)));
disp(['recover error=' num2str(error)]);

D_hat = as_rpca(D,0.05,15,1.3);

error = max(max(abs(D0 - D_hat)))./max(max(abs(D0)));
disp(['recover error=' num2str(error)]);
end


%% Axes and Antipodal Symmetry
%
%% Directions vs. Axes
%
% In MTEX it is possible to consider three dimensional vectors either as
% directions or as axes. The key option to distinguish between both
% interpretations is *antipodal*.
%
% Consider a pair of vectors

v1 = vector3d(1,1,2);
v2 = vector3d(1,1,-2);

%%
% and plots them in a spherical projection

plot([v1,v2],'label',{'v_1','v_2'})

%%
% These vectors will appear either on the upper or on the lower hemisphere.
% In order to treat these vectors as axes, i.e. in order to assume
% antipodal symmetry - one has to use the keyword *antipodal*.

plot([v1,v2],'label',{'v_1','v_2'},'antipodal')

%%
% Now the direction *v_2* is identified with the direction *-v_2* which
% plots at the upper hemisphere.

%% The Angle between Directions and Axes
%
% As a consequence the angle between two axes *v1*, *v2* will always be the
% smallest angle between the directions *v1*, *v2* and *v1*, *-v2*, i.e. it
% will always be smaller than 90 degree. In the absence of antipodal
% symmetry we obtain

angle(v1,v2) / degree

%%
% whereas, if antipodal symmetry is assumed we obtain

angle(v1,v2,'antipodal') / degree

%% Antipodal Symmetry in Density Estimation
%
% Another example, where antipodal symmetry matters is
% <VectorsDensityEstimation.html density estimation>. For ordinary
% directions we obtain an arbitrary spherical function

v = vector3d.rand(100)
density = v.calcDensity;
plot(density)

%%
% Whereas, if antipodal symmetry is present the resulting density function
% will have antipodal symmetry as well

density = v.calcDensity('antipodal')
plot(density,'complete')

%% Antipodal Symmetry in Experimental Pole Figures
%
% Due to Friedel's law experimental pole figures always provide antipodal
% symmetry. One consequence of this fact is that MTEX plots pole figure
% data always on the upper hemisphere. Moreover if you annotate a certain
% direction to pole figure data, it is always interpreted as an axis, i.e.
% projected to the upper hemisphere if necessary

mtexdata dubna
CS = pf.CS;

% plot the first pole figure
plot(pf({1}))

% annotate a axis on the souther hemisphere
annotate(vector3d(1,0,-1),'labeled','backgroundColor','w')

%% Antipodal Symmetry in Recalculated Pole Figures
%
% However, in the case of pole figures calculated from an ODF antipodal
% symmetry is in general not present.

% some prefered orientation
o = orientation.byEuler(20*degree,30*degree,0,'ZYZ',CS);

% define an unimodal ODF
odf = unimodalODF(o);

% plot pole figures
plotPDF(odf,[Miller(1,2,2,CS),-Miller(1,2,2,CS)])

%%
% Hence, if one wants to compare calculated pole figures with experimental
% ones, one has to add antipodal symmetry.

plotPDF(odf,Miller(1,2,2,CS),'antipodal')

%% Antipodal Symmetry in Inverse Pole Figures
%
% The same reasoning as above holds true for inverse pole figures. If we
% look at complete, inverse pole figures they do not posses antipodal symmetry
% in general

plotIPDF(odf,[yvector,-yvector],'complete')

%%
% However, if we add the keyword antipodal, antipodal symmetry is enforced.

plotIPDF(odf,yvector,'antipodal','complete')

%%
% Notice how MTEX, automatically reduces the fundamental region of inverse
% pole figures in the case that antipodal symmetry is present.

plotIPDF(odf,yvector)

%%
plotIPDF(odf,yvector,'antipodal')


function att = coarsealign3(imudata,lat,smoothepoch)
% att = coarsealign3(imudata,lat,smoothepoch);
% initial coarse alignment based on the acceleration and gyro measurement
% given the initial latitude using Dr. Jekeli's book
% Author: Yudan Yi
% Aug. 2004
% Feb. 2005
%--------------------------------------------------------------------------
if (nargin<2) error('error in data'); end;
if (isempty(imudata)|isempty(lat)) error('error in data'); end;
[n,m] = size(imudata);
if (n<1 || m<7) error('error in data'); end;
if (isempty(lat)) error('error in data'); end;
if (nargin<3) smoothepoch = min(256,n); end;
if (isempty(smoothepoch) || smoothepoch<1) smoothepoch = min(134,n); end;
%--------------------------------------------------------------------------
we = 7.292115147e-05;  % Earth Rotate rate
att = [];
att_index = 0;
smoothepoch=fix(smoothepoch);
for index=1:smoothepoch:n
 	x=index:min(index+smoothepoch-1,n);
    curacc = sum(imudata(x,2:4))/size(imudata,1);
    curgyro = sum(imudata(x,5:7))/size(imudata,1);
    curaccgyro = cross(-curacc,curgyro);
%     gravmag = norm(curaccgyro);
    gravmag = norm(curacc);
    matrix1=[ 0 0 -gravmag; we*cos(lat) 0 -we*sin(lat); 0 gravmag*we*cos(lat) 0];
    matrix2=[curacc;curgyro;curaccgyro];
    c_nb = matrix1\matrix2;% see Jekeli, 2000, page 243, (8.12)
    curatt = Cbn2att(c_nb);

    att_index = att_index+1;
    att(att_index,:)= [imudata(index,1) curatt(:)'];
end
% [att1, pivot] = extract_pivot_angle(att(:,2:4)');
% meanatt = (mean(att1')'+pivot(:))*180/pi;
% stdatt = (std(att1'))*180/pi;
% fprintf('Mean:\t%15.12f\t%15.12f\t%15.12f\n',meanatt);
% fprintf('Std:\t%15.12f\t%15.12f\t%15.12f\n',stdatt);
%--------------------------------------------------------------------------
% figure
% plot(att(:,2:4)*180/pi,'-','Marker','.');
% legend(['\mu=' num2str(meanatt(1),'%5.2f') '\circ \sigma=' num2str(stdatt(1),'%5.2f') '\circ r'],...
%        ['\mu=' num2str(meanatt(2),'%5.2f') '\circ \sigma=' num2str(stdatt(2),'%5.2f') '\circ p'],...
%        ['\mu=' num2str(meanatt(3),'%5.2f') '\circ \sigma=' num2str(stdatt(3),'%5.2f') '\circ h']);
% grid
% xlabel('time');
% ylabel('\circ');
% title('initial coarse alignment for attitude (h,p,r): deg(\circ)');
% % roll, pitch, heading => heading, pitch and roll
% att(:,2) = meanatt(3)*pi/180; % heading
% att(:,3) = meanatt(2)*pi/180; % pitch
% att(:,4) = meanatt(1)*pi/180; % roll


function  [C] =  TWSC_ADMM( Y, D, S, W1, W2, Par )
% This routine solves the following trilateral weighted sparse coding problem
%
% min_{C,Z} |W1(Y-DSC)W2|_F,2 + |Z|_1 s.t.  C=Z
%
% inputs:
%        Y -- d*M data matrix, d is the data dimension, and M is the number
%             of image patches.
%        W1 -- d*d matrix of row weights
%        W2 -- M*M matrix of column weights
% outputs:
%        C -- d*M data matrix, sparse coding coefficient matrix
%        Z -- d*M data matrix, auxiliary variable, equal to C

tol = 1e-8;
Par.maxrho = 100;
Par.maxIter = 10;
Par.rho = 0.5;
Par.mu = 1.1;
Par.display = 0;
% Initializing optimization variables
C = zeros(size(S, 1), size(Y, 2));
Z = zeros(size(C));
U = zeros(size(C));
% Start main loop
iter = 0;
stopCZ = zeros(Par.maxIter, 1);
stopC = zeros(Par.maxIter, 1);
stopZ = zeros(Par.maxIter, 1);
while iter < Par.maxIter
    iter = iter + 1;
    Cpre = C;
    Zpre = Z;
    %% update C, fix Z and U
    % min_{C} ||W1 * (Y - DSC) * W2||_F^2 + 0.5 * rho * ||C - Z + 1/rho * U||_F^2
    % The solution is equal to solve A * X + X * B = E
    A = S' * D' * diag(W1.^2) * D * S;
    W2inv = diag(1./(W2.^2));
    B = 0.5 * Par.rho * W2inv;
    E = S' * D' * diag(W1.^2) * Y + 0.5 * (Par.rho * Z - U) * W2inv;
    C = sylvester(A, B, E);

    %     %% faster solution
    %     [Ua, Sa, ~] = svd(A);
    %     I1 = eye(size(A, 2));
    %     I2 = eye(size(B, 1));
    %     K = kron(I1, A) + kron(B', I2);
    %     invK = 1./diag(K);
    %     UTE = Ua'*E;
    %     vecUTE = UTE(:);
    %     vecUTC = invK .* vecUTE;
    %     MatvecUTC = reshape(vecUTC, [size(UTE, 1) size(UTE, 2)]);
    %     C = Ua*MatvecUTC;

    %% update Z, fix X and D
    % min_{Z} 0.5 * rho * ||Z - (C + 1/rho * U)||_F^2 + ||Z||_1
    Temp = C + U/Par.rho;
    Z = sign(Temp) .* max( abs(Temp) - 1/Par.rho, 0 );

    %% check the convergence conditions
    stopCZ(iter) = max(max(abs(C - Z)));
    stopC(iter) = max(max(abs(C - Cpre)));
    stopZ(iter) = max(max(abs(Z - Zpre)));
    if Par.display %&& (iter==1 || mod(iter,10)==0 || stopC<tol)
        disp(['iter ' num2str(iter) ', mu=' num2str(Par.mu,'%2.1e') ...
            ', max(||c-z||)=' num2str(stopCZ(iter),'%2.3e') ...
            ', max(||c-cpre||)=' num2str(stopC(iter),'%2.3e') ...
            ', max(||z-zpre||)=' num2str(stopZ(iter),'%2.3e')]);
    end
    if stopCZ(iter) < tol && stopC(iter) < tol && stopZ(iter) < tol
        break;
    else
        %% update the augmented multiplier D, fix Z and X
        U = U + Par.rho * (C - Z);
        Par.rho = min(Par.maxrho, Par.mu * Par.rho);
    end
end
return;


%  Figure 10.18      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
%  fig10_18.m is a script to generate Fig 10.18,
%  the symmetric  rootlocus for the LQR symmetric rootlocus
%  compensator of the satellite position control, non-collocated case

% parameter values
m=[1, 0.1]; k=[0, 0.091] ; d=[0, 0.0036]; k1=[0, 0.4];
% call model
[f,g,h,j] = twomass(m,k,d);
s=[f, g;h, 0];r=[0*g;1]; n=s\r;nx=n(1:4);nu=n(5);
% call model
[f1,g,h,j] = twomass(m,k1,d);
% Form G(s)G(-s) in state-space
a=[f, 0*f;
-h'*h, -f'];
b=[g;0*g];
c=[0*h, g'];
d=[0];

hold off; clf;
sysGG=ss(a,b,c,d);
rlocus(sysGG);
grid;
v=[-2 2 -1.5 1.5];axis(v);
title('Fig. 10.18 Symmetric rootlocus for the satellite')


function N = NormDirection(vertex, tri)

% norm of each triangles
pt1 = vertex(:, tri(1, :));
pt2 = vertex(:, tri(2, :));
pt3 = vertex(:, tri(3, :));
n_tri = cross(pt1 - pt2, pt1 - pt3);

% norm of each vertex
N = zeros(3, size(vertex, 2));
% for i = 1 : size(tri, 2)
%     N(:, tri(1, i)) = N(:, tri(1, i)) + n_tri(:, i);
%     N(:, tri(2, i)) = N(:, tri(2, i)) + n_tri(:, i);
%     N(:, tri(3, i)) = N(:, tri(3, i)) + n_tri(:, i);
% end

N = Tnorm_VnormC(double(n_tri), double(tri), double(size(tri,2)), double(size(vertex,2)));

% normalize to unit length
mag = sum(N .* N);
% deal with zero vector
co = find(mag == 0);
mag(co) = 1;
N(1, co) = ones(length(co),1);
N = N ./ sqrt(repmat(mag, 3, 1));
N = -N;


% This demonstrates the smoothing properties of the Kalman lagged filter.
% Hidden states follow a triangular wave, whose observation is perturbed
% with white noise. Critically, we render the inversion scheme blind during
% half a period of the oscillation. We then invert the model (under AR
% priors on hidden states), with and without large backward lag.

clear variables
close all

nt = 10;
eta = 0.*randn(1,3*nt);
x = 1:nt;
x = [x,fliplr(x),x] + eta;

e = randn(1,3*nt);
y = x+e;

theta = [];
phi = [];
alpha = 1/var(eta);
sigma = 1/var(e);

displaySimulations(y,x,eta,e);

f_fname = @f_AR;
g_fname = @g_Id;
dim.n_theta = size(theta,1);
dim.n_phi = size(phi,1);
dim.n = size(x,1);

options.isYout = zeros(1,3*nt);
options.isYout(nt:2*nt+1) = 1;
options.MaxIterInit = 0;
options.priors.a_alpha = 1;
options.priors.b_alpha = 1;

% VB-Kalman-filter
[p1,o1] = VBA_NLStateSpaceModel(y,[],f_fname,g_fname,dim,options);

% VB-Kalman-smoother (lag = size of blind window)
options.backwardLag = nt+1;
[p2,o2] = VBA_NLStateSpaceModel(y,[],f_fname,g_fname,dim,options);
set(gcf,'name',['Kalman lag = ',num2str(o2.options.backwardLag)])

VBA_ReDisplay(p1,o1,1);
set(gcf,'name',['Kalman lag = ',num2str(o1.options.backwardLag)])


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

%Fourier Transfrom properties

%	Time shifting

%x(t)=cos(t)
syms t w
x=cos(t);
t0=2;
xt0=cos(t-t0);
Left=fourier(xt0,w)
X=fourier(x,w);

Right=exp(-j*w*t0)*X


function [HR, HRS, HlR, HlRS, HiR, HiRS, ChiR, HshR, HshRS] = gaussian_method_v7_1_0(R, pars)
%GAUSSIAN_METHOD_V7_1_0 Estimate entropy values using the Gaussian Method.
%
% ---------
% ARGUMENTS
% ---------
% R    - response matrix
% pars - parameters structure

%   Copyright (C) 2009 Cesare Magri
%   Version 7.1.0

% -------
% LICENSE
% -------
% This software is distributed free under the condition that:
%
% 1. it shall not be incorporated in software that is subsequently sold;
%
% 2. the authorship of the software shall be acknowledged and the following
%    article shall be properly cited in any publication that uses results
%    generated by the software:
%
%      Magri C, Whittingstall K, Singh V, Logothetis NK, Panzeri S: A
%      toolbox for the fast information analysis of multiple-site LFP, EEG
%      and spike train recordings. BMC Neuroscience 2009 10(1):81;
%
% 3.  this notice shall remain in place in each source file.

persistent previous_Nt previous_mask;

% HiR, HiRS and ChiR are not computed for the gaussian case, null values
% are returned instead:
HiR  = NaN;
HiRS = NaN;
ChiR = NaN;

% We have to recompute Nc, Nt, maxNt (in the case this routine is called by
% quadratic extrapolation)
Nt    = pars.Nt;
Nc    = size(R,1);
maxNt = size(R,2);
totNt = sum(Nt);
Ns    = pars.Ns;

logOf2times2 = 2*log(2);
logOf2piTimesExp1 = log(2*pi*exp(1));

if ~isequal(previous_Nt, Nt)
  mask = build_logical_mask(Nt, maxNt, Ns);
else
	mask = previous_mask;
end
R(:,~mask) = nan;

% H(R) and H_lin(R) -------------------------------------------------------
if pars.doHR || pars.doHlR

    % Removing mean:
    %meanRvec = mean(R(:,mask),2);
		meanRvec = (sum(sum(R,3),2)+maxNt*Ns-totNt)./totNt; %JM: this is faster, some juggling is needed to account for the -1's

	  %Rvec = R(:,mask) - meanRvec(:,ones(totNt,1));
    Rvec = R(:,mask) - meanRvec*ones(1,totNt);

    covPr = (Rvec * Rvec') ./ (totNt-1);
end

% H(R)
if pars.doHR
    HR = (Nc * logOf2piTimesExp1 + log(det(covPr))) / logOf2times2;

    % Bias correction:
    if pars.biasCorrNum==3
        HRbias = gaussian_bias(totNt, Nc);
        HR = HR - HRbias;
    end
else
    HR = 0;
end

% H_lin(R)
if pars.doHlR
    HlR = (Nc * logOf2piTimesExp1 + sum(log(diag(covPr)), 1)) / logOf2times2;

    % Bias correction:
    if pars.biasCorrNum==3
        HlRbias = Nc * gaussian_bias(totNt, 1);
        HlR = HlR - HlRbias;
    end
else
    HlR = 0;
end

% H(R|S) and H_lin(R|S) ---------------------------------------------------
if pars.doHRS || pars.doHlRS

    % Removing mean
    meanR = sum(R.*permute(mask(:,:,ones(Nc,1)), [3 1 2]), 2) ./ permute(Nt(:, ones(Nc,1)), [2 3 1]);
    Rzeromean = R - meanR(:, ones(maxNt,1), :);

    if Nc==1
        % HlRS is never computed for Nc==1, if we got here it's cause we
        % need to compute H(R|S):

				%covPrsMat = covPrsForSingletonNc(Rzeromean, Nt, mask, maxNt, Ns);
        covPrsMat = reshape(nansum(Rzeromean.*Rzeromean,2),1,[])./(Nt'-1); %JM:

        HRS  = (Nc * logOf2piTimesExp1 + log(covPrsMat(:))) / logOf2times2;

        HlRS = 0;

        % Bias correction:
        if pars.biasCorrNum==3
            HRSbias = gaussian_bias(Nt, 1);
            HRS = HRS - HRSbias;
        end
    else
        HRS  = zeros(Ns,1);
        HlRS = zeros(Ns,1);

        for s=1:Ns
            covPrs = Rzeromean(:, 1:Nt(s), s) * Rzeromean(:, 1:Nt(s), s).' ./ (Nt(s)-1);

            if pars.doHRS
                detCovPrs = det(covPrs);

                HRS(s) = (Nc * logOf2piTimesExp1 + log(detCovPrs)) / logOf2times2;

                % Bias correction:
                if pars.biasCorrNum==3
                    HRSbias = gaussian_bias(Nt(s), Nc);
                    HRS(s) = HRS(s) - HRSbias;
                end
            end;

            if pars.doHlRS
                diagCovPrs = diag(covPrs);

                HlRS(s) = (length(diagCovPrs) * logOf2piTimesExp1 + sum(log(diagCovPrs))) / logOf2times2;

                % Bias correction:
                if pars.biasCorrNum==3
                    HlRSbias = Nc * gaussian_bias(Nt(s), 1);
                    HlRS(s) = HlRS(s) - HlRSbias;
                end
            end
        end
    end

    if ~pars.doHRS
        HRS = 0;
    end

    if ~pars.doHlRS
        HlRS = 0;
    end

else
    HRS  = 0;
    HlRS = 0;
end

% H_sh(R) and H_sh(R|S) ---------------------------------------------------
if pars.doHshR || pars.doHshRS

    Rsh = shuffle_R_across_cells(R, Nt);

    if pars.doHshR
        % Removing mean:
        meanRshvec = mean(Rsh(:,mask),2);
        Rshvec = Rsh(:,mask) - meanRshvec(:,ones(totNt,1));

        covPshr = (Rshvec * Rshvec') ./ (totNt-1);

        HshR = (Nc * logOf2piTimesExp1 + log(det(covPshr))) / logOf2times2;

        % Bias correction:
        if pars.biasCorrNum==3
            HshRbias = gaussian_bias(totNt, Nc);
            HshR = HshR - HshRbias;
        end
    else
        HshR = 0;
    end

    % Remark: HshRS not computed for Nc==1
    if pars.doHshRS
        % Removing mean
        meanRsh = sum(Rsh.*permute(mask(:,:,ones(Nc,1)), [3 1 2]), 2) ./ permute(Nt(:, ones(Nc,1)), [2 3 1]);
        Rsh = Rsh - meanRsh(:,ones(maxNt,1),:);

        HshRS = zeros(Ns,1);
        for s=1:Ns
            covPshrs = Rsh(:, 1:Nt(s), s) * Rsh(:, 1:Nt(s), s).' ./ (Nt(s)-1);
            detCovPshrs = det(covPshrs);

            HshRS(s) = (Nc * logOf2piTimesExp1 + log(detCovPshrs)) / logOf2times2;
        end

        % Bias correction:
        if pars.biasCorrNum==3
            HshRSbias = gaussian_bias(Nt(:), Nc);
            HshRS = HshRS - HshRSbias;
        end
    else
        HshRS = 0;
    end

else
    HshR  = 0;
    HshRS = 0;
end

previous_Nt   = Nt;
previous_mask = mask;

function covPrs = covPrsForSingletonNc(R, Nt, mask, maxNt, Ns)

% Removing first singleton dimension:
R2D = zeros(maxNt, Ns);
R2D(mask) = R(1,mask);

% Computing the variance (R.*R faster than R.^2):
covPrs = (sum(R2D.*R2D,1)) ./ (Nt.' - 1);


% GRADIENT CALLBACK function.
% This function is used as a callback function for fminunc and it aggregates
% cost and gradient values.
function [cost, gradients] = gradient_callback(X, y, theta, lambda)
    % X - training set.
    % y - training output values.
    % theta - model parameters.
    % lambda - regularization parameter.

    % Calculate cost function.
    cost = cost_function(X, y, theta, lambda);

    % Do one gradient step.
    gradients = gradient_step(X, y, theta, lambda);
end


function [H,L,Zf1,Zf0] = nciab(x,BL)
% NCIAB(X) takes data in vector x, filters it with half-band allpass
% filter derived from 5th-order Butterworth filter (ref. Mitra P10.40)
% and returns sub-band coefficients for high-pass channel H and low-pass
% channel L.  BL is buffer length to frame data to transmit final all-
% pass filter conditions.  Zf1 and Zf0 are final condition vectors from
% outputs of analysis filters.
%
% Evan Ruzanski, 4/25/2003

% Set params
M = 2; % DS factor

% Prepare input data
while mod(length(x),2*BL) ~= 0 % Make input even length
    x = [x 0];
end

% Filters (5th order Butterworth per Problem 10.40 Mitra (2e))
numa0 = [0.1056 1];
dena0 = [1 0.1056];

numa1 = [0.5278 1];
dena1 = [1 0.5278];

% Create signals (ref. Fig. 10.55 Mitra (2e))
x0 = [x];
x1 = [x];

% Downsample x0
v0 = x0(1:M:length(x0));

% Downsample x1
v1 = x1(2:M:length(x1)); % Delay = 1

% Filter each channel
lenv = length(v0);
count = lenv/BL;
pointer = 1;
win = [pointer:pointer + BL - 1]; % Window frames of data

bcf = 1; % Init buffer switches
bcb = 4;

while count > 0
    buf0(bcf,1:BL) = v0(win); % Use recursive double buffers
    buf1(bcf,1:BL) = v1(win);

    [u01,Zf0(1,count)] = filter(numa0,dena0,buf0(bcf,1:BL));
    [u11,Zf1(1,count)] = filter(numa1,dena1,buf1(bcf,1:BL));

    buf0(bcb,1:BL) = u01;
    buf1(bcb,1:BL) = u11;

    u0p(win) = buf0(bcb,1:BL);
    u1p(win) = buf1(bcb,1:BL);

    bcf = mod(bcf+2,2)+1; % Switch input buffer
    bcb = mod(bcb+2,2)+3; % Switch output buffer
    pointer = pointer + BL;
    win = [pointer:pointer + BL - 1]; % Slide window
    count = count - 1;
end

% Create/tx band coefficients
L = u0p + u1p;
H = u0p - u1p;


function [varargout]=discQuadMesh(varargin)

% function [F,V,C,indEdge]=discQuadMesh(nElements,r,f)
% ------------------------------------------------------------------------
% This function meshes a circle using quadrilaterial elements.
%
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 10/05/2016 Updated for GIBBON
%------------------------------------------------------------------------

%% Parse input
switch nargin
    case 1
        nElements=varargin{1};
        r=1;
        f=0.5;
    case 2
        nElements=varargin{1};
        r=varargin{2};
        f=0.5;
    case 3
        nElements=varargin{1};
        r=varargin{2};
        f=varargin{3};
end

%% Creating central regular quad mesh

nElements=nElements+~iseven(nElements);%Force even

[X_centralMesh,Y_centralMesh]=meshgrid(linspace(-1,1,nElements+1));
[F_centralMesh,V_centralMesh] = surf2patch(X_centralMesh,Y_centralMesh,zeros(size(X_centralMesh)));
V_centralMesh=V_centralMesh(:,1:2);

%Edge of central mesh
logicCentralMeshEdge=(X_centralMesh==1)|(Y_centralMesh==1)|(X_centralMesh==-1)|(Y_centralMesh==-1);
nEdge=(nElements*4);

% Scaling radius
[ThetaMesh,RadiusMesh]=cart2pol(V_centralMesh(:,1),V_centralMesh(:,2));
RadiusMesh=f*(1/2)*sqrt(2)*RadiusMesh;
[V_centralMesh(:,1),V_centralMesh(:,2)]=pol2cart(ThetaMesh,RadiusMesh);

%% Creating outer mesh

RadiusOuterEdge=ones(1,nEdge);
ThetaOuterEdge=linspace(0,pi*2,nEdge+1);
ThetaOuterEdge=ThetaOuterEdge(2:end)-pi;

[xOuterEdge,yOuterEdge]=pol2cart(ThetaOuterEdge,RadiusOuterEdge);
V_outerEdge=[xOuterEdge(:) yOuterEdge(:)];

V_innerEdge=V_centralMesh(logicCentralMeshEdge,:);
[ThetaEdge,RadiusEdge]=cart2pol(V_innerEdge(:,1),V_innerEdge(:,2));
[ThetaEdge,sortInd]=sort(ThetaEdge);
RadiusEdge=RadiusEdge(sortInd);
[V_innerEdge(:,1),V_innerEdge(:,2)]=pol2cart(ThetaEdge,RadiusEdge);

[Xr]=linspacen(V_innerEdge(:,1),V_outerEdge(:,1),nElements/2+1); Xr(end+1,:)=Xr(1,:);
[Yr]=linspacen(V_innerEdge(:,2),V_outerEdge(:,2),nElements/2+1); Yr(end+1,:)=Yr(1,:);

[Fs2,Vs2] = surf2patch(Xr,Yr,zeros(size(Xr)));
Vs2=Vs2(:,1:2);

V=[V_centralMesh;Vs2];
F=[F_centralMesh;Fs2+size(V_centralMesh,1)];
C=[ones(size(F_centralMesh,1),1); 2*ones(size(Fs2,1),1); ];

indEdge=((size(V,1)-size(Xr,1))+1):size(V,1);

%% Removing double points

[F,V,~,IND_IND]=mergeVertices(F,V);
indEdge=IND_IND(indEdge(1:end-1));

%Scaling radius
[ThetaMesh,RadiusMesh]=cart2pol(V(:,1),V(:,2));
RadiusMesh=r*RadiusMesh;
[V(:,1),V(:,2)]=pol2cart(ThetaMesh,RadiusMesh);

varargout{1}=F;
varargout{2}=V;
varargout{3}=C;
varargout{4}=indEdge;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [ fea, out ] = ex_navierstokes1( varargin )
%EX_NAVIERSTOKES1 2D Example for incompressible stationary flow in a channel.
%
%   [ FEA, OUT ] = EX_NAVIERSTOKES1( VARARGIN ) Sets up and solves stationary Poiseuille
%   flow in a rectangular channel. The inflow profile is constant and the outflow
%   should assume a parabolic profile ( u(y)=U_max*4/h^2*y*(h-y) ).
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       rho         scalar {1}             Density
%       miu         scalar {0.001}         Molecular/dynamic viscosity
%       umax        scalar {0.3}           Maximum magnitude of inlet velocity
%       h           scalar {0.5}           Channel height
%       l           scalar {2.5}           Channel length
%       igrid       scalar 1/{0}           Cell type (0=quadrilaterals, 1=triangles)
%       hmax        scalar {0.04}          Max grid cell size
%       sf_u        string {sflag1}        Shape function for velocity
%       sf_p        string {sflag1}        Shape function for pressure
%       iphys       scalar 0/{1}           Use physics mode to define problem (=1)
%       solver      string openfoam/su2/{} Use OpenFOAM, SU2, FEniCS, or default solver
%       ischeme     scalar {0}             Time stepping scheme (0 = stationary)
%       iplot       scalar 0/{1}           Plot solution and error (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct
%
%   See also EX_NAVIERSTOKES1B

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'rho',      1;
  'miu',      1e-3;
  'umax',     0.3;
  'h',        0.5;
  'l',        2.5;
  'igrid',    1;
  'hmax',     0.04;
  'sf_u',     'sflag1';
  'sf_p',     'sflag1';
  'iphys',    1;
  'solver',   '';
  'ischeme',  0;
  'iplot',    1;
  'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

% Model parameters.
rho       = opt.rho;     % Density.
miu       = opt.miu;     % Molecular/dynamic viscosity.
umax      = opt.umax;    % Maximum magnitude of inlet velocity.
% Geometry and grid parameters.
h         = opt.h;       % Height of rectangular domain.
l         = opt.l;       % Length of rectangular domain.
% Discretization parameters.
sf_u      = opt.sf_u;    % FEM shape function type for velocity.
sf_p      = opt.sf_p;    % FEM shape function type for pressure.

% Geometry definition.
gobj = gobj_rectangle( 0, l, 0, h );
fea.geom.objects = { gobj };
fea.sdim = { 'x' 'y' };   % Coordinate names.

% Grid generation.
if ( opt.igrid==1 )
  fea.grid = gridgen(fea,'hmax',opt.hmax,'fid',fid);
else
  fea.grid = rectgrid(round(l/opt.hmax),round(h/opt.hmax),[0 l;0 h]);
  if( opt.igrid<0 )
    fea.grid = quad2tri( fea.grid );
  end
end
n_bdr = max(fea.grid.b(3,:));           % Number of boundaries.

% Boundary conditions.
dtol      = opt.hmax;
i_inflow  = findbdr( fea, ['x<',num2str(dtol)] );     % Inflow boundary number.
i_outflow = findbdr( fea, ['x>',num2str(l-dtol)] );   % Outflow boundary number.
s_inflow  = ['2/3*',num2str(umax)];                                            % Definition of inflow profile.
s_refsol  = ['4*',num2str(umax),'*(y*(',num2str(h),'-y))/',num2str(h),'^2'];   % Definition of velocity profile.

% Problem definition.
if ( opt.iphys==1 )

  fea = addphys(fea,@navierstokes);     % Add Navier-Stokes equations physics mode.
  fea.phys.ns.eqn.coef{1,end} = { rho };
  fea.phys.ns.eqn.coef{2,end} = { miu };
  fea.phys.ns.eqn.coef{5,end} = { s_inflow };
  if( any(strcmp(opt.solver,{'openfoam','su2'})) )
    fea.phys.ns.sfun = { 'sflag1', 'sflag1', 'sflag1' };
  else
    fea.phys.ns.sfun = { sf_u sf_u sf_p };           % Set shape functions.
  end
  fea.phys.ns.bdr.sel(i_inflow)  = 2;
  fea.phys.ns.bdr.sel(i_outflow) = 4;
  fea.phys.ns.bdr.coef{2,end}{1,i_inflow} = s_inflow;         % Set inflow profile.
  fea = parsephys(fea);                 % Check and parse physics modes.

else

  fea.dvar  = { 'u'  'v'  'p'  };       % Dependent variable name.
  fea.sfun  = { sf_u sf_u sf_p };       % Shape function.

  % Define equation system.
  cvelx = [num2str(rho),'*',fea.dvar{1}];   % Convection velocity in x-direction.
  cvely = [num2str(rho),'*',fea.dvar{2}];   % Convection velocity in y-direction.
  fea.eqn.a.form = { [2 3 2 3;2 3 1 1]       [2;3]                   [1;2];
                     [3;2]                   [2 3 2 3;2 3 1 1]       [1;3];
                     [2;1]                   [3;1]                   []   };
  fea.eqn.a.coef = { {2*miu miu cvelx cvely}  miu                    -1;
                      miu                    {miu 2*miu cvelx cvely} -1;
                      1                       1                      [] };
  fea.eqn.f.form = { 1 1 1 };
  fea.eqn.f.coef = { 0 0 0 };

  % Define boundary conditions.
  fea.bdr.d = cell(3,n_bdr);
 [fea.bdr.d{1:2,:}]         = deal( 0 );

  fea.bdr.d{1,i_inflow}     = s_inflow;

 [fea.bdr.d{:,i_outflow  }] = deal([]);
  % fea.bdr.d{end,i_outflow}  = 0;   % Set pressure to zero on outflow boundary.

  fea.bdr.n = cell(3,n_bdr);
end

% Parse and solve problem.
fea = parseprob(fea);             % Check and parse problem struct.
if( opt.iphys==1 && strcmp(opt.solver,'fenics') )
  fea = fenics( fea, 'fid', fid, 'ischeme', opt.ischeme, 'tmax', 10 );
elseif( opt.iphys==1 && strcmp(opt.solver,'openfoam') )
  if( opt.ischeme==0 )
    dt = 1.0;
    tstop = 1000;
    ddtScheme = 'steadyState';
  elseif( opt.ischeme==1 )
    dt = 0.1;
    tstop = 100;
    ddtScheme = 'backward';
  elseif( opt.ischeme>=2 )
    dt = 0.1;
    tstop = 100;
    ddtScheme = 'CrankNicolson 0.9';
  end
  logfid = fid; if( ~got.fid ), fid = []; end
  fea.sol.u = openfoam( fea, 'fid', fid, 'logfid', logfid, 'ddtScheme', ddtScheme, 'deltaT', dt, 'endTime', tstop );
  fid = logfid;
elseif( opt.iphys==1 && strcmp(opt.solver,'su2') )
  logfid = fid; if( ~got.fid ), fid = []; end
  fea.sol.u = su2( fea, 'fid', fid, 'logfid', logfid, 'ischeme', opt.ischeme, 'tstep', 0.5, 'tmax', 20+30*(opt.ischeme==1) );
  fid = logfid;
else
  if( opt.ischeme==0 )
    jac.form  = {[1;1] [1;1] [];[1;1] [1;1] []; [] [] []};
    jac.coef  = {[num2str(rho),'*ux'] [num2str(rho),'*uy'] []; [num2str(rho),'*vx'] [num2str(rho),'*vy'] []; [] [] []};
    fea.sol.u = solvestat( fea, 'fid', fid, 'nsolve', 2, 'jac', jac );   % Call to stationary solver.
  else
    fea.sol.u = solvetime( fea, 'fid', fid, 'ischeme', opt.ischeme, 'tmax', 10 );
  end
end
fea.sol.u = fea.sol.u(:,end);

% Postprocessing.
s_velm = 'sqrt(u^2+v^2)';
s_err  = ['abs(sqrt((',s_refsol,')^2)-(',s_velm,'))'];
s_len  = ['(x>',num2str(3/4*l),')'];
if ( opt.iplot>0 )
  figure
  subplot(3,1,1)
  postplot(fea,'surfexpr',s_velm,'evaltype','exact')
  title('Velocity field')
  subplot(3,1,2)
  postplot(fea,'surfexpr','p','evaltype','exact')
  title('Pressure')
  subplot(3,1,3)
  postplot(fea,'surfexpr',[s_err,'*',s_len],'evaltype','exact')
  title('Error')
end

% Error checking.
if ( size(fea.grid.c,1)==4 )
  xi = [0;0];
else
  xi = [1/3;1/3;1/3];
end
c_ind = find(evalexpr0(s_len,xi,1,1:size(fea.grid.c,2),[],fea))';
err = evalexpr0(s_err,xi,1,c_ind,[],fea);
ref = evalexpr0(['sqrt((',s_refsol,')^2)'],xi,1,c_ind,[],fea);
err = sqrt(sum(err.^2)/sum(ref.^2));

if( ~isempty(fid) )
  fprintf(fid,'\nL2 Error: %f\n',err)
  fprintf(fid,'\n\n')
end

out.err  = err;
out.pass = err<0.06;
if ( nargout==0 )
  clear fea out
end


function [rect center corners]=calcRectCenter(varargin)
% function drawbox(width,height, param, properties)
%                 ([width,height], param, properties)
%
%   param, properties are optional
%

%% Copyright (C) Jongwoo Lim and David Ross.
%% All rights reserved.

%----------------------------------------------------------
% Process the input.
%----------------------------------------------------------
if (length(varargin{1}) == 2)
  w = varargin{1}(1);
  h = varargin{1}(2);
  varargin(1) = [];
else
  [w,h] = deal(varargin{1:2});
  varargin(1:2) = [];
end

if (length(varargin) < 1 || any(length(varargin{1}) ~= 6))
  M = [0,1,0; 0,0,1];
else
  p = varargin{1};
  if (length(varargin) > 1 && strcmp(varargin{2},'geom'))
    p = affparam2mat(p);
    varargin(1:2) = [];
  else
    varargin(1) = [];
  end
  M = [p(1) p(3) p(4); p(2) p(5) p(6)];
end

%----------------------------------------------------------
% Draw the box.
%----------------------------------------------------------

corners = [ 1,-w/2,-h/2; 1,w/2,-h/2; 1,w/2,h/2; 1,-w/2,h/2; 1,-w/2,-h/2 ]';
corners = M * corners;

result_corners = floor(corners(:,1:4));
x=result_corners(1,1);
y=result_corners(2,1);
w=result_corners(1,3)-result_corners(1,1);
h=result_corners(2,3)-result_corners(2,1);
rect = [x y w h];

center = mean(corners(:,1:4),2);


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

%Fourier Transfrom properties

%	Time reversal

%x(t)=t*u(t)

%X(-w)
x=t*heaviside(t);
X=fourier(x,w) ;
Right=subs(X,w,-w)

%x(-t)
x_t=subs(x,t,-t);
Left=fourier(x_t,w)


% blob_unity1.m
% find blob parameters that make the best "partition of unity"
% jeff fessler

% 1d
% J m alpha	alpha/J	(max/min-1)%
% 2 2 2.502	1.251	0.0192		1 min
% 3 2 2.472	0.824	0.0021		2 min
% 3 2 7.464	2.488	0.0587776%	"
% 4 2 2.461	0.61525 0.000315658%	3 min
% 4 2 8.687	2.17175	0.0102284%	"
% 4 2 11.17	2.7925	0.00177428%	"

% 2d
% J m alpha	alpha/J	(max/min-1)%
% 4 2 7.888	1.972	0.0411645
% 4 2 10.83	2.7075	0.0180456%

% 3d
% J m alpha	alpha/J	(max/min-1)%
% 4 2 10.4	2.6	0.0290578	and mae and rmse < 0.01%.
% for CT this is a fraction of a HU!

if ~isvar('J')
	J = 4; % a = J/2

	alf_list = linspace(0.5*J, 3.0*J, 26+0*101);
	alf_list = linspace(7, 12, 5*10+1); % 2d
	alf_list = linspace(10.5, 11, 5*10+1); % 2d
	alf_list = linspace(2, 12, 10001); % 1d
	alf_list = linspace(7.0, 8.0, 1001); % 1d fine, J=3
alf_list = linspace(0.1, 20, 1001); % 1d coarse
	alf_list = linspace(8.0, 9.0, 1001); % 1d fine, J=4
%	alf_list = linspace(10.3, 10.5, 3); % 3d
%	alf_list = 10.4; % 3d
%	m_list = linspace(1.9, 2.1, 21);
	m_list = 2.0;
	%alf_list = 2.34 * J;
	%m_list = 2;
	[aa mm] = ndgrid(alf_list, m_list);

	x1 = linspace(0, J/2, 101)';
	x2 = linspace(0, J/2, 101)';
	x3 = linspace(0, J/2, 101)';
	x2 = 0; % 1d
	x3 = 0; % 2d
	[xx1 xx2 xx3] = ndgrid(x1, x2, x3);

	% get all j that affect x in [0,J/2]
	j1max = ceil(J-1);
	j1min = floor(-J/2+1);
	ndim = 1 + (length(x2) > 1);
	nj1 = j1max-j1min+1;

	if ndim >= 3
		nj3 = nj1;
		j3min = j1min;
		j3max = j1max;
	else
		nj3 = 1;
		j3min = 0;
		j3max = 0;
	end

	if ndim >= 2
		nj2 = nj1;
		j2min = j1min;
		j2max = j1max;
	else
		nj2 = 1;
		j2min = 0;
		j2max = 0;
	end
end

%
% precompute r samples for each j offset
%
if ~isvar('rr'), disp 'do rr'
	rr = zeros([length(x1) length(x2) length(x3) nj1 nj2 nj3]);
	for j3=j3min:j3max
		for j2=j2min:j2max
			for j1=j1min:j1max
				rr(:,:,:,j1-j1min+1,j2-j2min+1,j3-j3min+1) = ...
				sqrt((xx1-j1).^2 + (xx2-j2).^2 + (xx3-j3).^2);
			end
		end
	end
	%im(x1, x2, rr)
end

if ~isvar('bad'), disp 'do bad'
	bad = zeros(size(mm));
	bb = zeros(length(x1), length(x2), length(x3), nj1*nj2*nj3);

	for ii=1:numel(aa)
		ticker(mfilename, ii, numel(aa))
		kb_a = aa(ii);
		kb_m = mm(ii);

		bb = kaiser_bessel(rr, J, kb_a, kb_m);
		bsum = sum(sum(sum(bb, 3), 4), 5);
		bad(ii) = max(bsum(:)) / min(bsum(:));
	end
	bad = abs(bad-1);
end

if 1
	im clf, im(121, alf_list/J, m_list, bad), cbar
	axis normal
	hold on
	ibest = imin(bad, 2);
	plot(alf_list(ibest(1))/J, m_list(ibest(2)), '*')
	hold off
	xlabel '\alpha/J', ylabel 'm'
end

if ~isvar('bmean')
	kb_a = alf_list(ibest(1));
	kb_m = m_list(ibest(2));

	bb = kaiser_bessel(rr, J, kb_a, kb_m);
	bsum = sum(sum(sum(bb, 3), 4), 5);
	bmean = mean(bsum(:));
end

printf('J m alpha alpha/J ratio-1')
printf('%g %g %g %g %g%%', ...
	J, kb_m, kb_a, kb_a / J, (max(bsum(:)) / min(bsum(:))-1)*100)
printf('mea=%g%%', mean(abs(bsum(:)-bmean))/bmean * 100)
printf('rmse=%g%%', sqrt(mean(abs(bsum(:)-bmean).^2))/bmean * 100)

if 1
	subplot(122)
	if ndim == 1
		plot(x1, reshape(bb, length(x1), []), '--', x1, bsum, '-')
		clf, semilogy(alf_list, bad, '.-'), xlabel '\alpha'
	else
		im(x1, x2, bsum/bmean, 'bsum/bmean'), cbar
	end
end

if 0
	clf
	for j1=1:nj1
		for j2=1:nj2
			subplot(nj1, nj2, j1 + (j2-1)*nj1)
			if ndim == 1
				plot(x1, bb(:,1,j1,j2))
			else
				im(x1, x2, bb(:,:,j1,j2))
			end
		end
	end
end


%
%
% Copyright (c) 2012 University of Crete - Computer Science Department (UOC-CSD)
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function p = wrappednormcdf(x, m, s, N)

    % N=10 gives a log error smaller than 1e-12 for s=8
    % with s=8 distribution is "almost" uniform (std < 1e-12)
    if nargin<4; N=10; end

    p = 0;
    for k=-N:N
        p = p + normcdf(x+2.*pi.*k, m, s);
    end

    p = p - N;

return


classdef CoSaMP < handle
    % Implements CoSaMP-MMV algorithm for sparse recovery

    properties
        % These properties can be configured before running cosamp
        % Default threshold
        errorNormThreshold = 1e-6;
        % Maximum number of iterations for approximation
        MaxIters
        Verbose = false
        % Indicates if matching pursuit should be used for identification
        % UseMPIdentification = false
        % Indicates if least squares should be run on final support
        LSOnFinalSupport = false
        % The norm to be chosen for rows
        P
        % Indicates that residuals should be orthogonalized for rank awareness
        RankAwareResidual = false
    end

    properties(SetAccess=private)
        % The dictionary
        Dict
        % Ambient signal dimensions
        N
        % Number of atoms in dictionary
        D
        % Sparsity level of representations (may be negative)
        K
        % Result of a solver
        result
    end

    methods
        function self  = CoSaMP(Dict, K, P, options)
            if nargin < 3
                % By default we apply l_1 norm on rows
                P = 1;
            end
            if P ~= 1 &&  P ~= 2
                error('Only l_1 and l_2 norms are supported.');
            end
            self.P = P;
            % We assume that all the columns in dictionary are normalized.
            if isa(Dict, 'spx.dict.Operator')
                self.Dict = Dict;
            elseif ismatrix(Dict)
                self.Dict = spx.dict.MatrixOperator(Dict);
            else
                error('Unsupported operator.');
            end
            [self.N, self.D] = size(Dict);
            if nargin < 2
                % No sparsity level has been pre-specified.
                % We make an estimate of sparsity level
                % based on phase transition analysis by Donoho
                K = round((self.N / 2) * log (self.D));
            end
            self.K = K;
            % Maximum number of iterations
            maxIter = 30;
            self.MaxIters = maxIter;
            if nargin >= 4
                % options have been specified
                if isfield(options, 'RankAwareResidual')
                    self.RankAwareResidual = options.RankAwareResidual;
                end
            end
        end

        function result  = solve(self,Y)
            n = self.N;
            d = self.D;
            k = self.K;
            % The number of signals being approximated.
            s = size(Y, 2);
            dict = self.Dict;
            % Current estimate
            solution_nz_mat = zeros(k,s);
            % Current residual
            residual_mat = Y;
            % Number of iterations
            iterations = 0;
            y_norm = norm(Y, 'fro');
            old_residual_norm = 1;
            min_residual_norm = old_residual_norm;
            errorNormThreshold = self.errorNormThreshold;
            maxIterations = self.MaxIters;
            result.halted_on_max_iter = false;
            result.halted_on_residual_norm = false;
            result.halted_on_norm_change = false;
            result.halted_on_support_change = false;
            % K indices for current support
            current_support = [];
            while true
                iterations  = iterations+1;
                % We identify the support for largest 2K entries
                extra = 2*k;
                % if isempty(current_support)
                %     extra = k + extra;
                % end
                if self.RankAwareResidual
                    residual_mat = orth(residual_mat);
                end
                % Compute the proxy
                proxy_mat = dict.apply_ctranspose(residual_mat);
                % identify the largest entries
                largest_2k  = spx.pursuit.joint.CoSaMP.largest_k_l2(proxy_mat, extra);
                % We now compute our support
                % build an array holding up to 3 K largest indices
                support_3k = false(d, 1);
                % the older K columns
                support_3k(current_support) = true;
                % New 2K columns
                support_3k(largest_2k) = true; % T <= 3K
                % We pickup corresponding columns from Phi
                subdict = dict.columns(support_3k); % MxT
                % We compute signal estimate over these columns
                % B_subdict =  linsolve(subdict, Y); % TxM * MxS
                B_subdict = spx.pursuit.joint.CoSaMP.least_squares_on_support(subdict, Y);
                % sort them in descending row norm order
                pruned_largest_k =  spx.pursuit.joint.CoSaMP.largest_k_l2(B_subdict, k);
                % keep only first k rows as new solution
                solution_nz_mat = B_subdict(pruned_largest_k, :);
                old_support = current_support;
                % update current support by choosing largest k indices
                support_3k = find(support_3k == 1);
                current_support = support_3k(pruned_largest_k);
                % compute the measurement estimate
                y_estimate_matrix = dict.apply_columns(...
                    solution_nz_mat, current_support);
                % compute the new residual
                residual_mat = Y - y_estimate_matrix;
                % compute the residual norm
                residual_norm = norm(residual_mat, 'fro');
                residual_norm = residual_norm / y_norm;
                % how many indices have been added and/or removed
                support_change = length(union(old_support,current_support)) - length(intersect(old_support,current_support));
                % if residual_norm > 1.2*min_residual_norm
                %     % We are diverging
                %     break;
                % end
                if residual_norm < old_residual_norm
                    improvement = (old_residual_norm - residual_norm) / old_residual_norm;
                    % if improvement < 1e-8
                    %     % No improvement in this iteration
                    %     result.halted_on_norm_change = true;
                    %     break;
                    % end
                end
                if(residual_norm < errorNormThreshold)
                    result.halted_on_residual_norm = true;
                    break;
                end
                if iterations >= maxIterations
                    % Too many iterations we are going nowhere
                    result.halted_on_max_iter = true;
                    break;
                end
                if ~isempty(old_support) && support_change == 0
                    result.halted_on_support_change = true;
                    break;
                end
                % TODO add support for detection of no change
                % in support
                old_residual_norm = residual_norm;
                min_residual_norm = min(residual_norm, min_residual_norm);
            end
            % CoSaMP is done
            if  self.LSOnFinalSupport
                % This is used only in high coherence situations.
                % The least squares estimate is not good enough
                % as it allows more distribution of energy on to other 2K indices
                % solve another least squares problem.
                subdict = dict.columns(current_support);
                solution_nz_mat = linsolve(subdict, Y);
            end
            % copy the solution
            result.Z = zeros(d, s);
            result.Z(current_support, :) = solution_nz_mat;
            % copy the measurement residual
            result.R = residual_mat;
            result.iterations = iterations;
            result.support = current_support;
        end

    end

    methods(Static)
        function largest_indices  = largest_thresholding(dict, R, count)
            % We create the proxy of current residual
            E = dict.apply_ctranspose(R);
            tmp = abs(e);
            [~, indices] = sort(tmp, 'descend');
            % We identify the support for largest count entries
            largest_indices  = indices(1:count);
        end

        function largest_indices = largest_k_l1(data_matrix, K)
            sums = spx.norm.norms_l1_rw(data_matrix);
            [~, indices] = sort(sums, 'descend');
            % We identify the support for largest K entries
            largest_indices  = indices(1:K);
        end

        function largest_indices = largest_k_l2(data_matrix, K)
            sums = spx.norm.norms_l2_rw(data_matrix);
            [~, indices] = sort(sums, 'descend');
            % We identify the support for largest K entries
            largest_indices  = indices(1:K);
        end

        function largest_indices  = largest_mp(dict, r, count)
            % Uses matching pursuit to identify largest indices.
            % We create the proxy of current residual
            d = size(dict,2);
            indices = false(1, d);
            k = 0;
            while k < count
                products = dict.apply_ctranspose(r);
                abs_products = abs(products);
                % Find the highest inner product
                [~, index] = max(abs_products);
                % Add this index to support
                if ~indices(index)
                    % we have discovered a new atom.
                    k = k + 1;
                    indices(index) = true;
                end
                % pick up the coefficient of this inner product
                coeff = products(index);
                % update residual
                r = r - coeff * dict.column(index);
            end
            % We identify the support for largest 2K entries
            largest_indices  = find(indices);
        end

        function X = least_squares_on_support(subdict, Y)
            [U,s,V] = csvd(subdict);
            num_signals = size(Y, 2);
            num_cols = size(subdict, 2);
            X = zeros(num_cols, num_signals);
            x = zeros(num_cols, 1);
            for ns=1:num_signals
                y = Y(:, ns);
                normBound = 2*norm(y);
                [alpha2,lambda,maxIterReached] = lsqi(U,s,V,y,normBound);
                if maxIterReached
                    % lsqi did not converge; use cvx (slower) instead
                    [aa,bb] = size(subdict);
                    cvx_begin quiet
                      variable alpha3(bb) complex;
                      minimize norm(y - subdict*alpha3)
                      subject to
                      norm(alpha3) <= normBound;
                    cvx_end
                    x = alpha3;
                else
                    x = alpha2;
                end
                X(:, ns) = x;
            end
        end
    end
end


function [ centroids ] = Kmeans_update( data, centroids, assignments )
%UNTITLED2 Summary of this function goes here
%   Detailed explanation goes here

for i = 1:size(centroids,3);
    pos = find(assignments == i);
    newCenter = zeros(size(centroids,1), size(centroids,2));
    for j = pos
        newCenter(:,:) = newCenter(:,:) + data(:,:,j);
    end
    newCenter = newCenter ./ length(pos);
    centroids(:,:,i) = newCenter;
end

end


function [F] = TriScatteredInterpVector(V,D)
  % TRISCATTEREDINTERPVECTOR simple extension of TriScatteredInterp to handle
  % vector values at each data point
  %
  % [F] = TriScatteredInterpVector(V,D)
  %
  % Inputs:
  %   V  #V by 2 list of data positions
  %   D  #V by m list of data vectors
  % Outputs:
  %   A  m-size cell of TriScatteredInterp classes corrsepnding to each
  %     coordinate of data vectors
  %   ev  function handle to evaluate data at given points (X,Y)
  %

  m = size(D,2);

  % make room in cell array
  A = cell(1,m);
  % loop over data coordinates
  for ii = 1:m
    % build interpolant for this coordinate
    interp_func = TriScatteredInterp(V,D(:,ii));
    A{ii} = interp_func;
  end

  % function handle to eval interps
  F = @EvalTriScatteredInterpVector;

  function DXY = EvalTriScatteredInterpVector(X,Y)
    % for convenience build the interpolantion evaluation function handle
    f = @(I) I(X,Y);
    DXY = cellfun(f,A, 'UniformOutput', false);
    DXY = reshape(cell2mat(DXY),[size(X) m]);
  end
end


function [wout, hout] = nomalize_wh(win,hin)
% [wout, hout] = nomalize_wh(win,hin,k)

sumw = sum(win,1);
wout = win./repmat(sumw,size(win,1),1);                %normalization of each component
hout = hin.*repmat(sumw',1,size(hin,2));    %to keep the multiplication equal


% DEMO
%
% Demonstrate the use of the Support Vector Machine toolbox to distinguish
% examples of Versicolour from Setosa and Virginica varieties of Iris, given
% petal width an length attributes.  The data is taken from the well known
% Iris benchmark [1], available from the UCI Repository of Machine Learning
% Databases (http://www.ics.uci.edu/~mlearn/MLRepository.html).
%
% [1] R. A. Fisher,
%     "The use of multiple measurements in taxonomic problems",
%     Annual Eugenics, 7(2), pp 179-188, 1936.

%
% File        : demo.m
%
% Date        : Saturday 16th September 2000
%
% Author      : Dr Gavin C. Cawley
%
% Description : Test harness for object oriented implementation of Vapnik's
%               linear support vector machine (SVM) [1].
%
% References  : [1] V.N. Vapnik,
%                   "The Nature of Statistical Learning Theory",
%                   Springer-Verlag, New York, ISBN 0-387-94559-8,
%                   1995.
%
% History     : 16/08/1999 - v1.00
%               13/09/2000 - v1.01 minor changes to comments etc
%               13/09/2000 - v1.02 updated to use gateway method svc/train
%               16/09/2000 - v1.10 added xi-alpha estimate of l-o-o error
%               24/11/2000 - v1.11 minor bug-fix for loading iris data under
%                                  all operating systems.
%
% Copyright   : (c) Dr Gavin C. Cawley, November 2000.
%
%    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 should have received a copy of the GNU General Public License
%    along with this program; if not, write to the Free Software
%    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
%

% start from a clean slate

clf;

clear classes all;

% load some data

fprintf(1,'loading training data...\n');

iris = load('data/iris.txt');

x = iris(:,3:4);
y = 2*(iris(:,5) == 2)-1;

% define useful constants

l = size(x,1);

% display data

fprintf(1,'displaying data...\n');

plot(x(find(y==1),1)',x(find(y==1),2)','bs',x(find(y==-1),1)',x(find(y==-1),2)','go')
axis(axis + [-0.1 +0.1 -0.1 +0.1])
legend('class one','class two',0);
drawnow;

% create tutor

fprintf(1,'creating tutor...\n');

kernel = rbf(0.5);
C      = 1.0;
tutor  = smosvctutor;

% train support vector machine

fprintf(1,'training support vector machine...\n');

net = train(svc, tutor, x, y, C, kernel);

net = fixduplicates(net, x, y);

net2 = strip(net);

% display support vectors

fprintf(1,'displaying support vectors...\n');

sv = getsv(net2);

hold on
plot(sv(:,1)',sv(:,2)','k+');
legend('class one','class two','support vector',0);
hold off

fprintf(1, 'there are %d support vectors\n', getnsv(net2));

% compute correctness

fprintf(1,'correctness = %4.1f%%\n', 100*sum(sign(fwd(net2,x))==y)/l);

% display decision boundary

fprintf(1,'displaying decision boundary...\n');

a     = axis;
[X,Y] = meshgrid(a(1):0.05:a(2),a(3):0.05:a(4));
X2    = [reshape(X,prod(size(X)),1) reshape(Y,prod(size(X)),1)];
z     = fwd(net2,X2);
z     = reshape(z,size(X));

hold on
contour(X,Y,z,[+1 +1],'b');
contour(X,Y,z,[+0 +0],'r');
contour(X,Y,z,[-1 -1],'g');
legend('class one','class two','support vector','class one margin','decision boundary','class two margin',0);
hold off

% highlight estimated leave-one-out errors

e = xialpha(net);

i = find(e);

fprintf(1,'l-o-o correctness >= %4.1f%%\n', 100*correctness(e));

hold on
plot(x(i,1)', x(i,2)', 'rx');
legend('class one','class two','support vector','class one margin','decision boundary','class two margin','l-o-o error',0);
hold off

% all done

fprintf(1,'bye bye...\n');

% bye bye...


function complex_example_AD()
% A basic example that shows how to define the cost funtion for
% optimization problems on complex manifolds.
%
% Note that automatic differentiation for complex numbers is not supported
% for Matlab R2021a or earlier. To fully exploit the convenience of AD,
% please update to the latest version if possible. If the user cannot have
% access to Matlab R2021b or later, manopt provides an alternative way to
% deal with complex problems which requires the user to define the cost
% funtion using the basic functions listed in the folder /functions_AD or
% to define their own functions following the rules described in that file.
% See the following as an example.
%
% See also: manoptADhelp

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Xiaowen Jiang, August, 31, 2021
% Contributors: Nicolas Boumal
% Change log:
%

    % Verify that the deep learning tool box was installed
    assert(exist('dlarray', 'file') == 2, ['Deep learning tool box is '...
    'needed for automatic differentiation.\n Please install the'...
    'latest version of the deep learning tool box and \nupgrade to Matlab'...
    ' R2021b if possible.'])

    % Generate the problem data.
    n = 100;
    A = randn(n, n) + 1i*randn(n, n);
    A = .5*(A+A');

    % Create the problem structure.
    S = spherecomplexfactory(n);
    problem.M = S;

    % Define the problem cost function
    % For Matlab R2021b or later, define the problem cost function as usual
    % problem.cost  = @(X) -.5*real(X'*A*X);

    % For Matlab R2021a or earlier, translate the cost function into a
    % particular format with the basic functions in /functions_AD
    problem.cost  = @(X) -creal(cprod(cprod(ctransp(X), A), X));

    % Define the gradient and the hessian via automatic differentiation
    problem = manoptAD(problem);

    % Numerically check gradient and Hessian consistency.
    figure;
    checkgradient(problem);
    figure;
    checkhessian(problem);

    % Solve.
    [x, xcost, info] = trustregions(problem);          %#ok<ASGLU>

    % Display some statistics.
    figure;
    semilogy([info.iter], [info.gradnorm], '.-');
    xlabel('Iteration #');
    ylabel('Gradient norm');
    title(['Convergence of the trust-regions algorithm on the'...
        'complex sphere power manifold']);

end


function geometry_test0321 ( )

%*****************************************************************************80
%
%% TEST0321 tests HEXAGON_VERTICES_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0321\n' );
  fprintf ( 1, '  HEXAGON_VERTICES_2D: the vertices of the unit hexagon.\n' );

  p = hexagon_vertices_2d ( );

  r8mat_transpose_print ( dim_num, 6, p, '  Vertices:' );

  return
end


function psi = data2psiX(data,srate,freqbins,permtest)
% calculates phase slope index (PSI) as formulated in the paper:
%    Nolte G, Ziehe A, Nikulin VV, Schlogl A, Kramer N, Brismar T, Muller KR.
%    Robustly estimating the flow direction of information in complex physical systems.
%    Physical Review Letters. To appear.
%    (for further information:    http://doc.ml.tu-berlin.de/causality/ )
%
% Usage:
%   psi = data2psiX(data,srate,freqbins,permtest);
%
% Input:
%     data:  MxNxT matrix for M channels, N timepoints, and T trials
%    srate:  data sampling rate in Hz
% freqbins:  KxQ matrix containing frequency boundaries in rows K.
% permtest:  Permutation test (boolean). If true, psi output values are in standardizes Z values
%               relative to a null hypothesis distribution
%
% Output:
%      psi:  phase-slope-index values. For M channels PSI is an MxM matrix if
%               one frequency bin, or MxMxK if freqbins has K rows (with K>1).
%               psi(i,j) is the directed connectivity from channel i to
%               channel j, (e.g., channel i is the sender if psi(i,j) is
%               positive; channel i is the receiver if negative)
%
%
%  (This function was modified from the original by Mike X Cohen)

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

% modified from the original dat2psi.m by mikexcohen@gmail.com

%% check inputs and initialize

if nargin<2
    help data2psiX
    error('Read help file.');
elseif nargin==2
    permtest = 0;
    freqbins = [];
elseif nargin==3
    permtest = 0;
end

n_permutes = 1000;

% data dimensions
[nchan,npnts,ntrials]=size(data);

% define frequencies
hz  = linspace(0,srate/2,floor(npnts/2)+1);
nhz = length(hz);

% initialize
cs  = zeros(nchan,nchan,nhz);
psi = zeros(nchan,nchan,size(freqbins,1));

%% compute cross-spectral density

datafft = fft(bsxfun(@times,data,hanning(npnts)'),[],2);

% trial-average cross-spectral density
for triali=1:ntrials
    for freqi=1:nhz

        tempfftdat = squeeze(datafft(:,freqi,triali))';
        cs(:,:,freqi) = cs(:,:,freqi) + tempfftdat'*conj(tempfftdat);
    end
end
cs = cs./triali;

%% compute PSI

for freqbini=1:size(freqbins,1)

    % find FFT indices of requested frequency bands
    freqidx = dsearchn(hz',freqbins(freqbini,1)) : dsearchn(hz',freqbins(freqbini,2));
    nfidx   = length(freqidx);

    if nfidx<4
        warning('There are fewer than four frequency bins. Consider using longer time windows or wider frequency bands.')
    end

    % temporary phase-frequency matrix from this frequency band
    pp = zeros(nchan,nchan,nfidx);

    for fi=1:nfidx
        pp(:,:,fi) = cs(:,:,freqidx(fi))./sqrt(diag(cs(:,:,freqidx(fi)))*diag(cs(:,:,freqidx(fi)))');
    end

    % average phase slope for each frequency band
    psi(:,:,freqbini) = sum(imag(conj(pp(:,:,1:end-1)).*pp(:,:,2:end)),3);

    %% optional permutation testing

    if permtest

        nulldist = zeros(nchan,nchan,n_permutes);
        for permi=1:n_permutes
            nulldist(:,:,permi) = sum(imag(conj(pp(:,:,randperm(nfidx))).*pp(:,:,randperm(nfidx))),3);
        end

        psi(:,:,freqbini) = ( psi(:,:,freqbini)-mean(nulldist,3) ) ./ std(nulldist,[],3);
    end

    % zero-out diagonals
    tmp = squeeze(psi(:,:,freqbini));
    tmp(logical(eye(nchan))) = 0;
    psi(:,:,freqbini) = tmp;

end

%% end


function [C,T,A]=naca4_3d(airfoil,y,nc)
% determine airfoil skin and camber coordinates from NACA designation
%% NacaCoord.m
% NACA 4-digit or 5-digit series airfoil
%
% outputs:  C = matrix of camber line coordinates, x,y,z
%               ordered trailing edge first
%           T = matrix of skin coordinates
%               ordered from trailing edge over top surface then
%               along bottom surface to trailing edge
%           A = area of airfoil cross section (normalized)
%
% NOTE: Chord of output airfoil is 1
%
% inputs:   airfoil = NACA 4- or 5-digit airfoil as string
%           y = spanwise location
%           nc = number of chordwise panels
%
% Ref:      http://www.aerospaceweb.org/question/airfoils/q0041.shtml

%           Ira H. Abbott and Albert E. Von Doenhoff, Theory of Wing
%           Sections, 1959, Dover Publications, NY.
%
% Coordinate system:
%   x: chord direction, origin at LE, positive is pointing toward TE
%   y: station along span, perpendicular to aircraft midplane
%   z: perpendicular to chord, positive is up

%This 'if' statement establishes the constants for either 4- or 5- digit
%airfoils.
if length(airfoil) == 4
    m = str2num(airfoil(1))/100;
    p = str2num(airfoil(2))/10;
    t = str2num(airfoil(3:4))/100;

elseif length(airfoil) == 5
    switch airfoil(1:3)
        case '210'
            m = 0.0580;
            k1 = 361.4;
        case '220'
            m = 0.1260;
            k1 = 51.64;
        case '230'
            m = 0.2025;
            k1 = 15.957;
        case '240'
            m = 0.2900;
            k1 = 6.643;
        case '250'
            m = 0.3910;
            k1 = 3.230;
        otherwise
    end
    t = str2num(airfoil(4:5))/100;
end

%% Constants
% nc          % number of line segments describing top surface
% (= number of line segments describing bottom surface)
npc = nc+1;      % number of points along mean camber line
nps = 2*nc+1;    % number of points along surface
dx=1/nc;        % x-direction increment (along chord)

%% Fill coordinate matrices with y coordinate
C = zeros(npc,3); % mean camber
C(:,2) = y;
T = zeros(nps,3); %skin
T(:,2) = y;

%% Mean Camber Line and Slope
% dzcdx is the derivative of the mean camber line
% theta is the local angle of the mean camber line
% x=1:-dx:0; % order x locations starting at trailing edge, x=1
x=0:dx:1; % order x locations starting at leading edge, x=0
if length(airfoil) == 4
    for i = 1:nc+1
        if x(i)<p    % leading edge
            zc(i) = m/(p^2)*(2*p*x(i)-x(i)^2);  %Eqn 6.4 Abbot
            dzcdx(i) = m/p^2*(2*p-2*x(i));
            theta(i) = atan(dzcdx(i));
        else            % trailing edge
            zc(i) = m/(1-p)^2*((1-2*p)+2*p*x(i)-x(i)^2);    %Eqn 6.4 Abbot
            dzcdx(i) = m/(1-p)^2*(2*p-2*x(i));
            theta(i) = atan(dzcdx(i));
        end
    end
elseif length(airfoil) == 5
    for i = 1:nc+1
        if x(i)<m    % leading edge
            zc(i) = 1/6*k1*(x(i)^3-3*m*x(i)^2+m^2*(3-m)*x(i));  %Eqn 6.6 Abbot
            dzcdx(i) = 1/6*k1*(3*x(i)^2-6*m*x(i)+m^2*(3-m));
            theta(i) = atan(dzcdx(i));
        else            % trailing edge
            zc(i) = 1/6*k1*m^3*(1-x(i));    %Eqn 6.6 Abbot
            dzcdx(i) = -1/6*k1*m^3;
            theta(i) = atan(dzcdx(i));
        end
    end
end

C(:,1)=-x;  % output mean camber line coordinates; negative x due to axis convention
C(:,3)=-zc; %negative z due to axis convention

%% Thickness distribution is the same for both 4- and 5-digit airfoils
for i=1:nc+1
    zt(i) = t/0.2*[0.2969*x(i)^0.5-0.1260*x(i)-0.3516*x(i)^2+0.2843*x(i)^3-0.1015*x(i)^4];  %Eqn 6.2 Abbott
end

%% Area
A=0;
for i=1:nc
    da=dx*(zt(i)+zt(i+1));  %Take the average of zt(i) and zt(i+1) and double it
    A=A+da;
end
A;

%% Upper skin
for i=1:nc+1
    xu(i) = x(i)-zt(i)*sin(theta(i));  %Eqn 6.1 Abbott
    zu(i) = zc(i)+zt(i)*cos(theta(i));  %Eqn 6.1 Abbott
    T(i,1) = -xu(i);  %negative x due to sign convention
    T(i,3) = -zu(i);  %negative z due to sign convention
end

%% lower skin
% exclude point nc+1 at leading edge
% because it is included as the last point of the upper surface

for i=1:nc
    xl(i) = x(i)+zt(i)*sin(theta(i));    %Eqn 6.1 Abbott
    zl(i) = zc(i)-zt(i)*cos(theta(i));    %Eqn 6.1 Abbott
end

% reverse order so LE to TE
for i=nc+2:nps
    T(i,1) = -xl(2*(nc+1)-i);%negative x due to sign convention
    T(i,3) = -zl(2*(nc+1)-i);%negative z due to sign convention
end
%figure
%plot(xu,zu,'o')
%hold on
%plot(xl,zl,'or')
%axis equal


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

t = 0;              % current time
T = 1;              % maturity

F = 80:.25:120;       % possible forwards in T
A = 0.01:0.01:0.4;  % possible volatilities in T

f = 100;            % current spot asset
alpha = 0.2;        % current spot volatility

beta = 0.5;         % SABR CEV coeff
nu = 0.2;           % Vol of vol
rho = 0;         % Correlation

d_base = @(x,y) psabr(t,T,f,x,alpha,y,beta,nu,rho);   % approx density

xret = ((F-f)/f);   % possible relative changes

low = 0;            % limit integration low
up = 1;             % limit integration high

y_base = density1d(F,d_base,low,up);    % compute density
legend_base = 'Base';
title_plot = 'SABR Density';

y2_base = density2d(F,A,d_base);

%% Change alpha
d_low = @(x,y) psabr(t,T,f,x,alpha-0.1,y,beta,nu,rho); % low density
d_high = @(x,y) psabr(t,T,f,x,alpha+0.1,y,beta,nu,rho);% high density

y_low = density1d(F,d_low,low,up);
y_high = density1d(F,d_high,low,up);
legend_low = 'Changing \alpha low';
legend_high = 'Changing \alpha high';

createfigure_density(xret,y_base',y_low',y_high',title_plot,legend_base,legend_low,legend_high);

%% change beta
d_low = @(x,y) psabr(t,T,f,x,alpha,y,beta-.2,nu,rho); % low density
d_high = @(x,y) psabr(t,T,f,x,alpha,y,beta+.2,nu,rho);% high density

y_low = density1d(F,d_low,low,up);
y_high = density1d(F,d_high,low,up);
legend_low = 'Changing \beta low';
legend_high = 'Changing \beta high';

createfigure_density(xret,y_base',y_low',y_high',title_plot,legend_base,legend_low,legend_high);

y2_low = density2d(F,A,d_low);
y2_high = density2d(F,A,d_high);

create_density_2d(F',A,y2_low', y2_base', y2_high', legend_low, legend_base, legend_high, title_plot);

%% change nu
d_low = @(x,y) psabr(t,T,f,x,alpha,y,beta,nu-.1,rho); % low density
d_high = @(x,y) psabr(t,T,f,x,alpha,y,beta,nu+.1,rho);% high density

y_low = density1d(F,d_low,low,up);
y_high = density1d(F,d_high,low,up);
legend_low = 'Changing \nu low';
legend_high = 'Changing \nu high';

createfigure_density(xret,y_base',y_low',y_high',title_plot,legend_base,legend_low,legend_high);

%% change rho
d_low = @(x,y) psabr(t,T,f,x,alpha,y,beta,nu,rho-.6); % low density
d_high = @(x,y) psabr(t,T,f,x,alpha,y,beta,nu,rho+.6);% high density

y_low = density1d(F,d_low,low,up);
y_high = density1d(F,d_high,low,up);
legend_low = 'Changing \rho low';
legend_high = 'Changing \rho high';

createfigure_density(xret,y_base',y_low',y_high',title_plot,legend_base,legend_low,legend_high);