% Copyright (c) 2012-2016 Ali Akbar Eftekhari
% See the license file
% Coupled nonlinear PDE's
% Buckley Leverett equation
% dependent variables: pressure and water saturation
% Prepared for educational purposes by ** AAE **
% Written by Ali A. Eftekhari
% Last checked: June 2021
% This code works fine (two phase flow + Pc) and shows the effect of Pc
% in the simulation of a heterogeneous 2D field.
clc
%% define the geometry
Nx = 100; % number of cells in x direction
Ny = 30; % number of cells in y direction
W = 300; % [m] length of the domain in x direction
H = 30; % [m] length of the domain in y direction
% m = createMesh1D(Nx, W);
m = createMesh2D(Nx, Ny, W, H); % creates a 2D mesh
%% define the physical parametrs
krw0 = 1.0;
kro0 = 0.76;
nw = 2.4;
no = 2.0;
sor=0.12;
swc=0.09;
sws=@(sw)((sw>swc).*(sw<1-sor).*(sw-swc)/(1-sor-swc)+(sw>=1-sor).*ones(size(sw)));
kro=@(sw)((sw>=swc).*kro0.*(1-sws(sw)).^no+(sw<swc).*(1+(kro0-1)/swc*sw));
krw=@(sw)((sw<=1-sor).*krw0.*sws(sw).^nw+(sw>1-sor).*(-(1-krw0)/sor.*(1.0-sw)+1.0));
dkrwdsw=@(sw)((sw<=1-sor).*nw.*krw0.*(1/(1-sor-swc)).*sws(sw).^(nw-1)+(sw>1-sor)*((1-krw0)/sor));
dkrodsw=@(sw)((sw>=swc).*(-kro0*no*(1-sws(sw)).^(no-1))/(-swc-sor+1)+(sw<swc).*((kro0-1)/swc));
p0 = 100e5; % [bar] pressure
pin = 150e5; % [bar] injection pressure at the left boundary
u_in= 1.0/(24*3600); % [m/s] equal to 1 m/day
sw0 = swc+0.1; % initial water saturation
sw_in = 1;
mu_oil = 2e-3; % [Pa.s] oil viscosity
mu_water = 1e-3; % [Pa.s] water viscosity
% reservoir
k0 = 2e-12; % [m^2] average reservoir permeability
phi0 = 0.2; % average porosity
teta_ow=deg2rad(30);
gama_ow=0.03; % N/m
labda=10.0;
eps1=1e-6;
clx=0.2;
cly=0.2;
V_dp=0.7; % Dykstra-Parsons coef.
perm_val= field2d(Nx,Ny,k0,V_dp,clx,cly);
k=createCellVariable(m, perm_val);
phi=createCellVariable(m, phi0);
pce=gama_ow*cos(teta_ow)*(phi./k).^0.5;
pce.value(isnan(pce.value)) = 1.0;
pce_face=gama_ow*cos(teta_ow)*(arithmeticMean(phi)./geometricMean(k)).^0.5;
pc=@(sw)(pce.*(sws(sw)+eps).^(-1/labda)); % it can also be defined for each block
dpc=@(sw)((-1/labda)*(1/(1-sor-swc)).*pce_face.*(sws(sw)+eps).^(-1/labda-1));
dpcdk=@(sw)(0.5*gama_ow*cos(teta_ow)*(geometricMean(k)./arithmeticMean(phi)).^0.5.*(sws(sw)+eps).^(-1/labda));
grad_phik=gradientTerm(phi./k);
% sw_plot=linspace(0,1, 10000);
% plot(sw_plot, pc(sw_plot))
lw = geometricMean(k)/mu_water;
lo = geometricMean(k)/mu_oil;
Lw = @(sw)(krw(sw));
Lo = @(sw)(k/mu_oil*kro(sw));
dLwdsw = @(sw)(k/mu_water*dkrwdsw(sw));
dLodsw = @(sw)(k/mu_oil*dkrodsw(sw));
%% Define the boundaries
BCp = createBC(m); % Neumann BC for pressure
BCs = createBC(m); % Neumann BC for saturation
% left boundary pressure gradient
BCp.left.a(:)=(krw(sw_in)*lw.xvalue(1,:)+kro(sw_in)*lo.xvalue(1,:)); BCp.left.b(:)=0; BCp.left.c(:)=-u_in;
% change the right boandary to constant pressure (Dirichlet)
% BCp.left.a(:)=0; BCp.left.b(:)=1; BCp.left.c(:)=pin;
BCp.right.a(:)=0; BCp.right.b(:)=1; BCp.right.c(:)=p0;
% BCs.right.a(:)=0; BCs.right.b(:)=1; BCs.right.c(:)=0.8; % end effect
% change the left boundary to constant saturation (Dirichlet)
BCs.left.a(:)=0; BCs.left.b(:)=1; BCs.left.c(:)=1;
%% define the time step and solver properties
% dt = 1000; % [s] time step
dt=(W/Nx)/u_in/20; % [s]
t_end = 1000*dt; % [s] final time
eps_p = 1e-5; % pressure accuracy
eps_sw = 1e-5; % saturation accuracy
%% define the variables
sw_old = createCellVariable(m, sw0, BCs);
p_old = createCellVariable(m, p0, BCp);
sw = sw_old;
p = p_old;
uw = -gradientTerm(p_old); % an estimation of the water velocity
%% start the main loop
% generate intial pressure profile (necessary to initialize the fully
% implicit solver)

t = 0;
while (t<t_end)
    error_p = 1e5;
    error_sw = 1e5;
    % Implicit loop
    while ((error_p>eps_p) || (error_sw>eps_sw))
        % calculate parameters
        pgrad = gradientTerm(p);
%         pcgrad=gradientTerm(pc(sw));
        sw_face = upwindMean(sw, -pgrad); % average value of water saturation
        sw_grad=gradientTerm(sw);
        sw_ave=arithmeticMean(sw);
        pcgrad=dpc(sw_ave).*sw_grad+dpcdk(sw_ave).*grad_phik;
        labdao = lo.*funceval(kro, sw_face);
        labdaw = lw.*funceval(krw, sw_face);
        dlabdaodsw = lo.*funceval(dkrodsw, sw_face);
        dlabdawdsw = lw.*funceval(dkrwdsw, sw_face);
        labda = labdao+labdaw;
        dlabdadsw = dlabdaodsw+dlabdawdsw;
        % compute [Jacobian] matrices
        Mdiffp1 = diffusionTerm(-labda);
        Mdiffp2 = diffusionTerm(-labdaw);
%         Mconvsw1 = convectionUpwindTerm(-dlabdadsw.*pgrad); % without capillary
        Mconvsw1 = convectionUpwindTerm(-dlabdawdsw.*pgrad-dlabdaodsw.*(pgrad+pcgrad)); % with capillary
        Mconvsw2 = convectionUpwindTerm(-dlabdawdsw.*pgrad);
        [Mtranssw2, RHStrans2] = transientTerm(sw_old, dt, phi);
        % Compute RHS values
        RHSpc1=divergenceTerm(labdao.*pcgrad);
        RHS1 = divergenceTerm((-dlabdawdsw.*pgrad-dlabdaodsw.*(pgrad+pcgrad)).*sw_face); % with capillary
        RHS2 = divergenceTerm(-dlabdawdsw.*sw_face.*pgrad);
        % include boundary conditions
        [Mbcp, RHSbcp] = boundaryCondition(BCp);
        [Mbcsw, RHSbcsw] = boundaryCondition(BCs);
        % Couple the equations; BC goes into the block on the main diagonal
        M = [Mdiffp1+Mbcp Mconvsw1; Mdiffp2 Mconvsw2+Mtranssw2+Mbcsw];
        RHS = [RHS1+RHSpc1+RHSbcp; RHS2+RHStrans2+RHSbcsw];
        % solve the linear system of equations
        x = M\RHS;
        % x = agmg(M, RHS, [], 1e-10, 500, [], [p.value(:); sw.value(:)]);
        % separate the variables from the solution
        p_new = reshapeCell(m,full(x(1:(Nx+2)*(Ny+2))));
        sw_new = reshapeCell(m,full(x((Nx+2)*(Ny+2)+1:end)));
        % calculate error values
        error_p = max(abs((p_new(:)-p.value(:))./p_new(:)));
        error_sw = max(abs(sw_new(:)-sw.value(:)));
        % assign new values of p and sw
        p.value = p_new;
        sw.value = sw_new;
    end
    t=t+dt;
    p_old = p;
    sw_old = sw;
    figure(1);visualizeCells(sw); drawnow;
end


function [fi,ps] = VF(r,T,p)
% VF Conversion of relative humidity to volume fraction of water vapor
%
% This program transfers relative humidity, absolute temperature and
% atmospheric pressure to volume fraction of water vapor and saturation
% vapour pressure using Goff-Gratch equation over plane surfaces of pure
% ice. Relation is based on integration of Clausius-Clapeyron equation and
% on experimental data. Stated range of equation validity is -100 to 0 °C.
%
% [fi,ps] = VF(r,T,p)
%
% Inputs:
%   r is the relative humidity (%). r may be a vector or a scalar
%     variable in the interval [0,100].
%
%   T means the absolute temperature (degrees K). T must have
%     the same number of elements as r. A warning is generated
%     if any elements fall outside the range [173,273].
%
%   p is the atmospheric pressure (kPa). p must have the same number
%     of elements as r. All elements must be non-negative.
%
% Results:
%   fi is the volume fraction of water vapor (%).
%
%   ps is the saturation vapour pressure (kPa).
%
% Example: At an rh of 35 %, Temp = 265 °K, atmospheric pressure = 98.9 kPa
%  [fi,ps] = VF(0.35,265,98.9)
%
%   fi =
%       0.00108
%
%   ps =
%       0.305
%
% For citation ensue following format: Malec L. (2007). VF: Conversion
% of relative humidity to volume fraction. A MATLAB file. [WWW document].
% URL http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?
% objectId=14165
%
% List R. J.: Smithsonian Meteorological Tables. Smithsonian Institution,
% Washington 1984.
%
% Copyright. February 26, 2007.

if any(r(:)<0) || any(r(:)>100)
  error 'Relative humidity(r) must be percent'
end
if any(T(:)<=0)
  error 'Temperature(T) must be positive (°K)'
elseif any(T(:)<173) || any(T(:)>273)
  warning 'Temperature(T) fell outside the working range of this relation'
end
if any(p(:)<0)
  error 'Pressure(p) must be positive (kPa)'
end

a1 = -9.09718;
a2 = -3.56654;
a3 = 0.876793;

% Induction of ice point and saturation vapour pressure.
To = 273.16;
eo = 6.1071;

% Saturation vapour pressure and water fraction computation.
ps = 10.^(a1 * (To ./ T - 1) + a2 * log10(To ./ T) + a3 * (1 - T / To) ...
    + log10(eo));
ps = ps / 10;
fi = r .* ps ./ p;


function [px, py, gx, gy] = position_7_link(z,P)
%[px,py,gx,gy] = POSITION_7_LINK(Z,P)
%
%FUNCTION:  This function computes the cartesian positions
%    of the CoM and tip of each link
%INPUTS:
%
%
%OUTPUTS:
%    px = [nLink X nTime] position of the end of each link
%    py = [nLink X nTime] position of the end of each link
%    gx = [nLink X nTime] position of the CoM of each link
%    gy = [nLink X nTime] position of the CoM of each link
%
%NOTES:
%    This file was automatically generated by writePosition.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
m4 = P.m(4); % Link 4 mass
m5 = P.m(5); % Link 5 mass
m6 = P.m(6); % Link 6 mass
m7 = P.m(7); % Link 7 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
l4 = P.l(4); % Link 4 length
l5 = P.l(5); % Link 5 length
l6 = P.l(6); % Link 6 length
l7 = P.l(7); % Link 7 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
I4 = P.I(4); % Link 4 moment of inertia about its center of mass
I5 = P.I(5); % Link 5 moment of inertia about its center of mass
I6 = P.I(6); % Link 6 moment of inertia about its center of mass
I7 = P.I(7); % Link 7 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint
d4 = P.d(4); % Link 4 distance between center of mass and parent joint
d5 = P.d(5); % Link 5 distance between center of mass and parent joint
d6 = P.d(6); % Link 6 distance between center of mass and parent joint
d7 = P.d(7); % Link 7 distance between center of mass and parent joint

th1 = z(1,:);
th2 = z(2,:);
th3 = z(3,:);
th4 = z(4,:);
th5 = z(5,:);
th6 = z(6,:);
th7 = z(7,:);

nTime = length(th1);
px = zeros(7,nTime);
py = zeros(7,nTime);
gx = zeros(7,nTime);
gy = zeros(7,nTime);

px(1,:) = l1.*cos(th1);
py(1,:) = l1.*sin(th1);
gx(1,:) = d1.*cos(th1);
gy(1,:) = d1.*sin(th1);

px(2,:) = l1.*cos(th1) + l2.*cos(th2);
py(2,:) = l1.*sin(th1) + l2.*sin(th2);
gx(2,:) = d2.*cos(th2) + l1.*cos(th1);
gy(2,:) = d2.*sin(th2) + l1.*sin(th1);

px(3,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
py(3,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);
gx(3,:) = d3.*cos(th3) + l1.*cos(th1) + l2.*cos(th2);
gy(3,:) = d3.*sin(th3) + l1.*sin(th1) + l2.*sin(th2);

px(4,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4);
py(4,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4);
gx(4,:) = d4.*cos(th4) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
gy(4,:) = d4.*sin(th4) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);

px(5,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5);
py(5,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5);
gx(5,:) = d5.*cos(th5) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4);
gy(5,:) = d5.*sin(th5) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4);

px(6,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5) + l6.*cos(th6);
py(6,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5) + l6.*sin(th6);
gx(6,:) = d6.*cos(th6) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5);
gy(6,:) = d6.*sin(th6) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5);

px(7,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5) + l6.*cos(th6) + l7.*cos(th7);
py(7,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5) + l6.*sin(th6) + l7.*sin(th7);
gx(7,:) = d7.*cos(th7) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5) + l6.*cos(th6);
gy(7,:) = d7.*sin(th7) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5) + l6.*sin(th6);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% SABR European option pricer comparison
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Descritpion: Script to Compare European Option pricers in SABR Model
% Author:      Justin Kirkby
% References:  (1) General Valuation Framework for SABR and Stochastic Local Volatility
%                   Models. SIAM J. Financial Mathematics, 2018.
%              (2) Full Fledge SABR with Markov Chains. Wimott, 2019.
% Disclaimer: this is research code, not production code. The parameter settings etc
%            should not be expected to work as is in all cases. Determining the right parameter
%            settings will depend on your usecase. This is left up to the user.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[folder, name, ext] = fileparts(which( mfilename('fullpath')));
cd(folder);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 1) CHOOSE CONTRACT/GENERAL PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T    = 1;         % time to matruity
r    = 0.05;      % interest rate
F_0  = 1.1;       % Futures price
K    = F_0*0.95;  % strike
call = 1;         % 1 = call option, else put

% -----------------
% Model Params
% -----------------
ModParams.beta   = .6;
ModParams.alpha  = 0.08;
ModParams.v0     = 0.2;
ModParams.rho    = 0.0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Reference Price (Using Obloj Asymptotic Formula with correction)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../Approx/SABR')
addpath('../../Analytical/BlackScholes')

tic
IV = SABR_Hagan_Obloj_ImpliedVol( K, F_0, ModParams.v0, T, ModParams.alpha, ModParams.beta, ModParams.rho );
price_obloj = BSM_Greeks( 0, F_0/exp(r*T), IV, r, 0, T, K, call)
time_oblog=toc;

price_ref = price_obloj;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Antonov Approximation (Using Asymptotic Formula)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tic
price_ant = SABR_European_AntonovApprox(F_0,K,T,call,r,ModParams)
time_ant = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Monte Carlo (Euler)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../Monte_Carlo')
addpath('../../Monte_Carlo/European')
M = 400;
N_sim = 5*10^4;

tic;
Spath = Simulate_SLV_func( N_sim, M, T, F_0, 0, 0, 2, ModParams);
disc = exp(-r*T);
[price_MC, stdErr] = Price_MC_European_Strikes_func(Spath, disc, call, K );

price_MC_L = price_MC - 2*stdErr; price_MC_U = price_MC + 2*stdErr;
time_MC = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  CTMC-Double Layer Method
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../PROJ/SABR/European_American_Barrier/')
addpath('../../PROJ/SABR/Helper_Functions/')

% -----------------
% Numerical Params
% -----------------
M    = 100;   % number of time steps
contract_type = 1;    % 1 = European, 2 = American, 3 = Down and Out Barrier
L    = 0.6*F_0;     % For barrier contract, this is the barrier

CTMCParams.m_0 = 26;
CTMCParams.N = 90;
CTMCParams.gridMult_v = 0.5;
CTMCParams.gridMult_s = 0.05;  %Grid mult param for S
CTMCParams.gamma = 6;  %Grid width param for variance grid

tic
price_ctmc = SABR_EurBarAmer_func(call, M, T, F_0, K, r, CTMCParams, ModParams, contract_type, L)
time_ctmc = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% COMPARE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('\n---------------------------------------------\n')
fprintf('Method      |    Price    |    Err   |  CPU \n')
fprintf('---------------------------------------------\n')
fprintf('Ref (Obloj) | %.8f  |          |       \n', price_ref)
fprintf('---------------------------------------------\n')
fprintf('Antonov     | %.8f  | %.2e | %.4f \n', price_ant, abs(price_ref-price_ant), time_ant)
fprintf('CTMC        | %.8f  | %.2e | %.4f \n', price_ctmc, abs(price_ref-price_ctmc), time_ctmc)
fprintf('MC-Euler    |[%.3f,%.3f]| %.2e | %.4f \n', price_MC_L, price_MC_U, abs(price_ref-price_MC), time_MC)
fprintf('---------------------------------------------\n')


function y = smoothRamp(x,alpha,order)
%
% y = smoothRamp(x,alpha,order)
%
% FUNCTION:
%This function is a smooth version of the standard ramp function, which is:
%   ramp(x) = 0 ; (x<0)
%   ramp(x) = x ; (x>=0)
% Smoothing is done using a polynomial transition, the degree of smoothness
% at the transitions is set using order.
%
% INPUTS:
%   x = a vector of matrix of real numbers
%   alpha = smoothing parameter: -alpha<x<alpha will be smoothed
%   order = the order of the smoothing (0=no smoothing)
%           order must be in the set {0,1,..,8}
%
% OUTPUTS:
%   y(x<-alpha) = 0;
%   y(x>alpha) = x;
%   y(else) = smooth_transition
%
        y=x;
        c1 = x < -alpha;
        c2 = x > alpha;
switch order
    case 0 % No Smoothing
        y(x<0)=0;
    case 1
        y=x;
        y(c1)=0;
        p = [1/4,1/2,1/4];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 2
        y=x;
        y(c1)=0;
        p = [-1/16,0,3/8,1/2,3/16];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 3
        y=x;
        y(c1)=0;
        p = [1/32,0,-5/32,0,15/32,1/2,5/32];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 4
        y=x;
        y(c1)=0;
        p = [-5/256,0,7/64,0,-35/128,0,35/64,1/2,35/256];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 5
        y=x;
        y(c1)=0;
        p = [7/512,0,-45/512,0,63/256,0,-105/256,0,315/512,1/2,63/512];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 6
        y=x;
        y(c1)=0;
        p = [-21/2048,0,77/1024,0,-495/2048,0,231/512,0,-1155/2048,0,693/1024,1/2,231/2048];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 7
        y=x;
        y(c1)=0;
        p = [33/4096,0,-273/4096,0,1001/4096,0,-2145/4096,0,3003/4096,0,-3003/4096,0,3003/4096,1/2,429/4096];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 8
        y=x;
        y(c1)=0;
        p = [-429/65536,0,495/8192,0,-4095/16384,0,5005/8192,0,-32175/32768,0,9009/8192,0,-15015/16384,0,6435/8192,1/2,6435/65536];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    otherwise
        error('Order not supported');
end
end


function pi = predlssvm(model,Xt,alpha,conftype)

% Construction of bias corrected 100(1-\alpha)% pointwise or
% simultaneous prediction intervals
%
% >> pi = predlssvm({X,Y,type,gam,kernel_par,kernel,preprocess}, Xt, alpha,conftype)
% >> pi = predlssvm(model, Xt, alpha, conftype)
%
% This function calculates bias corrected 100(1-\alpha)% pointwise or
% simultaneous prediction intervals. The procedure support homoscedastic
% data sets as well heteroscedastic data sets. The construction of the
% prediction intervals are based on the central limit theorem for linear
% smoothers combined with bias correction and variance estimation.
%
% 1. Using the functional interface:
%
%
% >> pi = predlssvm({X,Y,type,gam,kernel_par,kernel,preprocess})
% >> pi = predlssvm({X,Y,type,gam,kernel_par,kernel,preprocess}, alpha)
% >> pi = predlssvm({X,Y,type,gam,kernel_par,kernel,preprocess}, alpha, conftype)
%
%
%      Outputs
%        pi            : N x 2 matrix containing the lower and upper prediction intervals
%
%      Inputs
%        X             : N x d matrix with the inputs of the training data
%        Y             : N x 1 vector with the outputs of the training data
%        type          : 'function estimation' ('f') or 'classifier' ('c')
%        gam           : Regularization parameter
%        sig2          : Kernel parameter(s) (bandwidth in the case of the 'RBF_kernel')
%        kernel(*)     : Kernel type (by default 'RBF_kernel')
%        preprocess(*) : 'preprocess'(*) or 'original'
%        alpha(*)      : Significance level (by default 5%)
%        conftype(*)   : Type of prediction interval 'pointwise' or 'simultaneous' (by default 'simultaneous')
%
% 2. Using the object oriented interface:
%
%
% >> pi = predlssvm(model)
% >> pi = predlssvm(model, alpha)
% >> pi = predlssvm(model, alpha, conftype)
%
%
%      Outputs
%        pi          : N x 2 matrix containing the lower and upper confidence intervals
%
%      Inputs
%        model       : Object oriented representation of the LS-SVM model
%        alpha       : Significance level (by default 5%)
%        conftype    : Type of prediction interval 'pointwise' or 'simultaneous' (by default 'simultaneous')
%
%
%  See also:
%    trainlssvm, simlssvm, cilssvm

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

if iscell(model)
    model = initlssvm(model{:});
end

if nargin == 1
    error('Please specify a test set');
elseif nargin <= 2
    alpha = 0.05;
    conftype = 'simul';

elseif nargin <= 3
    conftype = 'simul';
end

if model.preprocess(1)=='p'
    [x,y] = postlssvm(model,model.xtrain,model.ytrain);
else
    x = model.xtrain; y = model.ytrain;
end

% train model
model = trainlssvm(model);

% smoother on test
S = smootherlssvm(model,Xt);

% bias: double smoothing with fourt order kernel RBF4
modelb = initlssvm(x,y,'f',[],[],'RBF4_kernel','o');
modelb = tunelssvm(modelb,'simplex','crossvalidatelssvm',{10,'mse'});
modelb = trainlssvm(modelb);

% output on test Yphat
Yphat = simlssvm(modelb,Xt);

% output of the smoother based on fourth order kernel on training data Yhat
Yhat = simlssvm(modelb,x);

biascorr = S*Yhat-Yphat;

%1) estimate variance nonparametrically
[sigma2,modele] = varest(model);
% smoother on training data
L = smootherlssvm(model);
sigma2t = varfun(modele,L,Xt);

%2) calculate var-cov matrix
S = S*diag(sigma2)*S';

%2b) find standardized absolute maxbias
delta = max(abs(biascorr./sqrt(diag(S))));

%3) pointwise or simultaneous
if conftype(1)=='s'
    if model.preprocess(1)=='p'
        model.xtrain = prelssvm(model,x);
    end
    z = tbform(model,alpha) + delta;
elseif conftype(1)=='p'
    z = norminv(alpha/2);
    Yphat = Yphat - biascorr;
else
    error('Wrong type of confidence interval. Please choose ''pointwise'' or ''simultaneous''');
end

V = sqrt(sigma2t+diag(S));
pi = [Yphat+z*V Yphat-z*V];

function V = varfun(modele,L,xt)
% estimate variance op new data, given the smooth of squared residuals
% "modele" and smoother matrix of the original smooth L
V = max(simlssvm(modele,xt),0);
% make estimate of V unbiased in homoscedastic case if regression
% estimate is unbiased
S = smootherlssvm(modele,xt);
V = V./(ones(size(xt,1),1)+S*diag(L*L'-2*L));

function [var,modele] = varest(model)

% if preprocessed data, construct original data
if model.preprocess(1)=='p'
    [x,y] = postlssvm(model,model.xtrain,model.ytrain);
else
    x = model.xtrain; y = model.ytrain;
end

model = trainlssvm(model);

Yh = simlssvm(model,x);

% Squared normalized residuals
e2 = (y-Yh).^2;

% Make variance model
if model.nb_data <= 200
    costfun = 'leaveoneoutlssvm'; costargs = {'mae'};
else
    costfun = 'crossvalidatelssvm'; costargs = {10,'mae'};
end
modele = initlssvm(x,e2,'f',[],[],'RBF_kernel');
modele = tunelssvm(modele,'simplex',costfun,costargs);
modele = trainlssvm(modele);

% variance model
var = max(simlssvm(modele,x),0);

% make estimate of Var unbiased in homoscedastic case if regression
% estimate is unbiased
L = smootherlssvm(model);
S = smootherlssvm(modele);

var = var./(ones(size(x,1),1)+S*diag(L*L'-L-L'));


function histo_gram = r8vec_histogram ( n, a, a_lo, a_hi, histo_num )

%*****************************************************************************80
%
%% R8VEC_HISTOGRAM computes a histogram of the elements of an R8VEC.
%
%  Discussion:
%
%    Values between A_LO and A_HI will be histogrammed into the bins
%    1 through HISTO_NUM.  Values below A_LO are counted in bin 0,
%    and values greater than A_HI are counted in bin HISTO_NUM+1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of elements of A.
%
%    Input, real A(N), the array to examine.
%
%    Input, real A_LO, A_HI, the lowest and highest
%    values to be histogrammed.  These values will also define the bins.
%
%    Input, integer HISTO_NUM, the number of bins to use.
%
%    Output, integer HISTO_GRAM(0:HISTO_NUM+1), contains the number of
%    entries of A in each bin.
%
  histo_gram(1:histo_num+2) = 0;

  delta = ( a_hi - a_lo ) / ( 2 * histo_num );

  for i = 1 : n

    if ( a(i) < a_lo )

      histo_gram(1) = histo_gram(2) + 1;

    elseif ( a(i) <= a_hi )

      j = round ( ...
        ( ( a_hi -       delta - a(i)        ) * ( 1         )   ...
        + (      -       delta + a(i) - a_lo ) * ( histo_num ) ) ...
        / ( a_hi - 2.0 * delta        - a_lo ) );

      histo_gram(j+1) = histo_gram(j+1) + 1;

    elseif ( a_hi < a(i) )

      histo_gram(histo_num+2) = histo_gram(histo_num+2) + 1;

    end

  end

  return
end


function M = Q2M(Q)
% Generate a rotation matrix from a quaternion xi+yj+zk+w,
% where Q = [x y z], and w = 1-x^2-y^2-z^2.
% See: http://skal.planet-d.net/demo/matrixfaq.htm
% _______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

%
% $Id$

Q = Q(1:3); % Assume rigid body
w = sqrt(1 - sum(Q.^2));
x = Q(1); y = Q(2); z = Q(3);
if w<1e-7,
    w = 1/sqrt(x*x+y*y+z*z);
    x = x*w;
    y = y*w;
    z = z*w;
    w = 0;
end;
xx = x*x; yy = y*y; zz = z*z; ww = w*w;
xy = x*y; xz = x*z; xw = x*w;
yz = y*z; yw = y*w; zw = z*w;
M = [...
(xx-yy-zz+ww)      2*(xy-zw)      2*(xz+yw) 0
    2*(xy+zw) (-xx+yy-zz+ww)      2*(yz-xw) 0
    2*(xz-yw)      2*(yz+xw) (-xx-yy+zz+ww) 0
           0              0              0  1];
return;


function g_nm = conjCoeffs(f_nm)
%CONJCOEFFS Get the complex SH coefficients of a conjugate sph. function
%
%   f_nm:   (N+1)^2 coefficients of original spherical function f(\theta,\phi)
%
%   g_nm:   (N+1)^2 coefficients of conjugate spherical function
%               g(\theta,\phi) = (f(\theta,\phi))*
%
%   The conversion is based on the relation Y^*_{nm} = (-1)^m Y_{n(-m)}
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   Archontis Politis, 10/10/2013
%   archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% order
N = sqrt(length(f_nm)) - 1;

g_nm = zeros((N+1)^2, 1);
for n=0:N
    for m=-n:n
        q_g = n*(n+1)+m;
        q_f = n*(n+1)-m;
        g_nm(q_g+1) = (-1)^m * conj(f_nm(q_f+1));
    end
end

end


%% Orientation Dependent Functions
%
%%
% An orientation dependent function is a function that assigns to each
% rotation or orientation a numerical value. An import example of a
% rotational function is the <ODFTheory.html orientation density function
% (ODF)> that assignes to each crystal orientation the probability of its
% occurence within a specimen. Other examples are the Schmidt or the Taylor
% factor as a function of the crystal orientation.
%
%% Definition of a orientation dependent function
%
% Within MTEX a rotational function is represented by a variable of type
% <SO3Fun.SO3Fun.html |SO3Fun|>. Let us consider as an example the function
% that takes an orientation and returns it rotational angle modulo cubic
% crystal symmetry. In MTEX the rotational angle is computed by the command
% <orientation.angle.html |angle(ori)|>. In order to turn this
% correspondence into a |SO3Fun| we use the command @SO3FunHandle and pass
% the angle command as an
% <https://de.mathworks.com/help/matlab/matlab_prog/anonymous-functions.html
% anonymous function>.

% define the crystal symmetry
cs = crystalSymmetry('cubic');

% construct the SO3Fun
SO3F = SO3FunHandle(@(ori) angle(ori) ./ degree, cs)

%%
% Many more methods for defining orientation dependent functions are
% discussed <SO3FunDefinition.html here>.
%
%%
% The entire information abot the orientation dependent function is now
% stored in the variable |SO3F|. In order to determine its value for a
% specific orientation |ori| the function <SO3Fun.eval.html |eval(ori)|> is
% used

ori = orientation.rand(cs)
SO3F.eval(ori)

%% Plotting an orientation Dependent Function
%
% Orientation dependent functions are most of visualized by sections
% according to the third Euler angle $\varphi_2$
%

plotSection(SO3F)

%%
% The plot tells us for which Euler angles the the resulting rotational
% angle is large and for which Euler angles it is low. The plot of this
% "angle function" |SO3F| becomes trivial if represented in an axis angle
% sections

plotSection(SO3F,'axisAngle','upper')
mtexColorbar
mtexColorMap parula

%%
% as obviously, the function value is constant in each section.
% Many more methods for visualizing orientation dependent functions are
% discussed <SO3FunVisualization.html here>.
%
%% Computing with orientation dependent functions
%
% The power of representing an orientation dependent functions as a
% variables of type @SO3Fun is that we may apply to it a
% <SO3FunOperations.html large number of analysis tools>. In particular,
% one can add, subtract and mutiply orientation dependent functions, plot
% them in various projections or detect the local minima or maxima. In the
% case of our example function the local maxima refers to the orientations
% with maximum rotational angle in cubic symmetry. We may compute them by
% the command <SO3Fun.max.html |max|>

[value,ori] = max(SO3F,'numLocal',10)

%%
% We observe that there are exactly six symetrically not equivalent
% orientations that realize an orientation angle of about 62.77 degree and
% form the vertices of the fundamental region in orienation space

plot(ori.symmetrise,'axisAngle','filled','markerSize',15,'restrict2FundamentalRegion')

%% Representations of Rotational Functions
%
% Internally MTEX represents rotational functions in different ways:
%
% || by a harmonic series expansion || <SO3FunHarmonicRepresentation.html SO3FunHarmonic> ||
% || as superposition of radial function || <RadialODFs.html SO3FunRBF> ||
% || as superposition of fibre elements || <FibreODFs.html SO3FunCBF> ||
% || as Bingham distribution || <BinghamODFs.html SO3FunBingham> ||
% || as sum of different components || @SO3FunComposition ||
% || explicitely given by a formula || @SO3FunHandle ||
%
% All representations allow the same operations which are specified for
% the abstact class |@SO3Fun|. In particular it is possible
% to calculate with $SO(3)$ functions as with ordinary numbers, i.e., you
% can add, multiply arbitrary functions, take the mean, integrate them or
% compute gradients, see <SO3FunOperations.html Operations>.
%
%% Generalizations of Rotational Functions
%
% || rotational vector fields || <SO3FunVectorField.html SO3VectorField> ||
% || radial rotational functions || <SO3Kernels.html SO3Kernel> ||
%


classdef lasso
%function [Problem] = lasso(A, b, lambda, sub_mode, smooth_lambda)
% This file defines the lasso (least absolute shrinkage and selection operator) problem for L1 norm.
%
% Inputs:
%       A               dictionary matrix of size dxn.
%       b               observation vector of size dx1.
%       lambda          l1-regularized parameter.
%       sub_mode        {'prox_reg', 'subgrad_reg', 'smooth', 'const'}
%       smooth_lambda
% Output:
%       Problem     problem instance.
%
%
% The problem of interest is defined as
%
%       (1) sub_mode = 'prox_reg' for L1 regularizer solved by proximal methods
%
%           min f(w) = 1/2 * || A * w - b ||_2^2 + lambda * || w ||_1
%
%       (2) sub_mode = 'subgrad_reg' for L1 regularizer solved by subgrad methods
%
%           min f(w) = 1/2 * || A * w - b ||_2^2 + lambda * || w ||_1
%
%       (3) sub_mode = 'smooth' for smoothing
%
%           min f(w) = 1/2 * || A * w - b ||_2^2 + smooth_lambda * || w ||_1.
%
%       (43) sub_mode = 'const' for L1 constraint
%
%           min f(w) = 1/2 * || A * w - b ||_2^2,      s.t. || w ||_1 < r
%
%
% "w" is the model parameter of size d vector.
%
%
% This file is part of GDLibrary and SGDLibrary.
%
% Created by H.Kasai on Apr. 17, 2017
% Modified by H.Kasai on Mar. 25, 2018
% Modified by H.Kasai on Mar. 27, 2020
% Modified by H.Kasai on Mar. 30, 2020

    properties
        name;
        dim;
        samples;
        sub_mode;
        lambda;
        d;
        n;
        A;
        b;
        AtA;
        Atb;
        L;
        prox_flag;
        sign_flag;
        idx;
        smooth_mu;      % for smoothing
        smooth_type;    % for smoothing
    end

    methods
        function obj = lasso(A, b, lambda, sub_mode, varargin)

            obj.A = A;
            obj.b = b;

            if strcmp(sub_mode, 'prox_reg')
                obj.prox_flag = true;
            elseif strcmp(sub_mode, 'subgrad_reg')
                obj.prox_flag = false;
            elseif strcmp(sub_mode, 'const')
                obj.prox_flag = true;
            elseif strcmp(sub_mode, 'smooth')
                obj.prox_flag = false;
            else
                error('Not correctly specified for sub_mode');
            end

            obj.sub_mode = sub_mode;
            obj.lambda = lambda;

            if nargin < 6
                obj.smooth_type = 'type1';
            else
                obj.smooth_type = varargin{2};
            end

            if nargin < 5
                obj.smooth_mu = 0.1;
            else
                obj.smooth_mu = varargin{1};
            end

            obj.d = size(obj.A, 2);
            obj.n = size(obj.A, 2);

            obj.name = 'lasso';
            obj.dim = obj.d;
            obj.samples = obj.n;

            obj.AtA = obj.A'*obj.A;
            obj.Atb = obj.A'*obj.b;
            %L = max(eig(AtA));
            fprintf('Calculated Lipschitz constant (L), i.e., max(eig(AtA)), .... ')
            obj.L = eigs(obj.AtA, 1);
            fprintf('is L = %f.\n', obj.L);
        end

        function v = prox(obj, w, t)
            if strcmp(obj.sub_mode, 'prox_reg')
                v = soft_thresh(w, t * obj.lambda);
            elseif strcmp(obj.sub_mode, 'const')
                %v = sign(w) .* proj_simplex(abs(w), obj.lambda, 'ineq');
                v = proj_l1_ball(w, obj.lambda);
            else
                error('Not correctly specified for sub_mode');
            end
        end

        function f = cost(obj, w)
            reg = obj.reg(w);
            f = 1/2 * sum((obj.A * w - obj.b).^2) + obj.lambda * reg;
        end

        % calculate l1 norm
        function r = reg(obj, w)
            r = norm(w,1);
        end

        function r = residual(obj, w)
            r = - obj.A * w + obj.b;
        end

        function f = cost_batch(obj, w, indices)
            error('Not implemted yet.');
        end

        function g = full_grad(obj, w)
            %g = obj.A' * (obj.A * w - obj.obj.b);
            g = obj.AtA * w - obj.Atb;
        end

        function g = grad(obj, w, indices)
            A_partial = obj.A(:,indices);
            g = A_partial' * (A_partial * w - obj.b);
        end

        % for Subgradient descent
        function subg = full_subgrad(obj, w)
            subg = obj.lambda * sign(w);
        end

        % for Smoothing
        function smooth_g = full_smooth_grad(obj, w)
            smooth_g = zeros(obj.n, 1);

            if strcmp(obj.smooth_type, 'type1') % f_mu(w) = w^2/2mu for abs(w)<= mu, abs(w)-mu/2 for abs(w)> mu.
                for i = 1 : obj.n
                    if abs(w(i)) <= obj.smooth_mu
                        smooth_g(i) = w(i) / obj.smooth_mu;
                    elseif w(i) > obj.smooth_mu
                        smooth_g(i) =1;
                    else
                        smooth_g(i) =-1;
                    end
                end
            elseif strcmp(obj.smooth_type, 'type2') % f_mu(w) = (w^2+mu^2)^(1/2)
                for i = 1 : obj.n
                    smooth_g(i) = w(i)/sqrt(w(i)^2+obj.smooth_mu^2);
                end
            elseif strcmp(obj.smooth_type, 'type3') % f_mu(w) = mu log((exp(-w/mu) + exp(w/mu))/2)
                for i = 1 : obj.n
                    p = exp(-w(i)/obj.smooth_mu);
                    q = exp(w(i)/obj.smooth_mu);
                    smooth_g(i) = (q-p)/(p+q);
                end
                %smooth_g = smooth_g;
            end

            smooth_g = obj.lambda * smooth_g;
        end

        % for Frank-Wolfe, a.k.a. Condtional Gradient
        function [s, idx, sign_flag] = LMO(obj, grad)
            [val, idx] = max( abs(grad) );
            obj.sign_flag = -1 * sign(grad(idx));
            obj.idx = idx;

            s = zeros(size(grad));
            s(idx) = obj.sign_flag * obj.lambda;

            sign_flag = obj.sign_flag;
        end

        function h = hess(obj, w, indices)
            error('Not implemted yet.');
        end

        function h = full_hess(obj, w)
            h = obj.AtA;
        end

        function hv = hess_vec(obj, w, v, indices)
            error('Not implemted yet.');
        end

        function step = exact_line_search(obj, w, dir, idx, sign_flag)
            As = sign_flag * obj.lambda * obj.A(:,idx); % = the i-th column of the dictionary matrix A
            As_minus_Ax = As - obj.A*w;
            step = max(0, min(1, As_minus_Ax' * (obj.b-obj.A*w) / (As_minus_Ax' * As_minus_Ax)));
        end

        function w_opt = calc_solution(obj, method, options_in)

            if nargin < 1
                method = 'ista';
            end

            options.max_epoch = options_in.max_epoch;
            options.w_init = options_in.w_init;
            options.verbose = options_in.verbose;
            options.tol_optgap = 1.0e-24;
            options.tol_gnorm = 1.0e-16;
            options.step_alg = 'backtracking';

            if ~options.verbose
                fprintf('Calclation of solution started ... ');
            end

            if strcmp(method, 'sd')
                [w_opt,~] = sd(obj, options);
            elseif strcmp(method, 'cg')
                [w_opt,~] = ncg(obj, options);
            elseif strcmp(method, 'newton')
                options.sub_mode = 'INEXACT';
                options.step_alg = 'non-backtracking';
                [w_opt,~] = newton(obj, options);
            elseif strcmp(method, 'ista')
                options.step_alg = 'fix';
                options.step_init = 1/obj.L;
                options.sub_mode  = 'FISTA';
                [w_opt,~] = ag(obj, options);
            else
                options.step_alg = 'backtracking';
                options.mem_size = 5;
                [w_opt,~] = lbfgs(obj, options);
            end

            if ~options.verbose
                fprintf('done\n');
            end
        end
    end
end


%RODRIGUES  Converts a rotation matrix to a rotation vector or vice versa
%
%     dst = cv.Rodrigues(src)
%     [dst,jacobian] = cv.Rodrigues(src)
%
% ## Input
% * __src__ Input rotation vector (3x1 or 1x3) or rotation matrix (3x3). Both
%   single and double-precision floating-point types are supported.
%
% ## Output
% * __dst__ Output rotation matrix (3x3) or rotation vector (3x1 or 1x3),
%   respectively. Same data type as `src`.
% * __jacobian__ Optional output Jacobian matrix, 3x9 or 9x3, which is a
%   matrix of partial derivatives of the output array components with respect
%   to the input array components. Same data type as `src`.
%
% The function transforms a rotation matrix in the following way:
%
%     theta <- norm(r)
%     r <- r/theta
%     R = cos(theta) * I + (1 - cos(theta)) * r * r^T + sin(theta) * A
%     A = [0, -rz, ry; rz, 0, -rx; -ry, rx, 0]
%
% Inverse transformation can be also done easily, since
%
%     sin(theta) * A = (R - R^T) / 2
%
% A rotation vector is a convenient and most compact representation of a
% rotation matrix (since any rotation matrix has just 3 degrees of
% freedom). The representation is used in the global 3D geometry
% optimization procedures like cv.calibrateCamera, cv.stereoCalibrate, or
% cv.solvePnP.
%
% See also: cv.calibrateCamera, cv.stereoCalibrate, cv.solvePnP,
%  rotationMatrixToVector, rotationVectorToMatrix
%


%%*********************************************************************
%% norm_min: matrix 2-norm minimization problem
%%
%% (primal) minimize || B0 + x1*B1 + ... + xm*Bm ||_2
%%
%% (dual)   maximize    - Tr B0*Q
%%          subject to  Tr Bk*Q = 0, k=1,...,m
%%                      sum of singular values of Q is less than one.
%%
%% The matrices Bk are of size p x q.
%%
%% The problem is equivalent to the following SDP:
%%
%%    minimize    t
%%                         [0,   Bk]
%%    s.t.  sum_{k=1}^m xk*[       ]
%%                         [Bk^*, 0]
%%                               [0,     i*Bk]          [0,   B0]
%%         + sum_{k=m+1}^{2m} xk*[           ] -t*I <= -[       ]
%%                               [(i*Bk)^*, 0]          [B0^*, 0]
%%
%% Adapted from Vandenberghe and Boyd, SIAM Review 96.
%% see also igmres.m
%%--------------------------------------------------------------------
%%
%%  [blk,Avec,C,b,X0,y0,Z0,objval,x] = norm_min(B,feas,solve);
%%
%%  Input:   B = cell array such that B{k} = B_k.
%%           feas  = 1 if want feasible starting point
%%                 = 0 if otherwise.
%%           solve = 0 just to initialize
%%                 = 1 if want to solve the problem.
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%*********************************************************************

   function [blk,Avec,C,b,X0,y0,Z0,objval,x] = norm_min(B,feas,solve);

   if (nargin < 3); solve = 0; end;
   if (nargin < 2); feas = 0; end;
   if isempty(feas); feas = 0; end
%%
%% dimensions
%%
   m = size(B,2)-1;
   [p,q] = size(B{1}); isrealB = 1;
   for k = 1:m;
       [p1,q1] = size(B{k});
       if any([p q]-[p1 q1]);
          error('size of B_k is not the same for all k');
       end
       isrealB = min(isrealB, isreal(B{k}));
   end
   cmp = 1-isrealB;
%%
%% A{k} = [0 Bk; Bk' 0],  i=1,...,m+1;  A{m+1} = I
%%
   if (~cmp)
       n = p+q;
       blk{1,1} = 's'; blk{1,2} = n;
       b = [zeros(m,1); -1];
       C = -[zeros(p), B{1}; B{1}', zeros(q)];
       A = cell(1,m+1);
       for k = 1:m
           Bk = B{k+1};
           A{k} = [zeros(p), Bk; Bk', zeros(q)];
       end
       A{m+1} = -speye(n,n);
       Avec = svec(blk,A,ones(size(blk,1),1));
   elseif (cmp)
       n = p+q;
       blktmp{1,1} = 's'; blktmp{1,2} = n;
       b = [zeros(2*m,1); -1];
       Ctmp{1} = -[zeros(p),  B{1}; B{1}', zeros(q)];
       A = cell(1,2*m+1); ii = sqrt(-1);
       for k = 1:m
           Bk = B{k+1};
           A{k}   = [zeros(p), Bk; Bk', zeros(q)];
           A{k+m} = [zeros(p), ii*Bk; -ii*Bk', zeros(q)];
       end
       A{2*m+1} = -speye(n,n);
       [blk,Avec,C,b] = convertcmpsdp(blktmp,A,Ctmp,b);
   end
%%
%%
   if (feas == 1);
      if (~cmp)
         X0{1} = eye(n)/n;
      else
         X0{1} = eye(2*n)/(2*n);
      end
      y0 = 1.1*ops(C,'norm')*[zeros(length(b)-1,1); 1];
      Z0 = ops(C,'-',Atyfun(blk,Avec,[],[],y0));
   elseif (feas == 0)
      [X0,y0,Z0] = infeaspt(blk,Avec,C,b);
   end
   if (solve)
      [obj,X,y,Z] = sqlp(blk,Avec,C,b,[],X0,y0,Z0);
      objval = -mean(obj);
      if ~cmp; x = y(1:m);
      else;    x = y(1:m) +sqrt(-1)*y(m+1:2*m);
      end;
   else
      objval = []; x = [];
   end
%%*********************************************************************


function asa007_test02 ( )

%*****************************************************************************80
%
%% TEST02 demonstrates the use of SYMINV.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2008
%
%  Author:
%
%    John Burkardt
%
  n_max = 15;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02:\n' );
  fprintf ( 1, '  SYMINV computes the inverse of a positive\n' );
  fprintf ( 1, '  definite symmetric matrix.\n' );
  fprintf ( 1, '  A compressed storage format is used.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Here we look at the Hilbert matrix\n' );
  fprintf ( 1, '  A(I,J) = 1 / ( I + J - 1 )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We expect errors to grow quickly with N.\n' );

  for n = 1 : n_max
%
%  Set A to the Hilbert matrix.
%
    k = 0;
    for i = 1 : n
      for j = 1 : i
        k = k + 1;
        a(k) = 1.0 / ( i + j - 1 );
      end
    end

    [ c, nullty, ifault ] = syminv ( a, n );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Matrix order N = %d\n', n );
    fprintf ( 1, '  Maxtrix nullity NULLTY = %d\n', nullty );

    k = 0;
    for j = 1 : n
      for i = 1 : j - 1
        k = k + 1;
        cfull(i,j) = c(k);
        cfull(j,i) = c(k);
      end
      k = k + 1;
      cfull(j,j) = c(k);
    end

    k = 0;
    for j = 1 : n
      for i = 1 : j - 1
        k = k + 1;
        afull(i,j) = a(k);
        afull(j,i) = a(k);
      end
      k = k + 1;
      afull(j,j) = a(k);
    end
%
%  Compute C * A - I.
%
    b(1:n,1:n) = cfull(1:n,1:n) * afull(1:n,1:n) - eye(n);

    diff = sqrt ( sum ( sum ( b(1:n,1:n).^2 ) ) );

    fprintf ( 1,  '  RMS ( C * A - I ) = %e\n', diff );
  end

  return
end


function f=evalContFrac(bFunc,aFunc,tinyVal,maxIter)
%%EVALCONTFRAC Evaluate a continued fraction of the form
%              f=b(1)+a(1)/(b(2)+a(2)/(b(3)+a(3)/(b(4)+a(4)/...
%              Given either matrices for a finite number of a and b terms
%              of function for the a and b values such that the required
%              number of terms can be requested.
%
%INPUTS: bFunc This can either be a function such that bFunc(i) provides
%              b(i) in the continued fraction, or this can be a maxNX1 or
%              1XmaxN vector of explicit b values.
%        aFunc This has to be of the same type as bFunc, either a function
%              handle or a vector to provide the a values. If a vector,
%              then it is 1X(maxN-1) or (maxN-1)X1 as it takes one fewer
%              call to the a terms than the b terms to evaluate the
%              continued fraction.
%      tinyVal An optional parameter that is less than typical values of
%              eps(b), but is not zero. This parameter is used to mitigate
%              divide-by zero errors. The default if this parameter is
%              omitted or an empty matrix is passed if 2e-100.
%      maxIter An optional parameter considering the maximum number of
%              terms to evaluate in the continued fraction. The default
%              value if omitted is 1000. This parameter is only used if
%              aFunc and bFunc are functions, not arrays. If they are
%              arrays, then the maximum number of terms used is determined
%              by the array length.
%
%OUTPUTS: f The value of the continued fraction. The smallest possible
%           value that this can take if b(0)=0 and all a and b are
%           non-negative is eps, due to how the function is initialized to
%           be numerically robust.
%
%The algorithm is the modified Lentz's method. A good description of the
%method, demonstrating its origin is given in Chapter II and Appendix C of
%[1]. The origin of the algorithm is [2]. However, the algorithm is harder
%to understand outside of the context of Bessel functions when consulting
%[2].
%
%An example of a continued fraction is an approximation to pi:
% bFunc=@(i)((i==1)*0+(i~=1)*(2*(i-1)-1));
% aFunc=@(i)(4*(i==1)+(i-1)^2*(i~=1));
% piApprox=evalContFrac(bFunc,aFunc)
%
%REFERENCES:
%[1] C.-Y. Shih, "An investigation of a stable algorithm for the evaluation
%    of fractional order Bessel functions," Master's thesis, Emporia
%    State University, Emporia, KS, Nov. 1993. [Online]. Available:
%    https://esirc.emporia.edu/bitstream/handle/123456789/1773/Shih%201993.pdf
%[2] W. J. Lentz, "Generating Bessel functions in Mie scattering
%    calculations using continued fractions," Applied optics, vol. 15, no.
%    3, pp. 668-671, Mar. 1976.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The floating point precision.
epsVal=eps();

if(nargin<3)
    tinyVal=2^(-100);
end

if(isa(bFunc,'double'))
    maxN=length(bFunc);
    maxIter=maxN-1;
elseif(nargin<4)
    maxIter=1000;
end

b0=bFunc(1);
if(b0==0)
    f=tinyVal;
else
    f=b0;
end

CPrev=f;
DPrev=0;

j=1;
while(j<maxIter)
    bj=bFunc(j+1);
    aj=aFunc(j);

    DCur=bj+aj*DPrev;
    if(DCur==0)
        DCur=tinyVal;
    end
    DCur=1/DCur;
    DPrev=DCur;

    CCur=bj+aj/CPrev;
    if(CCur==0)
        CCur=tinyVal;
    end
    CPrev=CCur;

    Delta=CCur*DCur;
    f=f*Delta;

    if(all(abs(Delta-1)<epsVal))
        break;
    end
    j=j+1;
end
end

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


function a = wathen ( nx, ny, n )

%*****************************************************************************80
%
%% WATHEN returns the WATHEN matrix.
%
%  Discussion:
%
%    The Wathen matrix is a finite element matrix which is sparse.
%
%    The entries of the matrix depend in part on a physical quantity
%    related to density.  That density is here assigned random values between
%    0 and 100.
%
%    The matrix order N is determined by the input quantities NX and NY,
%    which would usually be the number of elements in the X and Y directions.
%    The value of N is
%
%      N = 3*NX*NY + 2*NX + 2*NY + 1,
%
%    and sufficient storage in A must have been set aside to hold
%    the matrix.
%
%    A is the consistent mass matrix for a regular NX by NY grid
%    of 8 node serendipity elements.
%
%    The local element numbering is
%
%      3--2--1
%      |     |
%      4     8
%      |     |
%      5--6--7
%
%    Here is an illustration for NX = 3, NY = 2:
%
%     23-24-25-26-27-28-29
%      |     |     |     |
%     19    20    21    22
%      |     |     |     |
%     12-13-14-15-16-17-18
%      |     |     |     |
%      8     9    10    11
%      |     |     |     |
%      1--2--3--4--5--6--7
%
%    For this example, the total number of nodes is, as expected,
%
%      N = 3 * 3 * 2 + 2 * 2 + 2 * 3 + 1 = 29
%
%  Properties:
%
%    A is symmetric positive definite for any positive values of the
%    density RHO(X,Y), which is here given the value 1.
%
%    The problem could be reprogrammed so that RHO is nonconstant,
%    but positive.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
%    Andrew Wathen,
%    Realistic eigenvalue bounds for the Galerkin mass matrix,
%    IMA Journal of Numerical Analysis,
%    Volume 7, Number 4, October 1987, pages 449-457.
%
%  Parameters:
%
%    Input, integer NX, NY, values which determine the size of A.
%
%    Input, integer N, the order of N, as determined by NX and NY.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  em =  [ ...
     6.0, -6.0,  2.0, -8.0,  3.0, -8.0,  2.0, -6.0; ...
    -6.0, 32.0, -6.0, 20.0, -8.0, 16.0, -8.0, 20.0; ...
     2.0, -6.0,  6.0, -6.0,  2.0, -8.0,  3.0, -8.0; ...
    -8.0, 20.0, -6.0, 32.0, -6.0, 20.0, -8.0, 16.0; ...
     3.0, -8.0,  2.0, -6.0,  6.0, -6.0,  2.0, -8.0; ...
    -8.0, 16.0, -8.0, 20.0, -6.0, 32.0, -6.0, 20.0; ...
     2.0, -8.0,  3.0, -8.0,  2.0, -6.0,  6.0, -6.0; ...
    -6.0, 20.0, -8.0, 16.0, -8.0, 20.0, -6.0, 32.0 ];

  for j = 1 : ny

    for i = 1 : nx
%
%  For the element (I,J), determine the indices of the 8 nodes.
%
      node(1) = 3 * j * nx + 2 * j + 2 * i + 1;
      node(2) = node(1) - 1;
      node(3) = node(1) - 2;

      node(4) = ( 3 * j - 1 ) * nx + 2 * j + i - 1;
      node(8) = node(4) + 1;

      node(5) = ( 3 * j - 3 ) * nx + 2 * j + 2 * i - 3;
      node(6) = node(5) + 1;
      node(7) = node(5) + 2;

      rho = 0.5;
%     [ rho, seed ] = r8_uniform_01 ( seed );
      rho = 100.0 * rho;

      for krow = 1 : 8
        for kcol = 1 : 8

          a(node(krow),node(kcol)) = a(node(krow),node(kcol)) ...
            + rho * em(krow,kcol);

        end
      end

    end
  end

  return
end


% MAKE_B creates a weighted symmetric filter matrix encoding the
% neighborhood for isotropic label diffusion.
%
% Usage:
%
% B = make_B(1,4);
% Returns a 3 x 3 symmetric filter matrix encoding a 4-neighborhood
% corresponding to 4 equally spaced points on a 1 point radius of the
% center point.
%
% B = make_B(2,16);
% Returns a 5 x 5 symmetric filter matrix encoding 16 equally spaced points
% on a 2 point radius of the center point.
%
% Inputs:
%
% radius: distance from the center point used to determine filter matrix.
% num_points: number of neighbors used in the filter matrix.
%
% Output:
%
% B: filter matrix (approximately) encoding num_points equally spaced points
% on the speccified radius from the center point.
%
% Copyright (c) Roman Garnett, 2012--2014.

function B = make_B(radius, num_points)

  factor = 100;

  small_length = 2 * radius + 1;
  big_length = factor * small_length;

  big_B = zeros(big_length);

  offset = big_length / 2;
  side = factor / 2;

  for i = 1:num_points
    angle = (i - 1) / num_points * 2 * pi;

    x = round(offset + factor * radius * cos(angle));
    y = round(offset + factor * radius * sin(angle));

    x_lower = max(x - side, 1);
    x_upper = min(x + side, big_length);
    y_lower = max(y - side, 1);
    y_upper = min(y + side, big_length);

    big_B(x_lower:x_upper, y_lower:y_upper) = ...
        big_B(x_lower:x_upper, y_lower:y_upper) + 1;
  end

  B = zeros(small_length, small_length);

  for i = 1:small_length
    for j = 1:small_length
      x_range = (1 + (i - 1) * factor):(i * factor);
      y_range = (1 + (j - 1) * factor):(j * factor);

      B(i, j) = sum(sum(big_B(x_range, y_range)));
    end
  end

  B = B + rot90(B, 1) + rot90(B, 2) + rot90(B, 3);
  B = B / sum(B(:));

  B = round(B * 100) / 100;
end


% ======================================================== %
% Files of the Matlab programs included in the book:       %
% Xin-She Yang, Nature-Inspired Metaheuristic Algorithms,  %
% Second Edition, Luniver Press, (2010).   www.luniver.com %
% ======================================================== %

% -------------------------------------------------------- %
% Bat-inspired algorithm for continuous optimization (demo)%
% Programmed by Xin-She Yang @Cambridge University 2010    %
% -------------------------------------------------------- %
% Usage: bat_algorithm([20 1000 0.5 0.5]);                 %

% -------------------------------------------------------------------
% This is a simple demo version only implemented the basic          %
% idea of the bat algorithm without fine-tuning the parameters,     %
% Then, though this demo works very well, it is expected that       %
% this demo is much less efficient than the work reported in        %
% the following papers:                                             %
% (Citation details):                                               %
% 1) Yang X.-S., A new metaheuristic bat-inspired algorithm,        %
%    in: Nature Inspired Cooperative Strategies for Optimization    %
%    (NISCO 2010) (Eds. J. R. Gonzalez et al.), Studies in          %
%    Computational Intelligence, Springer, vol. 284, 65-74 (2010).  %
% 2) Yang X.-S., Nature-Inspired Metaheuristic Algorithms,          %
%    Second Edition, Luniver Presss, Frome, UK. (2010).             %
% 3) Yang X.-S. and Gandomi A. H., Bat algorithm: A novel           %
%    approach for global engineering optimization,                  %
%    Engineering Computations, Vol. 29, No. 5, pp. 464-483 (2012).  %
% -------------------------------------------------------------------

% Main programs starts here
function [best,fmin,N_iter]=bat_algorithm(para)
% Display help
 help bat_algorithm.m

% Default parameters
if nargin<1,  para=[20 1000 0.5 0.5];  end
n=para(1);      % Population size, typically 10 to 40
N_gen=para(2);  % Number of generations
A=para(3);      % Loudness  (constant or decreasing)
r=para(4);      % Pulse rate (constant or decreasing)
% This frequency range determines the scalings
% You should change these values if necessary
Qmin=0;         % Frequency minimum
Qmax=2;         % Frequency maximum
% Iteration parameters
N_iter=0;       % Total number of function evaluations
% Dimension of the search variables
d=10;           % Number of dimensions
% Lower limit/bounds/ a vector
Lb=-2*ones(1,d);
% Upper limit/bounds/ a vector
Ub=2*ones(1,d);
% Initializing arrays
Q=zeros(n,1);   % Frequency
v=zeros(n,d);   % Velocities
% Initialize the population/solutions
for i=1:n,
  Sol(i,:)=Lb+(Ub-Lb).*rand(1,d);
  Fitness(i)=Fun(Sol(i,:));
end
% Find the initial best solution
[fmin,I]=min(Fitness);
best=Sol(I,:);

% ======================================================  %
% Note: As this is a demo, here we did not implement the  %
% reduction of loudness and increase of emission rates.   %
% Interested readers can do some parametric studies       %
% and also implementation various changes of A and r etc  %
% ======================================================  %

% Start the iterations -- Bat Algorithm (essential part)  %
for t=1:N_gen,
% Loop over all bats/solutions
        for i=1:n,
          Q(i)=Qmin+(Qmin-Qmax)*rand;
          v(i,:)=v(i,:)+(Sol(i,:)-best)*Q(i);
          S(i,:)=Sol(i,:)+v(i,:);
          % Apply simple bounds/limits
          Sol(i,:)=simplebounds(Sol(i,:),Lb,Ub);
          % Pulse rate
          if rand>r
          % The factor 0.001 limits the step sizes of random walks
              S(i,:)=best+0.001*randn(1,d);
          end

     % Evaluate new solutions
           Fnew=Fun(S(i,:));
     % Update if the solution improves, or not too loud
           if (Fnew<=Fitness(i)) & (rand<A) ,
                Sol(i,:)=S(i,:);
                Fitness(i)=Fnew;
           end

          % Update the current best solution
          if Fnew<=fmin,
                best=S(i,:);
                fmin=Fnew;
          end
        end
        N_iter=N_iter+n;
end
% Output/display
disp(['Number of evaluations: ',num2str(N_iter)]);
disp(['Best =',num2str(best),' fmin=',num2str(fmin)]);

% Application of simple limits/bounds
function s=simplebounds(s,Lb,Ub)
  % Apply the lower bound vector
  ns_tmp=s;
  I=ns_tmp<Lb;
  ns_tmp(I)=Lb(I);

  % Apply the upper bound vector
  J=ns_tmp>Ub;
  ns_tmp(J)=Ub(J);
  % Update this new move
  s=ns_tmp;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Objective function: your own objective function can be written here
% Note: When you use your own function, please remember to
%       change limits/bounds Lb and Ub (see lines 52 to 55)
%       and the number of dimension d (see line 51).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function z=Fun(u)
% Sphere function with fmin=0 at (0,0,...,0)
z=sum(u.^2);
%%%%% ============ end ====================================


%cells(.5*n-1,.5*n-1)=1;
%cells(.5*n-2,.5*n-2)=1;
%cells(.5*n-3,.5*n-3)=1;
cells = (rand(n,n))<.5 ;
%how long for each case to stability or simple oscillators
%build an image and display it
imh = image(cat(3,cells,z,z));
set(imh, 'erasemode', 'none')
axis equal
axis tight
%index definition for cell update
x = 2:n-1;
y = 2:n-1;
%Main event loop
stop= 0; %wait for a quit button push
run = 0; %wait for a draw
freeze = 0; %wait for a freeze
while (stop==0)

 if (run==1)
 %nearest neighbor sum
 sum(x,y) = cells(x,y-1) + cells(x,y+1) + ...
 cells(x-1, y) + cells(x+1,y) + ...
 cells(x-1,y-1) + cells(x-1,y+1) + ...
 cells(3:n,y-1) + cells(x+1,y+1);
 % The CA rule
 cells = (sum==3) | (sum==2 & cells);
 %draw the new image
 set(imh, 'cdata', cat(3,cells,z,z) )
 %update the step number diaplay
 stepnumber = 1 + str2num(get(number,'string'));
 set(number,'string',num2str(stepnumber))
 end

 if (freeze==1)
 run = 0;
 freeze = 0;
 end
 drawnow %need this in the loop for controls to work
end
cells = (rand(n,n))<.5 ;
%how long for each case to stability or simple oscillators
%build an image and display it
imh = image(cat(3,cells,z,z));
set(imh, 'erasemode', 'none')
axis equal
axis tight
%index definition for cell update
x = 2:n-1;
y = 2:n-1;
%Main event loop
stop= 0; %wait for a quit button push
run = 0; %wait for a draw
freeze = 0; %wait for a freeze
while (stop==0)

 if (run==1)
 %nearest neighbor sum
 sum(x,y) = cells(x,y-1) + cells(x,y+1) + ...
 cells(x-1, y) + cells(x+1,y) + ...
 cells(x-1,y-1) + cells(x-1,y+1) + ...
 cells(3:n,y-1) + cells(x+1,y+1)+...
 cells(x,y);
 % The CA rule
 cells = ~((sum<4) | (sum==5));
 %draw the new image
 set(imh, 'cdata', cat(3,cells,z,z) )
 %update the step number diaplay
 stepnumber = 1 + str2num(get(number,'string'));
 set(number,'string',num2str(stepnumber))
 end

 if (freeze==1)
 run = 0;
 freeze = 0;
 end

 drawnow %need this in the loop for controls to work

end


function [bc] = PoissonIPDGbc3D(ubc)

% function [bc] = PoissonIPDGbc3D(ubc)
% Purpose: Set up the discrete Poisson matrix directly
%          using IP. The operator is set up in the weak form

Globals3D;

% face mass matrix 1
Fm = Fmask(:,1); faceR = r(Fm); faceS = s(Fm);
V2D = Vandermonde2D(N, faceR, faceS);
massEdge(Fm,Fm,1) = inv(V2D*V2D');

% face mass matrix 2
Fm = Fmask(:,2); faceR = r(Fm); faceT = t(Fm);
V2D = Vandermonde2D(N, faceR, faceT);
massEdge(Fm,Fm,2) = inv(V2D*V2D');

% face mass matrix 3
Fm = Fmask(:,3); faceS = s(Fm); faceT = t(Fm);
V2D = Vandermonde2D(N, faceS, faceT);
massEdge(Fm,Fm,3) = inv(V2D*V2D');

% face mass matrix 4
Fm = Fmask(:,4); faceS = s(Fm); faceT = t(Fm);
V2D = Vandermonde2D(N, faceS, faceT);
massEdge(Fm,Fm,4) = inv(V2D*V2D');

% build DG right hand side
bc = zeros(Np, K);

for k1=1:K
  if(~mod(k1,1000)) k1, end;

  for f1=1:Nfaces

    if(EToE(k1,f1)==k1)

      Fm1 = Fmask(:,f1);
      fidM  = (k1-1)*Nfp*Nfaces + (f1-1)*Nfp + (1:Nfp)';

      id = 1+(f1-1)*Nfp + (k1-1)*Nfp*Nfaces;
      lnx = nx(id);  lny = ny(id);  lnz = nz(id);
      lsJ = sJ(id); hinv = Fscale(id);

      Dx = rx(1,k1)*Dr + sx(1,k1)*Ds + tx(1,k1)*Dt;
      Dy = ry(1,k1)*Dr + sy(1,k1)*Ds + ty(1,k1)*Dt;
      Dz = rz(1,k1)*Dr + sz(1,k1)*Ds + tz(1,k1)*Dt;
      Dn1 = lnx*Dx + lny*Dy +lnz*Dz;

      mmE = lsJ*massEdge(:,:,f1);

      gtau = 2*(N+1)*(N+1)*hinv; % set penalty scaling

      bc(:,k1) = bc(:,k1) + (gtau*mmE(:,Fm1) - Dn1'*mmE(:,Fm1))*ubc(fidM);

    end
  end
end
return


function D=dist_mat(P1, P2)

n1 = zeros(1,size(P1,1));
n2 = zeros(1,size(P2,1));
for i=1:size(P1,1)
    n1(i) = norm(P1(i,:));
end
for i=1:size(P2,1)
    n2(i) = norm(P2(i,:));
end
n1 = n1.^2;
n2 = n2.^2;

D = -2*P1*P2';
N = repmat(n1',[1,size(P2,1)]) + repmat(n2,[size(P1,1),1]);
D = D+N;

D = sqrt(D);


function [y,zf,u,p]=randfilt(pb,pa,ny,zi)
%RANDFILT Generate filtered gaussian noise without initial transient
%
%  Inputs: pb(1,:)  Numerator polynomial of discrete time filter
%          pa(1,:)  Denominator polynomial of discrete time filter
%          ny       Number of output samples required
%          zi       Optional initial filter state
%
% Outputs: y(ny,1)  Filtered Gaussian noise
%          zf       final filter state (can be used to extend the noise sequence)
%          u        The state covariance matrix, <zf*zf'>, is u*u'
%          p        Is the expected value of y(i)^2
%
% zf and zi are the output and optional input state as defined in filter()
% If zi is not specified, random numbers with the correct covariance will be used.
% output u*u' is the state covariance matrix for filter(). Output p is the
% mean power of the output signal y.

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: randfilt.m 5885 2015-03-18 09:35:01Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% first normalize the denominator polynomial if necessary

if pa(1)~=1
    pb=pb/pa(1); pa=pa/pa(1);
end

% check to see if we must generate zi

if nargin<4 | nargout>2
    lb=length(pb);
    la=length(pa);

    k=max(la,lb)-1;
    l=la-1;
    ii=k+1-l:k;

    % form controllability matrix

    q=zeros(k,k);
    [z,q(:,1)]=filter(pb,pa,1);
    for i=2:k [z,q(:,i)]=filter(pb,pa,0,q(:,i-1)); end

    % we generate m through the step-down procedure
    s=randn(k,1);
    if l
        m=zeros(l,l);
        g=pa;
        for i=1:l
            g=(g(1)*g(1:end-1)-g(end)*g(end:-1:2))/sqrt((g(1)-g(end))*(g(1)+g(end)));
            m(i,i:l)=g;
        end
        s(ii)=triu(toeplitz(pa(1:l)))*(m\s(ii));
        if nargout>2
            u=q;
            u(:,ii)=q(:,ii)*triu(toeplitz(pa(1:l)))/m;
        end
    else
        if nargout>2
            u=q;
        end
    end
    if nargin < 4
        if k
            zi=q*s;
        else
            zi=[];
        end
    end
end
if nargout>2
    if ~numel(u)
        p=pb(1).^2;
    else
        p=u(1,:)*u(1,:)'+pb(1).^2;
    end
end
if nargin>2 && ny>0
    [y,zf]=filter(pb,pa,randn(ny,1),zi);
else
    zf=zi;
    y=[];
end


function [ d2q ] = gen_DirectDynamics(q,Pcii,Icii,mcii,dq,taw)
%% This function is used to calculate the direct dynamics of the KUKA iiwa 7  R 800
% This function proposes that the robot is mounted with the base in the
% horizontal poistion.

% Arreguments:
%--------------------
% q: is 1x7 vector, joint nagles vector of the manipulator
% dq: is 1x7 vector, joint angular velocity vector
% taw: 1x7 vector, the torques on the joints.
% Pcii: is 3X7 matrix while each column represents the local coordinates
% of the center of mass of each link.
% Icii: is (3x3x7) matrix, each 3x3 matrix of which represnets the
% associated link inertial tensor represented in its local inertial frame
% mcii: is (1x7) vector, each element of which specifies a mass of one of
% the links

% Return value:
%--------------------
% d2q: is 7x1 vector, joint angular acceleration vector

% Copyright: Mohammad SAFEEA, 9th-April-2018

[M]=gen_MassMatrix(q,Pcii,Icii,mcii);
[B]=gen_CoriolisMatrix(q,Pcii,Icii,mcii,dq);
[ G ] = gen_GravityVector(q,Pcii,mcii);
% convert angular velocity/acceleration into column vectors
dq=columnVec(dq);
taw=columnVec(taw);
d2q=M\(taw-B*dq-G);
end

function y=columnVec(x)
if(size(x,2)==1)
    y=x;
else
    y=x';
end
end


function sc = census(I,J)

% CENSUS  Census Correlation coefficient
%   CENSUS(I,J) computes the census score of matrices I and J:
%
%     CENSUS = 1/N * ~xor( I > Io , J > Jo )
%
%   where N  = prod(size(I))
%         Io = I(central pixel)
%         Jo = J(central pixel)
%   and   size(I) = size(J)
%
%   See also PATCHCORR, ZNCC, SSD

% (c) 2005 Joan Sola

if size(I) ~= size(J)
    error ('Matrices must be the same size.')
else
    s = size(I); % patch size
    if any(iseven(s))
        error('Matrix sizes must be odd.')
    else
        c = (s+1)/2; % patch center
        i = I(c(1),c(2)); % central pixel
        j = J(c(1),c(2)); % central pixel

        V = I > i;
        W = J > j;

        SC = ~xor(V,W);
        sc = sum(sum(SC))/numel(I);
    end
end
% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [logp, yhat, res] = tapas_rs_surprise(r, infStates, ptrans)
% Calculates the log-probability of response speed y (in units of ms^-1) according to the surprise
% model introduced in:
%
% Vossel, S.*, Mathys, C.*, Daunizeau, J., Bauer, M., Driver, J., Friston, K. J., and Stephan, K. E.
% (2013). Spatial Attention, Precision, and Bayesian Inference: A Study of Saccadic Response Speed.
% Cerebral Cortex.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform zetas to their native space
ze1v = exp(ptrans(1));
ze1i = exp(ptrans(2));
ze2  = exp(ptrans(3));
ze3  = exp(ptrans(4));

% Initialize returned log-probabilities, predictions,
% and residuals as NaNs so that NaN is returned for all
% irregualar trials
n = size(infStates,1);
logp = NaN(n,1);
yhat = NaN(n,1);
res  = NaN(n,1);

% Weed irregular trials out from inferred states, responses, and inputs
mu1hat = infStates(:,1,1);
mu1hat(r.irr) = [];

y = r.y(:,1);
y(r.irr) = [];

u = r.u(:,1);
u(r.irr) = [];

% Calculate alpha (i.e., attention)
alpha = 1./(1-log2(mu1hat));

% Calculate predicted response speed
rs = u.*(ze1v + ze2*alpha) + (1-u).*(ze1i + ze2*(1-alpha));

% Calculate log-probabilities for non-irregular trials
% Note: 8*atan(1) == 2*pi (this is used to guard against
% errors resulting from having used pi as a variable).
reg = ~ismember(1:n,r.irr)
logp(reg) = -1/2.*log(8*atan(1).*ze3) -(y-rs).^2./(2.*ze3);
yhat(reg) = rs;
res(reg) = y-rs;

return;


function w = hermite_cubic_spline_quad_rule ( nn, xn )

%*****************************************************************************80
%
%% HERMITE_CUBIC_SPLINE_QUAD_RULE: Hermite cubic spline quadrature rule.
%
%  Discussion:
%
%    The integral is taken over the definition interval [X(1),X(NN)].
%
%    Note that if the intervals are equal in size, then the derivative
%    information in DN has no effect on the integral value,
%    except for the first and last entries.
%
%    The quadrature rule is
%
%      Integral ( XN(1) <= X <= XN(NN) ) F(X) dX is approximately
%
%      Sum ( 1 <= I <= NN ) W(1,I) * F(X(I)) + W(2,I) * F'(X(I))
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2011
%
%  Author:
%
%    John Burkardt.
%
%  Reference:
%
%    Fred Fritsch, Ralph Carlson,
%    Monotone Piecewise Cubic Interpolation,
%    SIAM Journal on Numerical Analysis,
%    Volume 17, Number 2, April 1980, pages 238-246.
%
%  Parameters:
%
%    Input, integer NN, the number of data points.
%
%    Input, real XN(NN), the coordinates of the data points.
%    The entries in XN must be in strictly ascending order.
%
%    Output, real W(2,NN), the quadrature weights for F(1:NN)
%    and DN(1:NN).
%
  w = zeros ( 2, nn );

  w(1,1)      = 0.5 * ( xn(1,2)    - xn(1,1)      );
  w(1,2:nn-1) = 0.5 * ( xn(1,3:nn) - xn(1,1:nn-2) );
  w(1,nn)     = 0.5 * ( xn(1,nn)   - xn(1,nn-1)   );

  w(2,1)      =   ( xn(1,2) - xn(1,1) ).^2 / 12.0;
  w(2,2:nn-1) =   ( xn(1,3:nn) - xn(1,1:nn-2) ) ...
                .* ( xn(1,3:nn) - 2.0 * xn(1,2:nn-1) + xn(1,1:nn-2) ) / 12.0;
  w(2,nn)     = - ( xn(1,nn-1) - xn(1,nn) ).^2 / 12.0;

  return
end


%==========================================================================
%                         copywrite 2011
%           iterative-Maximum ratio combining (i-MRC) detector
%     for spatial Modulation (SM) MIMO transmission technique.
%
% This Matlab code computes the BER of the SM for the case where the
% channel coefficients are modeled as uncorrelated  Rayleigh fading channel
% M is the modulation order of M-QAM schemes.
% Mt = number of transmit antennas.
% Mr = number of receive antennas.
%                This code is written by Rajab Legnain
%==========================================================================
function simulation_of_SM_iMRC()
clear all;   clc;
mode = 1;                    % mode = 1 the channel is     normalized,
                             % mode = 2 the channel is not normalized
global M Mt hmodem bit_T
M = 16;
Mt = 4;
Mr = 4;
bit_SMsym = log2(M*Mt);      % number of bit per spatial modulation sysmbol
Nbits = bit_SMsym*1e4;       % Number of bits to be simulated.

hmodem =   modem.qammod('M',M,  'SymbolOrder', 'Gray','InputType', 'bit');
hdemodem = modem.qamdemod('M', M,'SymbolOrder','Gray','OutputType','bit');
Eac = (mean(hmodem.Constellation .* conj(hmodem.Constellation)));
SNR = 0 : 2 :24;             % signal-to-noise ratio in dB
No= (Eac)*10.^(-SNR/10);     % noise variance

L_SNR=length(SNR);
ber= zeros (L_SNR,1);
bit_R=zeros(Nbits, 1);

%%
bit_T = randi([0 1],Nbits,1);
[mod_T ante]= SpatialMod();    % Spatial Modulation
mod_T = mod_T.';

for ii=1:L_SNR
    for j = 1 : size(mod_T ,2)
        channel = sqrt(.5)*( randn(Mr,Mt,1) + 1i*randn(Mr,Mt,1));
        noise = sqrt(.5)*(randn(Mr , 1) + 1i*randn(Mr , 1))* sqrt(No(ii));
        p = diag(channel'*channel);
        switch mode
            case 1
               y = channel(:,ante(j))*(mod_T(ante(j) ,j)./sqrt(p(ante(j)))) + noise;
               z = (channel'*y)./p.^.5;
            case 2
               y = channel(:,ante(j)) * mod_T(ante(j) ,j) + noise ;
               z = (channel'*y)./p;
        end
        [~,ant_est] = max(abs(z));
        bi_ant = de2bi(ant_est - 1 ,  log2(Mt)  ,'left-msb');
        bi_mod = demodulate(hdemodem, z(ant_est,1) );
        bit_R( (j-1)*bit_SMsym+1 : (j-1)*bit_SMsym+bit_SMsym,1) = [bi_ant.' ; bi_mod];

    end
    [~,ber(ii,1)] = biterr(bit_T(:,1),bit_R(:));
end

semilogy(SNR,ber(:,1),'color',[0,0.75,0.75],'linestyle','-','LineWidth',1);
grid on;
xlabel('$$SNR$$','Interpreter','latex')
ylabel('BER','Interpreter','latex')
title('Bit Error Rate of SM using i-MRC detector')
legend([num2str(M),'-QAM'] ,  'Location','NorthEast')
end

function [signal ant_no]= SpatialMod()
global M Mt hmodem bit_T
Nt    = log2(Mt);
Nobit = log2(M);
x = reshape(bit_T,Nobit+Nt,[]);

ant_no =  bi2de([x(1:Nt,:)].' , 'left-msb') + 1;
digMod = modulate(hmodem,x(Nt+1:end,:));

signal = zeros(Mt , length(digMod));

for i = 1 : length(digMod)
    signal( ant_no(i) , i) = digMod(i);
end

signal = signal.';
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve the following exercises:
%   A) Parabolic. with friction.
%
% Help: Function prototype
% [y, t] = runge_kutta(f, y0, [t0 tfinal], timestep)
% where f is the function being integrated as dy/dt = f(t, y).
% y0 are the initial conditions
% t0: initial time
% tfinal: final time.
% h: time step for the calculations.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function parabolic_with_friction()
global g k m
close all;
g = 9.81; %m/s^ 2. gravity at equator
% datos de ajustados a 1000 m de https://en.wikipedia.org/wiki/Ballistic_table#/media/File:Ballistic_table_for_7.62x51_mm_NATO_(mil_and_moa).png
% 7.62x51 mm NATO ammunition
theta0 = 0; %initial angle
k = 0.0000005; % N*s/m friction coefficient
m=9.7/1000; %mass of the bullet kg
v=860; % m/s initial speed

theta0 = 0; %initial angle
k = 0.0000005; % N*s/m friction coefficient
m=0.47; %mass of the bullet kg
v=609; % m/s initial speed
 % 914

t0 = 0;
tfinal = 2.2;
x0 = 0;
y0 = 0;
vx0 = v*cos(theta0);
vy0 = v*sin(theta0);

[t, x] = runge_kutta(@parabolic_friction_square, [x0 y0 vx0 vy0]', [t0 tfinal], 0.01);
x = x(:, 1:length(t));
plot(t, x(1,:), 'r'), hold
plot(t, x(2,:), 'g')
plot(t, x(3,:), 'b')
plot(t, x(4,:), 'c')
legend('Position X (m) RK4', 'Position Y (m) RK4', 'Speed X (m/s) RK4', 'Speed Y (m/s) RK4')

figure,
plot(x(1,:), x(2,:), 'r')
xlabel('X Position (m)')
ylabel('Y Position (m)')

x(2,end)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to solve a constant acceleration on x axis on a ramp.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = parabolic_friction_square(t, x)
global k g m
% We must return the solution of
% [dx1/dt; dx2/dt; dx3/dt dx4/dt]
vx = x(3);
vy = x(4);

%forces in both axes
sq = sqrt(vx^2+vy^2);
xd(1) = vx;
xd(2) = vy;
xd(3) = -(k/m)*vx*sq;
xd(4) = -(k/m)*vy*sq - g;
xd = xd(:);


function err = getL2error3NE1(node,elem,exactE,Eh,markedElem)
%% GETL2ERROR3NE1 L2 norm of the linear Nedelec edge element.
%
%  err = getL2error3NE(node,elem,exactE,Eh,markedElem);
%
% Example
%
%     node = [-1,-1,-1; 1,-1,-1; 1,1,-1; -1,1,-1; -1,-1,1; 1,-1,1; 1,1,1; -1,1,1];
%     elem = [1,2,3,7; 1,6,2,7; 1,5,6,7; 1,8,5,7; 1,4,8,7; 1,3,4,7];
%     maxIt = 4;
%     L2Err = zeros(maxIt,1);
%     N = zeros(maxIt,1);
%     for k = 1:maxIt
%         [node,elem] = uniformbisect3(node,elem);
%         [elem2dof,edge] = dof3edge(elem);
%         pde = Maxwelldata2;
%         uI = edgeinterpolate1(pde.exactu,node,edge);
%         L2Err(k) = getL2error3NE1(node,elem,pde.exactu,uI);
%         N(k) = length(uI);
%     end
%     r = showrate(N,L2Err,1,'b-+');
%     legend('||u-u_I||',['N^{' num2str(r) '}'],'LOCATION','Best');
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Sort elem to ascend ordering
elem = sort(elem,2);

%% Construct Data Structure
elem2dof = dof3edge(elem);
NT = size(elem,1); Ndof = max(elem2dof(:)); %N = size(node,1);
[Dlambda,volume] = gradbasis3(node,elem);
locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];

%% Compute square of the L2 error element-wise
[lambda,w] = quadpts3(3); % quadrature order is 3
nQuad = size(lambda,1);
err = zeros(NT,1);
for p = 1:nQuad
    % quadrature points in the x-y-z coordinate
    pxy = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:) ...
        + lambda(p,4)*node(elem(:,4),:);
    Ep = exactE(pxy);
    Ehp = zeros(NT,3);
    for k = 1:6 % for each basis
        i = locEdge(k,1); j = locEdge(k,2);
        % phi_k = lambda_iDlambda_j - lambda_jDlambda_i;
        Ehp = Ehp + repmat(Eh(elem2dof(:,k)),1,3).*...
                   (lambda(p,i)*Dlambda(:,:,j)-lambda(p,j)*Dlambda(:,:,i));
        % psi_k = lambda_iDlambda_j + lambda_jDlambda_i;
        Ehp = Ehp + repmat(Eh(elem2dof(:,k)+Ndof),1,3).*...
                   (lambda(p,i)*Dlambda(:,:,j)+lambda(p,j)*Dlambda(:,:,i));
    end
    err = err + w(p)*sum((Ep - Ehp).^2,2);
end
err = err.*volume;
if (nargin == 5) && ~isempty(markedElem)
    err = err(markedElem); % L2 err on some marked region
end
err = sqrt(sum(err));


function cheby_t_poly_test ( )

%*****************************************************************************80
%
%% CHEBY_T_POLY_TEST tests CHEBY_T_POLY.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 March 2012
%
%  Author:
%
%    John Burkardt
%
  n_max = 12;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBY_T_POLY_TEST:\n' );
  fprintf ( 1, '  CHEBY_T_POLY evaluates the Chebyshev T polynomial.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N      X        Exact F       T(N)(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, x, fx ] = cheby_t_poly_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = cheby_t_poly ( 1, n, x );

    fprintf ( 1, '  %6d  %8f  %12f  %12f\n', n, x, fx, fx2(n+1) );

  end

  return
end


% function x = cwt_iw(Wx, type, opt)
%
% The inverse wavelet transform of signal Wx
%
% Implements Eq. (4.67) of [1].
%
% 1. Mallat, S., Wavelet Tour of Signal Processing 3rd ed.
%
% Inputs:
%  Wx: wavelet transform of a signal, see help cwt_fw
%  type: wavelet used to take the wavelet transform,
%        see help cwt_fw and help wfiltfn
%  opt: options structure used for forward wavelet transform.
%
% Output:
%  x: the signal, as reconstructed from Wx
%
%---------------------------------------------------------------------------------
%    Synchrosqueezing Toolbox
%    Authors: Eugene Brevdo (http://www.math.princeton.edu/~ebrevdo/)
%---------------------------------------------------------------------------------
function x = cwt_iw(Wx, type, opt)
    if nargin<3, opt = struct(); end

    [na, n] = size(Wx);

    [N,n1,n2] = p2up(n);
    Wxp = zeros(na, N);

    % TODO - do we want to pad the wavelet representation here?  Or not
    % cut off Wx in cwt_fw and then use that to reconstruct?
    Wxp(:, n1+1:n1+n) = Wx;
    Wx = Wxp; clear Wxp;

    % Following the same value in cwt_fw
    noct = log2(N)-1;
    nv = na/noct;
    as = 2^(1/nv) .^ (1:1:na);

    assert(mod(noct,1) == 0);
    assert(nv>0 && mod(nv,1)==0); % integer

    % Find the admissibility coefficient Cpsi
    Cpsi = cwt_adm(type, opt);

    x = zeros(1, N);
    for ai=1:na
        a = as(ai);
        Wxa = Wx(ai, :);

        psih = wfilth(type, N, a, opt);

        % Convolution theorem here
        Wxah = fft(Wxa);
        xah = Wxah .* psih;
        xa = ifftshift(ifft(xah));

        x = x + xa/a;
    end

     % Take real part and normalize by log_e(a)/Cpsi
     x = log(2^(1/nv))/Cpsi * real(x);

     % Keep the unpadded part
     x = x(n1+1: n1+n);

end % cwt_iw


function fd1d_burgers_lax ( )

%*****************************************************************************80
%
%% FD1D_BURGERS_LAX solves the nonviscous Burgers equation using Lax-Wendroff.
%
%  Discussion:
%
%    The non-viscous time-dependent Burgers equation is:
%
%      du/dt + u du/dx = 0
%
%    which can be written in conservative form as
%
%      du/dt + 1/2 d/dx ( u^2 ) = 0
%
%    or
%
%      du/dt + dF/dx = 0
%
%    For the Burgers equation, we define
%
%      F(x,t) = 1/2 u^2,
%      A(x,t) = dF/dx = u
%
%    and then the Lax-Wendroff method approximates the solution
%    using the iteration:
%
%      u(x,t+dt) = u(t) - dt dF/dx + 1/2 dt^2 d/dx A dF/dx
%
%    which can be written:
%
%      u(x,t+dt) = u(x,t) - dt ( F(x+dx,t) - F(x-dx,t) ) / ( 2 * dx )
%        + 1/2 dt^2/dx^2 ( A(x+dx/2,t) * ( F(x+dx,t) - F(x,t) )
%                        - A(x-dx/2,t) * ( F(x,t) - F(x-dx,t) )
%
%    where we approximate:
%
%      A(x+dx/2,t) = 1/2 ( u(x+dx,t) + u(x,t) )
%      A(x-dx/2,t) = 1/2 ( u(x,t) + u(x-dx,t) )
%
%    There is a stability condition that applies here, which requires that
%
%      dt * max ( abs ( u ) ) / dx <= 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FD1D_BURGERS_LAX:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Solve the non-viscous time-dependent Burgers equation,\n' );
  fprintf ( 1, '  using the Lax-Wendroff method.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Equation to be solved:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    du/dt + u * du/dx = 0\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  for x in [ a, b ], for t in [t_init, t_last]\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  with initial conditions:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    u(x,o) = u_init\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  and boundary conditions:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    u(a,t) = u_a(t), u(b,t) = u_b(t)\n' );
%
%  Set and report the problem parameters.
%
  n = 41;
  a = -1.0;
  b = +1.0;
  dx = ( b - a ) / ( n - 1 );
  step_num = 80;
  t_init = 0.0;
  t_last = 1.0;
  dt = ( t_last - t_init ) / step_num;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  %f <= X <= %f\n', a, b );
  fprintf ( 1, '  Number of nodes = %d\n', n );
  fprintf ( 1, '  DX = %f\n', dx );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %f <= t <= %f\n', t_init, t_last );
  fprintf ( 1, '  Number of time steps = %d\n', step_num );
  fprintf ( 1, '  DT = %f\n', dt );

  x = r8vec_even ( n, a, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  X:\n' );
  fprintf ( 1, '\n' );
  for ilo = 1 : 5 : n
    ihi = min ( ilo + 4, n );
    for i = ilo : ihi
      fprintf ( 1, '  %16f', x(i,1) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Set the initial condition,
%  and apply boundary conditions to first and last entries.
%
  step = 0;
  t = t_init;
  un(1:n,1) = u_init ( n, x, t );
  un(1,1) = u_a ( x(1,1), t );
  un(n,1) = u_b ( x(n,1), t );

  stability = ( dt / dx ) * max ( abs ( un(1:n,1) ) );
  report ( step, step_num, n, x, t, un, stability );

  if ( 1 )
    plot ( x, un );
    title ( sprintf ( 'Step %d, Time %f', step, t ) );
    pause
  end
%
%  March in time.
%
  c1 = - ( 0.5 * dt   / dx );
  c2 = - ( 0.5 * dt^2 / dx^2 );

  for step = 1 : step_num

    t = ( ( step_num - step ) * t_init   ...
        + (            step ) * t_last ) ...
        / ( step_num        );

    uo(1:n,1) = un(1:n,1);

    un(2:n-1,1) = uo(2:n-1,1) ...
      -       ( dt    / dx  ) * ( uo(3:n,1).^2 - uo(1:n-2,1).^2 ) ...
      + 0.5 * ( dt^2 / dx^2 ) * (  0.5 * ( uo(3:n,1)   + uo(2:n-1,1) ) ...
                                   .* ( uo(3:n,1).^2   - uo(2:n-1,1).^2 ) ...
                                 - 0.5 * ( uo(2:n-1,1) + uo(1:n-2,1) ) ...
                                   .* ( uo(2:n-1,1).^2 - uo(1:n-2,1).^2 ) );

    un(1,1) = u_a ( x(1,1), t );
    un(n,1) = u_b ( x(n,1), t );

    stability = ( dt / dx ) * max ( abs ( un(1:n,1) ) );
    report ( step, step_num, n, x, t, un, stability );

    if ( 1 )
      plot ( x, un );
      title ( sprintf ( 'Step %d, Time %f', step, t ) );
      pause
    end

  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FD1D_BURGERS_LAX:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function a = r8vec_even ( n, alo, ahi )

%*****************************************************************************80
%
%% R8VEC_EVEN returns N real values, evenly spaced between ALO and AHI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 January 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of values.
%
%    Input, real ALO, AHI, the low and high values.
%
%    Output, real A(N), N evenly spaced values.
%    Normally, A(1) = ALO and A(N) = AHI.
%    However, if N = 1, then A(1) = 0.5*(ALO+AHI).
%
  if ( n == 1 )

    a(1,1) = 0.5 * ( alo + ahi );

  else

    a(1:n,1) = ( (n-1:-1:0) * alo + (0:n-1) * ahi ) / ( n - 1 );

  end

  return
end
function report ( step, step_num, n, x, t, u, stability )

%*****************************************************************************80
%
%% REPORT prints or plots or saves the data at the current time step.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer STEP, the index of the current step,
%    between 0 and STEP_NUM.
%
%    Input, integer STEP_NUM, the number of steps to take.
%
%    Input, integer N, the number of nodes.
%
%    Input, real X(N), the coordinates of the nodes.
%
%    Input, real T, the current time.
%
%    Input, real U(N), the initial values U(X,T).
%
%    Input, real STABILITY, the stability factor, which should be
%    no greater than 1.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  STEP = %d\n', step );
  fprintf ( 1, '  TIME = %f\n', t );
  fprintf ( 1, '  STABILTY = %f\n', stability )
  fprintf ( 1, '\n' );
  for ilo = 1 : 5 : n
    ihi = min ( ilo + 4, n );
    for i = ilo : ihi
      fprintf ( 1, '  %14g', u(i) );
    end
    fprintf ( 1, '\n' );
  end

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end
function ua = u_a ( x, t )

%*****************************************************************************80
%
%% U_A sets the boundary condition for U at A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, T, the position and time.
%
%    Output, real UA, the prescribed value of U(X,T).
%
  ua = + 0.5;

  return
end
function ub = u_b ( x, t )

%*****************************************************************************80
%
%% U_B sets the boundary condition for U at B.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, T, the position and time.
%
%    Output, real UB, the prescribed value of U(X,T).
%
  ub = - 0.5;

  return
end
function u = u_init ( n, x, t )

%*****************************************************************************80
%
%% U_INIT sets the initial condition for U.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of nodes.
%
%    Input, real X(N), the coordinates of the nodes.
%
%    Input, real T, the current time.
%
%    Output, real U(N), the initial values U(X,T).
%
  ua = u_a ( x(1,1), t );
  ub = u_b ( x(n,1), t );

  q = 2.0 * ( ua - ub ) / pi;
  r = ( ua + ub ) / 2.0;
%
%  S can be varied.  It is the slope of the initial condition at the midpoint.
%
  s = 1.0;
  u(1:n,1) = ( 2 * x(1:n,1) - x(n,1) - x(1,1) ) ...
           / (                x(n,1) - x(1,1) );

  u(1:n,1) = - q * atan ( s * u(1:n,1) ) + r;

  return
end


function [Y, spectrum] = slgembed(G, Gc, d, fm, varargin)
%SLGEMBED Solves the general graph-based embedding
%
% $ Syntax $
%   - Y = slgembed(G, Gc, d, fm, ...)
%   - [Y, spectrum] = slgembed(G, Gc, d, fm, ...)
%
% $ Arguments $
%   - G:        The graph to be optimized
%   - Gc:       The constraint graph
%   - d:        The dimension of the embedding
%   - fm:       The type of formulation
%   - Y:        The embedding sample coordinates
%
% $ Description $
%   - Y = slgembed(G, Gc, d, fm, ...) solves the general graph-based
%     embedding of dimension d. In mathematics, it is to solve the
%     following optimization problem:
%           min/max y^T M y,  s.t. y^T C y = I
%     Based on different fm, the formulations of M and C are different:
%     fm is a cell array of two char string elements:
%           fm = {fg, fc}
%     fg indicates the formulation of M, fc indicates the formulation of
%     the constraint matrix C.
%       fg has the following different values:
%           - 'minW':   minimization using M = W, (W is the adjmat of G)
%           - 'maxW':   maximization using M = W
%           - 'minL':   minimization using M = L = D - W
%           - 'maxL':   maximization using M = L = D - W
%       fc has the following different values:
%           - 'I':      use C = I, that is y^T * y = I
%           - 'D':      use C = D, that is y^T * D * y = I (based on G)
%           - 'WC':      use C = W, adjacency matrix (based on Gc)
%           - 'LC':      use C = L = D - W, (based on Gc)
%     For example, if you specify fm = {'maxW', 'D'}, then the function
%     will solve the following optimization problem:
%           maximize y^T W y,   s.t. y^T * D * y = I
%     You can also specify the following properties:
%       - 'inv':        The parameters do to eigenvalue inverse
%                       {method, ...}
%                       (refer to slinvevals for details)
%                       This parameter take effects only when fc = 'WC" or
%                       fc = 'LC'.
%       - 'skip':       How many eigen-components to skip. default = 0.
%                       (In some algorithms, it is necessary to skip
%                        the first or first several eigen-components).
%
%   - [Y, spectrum] = slgembed(G, Gc, d, fm, ...) additionally return
%     the spectrum of the embedding, the eigenvalues of the whitened
%     C^(-1/2) M C^(-1/2).
%
% $ Remarks $
%   - The fc can be 'WC' or 'LC' only when Gc is full matrix, in current
%     version of implementation.
%
% $ History $
%   - Created by Dahua Lin, on Sep 12, 2006
%

%% parse and verify input arguments

if nargin < 4
    raise_lackinput('slgembed', 4);
end

gi = slgraphinfo(G, {'square'});
n = gi.n;
W = sladjmat(G, 'valtype', 'numeric', 'sparse', issparse(G));

[fg, fc] = deal(fm{:});

Wc = [];
if strcmp(fc, 'WC') || strcmp(fc, 'LC')
    if ~isempty(Gc)
        if ~isnumeric(Gc) || issparse(Gc) || ~isequal(size(Gc), [n n])
            error('sltoolbox:invalidarg', ...
                'In current implementation, Gc must be an n x n full numeric matrix for fc = WC or GC');
        end
        Wc = Gc;
    else
        error('sltoolbox:invalidarg', ...
            'When fc is WC or LC, Wc should be specified');
    end
end

opts.inv = {};
opts.skip = 0;
opts = slparseprops(opts, varargin{:});

if d + opts.skip > n
    error('sltoolbox:invalidarg', ...
        'The embedding dimension d plus the skip dimension should not exceed n');
end

%% main skeleton

W = W + W';
if ~isempty(Wc)
    Wc = Wc + Wc';
end

% parse formulation and decide scheme

switch fg
    case 'minW'
        Mfunc = @make_Wmat;
        optype = 'max';
    case 'maxW'
        Mfunc = @make_Wmat;
        optype = 'max';
    case 'minL'
        Mfunc = @make_Lmat;
        optype = 'min';
    case 'maxL'
        Mfunc = @make_Lmat;
        optype = 'max';
    otherwise
        error('sltoolbox:invalidarg', 'Invalid fg type: %s', fg);
end

switch fc
    case 'I'
        whfunc = [];
    case 'D'
        whfunc = @(M) whM_by_D(M, W);
        nyfunc = @(Y, T) nY_by_S(Y, T);
    case 'WC'
        whfunc = @(M) whM_by_W(M, Wc, opts.inv);
        nyfunc = @(Y, T) nY_by_T(Y, T);
    case 'LC'
        whfunc = @(M) whM_by_L(M, Wc, opts.inv);
        nyfunc = @(Y, T) nY_by_T(Y, T);
    otherwise
        error('sltoolbox:invalidarg', 'Invalid fc type: %s', fc);
end

% make M
M = Mfunc(W);

% whiten M
if ~isempty(whfunc)
    MTcell = whfunc(M);
    M = MTcell{1};
    T = MTcell{2};
    clear MTcell;
end

% optimize to solve embedding
[Y, spectrum] = optim_embed(M, optype, d, opts.skip);

% normalize the embedding to satisfy the constraint
if ~isempty(whfunc)
    Y = nyfunc(Y, T);
end
Y = Y';

%% Core routines

function M = make_Wmat(W)

M = W;

function vD = make_Dvec(W)

vD = sum(W, 1)';
if issparse(vD)
    vD = full(vD);
end

function L = make_Lmat(W)

vD = make_Dvec(W);
n = size(W, 1);
if issparse(W)
    D = sparse((1:n)', (1:n)', vD, n, n, n);
    L = D - W;
else
    L = -W;
    dinds = (1:n)'*(n+1)-n;
    L(dinds) = L(dinds) + vD;
end

function MTcell = whM_by_D(M, W)

vD = make_Dvec(W);
vD(vD < eps) = eps;
cv = 1 ./ sqrt(vD);

n = size(M, 1);
if issparse(M)
    Mw = M;
    for i = 1 : n
        Mw(:,i) = Mw(:,i) * cv(i);
    end
    for i = 1 : n
        Mw(i,:) = Mw(i,:) * cv(i);
    end
else
    Mw = slmulrowcols(M, cv', cv);
end

MTcell = {Mw, cv};

function MTcell = whM_by_W(M, W, invparams)

T = slwhiten_from_cov(W, invparams{:});
Mw = T' * M * T;
MTcell = {Mw, T};

function MTcell = whM_by_L(M, W, invparams)

L = make_Lmat(W);
MTcell = whM_by_W(M, L, invparams);

function [Y, spectrum] = optim_embed(M, optype, d, dskip)

switch optype
    case 'min'
        ord = 'ascend';
    case 'max'
        ord = 'descend';
end

d0 = d + dskip;
[spectrum, Y] = slsymeig(M, d0, ord);
if dskip > 0
    spectrum = spectrum(dskip+1:d0);
    Y = Y(:, dskip+1:d0);
end

function Y = nY_by_S(Y, T)

Y = slmulvec(Y, T, 1);

function Y = nY_by_T(Y, T)

Y = T * Y;


function b = isposdef(a)
% ISPOSDEF   Test for positive definite matrix.
%    ISPOSDEF(A) returns 1 if A is positive definite, 0 otherwise.
%    Using chol is much more efficient than computing eigenvectors.

% Written by Tom Minka

[R,p] = chol(a);
b = (p == 0);