%% Complex Step Diff Checking
clc
format long g
F = @(x) exp(x)/((cos(x))^3 + (sin(x))^3)
G = symJac(F);
H = symHess(F);
x0 = pi/2;
true1d = G(x0)
true2d = H(x0)
I = sqrt(2)/2*(1i + 1);

k = 6;
cs1d = imag(F(x0 + 1i*10^-k)/(1*10^-k))
cs2d = imag((F(x0 + I*10^-k) + F(x0 - I*10^-k))/(10^-k)^2)

%% Multivariable 2nd der
clc
F = @(x) exp(x(1))/((cos(x(2)))^3 + (sin(x(2)))^3)
G = symJac(F);
H = symHess(F);
x0 = [2;2];
true1d = G(x0)
true2d = H(x0)

cstepHess(F,x0)

%% Multivariable 2nd der 4x4
clc
f = @(x) x(1)*x(4)*sum(x(1:3)) + x(3);
g = @(x) [ x(1)*x(4) + x(4)*sum(x(1:3))
                x(1)*x(4)
                x(1)*x(4) + 1
                x(1)*sum(x(1:3)) ];
c = @(x) [ prod(x); sum(x.^2) ];
j = @(x) [ prod(x)./x 2*x ];
H = @(x,sigma,lambda) sigma*[ 2*x(4)             0      0   0;
                      x(4)               0      0   0;
                      x(4)               0      0   0;
                      2*x(1)+x(2)+x(3)  x(1)  x(1)  0 ] +....
          lambda(1)*[    0          0         0         0;
                              x(3)*x(4)     0         0         0;
                              x(2)*x(4) x(1)*x(4)     0         0;
                              x(2)*x(3) x(1)*x(3) x(1)*x(2)     0  ] +...
          lambda(2)*diag([2 2 2 2]);

x0 = randn(4,1);
sig = randn(1,1); lambda = randn(2,1);
% x0 = [0.1;0.2;0.3;0.4];

%Gradient
cstepJac(f,x0) - g(x0)'
%Jacobian
cstepJac(c,x0) - j(x0)'
%Hessian Of Objective
cstepHess(f,x0,true) - H(x0,1,[0;0])
%Hessian Of Lagrangian
cstepHessLag(f,c,x0,sig,lambda,true) - H(x0,sig,lambda)

%% Speed Test
clc
La = 0.6604; Lh = 0.1778; m_b = 0.575; m_f = 0.575; Lw = 0.4699; m_w = 1.87; g = 9.81; Kf = 0.1188; u = [1;1];
F = @(x) (0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f + 0.6e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_b * m_f * m_w + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 4 * Lh * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * Lh * Lw ^ 4 * m_b * m_w ^ 2 + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 4 * Lh * m_b ^ 2 * m_f + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_b ^ 2 * m_w + cos(x(2)) ^ 3 * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.2e1 * cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w * cos(x(1)) ^ 2 + cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w * cos(x(1)) ^ 2 + cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w * cos(x(1)) ^ 2 + 0.4e1 * cos(x(2)) ^ 3 * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * Lh * Lw ^ 4 * m_f * m_w ^ 2 - cos(x(2)) ^ 3 * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - 0.4e1 * cos(x(2)) ^ 3 * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(4) ^ 2 + ((0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * La ^ 4 * Lh * m_b ^ 2 * m_f + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * Lh * Lw ^ 4 * m_f * m_w ^ 2 + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * La ^ 4 * Lh * m_b * m_f ^ 2 + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * Lh * Lw ^ 4 * m_b * m_w ^ 2 - 0.2e1 * cos(x(1)) * La ^ 2 * Lh * Lw ^ 2 * m_f ^ 2 * m_w - 0.2e1 * cos(x(1)) * La ^ 2 * Lh * Lw ^ 2 * m_b ^ 2 * m_w - 0.4e1 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w - 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 + 0.12e2 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * La ^ 2 * Lh * Lw ^ 2 * m_b * m_f * m_w + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * cos(x(2)) ^ 2 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * La ^ 2 * Lh * Lw ^ 2 * m_b ^ 2 * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 3 * La ^ 2 * Lh * Lw ^ 2 * m_f ^ 2 * m_w - 0.12e2 * cos(x(1)) * La ^ 2 * Lh * Lw ^ 2 * m_b * m_f * m_w - 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - 0.2e1 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w - 0.2e1 * cos(x(1)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w - 0.8e1 * cos(x(1)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f - 0.8e1 * cos(x(1)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 - 0.8e1 * cos(x(1)) * La ^ 4 * Lh * m_b ^ 2 * m_f - 0.8e1 * cos(x(1)) * La ^ 4 * Lh * m_b * m_f ^ 2 - 0.2e1 * cos(x(1)) * Lh * Lw ^ 4 * m_b * m_w ^ 2 - 0.2e1 * cos(x(1)) * Lh * Lw ^ 4 * m_f * m_w ^ 2) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(6) + (0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) * sin(x(1)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f + 0.4e1 * cos(x(2)) ^ 2 * cos(x(1)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w + 0.8e1 * cos(x(2)) ^ 2 * cos(x(1)) * sin(x(1)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(5)) * x(4) + (0.2e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w - cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_b ^ 2 * m_w + cos(x(2)) ^ 3 * cos(x(1)) ^ 3 * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) ^ 3 * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + 0.2e1 * cos(x(2)) * cos(x(1)) ^ 3 * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w - cos(x(2)) ^ 3 * cos(x(1)) ^ 3 * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w - cos(x(2)) * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_f ^ 2 * m_w - 0.4e1 * cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w * cos(x(1)) ^ 2 - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f - 0.2e1 * cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w * cos(x(1)) ^ 2 - 0.2e1 * cos(x(2)) * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w * cos(x(1)) ^ 2 - cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * Lh * Lw ^ 4 * m_f * m_w ^ 2 + 0.4e1 * cos(x(2)) ^ 3 * cos(x(1)) ^ 3 * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f - 0.4e1 * cos(x(2)) ^ 3 * cos(x(1)) ^ 3 * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 4 * Lh * m_b ^ 2 * m_f - 0.4e1 * cos(x(2)) * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 + 0.4e1 * cos(x(2)) * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 3 * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f + cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w + 0.8e1 * cos(x(2)) * cos(x(1)) ^ 3 * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 4 * Lh * m_b * m_f ^ 2 + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 - cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * Lh * Lw ^ 4 * m_b * m_w ^ 2 - 0.6e1 * cos(x(2)) * cos(x(1)) ^ 4 * sin(x(2)) * La ^ 2 * Lh * Lw ^ 2 * m_b * m_f * m_w) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(6) ^ 2 + (-0.2e1 * cos(x(2)) * cos(x(1)) ^ 3 * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_f ^ 2 * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b ^ 2 * m_w - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La ^ 2 * Lh ^ 3 * m_b ^ 2 * m_f + 0.2e1 * cos(x(2)) * cos(x(1)) ^ 3 * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * La ^ 2 * Lh ^ 3 * m_b * m_f ^ 2 - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * Lh ^ 3 * Lw ^ 2 * m_b * m_f * m_w - 0.8e1 * cos(x(2)) * cos(x(1)) ^ 3 * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f + 0.2e1 * cos(x(2)) * cos(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.8e1 * cos(x(2)) * cos(x(1)) ^ 3 * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 - 0.8e1 * cos(x(2)) * cos(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 + 0.8e1 * cos(x(2)) * cos(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(5) * x(6) + (-0.4e1 * cos(x(2)) * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b * m_f ^ 2 - cos(x(2)) * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) * sin(x(1)) * La * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + 0.4e1 * cos(x(2)) * cos(x(1)) * sin(x(1)) * La ^ 3 * Lh ^ 2 * m_b ^ 2 * m_f) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w) * x(5) ^ 2 + (-0.3e1 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 2 * m_f + sin(x(2)) * sin(x(1)) * g * Lh * Lw ^ 3 * m_b * m_w ^ 2 + sin(x(2)) * sin(x(1)) * g * Lh * Lw ^ 3 * m_f * m_w ^ 2 - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 3 * Lw * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 3 * Lw * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 2 * g * La * Lw ^ 3 * m_b * m_w ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * g * La * Lw ^ 3 * m_f * m_w ^ 2 + cos(x(2)) * cos(x(1)) * g * Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) * g * Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 3 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_f ^ 3 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 3 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_f ^ 3 + cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 3 * Lw * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 3 * Lw * m_f ^ 2 * m_w - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f - 0.3e1 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b * m_f ^ 2 - 0.3e1 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 2 * m_f - 0.3e1 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b * m_f ^ 2 + sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b ^ 2 * m_w + sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_f ^ 2 * m_w + 0.4e1 * cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 3 * g * La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 3 * g * La * Lw ^ 3 * m_b * m_w ^ 2 - cos(x(2)) * cos(x(1)) ^ 3 * g * La * Lw ^ 3 * m_f * m_w ^ 2 + 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_f * m_w - 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_b * m_w - 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_f * m_w - sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_f ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_f ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_f ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b ^ 2 - sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 + 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_b * m_w + 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b * m_f - 0.2e1 * cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b * m_f - cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_b * m_w - cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_b * m_w + 0.2e1 * cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b * m_f - 0.2e1 * cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b * m_f + cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_f * m_w + cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_f * m_w + cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 * cos(x(2)) ^ 2 + cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 * cos(x(2)) ^ 2 + cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 * cos(x(1)) ^ 2 + cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 * cos(x(1)) ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 * cos(x(2)) ^ 2 - cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 * cos(x(1)) ^ 2 - cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 * cos(x(1)) ^ 2 - cos(x(1)) * g * La * Lw ^ 3 * m_b * m_w ^ 2 * cos(x(2)) ^ 2 - 0.4e1 * cos(x(2)) * cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 - cos(x(1)) * g * La * Lh ^ 2 * Lw * m_f ^ 2 * m_w + 0.4e1 * cos(x(2)) * cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + cos(x(1)) * g * La * Lh ^ 2 * Lw * m_b ^ 2 * m_w + cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f * cos(x(2)) ^ 2 + cos(x(1)) * g * La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w * cos(x(2)) ^ 2 + cos(x(1)) * g * La ^ 3 * Lw * m_f ^ 2 * m_w * cos(x(2)) ^ 2 + cos(x(1)) * g * La * Lw ^ 3 * m_f * m_w ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 3 * Lw * m_b ^ 2 * m_w * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w * cos(x(2)) ^ 2 + 0.2e1 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b * m_f - 0.2e1 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b * m_f + Kf * Lh ^ 2 * Lw ^ 2 * u(1) * m_b * m_w + Kf * Lh ^ 2 * Lw ^ 2 * u(1) * m_f * m_w - Kf * Lh ^ 2 * Lw ^ 2 * u(2) * m_b * m_w - Kf * Lh ^ 2 * Lw ^ 2 * u(2) * m_f * m_w + cos(x(2)) * cos(x(1)) ^ 3 * g * Lw ^ 4 * m_b * m_w ^ 2 - cos(x(2)) * cos(x(1)) ^ 3 * g * Lw ^ 4 * m_f * m_w ^ 2 - sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 3 - sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_f ^ 3 - sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 3 - sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_f ^ 3 + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 4 * m_b ^ 2 * m_f - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 4 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lh ^ 2 * m_b ^ 3 - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 2 * Lh ^ 2 * m_f ^ 3 - cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b ^ 2 - cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_f ^ 2 - 0.2e1 * cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b * m_f + 0.2e1 * cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b * m_f + 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(1) * m_b * m_f - 0.2e1 * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(2) * m_b * m_f - 0.2e1 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b * m_f + 0.2e1 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b * m_f + cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b ^ 2 + cos(x(2)) ^ 2 * cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(1) * m_b ^ 2 - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(1) * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(2) * m_b ^ 2 - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 4 * u(2) * m_f ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 - cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 - cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 + cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 + cos(x(1)) * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 + cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 + cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 - cos(x(1)) * Kf * La ^ 4 * u(2) * m_f ^ 2 * cos(x(2)) ^ 2 + cos(x(1)) * Kf * La ^ 4 * u(1) * m_b ^ 2 * cos(x(2)) ^ 2 + cos(x(1)) * Kf * La ^ 4 * u(2) * m_b ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * Kf * La ^ 4 * u(1) * m_f ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w - cos(x(1)) * g * La ^ 3 * Lw * m_f ^ 2 * m_w - cos(x(1)) * g * La * Lw ^ 3 * m_f * m_w ^ 2 + cos(x(1)) * g * La ^ 3 * Lw * m_b ^ 2 * m_w + cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + cos(x(1)) * g * La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + cos(x(1)) * g * La * Lw ^ 3 * m_b * m_w ^ 2 + cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b ^ 3 * cos(x(2)) ^ 2 + cos(x(1)) * g * La ^ 4 * m_b ^ 2 * m_f * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 4 * m_b * m_f ^ 2 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_f ^ 3 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b ^ 2 * m_w + sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_f ^ 2 * m_w - sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_b ^ 2 * m_w - sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_f ^ 2 * m_w + 0.2e1 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b * m_f * m_w - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b ^ 2 * m_w - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 3 * g * La * Lh ^ 2 * Lw * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) ^ 3 * g * La * Lh ^ 2 * Lw * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 3 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_f ^ 3 - cos(x(2)) * cos(x(1)) ^ 2 * g * La * Lh ^ 2 * Lw * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * g * La * Lh ^ 2 * Lw * m_f ^ 2 * m_w - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 2 * m_f - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b * m_f ^ 2 + 0.3e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 2 * m_f + 0.3e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_f ^ 3 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 3 - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_f ^ 2 - 0.2e1 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b * m_f + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_f ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_f ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_f ^ 2 - 0.2e1 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b * m_f + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b ^ 2 - 0.2e1 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b * m_f + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_f ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b ^ 2 + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b ^ 2 - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b ^ 2 - 0.2e1 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b * m_f + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_f ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b ^ 2 + cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_b * m_w - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_f * m_w + cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_b * m_w - cos(x(2)) * cos(x(1)) ^ 2 * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_f * m_w - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_f ^ 2 - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b ^ 2 + cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_b * m_w * cos(x(2)) ^ 2 + cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_b * m_w * cos(x(2)) ^ 2 - 0.2e1 * cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b * m_f * cos(x(1)) ^ 2 + 0.2e1 * cos(x(2)) ^ 2 * Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b * m_f * cos(x(1)) ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(1) * m_f * m_w * cos(x(2)) ^ 2 - cos(x(1)) * Kf * La ^ 2 * Lw ^ 2 * u(2) * m_f * m_w * cos(x(2)) ^ 2 + cos(x(1)) * g * La * Lh ^ 2 * Lw * m_f ^ 2 * m_w * cos(x(2)) ^ 2 - cos(x(1)) * g * La * Lh ^ 2 * Lw * m_b ^ 2 * m_w * cos(x(2)) ^ 2 + Kf * Lh ^ 4 * u(1) * m_b ^ 2 + Kf * Lh ^ 4 * u(1) * m_f ^ 2 - Kf * Lh ^ 4 * u(2) * m_b ^ 2 - Kf * Lh ^ 4 * u(2) * m_f ^ 2 + cos(x(1)) * g * La ^ 4 * m_f ^ 3 - cos(x(1)) * g * La ^ 4 * m_b ^ 3 + 0.2e1 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b * m_f * m_w - 0.2e1 * sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_b * m_f * m_w - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b ^ 2 * m_w - cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_f ^ 2 * m_w - 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b * m_f * m_w - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b ^ 2 * m_f - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 3 * Lh * m_b * m_f ^ 2 + 0.3e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 2 * m_f + 0.3e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b * m_f ^ 2 - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_f ^ 2 * m_w - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b ^ 2 * m_f - 0.4e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh ^ 3 * m_b * m_f ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_f ^ 2 * m_w - sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(1) * m_b * m_w - sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(1) * m_f * m_w - sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(2) * m_b * m_w - sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(2) * m_f * m_w - 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b * m_f - 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b * m_f + 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b * m_f + 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b * m_f + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_f ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_f ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_f ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b ^ 2 + cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_f ^ 2 - Kf * La ^ 2 * Lh ^ 2 * u(2) * m_b ^ 2 - Kf * La ^ 2 * Lh ^ 2 * u(2) * m_f ^ 2 + 0.2e1 * Kf * Lh ^ 4 * u(1) * m_b * m_f - 0.2e1 * Kf * Lh ^ 4 * u(2) * m_b * m_f + Kf * La ^ 2 * Lh ^ 2 * u(1) * m_b ^ 2 + Kf * La ^ 2 * Lh ^ 2 * u(1) * m_f ^ 2 - cos(x(1)) * g * La ^ 4 * m_f ^ 3 * cos(x(2)) ^ 2 + cos(x(1)) * g * La ^ 4 * m_b ^ 3 * cos(x(2)) ^ 2 - cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_b ^ 3 - cos(x(1)) * g * La ^ 4 * m_b ^ 2 * m_f + cos(x(1)) * g * La ^ 4 * m_b * m_f ^ 2 + cos(x(1)) * g * La ^ 2 * Lh ^ 2 * m_f ^ 3 + cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 4 * m_b ^ 3 - cos(x(2)) * cos(x(1)) ^ 2 * g * La ^ 4 * m_f ^ 3 - cos(x(1)) * Kf * La ^ 4 * u(1) * m_b ^ 2 - cos(x(1)) * Kf * La ^ 4 * u(2) * m_b ^ 2 + cos(x(1)) * Kf * La ^ 4 * u(1) * m_f ^ 2 + cos(x(1)) * Kf * La ^ 4 * u(2) * m_f ^ 2 - cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b ^ 2 - cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_f ^ 2 + cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b ^ 2 + cos(x(2)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_f ^ 2 + cos(x(1)) ^ 2 * Kf * La ^ 4 * u(1) * m_b ^ 2 + cos(x(1)) ^ 2 * Kf * La ^ 4 * u(1) * m_f ^ 2 - cos(x(1)) ^ 2 * Kf * La ^ 4 * u(2) * m_b ^ 2 - cos(x(1)) ^ 2 * Kf * La ^ 4 * u(2) * m_f ^ 2 - cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_b ^ 2 - cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(1) * m_f ^ 2 + cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_b ^ 2 + cos(x(1)) ^ 2 * Kf * Lh ^ 4 * u(2) * m_f ^ 2 + cos(x(1)) ^ 2 * Kf * Lw ^ 4 * u(1) * m_w ^ 2 - cos(x(1)) ^ 2 * Kf * Lw ^ 4 * u(2) * m_w ^ 2 + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_f ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_b ^ 2 * m_w + cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_f ^ 2 * m_w + 0.2e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b * m_f * m_w + 0.2e1 * cos(x(2)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b * m_f * m_w - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b ^ 2 * m_w - cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_f ^ 2 * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * Lh ^ 3 * Lw * m_b * m_f * m_w + 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(1) * m_b * m_f + 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh ^ 3 * u(2) * m_b * m_f + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(1) * m_b * m_w - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(1) * m_f * m_w - 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(2) * m_b * m_w + 0.2e1 * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La * Lh * Lw ^ 2 * u(2) * m_f * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(1) * m_b * m_f - 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * Kf * La ^ 3 * Lh * u(2) * m_b * m_f - 0.2e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b * m_f * m_w - 0.2e1 * cos(x(2)) * cos(x(1)) ^ 2 * sin(x(2)) * sin(x(1)) * g * La * Lh * Lw ^ 2 * m_b * m_f * m_w + 0.2e1 * cos(x(2)) * cos(x(1)) * sin(x(2)) * sin(x(1)) * g * La ^ 2 * Lh * Lw * m_b * m_f * m_w) / Lh / cos(x(1)) ^ 2 / (0.4e1 * La ^ 4 * m_b ^ 2 * m_f + 0.4e1 * La ^ 4 * m_b * m_f ^ 2 + Lw ^ 4 * m_b * m_w ^ 2 + Lw ^ 4 * m_f * m_w ^ 2 + 0.6e1 * La ^ 2 * Lw ^ 2 * m_b * m_f * m_w + La ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + La ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b ^ 2 * m_f + 0.4e1 * La ^ 2 * Lh ^ 2 * m_b * m_f ^ 2 + 0.2e1 * Lh ^ 2 * Lw ^ 2 * m_b * m_f * m_w + Lh ^ 2 * Lw ^ 2 * m_b ^ 2 * m_w + Lh ^ 2 * Lw ^ 2 * m_f ^ 2 * m_w);
x0 = [1;1;1;1;1;1];

tic
mj = mklJac(F,x0);
toc
tic
aj = autoJac(F,x0);
toc
tic
cj = cstepJac(F,x0');
toc

aj-cj

%% Vector Fun Test
clc
%Function
i = (1:50)';
fun = @(x) x(1)*exp(-x(2)*i) + x(3);
x0 = ones(3,1);

tic
mj = mklJac(fun,x0);
toc
tic
aj = autoJac(fun,x0);
toc
tic
cj = cstepJac(fun,x0);
toc


classdef SO3DeLaValleePoussinKernel < SO3Kernel
% The rotational de la Vallee Poussin kernel is defined by
%
% $$ K(t) = \frac{B(\frac32,\frac12)}{B(\frac32,\kappa+\frac12)}\,t^{2\kappa}$$
%
% for $t\in[0,1]$, where $B$ denotes the Beta function. The de la Vallee
% Poussin kernel additionaly has the unique property that for
% a given halfwidth it can be described exactly by a finite number of
% Fourier coefficients. This kernel is recommended for Texture analysis as
% it is always positive in orientation space and there is no truncation
% error in Fourier space.
%
% Hence we can define the de la Vallee Poussin kernel $\psi_{\kappa}$
% depending on a parameter $\kappa \in \mathbb N \setminus \{0\}$ by its
% finite Chebyshev expansion
%
% $$ \psi_{\kappa}(t) = \frac{(\kappa+1)\,2^{2\kappa-1}}{\binom{2\kappa-1}{\kappa}}
% \, t^{2\kappa}  = \binom{2\kappa+1}{\kappa}^{-1} \,
% \sum\limits_{n=0}^{\kappa} (2n+1)\,\binom{2\kappa+1}{\kappa-n} \,
% \mathcal U_{2n}(t)$$.
%
% Syntax
%   psi = SO3DeLaValleePoussinKernel(100)
%   psi = SO3DeLaValleePoussinKernel('halfwidth',5*degree)
%
% Input
%  kappa - kernel parameter
%
% Options
%  halfwidth - angle at which the kernel function has reduced to half its peak value
%  bandwidth - harmonic degree
%
% See also
% SO3Kernel

properties
  kappa = 90;
  C = [];
end

methods

  function psi = SO3DeLaValleePoussinKernel(varargin)

    % extract parameter and halfwidth
    if check_option(varargin,'halfwidth')
      hw = get_option(varargin,'halfwidth');
      psi.kappa = 0.5 * log(0.5) / log(cos(hw/2));
    elseif nargin > 0
      psi.kappa = varargin{1};
    end

    % extract bandwidth
    L = get_option(varargin,'bandwidth',round(psi.kappa));

    % some constant
    psi.C = beta(1.5,0.5)/beta(1.5,psi.kappa+0.5);

    % compute Chebyshev coefficients
    psi.A = ones(1,L+1);
    psi.A(2) = psi.kappa/(psi.kappa+2);

    for l=1:L-1
      psi.A(l+2) = ((psi.kappa-l+1) * psi.A(l) - ...
        (2*l+1) * psi.A(l+1)) / (psi.kappa + l + 2);
    end

    for l=0:L, psi.A(l+1) = (2*l+1) * psi.A(l+1); end

    psi.A = psi.cutA;

  end

  function c = char(psi)
    c = ['de la Vallee Poussin, halfwidth ' ...
      xnum2str(psi.halfwidth/degree) mtexdegchar];
  end

  function value = eval(psi,co2)
    value   =  psi.C * co2.^(2*psi.kappa);
  end

  function value = grad(psi,co2)
    % the derivative of the kernel function
    % DK(omega) = - kappa * C * sin(omega/2)*cos(omega/2)^(2kappa-1)

    if nargin == 2
      value = -psi.C * psi.kappa * sqrt(1-co2.^2) .* co2.^(2*psi.kappa-1);
      %value = 2 * psi.C * psi.kappa *co2.^(2*psi.kappa-1);
    else
      value = SO3KernelHandle(@(co2) -psi.C * psi.kappa * sqrt(1-co2.^2) .* co2.^(2*psi.kappa-1));
    end

  end

  function S2K = radon(psi)
    S2K = S2DeLaValleePoussinKernel(psi.kappa);
  end

  function hw = halfwidth(psi)
    hw = 2*acos(0.5^(1/2/psi.kappa));
  end

end

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function [Optionspreis] = BlackScholesPrice(S,K,r,T,sigma,type)
% calculates the black scholes price using matrices
    matS = repmat(S,1,length(T));   % S matrix
    matT = repmat(T',length(S),1);  % T matrix

    d1=(log(matS./K)+(r+sigma^2/2)*matT)./(sigma*sqrt(matT));
    d2=(log(matS./K)+(r-sigma^2/2)*matT)./(sigma*sqrt(matT));

    if type==0
        Optionspreis = K*exp(-r*matT).*normcdf(-d2)-matS.*normcdf(-d1);
    else
        Optionspreis = matS.*normcdf(d1)-K*exp(-r*matT).*normcdf(d2);
    end

end


function [ lam, mu ] = cycle_brent ( f, x0 )

%*****************************************************************************80
%
%% CYCLE_BRENT finds a cycle in an iterated mapping using Brent's method.
%
%  Discussion:
%
%    Suppose we a repeatedly apply a function f(), starting with the argument
%    x0, then f(x0), f(f(x0)) and so on.  Suppose that the range of f is finite.
%    Then eventually the iteration must reach a cycle.  Once the cycle is reached,
%    succeeding values stay within that cycle.
%
%    Starting at x0, there is a "nearest element" of the cycle, which is
%    reached after MU applications of f.
%
%    Once the cycle is entered, the cycle has a length LAM, which is the number
%    of steps required to first return to a given value.
%
%    This function uses Brent's method to determine the values of MU and LAM,
%    given F and X0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Richard Brent,
%    An improved Monte Carlo factorization algorithm,
%    BIT,
%    Volume 20, Number 2, 1980, pages 176-184.
%
%  Parameters:
%
%    Input, integer F ( integer I ), the name of the function
%    to be analyzed.
%
%    Input, integer X0, the starting point.
%
%    Output, integer LAM, the length of the cycle.
%
%    Output, integer MU, the index in the sequence starting
%    at X0, of the first appearance of an element of the cycle.
%
  power = 1;
  lam = 1;
  tortoise = x0;
  hare = f ( x0 );

  while ( tortoise ~= hare )
    if ( power == lam )
      tortoise = hare;
      power = power * 2;
      lam = 0;
    end
    hare = f ( hare );
    lam = lam + 1;
  end

  mu = 0;
  tortoise = x0;
  hare = x0;

  for i = 0 : lam - 1
    hare = f ( hare );
  end

  while ( tortoise ~= hare )
    tortoise = f ( tortoise );
    hare = f ( hare );
    mu = mu + 1;
  end

  return
end


function segment = p03_boundary_segment ( segment_index, m, segment_length )

%*****************************************************************************80
%
%% P03_BOUNDARY_SEGMENT returns a boundary segment in problem 03.
%
%  Discussion:
%
%    For boundary segment #1, the value of SEGMENT_LENGTH should be
%    at least 5.  Values of 4*N+1 will result in an "even" mesh.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer SEGMENT_INDEX, the index of the boundary segment.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer SEGMENT_LENGTH, the number of points in the segment.
%
%    Output, SEGMENT(M,SEGMENT_LENGTH), points on the boundary segment.
%
  center = [ 0.0, 0.0 ];
  r2 = 0.4;

  if ( segment_index == 1 )

    n1 = round ( ( segment_length - 1 ) / 4 );
    n2 = round ( ( 2 * ( segment_length - 1 ) ) / 4 ) - n1;
    n3 = round ( ( 3 * ( segment_length - 1 ) ) / 4 ) - n1 - n2;
    n4 = segment_length - 1 - n1 - n2 - n3;

    s(1:2,1) = [ -1.0, -1.0 ]';
    s(1:2,2) = [  1.0, -1.0 ]';
    s(1:2,3) = [  1.0,  1.0 ]';
    s(1:2,4) = [ -1.0,  1.0 ]';

    j = 0;

    for i = 1 : n1
      j = j + 1;
      segment(1:2,j) = ( ( n1 - i + 1 ) * s(1:2,1)   ...
                       + (      i - 1 ) * s(1:2,2) ) ...
                       / ( n1         );
    end

    for i = 1 : n2
      j = j + 1;
      segment(1:2,j) = ( ( n2 - i + 1 ) * s(1:2,2)   ...
                       + (      i - 1 ) * s(1:2,3) ) ...
                       / ( n2         );
    end

    for i = 1 : n3
      j = j + 1;
      segment(1:2,j) = ( ( n3 - i + 1 ) * s(1:2,3)   ...
                       + (      i - 1 ) * s(1:2,4) ) ...
                       / ( n3         );
    end

    for i = 1 : n4
      j = j + 1;
      segment(1:2,j) = ( ( n4 - i + 1 ) * s(1:2,4)   ...
                       + (      i - 1 ) * s(1:2,1) ) ...
                       / ( n4         );
    end

    j = j + 1;
    segment(1:2,j) = s(1:2,1);

  elseif ( segment_index == 2 )

    for i = 1 : segment_length
      angle = 2.0 * pi * ( segment_length - i ) / ( segment_length - 1 );
      segment(1,i) = center(1) + r2 * cos ( angle );
      segment(2,i) = center(2) + r2 * sin ( angle );
    end

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'P03_BOUNDARY_SEGMENT - Fatal error!\n' );
    fprintf ( 1, '  Illegal SEGMENT_INDEX = %d\n', segment_index );
    error ( 'P03_BOUNDARY_SEGMENT - Fatal error!' );

  end

  return
end


function ssim  =  cal_ssim( im1, im2, b_row, b_col )

[h w ch]  =  size( im1 );
ssim  = 0;
if ch==1
    ssim   = ssim_index( im1(b_row+1:h-b_row, b_col+1:w-b_col), im2( b_row+1:h-b_row, b_col+1:w-b_col) );
else
    for i = 1:ch
        ssim   = ssim + ssim_index( im1(b_row+1:h-b_row, b_col+1:w-b_col, i), im2( b_row+1:h-b_row, b_col+1:w-b_col, i) );
    end
    ssim   =  ssim/3;
end
return;

function [mssim, ssim_map] = ssim_index(img1, img2, K, window, L)

%========================================================================
%SSIM Index, Version 1.0
%Copyright(c) 2003 Zhou Wang
%All Rights Reserved.
%
%The author was with Howard Hughes Medical Institute, and Laboratory
%for Computational Vision at Center for Neural Science and Courant
%Institute of Mathematical Sciences, New York University, USA. He is
%currently with Department of Electrical and Computer Engineering,
%University of Waterloo, Canada.
%
%----------------------------------------------------------------------
%Permission to use, copy, or modify this software and its documentation
%for educational and research purposes only and without fee is hereby
%granted, provided that this copyright notice and the original authors'
%names appear on all copies and supporting documentation. This program
%shall not be used, rewritten, or adapted as the basis of a commercial
%software or hardware product without first obtaining permission of the
%authors. The authors make no representations about the suitability of
%this software for any purpose. It is provided "as is" without express
%or implied warranty.
%----------------------------------------------------------------------
%
%This is an implementation of the algorithm for calculating the
%Structural SIMilarity (SSIM) index between two images. Please refer
%to the following paper:
%
%Z. Wang, A. C. Bovik, H. R. Sheikh, and E. P. Simoncelli, "Image
%quality assessment: From error measurement to structural similarity"
%IEEE Transactios on Image Processing, vol. 13, no. 4, Apr. 2004.
%
%Kindly report any suggestions or corrections to zhouwang@ieee.org
%
%----------------------------------------------------------------------
%
%Input : (1) img1: the first image being compared
%        (2) img2: the second image being compared
%        (3) K: constants in the SSIM index formula (see the above
%            reference). defualt value: K = [0.01 0.03]
%        (4) window: local window for statistics (see the above
%            reference). default widnow is Gaussian given by
%            window = fspecial('gaussian', 11, 1.5);
%        (5) L: dynamic range of the images. default: L = 255
%
%Output: (1) mssim: the mean SSIM index value between 2 images.
%            If one of the images being compared is regarded as
%            perfect quality, then mssim can be considered as the
%            quality measure of the other image.
%            If img1 = img2, then mssim = 1.
%        (2) ssim_map: the SSIM index map of the test image. The map
%            has a smaller size than the input images. The actual size:
%            size(img1) - size(window) + 1.
%
%Default Usage:
%   Given 2 test images img1 and img2, whose dynamic range is 0-255
%
%   [mssim ssim_map] = ssim_index(img1, img2);
%
%Advanced Usage:
%   User defined parameters. For example
%
%   K = [0.05 0.05];
%   window = ones(8);
%   L = 100;
%   [mssim ssim_map] = ssim_index(img1, img2, K, window, L);
%
%See the results:
%
%   mssim                        %Gives the mssim value
%   imshow(max(0, ssim_map).^4)  %Shows the SSIM index map
%
%========================================================================

if (nargin < 2 | nargin > 5)
   mssim = -Inf;
   ssim_map = -Inf;
   return;
end

if (size(img1) ~= size(img2))
   mssim = -Inf;
   ssim_map = -Inf;
   return;
end

[M N] = size(img1);

if (nargin == 2)
   if ((M < 11) | (N < 11))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   window = fspecial('gaussian', 11, 1.5);	%
   K(1) = 0.01;								      % default settings
   K(2) = 0.03;								      %
   L = 255;                                  %
end

if (nargin == 3)
   if ((M < 11) | (N < 11))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   window = fspecial('gaussian', 11, 1.5);
   L = 255;
   if (length(K) == 2)
      if (K(1) < 0 | K(2) < 0)
		   mssim = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   mssim = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

if (nargin == 4)
   [H W] = size(window);
   if ((H*W) < 4 | (H > M) | (W > N))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   L = 255;
   if (length(K) == 2)
      if (K(1) < 0 | K(2) < 0)
		   mssim = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   mssim = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

if (nargin == 5)
   [H W] = size(window);
   if ((H*W) < 4 | (H > M) | (W > N))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   if (length(K) == 2)
      if (K(1) < 0 | K(2) < 0)
		   mssim = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   mssim = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

C1 = (K(1)*L)^2;
C2 = (K(2)*L)^2;
window = window/sum(sum(window));
img1 = double(img1);
img2 = double(img2);

mu1   = filter2(window, img1, 'valid');
mu2   = filter2(window, img2, 'valid');
mu1_sq = mu1.*mu1;
mu2_sq = mu2.*mu2;
mu1_mu2 = mu1.*mu2;
sigma1_sq = filter2(window, img1.*img1, 'valid') - mu1_sq;
sigma2_sq = filter2(window, img2.*img2, 'valid') - mu2_sq;
sigma12 = filter2(window, img1.*img2, 'valid') - mu1_mu2;

if (C1 > 0 & C2 > 0)
   ssim_map = ((2*mu1_mu2 + C1).*(2*sigma12 + C2))./((mu1_sq + mu2_sq + C1).*(sigma1_sq + sigma2_sq + C2));
else
   numerator1 = 2*mu1_mu2 + C1;
   numerator2 = 2*sigma12 + C2;
	denominator1 = mu1_sq + mu2_sq + C1;
   denominator2 = sigma1_sq + sigma2_sq + C2;
   ssim_map = ones(size(mu1));
   index = (denominator1.*denominator2 > 0);
   ssim_map(index) = (numerator1(index).*numerator2(index))./(denominator1(index).*denominator2(index));
   index = (denominator1 ~= 0) & (denominator2 == 0);
   ssim_map(index) = numerator1(index)./denominator1(index);
end

mssim = mean2(ssim_map);

return


function G = get_payoff_G_matrix_from_ygrid_2d( y_1, y_2, S_0s, sigmas, rho, contractParams)
%UNTITLED5 Summary of this function goes here
%   Detailed explanation goes here

payoff_type = contractParams.payoff_type;

if payoff_type == 1  % G = S_1
    payoff = @(y1,y2)S_0s(1)*exp(sigmas(1)*y1);

elseif payoff_type == 2  % G = S_2
    payoff = @(y1,y2)S_0s(2)*exp(sigmas(2)*(y2 + rho*y1));

elseif payoff_type == 3  % Exchange:  G = (S_1 - S_2)^+
    payoff = @(y1,y2) max(0, S_0s(1)*exp(sigmas(1)*y1) - S_0s(2)*exp(sigmas(2)*(y2 + rho*y1)));

elseif payoff_type == 4  % Spread:  G = (S_1 - S_2 - K)^+
    K = contractParams.K;
    payoff = @(y1,y2) max(0, S_0s(1)*exp(sigmas(1)*y1) - S_0s(2)*exp(sigmas(2)*(y2 + rho*y1)) - K);

elseif payoff_type == 5  % Geometric Basket Call / Put:  G = (sqrt(S_1) * sqrt(S_2) - K)^+ and
    K = contractParams.K;
    if contractParams.call == 1
        payoff = @(y1,y2) max(0, sqrt(S_0s(1)*exp(sigmas(1)*y1)) * sqrt(S_0s(2)*exp(sigmas(2)*(y2 + rho*y1))) - K);
    else
        payoff = @(y1,y2) max(0, K - sqrt(S_0s(1)*exp(sigmas(1)*y1)) * sqrt(S_0s(2)*exp(sigmas(2)*(y2 + rho*y1))));
    end

elseif payoff_type == 6  % Arithmetic Basket Call / Put:  G = (sqrt(S_1) * sqrt(S_2) - K)^+
    K = contractParams.K;
    if contractParams.call == 1
        payoff = @(y1,y2) max(0, 0.5*S_0s(1)*exp(sigmas(1)*y1) + 0.5*S_0s(2)*exp(sigmas(2)*(y2 + rho*y1)) - K);
    else
        payoff = @(y1,y2) max(0, K - 0.5*S_0s(1)*exp(sigmas(1)*y1) - 0.5*S_0s(2)*exp(sigmas(2)*(y2 + rho*y1)));
    end

elseif payoff_type == 7  % Call-on-Max and Put-on-Min
    K = contractParams.K;
    if contractParams.call == 1
        payoff = @(y1,y2) max(0, max(S_0s(1)*exp(sigmas(1)*y1), S_0s(2)*exp(sigmas(2)*(y2 + rho*y1))) - K);
    else
        payoff = @(y1,y2) max(0, K - min(S_0s(1)*exp(sigmas(1)*y1), S_0s(2)*exp(sigmas(2)*(y2 + rho*y1))));
    end
elseif payoff_type == 8  % Call/put on Just S_2
    K = contractParams.K;
    if contractParams.call == 1
        payoff = @(y1,y2) max(0, S_0s(2)*exp(sigmas(2)*(y2 + rho*y1)) - K);
    else
        payoff = @(y1,y2) max(0, K - S_0s(2)*exp(sigmas(2)*(y2 + rho*y1)));
    end
elseif payoff_type == 9  % Best-of / worst of
    if contractParams.best == 1
        payoff = @(y1,y2) max(S_0s(1)*exp(sigmas(1)*y1), S_0s(2)*exp(sigmas(2)*(y2 + rho*y1)));
    else  % worst of
        payoff = @(y1,y2) min(S_0s(1)*exp(sigmas(1)*y1), S_0s(2)*exp(sigmas(2)*(y2 + rho*y1)));
    end
end

m_0 = length(y_1);
G = zeros(m_0, m_0);

for i=1:m_0
    for j=1:m_0
        G(i,j) = payoff(y_1(i), y_2(j));
    end
end

end


% Fig. 9.15   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
% script to generate Figure 9.15
sys=tf(1,[1 0.2 1 0]);
rlocus(sys)
title('Figure 9.15  Root locus of 1/(s^2+0.2s + 1)')
axis([-3 1 -1.5 1.5])
hold on
r=roots([1 .2 1 .5]);
 plot(r,'*')
 z=0:.1:.9;
wn= .5:.5:3;
 sgrid(z, wn)
 hold off


function s = i4_to_binary ( i )

%*****************************************************************************80
%
%% I4_TO_BINARY produces the binary representation of an integer.
%
%  Example:
%
%     I       S
%    --  ------
%     1      '1'
%     2     '10'
%     3     '11'
%     4    '100'
%    -9  '-1001'
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, an integer to be represented.
%
%    Output, string S, the binary representation.
%
  s = [];

  if ( i == 0 )
    s = strcat ( '0', s );
    return
  end
%
%  Write the string backwards.
%
  i_copy = abs ( i );

  while ( 0 < i_copy )

    if ( mod ( i_copy, 2 ) == 1 )
      s = strcat ( '1', s );
    else
      s = strcat ( '0', s );
    end

    i_copy = floor ( i_copy / 2 );

  end
%
%  Attach a minus sign, if needed.
%
  if ( i < 0 )
    s = strcat ( '-', s );
  end

  return
end


classdef SOP_F8 < PROBLEM
% <single> <real> <expensive/none>
% Generalized Schwefel's function 2.26

%------------------------------- Reference --------------------------------
% X. Yao, Y. Liu, and G. Lin, Evolutionary programming made faster, IEEE
% Transactions on Evolutionary Computation, 1999, 3(2): 82-102.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 1;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D) - 500;
            obj.upper    = zeros(1,obj.D) + 500;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            PopObj = -sum(PopDec.*sin(sqrt(abs(PopDec))),2);
        end
        %% Generate the minimum objective value
        function R = GetOptimum(obj,N)
            R = -418.9829*obj.D;
        end
    end
end


function Y = prtRvUtilLaplacePdf(X,mu,theta)
% Y = laplacepdf(X,mu,theta)

if numel(X) > length(X)
    error('X must have 1 singleton dimension')
end

Y = 1/(2*theta) * exp(-abs(X-mu)./theta);


function [K,MK,data] = hks(V,F,varargin)
  % HKS Heat Kernel Signature "A Concise and Provably Informative Multi-Scale
  % Signature Based on Heat Diffusion" [Sun et al. 2009]
  %
  % Inputs:
  %   V  #V by dim list of mesh vertex positions
  %   F  #F by ss list of element indices into V
  % Outputs:
  %   K  #V by #K list of HKS feature vectors for each vertex
  %   MK  #K list of mass-weighted spatial sums
  %   data
  %     lambda  #lambda list of eigen values
  %     phi  #V by #lambda list of eigen functions
  %     phi_sqr  #V by #lambda list of squared eigen functions
  %     exp_m_lambda  #lambda list of exp(- eigen values)
  %     M  #V by #V mass matrix
  %     L  #V by #V Laplacian matrix
  %
  % Example:
  %   [K,MK] = hks(V,F);
  %   % Compare all vertices' signatures to first using metric of [Sun et al.
  %   % 2009]
  %   tsurf(F,V,'CData',sqrt(sum(((K-K(1,:))./MK).^2,2)),fphong);

  tmin = [];
  tmax = [];
  data.t = [];
  data.exp_m_lambda = [];
  data.phi_sqr = [];
  data.M = [];
  k = 300;
  nt = 100;
  data = [];
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'tmax','tmin','Data','NumModes','NumTimeSteps'}, ...
    {'tmax','tmin','data','k','nt'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  if isempty(data)
    data.L = cotmatrix(V,F);
    data.M = massmatrix(V,F);
    [data.phi,data.lambda] = eigs(-data.L,data.M,k,'sm');
    data.lambda = diag(data.lambda);
    data.phi_sqr = data.phi.^2;
    data.exp_m_lambda = exp(-data.lambda);
  end
  if isempty(tmin)
    tmin = 4*log(10)/max(data.lambda);
  end
  if isempty(tmax)
    tmax = 4*log(10)/min(data.lambda(data.lambda>min(data.lambda)));
  end

  data.t = exp(linspace(log(tmin),log(tmax),nt));

  K = (data.phi_sqr)*(data.exp_m_lambda.^data.t);
  MK = diag(data.M)'*K;

end


function y1 = p00_rk_step ( test, neqn, order, t0, y0, t1 )

%*****************************************************************************80
%
%% P00_RK_STEP takes a single Runge-Kutta step from (T0,Y0) to (T1,Y1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer TEST, the problem number.
%
%    Input, integer NEQN, the number of equations.
%
%    Input, integer ORDER, the order of the Runge-Kutta method to be
%    employed.  Legal values are 1 through 5.
%
%    Input, real T0, Y0(NEQN), the arguments of the derivative
%    function.
%
%    Input, real T1, the point at which an estimate of the solution
%    is desired.
%
%    Output, real Y1(NEQN), the estimated solution at T1.
%
  dt = t1 - t0;

  if ( order == 1 )

    yp0 = p00_fun ( test, neqn, t0, y0 );

    y1(1:neqn,1) = y0(1:neqn,1) + dt * yp0(1:neqn,1);

  elseif ( order == 2 )

    yp0 = p00_fun ( test, neqn, t0, y0 );

    yk1(1:neqn,1) = y0(1:neqn,1) + dt * yp0(1:neqn,1);

    tk1 = t0 + dt;

    ypk1 = p00_fun ( test, neqn, tk1, yk1 );

    y1(1:neqn,1) = y0(1:neqn,1) + dt * ( yp0(1:neqn,1) + ypk1(1:neqn,1) ) / 2.0;

  elseif ( order == 3 )

    yp0 = p00_fun ( test, neqn, t0, y0 );

    yk1(1:neqn,1) = y0(1:neqn,1) + 0.5 * dt * yp0(1:neqn,1);

    tk1 = t0 + 0.5 * dt;
    ypk1 = p00_fun ( test, neqn, tk1, yk1 );

    yk2(1:neqn,1) = y0(1:neqn,1) + dt * ( 2.0 * ypk1(1:neqn,1) - yp0(1:neqn,1) );

    tk2 = t0 + dt;
    ypk2 = p00_fun ( test, neqn, tk2, yk2 );

    y1(1:neqn,1) = y0(1:neqn,1) + ( dt / 6.0 ) ...
      * ( yp0(1:neqn,1) + 4.0 * ypk1(1:neqn,1) + ypk2(1:neqn,1) );

  elseif ( order == 4 )

    yp0 = p00_fun ( test, neqn, t0, y0 );

    yk1(1:neqn,1) = y0(1:neqn,1) + 0.5 * dt * yp0(1:neqn,1);

    tk1 = t0 + 0.5 * dt;

    ypk1 = p00_fun ( test, neqn, tk1, yk1 );

    yk2(1:neqn,1) = y0(1:neqn,1) + 0.5 * dt * ypk1(1:neqn,1);

    tk2 = t0 + 0.5 * dt;
    ypk2 = p00_fun ( test, neqn, tk2, yk2 );

    tk3 = t0 + dt;

    yk3(1:neqn,1) = y0(1:neqn,1) + dt * ypk2(1:neqn,1);

    ypk3 = p00_fun ( test, neqn, tk3, yk3 );

    y1(1:neqn,1) = y0(1:neqn,1) + ( dt / 6.0 ) * ( ...
                  yp0(1:neqn,1) ...
          + 2.0 * ypk1(1:neqn,1) ...
          + 2.0 * ypk2(1:neqn,1) ...
      +           ypk3(1:neqn,1) );

  elseif ( order == 5 )

    yp0 = p00_fun ( test, neqn, t0, y0 );

    yk1(1:neqn,1) = y0(1:neqn,1) + 0.25 * dt * yp0(1:neqn,1);

    tk1 = t0 + 0.25 * dt;

    ypk1 = p00_fun ( test, neqn, tk1, yk1 );

    yk2(1:neqn,1) = y0(1:neqn,1) + dt * ( ...
        3.0 * yp0(1:neqn,1) ...
      + 9.0 * ypk1(1:neqn,1) ) / 32.0;

    tk2 = t0 + 3.0 * dt / 8.0;

    ypk2 = p00_fun ( test, neqn, tk2, yk2 );

    yk3(1:neqn,1) = y0(1:neqn,1) + dt * ( ...
        1932.0 * yp0(1:neqn,1) ...
      - 7200.0 * ypk1(1:neqn,1) ...
      + 7296.0 * ypk2(1:neqn,1) ) / 2197.0;

    tk3 = t0 + 12.0 * dt / 13.0;

    ypk3 = p00_fun ( test, neqn, tk3, yk3 );

    yk4(1:neqn,1) = y0(1:neqn,1) + dt * ( ...
      + (  439.0 /  216.0 ) * yp0(1:neqn,1) ...
      -      8.0            * ypk1(1:neqn,1) ...
      + ( 3680.0 /  513.0 ) * ypk2(1:neqn,1) ...
      - (  845.0 / 4104.0 ) * ypk3(1:neqn,1) );

    tk4 = t0 + dt;

    ypk4 = p00_fun ( test, neqn, tk4, yk4 );

    yk5(1:neqn,1) = y0(1:neqn,1) + dt * ( ...
      - (    8.0 /   27.0 ) * yp0(1:neqn,1) ...
      + (    2.0          ) * ypk1(1:neqn,1) ...
      - ( 3544.0 / 2565.0 ) * ypk2(1:neqn,1) ...
      + ( 1859.0 / 4104.0 ) * ypk3(1:neqn,1) ...
      - (   11.0 /   40.0 ) * ypk4(1:neqn,1) );

    tk5 = t0 + 0.5 * dt;

    ypk5 = p00_fun ( test, neqn, tk5, yk5 );

    y1(1:neqn,1)  = y0(1:neqn,1) + dt * ( ...
        (    16.0 / 135.0   ) * yp0(1:neqn,1) ...
      + (  6656.0 / 12825.0 ) * ypk2(1:neqn,1) ...
      + ( 28561.0 / 56430.0 ) * ypk3(1:neqn,1) ...
      - (     9.0 / 50.0    ) * ypk4(1:neqn,1) ...
      + (     2.0 / 55.0    ) * ypk5(1:neqn,1) );

  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P00_RK_STEP - Fatal error!\n' );
    fprintf ( 1, '  Unavailable Runge Kutta order = %d\n', order );
    error ( 'P00_RK_STEP - Fatal error!' );
  end

  return
end


% Test file for singfun/feval.m

function pass = test_feval(pref)

% Get preferences.
if ( nargin < 1 )
    pref = chebfunpref();
end

% Generate a few random points to use as test values.
seedRNG(786);
x = -1 + 2*rand(100, 1);

%%
% Check feval on empty set of points
data.exponents = [0, 0];
data.singType = {'none', 'none'};
f = singfun(@(x) x, data, pref);
pass(1) = isempty(feval(f, []));
%%
% Check feval on a SINGFUN without exponents
fh = @(x) sin(cos(10*x.^2));
data.exponents = [0, 0];
data.singType = {'none', 'none'};
f = singfun(fh, data, pref);
pass(2) = norm(feval(f,x) - feval(fh,x), inf) < 1e1*eps;

%%
% Check feval on a SINGFUN with negative exponents
a = 1 + rand();
b = 1 + rand();
fh = @(x) sin(cos(10*x.^2))./((1+x).^a.*(1-x).^b);
data.exponents = [-a, -b];
data.singType = {'sing', 'sing'};
f = singfun(fh, data, pref);
err = norm(feval(f,x) - feval(fh,x), inf);
tol = 2e3*eps;
pass(3) = err < tol;

%%
% Check feval on a SINGFUN with positive exponents
a = rand();
b = rand();
fh = @(x) sin(cos(10*x.^2)).*(1+x).^a.*(1-x).^b;
data.exponents = [a, b];
data.singType = {'root', 'root'};
f = singfun(fh, data, pref);
pass(4) = norm(feval(f,x) - feval(fh,x), inf) < 1e1*eps;

end


classdef Gaussian2C
%%GAUSSIAN2C A collection of functions for the bivariate Gaussian copulae.
%            The bivariate log-normal and Gaussian copulae have been proven
%            equivalent due to the monotonic relationship between the two
%            distributions [2].
%            Implemented functions include: PDF, CDF, tau, tau2CorrMat.
%
%REFERENCES:
%[1] H. Joe and D. Kurowicka, Dependence Modeling: Vine Copula Handbook.
%    World Scientific, 2011.
%[2] X. Zeng, J. Ren, Z. Wang, S. Marshall, and T. Durrani, "Copulas for
%    statistical signal processing (part i): Extensions and
%    generalization," Signal Processing, vol. 94, pp. 691-702, 2014,
%    ISSN: 0165-1684. DOI: https://doi.org/10.1016/j.sigpro.2013.07.009.
%    [Online]. Available: http://www.sciencedirect.com/science/article/pii/S0165168413002880.
%
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    methods(Static)
        function jprob = PDF(u,R)
            %PDF Generates a joint probability for column vector u or
            %    a row vector of joint probabilities for matrix u
            %    with each column as the input vector.
            %
            %INPUTS:
            % u: A 2-by-n matrix where each column represents a
            %    2-dimensional random vector and n is the number of
            %    random vectors. Entries must be in [0,1].
            % R: A 2-by-2 correlation matrix, or a scalar which is the
            %    correlation.
            %
            %OUTPUTS: jprob: A 1-by-n vector of joint probabilities.
            %
            %EXAMPLE 1: Make a contour plot of the density.
            % x = linspace(0,1,1e2);
            % [X,Y] = meshgrid(x);
            % Z = zeros(length(X),length(Y));
            % for idx = 1:length(X)
            %     for iidx = 1:length(Y)
            %         Z(idx,iidx) = Gaussian2C.PDF([X(idx,iidx);Y(idx,iidx)],0.5);
            %     end
            % end
            % contourf(X,Y,Z)
            %
            %EXAMPLE 2: Generate a joint density plot from random samples
            %           with histograms.
            % rvecs = randn(2,1e4);
            % rho = 0.65;
            % R = chol([1,rho;rho,1],'lower');
            % for idx = 1:size(rvecs,2)
            %     rvecs(:,idx) = R*rvecs(:,idx);
            % end
            % c = corrCoeffSpearman(rvecs(1,:),rvecs(2,:)); %diagonal entries close to rho
            % u1 = GaussianD.CDF(rvecs(1,:));
            % u2 = GaussianD.CDF(rvecs(2,:));
            % pdf = Gaussian2C.PDF([u1;u2],rho).*GaussianD.PDF(rvecs(1,:)).*GaussianD.PDF(rvecs(2,:));
            % fig = jointPlot2D(rvecs(1,:),rvecs(2,:),pdf);
            %
            %EXAMPLE 3: Generate an exact joint density plot.
            % x = linspace(0,1,1e2);
            % [X,Y] = meshgrid(x);
            % Z = zeros(length(X),length(Y));
            % for idx = 1:length(X)
            % for iidx = 1:length(Y)
            % Z(idx,iidx) = Gaussian2C.PDF([X(idx,iidx);Y(idx,iidx)],0.95);
            % X(idx,iidx) = GumbelD.invCDF(X(idx,iidx),1,2);
            % Y(idx,iidx) = GaussianD.invCDF(Y(idx,iidx));
            % Z(idx,iidx) = Z(idx,iidx).*GumbelD.PDF(X(idx,iidx),1,2).*GaussianD.PDF(Y(idx,iidx));
            % end
            % end
            % surf(X,Y,Z)
            %
            %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
            %
            if ~exist('u','var') || isempty(u)
                jprob = [];
                return
            end
            if ~exist('R','var') || isempty(R)
                rho = 0;
            elseif ~isscalar(R)
                rho = R(2,1);
            else
                rho = R;
            end

            a = sqrt(2)*erfinv(2*u(1,:)-1);
            b = sqrt(2)*erfinv(2*u(2,:)-1);
            c = (0.5*(2*a.*b*rho-rho^2*(a.^2+b.^2))/(1-rho^2));

            jprob = (1/sqrt(1-rho^2))*exp(c);

            %NaN results from subtraction of infinities in the exponential.
            %These should be 0s.
            nanIdxs = isnan(jprob);
            jprob(nanIdxs) = 0;

        end

        function jdist = CDF(u,R)
            %CDF Generates a joint distribution for column vector u or
            %    a row vector of joint probabilities for matrix u
            %    with each column as the input vector.
            %
            %INPUTS:
            % u: A 2-by-n matrix where each column represents a
            %    2-dimensional random vector and n is the number of
            %    random vectors. Entries must be in [0,1].
            % R: A 2-by-2 correlation matrix, or a scalar which is the
            %    correlation.
            %
            %OUTPUTS: jprob: A 1-by-n vector of joint distribution values.
            %
            %Note: This function uses a Monte Carlo method for
            %      approximating the CDF. It will take a few seconds to run
            %      the examples below.
            %
            %EXAMPLE 1: Make a contour plot of the distribution.
            % x = linspace(0,1,1e2);
            % [X,Y] = meshgrid(x);
            % Z = zeros(length(X),length(Y));
            % for idx = 1:length(X)
            %     for iidx = 1:length(Y)
            %         Z(idx,iidx) = Gaussian2C.CDF([X(idx,iidx);Y(idx,iidx)],0.65);
            %     end
            % end
            % contourf(X,Y,Z)
            %
            %EXAMPLE 2: Generate an exact joint distribution plot.
            % x = linspace(0,1,1e2);
            % [X,Y] = meshgrid(x);
            % Z = zeros(length(X),length(Y));
            % for idx = 1:length(X)
            %     for iidx = 1:length(Y)
            %         Z(idx,iidx) = Gaussian2C.CDF([X(idx,iidx);Y(idx,iidx)],0.65);
            %         X(idx,iidx) = GumbelD.invCDF(X(idx,iidx),1,2);
            %         Y(idx,iidx) = GaussianD.invCDF(Y(idx,iidx));
            %         Z(idx,iidx) = Z(idx,iidx);
            %     end
            % end
            % tiledlayout(1,2)
            % nexttile
            % surf(X,Y,Z)
            % Zsmooth = zeros(size(Z));
            % Zsmooth(:,end) = Z(:,end);
            % Zsmooth(:,1) = Z(:,1);
            % Zsmooth(end,:) = Z(end,:);
            % Zsmooth(1,:) = Z(1,:);
            % for idx = 2:size(Z,1)-1
            %     for iidx = 2:size(Z,2)-1
            %         Zsmooth(idx,iidx) = sum(Z(idx-1:idx+1,iidx-1:iidx+1),'all')/9;
            %     end
            % end
            % nexttile
            % surf(X,Y,Zsmooth)
            %
            %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
            %
            if ~exist('u','var') || isempty(u)
                jdist = [];
                return
            end
            if ~exist('R','var') || isempty(R)
                rho = 0;
            elseif ~isscalar(R)
                rho = R(2,1);
            else
                rho = R;
            end

            R = [1,rho;rho,1]^2;

            a = sqrt(2)*erfinv(2*u(1,:)-1);
            b = sqrt(2)*erfinv(2*u(2,:)-1);

            %Though this approximation could be used, the function
            %bivarGaussRectangleCDF produces a more exact result. Moreover,
            %it can handle -Inf lower bound, whereas the approximation
            %would have to set some arbitrary llim as the lower bound.
            %  jdist = arrayfun(@(ele1,ele2)GaussianD.integralOverRegion([0;0],R,[llim;llim],[ele1;ele2]),a,b);
            jdist = arrayfun(@(ele1,ele2)bivarGaussRectangleCDF([-Inf;-Inf],[ele1;ele2],[0;0],R),a,b);
        end

        function t = tau(R)
            %%TAU Compute Kendall's tau for the copula.
            %
            %INPUTS:
            % R: A 2-by-2 correlation matrix, or a scalar which is the
            %    correlation.
            %
            %OUTPUTS:
            % t: The scalar value of Kendall's tau.
            %
            %EXAMPLE: Shows Gaussian distribution of independent
            %         correlation parameter estimates using Kendall's
            %         tau.
            % for idx = 1:1e3
            %    x = GaussianD.rand(1e3,[0;0],[1,0.3;0.3,1]);
            %    u = GaussianD.CDF(x);
            %    s1 = GammaD.invCDF(u(1,:),2,1);
            %    s2 = GammaD.invCDF(u(2,:),4,4);
            %    tauEst(idx) = corr(s1',s2','Type','Kendall');
            % end
            % histogram(tauEst)
            % mu = mean(tauEst);
            % sigma = std(tauEst);
            % xline(mu,'k','LineWidth',2,'Label','\mu');
            % xline(mu-sigma,'r--','LineWidth',2,'Label','\mu-\sigma');
            % xline(mu+sigma,'r--','LineWidth',2,'Label','\mu+\sigma');
            %
            %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
            %
            if ~exist('R','var') || isempty(R)
                rho = 0;
            elseif ~isscalar(R)
                rho = R(2,1);
            else
                rho = R;
            end

            t = 2*arcsin(rho)/pi;
        end

        function R = tau2corrMat(t)
            %%TAU2CORRMAT Compute a 2-by-2 correlation matrix estimate
            %             from Kendall's tau.
            %
            %INPUTS:
            % t: Kendall's tau
            %
            %OUTPUTS:
            % R: A 2-by-2 correlation matrix
            %
            %EXAMPLE 1: Generates Gamma distributed marginals correlated
            %           via a Gaussian copula and estimates the correlation
            %           matrix.
            % x = GaussianD.rand(1e4,[0;0],[1,0.3;0.3,1]);
            % u = GaussianD.CDF(x);
            % s1 = GammaD.invCDF(u(1,:),2,1);
            % s2 = GammaD.invCDF(u(2,:),4,4);
            % tau = corr(s1',s2','Type','Kendall');
            % Gaussian2C.tau2corrMat(tau)
            %
            %EXAMPLE 2: Shows Gaussian distribution of independent
            %           correlation parameter estimates using Kendall's
            %           tau.
            % for idx = 1:1e3
            %    x = GaussianD.rand(1e3,[0;0],[1,0.3;0.3,1]);
            %    u = GaussianD.CDF(x);
            %    s1 = GammaD.invCDF(u(1,:),2,1);
            %    s2 = GammaD.invCDF(u(2,:),4,4);
            %    tauEst(idx) = corr(s1',s2','Type','Kendall');
            %    rho = Gaussian2C.tau2corrMat(tauEst(idx));
            %    theta(idx) = rho(1,2);
            % end
            % histogram(theta)
            % mu = mean(theta);
            % sigma = std(theta);
            % xline(mu,'k','LineWidth',2,'Label','\mu');
            % xline(mu-sigma,'r--','LineWidth',2,'Label','\mu-\sigma');
            % xline(mu+sigma,'r--','LineWidth',2,'Label','\mu+\sigma');
            %
            %April 2022 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
            %
            rho = sin(t*pi/2);
            R = [1,rho;rho,1];
        end
    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 result = sphere01_triangle_qua02 ( v1, v2, v3, f )

%*****************************************************************************80
%
%% SPHERE01_TRIANGLE_QUAD02 quadrature over a triangle on the unit sphere.
%
%  Discussion:
%
%    The integral is approximated by the average of the vertex values,
%    multiplied by the area.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real V1(3), V2(3), V3(3), the XYZ coordinates of
%    the vertices of the triangle.
%
%    Input, function v = F ( x ), evaluates the integrand at X.
%
%    Output, real RESULT, the approximate integral.
%
  area = sphere01_triangle_vertices_to_area ( v1, v2, v3 );

  quad = ( f ( v1 ) + f ( v2 ) + f ( v3 ) ) / 3.0;

  result = quad * area;

  return
end


function [G]=gsp_spectrum_cdf_approx(G,param)
%GSP_SPECTRUM_CDF_APPROX  Compute an approximation of the cumulative density function of the graph Laplacian eigenvalues
%   Usage:  G=gsp_spectrum_cdf_approx(G);
%           G=gsp_spectrum_cdf_approx(G,param);
%
%   Input parameters:
%         G                     : Graph structure.
%   Output parameters:
%         G                     : Graph structure, including the addition of G.spectral_warp_fn
%         param                 : Structure of additional parameter
%   Additional parameters:
%         param.num_pts         : Number of interpolation points
%         param.use_speedup     : Only perform step 1 (ldlsymbol) of the ldl algorithm once and then repeat the numeric step 2
%         param.use_permutation : Use a sparsity-preserving permutation
%         param.use_ldl_package : If the mex files for Timothy Davis' ldlsparse package are present. Otherwise use MATLAB's 'ldl' function
%         param.ldl_thresh      : Threshold parameter for MATLAB 'ldl' function
%
%
%   'gsp_spectrum_cdf_approx(G)' computes an approximation of the
%   cumulative density function of the graph Laplacian eigenvalues using
%   spectrum slicing techniques. The algorithm is as follows:
%
%   Step 1: Take $Q+1$ (param.num_pts) evenly spaced points on the interval
%   $[0,\lambda_{\max}]$.
%
%   Step 2: For every $q \in \{1,2,\ldots,Q\}$, compute a triangular
%   factorization:
%
%   .. \{\cal L}-\frac{q \lambda_{\max}}{Q}I = L_q \Delta_q L_q^*
%
%   .. math:: {\cal L}-\frac{q \lambda_{\max}}{Q}I = L_q \Delta_q L_q^*
%
%   where $\Delta_q$ is a diagonal matrix and $L_q$ is a lower triangular
%   matrix. By a corollary of Sylverster's law of inertia, the number of
%   negative eigenvalues of the diagonal matrix $\Delta_q$ is equal to the
%   number of eigenvalues of ${\cal L}$ less than $q \lambda_{\max} / Q$.
%
%   Step 3: Use monotonic cubic polynomial interpolation to interpolate the
%   points
%
%   .. { ( q\lambda_{max}/Q , mu_q/(N-1) ) }_q=0,1,\ldots,Q
%
%   .. math:: \left\{\left(\frac{q \lambda_{upper}}{Q},\frac{\mu_q}{N-1} \right)\right\}_{q=0,1,\ldots,Q}
%
%   where $\mu_q$ is the number of diagonal elements of $\Delta_q$ less than zero.
%
%   To perform the triangular factorizations, we use the LDL sparse
%   Cholesky package, written by Timothy Davis. The option
%   param.use_permutation enables a sparsity preserving permutation with
%   the function 'symamd'. If the option param.use_speedup is set to 1, the
%   symbolic analysis step of the LDL algorithm is only performed once for
%   all shifts.
%
%   See also:
%
%   Demos:
%
%   Requires: ldlsymbol_extra, ldlnumeric, ldlsparse
%
%   References: parlett1998symmetric davis2005ldl davis2011ldl
%

%TODO this function need to be cleaned

%   AUTHOR : David I Shuman.
%   TESTING:
%   REFERENCE:

if isfield(G, 'spectral_warp_fn')
    warning('Overwriting spectral warping function');
end

% if isfield(G,'e');
%     [x,y] = gsp_point2dcdf(G.e);
%     G.spectrum_cdf_approx = @(s) gsp_mono_cubic_warp_fn(x,y,s);
%     return
% end

if nargin<2
    param = struct;
end

if ~isfield(param, 'num_pts')
    num_pts=25;
else
    num_pts = param.num_pts;
end

if ~isfield(param, 'use_speedup')
    if ( exist('ldlsymbol_extra','file')==3 && exist('ldlnumeric','file')==3 )
        use_speedup=1;
    else
        use_speedup=0;
    end
else
    use_speedup = param.use_speedup;
end

if ~isfield(param, 'use_ldl_package')
    if exist('ldlsparse','file')==3
        use_ldl_package=1;
    else
        use_ldl_package=0;
    end
else
    use_ldl_package = param.use_ldl_package;
end

if ~isfield(param, 'use_permutation')
    use_permutation=1;
else
    use_permutation = param.use_permutation;
end

if ~isfield(G, 'lmax')
    warning(['GSP_SPECTRUM_CDF_APPROX: The variable lmax is not ',...
            'available. The function will compute it for you. ',...
            'However, if you apply many time this function, you ',...
            'should precompute it using the function: ',...
            'gsp_estimate_lmax']);
    G = gsp_estimate_lmax(G);
end

if ~isfield(param, 'ldl_thresh')
    ldl_thresh=.001;
else
    ldl_thresh = param.ldl_thresh;
end

counts=zeros(num_pts,1);
counts(num_pts)=G.N-1;

interp_x=(0:num_pts-1)*G.lmax/(num_pts-1);
interp_x=interp_x';

identity=speye(G.N);

if use_speedup
    if use_permutation
        P=symamd(G.L);
        [Parent, Lp, PO, PIn, flopcount] = ldlsymbol_extra(G.L,P);
        for i=1:num_pts-2
            mat=G.L-interp_x(i+1)*identity;
            [~, HD]=ldlnumeric(mat,Lp,Parent,PO,PIn);
            counts(i+1)=sum(diag(HD)<0);
        end
    else
        [Parent, Lp, flopcount] = ldlsymbol_extra(G.L);
        for i=1:num_pts-2
            mat=G.L-interp_x(i+1)*identity;
            [~, HD]=ldlnumeric(mat,Lp,Parent);
            counts(i+1)=sum(diag(HD)<0);
        end
    end
else
    if use_permutation
        P=symamd(G.L);
        for i=2:num_pts-1
            mat=G.L-interp_x(i)*identity;
            if use_ldl_package
                [~,HD]=ldlsparse(mat,P);
            else
                [~,HD,~]=ldl(mat(P,P),ldl_thresh);
            end
            counts(i)=sum(diag(HD)<0);
        end
    else
        for i=2:num_pts-1
            mat=G.L-interp_x(i)*identity;
            if use_ldl_package
                [~,HD]=ldlsparse(mat);
            else
                [~,HD,~]=ldl(mat,ldl_thresh);
            end
            counts(i)=sum(diag(HD)<0);
        end
    end
end
interp_y=counts/(G.N-1);

G.spectrum_cdf_approx = @(s) gsp_mono_cubic_warp_fn(interp_x,interp_y,s);

end


%%% ADMM
% A = L + S + Z
% L: low-rank
% S: sparse
% Z: stochastic or deterministic pertubation
function results = ADMM(M)
N = 3;
[mm,nn] = size(M);
g2_max = norm(M(:),inf); % Inf-norm
g3_max = norm(M); % L2-norm
g2 = 0.15*g2_max;
g3 = 0.15*g3_max;
MAX_ITER = 100;
ABSTOL   = 1e-4;
RELTOL   = 1e-2;
lambda = 1;
rho = 1/lambda;
Z = zeros(mm,nn); S = zeros(mm,nn); L = zeros(mm,nn);
z = zeros(mm,N*nn); U = zeros(mm,nn);
fprintf('\n%3s\t%10s\t%10s\t%10s\t%10s\t%10s\n', 'iter', ...
    'r norm', 'eps pri', 's norm', 'eps dual', 'objective');
for k = 1:MAX_ITER
    B = avg(Z, S, L) - M./N + U;

    % x-update
    Z = (1/(1+lambda))*(Z - B);
    S = prox_l1(S - B, lambda*g2);
    L = prox_matrix(L - B, lambda*g3, @prox_l1);

    % (for termination checks only)
    x = [Z S L];
    zold = z;
    z = x + repmat(-avg(Z, S, L) + M./N, 1, N);

    % u-update
    U = B;

    % diagnostics, reporting, termination checks
    results.objval(k)   = objective(Z, g2, S, g3, L);
    results.r_norm(k)   = norm(x - z,'fro');
    results.s_norm(k)   = norm(-rho*(z - zold),'fro');
    results.eps_pri(k)  = sqrt(mm*nn*N)*ABSTOL + RELTOL*max(norm(x,'fro'), norm(-z,'fro'));
    results.eps_dual(k) = sqrt(mm*nn*N)*ABSTOL + RELTOL*sqrt(N)*norm(rho*U,'fro');

    if k == 1 || mod(k,1) == 0
      fprintf('%4d\t%10.4f\t%10.4f\t%10.4f\t%10.4f\t%10.2f\n', k, ...
        results.r_norm(k), results.eps_pri(k), results.s_norm(k), results.eps_dual(k), results.objval(k));
    end
    if results.r_norm(k) < results.eps_pri(k) && results.s_norm(k) < results.eps_dual(k)
       break;
    end
end
results.iter = k;
results.Z = Z;
results.S = S;
results.L = L;
M_hat = L + S + Z;

error = norm(M_hat(:)-M(:))/norm(M(:));
disp(['Error: ' num2str(error)]);
end


%  Figure 3.40      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
% script to generate Fig. 3.40
fh=@(ki,k) 6+3*k-ki;
ezplot(fh)
hold on;
f=@(ki,k) ki;
ezplot(f);

% add shading
fillvert = [0, -2
	        0,  7
			7, 7
			7, 0];
fcolor = [ .1 .1 .1 .1];
patch(fillvert(:,1)', fillvert(:,2)', fcolor)

xlabel('K_I');
ylabel('K');
title('Allowable region for stability');
nicegrid;
hold off


function [lmval,indd]=lmax_pw(xx, dx)
%   Find  piece-wise  local maxima in vector XX,where
%	LMVAL is the output vector with maxima values, INDD  is the
%	corresponding indexes, DX is length of piece where maxima is searched,
%   IMPORTANT:      FIRST and LAST point in vector are excluded
%   IMPORTANT:      XX must be single column vector
%   IMPORTANT:      Length of DX must be very carefully selected
%	For example compare dx=30; and dx=1000;
%
%   dx=150; xx=[0:0.01:35]'; y=sin(xx .* cos(xx /4.5)) + cos(xx);
%    plot(xx,y); grid; hold on;
%   %   Excluding first and last points
%   [b,a]=lmax_pw(y,dx); plot(xx(a),y(a),'r+')
%   % Way to include first and last points can be as:
%   y(1)=1.5; yy=[0; y; -1;];   % padd with smaller values
%   [b,a]=lmax_pw(yy,dx); a=a-1; plot(xx(a),y(a),'go')
%
%	see also LMIN, LMAX,  LMIN_PW, MATCH

% 	Sergei Koptenko, Applied Acoustic Technologies, Toronto, Canada
%   sergei.koptenko@sympatico.ca,  March/11/2003

if nargin <2,
	disp('Not enough arguments'); return
end

len_x = length(xx);
xx = [xx; xx(len_x); xx(len_x)];
nn=floor(len_x/dx);
ncount=1; lmval=[]; indd=[];
	for ii=1:nn,
        [lm,ind] = max(xx(ncount: ii*dx+2)) ;
        ind=ind+(ii-1)*dx;
                 if (ind ~=ncount) & (ind~=ii*dx+2),
                    lmval=[lmval, lm]; indd=[indd, ind];
                end
        ncount=ncount +dx;
	end
[lm,ind] = max(xx(ii*dx:len_x));
        if (ind ~=len_x) & (ind~=ii*dx),
            lmval=[lmval, lm]; indd=[indd, (ind+ii*dx-1)];
        end

       if indd(end)==len_x,
           indd=  indd(1:end-1);
           lmval=lmval(1:end-1);
       end
return


function a = wishart_sample_inverse ( m, df, sigma )

%*****************************************************************************80
%
%% WISHART_SAMPLE_INVERSE returns the inverse of a sample Wishart matrix.
%
%  Discussion:
%
%    This function requires functions from the PDFLIB and RNGLIB libraries.
%
%    The "initialize()" function from RNGLIB must be called before using
%    this function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Patrick Odell, Alan Feiveson,
%    A numerical procedure to generate a sample covariance matrix,
%    Journal of the American Statistical Association,
%    Volume 61, Number 313, March 1966, pages 199-203.
%
%    Stanley Sawyer,
%    Wishart Distributions and Inverse-Wishart Sampling,
%    Washington University,
%    30 April 2007, 12 pages.
%
%  Parameters:
%
%    Input, integer M, the order of the matrix.
%
%    Input, integer DF, the number of degrees of freedom.
%    M <= DF.
%
%    Input, real SIGMA(M,M), the covariance matrix, which should be
%    a symmetric positive definite matrix.
%
%    Output, real A(M,M), the inverse of a sample matrix from the Wishart
%    distribution.
%
  if ( df < m )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'WISHART_SAMPLE_INVERSE - Fatal error!\n' );
    fprintf ( 1, '  DF = %d < M = %d.\n', df, m );
    error ( 'WISHART_SAMPLE_INVERSE - Fatal error!\n' );
  end
%
%  Get R, the upper triangular Cholesky factor of SIGMA.
%
  r = chol ( sigma );
%
%  Get S, the inverse of R.
%
  s = r8ut_inverse ( m, r );
%
%  Get UA, the inverse of a sample from the unit Wishart distribution.
%
  ua = wishart_unit_sample_inverse ( m, df );
%
%  Construct the matrix A = S * UA * S'.
%
  a = s * ua * s';

  return
end


function pass = test_roots08( pref )
% Check that the marching squares and Bezoutian agree with each other.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 1e3 * pref.cheb2Prefs.chebfun2eps;
j = 1;

%%
f = chebfun2(@(x,y)sin(10*x-y/10));
g = chebfun2(@(x,y)cos(3*x.*y));
r1 = roots([f;g],'ms');
r2 = roots([f;g],'resultant');
pass(j) = ( norm(sort(r1(:,1))-sort(r2(:,1))) < tol ); j = j + 1;
pass(j) = ( norm(sort(r1(:,2))-sort(r2(:,2))) < tol ); j = j + 1;

%%
f = chebfun2(@(x,y)sin(10*x-y/10) + y);
g = chebfun2(@(x,y)cos(10*y-x/10) - x);
r1 = roots([f;g],'ms');
r2 = roots([f;g],'resultant');
pass(j) = ( norm(sort(r1(:,1))-sort(r2(:,1))) < tol ); j = j + 1;
pass(j) = ( norm(sort(r1(:,2))-sort(r2(:,2))) < tol ); j = j + 1;

end


function imgOut = bilateralNoiseRemoval(img, sigma_s, sigma_r)
%
%       imgOut = bilateralNoiseRemoval(img, sigma_s, sigma_r)
%
%
%        Input:
%           -img: input image to be denoised
%           -simga_s: spatial sigma; size of the neighborhood to be used
%           for denosing
%           -sigma_r: range sigma; what intesities to use for filtering
%
%        Output:
%           -imgOut: filtered denoised image
%
%     Copyright (C) 2013  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

col = size(img,3);

if(~exist('sigma_s', 'var'))
    sigma_r = 4;
end

if(sigma_s<4)
    sigma_s = 4;
end

if(~exist('sigma_r', 'var'))
    sigma_r = 0.01;
end

if(sigma_r <= 0.0)
    sigma_r = 0.01;
end

switch col
	case 1
    	minC = min(img(:));
        maxC = max(img(:));
        delta = (maxC - minC);
        img = (img - minC) / delta;
        imgOut = bilateralFilter(img, [], 0.0, 1.0, sigma_s, sigma_r) * delta + minC;

    case 3
        imgLab = ConvertXYZtoCIELab(ConvertRGBtoXYZ(img, 0), 0);

        for i=1:col
            tmp = imgLab(:,:,i);
            minC = min(tmp(:));
            maxC = max(tmp(:));
            delta = (maxC - minC);
            tmp = (tmp - minC) / delta;
            imgLab(:,:,i) = bilateralFilter(tmp,[], 0.0, 1.0,sigma_s,sigma_r) * delta + minC;
        end

        imgOut = ConvertRGBtoXYZ(ConvertXYZtoCIELab(imgLab,1),1);

    otherwise
        imgOut = zeros(size(img));

        for i=1:col
            tmp = img(:,:,i);
            minC = min(tmp(:));
            maxC = max(tmp(:));
            delta = (maxC - minC);
            tmp = (tmp - minC) / delta;
            imgOut(:,:,i) = bilateralFilter(tmp, [], 0.0, 1.0,sigma_s, sigma_r) * delta + minC;
        end
end

end


function accuracy = evalAccuracyHungarian(label, labelTruth)
% Find segmentation accuracy.
% Input:
%   label: label given by algorithm(e.g. SSC).
%   labelTruth: ground truth label.

labelVal = unique(label);% e.g. [0 128 255];
labelTruthVal = unique(labelTruth);% e.g.[1 2 3 4]
n = length(labelVal);% e.g. 3
nTruth = length(labelTruthVal);% e.g. 4

if n > nTruth
    accuracy = evalAccuracyHungarian(labelTruth, label);
    return;
end

minBadLabelCount = +inf;
combMat = nchoosek(labelTruthVal, n);
costMat = zeros(n);
for ii = 1:size(combMat, 1)
    combVal = combMat(ii, :);% e.g. [1 3 4]

    for jj = 1:n
        for kk = 1:n
            costMat(jj, kk) = sum( labelTruth( label == labelVal(jj) ) ~= combVal(kk) );
        end
    end
    [~, badLabelCount] = munkres(costMat);
    if badLabelCount < minBadLabelCount
        minBadLabelCount = badLabelCount;
    end
end
accuracy = 1 - minBadLabelCount / length(label);

end


function[pixelAccuracy, meanAccuracy, meanIU] = confMatToAccuracies(confusion)
% [pixelAccuracy, meanAccuracy, meanIU] = confMatToAccuracies(confusion)
%
% Compute accuracies from a confusion matrix.
%
% Copyright by Holger Caesar, 2016

% compute various statistics of the confusion matrix
total = sum(confusion(:));
pos = sum(confusion, 2);
res = sum(confusion, 1)';
tp = diag(confusion);
IUs = tp ./ (pos + res - tp);
missing = pos == 0;

% Modified metrics to ignore classes for which we didn't see
% any pixels yet in this epoch
pixelAccuracy = sum(tp) / total;
meanAccuracy = nanmean(tp ./ pos);
meanIU = mean(IUs(~missing));


function [g,filtertype] = gsp_jtv_design_meyer(G,Nf)
%GSP_DESIGN_JTV_MEYER Design the jtv Meyer tight filterbank
%   Usage: [g,filtertype] = gsp_design_jtv_meyer(G);
%          [g,filtertype] = gsp_design_jtv_meyer(G,Nf);
%
%   Input parameters:
%       G       : Time-Vertex graph structure
%       Nf      : Number of filters for each domain (total number Nf^2, default Nf = 4)
%   Output parameters:
%       g          : Cell array of time-vertex filters
%       filtertype : Filter domain js
%

% Author :  Francesco Grassi
% Date   : September 2016

if nargin<2
    Nf = 4;
end

%graph meyer
g1 = gsp_design_meyer(G,Nf);

%time meyer
g2 = gsp_design_meyer(0.5/G.jtv.fs,Nf);

%building jtv meyer separable filterbank
n = 0;
g = cell(Nf^2,1);
for ii=1:Nf
    for jj=1:Nf
        n = n+1;
        g{n} = @(x,y) g1{ii}(x).*g2{jj}(abs(y));
    end
end

filtertype = 'js';

end


function jed = ymdf_to_jed_bahai ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_JED_BAHAI converts a Bahai YMDF date to a JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Algorithm E,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999, pages 323-324.
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, real JED, the corresponding Julian Ephemeris Date.
%

%
%  Convert the calendar date to a computational date.
%
  y_prime = y + 6560 - floor ( ( 39 - m ) / 20 );
  m_prime = mod ( m, 20 );
  d_prime = d - 1;
%
%  Convert the computational date to a JED.
%
  j1 = floor ( ( 1461 * y_prime ) / 4 );

  j2 = 19 * m_prime;

  g = floor ( ( y_prime + 184 ) / 100 );
  g = floor ( ( 3 * g ) / 4 ) - 50;
  jed = j1 + j2 + d_prime - 1412 - g - 0.5;
  jed = jed + f;

  return
end


%% h = HEXSCATTER( x, y, ... )
% Gordon Bean, February 2014
% A scatter-plot substitute - generate a density plot using hexagonal
% patches.
%
% Syntax
% hexscatter(xdata, ydata)
% hexscatter(xdata, ydata, 'Name', Value, ...)
% h = hexscatter(...)
%
% Description
% hexscatter(xdata, ydata) creates a density plot of the ydata versus the
% xdata using hexagonal tiles. xdata and ydata should be vectors. NaN
% values (and their corresponding values in the other vector) are ignored.
%
% hexscatter(xdata, ydata, 'Name', Value, ...) accepts name-value pairs of
% arguments from the following list (defaults in {}):
%  'xlim' { [min(xdadta(:) max(xdata(:))] } - a 2-element vector containing
%  the lower and upper bounds of the 2nd dimension of the grid.
%  'ylim' { [min(ydadta(:) max(ydata(:))] } - a 2-element vector containing
%  the lower and upper bounds of the 1st dimension of the grid.
%  'res' { 50 } - the resolution, or number of bins in each dimension. The
%  total number of bins will be the resolution squared.
%  'drawEdges' { false } - if true, edges are drawn around each hexagonal
%  patch.
%  'showZeros' { false } - if true, bins with 0 counts are shaded; if
%  false, only bins with non-zero counts are colored.
%
% h = hexscatter( ... ) returns the object handle to the patch object
% created.
%
% Examples
% hexscatter(rand(2000,1), rand(2000,1))
%
% hexscatter(rand(2000,1), rand(2000,1), 'res', 90)
%
% Also available in the Bean Matlab Toolkit:
% https://github.com/brazilbean/bean-matlab-toolkit

function h = hexscatter( xdata, ydata, varargin )
    params = default_param( varargin, ...
        'xlim', [min(xdata(:)) max(xdata(:))], ...
        'ylim', [min(ydata(:)) max(ydata(:))], ...
        'res', 50, ...
        'drawEdges', false, ...
        'showZeros', false);

    if params.drawedges
        ec = 'flat';
    else
        ec = 'none';
    end

    %% Determine grid
    xl = params.xlim;
    yl = params.ylim;

    xbins = linspace(xl(1), xl(2), params.res);
    ybins = linspace(yl(1), yl(2), params.res);
    dy = diff(ybins([1 2]))*0.5;

    [X, Y] = meshgrid(xbins, ybins);
    n = size(X,1);
    Y(:,1:fix(end/2)*2) = ...
        Y(:,1:fix(end/2)*2) + repmat([0 dy],[n,fix(n/2)]);

    %% Map points to boxes
    nix = isnan(xdata) | isnan(ydata);
    xdata = xdata(~nix);
    ydata = ydata(~nix);

    % Which pair of columns?
    dx = diff(xbins([1 2]));
    foox = floor((xdata - xbins(1)) ./ dx)+1;
    foox(foox > length(xbins)) = length(xbins);

    % Which pair of rows?
    % Use the first row, which starts without an offset, as the standard
    fooy = floor((ydata - ybins(1)) ./ diff(ybins([1 2])))+1;
    fooy(fooy > length(ybins)) = length(ybins);

    % Which orientation
    orientation = mod(foox,2) == 1;

    % Map points to boxes
    foo = [xdata - xbins(foox)', ydata - ybins(fooy)'];

    % Which layer
    layer = foo(:,2) > dy;

    % Convert to block B format
    toflip = layer == orientation;
    foo(toflip,1) = dx - foo(toflip,1);

    foo(layer==1,2) = foo(layer==1,2) - dy;

    % Find closest corner
    dist = sqrt(sum(foo.^2,2));
    dist2 = sqrt(sum(bsxfun(@minus, [dx dy], foo).^2, 2));

    topright = dist > dist2;

    %% Map corners back to bins
    % Which x bin?
    x = foox + ~(orientation == (layer == topright));
    x(x > length(xbins)) = length(xbins);

    % Which y bin?
    y = fooy + (layer & topright);
    y(y > length(ybins)) = length(ybins);

    ii = sub2ind(size(X), y, x);

    %% Determine counts
    counts = sum(bsxfun(@eq, ii, 1:numel(X)),1);

    newplot;
    xscale = diff(xbins([1 2]))*2/3;
    yscale = diff(ybins([1 2]))*2/3;
    theta = 0 : 60 : 360;
    x = bsxfun(@plus, X(:), cosd(theta)*xscale)';
    y = bsxfun(@plus, Y(:), sind(theta)*yscale)';

    if params.showzeros
        h = patch(x, y, counts, 'edgeColor', ec);
    else
        jj = counts > 0;
        h = patch(x(:,jj), y(:,jj), counts(jj), 'edgeColor', ec);
    end

    if nargout == 0
        clear h;
    end

    %% Function: default_param
    % Gordon Bean, March 2012
    % Copied from https://github.com/brazilbean/bean-matlab-toolkit
    function params = default_param( params, varargin )
        if (iscell(params))
            params = get_params(params{:});
        end
        defaults = get_params(varargin{:});

        for f = fieldnames(defaults)'
            field = f{:};
            if (~isfield( params, lower(field) ))
                params.(lower(field)) = defaults.(field);
            end
        end
    end

    %% Function: get_params - return a struct of key-value pairs
    % Gordon Bean, January 2012
    %
    % Usage
    % params = get_params( ... )
    %
    % Used to parse key-value pairs in varargin - returns a struct.
    % Converts all keys to lower case.
    %
    % Copied from https://github.com/brazilbean/bean-matlab-toolkit
    function params = get_params( varargin )
        params = struct;

        nn = length(varargin);
        if (mod(nn,2) ~= 0)
            error('Uneven number of parameters and values in list.');
        end

        tmp = reshape(varargin, [2 nn/2]);
        for kk = 1 : size(tmp,2)
            params.(lower(tmp{1,kk})) = tmp{2,kk};
        end
    end
end


function plotAreaUnderACurve(handles, integrationLimits, coefficients, plot1_xAxis, plot1_yAxis)

% Get the handle to the axes
axes(handles.integrationPlotAxes)

% Find the roots of the polynomial
fx = coefficients(end:-1:1);
rootsFx = roots(fx);
realRootsFx = [];
for idx = 1:length(rootsFx)
    if isreal(rootsFx(idx))
        realRootsFx(end+1,1) = rootsFx(idx);
    end
end

prevXstart = integrationLimits(1);

if ~isempty(realRootsFx)
    realRootsFx = sortrows(realRootsFx);
    % Plot the shaded area (above x-axis in blue and below x-axis in red)
    for idx = 1:length(realRootsFx)
        if ((realRootsFx(idx) > integrationLimits(1)) && (realRootsFx(idx) < integrationLimits(2)))
            % Set the x and y points
            xiPrec = (realRootsFx(idx) - prevXstart)/20;
            xi = (prevXstart:xiPrec:realRootsFx(idx));
            yi = coefficients(1)+coefficients(2)*xi+coefficients(3)*xi.^2+coefficients(4)*xi.^3+coefficients(5)*xi.^4+coefficients(6)*xi.^5;
            faceColor = [0 0.5 1];
            if sign(yi(end-1)) == -1
                faceColor = [1 0 0];
            end
            area(xi,yi,'FaceColor',faceColor);
            hold on
            prevXstart = realRootsFx(idx);
        end
    end
end

xiPrec = (integrationLimits(2) - prevXstart)/20;
xi = (prevXstart:xiPrec:integrationLimits(2));
yi = coefficients(1)+coefficients(2)*xi+coefficients(3)*xi.^2+coefficients(4)*xi.^3+coefficients(5)*xi.^4+coefficients(6)*xi.^5;
faceColor = [0 0.5 1];
if sign(yi(end-1)) == -1
    faceColor = [1 0 0];
end
area(xi,yi,'FaceColor',faceColor);
hold on

x = (integrationLimits(1):integrationLimits(2));
y = coefficients(1)+coefficients(2)*x+coefficients(3)*x.^2+coefficients(4)*x.^3+coefficients(5)*x.^4+coefficients(6)*x.^5;
plot(x,y,'LineWidth',2.5,'Color',[0 0 0]);
xlabel('x','Fontweight','b');
ylabel('f(x)','Fontweight','b');
axis([plot1_xAxis(1) plot1_xAxis(end) plot1_yAxis(1) plot1_yAxis(end)]);
title('Integration');
grid on
hold off


function [b,a]=potsband(fs)
%POTSBAND Design filter for 300-3400 telephone bandwidth [B,A]=(FS)
%
%Input: FS=sample frequency in Hz
%
%Output: B/A is a discrete time bandpass filter with a passband gain of 1
%
%The filter meets the specifications of G.151 for any sample frequency
%and has a gain of -3dB at the passband edges.

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

szp=[0.19892796195357i; -0.48623571568937+0.86535995266875i];
szp=[[0; -0.97247143137874] szp conj(szp)];
% s-plane zeros and poles of high pass 3'rd order chebychev2 filter with -3dB at w=1
zl=2./(1-szp*tan(300*pi/fs))-1;
al=real(poly(zl(2,:)));
bl=real(poly(zl(1,:)));
sw=[1;-1;1;-1];
bl=bl*(al*sw)/(bl*sw);
zh=2./(szp/tan(3400*pi/fs)-1)+1;
ah=real(poly(zh(2,:)));
bh=real(poly(zh(1,:)));
bh=bh*sum(ah)/sum(bh);
b=conv(bh,bl);
a=conv(ah,al);


function s = snrdbadd(x,n,db)

ls = length(x);

%% signal and noise power

sigpower = 10*log10(sum(x.^2)/ls);

noisepower = 10*log10(sum(n.^2)/ls);

%%

npower = sigpower-noisepower-db;

%%

s = x+sqrt(10^(npower/10)).*n;