function [img_pca,img_pca_null]=connectopic_laplacian(s,ind_ins,N,Vn,Null)

% Input:
% s: similarity matrx
% ind_ins: index of subcortical voxels
% N: size of image (MNI152:91x109x91)
% K: degree, used when Disparity=1
% Vn: index of gradient to compute: Vn=2 -> Gradint I; Vn=3 -> Gradient II; Vn=4 -> Gradient III
% Null: property of null data,structural variable including the following:

% Null.NumNull: Number of randomizations if compute the null data
% Null.T: number of time points in the empirical data
% Null.FWHM: Gaussian smooth kernel used in the empirical data
% Null.voxelsize: voxel size in mm in the empirical data

% Output:
% img_pca: eigenmap (main output)
% img_pca_null: eigenmap computed from null data

%Global thresholding. Haak et al 2017
img_pca=zeros(N);
w=squareform(pdist(s));  %similarity to distance mapping

fprintf('Thresholding to minimum density needed for graph to remain connected\n');
ind_upper=find(triu(ones(length(w),length(w)),1));
[~,ind_srt]=sort(w(ind_upper));
w_thresh=zeros(length(w),length(w));
dns=linspace(0.001,1,1000);
for i=1:length(dns)
    ttl=ceil(length(ind_upper)*dns(i));
    w_thresh(ind_upper(ind_srt(1:ttl)))=s(ind_upper(ind_srt(1:ttl)));
    [~,comp_sizes]=get_components(~~w_thresh+~~w_thresh');
    if length(comp_sizes)==1
        break
    end
end

fprintf('Density=%0.2f%%\n',100*(length(find(~~w_thresh))/length(ind_upper)));
dns=dns(i);
w_thresh=w_thresh+w_thresh';

fprintf('Computing Laplacian\n');
L=diag(sum(w_thresh))-w_thresh;

fprintf('Finding eigenvectors\n');
[v,d]=eig(L);d=diag(d);

% Variance explained
per=1./d(2:end);
per=per/sum(per)*100;

%figure; plot(per(1:20));

if v(1,Vn)>v(end,Vn)
    y=v(:,Vn);
else
    y=-v(:,Vn);
end
min_val=min(y);
y=y-min_val;
img_pca(ind_ins)=y;

if nargin==5
    fprintf('Null Model: Synthetic data + MST + Geometry\n');
    NumNull=Null.NumNull;
    T=Null.T;
    FWHM=Null.FWHM;
    voxelsize=Null.voxelsize;

    % Reference matrix
    fprintf('Generating reference matrix\n')
    M=zeros(length(ind_ins),length(ind_ins));
    for i=1:length(ind_ins)
        for j=1:length(ind_ins)
            [xx1,yy1,zz1]=ind2sub(N,ind_ins(i));
            [xx2,yy2,zz2]=ind2sub(N,ind_ins(j));
            dd=sqrt((xx1-xx2)^2 + (yy1-yy2)^2 + (zz1-zz2)^2);
            if dd<=sqrt(2)
                M(i,j)=1;
            end
        end
    end
    ind_m_upper=find(triu(M,1)); % All the available locations (neighboring only)

    % Randomizations

    img_pca_null=zeros([N,NumNull]);
    for nn=1:NumNull
        fprintf('Simulating random data %d\n',nn)
        x=randn([N,T]);
        x_ins=zeros(T,length(ind_ins));
        frst=0;
        for i=1:T
            x(:,:,:,i)=imgaussfilt3(x(:,:,:,i),FWHM/voxelsize/2.355);
            tmp=x(:,:,:,i);
            x_ins(i,:)=tmp(ind_ins);
            show_progress(i,T,frst);frst=1;
        end
        clear x
        % Normalization
        x_ins=detrend(x_ins,'constant'); x_ins=x_ins./repmat(std(x_ins),T,1);

        % Correlation
        c=x_ins'*x_ins; c=c/T;z=atanh(c);

        [~,ind_srt_z]=sort(z(ind_upper),'descend');
        ind_z_upper=ind_upper(ind_srt_z(1:ttl)); % Available locations;ttl is computed from acutual data

        % MST ensures that the graph is fully connected
        % Only allow MST found in the neighboring locations
        ss=zeros(size(s));
        ss(ind_m_upper)=rand(length(ind_m_upper),1);
        ss=ss+ss';

        mst=adjacency(minspantree(graph(ss)));
        ind_mst_upper=find(triu(mst,1));

        % Randomise top weighted edges.
        ind_srt_ttl=ind_upper(ind_srt(1:ttl));
        ind_rand=randperm(length(ind_srt_ttl));
        ind_srt_ttl=ind_srt_ttl(ind_rand);

        % Add edges to mst locations first
        Nm=length(ind_mst_upper);
        w_thresh_null=zeros(size(s));
        w_thresh_null(ind_mst_upper)=s(ind_srt_ttl(1:Nm));

        % Then, add remaining edges to remaining desired locations
        ind_remain=setdiff(ind_z_upper,ind_mst_upper);
        ind_rand=randperm(length(ind_remain));
        ind_remain=ind_remain(ind_rand);
        w_thresh_null(ind_remain(1:ttl-Nm))=s(ind_srt_ttl(Nm+1:ttl));

        [~,comp_sizes]=get_components(~~w_thresh_null+~~w_thresh_null');
        if length(comp_sizes)~=1
            fprintf('Warning: Null model is not fully connected\n')
        end
        dns_null=length(find(~~w_thresh_null))/length(ind_upper);
        fprintf('Density null=%0.2f%%\n',dns_null*100);

        w_thresh_null=w_thresh_null + w_thresh_null';

        fprintf('Computing Laplacian\n');
        L=diag(sum(w_thresh_null))-w_thresh_null;

        fprintf('Finding eigenvectors\n');
        [v,d]=eig(L);d=diag(d);

        if v(1,Vn)>v(end,Vn)
            y=v(:,Vn);
        else
            y=-v(:,Vn);
        end
        min_val_null=min(y);
        y=y-min_val_null;
        tmp=zeros(N);
        tmp(ind_ins)=y;
        img_pca_null(:,:,:,nn)=tmp;

    end
else
    img_pca_null=[];
end


function outpoints = icbm_spm2tal(inpoints)
%
% This function converts coordinates from MNI space (normalized
% using the SPM software package) to Talairach space using the
% icbm2tal transform developed and validated by Jack Lancaster
% at the Research Imaging Center in San Antonio, Texas.
%
% http://www3.interscience.wiley.com/cgi-bin/abstract/114104479/ABSTRACT
%
% FORMAT outpoints = icbm_spm2tal(inpoints)
% Where inpoints is N by 3 or 3 by N matrix of coordinates
% (N being the number of points)
%
% ric.uthscsa.edu 3/14/07

% find which dimensions are of size 3
dimdim = find(size(inpoints) == 3);
if isempty(dimdim)
  error('input must be a N by 3 or 3 by N matrix')
end

% 3x3 matrices are ambiguous
% default to coordinates within a row
if dimdim == [1 2]
  disp('input is an ambiguous 3 by 3 matrix')
  disp('assuming coordinates are row vectors')
  dimdim = 2;
end

% transpose if necessary
if dimdim == 2
  inpoints = inpoints';
end

% Transformation matrices, different for each software package
icbm_spm = [0.9254 0.0024 -0.0118 -1.0207
	   	   -0.0048 0.9316 -0.0871 -1.7667
            0.0152 0.0883  0.8924  4.0926
            0.0000 0.0000  0.0000  1.0000];

% apply the transformation matrix
inpoints = [inpoints; ones(1, size(inpoints, 2))];
inpoints = icbm_spm * inpoints;

% format the outpoints, transpose if necessary
outpoints = inpoints(1:3, :);
if dimdim == 2
  outpoints = outpoints';
end


function [projmat_art,D] = tomoproj_art_2(im,angles)
%TOMOPROJ_ART   [projmat_art,D] = tomoproj_art(im,angles)
%   unit of angles: Degree;
%   projection direction: When angle == 0, X-ray passes through up-down
%    direction;
%
%   Phymhan
%   02-Aug-2013 14:07:06

%Pad image
[im_pad,D] = impad(im);
%Calculate projection
num_proj = length(angles);
projmat_art = zeros(num_proj,2*D);
for k = 1:num_proj
    im_rot = imrotate(im_pad,-angles(k),'bilinear','crop');
    projmat_art(k,  1:  D) = sum(im_rot,1) ;
    projmat_art(k,D+1:2*D) = sum(im_rot,2)';
end
end


function collocation_test ( )

%*****************************************************************************80
%
%% COLLOCATION_TEST tests the COLLOCATION library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 March 2015
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'COLLOCATION_TEST\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Test the COLLOCATION library.\n' );

  collocation_test01 ( );
  collocation_test02 ( );
  collocation_test03 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'COLLOCATION_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function X = ttm(X,V,varargin)
%TTM Tensor times matrix for ktensor.
%
%   Y = TTM(X,A,N) computes the n-mode product of the ktensor X with a
%   matrix A; i.e., X x_N A.  The integer N specifies the dimension
%   (or mode) of X along which A should be multiplied.  If size(A) =
%   [J,I], then X must have size(X,N) = I.  The result will be a
%   ktensor of the same order and size as X except that size(Y,N) = J.
%
%   Y = TTM(X,{A,B,C,...}) computes the n-mode product of the ktensor
%   X with a sequence of matrices in the cell array.  The n-mode
%   products are computed sequentially along all dimensions (or modes)
%   of X. The cell array contains ndims(X) matrices.
%
%   Y = TTM(X,{A,B,C,...},DIMS) computes the sequence tensor-matrix
%   products along the dimensions specified by DIMS.
%
%   Y = TTM(...,'t') performs the same computations as above except
%   the matrices are transposed.
%
%   Examples
%   X = ktensor({rand(5,2),rand(3,2),rand(4,2),rand(2,2)});
%   A = rand(4,5); B = rand(4,3); C = rand(3,4); D = rand(3,2);
%   Y = ttm(X, A, 1)         %<-- computes X times A in mode-1
%   Y = ttm(X, {A,B,C,D}, 1) %<-- same as above
%   Y = ttm(X, A', 1, 't')   %<-- same as above
%   Y = ttm(X, {A,B,C,D}, [1 2 3 4]) %<-- 4-way multiply
%   Y = ttm(X, {D,C,B,A}, [4 3 2 1]) %<-- same as above
%   Y = ttm(X, {A,B,C,D})            %<-- same as above
%   Y = ttm(X, {A',B',C',D'}, 't')   %<-- same as above
%   Y = ttm(X, {C,D}, [3 4])     %<-- X times C in mode-3 & D in mode-4
%   Y = ttm(X, {A,B,C,D}, [3 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, [1 2 4])   %<-- 3-way multiply
%   Y = ttm(X, {A,B,C,D}, [1 2 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, -3)        %<-- same as above
%   Y = ttm(X, {A,B,C,D}, -3)      %<-- same as above
%
%   See also KTENSOR, KTENSOR/TTV, TENSOR/TTM.
%
%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

%%%%%%%%%%%%%%%%%%%%%%
%%% ERROR CHECKING %%%
%%%%%%%%%%%%%%%%%%%%%%

% Check the number of arguments
if (nargin < 2)
    error('TTM requires at least two arguments.');
end

% Check for transpose option
isTranspose = false;
if numel(varargin) > 0
  if isnumeric(varargin{1});
    dims = varargin{1};
  end
  isTranspose =  (ischar(varargin{end}) && (varargin{end} == 't'));
end

% Check for dims argument
if ~exist('dims','var')
    dims = [];
end

% Check that 2nd argument is cell array. If not, recall with V as a
% cell array with one element.
if ~iscell(V)
    X = ttm(X,{V},dims,varargin{end});
    return;
end

% Get sorted dims and index for multiplicands
[dims,vidx] = tt_dimscheck(dims,ndims(X),numel(V));

% Determine correct size index
if isTranspose
  j = 1;
else
  j = 2;
end

% Check that each multiplicand is the right size.
for i = 1:numel(dims)
    if (ndims(V) ~= 2) || (size(V{vidx(i)},j) ~= size(X,dims(i)))
disp(size(V{vidx(i)}))
disp(size(X))

        error('Multiplicand is wrong size');
    end
end

% Do the multiplications in the specified modes.
for i = 1:numel(dims)
  if isTranspose
    X.u{dims(i)} = V{vidx(i)}'* X.u{dims(i)};
  else
    X.u{dims(i)} = V{vidx(i)} * X.u{dims(i)};
  end
end


function [xu,yu,xl,yl,xc,yc]=GenerateNACASeries5Airfoil(afid,dotcount,xmod)

%--------------------------------------------------------------------------
%GenerateNACASeries5Airfoil
%Version 1.20
%Created by Stepen (zerocross_raptor@yahoo.com)
%Created 20 November 2010
%Last modified 30 November 2011
%--------------------------------------------------------------------------
%GenerateNACASeries5Airfoil generates the airfoil vertexes' coordinate of
%the given NACA Series 5 Airfoil. The math equation used to generates the
%airfoil coordinates is based on Theory of Wing Section Chapter 6 by Abbott
%and Doenhoff.
%--------------------------------------------------------------------------
%Syntax:
%[xu,yu,xl,yl,xc,yc]=GenerateNACASeries5Airfoil(afid,dotcount,xmod)
%Input argument:
%- afid (1 x 5 str) specifies NACA Series 5 Airfoil identifier.
%- dotcount (1 x 1 int) specifies the number of vertexes to be generated on
%  the airfoil's camber/mean line.
%- xmod (str) specifies the mode of airfoil vertex distribution. Enter
%  'Uniform' to create uniform vertex distribution or 'Cosine' to create
%  vertex distribution based on cosine function (More vertex on the leading
%  edge region).
%Output argument:
%- xu (i x 1 num) specifies the x axis location of airfoil's upper surface
%  vertexes in fraction of chord. The airfoil's upper surface vertex are
%  arranged from leading edge (the first element of xu) to the trailing
%  edge (the last element of xu).
%- yu (i x 1 num) specifies the y axis location of airfoil's upper surface
%  vertexes in fraction of chord. The airfoil's upper surface vertex are
%  arranged from leading edge (the first element of yu) to the trailing
%  edge (the last element of yu).
%- xl (i x 1 num) specifies the x axis location of airfoil's lower surface
%  vertexes in fraction of chord. The airfoil's lower surface vertex are
%  arranged from leading edge (the first element of xl) to the trailing
%  edge (the last element of xl).
%- yl (i x 1 num) specifies the y axis location of airfoil's lower surface
%  vertexes in fraction of chord. The airfoil's lower surface vertex are
%  arranged from leading edge (the first element of yl) to the trailing
%  edge (the last element of yl).
%- xc (i x 1 num) specifies the x axis location of airfoil's camber line
%  vertexes in fraction of chord. The airfoil's camber line vertex are
%  arranged from leading edge (the first element of xc) to the trailing
%  edge (the last element of xc).
%- yc (i x 1 num) specifies the y axis location of airfoil's camber line
%  vertexes in fraction of chord. The airfoil's camber line vertex are
%  arranged from leading edge (the first element of yc) to the trailing
%  edge (the last element of yc).
%--------------------------------------------------------------------------

%CodeStart-----------------------------------------------------------------
%Checking input afid
    if ~ischar(afid)
        error('Airfoil identifier must be a string!')
    end
    if numel(afid)~=5
        error('Airfoil identifier must be a 5 digit number!')
    end
    if isempty(str2double(afid))
        error('Airfoil identifier must be a 5 digit number!')
    end
    id=str2double(afid([2,3]));
    if (id~=10)&&(id~=20)&&(id~=30)&&(id~=40)&&(id~=50)
        error(['Airfoil identifier must start with X10,',...
               ' X20, X30, X40, or X50!'])
    end
%Checking input dotcount
    if numel(dotcount)~=1
        error('Number of vertex must be scalar!')
    end
    if (mod(dotcount,1~=0))||(dotcount<0)
        error('Number of vertex must be positive integer!')
    end
%Checking input xmod
    if nargin<3
        xmod='Cosine';
    end
    if (~strcmpi(xmod,'Uniform'))&&(~strcmpi(xmod,'Cosine'))
        error('Vertex distribution input must be Uniform or Cosine!')
    end
%Declaring look-up table
    mtable=[0.0580,0.1260,0.2025,0.2900,0.3910];
    ktable=[361.4000,51.6400,15.9570,6.6430,3.2300];
%Assigning identifier to equation coefficient
    id1=str2double(afid(1));
    id2=str2double(afid([2,3]));
    id3=str2double(afid([4,5]));
    cl=id1*(3/2)*(10/100);      %Design lift coefficient
    p=id2*(1/2)*(1/100);        %Maximum camber location
    t=id3*(1/100);              %Maximum thickness
    if t==0
        warning(['Zero thickness airfoil!',...
                 ' Airfoil will be just a camber line!'])
    end
    m=mtable(str2double(afid(2)));
    k=ktable(str2double(afid(2)));
%Calculating x-axis location of camber line vertexes
    panelcount=dotcount-1;
    if strcmpi(xmod,'Uniform')
        panellength=1/panelcount;
        xc=(0:panellength:1)';
    elseif strcmpi(xmod,'Cosine')
        deltadeg=90/panelcount;
        xc=1-cosd(0:deltadeg:90)';
    end
%Preallocating array for speed
    yc=zeros(dotcount,1);
    gc=zeros(dotcount,1);
    yt=zeros(dotcount,1);
    xu=zeros(dotcount,1);
    xl=zeros(dotcount,1);
    yu=zeros(dotcount,1);
    yl=zeros(dotcount,1);
%Calculating y-axis location of camber line vertexes and camber gradient
    if m~=0
        for i=1:1:dotcount
            if xc(i)<=p
                yc(i)=(k/6)*((xc(i)^3)-...
                             (3*m*(xc(i)^2))+...
                             ((m^2)*(3-m)*xc(i)));
                gc(i)=(k/6)*((2*(xc(i)^2))-...
                             (6*m*xc(i))+...
                             (((3*(m^2))-(m^3))));
            elseif xc(i)>p
                yc(i)=(k/6)*(m^3)*(1-xc(i));
                gc(i)=-1*(k/6)*(m^3);
            end
        end
    end
%Correcting camber line vertexes and camber gradient for extended series
    yc=yc*(id1/2);
    gc=gc*(id1/2);
%Converting camber gradient to camber slope and normal
    sc=atand(gc);
%Calculating thickness distribution
    for i=1:1:dotcount
        yt(i)=5*t*((0.29690*(xc(i)^0.5))-...
                   (0.12600*xc(i))-...
                   (0.35160*(xc(i)^2))+...
                   (0.28430*(xc(i)^3))-...
                   (0.10150*(xc(i)^4)));
    end
%Generating airfoil vertexes
    for i=1:1:dotcount
        xu(i)=xc(i)-yt(i)*sind(sc(i));
        yu(i)=yc(i)+yt(i)*cosd(sc(i));
        xl(i)=xc(i)+yt(i)*sind(sc(i));
        yl(i)=yc(i)-yt(i)*cosd(sc(i));
    end
%CodeEnd-------------------------------------------------------------------

end


function [PSNR, SSIM, IFC] = evaluate_SR(img_GT, img_HR, scale, compute_ifc)
% -------------------------------------------------------------------------
%   Description:
%       Compute PSNR, SSIM and IFC for SR
%       We convert RGB image to grayscale and crop boundaries for 'scale'
%       pixels
%
%   Input:
%       - img_GT        : Ground truth image
%       - img_HR        : predicted HR image
%       - scale         : upsampling scale
%       - compute_ifc   : evaluate IFC [default = 0 since it's slow]
%
%   Citation:
%       Fast and Accurate Image Super-Resolution with Deep Laplacian Pyramid Networks
%       Wei-Sheng Lai, Jia-Bin Huang, Narendra Ahuja, and Ming-Hsuan Yang
%       arXiv, 2017
%
%   Contact:
%       Wei-Sheng Lai
%       wlai24@ucmerced.edu
%       University of California, Merced
% -------------------------------------------------------------------------

    if ~exist('compute_ifc', 'var')
        compute_ifc = 0;
    end

    %% quantize pixel values
    img_GT = im2double(im2uint8(img_GT));
    img_HR = im2double(im2uint8(img_HR));

    %% convert to gray scale
    if( size(img_GT, 3) > 1 )
        img_GT = rgb2ycbcr(img_GT); img_GT = img_GT(:, :, 1);
        img_HR = rgb2ycbcr(img_HR); img_HR = img_HR(:, :, 1);
    end

    %% crop boundary
    img_GT = shave_bd(img_GT, scale);
    img_HR = shave_bd(img_HR, scale);

    % evaluate
    PSNR = psnr(img_GT, img_HR);
    SSIM = ssim(img_GT, img_HR);

    % comment IFC to speed up testing
    IFC = 0;
    if compute_ifc
        IFC = ifcvec(img_GT, img_HR);
        if( ~isreal(IFC) )
            IFC = 0;
        end
    end

end


function K = white_noise_correlation ( s, t )

%*****************************************************************************80
%
%% WHITE_NOISE_CORRELATION evaluates the white_noise correlation function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Petter Abrahamsen,
%    A Review of Gaussian Random Fields and Correlation Functions,
%    Norwegian Computing Center, 1997.
%
%  Parameters:
%
%    Input, real S(*), T(*), pairs of argument values.
%
%    Output, real K(*), the correlation function values
%
  K = zeros ( size ( s ) );

  i = find ( s == t );

  K(i) = 1.0;

  return
end


function shadedTimeSeries(X, Ydata, indicator, Xlabel, Ylabels, colour, Yspacing)
% SHADEDTIMESERIES(X, Ydata, indicator, Xlabel, Ylabels, colour, Yspacing)
%
% Plot time series one above the other with coloured strips highlighting
% interesting features.
%
% All data is in columns.
%
% X is the time vector.
% Each column of Ydata will be plotted in a separate subplot, one beneath
% the other.
% Indicator is a logical array where the ones indicate the areas of
% interest that will be shaded.
% Xlabel is a string for labeling the bottom horizontal axis.
% Ylabels is a cell array of strings used for labeling the Y axes.
% Colour is an RGB colour array that will be used for shading.
% Yspacing is the percentage of vertical padding to leave above and below
% each plot so that they don't touch the top and bottom of their respective
% plots.
%
% If Ydata has only one column then the function will draw in the current
% plot or subplot; this is useful if you want to organise your own plots
% and subplots but still use the shading feature. If Ydata has more than
% one column then the function will create a new figure with a subplot for
% each column of Ydata.
%
% Example 1 - same indicator for all subplots:
%           time = [1:50]';
%           acceleration = rand(50,1);
%           velocity = cumtrapz(acceleration, time);
%           position = cumtrapz(velocity, time);
%           indicator = [zeros(1,10) ones(1,10) zeros(1,25) ones(1,5)]';
%           shadedTimeSeries(time, [acceleration velocity position], indicator, 'Time', {'Acceleration' 'Velocity' 'Position'}, [0 0.8 0.3], 10);
%
%
% Example 2 - different indicators for each subplot:
%           time = [1:50]';
%           acceleration = rand(50,1);
%           velocity = cumtrapz(acceleration, time);
%           position = cumtrapz(velocity, time);
%           indicator = [zeros(1,10) ones(1,10) zeros(1,25) ones(1,5); ones(1,3) zeros(1,10) ones(1,30) zeros(1,7); zeros(1,20) ones(1,20) zeros(1,10)]';
%           shadedTimeSeries(time, [acceleration velocity position], indicator, 'Time', {'Acceleration' 'Velocity' 'Position'}, [0 0.8 0.3], 10);
%
%
%
% Example 3 - single plot at a time for full control:
%           time = [1:50]';
%           acceleration = rand(50,1);
%           velocity = cumtrapz(acceleration, time);
%           position = cumtrapz(velocity, time);
%           indicator = [zeros(1,10) ones(1,10) zeros(1,25) ones(1,5)]';
%           figure;
%           subplot(3,1,1);
%           shadedTimeSeries(time, acceleration, indicator, 'Time', {'Acceleration'}, [1 .7 1], 10);
%           hold on
%           text(10,1,'Custom text.');
%           subplot(3,1,2);
%           shadedTimeSeries(time, velocity, indicator, 'Time', {'Velocity'}, [1 .7 1], 10);
%           xlabel('');
%           subplot(3,1,3);
%           shadedTimeSeries(time, position, indicator, 'Time', {'Position'}, [1 .7 1], 10);
%           grid on;
%
% Carl Fischer. March 2011.
% http://eis.comp.lancs.ac.uk/~carl/blog/

if nargin < 3
    error('Please provide at least X, Ydata and indicator parameters.');
end
% Check consistency of compulsory parameters first.
if length(X) ~= size(Ydata,1)
    if length(X) == size(Ydata,2)
        Ydata = Ydata';
    else
        error('X and Ydata must have the same length.');
    end
end
if length(X) ~= size(indicator,1)
    if length(X) == size(indicator,2)
        indicator = indicator';
    else
        error('X and indicator must have the same length.');
    end
end
if nargin < 4
    Xlabel = '';
end
if nargin < 5
    Ylabels = cell(1,size(Ydata,2));
end
if nargin < 6 || isempty(colour)
    colour = [0 .7 .7];
end
if nargin < 7 || isempty(Yspacing)
    Yspacing = 5;
end

if length(Ylabels) ~= size(Ydata,2)
    error('The number of Ylabels should match the number of data traces in Ydata.');
end
if size(indicator,2) > 1 && size(indicator,2) ~= size(Ydata,2)
    error('If there is more than one column in indicator, their number must match the number of columns in Ydata.');
end

for column_idx = 1:size(indicator,2)
    start_marks{column_idx} = find(diff(indicator(:,column_idx)) > 0);
    end_marks{column_idx} = find(diff(indicator(:,column_idx)) < 0);
    if start_marks{column_idx}(1) > end_marks{column_idx}(1) % plot is shaded from start
        start_marks{column_idx} = [1; start_marks{column_idx}];
    end
    if start_marks{column_idx}(end) > end_marks{column_idx}(end) % plot is shaded until end
        end_marks{column_idx} = [end_marks{column_idx}; length(X)];
    end
end
if size(Ydata,2) > 1
    figure;
end
for i = 1:size(Ydata,2)
    if size(indicator,2) == 1
        plot_partial(X, Ydata(:,i), start_marks{1}, end_marks{1}, i, Ylabels{i}, size(Ydata,2));
    else
        plot_partial(X, Ydata(:,i), start_marks{i}, end_marks{i}, i, Ylabels{i}, size(Ydata,2));
    end
    if i ~= size(Ydata,2)
        set(gca,'xticklabel',[]) % remove x label for all except bottom plot
    end
end
xlabel(Xlabel);

    function plot_partial(x, ydata, start_marks, end_marks, number, name, total)
        if size(Ydata,2) > 1
            subplot(total,1,number);
        end
        hold on;
        ylabel(name);
        xlim([x(1) x(end)]);% force plot to go from edge to edge
        padding = Yspacing/100*(max(ydata)-min(ydata));% spacing at top and bottom of plotted line
        ylim([min(ydata)-padding max(ydata)+padding]); % default leaves too much space around graph

        for j = 1:min(length(start_marks), length(end_marks)) % create all the shaded areas
            % This patch line was ripped from ShadePlotForEmphasis.m by
            % Michael Robbins on Matlab Central.
            patch([repmat( x(start_marks(j)),1,2) repmat( x(end_marks(j)),1,2)], ...
                [get(gca,'YLim') fliplr(get(gca,'YLim'))], ...
                [0 0 0 0],colour,'EdgeColor', 'none');
        end
        plot(x, ydata, 'LineWidth', 3); % plot data line on top of colour patch
        set(gca, 'layer', 'top'); % put ticks back on top of colour patch
        hold off;
    end
end


function [Vout,HT_index, HT_values]=make_halfway_vertices(EV_table,ET_table,ETV_index,V,Ne)
% ETV_index_sparse=sparse(size(V,1),size(V,1));
%
% for i=1:size(ETV_index,1)
% 	if(ETV_index(i,1)>0)
% 		ETV_index_sparse(ETV_index(i,1),ETV_index(i,2))=i;
% 	else
% 		ET_table=ET_table(1:(i-1),:);
% 		EV_table=EV_table(1:(i-1));
% 		break
% 	end
% end

% Table to cell array
ETV_index_vall=cell(length(V),1);
for i=1:size(ETV_index,1)
    if(ETV_index(i,1)>0)
        ETV_index_vall{ETV_index(i,1)}=[ETV_index_vall{ETV_index(i,1)} i];
    else
        ET_table=ET_table(1:(i-1),:);
        EV_table=EV_table(1:(i-1));
        break;
    end
end

HT_index=cell(length(V),1);
HT_values=cell(length(V),1);

% Make output V
Vout=zeros(size(V,1)*4,3);
Vout(1:size(V,1),:)=V;
Vindex=size(V,1);

for i=1:length(V)
    Pneig=Ne{i};
    for j=1:length(Pneig);
        % Get the tangent and velocity of the edge P -> Pneig
        index=Ne{i}; vals=ETV_index_vall{i}; select1=vals(index==Pneig(j));
        Va=EV_table( select1); Ea=ET_table( select1,:);
        % Get the tangent and velocity of the edge Pneig -> P
        index=Ne{Pneig(j)}; vals=ETV_index_vall{Pneig(j)}; select2=vals(index==i);
        Vb=EV_table(select2); Eb=ET_table(select2,:);

        % The four points describing the spline
        P0=V(i,:);
        P3=V(Pneig(j),:);
        P1=P0+Ea*Va/3;
        P2=P3+Eb*Vb/3;

        % Spline used to calculated the xyz coordinate of the middle of each edge;
        c = 3*(P1 - P0);
        b = 3*(P2 - P1) - c;
        a = P3 - P0 - c - b;

        halfwayp = a*0.125 + b*0.250 + c*0.500 + P0;

        % Save the edge middle point
        if(sum(HT_index{i}==Pneig(j))==0)
            Vindex=Vindex+1;
            Vout(Vindex,:)=halfwayp;
            HT_index {i}= [HT_index{i} Pneig(j)];
            HT_values{i}=[HT_values{i} Vindex];
            HT_index {Pneig(j)}=[HT_index{ Pneig(j)} i];
            HT_values{Pneig(j)}=[HT_values{Pneig(j)} Vindex];
        end
    end
end
Vout=Vout(1:Vindex,:);


function f2=f2(x)
Bound=[-10 10];

if nargin==0
    f2 = Bound;
else
    f2=sum(abs(x))+prod(abs(x));
end


function  idsNms = selectBoundingBoxesNonMaxSup( boundingBoxes, scores, varargin )
%selectBoundingBoxesNonMaxSup performs the NMS on the candidate bounding boxes
% boxes should be in [X1,Y1,W,H] format
%
% idsNms = selectBoundingBoxesNonMaxSup( boundingBoxes, scores )
%
% Input:
%   boundingBoxes - double[ numBoxes x 4], each line correponds to the bounding box in [X1,Y1,W,H] format
%   scores - double[numBoxes x 1], scores of the bounding boxes, will be sorted in the decreasing order
%
% Extra parameters:
%   nmsIntersectionOverAreaThreshold - IoA threshold used to select boxes
%   numBoundingBoxMax - maximum number of boxes selected by NMS
%
% Output:
%   idsNms - indices of the bounding boxes selected by NMS

if ~exist('varargin', 'var')
    varargin = {};
end
%% parameters
opts = struct;
opts.numBoundingBoxMax = inf;
opts.nmsIntersectionOverAreaThreshold = 0.3;
opts = vl_argparse(opts, varargin);

% if opts.nmsIntersectionOverAreaThreshold == inf the code will get into the infinite loop
opts.nmsIntersectionOverAreaThreshold = min( opts.nmsIntersectionOverAreaThreshold, 100 );

%% do the job
numBbs = length(scores);
[~, ids] = sort(scores, 'descend');

idsNms = nan( min(opts.numBoundingBoxMax, numel(ids)), 1 );
idsNms(1) = ids(1);

numBbNms = 1;
iBb = 1;
while numBbNms < opts.numBoundingBoxMax && iBb < numBbs
    curIou = inf;
    while max( curIou(:) ) > opts.nmsIntersectionOverAreaThreshold && iBb < numBbs
        iBb = iBb + 1;
        curIou = bbIntersectionOverArea( boundingBoxes( idsNms(1 : numBbNms), : ), boundingBoxes( ids(iBb), : ) );
    end

    if max( curIou(:) ) <= opts.nmsIntersectionOverAreaThreshold
        numBbNms = numBbNms + 1;
        idsNms( numBbNms ) = ids( iBb );
    end
end

idsNms = idsNms(1 : numBbNms);

end


function newFaces = minConvexHull(points, varargin)
%MINCONVEXHULL Return the unique minimal convex hull of a set of 3D points
%
%   FACES = minConvexHull(PTS)
%   NODES is a set of 3D points  (as a Nx3 array). The function computes
%   the convex hull, and merge contiguous coplanar faces. The result is a
%   set of polygonal faces, such that there are no coplanar faces.
%   FACES is a cell array, each cell containing the vector of indices of
%   nodes given in NODES for the corresponding face.
%
%   FACES = minConvexHull(PTS, PRECISION)
%   Adjust the threshold for deciding if two faces are coplanar or
%   parallel. Default value is 1e-14.
%
%   Example
%     % extract square faces from a cube
%     [n, e, f] = createCube;
%     f2 = minConvexHull(n);
%     drawMesh(n, f2);
%
%     % Subdivides and smooths a mesh rpresenting a cube
%     [n, e, f] = createCube;
%     [n2, f2] = subdivideMesh(n, triangulateFaces(f), 4);
%     [n3, f3] = smoothMesh(n2, f2);
%     figure; drawMesh(n3, f3);
%     axis equal; view(3);
%     % merge coplanar faces, making apparent the faces of the original cube
%     f4 = minConvexHull(n3);
%     figure; drawMesh(n3, f4);
%     axis equal; view(3);
%
%
%   See also
%   meshes3d, mergeCoplanarFaces, drawMesh, convhull, convhulln
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2006-07-05
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

% HISTORY
%   20/07/2006 add tolerance for coplanarity test
%   21/08/2006 fix small bug due to difference of methods to test
%       coplanarity, sometimes resulting in 3 points of a face being not
%       coplanar! Also add control on precision
%   18/09/2007 ensure faces are given as horizontal vectors

% set up precision
acc = 1e-14;
if ~isempty(varargin)
    acc = varargin{1};
end

% triangulated convex hull. It is not uniquely defined.
faces = convhulln(points);

% compute centroid of the nodes
pointsCentroid = centroid(points);

% number of base triangular faces
N = size(faces, 1);

% compute normals of given faces
normals = planeNormal(createPlane(...
    points(faces(:,1),:), points(faces(:,2),:), points(faces(:,3),:)));

% initialize empty faces
newFaces = {};

% Processing flag for each triangle
% 1 : triangle to process, 0 : already processed
% in the beginning, every triangle face need to be processed
flag = ones(N, 1);

% iterate on each triangular face of the convex hull
for iFace = 1:N

    % check if face was already performed
    if ~flag(iFace)
        continue;
    end

    % indices of faces with same normal
    ind = find(abs(vectorNorm3d(cross(repmat(normals(iFace, :), [N 1]), normals)))<acc);
    ind = ind(ind~=iFace);

    % keep only coplanar faces (test coplanarity of points in both face)
    ind2 = iFace;
    for j = 1:length(ind)
        if isCoplanar(points([faces(iFace,:) faces(ind(j),:)], :), acc)
            ind2 = [ind2 ind(j)]; %#ok<AGROW>
        end
    end

    % compute order of the vertices in current face
    faceVertices = unique(faces(ind2, :));
    [tmp, I]  = angleSort3d(points(faceVertices, :)); %#ok<ASGLU>

    % create the new face, ensuring it is a row vector
    face = faceVertices(I);
    face = face(:)';

    % ensure face has normal pointing outwards
    outerNormal = meshFaceCentroids(points, face) - pointsCentroid;
    if dot(meshFaceNormals(points, face), outerNormal, 2) < 0
        face = face([1 end:-1:2]);
    end

    % add a new face to the list
    newFaces = [newFaces {face}]; %#ok<AGROW>

    % mark processed faces
    flag(ind2) = 0;
end


function [FRF,FBB] = Receiver(Fopt,NRF)

% randomly generate FRF
[Nt,Ns] = size(Fopt);
FRF = [];
for i = 1:NRF
    FRF = blkdiag(FRF, exp(sqrt(-1) * unifrnd (0,2*pi,[Nt/NRF,1])));
end
FRF = 1/sqrt(Nt)*FRF;

y = [];
while(isempty(y) || abs(y(1)-y(2))>1e-3)
    % fix FRF, optimize FBB
    FBB = pinv(FRF) * Fopt;

    y(1) = norm(Fopt-FRF*FBB,'fro')^2;

    % fix FBB, optimize FRF
    for i = 1:Nt
        m = ceil(i*NRF/Nt);
        FRF(i,m) = 1/sqrt(Nt) * exp( sqrt(-1) * angle( Fopt(i,:)*FBB(m,:)' ) );
    end

    y(2) = norm(Fopt-FRF*FBB,'fro')^2;
end

end


function y2=K_q_escorTsallis(P1,P2,q)
  [M,N]=size(P1);
    y=ones(1,N);
    A1=ones(1,N);
    for n=1:N
    A1=sum(P1(:,n).^(1/q));
    A2=sum(P2(:,n).^(1/q));
       y(1,n)=sum( (P1(:,n)./((A1)^q)).*( ((P2(:,n).^(1/q))/A2).^(1-q) -((P1(:,n).^(1/q))/A1).^(1-q)) );
    end
    y2=y/(q-1);


%EXnmr_cgls_mrnsd Example script, 2D NMR relaxometry

% Silvia Gazzola, University of Bath
% Per Christian Hansen, Technical University of Denmark
% James G. Nagy, Emory University
% April, 2018.

% Clear workspace and command window.
clear, clc

% Choose if you would like to see the results displayed in a single figure
% window ('subplots') or in multiple figure windows ('manyplots').
dispres = 'subplots';
% dispres = 'manyplots';

LW = 2;  % Plot line width.
MS = 10; % Size of markers on plots.

rng(0);  % Make sure this test is repeatable.

% Define the test problem.
n = 64;
NoiseLevel = 0.05;
[A, b, x, ProbInfo] = PRnmr(n);
bn = PRnoise(b, NoiseLevel);

% Compute a CGLS solution, specifying the noise level for the discrepancy
% principle, and use true solution to compute error norms.
eta = 1.02;
options = IRset('x_true', x, 'NoiseLevel', NoiseLevel, 'eta', eta, 'NoStop', 'on');
[x_cgls, IterInfo_cgls] = IRcgls(A, bn, 1:500, options);

% Compute MRNSD reconstruction.
K = [1, 100:100:20000];
[x_mrnsd, IterInfo_mrnsd] = IRmrnsd(A, bn, K, options);

% Display the reconstructions;
% uncomment as appropriate to avoid displaying titles and legends.
if strcmp(dispres, 'subplots')
    figure(1), clf
    subplot(3,3,1), PRshowx(x, ProbInfo), colormap hsv
    title('True solution','interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
    %
    subplot(3,3,4), PRshowx(IterInfo_cgls.BestReg.X, ProbInfo), colormap hsv
    title('Best CGLS solution','interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
    %
    subplot(3,3,2), semilogy(IterInfo_mrnsd.Enrm,'linewidth',1.5)
    hold on
    hl = legend('{\tt info.Enrm}');
    % hl = legend('IRmrnsd error');
    set(hl,'interpreter','latex','fontsize',12)
    semilogy(IterInfo_mrnsd.BestReg.It, IterInfo_mrnsd.BestReg.Enrm, 'ro', 'LineWidth', 1.5, 'MarkerSize', 6)
    axis([0 max(K) 0.08 1.2])
    semilogy(IterInfo_mrnsd.StopReg.It, IterInfo_mrnsd.StopReg.Enrm, 'ms', 'LineWidth', 1.5, 'MarkerSize', 6)
    set(gca,'fontsize',12)
    title('Error history','interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
    %
    subplot(3,3,5), PRshowx(IterInfo_mrnsd.BestReg.X, ProbInfo)
    title(['Best MRNSD sol., $k$ = ',num2str(IterInfo_mrnsd.BestReg.It)],...
    'interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
    %
    subplot(3,3,3)
    semilogy(K,IterInfo_mrnsd.Rnrm(K),'-',K,eta*NoiseLevel*ones(size(K)),'--','linewidth',1.5)
    hl = legend('{\tt info.Rnrm}','{\tt eta*NoiseLevel}','location','northwest');
    % hl = legend('IRmrnsd residual','{\tt eta*NoiseLevel}','location','northwest');
    set(hl,'interpreter','latex')
    axis([0 max(K) 0.045 0.08])
    title('Residula history','interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
    %
    subplot(3,3,6), PRshowx(IterInfo_mrnsd.StopReg.X, ProbInfo)
    title(['DP MRNSD sol., $k$ = ',num2str(IterInfo_mrnsd.StopReg.It)],...
    'interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
elseif strcmp(dispres, 'manyplots')
    figure(1), clf
    PRshowx(x,ProbInfo), colormap hsv
    title('True solution','interpreter','latex','fontsize',24)
    set(gca,'fontsize',24)
    %
    figure(2), clf
    PRshowx(IterInfo_cgls.BestReg.X, ProbInfo), colormap hsv
    title('Best CGLS solution','interpreter','latex','fontsize',24)
    set(gca,'fontsize',24)
    %
    figure(3), clf
    semilogy(IterInfo_mrnsd.Enrm,'linewidth',LW)
    hold on
    semilogy(IterInfo_mrnsd.BestReg.It, IterInfo_mrnsd.BestReg.Enrm, 'ro', 'LineWidth', LW, 'MarkerSize', MS)
    semilogy(IterInfo_mrnsd.StopReg.It, IterInfo_mrnsd.StopReg.Enrm, 'ms', 'LineWidth', LW, 'MarkerSize', MS)
    axis([0 max(K) 0.08 1.2])
    % hl = legend('{\tt info.Enrm}','location','North');
    % set(hl,'interpreter','latex','fontsize',24)
    hl = legend('IRmrnsd error','optimal stopping iteration', ...
      'DP stopping iteration','location','North');
    set(hl,'interpreter','latex','fontsize',16)
    title('Error history','interpreter','latex','fontsize',24)
    set(gca,'fontsize',30)
    %
    figure(4), clf
    PRshowx(IterInfo_mrnsd.BestReg.X, ProbInfo), colormap hsv
    title(['Best MRNSD sol., $k$ = ',num2str(IterInfo_mrnsd.BestReg.It)],...
    'interpreter','latex','fontsize',24)
    set(gca,'fontsize',24)
    %
    figure(5), clf
    semilogy(K,IterInfo_mrnsd.Rnrm(K),'-',K,eta*NoiseLevel*ones(size(K)),'--','linewidth',LW)
    % hl = legend('{\tt info.Rnrm}','{\tt eta*NoiseLevel}','location','north');
    hl = legend('IRmrnsd residual','{\tt eta*NoiseLevel}','location','north');
    set(hl,'interpreter','latex','fontsize',24)
    title('Residual history','interpreter','latex','fontsize',24)
    axis([0 max(K) 0.045 0.08])
    set(gca,'fontsize',30)
    %
    figure(6), clf
    PRshowx(IterInfo_mrnsd.StopReg.X,ProbInfo), colormap hsv
    title(['DP MRNSD sol., $k$ = ',num2str(IterInfo_mrnsd.StopReg.It)],...
    'interpreter','latex','fontsize',24)
    set(gca,'fontsize',24)
end

return

% A number of instructions useful to save the displayed figures follow;
% the defualt is not to execute them. If you wish to save the displayed
% figures in the dedicated 'Results' folder, please comment the above
% return statement
oldcd = cd;
if strcmp(dispres, 'subplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    figure(1), print -dpng -r300 EXnmr
elseif strcmp(dispres, 'manyplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    % save as eps
    figure(1), print -depsc -r300 EXnmr_a.eps
    figure(2), print -depsc -r300 EXnmr_d.eps
    figure(3), print -depsc -r300 EXnmr_b.eps
    figure(4), print -depsc -r300 EXnmr_e.eps
    figure(5), print -depsc -r300 EXnmr_c.eps
    figure(6), print -depsc -r300 EXnmr_f.eps
    % save as png
    figure(1), print -dpng -r300 EXnmr_a
    figure(2), print -dpng -r300 EXnmr_d
    figure(3), print -dpng -r300 EXnmr_b
    figure(4), print -dpng -r300 EXnmr_e
    figure(5), print -dpng -r300 EXnmr_c
    figure(6), print -dpng -r300 EXnmr_f
end
cd(oldcd)

% Uncomment the following return statement if you wish to save the
% displayed figures as MATLAB figures

% return

oldcd = cd;
if strcmp(dispres, 'subplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    figure(1), saveas('EXnmr.fig')
elseif strcmp(dispres, 'manyplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    saveas(figure(1), 'EXnmr_a.fig')
    saveas(figure(2), 'EXnmr_b.fig')
    saveas(figure(3), 'EXnmr_c.fig')
    saveas(figure(4), 'EXnmr_d.fig')
    saveas(figure(5), 'EXnmr_e.fig')
    saveas(figure(6), 'EXnmr_f.fig')
end
cd(oldcd)


function subset_check ( n, t )

%*****************************************************************************80
%
%% SUBSET_CHECK checks a subset.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the number of elements in the master set.
%    N must be positive.
%
%    Input, integer T(N), the subset.  If T(I) = 0, item I is
%    not in the subset; if T(I) = 1, item I is in the subset.
%
  if ( n < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SUBSET_COLEX_RANK - Fatal error!\n' );
    fprintf ( 1, '  N = %d < 1.\n', n );
    error ( 'SUBSET_COLEX_RANK - Fatal error!' );
  end

  for i = 1 : n

    if ( t(i) ~= 0 && t(i) ~= 1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SUBSET_COLEX_RANK - Fatal error!\n' );
      fprintf ( 1, '  T(%d) = %d, but must be 0 or 1.\n', i, t(i) );
      error ( 'SUBSET_COLEX_RANK - Fatal error!' );
    end

  end

  return
end


classdef RMMEDA_F5 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing RM-MEDA

%------------------------------- Reference --------------------------------
% Q. Zhang, A. Zhou, and Y. Jin, RM-MEDA: A regularity model-based
% multiobjective estimation of distribution algorithm, IEEE Transactions on
% Evolutionary Computation, 2008, 12(1): 41-63.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

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


% ACOT   Inverse cotangent, result in radian.
%    ACOT(X) is the inverse cotangent of the elements of X.
%
%    Class support for input X:
%       float: double, single
%
%    See also COT, ACOTD.
%
%    Reference page in Doc Center
%       doc acot
%
%    Other functions named acot
%
%       codistributed/acot    gpuArray/acot    sym/acot    ts/acot
%


function suborder = triangle_nco_suborder ( rule, suborder_num )

%*****************************************************************************80
%
%% TRIANGLE_NCO_SUBORDER returns the suborders for an NCO rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Peter Silvester,
%    Symmetric Quadrature Formulae for Simplexes,
%    Mathematics of Computation,
%    Volume 24, Number 109, January 1970, pages 95-100.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Input, integer SUBORDER_NUM, the number of suborders of the rule.
%
%    Output, integer SUBORDER(SUBORDER_NUM), the suborders of the rule.
%
  if ( rule == 1 )
    suborder(1:suborder_num) = [ ...
      1 ];
  elseif ( rule == 2 )
    suborder(1:suborder_num) = [ ...
      3 ];
  elseif ( rule == 3 )
    suborder(1:suborder_num) = [ ...
      3, 3 ];
  elseif ( rule == 4 )
    suborder(1:suborder_num) = [ ...
      3, 6, 1 ];
  elseif ( rule == 5 )
    suborder(1:suborder_num) = [ ...
      3, 6, 3, 3 ];
  elseif ( rule == 6 )
    suborder(1:suborder_num) = [ ...
      3, 6, 6, 3, 3 ];
  elseif ( rule == 7 )
    suborder(1:suborder_num) = [ ...
      3, 6, 6, 3, 3, 6, 1 ];
  elseif ( rule == 8 )
    suborder(1:suborder_num) = [ ...
      3, 6, 6, 3, 6, 6, 3, 3 ];
  elseif ( rule == 9 )
    suborder(1:suborder_num) = [ ...
      3, 6, 6, 3, 6, 6, 3, 6, 3, 3  ];

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGLE_NCO_SUBORDER - Fatal error!\n' );
    fprintf ( 1, '  Illegal RULE = %d\n', rule );
    error ( 'TRIANGLE_NCO_SUBORDER - Fatal error!' )

  end

  return
end


function [K,H] = calc_Laplacians(I,seg_map,M,eta,beta1,beta2)
[rows,cols,B] = size(I);
N = rows*cols;

if isempty(seg_map)
    [W,Neighbors] = image2graph(I,eta,1e-9);
else
    [W,Neighbors] = image2graph(double(seg_map),1e-3,1e-9);
end

D = diag(sum(W,2));
L = D - W;
L = sparse(L);
K = L - beta2/beta1*speye(N);

W = ones(M,M);
D = diag(sum(W,2));
H = D - W;

W = diag(ones(1,B-1),-1) + diag(ones(1,B-1),1);
D = diag(sum(W,2));
G = D - W;
G = sparse(G);


function smallpot = marginalize_pot(bigpot, keep, maximize, useC)
% MARGINALIZE_POT Marginalize a cpot onto a smaller domain.
% smallpot = marginalize_pot(bigpot, keep, maximize, useC)
%
% The maximize argument is ignored - maxing out a Gaussian is the same as summing it out,
% since the mode and mean are equal.
% The useC argument is ignored.

node_sizes = sparse(1, max(bigpot.domain));
node_sizes(bigpot.domain) = bigpot.sizes;
sum_over = mysetdiff(bigpot.domain, keep);

if sum(node_sizes(sum_over))==0 % isempty(sum_over)
  %smallpot = bigpot;
  smallpot = cpot(keep, node_sizes(keep), bigpot.g, bigpot.h, bigpot.K);
else
  [h1, h2, K11, K12, K21, K22] = partition_matrix_vec(bigpot.h, bigpot.K, sum_over, keep, node_sizes);
  n = length(h1);
  K11inv = inv(K11);
  g = bigpot.g + 0.5*(n*log(2*pi) - log(det(K11)) + h1'*K11inv*h1);
  if length(h2) > 0 % ~isempty(keep) % we are are actually keeping something
    A = K21*K11inv;
    h = h2 - A*h1;
    K = K22 - A*K12;
  else
    h = [];
    K = [];
  end
  smallpot = cpot(keep, node_sizes(keep), g, h, K);
end


function [y] = spm_gx_fmri_linear(x,u,P,M)
% Simulated BOLD response to input (linear version)
% FORMAT [y] = spm_gx_fmri_linear(x,u,P,M)
% y          - BOLD response (%)
% x          - state vector     (see spm_fx_fmri)
% P          - Parameter vector (see spm_fx_fmri)
% M          - model specification structure (see spm_nlsi)
%__________________________________________________________________________
%
% This function implements the BOLD signal model described in:
%
% Stephan KE, Weiskopf N, Drysdale PM, Robinson PA, Friston KJ (2007)
% Comparing hemodynamic models with DCM. NeuroImage 38: 387-401.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston & Klaas Enno Stephan
% $Id: spm_gx_fmri_linear.m 6262 2014-11-17 13:47:56Z karl $

% Biophysical constants for 1.5T
%==========================================================================

% time to echo (TE) (default 0.04 sec)
%--------------------------------------------------------------------------
try, TE = M.TE; catch, TE = 0.04; end

% resting venous volume (%)
%--------------------------------------------------------------------------
V0  = 4;

% estimated region-specific ratios of intra- to extra-vascular signal
%--------------------------------------------------------------------------
ep  = 1*exp(P.epsilon);

% slope r0 of intravascular relaxation rate R_iv as a function of oxygen
% saturation S:  R_iv = r0*[(1 - S)-(1 - S0)] (Hz)
%--------------------------------------------------------------------------
r0  = 25;

% frequency offset at the outer surface of magnetized vessels (Hz)
%--------------------------------------------------------------------------
nu0 = 40.3;

% resting oxygen extraction fraction
%--------------------------------------------------------------------------
E0  = 0.4;

%-Coefficients in BOLD signal model
%==========================================================================
k1  = 4.3*nu0*E0*TE;
k2  = ep*r0*E0*TE;
k3  = 1 - ep;

%-Output equation of BOLD signal model
%==========================================================================
v   = x(:,4) + 1;
q   = x(:,5) + 1;
y   = V0*((k1 + k2).*(1 - q) + (k3 - k2).*(1 - v));


function cache = create_loc_cc_fftn_cache(metric, metric_param_pix, volmov, volfix, internal_dtype, maskfix, deps, loc_cc_approximate)
%     deps = 1e-5;
    Nd = 3;
    if size(volmov, 3) == 1
        Nd = 2;
    end
    szvolfix = [size(volfix, 1), size(volfix, 2), size(volfix, 3)];

    cache = [];
    if numel(metric_param_pix) == 1
        metric_param_pix = metric_param_pix * ones(1,Nd);
    end
    metric_param_pix = metric_param_pix(1:Nd);

    metr_threshold = 0.4;
    metr_threshold = 0.8;
    metric_param_pix(metric_param_pix < metr_threshold) = metr_threshold;
    sgm = metric_param_pix;
    hsz = max(2*ceil(2*sgm) + 1, 5);
    filterRadius = (hsz-1)/2;

    pad_size = ceil(filterRadius);
    start = pad_size+1;

    stop = start + size(volfix) - 1;
    vol_fix_p = padarray(volfix, pad_size, 'symmetric');

    if Nd == 2
        [n1, n2] = ndgrid(1:size(vol_fix_p,1), 1:size(vol_fix_p,2));
    elseif Nd == 3
        [n1, n2, n3] = ndgrid(1:size(vol_fix_p,1), 1:size(vol_fix_p,2), 1:size(vol_fix_p,3));
    end
    c = ceil(size(n1)/2)+1;
    if Nd == 2
        g = exp(- (n1-c(1)).^2/(2*sgm(1)^2) - (n2-c(2)).^2/(2*sgm(2)^2));
    elseif Nd == 3
        g = exp(- (n1-c(1)).^2/(2*sgm(1)^2) - (n2-c(2)).^2/(2*sgm(2)^2) - (n3-c(3)).^2/(2*sgm(3)^2));
    end

    g = g/sum(g(:));
    g = fftshift(g);
    fg = fftn(g, size(vol_fix_p));
    convop = @(fx) (ifftn(fftn(fx) .* fg, 'symmetric'));

    mean_fix = convop(vol_fix_p);
    sgm_fix = convop(vol_fix_p.^2) - mean_fix.^2 + deps;
    sgm_fix = sqrt(sgm_fix);

%     maskfix_pad = [];
    if ~isempty(maskfix)
        maskfix_pad = padarray(maskfix, pad_size, 'symmetric');
        sgm_fix = sgm_fix ./ maskfix_pad;
        sgm_fix_inv = maskfix_pad ./ sgm_fix;
    else
        sgm_fix_inv = 1 ./ sgm_fix;
    end

    cache.fg = fg;
    cache.vol_fix_p = vol_fix_p;
    cache.mean_fix = mean_fix;
    cache.sgm_fix = sgm_fix;
    cache.sgm_fix_inv = sgm_fix_inv;
    cache.start = start;
    cache.stop = stop;

    cache.pad_size = pad_size;
    cache.metric_name = metric;
    cache.sigma = metric_param_pix;
    cache.internal_dtype = internal_dtype;
    cache.deps = deps;
%     cache.maskfix = maskfix;
%     cache.maskfix_pad = maskfix_pad;
    cache.maskfix_pad = [];
    cache.loc_cc_approximate = loc_cc_approximate;

    if strcmp(metric, 'loc_cc_fftn_gpu')
        cache.fg = gpuArray((cache.fg));
        cache.vol_fix_p = gpuArray((cache.vol_fix_p));
        cache.mean_fix = gpuArray((cache.mean_fix));
        cache.sgm_fix = gpuArray((cache.sgm_fix));
        cache.deps = gpuArray(cache.deps);
        cache.sgm_fix_inv = gpuArray(sgm_fix_inv);
%         cache.maskfix = gpuArray(cache.maskfix);
%         cache.maskfix_pad = gpuArray(cache.maskfix_pad);
    elseif strcmp(metric, 'loc_cc_fftn_gpu_single')
        cache.fg = gpuArray(single(cache.fg));
        cache.vol_fix_p = gpuArray(single(cache.vol_fix_p));
        cache.mean_fix = gpuArray(single(cache.mean_fix));
        cache.sgm_fix = gpuArray(single(cache.sgm_fix));
        cache.deps = gpuArray(single(cache.deps));
        cache.sgm_fix_inv = gpuArray(single(sgm_fix_inv));
%         cache.maskfix = gpuArray(single(cache.maskfix));
%         cache.maskfix_pad = gpuArray(single(cache.maskfix_pad));
    elseif strcmp(metric, 'loc_cc_fftn_single')
        cache.fg = (single(cache.fg));
        cache.vol_fix_p = (single(cache.vol_fix_p));
        cache.mean_fix = (single(cache.mean_fix));
        cache.sgm_fix = (single(cache.sgm_fix));
        cache.deps = (single(cache.deps));
        cache.sgm_fix_inv = (single(sgm_fix_inv));
    end
end


function [f]=comp_idwilt(coef,g)
%COMP_IDWILT  Compute Inverse discrete Wilson transform.
%
%   This is a computational routine. Do not call it
%   directly.

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: OK
%   REFERENCE: OK

M=size(coef,1)/2;
N=2*size(coef,2);
W=size(coef,3);

a=M;

L=N*a;

coef2=zeros(2*M,N,W,assert_classname(coef,g));

% First and middle modulation are transferred unchanged.
coef2(1,1:2:N,:) = coef(1,:,:);
if mod(M,2)==0
  coef2(M+1,1:2:N,:) = coef(M+1,:,:);
else
  coef2(M+1,2:2:N,:) = coef(M+1,:,:);
end;

if M>2
  % cosine, first column.
  coef2(3:2:M,1:2:N,:)        = 1/sqrt(2)*coef(3:2:M,:,:);
  coef2(2*M-1:-2:M+2,1:2:N,:) = 1/sqrt(2)*coef(3:2:M,:,:);

  % sine, second column
  coef2(3:2:M,2:2:N,:)        = -1/sqrt(2)*i*coef(M+3:2:2*M,:,:);
  coef2(2*M-1:-2:M+2,2:2:N,:) =  1/sqrt(2)*i*coef(M+3:2:2*M,:,:);
end;

% sine, first column.
coef2(2:2:M,1:2:N,:)        = -1/sqrt(2)*i*coef(2:2:M,:,:);
coef2(2*M:-2:M+2,1:2:N,:)   =  1/sqrt(2)*i*coef(2:2:M,:,:);

% cosine, second column
coef2(2:2:M,2:2:N,:)        = 1/sqrt(2)*coef(M+2:2:2*M,:,:);
coef2(2*M:-2:M+2,2:2:N,:)   = 1/sqrt(2)*coef(M+2:2:2*M,:,:);

f = comp_isepdgt(coef2,g,L,a,2*M,0);

% Apply the final DGT
%f=comp_idgt(coef2,g,a,[0 1],0,0);

% Clean signal if it is known to be real
if (isreal(coef) && isreal(g))
  f=real(f);
end;


% SP_EXTERIOR_DERIVAITVE: computes the exterior derivative as a matrix with size
%  given by the dimension of two consecutive spaces in the De Rham sequence.
%
%   diff_op = sp_exterior_derivative (space1, space2);
%
% INPUT:
%
%   space1:  domain space of the exterior derivative (number of columns)
%   space2:  image space of the exterior derivative (number of rows)
%
% OUTPUT:
%
%   diff_op: sparse matrix representation of the differential operator.
%
% Copyright (C) 2020-2023 Bernard Kapidani, Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function diff_op = sp_exterior_derivative (space1, space2)

  assert (strcmpi(space1.transform, 'grad-preserving'), ...
    'The first space cannot be the one for integral-preserving splines (or n-forms)')

  ndim = numel (space1.knots);
  if (ndim == 1)
    grad_curl = 'grad';
    assert (space1.degree == space2.degree+1, 'The degrees are not compatible')
    assert (numel(space1.knots{1}) == numel(space2.knots{1})+2, 'The knot vectors are not compatible')
  elseif (ndim == 2)
    if (strcmpi (space2.transform, 'curl-preserving'))
      grad_curl = 'grad';
      deg_shift = {[1 0], [0 1]};
      knt_shift = {[2 0], [0 2]};
    elseif (strcmpi (space2.transform, 'div-preserving'))
      grad_curl = 'curl';
      deg_shift = {[0 1], [1 0]};
      knt_shift = {[0 2], [2 0]};
    else
      error ('The second space should be either curl-preserving or div-preserving')
    end
    for idim = 1:ndim
      assert (all(space1.degree == space2.scalar_spaces{idim}.degree+deg_shift{idim}), 'The degrees are not compatible')
      assert (all(cellfun(@numel,space1.knots) == (cellfun(@numel, space2.scalar_spaces{idim}.knots)+knt_shift{idim})), ...
        'The knot vectors are not compatible')
    end
  elseif (ndim == 3)
    grad_curl = 'grad';
    deg_shift = {[1 0 0], [0 1 0], [0 0 1]};
    knt_shift = {[2 0 0], [0 2 0], [0 0 2]};
    assert (strcmpi(space2.transform, 'curl-preserving'), ...
      'The second space should be the one for curl-conforming splines')
    for idim = 1:ndim
      assert (all(space1.degree == space2.scalar_spaces{idim}.degree+deg_shift{idim}), 'The degrees are not compatible')
      assert (all(cellfun(@numel,space1.knots) == (cellfun(@numel, space2.scalar_spaces{idim}.knots)+knt_shift{idim})), ...
        'The knot vectors are not compatible')
    end
  end

  diff_ops = op_geom_exterior (space1.knots, space1.degree, grad_curl);
  diff_op = diff_ops{1};
end


function t_modcost(quiet)
%T_MODCOST  Tests for code in MODCOST.

%   MATPOWER
%   Copyright (c) 2010-2016, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

if nargin < 1
    quiet = 0;
end

n_tests = 162;

t_begin(n_tests, quiet);

%% define named indices into data matrices
[GEN_BUS, PG, QG, QMAX, QMIN, VG, MBASE, GEN_STATUS, PMAX, PMIN, ...
    MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN, PC1, PC2, QC1MIN, QC1MAX, ...
    QC2MIN, QC2MAX, RAMP_AGC, RAMP_10, RAMP_30, RAMP_Q, APF] = idx_gen;
[PW_LINEAR, POLYNOMIAL, MODEL, STARTUP, SHUTDOWN, NCOST, COST] = idx_cost;

%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
gencost0 = [
	2	0	0	3	0.01	0.1	1	0	0	0	0	0;
	2	0	0	5	0.0006	0.005	0.04	0.3	2	0	0	0;
	1	0	0	4	0	0	10	200	20	600	30	1200;
	1	0	0	4	-30	-2400	-20	-1800	-10	-1000	0	0;
];

%%-----  scalar values for alpha  -----
gencost = modcost(gencost0, 5, 'SCALE_F');

t = 'modcost SCALE_F - quadratic';
t_is(totcost(gencost, [0;0;0;0])/5, [1;2;0;0], 8, t);
t_is(totcost(gencost, [1;0;0;0])/5, [1.11;2;0;0], 8, t);
t_is(totcost(gencost, [2;0;0;0])/5, [1.24;2;0;0], 8, t);

t = 'modcost SCALE_F - 4th order polynomial';
t_is(totcost(gencost, [0;0;0;0])/5, [1;2;     0;0], 8, t);
t_is(totcost(gencost, [0;1;0;0])/5, [1;2.3456;0;0], 8, t);
t_is(totcost(gencost, [0;2;0;0])/5, [1;2.8096;0;0], 8, t);

t = 'modcost SCALE_F - pwl (gen)';
t_is(totcost(gencost, [0;0;5;0 ])/5, [1;2;100;0], 8, t);
t_is(totcost(gencost, [0;0;10;0])/5, [1;2;200;0], 8, t);
t_is(totcost(gencost, [0;0;15;0])/5, [1;2;400;0], 8, t);
t_is(totcost(gencost, [0;0;20;0])/5, [1;2;600;0], 8, t);
t_is(totcost(gencost, [0;0;25;0])/5, [1;2;900;0], 8, t);
t_is(totcost(gencost, [0;0;30;0])/5, [1;2;1200;0], 8, t);
t_is(totcost(gencost, [0;0;35;0])/5, [1;2;1500;0], 8, t);

t = 'modcost SCALE_F - pwl (load)';
t_is(totcost(gencost, [0;0;0;-5 ])/5, [1;2;0;-500], 8, t);
t_is(totcost(gencost, [0;0;0;-10])/5, [1;2;0;-1000], 8, t);
t_is(totcost(gencost, [0;0;0;-15])/5, [1;2;0;-1400], 8, t);
t_is(totcost(gencost, [0;0;0;-20])/5, [1;2;0;-1800], 8, t);
t_is(totcost(gencost, [0;0;0;-25])/5, [1;2;0;-2100], 8, t);
t_is(totcost(gencost, [0;0;0;-30])/5, [1;2;0;-2400], 8, t);
t_is(totcost(gencost, [0;0;0;-35])/5, [1;2;0;-2700], 8, t);

gencost = modcost(gencost0, 2, 'SCALE_X');

t = 'modcost SCALE_X - quadratic';
t_is(totcost(gencost, [0;0;0;0]*2), [1;2;0;0], 8, t);
t_is(totcost(gencost, [1;0;0;0]*2), [1.11;2;0;0], 8, t);
t_is(totcost(gencost, [2;0;0;0]*2), [1.24;2;0;0], 8, t);

t = 'modcost SCALE_X - 4th order polynomial';
t_is(totcost(gencost, [0;0;0;0]*2), [1;2;     0;0], 8, t);
t_is(totcost(gencost, [0;1;0;0]*2), [1;2.3456;0;0], 8, t);
t_is(totcost(gencost, [0;2;0;0]*2), [1;2.8096;0;0], 8, t);

t = 'modcost SCALE_X - pwl (gen)';
t_is(totcost(gencost, [0;0;5;0 ]*2), [1;2;100;0], 8, t);
t_is(totcost(gencost, [0;0;10;0]*2), [1;2;200;0], 8, t);
t_is(totcost(gencost, [0;0;15;0]*2), [1;2;400;0], 8, t);
t_is(totcost(gencost, [0;0;20;0]*2), [1;2;600;0], 8, t);
t_is(totcost(gencost, [0;0;25;0]*2), [1;2;900;0], 8, t);
t_is(totcost(gencost, [0;0;30;0]*2), [1;2;1200;0], 8, t);
t_is(totcost(gencost, [0;0;35;0]*2), [1;2;1500;0], 8, t);

t = 'modcost SCALE_X - pwl (load)';
t_is(totcost(gencost, [0;0;0;-5 ]*2), [1;2;0;-500], 8, t);
t_is(totcost(gencost, [0;0;0;-10]*2), [1;2;0;-1000], 8, t);
t_is(totcost(gencost, [0;0;0;-15]*2), [1;2;0;-1400], 8, t);
t_is(totcost(gencost, [0;0;0;-20]*2), [1;2;0;-1800], 8, t);
t_is(totcost(gencost, [0;0;0;-25]*2), [1;2;0;-2100], 8, t);
t_is(totcost(gencost, [0;0;0;-30]*2), [1;2;0;-2400], 8, t);
t_is(totcost(gencost, [0;0;0;-35]*2), [1;2;0;-2700], 8, t);

gencost = modcost(gencost0, 3, 'SHIFT_F');

t = 'modcost SHIFT_F - quadratic';
t_is(totcost(gencost, [0;0;0;0])-3, [1;2;0;0], 8, t);
t_is(totcost(gencost, [1;0;0;0])-3, [1.11;2;0;0], 8, t);
t_is(totcost(gencost, [2;0;0;0])-3, [1.24;2;0;0], 8, t);

t = 'modcost SHIFT_F - 4th order polynomial';
t_is(totcost(gencost, [0;0;0;0])-3, [1;2;     0;0], 8, t);
t_is(totcost(gencost, [0;1;0;0])-3, [1;2.3456;0;0], 8, t);
t_is(totcost(gencost, [0;2;0;0])-3, [1;2.8096;0;0], 8, t);

t = 'modcost SHIFT_F - pwl (gen)';
t_is(totcost(gencost, [0;0;5;0 ])-3, [1;2;100;0], 8, t);
t_is(totcost(gencost, [0;0;10;0])-3, [1;2;200;0], 8, t);
t_is(totcost(gencost, [0;0;15;0])-3, [1;2;400;0], 8, t);
t_is(totcost(gencost, [0;0;20;0])-3, [1;2;600;0], 8, t);
t_is(totcost(gencost, [0;0;25;0])-3, [1;2;900;0], 8, t);
t_is(totcost(gencost, [0;0;30;0])-3, [1;2;1200;0], 8, t);
t_is(totcost(gencost, [0;0;35;0])-3, [1;2;1500;0], 8, t);

t = 'modcost SHIFT_F - pwl (load)';
t_is(totcost(gencost, [0;0;0;-5 ])-3, [1;2;0;-500], 8, t);
t_is(totcost(gencost, [0;0;0;-10])-3, [1;2;0;-1000], 8, t);
t_is(totcost(gencost, [0;0;0;-15])-3, [1;2;0;-1400], 8, t);
t_is(totcost(gencost, [0;0;0;-20])-3, [1;2;0;-1800], 8, t);
t_is(totcost(gencost, [0;0;0;-25])-3, [1;2;0;-2100], 8, t);
t_is(totcost(gencost, [0;0;0;-30])-3, [1;2;0;-2400], 8, t);
t_is(totcost(gencost, [0;0;0;-35])-3, [1;2;0;-2700], 8, t);

gencost = modcost(gencost0, -4, 'SHIFT_X');

t = 'modcost SHIFT_X - quadratic';
t_is(totcost(gencost, [0;0;0;0]-4), [1;2;0;0], 8, t);
t_is(totcost(gencost, [1;0;0;0]-4), [1.11;2;0;0], 8, t);
t_is(totcost(gencost, [2;0;0;0]-4), [1.24;2;0;0], 8, t);

t = 'modcost SHIFT_X - 4th order polynomial';
t_is(totcost(gencost, [0;0;0;0]-4), [1;2;     0;0], 8, t);
t_is(totcost(gencost, [0;1;0;0]-4), [1;2.3456;0;0], 8, t);
t_is(totcost(gencost, [0;2;0;0]-4), [1;2.8096;0;0], 8, t);

t = 'modcost SHIFT_X - pwl (gen)';
t_is(totcost(gencost, [0;0;5;0 ]-4), [1;2;100;0], 8, t);
t_is(totcost(gencost, [0;0;10;0]-4), [1;2;200;0], 8, t);
t_is(totcost(gencost, [0;0;15;0]-4), [1;2;400;0], 8, t);
t_is(totcost(gencost, [0;0;20;0]-4), [1;2;600;0], 8, t);
t_is(totcost(gencost, [0;0;25;0]-4), [1;2;900;0], 8, t);
t_is(totcost(gencost, [0;0;30;0]-4), [1;2;1200;0], 8, t);
t_is(totcost(gencost, [0;0;35;0]-4), [1;2;1500;0], 8, t);

t = 'modcost SHIFT_X - pwl (load)';
t_is(totcost(gencost, [0;0;0;-5 ]-4), [1;2;0;-500], 8, t);
t_is(totcost(gencost, [0;0;0;-10]-4), [1;2;0;-1000], 8, t);
t_is(totcost(gencost, [0;0;0;-15]-4), [1;2;0;-1400], 8, t);
t_is(totcost(gencost, [0;0;0;-20]-4), [1;2;0;-1800], 8, t);
t_is(totcost(gencost, [0;0;0;-25]-4), [1;2;0;-2100], 8, t);
t_is(totcost(gencost, [0;0;0;-30]-4), [1;2;0;-2400], 8, t);
t_is(totcost(gencost, [0;0;0;-35]-4), [1;2;0;-2700], 8, t);

t = 'modcost empty gencost';
gencost = modcost([], 7);
t_ok(isempty(gencost), t);

%%-----  vector values for alpha  -----
alpha = [10; 9; 8; 7];
gencost = modcost(gencost0, alpha, 'SCALE_F');

t = 'modcost vector SCALE_F - quadratic';
t_is(totcost(gencost, [0;0;0;0])./alpha, [1;2;0;0], 8, t);
t_is(totcost(gencost, [1;0;0;0])./alpha, [1.11;2;0;0], 8, t);
t_is(totcost(gencost, [2;0;0;0])./alpha, [1.24;2;0;0], 8, t);

t = 'modcost vector SCALE_F - 4th order polynomial';
t_is(totcost(gencost, [0;0;0;0])./alpha, [1;2;     0;0], 8, t);
t_is(totcost(gencost, [0;1;0;0])./alpha, [1;2.3456;0;0], 8, t);
t_is(totcost(gencost, [0;2;0;0])./alpha, [1;2.8096;0;0], 8, t);

t = 'modcost vector SCALE_F - pwl (gen)';
t_is(totcost(gencost, [0;0;5;0 ])./alpha, [1;2;100;0], 8, t);
t_is(totcost(gencost, [0;0;10;0])./alpha, [1;2;200;0], 8, t);
t_is(totcost(gencost, [0;0;15;0])./alpha, [1;2;400;0], 8, t);
t_is(totcost(gencost, [0;0;20;0])./alpha, [1;2;600;0], 8, t);
t_is(totcost(gencost, [0;0;25;0])./alpha, [1;2;900;0], 8, t);
t_is(totcost(gencost, [0;0;30;0])./alpha, [1;2;1200;0], 8, t);
t_is(totcost(gencost, [0;0;35;0])./alpha, [1;2;1500;0], 8, t);

t = 'modcost vector SCALE_F - pwl (load)';
t_is(totcost(gencost, [0;0;0;-5 ])./alpha, [1;2;0;-500], 8, t);
t_is(totcost(gencost, [0;0;0;-10])./alpha, [1;2;0;-1000], 8, t);
t_is(totcost(gencost, [0;0;0;-15])./alpha, [1;2;0;-1400], 8, t);
t_is(totcost(gencost, [0;0;0;-20])./alpha, [1;2;0;-1800], 8, t);
t_is(totcost(gencost, [0;0;0;-25])./alpha, [1;2;0;-2100], 8, t);
t_is(totcost(gencost, [0;0;0;-30])./alpha, [1;2;0;-2400], 8, t);
t_is(totcost(gencost, [0;0;0;-35])./alpha, [1;2;0;-2700], 8, t);

gencost = modcost(gencost0, alpha, 'SCALE_X');

t = 'modcost vector SCALE_X - quadratic';
t_is(totcost(gencost, [0;0;0;0].*alpha), [1;2;0;0], 8, t);
t_is(totcost(gencost, [1;0;0;0].*alpha), [1.11;2;0;0], 8, t);
t_is(totcost(gencost, [2;0;0;0].*alpha), [1.24;2;0;0], 8, t);

t = 'modcost vector SCALE_X - 4th order polynomial';
t_is(totcost(gencost, [0;0;0;0].*alpha), [1;2;     0;0], 8, t);
t_is(totcost(gencost, [0;1;0;0].*alpha), [1;2.3456;0;0], 8, t);
t_is(totcost(gencost, [0;2;0;0].*alpha), [1;2.8096;0;0], 8, t);

t = 'modcost vector SCALE_X - pwl (gen)';
t_is(totcost(gencost, [0;0;5;0 ].*alpha), [1;2;100;0], 8, t);
t_is(totcost(gencost, [0;0;10;0].*alpha), [1;2;200;0], 8, t);
t_is(totcost(gencost, [0;0;15;0].*alpha), [1;2;400;0], 8, t);
t_is(totcost(gencost, [0;0;20;0].*alpha), [1;2;600;0], 8, t);
t_is(totcost(gencost, [0;0;25;0].*alpha), [1;2;900;0], 8, t);
t_is(totcost(gencost, [0;0;30;0].*alpha), [1;2;1200;0], 8, t);
t_is(totcost(gencost, [0;0;35;0].*alpha), [1;2;1500;0], 8, t);

t = 'modcost vector SCALE_X - pwl (load)';
t_is(totcost(gencost, [0;0;0;-5 ].*alpha), [1;2;0;-500], 8, t);
t_is(totcost(gencost, [0;0;0;-10].*alpha), [1;2;0;-1000], 8, t);
t_is(totcost(gencost, [0;0;0;-15].*alpha), [1;2;0;-1400], 8, t);
t_is(totcost(gencost, [0;0;0;-20].*alpha), [1;2;0;-1800], 8, t);
t_is(totcost(gencost, [0;0;0;-25].*alpha), [1;2;0;-2100], 8, t);
t_is(totcost(gencost, [0;0;0;-30].*alpha), [1;2;0;-2400], 8, t);
t_is(totcost(gencost, [0;0;0;-35].*alpha), [1;2;0;-2700], 8, t);

gencost = modcost(gencost0, alpha, 'SHIFT_F');

t = 'modcost vector SHIFT_F - quadratic';
t_is(totcost(gencost, [0;0;0;0])-alpha, [1;2;0;0], 8, t);
t_is(totcost(gencost, [1;0;0;0])-alpha, [1.11;2;0;0], 8, t);
t_is(totcost(gencost, [2;0;0;0])-alpha, [1.24;2;0;0], 8, t);

t = 'modcost vector SHIFT_F - 4th order polynomial';
t_is(totcost(gencost, [0;0;0;0])-alpha, [1;2;     0;0], 8, t);
t_is(totcost(gencost, [0;1;0;0])-alpha, [1;2.3456;0;0], 8, t);
t_is(totcost(gencost, [0;2;0;0])-alpha, [1;2.8096;0;0], 8, t);

t = 'modcost vector SHIFT_F - pwl (gen)';
t_is(totcost(gencost, [0;0;5;0 ])-alpha, [1;2;100;0], 8, t);
t_is(totcost(gencost, [0;0;10;0])-alpha, [1;2;200;0], 8, t);
t_is(totcost(gencost, [0;0;15;0])-alpha, [1;2;400;0], 8, t);
t_is(totcost(gencost, [0;0;20;0])-alpha, [1;2;600;0], 8, t);
t_is(totcost(gencost, [0;0;25;0])-alpha, [1;2;900;0], 8, t);
t_is(totcost(gencost, [0;0;30;0])-alpha, [1;2;1200;0], 8, t);
t_is(totcost(gencost, [0;0;35;0])-alpha, [1;2;1500;0], 8, t);

t = 'modcost vector SHIFT_F - pwl (load)';
t_is(totcost(gencost, [0;0;0;-5 ])-alpha, [1;2;0;-500], 8, t);
t_is(totcost(gencost, [0;0;0;-10])-alpha, [1;2;0;-1000], 8, t);
t_is(totcost(gencost, [0;0;0;-15])-alpha, [1;2;0;-1400], 8, t);
t_is(totcost(gencost, [0;0;0;-20])-alpha, [1;2;0;-1800], 8, t);
t_is(totcost(gencost, [0;0;0;-25])-alpha, [1;2;0;-2100], 8, t);
t_is(totcost(gencost, [0;0;0;-30])-alpha, [1;2;0;-2400], 8, t);
t_is(totcost(gencost, [0;0;0;-35])-alpha, [1;2;0;-2700], 8, t);

gencost = modcost(gencost0, -alpha, 'SHIFT_X');

t = 'modcost vector SHIFT_X - quadratic';
t_is(totcost(gencost, [0;0;0;0]-alpha), [1;2;0;0], 8, t);
t_is(totcost(gencost, [1;0;0;0]-alpha), [1.11;2;0;0], 8, t);
t_is(totcost(gencost, [2;0;0;0]-alpha), [1.24;2;0;0], 8, t);

t = 'modcost vector SHIFT_X - 4th order polynomial';
t_is(totcost(gencost, [0;0;0;0]-alpha), [1;2;     0;0], 8, t);
t_is(totcost(gencost, [0;1;0;0]-alpha), [1;2.3456;0;0], 8, t);
t_is(totcost(gencost, [0;2;0;0]-alpha), [1;2.8096;0;0], 8, t);

t = 'modcost vector SHIFT_X - pwl (gen)';
t_is(totcost(gencost, [0;0;5;0 ]-alpha), [1;2;100;0], 8, t);
t_is(totcost(gencost, [0;0;10;0]-alpha), [1;2;200;0], 8, t);
t_is(totcost(gencost, [0;0;15;0]-alpha), [1;2;400;0], 8, t);
t_is(totcost(gencost, [0;0;20;0]-alpha), [1;2;600;0], 8, t);
t_is(totcost(gencost, [0;0;25;0]-alpha), [1;2;900;0], 8, t);
t_is(totcost(gencost, [0;0;30;0]-alpha), [1;2;1200;0], 8, t);
t_is(totcost(gencost, [0;0;35;0]-alpha), [1;2;1500;0], 8, t);

t = 'modcost vector SHIFT_X - pwl (load)';
t_is(totcost(gencost, [0;0;0;-5 ]-alpha), [1;2;0;-500], 8, t);
t_is(totcost(gencost, [0;0;0;-10]-alpha), [1;2;0;-1000], 8, t);
t_is(totcost(gencost, [0;0;0;-15]-alpha), [1;2;0;-1400], 8, t);
t_is(totcost(gencost, [0;0;0;-20]-alpha), [1;2;0;-1800], 8, t);
t_is(totcost(gencost, [0;0;0;-25]-alpha), [1;2;0;-2100], 8, t);
t_is(totcost(gencost, [0;0;0;-30]-alpha), [1;2;0;-2400], 8, t);
t_is(totcost(gencost, [0;0;0;-35]-alpha), [1;2;0;-2700], 8, t);

t = 'modcost vector empty gencost';
gencost = modcost([], alpha);
t_ok(isempty(gencost), t);

t_end;


% Undistort points using camera parameters
%
% This function is much faster than Matlab's undistortPoints and works on large
% point sets. The code is based on cvUndistortPoints function from OpenCV library.
% Also this function works with NaN values.
%
%  USAGE
%   undistortedPoints = undistortPoints(points, cameraParams)
%   points              Nx2 ([x y]) matrix of points.
%   cameraParams        Camera calibration information.
%   undistortedPoints   Nx2 ([x y]) matrix with undistorted points.
%
function undistortedPoints = undistortPoints(points, cameraParams)
    origin = [cameraParams.IntrinsicMatrix(3, 1:2)];
    cx = origin(1);
    cy = origin(2);
    fx = cameraParams.IntrinsicMatrix(1, 1);
    fy = cameraParams.IntrinsicMatrix(2, 2);
    ifx = 1 / fx;
    ify = 1 / fy;
    iters = 50;
    k = cameraParams.RadialDistortion;
    p_coef = cameraParams.TangentialDistortion;

    if length(k) == 2
        k(3) = 0;
    end

    x = points(:, 1);
    y = points(:, 2);
    x = (x - cx) .* ifx;
    y = (y - cy) .* ify;
    x0 = x;
    y0 = y;

    for j = 1:iters
        r2 = x.*x + y.*y;
        icdist = 1 ./ (1 + ((k(3).*r2 + k(2)).*r2 + k(1)) .* r2);
        deltaX = 2.*p_coef(1).*x.*y + p_coef(2).*(r2 + 2.*x.*x);
        deltaY = p_coef(1) .* (r2 + 2.*y.*y) + 2.*p_coef(2).*x.*y;
        x = (x0 - deltaX) .* icdist;
        y = (y0 - deltaY) .* icdist;
    end

    undistortedPoints(:, 1) = round(x .* fx + cx);
    undistortedPoints(:, 2) = round(y .* fy + cy);
end


function [d,S] = solve_for_d_S(Y,A,R,options)
[N,B] = size(Y);
M = size(A,2);

sigma0 = 0.1; % initial sigma
sigma_max = 1;
sigma_min = 1e-9;
delta_t0 = 1e-9;

disp('Start estimating D and S');
C = kron((A'*A),eye(B));
YAR = (Y-A*R)';
Z = YAR*A;
E = diag(sum(YAR.^2,2));
F1 = YAR*A;
F = zeros(M*B,B);
for i = 1:M
    F((i-1)*B+1:i*B,:) = diag(F1(:,i));
end

d = (1/N)*sum(YAR.^2,2);
d = 1./sqrt(d);

S = eye(M*B,M*B);
if 0
    u = 1/sqrt(B) * ones(B,1);
    S1 = sigma0^2*u*u' + 1e-4*eye(B);
    S1 = inv(S1);
else
    S1 = sigma0^(-2)*eye(B);
end

for j = 1:M
    inds = (j-1)*B+1:j*B;
    S(inds,inds) = diag(1./d) * S1 * diag(1./d);
end

errors2 = eval_obj_fun_D_S(S,d,C,YAR,Z,M,B,N);
for iter = 1:300
    % solve for S
    z = diag(d)*Z;
    z = z(:);
    if 0
        obj_fun_S = @(S) eval_obj_fun_S(S,C,z,M,B);
        fun_der_S = @(S) calc_der_S(S,C,z,M,B);
        proj_fun_S = @(S) project_to_spd(S,d,sigma_min,sigma_max,M,B);

        S = projected_gradient_descent(S, obj_fun_S, fun_der_S, ...
            proj_fun_S, delta_t0);
    else
        if 1
            proj_fun_S = @(S) project_to_spd(S,d,sigma_min,sigma_max,1,B);
            for j = 1:M
                obj_fun_Sj = @(Sj) eval_obj_fun_Sj(Sj,S,C,z,M,B,j);
                fun_der_Sj = @(Sj) calc_der_Sj(Sj,S,C,z,B,j);
                inds = (j-1)*B+1:j*B;
                Sj = S(inds,inds);
                Sj = projected_gradient_descent(Sj, obj_fun_Sj, fun_der_Sj, ...
                    proj_fun_S, 1e-5);
                S(inds,inds) = Sj;
            end
        else
            der_S = calc_der_S(S,C,z,M,B);
            proj_fun_S = @(S) project_to_spd(S,d,sigma_min,sigma_max,1,B);
            for j = 1:M
                obj_fun_Sj = @(Sj) eval_obj_fun_Sj(Sj,S,C,z,M,B,j);
                fun_der_Sj = @(Sj) calc_der_Sj_approx(Sj,der_S,B,j);
                inds = (j-1)*B+1:j*B;
                Sj = S(inds,inds);
                Sj = projected_gradient_descent(Sj, obj_fun_Sj, fun_der_Sj, ...
                    proj_fun_S, 1e-3);
                S(inds,inds) = Sj;
            end
        end
    end

    % solve for D
    Q = S + C;
    G = (1/N)*(E-F'*(Q\F));
    fcn_d = @(d) G*d - 1./d;
    fsolve_opts = optimset('Display','off');
    d = fsolve(fcn_d, 1./sqrt((1/N)*diag(E)), fsolve_opts);

    % calc error
    err = eval_obj_fun_D_S(S,d,C,YAR,Z,M,B,N);
    errors2 = [errors2;err];
    if test_convergence(errors2, 1e-8), break; end

    if mod(iter,10) == 0
        disp(['Process iteration ',num2str(iter),'. The objective ',...
            'function has value ',num2str(err)]);
    end
end

function S = project_to_spd(S,d,sigma_min,sigma_max,M,B)
touch_bd = 0;
S = (S + S')/2;
for j = 1:M
    inds = (j-1)*B+1:j*B;
    S3 = S(inds,inds);
    SigmaInv = diag(d) * S3 * diag(d);
    [V,D] = eig(SigmaInv);
    d1 = diag(D);
    low_bd = 1/(sigma_max^2);
    up_bd = 1/(sigma_min^2);

    if ~isempty(find(d1<low_bd, 1))
        touch_bd = touch_bd + 1;
%         break;
    end

    d1(d1<low_bd) = low_bd;
    d1(d1>up_bd) = up_bd;
    S(inds,inds) = diag(1./d)*V*diag(d1)*V'*diag(1./d);
end

if touch_bd >= 1
%     disp(['Uncertainty amount upper bound touched ',num2str(touch_bd),' times']);

    % Once uncertainty amount upper bound touched. It means the change of
    % the covariance is too large. Setting S to 0 will make the objective
    % function be Inf, thus revoke the delta_t.

%     if touch_bd >= 1
%         S = 0 * eye(M*B);
%     end
end

function val = eval_obj_fun_Sj(Sj,S,C,z,M,B,j)
inds = (j-1)*B+1:j*B;
S(inds,inds) = Sj;
val = eval_obj_fun_S(S,C,z,M,B);

function val = eval_obj_fun_S(S,C,z,M,B)
Q = S + C;
% R = chol(Q);
val1 = 0;
for j = 1:M
    inds = (j-1)*B+1:j*B;
    val1 = val1 + logdet(S(inds,inds));
end

% val = -z'*(Q\z) + logdet(Q) - val1;
R = chol(Q);
y = R'\z;
val = -sum(y.^2) + 2*sum(log(diag(R))) - val1;

function val = eval_obj_fun_D_S(S,d,C,YAR,Z,M,B,N)
z = diag(d)*Z;
z = z(:);

val = eval_obj_fun_S(S,C,z,M,B);
val = val + sum(sum((diag(d)*YAR).^2)) - 2*N*sum(log(d));

function der = calc_der_S(S,C,z,M,B)
Q = S + C;
invQ = inv(Q);
y = invQ*z;
T = y*y';
der = zeros(M*B,M*B);
for j = 1:M
    inds = (j-1)*B+1:j*B;
    Sj = S(inds,inds);
    der(inds,inds) = T(inds,inds) + invQ(inds,inds) - inv(Sj);
end

% der_norm = norm(der,'fro');
% der = der/der_norm;

function der = calc_der_Sj(Sj,S,C,z,B,j)
inds = (j-1)*B+1:j*B;
S(inds,inds) = Sj;

Q = S + C;
invQ = inv(Q);
y = invQ*z;
T = y*y';
der = zeros(B,B);

der = T(inds,inds) + invQ(inds,inds) - inv(Sj);

der_norm = norm(der,'fro');
der = der/der_norm;

function der = calc_der_Sj_approx(Sj,der_S,B,j)
inds = (j-1)*B+1:j*B;
der = der_S(inds,inds);

der_norm = norm(der,'fro');
der = der/der_norm;

function d = solve_for_d1(Y,A,R,S)
[N,B] = size(Y);
M = size(A,2);

YAR = Y - A*R;
E = diag(sum(YAR.^2,1));

Q = S + kron((A'*A),eye(B));

F1 = A'*YAR;
F = zeros(M*B,B);
for i = 1:M
    F((i-1)*B+1:i*B,:) = diag(F1(i,:));
end
G = (1/N)*(E-F'*(Q\F));

fcn_d = @(d) G*d - 1./d;
d = fsolve(fcn_d, 1./sqrt((1/N)*diag(E)));


function ef = cpf_qlim_event(cb_data, cx)
%CPF_QLIM_EVENT  Event function to detect gen reactive power limit violations
%   EF = CPF_QLIM_EVENT(CB_DATA, CX)
%
%   CPF event function to detect generator reactive power limit violations,
%   i.e. Qg <= Qmin or Qg >= Qmax.
%
%   Inputs:
%       CB_DATA : struct of data for callback functions
%       CX : struct containing info about current point (continuation soln)
%
%   Outputs:
%       EF : event function value

%   MATPOWER
%   Copyright (c) 2016, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%   and Shrirang Abhyankar, Argonne National Laboratory
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% event function value is 2 ng x 1 vector equal to:
%%      [ Qg - Qmax ]
%%      [ Qmin - Qg ]

%% define named indices into bus, gen, branch matrices
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
[GEN_BUS, PG, QG, QMAX, QMIN, VG, MBASE, GEN_STATUS, PMAX, PMIN, ...
    MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN, PC1, PC2, QC1MIN, QC1MAX, ...
    QC2MIN, QC2MAX, RAMP_AGC, RAMP_10, RAMP_30, RAMP_Q, APF] = idx_gen;

%% get updated MPC
d = cb_data;
mpc = cpf_current_mpc(d.mpc_base, d.mpc_target, ...
    d.Ybus, d.Yf, d.Yt, d.ref, d.pv, d.pq, cx.V, cx.lam, d.mpopt);

%% compute Qg violations for on-line gens, not at PQ buses
nb = size(mpc.bus, 1);
ng = size(mpc.gen, 1);
on = find(mpc.gen(:, GEN_STATUS) > 0 & ...  %% which generators are on?
          mpc.bus(mpc.gen(:, GEN_BUS), BUS_TYPE) ~= PQ);  %% ... and are not PQ buses
gbus = mpc.gen(on, GEN_BUS);                %% what buses are they at?
ngon = size(on, 1);

%% build connection matrix, element i, j is 1 if gen on(i) at bus j is ON
Cg = sparse((1:ngon)', gbus, ones(ngon, 1), ngon, nb);
C = Cg * Cg';

%% violations are based on total violation at bus, not individual violations
%% (see https://github.com/MATPOWER/matpower/issues/26)
v_Qmax = NaN(ng, 1);
v_Qmin = v_Qmax;
v_Qmax(on) = C * (mpc.gen(on, QG) - mpc.gen(on, QMAX));
v_Qmin(on) = C * (mpc.gen(on, QMIN) - mpc.gen(on, QG));

%% assemble event function value
ef = [v_Qmax; v_Qmin];


% [INPUT]
% data = A float t-by-n matrix containing the time series to be sanitized.
% x = A vector of length t containing the numeric observation dates of the time series (if empty, observations are assumed to be linearly spaced between 1 and t).
% w = An integer [5,21] representing the length of the moving window used to detect outliers (if empty, no outliers replacement is performed).
% m = A vector of 2 floats (-Inf,Inf) containing minimum and maximum clamping values (if empty, no clamping is performed).
%
% [OUTPUT]
% data = A float t-by-n matrix containing the sanitized time series.

function data = sanitize_data(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('data',@(x)validateattributes(x,{'double'},{'real' '2d' 'nonempty'}));
        ip.addRequired('x',@(x)validateattributes(x,{'double'},{'real'}));
        ip.addRequired('w',@(x)validateattributes(x,{'double'},{'real'}));
        ip.addRequired('m',@(x)validateattributes(x,{'double'},{'real'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    data = ipr.data;
    [x,w,m] = validate_input(data,ipr.x,ipr.w,ipr.m);

    nargoutchk(1,1);

    data = sanitize_data_internal(data,x,w,m);

end

function data = sanitize_data_internal(data,x,w,m)

    for i = 1:size(data,2)
        y = data(:,i);

        nan_indices = isnan(y);

        if (all(nan_indices))
            continue;
        end

        if (any(nan_indices))
            y = fill_missing_values(y,x,nan_indices);
        end

        if (~isempty(w))
            y = replace_outliers(y,x,w);
        end

        if (~isempty(m))
            y = min(max(y,m(1)),m(2));
        end

        data(:,i) = y;
    end

end

function y = fill_missing_values(y,x,nan_indices)

    d = diff(nan_indices);

    if (nan_indices(1))
        z = find(d == -1,1,'first');
        y(1:z) = y(z+1);
        nan_indices(1:z) = 0;
    end

    if (nan_indices(end))
        z = find(d == 1,1,'last');
        y(z+1:end) = y(z);
        nan_indices(z+1:end) = 0;
    end

    y(nan_indices) = spline(x(~nan_indices),y(~nan_indices),x(nan_indices));

end

function y = replace_outliers(y,x,w)

    f = 3 * (-1 / (sqrt(2) * erfcinv(1.5)));
    k = floor(w * 0.5);
    n = numel(y);
    p = nan(k,1);

    xp = [p; y; p];

    m_med = zeros(n,1);
    m_mad = zeros(n,1);

    for i = 1:k
        x_i = y(1:k+i);
        m = median(x_i);

        m_med(i) = m;
        m_mad(i) = median(abs(x_i - m));
    end

    for i = k+1:n-k-1
        x_i = xp(i:i+w,:);
        m = median(x_i);

        m_med(i) = m;
        m_mad(i) = median(abs(x_i - m));
    end

    for i = n-k:n
        x_i = y(i-k:end);
        m = median(x_i);

        m_med(i) = m;
        m_mad(i) = median(abs(x_i - m));
    end

    b = m_mad .* f;
    lb = m_med - b;
    ub = m_med + b;

    is_outlier = (y > ub) | (y < lb);
    d = diff(is_outlier);

    if (is_outlier(1) == 1)
        z = find(d == -1,1,'first');
        y(1:z) = y(z+1);
        is_outlier(1:z) = 0;
    end

    if (is_outlier(end))
        z = find(d == 1,1,'last');
        y(z+1:end) = y(z);
        is_outlier(z+1:end) = 0;
    end

    y(is_outlier) = spline(x(~is_outlier),y(~is_outlier),x(is_outlier));

end

function [x,w,m] = validate_input(data,x,w,m)

    t = size(data,1);

    if (isempty(x))
        x = 1:t;
    else
        if (~isvector(x))
            error('The value of ''x'' is invalid. Expected input to be a vector.');
        end

        if (numel(x) ~= t)
            error(['The value of ''x'' is invalid. Expected input to contain ' num2str(t) ' elements.']);
        end

        if (~all(isfinite(x)))
            error('The value of ''x'' is invalid. Expected input to contain finite elements.');
        end

        if (~all(diff(x) > 0))
            error('The value of ''x'' is invalid. Expected input to contain increasing elements.');
        end
    end

    if (~isempty(w))
        if (~isscalar(w))
            error('The value of ''w'' is invalid. Expected input to be a scalar.');
        end

        if (~isfinite(w))
            error('The value of ''w'' is invalid. Expected input to be finite.');
        end

        if (floor(w) ~= w)
            error('The value of ''w'' is invalid. Expected input to be an integer.');
        end

        if ((w < 5) || (w > 21))
            error('The value of ''w'' is invalid. Expected input to have a value >= 5 and <= 21.');
        end

        w = w - 1;
    end

    if (~isempty(m))
        if (~isvector(m))
            error('The value of ''m'' is invalid. Expected input to be a vector.');
        end

        if (numel(m) ~= 2)
            error('The value of ''m'' is invalid. Expected input to contain 2 elements.');
        end

        if (~all(isfinite(m)))
            error('The value of ''m'' is invalid. Expected input to contain finite elements.');
        end

        if (any(floor(m) ~= m))
            error('The value of ''m'' is invalid. Expected input to contain integer elements.');
        end

        if (m(1) >= m(2))
            error('The value of ''m'' is invalid. Expected input first element to be less than the input second element.');
        end
    end

end


function [model, B, elapse] = AGH2_learn(A, maxbits, Anchor,s)
%   This is a function of Two Layer AGH (Anchor Graph Hashing) learning.
%
%	Usage:
%	[model, B,elapse] = AGH2_learn(A, maxbits, Anchor,s)
%
%	      A: Rows of vectors of data points. Each row is sample point
%   maxbits: Code length
%    Anchor: Anchors (landmarks), Each row is sample point
%         s: Number of nearest anchor to learn a representation
%            of the sample vector
%
%     model: Used for encoding a test sample point.
%	      B: The binary code of the input data A. Each row is sample point
%    elapse: The coding time (training time).
%
%
%
%   version 2.0 --Nov/2016
%   version 1.0 --Jan/2014
%
%   Written by  Yue Lin (linyue29@gmail.com)
%               Deng Cai (dengcai AT gmail DOT com)
%

tmp_T = tic;

options.CodingMethod = 'Gaussian';

nAnchor = 1500;
if ~exist('Anchor','var')
    [~,Anchor]=litekmeans(A,nAnchor,'MaxIter',5,'Replicates',1);
end
if ~exist('s','var')
    s = 50;
end

[B, W, Thres, sigma] = TwoLayerAGH_Train(A, Anchor, maxbits, s, 0, options);

model.W = W;
model.Thres = Thres;
model.sigma = sigma;
model.Anchor = Anchor;
model.s = s;
model.options = options;

elapse = toc(tmp_T);
end


function edge = lineToEdge(line)
%LINETOEDGE Convert a straight line to a finite edge.
%
%   EDGE = lineToEdge(LINE)
%   Returns the edge with same origin as the line LINE, and with second
%   extremity corresponding to the addition of line origin and direction.
%   LINE is represented as [X0 Y0  DX DY]
%   EDGE is represented as [X1 Y1  X2 Y2]
%
%   Example
%     line = [3 4  1 2];
%     edge = lineToEdge(line)
%     edge =
%          3   4   4   6
%
%   See also
%     lines2d, edges2d, edgeToLine

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2019-05-07, using Matlab 9.6.0.1072779 (R2019a)
% Copyright 2019-2022 INRA - Cepia Software Platform

edge = [line(:, 1:2) line(:,1:2)+line(:,3:4)];


function [varargout] = ndgrid(varargin)

%   NDGRID Generation of arrays for N-D functions and interpolation.
%   [X1,X2,X3,...] = NDGRID(x1,x2,x3,...) transforms the domain
%   specified by vectors x1,x2,x3, etc. into arrays X1,X2,X3, etc. that
%   can be used for the evaluation of functions of N variables and N-D
%   interpolation.  The i-th dimension of the output array Xi are copies
%   of elements of the vector xi.
%
%   [X1,X2,...] = NDGRID(x) is the same as [X1,X2,...] = NDGRID(x,x,...).
%
%   For example, to evaluate the function  x2*exp(-x1^2-x2^2-x^3) over the
%   range  -2 < x1 < 2,  -2 < x2 < 2, -2 < x3 < 2,
%
%       [x1,x2,x3] = ndgrid(-2:.2:2, -2:.25:2, -2:.16:2);
%       z = x2 .* exp(-x1.^2 - x2.^2 - x3.^2);
%       slice(x2,x1,x3,z,[-1.2 .8 2],2,[-2 -.2])
%
%   NDGRID is like MESHGRID except that the order of the first two input
%   arguments are switched (i.e., [X1,X2,X3] = NDGRID(x1,x2,x3) produces
%   the same result as [X2,X1,X3] = MESHGRID(x2,x1,x3)).  Because of
%   this, NDGRID is better suited to N-D problems that aren't spatially
%   based, while MESHGRID is better suited to problems in cartesian
%   space (2-D or 3-D).
%
%   This is a drop-in replacement for the matlab version in elmat, which is
%   relatively slow for big grids. FIXME this function still only works up
%   to 5 dimensions
%
%   See also MESHGRID, INTERPN.

%   Copyright(C) 2010, Jan-Mathijs Schoffelen, DCCN
%
% This file is part of FieldTrip, see http://www.ru.nl/neuroimaging/fieldtrip
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id: ndgrid.m 2956 2011-02-25 19:57:31Z jansch $

if nargin==0
  error('MATLAB:ndgrid:NotEnoughInputs', 'Not enough input arguments.');
end
if nargin==1, varargin = repmat(varargin,[1 max(nargout,2)]); end

ndims = numel(varargin);
switch ndims
case 2
  ones1 = ones(1,numel(varargin{1}));
  ones2 = ones(1,numel(varargin{2}));

  x   = varargin{1}(:);
  y   = varargin{2}(:)';

  varargout{1} = x(:, ones2);
  varargout{2} = y(ones1, :);
case 3
  ones1 = ones(1,numel(varargin{1}));
  ones2 = ones(1,numel(varargin{2}));
  ones3 = ones(1,numel(varargin{3}));

  x   = varargin{1}(:);
  y   = varargin{2}(:)';
  z   = zeros(1,1,numel(varargin{3}));
  z(:) = varargin{3};

  varargout{1} = x(:, ones2, ones3);
  varargout{2} = y(ones1, :, ones3);
  varargout{3} = z(ones1, ones2, :);
case 4
  ones1 = ones(1,numel(varargin{1}));
  ones2 = ones(1,numel(varargin{2}));
  ones3 = ones(1,numel(varargin{3}));
  ones4 = ones(1,numel(varargin{4}));

  x   = varargin{1}(:);
  y   = varargin{2}(:)';
  z   = zeros(1,1,numel(varargin{3}));
  z(:) = varargin{3};
  xx   = zeros(1,1,1,numel(varargin{4}));
  xx(:) = varargin{4};

  varargout{1} = x(:, ones2, ones3, ones4);
  varargout{2} = y(ones1, :, ones3, ones4);
  varargout{3} = z(ones1, ones2, :, ones4);
  varargout{4} = xx(ones1, ones2, ones3, :);
case 5
  ones1 = ones(1,numel(varargin{1}));
  ones2 = ones(1,numel(varargin{2}));
  ones3 = ones(1,numel(varargin{3}));
  ones4 = ones(1,numel(varargin{4}));
  ones5 = ones(1,numel(varargin{5}));

  x   = varargin{1}(:);
  y   = varargin{2}(:)';
  z   = zeros(1,1,numel(varargin{3}));
  z(:) = varargin{3};
  xx   = zeros(1,1,1,numel(varargin{4}));
  xx(:) = varargin{4};
  yy   = zeros(1,1,1,1,numel(varargin{5}));
  yy(:) = varargin{5};

  varargout{1} = x(:, ones2, ones3, ones4, ones5);
  varargout{2} = y(ones1, :, ones3, ones4, ones5);
  varargout{3} = z(ones1, ones2, :, ones4, ones5);
  varargout{4} = xx(ones1, ones2, ones3, :,ones5);
  varargout{5} = yy(ones1, ones2, ones3, :,ones5);
otherwise
  error('this version of ndgrid supports inputs up to 5 dimensions');
  %call the ndgrid from elmat
  %FIXME this has to be done
end


function blas1_z_test02 ( )

%*****************************************************************************80
%
%% TEST02 tests DZNRM2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 January 2011
%
%  Author:
%
%    John Burkardt
%
  n = 5;

  x = [ ...
     2.0 - 1.0 * i, ...
    -4.0 - 2.0 * i, ...
     3.0 + 1.0 * i, ...
     2.0 + 2.0 * i, ...
    -1.0 - 1.0 * i ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02\n' );
  fprintf ( 1, '  DZNRM2 returns the Euclidean norm of a complex vector.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The vector X:\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  %10f  %10f\n', j, real ( x(j) ), imag ( x(j) ) );
  end

  incx = 1;
  norm = dznrm2 ( n, x, incx );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The L2 norm of X is %f\n', norm );

  return
end


function test_suite=test_pca
% tests for cosmo_pca
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #
    try % assignment of 'localfunctions' is necessary in Matlab >= 2016
        test_functions=localfunctions();
    catch % no problem; early Matlab versions can use initTestSuite fine
    end
    initTestSuite;

function [pca_samples,coef,mu,expl]=helper_cosmo_pca_wrapper(samples,...
                                                            keep_count)
    if isnan(keep_count)
        args={};
    else
        args={keep_count};
    end

    [pca_samples,params]=cosmo_pca(samples,args{:});
    coef=params.coef;
    mu=params.mu;
    expl=params.explained;

function [pca_samples,coef,mu,expl]=helper_matlab_pca_wrapper(samples,...
                                                            keep_count)
    % PCA implementation using Matlab statistics toolbox
    cosmo_check_external('!pca',true);

    if isnan(keep_count)
        args={};
    else
        args={'NumComponents',keep_count};
    end

    [coef,pca_samples,unused,unused,expl,mu]=pca(samples,args{:});
    expl=expl';

function test_pca_more_samples_than_features
    nsamples=ceil(rand()*10)+10;
    nfeatures=nsamples+10;

    nfeatures=5;
    nsamples=2;

    helper_test_pca_correspondence(nsamples,nfeatures)

function test_pca_more_features_than_samples
    nfeatures=ceil(rand()*10)+10;
    nsamples=nfeatures+10;

    helper_test_pca_correspondence(nsamples,nfeatures)

function test_pca_col_vector
    nfeatures=1;
    nsamples=ceil(rand()*10)+10;

    helper_test_pca_correspondence(nsamples,nfeatures)

function test_pca_row_vector
    nfeatures=ceil(rand()*10)+10;
    nsamples=1;

    helper_test_pca_correspondence(nsamples,nfeatures)

function test_pca_near_square_samples
    nfeatures=ceil(rand()*10)+10;
    for nsamples=nfeatures+(-1:1);
        helper_test_pca_correspondence(nsamples,nfeatures)
    end

function test_pca_too_many_components
    nsamples=ceil(rand()*10)+10;
    nfeatures=nsamples;
    for nkeep=nfeatures+(-1:1)
        handle=@()cosmo_pca(rand(nsamples,nfeatures),nkeep);
        if nkeep>nfeatures
            assertExceptionThrown(handle,'');
        else
            handle(); % should be ok
        end
    end

function test_pca_regression
    xs=[   2.032   -0.8918  -0.8258    1.163    1.157   -1.291   ;...
           0.5838   1.844    1.166    -0.8484   3.493   -0.1991  ;...
          -1.444   -0.2617  -1.921     3.085   -1.372    1.727   ;...
          -0.5177   2.339    0.4412    1.856    0.4794   0.08323 ;...
           1.191   -0.204   -0.2088    1.755   -0.9548   0.5012  ;...
          -1.326    2.724    0.1476    0.5024   3.407   -0.4803  ];

    s=[  -0.5008    2.8648   -0.7589   -0.5301    0.0144  ;...
          3.5030    0.5915    0.2537    0.8888    0.0226  ;...
         -3.8914   -1.6525   -0.7549    0.4558    0.0157  ;...
          0.1140   -1.3350    1.0615   -0.7253    0.0293  ;...
         -2.1851    1.0744    0.9553    0.2445   -0.0444  ;...
          2.9603   -1.5432   -0.7566   -0.3338   -0.0376];

    coef=[  0.0014    0.7569    0.3369   -0.0540    0.3234  ;...
            0.4052   -0.5164    0.4105   -0.3355   -0.0290  ;...
            0.3133    0.0275    0.6618    0.0901    0.1745  ;...
           -0.4294   -0.2190   -0.0420   -0.5306    0.6797  ;...
            0.7090    0.0436   -0.5070    0.0744    0.4815  ;...
           -0.2251   -0.3313    0.1456    0.7677    0.4127];

    mu=[0.0864    0.9248   -0.2001    1.2522    1.0349    0.0568];

    explained=[64.7794   26.0994    6.0071    3.1059    0.0082];

    for nkeep=[NaN,1:7]
        if isnan(nkeep)
            args={};
            ncomp=5;
        else
            args={nkeep};
            ncomp=min(nkeep,5);
        end

        if nkeep>6
            assertExceptionThrown(@()cosmo_pca(xs,args{:}),'');
        else
            [xs_pca,param]=cosmo_pca(xs,args{:});

            tolerance_arg={'absolute',5e-3};
            assertElementsAlmostEqual(xs_pca,s(:,1:ncomp),...
                                        tolerance_arg{:});

            expected_fieldnames={'coef','explained','mu'};
            assertEqual(sort(fieldnames(param)),...
                            sort(expected_fieldnames(:)));
            assertElementsAlmostEqual(param.coef,coef(:,1:ncomp),...
                                                tolerance_arg{:});
            assertElementsAlmostEqual(param.mu,mu,...
                                                tolerance_arg{:});
            assertElementsAlmostEqual(param.explained,explained,...
                                                tolerance_arg{:});

        end
    end

function test_pca_basic_properties
    nfeatures=ceil(rand()*10+10);
    nsamples=ceil(rand()*10+10)+nfeatures;

    x=randn(nsamples,nfeatures);
    [y,param]=cosmo_pca(x);

    % explained variance is on diagonal
    d=y'*y;
    assertElementsAlmostEqual(100*diag(d)/trace(d),param.explained');

    % components are orthogonal
    d_zero_diag=d-diag(diag(d));
    assertElementsAlmostEqual(d_zero_diag,zeros(nfeatures));

    % average is computed correctly
    assertElementsAlmostEqual(mean(x,1),param.mu);

    % x can be reconstructed
    assertElementsAlmostEqual(x,bsxfun(@plus,param.mu,y*param.coef'));

function test_pca_exceptions()
    aet=@(varargin)assertExceptionThrown(@()...
                    cosmo_pca(varargin{:}),'');
    aet(struct);
    aet({1});
    aet(randn([2 2 2 ]));

function helper_test_pca_correspondence(nsamples,nfeatures)
    if cosmo_skip_test_if_no_external('!pca')
        return;
    end

    for nkeep=[NaN,-1,0,1,...
                ceil(nsamples/2),ceil(nfeatures/2),...
                nsamples-1,nfeatures-1,...
                nsamples,nfeatures,nsamples+1,nfeatures+1]
        helper_test_pca_correspondence_nkeep(nsamples,nfeatures,nkeep);
    end

function helper_test_pca_correspondence_nkeep(nsamples,nfeatures,nkeep)
    x=rand(nsamples,nfeatures);
    try
        % if the following statement throws an exception, then
        % matlab's pca must also throw an exception
        [p1,c1,m1,e1]=helper_matlab_pca_wrapper(x,nkeep);
    catch
        % cosmo pca should also throw error
        assertExceptionThrown(@()helper_cosmo_pca_wrapper(x,nkeep),'');
        return
    end

    % no error, verify that output match
    [p2,c2,m2,e2]=helper_cosmo_pca_wrapper(x,nkeep);

    tolerance_arg={'relative',1e-5};
    assertElementsAlmostEqual(p1,p2,tolerance_arg{:});
    assertElementsAlmostEqual(c1,c2,tolerance_arg{:});
    assertElementsAlmostEqual(m1,m2,tolerance_arg{:});
    assertElementsAlmostEqual(e1,e2,tolerance_arg{:});

function test_pca_retain_is_row_vector()
    nsamples=ceil(10+rand()*10);
    x=randn(nsamples);
    [y,params]=cosmo_pca(x);
    assertEqual(size(params.explained),[1 nsamples-1]);