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


%
% Smooth point-set registration method using neighboring constraints
% -------------------------------------------------------------------
%
% Authors: Gerard Sanromà, René Alquézar and Francesc Serratosa
%
% Contact: gsanorma@gmail.com
% Date: 15/02/2012
%
% Performs cleaning of a matrix with an extra row and column of zeros
% corresponding to the outlier assignments
%
% Input:
%   S: matrix of benefit coefficients (with extra row and column)
%
% Output:
%   Sd: discrete {0,1}-match matrix
%

function Sd = clean_sinkhorn(S)

S = S - min(S(:)); % translate so that min is zero
S(end,end) = 0;
fils = size(S,1)-1;
cols = size(S,2)-1;
Sd = zeros(fils,cols);

while sum(S(:)) > 0
    [v i] = max(S(:));
    [f,c] = ind2sub(size(S),i);
    if f <= fils, S(f,:) = 0; end
    if c <= cols, S(:,c) = 0; end
    if f <= fils && c<= cols, Sd(f,c) = 1; end
end


function [f,g] = tt_cp_fun(x,Z,Znormsqr)
%TT_CP_FUN Calculate function and gradient for CP fit function.
%
%  [F,G] = TT_CP_FUN(X,Z) where X is a vector containing the entries of the
%  components of the model and Z is the tensor to be fit.
%
%  See also TT_CP_VEC_TO_FAC, TT_FAC_TO_VEC, TT_CP_FG, CP_OPT
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Convert x to a cell array of matrices
A = tt_cp_vec_to_fac(x,Z);

%% Call cp_fit and cp_gradient using cp_fg
[f,G] = tt_cp_fg(Z,A,Znormsqr);

%% Convert a cell array to a vector
g = tt_fac_to_vec(G);


function [Population,FrontNo,WHVLoss] = EnvironmentalSelection(Population,N,wz,AA,RA)
% The environmental selection of I-SIBEA

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Non-dominated sorting
    [FrontNo,MaxFNo] = NDSort(Population.objs,N);
    Next = FrontNo < MaxFNo;

    %% Calculate the WHV loss of each solution front by front
    WHVLoss = CalWHVLoss(Population.objs,FrontNo,wz,AA,RA);

    %% Select the solutions in the last front based on their WHV loss
    Last     = find(FrontNo==MaxFNo);
    [~,Rank] = sort(WHVLoss(Last),'descend');
    Next(Last(Rank(1:N-sum(Next)))) = true;

    %% Population for next generation
    Population = Population(Next);
    FrontNo    = FrontNo(Next);
    WHVLoss    = WHVLoss(Next);
end


function y = logsumexp( varargin )

%LOGSUMEXP    log(sum(exp(x))).
%   LOGSUMEXP(X) = LOG_SUM_EXP(X) = LOG(SUM(EXP(X)). We have replaced this
%   function with LOG_SUM_EXP to better match our function naming
%   conventions. Please start using it instead.

warning( 'CVX:Renamed', [ ...
    'The function "logsumexp" has been renamed "log_sum_exp". Please start\n', ...
    'using the new name. The old name will be removed in a future release.' ], 1 );

y = log_sum_exp( varargin{:} );

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


function dfdx = shadowjacobian(f,x)
% See SDPVAR/jacobian

if isa(f,'double')
    dfdx = zeros(size(f,1),length(x));
    return
end

if ~isempty(intersect(deepdepends(f),depends(x)))
    % Under development
end

if nargin==1
    if isa(f,'sdpvar')
        x = recover(depends(f));
    else
        x = 0;
    end
else
    if length(getvariables(x))<length(x)
      error('x should be a vector of scalar independant variables');
    end
end

[n,m]=size(f);

if m>1
   error('Jacobian only defined for column vectors.')
end

if n*m==1
    dfdx = scalar_jacobian(f,x);
    % Argh, fix this (sorts inside scalar_jacobian
    for i = 1:length(x)
        var(i,1)=getvariables(x(i));
    end
    [i,j]=sort(var);
    dfdx = dfdx(1,j);
    return

else
    dfdx = [];
    AllVars = recover(unique([depends(f) getvariables(x)]));

    for i = 1:length(f)
        dfdx = [dfdx;scalar_jacobian(f(i),x,AllVars)];
    end
    % Argh, fix this (sorts inside scalar_jacobian
    for i = 1:length(x)
        var(i,1)=getvariables(x(i));
    end
    [i,j]=sort(var);
    dfdx = dfdx(:,j);
end

function [dfdx,dummy] = scalar_jacobian(f,x,AllVars)

if isa(f,'double')
    dfdx = zeros(1,length(x));
    return
end

if nargin==2
    AllVars = recover(uniquestripped([depends(f) getvariables(x)]));
    %AllVars = recover(uniquestripped([deepdepends(f) depends(f) getvariables(x)]));
end

exponent_p = exponents(f,AllVars);

coefficients = getbase(f);
coefficients = coefficients(2:end);
coefficients = coefficients(:);
if nnz(exponent_p(1,:))==0
    exponent_p=exponent_p(2:end,:);
end

x_variables = getvariables(x);
AllVars_variables = getvariables(AllVars);
%AllDeriv = [];
AllDeriv2 = [];
for k = 1:length(x)
    wrt = find(ismembc(AllVars_variables,x_variables(k)));
    deriv = exponent_p;
    deriv(:,wrt) = deriv(:,wrt)-1;
    keep{k} = find(deriv(:,wrt)~=-1);
    AllDeriv2 = [AllDeriv2;deriv(keep{k},:)];
end

if size(AllDeriv2,1)==0
    dummy = 1;
else
    dummy = recovermonoms(AllDeriv2,AllVars);
end

top = 1;
dfdx=[];
for k = 1:length(x)
    wrt = find(ismembc(AllVars_variables,x_variables(k)));
    m = length(keep{k});
    if m>0
        poly = sum((coefficients(keep{k}(:)).*exponent_p(keep{k}(:),wrt)).*dummy(top:top+m-1),1);
        top = top + m;
    else
        poly = 0;
    end
    dfdx = [dfdx ; poly];
end
dfdx = dfdx';


% len is frame size, len1 is overlap
function wav = complexSpec2wav(complex_x, framelen, overlap)
nFFT = size(complex_x,2)*2-2;
img = sqrt(-1);

complex_x(:,nFFT/2+2:nFFT) = conj(complex_x(:,nFFT/2:-1:2));
xi = ifft(complex_x');
xi = real(xi);
wav = my_ola(xi, framelen, overlap);

%A = [1 -0.97];
%wav = filter(1, A, wav);
end


classdef GPSO < ALGORITHM
% <single> <real> <large/none> <constrained/none>
% Gradient based particle swarm optimization algorithm

%------------------------------- Reference --------------------------------
% M. M. Noel, A new gradient based particle swarm optimization algorithm
% for accurate computation of global minimum, Applied Soft Computing, 2012,
% 12: 353-359.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB Platform
% for Evolutionary Multi-Objective Optimization [Educational Forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Generate random population
            Population = Problem.Initialization();
            Pbest      = Population;
            [~,best]   = min(FitnessSingle(Pbest));
            Gbest      = LocalSearch(Problem,Pbest(best));

            %% Optimization
            while Algorithm.NotTerminated(Population)
                Population     = OperatorPSO(Problem,Population,Pbest,Gbest);
                replace        = FitnessSingle(Pbest) > FitnessSingle(Population);
                Pbest(replace) = Population(replace);
                [~,best]       = min(FitnessSingle(Pbest));
                Gbest          = LocalSearch(Problem,Pbest(best));
            end
        end
	end
end


function  Result = Metric(I,V,X)
I=double(I);
V=double(V);
X=double(X);
grey_level=256;
Result.Total = [];
Result.EN=entropy_fusion(X,grey_level);
Result.Total = [Result.Total; Result.EN];

Result.MI=mutural_information(I,V,X,grey_level);
Result.Total = [Result.Total; Result.MI];

Result.Q_G=Qp_ABF(I,V,X);
Result.Total = [Result.Total; Result.Q_G];

%Result.FMI = fmi(I,V,X, 'none', 3);  % it is time consuming
%Result.Total = [Result.Total; Result.FMI];


% this script will plot the cumulative moment
% release as a function of time

%  Stefan Wiemer  2/95

report_this_filefun(mfilename('fullpath'));

% open a new figure
figure
set(gcf,'PaperPosition',[2 1 5.5 7.5])

% create the buttons

matdraw

%  Do the calculation
%  newt2 is the currently selected catalog, newt2.Magnitude is the
% vextor containing the magnitudes
c = cumsum( 10.^(1.5*newt2.Magnitude + 16.1));

% plot the results in an xy plot
pl = plot(newt2.Date,c);
set(pl,'LineWidth',2.0)
xlabel('Time in years ','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
ylabel('Cumulative Moment ','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)

% add text -  maybe
%te = text(0.1,0.9,'log10(Mo) = 1.5Ms + 16.1;','Units','normalized','FontWeight','bold')

% change the layout of the axes slightly
set(gca,'visible','on','FontSize',ZmapGlobal.Data.fontsz.m,'FontWeight','bold',...
    'FontWeight','bold','LineWidth',1.5,...
    'Box','on')

hold on
grid


function SO3VF = quadrature(f, varargin)
%
% Syntax
%   SO3VF = SO3VectorFieldHarmonic.quadrature(rot, value)
%   SO3VF = SO3VectorFieldHarmonic.quadrature(f)
%   SO3VF = SO3VectorFieldHarmonic.quadrature(f, 'bandwidth', bw)
%
% Input
%   rot - @rotation, @orientation
%   value - @vector3d
%   f - function handle in @SO3VectorField
%
% Output
%   SO3VF - @SO3VectorFieldHarmonic
%
% Options
%   bw - degree of the Wigner-D functions (default: 128)
%

if isa(f,'rotation')
  v = f;
  y = getClass(varargin,'vector3d'); % function values
  y = y.xyz;
  SO3F = SO3FunHarmonic.quadrature(v, y, varargin{:});
  SO3VF = SO3VectorFieldHarmonic(SO3F);
  return
end

if isa(f,'function_handle')
  [SRight,SLeft] = extractSym(varargin);
  f = SO3FunHandle(f,SRight,SLeft);
end

SO3F = SO3FunHarmonic.quadrature(@(rot) g(rot),f.CS,f.SS,varargin{:});
SO3VF = SO3VectorFieldHarmonic(SO3F);

function g = g(rot)
g = f.eval(rot);
g = g.xyz;
end

end


load('pend_data', 'S', 'n');

lmi = lmistruct(S, n);
lmi = lmi_asym_decay(lmi, 0.5); % |x(t)| < C * exp(-0.5 t)
lmi = lmi_input(lmi, 30, 0.3);  % max 30N force when |x| < 0.3
K = lmi_solve(lmi);

save('pend_data', '-append', 'K');


function hf_out = lhs_operation_joint(hf, samplesf, reg_filter, init_samplef, XH, init_hf, proj_reg)

% This is the left-hand-side operation in Conjugate Gradient

hf_out = cell(size(hf));

% Extract projection matrix and filter separately
P = cellfun(@real, hf(2,1,:), 'uniformoutput',false);
hf = hf(1,1,:);

% Get sizes
num_features = length(hf);
filter_sz = zeros(num_features,2);
for k = 1:num_features
    filter_sz(k,:) = [size(hf{k},1), size(hf{k},2)];
end
[~, k1] = max(filter_sz(:,1));  % Index for the feature block with the largest spatial size
block_inds = 1:num_features;
block_inds(k1) = [];
output_sz = [size(hf{k1},1), 2*size(hf{k1},2)-1];

% Compute the operation corresponding to the data term in the optimization
% (blockwise matrix multiplications)
%implements: A' diag(sample_weights) A f

% sum over all features and feature blocks
pad_sz = cell(1,1,num_features);
sh = mtimesx(samplesf{k1}, permute(hf{k1}, [3 4 1 2]), 'speed');    % assumes the feature with the highest resolution is first
for k = block_inds
    pad_sz{k} = (output_sz - [filter_sz(k,1), 2*filter_sz(k,2)-1]) / 2;

    sh(:,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end) = ...
        sh(:,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end) + mtimesx(samplesf{k}, permute(hf{k}, [3 4 1 2]), 'speed');
end

% weight all the samples
% sh = bsxfun(@times,sample_weights,sh);

% multiply with the transpose
hf_out1 = cell(1,1,num_features);
hf_out1{k1} = permute(conj(mtimesx(sh, 'C', samplesf{k1}, 'speed')), [3 4 2 1]);
for k = block_inds
    hf_out1{k} = permute(conj(mtimesx(sh(:,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end), 'C', samplesf{k}, 'speed')), [3 4 2 1]);
end

% compute the operation corresponding to the regularization term (convolve
% each feature dimension with the DFT of w, and the tramsposed operation)
% add the regularization part
% hf_conv = cell(1,1,num_features);
for k = 1:num_features
    reg_pad = min(size(reg_filter{k},2)-1, size(hf{k},2)-1);

    % add part needed for convolution
    hf_conv = cat(2, hf{k}, conj(rot90(hf{k}(:, end-reg_pad:end-1, :), 2)));

    % do first convolution
    hf_conv = convn(hf_conv, reg_filter{k});

    % do final convolution and put toghether result
    hf_out1{k} = hf_out1{k} + convn(hf_conv(:,1:end-reg_pad,:), reg_filter{k}, 'valid');
end

% Stuff related to the projection matrix

% B * P
BP_cell = cell(1,1,num_features);
for k = 1:num_features
    BP_cell{k} = mtimesx(mtimesx(init_samplef{k}, P{k}, 'speed'), init_hf{k}, 'speed');
end

BP = BP_cell{k1};
for k = block_inds
    BP(1,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end) = ...
        BP(1,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end) + BP_cell{k};
end

% multiply with the transpose: A^H * BP
hf_out{1,1,k1} = hf_out1{k1} +  permute(bsxfun(@times, BP, conj(samplesf{k1})), [3 4 2 1]);

% B^H * BP
fBP = cell(1,1,num_features);
fBP{k1} = reshape(bsxfun(@times, conj(init_hf{k1}), BP), size(init_hf{k1},1), []).';

% Compute proj matrix part: B^H * A_m * f
shBP = cell(1,1,num_features);
shBP{k1} = reshape(bsxfun(@times, conj(init_hf{k1}), sh), size(init_hf{k1},1), []).';

for k = block_inds
    % multiply with the transpose: A^H * BP
    hf_out{1,1,k} = hf_out1{k} +  permute(bsxfun(@times, BP(1,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end), conj(samplesf{k})), [3 4 2 1]);

    % B^H * BP
    fBP{k} = reshape(bsxfun(@times, conj(init_hf{k}), BP(1,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end)), size(init_hf{k},1), []).';

    % Compute proj matrix part: B^H * A_m * f
    shBP{k} = reshape(bsxfun(@times, conj(init_hf{k}), sh(1,1,1+pad_sz{k}(1):end-pad_sz{k}(1), 1+pad_sz{k}(2):end)), size(init_hf{k},1), []).';
end

% hf_out2 = cell(1,1,num_features);
for k = 1:num_features
    fi = size(hf{k},1) * (size(hf{k},2)-1) + 1;  % index where the last frequency column starts

    % B^H * BP
    hf_out2 = 2*real(XH{k} * fBP{k} - XH{k}(:,fi:end) * fBP{k}(fi:end,:)) + proj_reg * P{k};

    % Compute proj matrix part: B^H * A_m * f
    hf_out{2,1,k} = hf_out2 + (2*real(XH{k} * shBP{k} - XH{k}(:,fi:end) * shBP{k}(fi:end,:)));
end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       Anti-smearing function                     %
%       code by Fabrizio Conso, university of pavia, student       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                  %
 function fout = antismear(fin,fs,nfft)
 % fin  = normalized signal frequency
 % fs   = normalized sampling frequency
 % nfft = number of fft points
 format long;
 ts=1/fs;
 c=primes(nfft*ts*fin);
 maxprimes=max(c);
 fout=maxprimes/(nfft*ts);
%                                                                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [filterfunc,winbw] = helper_filtergeneratorfunc(wintype,winCell,fs,bwmul,min_win,trunc_at,audscale,do_subprec,do_symmetric,do_warped)
firwinflags=getfield(arg_firwin,'flags','wintype');
freqwinflags=getfield(arg_freqwin,'flags','wintype');
probelen = 10000;

subprecflag = 'pedantic';
if ~do_subprec, subprecflag = 'nopedantic'; end

switch wintype
    case firwinflags
        winbw=norm(firwin(wintype,probelen)).^2/probelen;
        % This is the ERB-type bandwidth of the prototype

        if do_symmetric
            filterfunc = @(fsupp,fc,scal)...
                         blfilter(winCell,fsupp,fc,'fs',fs,'scal',scal,...
                                  'inf','min_win',min_win,subprecflag);
        else
            fsupp_scale=1/winbw*bwmul;
            filterfunc = @(fsupp,fc,scal)...
                         warpedblfilter(winCell,fsupp_scale,fc,fs,...
                                        @(freq) freqtoaud(freq,audscale),...
                                        @(aud)  audtofreq(aud,audscale),...
                                        'scal',scal,'inf');
        end
        bwtruncmul = 1;
    case freqwinflags
        if do_warped
            error('%s: TODO: Warping is not supported for windows from freqwin.',...
                upper(mfilename));
        end

        probebw = 0.01;

        % Determine where to truncate the window
        H = freqwin(winCell,probelen,probebw);
        winbw = norm(H).^2/(probebw*probelen/2);
        bwrelheight = 10^(-3/10);

        if trunc_at <= eps
            bwtruncmul = inf;
        else
            try
                bwtruncmul = winwidthatheight(abs(H),trunc_at)/winwidthatheight(abs(H),bwrelheight);
            catch
                bwtruncmul = inf;
            end
        end

        filterfunc = @(fsupp,fc,scal)...
                     freqfilter(winCell, fsupp, fc,'fs',fs,'scal',scal,...
                                'inf','min_win',min_win,...
                                'bwtruncmul',bwtruncmul,subprecflag);
end

function width = winwidthatheight(gnum,atheight)

width = zeros(size(atheight));
for ii=1:numel(atheight)
    gl = numel(gnum);
    gmax = max(gnum);
    frac=  1/atheight(ii);
    fracofmax = gmax/frac;

    ind =find(gnum(1:floor(gl/2)+1)==fracofmax,1,'first');
    if isempty(ind)
        %There is no sample exactly half of the height
        ind1 = find(gnum(1:floor(gl/2)+1)>fracofmax,1,'last');
        ind2 = find(gnum(1:floor(gl/2)+1)<fracofmax,1,'first');
        rest = 1-(fracofmax-gnum(ind2))/(gnum(ind1)-gnum(ind2));
        width(ii) = 2*(ind1+rest-1);
    else
        width(ii) = 2*(ind-1);
    end
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  Theta0 = InitialPara_random_MarkovDep_NN(X,K)
if nargin<2
    K=150;
end
[P,N]=size(X);

%--------Initialize Parameter D,S,Z,Pi,gamma_epsi,gamma_s-----------------
D=zeros(P,K);
for k=1:K
    D(:,k)=randn(P,1)*sqrt(1/P);
end

S=zeros(K,N);
for n=1:N
    S(:,n)=randn(K,1);
end

Z = zeros(K,1);
%Z = ones(K,1);
Delta = ones(K,1);
Tao = ones(K,1);
Pi = 0.5*ones(K,1);
gamma_epsi = 1e3*ones(1,N);

%sampe sparse component
gamma_s = 1;
S2 = randn(P,N);
Z2 = zeros(P,N);
Pi2 = 0.5*ones(P,N);

Theta0.D = D;
Theta0.S = S;
Theta0.Z = Z;
Theta0.Delta = Delta;
Theta0.Tao = Tao;
Theta0.Pi = Pi;
Theta0.gamma_epsi = gamma_epsi;
Theta0.S2 = S2;
Theta0.Z2 = Z2;
Theta0.gamma_s = gamma_s;
Theta0.Pi2 = Pi2;
end


%TRPLOT Plot a 3D coordinate frame
%
% TRPLOT(T, OPTIONS) draws a 3D coordinate frame represented by the SE(3) homogeneous
% transform T (4x4).
%
% H = TRPLOT(T, OPTIONS) as above but returns a handle.
%
% TRPLOT(R, OPTIONS) as above but the coordinate frame is rotated about the
% origin according to the orthonormal rotation matrix R (3x3).
%
% H = TRPLOT(R, OPTIONS) as above but returns a handle.
%
% H = TRPLOT() creates a default frame EYE(3,3) at the origin and returns a
% handle.
%
% Animation::
%
% Firstly, create a plot and keep the the handle as per above.
%
% TRPLOT(T, 'handle', H) moves the coordinate frame described by the handle H to
% the pose T (4x4).
%
% Options::
% 'handle',h             Update the specified handle
% 'axhandle',A           Draw in the MATLAB axes specified by the axis handle A
%
% 'color',C              The color to draw the axes, MATLAB ColorSpec
% 'axes'                 Show the MATLAB axes, box and ticks (default true)
% 'axis',A               Set dimensions of the MATLAB axes to A=[xmin xmax ymin ymax zmin zmax]
% 'frame',F              The coordinate frame is named {F} and the subscript on the axis labels is F.
% 'framelabel',F         The coordinate frame is named {F}, axes have no subscripts.
% 'framelabeloffset',O   Offset O=[DX DY] frame labels in units of text box height
% 'text_opts', opt       A cell array of MATLAB text properties
% 'length',s             Length of the coordinate frame arms (default 1)
% 'thick',t              Thickness of lines (default 0.5)
% 'text'                 Enable display of X,Y,Z labels on the frame (default true)
% 'labels',L             Label the X,Y,Z axes with the 1st, 2nd, 3rd character of the string L
% 'rgb'                  Display X,Y,Z axes in colors red, green, blue respectively
% 'rviz'                 Display chunky rviz style axes%
% 'arrow'                Use arrows rather than line segments for the axes
% 'width', w             Width of arrow tips (default 1)
%
% 'perspective'          Display the axes with perspective projection (default off)
% '3d'                   Plot in 3D using anaglyph graphics
% 'anaglyph',A           Specify anaglyph colors for '3d' as 2 characters for
%                        left and right (default colors 'rc'): chosen from
%                        r)ed, g)reen, b)lue, c)yan, m)agenta.
% 'dispar',D             Disparity for 3d display (default 0.1)
% 'view',V               Set plot view parameters V=[az el] angles, or 'auto'
%                        for view toward origin of coordinate frame
% 'lefty'                Draw left-handed frame (dangerous)
%
% Examples::
%
%       trplot(T, 'frame', 'A')
%       trplot(T, 'frame', 'A', 'color', 'b')
%       trplot(T1, 'frame', 'A', 'text_opts', {'FontSize', 10, 'FontWeight', 'bold'})
%       trplot(T1, 'labels', 'NOA');
%
%       h = trplot(T, 'frame', 'A', 'color', 'b');
%       trplot(h, T2);
%
% 3D anaglyph plot
%       trplot(T, '3d');
%
% Notes::
% - Multiple frames can be added using the HOLD command
% - When animating a coordinate frame it is best to set the axis bounds initially.
% - The 'rviz' option is equivalent to 'rgb', 'notext', 'noarrow',
%   'thick', 5.
% - The 'arrow' option requires https://www.mathworks.com/matlabcentral/fileexchange/14056-arrow3

%## 3d homogeneous graphics

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

%TODO:
% 'rviz', chunky RGB lines, no arrows

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: 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.
%
% RTB 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.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

% TODO
%  need to decide how to handle scaling
%  what does hold on mean?  don't touch scaling?

function hout = trplot(T, varargin)

    if nargin == 0
        T = eye(3,3);
    end

    if size(T,3) > 1
        error('SMTB:trplot:badarg', 'trplot cannot operate on a sequence');
    end
    if isa(T, 'SE3')
        T = T.T;
    elseif ~ishomog(T) && ~isrot(T)
        error('SMTB:trplot:badarg', 'trplot operates only on transform (4x4) or rotation matrix (3x3)');
    end

    opt.color = 'b';
    opt.textcolor = [];
    opt.rgb = false;
    opt.axes = true;
    opt.axis = [];
    opt.frame = [];
    opt.framelabel = [];
    opt.text_opts = [];
    opt.view = [];
    opt.width = 1;
    opt.arrow = false;
    opt.labels = 'XYZ';
    opt.axhandle = [];
    opt.anaglyph = 'rc';
    opt.d_3d = false;
    opt.dispar = 0.1;
    opt.thick = 0.5;
    opt.length = 1;
    opt.text = true;
    opt.lefty = false;
    opt.rviz = false;
    opt.framelabeloffset = [0 0];
    opt.handle = [];
    opt.perspective = false;

    [opt,args] = tb_optparse(opt, varargin);

    if opt.arrow && ~exist('arrow3')
        opt.arrow = false;
        warning('SMTB:trplot:badarg', 'arrow option requires arrow3 from FileExchange');
    end

    if isscalar(T) && ishandle(T)
        warning('SMTB:trplot:deprecated', 'Use ''handle'' option');
        % trplot(H, T)
        opt.handle = T; T = args{1};
    end

    % ensure it's SE(3)
    if isrot(T)
        T = r2t(T);
    end

    if ~isempty(opt.handle)
        set(opt.handle, 'Matrix', T);
        % for the 3D case retrieve the right hgtransform and set it
        hg2 = get(opt.handle, 'UserData');
        if ~isempty(hg2)
            set(hg2, 'Matrix', T);
        end
        if nargout > 0
            hout = opt.handle;
        end
        return;
    end

    if opt.rviz
        opt.thick = 5;
        opt.arrow = false;
        opt.rgb = true;
        opt.text = false;
    end

    if opt.rgb && opt.d_3d
        error('SMTB:trplot:badarg', 'cannot specify ''rgb'' and ''3d'', use ''anaglyph'' option');
    end
    if isempty(opt.textcolor)
        opt.textcolor = opt.color;
    end
    if isempty(opt.text_opts)
        opt.text_opts = {};
    end

    if opt.d_3d
        opt.color = ag_color(opt.anaglyph(1));
    end

    % figure the dimensions of the axes, if not given
    if isempty(opt.axis)
        % determine some default axis dimensions

        % get the origin of the frame
        if isrot(T)
            c = [0 0 0];  % at zero for a rotation matrix
        else
            c = transl(T);
        end

        d = 1.2;
        opt.axis = [c(1)-d c(1)+d c(2)-d c(2)+d c(3)-d c(3)+d];

    end

    if ~isempty(opt.axhandle)
        hax = opt.axhandle;
        hold(hax);
    else
        ih = ishold;
        if ~ih
            % if hold is not on, then clear the axes and set scaling
            cla
            if ~isempty(opt.axis)
                axis(opt.axis);
            end
            daspect([1 1 1]);

            if opt.axes
                xlabel( 'X');
                ylabel( 'Y');
                zlabel( 'Z');
                rotate3d on
            end
            new_plot = true;
        end
        hax = gca;
        hold on
    end
    % hax is the handle for the axis we will work with, either new or
    % passed by option 'handle'

    opt.text_opts = [opt.text_opts, 'Color', opt.color];

    if opt.perspective
        hax.Projection = 'perspective';
    end
    hg = hgtransform('Parent', hax);

    % trplot( Q.R, fmt, color);
    if isrot(T)
        T = r2t(T);
    end

    % create unit vectors
    o =  [0 0 0]';
    x1 = opt.length*[1 0 0]';
    y1 = opt.length*[0 1 0]';
    if opt.lefty
        z1 = opt.length*[0 0 -1]';
    else
        z1 = opt.length*[0 0 1]';
    end

    % draw the axes

    mstart = [o o o]';
    mend = [x1 y1 z1]';

    if opt.rgb
        axcolors = {'r', 'g', 'b'};
    else
        axcolors = { opt.color, opt.color, opt.color};
    end

    if opt.arrow
%         % draw the 3 arrows
%         S = [opt.color num2str(opt.width)];
%         ha = arrow3(mstart, mend, S);
%         for h=ha'
%             set(h, 'Parent', hg);
%         end
          daspect([1,1,1])
          for i=1:3
              ha = arrow3(mstart(i,1:3), mend(i,1:3), axcolors{i}, opt.width);
              set(ha, 'Parent', hg);
          end
    else
        for i=1:3
            plot2([mstart(i,1:3); mend(i,1:3)], 'Color', axcolors{i}, ...
                'LineWidth', opt.thick, ...
                'Parent', hg);
        end
    end

    % label the axes
    if isempty(opt.frame)
        fmt = '%c';
    else
        fmt = sprintf('%%c_{%s}', opt.frame);
    end

    if opt.text
        % add the labels to each axis
        h = text(x1(1), x1(2), x1(3), sprintf(fmt, opt.labels(1)), 'Parent', hg);
        set(h, opt.text_opts{:});

        h = text(y1(1), y1(2), y1(3), sprintf(fmt, opt.labels(2)), 'Parent', hg);
        set(h, opt.text_opts{:});

        h = text(z1(1), z1(2), z1(3), sprintf(fmt, opt.labels(3)), 'Parent', hg);
        set(h, opt.text_opts{:});
    end

    if ~isempty(opt.framelabel)
        opt.frame = opt.framelabel;
    end
    % label the frame
    if ~isempty(opt.frame)
        h = text(o(1), o(2), o(3), ...
            ['\{' opt.frame '\}'], 'Parent', hg);
        set(h, 'VerticalAlignment', 'top', ...
            'HorizontalAlignment', 'center', opt.text_opts{:}, ...
            'FontUnits', 'normalized');
        e = get(h, 'Extent');
        d = e(4); % use height of text box as a scale factor
        e(1:2) = e(1:2) - opt.framelabeloffset * d;
        set(h, 'Position', e(1:2));

    end

    if ~opt.axes
        set(gca, 'visible', 'off');
    end
    if ischar(opt.view) && strcmp(opt.view, 'auto')
        cam = x1+y1+z1;
        view(cam(1:3));
    elseif ~isempty(opt.view)
        view(opt.view);
    end
    if isempty(opt.handle) && ~ih
        grid on
        hold off
    end

    % now place the frame in the desired pose
    set(hg, 'Matrix', T);

    if opt.d_3d
        % 3D display.  The original axes are for the left eye, and we add
        % another set of axes to the figure for the right eye view and
        % displace its camera to the right of that of that for the left eye.
        % Then we recursively call trplot() to create the right eye view.

        left = gca;
        right = axes;

        % compute the offset in world coordinates
        off = -t2r(view(left))'*[opt.dispar 0 0]';
        pos = get(left, 'CameraPosition');

        set(right, 'CameraPosition', pos+off');
        set(right, 'CameraViewAngle', get(left, 'CameraViewAngle'));
        set(right, 'CameraUpVector', get(left, 'CameraUpVector'));
        target = get(left, 'CameraTarget');
        set(right, 'CameraTarget', target+off');

        % set perspective projections
        set(left, 'Projection', 'perspective');
        set(right, 'Projection', 'perspective');

        % turn off axes for right view
        set(right, 'Visible', 'Off');

        % set color for right view
        hg2 = trplot(T, 'color', ag_color(opt.anaglyph(2)));

        % the hgtransform for the right view is user data for the left
        % view hgtransform, we need to change both when we rotate the
        % frame.
        set(hg, 'UserData', hg2);
    end

    % optionally return the handle, for later modification of pose
    if nargout > 0
        hout = hg;
    end
end

function out = ag_color(c)

% map color character to an color triple, same as anaglyph.m

    % map single letter color codes to image planes
    switch c
    case 'r'
        out = [1 0 0];        % red
    case 'g'
        out = [0 1 0];        % green
    case 'b'
        % blue
        out = [0 0 1];
    case 'c'
        out = [0 1 1];        % cyan
    case 'm'
        out = [1 0 1];        % magenta
    case 'o'
        out = [1 1 0];        % orange
    end
end


function xyz_ls=plot_ROI_3D(CONST,idlist)
% plot yx,yx image
% Note imagesc yx axis are mirrored to scatter3d
% change y to -y to match the two

marker_size=64;

z_ratio=19.7; % adjust scale between z and xy

Nid=length(idlist);
xyz_ls=zeros(3,Nid);

% for i=1:Nid
%     center_i=CONST.CIF(idlist(i));
%     xyz_ls(3,i)=center_i.slice*z_ratio;
%     xyz_ls(2,i)=center_i.center(1);
%     xyz_ls(1,i)=center_i.center(2);
% end;
CIFs=CONST.CIF(idlist);
xyz_ls(3,:)=[CIFs.slice]*z_ratio;
xyz_ls([2,1],:)=reshape([CIFs.center],2,[]);

marker_option={marker_size,'b','filled'};

scatter3(xyz_ls(1,:),-xyz_ls(2,:),xyz_ls(3,:),marker_option{:});
axis equal;

end


function [multiLineCols] = PTlinecmap(nColors)
%% [multiLineCols] = PTlinecmap(nColors)
% input number of color lines, output colormap

% ----------------------------------------------------------------------------------
% "THE BEER-WARE LICENSE" (Revision 42):
% <brian.white@queensu.ca> wrote this file. As long as you retain this notice you
% can do whatever you want with this stuff. If we meet some day, and you think
% this stuff is worth it, you can buy me a beer in return. -Brian White
% ----------------------------------------------------------------------------------

    cmap=flipud(colormap(jet));
    multiLineCols=(downsample(cmap,ceil(length(cmap)/nColors)));
    for i = find(multiLineCols(:,1) > .5 & multiLineCols(:,2) > .7 & multiLineCols(:,3) < .3)
        multiLineCols(i,:) = multiLineCols(i,:) * .78;
    end
    round(100/nColors)
    multiLineCols = repmat(multiLineCols, round(100/nColors),1);% repeats colormap to be 100 rows long

end


function index_test01 ( )

%*****************************************************************************80
%
%% INDEX_TEST01 tests INDEX0 and INDEX1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 November 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'INDEX_TEST01\n' );
  fprintf ( 1, '  INDEX0 indexes a 1D array with zero base,\n' );
  fprintf ( 1, '  INDEX1 indexes a 1D array with  unit base.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             Min Index   Max\n' );
  fprintf ( 1, '\n' );

  i_min = 1;
  i = 3;
  i_max = 5;
  fprintf ( 1, '  1D Index  %4d  %4d  %4d\n', i_min,     i,     i_max );

  value = index0 ( i_min, i, i_max );
  index_min = 0;
  index_max = index_min + i_max - i_min;
  fprintf ( 1, '  Index0:   %4d  %4d  %4d\n', index_min, value, index_max );

  value = index1 ( i_min, i, i_max );
  index_min = 1;
  index_max = index_min + i_max - i_min;
  fprintf ( 1, '  Index1:   %4d  %4d  %4d\n', index_min, value, index_max );

  return
end


function t = toeplitz( c, r )

%   Disciplined convex/geometric programming information for TOEPLITZ:
%      TOEPLITZ imposes no convexity restrictions on its arguments.
%      Instead of using the TOEPLITZ function, however, consider
%      creating a matrix variable using the 'toeplitz' keyword; e.g.
%          variable X(5,5) toeplitz;

%
% Check arguments
%

if nargin < 2,
    c    = vec( c );
    m    = length( c );
    p    = m;
    x    = [ cvx_subsref( c, p : -1 : 1 ) ; conj( cvx_subsref( c, 2 : p ) ) ];
else
    temp = cvx_subsref( r, 1 ) - cvx_subsref( c, 1 );
    if ~cvx_isconstant( temp ) || cvx_constant( temp ) ~= 0,
        warning('MATLAB:toeplitz:DiagonalConflict',['First element of ' ...
               'input column does not match first element of input row. ' ...
               '\n         Column wins diagonal conflict.'])
    end
    r = vec( r );
    c = vec( c );
    p = length( r );
    m = length( c );
    x = [ cvx_subsref( r, p : -1 : 2 ) ; c ];
end

%
% Construct matrix
%

cidx = ( 0 : m - 1 )';
ridx = p : -1 : 1;
t    = cidx( :, ones( p, 1 ) ) + ridx( ones( m, 1 ) , : );
t    = reshape( cvx_subsref( x, t ), size( t ) );

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


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 X = rocholTransMultiply(ch, Y);

% ROCHOLTRANSMULTIPLY Multiply by the transposed version of the rank one Cholesky.
% ROCHOL

%/~
% This would be the long way of doing it.
%L = rocholExtract(ch);
%X1 = L'*Y;
%~/
if size(Y, 1) ~= ch.n
  error('Inner matrix dimensions do not match');
end
X = zeros(size(Y));
t = zeros(1, size(Y, 2));
X(ch.n, :) = Y(ch.n, :)*ch.s(ch.n);
for i = ch.n-1:-1:1
  t = t + ch.v(i+1)*Y(i+1, :);
  X(i, :) = ch.s(i)*(Y(i, :)+ ch.u(i)*t);
end

%/~
%disp(max(max(X - X1)));
%~/


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


function  g =  lfmGradientSigmaH4AP(gamma1_p, gamma1_m, sigma2, t1, ...
    preFactor, preExp, mode)

% LFMGRADIENTSIGMAH4AP Gradient of the function h_i(z) with respect \sigma.
% FORMAT
% DESC Computes the gradient of the function h_i(z) with respect to the
% length-scale of the input "force", \sigma.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode: indicates in which way the vectors t1 and t2 must be transposed
% RETURN g : Gradient of the function with respect to \sigma.
%
% COPYRIGHT : Mauricio Alvarez, 2010

% KERN

if mode==0
    g =  lfmapGradientSigmaUpsilonVector(gamma1_p,sigma2, t1)*( preExp(:,1)/preFactor(1) - preExp(:,2)/preFactor(2)).' ...
        + lfmapGradientSigmaUpsilonVector(gamma1_m,sigma2, t1)*( preExp(:,2)/preFactor(3) - preExp(:,1)/preFactor(4)).';
else
    g =  lfmGradientSigmaUpsilonVector(gamma1_p,sigma2, t1)*(preExp(:,1)/preFactor(1) - preExp(:,2)/preFactor(2)).' ...
        + lfmGradientSigmaUpsilonVector(gamma1_m,sigma2, t1)*(preExp(:,2)/preFactor(3) - preExp(:,1)/preFactor(4)).';
end


function is=lpcrf2is(rf)
%LPCRF2IS Convert reflection coefficients to inverse sines IS=(RF)

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

is=asin(rf)*2/pi;


%% Default template
%% Visualize the Data

plot(ebsd)

%% Calculate an ODF

odf = calcODF(ebsd)

%% Detect grains

%segmentation angle
segAngle = 10*degree;

grains = calcGrains(ebsd,'threshold',segAngle);

%% Orientation of Grains

plot(grains)