classdef AeroAnglesPolySteeringModel < AbstractAnglePolySteeringModel
    %AeroAnglesPolySteeringModel Summary of this class goes here
    %   Detailed explanation goes here

    properties
        bankModel(1,1) PolynominalModel = PolynominalModel(0,0,0,0);
        aoAModel(1,1) PolynominalModel = PolynominalModel(0,0,0,0);
        slipModel(1,1) PolynominalModel = PolynominalModel(0,0,0,0);

        bankContinuity(1,1) logical = true;
        aoAContinuity(1,1) logical = true;
        slipContinuity(1,1) logical = true;
    end

    methods
        function R_body_2_inertial = getBody2InertialDcmAtTime(obj, ut, rVect, vVect, bodyInfo)
            bankAng = obj.bankModel.getValueAtTime(ut);
            angOfAttack = obj.aoAModel.getValueAtTime(ut);
            angOfSideslip = obj.slipModel.getValueAtTime(ut);

%             [~, ~, ~, dcm] = computeBodyAxesFromAeroAngles(ut, rVect, vVect, bodyInfo, angOfAttack, angOfSideslip, bankAng);
            baseFrame = bodyInfo.getBodyFixedFrame();
            [~, ~, ~, R_body_2_inertial] = computeInertialBodyAxesFromFrameAeroAngles(ut, rVect, vVect, bodyInfo, bankAng, angOfAttack, angOfSideslip, baseFrame);
        end

        function [angleModel, continuity] = getAngleNModel(obj, n)
            angleModel = PolynominalModel.empty(1,0);

            switch n
                case 1
                    angleModel = obj.bankModel;
                    continuity = obj.bankContinuity;
                case 2
                    angleModel = obj.aoAModel;
                    continuity = obj.aoAContinuity;
                case 3
                    angleModel = obj.slipModel;
                    continuity = obj.slipContinuity;
            end
        end

        function t0 = getT0(obj)
            t0 = obj.slipModel.t0;
        end

        function setT0(obj, newT0)
            obj.bankModel.t0 = newT0;
            obj.aoAModel.t0 = newT0;
            obj.slipModel.t0 = newT0;
        end

        function setConstTerms(obj, bankConst, aoaConst, slipConst)
            obj.bankModel.constTerm = bankConst;
            obj.aoAModel.constTerm = aoaConst;
            obj.slipModel.constTerm = slipConst;
        end

        function setLinearTerms(obj, bank, aoa, slip)
            obj.bankModel.linearTerm = bank;
            obj.aoAModel.linearTerm = aoa;
            obj.slipModel.linearTerm = slip;
        end

        function setAccelTerms(obj, bank, aoa, slip)
            obj.bankModel.accelTerm = bank;
            obj.aoAModel.accelTerm = aoa;
            obj.slipModel.accelTerm = slip;
        end

        function setTimeOffsets(obj, timeOffset)
            obj.bankModel.tOffset = timeOffset;
            obj.aoAModel.tOffset = timeOffset;
            obj.slipModel.tOffset = timeOffset;
        end

        function [angle1Cont, angle2Cont, angle3Cont] = getContinuityTerms(obj)
            angle1Cont = obj.bankContinuity;
            angle2Cont = obj.aoAContinuity;
            angle3Cont = obj.slipContinuity;
        end

        function setContinuityTerms(obj, angle1Cont, angle2Cont, angle3Cont)
            obj.bankContinuity = angle1Cont;
            obj.aoAContinuity = angle2Cont;
            obj.slipContinuity = angle3Cont;
        end

        function setConstsFromDcmAndContinuitySettings(obj, dcm, ut, rVect, vVect, bodyInfo)
            if(obj.bankContinuity || obj.aoAContinuity || obj.slipContinuity)
                [bankAng,angOfAttack,angOfSideslip] = computeAeroAnglesFromBodyAxes(ut, rVect, vVect, bodyInfo, dcm(:,1), dcm(:,2), dcm(:,3));

                if(obj.bankContinuity)
                    obj.bankModel.constTerm = bankAng;
                end

                if(obj.aoAContinuity)
                    obj.aoAModel.constTerm = angOfAttack;
                end

                if(obj.slipContinuity)
                    obj.slipModel.constTerm = angOfSideslip;
                end
            end
        end

        function setInitialAttitudeFromState(obj, stateLogEntry, tOffsetDelta)
            t0 = stateLogEntry.time;
            obj.setT0(t0);

            obj.bankModel.tOffset = obj.bankModel.tOffset + tOffsetDelta;
            obj.aoAModel.tOffset = obj.aoAModel.tOffset + tOffsetDelta;
            obj.slipModel.tOffset = obj.slipModel.tOffset + tOffsetDelta;
        end

        function [angle1Name, angle2Name, angle3Name] = getAngleNames(~)
            angle1Name = 'Bank Angle';
            angle2Name = 'Angle of Attack';
            angle3Name = 'Side Slip Angle';
        end

        function newSteeringModel = deepCopy(obj)
            newSteeringModel = AeroAnglesPolySteeringModel(obj.bankModel.deepCopy(), obj.aoAModel.deepCopy(), obj.slipModel.deepCopy());
            newSteeringModel.bankContinuity = obj.bankContinuity;
            newSteeringModel.aoAContinuity = obj.aoAContinuity;
            newSteeringModel.slipContinuity = obj.slipContinuity;
        end

        function optVar = getNewOptVar(obj)
            optVar = SetAeroSteeringModelActionOptimVar(obj);
        end

        function optVar = getExistingOptVar(obj)
            optVar = obj.optVar;
        end

        function [addActionTf, steeringModel] = openEditSteeringModelUI(obj, lv, useContinuity)
            output = AppDesignerGUIOutput({false, obj});
            lvd_EditActionSetSteeringModelGUI_App(obj, lv, useContinuity, output);
            addActionTf = output.output{1};
            steeringModel = output.output{2};
        end
    end

    methods(Access=private)
        function obj = AeroAnglesPolySteeringModel(bankModel, aoAModel, slipModel)
            obj.bankModel = bankModel;
            obj.aoAModel = aoAModel;
            obj.slipModel = slipModel;
        end
    end

    methods(Static)
        function model = getDefaultSteeringModel()
            bankModel = PolynominalModel(0,0,0,0);
            aoAModel = PolynominalModel(0,0,0,0);
            slipModel = PolynominalModel(0,0,0,0);

            model = AeroAnglesPolySteeringModel(bankModel, aoAModel, slipModel);
        end

        function typeStr = getTypeNameStr()
            typeStr = SteeringModelEnum.AeroAnglesPoly.nameStr;
        end
    end
end


% LOTKA-VOLTERRA system
% System identification: DelayDMDc

clear all, close all, clc
figpath = '../FIGURES/';
datapath = '../DATA/';
addpath('../utils');

ModelName = 'SparseModels';
%% Generate Data
InputSignalType = 'sphs';%prbs; chirp; noise; sine2; sphs; mixed
Ndelay = 1;%35;%35;
ONLY_TRAINING_LENGTH = 1;
getTrainingData

DataTrain.x = x;
DataTrain.t = t;
DataTrain.tspan = tspan;
DataTrain.u = u;
DataTrain.xmean = xmean;
xstd = std(DataTrain.x(:,1));

%% Parameters
close all
rng(0,'twister')

eta_vec = [0.01 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5];
N_ETA = length(eta_vec);

lambda_vec = [0,1e-5,1e-4,1e-3,1e-2,1e-1,1e-0,1e+1];
N_LAMBDA = length(lambda_vec);

model_collection = {'sDMDc','sEDMDc2', 'sEDMDc3','SINDYc'};
N_MODEL = length(model_collection);

N_REP = 30;

options_method.sparsify = 'ILSTH'; % 'LASSO'
options_method.usesine  = 0;

nVars = 2;
%% Initialization
TrainingError = inf*ones(N_ETA,N_LAMBDA,N_MODEL,N_REP);
ValidationError = inf*ones(N_ETA,N_LAMBDA,N_MODEL,N_REP);

PredictionResults_Training = zeros(1000,nVars,N_ETA,N_LAMBDA,N_MODEL,N_REP);
PredictionResults_Validation = zeros(1000,nVars,N_ETA,N_LAMBDA,N_MODEL,N_REP);

%% Model Identification

for iR = 1:N_REP
    counter = 0;

    tstart = tic;
    for iEta = 1:N_ETA

        % Add noise to data
        eps     = eta_vec(iEta)*xstd;
        x       = DataTrain.x + eps*randn(size(DataTrain.x));
        u       = DataTrain.u;
        M       = length(x);

        for iLambda = 1:N_LAMBDA
            lambda = lambda_vec(iLambda);

            %% Model Identification
            Hx  = getHankelMatrix_MV(x - repmat(DataTrain.xmean,[M 1]),1); % No time delay (=1)
            Hu  = getHankelMatrix_MV(u',1);

            % sparse DMDc
            options_method.order  = 1;
            yout = poolData(Hx',nVars,options_method.order,0);
            Y = yout(1:end-1,2:end)'; % remove constant
            Yp = yout(2:end,2:end)';
            U = Hu(1:end-1);
            sys_DMDc = SparseRegression(Y,Yp,U,dt, options_method, lambda);

            % sparse EDMDc
            options_method.order  = 2;
            yout = poolData(Hx',nVars,options_method.order,0);
            Y = yout(1:end-1,2:end)'; % remove constant
            Yp = yout(2:end,2:end)';
            U = Hu(1:end-1);
            sys_EDMDc2 = SparseRegression(Y,Yp,U,dt, options_method, lambda);

            % sparse EDMDc
            options_method.order  = 3;
            yout = poolData(Hx',nVars,options_method.order,0);
            Y = yout(1:end-1,2:end)'; % remove constant
            Yp = yout(2:end,2:end)';
            U = Hu(1:end-1);
            sys_EDMDc3 = SparseRegression(Y,Yp,U,dt, options_method, lambda);

            % SINDYc (not xmean-substracted)
            options_method.order  = 3;
            Xi = NonlinearSparseRegression(x,u',dt,options_method,lambda);

            %% Prediction on training data
            Nt = M-1;
            % sparse DMDc
            options_method.order  = 1;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xDMDc,~] = lsim(sys_DMDc,Hu,tspan(1:Nt),x0train);
            xDMDc = xDMDc(:,end-1:end);
            xDMDc = xDMDc + repmat(xmean,[Nt 1]);
            xPredTrain{1}.x = xDMDc;

            % sparse EDMDc
            options_method.order  = 2;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xEMD2,~] = lsim(sys_EDMDc2,Hu,tspan(1:Nt),x0train);
            xEMD2 = xEMD2(:,1:2);
            xEMD2 = xEMD2 + repmat(xmean,[Nt 1]);
            xPredTrain{2}.x = xEMD2;

            % sparse EDMDc
            options_method.order  = 3;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xEMD3,~] = lsim(sys_EDMDc3,Hu,tspan(1:Nt),x0train);
            xEMD3 = xEMD3(:,1:2);
            xEMD3 = xEMD3 + repmat(xmean,[Nt 1]);
            xPredTrain{3}.x = xEMD3;

            % SINDYc
            p.ahat = Xi(:,1:2);
            p.polyorder = options_method.order; p.usesine = options_method.usesine; p.dt = dt;
            [Ns,N] = size(Hx);
            xSINDYc = zeros(Ns,N); xSINDYc(:,1) = x(1,:)';
            for ct=1:N-1
                xSINDYc(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xSINDYc(:,ct),DataTrain.u(ct),dt,1,[],p);
            end
            xSINDYc = xSINDYc';
            xPredTrain{4}.x = xSINDYc;

            % Store results
            for iM = 1:N_MODEL
                PredictionResults_Training(:,:,N_ETA,N_LAMBDA,iM,N_REP) = xPredTrain{iM}.x;
            end

            %             clear ph
            %             figure,box on,
            %             ccolors = get(gca,'colororder');
            %             ph(1) = plot(tspan(1:Nt),x(1:Nt,1),'-','Color','k','LineWidth',1); hold on
            %             plot(tspan(1:Nt),x(1:Nt,2),'-','Color','k','LineWidth',1);
            %             for iM = 1:N_MODEL
            %                 for iVar = 1:nVars
            %                     ph(iM+1) = plot(tspan(1:Nt),xPredTrain{iM}.x(:,iVar),'--','Color',ccolors(iM,:),'LineWidth',1); hold on
            %                 end
            %             end
            %             legend(ph,model_collection)

            for iM = 1:N_MODEL
                TrainingError(iEta,iLambda,iM,iR) = norm(x(1:Nt,:)-xPredTrain{iM}.x,'fro');
            end

            %disp(['Training Error: ', num2str(squeeze(TrainingError(iEta,iLambda,:,iR))')])
            %             disp(['                DMDc: ', num2str(DMDc_err)])
            %             disp(['          sparseDMDc: ', num2str(sparseDMDc_err)])
            %% Prediction on validation data
            Nt = M-1;
            % sparse DMDc
            options_method.order  = 1;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xDMDc,~] = lsim(sys_DMDc,Hu,tspan(1:Nt),x0train);
            xDMDc = xDMDc(:,end-1:end);
            xDMDc = xDMDc + repmat(xmean,[Nt 1]);
            xPredTrain{1}.x = xDMDc;

            % sparse EDMDc
            options_method.order  = 2;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xEMD2,~] = lsim(sys_EDMDc2,Hu,tspan(1:Nt),x0train);
            xEMD2 = xEMD2(:,1:2);
            xEMD2 = xEMD2 + repmat(xmean,[Nt 1]);
            xPredTrain{2}.x = xEMD2;

            % sparse EDMDc
            options_method.order  = 3;
            x0train = poolData(Hx(:,1)',nVars,options_method.order,0)';
            x0train = x0train(2:end);
            [xEMD3,~] = lsim(sys_EDMDc3,Hu,tspan(1:Nt),x0train);
            xEMD3 = xEMD3(:,1:2);
            xEMD3 = xEMD3 + repmat(xmean,[Nt 1]);
            xPredTrain{3}.x = xEMD3;

            % SINDYc
            p.ahat = Xi(:,1:2);
            p.polyorder = options_method.order; p.usesine = options_method.usesine; p.dt = dt;
            [Ns,N] = size(Hx);
            xSINDYc = zeros(Ns,N); xSINDYc(:,1) = x(1,:)';
            for ct=1:N-1
                xSINDYc(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xSINDYc(:,ct),DataTrain.u(ct),dt,1,[],p);
            end
            xSINDYc = xSINDYc';
            xPredTrain{4}.x = xSINDYc;

            % Store results
            for iM = 1:N_MODEL
                PredictionResults_Validation(:,:,N_ETA,N_LAMBDA,iM,N_REP) = xPredTrain{iM}.x;
            end

            for iM = 1:N_MODEL
                ValidationError(iEta,iLambda,iM,iR) = norm(x(1:Nt,:)-xPredTrain{iM}.x,'fro');
            end

            counter = counter + 1;
        end
    end

    tstop = toc(tstart);
    disp(['STATUS (',num2str(iR),' of ', num2str(N_REP),'): ', num2str(100*counter/(N_ETA*N_LAMBDA)),'%', '[Time=',num2str(tstop),'s]'])
end

%% STATS
idxnan = isnan(TrainingError(:));
maxerr = max(TrainingError(:));
TrainingError(idxnan) = maxerr; % change later
ErrStats.mean = zeros(N_ETA,N_LAMBDA,N_MODEL);
ErrStats.median = zeros(N_ETA,N_LAMBDA,N_MODEL);
ErrStats.std = zeros(N_ETA,N_LAMBDA,N_MODEL);
ErrStats.min = zeros(N_ETA,N_LAMBDA,N_MODEL);
for iEta = 1:N_ETA
    for iLambda = 1:N_LAMBDA
        for iModel = 1:N_MODEL
            ErrStats.mean(iEta,iLambda,iModel) = mean(TrainingError(iEta,iLambda,iModel,:));
            ErrStats.median(iEta,iLambda,iModel) = median(TrainingError(iEta,iLambda,iModel,:));
            ErrStats.std(iEta,iLambda,iModel) = std(TrainingError(iEta,iLambda,iModel,:));
            ErrStats.min(iEta,iLambda,iModel) = min(TrainingError(iEta,iLambda,iModel,:));
        end
    end
end

%%
err_axis = [min(TrainingError(:)), 1e3];%max(TrainingError(:))
figure,
for iModel = 1:N_MODEL
    subplot(4,4,(iModel-1)*4+1)
    surf(lambda_vec,eta_vec,ErrStats.median(:,:,iModel)), shading interp
    xlabel('\lambda'), ylabel('\eta'), zlabel('median')
    %     zlim([0 1e6])
    caxis(err_axis);
    set(gca,'xscale','log')
    view(2)

    subplot(4,4,(iModel-1)*4+2)
    surf(lambda_vec,eta_vec,ErrStats.mean(:,:,iModel)) , shading interp
    xlabel('\lambda'), ylabel('\eta'), zlabel('mean')
    %     zlim([0 1e6])
    caxis(err_axis);
    set(gca,'xscale','log')
    view(2)

    subplot(4,4,(iModel-1)*4+3)
    surf(lambda_vec,eta_vec,ErrStats.std(:,:,iModel)) , shading interp
    xlabel('\lambda'), ylabel('\eta'), zlabel('std')
    %     zlim([0 1e6])
    caxis(err_axis);
    set(gca,'xscale','log')
    view(2)

    subplot(4,4,(iModel-1)*4+4)
    surf(lambda_vec,eta_vec,ErrStats.min(:,:,iModel)) , shading interp
    xlabel('\lambda'), ylabel('\eta'), zlabel('min')
    %     zlim([0 1e6])
    caxis(err_axis);
    set(gca,'xscale','log')
    view(2)
end
% for iEta = 1:N_ETA
%     for iLambda = 1:N_LAMBDA
%         plot3()
%     end
% end

%% TIme series stats
iEta = 1; iLambda = 1; iM = 4;
data = squeeze(PredictionResults_Training(:,:,iEta,iLambda,iM,N_REP));

xBmin = min(data(:,1:nVars,:),[],3);
xBmax = max(data(:,1:nVars,:),[],3);
clear ph
figure('visible','off'),box on, hold on,
ccolors = get(gca,'colororder');
plot([tB(1),tB(1)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
plot([t(end),t(end)],[-25 65],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
ylim([-25 65])
t1 = text(5,55,'Training', 'FontSize',12);
t2 = text(5+tA(1),55,'Validation', 'FontSize',12);

X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
Y=[xBmin(:,1)',flipud(xBmax(:,1))'];              %#create y values for out and then back
fillh1 = fill(X,Y,ccolors(1,:)-[0 0.2 0.2]);                  %#plot filled area
fillh1.EdgeColor = ccolors(1,:)-[0 0.2 0.2]; fillh1.FaceAlpha = 0.5;

X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
Y=[xBmin(:,2)',flipud(xBmax(:,2))'];              %#create y values for out and then back
fillh = fill(X,Y,ccolors(2,:)-[0.1 0.2 0.09]);                  %#plot filled area
fillh.EdgeColor = ccolors(2,:)-[0.1 0.2 0.09]; fillh.FaceAlpha = 0.5;

X=[tB(2:end)',fliplr(tB(2:end)')];                %#create continuous x value array for plotting
Y=[xBmin(:,3)',flipud(xBmax(:,3))'];              %#create y values for out and then back
fillh = fill(X,Y,ccolors(3,:)-[0.1 0.2 0.09]);                  %#plot filled area
fillh.EdgeColor = ccolors(3,:)-[0.1 0.2 0.09]; fillh.FaceAlpha = 0.5;

%             plot(tB(2:end),xBmin(:,1),'-k','LineWidth',2)
%             plot(tB(2:end),xBmin(:,1),'--g','LineWidth',2)

if eps~=0
    ph(4) = plot(tspan,x(:,1),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(1,:)+[0.15 0.3 0.25]
    plot(tspan,x(:,2),'-','Color',0.7*ones(1,3),'LineWidth',1); %ccolors(2,:)+[0.15 0.3 0.25]
    plot(tspan,x(:,3),'-','Color',0.7*ones(1,3),'LineWidth',1);
    ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',1);
    ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',1);
    ph(3) = plot([DataTrain.t;tA],[DataTrain.x(:,3);xA(:,3)],'-','Color',ccolors(3,:),'LineWidth',1);
else
    ph(1) = plot([DataTrain.t;tA],[DataTrain.x(:,1);xA(:,1)],'-','Color',ccolors(1,:),'LineWidth',1);
    ph(2) = plot([DataTrain.t;tA],[DataTrain.x(:,2);xA(:,2)],'-','Color',ccolors(2,:),'LineWidth',1);
    ph(3) = plot([DataTrain.t;tA],[DataTrain.x(:,3);xA(:,3)],'-','Color',ccolors(3,:),'LineWidth',1);
    ph(4) = plot(t,x(:,1),'--','Color',[0 1 0],'LineWidth',1); % Training data
    plot(t,x(:,2),'--','Color',[0 1 0],'LineWidth',1);
    plot(t,x(:,3),'--','Color',[0 1 0],'LineWidth',1);
end

%             ph(4) = plot(tB,xB(:,1),'-.','Color',ccolors(1,:)-[0 0.2 0.2],'LineWidth',2);
%             ph(5) = plot(tB,xB(:,2),'-.','Color',ccolors(2,:)-[0.1 0.2 0.09],'LineWidth',2);
grid off
xlim([0 tv(end)])
xlabel('Time')
ylabel('xi')
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto');

if eps~=0
    lh = legend([ph([1,4]),fillh1],'Truth','Training',ModelName,'Location','NorthWest');
    %             lh.Position = [lh.Position(1)+0.13,lh.Position(2)-0.2,lh.Position(3:4)];
    lh.Position = [lh.Position(1)+0.02,lh.Position(2)-0.06,lh.Position(3:4)];
else
    lh = legend(ph([1,4]),'Truth','Training',ModelName,'Location','NorthWest');
    %             lh.Position = [lh.Position(1)+0.13,lh.Position(2)-0.2,lh.Position(3:4)];
    lh.Position = [lh.Position(1)+0.02,lh.Position(2)-0.06,lh.Position(3:4)];
end

print('-depsc2', '-painters','-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_STATS.eps']);

delete(lh), delete(t1), delete(t2)
%             print('-depsc2', '-painters','-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_Validation_N',sprintf('%04g',Ntrain_vec(iN)),'_Eta',sprintf('%03g',100*eta_vec(iNoise)),'_STATS_noleg.eps']);

return
%% Show validation
clear ph
figure,box on,
ccolors = get(gca,'colororder');
ph(1) = plot(tspan,x(:,1),'-','Color','k','LineWidth',1); hold on
ph(2) = plot(tspan,x(:,2),'-','Color','k','LineWidth',1);
ph(3) = plot(tspan(Ndelay:Nt+Ndelay-1),xDMDc(:,1),'--','Color','r','LineWidth',2);
ph(4) = plot(tspan(Ndelay:Nt+Ndelay-1),xDMDc(:,2),'--','Color','r','LineWidth',2);
ph(5) = plot(tspan(Ndelay:Nt+Ndelay-1),xDMDc_sparse(:,1),':','Color','b','LineWidth',2);
ph(6) = plot(tspan(Ndelay:Nt+Ndelay-1),xDMDc_sparse(:,2),':','Color','b','LineWidth',2);
xlim([0 100]), ylim([0,120]);
xlabel('Time')
ylabel('Population size')
% legend('Prey (True)','Predator (True)', 'Prey (DMDc)','Predator (DMDc)')
legend(ph([1,3,5]),'True','DMDc',ModelName)
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose', '-cmyk', [figpath,'EX_LOTKA_SI_',ModelName,'_',InputSignalType,'.eps']);

DMDc_err = norm(x(1:end-1,:)-xDMDc,'fro');
sparseDMDc_err = norm(x(1:end-1,:)-xDMDc_sparse,'fro');
disp(['Training Error: '])
disp(['                DMDc: ', num2str(DMDc_err)])
disp(['          sparseDMDc: ', num2str(sparseDMDc_err)])

%% Prediction
% Reference
tspanV   = [100:dt:200];
xA      = xv;
tA      = tv;

% Model DMDc
x0      = [x(end,1:2)];
Hunew   = [u(end),uv(1:end)];
[xB,tB] = lsim(sysmodel_DMDc,Hunew,tspanV,[x0-[xmean]]');
xB = xB + repmat(xmean,[length(tB) 1]);

% Model sparseDMDc / sparseEDMDc
if Nstates == 2
    [xB_sparse,tB] = lsim(sysmodel_sparseDMDc,Hunew,tspanV,[x0-[xmean]]');
    xB_sparse = xB_sparse(:,end-1:end);
    xB_sparse = xB_sparse + repmat(xmean,[length(tB) 1]);
elseif Nstates > 2
    x0dmdc = [x0(1)-xmean(1); x0(2)-xmean(2); (x0(1)-xmean(1)).^2; (x0(1)-xmean(1)).*(x0(2)-xmean(2)); (x0(2)-xmean(2)).^2];
    [xB_sparse,tB] = lsim(sysmodel_sparseDMDc,Hunew,tspanV,x0dmdc);
    xB_sparse = xB_sparse(:,1:2);
    xB_sparse = xB_sparse + repmat(xmean,[length(tB) 1]);
end

DMDc_err = norm(xA-xB(1:end-1,:),2);
sparseDMDc_err = norm(xA-xB_sparse(1:end-1,:),2);
disp(['Validation Error: '])
disp(['                DMDc: ', num2str(DMDc_err)])
disp(['          sparseDMDc: ', num2str(sparseDMDc_err)])
%% Show training and prediction
VIZ_SI_Validation

%% Save Data
Model.name = 'sparseDMDc';
Model.sys = sysmodel_DMDc;
Model.Ndelay = Ndelay;
Model.dt = dt;
save(fullfile(datapath,['EX_LOTKA_SI_',ModelName,'_',InputSignalType,'.mat']),'Model')


function []=olsfeval(data_endo_c,stringdates3,Fstartdate,Fcenddate,Fcperiods,Fcomp,n,forecast_estimates,names,endo,pref)

% function []=olsfeval(data_endo_c,stringdates3,Fstartdate,Fcenddate,Fcperiods,Fcomp,n,forecast_estimates,names,endo,datapath)
% calculates, display and saves forecast evaluation results for a OLS VAR model
% inputs:  - matrix 'data_endo_c': matrix of endogenous data for the forecast evaluation period (i.e. period for which forecast is estimated and actual data exists)
%          - cell 'stringdates3': date strings for the forecast evaluation period (i.e. period for which forecast is estimated and actual data exists)
%          - string 'Fstartdate': start date of the forecasts
%          - string 'Fcenddate': end date of the forecat evaluation (i.e. period for which forecast is estimated and actual data exists)
%          - integer 'Fcperiods': number of periods for which forecast evaluation can be conducted (i.e.for which forecast is estimated and actual data exists)
%          - integer 'Fcperiods': number of periods for which forecast evaluation can be conducted (i.e.for which forecast is estimated and actual data exists)
%          - integer 'Fcomp': 0-1 value indicating if forecast evaluation is possible
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - cell 'forecast_estimates': lower bound, point estimates, and upper bound for the unconditional forecasts
%          - cell 'names': cell containing the excel spreadsheet labels (names and dates)
%          - cell 'endo': list of endogenous variables of the model
%          - string 'datapath': user-supplied path to excel data spreadsheet
% outputs: none

% first, note that forecast evaluation can only be conducted if there is some observable data after the beginning of the forecast
if Fcomp==1

   % preliminary task: obtain a matrix of forecasts over the common periods
   for ii=1:n
   forecast_c(:,ii)=forecast_estimates{ii,1}(2,1:Fcperiods)';
   end
   % then compute the matrix of forecast errors
   ferrors=data_endo_c-forecast_c;

   % compute first the sequential RMSE, defined in (a.8.11)

   % square the forecast error matrix entrywise
   sferrors=ferrors.^2;
   % sum entries sequentially
   sumsferrors=sferrors(1,:);
   for ii=2:Fcperiods
   sumsferrors(ii,:)=sumsferrors(ii-1,:)+sferrors(ii,:);
   end
   % divide by the number of forecast periods and take square roots to obtain RMSE
   for ii=1:Fcperiods
   RMSE(ii,:)=((1/ii)*sumsferrors(ii,:)).^0.5;
   end

   % compute then the sequential MAE, defined in (a.8.12)

   % take the absolute value of the forecast error matrix
   absferrors=abs(ferrors);
   % sum entries sequentially
   sumabsferrors=absferrors(1,:);
   for ii=2:Fcperiods
   sumabsferrors(ii,:)=sumabsferrors(ii-1,:)+absferrors(ii,:);
   end
   % divide by the number of forecast periods to obtain MAE
   for ii=1:Fcperiods
   MAE(ii,:)=(1/ii)*sumabsferrors(ii,:);
   end

   % compute the sequential MAPE, defined in (a.8.13)

   % divide entrywise by actual values and take absolute values
   absratioferrors=abs(ferrors./data_endo_c);
   % sum entries sequentially
   sumabsratioferrors=absratioferrors(1,:);
   for ii=2:Fcperiods
   sumabsratioferrors(ii,:)=sumabsratioferrors(ii-1,:)+absratioferrors(ii,:);
   end
   % divide by 100*(number of forecast periods) to obtain MAPE
   for ii=1:Fcperiods
   MAPE(ii,:)=(100/ii)*sumabsratioferrors(ii,:);
   end

   % compute the Theil's inequality coefficient, defined in (a.8.14)

   % first compute the left term of the denominator
   % square entrywise the matrix of actual data
   sendo=data_endo_c.^2;
   % sum entries sequentially
   sumsendo=sendo(1,:);
   for ii=2:Fcperiods
   sumsendo(ii,:)=sumsendo(ii-1,:)+sendo(ii,:);
   end
   % divide by the number of forecast periods and take square roots
   for ii=1:Fcperiods
   leftterm(ii,:)=((1/ii)*sumsendo(ii,:)).^0.5;
   end
   % then compute the right term of the denominator
   % square entrywise the matrix of forecast values
   sforecasts=forecast_c.^2;
   % sum entries sequentially
   sumsforecasts=sforecasts(1,:);
   for ii=2:Fcperiods
   sumsforecasts(ii,:)=sumsforecasts(ii-1,:)+sforecasts(ii,:);
   end
   % divide by the number of forecast periods and take square roots
   for ii=1:Fcperiods
   rightterm(ii,:)=((1/ii)*sumsforecasts(ii,:)).^0.5;
   end
   % finally, compute the U stats
   Ustat=RMSE./(leftterm+rightterm);

% if forecast evaluation is not possible, do not do anything
elseif Fcomp==0
end

% now, print the results and display them

filelocation=fullfile(pref.results_path, [pref.results_sub '.txt']);
fid=fopen(filelocation,'at');

fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');
fprintf('%s\n','');
fprintf(fid,'%s\n','');

Fevalinfo='Forecast evaluation:';
fprintf('%s\n',Fevalinfo);
fprintf(fid,'%s\n',Fevalinfo);

fprintf('%s\n','');
fprintf(fid,'%s\n','');

% if forecast evaluation is not possible, return a message to signal it

if Fcomp==0

finfo1=['Forecast evaluation cannot be conducted.'];
fprintf('%s\n',finfo1);
fprintf(fid,'%s\n',finfo1);
finfo2=['Forecasts start in ' Fstartdate ', while observable data is available only until ' names{end,1} '.'];
fprintf('%s\n',finfo2);
fprintf(fid,'%s\n',finfo2);
finfo3=['To obtain forecast evaluation, the forecast start date must be anterior to the end of the data set.'];
fprintf('%s\n',finfo3);
fprintf(fid,'%s\n',finfo3);

% if forecast evaluation is possible, display the results
elseif Fcomp==1

finfo1=['Evaluation conducted over ' num2str(Fcperiods) ' periods (from ' Fstartdate ' to ' Fcenddate ').'];
fprintf('%s\n',finfo1);
fprintf(fid,'%s\n',finfo1);

   % loop over endogenous variables
   for ii=1:n

   fprintf('%s\n','');
   fprintf(fid,'%s\n','');

   endoinfo=['Endogenous: ' endo{ii,1}];
   fprintf('%s\n',endoinfo);
   fprintf(fid,'%s\n',endoinfo);

   temp='fprintf(''%12s';
      for jj=1:Fcperiods-1
      temp=[temp ' %10s'];
      end
   temp=[temp ' %10s\n'','''''];
      for jj=1:Fcperiods
      temp=[temp ',''' stringdates3{jj,1} ''''];
      end
   temp=[temp ');'];
   eval(temp);
   temp='fprintf(fid,''%12s';
      for jj=1:Fcperiods-1
      temp=[temp ' %10s'];
      end
   temp=[temp ' %10s\n'','''''];
      for jj=1:Fcperiods
      temp=[temp ',''' stringdates3{jj,1} ''''];
      end
   temp=[temp ');'];
   eval(temp);

   label='RMSE:       ';
   values=RMSE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='MAE:        ';
   values=MAE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='MAPE:       ';
   values=MAPE(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

   label='Theil''s U:  ';
   values=Ustat(1:Fcperiods,ii)';
   temp='fprintf(''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);
   temp='fprintf(fid,''%12s';
   for jj=1:Fcperiods-1
   temp=[temp ' %10.3f'];
   end
   temp=[temp ' %10.3f\n'''];
   temp=[temp ',label,values);'];
   eval(temp);

    end

end

fclose(fid);


function r8_sind_test ( )

%*****************************************************************************80
%
%% R8_SIND_TEST tests R8_SIND.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 January 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8_SIND_TEST\n' );
  fprintf ( 1, '  R8_SIND computes the sine of an angle\n' );
  fprintf ( 1, '  given in degrees.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ANGLE    R8_SIND(ANGLE)\n' );
  fprintf ( 1, '\n' );

  for i = 0 : 15 : 360
    angle = i;
    fprintf ( 1, '  %8.2f  %14.6g\n', angle, r8_sind ( angle ) );
  end

  return
end


function f = fint(x)
%fint   subdivision function
%   f = fint(x);
%   called by fitint
%   IFISS function: DJS; 28 February 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      global global_N global_INTL global_LASTDL
      f=(global_INTL*(x-1.0)-global_LASTDL)*(x^global_N) + global_LASTDL;
%     fprintf('\n %10.4e   %10.4e',x,f)
      return


function plotbyfactor(x,y,f)
%
% very simplistic version of plotbyfactor.
% no symbols, will crash if more than 8 levels.
%

z = unique(f);
cols = ['b','r','g','m','c','y','k','w'];

for (i = 1:length(z))
  u = find(f==z(i));
  plot(x(u),y(u),'.','color',cols(i));
  hold on;
end;
hold off;

return;


% Attenuation Compensation Using Time Reversal Example
%
% This example demonstrates how the acoustic attenuation present in
% photoacoustic forward problem can be compensated for using time reversal
% image reconstruction. It builds on the 2D Time Reversal Reconstruction
% For A Circular Sensor Example.
%
% For a more detailed discussion of this example and the underlying
% techniques, see B. E. Treeby, E. Z. Zhang, and B. T. Cox, "Photoacoustic
% tomography in absorbing acoustic media using time reversal," Inverse
% Problems, vol. 26, no. 11, p. 115003, 2010.
%
% author: Bradley Treeby
% date: 6th September 2010
% last update: 25th August 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

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

clear all;

% =========================================================================
% FORWARD SIMULATION
% =========================================================================

% define the size of the simulation grid and the PML
simulation_size = 512;              % [grid points]
PML_size = 20;                      % [grid points]
x = 52e-3;                          % [m]
y = x;                              % [m]

% reduce the number of grid points in Nx and Ny by the size of the PML so
% that using 'PMLInside' set to false will still give the correct
% simulation size
Nx = simulation_size - 2*PML_size;  % [grid points]
Ny = Nx;                            % [grid points]
dx = x/Nx;                          % [m]
dy = dx;                            % [m]

% create the computational grid
kgrid = makeGrid(Nx, dx, Ny, dy);

% define the properties of the propagation medium
medium_non_absorbing.sound_speed = 1510;	% [m/s]

% create a duplicate of the propagation medium structure and append the
% absorption properties
medium = medium_non_absorbing;
medium.alpha_power = 1.5;
medium.alpha_coeff = 3;              % [dB/(MHz^y cm)]

% store maximum supported frequency
f_max = kgrid.k_max*medium.sound_speed/(2*pi);

% load the shepp logan phantom (note if the simulation or PML sizes are
% changed, the loaded data will need to be resized)
load EXAMPLE_shepp_logan

% smooth the phantom and assign it to the initial pressure
shepp_logan = smooth(kgrid, shepp_logan, true);
source.p0 = shepp_logan;

% define a circular Cartesian sensor mask
sensor_radius = 25e-3;              % [m]
sensor_points = 200;
cart_sensor_mask = makeCartCircle(sensor_radius, sensor_points);
sensor.mask = cart_sensor_mask;

% create the time array used for the simulation, with t_max defined using
% Huygens' principle to avoid artifact trapping in the reconstruction
t_max = 2*sensor_radius/medium.sound_speed;
[kgrid.t_array, dt] = makeTime(kgrid, medium.sound_speed, [], t_max);

% set the input options, switching off the smoothing (the input has already
% been smoothed), setting the PML to be outside the defined grid, casting
% to 'single' to speed up the example, and switching off visualisation
input_args = {'Smooth', false, 'PMLInside', false, 'PMLSize', PML_size, 'DataCast', 'single', 'PlotSim', false};

% run the forward simulation
sensor_data = kspaceFirstOrder2D(kgrid, medium, source, sensor, input_args{:});

% add noise to the recorded sensor data
signal_to_noise_ratio = 40;         % [dB]
sensor_data = addNoise(sensor_data, signal_to_noise_ratio, 'peak');

% =========================================================================
% IMAGE RECONSTRUCTION WITHOUT ABSORPTION COMPENSATION
% =========================================================================

% create a second computation grid for the reconstruction to avoid the
% inverse crime
PML_size = 25;                      % [grid points]
Nx = simulation_size - 2*PML_size;  % [grid points]
Ny = Nx;                            % [grid points]
dx = x/Nx;                          % [m]
dy = dx;                            % [m]
kgrid_recon = makeGrid(Nx, dx, Ny, dy);

% attach the original time array
kgrid_recon.t_array = kgrid.t_array;

% remove the initial pressure field from the source structure
source = rmfield(source, 'p0');

% create a continuous binary sensor mask with the same radius as the
% Cartesian sensor mask used in the forward simulation
pixel_radius = round(sensor_radius/kgrid_recon.dx);
binary_sensor_mask = makeCircle(kgrid_recon.Nx, kgrid_recon.Ny, floor(kgrid_recon.Nx/2) + 1, floor(kgrid_recon.Ny/2) + 1, pixel_radius);

% assign the sensor mask to the sensor structure
sensor.mask = binary_sensor_mask;

% interpolate the simulated sensor data onto the continuous binary sensor
% mask to remove any gaps and assign to the time reversal field
sensor.time_reversal_boundary_data = interpCartData(kgrid_recon, sensor_data, cart_sensor_mask, binary_sensor_mask, 'linear');

% re-assign the input options (the PML_size has changed)
input_args = {'Smooth', false, 'PMLInside', false, 'PMLSize', PML_size, 'DataCast', 'single', 'PlotSim', false};

% run the time-reversal reconstruction using the non absorbing medium
p0_recon = kspaceFirstOrder2D(kgrid_recon, medium_non_absorbing, source, sensor, input_args{:});

% =========================================================================
% CHOOSING THE CUTOFF FREQUENCY
% =========================================================================

% get the average frequency spectrum of the simulated sensor data
num_signals = length(sensor_data(:,1));
[as_f, as] = spect(sensor_data(1, :), 1/dt);
for index = 2:num_signals
    [as_f, sp] = spect(sensor_data(index, :), 1/dt);
    as = as + sp;
end
as = as/num_signals;

% compute the relative power spectrum
ps = log10(as.^2);
offset = max(ps(:));
ps = ps - offset;

% get the frequency spectrum of a single measurement
as_sing = spect(sensor_data(1,:), 1/dt);
ps_sing = log10(as_sing.^2) - offset;

% scale the frequency variable
[f_sc, scale, prefix] = scaleSI(as_f(end));

% define the cutoff frequency for the filter
f_cutoff = 3e6;

% =========================================================================
% IMAGE RECONSTRUCTION WITH ABSORPTION COMPENSATION
% =========================================================================

% create the filter to regularise the absorption parameters
medium.alpha_filter = getAlphaFilter(kgrid_recon, medium, f_cutoff);

% reverse the sign of the absorption proportionality coefficient
medium.alpha_sign = [-1, 1];        % [absorption, dispersion];

% run the time-reversal reconstruction
p0_recon_compensated = kspaceFirstOrder2D(kgrid_recon, medium, source, sensor, input_args{:});

% =========================================================================
% VISUALISATION
% =========================================================================

% plot the amplitude spectrum
figure;
plot(as_f*scale, ps_sing, 'r-');
ylabel('Power Spectrum [dB]');
xlabel(['Frequency [' prefix 'Hz]']);
hold on;
plot(as_f*scale, ps, 'k-');
ylim = [-8 0];
plot([f_cutoff*scale f_cutoff*scale], [ylim(1) ylim(2)], 'k--');
plot([f_max*scale f_max*scale], [ylim(1) ylim(2)], 'k--');
set(gca, 'XLim', [0 15], 'Ylim', ylim);

% plot the filter, reducing the number of plotting points by a factor of 10
figure;
mesh(medium.alpha_filter(1:10:end, 1:10:end), 'EdgeColor', 'black');
axis tight;
view([-29, 46]);

% set the pressure outside the sensor mask to be zero
binary_sensor_map = makeDisc(kgrid_recon.Nx, kgrid_recon.Ny, kgrid_recon.Nx/2, kgrid_recon.Ny/2, pixel_radius - 50);
p0_recon(binary_sensor_map ~= 1) = 0;
p0_recon_compensated(binary_sensor_map ~= 1) = 0;

% shrink the data sets
phantom_padding = 50;
shepp_logan = shepp_logan(1+ phantom_padding:end-phantom_padding, 1+ phantom_padding:end-phantom_padding);
p0_recon = p0_recon(1+ phantom_padding:end-phantom_padding, 1+ phantom_padding:end-phantom_padding);
p0_recon_compensated = p0_recon_compensated(1+ phantom_padding:end-phantom_padding, 1+ phantom_padding:end-phantom_padding);

% plot the initial pressure distribution
figure;
subplot(2, 2, 1), imagesc(kgrid_recon.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, kgrid_recon.x_vec(1+ phantom_padding:end-phantom_padding)*1e3, shepp_logan, [0 1]);
ylabel('x-position [mm]');
xlabel('y-position [mm]');
axis image;
colormap(flipud(gray));

% plot the reconstruction without attenuation compensation
subplot(2, 2, 2), imagesc(kgrid_recon.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, kgrid_recon.x_vec(1+ phantom_padding:end-phantom_padding)*1e3, p0_recon, [0 1]);
ylabel('x-position [mm]');
xlabel('y-position [mm]');
axis image;
colormap(flipud(gray));

% plot the reconstruction with attenuation compensation
subplot(2, 2, 3), imagesc(kgrid_recon.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, kgrid_recon.x_vec(1+ phantom_padding:end-phantom_padding)*1e3, p0_recon_compensated, [0 1]);
ylabel('x-position [mm]');
xlabel('y-position [mm]');
axis image;
colormap(flipud(gray));

% plot a reconstruction profile
figure;
plot(kgrid.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, shepp_logan(end/2,:), 'k');
hold on;
plot(kgrid_recon.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, p0_recon(end/2,:), 'r-');
plot(kgrid_recon.y_vec(1+ phantom_padding:end-phantom_padding)*1e3, p0_recon_compensated(end/2, :), 'b-');
set(gca, 'XLim', [-20, 20], 'YLim', [-0.1 1.1]);
ylabel('Pressure Magnitude [au]');
xlabel('y-position [mm]');
legend('Original', 'No Compensation', 'With Attenuation Compensation', 'Location', 'Best');


function varargout = colsubset(varargin)
% VL_COLSUBSET Select a given number of columns
%   Y = VL_COLSUBSET(X, N) returns a random subset Y of N columns of
%   X. The selection is order-preserving and without replacement. If N
%   is larger or equal to the number of columns of X (e.g. N = Inf),
%   then the function returns all the columns (i.e., Y = X).
%
%   If 0 < N < 1, then the function returns a fraction N of the
%   columns (rounded to the closest integer).
%
%   [Y, SEL] = VL_COLSUBSET(...) returns the indexes SEL of the
%   selected columns.
%
%   The function accepts the following options:
%
%   Beginning::
%     Returns the fist N columns.
%
%   Ending::
%     Returns the last N columns.
%
%   Random:: [default]
%     Returns N columns selected at random (using RANDPERM()).
%
%   Uniform::
%     Returns N uniformly spaced columns.
%
%   Largest::
%     Returns the N largest columns (using SORTROWS()).
%
%   Smallest::
%     Returns the N smallest columns (using SORTROWS()).
%
%  See also: VL_HELP().
[varargout{1:nargout}] = vl_colsubset(varargin{:});


function plotFlow(u, v, imgOriginal, rSize, scale)
% Creates a quiver plot that displays the optical flow vectors on the
% original first frame (if provided). See the MATLAB Function Reference for
% "quiver" for more info.
%
% Usage:
% plotFlow(u, v, imgOriginal, rSize, scale)
%
% u and v are the horizontal and vertical optical flow vectors,
% respectively. imgOriginal, if supplied, is the first frame on which the
% flow vectors would be plotted. use an empty matrix '[]' for no image.
% rSize is the size of the region in which one vector is visible. scale
% over-rules the auto scaling.
%
% Author: Mohd Kharbat at Cranfield Defence and Security
% mkharbat(at)ieee(dot)org , http://mohd.kharbat.com
% Published under a Creative Commons Attribution-Non-Commercial-Share Alike
% 3.0 Unported Licence http://creativecommons.org/licenses/by-nc-sa/3.0/
%
% October 2008
% Rev: Jan 2009

figure();

if nargin>2
    if sum(sum(imgOriginal))~=0
        imshow(imgOriginal,[0 255]);
        hold on;
    end
end
if nargin<4
    rSize=5;
end
if nargin<5
    scale=3;
end

% Enhance the quiver plot visually by showing one vector per region
for i=1:size(u,1)
    for j=1:size(u,2)
        if floor(i/rSize)~=i/rSize || floor(j/rSize)~=j/rSize
            u(i,j)=0;
            v(i,j)=0;
        end
    end
end
quiver(u, v, scale, 'color', 'b', 'linewidth', 2);
set(gca,'YDir','reverse');


function [feat,idxs_bbox_pair,cb1,cb2,o] = get_spatial_features_same_img(locs,idxs_bbox_pair,scores,class_id,bVis)

if (nargin < 3)
    scores = [];
end

if (nargin < 5)
    bVis = false;
end

if (nargin < 6)
    lab = ones(size(locs,1),1);
end

half_size = 132;

xs = locs(:, 1);
ys = locs(:, 2);
boxes = [xs-half_size, ys-half_size, xs+half_size, ys+half_size];

cb1 = [mean(boxes(idxs_bbox_pair(:,1),[1 3]),2) mean(boxes(idxs_bbox_pair(:,1),[2 4]),2)];
cb2 = [mean(boxes(idxs_bbox_pair(:,2),[1 3]),2) mean(boxes(idxs_bbox_pair(:,2),[2 4]),2)];

h1 = boxes(idxs_bbox_pair(:,1),4) - boxes(idxs_bbox_pair(:,1),2);
h2 = boxes(idxs_bbox_pair(:,2),4) - boxes(idxs_bbox_pair(:,2),2);
assert(any(h1>0));
assert(any(h2>0));

h_m = (h1+h2)/2;
offset = abs(cb1 - cb2)./[h_m h_m];
sc = abs(h1 - h2)./h_m;
offsetM = offset(:,1).*offset(:,2);
[o,o2,o3] = boxoverlapMx(boxes(idxs_bbox_pair(:,1),:), boxes(idxs_bbox_pair(:,2),:));

feat = cat(2, offset, offsetM, sc, o, o2, o3, ...
            offset.^2, offsetM.^2, sc.^2, o.^2, o2.^2, o3.^2, ...
            exp(-offset), exp(-offsetM), exp(-sc), exp(-o), exp(-o2), exp(-o3));
if (~isempty(scores))
    scores1 = scores(idxs_bbox_pair(:,1),:);
    scores2 = scores(idxs_bbox_pair(:,2),:);
    feat = cat(2, feat, scores1, scores2);
end

if (bVis)
    idxs = 1:min(size(cb1,1),100);

    plot(cb1(idxs,1),cb1(idxs,2),'b+','MarkerSize',10);
    plot(cb2(idxs,1),cb2(idxs,2),'g+','MarkerSize',10);

    for i = 1:length(idxs)
        if (lab(idxs(i)) == 1)
            plot([cb1(idxs(i),1); cb2(idxs(i),1)],[cb1(idxs(i),2); cb2(idxs(i),2)],'r-','lineWidth',1);
        else
            plot([cb1(idxs(i),1); cb2(idxs(i),1)],[cb1(idxs(i),2); cb2(idxs(i),2)],'b-','lineWidth',1);
        end
    end

    legendName = {['pidx' num2str(class_id)]};
    legend(legendName);
end


function z = r82_to_c8 ( a, b )

%*****************************************************************************80
%
%% R82_TO_C8 makes a complex number from two R8's.
%
%  Discussion:
%
%    It is extremely unnatural for a programmer to find that
%    "i" is, by default, the imaginary unit in MATLAB.
%
%    So the only safe thing to do, in my opinion, is to hide
%    it somewhere, as in this function!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the real and imaginary parts.
%
%    Output, complex Z, the value of ( A + B * i ).
%
  z = a + b * i;

  return
end


%% istypeof
% Check the type of an object.

%%% Syntax
%  truth = istypeof(obj, typename)

%%% Description
% |istypeof(obj, typename)| returns |true| if |obj| is a scalar or array of the
% type described by a string |typename|. Exmaples of |typename| are |'int'|,
% |'real'|, |'complex'|, |'logical'| (for 'true' and 'false'), and the names of classes.
%
% To test if |obj| is a cell array whose elements are scalars or arrays of a
% specific type, a _cell type_ can be specified as |typename|.  A _cell_type_ is
% a conventional type name followed by 'cell', such as |'intcell'|,
% |'realcell'|, |'complexcell'|.
%
% For |obj == []|, the function returns |true| for any |typename| that is a
% non-cell type. For |obj == {}|, the function returns |true| for any |typename|
% that is a cell type.

%%% Example
%   % Test int.
%   none = []; s = 1; v = 1:10; m = ones(3, 3); type = 'int';
%   fprintf('Test 1: are all elements of the objects %s?  ', type);
%   if istypeof(none, type) && istypeof(s, type) && istypeof(v, type) && istypeof(m, type)
%       fprintf('Yes.\n');
%   else
%       fprintf('No.\n');
%   end
%
%   % Test real.
%   none = []; s = 2.4; v = linspace(1, 10, 17); m = [pi 1; exp(1) 0]; type = 'real';
%   fprintf('Test 2: are all elements of the objects %s?  ', type);
%   if istypeof(none, type) && istypeof(s, type) && istypeof(v, type) && istypeof(m, type)
%       fprintf('Yes.\n');
%   else
%       fprintf('No.\n');
%   end
%
%   % Test Axis.
%   none = []; s = Axis.x; v = Axis.elems; m = [Axis.x Axis.y; Axis.y Axis.z]; type = 'Axis';
%   fprintf('Test 3: are all elements of the objects %s?  ', type);
%   if istypeof(none, type) && istypeof(s, type) && istypeof(v, type) && istypeof(m, type)
%       fprintf('Yes.\n');
%   else
%       fprintf('No.\n');
%   end
%
%   % Test a cell type.
%   none = {}; c = {1, [2 3]; 6, [7 8; 9 10]}; type = 'intcell';
%   fprintf('Test 4: are all objects %s?  ', type);
%   if istypeof(none, type) && istypeof(c, type)
%       fprintf('Yes.\n');
%   else
%       fprintf('No.\n');
%   end

%%% See Also
% <istypesizeof.html istypesizeof>

function truth = istypeof(obj, typename)

chkarg(ischar(typename), '"typename" should be string.');
iscelltype = false;
if ~isempty(strfind(typename, 'cell'))  && isequal(typename(end-3:end), 'cell')  % typename ends with 'cell'
	typename = typename(1:end-4);
	iscelltype = true;
end

if isempty(obj)
	truth = ~xor(iscelltype, iscell(obj));  % true if "typename" is a cell type and obj == {}, or if "typename" is a non-cell type and obj == []
elseif iscelltype
	truth = iscell(obj);
	obj = obj(:);
	for i = 1:length(obj)
		truth = truth && istypeof(obj{i}, typename);
	end
elseif isequal(typename, 'int')
	truth = isnumeric(obj) && isint(obj);  % note isint('abc') == true
elseif isequal(typename, 'real')
	truth = isnumeric(obj) && isreal(obj);  % note isreal('abc') == true
elseif isequal(typename, 'complex')
	truth = isnumeric(obj);
elseif isequal(typename, 'arbitrary')
	truth = true;
else
	truth = isa(obj, typename);
end

function truth = isint(obj)

truth = isreal(obj) && all(mod(obj(:),1)==0);


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

% link silder bar and edit for matrix dimension
% update current matrix according to dimension pointer

function MU_linkSliderEditDim(Temp,Event,active_handle,slider_handle,edit_handle,dimFlag)
handles = guidata(active_handle);

% sync silder bar and edit
switch get(active_handle,'Style')
    case 'slider'
        set(edit_handle,'String',num2str(round(get(slider_handle,'Value'))));
        set(slider_handle,'Value',round(get(slider_handle,'Value')));
    case 'edit'
        editValue = round(str2double(get(edit_handle,'String')));
        if editValue<=get(slider_handle,'Max') & editValue>=get(slider_handle,'Min')
            set(slider_handle,'Value',editValue);
            set(edit_handle,'String',editValue);
        else
            set(edit_handle,'String',num2str(round(get(slider_handle,'Value'))));
        end
end

% update current display matrix
handles=MU_update_image(handles.Matrix_display_axes,{handles.TMatrix,handles.Mask},handles,dimFlag);
guidata(handles.MU_matrix_display, handles);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Angle estimation for each peak in detout
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Resel_agl, vel_ambg_list, rng_excd_list] = angle_estim_dets(detout, ...
    Velocity_FFT, fft_Vel, fft_Ang, Rx, Tx, num_crop)

Resel_agl = [];
vel_ambg_list = [];
rng_excd_list = [];
fft_Rang = size(Velocity_FFT, 2);

for ai = 1:size(detout, 2)
    rx_vect = squeeze(Velocity_FFT(:, detout(2,ai), detout(1,ai)));

    % Phase Compensation on the range-velocity bin for virtual elements
    pha_comp_term = exp(-1i * (pi * (detout(1,ai) - fft_Vel/2 - 1) / fft_Vel));
    rx_vect(Rx+1:Rx*Tx) = rx_vect(Rx+1:Rx*Tx) * pha_comp_term;

    % Estimate Angle on set1
    Angle_FFT1 = fftshift(fft(rx_vect, fft_Ang));
    [MM,II] = max(abs(Angle_FFT1));
    Resel_agl = [Resel_agl, II];

    % Velocity disambiguation on set2 -- flip the sign of the symbols
    % corresponding to Tx2
    rx_vect(Rx+1:Rx*Tx) = - rx_vect(Rx+1:Rx*Tx);
    Angle_FFT1_flip = fftshift(fft(rx_vect, fft_Ang));
    [MM_flip,II_flip] = max(abs(Angle_FFT1_flip));

    if MM_flip > 1.2 * MM
        % now has velocity ambiguration, need to be corrected
        vel_ambg_list = [vel_ambg_list, ai];
    end

    if detout(2,ai) <= num_crop || detout(2,ai) > fft_Rang - num_crop
        rng_excd_list = [rng_excd_list, ai];
    end
end

end


function [g1L, g2L, g3L, g4L] = sdlfmvXsdlfmvKernGradientICBlock(lfmKern1, ...
    lfmKern2, tInit1, tInit2, kyy, kyv, kvy, kvv, i, j, generalConst, ...
    generalConstGrad, gkyy1, gkyy2, gkyy3, gkyy4, gkyv1, gkyv2, ...
    gkyv3, gkyv4, gkvy1, gkvy2, gkvy3, gkvy4, gkvv1, gkvv2, gkvv3, gkvv4)

% SDLFMVXSDLFMVKERNGRADIENTICBLOCK Partial derivatives initial conditions
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

g1L = cell(2);
g2L = cell(2);
g3L = cell(2);
g4L = cell(2);

typeParam = {'sdlfmv', 'sdlfmv'};

% Compute for tInit1(1) and tInit2(2)

[g1L{1,2}, g2L{1,2}, g3L{1,2}] = sdlfmvXsdlfmvKernGradientBlock(lfmKern1, lfmKern2, tInit1(1), ...
    tInit2(2), kyy, kyv, kvy, kvv, i, j, generalConst, generalConstGrad, 1);

g1Plus = sdlfmaXsdlfmvKernComputeBlock(lfmKern1, lfmKern2, tInit1(1), ...
    tInit2(2), kyy, kyv, kvy, kvv, i, j, generalConst);

g2Plus = sdlfmaXsdlfmvKernComputeBlock(lfmKern2, lfmKern1, tInit2(2), ...
    tInit1(1), kyy, kvy, kyv, kvv, j, i, generalConst');

if (i==1 && j==1) || (i==1) && (j~=1)

    if i~=1 || j~=1
        [gkyy1IC, gkyy2IC, gkyy3IC, gkyy4IC] = sdlfmXsdlfmKernGradientIC(lfmKern1, lfmKern2, ...
            tInit1(1), tInit2(2), gkyy1{i,j}, gkyy2{i,j}, gkyy3{i,j}, gkyy4{i,j}, gkyv1{i,j}, gkyv2{i,j}, ...
            gkyv3{i,j}, gkyv4{i,j}, gkvy1{i,j}, gkvy2{i,j}, gkvy3{i,j}, gkvy4{i,j}, gkvv1{i,j}, gkvv2{i,j}, ...
            gkvv3{i,j}, gkvv4{i,j}, 1, typeParam);
        for k=1:3
            g1L{1,2}{k} = g1L{1,2}{k} + gkyy1IC{k};
            g2L{1,2}{k} = g2L{1,2}{k} + gkyy2IC{k};
        end
        cLength = length(gkyy3IC);
        g3L{1,2}(1:cLength) = g3L{1,2}(1:cLength) + gkyy3IC;
        g3L{1,2}(i)   = g3L{1,2}(i) + g1Plus;
        g3L{1,2}(j+1) = g2Plus;
        g4L{1,2} = gkyy4IC;
    else
        g3L{1,2}(i)   = g3L{1,2}(i) + g1Plus;
        g3L{1,2}(j+1) = g2Plus;
        g4L{1,2} = sdlfmKernMeanCovPartial(lfmKern1(1), lfmKern2(1), tInit1(1), ...
            tInit2(2), 1, typeParam);
    end
end

% Compute for tInit1(2) and tInit2(1)

[g1L{2,1}, g2L{2,1}, g3L{2,1}] = sdlfmvXsdlfmvKernGradientBlock(lfmKern1, lfmKern2, tInit1(2), ...
    tInit2(1), kyy, kyv, kvy, kvv, i, j, generalConst, generalConstGrad, 1);

g1Plus = sdlfmaXsdlfmvKernComputeBlock(lfmKern1, lfmKern2, tInit1(2), ...
    tInit2(1), kyy, kyv, kvy, kvv, i, j, generalConst);

g2Plus = sdlfmaXsdlfmvKernComputeBlock(lfmKern2, lfmKern1, tInit2(1), ...
    tInit1(2), kyy, kvy, kyv, kvv, j, i, generalConst');

if (i==1 && j==1) || (i~=1) && (j==1)
    if i~=1 || j~=1
        [gkyy1IC, gkyy2IC, gkyy3IC, gkyy4IC] = sdlfmXsdlfmKernGradientIC(lfmKern1, lfmKern2, ...
            tInit1(2), tInit2(1), gkyy1{i,j}, gkyy2{i,j}, gkyy3{i,j}, gkyy4{i,j}, gkyv1{i,j}, gkyv2{i,j}, ...
            gkyv3{i,j}, gkyv4{i,j}, gkvy1{i,j}, gkvy2{i,j}, gkvy3{i,j}, gkvy4{i,j}, gkvv1{i,j}, gkvv2{i,j}, ...
            gkvv3{i,j}, gkvv4{i,j}, 1, typeParam);
        for k=1:3
            g1L{2,1}{k} = g1L{2,1}{k} + gkyy1IC{k};
            g2L{2,1}{k} = g2L{2,1}{k} + gkyy2IC{k};
        end
        cLength = length(gkyy3IC);
        g3L{2,1}(1:cLength) = g3L{2,1}(1:cLength) + gkyy3IC;
        g3L{2,1}(j)   =  g3L{2,1}(j) + g2Plus;
        g3L{2,1}(i+1) =  g1Plus;
        g4L{2,1} = gkyy4IC;
    else
        g3L{2,1}(j)   =  g3L{2,1}(j) + g2Plus;
        g3L{2,1}(i+1) =  g1Plus;
        g4L{2,1} = sdlfmKernMeanCovPartial(lfmKern1(1), lfmKern2(1), tInit1(2), ...
            tInit2(1), 1, typeParam);
    end
end

% Compute for tInit1(2) and tInit2(2)

[g1L{2,2}, g2L{2,2}, g3Local] = sdlfmvXsdlfmvKernGradientBlock(lfmKern1, lfmKern2, tInit1(2), ...
    tInit2(2), kyy, kyv, kvy, kvv, i, j, generalConst, generalConstGrad, 1);

g1Plus = sdlfmaXsdlfmvKernComputeBlock(lfmKern1, lfmKern2, tInit1(2), ...
    tInit2(2), kyy, kyv, kvy, kvv, i, j, generalConst);

g2Plus = sdlfmaXsdlfmvKernComputeBlock(lfmKern2, lfmKern1, tInit2(2), ...
    tInit1(2), kyy, kvy, kyv, kvv, j, i, generalConst');

if i~=1 || j~=1
    [gkyy1IC, gkyy2IC, gkyy3IC, gkyy4IC] = sdlfmXsdlfmKernGradientIC(lfmKern1, lfmKern2, ...
        tInit1(2), tInit2(2), gkyy1{i,j}, gkyy2{i,j}, gkyy3{i,j}, gkyy4{i,j}, gkyv1{i,j}, gkyv2{i,j}, ...
        gkyv3{i,j}, gkyv4{i,j}, gkvy1{i,j}, gkvy2{i,j}, gkvy3{i,j}, gkvy4{i,j}, gkvv1{i,j}, gkvv2{i,j}, ...
        gkvv3{i,j}, gkvv4{i,j}, 1, typeParam);
    if i>=2 && j>=2
        g1L{2,2}{1} = g1L{2,2}{1} + gkyy1IC{1};
        g2L{2,2}{1} = g2L{2,2}{1} + gkyy2IC{1};
        g1L{2,2}{2} = [gkyy1IC{2} g1L{2,2}{2}];
        g2L{2,2}{2} = [gkyy2IC{2} g2L{2,2}{2}];
        g1L{2,2}{3} = [gkyy1IC{3} g1L{2,2}{3}];
        g2L{2,2}{3} = [gkyy2IC{3} g2L{2,2}{3}];
    else
        for k=1:3
            g1L{2,2}{k} = g1L{2,2}{k} + gkyy1IC{k};
            g2L{2,2}{k} = g2L{2,2}{k} + gkyy2IC{k};
        end
    end
    cLength = length(g3Local);
    g3L{2,2} = zeros(1, cLength+1);
    g3L{2,2}(1:cLength) = g3L{2,2}(1:cLength) + g3Local;
    cLength = length(gkyy3IC);
    g3L{2,2}(1:cLength) = g3L{2,2}(1:cLength) + gkyy3IC;
    g3L{2,2}(i+1) = g3L{2,2}(i+1) + g1Plus;
    g3L{2,2}(j+1) = g3L{2,2}(j+1) + g2Plus;
    g4L{2,2} = gkyy4IC;
else
    g3L{2,2} = g3Local;
    g3L{2,2}(i+1) = g1Plus;
    g3L{2,2}(j+1) = g3L{2,2}(j+1) + g2Plus;
    g4L{2,2} = sdlfmKernMeanCovPartial(lfmKern1(1), lfmKern2(1), tInit1(2), ...
    tInit2(2), 1, typeParam);
end


%%%  HeightControl_HIL
clear
path( './icon/',path);
load vehicle_local_position.mat;
% constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
% throttle when UAV is hovering
THR_HOVER = 0.609;

%% control parameter
% attitude PID parameters
Kp_PITCH_ANGLE = 6.5;
Kp_PITCH_AngleRate = 0.1;
Ki_PITCH_AngleRate = 0.02;
Kd_PITCH_AngleRate = 0.001;
Kp_ROLL_ANGLE = 6.5;
Kp_ROLL_AngleRate = 0.1;
Ki_ROLL_AngleRate = 0.02;
Kd_ROLL_AngleRate = 0.001;

Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;
% position PID parameters
Kpxp = 1.0;
Kpyp = 1.0;
Kpzp = 4.0;
Kvxp = 2.5; Kvxi = 0.4; Kvxd = 0.01;
Kvyp = 2.5; Kvyi = 0.4; Kvyd = 0.01;
Kvzp = 0.45; Kvzi = 0.01; Kvzd = 0.005;
% integral saturation
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
Saturation_I_ah = 3.43;
Saturation_I_az = 5;

% max control angle,default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
% max control angle rate,rad/s
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
% max control speed, m/s
MAX_CONTROL_VELOCITY_XY = 5;
MAX_CONTROL_VELOCITY_Z = 3;
% throttle amplitude
MAX_MAN_THR = 0.9;
MIN_MAN_THR = 0.05;
%% run simulink model
HeightControl_HIL


function prob = problem_type(om, recheck)
%PROBLEM_TYPE  Return a string identifying the type of mathematical program
%   PROB_TYPE = OM.PROBLEM_TYPE()
%   PROB_TYPE = OM.PROBLEM_TYPE(RECHECK)
%
%   Returns a string identifying the type of mathematical program
%   represented by the current model, based on the variables, costs,
%   and constraints that have been added to the model. Used to
%   automatically select an appropriate solver.
%
%   Linear and nonlinear equations are models with no costs, no inequality
%   constraints, and an equal number of continuous variables and equality
%   constraints. If the number of variables in a nonlinear equation model
%   is one more than the number of constraints, it is a parameterized
%   nonlinear equation.
%
%   Outputs:
%       PROB_TYPE : problem type, one of the following strings:
%           'LEQ'   - linear equations
%           'NLEQ'  - nonlinear equations
%           'PNE'   - parameterized nonlinear equations
%           'LP'    - linear program
%           'QP'    - quadratic program
%           'NLP'   - nonlinear program
%           'MILP'  - mixed-integer linear program
%           'MIQP'  - mixed-integer quadratic program
%           'MINLP' - mixed-integer nonlinear program
%
%   The output value is cached for future calls, but calling with a true
%   value for the optional RECHECK argument will force it to recheck in
%   case the problem type has changed due to modifying the variables,
%   constraints or costs in the model.
%
%   See also OPT_MODEL

%   MP-Opt-Model
%   Copyright (c) 2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MP-Opt-Model.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/mp-opt-model for more info.

if isempty(om.prob_type) || nargin > 1 && recheck
    nleN = om.getN('nle');      %% nonlinear equalities
    nliN = om.getN('nli');      %% nonlinear inequalities
    nlcN = om.getN('nlc');      %% general nonlinear costs
    qdcN = om.getN('qdc');      %% quadratic costs
    linN = om.getN('lin');      %% linear constraints
    varN = om.getN('var');      %% variables
    if varN == 0
        prob = '';
    elseif nlcN || qdcN         %% problem has costs
        if nliN || nleN || nlcN %% nonlinear
            prob = 'NLP';           %% nonlinear program
        else                    %% linear constraints, no general nonlinear costs
            %% get quadratic cost coefficients
            H = om.params_quad_cost();
            if isempty(H) || ~any(any(H))
                prob = 'LP';        %% linear program
            else
                prob = 'QP';        %% quadratic program
            end
        end
    else                    %% problem has no costs
        if nliN
            error('@opt_model/problem_type: invalid problem - nonlinear inequality constraints with no costs');
        end
        if nleN + linN == varN || nleN + linN == varN - 1   %% square (or almost) system
            if linN > 0
                %% get lower & upper bounds
                [A, l, u] = om.params_lin_constraint();
                if any(l ~= u)
                    error('@opt_model/problem_type: invalid problem - linear inequality constraints with no costs');
                end
            end
            if nleN + linN == varN  %% square system
                if nleN
                    prob = 'NLEQ';      %% square nonlinear set of equations
                else
                    prob = 'LEQ';       %% square linear set of equations
                end
            elseif nleN + linN + 1 == varN  %% square + 1 extra (parameterization) variable
                if nleN
                    prob = 'PNE';       %% parameterized nonlinear set of equations
                else
                    prob = 'PLEQ';      %% parameterized linear set of equations
                    error('@opt_model/problem_type: invalid problem - PNE not implemented for for linear constraints only');
                end
            else
                error('@opt_model/problem_type: invalid problem - PNE must have num of vars = num of constraints + 1');
            end
        else
            error('@opt_model/problem_type: invalid problem - non-square system with no costs');
        end
    end
    if om.is_mixed_integer() && ~strcmp(prob, 'NLEQ')
        prob = ['MI' prob];
    end
    om.prob_type = prob;    %% cache it
else
    prob = om.prob_type;    %% return cached type
end


function cmap = overlapCmap(numGrays,numColors,rgyFlag)
%
% cmap = overlapCmap(numGrays,numColors,rgyFlag)
%
% Makes color map array with numGrays grays,
% and a numColors sub-map that's divided into
% 3 color bands (red, green, yellow, or red,
% blue purple, depending on the setting of rgyFlag).
% Used for visualizing overlap maps, though may be
% useful for other purposes.
%
% ras 2/04
if ieNotDefined('rgyFlag')
    rgyFlag = 1;
end

if ~exist('numGrays','var')
    numGrays=128;
end
if ~exist('numColors','var')
    numColors=128;
end

if rgyFlag==1
    % red/green/yellow colors
    colA = [.9 0 0];
    colB = [0 .9 0];
    colC = [.9 .9 0];
else
    % red/blue/purple colors
    colA = [.9 0 0];
    colB = [0 0 .9];
    colC = [.9 0 .9];
end

rngA = 1:round(numColors/3);
rngB = round(numColors/3)+1:ceil((2/3)*numColors);
rngC = ceil((2/3)*numColors)+1:numColors;

colors = zeros(numColors,3);
colors(rngA,:) = repmat(colA,[length(rngA) 1]);
colors(rngB,:) = repmat(colB,[length(rngB) 1]);
colors(rngC,:) = repmat(colC,[length(rngC) 1]);

cmap = [gray(numGrays); colors];

return


function h=gridpcolor(ax,xs, ys, values, mask, name)
    % GRIDPCOLOR plots values, sampled at the intersection of xcenters and ycenters
    %
    % h=GRIDPCOLOR(ax,xs, ys, values, mask) where XS and YS are the same sized matrices of points.
    % and MASK is
    %
    % Pcolor typically uses the points as edges, and ignores the last values.
    %
    if isempty(ax)
        ax=gca;
    end
    if ~exist('name','var')
        name='';
    end
    name(name=='_')=' ';

    INCLUDENUMBERS=false;
    INCLUDECOORDS=false;

    if exist('mask','var')
        if islogical(values),values=double(values);end
        values(~mask)=nan;
    else
        mask=true(size(xs)+[1,1]);
    end

    [xs,ys, values]=centers2edges(xs,ys,values);
    ax.NextPlot='add';
    h=pcolor(ax,xs, ys, values);
    %if ~isempty('name')
    set(h,'DisplayName',name);
    %end
    if INCLUDENUMBERS
        for n=1:numel(xs)
            v=values(n);
            if ~isnan(v) && mask(n)
                if INCLUDECOORDS
                    text(ax,xs(n),ys(n),sprintf('(%.2f, %.2f)\n%s',xs(n),ys(n),num2str(v)));
                else
                    text(ax,xs(n),ys(mn),num2str(v));
                end
            end
        end
    end


function [M0,q0,X,x,f,M1,L] = spm_fp(M,x,u)
% Fokker-Planck operators and equilibrium density for dynamic systems
% FORMAT [M0,q0,X,x,f,M1,L] = spm_fp(M,x,u)
%--------------------------------------------------------------------------
% M   - model specification structure
% Required fields:
%    M.f   - dx/dt    = f(x,u,P) or f(x,u,a,P)  {function string or m-file}
%    M.g   - y(t)     = g(x,u,P)                {function string or m-file}
%    M.m   - m inputs
%    M.n   - n states
%    M.l   - l outputs
%    M.x   - (n x 1) = x(0) = expansion point
%    M.W   - (n x n) - precision matrix of state noise
% x    - cell array of vectors specifying evaluation grid
% u    - expansion point for inputs or causes;
%
% M0   - 1st order FP operator dq/dt = M0*q + u*M1*q,  q = p(X);
% q0   - stable or equilibrium mode: M0*q0 = 0
% X    - evaluation points of state space
% x    - cell array of vectors specifying evaluation grid
% f    - flow
% M1   - 2nd order FP operator
% L    - output matrix                 <y> = L*q;
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fp.m 5219 2013-01-29 17:07:07Z spm $

% default: first level of hierarchical model
%--------------------------------------------------------------------------
M   = M(1);

% default expansion point for inputs
%--------------------------------------------------------------------------
if nargin < 3
    u0 = zeros(M.m,1);
else
    u0 = u;
end

% event space: get or create X - coordinates of evaluation grid
%--------------------------------------------------------------------------
n  = length(M.x);
if nargin < 2

    % use M.X
    %----------------------------------------------------------------------
    X     = M.X;
    for i = 1:size(X,2)
        x{i} = unique(X(:,i));
    end
else

    % use x
    %----------------------------------------------------------------------
    [X,x] = spm_ndgrid(x);
end

% f(x,0)
%--------------------------------------------------------------------------
N     = length(X);
f     = sparse(n,N);
try
    for i = 1:N
        f(:,i) = feval(M.f,X(i,:)',u0,M.pE);
    end
catch
    for i = 1:N
        f(:,i) = feval(M.f,X(i,:)',u0,0,M.pE);
    end
end

% Fokker-Planck operator; i.e., Jacobian J - M0
%==========================================================================
for i = 1:n
    d(i)  = length(x{i});
    dx(i) = x{i}(2) - x{i}(1);
    I{i}  = speye(d(i),d(i));
end
J     = sparse(N,N);

% cycle over dimensions of state space
%--------------------------------------------------------------------------
for i = 1:length(x)

    % differential operators (for positive and negative flows)
    %----------------------------------------------------------------------
    j          = find(f(i,:) < 0);
    u          = sparse(j,1,f(i,j),N,1);
    j          = find(f(i,:) > 0);
    v          = sparse(j,1,f(i,j),N,1);

    dp         = spdiags(ones(d(i),1)*[1 -1],[ 0;1],d(i),d(i))/dx(i);
    dp(:,1)    = 0;
    dq         = spdiags(ones(d(i),1)*[1 -1],[-1;0],d(i),d(i))/dx(i);
    dq(:,end)  = 0;

    % Kronecker tensor products
    %----------------------------------------------------------------------
    Dp         = 1;
    Dq         = 1;
    for j = 1:(i - 1)
        Dp = spm_kron(I{j},Dp);
        Dq = spm_kron(I{j},Dq);
    end
    Dp    = spm_kron(dp,Dp);
    Dq    = spm_kron(dq,Dq);
    for j = (i + 1):n
        Dp = spm_kron(I{j},Dp);
        Dq = spm_kron(I{j},Dq);
    end
    DP{i} = Dp;

    % augment Jacobian
    %----------------------------------------------------------------------
    J     = J + Dp*spdiags(u,0,N,N) + Dq*spdiags(v,0,N,N);

end

% dispersion
%--------------------------------------------------------------------------
C     = inv(M.W);
if length(C) ~= n
    C = C(1)*speye(n,n);
end
for i = 1:n
    for j = 1:n
        if C(i,j)
            J  = J - C(i,j)*DP{i}*DP{j}'/2;
        end
    end
end

% return if only M0 is required
%--------------------------------------------------------------------------
M0    = J;
if nargout == 1, return, end

% stable mode - stochastic iteration
%==========================================================================
q     = sparse(N,1) + 1/N;
for i = 1:1024
    dq = M0*q;
    if norm(dq,1) < exp(-4), break, end
    while min(q + dq) < 0
        dq = dq - dq/2;
    end
    q  = q + dq;
end
for i = 1:n
    nx{i} = length(x{i});
end
q0    = reshape(q,nx{:});

if nargout < 6,  return,  end

% input induced changes to Jacobian dJ/du - M1
%==========================================================================
M1    = spm_diff('spm_fp',M,x,u0,3);
if nargout < 6,  return,  end

% Output matrix L - M1
%==========================================================================

% l(x,0)
%--------------------------------------------------------------------------
L     = sparse(M.l,N);
for i = 1:N
    try
        L(:,i) = feval(M.g,X(i,:)',u0,M.pE);
    catch
        L(:,i) = feval(M.g,X(i,:)',u0,[],M.pE);
    end
end

return

% NOTES: analytic solution for q0
%--------------------------------------------------------------------------
q   = length(M0);
J   = [M0; ones(1,q)];
q0  = sum(inv(J'*J),2);


function f=loglikGaPExpWfixPriorH(x, varargin)
% f=loglikGaP(x, varargin)
% complete log likellihood of the GaP model
% Vxt = varargin{1};      %data
% sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
% alpha = varargin{3}; %parameters of the Gamma prior on the blinking
% beta = varargin{4}; %parameters of the Gamma prior on the blinking
% peval = varargin{5}; %parameters
% x(1:end-2*peval.ncomp) is Hkt

Vxt = varargin{1};      %data
sigpsf = varargin{2};  %std deviation of the PSF gaussian approx
alpha = varargin{3}; %parameters of the Gamma prior on the blinking
beta = varargin{4}; %parameters of the Gamma prior on the blinking
peval = varargin{5}; %parameters

[Hkt_r, cx, cy, Wxk] = loglikGaPreadparamWfix(x,varargin);
Hkt=exp(Hkt_r); %nonnegativity constrains

[Wxkbg,Hktbg]=addbg(Wxk, Hkt, peval.bg);
P=Wxkbg*Hktbg; %current approximation

%Poisson contribution
t1=Vxt.*log(P) - P;
%Gamma contribution
t2=(alpha-1)*log(Hkt)-1/beta*Hkt-alpha*log(beta)-log(gamma(alpha));

f=sum(t1(:))+sum(t2(:));
f=-f; %conjugate gradient is mimimizing!
end


function [ know, x ] = p40_sol ( n )

%*****************************************************************************80
%
%% P40_SOL returns the solution for problem 40.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the problem.  This value
%    is only needed for those problems with variable N.
%
%    Output, integer KNOW.
%    If KNOW is 0, then the solution is not known.
%    If KNOW is positive, then the solution is known, and is returned in X.
%
%    Output, real X(N), the solution, if known.
%
  know = 1;

  x = zeros ( n, 1 );

  return
end


function A = connect_by_inherit_neigh(pts, spls, corresp, neigh)
% build a connection matrix of downsamples (spls) by inherit neighbors of samples (pts) they
% correspond.
% There are two kinds of ways to use the neighbors, seeing the code.
%
% pts: samples
% spls: downsamples
% corresp: correspondence between pts and spls, array of |pts|*1
% neigh: neighbors of pts, a |pts|*? cell, the neighbors are sorted in ascending order by distance
% A: connection matrix of downsamples (spls)
%
% @author: JJCAO
% Changed from Andrea Tagliasacchi's code
% @data:   2010-5-7
% @version 2.0

%% visual debug conditions
SHOW_CONNECT_PROGRESS = false;
SHOW_RESULTS = false;

if SHOW_CONNECT_PROGRESS || SHOW_RESULTS
    close all;
    figure(1); movegui('northwest');set(gcf,'color','white');hold on;
    plot3( pts(:,1), pts(:,2), pts(:,3), '.r', 'markersize', 1);
    plot3( spls(:,1), spls(:,2), spls(:,3), '.g', 'markersize', 20);
    axis off; axis equal;set(gcf,'Renderer','OpenGL');
end
if ~iscell(neigh)
    tmp = cell(size(neigh,1),1);
    for i = 1:size(neigh,1)
        tmp{i}=neigh(i,:);
    end
    neigh = tmp;  clear tmp;
end
%pause(5);
%change to use the
%%
A = zeros( length(spls), length(spls) );
for pIdx=1:length(pts)
    ns =neigh{pIdx};
    pc = corresp(pIdx);
    if pc == 0
        warning('some points have no correspondence');continue;
    end

    for nIdx=1:length(ns)
        nc = corresp(ns(nIdx));
        if nc == 0
            warning('some points have no correspondence');continue;
        end
        if nc~=pc
            A(pc,nc) = A(pc,nc) + 1;
            A(nc,pc) = A(nc,pc) + 1;
            if SHOW_CONNECT_PROGRESS
                figure(1); idx = [pc, nc];
                line( spls(idx,1),spls(idx,2),spls(idx,3), 'LineWidth', 2,'Color','b');
            end
            break;%new method, generates less edges
%           %continue;old method, generates more edges.
        end
    end
end

%% if there are isolate points, connect it with its nearest neighbors.
isopts = zeros(1,0);
for i=1:size(A,1)
    A(i,i) = 1;
    if length( find( A(i,:)>0) ) == 1
        isopts(1, end+1) = i;
        warning('there are isolate points: %d', i);
    end
end
if isempty(isopts)
    spls_kdtree = kdtree_build( spls );
    for i=isopts
        neighs = kdtree_k_nearest_neighbors( spls_kdtree, spls(i,:), 2)';
        for j = neighs
            A(i,j) = 1;
            A(j,i) = 1;
        end
    end
    kdtree_delete( spls_kdtree );
end


function [breadth, labels] = imMeanBreadthEstimate(img, varargin)
% Estimate mean breadth of a binary structure.
%
%   BREADTH = imMeanBreadthEstimate(IMG)
%   The aim of this function is to be called by the "imMeanBreadthDensity"
%   function, for providing an estimate of mean breadth density within a
%   representative volume of interest.
%
%   BREADTH = imMeanBreadthEstimate(IMG, RESOL)
%   Also specifies the resolution of input image.
%
%   BREADTH = imMeanBreadthEstimate(IMG, RESOL, NDIRS)
%   Also specifies the number of directions used for computation. Can be
%   either 3 or 13 (the default).
%
%   Example
%
%   See also
%     imMeanBreadthEstimate, imSurfaceAreaEstimate
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2015-04-20,    using Matlab 8.4.0.150421 (R2014b)
% Copyright 2015 INRAE - Cepia Software Platform.

%% Basic error checking

% check image dimension
if ndims(img) ~= 3
    error('first argument should be a 3D image');
end

% in case of a label image, return a vector with a set of results
if ~islogical(img)
    % identify the labels in image
    labels = imFindLabels(img);

    % allocate result array
    nLabels = length(labels);
    breadth = zeros(nLabels, 1);

    % compute bounding box of each label
    boxes = imBoundingBox(img);

    % Compute mean breadth of each label considered as binary image
    % The computation is performed on a subset of the image for reducing
    % memory footprint.
    for i = 1:nLabels
        label = labels(i);

        % convert bounding box to image extent, in x, y and z directions
        box = boxes(i,:);
        i0 = ceil(box([3 1 5]));
        i1 = floor(box([4 2 6]));

        % crop image of current label
        bin = img(i0(1):i1(1), i0(2):i1(2), i0(3):i1(3)) == label;
        breadth(i) = imMeanBreadthEstimate(bin, varargin{:});
    end

    return;
end

%% Process input arguments

% in case of binary image, compute only one label...
labels = 1;

% default number of directions
nDirs = 13;

% default image resolution
delta = [1 1 1];

% Process user input arguments
while ~isempty(varargin)
    var = varargin{1};
    if ~isnumeric(var)
        error('option should be numeric');
    end

    % option is either connectivity or resolution
    if isscalar(var)
        nDirs = var;
    else
        delta = var;
    end
    varargin(1) = [];
end

%% Compute mean breadth estimate using Look-up-Table

histo = imBinaryConfigHisto(img);

lut = imMeanBreadthLut(delta, nDirs);

breadth = sum(histo .* lut);


% Test file for chebtech/mat2cell.m

function pass = test_mat2cell(pref)

if ( nargin < 2 )
    pref = chebtech.techPref();
end

for n = 1:2
    if ( n == 1 )
        testclass = chebtech1();
    else
        testclass = chebtech2();
    end

    f = testclass.make(@(x) [sin(x) cos(x) exp(x)], [], pref);
    g = testclass.make(@(x) sin(x), [], pref);
    h = testclass.make(@(x) [cos(x) exp(x)], [], pref);

    F = mat2cell(f, 1, [1 2]);
    pass(n, 1) = sum(F{1} - g) < 10*vscale(g)*eps;
    pass(n, 2) = all( sum(F{2} - h) < 10*max(vscale(h)*eps) );
end

end


function gX = rbfardKernDiagGradX(kern, X)

% RBFARDKERNDIAGGRADX Gradient of RBFARD kernel's diagonal with respect to X.
% FORMAT
% DESC computes the gradient of the diagonal of the automatic relevance determination radial basis function kernel matrix with
% respect to the elements of the design matrix given in X.
% ARG kern : the kernel structure for which gradients are being computed.
% ARG X : the input data in the form of a design matrix.
% RETURN gX : the gradients of the diagonal with respect to each element
% of X. The returned matrix has the same dimensions as X.
%
% SEEALSO : rbfardKernParamInit, kernDiagGradX, rbfardkernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% KERN

gX = zeros(size(X));


classdef ParameterAnalysis
    %PARAMETERANALYSIS Summary of this class goes here
    %   Detailed explanation goes here

    properties
        % 1 for multiplication, 2 for addition
        StepSizeMode
    end

    properties
        % 1 for 1-order.
        % 2 for 2-order (cartesian product).
        % A vector to indicate size for each parameter, e.g. (5,5) means
        % 5 neighbors (including current value) for each of the 2
        % parameters.
        NeighborhoodMode
    end

    properties
        PrintListSearch
    end

    properties
        % Use 'Greedy' or 'BrutalForce' or 'BlockCoordinateDescent'
        Algorithm
    end

    %% parameters used by brutal force search
    properties
        BrutalForceDepth
    end

    %% parameters used by BCD
    properties
        BlockCoordinateDescentDepth
    end

    properties
        BlockCoordinateDescentGroup
    end

    properties
        MaxNumberOfIterations
    end

    %% parameters used by all
    properties
        CheckValueInListSearchOptions
    end

    methods
        function obj = ParameterAnalysis()
            obj.StepSizeMode = 1;
            obj.NeighborhoodMode = 1;
            obj.PrintListSearch = 0;
            obj.Algorithm = 'Greedy';
            obj.BrutalForceDepth = 3;
            obj.CheckValueInListSearchOptions = 1;

            obj.MaxNumberOfIterations = 50;
            obj.BlockCoordinateDescentDepth = 5;
        end

        function options = autoParamSelection(obj, fcn_run, options, ...
                list_params, step_size, range)
            disp('Start searching best parameters. This could take a while.');

            switch obj.Algorithm
                case 'Greedy'
                    options = autoParamSelectionByGreedyAlgo(obj, ...
                        fcn_run, options, list_params, step_size, range);
                case 'BrutalForce'
                    options = autoParamSelectionByBrutalForce(obj, ...
                        fcn_run, options, list_params, step_size, range);
                case 'BlockCoordinateDescent'
                    options = autoParamSelectionByBCD(obj, ...
                        fcn_run, options, list_params, step_size, range);
                otherwise
            end
        end

        function options = autoParamSelectionByGreedyAlgo(obj, fcn_run, ...
                options, list_params, step_size, range)
%             if 0
            curr_val = fcn_run(options);
            list_search_options = options;
            list_search_value = curr_val;
            while 1
                nbhds = obj.findNeighboringOptions(options, list_params, ...
                    step_size, range, obj.NeighborhoodMode);
                nbhds_vals = zeros(1,length(nbhds));
                for i = 1:length(nbhds)
                    [val, list_search_options, list_search_value] = ...
                        obj.run(fcn_run, list_params, nbhds(i), ...
                        list_search_options, list_search_value);
                    nbhds_vals(i) = val;
                end

                if obj.testLocalMinimum(curr_val, nbhds_vals)
                    break;
                else
                    [min_val,ind] = min(nbhds_vals);
                    options = nbhds(ind);
                    curr_val = min_val;
                end

                if obj.PrintListSearch
                    obj.printListSearch(list_search_options, list_search_value, ...
                        list_params);
                end
            end
            save('param_analysis.mat','list_search_options',...
                'list_search_value','options');
%             end

            disp('The best searched parameters are');
            options
        end

        function options = autoParamSelectionByBrutalForce(obj, fcn_run, ...
                options, list_params, step_size, range)
            list_search_options = [];
            list_search_value = [];
            for k = 1:obj.BrutalForceDepth
                nbhds = obj.findNeighboringOptions(options, list_params, ...
                    step_size, range, obj.NeighborhoodMode);
                nbhds_vals = zeros(1,length(nbhds));
                for i = 1:length(nbhds)
                    [val, list_search_options, list_search_value] = ...
                        obj.run(fcn_run, list_params, nbhds(i), ...
                        list_search_options, list_search_value);
                    nbhds_vals(i) = val;
                end

                [min_val,ind] = min(nbhds_vals);
                options = nbhds(ind);
                step_size = step_size / 2;
            end
            save('param_analysis.mat','list_search_options',...
                'list_search_value','options');

            if obj.PrintListSearch
                obj.printListSearch(list_search_options, list_search_value, ...
                    list_params);
            end
            disp('The best searched parameters are');
            options

        end

        function options = autoParamSelectionByBCD(obj, ...
                fcn_run, options, list_params, step_sizes, range)
            last_options = options;
            for iter = 1:obj.MaxNumberOfIterations
                for j = 1:length(obj.BlockCoordinateDescentGroup)
                    inds = obj.BlockCoordinateDescentGroup{j};
                    list_search_options = [];
                    list_search_value = [];
                    step_size = step_sizes(inds);
                    for k = 1:obj.BlockCoordinateDescentDepth
                        nbhds = obj.findNeighboringOptions(options, list_params(inds), ...
                            step_size, range(:,inds), obj.NeighborhoodMode(inds));
                        nbhds_vals = zeros(1,length(nbhds));
                        for i = 1:length(nbhds)
                            [val, list_search_options, list_search_value] = ...
                                obj.run(fcn_run, list_params, nbhds(i), ...
                                list_search_options, list_search_value);
                            nbhds_vals(i) = val;
                        end

                        [min_val,ind] = min(nbhds_vals);
                        options = nbhds(ind);
                        step_size = step_size / 2;
                    end
                end
                if ParameterAnalysis.isOptionsEqual(options, last_options, list_params)
                    disp(['BCD stopped at iteration ',num2str(iter)]);
                    break;
                else
                    last_options = options;
                end
            end

            disp('The best searched parameters are');
            options
        end

        function printListSearch(obj, list_search_options, list_search_value,...
                list_params)
            disp('--------------------- Search Results -----------------');
            list_params = [list_params,{'value'}];
            disp(list_params);

            N = length(list_search_options);
            M = length(list_params);
            X = zeros(N, M);
            for i = 1:N
                options = list_search_options(i);
                value = list_search_value(i);
                for j = 1:M-1
                    name = list_params{j};
                    X(i,j) = options.(name);
                end
                X(i,M) = value;
            end
            disp(X);
        end

        function [val, list_search_options, list_search_value] = run( ...
                obj, fcn_run, list_params, options, list_search_options,...
                list_search_value)
            find = 0;
            if obj.CheckValueInListSearchOptions
                for i = 1:length(list_search_options)
                    if ParameterAnalysis.isOptionsEqual(options, ...
                            list_search_options(i), list_params)
                        find = 1;
                        val = list_search_value(i);
                        break;
                    end
                end
            end
            if ~find
                val = fcn_run(options);
                list_search_options = [list_search_options, options];
                list_search_value = [list_search_value, val];
            end
        end

        function T = testLocalMinimum(obj, curr_val, nbhds_vals)
            T = all(curr_val <= nbhds_vals);
        end

        function nbhds = findNeighboringOptions(obj, options, list_params, ...
                step_size, range, nbhd_mode)
            if length(obj.StepSizeMode) == 1
                step_mode = repmat(obj.StepSizeMode, [length(step_size),1]);
            else
                step_mode = obj.StepSizeMode;
            end

            if isscalar(nbhd_mode) && nbhd_mode == 1
                % 1 order neighborhood will not consider the current point
                nbhds = find1OrderNeighborhood(obj, options, ...
                    list_params, step_size, range, step_mode);
            elseif isscalar(nbhd_mode) && nbhd_mode == 2
                nbhds = find2OrderNeighborhood(obj, options, ...
                    list_params, step_size, range, step_mode);
            else % it's a vector specifying the size of nbhd
                nbhds = findBigNeighborhood(obj, options, ...
                    list_params, step_size, range, step_mode, ...
                    nbhd_mode);
            end
        end

        function nbhds = find1OrderNeighborhood(obj, options, ...
                list_params, step_size, range, modes)
            nbhds = [];
            options_start = options;
            for i = 1:length(list_params)
                options = options_start;
                name = list_params{i};
                forw_val = obj.findNeighboringValue(options_start.(name),...
                    step_size, i, 1, modes, range);
                back_val = obj.findNeighboringValue(options_start.(name),...
                    step_size, i, 0, modes, range);

                options.(name) = forw_val;
                nbhds = [nbhds, options];
                options.(name) = back_val;
                nbhds = [nbhds, options];
            end
        end

        function nbhds = find2OrderNeighborhood(obj, options, ...
                list_params, step_size, range, modes)
            % modes are the step mode (multiplication or addition)
            nbhd_size = 3 * ones(size(list_params));
            nbhds = findBigNeighborhood(obj, options, ...
                list_params, step_size, range, modes, nbhd_size);
        end

        function nbhds = findBigNeighborhood(obj, options, ...
                list_params, step_size, range, modes, nbhd_size)
            % num_steps takes the form like (3,3) for 2 parameters where
            % 3 values including the current value are considered as nbhd
            num_steps = floor(nbhd_size/2);
            options_start = options;
            params = cell(1,length(list_params));
            for i = 1:length(list_params)
                name = list_params{i};
                curr_val = options_start.(name);
                vals = curr_val;
                for j = 1:num_steps(i)
                    back_val = obj.findNeighboringValue(vals(1), ...
                        step_size, i, 0, modes, range);
                    vals = cat(1,back_val,vals);
                end
                for j = 1:num_steps(i)
                    forw_val = obj.findNeighboringValue(vals(end), ...
                        step_size, i, 1, modes, range);
                    vals = cat(1,vals,forw_val);
                end
                params{i} = vals;
            end
            % cartprod can't handle duplicate entries
%             C = cartprod(params{:});
            C = ParameterAnalysis.cartesian(params{:});

            nbhds = repmat(options_start, [size(C,1),1]);
            for i = 1:size(C,1)
                for j = 1:length(list_params)
                    nbhds(i).(list_params{j}) = C(i,j);
                end
            end
        end

        function new_val = findNeighboringValue(obj, curr_val, ...
                step_size, i, direction, modes, range)
            order_of_magnitude = @(x) floor(log10(x));

            if isnumeric(step_size)
                size = step_size(i);
                mode = modes(i);
                new_val = obj.calcNewValue(direction, curr_val, size, mode);
            elseif isequal(step_size,'medium')
                possible_vals = [1,2,5];
                size = 2;
                mode = 1;
                new_val = obj.calcNewValue(direction, curr_val, size, mode);
                order = order_of_magnitude(new_val);
                new_val = new_val/(10^order);
                [~,ind] = min(abs(new_val - possible_vals));
                new_val = possible_vals(ind);
                new_val = new_val * (10^order);
            end

            if direction == 1
                upper_bd = range(2,i);
                new_val = min(new_val, upper_bd);
            else
                lower_bd = range(1,i);
                new_val = max(new_val, lower_bd);
            end
        end

        function new_val = calcNewValue(obj, direction, curr_val, size, mode)
            if mode == 1 % multiplication
                if direction == 1
                    new_val = curr_val * size;
                else
                    new_val = curr_val / size;
                end
            elseif mode == 2 % addition
                if direction == 1
                    new_val = curr_val + size;
                else
                    new_val = curr_val - size;
                end
            end
        end
    end
    methods (Static)
        function C = cartesian(varargin)
            args = varargin;
            n = nargin;

            [F{1:n}] = ndgrid(args{:});

            for i=n:-1:1
                G(:,i) = F{i}(:);
            end

            C = unique(G , 'rows');
        end

        function t = isOptionsEqual(options1, options2, list_params)
            t = 1;
            for i = 1:length(list_params)
                if options1.(list_params{i}) ~= options2.(list_params{i})
                    t = 0;
                    break;
                end
            end
        end
    end
end


function varExplained = rmVarExplained(view, roi, modelNum);
% Compute the proportion variance explained by a retinotopy model fit.
%
% varExplained = rmVarExplained([view], [coords or roi], [modelNum]);
%
%
% ras, 12/2006
if notDefined('view'),      view = getCurView;                          end
if notDefined('modelNum'),  modelNum = viewGet(view, 'rmModelNum');     end
if notDefined('coords'),    roi = rmGetCoords(view, modelNum);       end

roi = tc_roiStruct(view, roi);

verbose = prefsVerboseCheck;
if verbose, hwait = mrvWaitbar(0, 'Loading % Variance Explained...'); end

params = viewGet(view, 'rmParams');
[tSeries coords params] = rmLoadTSeries(view, params, roi);
if verbose, mrvWaitbar(.5, hwait); end

fit = rmPredictedTSeries(view, coords, modelNum);
if verbose,  mrvWaitbar(.7, hwait); end

residual = tSeries - fit;
if verbose,  mrvWaitbar(1, hwait); end

nVoxels = size(tSeries, 2);
for v = 1:nVoxels
    varExplained(v) = 1 - (sum(residual(:,v).^2) ./ sum(tSeries(:,v).^2));
end

return


function [LLF, likelihoods, errors] = sarimax_likelihood(parameters,p,q,constant,seasonal,y,x,sigma)
% PURPOSE:
%   Likelihood function for armaxfilter
%
% USAGE:
%   [LLF, LIKELIHOODS, ERRORS] = armaxfilter_likelihood(PARAMETERS,P,Q,CONSTANT,Y,X,M)
%   [LLF, LIKELIHOODS, ERRORS] = armaxfilter_likelihood(PARAMETERS,P,Q,CONSTANT,Y,X,M)
%
% INPUTS:
%   PARAMETERS  - A vector of GARCH process aprams of the form [constant, arch, garch]
%   P           - Vector containing lag indices of the AR component
%   Q           - Vector containing lag indices of the MA component
%   CONSTANT    - Value indicating whether the model contains a constant (1) or not (0)
%   Y           -
%   X           -
%   M           - Index to first element to use in the recursive residual calculation
%   SIGMA       - T by 1 vector
%
% OUTPUTS:
%   LLF         - Minus 1 times the log likelihood
%   LIKELIHOODS - Time series of likelihoods
%   ERRORS      - Time series of model errors
%
% COMMENTS:
%
% See also armaerrors

% Author: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 4/1/2004

m = size(x,2);
[armaParameters, p, q] = sarma2arma(parameters(m+1:length(parameters)), p, q, seasonal);
parameters = [parameters(1:m);armaParameters];

e = armaxerrors(parameters,p,q,constant,y,x,m,ones(size(y)));
T = length(e);

% Do not divide e by sigma since this is done in armaxerrors
likelihoods =  0.5*(2*log(sigma) + (e./sigma).^2 + log(2*pi));

if isempty(p)
    p = 0;
end
if isempty(q)
    q = 0;
end
if max(q)>max(p) % prepend y and x, if needed
    t = (max(q)-max(p))+1:T;
else
    t = 1:T;
end

likelihoods = likelihoods(t);
errors=e(t);
LLF = sum(likelihoods);

if isnan(LLF)
    LLF=1e7;
end


function varargout = implies(varargin)
%IMPLIES Logical implication
%
% IMPLIES(X,Y) creates a mixed integer representation of
% the constraint X --> Y, i.e. Y is true if X is true.
%
% Syntax
%   F = implies(X,Y,tol)
%
% Input
%   X : binary SDPVAR variable or a constraint
%   Y : binary SDPVAR variable or a constraint
%  tol: Optional threshhold for defining zero (see NOTE)
%
% Output
%   F : SET object
%
% Examples
%
%  binvar X Y; F = implies(X,Y);
%  binvar X;sdpvar Y; F = [implies(X,Y>=5), -10 <= Y <= 100];
%  binvar X;Y=sdpvar(3,1); F = [implies(X,[sum(Y);Y(2)]>=[5;0]), -1<= Y <= 10];
%
% Note
%
%  All variables in the expressions have to be explicitly bounded somewhere
%  in the model (implicit constraints are not sufficients such as [Y <= Z,...,Z<= 10]
%
%  Using implies with X non-binary is highly sensitive numerically.
%  The problem comes from the definition of 0 in a floating-point
%  environment, and precision in the solver. To account for this,
%  the user can supply a third argument to define a dead-zone around
%  zero, i.e Implies(X<=0,Y) will be replaced with IMPLIES(X<=-tol,Y)
%  Note, you typically need to tweak this number for your
%  application/solver. By default, YALMIP uses tol = 0, which means you
%  easily can get garbage... A positive number means YALMIP is cautious in
%  terms of activating the condition, while a negative number means YALMIP
%  will be aggressive  in activating the condition.
%
%   See also @SDPVAR/AND, @SDPVAR/OR, IFF

% There are some cases to take care of...
%
% X --> Y     binary/binary :                     Implemented
% X --> Y     binary/(LP,equality,sdp)            Implemented
% X --> Y     (LP,equality,sdp)/binary            Not implemented
% X --> Y     (LP,equality,sdp)/(LP,equality,sdp) Not implemented

X = varargin{1};
Y = varargin{2};

if isempty(X)
    varargout{1} = [];
elseif isa(X,'double') && isequal(X,0)
    varargout{1} = [];
elseif isa(X,'double') && isequal(X,1)
    varargout{1} = Y;
end

switch class(X)

    case {'sdpvar','constraint','lmi'}

        if nargin > 2
            if ~isa(varargin{3},'double')
                error('Third argument in IMPLIES should be a numerical value (tolerance)');
            end
        end
        varargout{1} = setupMeta(lmi([]), mfilename,varargin{:});

    case 'char'
        varargout{1} = implies_internal(varargin{3:end});

    case 'logical'
        if length(X)==1
            if X
                varargout{1} = Y;
            else
                varargout{1} = [];
            end
        else
            if length(X) == length(Y)
                i = find(X);
                if isempty(i)
                    varargout{1} = [];
                else
                    varargout{1} = Y(i);
                end
            else
                error('Size mismatch in input arguments');
            end
        end
end