function [lp,lj,tr] = lineline(pa,pb,pc,pd,varargin)
%LINELINE intersection between lines in d-dimensional space.
%   [LP,LI] = LINELINE(PA,PB,PC,PD) finds intersections bet-
%   ween line segments in d-dimensions. Lines are specified
%   as a set of endpoints [PA,PB] and [PC,PD] where PA, PB,
%   PC and PD are NL-by-ND arrays of coordinates, where ND
%   is the number of dimensions.
%
%   A set of intersecting lines from [PA,PB] is returned for
%   each query line in [PC,PB], such that the II-th query
%   line is associated with the lines LI(LP(II,1):LP(II,2)).
%   Lines without intersections have LP(II,1) == 0.
%
%   [LP,LI,TR] = LINELINE(PA,PB,PC,PD) additionally returns
%   the supporting aabb-tree used internally to compute the
%   query. If the underlying collection [PA,PB] is static,
%   the  tree TR may be recycled for subsequent calls, using
%   [LP,LI,TR] = FINDLINE(PA,PB,PC,PD,TR). This syntax may
%   lead to improved performance, especially when the number
%   of lines is large w.r.t. the number of query lines. Note
%   that in such cases the distribution of underlying lines
%   is NOT permitted to change between calls, or erroneous
%   results may be returned. Additional parameters used to
%   govern the creation of the underlying aabb-tree may be
%   passed via [...] = LINELINE(...,TR,OP). See MAKETREE for
%   additional information.
%
%   See also MAKETREE, FINDTRIA, FINDBALL, FINDLINE

% Please see the following for additional information:
%
%   Darren Engwirda, "Locally-optimal Delaunay-refinement &
%   optimisation-based mesh generation". Ph.D. Thesis, Scho-
%   ol of Mathematics and Statistics, Univ. of Sydney, 2014:
%   http://hdl.handle.net/2123/13148

%-----------------------------------------------------------
%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 10/10/2017
%-----------------------------------------------------------

    lp = []; lj = []; tr = []; op = [];

%---------------------------------------------- basic checks
    if (nargin < +4 || nargin > +6)
        error('lineline:incorrectNumInputs', ...
            'Incorrect number of inputs.');
    end

    if (nargin >= +5), tr = varargin{1}; end
    if (nargin >= +6), op = varargin{2}; end

%------------------------------ quick return on empty inputs
    if (isempty(pa)), return ; end
    if (isempty(pb)), return ; end
    if (isempty(pc)), return ; end
    if (isempty(pd)), return ; end

%---------------------------------------------- basic checks
    if (~isnumeric(pa) || ~isnumeric(pb) || ...
        ~isnumeric(pc) || ~isnumeric(pd) )
        error('lineline:incorrectInputClass', ...
            'Incorrect input class.') ;
    end

    if (ndims(pa) ~= +2 || size(pa,2) < +2 || ...
        ndims(pb) ~= +2 || size(pb,2) < +2 || ...
        ndims(pc) ~= +2 || size(pc,2) < +2 || ...
        ndims(pd) ~= +2 || size(pd,2) < +2 )
        error('lineline:incorrectDimensions', ...
            'Incorrect input dimensions.');
    end
    if (size(pa,1) ~= size(pb,1) || ...
        size(pc,1) ~= size(pd,1) )
        error('lineline:incorrectDimensions', ...
            'Incorrect input dimensions.');
    end

    if (~isempty(tr) && ~isstruct(tr) )
        error('lineline:incorrectInputClass', ...
            'Incorrect input class.') ;
    end
    if (~isempty(op) && ~isstruct(op) )
        error('lineline:incorrectInputClass', ...
            'Incorrect input class.') ;
    end

    nd = size(pa,2);
    nl = size(pa,1);
    ml = size(pc,1);

    if (isempty(tr))
%------------------------------ compute aabb-tree for d-line
    ab = zeros(nl,nd*+2) ;
    for ax = +1:nd            % compute aabb's
    ab(:,ax+nd*0) = min(pa(:,ax), ...
                        pb(:,ax)) ;
    ab(:,ax+nd*1) = max(pa(:,ax), ...
                        pb(:,ax)) ;
    end
    tr = maketree(ab,op) ;

    end

%------------------------------ compute tree-to-vert mapping
    ab = zeros(ml,nd*+2) ;
    for ax = +1:nd            % compute aabb's
    ab(:,ax+nd*0) = min(pc(:,ax), ...
                        pd(:,ax)) ;
    ab(:,ax+nd*1) = max(pc(:,ax), ...
                        pd(:,ax)) ;
    end
    tm = maprect (tr,ab) ;

%------------------------------ compute line-to-line queries
   [li,ip,lj] = queryset( ...
    tr,tm,@linekern,pc,pd,pa,pb) ;

%------------------------------ re-index onto full obj. list
    lp = zeros(size(pc,1),2) ;
    lp( :,2) = -1 ;

    if (isempty(li)), return ; end

    lp(li,:) = ip ;

end

function [i1,i2] = linekern(l1,l2,pa,pb,pc,pd)
%LINEKERN d-dim. line//line intersection kernel routine.

        m1 = length(l1) ;
        m2 = length(l2) ;

    %-------------------------- push line/vert onto n*m tile
        l1 = l1.' ;

        l1 = l1(ones(m2,1),:) ;
        l1 = l1(:);
        l2 = l2(:,ones(1,m1)) ;
        l2 = l2(:);

    %-------------------------- compute O(n*m) intersections
       [ok,tp,tq] = linenear( ...
           pa(l1,:),pb(l1,:), ...
           pc(l2,:),pd(l2,:)) ;

        rt = +1.+eps;

        ix = abs(tp) <= +rt & ...
             abs(tq) <= +rt ;

        ix(~ok) = false ;

        i1 = l1(ix) ;
        i2 = l2(ix) ;

end


function ins=ins_mech(ins,imu)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INS mechanization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2020-2025, by Kai Chen, All rights reserved.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent old_dw old_dv

ins.time=imu.time;
if isempty(old_dw)
    old_dw=zeros(3,1);
end
if isempty(old_dv)
    old_dv=zeros(3,1);
end

% correct bias and scaling factor errors
dw0 = imu.dw';
dv0 = imu.dv';
dw = ins.Kg*dw0-ins.bg*ins.nt;
dv = ins.Ka*dv0-ins.ba*ins.nt;

% extrapolate velocity and position
vel_mid = ins.vel+ins.acc*(ins.nt/2);
pos_mid = ins.pos+ins.Mpv*(ins.vel+vel_mid)/2*ins.nt;

% update the related parameters
ins.eth = earth_update(pos_mid,vel_mid);
ins.wib = dw/ins.nt;
ins.fb  = dv/ins.nt;
ins.fn  = ins.Cnb*ins.fb;
ins.web = ins.wib-ins.Cnb'*ins.eth.wnie;

% update velocity
dv_rot  = 0.5*cross(dw0,dv0);
dv_scul = 1/12*(cross(old_dw,dv0)+cross(old_dv,dw0));
dv_sf   = (eye(3)-0.5*ins.nt*askew(ins.eth.wnin))*ins.Cnb*dv + ins.Cnb*(dv_rot+dv_scul);
dv_cor  = ins.eth.gcc*ins.nt;
vel_new = ins.vel+dv_sf+dv_cor;

% update position
ins.Mpv(2) = ins.eth.Mpv2;
ins.Mpv(4) = ins.eth.Mpv4;
pos_new    = ins.pos + ins.Mpv*(ins.vel+vel_new)/2*ins.nt;

% update attitude
dw_cone  = 1/12*cross(old_dw,dw0);
phi_b_ib = dw+dw_cone;
phi_n_in = ins.eth.wnin*ins.nt;
Cbb = rvec2mat(phi_b_ib);
Cnn = rvec2mat(phi_n_in)';
Cnb_new = Cnn*ins.Cnb*Cbb;
att_new = Cnb2att(Cnb_new);

% update INS result
ins.Cnb = Cnb_new;
ins.att = att_new;
ins.vel = vel_new;
ins.pos = pos_new;
ins.x = [ins.att;ins.vel;ins.pos;ins.bg;ins.ba];

old_dw=dw0;
old_dv=dv0;

return


function b = zhpsl ( ap, n, ipvt, b )

%*****************************************************************************80
%
%% ZHPSL solves a complex hermitian system factored by ZHPFA.
%
%  Discussion:
%
%    A division by zero may occur if ZHPCO set RCOND to 0.0
%    or ZHPFA set INFO nonzero.
%
%    To compute
%
%      inverse ( A ) * C
%
%    where C is a matrix with P columns
%
%      call zhpfa(ap,n,ipvt,info)
%
%      if ( info == 0 )
%        do j = 1, p
%          call zhpsl(ap,n,ipvt,c(1,j))
%        end do
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex AP(N*(N+1)/2), the output from ZHPFA.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer IPVT(N), the pivot vector from ZHPFA.
%
%    Input, complex B(N), the right hand side.
%
%    Output, complex B(N), the solution.
%

%
%  Loop backward applying the transformations and inverse ( D ) to B.
%
  k = n;
  ik = ( n * ( n - 1 ) ) / 2;

  while ( 0 < k )

    kk = ik + k;
%
%  1 x 1 pivot block.
%
    if ( 0 <= ipvt(k) )

      if ( k ~= 1 )

        kp = ipvt(k);

        if ( kp ~= k )
          t     = b(k);
          b(k)  = b(kp);
          b(kp) = t;
        end

        b(1:k-1) = b(1:k-1) + b(k) * ap(ik+1:ik+k-1);

      end
%
%  Apply D inverse.
%
      b(k) = b(k) / ap(kk);
      k = k - 1;
      ik = ik - k;

    else
%
%  2 x 2 pivot block.
%
      ikm1 = ik - ( k - 1 );

      if ( k ~= 2 )

        kp = abs ( ipvt(k) );

        if ( kp ~= k - 1 )
          t      = b(k-1);
          b(k-1) = b(kp);
          b(kp)  = t;
        end

        b(1:k-2) = b(1:k-2) + b(k)   * ap(ik+1:ik+k-2);
        b(1:k-2) = b(1:k-2) + b(k-1) * ap(ikm1+1:ikm1+k-2);

      end
%
%  Apply D inverse.
%
      km1k = ik + k - 1;
      kk = ik + k;
      ak = ap(kk) / conj ( ap(km1k) );
      km1km1 = ikm1 + k - 1;
      akm1 = ap(km1km1) / ap(km1k);
      bk = b(k) / conj ( ap(km1k) );
      bkm1 = b(k-1) / ap(km1k);
      denom = ak * akm1 - 1.0;
      b(k) = ( akm1 * bk - bkm1 ) / denom;
      b(k-1) = ( ak * bkm1 - bk ) / denom;
      k = k - 2;
      ik = ik - ( k + 1 ) - k;

    end

  end
%
%  Loop forward applying the transformations.
%
  k = 1;
  ik = 0;

  while ( k <= n )
%
%  1 x 1 pivot block.
%
    if ( 0 <= ipvt(k) )

      if ( k ~= 1 )

        b(k) = b(k) + conj ( ap(ik+1:ik+k-1) ) * transpose ( b(1:k-1) );
        kp = ipvt(k);

        if ( kp ~= k )
          t     = b(k);
          b(k)  = b(kp);
          b(kp) = t;
        end

      end

      ik = ik + k;
      k = k + 1;
%
%  2 x 2 pivot block.
%
    else

      if ( k ~= 1 )

        b(k) = b(k) + conj ( ap(ik+1:ik+k-1) ) * transpose ( b(1:k-1) );
        ikp1 = ik + k;
        b(k+1) = b(k+1) ...
          + conj ( ap(ikp1+1:ikp1+k-1) ) * transpose ( b(1:k-1) );
        kp = abs ( ipvt(k) );

        if ( kp ~= k )
          t     = b(k);
          b(k)  = b(kp);
          b(kp) = t;
        end

      end

      ik = ik + k + k + 1;
      k = k + 2;

    end

  end

  return
end


function [gx,dG_dX,dG_dPhi] = g_fullDCM4fmri(Xt,Phi,ut,inG)
% full DCM observation function (embedding dynamic Ballon model)

% function [gx,dG_dX,dG_dPhi] = g_fullDCM4fmri(Xt,Phi,ut,inG)
% This function evaluates the observation function of the neuronal level in
% DCM for fMRI. Note that this "generalized" observation function includes
% the HRF convolution model (balloon model).

persistent xh dxhdx dxhdp ph ti ko
% These variables are described bellow:
%   - xh{i}: current hemodynamic states for region i.
%   - dxhdx{i}: derivatives of the hemodynamic states of region i w.r.t.
%   neuronal state of region i.
%   - dxhdp{i}: derivatives of the hemodynamic states of region i w.r.t.
%   hemodynamic parameters of region i.
%   - ph: the last remembered hemodynamic parameters. This is only used
%   when this function is called from f_fullDCM4fmri.m. It is updated each
%   time this function is called outside f_fullDCM4fmri.m.
%   - ti: the index within the time series. This is used to reinitialize
%   the persistent variables.
% These are reinitialized at the beginning of the time series.

try % just to get lastly updated phi

    inG.getPhi;
    ph = Phi;

catch

    n = size(Xt,1);

    % Check whether the system is at initial state
    if isempty(xh)
        ti = 0;
        xh = kron(ones(n,1),[0;0;0;0]);
        dxhdx = zeros(n,n*4);
        dxhdp = zeros(size(inG.Phi,1),n*4);
        ko = kron(eye(n),[1 0 0 0]);
    end

    if isempty(Phi)    %- Function is called from f_fullDCM4fmri

        % Get current observation parameters mode
        if ~isempty(ph)
            Phi = ph;
        else % only the first iteration
            Phi = inG.Phi;
        end

        %- hemodynamic response convolution operation
        [fx,dfdxh,dfdp] = f_HRF2(xh,Phi,Xt,inG);

        xh = fx;
        % update gradients
        dxhdx = dxhdx*dfdxh + ko;
        dxhdp = dfdp + dxhdp*dfdxh;

    else    %- Function is called outside of f_fullDCM4fmri

        % Update persistent observation parameters
        ph = Phi;
        ti = ti+1;

        %- Get predicted observations and gradients
        [gx,dgdxh,dgdp] = g_HRF3(xh,Phi,ut,inG);
        %
        %     dgdxh = numericDiff('g_HRF',1,xh,Phi,ut,inG);
        %     dgdp = numericDiff('g_HRF',2,xh,Phi,ut,inG);

        dG_dX = dxhdx*dgdxh;
        dG_dPhi = dgdp + dxhdp*dgdxh;
        %         % empty gradients w.r.t. states
        %         dxhdx = zeros(n,n*4);

    end

    if ti==inG.n_t
        xh = [];
    end
end


% op_CSIFOurierTransform.m
% Brenden Kadota, SunnyBrook Hosptial 2021.
%
% Does spectral and spatial fourier transform on MRSI structure. The fast
% fourier transform is done on the spectral dimension. If the spatial k
% space is cartesian, the fast fourier transform is applied.
% USAGE:
% out = op_CSIFourierTransform(out, spatialFT, spectralFT, isCartesian)
%
%
% input:    in         = Twix object of CSI data
%           spatialFT  = Boolean flag (1 or 0) to compute fourier
%                        transfrom along spatial dimension
%           spectralFT = Same as above but for the spectral dimension
%           isCartesian= boolean flag for fft or slow fourier tranform alon
%                        the spatial dimension
%
% output:   in         = Twix object with new out.specs field of the
%                         fourier transformed data
function MRSIStruct = op_CSIFourierTransform(MRSIStruct, k_file, fourierTransform)
    arguments
        MRSIStruct (1,1) struct
        k_file (1,:) char {mustBeFileorDefault} = ""
        fourierTransform.spatial (1,1) logical {mustHaveSpatial(fourierTransform.spatial, MRSIStruct)}
        fourierTransform.spectral (1,1)  logical {mustHaveSpectral(fourierTransform.spectral, MRSIStruct)}
    end
    % set default values for spatial and spectral fourier transform
    fourierTransform = setDefaultFlags(fourierTransform, MRSIStruct);

    % spatial dimension fourier transform
    if (fourierTransform.spatial == 1)
        disp('Calculating spatial dimension');
        if(k_file == "")
            %applying the fast fourier transform if k space is cartesian
            MRSIStruct = applyFastFourierTransformSpatial(MRSIStruct);
        else
            [kTrajectory, numSpatial, numSpectral] = readKSpaceFile(k_file, MRSIStruct);
            MRSIStruct = slowFourierTransfrom(MRSIStruct, kTrajectory, numSpatial, numSpectral);
            MRSIStruct = calculateSpectralValues(MRSIStruct, numSpatial, numSpectral);
        end

        MRSIStruct = setFlags(MRSIStruct, 'spatialFT', true);
        %waterRemoved = op_CSIRemoveWater(MRSIStruct);
    end

    % spectral fourier transform
    if (fourierTransform.spectral)
        % fast fourier transform along the time dimension
        disp('Calculating spectral dimension');
        MRSIStruct = fastFourierTransformTime(MRSIStruct);
    end

end

function sft2_Oper = sft2_Operator(InTraj, OutTraj, Ift_flag)
    %
    % sft2 Perform 2-dimensional slow Fourier transform
    %
    % This function was written by Bernhard Strasser, April 2018.
    %
    %
    % The function performs a slow Fourier transform in two spatial dimensions by using the definition of the Fourier Transform
    % FT(f)(a_j) = sum(k=-N/2;N/2-1) f(x_k)exp(-2pi i x_k a_j)
    % (For even N. For odd, change sum slightly)
    %
    %
    % sft2Operator = sft2(A,Ifft_flag)
    %
    % Input:
    % -         Ift_flag                    ...     Flag of 0 or 1 determining if an inverse Fourier transform or normal should be performed.
    % -         InputSize                   ...     Size of Input data.
    %
    % Output:
    % -         sft2Operator                ...     Fourier transformation matrix which can be applied to the data by Out = sft2Operator*In
    %
    %
    % Feel free to change/reuse/copy the function.
    % If you want to create new versions, don't degrade the options of the function, unless you think the kicked out option is totally useless.
    % Easier ways to achieve the same result & improvement of the program or the programming style are always welcome!
    % File dependancy: ?
    % 0. Preparations

    % Define Exponent of exp
    if(~Ift_flag)
        Expy = -2*pi*1i;
    else
        Expy = 2*pi*1i;
    end

    % Define Output Size
    NOut = size(OutTraj,1);

    % 1. FT

    sft2_Oper = zeros([size(OutTraj,1) size(InTraj,1)]);
    for j=1:NOut
        sft2_Oper(j,:) = exp(Expy*(OutTraj(j,1)*InTraj(:,1) ...
            + OutTraj(j,2)*InTraj(:,2)));
    end

    if(Ift_flag)
        sft2_Oper = sft2_Oper / (size(InTraj,1));
    end

end

function mustHaveSpatial(a, in)
    if isfield(in, 'spatialFT') && (a == true && in.spatialFT == 1)
        eidType = 'spatialTransform:noSpatialTransform';
        msgType = 'Spaitial Fourier Transform already done!';
        throwAsCaller(MException(eidType,msgType))
    end
end

function mustHaveSpectral(a, in)
    if isfield(in, 'spectral') && (a == true && in.spectral == 1)
        eidType = 'spectralTransform:noSpectralTransform';
        msgType = 'Spectral Fourier Transform already done!';
        throwAsCaller(MException(eidType,msgType))
    end
end

function mustBeFileorDefault(file)
    if ~isfile(file) && ~strcmp(file, "")
        eidType = 'mustBeFile:notAFile';
        msgType = 'Invalid value fo k_file, must be a file';
        throwAsCaller(MException(eidType,msgType))
    end
end

function fourierTransform = setDefaultFlags(fourierTransform, in)
    if(~isfield(fourierTransform, 'spatial'))
        if(in.flags.spatialFT)
            fourierTransform.spatial = 0;
        else
            fourierTransform.spatial = 1;
        end
    end
    if(~isfield(fourierTransform, 'spectral'))
        if(in.flags.spectralFT)
            fourierTransform.spectral = 0;
        else
            fourierTransform.spectral = 1;
        end
    end
end

function MRSIStruct = applyFastFourierTransformSpatial(MRSIStruct)
    disp('Applying fast fourier transform');
    %apply half pixel shift
    MRSIStruct = halfPixelShift(MRSIStruct);
    %get data
    data = getData(MRSIStruct);
    xDimension = getDimension(MRSIStruct, 'kx');
    if(mod(getSizeFromDimensions(MRSIStruct, {'kx'}), 2) == 1)
        data = circshift(data, 1, xDimension);
    end
    data = fftshift(fft(fftshift(data, xDimension), [], xDimension), xDimension);

    yDimension = getDimension(MRSIStruct, 'ky');
    if(mod(getSizeFromDimensions(MRSIStruct,{'ky'}), 2) == 1)
        data = circshift(data, 1, yDimension);
    end
    data = fftshift(fft(fftshift(data, yDimension), [], yDimension), yDimension);
    MRSIStruct = setData(MRSIStruct, data);

    MRSIStruct = setDimension(MRSIStruct, 'x', getDimension(MRSIStruct, 'kx'));
    MRSIStruct = setDimension(MRSIStruct, 'y', getDimension(MRSIStruct, 'ky'));
    MRSIStruct = setDimension(MRSIStruct, 'z', getDimension(MRSIStruct, 'kz'));
    MRSIStruct = setDimension(MRSIStruct, 'kx', 0);
    MRSIStruct = setDimension(MRSIStruct, 'ky', 0);
    MRSIStruct = setDimension(MRSIStruct, 'kz', 0);
end

function MRSIStruct = slowFourierTransfrom(MRSIStruct, kTrajectory, numSpatial, numSpectral)

    [xCoordinates, yCoordinates, imageTrajectory] = getImageTrajectory(MRSIStruct);

    %creating fourier transform operator for spatial domain
    sftOperator = sft2_Operator(kTrajectory, imageTrajectory, 0);

    %permute so first 3 dimensions are x, y and t
    [MRSIStruct, prevPermute, prevSize] = reshapeDimensions(MRSIStruct, {'t', 'ky'});
    data = getData(MRSIStruct);

    % apply slow fourier transform matrix to data
    image = applySlowFourierTranformMatrix(MRSIStruct, sftOperator, data, numSpectral, numSpatial);
    MRSIStruct = setData(MRSIStruct, image);

    kyDimension = getDimension(MRSIStruct, 'ky');
    prevPermute = removeDimPrevPermute(prevPermute, kyDimension);
    prevPermute = addDimPrevPermute(prevPermute, 'y', kyDimension);
    prevPermute = addDimPrevPermute(prevPermute, 'x', kyDimension + 1);

    prevSize(1) = numSpectral;
    prevSize(2) = length(yCoordinates);
    prevSize = [prevSize(1:2), length(xCoordinates), prevSize(3:end)];
    MRSIStruct = reshapeBack(MRSIStruct, prevPermute, prevSize);
end

function image = applySlowFourierTranformMatrix(MRSIStruct, sftOperator, data, numSpectral, numSpatial)
    yLength = length(getCoordinates(MRSIStruct, 'y'));
    xLength = length(getCoordinates(MRSIStruct, 'x'));

    %image dimensions after fourier tranform is time, y, x, and extras.
    imageDimensions = [numSpectral, yLength, xLength, ...
                                        getSizeFromDimensions(MRSIStruct, {'extras'})];
    image = zeros(imageDimensions);

    for iPoint = 1:numSpectral
        startingPoint = (iPoint - 1)*numSpatial + 1;
        endingPoint = iPoint * numSpatial;
        kSpaceSlice = data(startingPoint:endingPoint, :, :);
        vectorizedSlice = reshape(kSpaceSlice, [], size(kSpaceSlice,3));
        ftVectorizedSlice = sftOperator*vectorizedSlice;

        imageSlice = reshape(ftVectorizedSlice, [yLength, xLength, size(ftVectorizedSlice,2)]);
        image(iPoint, :, :, :) = imageSlice;
    end
end

function [xCoordinates, yCoordinates, imageTrajectory] = getImageTrajectory(MRSIStruct)
    xCoordinates = getCoordinates(MRSIStruct, 'x');
    yCoordinates = getCoordinates(MRSIStruct, 'y');

    %applying the slow fourier transform if the k space is non cartesian
    [x, y] = meshgrid(xCoordinates, yCoordinates);
    imageTrajectory = [x(:), y(:)];
end

function MRSIStruct = fastFourierTransformTime(MRSIStruct)
    data = getData(MRSIStruct);
    timeDimension = getDimension(MRSIStruct, 't');
    %fourier transform in the spectral domain
    data = fftshift(fft(data, [], timeDimension), timeDimension);

    MRSIStruct = setData(MRSIStruct, data);
    ppm = calculatePPM(MRSIStruct);

    if strcmp(MRSIStruct.nucleus,'1H')
        ppm = ppm + 4.65;
    end

    MRSIStruct = setPPM(MRSIStruct, ppm);
    %flip ppm
    MRSIStruct = setFlags(MRSIStruct, 'spectralFT', true);
end

function ppm = calculatePPM(MRSIStruct)
    %get gamma and nucleus
    gamma = MRSIStruct.gamma;

    %lower bounds of frequency
    spectralWidth = getSpectralWidth(MRSIStruct);
    timeSize = getSizeFromDimensions(MRSIStruct, {'t'});

    step = spectralWidth/timeSize;
    lowerBound = -spectralWidth/2 + step/2;
    %upper bounds of frequency
    upperBound = spectralWidth/2 - step/2;

    %calculating the frequency
    frequency=lowerBound:step:upperBound;

    %calculating the ppm
    ppm=-frequency/(MRSIStruct.Bo*gamma);
end

function MRSIStruct = calculateSpectralValues(MRSIStruct, numSpatial, numSpectral)
    spectralDwellTime = calculateSpectralDwellTime(MRSIStruct, numSpatial);
    spectralWidth = 1/spectralDwellTime;
    spectralTime = calculateSpectralTime(spectralDwellTime, numSpectral);

    MRSIStruct = setSpectralWidth(MRSIStruct, spectralWidth);
    MRSIStruct = setSpectralDwellTime(MRSIStruct, spectralDwellTime);
    MRSIStruct = setSpectralTime(MRSIStruct, spectralTime);
end

function spectralDwellTime = calculateSpectralDwellTime(MRSIStruct, spatialPoints)
    adcDwellTime = getAdcDwellTime(MRSIStruct);
    spectralDwellTime = spatialPoints * adcDwellTime;
end

function spectralTime = calculateSpectralTime(spectralDwellTime, spatialPoints)
    spectralTime = 0:spectralDwellTime:spectralDwellTime*(spatialPoints - 1);
end

function MRSIStruct = halfPixelShift(MRSIStruct)
    kx = getCoordinates(MRSIStruct, 'kx');
    ky = getCoordinates(MRSIStruct, 'ky');
    halfPixelX = getVoxSize(MRSIStruct, 'x')/2;
    halfPixelY = getVoxSize(MRSIStruct, 'y')/2;
    kShift = kx*halfPixelX + ky'*halfPixelY;

    [MRSIStruct, prevPermute, prevSize] = reshapeDimensions(MRSIStruct, {'ky', 'kx'});
    data = getData(MRSIStruct);
    data = data .* exp(-1i*2*pi*kShift);
    MRSIStruct = setData(MRSIStruct, data);
    MRSIStruct = reshapeBack(MRSIStruct, prevPermute, prevSize);
end


function res = eq(a,b)
%EQ           Implements  a == b  elementwise for intervals a and b
%
% a and b must be either both real or both complex
%

% written  10/16/98     S.M. Rump
% modified 11/30/98     S.M. Rump
% modified 06/24/98     S.M. Rump  multi-dimensional arrays
% modified 09/01/00     S.M. Rump  result array
%                                  comparison real/complex
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  if ~isa(a,'intval')
    if b.complex
      res = ( b.mid==a ) & ( b.rad==0 );
    else
      res = ( b.inf==a ) & ( b.sup==a );
    end
    return
  end

  if ~isa(b,'intval')
    if a.complex
      res = ( a.mid==b ) & ( a.rad==0 );
    else
      res = ( a.inf==b ) & ( a.sup==b );
    end
    return
  end

  if ( a.complex ~= b.complex )
    if a.complex
      error('intval comparison == of complex and real')
    else
      error('intval comparison == of real and complex')
    end
  end

  if a.complex
    res = (a.mid==b.mid) & (a.rad==b.rad);
  else
    res = (a.inf==b.inf) & (a.sup==b.sup);
  end


function CA184rule()
n=1000;
X=rand(1,n)>0.9;
X(n+1)=X(1);
Timestep=500;
data=zeros(Timestep,n);
Y=zeros(1,n);
for j=1:Timestep
    for i=1:n
        if i~=1
            if X(i-1)==1&&X(i)==0
                Y(i)=1;
           % elseif X(i)==1&&X(i+1)==0
              %  Y(i)=0;
            elseif X(i)==1&&X(i+1)==1
                Y(i)=1;
            else Y(i)=0;
            end
        else
            if X(n)==1&&X(i)==0
                Y(i)=1;
            %elseif X(i)==1&&X(i+1)==0
               % Y(i)=0;
            elseif X(i)==1&&X(i+1)==1
                Y(i)=1;
            else Y(i)=0;
            end
        end
    end
   X=Y;
   X(n+1)=Y(1);
   data(j,:)=Y;
end
imshow(data);
axis on;


function [reg_corner,rho,eta,reg_param] = l_curve(U,sm,b,method,L,V)
%L_CURVE Plot the L-curve and find its "corner".
%
% [reg_corner,rho,eta,reg_param] =
%                  l_curve(U,s,b,method)
%                  l_curve(U,sm,b,method)  ,  sm = [sigma,mu]
%                  l_curve(U,s,b,method,L,V)
%
% Plots the L-shaped curve of eta, the solution norm || x || or
% semi-norm || L x ||, as a function of rho, the residual norm
% || A x - b ||, for the following methods:
%    method = 'Tikh'  : Tikhonov regularization   (solid line )
%    method = 'tsvd'  : truncated SVD or GSVD     (o markers  )
%    method = 'dsvd'  : damped SVD or GSVD        (dotted line)
%    method = 'mtsvd' : modified TSVD             (x markers  )
% The corresponding reg. parameters are returned in reg_param.  If no
% method is specified then 'Tikh' is default.  For other methods use plot_lc.
%
% Note that 'Tikh', 'tsvd' and 'dsvd' require either U and s (standard-
% form regularization) or U and sm (general-form regularization), while
% 'mtvsd' requires U and s as well as L and V.
%
% If any output arguments are specified, then the corner of the L-curve
% is identified and the corresponding reg. parameter reg_corner is
% returned.  Use routine l_corner if an upper bound on eta is required.

% Reference: P. C. Hansen & D. P. O'Leary, "The use of the L-curve in
% the regularization of discrete ill-posed problems",  SIAM J. Sci.
% Comput. 14 (1993), pp. 1487-1503.

% Per Christian Hansen, IMM, July 26, 2007.

% Set defaults.
if (nargin==3), method='Tikh'; end  % Tikhonov reg. is default.
npoints = 200;  % Number of points on the L-curve for Tikh and dsvd.
smin_ratio = 16*eps;  % Smallest regularization parameter.

% Initialization.
[m,n] = size(U); [p,ps] = size(sm);
if (nargout > 0), locate = 1; else locate = 0; end
beta = U'*b; beta2 = norm(b)^2 - norm(beta)^2;
if (ps==1)
  s = sm; beta = beta(1:p);
else
  s = sm(p:-1:1,1)./sm(p:-1:1,2); beta = beta(p:-1:1);
end
xi = beta(1:p)./s;

if (strncmp(method,'Tikh',4) | strncmp(method,'tikh',4))

  eta = zeros(npoints,1); rho = eta; reg_param = eta; s2 = s.^2;
  reg_param(npoints) = max([s(p),s(1)*smin_ratio]);
  ratio = (s(1)/reg_param(npoints))^(1/(npoints-1));
  for i=npoints-1:-1:1, reg_param(i) = ratio*reg_param(i+1); end
  for i=1:npoints
    f = s2./(s2 + reg_param(i)^2);
    eta(i) = norm(f.*xi);
    rho(i) = norm((1-f).*beta(1:p));
  end
  if (m > n & beta2 > 0), rho = sqrt(rho.^2 + beta2); end
  marker = '-'; txt = 'Tikh.';

elseif (strncmp(method,'tsvd',4) | strncmp(method,'tgsv',4))

  eta = zeros(p,1); rho = eta;
  eta(1) = abs(xi(1))^2;
  for k=2:p, eta(k) = eta(k-1) + abs(xi(k))^2; end
  eta = sqrt(eta);
  if (m > n)
    if (beta2 > 0), rho(p) = beta2; else rho(p) = eps^2; end
  else
    rho(p) = eps^2;
  end
  for k=p-1:-1:1, rho(k) = rho(k+1) + abs(beta(k+1))^2; end
  rho = sqrt(rho);
  reg_param = (1:p)'; marker = 'o';
  if (ps==1)
    U = U(:,1:p); txt = 'TSVD';
  else
    U = U(:,1:p); txt = 'TGSVD';
  end

elseif (strncmp(method,'dsvd',4) | strncmp(method,'dgsv',4))

  eta = zeros(npoints,1); rho = eta; reg_param = eta;
  reg_param(npoints) = max([s(p),s(1)*smin_ratio]);
  ratio = (s(1)/reg_param(npoints))^(1/(npoints-1));
  for i=npoints-1:-1:1, reg_param(i) = ratio*reg_param(i+1); end
  for i=1:npoints
    f = s./(s + reg_param(i));
    eta(i) = norm(f.*xi);
    rho(i) = norm((1-f).*beta(1:p));
  end
  if (m > n & beta2 > 0), rho = sqrt(rho.^2 + beta2); end
  marker = ':';
  if (ps==1), txt = 'DSVD'; else txt = 'DGSVD'; end

elseif (strncmp(method,'mtsv',4))

  if (nargin~=6)
    error('The matrices L and V must also be specified')
  end
  [p,n] = size(L); rho = zeros(p,1); eta = rho;
  [Q,R] = qr(L*V(:,n:-1:n-p),0);
  for i=1:p
    k = n-p+i;
    Lxk = L*V(:,1:k)*xi(1:k);
    zk = R(1:n-k,1:n-k)\(Q(:,1:n-k)'*Lxk); zk = zk(n-k:-1:1);
    eta(i) = norm(Q(:,n-k+1:p)'*Lxk);
    if (i < p)
      rho(i) = norm(beta(k+1:n) + s(k+1:n).*zk);
    else
      rho(i) = eps;
    end
  end
  if (m > n & beta2 > 0), rho = sqrt(rho.^2 + beta2); end
  reg_param = (n-p+1:n)'; txt = 'MTSVD';
  U = U(:,reg_param); sm = sm(reg_param);
  marker = 'x'; ps = 2;  % General form regularization.

else
  error('Illegal method')
end

% Locate the "corner" of the L-curve, if required.
if (locate)
  [reg_corner,rho_c,eta_c] = l_corner(rho,eta,reg_param,U,sm,b,method);
end

% Make plot.
plot_lc(rho,eta,marker,ps,reg_param);
if locate
  ax = axis;
  HoldState = ishold; hold on;
  loglog([min(rho)/100,rho_c],[eta_c,eta_c],':r',...
         [rho_c,rho_c],[min(eta)/100,eta_c],':r')
  title(['L-curve, ',txt,' corner at ',num2str(reg_corner)]);
  axis(ax)
  if (~HoldState), hold off; end
end


function M=link_communities(W,type_clustering)
%LINK_COMMUNITIES     Optimal overlapping community structure
%
%   M = link_communities(W)
%   M = link_communities(W,'complete');
%
%   The optimal community structure is a subdivision of the network into
%   groups of nodes which have a high number of within-group connections
%   and a low number of between group connections.
%
%   This algorithm uncovers overlapping community structure via
%   hierarchical clustering of network links. This algorith is generalized
%   for weighted/directed/fully-connected networks.
%
%   Input:      W,                  directed (weighted or binary) connection matrix.
%               type_clustering,    type of hierarchical clustering (optional)
%                                       'single'        single-linkage (default)
%                                       'complete'      complete-linkage
%
%   Output:     M,                  nodal community-affiliation matrix
%                                   binary matrix of size CxN [communities x nodes]
%
%   NB: The algorithm can be slow and memory intensive.
%
%   Reference: Ahn, Bagrow and Lehmann (2010) Nature 466, 761–764.
%
%   Mika Rubinov, U Cambridge, 2014-2015

%% initialize

n=size(W,1);                                                        % number of nodes
W(1:n+1:end)=0;
W=W./max(W(:));                                                     % normalize weights

if ~exist('type_clustering','var')
    type_clustering='single';
end

%% get node similarity

W(1:n+1:end) = ( sum(W)/sum(W~=0) + sum(W.')/sum(W.'~=0) )/2;       % mean weight on diagonal
No=sum(W.^2,2);                                                     % out-norm squared
Ni=sum(W.^2,1);                                                     % in-norm squared

Jo=zeros(n);                                                        % weighted in-Jaccard
Ji=zeros(n);                                                        % weighted ou-Jaccard
for b=1:n
    for c=1:n
        Do=W(b,:)*W(c,:).';
        Jo(b,c)=Do./(No(b)+No(c)-Do);

        Di=W(:,b).'*W(:,c);
        Ji(b,c)=Di./(Ni(b)+Ni(c)-Di);
    end
end

%% get link similarity

[A,B]=find( (W|W.') & triu(ones(n),1));
m=length(A);
Ln=zeros(m,2);                                                      % link nodes
Lw=zeros(m,1);                                                      % link weights
for i=1:m
    Ln(i,:) = [A(i) B(i)];                                          % link nodes
    Lw(i) = (W(A(i),B(i))+W(B(i),A(i)))/2;                          % link weight
end

ES=zeros(m,m,'single');                                             % link similarity
for i=1:m
    for j=1:m
        if      Ln(i,1)==Ln(j,1); a=Ln(i,1); b=Ln(i,2); c=Ln(j,2);
        elseif  Ln(i,1)==Ln(j,2); a=Ln(i,1); b=Ln(i,2); c=Ln(j,1);
        elseif  Ln(i,2)==Ln(j,1); a=Ln(i,2); b=Ln(i,1); c=Ln(j,2);
        elseif  Ln(i,2)==Ln(j,2); a=Ln(i,2); b=Ln(i,1); c=Ln(j,1);
        else    continue
        end

        ES(i,j) = (W(a,b)*W(a,c)*Ji(b,c) + W(b,a)*W(c,a)*Jo(b,c))/2;
    end
end
ES(1:m+1:end)=0;

%% perform hierarchical clustering

C=zeros(m,m,'single');                                              % community affiliation matrix
Nc=C; Mc=C; Dc=C;                                                   % communities nodes, links and density
U=1:m;                                                              % initial community assignments
C(1,:)=U;                                                           % as above, in the matrix

for i=1:m-1; fprintf('hierarchy%8d\n',i)                            % hierarchy level

    % compute densities
    for j=1:length(U)                                               % loop over communities
        idx = C(i,:)==U(j);                                         % get link indices
        links = sort(Lw(idx));                                      % sort link weights
        nodes = sort(reshape(Ln(idx,:),2*nnz(idx),1));
        nodes = nodes([true;nodes(2:end)~=nodes(1:end-1)]);         % get unique nodes

        nc = numel(nodes);                                          % community nodes
        mc = sum(links);                                            % community weights
        min_mc = sum(links(1:nc-1));                                % minimal weight
        dc = (mc - min_mc) / (nc.*(nc-1)/2 - min_mc);               % community density

        Nc(i,j)=nc;
        Mc(i,j)=mc;
        Dc(i,j)=dc;
    end

    % cluster
    C(i+1,:)=C(i,:);                                                % copy current partition
    [u1,u2]=find(ES(U,U)==max(max(ES(U,U))));                       % on this line MAXs MUST BE MAXs

    V=U(unique(sortrows(sort([u1 u2],2)),'rows'));                  % get unique links
    for j=1:size(V,1)
        switch type_clustering
            case 'single';      x = max(ES(V(j,:),:),[],1);         % max -> single linkage
            case 'complete';    x = min(ES(V(j,:),:),[],1);         % min -> complete linkage
            otherwise; error('Unknown clustering type.');
        end
        ES(V(j,:),:) = [x;x];                                       % assign distances to whole clusters
        ES(:,V(j,:)) = [x;x].';
        ES(V(j,1),V(j,1)) = 0;                                      % clear diagonal
        ES(V(j,2),V(j,2)) = 0;                                      % clear diagonal

        C(i+1,C(i+1,:)==V(j,2)) = V(j,1);                           % merge communities
        V(V==V(j,2)) = V(j,1);                                      % merge indices
    end

    U=unique(C(i+1,:));                                             % get unique communities
    if numel(U)==1
        break;
    end
end

%%

Dc(isnan(Dc))=0;
[~,i]=max(sum(Dc.*Mc,2));                                           % get maximal density

U=unique(C(i,:));                                                   % unique communities
M=zeros(1,n);                                                       % nodal affiliations
for j=1:length(U)
    M(j,unique( Ln(C(i,:)==U(j),:)) )=1;
end
M=M(sum(M,2)>2,:);

% M2=zeros(n);                                                      % two dimensional nodal affiliation
% for i=1:size(M,1);
%     M2=M2+(M(i,:).'*ones(1,n) & ones(n,1)*M(i,:));
% end


function [lambda_perc, alpha_rad] = calcWheelSlips(omega_rad, VehicleState, DeltaWheel_rad, tw_front_m, tw_rear_m, l_front_m, l_rear_m, tyreradius_front_m, tyreradius_rear_m, v_min)

% initialize variables
alpha_rad = zeros(4, 1);
v_ref = zeros(4, 1);
v_wheel = zeros(4, 1);

vxCG_mps = VehicleState(1);
vyCG_mps = VehicleState(2);
dPsi_radps = VehicleState(3);

% calculate chassis velocities at wheel centers projected to the ground
vx_fr = vxCG_mps + (dPsi_radps*tw_front_m*0.5);
vx_fl = vxCG_mps - (dPsi_radps*tw_front_m*0.5);
vx_rr = vxCG_mps + (dPsi_radps*tw_rear_m*0.5);
vx_rl = vxCG_mps - (dPsi_radps*tw_rear_m*0.5);
vy_fr = vyCG_mps + (dPsi_radps*l_front_m);
vy_fl = vyCG_mps + (dPsi_radps*l_front_m);
vy_rr = vyCG_mps - (dPsi_radps*l_rear_m);
vy_rl = vyCG_mps - (dPsi_radps*l_rear_m);
% calculate rotation matrix for front tires to transform to tire coordinates
R = [cos(-DeltaWheel_rad), -sin(-DeltaWheel_rad);...
  sin(-DeltaWheel_rad), cos(-DeltaWheel_rad)];
% transform to tire coordinates
vxT_fl = R(1, :)*[vx_fl; vy_fl];
vyT_fl = R(2, :)*[vx_fl; vy_fl];
vxT_fr = R(1, :)*[vx_fr; vy_fr];
vyT_fr = R(2, :)*[vx_fr; vy_fr];
% calculate tire slip angles
if(vxCG_mps > v_min)
  alpha_rad(1) = atan2(-vyT_fl, vxT_fl);
  alpha_rad(2) = atan2(-vyT_fr, vxT_fr);
  alpha_rad(3) = atan2(-vy_rl, vx_rl);
  alpha_rad(4) = atan2(-vy_rr, vx_rr);

    % calculate absolute reference velocities of tire
    v_ref(1) = vxT_fl;
    v_ref(2) = vxT_fr;
    v_ref(3) = vx_rl;
    v_ref(4) = vx_rr;
    % calculate wheel over ground velocities
    v_wheel(1:2) = tyreradius_front_m.*omega_rad(1:2);
    v_wheel(3:4) = tyreradius_rear_m.*omega_rad(3:4);
    % calculate tireslips
    lambda_perc = 100*(v_wheel-v_ref)./(max(v_min, v_ref));
else
  alpha_rad = zeros(4,1);
  lambda_perc = zeros(4, 1);
end


function DEM_demo_fMRI_HMM
% Demonstration of Hidden Markov models for fMRI
%__________________________________________________________________________
%  This demonstration routine illustrates the modelling of state
%  transitions generating resting state fMRI timeseries. The hidden states
%  are modelled as a hidden Markov model, where each state corresponds to a
%  particular  point in the parameter space of effective connectivity. This
%  effective connectivity then generates complex cross spectral data
%  features  of the observed timeseries. Model specification requires prior
%  constraints on the probability transition matrix among hidden states,
%  which implicitly specifies the number of hidden states. The user also
%  has to specify the number of windows for epochs to apply to the
%  timeseries, where each epoch  places a lower bound on the duration of
%  each (discrete) state.
%     We first generate synthetic data using regular transitions among
%  three hidden states  (C.F., a discrete version of a heteroclinic
%  cycle  for orbit). The data are then converted by a routine that
%  combines a parametric empirical Bayesian model and a hidden Markov model
%  (as implemented as a special case of a Markov decision process). This
%  inversion is repeated for each model specified in terms of the
%  transition matrices (as prior Dirichlet concentration parameters).
%  Setting a prior transition parameter to 0 precludes that transition. In
%  this way, several different models of transitions and number of hidden
%  states can be  scored in terms of  the variational free energy.
%     Following inversion, the results are plotted in terms of expected
%  state transitions, fluctuations in connections that are allowed to
%  change (specified in the usual way by DCM.b), the deviations in
%  connectivity associated with each hidden state and the expected
%  probability transition matrix.
%     Finally, we consider Bayesian model comparison in terms of group
%  differences (here, simply the difference between the first and second
%  simulated subject).  Bayesian model comparison is simple to do in this
%  context  by comparing the free energy of a hidden Markov model in which
%  both groups share the same state dependent connections and transition
%  probabilities, with two independent models. These can be evaluated
%  efficiently using Bayesian model reduction implicit in PEB. in this
%  example, we did not introduce any differences between the two groups
%  (i.e., subjects) and therefore expected to infer no group effect.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEM_demo_fMRI_HMM.m 7679 2019-10-24 15:54:07Z spm $

% (I) Simulate fMRI timeseries
%==========================================================================
rng('default')

%  Assume we have P sessions with N epochs of T scans with a TR of 2 secs:
% -------------------------------------------------------------------------
%  These epochs could be from a single subject or result from the
%  concatenation of multiple sessions (under the assumption that they share
%  the same modes of connectivity).
% -------------------------------------------------------------------------
P  = 2;                               % number of sessions (e.g., subjects)
S  = 3;                               % number of latent (hidden) states
N  = 9;                               % number of epochs or windows
T  = 128;                             % number of observations (per epoch)
TR = 2;                               % repetition time or timing
t  = (1:(T*N))*TR;                    % observation times (seconds)
n  = 3;                               % number of regions or nodes

% setup model for generating timeseries
% -------------------------------------------------------------------------
options.nonlinear  = 0;
options.two_state  = 0;
options.stochastic = 0;
options.induced    = 1;

% get priors to generate simulated data
% -------------------------------------------------------------------------
a   = ones(n,n);
b   = zeros(n,n,0);
c   = zeros(n,n);
d   = zeros(n,n,0);
pP  = spm_dcm_fmri_priors(a,b,c,d,options);

% average parameters - a simple hierarchy of three nodes
% -------------------------------------------------------------------------
pP.A = [  0  -.3    0;
         .4    0  -.1;
          0   .3    0];
pP.C = eye(n,n);
pP.transit = randn(n,1)/64;

% generate spectral density of neuronal fluctuations and observation noise
% -------------------------------------------------------------------------
[Gu,Gn,Hz,dt] = spm_csd_fmri_gu(pP,TR);
Gu    = Gu(:,1,1)*ones(1,n);
Gn    = Gn(:,1,1)*ones(1,n);

% specify and generate Markovian succession of hidden states
%==========================================================================

% connections associated with hidden states: here, intrinsic connectivity
% -------------------------------------------------------------------------
B        = zeros(n,n,S);
B(1,1,1) = 1/2;
B(2,2,2) = 1/2;
B(3,3,3) = 1/2;
pP.B     = any(B,3);                     % state-dependent connections

% generate sequence of hidden states: here, a simple orbit
% -------------------------------------------------------------------------
bb    = spm_speye(S,S,-1); bb(1,S) = 1;
o     = kron(ones(1,N),1:S);
o     = o(1:N);

% State-dependent deviations in connectivity from average
% -------------------------------------------------------------------------
for i = 1:N
    tB(:,:,i) = B(:,:,o(i));
end
for i = 1:S
    for j = 1:S
        tB(i,j,:) = tB(i,j,:) - mean(tB(i,j,:));
    end
end

%% simulate epoch-specific responses to endogenous fluctuations
%==========================================================================
M.x   = sparse(n,5);
M.f   = 'spm_fx_fmri';
X     = cell(N,1);
Y     = cell(N,1);
E     = cell(N,1);
u     = cell(N,1);
for p = 1:P

    % parameters for this epoch, plus a small random effect
    % ---------------------------------------------------------------------
    gu    = spm_rand_power_law(Gu,Hz,dt,N*T);
    ge    = spm_rand_power_law(Gn,Hz,dt,N*T);
    for s = 1:N

        % parameters for this epoch, plus a small random effect
        % -----------------------------------------------------------------
        tP   = pP;
        tP.A = tP.A + tB(:,:,s);
        tP.A = tP.A.*(1 + randn(n,n)/64);
        tP.C = eye(n,n);

        % integrate states with endogenous fluctuations (gu)
        % -----------------------------------------------------------------
        j    = (1:T) + (s - 1)*T;
        M.f  = 'spm_fx_fmri';
        U.u  = gu(j,:);
        U.dt = TR;
        x    = spm_int_J(tP,M,U);
        M.x  = spm_unvec(x(end,:),M.x);

        % haemodynamic observer function to produce BOLD signal
        % -----------------------------------------------------------------
        for i = 1:T
            y(i,:) = spm_gx_fmri(spm_unvec(x(i,:),M.x),[],tP)';
        end

        % response with observation noise (ge)
        % -----------------------------------------------------------------
        e       = ge(j,:);
        X{s}    = x;
        Y{s}    = y + e;
        E{s}    = e;
        u{s}    = U.u;
        TP(s,p) = tP;

    end

    % concatenate epochs into a single timeseries
    %----------------------------------------------------------------------
    xY.dt = TR;
    xY.y  = spm_cat(Y);
    xY.u  = spm_cat(u);
    xY.X  = spm_cat(X);
    xY.E  = spm_cat(E);

    % and create DCM cell array
    %----------------------------------------------------------------------
    DCM{1,p}.options = options;
    DCM{1,p}.a = logical(pP.A);
    DCM{1,p}.b = logical(pP.B);
    DCM{1,p}.c = zeros(n,0);
    DCM{1,p}.d = zeros(n,n,0);
    DCM{1,p}.Y = xY;

end

%% show simulated responses and windows
%--------------------------------------------------------------------------
spm_figure('Getwin','Figure 1'); clf

subplot(3,2,1), plot(t,xY.u)
title('Endogenous fluctuations','FontSize',16)
xlabel('Time (seconds)'), ylabel('Amplitude'), axis square, spm_axis tight

subplot(3,2,2), hold off
plot(t,xY.X(:,(n + 1):end),'c'), hold on
plot(t,xY.X(:,1:n)),             hold off
title('Hidden states','FontSize',16)
xlabel('Time (seconds)'), ylabel('Amplitude'), axis square, spm_axis tight

subplot(3,2,3)
plot(t,xY.y,t,xY.E,':')
title('Hemodynamic response and noise','FontSize',16)
xlabel('Time (seconds)'), ylabel('Amplitude'), axis square, spm_axis tight

%  This completes the simulation of the data. We now turn to inverting the
%  data to see if one can recover the number of hidden states, the form of
%  the state transitions and the connectivity modes associated with each
%  state:
%--------------------------------------------------------------------------

% (II) Inversion under a hidden Markov model
%==========================================================================
%  Specify model space as a cell array of probability transition matrices:
%  here, the model space at the level of the HMM  will allow all
%  transitions among one to 4 hidden states. These models are specified in
%  terms of Dirichlet priors; starting with a small value of allowable
%  transitions (1/16)
%--------------------------------------------------------------------------
for i = 1:4
    b{i} = ones(i,i)/16;
end

% invert hidden Markov model: this is the routine demonstrated
%--------------------------------------------------------------------------
[HMM,CSD] = spm_dcm_HMM(DCM,N,b);

% This completes the inversion. We now just need to look at the results:
%--------------------------------------------------------------------------

%% (III) report analysis
%==========================================================================
spm_figure('Getwin','Figure 1');

%  plot windows
% -------------------------------------------------------------------------
subplot(3,2,3), hold on
for i = 1:N, plot(t,CSD{i,end}.W - 1), end, hold off

% show estimates for a single session
% -------------------------------------------------------------------------
subplot(3,2,4)
spm_plot_ci(CSD{end}.Ep,CSD{end}.Cp), hold on
bar(TP(end).A(:),1/4), hold off, axis square
title('True and MAP connections (Deterministic)','FontSize',16)

% show state-dependent changes in connectivity over sessions
% -------------------------------------------------------------------------
for i = 1:numel(CSD)
    tp(i,:) = spm_vec(TP(i).A);
    qp(i,:) = spm_vec(CSD{i}.Ep.A);
    pp(i,:) = spm_vec(HMM(S).Ep{i}.A);
end
subplot(3,3,7); imagesc(tp)
title('True connections','FontSize',16), axis square
subplot(3,3,8); imagesc(qp)
title('MAP estimates',   'FontSize',16), axis square
subplot(3,3,9); imagesc(pp)
title('PEB estimates',   'FontSize',16), axis square

% report hidden Markov model
%==========================================================================
spm_dcm_HMM_plot(HMM,S)

% And overlay true values, as cyan dots
%==========================================================================

% true state transitions
%--------------------------------------------------------------------------
x     = sparse(o,1:N,1,S,N);
x     = kron(ones(1,P),x);
N     = size(x,2);

% associate true and discovered states - and reorder
%--------------------------------------------------------------------------
r     = x*HMM(S).X';
j     = zeros(S,1);
for i = 1:S
    [d,m]  = max(r(:,i));
    j(i)   = m;
    r(m,:) = 0;
end
[o,i] = find(x(j,:));
B     = B(:,:,j);
bb    = bb(j,j);

% superimpose true values
%--------------------------------------------------------------------------
spm_figure('Getwin','HMM')

%  hidden states
%--------------------------------------------------------------------------
subplot(4,1,1), hold on
for i = 1:N, plot(i,o(i),'.c','MarkerSize',32), end, hold off

% state-dependent parameters - fluctuations
%--------------------------------------------------------------------------
subplot(4,1,2), hold on
for i = 1:N
    [j,k] = max(spm_vec(TP(i).A - pP.A));
    plot(i,k,'.c','MarkerSize',32)
end, hold off

subplot(4,1,3), hold on
for i = 1:N, pA(:,i) = spm_vec(TP(i).A); end
plot(1:N,pA(HMM(S).iP,:),'-.'), hold off, spm_axis tight

% state-dependent parameters - expectations
%--------------------------------------------------------------------------
subplot(4,2,7), hold on
for i = 1:S
    c     = spm_vec(B(:,:,i));
    [j,k] = max(c(HMM(S).iP));
    plot(i,k,'.c','MarkerSize',32)
end, hold off

% expected transition probabilities
%--------------------------------------------------------------------------
subplot(4,2,8), hold on
for i = 1:S, [j,k] = max(bb(:,i)); plot(i,k,'.c','MarkerSize',32), end
hold off

%% Bayesian model comparison in terms of group (i.e., subject) differences
%==========================================================================

% model as a single group or two separate groups
%--------------------------------------------------------------------------
hmm0 = HMM(S);

hmm1 = spm_dcm_HMM(CSD(:,1),b(S));
hmm2 = spm_dcm_HMM(CSD(:,2),b(S));

% compare the free energy of the combined groups with the combined
% free energy:
%--------------------------------------------------------------------------
F    = [hmm0.F; hmm1.F + hmm2.F];
F    = F - min(F);

% report model comparison in terms of free energy (i.e., log evidence)
%--------------------------------------------------------------------------
spm_figure('Getwin','HMM-F')
subplot(2,2,3)
bar(F,'c'),  title('Group difference','FontSize',16)
xlabel('Effect'), ylabel('Log evidence'), axis square
set(gca,'XTickLabel',{'None','Effect'})

% and show the independent maximum a posteriori estimates of state
% dependent connectivity
%--------------------------------------------------------------------------
subplot(4,2,6)
bar(hmm1.qP),  title('Group 1','FontSize',16)
xlabel('Parameter'), ylabel('Connectivity (log)'), axis square
subplot(4,2,8)
bar(hmm2.qP),  title('Group 2','FontSize',16)
xlabel('Parameter'), ylabel('Connectivity (log)'), axis square

return


% Program for Newton-Raphson Load Flow with STATCOM

Y = ybusppg();              % Calling ybusppg.m..
busdata = busdata30();       % Calling Busdata..
statdata = statdata30();     % Statcom Data..
baseMVA = 100;              % Base MVA..
bus = busdata(:,1);         % Bus Number..
type = busdata(:,2);        % Type of Bus 1-Slack, 2-PV, 3-PQ..
V = busdata(:,3);           % Specified Voltage..
del = busdata(:,4);         % Voltage Angle..
Pg = busdata(:,5);          % PGi..
Qg = busdata(:,6);          % QGi..
Pl = busdata(:,7);          % PLi..
Ql = busdata(:,8);          % QLi..
Qmin = busdata(:,9);        % Minimum Reactive Power Limit..
Qmax = busdata(:,10);       % Maximum Reactive Power Limit..
nbus = max(bus);            % To get no. of buses..
P = Pg - Pl;                % Pi = PGi - PLi..
Q = Qg - Ql;                % Qi = QGi - QLi..
P = P/baseMVA;              % Converting to p.u..
Q = Q/baseMVA;
Qmin = Qmin/baseMVA;
Qmax = Qmax/baseMVA;
Tol = 1;
Iter = 1;
Psp = P;
Qsp = Q;
G = real(Y);    % Conductance..
B = imag(Y);    % Susceptance..
Vsp = V;

% Details of STATCOM
statb = statdata(:,1); % Buses at which statcoms are placed..
Vsh = statdata(:,2);
Thst = statdata(:,3);
Qsmx = statdata(:,4);
Qsmn = statdata(:,5);
gsh = 0.9901;
bsh = -9.901;
Vshmx = 1.1; Vshmn = 0.9;
Thstmx = pi; Thstmn = -pi;
np = length(statb); % Number of STATCOMs..

pv = find(type == 2 | type == 1); % Index of PV Buses..
pq = find(type == 3); % Index of PQ Buses..

npv = length(pv); % Number of PV buses..
npq = length(pq); % Number of PQ buses..

while (Tol > 1e-5 && Iter <= 50)   % Iteration starting..

    P = zeros(nbus,1);
    Q = zeros(nbus,1);
    % Calculate P and Q
    for i = 1:nbus
        for k = 1:nbus
            P(i) = P(i) + V(i)* V(k)*(G(i,k)*cos(del(i)-del(k)) + B(i,k)*sin(del(i)-del(k)));
            Q(i) = Q(i) + V(i)* V(k)*(G(i,k)*sin(del(i)-del(k)) - B(i,k)*cos(del(i)-del(k)));
        end
        m = find(statb == i);
        if ~isempty(m)
            P(i) = P(i) + V(i)^2*gsh - V(i)*Vsh(m)*(gsh*cos(del(i)-Thst(m)) + bsh*sin(del(i)-Thst(m)));
            Q(i) = Q(i) - V(i)^2*bsh + V(i)*Vsh(m)*(bsh*cos(del(i)-Thst(m)) - gsh*sin(del(i)-Thst(m)));
        end
    end

    % Checking Q-limit violations..
    if Iter >= 2
        for n = 1:nbus
            if type(n) == 2
                if Q(n) < Qmin(n)
                    V(n) = V(n) + 0.01;
                elseif Q(n) > Qmax(n)
                    V(n) = V(n) - 0.01;
                end
            end
         end
    end

    % Calculating PE,
    PE = zeros(np,1);
    for i = 1:np
        m = statb(i);
        PE(i) = Vsh(i)^2*gsh - V(m)*Vsh(i)*(gsh*cos(del(m)-Thst(i)) - bsh*sin(del(m)-Thst(i)));
    end

    % Calculate F, Control Variables
    F = zeros(np,1);
    for i = 1:np
        m = statb(i);
        F(i) = V(m) - Vsp(m);
    end

    % Calculate change from specified value
    dPa = Psp-P;
    dQa = Qsp-Q;
    dQ = zeros(npq,1);
    k = 1;
    for i = 1:nbus
        if type(i) == 3
            dQ(k,1) = dQa(i);
            k = k+1;
        end
    end
    dP = dPa(2:nbus);
    dPE = -PE;
    dF = -F;
    M = [dP; dQ; dPE; dF];       % Mismatch Vector

    % Jacobian
    % J11 - Derivative of Real Power Injections with Angles..
    J11 = zeros(nbus-1,nbus-1);
    for i = 1:(nbus-1)
        m = i+1;
        p = find(statb == m);
        for k = 1:(nbus-1)
            q = k+1;
            if q == m
                for n = 1:nbus
                    J11(i,k) = J11(i,k) + V(m)* V(n)*(-G(m,n)*sin(del(m)-del(n)) + B(m,n)*cos(del(m)-del(n)));
                end
                J11(i,k) = J11(i,k) - V(m)^2*B(m,m);
                if ~isempty(p)
                    J11(i,k) = J11(i,k) + V(m)*Vsh(p)*(gsh*sin(del(m)-Thst(p)) - bsh*cos(del(m)-Thst(p)));
                end
            else
                J11(i,k) = V(m)* V(q)*(G(m,q)*sin(del(m)-del(q)) - B(m,q)*cos(del(m)-del(q)));
            end
        end
    end

    % J12 - Derivative of Real Power Injections with V..
    J12 = zeros(nbus-1,npq);
    for i = 1:(nbus-1)
        m = i+1;
        p = find(statb == m);
        for k = 1:npq
            q = pq(k);
            if q == m
                for n = 1:nbus
                    J12(i,k) = J12(i,k) + V(n)*(G(m,n)*cos(del(m)-del(n)) + B(m,n)*sin(del(m)-del(n)));
                end
                J12(i,k) = J12(i,k) + V(m)*G(m,m);
                if ~isempty(p)
                    J12(i,k) = J12(i,k) + 2*V(m)*gsh - Vsh(p)*(gsh*cos(del(m)-Thst(p)) + bsh*sin(del(m)-Thst(p)));
                end
            else
                J12(i,k) = V(m)*(G(m,q)*cos(del(m)-del(q)) + B(m,q)*sin(del(m)-del(q)));
            end
        end
    end

    % J13 - Derivative of Real Power Injections with Vsh..
    % J14 - Derivative of Real Power Injections with Thsh..
    J13 = zeros(nbus-1,np);
    J14 = zeros(nbus-1,np);
    for i = 1:(nbus-1)
        m = i+1;
        for k = 1:np
            p = statb(k);
            if m == p
                J13(i,k) = -V(m)*(gsh*cos(del(m)-Thst(k)) + bsh*sin(del(m)-Thst(k)));
                J14(i,k) = -V(m)*Vsh(k)*(gsh*sin(del(m)-Thst(k)) - bsh*cos(del(m)-Thst(k)));
            end
        end
    end

    % J21 - Derivative of Reactive Power Injections with Angles..
    J21 = zeros(npq,nbus-1);
    for i = 1:npq
        m = pq(i);
        p = find(statb == m);
        for k = 1:(nbus-1)
            q = k+1;
            if q == m
                for n = 1:nbus
                    J21(i,k) = J21(i,k) + V(m)* V(n)*(G(m,n)*cos(del(m)-del(n)) + B(m,n)*sin(del(m)-del(n)));
                end
                J21(i,k) = J21(i,k) - V(m)^2*G(m,m);
                if ~isempty(p)
                    J21(i,k) = J21(i,k) - V(m)*Vsh(p)*(gsh*cos(del(m)-Thst(p)) + bsh*sin(del(m)-Thst(p)));
                end
            else
                J21(i,k) = -V(m)* V(q)*(G(m,q)*cos(del(m)-del(q)) + B(m,q)*sin(del(m)-del(q)));
            end
        end
    end

    % J22 - Derivative of Reactive Power Injections with V..
    J22 = zeros(npq,npq);
    for i = 1:npq
        m = pq(i);
        p = find(statb == m);
        for k = 1:npq
            q = pq(k);
            if q == m
                for n = 1:nbus
                    J22(i,k) = J22(i,k) + V(n)*(G(m,n)*sin(del(m)-del(n)) - B(m,n)*cos(del(m)-del(n)));
                end
                J22(i,k) = J22(i,k) - V(m)*B(m,m);
                if ~isempty(p)
                    J22(i,k) = J22(i,k) - 2*V(m)*bsh - Vsh(p)*(gsh*sin(del(m)-Thst(p)) - bsh*cos(del(m)-Thst(p)));
                end
            else
                J22(i,k) = V(m)*(G(m,q)*sin(del(m)-del(q)) - B(m,q)*cos(del(m)-del(q)));
            end
        end
    end

    % J23 - Derivative of Reactive Power Injections with Vsh..
    % J24 - Derivative of Reactive Power Injections with Thsh..
    J23 = zeros(npq,np);
    J24 = zeros(npq,np);
    for i = 1:npq
        q = pq(i);
        m = i+1;
        for k = 1:np
            p = statb(k);
            if q == p
                J23(i,k) = -V(m)*(gsh*sin(del(m)-Thst(k)) - bsh*cos(del(m)-Thst(k)));
                J24(i,k) = V(m)*Vsh(k)*(gsh*cos(del(m)-Thst(k)) + bsh*sin(del(m)-Thst(k)));
            end
        end
    end

    % J31 - Derivative of PE with Angles..
    % J41 - Derivative of F with Angles..
    J31 = zeros(np,nbus-1);
    J41 = zeros(np,nbus-1);
    for i = 1:np
        m = statb(i);
        for k = 1:(nbus-1)
            if m == k+1
                J31(i,k) = V(m)*Vsh(i)*(gsh*sin(del(m)-Thst(i)) + bsh*cos(del(m)-Thst(i)));
            end
        end
    end

    % J32 - Derivative of PE with V..
    % J42 - Derivative of F with V..
    J32 = zeros(np,npq);
    J42 = zeros(np,npq);
    for i = 1:np
        m = statb(i);
        for k = 1:npq
            if m == pq(k)
                J32(i,k) = -Vsh(i)*(gsh*cos(del(m)-Thst(i)) - bsh*sin(del(m)-Thst(i)));
            end
            if m == pq(k)
                J42(i,k) = 1;
            end
        end
    end

    % J33 - Derivative of PE with Vsh..
    % J34 - Derivative of PE with Thsh..
    % J43 - Derivative of F with Vsh..
    % J44 - Derivative of F with Thsh..
    J33 = zeros(np,np);
    J34 = zeros(np,np);
    J43 = zeros(np,np);
    J44 = zeros(np,np);
    for i = 1:np
        m = statb(i);
        for k = 1:np
            p = statb(k);
            if m == p
                J33(i,k) = 2*Vsh(k)*gsh - V(m)*(gsh*cos(del(m)-Thst(k)) - bsh*sin(del(m)-Thst(k)));
                J34(i,k) = -V(m)*Vsh(k)*(gsh*sin(del(m)-Thst(k)) + bsh*cos(del(m)-Thst(k)));
            end
        end
    end

    J = [J11 J12 J13 J14; J21 J22 J23 J24; J31 J32 J33 J34; J41 J42 J43 J44];     % Jacobian
    clear J11 J12 J13 J14 J21 J22 J23 J24 J31 J32 J33 J34 J41 J42 J43 J44

    X = inv(J)*M;           % Correction Vector
    dTh = X(1:nbus-1);
    dV = X(nbus:nbus+npq-1);
    dVsh = X(nbus+npq:nbus+npq+np-1);
    dThst = X(nbus+npq+np:nbus+npq+2*np-1);
    del(2:nbus) = dTh + del(2:nbus);
    k = 1;
    for i = 2:nbus
        if type(i) == 3
            V(i) = dV(k) + V(i);
            k = k+1;
        end
    end
    Vsh = Vsh + dVsh;
    Thst = Thst + dThst;

    % Calculate Qsh..
    Qsh = zeros(np,1);
    for m = 1:np
        i = statb(m);
        Qsh(m) = -V(i)^2*bsh + V(i)*Vsh(m)*(bsh*cos(del(i)-Thst(m)) - gsh*sin(del(i)-Thst(m)));
    end
    Iter = Iter + 1;
    Tol = max(abs(M));
end

Iter = Iter - 1; % Number of Iterations took..
V;
Del = 180/pi*del;
Thst = 180/pi*Thst;
E2 = [V Del]; % Bus Voltages and angles..
disp('------------------------------');
disp('|  Bus  |    V    |  Angle   | ');
disp('|  No   |   pu    |  Degree  | ');
disp('------------------------------');
for m = 1:nbus
    fprintf('%4g', m), fprintf('    %8.4f', V(m)), fprintf('    %8.4f', Del(m)),fprintf('\n');
end
disp('-----------------------------');
disp('----------------------------------------');
disp('| STATCOM |  Vsh   |  Thst    |   Qsh  |');
disp('|   Bus   |   pu   |  Degree  |    pu  |');
disp('----------------------------------------');
for m = 1:np
    fprintf('  %4g',statb(m)), fprintf('   %8.4f', Vsh(m)), fprintf('    %8.4f', Thst(m)),fprintf('  %8.4f', Qsh(m)), fprintf('\n');
end
disp('----------------------------------------');


function op = prox_l1_and_sum( q, b, nColumns, zeroID, useMex )

%PROX_L1_AND_SUM    L1 norm with sum(X)=b constraints
%    OP = PROX_L1_AND_SUM( Q ) implements the nonsmooth function
%        OP(X) = norm(Q.*X,1) with constraints
%    Q is optional; if omitted, Q=1 is assumed. But if Q is supplied,
%    then it must be a positive real scalar (or must be same size as X).
%
%    OP = PROX_L1_AND_SUM( Q, B )
%       includes the constraints that sum(X(:)) == B
%       (Default: B=1)
%
%    OP = PROX_L1_AND_SUM( Q, B, nColumns )
%       takes the input vector X and reshapes it to have nColumns
%       and applies this prox to every column
%
%    OP = PROX_L1_AND_SUM( Q, B, nColumns, zeroID )
%       if zeroID == true (it is false by default)
%       then after reshaping X, enforces that X(i,i) = 0
%
%    OP = PROX_L1_AND_SUM( Q, B, nColumns, zeroID, useMex)
%       toggle the usage of prox_l1_and_sum_worker_mex.cc for the computation.
%       useMex defaults to true
%
%
% If available and compiled, this uses TFOCS/mexFiles/prox_l1_and_sum_worker_mex.cc.
% It also checks for TFOCS/mexFiles/shrink_mex2.cc to use as a slower backup.
% To control the number of threads these use, run
%
%   `prox_l1_and_sum_worker_mex(struct('num_threads', 4))`
%
% when constructing your problem.  This will cache the number of threads internally
% and use that number of threads until you restart MATLAB.
%
% Often useful for sparse subpsace clustering (SSC)
%   See, e.g., https://github.com/stephenbeckr/SSC

% Nov 2017, Stephen.Becker@Colorado.edu
% 18 Nov 2018, optimizations by jamesfolberth@gmail.com

if nargin == 0
    q = 1;
elseif ~isnumeric( q ) || ~isreal( q ) ||  any( q(:) < 0) || all(q(:)==0) || numel( q ) ~= 1
    error( 'Argument must be positive.' );
end
if nargin < 2 || isempty(b), b = 1; else, assert( numel(b) == 1 ); end
if nargin < 3 || isempty( nColumns), nColumns = 1;
else assert( numel(nColumns) == 1 && nColumns >= 1 ); end
if nargin < 4 || isempty( zeroID ), zeroID = false; end
if nargin < 5, useMex = true; end

if zeroID && nColumns == 1
    warning('TFOCS:prox_l1_and_sum:zeroDiag',...
        'You requested enforcing zero diagonals but did not set nColumns>1 which is probably a mistake');
end

if useMex
    if 3~=exist('prox_l1_and_sum_worker_mex','file')
        addpath( fullfile( tfocs_where, 'mexFiles' ) );
    end
    if 3==exist('prox_l1_and_sum_worker_mex','file')
        op = tfocs_prox( @(x)f(q,x), @(x,t)prox_f_using_worker(q,b,nColumns,zeroID,x,t) , 'vector' );
        return;
    end
    % else fall back to optimized MATLAB approach
end

% 3/15/18, adding:
%JMF 17 Nov 2018: determine if we have shrink_mex once when constructing the prox handle
if useMex
    if 3~=exist('shrink_mex2','file')
        addpath( fullfile( tfocs_where, 'mexFiles' ) );
    end
    if 3~=exist('shrink_mex2','file')
        useMex = false;
    end
end
if useMex
    %shrink  = @(x,tq) shrink_mex(x,tq); %NOTE: shrink_mex doesn't handle vector tq properly
    shrink  = @(x,tq) shrink_mex2(x,tq);
    shrink_nu = @(x,tq,nu) shrink_mex2(x,tq,nu);
else
    shrink  = @(x,tq) sign(x).*max( bsxfun(@minus,abs(x),tq), 0 );
    shrink_nu = @(x,tq,nu) shrink(bsxfun(@minus,x,nu),tq);
end

% This is Matlab and Octave compatible code
op = tfocs_prox( @(x)f(q,x), @(x,t)prox_f(q,b,nColumns,zeroID,shrink,shrink_nu,x,t) , 'vector' );

end

% These are now subroutines, that are NOT in the same scope
function v = f(qq,x)
    v = norm( qq(:).*x(:), 1 );
end

function x = prox_f(qq,b,nColumns,zeroID,shrink,shrink_nu,x,t) % stepsize is t
    tq = t .* qq; % March 2012, allowing vectorized stepsizes
    tq = reshape(tq, [1 numel(tq)]);

    if zeroID && nColumns > 1
        x   = reshape( x, [], nColumns );
        nRows   = size(x,1);
        if nColumns > nRows
            error('Cannot zero out the diagonal if columns > rows');
        end

        x = prox_l1sum_zeroID_matricized( x, tq, b, shrink_nu );

        x = reshape(x, nRows*nColumns, 1);

    else
        if nColumns > 1
            x   = reshape( x, [], nColumns );
            nRows   = size(x,1);

            x = prox_l1sum_matricized( x, tq, b, shrink_nu );

            x = reshape(x, nRows*nColumns, 1);
        else
            x   = prox_l1sum( x, tq, b, shrink_nu );
        end
    end
end

function x = prox_f_using_worker(qq,b,nColumns,zeroID,x,t) % stepsize is t
    tq = t .* qq; % March 2012, allowing vectorized stepsizes

    if nColumns > 1 % matrix
        x = reshape(x, [], nColumns);
        nRows = size(x,1);

        if zeroID && nColumns > nRows
            error('Cannot zero out the diagonal if columns > rows');
        end

        x = prox_l1_and_sum_worker_mex(x, tq, b, zeroID);

        x = reshape(x, nRows*nColumns, 1);

    else % vector
        x = prox_l1_and_sum_worker_mex(x, tq, b);
    end
end

% Main algorithmic part: if x0 is length n, takes O(n log n) time
function x = prox_l1sum( x0, lambda, b, shrink_nu )

    brk_pts = sort( [x0-lambda;x0+lambda], 'descend' );

    xnu     = @(nu) shrink_nu( x0 , lambda, nu );
    h       = @(x) sum(x) - b; % want to solve h(nu) = 0

    % Bisection
    lwrBnd       = 0;
    uprBnd       = length(brk_pts) + 1;
    iMax         = ceil( log2(length(brk_pts)) ) + 1;
    PRINT = false; % set to "true" for debugging purposes
    if PRINT
        dispp = @disp;
        printf = @fprintf;
    else
        dispp = @(varargin) 1;
        printf = @(varargin) 1;
    end
    dispp(' ');
    for i = 1:iMax
        if uprBnd - lwrBnd <= 1
            dispp('Bounds are too close; breaking');
            break;
        end
        j = round( (lwrBnd+uprBnd)/2 );
        %printf('j is %d (bounds were [%d,%d])\n', j, lwrBnd,uprBnd ); %
        if j==lwrBnd
            dispp('j==lwrBnd, so increasing');
            j = j+1;
        elseif j==uprBnd
            dispp('j==uprBnd, so increasing');
            j = j-1;
        end

        a   = brk_pts(j);
        x   = xnu(a);  % the prox
        p   = h(x);

        if p > 0
            uprBnd = j;
        elseif p < 0
            lwrBnd = j;
        end
        if PRINT
            % Don't rely on redefinition of printf,
            % since then we would still calculate find(~x)
            % which is slow
            printf('i=%2d, a = %6.3f, p = %8.3f, zeros ', i, a, p );
            if n < 100, printf('%d ', find(~x) ); end
            printf('\n');
        end
    end

    % Now, determine linear part, which we infer from two points.
    % If lwr/upr bounds are infinite, we take special care
    % e.g., we make a new "a" slightly lower/bigger, and use this
    % to extract linear part.
    if lwrBnd == 0
        a2 = brk_pts( uprBnd );
        a1 = a2 - 10; % arbitrary
        aBounds = [a1,a2];
    elseif uprBnd == length(brk_pts) + 1
        a1 = brk_pts( lwrBnd );
        a2 = a1 + 10; % arbitrary
        aBounds = [a1,a2];
    else
        % In general case, we can infer linear part from the two break points
        a1 = brk_pts( lwrBnd );
        a2 = brk_pts( uprBnd );
        aBounds = [a1,a2];
    end

    % Now we have the support, find exact value
    x       = xnu(( aBounds(1)+aBounds(2))/2 );  % to find the support
    supp    = find(x);

    sgn     = sign(x);
    nu      = ( sum(x0(supp) - lambda*sgn(supp) ) - b )/length(supp);

    x   = xnu( nu );

end

% This variant can handle several columns at once,
% and it takes exactly log2(n) iterations, as it doesn't stop early
%   since different columns might stop at different steps and that's
%   not easy to detect efficiently.
function x = prox_l1sum_matricized( x0, lambda, b, shrink_nu )

    brk_pts = sort( [x0-lambda;x0+lambda], 'descend' );

    xnu     = @(nu) shrink_nu( x0 , lambda, nu );

    h       = @(x) sum(x) - b; % want to solve h(nu) = 0

    nCols        = size( x0, 2 ); % allow matrices
    LDA          = size( brk_pts, 1 );
    offsets      = (0:nCols-1)*LDA;%i.e., [0, LDA, 2*LDA, ... ];
    num_brk_pts = LDA;

    lwrBnd       = zeros(1,nCols);
    uprBnd       = (length(brk_pts) + 1)*ones(1,nCols);
    iMax         = ceil( log2(length(brk_pts)) ) + 1;

    for i = 1:iMax

        j = round(mean([lwrBnd;uprBnd]));
        ind = find( j==lwrBnd );
        j( ind ) = j( ind ) + 1;
        ind = find( j==uprBnd );
        j( ind ) = j( ind ) - 1;

        a   = brk_pts(j+offsets); % need the offsets to correct it here
        x   = xnu(a);  % the prox
        p   = h(x);

        ind = find( p > 0 );
        uprBnd(ind) = j(ind);
        ind = find( p < 0 );
        lwrBnd(ind) = j(ind);

    end

    [a1,a2]     = deal( zeros(1,nCols) );
    ind = find( lwrBnd == 0 );
    a2(ind) = brk_pts( uprBnd(ind) + offsets(ind) );
    a1(ind) = a2(ind) + 10;
    ind2 = ind;

    ind = find( uprBnd == num_brk_pts + 1 );
    a1(ind) = brk_pts( lwrBnd(ind) + offsets(ind) );
    a2(ind) = a1(ind) - 10;

    indOther = setdiff( 1:nCols, [ind2,ind] );
    a1(indOther)    = brk_pts( lwrBnd(indOther) + offsets(indOther) );
    a2(indOther)    = brk_pts( uprBnd(indOther) + offsets(indOther) );

    a  = mean( [a1;a2] );
    x       = xnu( a );

    nu      = zeros(1,nCols);
    sgn     = sign(x);
    if numel(lambda) > 1
        for col = 1:nCols
            supp    = find( sgn(:,col) );
            nu(col)      = ( sum(x0(supp,col) - lambda(col)*sgn(supp,col) ) - b )/length(supp);
        end
    else
         for col = 1:nCols
            supp    = find( sgn(:,col) );
            nu(col)      = ( sum(x0(supp,col) - lambda*sgn(supp,col) ) - b )/length(supp);
        end
    end
    x   = xnu( nu );

end

% This variant can handle several columns at once,
% and it takes exactly log2(n) iterations, as it doesn't stop early
%   since different columns might stop at different steps and that's
%   not easy to detect efficiently.
%
% This hacks "tricks" the implementation into ignoring the diagonal to avoid an extra copy
function x = prox_l1sum_zeroID_matricized( x0, lambda, b, shrink_nu )

    [nRows, nCols] = size(x0);
    diag_inds = nRows*(0:nCols-1) + (1:nCols);

    % Sort all possible break points
    % We account for ignoring the diagonal by setting the diagonals to -inf,
    % so they're at the bottom of the sorted matrix.  We then need to handle the offsets
    % appropriately.

    brk_pts_minus = x0 - lambda; brk_pts_minus(diag_inds) = -inf;
    brk_pts_plus = x0 + lambda; brk_pts_plus(diag_inds) = -inf;
    brk_pts = [brk_pts_minus; brk_pts_plus];

    %brk_pts = [x0 - lambda; x0 + lambda];
    %inf_inds = [2*nRows*(0:nCols-1) + (1:nCols),
    %            2*nRows*(0:nCols-1) + (1:nCols) + nRows];
    %brk_pts(inf_inds) = -inf;

    brk_pts = sort( brk_pts, 'descend' );

    xnu     = @(nu) shrink_nu( x0, lambda, nu );

    h       = @(x) sum(x) - x(diag_inds) - b; % want to solve h(nu) = 0

    LDA          = size( brk_pts, 1 );
    offsets      = (0:nCols-1)*LDA;%i.e., [0, LDA, 2*LDA, ... ];
    num_brk_pts = LDA - 2;

    lwrBnd       = zeros(1,nCols);
    uprBnd       = (num_brk_pts + 1 )*ones(1,nCols);
    iMax         = ceil( log2(num_brk_pts) ) + 1;

    for i = 1:iMax

        j = round(mean([lwrBnd;uprBnd]));
        ind = find( j==lwrBnd );
        j( ind ) = j( ind ) + 1;
        ind = find( j==uprBnd );
        j( ind ) = j( ind ) - 1;

        a   = brk_pts(j+offsets); % need the offsets to correct it here
        x   = xnu(a);  % the prox
        p   = h(x);

        ind = find( p > 0 );
        uprBnd(ind) = j(ind);
        ind = find( p < 0 );
        lwrBnd(ind) = j(ind);

    end

    [a1,a2]     = deal( zeros(1,nCols) );
    ind = find( lwrBnd == 0 );
    a2(ind) = brk_pts( uprBnd(ind) + offsets(ind) );
    a1(ind) = a2(ind) + 10;
    ind2 = ind;

    ind = find( uprBnd == num_brk_pts + 1 );
    a1(ind) = brk_pts( lwrBnd(ind) + offsets(ind) );
    a2(ind) = a1(ind) - 10;

    indOther = setdiff( 1:nCols, [ind2,ind] );
    a1(indOther)    = brk_pts( lwrBnd(indOther) + offsets(indOther) );
    a2(indOther)    = brk_pts( uprBnd(indOther) + offsets(indOther) );

    a  = mean( [a1;a2] );
    x       = xnu( a );

    sgn     = sign(x);
    supp = (sgn ~= 0); supp(diag_inds) = 0;

    nu      = zeros(1,nCols);
    if numel(lambda) > 1
        for col = 1:nCols
            nu(col)      = ( sum(x0(supp(:,col),col) - lambda(col)*sgn(supp(:,col),col) ) - b )/sum(supp(:,col));
        end
    else
        UNSAFE_BUT_FAST = true;
        if UNSAFE_BUT_FAST
            % some MATLAB hackery to vectorize the below loop
            % Note that due to roundoff in the first cumsum, this is not necessarily safe
            % With data that are changing signs randomly, the numerical issues should be kept at bay.
            % In initial experiments, this seems to be okay.
            num_supp = sum(supp,1);
            sumthing = cumsum(x0(supp) - lambda*sgn(supp)); % possible numerical issues!
            inds = cumsum(num_supp);
            numerator = sumthing(inds) - b;
            numerator(2:nCols) = numerator(2:nCols) - sumthing(inds(1:nCols-1));
            nu = numerator.' ./ num_supp;
        else
            for col = 1:nCols
                nu(col)      = ( sum(x0(supp(:,col),col) - lambda*sgn(supp(:,col),col) ) - b )/sum(supp(:,col));
            end
        end
    end

    x   = xnu( nu );

    x(diag_inds) = 0;

end


function calc_bootfitloglike_a2(a,time,timef,bootloops,maepi)
    % function calc_bootfitloglike_a2(a,time,timef,bootloops,maepi);
    % --------------------------------------------------
    % Plots Ncum observed vs. Ncum modeled for specified time windows
    %
    % Input variables:
    % a         : earthquake catalog
    % time      : learning period fo fit Omori parameters
    % timef     : forecast period
    % bootloops : Number of bootstraps
    % maepi     : mainshock
    %
    % J.Woessner, S. Wiemer
    % last update: 05.08.03

report_this_filefun(mfilename('fullpath'));
    % Surpress warnings from fmincon
    warning off;

report_this_filefun(mfilename('fullpath'));
    %[m_main, main] = max(a.Magnitude);
    date_matlab = datenum(a.Date.Year,a.Date.Month,a.Date.Day,a.Date.Hour,a.Date.Minute,zeros(size(a,1),1));
    date_main = datenum(floor(maepi(3)),maepi(4),maepi(5),maepi(8),maepi(9),0);
    time_aftershock = date_matlab-date_main;
    % Select biggest aftershock earliest in time, but more than 1 day after mainshock
    fDay = 1;
    ft_c=fDay/365; % Time not considered to find biggest aftershock
    vSel = (a.Date > maepi(:,3)+ft_c & a.Date<= maepi(:,3)+time/365);
    mCat = a.subset(vSel);
    vSel = mCat(:,6) == max(mCat(:,6));
    vBigAf = mCat(vSel,:);
    if length(mCat(:,1)) > 1
        vSel = vBigAf(:,3) == min(vBigAf(:,3));
        vBigAf = vBigAf(vSel,:);
    end

    date_biga = datenum(floor(vBigAf(3)),vBigAf(4),vBigAf(5),vBigAf(8),vBigAf(9),0);
    fT1 = date_biga - date_main; % Time of big aftershock

    % Aftershock times
    l = time_aftershock(:) > 0;
    tas = time_aftershock(l);
    eqcatalogue = a.subset(l);

    % time_as: Learning period
    l = tas <= time;
    time_as=tas(l);

    % Times up to the forecast time
    lf = tas <= time+timef ;
    time_asf= [tas(lf) ];
    time_asf=sort(time_asf);

    % Calculate p,c,k for dataset
    prompt  = {'Enter model number (1:pck, 2:pckk, 3:ppckk, 4:ppcckk:'};
    title   = 'Model selection for fitting aftershock sequence';
    lines= 1;
    def     = {'1'};
    answer  = inputdlg(prompt,title,lines,def);
    nMod = str2double(answer{1});

    % Calculate uncertainty and mean values of p,c,and k
    [mMedModF, mStdL, loopout] = brutebootloglike_a2(time_as, time_asf, bootloops,fT1,nMod);
    pmed1 = mMedModF(1,1);
    pmed2 = mMedModF(1,3);
    cmed1 = mMedModF(1,5);
    cmed2 = mMedModF(1,7);
    kmed1 = mMedModF(1,9);
    kmed2 = mMedModF(1,11);

    % Compute model according to model choice
    if nMod == 1
        [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    elseif nMod == 2
        [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    elseif nMod == 3
        [pval1, pval2, cval1, cval2, kval1, kval2 , fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    else
        [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    end
    % Round values
    pval1 = round(100*pval1)/100;
    pval2 = round(100*pval2)/100;
    cval1 = round(100*cval1)/100;
    cval2 = round(100*cval2)/100;
    kval1 = round(10*kval1)/10;
    kval2 = round(10*kval2)/10;

    if (isnan(pval1) == 0 & isnan(pval2) == 0)

        figure_w_normalized_uicontrolunits('Numbertitle','off','Name','Forecast aftershock occurence')
        loopout = [loopout , loopout(:,1)*0];

        % Time until end of forecast
        for j = 1:length(loopout(:,1))
            cumnr = (1:length(time_asf))';
            cumnr_model = [];
            pval1 = loopout(j,1);
            pval2 = loopout(j,2);
            cval1 = loopout(j,3);
            cval2 = loopout(j,4);
            kval1 = loopout(j,5);
            kval2 = loopout(j,6);
            if nMod == 1
                for i=1:length(time_asf)
                    if pval1 ~= 1
                        cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_asf(i)+cval1)^(1-pval1));
                    else
                        cm = kval1*log(time_asf(i)/cval1+1);
                    end
                    cumnr_model = [cumnr_model; cm];
                end % END of FOR on length(time_asf)
                loopout(j,9) = max(cumnr_model);
            else
                for i=1:length(time_asf)
                    if time_asf(i) <= fT1
                        if pval1 ~= 1
                            cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_asf(i)+cval1)^(1-pval1));
                        else
                            cm = kval1*log(time_asf(i)/cval1+1);
                        end
                        cumnr_model = [cumnr_model; cm];
                    else
                        if (pval1 ~= 1 & pval2 ~= 1)
                            cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_asf(i)+cval1)^(1-pval1))+ kval2/(pval2-1)*(cval2^(1-pval2)-(time_asf(i)-fT1+cval2)^(1-pval2));
                        else
                            cm = kval1*log(time_asf(i)/cval1+1) + kval2*log((time_asf(i)-fT1)/cval2+1);
                        end
                        cumnr_model = [cumnr_model; cm];
                    end %END of IF on fT1
                end % End of FOR length(time_asf)
                loopout(j,9) = max(cumnr_model);
            end % End of if on nMod
            pfloop = plot(time_asf,cumnr_model,'color',[0.8 0.8 0.8]);
            hold on
            %drawnow
        end
        % 2nd moment of bootstrap number of forecasted number of events
        fStdBst = calc_StdDev(loopout(:,9));
        %
        % Plot the forecast ...
        cumnrf = (1:length(time_asf))';
        cumnr_modelf = [];
        if nMod == 1
            for i=1:length(time_asf)
                if pval1 ~= 1
                    cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_asf(i)+cval1)^(1-pval1));
                else
                    cm = kval1*log(time_asf(i)/cval1+1);
                end
                cumnr_modelf = [cumnr_modelf; cm];
            end % END of FOR on length(time_asf)
        else
            for i=1:length(time_asf)
                if time_asf(i) <= fT1
                    if pval1 ~= 1
                        cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_asf(i)+cval1)^(1-pval1));
                    else
                        cm = kval1*log(time_asf(i)/cval1+1);
                    end
                    cumnr_modelf = [cumnr_modelf; cm];
                else
                    if (pval1 ~= 1 & pval2 ~= 1)
                        cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_asf(i)+cval1)^(1-pval1))+ kval2/(pval2-1)*(cval2^(1-pval2)-(time_asf(i)-fT1+cval2)^(1-pval2));
                    else
                        cm = kval1*log(time_asf(i)/cval1+1) + kval2*log((time_asf(i)-fT1)/cval2+1);
                    end
                    cumnr_modelf = [cumnr_modelf; cm];
                end %END of IF on fT1
            end % End of FOR length(time_asf)
        end % End of if on nMod
        time_asf=sort(time_asf);
        cumnr_modelf=sort(cumnr_modelf);

        pf1 =  plot(time_asf,cumnr_modelf,'g-.','Linewidth',2);
        hold on
        %pf2 =  plot(time_asf,cumnrf, 'b-','Linewidth',2);
        %
        % Plot the  fit to the observed data
        % Cumulative number of observed events
        cumnr = (1:length(time_as))';
        cumnr_model = [];
        if nMod == 1
            for i=1:length(time_as)
                if pval1 ~= 1
                    cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_as(i)+cval1)^(1-pval1));
                else
                    cm = kval1*log(time_as(i)/cval1+1);
                end
                cumnr_model = [cumnr_model; cm];
            end % END of FOR on length(time_as)
        else
            for i=1:length(time_as)
                if time_as(i) <= fT1
                    if pval1 ~= 1
                        cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_as(i)+cval1)^(1-pval1));
                    else
                        cm = kval1*log(time_as(i)/cval1+1);
                    end
                    cumnr_model = [cumnr_model; cm];
                else
                    if (pval1 ~= 1 & pval2 ~= 1)
                        cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_as(i)+cval1)^(1-pval1))+ kval2/(pval2-1)*(cval2^(1-pval2)-(time_as(i)-fT1+cval2)^(1-pval2));
                    else
                        cm = kval1*log(time_as(i)/cval1+1) + kval2*log((time_as(i)-fT1)/cval2+1);
                    end
                    cumnr_model = [cumnr_model; cm];
                end %END of IF on fT1
            end % End of FOR length(time_as)
        end % End of if on nMod
        time_as=sort(time_as);
        cumnr_model=sort(cumnr_model);
        p1 = plot(time_as,cumnr_model,'r','Linewidth',2,'Linestyle','--');
        hold on;
        p2 = plot(time_as,cumnr,'b','Linewidth',2,'Linestyle','--');

        % Plot the forecast from median value
        cumnr_modelmed = [];
        if nMod == 1
            for i=1:length(time_asf)
                if pval1 ~= 1
                    cm = kmed1/(pmed1-1)*(cmed1^(1-pmed1)-(time_asf(i)+cmed1)^(1-pmed1));
                else
                    cm = kmed1*log(time_asf(i)/cmed1+1);
                end
                cumnr_modelmed = [cumnr_modelmed; cm];
            end % END of FOR on length(time_asf)
        else
            for i=1:length(time_asf)
                if time_asf(i) <= fT1
                    if pmed1 ~= 1
                        cm = kmed1/(pmed1-1)*(cmed1^(1-pmed1)-(time_asf(i)+cmed1)^(1-pmed1));
                    else
                        cm = kmed1*log(time_asf(i)/cmed1+1);
                    end
                    cumnr_modelmed = [cumnr_modelmed; cm];
                else
                    if (pmed1 ~= 1 & pmed2 ~= 1)
                        cm = kmed1/(pmed1-1)*(cmed1^(1-pmed1)-(time_asf(i)+cmed1)^(1-pmed1))+ kmed2/(pmed2-1)*(cmed2^(1-pmed2)-(time_asf(i)-fT1+cmed2)^(1-pmed2));
                    else
                        cm = kmed1*log(time_asf(i)/cmed1+1) + kmed2*log((time_asf(i)-fT1)/cmed2+1);
                    end
                    cumnr_modelmed = [cumnr_modelmed; cm];
                end %END of IF on fT1
            end % End of FOR length(time_asf)
        end % End of if on nMod
        time_asf=sort(time_asf);
        cumnr_modelmed=sort(cumnr_modelmed);
        pmedmod =  plot(time_asf,cumnr_modelmed,'y-.','Linewidth',2);

        % Plot observed events in forecast period from endpoint of modeled events in learning period
        vSel = time_asf >= max(time_as);
        vCumnr_forecast = cumnrf(vSel,:);
        vTime_forecast = time_asf(vSel,:);
        % Difference of modelled and observed number of events at time_as
        fDiff_timeas = cumnr_modelmed(length(time_as))-cumnrf(length(time_as));
        vCumnr_forecast = vCumnr_forecast+fDiff_timeas;
        pf3 = plot(vTime_forecast, vCumnr_forecast,'m-.','Linewidth',2);

        xlabel('Time [days]')
        ylabel('Cumulative number of aftershocks')
        xlim([0 max(time_asf)]);

        % Plot standard deviation from bootstrap
        ps2=errorbar(max(time_asf),max(cumnr_modelmed),fStdBst,fStdBst);
        set(ps2,'Linewidth',2,'Color',[1 0 0])
        %
        legend([p2 p1 pf1 pf3 pmedmod min(ps2)],'data','model to data','forecast','observed', 'Median Bst-model', '\sigma (Bst)','location','Best');
        %
        %     % Title
        % Find amount of events in forecast period for modeled data
        nummod2 = max(cumnr_modelf)-cumnr_modelf(length(time_as));
        nummod = max(cumnr_modelmed)-cumnr_modelmed(length(time_as));
        %     % Find amount of  events in forecast period for observed data
        l = time_asf <=time+timef & time_asf > time;
        numreal = sum(l); % observed number of aftershocks
        %     fRc_Flaw = (numreal-nummod)/sigma;
        fRc_Bst2 = (numreal-nummod2)/fStdBst;
        fRc_Bst = (numreal-nummod)/fStdBst;

        % Set line for learning period
        yy = get(gca,'ylim');
        plot([max(time_as) max(time_as)],[0 yy(2)],'k-.')
        string1=['p1 = ' num2str(pval1) '; c1 = ' num2str(cval1) '; k1 = ' num2str(kval1) ];
        string2=['p2 = ' num2str(pval2) '; c2 = ' num2str(cval2) '; k2 = ' num2str(kval2) ];
        string3=['pmed1 = ' num2str(pmed1) '+-' num2str(mStdL(1,1)) '; cmed1 = ' num2str(cmed1) '+-' num2str(mStdL(1,3)) '; kmed1 = ' num2str(kmed1) '+-' num2str(mStdL(1,5))];
        string4=['pmed2 = ' num2str(pmed2) '+-' num2str(mStdL(1,2)) '; cmed2 = ' num2str(cmed2) '+-' num2str(mStdL(1,4)) '; kmed2 = ' num2str(kmed2) '+-' num2str(mStdL(1,6))];
        text(max(time_asf)*0.1,yy(2)*0.9,string1,'FontSize',8);
        text(max(time_asf)*0.1,yy(2)*0.85,string2,'FontSize',8);
        text(max(time_asf)*0.1,yy(2)*0.8,string3,'FontSize',8);
        text(max(time_asf)*0.1,yy(2)*0.75,string4,'FontSize',8);
        string=['\sigma(Bst) = ' num2str(fStdBst) ' Rc(Med) = ' num2str(fRc_Bst) ];%' Rc(Obfit) = ' num2str(fRc_Bst2)];
        text(max(time_asf)*0.1,yy(2)*0.1,string,'FontSize',8);
        %     sInfoStr = ['Omori parameters fitting the original data: ' string1 string2 ...
        %         'Median values from bootstrapping: ' string3 string4];
        %     msgbox(sInfoStr,'Omori law parameters');
        sAIC = ['AIC = ' num2str(fAIC)];
        text(max(time_asf)*0.1,yy(2)*0.05,sAIC,'FontSize',8)
    else
        disp('no result')
    end