%% 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


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


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


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


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 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


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 [A,B,D,E,I]=E7x(R,C)
% Subprogram for 7th order Elliptical LPF
N=7;U=11;M=1;Y=11;
% create space for and zero A1 & B2 matrices
A1=zeros(U+N);B2=zeros(U+N,N+M);
%
% R=[R1 R2 R3 R4 R5 R6 R7 R8 R9 R10 R11 R12 R13 R14 R15]
%    1  2  3  4  5  6  7  8  9  10  11  12  13  14  15
%
% C = [C1 C2 C3 C4 C5 C6 C7];
%      1  2  3  4  5  6  7
%
% For column alignment:
%
% V1 V2 V3 V4 V5 V6 V7 V8 V9 V10 V11   iC1 iC2 iC3 iC4 iC5 iC6 iC7
% 1  2  3  4  5  6  7  8  9  10  11    12  13  14  15  16  17  18
%
% Create A1
%
%       -V1/R2      +iC1
A1(1,1)=-1/R(2);A1(1,12)=1; % -Ein/R1 - V1/R2 + iC1 = 0
%
G2=1/R(3)+1/R(5); % For row 2 below
%   +V1/R3        -V2*G2       +iC2
A1(2,1)=1/R(3);A1(2,2)=-G2;A1(2,13)=1; % V1/R3 -V2*G2 + iC2 = 0
%
A1(3,3)=-1/R(6);A1(3,4)=1/R(6);A1(3,13)=-1;A1(3,14)=1; % -V3/R6 + V4/R6 - iC2 + iC3 = 0
%
G4=1/R(6)+1/R(9)+1/R(10); % For row 4 below
%
% V3/R6        +V6/R9        +V8/R10         -V4*G4       +iC4 = 0
A1(4,3)=1/R(6);A1(4,6)=1/R(9);A1(4,8)=1/R(10);A1(4,4)=-G4;A1(4,15)=1;
A1(5,1)=1/R(4);A1(5,5)=-1/R(4);A1(5,15)=-1;A1(5,14)=-1;
A1(6,5)=1/R(7);A1(6,6)=-1/R(7);A1(6,14)=-1;
A1(7,6)=1/R(8);A1(7,7)=-1/R(8);A1(7,17)=-1;A1(7,18)=-1;
A1(8,4)=1/R(10);A1(8,8)=-(1/R(10)+1/R(11));A1(8,16)=1;
A1(9,10)=1/R(12);A1(9,9)=-1/R(12);A1(9,17)=1;A1(9,16)=-1;
G10=1/R(12)+1/R(14)+1/R(15); % For row 10 below
A1(10,18)=1;A1(10,10)=-G10;A1(10,11)=1/R(14);A1(10,9)=1/R(12);
A1(11,7)=1/R(13);A1(11,11)=-1/R(13);A1(11,17)=-1;
A1(12,1)=-1; % 0-V1=E1
A1(13,3)=1;A1(13,2)=-1; % V3-V2=E2
A1(14,6)=1;A1(14,3)=-1; % V6-V3=E3
A1(15,5)=1;A1(15,4)=-1; % V5-V4=E4
A1(16,9)=1;A1(16,8)=-1; % V9-V8=E5
A1(17,11)=1;A1(17,9)=-1; % V11-V9=E6
A1(18,7)=1;A1(18,10)=-1; % V7-V10=E7
%
% B2
%
E1=1;E2=1;E3=1;E4=1;E5=1;E6=1;E7=1;Ein=1;
% E1 E2 E3 E4 E5 E6 E7 Ein  Source
% 1  2  3  4  5  6  7  8    Column of B2
%
B2(12,1)=E1;
B2(13,2)=E2;
B2(14,3)=E3;
B2(15,4)=E4;
B2(16,5)=E5;
B2(17,6)=E6;
B2(18,7)=E7;
B2(1,8)=Ein/R(1);

%
P=diag([C(1) C(2) C(3) C(4) C(5) C(6) C(7)]);
%
% Template "canned" statements
%
V=A1\B2;H=V(U+1:U+N,1:N+M);I=eye(N);
AB=P\H;A=AB(1:N,1:N);B=AB(1:N,N+1:N+M);
D=V(Y:Y,1:N);E=V(Y:Y,N+1:N+M);


function yearly_tmax_correction(filenameout)

% this program is design to adjust the daily Tmax generated by WG using
% new yearly Tmax series produced by FFT.

% load the yearly averaged Tmax after FFT
load('Pnew_tmax');
tmax_FFT=Pnew_tmax';

% load WG generated daily Tmax
load(filenameout);
[n,m]=size(gTmax);
gTmax=gTmax';
tmax_WG=reshape(gTmax,[],1);

n=length(tmax_FFT); % years
m=length(tmax_WG);  % days

% calculate yearly Tmax generated by WG, namely (Y)
j=1;
Z=zeros(n,1);
for i=1:365:m
    Z(j,1)=mean(tmax_WG(i:i+365-1,1));
    j=j+1;
end

% calculate the ratio of yearly Tmax between those after FFT and
% generated by weather generator
for i=1:n
    tmax_ratio(i,1)=tmax_FFT(i,1)-Z(i,1);
end

% extend the yearly Tmax ratio to daily scale,the data in each
% year are the same
tmax_extent=zeros(m,1);
j=1;
for i=1:365:m
    tmax_extent(i:i+365-1,1)=tmax_ratio(j,1);
    j=j+1;
end

% adjust the daily Tmax generated by WG using above ratios
tmax_adjust=zeros(size(tmax_WG));
for i=1:m
    tmax_adjust(i,1)=tmax_WG(i,1)+tmax_extent(i,1);
end
yearly_corrected_tmax=tmax_adjust;
save('yearly_corrected_tmax','yearly_corrected_tmax')


function [W,H] = NNDSVD(A, k)
nSample = size(A, 1);
nFeature = size(A, 2);
k = k + 1;
% --------------------------------------------
[U, S, V] = svds(A, k);

% --------------------------------------------
W = zeros(nSample, k);
H = zeros(k, nFeature);
W(:,1) = sqrt(S(1,1))*U(:,1);
H(1,:) = sqrt(S(1,1))*V(:,1)';
for i = 2:k
    x = U(:,i);
    y = V(:,i);
    xp = pos(x); xn = neg(x);
    yp = pos(y); yn = neg(y);
    xpnorm = norm(xp); ypnorm = norm(yp); mp = xpnorm*ypnorm;
    xnnorm = norm(xn); ynnorm = norm(yn); mn = xnnorm*ynnorm;
    if mp > mn
        u = xp/xpnorm;
        v = yp/ypnorm;
        sigma = mp;
    else
        u = xn/xnnorm;
        v = yn/ynnorm;
        sigma = mn;
    end
    W(:,i) = sqrt(S(i,i)*sigma)*u;
    H(i,:) = sqrt(S(i,i)*sigma)*v';
end
W = W(:, 2:k);
H = H(2:k, :);
end

function [xp] = pos(x)
xp = (x >= 0).*x;
end

function [xn] = neg(x)
xn = (x < 0).*(-x);
end


function [filt, B, A] = ft_preproc_bandpassfilter(dat, Fs, Fbp, order, type, dir, instabilityfix, df, wintype, dev, plotfiltresp, usefftfilt)

% FT_PREPROC_BANDPASSFILTER applies a band-pass filter to the data and thereby
% removes the spectral components in the data except for the ones in the
% specified frequency band.
%
% Use as
%   [filt] = ft_preproc_bandpassfilter(dat, Fs, Fbp, order, type, dir, instabilityfix, df, wintype, dev, plotfiltresp, usefftfilt)
% where
%   dat             data matrix (Nchans X Ntime)
%   Fs              sampling frequency in Hz
%   Fbp             frequency band, specified as [Fhp Flp] in Hz
%   order           optional filter order, default is 4 (but) or dependent on frequency band and data length (fir/firls)
%   type            optional filter type, can be
%                     'but'       Butterworth IIR filter (default)
%                     'firws'     FIR filter with windowed sinc
%                     'fir'       FIR filter using MATLAB fir1 function
%                     'firls'     FIR filter using MATLAB firls function (requires MATLAB Signal Processing Toolbox)
%                     'brickwall' frequency-domain filter using forward and inverse FFT
%   dir             optional filter direction, can be
%                     'onepass'                   forward filter only
%                     'onepass-reverse'           reverse filter only, i.e. backward in time
%                     'onepass-zerophase'         zero-phase forward filter with delay compensation (default for firws, linear-phase symmetric FIR only)
%                     'onepass-reverse-zerophase' zero-phase reverse filter with delay compensation
%                     'onepass-minphase'          minimum-phase converted forward filter (non-linear, only for firws)
%                     'twopass'                   zero-phase forward and reverse filter (default, except for firws)
%                     'twopass-reverse'           zero-phase reverse and forward filter
%                     'twopass-average'           average of the twopass and the twopass-reverse
%   instabilityfix  optional method to deal with filter instabilities
%                     'no'       only detect and give error (default)
%                     'reduce'   reduce the filter order
%                     'split'    split the filter in two lower-order filters, apply sequentially
%   df              optional transition width (only for firws)
%   wintype         optional window type (only for firws), can be
%                     'hamming' (default)    maximum passband deviation 0.0022 [0.22%], stopband attenuation -53dB
%                     'hann'                 maximum passband deviation 0.0063 [0.63%], stopband attenuation -44dB
%                     'blackman'             maximum passband deviation 0.0002 [0.02%], stopband attenuation -74dB
%                     'kaiser'
%   dev             optional max passband deviation/stopband attenuation (only for firws with kaiser window, default = 0.001 [0.1%, -60 dB])
%   plotfiltresp    optional, 'yes' or 'no', plot filter responses (only for firws, default = 'no')
%   usefftfilt      optional, 'yes' or 'no', use fftfilt instead of filter (only for firws, default = 'no')
%
% Note that a one- or two-pass filter has consequences for the strength of the
% filter, i.e. a two-pass filter with the same filter order will attenuate the signal
% twice as strong.
%
% Further note that the filter type 'brickwall' filters in the frequency domain,
% but may have severe issues. For instance, it has the implication that the time
% domain signal is periodic. Another issue pertains to that frequencies are
% not well defined over short time intervals; particularly for low frequencies.
%
% If the data contains NaNs, these will affect the output. With an IIR
% filter, and/or with FFT-filtering, local NaNs will spread to the whole
% time series. With a FIR filter, local NaNs will spread locally, depending
% on the filter order.
%
% See also PREPROC

% Copyright (c) 2003-2022, Robert Oostenveld, Arjen Stolk, Andreas Widmann,
% Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% set the default filter order later
if nargin<4 || isempty(order)
  order = [];
end

% set the default filter type
if nargin<5 || isempty(type)
  type = 'but';
end

% set the default filter direction
if nargin<6 || isempty(dir)
  if strcmp(type, 'firws')
    dir = 'onepass-zerophase';
  else
    dir = 'twopass';
  end
end

% set the default method to deal with filter instabilities
if nargin<7|| isempty(instabilityfix)
  instabilityfix = 'no';
end

% Set default transition width later
if nargin < 8 || isempty(df)
  df = [];
end

% Set default window type
if nargin < 9 || isempty(wintype)
  wintype = 'hamming';
end

% Set default passband deviation/stopband attenuation for Kaiser window
if nargin < 10 || isempty(dev)
  if strcmp(wintype, 'kaiser')
    dev = 0.001;
  else
    dev = [];
  end
end

% Set default passband deviation/stopband attenuation for Kaiser window
if nargin < 11 || isempty(plotfiltresp)
  plotfiltresp = 'no';
end

% Set default filter function
if nargin < 12 || isempty(usefftfilt)
  usefftfilt = false;
else
  % convert to boolean value
  usefftfilt = istrue(usefftfilt);
end

% Filtering does not work on integer data
if ~isa(dat, 'double') && ~isa(dat, 'single')
  dat = cast(dat, 'double');
end

% preprocessing fails on channels that contain NaN
if any(isnan(dat(:)))
  ft_warning('FieldTrip:dataContainsNaN', 'data contains NaN values');
end

% Nyquist frequency
Fn = Fs/2;

% demean the data before filtering
meandat = nanmean(dat,2);
dat = bsxfun(@minus, dat, meandat);

% compute filter coefficients
switch type
  case 'but'
    if isempty(order)
      order = 4;
    end
    [B, A] = butter(order, [min(Fbp)/Fn max(Fbp)/Fn]);

  case 'firws'
    % Input arguments
    if length(Fbp) ~= 2
      ft_error('Two cutoff frequencies required.')
    end

    % Filter order AND transition width set?
    if ~isempty(order) && ~isempty(df)
      ft_warning('firws:dfOverridesN', 'Filter order AND transition width set - transition width setting will override filter order.')
    elseif isempty(order) && isempty(df) % Default transition width heuristic
      df = fir_df(Fbp, Fs);
    end

    % Compute filter order from transition width
    [foo, maxDf] = fir_df(Fbp, Fs); %#ok<ASGLU>
    isOrderLow = false;
    if ~isempty(df)
      if df > maxDf
        ft_error('Transition band too wide. Maximum transition width is %.2f Hz.', maxDf)
      end
      [order, dev] = firwsord(wintype, Fs, df, dev);
    else % Check filter order otherwise
      [df, dev] = invfirwsord(wintype, Fs, order, dev);
      if df > maxDf
        nOpt = firwsord(wintype, Fs, maxDf, dev);
        ft_warning('firws:filterOrderLow', 'Filter order too low. For better results a minimum filter order of %d is recommended. Effective cutoff frequency might deviate from requested cutoff frequency.', nOpt)
        isOrderLow = true;
      end
    end

    % Window
    if strcmp(wintype, 'kaiser')
      beta = kaiserbeta(dev);
      win = windows('kaiser', order + 1, beta);
    else
      win = windows(wintype, order + 1);
    end

    % Impulse response
    Fbp = sort(Fbp);
    B = firws(order, sort(Fbp) / Fn, 'band', win);
    A = 1;

    % Convert to minimum phase
    if strcmp(dir, 'onepass-minphase')
      B = minphaserceps(B);
    end

    % Twopass filtering
    if strncmp(dir, 'twopass', 7)
      pbDev = (dev + 1)^2 - 1;
      sbAtt = 20 * log10(dev^2);
      order = 2 * (length(B) - 1);
      isTwopass = true;
    else
      pbDev = dev;
      sbAtt = 20 * log10(dev);
      order = length(B) - 1;
      isTwopass = false;
    end

    % Reporting
    ft_info once
    ft_info('Bandpass filtering data: %s, order %d, %s-windowed sinc FIR\n', dir, order, wintype);
    if ~isTwopass && ~isOrderLow % Do not report shifted cutoffs
      ft_info('  cutoff (-6 dB) %g Hz and %g Hz\n', Fbp(1), Fbp(2));
      tb = [max([Fbp(1) - df / 2 0]), Fbp(1) + df / 2, Fbp(2) - df / 2, min([Fbp(2) + df / 2 Fn])]; % Transition band edges
      ft_info('  transition width %.1f Hz, stopband 0-%.1f Hz, passband %.1f-%.1f Hz, stopband %.1f-%.0f Hz\n', df, tb, Fn);
    end
    if ~isOrderLow
      ft_info('  maximum passband deviation %.4f (%.2f%%), stopband attenuation %.0f dB\n', pbDev, pbDev * 100, sbAtt);
    end

  case 'fir'
    if isempty(order)
      order = 3*fix(Fs / Fbp(1));
    end
    if order > floor( (size(dat,2) - 1) / 3)
      order=floor(size(dat,2)/3) - 1;
    end
    B = fir1(order, [min(Fbp)/Fn max(Fbp)/Fn]);
    A = 1;

  case 'firls' % from NUTMEG's implementation
    % Deprecated: see bug 2453
    ft_warning('The filter type you requested is not recommended for neural signals, only proceed if you know what you are doing.')
    if isempty(order)
      order = 3*fix(Fs / Fbp(1));
    end
    if order > floor( (size(dat,2) - 1) / 3)
      order=floor(size(dat,2)/3) - 1;
    end
    f = 0:0.001:1;
    if rem(length(f),2)~=0
      f(end)=[];
    end
    z = zeros(1,length(f));
    if(isfinite(min(Fbp)))
      [val,pos1] = min(abs(Fs*f/2 - min(Fbp)));
    else
      [val,pos2] = min(abs(Fs*f/2 - max(Fbp)));
      pos1=pos2;
    end
    if(isfinite(max(Fbp)))
      [val,pos2] = min(abs(Fs*f/2 - max(Fbp)));
    else
      pos2 = length(f);
    end
    z(pos1:pos2) = 1;
    A = 1;
    B = firls(order,f,z); % requires MATLAB signal processing toolbox

  case 'brickwall'
    ix = round((0:Fs) ./ (Fs ./ size(dat,2)));
    ax = ix ./ (size(dat,2)./Fs); % frequency axis, including the other end of the spectrum

    a    = ones(1, size(dat,2));
    fbin1 = nearest(ax, [min(Fbp)    max(Fbp)]);
    fbin2 = nearest(ax, [Fs-max(Fbp) Fs-min(Fbp)]); % same band at the other end of the spectrum

    a(1:(fbin1(1)-1))            = 0;
    a((fbin1(2)+1):(fbin2(1)-1)) = 0;
    a((fbin2(2)+1):end)          = 0;

    f    = fft(dat,[],2);             % FFT
    f    = f.*a(ones(size(dat,1),1),:); % brickwall
    filt = real(ifft(f,[],2));        % iFFT

  otherwise
    ft_error('unsupported filter type "%s"', type);
end

% Plot filter responses
if strcmp(plotfiltresp, 'yes')
  plotfresp(B, A, [], Fs, dir)
end

if ~isequal(type, 'brickwall')
  try
    filt = filter_with_correction(B,A,dat,dir,usefftfilt);
  catch
    switch instabilityfix
      case 'no'
        rethrow(lasterror);
      case 'reduce'
        ft_warning('off','backtrace');
        ft_warning('instability detected - reducing the %dth order filter to an %dth order filter', order, order-1);
        ft_warning('on','backtrace');
        filt = ft_preproc_bandpassfilter(dat,Fs,Fbp,order-1,type,dir,instabilityfix,df,wintype,dev,plotfiltresp,usefftfilt);
      case 'split'
        N1 = ceil(order/2);
        N2 = floor(order/2);
        ft_warning('off','backtrace');
        ft_warning('instability detected - splitting the %dth order filter in a sequential %dth and a %dth order filter', order, N1, N2);
        ft_warning('on','backtrace');
        filt = ft_preproc_bandpassfilter(dat ,Fs,Fbp,N1,type,dir,instabilityfix,df,wintype,dev,plotfiltresp,usefftfilt);
        filt = ft_preproc_bandpassfilter(filt,Fs,Fbp,N2,type,dir,instabilityfix,df,wintype,dev,plotfiltresp,usefftfilt);
      otherwise
        ft_error('incorrect specification of instabilityfix');
    end % switch
  end
end


function [Y,Xpad] = kuwahara(X,WINSZ,progress)
% [Y,Xpad] = KUWAHARA(X[,WINSZ][,progress])
% perform kuwahara nonlinear edge-preserving filtering on an intensity
% image
%
% * If no window size WINSZ is specified, the default is 5.
% * Setting progress to a nonzero value causes KUWAHARA to display
%   the current row it is processing.
%
% Description:
% The Kuwahara filter works on a window divided into 4 overlapping
% subwindows (typically 5x5 pixels, see below). In each subwindow, the mean and
% variance are computed. The output value (located at the center of the
% window) is set to the mean of the subwindow with the smallest variance.
%
%    ( a  a  ab   b  b)
%    ( a  a  ab   b  b)
%    (ac ac abcd bd bd)
%    ( c  c  cd   d  d)
%    ( c  c  cd   d  d)
%
% Notes:
% Image is converted to double format for processing.
%
% References:
% http://www.incx.nec.co.jp/imap-vision/library/wouter/kuwahara.html
%
% Copyright Art Barnes, 2005 artbarnes<at>ieee<dot>org

if nargin >= 3
    verboseFlag = true;
else
    verboseFlag = false;
end

if nargin < 2
    WINSZ = 5;
end

if ~isa(X,'double')
    X = im2double(X);
end

PADDING = floor(WINSZ/2);

Xpad = padarray(X,[PADDING PADDING],'replicate');
[padRows,padCols] = size(Xpad);
Y = zeros(size(X));

nRowIters = length((PADDING+1):(padRows-PADDING));
count = 1;
for i = (PADDING+1):(padRows-PADDING)
    for j = (PADDING+1):(padCols-PADDING)
        % window & subwindows
        W = Xpad((i-PADDING):(i+PADDING),(j-PADDING):(j+PADDING));
        Wnw = W(1:(PADDING+1),1:(PADDING+1));
        Wne = W(1:(PADDING+1),(PADDING+1):WINSZ);
        Wsw = W((PADDING+1):WINSZ,1:(PADDING+1));
        Wse = W((PADDING+1):WINSZ,(PADDING+1):WINSZ);

        % find the variances
        s = var([Wnw(:) Wne(:) Wsw(:) Wse(:)]);
        m = mean([Wnw(:) Wne(:) Wsw(:) Wse(:)]);
        [y,k] = min(s);

        % assign the mean of the subwindow with the least variance to
        % the center pixel
        Y(i,j) = m(k);
    end

    if verboseFlag
        fprintf('Kuwahara: %d/%d\n',count,nRowIters);
        count = count + 1;
    end
end


function [ a, ifact ] = i4vec_red ( n, a, incx )

%*****************************************************************************80
%
%% I4VEC_RED divides out common factors in an I4VEC.
%
%  Discussion:
%
%    If A is a simple vector, then it has dimension N.
%
%    If A is a row of a matrix, then INCX will not be 1, and
%    the actual dimension of A is at least 1+(N-1)*INCX.
%
%    On output, the entries of A have no common factor
%    greater than 1.
%
%    If A is a simple vector, then INCX is 1, and we simply
%    check the first N entries of A.
%
%    If A is a row of a matrix, then INCX will be the number
%    of rows declared in the matrix, in order to allow us to
%    "skip" along the row.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer A(*), the vector to be reduced.
%
%    Input, integer INCX, the distance between successive
%    entries of A that are to be checked.
%
%    Output, integer A(*), the reduced vector.
%
%    Output, integer IFACT, the common factor that was divided out.
%

%
%  Find the smallest nonzero value.
%
  ifact = 0;
  indx = 1;

  for i = 1 : n

    if ( a(indx) ~= 0 )

      if ( ifact == 0 )
        ifact = abs ( a(indx) );
      else
        ifact = min ( ifact, abs ( a(indx) ) );
      end

    end

    indx = indx + incx;

  end

  if ( ifact == 0 )
    return
  end
%
%  Find the greatest common factor of the entire vector.
%
  indx = 1;
  for i = 1 : n
    ifact = i4_gcd ( a(indx), ifact );
    indx = indx + incx;
  end

  if ( ifact == 1 )
    return
  end
%
%  Divide out the common factor.
%
  indx = 1;
  for i = 1 : n
    a(indx) = a(indx) / ifact;
    indx = indx + incx;
  end

  return
end