function cout=filterbank(cin,h,g,order,basis)

%tstoolbox/@core/filterbank
%   Syntax:
%     * filterbank(cin,H,G,ORDER,BASIS)
%
%   Input Arguments:
%     * H - lowpass filter
%     * G - highpass filter
%     * ORDER - indicates the type of tree:
%          + 0 - band sorting according to the filter bank
%          + 1 - band sorting according to the frequency decomposition
%     * BASIS - desired subband decomposition
%
%   calculates the Wavelet Packet Transform of cin. It can be obtained
%   using a selection algorithm function. It may be switched from one
%   format to another using CHFORMAT. The different bands are sorted
%   according to ORDER and BASIS. If BASIS is omitted, the output is a
%   matrix with the coefficients obtained from all the wavelet packet
%   basis in the library. Each column in the matrix represents the outputs
%   for a level in the tree. The first column is the original signal. If
%   the length of X is not a power of 2, the columns are zero padded to
%   fit the different lengths. Run the script 'BASIS' for help on the
%   basis format.
%   See also: IWPK, CHFORMAT, PRUNEADD, PRUNENON, GROWADD, GROWNON.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

%       Auxiliary input argument for recursion:
%       flag: if set to 1, indicates that no basis is specified
%          and the output will be a matrix
%       flag2: if set to 1 the function was called with the detail signal,
%          then the coefficients must be rearranged

cout = core(wpk(data(cin), h,g,order,basis));

function w = wpk(x, h,g,order,basis,flag,flag2)

if ((nargin==4)|(nargin==5))
        if nargin==4
                L=floor(log2(length(x)));       % initialization when
                basis=ones(2^L,1)*(L-1);        % no basis was specified
                flag=1;
                flag2=0;
        else                                    % basis was specified
                if (~basis)
                        w=x;
                        return
                end
                basis=basis-1;
                flag=0;
                flag2=0;
        end
        if size(x,2)~=1                 % shapes the signal as column
                x=x(:);                 % vector if necessary
                fil=1;
        else
                fil=0;
        end
end

wx=wt(x,h,g,1);                         % perform one analysis level
                                        % into the analysis tree

N=length(wx);                           % separate approximation and detail
a=wx(1:N/2);                            % at the analysis output
d=wx(N/2+1:N);

if (flag2*order)                        % if the recursion is going to be
        s1=d;s2=a;                      % performed on the detail signal,
else                                    % and it is frequency sorted, the
        s1=a;s2=d;                      % outputs must be swapped
end

if all(basis==0)                           % two ending nodes achieved
        w=[s1;s2];
        if (nargin==5)&(fil), w=w'; end
        return
end;

tope=1;                                 % finds the point where the
suma=2^(-basis(1));                     % basis vector must be divided
i=1;                                    % in order to call the recursion
while (suma<tope)
        i=i+1;
        suma=suma+2^(-basis(i));
end

if all(basis(1:i)~=0)                                   % a level with an ending node
        wa=wpk(s1,h,g,order,(basis(1:i)-1),flag,0);     % but the other node continues
else wa=s1;
end

if all(basis(i+1:length(basis))~=0)
        wd=wpk(s2,h,g,order,(basis(i+1:length(basis))-1),flag,1);   % complementary case
else wd=s2;
end

if flag                                 % output is a matrix
        lwd=length(wd);
        lwx=length(wx);
        nz=2*lwd-lwx;                   % find the number of zeros
                                        % to be inserted for padding

        w=[ [s1;zeros(nz/2,1);s2;zeros(nz/2,1)] , [wa;wd] ];
else
        w=[wa;wd];
end

if nargin==4
        N=size(w,1);
        w=[ [x;zeros(N-length(x),1)] w];
elseif (nargin==5)&(fil), w=w'; end

function y=wt(x,h,g,k,del1,del2)

% WT   Discrete Wavelet Transform.
%
%      WT(X,H,G,K) calculates the wavelet transform of vector X.
%      If X is a matrix (2D), WT will calculate the one dimensional
%      wavelet transform of each row vector. The second argument H
%      is the lowpass filter and the third argument G the highpass
%      filter.
%
%      The output vector contains the coefficients of the DWT ordered
%      from the low pass residue at scale K to the coefficients
%      at the lowest scale, as the following example ilustrates:
%
%      Output vector (k=3):
%
%      [------|------|------------|------------------------]
%         |       |        |                |
%         |       |        |                `-> 1st scale coefficients
%         |       |        `-----------> 2nd scale coefficients
%         |       `--------------------> 3rd scale coefficients
%         `----------------> Low pass residue  at 3rd scale
%
%
%      If X is a matrix, the result will be another matrix with
%      the same number of rows, holding each one its respective
%      transformation.
%
%      WT (X,H,G,K,DEL1,DEL2) calculates the wavelet transform of
%      vector X, but also allows the users to change the alignment
%      of the outputs with respect to the input signal. This effect
%      is achieved by setting to DEL1 and DEL2 the delays of H and
%      G respectively. The default values of DEL1 and DEL2 are
%      calculated using the function WTCENTER.

% -----------------------------------
%    CHECK PARAMETERS AND OPTIONS
% -----------------------------------

h=h(:)';        % Arrange the filters so that they are row vectors.
g=g(:)';

if length(x)<2^k
        disp('The scale is too high. The maximum for the signal is:')
        floor(log2(length(x)))
        return
end

[liy,lix]=size(x);

if lix==1               % And arrange the input vector to a row if
        x=x';           % it's not a matrix.
        trasp=1;        % (and take note of it)
        [liy,lix]=size(x);
else
        trasp=0;
end

%--------------------------
%    DELAY CALCULATION
%--------------------------

% Calculate delays as the C.O.E. of the filters
dlp=wtcenter(h);
dhp=wtcenter(g);

if rem(dhp-dlp,2)~=0            % difference between them.
        dhp=dhp+1;              % must be even
end;

if nargin==6,                   % Other experimental filter delays
        dlp=del1;               % can be forced from the arguments
        dhp=del2;
end;

%------------------------------
%    WRAPPAROUND CALCULATION
%------------------------------
llp=length(h);                  % Length of the lowpass filter
lhp=length(g);                  % Length of the highpass filter.

L=max([lhp,llp,dlp,dhp]);       % The number of samples for the
                                % wrapparound. Thus, we should need to
                                % move along any L samples to get the
                                % output wavelet vector phase equal to
                                % original input phase.

%------------------------------
%     START THE ALGORITHM
%------------------------------

for it=1:liy,           % For every row of the input matrix...
                        % (this makes one wavelet transform
                        % for each of the rows of the input matrix)
        tm=[];
        t=x(it,:);                      % Copy the vector to transform.

        for i=1:k                       % For every scale (iteration)...
                lx=length(t);
                if rem(lx,2)~=0         % Check that the number of samples
                        t=[t,0];        % will be even (because of decimation).
                        lx=lx+1;
                end
                tp=t;                   % Build wrapparound. The input signal
                pl=length(tp);          % can be smaller than L, so it can
                while L>pl              % be necessary to repeat it several
                        tp=[tp,t];      % times
                        pl=length(tp);
                end

                t=[tp(pl-L+1:pl),t,tp(1:L)];    % Add the wrapparound.

                yl=conv(t,h);           % Then do lowpass filtering ...
                yh=conv(t,g);           % ... and highpass filtering.

                yl=yl((dlp+1+L):2:(dlp+L+lx));    % Decimate the outputs
                yh=yh((dhp+1+L):2:(dhp+L+lx));    % and leave out wrapparound

                tm=[yh,tm];             % Put the resulting wavelet step
                                        % on its place into the wavelet
                                        % vector...
                t=yl;                   % ... and set the next iteration.
        end

        y(it,:)=[t,tm];                 % Wavelet vector (1 row vector)

end                             % End of the "rows" loop.

%------------------------------
%    END OF THE ALGORITHM
%------------------------------

if trasp==1                     % If the input data was a column vector
        y=y';                   % then transpose it.
end

function d=wtcenter(x,op);

%  WTCENTER Calculates the delay of filters for alignment.
%
%           WTCENTER (X) calculates the integer aproximation
%           of delay for filter X using the method set with
%           the WTMETHOD function, for alignment operations
%           in Wavelet transforms.
%
%           For a non integer value, use the CENTER function.
%
%           See also: WTMETHOD, CENTER, WT
%

global WTCENTERMETHOD

if size(WTCENTERMETHOD)==[0,0]
        WTCENTERMETHOD=0;
end

if WTCENTERMETHOD>3 | WTCENTERMETHOD<0
        WTCENTERMETHOD=0
end

d=floor(center(x,WTCENTERMETHOD));

% (Another long function !!!)

function d=center(x,op);

%  CENTER  Delay calculation for Wavelet transform alignment.
%
%          CENTER (X, OP) calculates the delay for filter in X
%          according to the alignment method indicated in OP.
%          This delay is used by Wavelet transform functions.
%          The value of OP can be:
%              0 : First Absolute Maxima Location
%              1 : Zero delay in analysis (Full for synthesis).
%              2 : Mass center (sum(m*d)/sum(m))
%              3 : Energy center (sum(m^2 *d)/sum(m^2))
%
%          If no output argument is given, then the vector X will
%          be plotted in the current figure, and a color line will be
%          marking the result.(red: OP=0; green: OP=1; cyan: OP=2;
%          blue: OP=4)

lx=length(x);
l=1:lx;

if op==1
        d=0;
else
        if op==2
                xx=abs(x(:)');
                L=l;
        end
        if op==3
                xx=x(:)'.^2;
                L=l;
        end
        if op==0
                [mx,d]=max(abs(x));
        else

                d=sum(xx.*L)/sum(xx);
        end
end


function [out_profile,out_IMU_bias_est,out_KF_SD,out_R_matrix,out_Q_matrix,corrections] =...
    Loosely_coupled_INS_GNSS(init_cond, filter_time, epoch, lla, gps, imu, LC_KF_config, est_IMU_bias)
%Loosely_coupled_INS_GNSS - Simulates inertial navigation using ECEF
% navigation equations and kinematic model, GNSS using a least-squares
% positioning algorithm, and loosely-coupled INS/GNSS integration.
%
% Software for use with "Principles of GNSS, Inertial, and Multisensor
% Integrated Navigation Systems," Second Edition.
%
% This function created 12/4/2012 by Paul Groves
%
% Significant changes made to data-formatting and time-handling by Adam
% Werries, 2015/2016.
%
% Inputs:
%   LC_KF_config
%     .init_att_unc           Initial attitude uncertainty per axis (rad)
%     .init_vel_unc           Initial velocity uncertainty per axis (m/s)
%     .init_pos_unc           Initial position uncertainty per axis (m)
%     .init_b_a_unc           Initial accel. bias uncertainty (m/s^2)
%     .init_b_g_unc           Initial gyro. bias uncertainty (rad/s)
%     .gyro_noise_PSD         Gyro noise PSD (rad^2/s)
%     .accel_noise_PSD        Accelerometer noise PSD (m^2 s^-3)
%     .accel_bias_PSD         Accelerometer bias random walk PSD (m^2 s^-5)
%     .gyro_bias_PSD          Gyro bias random walk PSD (rad^2 s^-3)
%     .pos_meas_SD            Position measurement noise SD per axis (m)
%     .vel_meas_SD            Velocity measurement noise SD per axis (m/s)
%     .init_biases            IMU bias initialization
%
% Outputs:
%   out_profile        Navigation solution as a motion profile array
%   out_IMU_bias_est   Kalman filter IMU bias estimate array
%   out_clock          GNSS Receiver clock estimate array
%   out_KF_SD          Output Kalman filter state uncertainties
%
% Format of motion profiles:
%  Column 1: time (sec)
%  Column 2: latitude (rad)
%  Column 3: longitude (rad)
%  Column 4: height (m)
%  Column 5: north velocity (m/s)
%  Column 6: east velocity (m/s)
%  Column 7: down velocity (m/s)
%  Column 8: roll angle of body w.r.t NED (rad)
%  Column 9: pitch angle of body w.r.t NED (rad)
%  Column 10: yaw angle of body w.r.t NED (rad)
%
% Format of output IMU biases array:
%  Column 1: time (sec)
%  Column 2: estimated X accelerometer bias (m/s^2)
%  Column 3: estimated Y accelerometer bias (m/s^2)
%  Column 4: estimated Z accelerometer bias (m/s^2)
%  Column 5: estimated X gyro bias (rad/s)
%  Column 6: estimated Y gyro bias (rad/s)
%  Column 7: estimated Z gyro bias (rad/s)
%
% Format of KF state uncertainties array:
%  Column 1: time (sec)
%  Column 2: X attitude error uncertainty (rad)
%  Column 3: Y attitude error uncertainty (rad)
%  Column 4: Z attitude error uncertainty (rad)
%  Column 5: X velocity error uncertainty (m/s)
%  Column 6: Y velocity error uncertainty (m/s)
%  Column 7: Z velocity error uncertainty (m/s)
%  Column 8: X position error uncertainty (m)
%  Column 9: Y position error uncertainty (m)
%  Column 10: Z position error uncertainty (m)
%  Column 11: X accelerometer bias uncertainty (m/s^2)
%  Column 12: Y accelerometer bias uncertainty (m/s^2)
%  Column 13: Z accelerometer bias uncertainty (m/s^2)
%  Column 14: X gyro bias uncertainty (rad/s)
%  Column 15: Y gyro bias uncertainty (rad/s)
%  Column 16: Z gyro bias uncertainty (rad/s)

old_time = filter_time(1);

% Initialize INS from GPS
old_est_r_eb_e = init_cond(1:3)';
old_est_v_eb_e = init_cond(4:6)';
[old_est_L_b,old_est_lambda_b,old_est_h_b,old_est_v_eb_n] = pv_ECEF_to_NED(old_est_r_eb_e,old_est_v_eb_e);
est_L_b = old_est_L_b;

% Initialize estimated attitude solution
% old_est_C_b_n = Initialize_NED_attitude(true_C_b_n,initialization_errors);
old_est_C_b_n = Euler_to_CTM(init_cond(7:9));
[~,~,old_est_C_b_e] = NED_to_ECEF(old_est_L_b,old_est_lambda_b,old_est_h_b,old_est_v_eb_n,old_est_C_b_n);
old_est_r_eb_e = old_est_r_eb_e + old_est_C_b_e*LC_KF_config.lever_arm + old_est_C_b_e*LC_KF_config.gps_correction;

% Initialize output profile record and errors record
out_profile = zeros(length(filter_time),10);

% Generate output profile record
out_profile(1,1) = old_time;
out_profile(1,2:4) = old_est_r_eb_e;
out_profile(1,5:7) = old_est_v_eb_e';
out_profile(1,8:10) = CTM_to_Euler(old_est_C_b_n')';

% Initialize Kalman filter P matrix and IMU bias states
P_matrix = Initialize_LC_P_matrix(LC_KF_config);

% Generate IMU bias and clock output records
out_IMU_bias_est(1,1) = old_time;
out_IMU_bias_est(1,2:7) = est_IMU_bias';

% Generate KF uncertainty record
out_KF_SD(1,1) = old_time;
for n = 1:15
    out_KF_SD(1,n+1) = sqrt(P_matrix(n,n));
end % for i

% Initialize R (not really used but it makes me feel better
pos_variance = LC_KF_config.gps_pos_stddev.^2*gps(1,16).^2.*ones(1,3);
vel_variance = LC_KF_config.gps_vel_stddev.^2*gps(1,16).^2.*ones(1,3);
R_matrix(1:3,1:3) = diag(max(LC_KF_config.pos_sd_min, ...
                         min(LC_KF_config.pos_sd_max,...
                             pos_variance)));
R_matrix(1:3,4:6) = zeros(3);
R_matrix(4:6,1:3) = zeros(3);
R_matrix(4:6,4:6) = diag(max(LC_KF_config.vel_sd_min,...
                         min(LC_KF_config.vel_sd_max,...
                         vel_variance)));

out_R_matrix = zeros(size(gps,1), 6);
for n = 1:6
    out_R_matrix(1,n) = R_matrix(n,n);
end % for i
out_Q_matrix = zeros(size(gps,1), 15);
Q_matrix = zeros(15,15);
for n = 1:15
    out_Q_matrix(1,n) = Q_matrix(n,n);
end % for i
% Main loop
GNSS_epoch = 2;
last_GNSS_epoch = GNSS_epoch;
corrections = zeros(15,size(gps,1)-1);
last_imu_index = 1;
numIMU = size(imu, 1);
imu_index_windowsize = floor(20*epoch/mean(diff(imu(:,1))));
for i = 2:length(filter_time)
    time = filter_time(i);
    % find range of imu measurements to use
    endcap = min(numIMU, last_imu_index+1 + imu_index_windowsize);
    indices = find(imu(last_imu_index+1:endcap,1) < time);
    imu_range_end = last_imu_index + indices(end) - 1;
    % Apply IMU bias estimates
    meas_f_ib_b = mean(imu(last_imu_index+1:imu_range_end,2:4))' - est_IMU_bias(1:3);
    meas_omega_ib_b = mean(imu(last_imu_index+1:imu_range_end,5:7))' - est_IMU_bias(4:6);

    % Update estimated navigation solution
    [est_r_eb_e,est_v_eb_e,est_C_b_e] = Nav_equations_ECEF(epoch,...
        old_est_r_eb_e,old_est_v_eb_e,old_est_C_b_e,meas_f_ib_b,...
        meas_omega_ib_b);
%     while GNSS_epoch < size(gps,1)+1 && ~gps(GNSS_epoch,3)
%         GNSS_epoch = GNSS_epoch + 1;
%     end
    % Determine whether to update GNSS simulation and run Kalman filter
    if GNSS_epoch <= size(gps,1) && time > gps(GNSS_epoch,1)
        tor_s = gps(GNSS_epoch,1) - gps(last_GNSS_epoch,1);  % KF time interval
        GNSS_r_eb_e = gps(GNSS_epoch,9:11)';
        GNSS_v_eb_e = gps(GNSS_epoch,12:14)';
        est_L_b = lla(GNSS_epoch,1);
        % Use the GPS-reported standard deviation values, but clamping
        % min/max values according to configuration
        pos_variance = LC_KF_config.gps_pos_stddev.^2*gps(GNSS_epoch,16).^2;
        vel_variance = LC_KF_config.gps_vel_stddev.^2*gps(GNSS_epoch,16).^2;
        R_matrix(1:3,1:3) = diag(max(LC_KF_config.pos_sd_min, ...
                                     min(LC_KF_config.pos_sd_max,...
                                         pos_variance.*ones(1,3)*tor_s)));
        R_matrix(4:6,4:6) = diag(max(LC_KF_config.vel_sd_min,...
                                     min(LC_KF_config.vel_sd_max,...
                                         vel_variance.*ones(1,3)*tor_s)));
%         disp(size(R_matrix));
%         disp(diag(R_matrix));

        % Run Integration Kalman filter
        [est_C_b_e,est_v_eb_e,est_r_eb_e,est_IMU_bias,P_matrix_new,corrections(:,GNSS_epoch-1), Phi_matrix, Q_matrix] =...
            LC_KF_Epoch(GNSS_epoch, GNSS_r_eb_e,GNSS_v_eb_e,tor_s,est_C_b_e,...
            est_v_eb_e,est_r_eb_e,est_IMU_bias,P_matrix,meas_f_ib_b,...
            est_L_b,LC_KF_config,Q_matrix,R_matrix,meas_omega_ib_b);
        if any(any(isnan(P_matrix))) || any(any(isinf(P_matrix)))
            disp('Filter instability detected. Time to kick the bucket.');
            fprintf('Died on iteration %d/%d\n',i,length(filter_time));
            fprintf('and GNSS epoch %d/%d\n',GNSS_epoch,size(gps,1));
            return
        end

        % Run adaptive algorithm
        [Q_matrix] = adapt_noise_covariance(Phi_matrix, P_matrix_new, P_matrix, Q_matrix, ...
                                            LC_KF_config.n, GNSS_epoch, corrections);
        P_matrix = P_matrix_new;

        % Generate IMU bias and clock output records
        out_IMU_bias_est(GNSS_epoch,1) = time;
        out_IMU_bias_est(GNSS_epoch,2:7) = est_IMU_bias';

        % Generate KF uncertainty output record
        out_KF_SD(GNSS_epoch,1) = time;
        for n = 1:15
            out_KF_SD(GNSS_epoch,n+1) = sqrt(P_matrix(n,n));
        end % for i
        for n = 1:6
            out_R_matrix(GNSS_epoch,n) = sqrt(abs(R_matrix(n,n)));
        end % for i
        for n = 1:15
            out_Q_matrix(GNSS_epoch,n) = sqrt(abs(Q_matrix(n,n)));
        end % for i
        last_GNSS_epoch = GNSS_epoch;
        GNSS_epoch = GNSS_epoch + 1;
    end % if time

    % Convert navigation solution to NED
    [~,~,~,~,est_C_b_n] =...
        ECEF_to_NED(est_r_eb_e,est_v_eb_e,est_C_b_e);

    % Generate output profile record
    out_profile(i,1) = time;
    out_profile(i,2:4) = est_r_eb_e;
    out_profile(i,5:7) = est_v_eb_e';
    out_profile(i,8:10) = CTM_to_Euler(est_C_b_n')';

    % Reset old values
    old_est_r_eb_e = est_r_eb_e;
    old_est_v_eb_e = est_v_eb_e;
    old_est_C_b_e = est_C_b_e;
    last_imu_index = imu_range_end;
end %epoch

% Ends


function result = make_epochs_per_sample(weights)
%MAKE_EPOCHS_PER_SAMPLE Given a set of weights generate the number of
% epochs per sample for each weight.
%
% result = MAKE_EPOCHS_PER_SAMPLE(weights)
%
% Parameters
% ----------
% weights: array of size (n_1_simplices, 1)
%     The weights of how much we wish to sample each 1-simplex.
%
% Note that the total number of epochs does not impact this result.
%
% Returns
% -------
% result: array of size (n_1_simplices, 1)
%     The number of epochs per sample, one for each 1-simplex.
%
%   AUTHORSHIP
%   Math Lead & Primary Developer:  Connor Meehan <connor.gw.meehan@gmail.com>
%   Secondary Developer: Stephen Meehan <swmeehan@stanford.edu>
%   Bioinformatics Lead:  Wayne Moore <wmoore@stanford.edu>
%   Provided by the Herzenberg Lab at Stanford University
%   License: BSD 3 clause
%

result = -1*ones(size(weights, 1), 1);
L = weights > 0;
result(L) = max(weights)*ones(sum(L), 1)./weights(L);


function [ySqrtPred,PInvSqrtPred,RwPred,RwxPred]=ESRIFDiscPred(ySqrtPrev,PInvSqrtPrev,f,FJacob,SQ,u,Gamma)
%%ESRIFDISCPRED Perform the discrete-time prediction step that comes with
%               the extended square root information filter with additive
%               process noise.
%
%INPUTS: ySqrtPrev The xDimX1 square root information state that is to be
%                  propagated. The previous information state is always
%                  PInvSqrtPrev times the previous target state estimate.
%     PInvSqrtPrev The previous inverse square root information matrix.
%                  If P is the covariance matrix of a Gaussian state x,
%                  then P=PSqrt*PSqrt' and PInvSqrtPrev=inv(PSqrt). This
%                  can be either upper triangular or lower triangular.
%                f A function handle for the state transition function
%                  that takes the state as its parameter.
%           FJacob A function handle for calculating the xDim X xDim state
%                  transition matrix. If an empty matrix is passed, then
%                  FJacob will be found using numerical differentiation
%                  via the numDiff function with default parameters.
%               SQ The lower-triangular square root of the process noise
%                  covariance matrix. In the standard linear dynamic
%                  model, one has
%                  x(k)=F(k-1)*x(k-1)+u(k-1)+noise where x is the state
%                  and Q is the covariance matrix of the noise. However,
%                  if the covariance matrix of the noise in such equation
%                  is singular, then one should rewrite it as
%                  x(k)=F(k-1)*x(k-1)+u(k-1)+Gamma*noise
%                  where gamma is some matrix and the covariance matrix of
%                  the untransformed noise is not singular. This SQ is the
%                  lower triangular square root of the noise in the linear
%                  dynamic equation.
%                u An optional xDim X1 vector that is the control input.
%                  If omitted, a zero control input (no control input) is
%                  used.
%            Gamma An optional matrix that transforms the process noise
%                  to the state domain if the process noise covariance
%                  matrix is singular, as disccused for the input SQ. If
%                  this is omitted an identity matrix is used (i.e. there
%                  is no Gamma).
%
%OUTPUTS:ySqrtPred The xDim X 1 predicted square root information state
%                  vector.
%     PInvSqrtPred The predicted xDim X xDim inverse square root state
%                  covariance matrix, which is upper-triangular.
%   RwPred,RwxPred  Noise matrices which can be saved for use in smoothing
%
%The time prediction step is taken from the algorithmic implementation
%of [1] described in chapters V and VI.
%
%Given a Gaussian predicted state with mean x and covariance matrix P, the
%square root information state is
%ySqrt=PInvSqrt*x
%where
%PSqrt=inv(PInvSqrt)
%and
%P=PSqrt*PSqrt';
%The matrix PInvSqrtPred can be upper or lower triangular, when supplied to
%this function. For example, a lower-triangular matrix can be obtained
%using PInvSqrt=inv(chol(P,'lower')). However, the output of this function,
%PInvSqrtUpdate is always upper triangular.
%
%REFERENCES:
%[1] G. J. Bierman, "Factorization Methods for Discrete Sequential
%    Estimation. Academic Press, New York, 1977.
%
%February 2015 David Karnick, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xDim=size(ySqrtPrev,1);
if(nargin<6||isempty(u))
    u=zeros(xDim,1);
end
if(nargin<7||isempty(Gamma))
    Gamma=eye(xDim);
end
if(isempty(FJacob))
    FJacob=@(x)numDiff(x,f,xDim);
end

xPrev=PInvSqrtPrev\ySqrtPrev;

F=FJacob(xPrev);
PInvSqrtTilde=PInvSqrtPrev/F;

%The mapping matrix found on p.121 of Bierman
A=[inv(SQ),                zeros(xDim,xDim), SQ\u;
    -PInvSqrtTilde*Gamma,   PInvSqrtTilde,   ySqrtPrev];
[~,T] = qr(A);

PInvSqrtPred=T((xDim+1):end,(end-xDim):(end-1));

%Since f may be nonlinear, the information state output described in
%Bierman is not valid, and the predicted state should be calculated
%independently.
ySqrtPred=PInvSqrtPred*f(xPrev);

if(nargout>2)
    RwPred=T(1:xDim,1:xDim);
    RwxPred=T(1:xDim,xDim+1:end-1);
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function S = xval_SVM(varargin)
%  SVM for repeated-measures (within-person) classification, with repeated cross-val and nested cross-val options
%
% :Usage:
% ::
%
% S = xval_SVM(X, Y, id, varargin)
%
% Steps and features:
% -------------------------------------------------------------------------
% - Select holdout sets, keeping images from the same id together and stratifying on the outcome to be predicted
% - Fit overall model and get "sanity check" accuracy
% - Cross-validation with standard a priori hyperparameter choices
% - Plot scores and ROC curves
% - Nested cross-val with hyperparameter optimization [optional]
%   * Estimate model performance accounting for search across hyperparams
%   * Updates S.yfit, S.dist_from_hyperplane_xval, S.class_probability_xval
%   * Updates crossval_accuracy, Y_within_id, scores_within_id, scorediff, crossval_accuracy_within, classification_d
%   * Retrain to obtain estimate of best hyperparameters for future testing (update S.modeloptions)
% - Repeat cross-validation of optimized model with different random splits [optional]
% - Fit model on full dataset with final hyperparameters to get predictor weights (betas, b)
% - Bootstrap model parameter estimates (w) and get P-values, FDR-corrected significant features
% - Print output and text compatible with report-generation
%
% Notes:
% -------------------------------------------------------------------------
% - Uses Matlab's Stats/ML Toolbox SVM object, fitcsvm, and hyperparameter
% optimization. Other options, e.g., fitclinear, may be better for some
% situations (large num. variables)
%
% - Single-interval accuracy from cross-val and ROC_plot may differ because
% ROC_plot chooses a new score threshold that maximizes overall balanced
% accuracy. Forced-choice accuracy should be identical.
%
% Optimizing hyperparameters:
% HyperparameterOptimizationOptions include defaults of:
% - 5-fold CV (not grouped by id) without repartitioning
% - Bayesian optimization, using best estimates from smooth function
% - maximizes accuracy, so may need large(ish) samples to be effective.
%
% - works on linear classifiers only, but can explore nonlinear models
% with a small change in the code (now commented out). This could be added
% as an optional flag as well.
%
% - If optimizing hyperparameters AND repeating cross-validation, accuracy
% estimates will use nested cross-validation, and can take a long time to run
%
%
% Class probability estimates:
% - crossval returns cross-validated yfit (class predictions), scores (dist_from_hyperplane_xval),
% class_probability_xval. Scores and class probabilties will diverge (may
% not be perfectly correlated) because the sigmoid scaling (Platt scaling) varies across folds.
% They will diverge more if there is no true signal.
%
% ..
%     Author and copyright information:
%
%     Copyright (C) 2020 Tor Wager
%
%     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/>.
% ..
%
% :Inputs:
%
%   **X:**
%        obs x variables numeric matrix of predictors
%
%   **Y:**
%        obs x 1 numeric vector of outcomes, effects-coded (1, -1)
%
%   **id:**
%        obs x 1 numeric vector of grouping codes, e.g., for participants
%        all obs from each group will be included together in a training or test set
%        if no grouping, use integers 1...n images or leave empty
%
% :Optional Inputs:
%   **'doplot', [logical flag]:**
%        Create plots; default = true. 'noplot' to turn off.
%
%   **'doverbose', [logical flag]:**
%        Verbose output; default = true. 'noverbose' to turn off.
%
%   **'dooptimize', [logical flag]:**
%        Optimize hyperparameters; default = true. 'nooptimize' to turn off.
%
%   **'doprepeats', [num repeats]:**
%        Repeat cross-val with different partitions; default = 10.
%        Enter number of repeats, 'norepeats' to turn off.
%
%   **'modeloptions', [modeloptions cell]:**
%        Options for model structure and hyperparameters
%        Cell array of keyword-value pairs, as specified in fitcsvm (Matlab Stats/ML toolbox)
%        Default: {'KernelFunction', 'linear'}
%
%   **'nfolds', [num folds]:**
%        Set number of cross-validation folds; default = 10.
%        Enter number of folds.
%
% :Outputs:
%
%   **S:**
%        Structure with model output
%        of them (partially consistent with this function).
%                           Y: Actual (obs) outcome - should be 1, -1 for SVM analysis
%                        yfit: Predicted outcome - should be 1, -1 for SVM analysis
%                              Cross-validated, so can be used as series
%                              of predicted values based on brain
%                              measures (very useful!)
%                          id: Grouping variable for within-participant observations
%                      accfun: Function handle to get accuracy (single-interval)
%                           w: Model weights/betas, weights on input variables
%                      nfolds: Number of folds in holdout set
%                 cvpartition: Object with information about training/test sets
%                       teIdx: Testing IDs for each fold (holdout set)
%                       trIdx: Training IDs for each fold (holdout set)
%             dist_from_hyperplane_xval: Cross-validated distance perpendicular to class boundary
%                               higher = stronger prediction in favor of Class 1, lower = in favor of class -1.
%                               Useful! use as continuous measure of
%                               observation scores. Can calculate
%                               effect sizes from this, for example.
%       class_probability_xval: Cross-validated distance expressed as probabilities of Class 1, using Platt scaling
%            crossval_accuracy: Cross-validated accuracy (input options; no hyperparam opt)
%  classification_d_singleinterval: Classification effect size (input options; no hyperparam opt)
%crossval_accuracy_opt_hyperparams: Cross-validated accuracy with optimized hyper-parameters
%                  Y_within_id: Outcomes arranged by id, for within-person comparisons
%             scores_within_id: SVM scores arranged by id, for within-person comparisons
%                    scorediff: Within-person SVM scores arranged by id, for within-person comparisons
%     crossval_accuracy_within: Within-person cross-validated accuracy (input options; no hyperparam opt)
%      classification_d_within: Within-person classification effect size (input options; no hyperparam opt)
%                 S.boot_w_ste: Bootstrapped standard errors of model weights (feature-level)
%                S.boot_w_mean: Bootstrapped mean model weights (feature-level)
%                         S.wZ: Bootstrapped Z-scores of model weights (feature-level)
%                         S.wP: Bootstrapped P-values for individual model weights (feature-level)
%                 S.wP_fdr_thr: P-value threshold for FDR q < 0.05
%              S.boot_w_fdrsig: Logical vector for which weights are significant with FDR
%               S.w_thresh_fdr: Thresholded weights at FDR q < 0.05 based on bootstrapping (feature-level)
%                   S.SVMModel: ClasssificationSVM model object trained on full dataset, final chosen parameter set; for predicting in subsequent validation samples.
%                               Y-hat = (X/s)'* w + b
%                               ClassificationSVM objects store w, b, and s in the properties Beta, Bias, and KernelParameters.Scale, respectively.
%
% :Examples:
% ::
%
% Simulate some data with true signal, with two observations per person
% -------------------------------------------------------------------------
% n = 50; % participants
% k = 120; % features
% true_sig = [repmat(randn(1, k), n, 1); repmat(randn(1, k), n, 1)]; % First n are Class 1, second n are Class 2
% noise = 10 * randn(2 * n, k);         % Noise var >> true signal var
% X = true_sig + noise;                  % Predictors
% Y = [ones(n, 1); -ones(n, 1)];         % Outcome to classify, coded [1 -1]
% id = [(1:n)'; (1:n)'];                 % Grouping ID codes for participants
%
% S = xval_SVM(X, Y, id, 'nooptimize', 'norepeats', 'nobootstrap');    % Fastest for quick cross-validated performance
% S = xval_SVM(X, Y, id, 'nooptimize', 'norepeats', 'nboot', 100);     % Very quick test of bootstrapping with few samples (not for final models)
% S = xval_SVM(X, Y, id, 'nooptimize', 'dorepeats', 5, 'nobootstrap'); % Quick test of repeated cross-val, 5x
% S = xval_SVM(X, Y, id, 'nooptimize', 'nobootstrap');                 % Repeat cross-val only
% S = xval_SVM(X, Y, id, 'norepeats', 'nobootstrap');                  % Optimize with nested cross-val only
% S = xval_SVM(X, Y, id);                                              % Optimize, repeat with optimal model, bootstrap
%
% S = xval_SVM(X, Y, id, 'nooptimize', 'norepeats', 'nobootstrap', 'noverbose', 'noplot');  % Returns only the output S
%
% Train the same model without within-person observations (i.e., 1 observation per id):
% This does single-interval classification only
%
% S = xval_SVM(X, Y, (1:length(Y))', 'norepeats', 'nobootstrap');
%
% :References:
%   See Mathworks functions
%
% :See also:
%   - fmri_data.predict, canlab_run_paired_SVM, other xval_ functions
%

% ..
%    Programmers' notes:
%    Created by Tor Wager, May 2020
%    ver 1.0 (no version number listed) - some bugs in optimization
%             Enforce [1 -1] inputs
%    ver 1.1 (Tor): fixed bugs, optimization changes and documentation, multiple small tweaks and some new plots
%             Updated to handle within-person or between-person obs only
%             ROC_plot should be at threshold 0 for single-interval
%             Get rid of plots for all folds in opt output
%             OptimizeHypParams returns point est. No need to rerun. Builds in 5-fold eval by default
%             Add ?Summary of nested cross-val accuracy with hyperparameter optimization?
%             Optim.: Allow control of verbosity and plots. No plots/verbose in nested eval.
%             Optim: use best estim, not best observed
%             Have a look at the data and add data plots. Sorted by cases
%   ver 1.2   Cosmetic documentation and look updates
%
%   Future:
%
%             Troubleshoot prediction vs. class probability reversals
%                   % either scores or pscores appear to be inconsistent in terms of which column is class -1 and which is class 1. Check for instability in scores, and select pscores to be consistent with this on each fold.
%             Add permutation test
%     v1.3
%     Linear/nonlinear - pass in hyparams to optimize.
%     Return model weights from folds for further analysis/application
%
%     v1.4
%     Add nfolds argument - Michael Sun, 07/07/2022
%
% ..

%% ----------------------------------------------------------------------
% Version
% ----------------------------------------------------------------------

ver = 1.2;

%% ----------------------------------------------------------------------
% Parse inputs
% ----------------------------------------------------------------------
% Uses the inputParser object. Older schemes are below.

% Logical flags - parse specially because they are not name-value pairs
% Parser accepts only name/value pairs, so this code allows a workaround
% We must remove these inputs because they will cause inputparser to error
% ----------------------------------------------------------------------
wh = strcmp(varargin, 'noverbose'); if any(wh), doverbose1 = false; varargin(wh) = []; end
wh = strcmp(varargin, 'noplot'); if any(wh), doplot1 = false; varargin(wh) = []; end
wh = strcmp(varargin, 'noplots'); if any(wh), doplot1 = false; varargin(wh) = []; end
wh = strcmp(varargin, 'nooptimize'); if any(wh), dooptimize1 = false; varargin(wh) = []; end
wh = strcmp(varargin, 'norepeats'); if any(wh), dorepeats1 = 0; varargin(wh) = []; end
wh = strcmp(varargin, 'nobootstrap'); if any(wh), dobootstrap1 = 0; varargin(wh) = []; end

% The handling of parsing keyword-value pairs, checking attributes, and
% specifying default values is handled here, using Matlab's inputparser object:
ARGS = parse_inputs(varargin{:});

fn = fieldnames(ARGS);

for i = 1:length(fn)
    str = sprintf('%s = ARGS.(''%s'');', fn{i}, fn{i});
    eval(str)
end

% Replace logical flags because they will be overwritten by the parser
if exist('doverbose1', 'var'), doverbose = doverbose1; end
if exist('doplot1', 'var'), doplot = doplot1; end
if exist('dooptimize1', 'var'), dooptimize = dooptimize1; end
if exist('dorepeats1', 'var'), dorepeats = dorepeats1; end
if exist('dobootstrap1', 'var'), dobootstrap = dobootstrap1; end
if ~exist('nfolds', 'var'), nfolds=10; end

if isempty(id), id = [1:length(Y)]'; end
if ~iscolumn(id), id = id'; end

%% Select holdout sets for outer loop
% - Keep images from the same id together
% - Stratify on the outcome to be predicted
% -------------------------------------------------------------------------

S = struct();   % Define structure for models and output; use variable names in fmri_data.predict when possible

S.Y = Y;
S.id = id;      % Subject grouping - not in fmri_data.predict

S.modeloptions = modeloptions;

S.accfun = @(Y, yfit) 100 .* nansum(Y == yfit) ./ sum(~isnan(Y));

if doverbose

    dashes = '---------------------------------------------------------------------------------';
    printhdr = @(str) fprintf('%s\n%s\n%s\n', dashes, str, dashes);

    printhdr(' ') %#ok<*UNRCH>
    printhdr(sprintf('xval_SVM ver %3.2f: Cross-validated Support Vector Machine classification', ver));
    printhdr(' ')

    disp(' ')
    disp(' ')
    printhdr('Selecting and analyzing cross-validation folds');

end

% Preliminary data plots
% -------------------------------------------------------------------------
if doplot
    prelim_data_plot(X, Y, id);
end

% Select train/test sets
% -------------------------------------------------------------------------

% [S.trIdx, S.teIdx] = xval_stratified_holdout_leave_whole_subject_out(S.Y, S.id, 'doverbose', doverbose, 'doplot', doplot);
[S.trIdx, S.teIdx] = xval_stratified_holdout_leave_whole_subject_out(S.Y, S.id, 'doverbose', doverbose, 'doplot', doplot, 'nfolds', nfolds);  % MS 6/16/2022: Crashes if you can't do 10-fold.
drawnow, snapnow

% Fit the overall a priori model: SVM with linear kernel
% -------------------------------------------------------------------------
if doverbose

    printhdr('Model training and cross-validation without optimization')
    fprintf('Training overall model. ')

    SVMModel = fitcsvm(X, S.Y, S.modeloptions{:}); %#ok<*NODEF>

    % non-cross-val accuracy: sanity check. should usually be 100% if training is overfit and n >> p
    label = predict(SVMModel, X);

    acc = S.accfun(S.Y, label);
    fprintf('Accuracy without cross-val: %3.0f%%\n', acc);

end

%% Cross-validation with standard a priori hyperparameter choices
% -------------------------------------------------------------------------
S.nfolds = length(S.trIdx);

% crossval returns cross-validated yfit (class predictions), scores (dist_from_hyperplane_xval),
% class_probability_xval. Scores and class probabilties will diverge (may
% not be perfectly correlated) because the sigmoid scaling (Platt scaling) varies across folds.
% They will diverge more if there is no true signal.

S = crossval(S, X, doverbose);

% Summarize accuracy
% -------------------------------------------------------------------------
% Given Y, yfit, cross-val scores (dist_from_hyperplane_xval), id
% Update crossval_accuracy,classification_d_singleinterval
% Y_within_id, scores_within_id, scorediff,
% crossval_accuracy_within, classification_d_within
S = update_accuracy_stats(S);

if doverbose

    fprintf('Accuracy (cross-validated): Single-interval: %3.0f%%, d = %3.2f\n', S.crossval_accuracy, S.classification_d_singleinterval);

    if S.mult_obs_within_person

        fprintf('                            Forced-choice within-person: %3.0f%% d = %3.2f\n', S.crossval_accuracy_within, S.classification_d_within);

    end

end

% Plot scores and ROC curves
% -------------------------------------------------------------------------
if doplot

    plot_scores_and_ROC(S);

    xval_plot_scores_vs_class_probability(S);

end

%% Nested cross-val with hyperparameter optimization
% -------------------------------------------------------------------------

if dooptimize

    if doverbose
        printhdr('Optimizing hyperparameters using nested cross-validation');
    end

    % Nested cross-validation (updates S.yfit, S.dist_from_hyperplane_xval,
    % S.class_probability_xval)
    % Estimate model performance accounting for search across hyperparams
    % ---------------------------------------------------------------------
    % Updates S.yfit, S.dist_from_hyperplane_xval, S.class_probability_xval
    S = crossval_nested(S, X, doverbose);

    % Given Y, yfit, cross-val scores (dist_from_hyperplane_xval), id
    % Update crossval_accuracy, Y_within_id, scores_within_id, scorediff, crossval_accuracy_within, classification_d
    S = update_accuracy_stats(S);

    % Retrain to obtain estimate of best hyperparameters for future testing (update S.modeloptions)
    % ---------------------------------------------------------------------
    % Modal parameters across folds is not
    % the best way to get the final hyperparameters. Instead, the model should
    % be re-fit using single cross-validation with all hparam options pick the
    % best relative model. Nested cross-val will estimate the accuracy of the
    % whole procedure, including the hparam search, but not return the final best
    % parameters.
    hparams_to_optimize = 'all'; % {'BoxConstraint' 'Standardize'}
    Mdl = fitcsvm(X, S.Y,...
        'OptimizeHyperparameters', hparams_to_optimize, 'HyperparameterOptimizationOptions',...
        struct('AcquisitionFunctionName','expected-improvement-plus', 'Verbose', double(doverbose), 'ShowPlots', double(doplot))); %#ok<*IDISVAR>

    % Notes:
    % - Extract hyperparameter choices in a format that we can pass back in
    % to train or test a new model.
    % - We could use the best observed point, but it's preferred to use the
    % best estimated values from a smooth model fit to observed samples.

    best_modeloptions = convert_hyperparameter_choices_to_cell(Mdl, []);                % optimizing "all"
    % best_modeloptions = convert_hyperparameter_choices_to_cell(Mdl, S.modeloptions);   % optimizing "linear" : ms 06242022 Changed default settings to not optimize

    S.modeloptions = best_modeloptions;

    % Summarize accuracy
    % -------------------------------------------------------------------------

    if doverbose

        disp('Summary of nested cross-val accuracy with hyperparameter optimization')
        disp(' ')

        disp('Optimized hyperparameters for each fold')
        disp(S.hyperparams_by_fold)

        disp(' ')
        disp('Best options, stored in S.modeloptions:');

        disp(best_modeloptions)

        fprintf('Accuracy (nested cross-validation): Single-interval: %3.0f%%, d = %3.2f\n', S.crossval_accuracy, S.classification_d_singleinterval);

        if S.mult_obs_within_person

            fprintf('                            Forced-choice within-person: %3.0f%% d = %3.2f\n', S.crossval_accuracy_within, S.classification_d_within);

        end

    end

    % Plot scores and ROC curves
    % -------------------------------------------------------------------------
    if doplot

        plot_scores_and_ROC(S, 'Cross-validated SVM scores with hyperparameter optimization');

    end

end % do optimize

%% Repeat cross-validation of optimized model, if specified
% - if hyperparams optimized, then use "consensus params" for repeats
% - these are stored in S.modeloptions now, so will be used
% - If optimizing, re-do the first accuracy with the optimized
% hyperparameters
% -------------------------------------------------------------------------
if dorepeats > 1

    if doverbose
        disp(' ')
        fprintf('Repeating cross-val %d times. ', dorepeats)
    end

    % if dooptimize, startat = 1; else, startat = 2; end
    startat = 2;

    for i = startat:dorepeats     % do remainder of repeats without verbose.

        if doverbose, fprintf('%d ', i), end

        Sr = S;

        % Get new cvpartition (or outer loop if optimizing)
%         [Sr.trIdx, Sr.teIdx] = xval_stratified_holdout_leave_whole_subject_out(Sr.Y, Sr.id, 'doverbose', false, 'doplot', false);
        [Sr.trIdx, Sr.teIdx] = xval_stratified_holdout_leave_whole_subject_out(Sr.Y, Sr.id, 'doverbose', false, 'doplot', false, 'nfolds', nfolds); % MS 6/16/2022 add nfold option, otherwise default nfold=10 will cause this to crash if there are fewer than 10 possible folds.

        if dooptimize
            % If optimizing, repeat nested xval procedure
            % This can take a long time!

            Sr = crossval_nested(Sr, X, doverbose);

        else

            % Cross-validate
            Sr = crossval(Sr, X, false);

        end

        % Save accuracy
        % -------------------------------------------------------------------------
        % Given Y, yfit, cross-val scores (dist_from_hyperplane_xval), id
        % Update crossval_accuracy,classification_d_singleinterval
        % Y_within_id, scores_within_id, scorediff,
        % crossval_accuracy_within, classification_d_within

        Sr = update_accuracy_stats(Sr);

        S.crossval_accuracy(i) = Sr.crossval_accuracy; % Sr.accfun(Sr.Y, Sr.yfit);
        S.classification_d_singleinterval(i) = Sr.classification_d_singleinterval;

        if S.mult_obs_within_person

            S.crossval_accuracy_within(i) = Sr.crossval_accuracy_within; % Sr.accfun(Sr.Y, Sr.yfit);
            S.classification_d_within(i) = Sr.classification_d_within;

        end

    end

    if doverbose

        fprintf('Done!\n')

        fprintf('CV single-interval accuracy across %d reps, mean = %3.0f%%, std = %3.0f%%, min = %3.0f%%, max = %3.0f%%\n', dorepeats, ...
            mean(S.crossval_accuracy), std(S.crossval_accuracy), min(S.crossval_accuracy), max(S.crossval_accuracy));

        disp('Performance metrics saved in S.crossval_accuracy, classification_d_singleinterval')

        if S.mult_obs_within_person

            fprintf('\nCV forced_choice accuracy across %d reps, mean = %3.0f%%, std = %3.0f%%, min = %3.0f%%, max = %3.0f%%\n', dorepeats, ...
                mean(S.crossval_accuracy_within), std(S.crossval_accuracy_within), min(S.crossval_accuracy_within), max(S.crossval_accuracy_within));

             disp('Performance metrics saved in S.crossval_accuracy_within, classification_d_within')

        end

        disp(' ')

    end

end

%% Fit model on full dataset with selected options (final model if optimizing hyperparams) to get betas

S.SVMModel = fitcsvm(X, S.Y, S.modeloptions{:});
S.w = S.SVMModel.Beta;                                       % Model weights/betas

% Bootstrap betas
if dobootstrap

    if doverbose
        disp(' ')
        printhdr(sprintf('Bootstrapping model param estimates (b): %d samples', nboot));
    end

    rng('shuffle');

    S.boot_w = NaN .* zeros(length(S.w), nboot);

    for i = 1:nboot

        [Xb, Yb] = get_bootstrap_sample_grouped_by_id(S, X);

        SVMModel = fitcsvm(Xb, Yb, S.modeloptions{:});

        S.boot_w(:, i) = SVMModel.Beta;                                       % Model weights/betas

    end

    % Inference
    % (from fmri_data.predict)

    S.boot_w_ste = squeeze(nanstd(S.boot_w, 0, 2)); %1/20/16 add squeeze for multiclass case
    S.boot_w_mean = squeeze(nanmean(S.boot_w, 2)); %1/20/16 add squeeze for  multiclass case
    S.boot_w_ste(S.boot_w_ste == 0) = Inf;  % in case unstable regression returns all zeros

    S.wZ = S.boot_w_mean ./ S.boot_w_ste;  % tor changed from wmean; otherwise bootstrap variance in mean inc in error; Luke renamed to avoid confusion
    S.wP = 2 * (1 - normcdf(abs(S.wZ)));
    S.wP_fdr_thr = FDR(S.wP, .05);
    if isempty(S.wP_fdr_thr), S.wP_fdr_thr = -Inf; end

    S.boot_w_fdrsig = S.wP <= S.wP_fdr_thr;  % equals because can get exact vals in some cases...
    S.w_thresh_fdr = S.w;
    S.w_thresh_fdr(~S.boot_w_fdrsig) = 0;

    if doverbose

        disp('Summary of significant individual features (two-tailed)');

        Threshold = [.05 .01 .001 S.wP_fdr_thr]';

        for i = 1:length(Threshold)

            Num_Sig_Features(i, 1) = sum(S.wP <= Threshold(i));

        end

        t = table(Threshold, Num_Sig_Features);
        disp(t)

    end
end

%%
% permutations would go here...future project.
% consider exchangeability issue given within-person design...winkler and
% nichols...
% indx = randperm(size(X, 1));
% X = X(indx, :); Y = Y(indx); id = id(indx);
% S = xval_SVM(X, Y, id, 'nooptimize', 'norepeats');

end % main function

%%

% -------------------------------------------------------------------------
% -------------------------------------------------------------------------

% Subfunctions

% -------------------------------------------------------------------------
% -------------------------------------------------------------------------

function S = crossval(S, X, doverbose)

if doverbose, fprintf('..X-val, %d folds...', S.nfolds), end

for i = 1:S.nfolds

    if doverbose, fprintf('%d ', i), end

    % Fit to training data for this fold
    SVM_fold = fitcsvm(X(S.trIdx{i}, :), S.Y(S.trIdx{i}), S.modeloptions{:});

    % Apply holdout test set for this fold
    [label, score] = predict(SVM_fold, X(S.teIdx{i}, :));   % Get raw scores
    score = score(:, 2);                                    % Decision boundary is symmetrical

    S.dist_from_hyperplane_xval(S.teIdx{i}, 1) = score;     % Unscaled SVM scores

    S.yfit(S.teIdx{i}, 1) = label;                          % Predicted class, cross-val

    SVM_fold = fitPosterior(SVM_fold);                      % Works for fitcsvm, not fitclinear

    % Note:
    % either scores or pscores appear to be inconsistent in terms of which
    % column is class -1 and which is class 1. Check for instability in
    % scores, and select pscores to be consistent with this on each fold.
    if mean(label(score > 0)) < mean(label(score < 0))
        disp('WARNING!!! Scores for label 1 are < scores for label -1, scores are reversed!!! This should not happen. Check code/implementation.');
    end

    [~, pscore] = predict(SVM_fold, X(S.teIdx{i}, :));

    % which pscores correlate more positively with score? use this one.
     r = corr([score pscore]);
    [~, wh] = max(r(1, 2:3));

    pscore = pscore(:, wh);                                  % Platt scaling scores (class probability)

    S.class_probability_xval(S.teIdx{i}, 1) = pscore;

end

if doverbose, fprintf('Done!\n'), end

end % crossval

% -------------------------------------------------------------------------
% Nested cross-validation to optimize hyperparameters
% -------------------------------------------------------------------------

function S = crossval_nested(S, X, doverbose)

if doverbose, fprintf('..X-val, %d folds...', S.nfolds), end

for i = 1:S.nfolds

    if doverbose, fprintf('%d ', i), end

    % Optimize hyperparameters within this fold
    % -------------------------------------------

    % Option 1: Optimize all, including kernel (linear/nonlinear)
    % Option 2: Optimize specific hyperparameters, Slack param C
    % (BoxConstraint) and Standardize inputs, linear model only

    hparams_to_optimize = 'all'; % {'BoxConstraint' 'Standardize'}

    Mdl = fitcsvm(X(S.trIdx{i}, :), S.Y(S.trIdx{i}),...
        'OptimizeHyperparameters', hparams_to_optimize, 'HyperparameterOptimizationOptions',...
        struct('AcquisitionFunctionName','expected-improvement-plus', 'Verbose', 0, 'ShowPlots', 0));

    % Notes: HyperparameterOptimizationOptions include defaults of:
    % - 5-fold CV (not grouped by id) without repartitioning
    % - Bayesian optimization, using best estimates from smooth function

    % Add hyperparameter results to table
    % -------------------------------------
    % Use estimated rather than observed. This uses the smooth function estimated across observations

    opt_hyperparam_table = Mdl.HyperparameterOptimizationResults.XAtMinEstimatedObjective;

    if i == 1
        S.hyperparams_by_fold = opt_hyperparam_table; % best parameters - table

    else
        S.hyperparams_by_fold(i, :) = opt_hyperparam_table;
    end

    fold_modeloptions = convert_hyperparameter_choices_to_cell(Mdl, []); % Optimizing "all"
    %fold_modeloptions = convert_hyperparameter_choices_to_cell(Mdl, S.modeloptions); % Optimizing "linear"

    S.fold_modeloptions{i} = fold_modeloptions;

    % Fit to training data for this fold
    SVM_fold = fitcsvm(X(S.trIdx{i}, :), S.Y(S.trIdx{i}), fold_modeloptions{:});

    % Updates S.yfit, S. S.dist_from_hyperplane_xval, S.class_probability_xval

    % Apply holdout test set for this fold
    [label, score] = predict(SVM_fold, X(S.teIdx{i}, :));   % Get raw scores
    score = score(:, 2);                                    % Decision boundary is symmetrical

    S.dist_from_hyperplane_xval(S.teIdx{i}, 1) = score;     % Unscaled SVM scores

    S.yfit(S.teIdx{i}, 1) = label;                          % Predicted class, cross-val

    SVM_fold = fitPosterior(SVM_fold);                      % Works for fitcsvm, not fitclinear

    [~, pscore] = predict(SVM_fold, X(S.teIdx{i}, :));
    pscore = pscore(:, 2);                                  % Platt scaling scores (class probability)

    S.class_probability_xval(S.teIdx{i}, 1) = pscore;

end

if doverbose, fprintf('Done!\n'), end

end % crossval

% -------------------------------------------------------------------------
% Get scores
% -------------------------------------------------------------------------

function [scores_within_id, scorediff, d] = get_scores_within_id(S, varname)

myvar = S.(varname);
u = unique(S.id);

for i = 1:length(u)

    wh_id = S.id == u(i) & S.Y == -1;
    scores_within_id(i, 1) = nanmean(myvar(wh_id));

    wh_id = S.id == u(i) & S.Y == 1;
    scores_within_id(i, 2) = nanmean(myvar(wh_id));

end

scorediff = diff(scores_within_id')';
d = nanmean(scorediff) ./ nanstd(scorediff);

end %get scores

% -------------------------------------------------------------------------
% bootstrapping
% -------------------------------------------------------------------------

function [Xb, Yb] = get_bootstrap_sample_grouped_by_id(S, X)
% Given X, S.Y, S.id, return a bootstrap sample, keeping all obs from an id together

u = unique(S.id);
n = length(u);

wh = ceil(rand(n, 1) * n);  % Bootstrap sample of p ids, with replacement

[Xboot, Yboot] = deal(cell(n, 1));

for j = 1:n                 % add obs, allowing repeats

    wh_obs = ismember(S.id, u(wh(j)));

    Xboot{j} = X(wh_obs, :);
    Yboot{j} = S.Y(wh_obs);

end

Xb = cat(1, Xboot{:});
Yb = cat(1, Yboot{:});

end % function

% -------------------------------------------------------------------------
% optimized hyperparameter aggregation
% -------------------------------------------------------------------------

function  best_modeloptions = convert_hyperparameter_choices_to_cell(Mdl, other_modeloptions)
% Notes:
% - Extract hyperparameter choices in a format that we can pass back in
% to train or test a new model.
% - We could use the best observed point, but it's preferred to use the
% best estimated values from a smooth model fit to observed samples.

% opt_hyperparam_table = Mdl.HyperparameterOptimizationResults.XAtMinObjective;
opt_hyperparam_table = Mdl.HyperparameterOptimizationResults.XAtMinEstimatedObjective;

best_modeloptions = other_modeloptions;

% Add optimal choices to the option set
for j = 1:size(opt_hyperparam_table, 2)
    vname = opt_hyperparam_table.Properties.VariableNames{j};

    paramval = opt_hyperparam_table.(vname)(1);

    % fix: categorical Standardize to text.
    % Leave KernelFunction as string
    if iscategorical(paramval) && strcmp(vname, 'Standardize')
        paramval = char(string(paramval));
        paramval = strcmp(paramval, 'true');
    elseif iscategorical(paramval)
        % For KernelFunction
        paramval = char(paramval);
    end

    % Get rid of NaN options passed back out from optimize 'all'
    if ~isnan(paramval)
        best_modeloptions{end + 1} = vname;
        best_modeloptions{end + 1} = paramval; %#ok<*AGROW>
    end

end

end

% function best_modeloptions = get_modal_params_across_folds(S)
%
% % This function is deprecated because modal parameters across folds is not
% % the best way to get the final hyperparameters. Instead, the model should
% % be re-fit using single cross-validation with all hparam options pick the
% % best relative model. Nested cross-val will estimate the accuracy of the
% % whole procedure, including the hparam search, but not return the final best
% % parameters.
%
% hbyfold = cat(1, S.fold_modeloptions{:});
% best_modeloptions = {};
%
% for i = 1:size(hbyfold, 2)
%
%     mydat = cat(1, hbyfold{:, i});
%
%     if ischar(mydat) || iscategorical(mydat) || islogical(mydat)
%
%         best_modeloptions{1, i} = mode(mydat);
%
%     elseif isnumeric(mydat)
%
%         best_modeloptions{1, i} = trimmean(mydat, 80);
%
%     else
%         error('Unknown model option class! Extend this code.')
%
%     end
%
% end
%
% end

% -------------------------------------------------------------------------
% accuracy
% -------------------------------------------------------------------------

function S = update_accuracy_stats(S)

S.crossval_accuracy = S.accfun(S.Y, S.yfit);

s1 = S.dist_from_hyperplane_xval(S.Y == 1);
s2 = S.dist_from_hyperplane_xval(S.Y == -1);

% d = difference / std pooled within, weighted by sample size
S.classification_d_singleinterval = (mean(s1) - mean(s2)) ./ sqrt( ( var(s1) .* length(s1) + var(s2) .* length(s2) ) ./ (length(s1) + length(s2)) );

% Do we have multiple obs within-person? If so return within-person stats
S.mult_obs_within_person = length(S.id) > length(unique(S.id));

if S.mult_obs_within_person

    varname = 'dist_from_hyperplane_xval';
    [scores_within_id, scorediff, d] = get_scores_within_id(S, varname);

    S.Y_within_id = get_scores_within_id(S, 'Y');

    S.scores_within_id = scores_within_id;
    S.scorediff = scorediff;
    S.crossval_accuracy_within = 100 * sum(scorediff > 0) ./ sum(~isnan(scorediff));
    S.classification_d_within = d;

end

end

% -------------------------------------------------------------------------
% plots
% -------------------------------------------------------------------------

function prelim_data_plot(X, Y, id)

create_figure('Data view', 1, 3);
imagesc(X); colorbar;
xlabel('Input features'); ylabel('Observation'); title('Predictor matrix (X)');
axis tight; set(gca, 'YDir', 'reverse');

subplot(1, 3, 2);
imagesc([Y scale(id)]);
axis tight; set(gca, 'YDir', 'reverse');
set(gca, 'XTick', [1 2], 'XTickLabel', {'Y' 'id'});
title('Outcome (Y) and id');

subplot(1, 3, 3);
r = corr(X');
[~, wh] = sort(Y);
imagesc(r(wh, wh));
n1 = sum(Y == -1);
n2 = sum(Y == 1);

h1 = drawbox(0, n1, 0, n1, 'k');
set(h1, 'FaceColor', 'none', 'EdgeColor', 'r', 'LineWidth', 2);

h1 = drawbox(n1, n2, n1, n2, 'k');
set(h1, 'FaceColor', 'none', 'EdgeColor', 'r', 'LineWidth', 2);

axis tight; set(gca, 'YDir', 'reverse');
% set(gca, 'XTick', [1 2], 'XTickLabel', {'Y' 'id'});
title('Inter-obs correlations sorted by Y [-1, 1]');

colorbar

cm = colormap_tor([0 0 1], [1 0 0], [1 1 1]);
colormap(cm)
set(gca, 'CLim', [-1 1])

drawnow, snapnow
end

function plot_scores_and_ROC(S, varargin)
% plot_scores_and_ROC(S, varargin) -> varargin = new title

create_figure('cross-val accuracy', 2, 2);
subplot(2, 2, 1); delete(gca); subplot(2, 2, 2); delete(gca);
axes('Position', [.13 .58 .77 .35]);
set(gca, 'FontSize', 16)
hold on

if S.mult_obs_within_person
    % Within-person scores

    n = size(S.scores_within_id, 1);
    x = [1:n; 1:n];
    plot(x(:, S.scorediff > 0), S.scores_within_id(S.scorediff > 0, :)', 'Color', [.3 .3 .3], 'LineWidth', 1);
    plot(x(:, S.scorediff < 0), S.scores_within_id(S.scorediff < 0, :)', 'r', 'LineWidth', 1);

    plot(x(1, :), S.scores_within_id(:, 1)', '^', 'Color', [.3 .5 1] / 2, 'MarkerFaceColor', [.3 .5 1],  'LineWidth', 1);
    plot(x(2, :), S.scores_within_id(:, 2)', 'v', 'Color', [1 .5 0] / 2, 'MarkerFaceColor', [1 .5 0],  'LineWidth', 1);

    xlabel(sprintf('ID, %3.0f%% single-interval acc, %3.0f%% forced-choice', S.crossval_accuracy, S.crossval_accuracy_within));

    disp('Black lines: Correct, Red lines: Errors. Red triangles: Scores for Class 1, Blue triangles: Scores for Class -1');

else

    % Scores, all between-person

    n = size(S.dist_from_hyperplane_xval, 1);
    x = 1:n;
    iscorrect = sign(S.dist_from_hyperplane_xval) == sign(S.Y);

    %plot(x(:, iscorrect), S.dist_from_hyperplane_xval(iscorrect, :)', 'o', 'MarkerSize', 10, 'Color', [.3 .3 .3], 'LineWidth', 1);
    plot(x(:, ~iscorrect), S.dist_from_hyperplane_xval(~iscorrect, :)', 'o', 'MarkerSize', 10, 'Color', [1 0 0], 'LineWidth', 1);

    h = plot_horizontal_line(0); set(h, 'LineStyle', '--');

    color1 = [.2 .8 .2];
    color2 = [.3 .3 1];

    plot(x(:, S.Y == 1), S.dist_from_hyperplane_xval(S.Y == 1, :)', 'v', 'MarkerSize', 6, 'Color', color1, 'MarkerFaceColor', color1, 'LineWidth', 1);
    plot(x(:, S.Y == -1), S.dist_from_hyperplane_xval(S.Y == -1, :)', '^', 'MarkerSize', 6, 'Color', color2, 'MarkerFaceColor', color2, 'LineWidth', 1);

    xlabel(sprintf('ID, %3.0f%% single-interval acc', S.crossval_accuracy));

    disp('Red cicles: Errors. Green triangles: Scores for Class 1, Blue triangles: Scores for Class -1');

    axis tight

end

ylabel('SVM Score');

title('Cross-validated SVM scores (no hyperparameter optimization)');
if ~isempty(varargin), title(varargin{1}); end

% ROC plot
subplot(2, 2, 3);
S.ROC_single_interval = roc_plot(S.dist_from_hyperplane_xval, logical(S.Y > 0), 'color', [.4 .4 .7], 'threshold', 0);
title('Single-interval ROC')
set(gca, 'FontSize', 16)

subplot(2, 2, 4);

if S.mult_obs_within_person

    % Paired forced-choice. Get complete cases - Remove NaNs id-wise
    outcomes = S.Y_within_id;
    scores = S.scores_within_id;
    [~, outcomes, scores] = nanremove(outcomes, scores);

    S.ROC_forced_choice = roc_plot(scores(:), logical(outcomes(:) > 0), 'color', [.4 .4 .7], 'twochoice');
    title('Forced-choice ROC')
    set(gca, 'FontSize', 16)

else
    % Scores, all between-person

    plot(S.Y(~iscorrect), S.dist_from_hyperplane_xval(~iscorrect, :)', 'o', 'MarkerSize', 10, 'Color', [1 0 0], 'LineWidth', 1);

    h = plot_horizontal_line(0); set(h, 'LineStyle', '--');

    color1 = [.2 .8 .2];
    color2 = [.3 .3 1];

    plot(ones(1, sum(S.Y == 1)), S.dist_from_hyperplane_xval(S.Y == 1, :)', 'v', 'MarkerSize', 6, 'Color', color1, 'MarkerFaceColor', color1, 'LineWidth', 1);
    plot(-ones(1, sum(S.Y == -1)), S.dist_from_hyperplane_xval(S.Y == -1, :)', '^', 'MarkerSize', 6, 'Color', color2, 'MarkerFaceColor', color2, 'LineWidth', 1);

    set(gca, 'XTick', [-1 1], 'XLim', [-1.5 1.5]);
    xlabel('True class');
    ylabel('Predicted class')

end

drawnow, snapnow

end

function xval_plot_scores_vs_class_probability(S)

% crossval returns cross-validated yfit (class predictions), scores (dist_from_hyperplane_xval),
% class_probability_xval. Scores and class probabilties will diverge (may
% not be perfectly correlated) because the sigmoid scaling (Platt scaling) varies across folds.
% They will diverge more if there is no true signal.

create_figure('xval scores vs. class probability estimates', 1, 2);

plot(S.dist_from_hyperplane_xval, S.class_probability_xval, 'o');

for j = 1:length(S.teIdx)

    plot(S.dist_from_hyperplane_xval(S.teIdx{j}), S.class_probability_xval(S.teIdx{j}), 'o', 'MarkerFaceColor', rand(1, 3));

end

xlabel('Cross-validated SVM scores (colors are folds)');
ylabel('Cross-validated class prob estimates');

hh = plot_vertical_line(0); set(hh, 'LineStyle', '--');
hh = plot_horizontal_line(.5); set(hh, 'LineStyle', '--');

subplot(1, 2, 2)

lineh = plot(S.dist_from_hyperplane_xval(S.Y == 1), S.class_probability_xval(S.Y == 1), 'v', 'MarkerFaceColor', [.2 .8 .2]);
lineh = [lineh plot(S.dist_from_hyperplane_xval(S.Y == -1), S.class_probability_xval(S.Y == -1), '^', 'MarkerFaceColor', [.2 .2 1])];

xlabel('Cross-validated SVM scores (colors are folds)');
ylabel('Cross-validated class prob estimates');

hh = plot_vertical_line(0); set(hh, 'LineStyle', '--');
hh = plot_horizontal_line(.5); set(hh, 'LineStyle', '--');

legend(lineh, {'True Class = 1' 'True Class = -1'});

drawnow, snapnow

end

% -------------------------------------------------------------------------
% Inputs
% -------------------------------------------------------------------------

function ARGS = parse_inputs(varargin)

p = inputParser;

% Validation functions - customized for each type of input
% ----------------------------------------------------------------------

% valfcn_scalar = @(x) validateattributes(x, {'numeric'}, {'nonempty', 'scalar'});

valfcn_number = @(x) validateattributes(x, {'numeric'}, {'nonempty'}); % scalar or vector

valfcn_cell = @(x) validateattributes(x, {'cell'}, {'nonempty'}); % scalar or vector

% Validation: Region object, structure, or [x1 x2 x3] triplet
% valfcn_custom = @(x) isstruct(x) || isa(x, 'region') || (~isempty(x) && all(size(x) - [1 3] == 0) && all(isnumeric(x)));

valfcn_logical = @(x) validateattributes(x, {}, {'nonempty', 'scalar', '>=', 0, '<=', 1}); % could enter numeric 0,1 or logical

valfcn_effectscode = @(x) validateattributes(x, {'numeric'}, {'nonempty', '<=', 1, '>=', -1});

% Required inputs
% ----------------------------------------------------------------------
p.addRequired('X', valfcn_number);
p.addRequired('Y', valfcn_effectscode);
p.addRequired('id', valfcn_number);

% Optional inputs
% ----------------------------------------------------------------------
% Pattern: keyword, value, validation function handle

p.addParameter('doplot', true, valfcn_logical);
p.addParameter('doverbose', true, valfcn_logical);
p.addParameter('dooptimize', true, valfcn_logical);
p.addParameter('dorepeats', 10, valfcn_number);
p.addParameter('dobootstrap', true, valfcn_logical);
p.addParameter('nboot', 1000, valfcn_number);
p.addParameter('nfolds',10, valfcn_number); % Added by Michael Sun 08/2/2022

p.addParameter('modeloptions', {'KernelFunction', 'linear'}, valfcn_cell);

% Parse inputs and distribute out to variable names in workspace
% ----------------------------------------------------------------------
% e.g., p.parse([30 1 0], [-40 0 10], 'bendpercent', .1);
p.parse(varargin{:});

ARGS = p.Results;

end % parse_inputs);


function mpc = case16am
%CASE16AM  Power flow data for 15 bus distribution system from Das, et al
%   Please see CASEFORMAT for details on the case file format.
%
%   Data from ...
%       Das D, Kothari DP, Kalam A (1995) Simple and efficient method for load
%       flow solution of radial distribution networks. Int J Electr Power
%       Energy Syst 17:335-346. doi: 10.1016/0142-0615(95)00050-0
%       URL: https://doi.org/10.1016/0142-0615(95)00050-0

%% MATPOWER Case Format : Version 2
mpc.version = '2';

%%-----  Power Flow Data  -----%%
%% system MVA base
mpc.baseMVA = 10;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [ %% (Pd and Qd are specified in kW & kVAr here, converted to MW & MVAr below)
	1	3	0	0	0	0	1	1	0	12.66	1	1	1;
	2	1	0	0	0	0	1	1	0	12.66	1	1.1	0.9;
	3	1	2000	1600	0	0	1	1	0	12.66	1	1.1	0.9;
	4	1	3000	400	0	0	1	1	0	12.66	1	1.1	0.9;
	5	1	2000	-400	0	0	1	1	0	12.66	1	1.1	0.9;
	6	1	1500	1200	0	0	1	1	0	12.66	1	1.1	0.9;
	7	1	4000	2700	0	0	1	1	0	12.66	1	1.1	0.9;
	8	1	5000	1800	0	0	1	1	0	12.66	1	1.1	0.9;
	9	1	1000	900	0	0	1	1	0	12.66	1	1.1	0.9;
	10	1	600	-500	0	0	1	1	0	12.66	1	1.1	0.9;
	11	1	4500	-1700	0	0	1	1	0	12.66	1	1.1	0.9;
	12	1	1000	900	0	0	1	1	0	12.66	1	1.1	0.9;
	13	1	1000	-1100	0	0	1	1	0	12.66	1	1.1	0.9;
	14	1	1000	900	0	0	1	1	0	12.66	1	1.1	0.9;
	15	1	2100	-800	0	0	1	1	0	12.66	1	1.1	0.9;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin	Pc1	Pc2	Qc1min	Qc1max	Qc2min	Qc2max	ramp_agc	ramp_10	ramp_30	ramp_q	apf
mpc.gen = [
	1	0	0	10	-10	1	100	1	10	0	0	0	0	0	0	0	0	0	0	0	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [  %% (r and x specified in ohms here, converted to p.u. below)
	1	2	0	1e-8	0	0	0	0	0	0	1	-360	360;    %% original reactance of 0 set to 1e-8
	2	3	0.1202	0.1603	0	0	0	0	0	0	1	-360	360;
	3	4	0.1282	0.1763	0	0	0	0	0	0	1	-360	360;
	3	5	0.1442	0.2885	0	0	0	0	0	0	1	-360	360;
	5	6	0.0641	0.0641	0	0	0	0	0	0	1	-360	360;
	2	7	0.1763	0.1763	0	0	0	0	0	0	1	-360	360;
	7	8	0.1282	0.1763	0	0	0	0	0	0	1	-360	360;
	7	9	0.1763	0.1763	0	0	0	0	0	0	1	-360	360;
	8	10	0.1763	0.1763	0	0	0	0	0	0	1	-360	360;
	8	11	0.1282	0.1763	0	0	0	0	0	0	1	-360	360;
	2	12	0.1763	0.1763	0	0	0	0	0	0	1	-360	360;
	12	13	0.1442	0.1923	0	0	0	0	0	0	1	-360	360;
	12	14	0.1282	0.1763	0	0	0	0	0	0	1	-360	360;
	14	15	0.0641	0.0641	0	0	0	0	0	0	1	-360	360;
];

%%-----  OPF Data  -----%%
%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	2	0	0	3	0	20	0;
];

%% convert branch impedances from Ohms to p.u.
[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;
[F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C, ...
    TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST, ...
    ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX] = idx_brch;
Vbase = mpc.bus(1, BASE_KV) * 1e3;      %% in Volts
Sbase = mpc.baseMVA * 1e6;              %% in VA
mpc.branch(:, [BR_R BR_X]) = mpc.branch(:, [BR_R BR_X]) / (Vbase^2 / Sbase);

%% convert loads from kW to MW
mpc.bus(:, [PD, QD]) = mpc.bus(:, [PD, QD]) / 1e3;


function sparse_grid_mixed_size_tabulate ( rule_1d, alpha_1d, beta_1d, ...
   dim_min, dim_max, level_max_min, level_max_max )

%****************************************************************************80
%
%% SPARSE_GRID_MIXED_SIZE_TABULATE tests SPARSE_GRID_MIXED_SIZE.
%
%  Discussion:
%
%    We do NOT consider mixed rules.  Instead, we are looking at sparse grid
%    rules for which all dimensions use the same 1D rule family.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer RULE_1D, the 1D rule.
%
%    Input, real ALPHA_1D, BETA_1D, the optional parameters.
%
%    Input, integer DIM_MIN, the minimum spatial dimension.
%
%    Input, integer DIM_MAX, the maximum spatial dimension.
%
%    Input, integer LEVEL_MAX_MIN, the minimum value of LEVEL_MAX.
%
%    Input, integer LEVEL_MAX_MAX, the maximum value of LEVEL_MAX.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_GRID_MIXED_SIZE_TABULATE\n' );
  fprintf ( 1, '  SPARSE_GRID_MIXED_SIZE returns the number of distinct\n' );
  fprintf ( 1, '  points in a sparse grid.\n' );

  if ( rule_1d == 0 )

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Here we report the total number of polynomials of\n' )
    fprintf ( 1, '  degree DEGREE or less in the given DIM dimensional space.\n' );
    fprintf ( 1, '  (This is essentially Pascal''s triangle.)\n' );
    fprintf ( 1, '\n' );

    fprintf ( 1, '   DIM: ' );
    for dim_num = dim_min : dim_max
      fprintf ( 1, '  %8d', dim_num );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '      DEGREE\n' );
    fprintf ( 1, '\n' );

    degree_max = 2 * level_max_max + 1;

    for degree = 0 : degree_max

      fprintf ( 1, '    %4d', degree );

      for dim_num = dim_min : dim_max

        point_num = i4_choose ( dim_num + degree, dim_num );

        fprintf ( 1, '  %8d', point_num );

      end

      fprintf ( 1, '\n' );

    end

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, '  We use the same rule in all dimensions, and count the points\n' );
    fprintf ( 1, '  for a range of dimensions and levels.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  1D rule index is %d\n', rule_1d );
    fprintf ( 1, '  ALPHA parameter is %f\n', alpha_1d );
    fprintf ( 1, '  BETA parameter is  %f\n', beta_1d );
    fprintf ( 1, '\n' );

    tol = sqrt ( eps );

    fprintf ( 1, '   DIM: ' );
    for dim_num = dim_min : dim_max
      fprintf ( 1, '  %8d', dim_num );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '   LEVEL_MAX\n' );
    fprintf ( 1, '\n' );

    for level_max = level_max_min : level_max_max

      fprintf ( 1, '    %4d', level_max );

      for dim_num = dim_min : dim_max

        rule(1:dim_num) = rule_1d;
        alpha(1:dim_num) = alpha_1d;
        beta(1:dim_num) = beta_1d;

        point_num = sparse_grid_mixed_size ( dim_num, level_max, rule, alpha, ...
          beta, tol );

        fprintf ( 1, '  %8d', point_num );

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


function x=subint(x1,x2,x3,x4,na,nb,nc)
%subint   geometrically stretched subdivision generator
%   x=subint(x1,x2,x3,x4,na,nb,nc);
%   input
%             x1      left coordinate
%             x2      limit of uniform expansion section
%             x3      limit of equal subinterval section
%             x4      right coordinate and limit of contraction section
%             na      number of uniformly expanded subintervals
%             nb      number of intermediate uniform subintervals
%             nc      number of contracting subintervals
%    output
%              x      vector of coordinates
%
%   calls function fitint
%   sets up global variables global_N, global_INTL, global_LASTDL
%   IFISS function: DJS; 28 February 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      global global_N global_INTL global_LASTDL
      intla=x2-x1;
      intlb=x3-x2;
      intlc=x4-x3;
      npts=na+nb+nc+1;
      bdl=intlb/nb;
      x=zeros(npts,1);
      x(1)=x1;
%
% uniform expansion section
      if na > 0
%% fixed value here
%        ratio = default('approximate expansion ratio (default 1.2)',1.2);
         ratio = 1.2;
         global_N=na;
         global_INTL=intla;
         global_LASTDL=bdl;
         [ratio,initdl]=fitint(ratio);
         fprintf('computed stretch ratio is %10.5g \n',ratio)
         dl=initdl;
         for node=2:na+1
            x(node)=x(node-1)+dl;
            dl=dl*ratio;
         end
         if (abs(x(na+1)-x2) > 1e-6)
            fprintf('\n warning ...\n calculated coordinate is %10.5g',x(na+1))
            fprintf('              \n      input coordinate is %10.5g',x2)
            x(na+1)=x2;
         end
      end
%
% uniform subinterval section
      dl=bdl;
      for node=na+2:na+nb+1
         x(node)=x(node-1) + dl;
      end
%
% uniform contraction section
      if nc > 0
%% fixed value here
%        ratio = default('approximate contraction ratio (default 1.2)',
         ratio = 1.2;
         global_N=nc;
         global_INTL=intlc;
         global_LASTDL=bdl;
         [ratio,initdl]=fitint(ratio);
%        fprintf('computed contraction ratio is %10.5g \n',ratio)
         dl=global_LASTDL/ratio;
         for node=na+nb+2:npts
            x(node)=x(node-1)+dl;
            dl=dl/ratio;
         end
         if (abs(x(npts)-x4) > 1e-6)
            fprintf('\n warning ...\n calculated coordinate is %10.5g',x(npts))
            fprintf('              \n      input coordinate is %10.5g',x4)
         end
      end
      x(npts)=x4;
      return


function [tfr,rtfr,hat] = tfrrgab(x,t,N,Nh,trace,K);
%TFRRGAB Reassigned Gabor spectrogram time-frequency distribution.
%	[TFR,RTFR,HAT] = TFRRGAB(X,T,N,NH,TRACE,K)
%	computes the Gabor spectrogram and its reassigned version.
%	This particular window (a Gaussian window) allows a 20 % faster
%	algorithm than the TFRRSP function.
%
%	X     : analysed signal
%	T     : the time instant(s)           (default : 1:length(X))
%	N     : number of frequency bins      (default : length(X))
%	NH    : length of the gaussian window (default : N/4))
%	TRACE : if nonzero, the progression of the algorithm is shown
%                                             (default : 0).
%	K     : value at both extremities     (default 0.001)
%	TFR,  : time-frequency representation and its reassigned
%	RTFR    version. When called without output arguments,
%	        TFRRGAB runs TFRQVIEW.
%	HAT   : Complex matrix of the reassignment vectors.
%
%	Example :
%	 sig=fmlin(128,0.1,0.4); tfrrgab(sig,1:128,128,19,1);
%
%	See also all the time-frequency representations listed in
%	 the file CONTENTS (TFR*)

%	F. Auger, May-July 1994, July 1995.
%       Copyright (c) 1996 by CNRS(France).
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin == 0),
 error('At least 1 parameter required');
end;
[xrow,xcol] = size(x);
if (nargin <= 2),
 N=xrow;
end;

hlength=floor(N/4);
hlength=hlength+1-rem(hlength,2);

if (nargin == 1),
 t=1:xrow;
end;

if (nargin <= 3),
 Nh=hlength; trace=0; K=0.001;
elseif (nargin == 4),
 trace = 0; K=0.001;
elseif (nargin == 5),
 K= 0.001;
end;

if (N<0),
 error('N must be greater than zero');
end;
[trow,tcol] = size(t);
if (xcol~=1),
 error('X must have only one column');
elseif (trow~=1),
 error('T must only have one row');
elseif (2^nextpow2(N)~=N & nargin==6),
 fprintf('For a faster computation, N should be a power of two\n');
end;

if (rem(Nh,2)==0),
 error('Nh must be odd');
elseif length(Nh)~=1,
 error('Nh must be a scalar');
end;

Nh2=Nh-2;
TFTBcontinue=1;
while TFTBcontinue,
 Nh2=Nh2+2;
 h=tftb_window(Nh2,'gauss',K^((Nh2-1)^2 /(Nh-1)^2));
 TFTBcontinue=(h(Nh2)*(Nh2-1)>2*K);
end;

K=K^((Nh2-1)^2 /(Nh-1)^2); Nh=Nh2; Lh=(Nh-1)/2;
h=h; Th=h.*[-Lh:Lh]';

if (tcol==1),
 Dt=1;
else
 Deltat=t(2:tcol)-t(1:tcol-1);
 Mini=min(Deltat); Maxi=max(Deltat);
 if (Mini~=Maxi),
  error('The time instants must be regularly sampled.');
 else
  Dt=Mini;
 end;
 clear Deltat Mini Maxi;
end;

tfr= zeros(N,tcol); tf2= zeros(N,tcol); tf3= zeros(N,tcol);
if trace, disp('Gabor spectrogram'); end;

for icol=1:tcol,
 if trace, disprog(icol,tcol,10); end;
 ti= t(icol);
 tau=-min([round(N/2)-1,Lh,ti-1]):min([round(N/2)-1,Lh,xrow-ti]);
 indices= rem(N+tau,N)+1;
 norm_h=norm(h(Lh+1+tau));
 tfr(indices,icol)=x(ti+tau).*conj( h(Lh+1+tau)) /norm_h;
 tf2(indices,icol)=x(ti+tau).*conj(Th(Lh+1+tau)) /norm_h;
end ;
tfr=fft(tfr); tf2=fft(tf2);
tfr=tfr(:); tf2=tf2(:);  tf3=tf3(:);
avoid_warn=find(tfr~=0.0);
tf3(avoid_warn)=round(imag(2*log(K)*N*tf2(avoid_warn)./tfr(avoid_warn)/(2.0*pi*Lh^2)));
tf2(avoid_warn)=round(real(tf2(avoid_warn)./tfr(avoid_warn)/Dt));
tfr=abs(tfr).^2;
if trace, fprintf ('\nreassignment: \n'); end;
tfr=reshape(tfr,N,tcol);
tf2=reshape(tf2,N,tcol);
tf3=reshape(tf3,N,tcol);

rtfr= zeros(N,tcol);
Ex=mean(abs(x(min(t):max(t))).^2); Threshold=1.0e-6*Ex;
for icol=1:tcol,
 if trace, disprog(icol,tcol,10); end;
 for jcol=1:N,
  if abs(tfr(jcol,icol))>Threshold,
   icolhat= icol + tf2(jcol,icol);
   icolhat=min(max(icolhat,1),tcol);
   jcolhat= jcol - tf3(jcol,icol);
   %while (jcolhat<1),jcolhat=jcolhat+N; end;
   %while (jcolhat>N),jcolhat=jcolhat-N; end;
   jcolhat=rem(rem(jcolhat-1,N)+N,N)+1;
   rtfr(jcolhat,icolhat)=rtfr(jcolhat,icolhat) + tfr(jcol,icol) ;
   tf2(jcol,icol)=jcolhat + j * icolhat;
  else
   tf2(jcol,icol)=inf*(1+j);
   rtfr(jcol,icol)=rtfr(jcol,icol) + tfr(jcol,icol) ;
  end;
 end;
end;

if trace, fprintf('\n'); end;
clear tf3;
if (nargout==0),
 TFTBcontinue=1;
 while (TFTBcontinue==1),
  choice=menu ('Choose the representation:',...
               'stop',...
               'Gabor spectrogram',...
               'reassigned Gabor spectrogram');
  if (choice==1), TFTBcontinue=0;
  elseif (choice==2),
   Q=round(tcol*N/xrow);
   tfrqview(tfr,x,t,'tfrgabor',tcol,Q,h);
  elseif (choice==3),
   tfrqview(rtfr,x,t,'tfrrgab',Nh);
  end;
 end;
elseif (nargout>2),
 hat=tf2;
end;


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
% ##2
%==============================================================================
%
%   - data                 MRI (head), Omega=(0,128)x(0,128), level=4:7, m=[128,128]
%   - viewer               viewImage2D
%   - interpolation        splineInter
%   - distance             NGF
%   - pre-registration     rigid2D
%   - regularizer          mbElastic
%   - optimization         lBFGS
% version 2015/05/20
% ===============================================================================

close all, help(mfilename);
setup2DMRIData

imgModel('reset','imgModel','splineInter','regularizer','moments','theta',1e-1);
distance('reset','distance','NGF','edge',50);
trafo('reset','trafo','rigid2D');
regularizer('reset','regularizer','mbElastic','alpha',0.1,'mu',1,'lambda',0);

PIRpara = optPara('lBFGS','solver','backslash');
NPIRpara = optPara('lBFGS','solver',regularizer('get','solver'));

[yc,wc,his] = MLIR(ML,'PIRobj',@PIRBFGSobjFctn,'PIRpara',PIRpara,...
  'NPIRobj',@NPIRBFGSobjFctn,'NPIRpara',NPIRpara,...
  'minLevel',4,'maxLevel',7,'parametric',1,'plotMLiter',0);

%==============================================================================


function vifp=vifp_mscale(ref,dist)

% -----------COPYRIGHT NOTICE STARTS WITH THIS LINE------------
% Copyright (c) 2005 The University of Texas at Austin
% All rights reserved.
%
% Permission is hereby granted, without written agreement and without license or royalty fees, to use, copy,
% modify, and distribute this code (the source files) and its documentation for
% any purpose, provided that the copyright notice in its entirety appear in all copies of this code, and the
% original source of this code, Laboratory for Image and Video Engineering (LIVE, http://live.ece.utexas.edu)
% at the University of Texas at Austin (UT Austin,
% http://www.utexas.edu), is acknowledged in any publication that reports research using this code. The research
% is to be cited in the bibliography as:
%
% H. R. Sheikh and A. C. Bovik, "Image Information and Visual Quality", IEEE Transactions on
% Image Processing, (to appear).
%
% IN NO EVENT SHALL THE UNIVERSITY OF TEXAS AT AUSTIN BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL,
% OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OF THIS DATABASE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF TEXAS
% AT AUSTIN HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
% THE UNIVERSITY OF TEXAS AT AUSTIN SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE DATABASE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS,
% AND THE UNIVERSITY OF TEXAS AT AUSTIN HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
%
% -----------COPYRIGHT NOTICE ENDS WITH THIS LINE------------
%
% This software release consists of a MULTISCALE PIXEL DOMAIN, SCALAR GSM implementation of the algorithm described in the paper:
%
% H. R. Sheikh and A. C. Bovik, "Image Information and Visual Quality"., IEEE Transactions on Image Processing, (to appear).
% Download manuscript draft from http://live.ece.utexas.edu in the Publications link.
%
% THE PIXEL DOMAIN ALGORITHM IS NOT DESCRIBED IN THE PAPER. THIS IS A COMPUTATIONALLY SIMPLER
% DERIVATIVE OF THE ALGORITHM PRESENTED IN THE PAPER
%
% Input : (1) img1: The reference image as a matrix
%         (2) img2: The distorted image (order is important)
%
% Output: (1) VIF the visual information fidelity measure between the two images
%
% Default Usage:
%    Given 2 test images img1 and img2, whose dynamic range is 0-255
%
%    vif = vifvec(img1, img2);
%
% Advanced Usage:
%    Users may want to modify the parameters in the code.
%    (1) Modify sigma_nsq to find tune for your image dataset.
% Email comments and bug reports to hamid.sheikh@ieee.org

sigma_nsq=2;

num=0;
den=0;
for scale=1:4

    N=2^(4-scale+1)+1;
    win=fspecial('gaussian',N,N/5);

    if (scale >1)
        ref=filter2(win,ref,'valid');
        dist=filter2(win,dist,'valid');
        ref=ref(1:2:end,1:2:end);
        dist=dist(1:2:end,1:2:end);
    end

    mu1   = filter2(win, ref, 'valid');
    mu2   = filter2(win, dist, 'valid');
    mu1_sq = mu1.*mu1;
    mu2_sq = mu2.*mu2;
    mu1_mu2 = mu1.*mu2;
    sigma1_sq = filter2(win, ref.*ref, 'valid') - mu1_sq;
    sigma2_sq = filter2(win, dist.*dist, 'valid') - mu2_sq;
    sigma12 = filter2(win, ref.*dist, 'valid') - mu1_mu2;

    sigma1_sq(sigma1_sq<0)=0;
    sigma2_sq(sigma2_sq<0)=0;

    g=sigma12./(sigma1_sq+1e-10);
    sv_sq=sigma2_sq-g.*sigma12;

    g(sigma1_sq<1e-10)=0;
    sv_sq(sigma1_sq<1e-10)=sigma2_sq(sigma1_sq<1e-10);
    sigma1_sq(sigma1_sq<1e-10)=0;

    g(sigma2_sq<1e-10)=0;
    sv_sq(sigma2_sq<1e-10)=0;

    sv_sq(g<0)=sigma2_sq(g<0);
    g(g<0)=0;
    sv_sq(sv_sq<=1e-10)=1e-10;

     num=num+sum(sum(log10(1+g.^2.*sigma1_sq./(sv_sq+sigma_nsq))));
     den=den+sum(sum(log10(1+sigma1_sq./sigma_nsq)));

end
vifp=num/den;


function [Vx,Vy,reliab]=opticalFlow( I1, I2, varargin )
% Coarse-to-fine optical flow using Lucas&Kanade or Horn&Schunck.
%
% Implemented 'type' of optical flow estimation:
%  LK: http://en.wikipedia.org/wiki/Lucas-Kanade_method
%  HS: http://en.wikipedia.org/wiki/Horn-Schunck_method
% LK is a local, fast method (the implementation is fully vectorized).
% HS is a global, slower method (an SSE implementation is provided).
%
% Common parameters for LK and HS: 'smooth' determines smoothing prior to
% flow computation and can make flow estimation more robust. 'resample' can
% be used to downsample an image for faster but lower quality results, e.g.
% resample=.5 makes flow computation about 4x faster. LK: 'radius' controls
% integration window size (and smoothness of flow). HS: 'alpha' controls
% tradeoff between data and smoothness term (and smoothness of flow) and
% 'nIter' determines number of gradient decent steps.
%
% USAGE
%  [Vx,Vy,reliab] = opticalFlow( I1, I2, pFlow )
%
% INPUTS
%  I1, I2   - input images to calculate flow between
%  pFlow    - parameters (struct or name/value pairs)
%   .type       - ['LK'] may be either 'LK' or 'HS'
%   .smooth     - [1] smoothing radius for triangle filter (may be 0)
%   .resample   - [1] resampling amount (must be a power of 2)
%   .radius     - [5] integration radius for weighted window [LK only]
%   .alpha      - [1] smoothness constraint [HS only]
%   .nIter      - [250] number of iterations [HS only]
%
% OUTPUTS
%  Vx, Vy   - x,y components of flow  [Vx>0->right, Vy>0->down]
%  reliab   - reliability of flow in given window [LK only]
%
% EXAMPLE - compute LK flow on test images
%  load opticalFlowTest;
%  [Vx,Vy]=opticalFlow(I1,I2,'smooth',1,'radius',10,'type','LK');
%  figure(1); im(I1); figure(2); im(I2);
%  figure(3); im([Vx Vy]); colormap jet;
%
% EXAMPLE - rectify I1 to I2 using computed flow
%  load opticalFlowTest;
%  [Vx,Vy]=opticalFlow(I1,I2,'smooth',1,'radius',10,'type','LK');
%  I1=imtransform2(I1,[],'vs',-Vx,'us',-Vy,'pad','replicate');
%  figure(1); im(I1); figure(2); im(I2);
%
% EXAMPLE - compare LK and HS flow
%  load opticalFlowTest;
%  prm={'smooth',1,'radius',10,'alpha',20,'nIter',200,'type'};
%  tic, [Vx1,Vy1]=opticalFlow(I1,I2,prm{:},'LK'); toc
%  tic, [Vx2,Vy2]=opticalFlow(I1,I2,prm{:},'HS'); toc
%  figure(1); im([Vx1 Vy1; Vx2 Vy2]); colormap jet;
%
% See also convTri, imtransform2
%
% Piotr's Image&Video Toolbox      Version 3.02
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

% get default parameters and do error checking
dfs={'type','LK','smooth',1,'resample',1,'radius',5,'alpha',1,'nIter',250};
[type,smooth,resample,radius,alpha,nIter]=getPrmDflt(varargin,dfs,1);
assert(any(strcmp(type,{'LK','HS'}))); useLk=strcmp(type,'LK');
if( ~ismatrix(I1) || ~ismatrix(I2) || any(size(I1)~=size(I2)) )
  error('Input images must be 2D and have same dimensions.'); end

% run optical flow in coarse to fine fashion
if(~isa(I1,'single')), I1=single(I1); I2=single(I2); end
[h,w]=size(I1); nScales=floor(log2(min(h,w)))-2;
for s=1:nScales + round(log2(resample))
  % get current scale and I1s and I2s at given scale
  scale=2^(nScales-s); h1=round(h/scale); w1=round(w/scale);
  if( scale==1 ), I1s=I1; I2s=I2; else
    I1s=imResample(I1,[h1 w1]); I2s=imResample(I2,[h1 w1]); end
  % initialize Vx,Vy or upsample from previous scale
  if(s==1), Vx=zeros(h1,w1,'single'); Vy=Vx; else r=sqrt(h1*w1/numel(Vx));
    Vx=imResample(Vx,[h1 w1])*r; Vy=imResample(Vy,[h1 w1])*r; end
  % transform I1s according to current estimate of Vx and Vy
  if(s), I1s=imtransform2(I1s,[],'pad','replciate','vs',-Vx,'us',-Vy); end
  % smooth images
  I1s=convTri(I1s,smooth); I2s=convTri(I2s,smooth);
  % run optical flow on current scale
  if( useLk ), [Vx1,Vy1,reliab]=opticalFlowLk(I1s,I2s,radius);
  else [Vx1,Vy1]=opticalFlowHs(I1s,I2s,alpha,nIter); reliab=[]; end
  Vx=Vx+Vx1; Vy=Vy+Vy1;
end
if(s~=nScales), r=sqrt(h*w/numel(Vx));
  Vx=imResample(Vx,[h w])*r; Vy=imResample(Vy,[h w])*r; end

end

function [Vx,Vy,reliab] = opticalFlowLk( I1, I2, radius  )
% Compute elements of A'A and also of A'b
radius=min(radius,floor(min(size(I1,1),size(I1,2))/2)-1);
[Ix,Iy]=gradient2(I1); It=I2-I1; AAxy=convTri(Ix.*Iy,radius);
AAxx=convTri(Ix.^2,radius)+1e-5; ABxt=convTri(-Ix.*It,radius);
AAyy=convTri(Iy.^2,radius)+1e-5; AByt=convTri(-Iy.*It,radius);
% Find determinant and trace of A'A
AAdet=AAxx.*AAyy-AAxy.^2; AAdeti=1./AAdet; AAtr=AAxx+AAyy;
% Compute components of velocity vectors (A'A)^-1 * A'b
Vx = AAdeti .* ( AAyy.*ABxt - AAxy.*AByt);
Vy = AAdeti .* (-AAxy.*ABxt + AAxx.*AByt);
% Check for ill conditioned second moment matrices
reliab = 0.5*AAtr - 0.5*sqrt(AAtr.^2-4*AAdet);
end

function [Vx,Vy] = opticalFlowHs( I1, I2, alpha, nIter )
% compute derivatives (averaging over 2x2 neighborhoods)
A00=shift(I1,0,0); A10=shift(I1,1,0);
A01=shift(I1,0,1); A11=shift(I1,1,1);
B00=shift(I2,0,0); B10=shift(I2,1,0);
B01=shift(I2,0,1); B11=shift(I2,1,1);
Ex=0.25*((A01+B01+A11+B11)-(A00+B00+A10+B10));
Ey=0.25*((A10+B10+A11+B11)-(A00+B00+A01+B01));
Et=0.25*((B00+B10+B01+B11)-(A00+A10+A01+A11));
Ex([1 end],:)=0; Ex(:,[1 end])=0;
Ey([1 end],:)=0; Ey(:,[1 end])=0;
Et([1 end],:)=0; Et(:,[1 end])=0;
Z=1./(alpha*alpha + Ex.*Ex + Ey.*Ey);
% iterate updating Ux and Vx in each iter
if( 1 )
  [Vx,Vy]=opticalFlowHsMex(Ex,Ey,Et,Z,nIter);
  Vx=Vx(2:end-1,2:end-1); Vy=Vy(2:end-1,2:end-1);
else
  Vx=zeros(size(I1),'single'); Vy=Vx;
  for i = 1:nIter
    Mx=.25*(shift(Vx,-1,0)+shift(Vx,1,0)+shift(Vx,0,-1)+shift(Vx,0,1));
    My=.25*(shift(Vy,-1,0)+shift(Vy,1,0)+shift(Vy,0,-1)+shift(Vy,0,1));
    m=(Ex.*Mx+Ey.*My+Et).*Z; Vx=Mx-Ex.*m; Vy=My-Ey.*m;
    Vx=Vx(2:end-1,2:end-1); Vy=Vy(2:end-1,2:end-1);
  end
end
end

function J = shift( I, y, x )
% shift I by -1<=x,y<=1 pixels
[h,w]=size(I); J=zeros(h+2,w+2,'single');
J(2-y:end-1-y,2-x:end-1-x)=I;
end


 function [mat, nexp] = filtmat(arg0, arg1, arg2, arg3, arg4)
%function mat = filtmat('1d', kernx, nx)
%			1d nonperiodic
%function mat = filtmat({'1d' 'per'}, kernx, nx)
%			1d periodic
%function mat = filtmat('2d,mul,sep', kernx, kerny, [nx ny])
%			2d multiplicatively separable nonperiodic
%function mat = filtmat('3d,mul,sep', kernx, kerny, kernz, [nx ny nz])
%			3d multiplicatively separable nonperiodic
%			kernel(x,y,z) = kernx(x) * kerny(y) * kernz(z)
%function [mat, nexp] = filtmat({'3d,mul,sep' 'expand'}, kernx, kerny, kernz, [nx ny nz])
%			3d multiplicatively separable nonperiodic, expanded
%function mat = filtmat('1d,causal', kernx, nx)
%			1d causal filter, ala convmtx
%
%	Form matrix to do 3d linear filtering - odd kernel length.
%	expanding is like what happens in 'conv' - output is longer

if ~nargin, help filtmat, error arg, end
flag_expand = any(strcmp(arg0, 'expand'));

if nargin == 3 && strcmp(arg0, '1d,causal')
	mat = filtmat1causal(arg1, arg2);
	return

elseif nargin == 3 && any(strcmp(arg0, '1d'))
	if any(strcmp(arg0, 'per'))
		mat = filtmat1per(arg1, arg2);
	else
		mat = filtmat1(arg1, arg2, flag_expand);
	end
	return

elseif nargin == 4 && any(strcmp(arg0, '2d,mul,sep'))
	mat = filtmat('3d,mul,sep', arg1, arg2, 1, [arg3 1]);
	return

elseif nargin == 5 && any(strcmp(arg0, '3d,mul,sep'))
	k.x = arg1;
	k.y = arg2;
	k.z = arg3;
	n.x = arg4(1);
	n.y = arg4(2);
	n.z = arg4(3);
else
	nargin, help filtmat, error arg
end

	if flag_expand

		[p.x, o.x] = filtmat1(k.x, n.x, flag_expand);
		[p.y, o.y] = filtmat1(k.y, n.y, flag_expand);
		[p.z, o.z] = filtmat1(k.z, n.z, 0);	% no z expansion

		p.x = kron(speye(n.y*n.z), p.x);
		p.y = kron(speye(n.z), kron(p.y, speye(o.x)));
		p.z = kron(p.z, speye(o.x*o.y));

		if 0
			n.xyz = n.x * n.y * n.z;
			t = (o.y-n.y)/2 * o.x;
			p.x = [sparse(t,n.xyz); p.x; sparse(t,n.xyz)];
			t = zeros(o.x,o.y);
			t([1:n.x]+(o.x-n.y)/2-1,:) = 1;
			tt = sparse(o.x*o.y, n.x*n.y);
			tt(find(t(:)),:) = p.y;
			p.y = tt;
			p.z = sparse(1,1);
			if n.z ~= 1, error notdone, end
		end
	else
		p.x = filtmat1(k.x, n.x, flag_expand);
		p.y = filtmat1(k.y, n.y, flag_expand);
		p.z = filtmat1(k.z, n.z, flag_expand);

		p.x = kron(speye(n.y*n.z), p.x);
		p.y = kron(speye(n.z), kron(p.y, speye(n.x)));
		p.z = kron(p.z, speye(n.x*n.y));
		o = n;
	end

%	mat = p.x + p.y + p.z;
	if flag_expand, warning('fix: expand may not work for multiplicative?'), end
	mat = p.z * p.y * p.x;
	nexp = [o.x o.y o.z];

function mat = filtmat1causal(kern, n)
	nk = length(kern);
	if nk < n, error n, end
	if n ~= nk, error 'not done', end
	t = kern(:,ones(1,n));
	t = [t; zeros(nk,nk)];
	t = t(:);
	t((end-nk+1):end) = [];
	mat = reshape(t, 2*nk-1,nk);
	mat = mat(1:nk,:);

function [mat, nexp] = filtmat1(kern, n, flag_expand)
%	1d matrix that does linear filtering - odd kernel length.
%	flag_expand means output has more entries than input (n+nk-1)

	nk = length(kern);
	ic = (nk+1)/2;		% center index
	if round(ic) ~= ic,	error('odd kernel only'), end

	if flag_expand
		n = n + nk-1;
	end
	nexp = n;

	mat = diag(ones(n,1)*kern(ic));
	for iv=1:(ic-1)
		mat = mat + diag(ones(n-iv,1) * kern(ic-iv), -iv) ...
			  + diag(ones(n-iv,1) * kern(ic+iv), iv);
	end

	if flag_expand
		mat = mat(:,ic:(end-ic+1));
	end
	mat = sparse(mat);

function mat = filtmat1per(kern, nn)
%	1d matrix that does periodic linear filtering.  odd or even kernel ok.

	nk = length(kern);
	ic = (nk+1)/2;

	mat = spdiag(ones(nn,1)*kern(ic));
	for kk=1:(ic-1)
		mat = mat ...
			+ diag(ones(nn-kk,1) * kern(ic-kk),	-kk) ...
			+ diag(ones(kk   ,1) * kern(ic-kk),	nn-kk);
	end

	for kk=1:floor(nk/2)
		mat = mat ...
			+ diag(ones(nn-kk,1) * kern(ic+kk),	kk) ...
			+ diag(ones(kk   ,1) * kern(ic+kk),	kk-nn);
	end
	mat = sparse(mat);