%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtHmxCompressorBox.m                         |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Compare compressor for separated boxes        |
%|  `---'  |                                                              |
%+========================================================================+

clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Taille de la boite
edg = 1;

% Precision ou nombre de points de quadrature polynomiale
tol = 1e-3;

% Regular cube
n       = 10;
x       = 0:1/(n-1):1;
[x,y,z] = meshgrid(x,x,x);

% Recepteurs
Nx     = 1000;
% X      = edg*rand(Nx,3);
X      = edg*[x(:),y(:),z(:)];

% Emmeteurs
Ny     = 1000;
% Y      = edg*rand(Ny,3);
Y      = edg*[x(:),y(:),z(:)];
Y(:,1) = 2*edg + Y(:,1);

% Potentiel aleatoire aux emmeteurs
V  = (-1-1i) + (2+2i)*rand(Ny,1);

% Representation graphique
figure(1)
plot3(X(:,1),X(:,2),X(:,3),'*b',Y(:,1),Y(:,2),Y(:,3),'r*')
grid on
axis equal

% Noyau de green Helmholtz
k     = 5;
rxy   = @(X,Y) sqrt( (X(:,1)-Y(:,1)).^2 + (X(:,2)-Y(:,2)).^2 + (X(:,3)-Y(:,3)).^2 );
green = @(X,Y) exp(1i*k*rxy(X,Y))./rxy(X,Y);

% Gridding
[I,J] = ndgrid(1:Nx,1:Ny);

% Noyau de green
Gxy = green(X(I,:),Y(J,:));
Gxy = reshape(Gxy,Nx,Ny);
% Gxy = [Gxy zeros(Nx,Ny) ; ...
%     zeros(Nx,Ny) Gxy];
% Gxy = [Gxy zeros(Nx,2*Ny) ; ...
%     zeros(Nx,Ny) Gxy zeros(Nx,Ny) ; ...
%     zeros(Nx,2*Ny) Gxy];
% figure
% imagesc(Gxy)

% Calcul du rang
tic
rk = rank(Gxy,tol);
toc
rk

% Compression ACA, pivotage total
tic
[A,B] = hmxACA(Gxy,tol);
toc
size(A)
norm(A*B-Gxy,'fro')./norm(Gxy,'fro')

% Compression ACA, pivotage partiel
tic
[A,B] = hmxACA(X,Y,@(X,Y) green(X,Y),tol);
toc
size(A)
norm(A*B-Gxy,'fro')./norm(Gxy,'fro')

% % Recompression ACA, pivotage total
% tic
% [Ap,Bp] = hmxACA(A,B,tol);
% toc
% size(Ap)
% norm(Ap*Bp-Gxy,'fro')./norm(Gxy,'fro')

% Recompression RSVD
tic
[Ap,Bp,flag] = hmxRSVD(A,B,tol);
toc
size(Ap)
norm(Ap*Bp-Gxy,'fro')./norm(Gxy,'fro')

% Recompression QRSVD
tic
[Ap,Bp] = hmxQRSVD(A,B,tol);
toc
size(Ap)
norm(Ap*Bp-Gxy,'fro')./norm(Gxy,'fro')

disp('~~> Michto gypsilab !')


%% Set Parameters
% Preferences
global ggamma rrho
ggamma = 2;        % coefficient of relative risk aversion
rrho = 0.01;       % rate of time preference

% Production function
global ddelta aalpha
ddelta = .025;     % capital depreciation
aalpha = 1 / 3;    % capital share

% Aggregate shock
global ssigmaTFP rrhoTFP
ssigmaTFP = .007;  % standard deviation of TFP shock
rrhoTFP = .95;     % quarterly autocorrelation of TFP shock

% Idiosyncratic shocks
global z
zz1 = 0;           % unemployed
zz2 = 1;           % employed
z = [zz1,zz2];

% Transition probabilities
global lla
llambda1 = 1 / 2;  % expected duration of unemployment is 2 quarters
llambda2 = (llambda1 / (zz2 * .93 - zz1))*(zz2 - zz2 * .93); % unemployment rate 7%
lla = [llambda1,llambda2];

% Tax system
global mmu ttau
mmu = .15;        % UI replacement rate 15%
ttau = (mmu / zz2) * (lla(2) / lla(1));	     % labor income tax

% Labor supply
global zAvg
zAvg = (lla(1) * z(2) + lla(2) * z(1)) / (lla(1) + lla(2));

%% Approximation Parameters
% Wealth grid
global I amin amax a da aa aaa
I = 100;           % can't be too fine for some reason
amin = 0;
amax = 100;
a = linspace(amin,amax,I)';
da = (amax - amin) / (I - 1);
aa = [a,a];
aaa = reshape(aa,2*I,1);

% Labor productivity
global zz zzz
zz = ones(I,1) * z;
zzz = reshape(zz,2*I,1);

% Idiosyncratic shocks for income
global Aswitch
Aswitch = [-speye(I) * lla(1),speye(I) * lla(1);speye(I) * lla(2),-speye(I) * lla(2)];

% Steady state computations
global rmin rmax r0 maxit crit Delta Ir crit_S
rmin = .0001;       % lower bound for steady state interest rate
rmax = rrho;        % upper bound for steady state interest rate
r0 = .005;          % initial guess for steady state interest rate
maxit = 100;        % maximum iterations on steady state HJB
crit = 1e-6;        % error criterion for steady state value function convergence
Delta = 1e4;        % update size for implicit scheme on steady state HJB
Ir = 100;           % maximum iterations on steady state interest rate
crit_S = 1e-5;      % error criterion for steady state interest rate

% Number of variables in the system
global nVars nEErrors n_v n_g n_p
n_v = 2 * I;
n_g = 2 * I-1 + 1;
n_p = 6;
nVars = n_v + n_g + n_p;
nEErrors = 2 * I;

% Spline parameters
global n_splined
n_knots = 12;
c_power = 7;
x = a';
n_post = 2;	        % This is from two income states
n_prior = 1;
n_splined = n_prior*n_knots*n_post;


classdef CBFDT < handle
    properties
        systemParam
        optType
        optParam
        xcurr
        timecurr
        solvertime
        xdim
        udim
        tlog = []
        xlog = []
        ulog = []
        omegalog = []
    end

    methods
        function self = CBFDT(system_param, x0, t0)
            self.systemParam = system_param;
            self.xcurr = x0;
            self.timecurr = t0;
            self.xdim = size(self.systemParam.A, 1);
            self.udim = size(self.systemParam.B, 2);
        end

        function setOpt(self, opt_type, opt_param)
            self.optType = opt_type;
            self.optParam = opt_param;
        end

        function sim(self, total_time)
            xk = self.xcurr;
            while self.timecurr <= total_time
                [feas, xopt, uopt, Jopt] = self.solve();
                if feas ~= 1
                    return;
                end
                uk = uopt(:,1);
                xk = self.systemParam.A * xk + self.systemParam.B * uk;
                self.xcurr = xk;
                self.timecurr = self.timecurr + self.systemParam.timestep;
                self.tlog = [self.tlog, self.timecurr];
                self.xlog = [self.xlog, xk];
                self.ulog = [self.ulog, uk];
            end
        end

        function [feas, xopt, uopt, Jopt] = solve(self)
            if strcmp(self.optType,'dclfdcbf')
                [feas, xopt, uopt, Jopt] = solveDCLFDCBF(self);
            elseif strcmp(self.optType,'nmpcdclfdcbf')
                [feas, xopt, uopt, Jopt] = solveNMPCDCLFDCBF(self);
            elseif strcmp(self.optType,'mpcdcbf')
                [feas, xopt, uopt, Jopt] = solveMPCDCBF(self);
            elseif strcmp(self.optType,'mpcgcbf')
                [feas, xopt, uopt, Jopt] = solveMPCGCBF(self);
            elseif strcmp(self.optType,'nmpcdcbf')
                [feas, xopt, uopt, Jopt] = solveNMPCDCBF(self);
            else
                disp('optimal control policy undefined');
            end
        end

        function [feas, xopt, uopt, Jopt] = solveDCLFDCBF(self)
            x = sdpvar(self.xdim, 2);
            u = sdpvar(self.udim, 1);
            s = sdpvar(1,1);
            cost = 0;
            constraints = [];
            % initial condition
            constraints = [constraints; x(:,1) == self.xcurr];
            % dynamics
            constraints = [constraints; x(:,2) == self.systemParam.A * x(:,1) + self.systemParam.B * u(:,1)];
            % DCLF constraint
            v = x(:,1)' * self.optParam.P * x(:,1);
            vnext = x(:,2)' * self.optParam.P * x(:,2);
            constraints = [constraints; vnext - v + self.optParam.alpha * v <= s];
            % DCBF constraint
            r = 1;
            b = x(:,1)' * x(:,1) - r^2;
            bnext = x(:,2)' * x(:,2) - r^2;
            constraints = [constraints; bnext - b + self.optParam.gamma * b >= 0];
            % input constraint
            constraints = [constraints; self.systemParam.ul <= u <= self.systemParam.uu];
            % cost
            cost = cost + u' * self.optParam.uWeight * u + s' * self.optParam.sWeight * s;
            opt_settings = sdpsettings('solver','ipopt','verbose',0);
            diagnostics = optimize(constraints, cost, opt_settings);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt= [];
                uopt = [];
                Jopt = value(cost);
            end
            self.solvertime = diagnostics.solvertime;
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end

        function [feas, xopt, uopt, Jopt] = solveNMPCDCLFDCBF(self)
            x = sdpvar(self.xdim, self.optParam.horizon + 1);
            u = sdpvar(self.udim, self.optParam.horizon);
            s = sdpvar(1,self.optParam.horizonCLF);
            omega = sdpvar(1,self.optParam.horizonCBF);
            cost = 0;
            constraints = [];
            % initial condition
            constraints = [constraints; x(:,1) == self.xcurr];
            % MPC cost and constraints
            for i = 1:self.optParam.horizon
                constraints = [constraints;
                    self.systemParam.ul <= u(:,i) <= self.systemParam.uu;
                    x(:,i+1) == self.systemParam.A * x(:,i) + self.systemParam.B * u(:,i)];
                cost = cost + x(:,i)'*self.optParam.xWeight*x(:,i);
                cost = cost + u(:,i)'*self.optParam.uWeight*u(:,i);
            end
            cost = cost + x(:,self.optParam.horizon)' * self.optParam.P * x(:,self.optParam.horizon);
            % CLF constraints
            for i = 1:self.optParam.horizonCLF
                v = x(:,1)' * self.optParam.P * x(:,1);
                vnext = x(:,2)' * self.optParam.P * x(:,2);
                constraints = [constraints; vnext - v + self.optParam.alpha * v <= s(i)];
                cost = cost + s(i)' * self.optParam.sWeight * s(i);
            end
            % CBF constraints
            for i = 1:self.optParam.horizonCBF
                r = 1;
                b = x(:,1)' * x(:,1) - r^2;
                bnext = x(:,2)' * x(:,2) - r^2;
                constraints = [constraints; bnext >= omega(i) * (1 - self.optParam.gamma) * b];
                constraints = [constraints; omega(i) >= 0];
                assign(omega(i), 1);
                cost = cost + self.optParam.omegaWeight * (omega(i) - 1)^2;
            end
            opt_settings = sdpsettings('solver','ipopt','verbose',0);
            diagnostics = optimize(constraints, cost, opt_settings);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt= [];
                uopt = [];
                Jopt = value(cost);
            end
            self.solvertime = diagnostics.solvertime;
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end

        function [feas, xopt, uopt, Jopt] = solveMPCDCBF(self)
            x = sdpvar(self.xdim, self.optParam.horizon + 1);
            u = sdpvar(self.udim, self.optParam.horizon);
            cost = 0;
            constraints = [];
            % initial condition
            constraints = [constraints; x(:,1) == self.xcurr];
            % MPC cost and constraints
            for i = 1:self.optParam.horizon
                constraints = [constraints;
                    self.systemParam.ul <= u(:,i) <= self.systemParam.uu;
                    x(:,i+1) == self.systemParam.A * x(:,i) + self.systemParam.B * u(:,i)];
                cost = cost + x(:,i)'*self.optParam.xWeight*x(:,i);
                cost = cost + u(:,i)'*self.optParam.uWeight*u(:,i);
            end
            cost = cost + x(:,self.optParam.horizon)' * self.optParam.P * x(:,self.optParam.horizon);
            % CBF constraints
            for i = 1:self.optParam.horizon
                r = 0;
                b = r - x(1,i);
                bnext = r - x(1,i+1);
                constraints = [constraints; bnext - b + self.optParam.gamma * b >= 0];
            end
            opt_settings = sdpsettings('solver','ipopt','verbose',0);
            diagnostics = optimize(constraints, cost, opt_settings);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt= [];
                uopt = [];
                Jopt = value(cost);
            end
            self.solvertime = diagnostics.solvertime;
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end

        function [feas, xopt, uopt, Jopt] = solveMPCGCBF(self)
            x = sdpvar(self.xdim, self.optParam.horizon + 1);
            u = sdpvar(self.udim, self.optParam.horizon);
            cost = 0;
            constraints = [];
            % initial condition
            constraints = [constraints; x(:,1) == self.xcurr];
            % MPC cost and constraints
            for i = 1:self.optParam.horizon
                constraints = [constraints;
                    self.systemParam.ul <= u(:,i) <= self.systemParam.uu;
                    x(:,i+1) == self.systemParam.A * x(:,i) + self.systemParam.B * u(:,i)];
                cost = cost + x(:,i)'*self.optParam.xWeight*x(:,i);
                cost = cost + u(:,i)'*self.optParam.uWeight*u(:,i);
            end
            cost = cost + x(:,self.optParam.horizon)' * self.optParam.P * x(:,self.optParam.horizon);
            % GCBF constraints
            num_HO = 3;
            r = 0;
            b = r - x(1,1);
            bnext = r - x(1,1+num_HO);
            constraints = [constraints; bnext >= (1 - self.optParam.gamma)^num_HO * b];
            opt_settings = sdpsettings('solver','ipopt','verbose',0);
            diagnostics = optimize(constraints, cost, opt_settings);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt= [];
                uopt = [];
                Jopt = value(cost);
            end
            self.solvertime = diagnostics.solvertime;
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end

        function [feas, xopt, uopt, Jopt] = solveNMPCDCBF(self)
            x = sdpvar(self.xdim, self.optParam.horizon + 1);
            u = sdpvar(self.udim, self.optParam.horizon);
            omega = sdpvar(1,self.optParam.horizonCBF);
            cost = 0;
            constraints = [];
            % initial condition
            constraints = [constraints; x(:,1) == self.xcurr];
            % MPC cost and constraints
            for i = 1:self.optParam.horizon
                constraints = [constraints;
                    self.systemParam.ul <= u(:,i) <= self.systemParam.uu;
                    x(:,i+1) == self.systemParam.A * x(:,i) + self.systemParam.B * u(:,i)];
                cost = cost + x(:,i)'*self.optParam.xWeight*x(:,i);
                cost = cost + u(:,i)'*self.optParam.uWeight*u(:,i);
            end
            cost = cost + x(:,self.optParam.horizon)' * self.optParam.P * x(:,self.optParam.horizon);
            % CBF constraints
            for i = 1:self.optParam.horizonCBF
                r = 0;
                b = r - x(1,i);
                bnext = r - x(1,i+1);
                constraints = [constraints; bnext >= omega(i) * (1 - self.optParam.gamma) * b];
                constraints = [constraints; omega(i) >= 0];
                assign(omega(i), 1);
                cost = cost + self.optParam.omegaWeight * (omega(i) - 1)^2;
            end
            opt_settings = sdpsettings('solver','ipopt','verbose',0);
            diagnostics = optimize(constraints, cost, opt_settings);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt= [];
                uopt = [];
                Jopt = value(cost);
            end
            self.solvertime = diagnostics.solvertime;
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end
    end
end


function ar=lpcim2ar(im)
%LPCIM2AR Convert impulse response to AR coefs AR=(IM)

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

[nf,p1]=size(im);
ar=zeros(nf,p1);
wz=[1 zeros(1,p1-1)];
for k=1:nf
  ar(k,:)=wz/toeplitz(wz,im(k,:)/im(k,1));
end


%
% state = [x, y, yaw, delta]
% input = [v_des, delta_des]
% ref = [x_ref, y_ref, yaw_ref, v_ref]
%
%
%

clear variables;
close all;

set(0, 'defaultAxesFontSize', 12);
set(0, 'defaultTextFontSize', 20);
set(0, 'DefaultAxesLineWidth', 1.0, 'DefaultLineLineWidth', 1.0);

addpath ../path_design

control_mode_option = ["pure_pursuit", "pid", "mpc", "mpc_no_constraints"];
control_mode = control_mode_option(3);

save_video = 0; %1:save, 0:no

%% preliminaries
rad2deg = 180 / pi;
deg2rad = pi / 180;
kmh2ms = 1000 / 3600;

simulation_time = 35;
simulation_rk4_time_step = 0.002; % simulation time step

vel_ref = 30 * kmh2ms;

% for dynamics model
param.tau = 0.27; % steering dynamics: 1d-approximated time constant
param.wheelbase = 2.69;
param.steer_lim = 30 * deg2rad;
param.vel_max = 10;
param.vel_min = -5;

param.input_delay = 0.24; % [s]
param.control_dt = 0.03; % [s]
param.measurement_noise_stddev = [0.1, 0.1, 1.0*deg2rad, 0.5*deg2rad]; % measurement noise
% param.measurement_noise_stddev = [0,0,0,0]; % measurement noise
param.steering_steady_state_error_deg = 1;

% for pure pursuit only
param.pure_pursuit_lookahead = 8.0; % [m]

% for mpc only
param.mpc_dt = 0.1;
param.mpc_n = 30;
param.mpc_constraint_steering_deg = 30;
param.mpc_constraint_steer_rate_deg = 280;
param.mpc_model_dim = 3;
param.mpc_Q = diag([1,2]);
param.mpc_R = 0.5;
param.mpc_delay_comp_step = round(param.input_delay / param.control_dt);
% param.mpc_delay_comp_step = 0.0;

% use the input ahead of the delay time
param.mpc_sensor_delay = param.input_delay;

% for mpc2
param.mpc2_dt = 0.2;
param.mpc2_n = 10;
param.mpc2_steering_lim_deg = 40;
param.mpc2_model_dim = 4;
param.mpc2_Q = diag([1,1,0]);
param.mpc2_R = 0.05;

%% simulation parameters

% initial position (x, y, yaw, delta)
x0 = [0, 0.5, 0, 0];

ts = 0;
dt = simulation_rk4_time_step;
tf = simulation_time;
t = ts:dt:tf;

%% reference trajectory design

path_design; % using spline
load path; % x, y, yaw

ref = zeros(length(path), 6);
IDX_X = 1;
IDX_Y = 2;
IDX_XY = 1:2;
IDX_XYYAW = 1:3;
IDX_YAW = 3;
IDX_VEL = 4;
IDX_CURVATURE = 5;
IDX_TIME = 6;

IDX_STEER = 4;

path_size_scale = 15;
path(:,IDX_XY) = path(:,IDX_XY) * path_size_scale;
ref(:,IDX_XYYAW) = path(:,IDX_XYYAW);

ref(:,IDX_VEL) = ones(length(path),1)*vel_ref;

% insert curvature into path
for i = 2:length(ref)-1
    p1_ = ref(i-1,IDX_XY);
    p2_ = ref(i, IDX_XY);
    p3_ = ref(i+1, IDX_XY);
    A_ = ((p2_(1)-p1_(1))*(p3_(2)-p1_(2)) - (p2_(2)-p1_(2))*(p3_(1)-p1_(1))) / 2;
    ref(i, IDX_CURVATURE) = 4 * A_ / (norm(p1_-p2_) * norm(p2_-p3_) * norm(p3_-p1_));
end

% insert relative time into path
for i = 2:length(ref)
    v_ = ref(i,IDX_VEL);
    d_ = norm(ref(i,IDX_XY)-ref(i-1,IDX_XY));
    dt_ = d_ / v_;
    ref(i, IDX_TIME) = ref(i-1, IDX_TIME) + dt_;
end

%% simulation

if control_mode == "pure_pursuit"
    [X, U, debug] = simulate_rk4(@kinematics_model, @pure_pursuit, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);
elseif control_mode == "pid"
   [X, U, debug] = simulate_rk4(@kinematics_model, @pid_controller, x0, ref, ts, dt, tf, param);
   lat_error_vec = debug(:,end);
elseif control_mode == "mpc"
    param.mpc_solve_without_constraint = false;
    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);
elseif control_mode == "mpc_no_constraints"
    param.mpc_solve_without_constraint = true;
    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);
elseif control_mode == "mpc2"
    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller2, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);
end
fprintf("lattitude error: mean square = %f, max = %f", norm(lat_error_vec)/simulation_time, max(lat_error_vec));

%% movie plot

sp_num = 18;
subpl1 = 'subplot(sp_num,sp_num, sp_num+1:sp_num*12);';
subpl2 = 'subplot(sp_num,sp_num, sp_num*13+1:sp_num*15);';
subpl3 = 'subplot(sp_num,sp_num, sp_num*16+1:sp_num*18);';

% tire2steer = 12.5;

fig_trajectory_result = figure(1);
set(fig_trajectory_result, 'Position', [716 735 1026 1146]);
eval(subpl1);
plot(ref(:,1), ref(:,2),'k-.'); hold on; grid on;
% plot(X(:,1), X(:,2));
% legend('ref','tracked');
xlabel('x [m]'); ylabel('y [m]');
% img_orig = imread('handle.jpg');
% img = imrotate(img_orig, 10);
% handle_plt = image([150, 170],[110, 90], img);
% handle_plt_point = [160, 100, 0];

eval(subpl2);
plot(t, lat_error_vec, 'b'); grid on; hold on;
xlabel('t [s]'); ylabel('latitude error [m]');
ulim = ceil(2*max(lat_error_vec))/2;
dlim = floor(2*min(lat_error_vec))/2;
ylim([dlim, ulim]);

eval(subpl3);
p1 = plot(t, X(:,IDX_STEER)*rad2deg, 'b'); grid on; hold on;
p2 = plot(t, U(:,2)*rad2deg, 'Color', [0.7 0. 1]); hold on;
legend([p1,p2], {'measured','command'})
xlabel('t [s]'); ylabel('steering angle [deg]');
ulim = round(2*max(X(:,IDX_STEER)*rad2deg))/2;
dlim = round(2*min(X(:,IDX_STEER)*rad2deg))/2;
ylim([dlim, ulim]);

z_axis = [0 0 1];
setpoint = []; rear_tire = []; front_tire = []; body = []; tracked = [];
setpoint_ideal = []; error_point = []; steer_point = []; time_bar_laterror = []; time_bar_steer = [];
L = param.wheelbase;
rear_length = 1;
front_length = 1;
side_width = 0.9;
fig_draw_i = 1:round(1/dt/20):length(t);

% for movie
clear frame_vec;
frame_vec(length(fig_draw_i)) = struct('cdata', [], 'colormap',[]);

j = 1;
fig_trajectory_result; hold on;
for i = fig_draw_i
    eval(subpl1);
    disp(t(i))
    rear_x = X(i,1);
    rear_y = X(i,2);
    yaw = X(i,3);
    delta = X(i,IDX_STEER);
    front_x = rear_x + L;
    front_y = rear_y;
    delete([setpoint, rear_tire, front_tire, body, tracked, setpoint_ideal, error_point, steer_point, time_bar_laterror, time_bar_steer]);
    tracked = plot(X(1:i,1), X(1:i,2),'r');
    title_draw = "t = "+num2str(t(i),'%5.1f') + "[s], steer = " + num2str(delta*rad2deg,'%+3.1f') + "[deg], v = " + ...
        num2str(vel_ref*3600/1000,'%3.1f') + "[km/h], lat error = "+num2str(lat_error_vec(i),'%+2.2f') + "[m]";
    title_draw = [title_draw; "Simulation: solver = rk4, sensor-delay = "  + num2str(param.input_delay*1000, '%d') + "[ms], control freq=" + ...
        num2str(1/param.control_dt, '%d') + "[hz]"];
    title_draw = [title_draw; "noise-sigma = " + num2str(param.measurement_noise_stddev(1),'%2.2f')+"(pos), "+ ...
        num2str(param.measurement_noise_stddev(3),'%2.2f')+"(yaw), "+num2str(param.measurement_noise_stddev(4),'%2.2f')+"(steer)"];
    if control_mode == "mpc" || control_mode == "mpc_no_constraints"
        title_draw = [title_draw; "MPC: dt = " + num2str(param.mpc_dt, '%3.3f') + "[s], horizon step = " + num2str(param.mpc_n, '%d')];
%         pred_states = debug(i, param.mpc_n+1:param.mpc_n*(4+1));
%         pred_states = reshape(pred_states, param.mpc_n, length(pred_states)/param.mpc_n);
%         setpoint = plot(pred_states(:,1), pred_states(:,2), 'bo'); % include linealize error
        pred_error = debug(i, param.mpc_n*(4+1)+1:param.mpc_n*(2+4+1));
        pred_error = reshape(pred_error, param.mpc_n, length(pred_error)/param.mpc_n);
        setpoint_ideal = plot(pred_error(:,1), pred_error(:,2), 'mx'); % without linealize error
    elseif control_mode == "mpc2"
        title_draw = [title_draw; "MPC2: dt = " + num2str(param.mpc_dt, '%3.3f') + "[s], horizon step = " + num2str(param.mpc_n, '%d')];
        pred_states = debug(i, param.mpc_n+1:param.mpc_n*(4+1));
        pred_states = transpose(reshape(pred_states, length(pred_states)/param.mpc_n, param.mpc_n));
        setpoint = plot(pred_states(:,1), pred_states(:,2), 'bo'); % include linealize error
        pred_error = debug(i, param.mpc_n*(4+1)+1:param.mpc_n*(4+4+1));
        pred_error = transpose(reshape(pred_error, length(pred_error)/param.mpc_n, param.mpc_n));
        setpoint_ideal = plot(pred_error(:,1), pred_error(:,2), 'mx'); % without linealize error
    elseif control_mode == "pure_pursuit"
        title_draw = [title_draw; "pure-pursuit: lookahead dist="+num2str(param.pure_pursuit_lookahead, '%1.1f')+"[m]"];
        sp = debug(i,:);
        setpoint = plot(sp(1), sp(2), 'ro');
    elseif control_mode == "pid"
        title_draw = [title_draw; "PID: kp = " + num2str(0.3, '%3.3f') + ", ki = " + num2str(0, '%3.3f') + ", kd = " + num2str(1.5, '%3.3f')];
        sp = debug(i,:);
        setpoint = plot(sp(1), sp(2), 'ro');
    end
    rear_tire = plot([rear_x-0.3, rear_x+0.3],[rear_y, rear_y], 'k', 'LineWidth', 2.0);
    front_tire = plot([front_x-0.3, front_x+0.3],[front_y, front_y], 'k', 'LineWidth', 2.0);
    body = plot([rear_x-rear_length, front_x+front_length, front_x+front_length, rear_x-rear_length, rear_x-rear_length], ...
        [rear_y-side_width, front_y-side_width, front_y+side_width, rear_y+side_width, rear_y-side_width],'k');
    rear_origin = [rear_x, rear_y, 0];
    front_origin = [rear_x + L*cos(yaw), rear_y + L*sin(yaw), 0];
    rotate(body, z_axis, yaw * rad2deg, rear_origin);
    rotate(rear_tire, z_axis, yaw * rad2deg, rear_origin);
    rotate(front_tire, z_axis, yaw * rad2deg, rear_origin);
    rotate(front_tire, z_axis, delta * rad2deg, front_origin);
%     rotate(handle_plt, [0,0,1], delta*tire2steer*rad2deg, handle_plt_point)
    title(title_draw);
    xlim([0 120]);

    % lat error
    eval(subpl2);
    error_point = plot(t(i), lat_error_vec(i), 'ko');
    time_bar_laterror = plot([t(i), t(i)], [100, -100], 'k');

    % steering
    eval(subpl3);
    steer_point = plot(t(i), X(i, IDX_STEER)*rad2deg, 'ko');
    time_bar_steer = plot([t(i), t(i)], [100, -100], 'k');
    legend([p1,p2], {'measured','command'})
    ylim([-40 40]);

    drawnow;
    frame_vec(j) = getframe(fig_trajectory_result);

    j = j + 1;
end

% for video save
if (save_video == 1)
    cd ./movie
    frame_vec(1) = [];
    vidObj = VideoWriter('result.avi');
    vidObj.FrameRate = 25;
    open(vidObj);
    writeVideo(vidObj, frame_vec);
    close(vidObj);
    cd ../
end

% plot_pid;


% [PYR, INDICES] = buildLpyr(IM, HEIGHT, FILT1, FILT2, EDGES)
%
% Construct a Laplacian pyramid on matrix (or vector) IM.
%
% HEIGHT (optional) specifies the number of pyramid levels to build. Default
% is 1+maxPyrHt(size(IM),size(FILT));
%
% FILT1 (optional) can be a string naming a standard filter (see
% namedFilter), or a vector which will be used for (separable)
% convolution.  Default = 'binom5'.  FILT2 specifies the "expansion"
% filter (default = filt1).  EDGES specifies edge-handling, and
% defaults to 'reflect1' (see corrDn).
%
% PYR is a vector containing the N pyramid subbands, ordered from fine
% to coarse.  INDICES is an Nx2 matrix containing the sizes of
% each subband.  This is compatible with the MatLab Wavelet toolbox.

% Eero Simoncelli, 6/96.

function [pyr,pind] = buildLpyr(im, ht, filt1, filt2, edges)

if (nargin < 1)
  error('First argument (IM) is required');
end

im_sz = size(im);

%------------------------------------------------------------
%% OPTIONAL ARGS:

if (exist('filt1') ~= 1)
  filt1 = 'binom5';
end

if isstr(filt1)
  filt1 = namedFilter(filt1);
end

if ( (size(filt1,1) > 1) & (size(filt1,2) > 1) )
  error('FILT1 should be a 1D filter (i.e., a vector)');
else
  filt1 = filt1(:);
end

if (exist('filt2') ~= 1)
  filt2 = filt1;
end

if isstr(filt2)
  filt2 = namedFilter(filt2);
end

if ( (size(filt2,1) > 1) & (size(filt2,2) > 1) )
  error('FILT2 should be a 1D filter (i.e., a vector)');
else
  filt2 = filt2(:);
end

max_ht = 1 + maxPyrHt(im_sz, max(size(filt1,1), size(filt2,1)));
if (exist('ht') ~= 1)
  ht = max_ht;
else
  if (ht > max_ht)
    error(sprintf('Cannot build pyramid higher than %d levels.',max_ht));
  end
end

if (exist('edges') ~= 1)
  edges= 'reflect1';
end

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

if (ht <= 1)

  pyr = im(:);
  pind = im_sz;

else

  if (im_sz(2) == 1)
    lo2 = corrDn(im, filt1, edges, [2 1], [1 1]);
  elseif (im_sz(1) == 1)
    lo2 = corrDn(im, filt1', edges, [1 2], [1 1]);
  else
    lo = corrDn(im, filt1', edges, [1 2], [1 1]);
    int_sz = size(lo);
    lo2 = corrDn(lo, filt1, edges, [2 1], [1 1]);
  end

  [npyr,nind] = buildLpyr(lo2, ht-1, filt1, filt2, edges);

  if (im_sz(1) == 1)
    hi2 = upConv(lo2, filt2', edges, [1 2], [1 1], im_sz);
  elseif (im_sz(2) == 1)
    hi2 = upConv(lo2, filt2, edges, [2 1], [1 1], im_sz);
  else
    hi = upConv(lo2, filt2, edges, [2 1], [1 1], int_sz);
    hi2 = upConv(hi, filt2', edges, [1 2], [1 1], im_sz);
  end

  hi2 = im - hi2;

  pyr = [hi2(:); npyr];
  pind = [im_sz; nind];

end


% Fig. 6.13   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clear all;
%close all;
clf

num=10;
den=[1 1 0];
w=logspace(-2,1,100);
[m,p]=bode(num,den,w);
wa=[.01 1.2];
ma=[1000 8.3333];
loglog(w,m,w,10*ones(1,100),'--',wa,ma);
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.13 Determination of Kv from the Bode plot');
bodegrid;


function W = upgrading_merge(W, miu)
N = size(W, 2);%N is not a constant if merge is needed
if N <= miu
    return
else
%         sum_before_merge = 0;
%         for i = 1 : N/2
%           sum_before_merge = sum_before_merge + norm(W(:, i) - W(end:-1:1, N - i + 1));
%         end
%         sum_before_merge
%
%         if sum_before_merge > 0.01
%             sum(W(1, :))
%              sum(W(2, :))
%              W
%         end
%
%         if sum_before_merge ~= 0
%             sum_before_merge
%             W
%             sum(W(1, :))
%             sum(W(2, :))
%             LLR = log(W(1,:)) - log(W(2,:))
%             [~, orderd] = sort(LLR, 'descend')
%         end

    epsilon = 1e-3;
    while(N > miu)

        W_first_half = W(:, 1 : N/2);
        LR = W_first_half(1, :)./W_first_half(2, :);
        numerical_warning = 0;

        for i = 1 : N/2 - 1
            ratio = LR(i)/LR(i + 1);
            if ratio < 1 + epsilon
                numerical_warning = 1;
                break;
            end
        end

        if numerical_warning == 1

            min_deltaI = realmax;
            min_index = 0;
            for i = 1 : N/2 - 1
                a2 = W(1, i);
                b2 = W(2, i);
                a1 = W(1,i + 1);
                b1 = W(2, i + 1);
                deltaI = delta_capacity_lemma9(a1, a2, b1, b2);
                if deltaI < min_deltaI %find minimum delta I
                    min_deltaI = deltaI;
                    min_index = i;
                end
            end

            if min_index == 0
                for k = 1 : N/2
                    if sum(W(:, k)) < 1e-20
                        min_index = k;
                        W(:, min_index) = [];
                        W(:, N - min_index) = [];
                        N = size(W, 2);
                        break;
                    end
                end
                continue;
            end

            a2 = W(1, min_index);
            b2 = W(2, min_index);
            a1 = W(1, min_index + 1);
            b1 = W(2, min_index + 1);

            if a2/b2 < inf
                lambda2 = a2/b2;
                alpha2 = lambda2 * (a1 + b1)/(lambda2 + 1);
                beta2 = (a1 + b1)/(lambda2 + 1);
            else
                alpha2 = a1 + b1;
                beta2 = 0;
            end

            W(1, min_index) =  a2 + alpha2;
            W(2, min_index) =  b2 + beta2;
            W(1, N - min_index + 1) =  b2 + beta2;
            W(2, N - min_index + 1) =  a2 + alpha2;

            W(:, min_index + 1) = [];
            W(:, N - min_index - 1) = [];
            N = size(W, 2);

        else

            min_deltaI = realmax;
            min_index = 0;

            for i = 1 : N/2 - 2
                a3 = W(1, i);
                b3 = W(2, i);
                a2 = W(1,i + 1);
                b2 = W(2, i + 1);
                a1 = W(1,i + 2);
                b1 = W(2, i + 2);

                deltaI = delta_capacity_lemma11(a1, a2, a3, b1, b2, b3);

                if deltaI < min_deltaI %find minimum delta I
                    min_deltaI = deltaI;
                    min_index = i;
                end
            end

            if min_index == 0
                for k = 1 : N/2
                    if sum(W(:, k)) < 1e-20
                        min_index = k;
                        W(:, min_index) = [];
                        W(:, N - min_index) = [];
                        N = size(W, 2);
                        break;
                    end
                end
                continue;
            end

            a3 = W(1, min_index);
            b3 = W(2, min_index);
            a2 = W(1, min_index + 1);
            b2 = W(2, min_index + 1);
            a1 = W(1, min_index + 2);
            b1 = W(2, min_index + 2);

            lambda1 = a1/b1;
            if a3/b3 < inf
                lambda3 = a3/b3;
                alpha1 = lambda1 * (lambda3 * b2 - a2) / (lambda3 - lambda1);
                beta1 = (lambda3 * b2 - a2) / (lambda3 - lambda1);
                alpha3 = lambda3 * (a2 - lambda1 * b2) / (lambda3 - lambda1);
                beta3 = (a2 - lambda1 * b2) / (lambda3 - lambda1);
            else
                alpha1 = lambda1 * b2;
                beta1 = b2;
                alpha3 = a2 - lambda1 * b2;
                beta3 = 0;
            end

            W(1, min_index) =  a3 + alpha3;
            W(2, min_index) =  b3 + beta3;

            W(1, min_index + 1) =  a1 + alpha1;
            W(2, min_index + 1) =  b1 + beta1;

            W(1, N - min_index + 1) =  b3 + beta3;
            W(2, N - min_index + 1) =  a3 + alpha3;

            W(1, N - min_index) =  b1 + beta1;
            W(2, N - min_index) =  a1 + alpha1;

            W(:, min_index + 2) = [];
            W(:, N - min_index - 2) = [];
            N = size(W, 2);

%             if N == miu
%                 W
%             end
        end
    end

%         sum_after_merge = 0;
%         for i = 1 : N/2
%             sum_after_merge = sum_after_merge + norm(W(:, i) - W(end:-1:1, N - i + 1));
%         end
%         sum_after_merge
% W
end
end


% RSADJUST - adjust l-values (Ramberg-Schmeiser distribution)
%                with respect to signal mean and variance
%
% Usage: p = rsadjust(l3, l4, m, var, skew)
%
% Input:
%   l3   - value lambda3 for Ramberg-Schmeiser distribution
%   l4   - value lambda4 for Ramberg-Schmeiser distribution
%   m    - mean of the signal distribution
%   var  - variance of the signal distribution
%   skew - skewness of the signal distribution (only the sign of
%          this parameter is used).
%
% Output:
%   l1  - value lambda3 for Ramberg-Schmeiser distribution
%   l2  - value lambda4 for Ramberg-Schmeiser distribution
%   l3  - value lambda3 for Ramberg-Schmeiser distribution (copy
%         from input)
%   l4  - value lambda4 for Ramberg-Schmeiser distribution (copy
%         from input)
%
% Author: Arnaud Delorme, SCCN, 2003
%
% See also: RSFIT, RSGET
%
% Reference: Ramberg, J.S., Tadikamalla, P.R., Dudewicz E.J., Mykkytka, E.F.
%            A probability distribution and its uses in fitting data.
%            Technimetrics, 1979, 21: 201-214.

% Copyright (C) 2003 Arnaud Delorme, SCCN, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [l1,l2,l3,l4] = rsadjust( l3, l4, mu, sigma2, m3);

    % swap l3 and l4 for negative skewness
    % ------------------------------------
    if m3 < 0
        ltmp = l4;
        l4   = l3;
        l3   = ltmp;
    end

    A = 1/(1 + l3) - 1/(1 + l4);
    B = 1/(1 + 2*l3) + 1/(1 + 2*l4) - 2*beta(1+l3, 1+l4);
    C = 1/(1 + 3*l3) - 1/(1 + 3*l4) ...
             - 3*beta(1+2*l3, 1+l4) + 3*beta(1+l3, 1+2*l4);

    % compute l2 (and its sign)
    % ------------------------
    l2 = sqrt( (B-A^2)/sigma2 );
    if m3 == 0, m3 = -0.000000000000001; end
    if (m3*(C - 2*A*B + 2*A^3)) < 0, l2 = -l2; end
    %l22 = ((C - 2*A*B + 2*A^3)/m3)^(1/3) % also equal to l2

    % compute l1
    % ----------
    l1 = mu - A/l2;

    return;

    % fitting table 1 of
    % ---------------
    [l1 l2]  = pdffitsolv2(-.0187,-.0388, 0, 1, 1)
    [l1 l2]  = pdffitsolv2(-.1359,-.1359, 0, 1, 0)

    % sign problem
    [l1 l2]  = pdffitsolv2(1.4501,1.4501, 0, 1)

    % numerical problem for l1
    [l1 l2]  = pdffitsolv2(-.00000407,-.001076, 0, 1, 2)
    [l1 l2]  = pdffitsolv2(0,-.000580, 0, 1, 2)


function VectorFitting_Sparam ()
clear; close all; FONTSIZE=20;

file='../DC_ringmod_type1_R=10,gap=180,Lc=0,wg=500,lambda=1550,mesh=2,angle=30.mat';
load (file)

c=3e8; wavelength=c./f*1e6;
fSCALING = 1e14; f=f/fSCALING; s=1i*2*pi*f;  Np=length(f);
hwait = waitbar(0,'Please wait...');

% average the S parameters for the symmetric parameters that were simulated twice,
% by considering amplitude and phase separately
S1221 = (abs(S12)+abs(S21))/2 .* exp ( 1i * (unwrap(angle(S12)) + unwrap(angle(S21)) ) /2 );
S12=S1221; S21 = S1221;
S4132 = (abs(S41)+abs(S32))/2 .* exp ( 1i * (unwrap(angle(S41)) + unwrap(angle(S32)) ) /2 );
S41=S4132; S32 = S4132;
S13=S31; S23=S41; S33=S11; S43=S21; S14=S32; S24=S42; S34=S12; S44=S22;

for i=1:Np
  Sparam = [ [ S11(i),S12(i),S13(i),S14(i)];
    [ S21(i),S22(i),S23(i),S24(i)];
    [ S31(i),S32(i),S33(i),S34(i)];
    [ S41(i),S42(i),S43(i),S44(i)] ] ;
  Test1(i) = norm(Sparam);
  Sparam_w (:,:,i)=Sparam;
end

% Check if S-Parameters are already passive; if not, perform Vector Fit,
% otherwise, export.
if ~isempty(find(Test1>1))

  % rational fit, sweep number of parameters, N:
  optsN=20:1:100;
  opts.poletype='lincmplx'; opts.parametertype='S'; opts.stable=0; opts.Niter_out=10;
  Npassive=[]; rms3passive=[];
  for i=1:length(optsN)
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    opts.N=optsN(i);    % Vector Fit:
    [SER,rmserr,Hfit,opts2]=VFdriver(Sparam_w,s,[],opts);

    %%%%%%%%%%%%%%%%%%%%%%  Enforce Passivity:
    [SER,H_passive,opts3,wintervals]=RPdriver(SER,s,opts);
    % Note: added output parameter wintervals to RPdriver.

    % Find rms error:
    tell=0; Nc=length(SER.D);
    for col=1:Nc
      for row=col:Nc % makes assumption that S = S'
        tell=tell+1; % make a single vector:
        Sparam10(tell,:)= squeeze(Sparam_w(row,col,:)).';
        fit10(tell,:)= squeeze(Hfit(row,col,:)).';
        fitP(tell,:)= squeeze(H_passive(row,col,:)).';
      end
    end
    rms2(i) = sqrt(sum(sum(abs((Sparam10-fit10).^2))))/sqrt(4*Np);
    rms3(i) = sqrt(sum(sum(abs((Sparam10-fitP).^2))))/sqrt(4*Np);

    % Determine if the Passivity Enforcement was successful.
    if (rms3(i) < 1) && isempty(wintervals)
      Npassive=[Npassive optsN(i)]; rms3passive = [rms3passive rms3(i)];
    end

    waitbar (i/length(optsN), hwait);
    if (rms3(i) < 1e-4) && isempty(wintervals) ; break; end
  end
  close (hwait);

  % Plot error versus number of fitting parameters:
  figure;
  semilogy(optsN(1:i),rms2,'ro-', 'LineWidth',2, 'MarkerSize',7); hold all;
  labels={}; labels{end+1} = 'Vector Fit, rms';
  plot (Npassive, rms3passive, 'kx', 'MarkerSize',14, 'LineWidth',3);
  labels{end+1} = 'Passivity-Enforced Fit, rms';
  xlabel('Fitting order, N');   ylabel('rms error');
  for ii=1:length(labels); labels{ii}=[labels{ii} ' ' char(31) ]; end;
  legend (labels,'Location','NorthEast');
  printfig(file,'convergence');

  % Plot residue values:
  figure;
  residues=sort(squeeze(prod(prod(abs(SER.R),1),2).^(1/4)), 1, 'descend');
  semilogy(residues,'-s', 'LineWidth',3, 'MarkerSize',8);
  xlabel('Residue index'); ylabel('Residue magnitude');
  printfig(file,'residues');

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Plot S-Parameters + fit functions:
  figure;
  Nc=length(SER.D);
  for row=1:Nc
    for col= row:Nc
      dum1=squeeze(Sparam_w(row,col,:));
      dum2=squeeze(H_passive(row,col,:));
      h1=semilogy(wavelength,abs(dum1),'b','LineWidth',3); hold on
      h2=semilogy(wavelength,abs(dum2),'r-.','LineWidth',4);
      h3=semilogy(wavelength,abs(dum2-dum1),'g--','LineWidth',3);
    end
  end
  hold off
  xlabel('Wavelength'); ylabel('Amplitude [S]');
  axis tight; Yl =ylim; ylim ([Yl(1)*10 Yl(2)*2]);
  labels={}; labels{end+1} = 'FDTD S-Parameters';
  labels{end+1} = 'Passivity-enforced Fit'; labels{end+1}='Deviation';
  for i=1:length(labels); labels{i}=[labels{i} ' ' char(31)]; end;
  legend (labels,'Location','Best');
  printfig(file,'VF2a');

  figure;
  Nc=length(SER.D);
  for row= 1:Nc  %1
    for col= row:Nc  %3
      dum1=squeeze(Sparam_w(row,col,:));
      dum2=squeeze(H_passive(row,col,:));
      h1=semilogy(wavelength,unwrap(angle(dum1)),'b','LineWidth',3); hold on
      h2=plot(wavelength,unwrap(angle(dum2)),'r-.','LineWidth',4);
      h3=plot(wavelength,abs(unwrap(angle(dum2))-unwrap(angle(dum1))),'g--','LineWidth',3);
    end
  end
  hold off
  xlabel('Wavelength'); ylabel('Phase [S]');
  axis tight; Yl =ylim; ylim ([Yl(1)*10 Yl(2)*2]);
  labels={}; labels{end+1} = 'FDTD S-Parameters';
  labels{end+1} = 'Passivity-enforced Fit'; labels{end+1}='Deviation';
  for i=1:length(labels); labels{i}=[labels{i} ' ' char(31)]; end;
  legend (labels,'Location','Best');
  printfig(file,'VF2p');

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Passivity test results:
  for i=1:length(f)
    Test0(i) = norm(Sparam_w(:,:,i));
    Test1(i) = norm(Hfit(:,:,i));
    Test2(i) = norm(H_passive(:,:,i));
  end
  figure;
  plot (wavelength,Test0,'LineWidth',2); hold all;
  plot (wavelength,Test1,'--','LineWidth',2)
  plot (wavelength,Test2,'LineWidth',4)
  plot(wavelength,ones(length(f),1),'--','LineWidth',3);
  labels={}; labels{1} = 'FDTD S-Parameters'; labels{2} = 'Rational Fit'; labels{3} = 'Passivity-enforced Fit'; labels{4}='Passivity limit';
  for i=1:length(labels); labels{i}=[labels{i} ' ' char(31)]; end; legend (labels,'Location','Best');
  axis tight
  xlabel ('Wavelength [\mum]');
  ylabel ('S-Parameter Passivity Test');
  printfig(file,'passivitytest3');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% export S parameters to INTERCONNECT
fid = fopen([file '.sparam'],'w');  Nc=4;
for row= 1:Nc
  for col= 1:Nc
    fprintf(fid,'%s\n',[ '(''port ' num2str(col) ''',''TE'',1,''port ' num2str(row) ''',1,''transmission'')' ]  );
    fprintf(fid,'%s\n',[ '(' num2str(Np) ',3)' ]  );
    dum2=squeeze(H_passive(row,col,:));
    mag = abs(dum2);
    phase = unwrap (angle(dum2)); % figure; plot (wavelength, phase);
    for i=1:Np
      fprintf(fid,'%g	%g	%g\n', f(i)*1e14, mag(i), phase(i));
    end
  end
end
fclose(fid);

function printfig (file, b)
global PRINT_titles;
PRINT_titles=0;
FONTSIZE=20;
set(get(gca,'xlabel'),'FontSize',FONTSIZE);
set(get(gca,'ylabel'),'FontSize',FONTSIZE);
set(get(gca,'title'),'FontSize',FONTSIZE-5);
set(gca,'FontSize',FONTSIZE-2);
if PRINT_titles==0
  delete(get(gca,'title'))
end
%a=strfind(file,'.'); file(a)=',';
pdf = [file(1:end-4) '_' b '.pdf'];
print ('-dpdf','-r300', pdf);
system([ 'pdfcrop ' pdf ' ' pdf ' &' ]);
% system(['acroread ' pdf '.pdf &']);


function [G11, G12, G21, G22] = d2Sbus_dV2(Ybus, V, lam, vcart)
%D2SBUS_DV2   Computes 2nd derivatives of power injection w.r.t. voltage.
%
%   The derivatives can be take with respect to polar or cartesian coordinates
%   of voltage, depending on the 4th argument.
%
%   [GAA, GAV, GVA, GVV] = D2SBUS_DV2(YBUS, V, LAM)
%   [GAA, GAV, GVA, GVV] = D2SBUS_DV2(YBUS, V, LAM, 0)
%
%   Returns 4 matrices containing the partial derivatives w.r.t. voltage angle
%   and magnitude of the product of a vector LAM with the 1st partial
%   derivatives of the complex bus power injections.
%
%   [GRR, GIR, GIR, GII] = D2SBUS_DV2(YBUS, V, LAM, 1)
%
%   Returns 4 matrices containing the partial derivatives w.r.t. real and
%   imaginary parts of voltage of the product of a vector LAM with the 1st
%   partial derivatives of the complex bus power injections.
%
%   Takes sparse bus admittance matrix YBUS, voltage vector V and nb x 1 vector
%   of multipliers LAM. Output matrices are sparse.
%
%   Examples:
%       [Ybus, Yf, Yt] = makeYbus(baseMVA, bus, branch);
%       [Gaa, Gav, Gva, Gvv] = d2Sbus_dV2(Ybus, V, lam);
%
%       Here the output matrices correspond to:
%           Gaa = d/dVa (dSbus_dVa.' * lam)
%           Gav = d/dVm (dSbus_dVa.' * lam)
%           Gva = d/dVa (dSbus_dVm.' * lam)
%           Gvv = d/dVm (dSbus_dVm.' * lam)
%
%       [Grr, Gri, Gir, Gii] = d2Sbus_dV2(Ybus, V, lam, 1);
%
%       Here the output matrices correspond to:
%           Grr = d/dVr (dSbus_dVr.' * lam)
%           Gri = d/dVi (dSbus_dVr.' * lam)
%           Gir = d/dVr (dSbus_dVi.' * lam)
%           Gii = d/dVi (dSbus_dVi.' * lam)
%
%   For more details on the derivations behind the derivative code used
%   in MATPOWER, see:
%
%   [TN2]  R. D. Zimmerman, "AC Power Flows, Generalized OPF Costs and
%          their Derivatives using Complex Matrix Notation", MATPOWER
%          Technical Note 2, February 2010. [Online]. Available:
%          https://matpower.org/docs/TN2-OPF-Derivatives.pdf
%          doi: 10.5281/zenodo.3237866
%   [TN4]  B. Sereeter and R. D. Zimmerman, "AC Power Flows and their
%          Derivatives using Complex Matrix Notation and Cartesian
%          Coordinate Voltages," MATPOWER Technical Note 4, April 2018.
%          [Online]. Available: https://matpower.org/docs/TN4-OPF-Derivatives-Cartesian.pdf
%          doi: 10.5281/zenodo.3237909

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

%% default input args
if nargin < 4
    vcart = 0;      %% default to polar coordinates
end

n = length(V);

diaglam = sparse(1:n, 1:n, lam, n, n);
if vcart
    E = diaglam * conj(Ybus);
    F = E + E.';
    G = 1j * (E - E.');

    G11 = F;        %% Grr
    G21 = G;        %% Gir
    G12 = G21.';    %% Gri
    G22 = G11;      %% Gii
else
    Ibus    = Ybus * V;
    diagV   = sparse(1:n, 1:n, V, n, n);

    A = sparse(1:n, 1:n, lam .* V, n, n);
    B = Ybus * diagV;
    C = A * conj(B);
    D = Ybus' * diagV;
    E = conj(diagV) * (D * diaglam - sparse(1:n, 1:n, D*lam, n, n));
    F = C - A * sparse(1:n, 1:n, conj(Ibus), n, n);
    G = sparse(1:n, 1:n, ones(n, 1)./abs(V), n, n);

    G11 = E + F;
    G21 = 1j * G * (E - F);
    G12 = G21.';
    G22 = G * (C + C.') * G;
end


 function proj = cuboid_proj(cg, params, varargin)
%function proj = cuboid_proj(cg, params, varargin)
%|
%| Compute set of 2d line-integral projection views of cuboids.
%| Works for both parallel-beam and cone-beam geometry.
%|
%| in
%|	cg			ct_geom()
%|	params [ne 9]		cuboid parameters:
%|			[x_center y_center z_center  x_radius y_radius z_radius
%|				xy_angle_degrees z_angle_degrees  amplitude]
%| options
%|	oversample		over-sampling factor for emulating "strips"
%|				(to account for finite detector size)
%|
%| out
%|	proj	[ns nt na]	projection views
%|
%| Yong Long, 2008-08-28, University of Michigan, adapted from ellipsoid_proj()
%| 2013-05-24 Jeff Fessler

if nargin == 1 && streq(cg, 'test'), cuboid_proj_test, return, end
if nargin < 2, ir_usage, end

arg.oversample = 1;
arg = vararg_pair(arg, varargin);

proj = cuboid_proj_do(params, cg.s, cg.t, cg.dt, cg.ar, cg.source_zs, ...
		cg.dso, cg.dod, cg.dfs, arg.oversample);

end % cuboid_proj()

% cuboid_proj_line1()
function [lxmin lxmax] = cuboid_proj_line1(rx, p1, e1)
tmp = (e1 == 0);
e1(tmp) = inf;
% bounds of l corresponding to rect(x/rx)
lxmin = (-rx/2 - p1) ./ e1;
lxmax = ( rx/2 - p1) ./ e1;
% re-arrange the bounds so that lxmin contains the minimum l values
% and lxmax contains the maximum l values
temp = lxmin;
lxmin = min(lxmin, lxmax);
lxmax = max(temp, lxmax);
% exclude points where e1=0 by setting lxmin = -Inf and lxmax = Inf
lxmin(tmp) = -inf;
lxmax(tmp) = inf;
end % cuboid_proj_line1()

% cuboid_proj_do()
function proj = cuboid_proj_do(params, ss, tt, dt, ...
		beta, ... % [radians]
		source_zs, dso, dod, dfs, oversample)

if size(params, 2) ~= 9, error '9 parameters per cuboid', end

if oversample > 1
	ds = ss(2) - ss(1);
%	dt = tt(2) - tt(1); % fails if nt=1 so pass dt instead
	if any(abs(diff(ss) / ds - 1) > 1e-6) ...
	|| any(abs(diff(tt) / dt - 1) > 1e-6)
		fail 'uniform spacing required for oversampling'
	end
	No = oversample;
	% determine new finer sampling positions
	ss = outer_sum([-(No-1):2:(No-1)]'/(2*No)*ds, ss(:)'); % [No ns]
	tt = outer_sum([-(No-1):2:(No-1)]'/(2*No)*dt, tt(:)'); % [No nt]
	proj = cuboid_proj_do(params, ss(:), tt(:), dt/No, ...
		beta, source_zs, dso, dod, dfs, 1);
	proj = downsample3(proj, [No No 1]);
return
end

% determine equivalent parallel-beam projection coordinates, at beta=0
ns = length(ss);
nt = length(tt);
[sss ttt] = ndgrid(ss, tt);

if isinf(dso) % parallel beam
	uu = sss;
	vv = ttt;
	azim0 = zeros(size(sss));
	polar = zeros(size(sss));

elseif isinf(dfs) % cone-beam with flat detector
	[uu vv azim0 polar] = ir_coord_cb_flat_to_par(sss, ttt, dso, dod);

elseif dfs == 0 % cone-beam with arc detector
	[uu vv azim0 polar] = ir_coord_cb_arc_to_par(sss, ttt, dso, dod);

else
	fail 'not done'
end

clear sss ttt

cpolar = cos(polar);
spolar = sin(polar);
proj = zeros(ns, nt, numel(beta));

% loop over cuboids
for ip = 1:size(params,1)
	par = params(ip,:);

	cx = par(1);	rx = par(4);
	cy = par(2);	ry = par(5);
	cz = par(3);	rz = par(6);
	xang = deg2rad(par(7)); % xy-plane rotation
	zang = deg2rad(par(8)); % z-plane rotation
	if zang, error 'z rotation not done', end
	val = par(9);

	for ib = 1:length(beta)
		az = beta(ib) + azim0;

		% shift property of 3D transform:
		cz_eff = cz - source_zs(ib); % center relative to source
		ushift = cx * cos(az) + cy * sin(az);
		vshift = (cx * sin(az) - cy * cos(az)) .* spolar + cz_eff * cpolar;

		az = az - xang;
		us = uu - ushift;
		vs = vv - vshift;
		p1 = us .* cos(az) + vs .* sin(az) .* spolar;
		p2 = us .* sin(az) - vs .* cos(az) .* spolar;
		p3 = vs .* cpolar;

		e1 = -sin(az) .* cpolar; % x = p1 + l*e1
		e2 = cos(az) .* cpolar; % y = p2 + l*e2
		e3 = spolar; % z = p3 + l*e3

		[lxmin lxmax] = cuboid_proj_line1(rx, p1, e1);
%		clear p1
		[lymin lymax] = cuboid_proj_line1(ry, p2, e2);
%		clear p2
		[lzmin lzmax] = cuboid_proj_line1(rz, p3, e3);
%		clear p3

		lmin = max(lxmin, lymin);
		lmin = max(lmin, lzmin); % lower bound for l

		lmax = min(lxmax, lymax);
		lmax = min(lmax, lzmax); % upper bound for l

		ll = max(lmax - lmin, 0); % intersection only when lmax > lmin

		% cases where e(k) = 0 (rays along axes)

		tmp = e1 == 0; % sin(az) = 0
		zero_e = (-rx/2 <= us(tmp)) & (us(tmp) <= rx/2);
		ll(tmp) = ll(tmp) .* zero_e;

		tmp = e2 == 0; % cos(az) = 0
		zero_e = (-ry/2 <= us(tmp)) & (us(tmp) <= ry/2);
		ll(tmp) = ll(tmp) .* zero_e;

		tmp = e3 == 0; % sin(polar) = 0
		zero_e = (-rz/2 <= vs(tmp)) & (vs(tmp) <= rz/2);
		ll(tmp) = ll(tmp) .* zero_e;

if 0 % old way.  todo cut.  and verify e1 and e2 cases above
		if (e3 == 0) % sin(polar) = 0; so line along z
			zero_e = (-rz/2 <= vs) & (vs <= rz/2);
			ll = ll .* zero_e;
		end

		if (e1 == 0) % sin(az) = 0;
			zero_e = (-rx/2 <= us) & (us <= rx/2);
			ll = ll .* zero_e;
		end

		if (e2 == 0) % cos(az) = 0;
			zero_e = (-ry/2 <= us) & (us <= ry/2);
			ll = ll .* zero_e;
		end
end

		proj(:,:,ib) = proj(:,:,ib) + val * ll;
		clear proj_i
	end
end

end % cuboid_proj_do()

% cuboid_proj_test()
% internal test routine
function cuboid_proj_test

if 1
	params = [30 20 5  200 100 20  45 0 10];
	fun_proj = @(cg) cuboid_proj(cg, params, 'oversample', 2); % analytical
	fun_im = @(ig) cuboid_im(ig, params, 'oversample', 2); %, 'checkfov', true);

	ir_proj3_compare1(fun_proj, fun_im, 'chat', 1);
return % todo
end

down = 30;
cg = ct_geom('fan', 'ns', round(888/down), 'nt', 64, ...
	'na', 18, ...
	'ds', 1.0*down, 'dt', 1.1, ...
	'down', 1, ... % only downsample s and beta
	'dsd', 949, 'dod', 408, 'dfs', 0, ... % 3rd gen CT
	'dsd', inf, ... % parallel-beam
	'pitch', 0.5, ...
	'offset_t', 0.0, ...
	'offset_s', 0.25); % quarter detector
%	'dsd', 949, 'dod', 408, 'dfs', inf, ... % flat detector

ig = image_geom('nx', 512/2^2, 'nz', 64, 'fov', 512, 'dz', 0.625);
%cg.plot3(ig);

params = [30 20 5  200 100 20  45 0 10];
x = cuboid_im(ig, params, 'oversample', 2);

im plc 2 2
im(1, x), cbar

ya = cuboid_proj(cg, params, 'oversample', 2);
im(2, cg.s, cg.t, ya), cbar
titlef('analytical cone-beam projections, dfs=%g', cg.dfs)
xlabel s, ylabel t

%im clf, im(cg.s, cg.ad, permute(ya, [1 3 2])), cbar
%xlabel s, ylabel '\beta'

A = Gcone(cg, ig);
yd = A * x;

im(3, cg.s, cg.t, yd, 'discrete projections'), cbar

im(4, cg.s, cg.t, ya-yd), cbar
titlef('analytical - discrete')

max_percent_diff(ya, yd)

end % cuboid_proj_test()


function [is_planar ksubgraph EI]=boyer_myrvold_planarity_test(A,varargin)
% BOYER_MYRVOLD_PLANARITY_TEST Test a graph for planarity
%
% is_planar = boyer_myrvold_planaity_test(A) yields 1 if A is a planar
% graph or 0 otherwise.  A planar graph can be drawn on the xy plane with
% no edge crossings.
%
% [is_planar K] = ... identifies a Kuratowski subgraph of A when A is not
% planar.
% [is_planar K EI] = ... computes the planar embedding edge order in
% EI.edge_order.  EI.vp gives the starting and ending entries in EI.edge_order
% for the order of edges from each vertex.  (EI is the compressed
% sparse row representation of the matrix A with the order of the column
% indices permuted to the planar graph embedding order.)
%
% ... = boyer_myrvold_planaity_test(A,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%   No additional options for this function
%
% Example:
%   G = grid_graph(6,5);
%   boyer_myrvold_planarity_test(G) % G is planar
%   K5 = clique_graph(5);
%   boyer_myrvold_planarity_test(K5) % K5 is not planar

% David Gleich
% Copyright, Stanford University, 2008

%% History
%  2007-10-06: Initial coding
%%

[trans check full2sparse] = get_matlab_bgl_options(varargin{:});
if full2sparse && ~issparse(A), A = sparse(A); end

options = struct();
options = merge_options(options,varargin{:});

if nargout <= 1
    is_planar = planar_test_mex(A,0);
else
    if nargout <= 2
        [is_planar ki kj] = planar_test_mex(A,0);
    else
        [is_planar ki kj eip eie] = planar_test_mex(A,0);
        EI = struct('vp',eip,'edge_order',eie);
    end
    if ~isempty(ki)
        ksubgraph = sparse(ki,kj,1,size(A,1),size(A,2));
    else
        ksubgraph = sparse([]);
    end
    ksubgraph = ksubgraph|ksubgraph'; % placed here to get the type right
end


function sparse_grid_mixed_weight_test ( dim_num, level_max_min, ...
  level_max_max, rule, alpha, beta, tol )

%*****************************************************************************80
%
%% SPARSE_GRID_MIXED_WEIGHT_TEST checks the sum of the quadrature weights.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 March 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX_MIN, LEVEL_MAX_MAX, the minimum and
%    maximum values of LEVEL_MAX.
%
%    Input, integer RULE(DIM_NUM), the rule in each dimension.
%     1, "CC",  Clenshaw Curtis, Closed Fully Nested rule.
%     2, "F2",  Fejer Type 2, Open Fully Nested rule.
%     3, "GP",  Gauss Patterson, Open Fully Nested rule.
%     4, "GL",  Gauss Legendre, Open Weakly Nested rule.
%     5, "GH",  Gauss Hermite, Open Weakly Nested rule.
%     6, "GGH", Generalized Gauss Hermite, Open Weakly Nested rule.
%     7, "LG",  Gauss Laguerre, Open Non Nested rule.
%     8, "GLG", Generalized Gauss Laguerre, Open Non Nested rule.
%     9, "GJ",  Gauss Jacobi, Open Non Nested rule.
%    10, "GW",  Golub Welsch, (presumed) Open Non Nested rule.
%    11, "CC_SE", Clenshaw Curtis Slow Exponential, Closed Fully Nested rule.
%    12, "F2_SE", Fejer Type 2 Slow Exponential, Closed Fully Nested rule.
%    13, "GP_SE", Gauss Patterson Slow Exponential, Closed Fully Nested rule.
%    14, "CC_ME", Clenshaw Curtis Moderate Exponential, Closed Fully Nested rule.
%    15, "F2_ME", Fejer Type 2 Moderate Exponential, Closed Fully Nested rule.
%    16, "GP_ME", Gauss Patterson Moderate Exponential, Closed Fully Nested rule.
%    17, "CCN", Clenshaw Curtis Nested, Linear, Closed Fully Nested rule.
%
%    Input, real ALPHA(DIM_NUM), BETA(DIM_NUM), parameters used for
%    Generalized Gauss Hermite, Generalized Gauss Laguerre, and Gauss Jacobi rules.
%
%    Input, real TOL, the tolerance for point equality.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_GRID_MIXED_WEIGHT_TEST:\n' );
  fprintf ( 1, '  Compute the weights of a sparse grid.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Each sparse grid is of spatial dimension DIM_NUM,\n' );
  fprintf ( 1, '  and is made up of product grids of levels up to LEVEL_MAX.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dimension      Rule     Alpha          Beta\n' );
  fprintf ( 1, '\n' );

  for dim = 1 : dim_num
    fprintf ( 1, '  %8d  %8d', dim, rule(dim) );
    if ( rule(dim) == 6 | rule(dim) == 8 | rule(dim) == 9 )
      fprintf ( 1, '  %12e', alpha(dim) );
    end
    if ( rule(dim) == 9 )
      fprintf ( 1, '  %12e', beta(dim) );
    end
    fprintf ( 1, '\n' );
  end

  weight_sum_exact = 1.0;

  for dim = 1 : dim_num

    if ( rule(dim) == 1 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 2 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 3 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 4 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 5 )
      weight_sum_exact = weight_sum_exact * sqrt ( pi );
    elseif ( rule(dim) == 6 )
      weight_sum_exact = weight_sum_exact * gamma ( 0.5 * ( alpha(dim) + 1.0 ) );
    elseif ( rule(dim) == 7 )
      weight_sum_exact = weight_sum_exact * 1.0;
    elseif ( rule(dim) == 8 )
      weight_sum_exact = weight_sum_exact * gamma ( alpha(dim) + 1.0 );
    elseif ( rule(dim) == 9 )
      arg1 = - alpha(dim);
      arg2 = 1.0;
      arg3 = beta(dim) + 2.0;
      arg4 = - 1.0;
      value1 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );
      arg1 = - beta(dim);
      arg2 = 1.0;
      arg3 = alpha(dim) + 2.0;
      arg4 = - 1.0;
      value2 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );
      weight_sum_exact = weight_sum_exact * ( ...
        value1 / ( beta(dim) + 1.0 ) + value2 / ( alpha(dim) + 1.0 ) );
    elseif ( rule(dim) == 10 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SPARSE_GRID_MIXED_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Do not know weight sum for rule 10.\n' );
      error ( 'SPARSE_GRID_MIXED_WEIGHT_TEST - Fatal error!' );
    elseif ( rule(dim) == 11 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 12 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 13 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 14 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 15 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 16 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 17 )
      weight_sum_exact = weight_sum_exact * 2.0;
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SPARSE_GRID_MIXED_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Unexpected value of RULE = %d\n', rule(dim) );
      error ( 'SPARSE_GRID_MIXED_WEIGHT_TEST - Fatal error!' );
    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  As a simple test, sum these weights.\n' );
  fprintf ( 1, '  They should sum to exactly %f\n', weight_sum_exact );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     Level      Weight sum  Expected sum    Difference\n' );
  fprintf ( 1, '\n' );

  for level_max = level_max_min : level_max_max

    point_total_num = sparse_grid_mixed_size_total ( dim_num, level_max, rule );

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

    sparse_unique_index = sparse_grid_mixed_unique_index ( ...
      dim_num, level_max, rule, alpha, beta, tol, point_num, point_total_num );

    sparse_weight = sparse_grid_mixed_weight ( dim_num, level_max, rule, ...
      alpha, beta, point_num, point_total_num, sparse_unique_index );

    weight_sum = sum ( sparse_weight(1:point_num) );

    weight_sum_error = abs ( weight_sum - weight_sum_exact );

    fprintf ( 1, '  %8d  %14e  %14e  %14e\n', ...
      level_max, weight_sum, weight_sum_exact, weight_sum_error );

  end

  return
end


%
%   colour transfer algorithm based on linear Monge-Kantorovitch solution
%
%   IR = colour_transfer_MKL(I_original, I_target, nbiterations);
%
%  (c) F. Pitie 2007
%
%  see reference:
%
%
function IR = colour_transfer_MKL(I0, I1)

if (ndims(I0)~=3)
    error('pictures must have 3 dimensions');
end

X0 = reshape(I0, [], size(I0,3));
X1 = reshape(I1, [], size(I1,3));

A = cov(X0);
B = cov(X1);

T = MKL(A, B);

mX0 = repmat(mean(X0), [size(X0,1) 1]);
mX1 = repmat(mean(X1), [size(X0,1) 1]);

XR = (X0-mX0)*T + mX1;

IR = reshape(XR, size(I0));

function [T] = MKL(A, B)
N = size(A,1);
[Ua,Da2] = eig(A);
Da2 = diag(Da2);
Da2(Da2<0) = 0;
Da = diag(sqrt(Da2 + eps));
C = Da*Ua'*B*Ua*Da;
[Uc,Dc2] = eig(C);
Dc2 = diag(Dc2);
Dc2(Dc2<0) = 0;
Dc = diag(sqrt(Dc2 + eps));
Da_inv = diag(1./(diag(Da)));
T = Ua*Da_inv*Uc*Dc*Uc'*Da_inv*Ua';


%
% LibQPEP: A Library for Globally Optimal Solving Quadratic Pose Estimation Problems (QPEPs),
%          It also gives highly accurate uncertainty description of the solutions.
%
%
% Article:
%      Wu, J., Zheng, Y., Gao, Z., Jiang, Y., Hu, X., Zhu, Y., Jiao, J., Liu, M. (2020)
%           Quadratic Pose Estimation Problems: Unified Solutions,
%           Solvability/Observability Analysis and Uncertainty Description
%           in A Globally Optimal Framework.
%
%
% Authors:      Jin Wu and Ming Liu
% Affiliation:  Hong Kong University of Science and Technology (HKUST)
% Emails:       jin_wu_uestc@hotmail.com; eelium@ust.hk
% Websites:     https://zarathustr.github.io
%               https://ram-lab.com

function R = q2R(q)
q0 = q(1); q1 = q(2); q2 = q(3); q3 = q(4);
R = [
        q0^2 + q1^2 - q2^2 - q3^2,         2*q0*q3 + 2*q1*q2,         2*q1*q3 - 2*q0*q2;
                2*q1*q2 - 2*q0*q3, q0^2 - q1^2 + q2^2 - q3^2,         2*q0*q1 + 2*q2*q3;
                2*q0*q2 + 2*q1*q3,         2*q2*q3 - 2*q0*q1, q0^2 - q1^2 - q2^2 + q3^2];
end


function [u,eqn,info] = fracLapP1P2(node,elem,pde,option)
%% FRACLAPP1P2 solves fractional Laplacian equation using P1-P2 element
%
% [u,eqn,info] = fracLapP1P1(node,elem,pde,option) solves the fractional
% Laplacian equation
%
%  (-\Delta^^s u = f in \Omega with u = 0 on \partial \Omega.

%% Options
if ~exist('option','var'), option = []; end
if ~isfield(option,'plotflag'), option.plotflag = 0; end

%% Parameters
global s;
alpha = 1-2*s;
if s == 0.5
    Gamma = 1;
else
    Gamma = 5/(2*s)+0.01;  % a different grading factor
end

%% Mesh
N = size(node,1); NT = size(elem,1);
% My = round(sqrt(NT));
L = 1+log10(NT)/3; % to control the truncated error, L should increase as logN
My = round(L*2*NT^(1/4)); % number of elements in y direction
% My = round(pde.L*sqrt(sqrt(NT/2)));
y = gradmap(0,L,Gamma,My);
Ny = length(y); % number of vertices in y-direction
NTy = Ny - 1;   % number of elements in y-direction
NTtotal = NT*NTy;
Nxdof = N;      % dof in x direction is number of vertices
Nydof = (Ny + NTy); % dof in y direction is number of vertices plus elements
Ndof = Nxdof*Nydof;

tic; % start the timing for the assembling
%% Stiffness matrix and mass matrix in the extended direction
% quantities in y direction
hy = diff(y);
a = zeros(NTy,5);
for i = 1:5
    a(:,i) = diff(y.^(alpha+i)/(alpha+i));
end
y1 = y(1:end-1);
y2 = y(2:end);
y12 = y1 + y2;
y1y2 = y1.*y2;
ym = y12/2;
% stiffness matrix in y direction
Ay = zeros(NTy,3,3);
Ay(:,1,1) = a(:,1)./hy.^2;
Ay(:,1,2) = -Ay(:,1,1);
Ay(:,2,1) = -Ay(:,1,1);
Ay(:,2,2) = Ay(:,1,1);
Ay(:,1,3) = 8*(a(:,2) - ym.*a(:,1))./hy.^3;
Ay(:,3,1) = Ay(:,1,3);
Ay(:,2,3) = -Ay(:,1,3);
Ay(:,3,2) = Ay(:,2,3);
Ay(:,3,3) = 64*(a(:,3) - 2*ym.*a(:,2) + ym.^2.*a(:,1))./hy.^4;
% mass matrix in y direction
My = zeros(NTy,3,3);
My(:,1,1) = (a(:,3) - 2*y1.*a(:,2) + y1.^2.*a(:,1))./hy.^2;
My(:,1,2) = (-a(:,3) + (y1+y2).*a(:,2) - y1.*y2.*a(:,1))./hy.^2;
My(:,2,1) = My(:,1,2);
My(:,2,2) = (a(:,3) - 2*y2.*a(:,2) + y2.^2.*a(:,1))./hy.^2;
My(:,1,3) = 4*(a(:,4) - (y12+y2).*a(:,3) + (y2.*y12 + y1y2).*a(:,2) ...
             - y2.*y1y2.*a(:,1))./hy.^3;
My(:,3,1) = My(:,1,3);
My(:,2,3) = 4*(-a(:,4) + (y12+y1).*a(:,3) - (y1.*y12 + y1y2).*a(:,2) ...
             + y1.*y1y2.*a(:,1))./hy.^3;
My(:,3,2) = My(:,2,3);
My(:,3,3) = 16*(a(:,5) - 2*y12.*a(:,4) + (y12.^2 + 2*y1y2).*a(:,3)...
               -2*y12.*y1.*y2.*a(:,2) + y1y2.^2.*a(:,1))./hy.^4;

%% Stiffness matrix and mass matrix in the original direction
[Dphi,area] = gradbasis(node,elem);
% Compute a piecewise constant diffusion coefficient
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'dquadorder'), option.dquadorder = 1; end
if ~isempty(pde.d) && isnumeric(pde.d)
   K = pde.d;                                 % d is an array
end
if ~isempty(pde.d) && ~isnumeric(pde.d)       % d is a function
    [lambda,weight] = quadpts(option.dquadorder);
    nQuad = size(lambda,1);
    K = zeros(NT,1);
    for p = 1:nQuad
		pxy = lambda(p,1)*node(elem(:,1),:) ...
			+ lambda(p,2)*node(elem(:,2),:) ...
			+ lambda(p,3)*node(elem(:,3),:);
        K = K + weight(p)*pde.d(pxy);
   end
end
if ~isempty(pde.d) % build the coefficients into the scaled area
    areaK = K.*area;
else
    areaK = area;
end
At = zeros(NT,3,3);
Mt = zeros(NT,3,3);
for i = 1:3
    for j = 1:3
        At(:,i,j) = (Dphi(:,1,i).*Dphi(:,1,j) + Dphi(:,2,i).*Dphi(:,2,j)).*areaK;
        Mt(:,i,j) = areaK*((i==j)+1)/12;
    end
end

%% Assemble stiffness matrix
dofMap = repmat(1:Nxdof,Nydof,1) + repmat((0:Nydof-1)'*Nxdof,1,Nxdof);
ii = zeros(9*9*NTtotal,1);
jj = zeros(9*9*NTtotal,1);
sA = zeros(9*9*NTtotal,1);
index = 0;
for m = 1:3
    for n = 1:3
        for i = 1:3
            for j = 1:3
                if m < 3
                    ii(index+1:index+NTtotal) = dofMap(m:Ny+m-2,elem(:,i));
                else % m = 3: middle points of elements in y direction
                    ii(index+1:index+NTtotal) = dofMap(Ny+1:Nydof,elem(:,i));
                end
                if n < 3
                    jj(index+1:index+NTtotal) = dofMap(n:Ny+n-2,elem(:,j));
                else % n = 3: middle points of elements in y direction
                    jj(index+1:index+NTtotal) = dofMap(Ny+1:Nydof,elem(:,j));
                end
                sA(index+1:index+NTtotal) = kron(At(:,i,j),My(:,m,n)) + ...
                                            kron(Mt(:,i,j),Ay(:,m,n));
                index = index + NTtotal;
            end
        end
    end
end
A = sparse(ii,jj,sA,Ndof,Ndof);
clear ii jj sA At Mt Ay My

%% Assemble the right hand side
b = zeros(Ndof,1);
u = zeros(Ndof,1);

%% Set up boundary conditions
% find Dirichlet boundary nodes
bdNode = findboundary(elem);  % boundary vertices of mesh in x-direction
lateralbd = dofMap(:,bdNode);
topbd = dofMap(Ny,:);

% Modify the matrix to include the Dirichlet boundary condition
bdidx = zeros(Ndof,1);
bdidx(lateralbd) = 1;
bdidx(topbd) = 1;
Tbd = spdiags(bdidx,0,Ndof,Ndof);
T = spdiags(1-bdidx,0,Ndof,Ndof);
A = T*A*T + Tbd;

% Compute boundary integral over the original domain
if ~isfield(option,'fquadorder')
    option.fquadorder = 4;
end
ft = zeros(NT,3);
ds = 2^(1-2*s)*gamma(1-s)/gamma(s);
if isfield(pde,'f') && ~ischar(pde.f)
    [lambdaf,weightf] = quadpts(option.fquadorder);
    phif = lambdaf;                 % linear bases
    nQuadf = size(lambdaf,1);
    for p = 1:nQuadf
        % quadrature points in the x-y coordinate
        pxy = lambdaf(p,1)*node(elem(:,1),:) ...
            + lambdaf(p,2)*node(elem(:,2),:) ...
            + lambdaf(p,3)*node(elem(:,3),:);
        fp = pde.f(pxy);
        for i = 1:3
            ft(:,i) = ft(:,i) + weightf(p)*phif(p,i)*fp;
        end
    end
elseif isfield(pde,'f') && ischar(pde.f)
    if strcmp(pde.f,'intx') % f is a distribution
        % compute (intx, d_x phi)
        [lambdaf,weightf] = quadpts(option.fquadorder);
        nQuad = size(lambdaf,1);
        bt = zeros(NT,1);
        for p = 1:nQuad
            pxy = lambdaf(p,1)*node(elem(:,1),:) ...
                + lambdaf(p,2)*node(elem(:,2),:) ...
                + lambdaf(p,3)*node(elem(:,3),:);
            bt = bt + weightf(p)*pde.intx(pxy);
        end
%         [Dphi,area] = gradbasis(node,elem);
        for i = 1:3
            ft(:,i) = Dphi(:,1,i).*bt; % (intx, d_xphi)
        end
    end
end
ft = ds*ft.*repmat(area,1,3);
b = b + accumarray(elem(:),ft(:),[Ndof,1]);
% Neumann edfts are considered as open set. So the corner points should be
% set as Dirichlet boundary condition!
b(bdNode) = 0;
clear ft

%% Record assembling time
assembleTime = toc;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
freeDof = find(bdidx==0);
if isempty(freeDof), return; end
% Set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for large size systems
        option.solver = 'mg';
    end
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeDof) = A(freeDof,freeDof)\b(freeDof);
        residual = norm(b - A*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
        option.x0 = u;
        option.solver = 'VCYCLE';
        option.freeDof = freeDof;
        [u,info] = mgfracLapP1P2(A,b,elem,option);
    case 'amg'
        option.solver = 'CG';
        [u(freeDof),info] = amg(A(freeDof,freeDof),b(freeDof),option);
end

%% Compute error using boundary integral
if isfield(pde,'exactu')
    err = zeros(NT,1);
    [lambda,weight] = quadpts(8);
    phi = lambda;                 % linear bases
    nQuadf = size(lambda,1);
    for p = 1:nQuadf
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        fp = pde.f(pxy);
        uhp = u(elem(:,1))*phi(p,1) + u(elem(:,2))*phi(p,2) + u(elem(:,3))*phi(p,3);
        up = pde.exactu(pxy);
        err = err + weight(p)*fp.*(up - uhp);
%         err = err + weight(p)*fp.*(up - 2*uhp);
    end
    err = ds*sum(err.*area);
%     err = ds*sum(err.*area) + u'*(A*u);
    err = sqrt(abs(err));
else
    err = 0;
end
info.errH1 = err;

%% Output information
eqn = struct('A',A,'b',b,'freeDof',freeDof);
info.assembleTime = assembleTime;


% [INPUT]
% p = A vector of floats [0,Inf) of length t representing the prices.
% r = A vector of floats (-Inf,Inf) of length t representing the logarithmic returns.
% v = A vector of floats [0,Inf) of length t representing the trading volumes.
% cp = A vector of floats [0,Inf) of length t representing the market capitalization.
% bwl = An integer [90,252] representing the dimension of the long bandwidth.
% bwm = An integer [21,90) representing the dimension of the medium bandwidth.
% bws = An integer [5,21) representing the dimension of the short bandwidth.
%
% [OUTPUT]
% hhlr = A column vector of floats [0,Inf) of length t representing the Hui-Heubel Liquidity Ratio.
% tr = A column vector of floats [0,Inf) of length t representing the Turnover Ratio.
% vr = A column vector of floats [0,Inf) of length t representing the Variance Ratio.

function [hhlr,tr,vr] = liquidity_metrics(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('p',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('r',@(x)validateattributes(x,{'double'},{'real' 'finite' 'vector' 'nonempty'}));
        ip.addRequired('v',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('cp',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('bwl',@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 90 '<=' 252 'scalar'}));
        ip.addRequired('bwm',@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 21 '<' 90 'scalar'}));
        ip.addRequired('bws',@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 5 '<' 21 'scalar'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    [p,r,v,cp,bwl,bwm,bws] = validate_input(ipr.p,ipr.r,ipr.v,ipr.cp,ipr.bwl,ipr.bwm,ipr.bws);

    nargoutchk(3,3);

    [hhlr,tr,vr] = liquidity_metrics_internal(p,r,v,cp,bwl,bwm,bws);

end

function [hhlr,tr,vr] = liquidity_metrics_internal(p,r,v,cp,bwl,bwm,bws)

    hhlr = calculate_hhlr(p,v,cp,bwl,bws);
    tr = calculate_tr(v,cp,bwl);
    vr = calculate_vr(r,bwl,bwm);

end

function hhlr = calculate_hhlr(p,v,cp,bwl,bws)

    tr = v ./ cp;
    tr(~isfinite(tr)) = 0;

    windows_p = extract_rolling_windows(p,bws);
    dp = cellfun(@(x)(max(x) - min(x)) / min(x),windows_p);

    alpha = 2 / (bwl + 1);

    hhlr = dp ./ tr;
    hhlr(~isfinite(hhlr)) = 0;
    hhlr(1:bws) = mean(hhlr(bws+1:bws*2+1));
    hhlr = [hhlr(1); filter(alpha,[1 (alpha - 1)],hhlr(2:end),(1 - alpha) * hhlr(1))];
    hhlr = (hhlr - min(hhlr)) ./ (max(hhlr) - min(hhlr));

end

function tr = calculate_tr(v,cp,bwl)

    alpha = 2 / (bwl + 1);

    tr = v ./ cp;
    tr(~isfinite(tr)) = 0;
    tr = [tr(1); filter(alpha,[1 (alpha - 1)],tr(2:end),(1 - alpha) * tr(1))];
    tr = (tr - min(tr)) ./ (max(tr) - min(tr));

end

function vr = calculate_vr(r,bwl,bwm)

    alpha = 2 / (bwl + 1);
    t = bwl / bwm;

    windows_long = extract_rolling_windows(r,bwl);
    var_long = cellfun(@var,windows_long);

    windows_short = extract_rolling_windows(r,bwm);
    var_short = cellfun(@var,windows_short);

    vr = var_long ./ (t .* var_short);
    vr(~isfinite(vr)) = 0;
    vr(1:bwm) = mean(vr(bwm+1:bwm*2+1));
    vr = [vr(1); filter(alpha,[1 (alpha - 1)],vr(2:end),(1 - alpha) * vr(1))];

end

function [p,r,v,cp,bwl,bwm,bws] = validate_input(p,r,v,cp,bwl,bwm,bws)

    data = {p(:) r(:) v(:) cp(:)};

    l = unique(cellfun(@numel,data));

    if (numel(l) ~= 1)
        error('The number of elements of ''p'', ''r'' and ''v'' must be equal.');
    end

    if (l < 5)
        error('The value of ''p'', ''r'' and ''v'' is invalid. Expected inputs to be vectors containing at least 5 elements.');
    end

    [p,r,v,cp] = deal(data{:});

    if (bwl < (bwm * 2))
        error(['The long bandwidth (' num2str(bwl) ') must be at least twice the medium bandwidth (' num2str(bwm) ').']);
    end

    if (bwm < (bws * 2))
        error(['The medium bandwidth (' num2str(bwm) ') must be at least twice the short bandwidth (' num2str(bws) ').']);
    end

end


%
%   Copyright (C) 2016  Starsky Wong <sununs11@gmail.com>
%
%   Note: The SIFT algorithm is patented in the United States and cannot be
%   used in commercial products without a license from the University of
%   British Columbia.  For more information, refer to the file LICENSE
%   that accompanied this distribution.

function [ descrs, locs ] = getFeatures( input_img )
% Function: Get sift features and descriptors
global gauss_pyr;
global dog_pyr;
global init_sigma;
global octvs;
global intvls;
global ddata_array;
global features;
if(size(input_img,3)==3)
    input_img = rgb2gray(input_img);
end
input_img = im2double(input_img);

%% Build DoG Pyramid
% initial sigma
init_sigma = 1.6;
% number of intervals per octave
intvls = 3;
s = intvls;
k = 2^(1/s);
sigma = ones(1,s+3);
sigma(1) = init_sigma;
sigma(2) = init_sigma*sqrt(k*k-1);
for i = 3:s+3
    sigma(i) = sigma(i-1)*k;
end
% default cubic method
input_img = imresize(input_img,2);
% assume the original image has a blur of sigma = 0.5
input_img = gaussian(input_img,sqrt(init_sigma^2-0.5^2*4));
% smallest dimension of top level is about 8 pixels
octvs = floor(log( min(size(input_img)) )/log(2) - 2);

% gaussian pyramid
[img_height,img_width] =  size(input_img);
gauss_pyr = cell(octvs,1);
% set image size
gimg_size = zeros(octvs,2);
gimg_size(1,:) = [img_height,img_width];
for i = 1:octvs
    if (i~=1)
        gimg_size(i,:) = [round(size(gauss_pyr{i-1},1)/2),round(size(gauss_pyr{i-1},2)/2)];
    end
    gauss_pyr{i} = zeros( gimg_size(i,1),gimg_size(i,2),s+3 );
end
for i = 1:octvs
    for j = 1:s+3
        if (i==1 && j==1)
            gauss_pyr{i}(:,:,j) = input_img;
        % downsample for the first image in an octave, from the s+1 image
        % in previous octave.
        elseif (j==1)
            gauss_pyr{i}(:,:,j) = imresize(gauss_pyr{i-1}(:,:,s+1),0.5);
        else
            gauss_pyr{i}(:,:,j) = gaussian(gauss_pyr{i}(:,:,j-1),sigma(j));
        end
    end
end
% dog pyramid
dog_pyr = cell(octvs,1);
for i = 1:octvs
    dog_pyr{i} = zeros(gimg_size(i,1),gimg_size(i,2),s+2);
    for j = 1:s+2
    dog_pyr{i}(:,:,j) = gauss_pyr{i}(:,:,j+1) - gauss_pyr{i}(:,:,j);
    end
end
% for i = 1:size(dog_pyr,1)
%     for j = 1:size(dog_pyr{i},3)
%         imwrite(im2bw(im2uint8(dog_pyr{i}(:,:,j)),0),['dog_pyr\dog_pyr_',num2str(i),num2str(j),'.png']);
%     end
% end

%% Accurate Keypoint Localization
% width of border in which to ignore keypoints
img_border = 5;
% maximum steps of keypoint interpolation
max_interp_steps = 5;
% low threshold on feature contrast
contr_thr = 0.04;
% high threshold on feature ratio of principal curvatures
curv_thr = 10;
prelim_contr_thr = 0.5*contr_thr/intvls;
ddata_array = struct('x',0,'y',0,'octv',0,'intvl',0,'x_hat',[0,0,0],'scl_octv',0);
ddata_index = 1;
for i = 1:octvs
    [height, width] = size(dog_pyr{i}(:,:,1));
    % find extrema in middle intvls
    for j = 2:s+1
        dog_imgs = dog_pyr{i};
        dog_img = dog_imgs(:,:,j);
        for x = img_border+1:height-img_border
            for y = img_border+1:width-img_border
                % preliminary check on contrast
                if(abs(dog_img(x,y)) > prelim_contr_thr)
                    % check 26 neighboring pixels
                    if(isExtremum(j,x,y))
                        ddata = interpLocation(dog_imgs,height,width,i,j,x,y,img_border,contr_thr,max_interp_steps);
                        if(~isempty(ddata))
                            if(~isEdgeLike(dog_img,ddata.x,ddata.y,curv_thr))
                                 ddata_array(ddata_index) = ddata;
                                 ddata_index = ddata_index + 1;
                            end
                        end
                    end
                end
            end
        end
    end
end

function [ flag ] = isExtremum( intvl, x, y)
% Function: Find Extrema in 26 neighboring pixels
    value = dog_imgs(x,y,intvl);
    block = dog_imgs(x-1:x+1,y-1:y+1,intvl-1:intvl+1);
    if ( value > 0 && value == max(block(:)) )
        flag = 1;
    elseif ( value == min(block(:)) )
        flag = 1;
    else
        flag = 0;
    end
end

%% Orientation Assignment
% number of detected points
n = size(ddata_array,2);
% determines gaussian sigma for orientation assignment
ori_sig_factr = 1.5;
% number of bins in histogram
ori_hist_bins = 36;
% orientation magnitude relative to max that results in new feature
ori_peak_ratio = 0.8;
% array of feature
features = struct('ddata_index',0,'x',0,'y',0,'scl',0,'ori',0,'descr',[]);
feat_index = 1;
for i = 1:n
    ddata = ddata_array(i);
    ori_sigma = ori_sig_factr * ddata.scl_octv;
    % generate a histogram for the gradient distribution around a keypoint
    hist = oriHist(gauss_pyr{ddata.octv}(:,:,ddata.intvl),ddata.x,ddata.y,ori_hist_bins,round(3*ori_sigma),ori_sigma);
    for j = 1:2
        smoothOriHist(hist,ori_hist_bins);
    end
    % generate feature from ddata and orientation hist peak
    % add orientations greater than or equal to 80% of the largest orientation magnitude
    feat_index = addOriFeatures(i,feat_index,ddata,hist,ori_hist_bins,ori_peak_ratio);
end

%% Descriptor Generation
% number of features
n = size(features,2);
% width of 2d array of orientation histograms
descr_hist_d = 4;
% bins per orientation histogram
descr_hist_obins = 8;
% threshold on magnitude of elements of descriptor vector
descr_mag_thr = 0.2;
descr_length = descr_hist_d*descr_hist_d*descr_hist_obins;
local_features = features;
local_ddata_array = ddata_array;
local_gauss_pyr = gauss_pyr;
clear features;
clear ddata_array;
clear gauss_pyr;
clear dog_pyr;
parfor feat_index = 1:n
    feat = local_features(feat_index);
    ddata = local_ddata_array(feat.ddata_index);
    gauss_img = local_gauss_pyr{ddata.octv}(:,:,ddata.intvl);
% computes the 2D array of orientation histograms that form the feature descriptor
    hist_width = 3*ddata.scl_octv;
    radius = round( hist_width * (descr_hist_d + 1) * sqrt(2) / 2 );
    feat_ori = feat.ori;
    ddata_x = ddata.x;
    ddata_y = ddata.y;
    hist = zeros(1,descr_length);
    for i = -radius:radius
        for j = -radius:radius
            j_rot = j*cos(feat_ori) - i*sin(feat_ori);
            i_rot = j*sin(feat_ori) + i*cos(feat_ori);
            r_bin = i_rot/hist_width + descr_hist_d/2 - 0.5;
            c_bin = j_rot/hist_width + descr_hist_d/2 - 0.5;
            if (r_bin > -1 && r_bin < descr_hist_d && c_bin > -1 && c_bin < descr_hist_d)
                mag_ori = calcGrad(gauss_img,ddata_x+i,ddata_y+j);
                if (mag_ori(1) ~= -1)
                    ori = mag_ori(2);
                    ori = ori - feat_ori;
                    while (ori < 0)
                        ori = ori + 2*pi;
                    end
                    % i think it's theoretically impossible
                    while (ori >= 2*pi)
                        ori = ori - 2*pi;
                        disp('###################what the fuck?###################');
                    end
                    o_bin = ori * descr_hist_obins / (2*pi);
                    w = exp( -(j_rot*j_rot+i_rot*i_rot) / (2*(0.5*descr_hist_d*hist_width)^2) );
                    hist = interpHistEntry(hist,r_bin,c_bin,o_bin,mag_ori(1)*w,descr_hist_d,descr_hist_obins);
                end
            end
        end
    end
    local_features(feat_index) = hist2Descr(feat,hist,descr_mag_thr);
end
% sort the descriptors by descending scale order
features_scl = [local_features.scl];
[~,features_order] = sort(features_scl,'descend');
% return descriptors and locations
descrs = zeros(n,descr_length);
locs = zeros(n,2);
for i = 1:n
    descrs(i,:) = local_features(features_order(i)).descr;
    locs(i,1) = local_features(features_order(i)).x;
    locs(i,2) = local_features(features_order(i)).y;
end

end