add octave testfiles to git

11 files changed, 809 insertions, 0 deletions

diff --git a/tests/aux/calculate_omega.m b/tests/aux/calculate_omega.m
new file mode 100644
index 0000000..972ab35
--- /dev/null
+++ b/tests/aux/calculate_omega.m
@@ -0,0 +1,37 @@
+more off;
+clear;
+
+rand('state', 21353242);
+
+n = 5;
+m = 3;
+K = 3;
+
+p = 1.213;
+
+y = [   2   1   3   2   3]';
+
+for i=1:numel(y)
+  fprintf('data->y[%i] = %.16f;\n', i-1, y(i));
+end
+
+H = 2*rand(n, m);
+
+for i=1:size(H, 1)
+  for j=1:size(H, 2)
+    fprintf('matrix_set(A, %i, %i, %i, %.16f);\n', size(H, 2), i-1, j-1, H(i, j));
+  end
+end
+
+R = zeros(n, K);
+I = eye(K);
+for i=1:n
+  R(i, :) = I(y(i, :), :);
+end
+R = ~logical(R);
+
+omega = (1/p)*(sum((H.^p).*R,2)).^(1/p - 1);
+
+for i=1:n
+  fprintf('mu_assert(fabs(gensvm_calculate_omega(model, %i) -\n%.16f) < 1e-14,\n"Incorrect omega at %i");\n', i-1, omega(i), i-1);
+end
\ No newline at end of file
diff --git a/tests/aux/get_Avalue_update_B.m b/tests/aux/get_Avalue_update_B.m
new file mode 100644
index 0000000..1c2eb6d
--- /dev/null
+++ b/tests/aux/get_Avalue_update_B.m
@@ -0,0 +1,43 @@
+clear;
+more off;
+rand('state', 21831);
+
+n = 5;
+K = 3;
+m = 3;
+
+y = [2 1 3 2 3]';
+
+R = zeros(n, K);
+I = eye(K);
+for i=1:n
+  R(i, :) = I(y(i, :), :);
+end
+R = ~logical(R);
+
+omega = [
+  0.7394076262220608
+  0.7294526264247443
+  0.6802499471888741
+  0.6886792032441273
+  0.8695329737474283
+];
+
+
+H = zeros(5, 3);
+
+H(1, 1) = 0.8465725800087526;
+H(1, 2) = 1.2876921677680249;
+H(1, 3) = 1.0338561593991831;
+H(2, 1) = 1.1891038526621391;
+H(2, 2) = 0.4034192031226095;
+H(2, 3) = 0.0;
+H(3, 1) = 0.5;
+H(3, 2) = 0.0;
+H(3, 3) = 1.1;
+H(4, 1) = 0.0;
+H(4, 2) = 0.0;
+H(4, 3) = 0.8189336598535132;
+H(5, 1) = 0.6164203008844277;
+H(5, 2) = 0.2456444285093894;
+H(5, 3) = 0.8184193969741095;
diff --git a/tests/aux/test_eigendecomp.m b/tests/aux/test_eigendecomp.m
new file mode 100644
index 0000000..921f44b
--- /dev/null
+++ b/tests/aux/test_eigendecomp.m
@@ -0,0 +1,43 @@
+clear;
+rand('state', 123456);
+more off; # for Octave
+
+n = 10;
+
+A = rand(n, n);
+K = A'*A;
+
+
+for ii=1:n
+  for jj=1:n
+    fprintf("matrix_set(K, n, %i, %i, %.16f);\n", ii-1, jj-1, K(ii, jj));
+  end
+end
+
+[P, Sigma] = eig(K);
+
+
+eigenvalues = diag(Sigma);
+ratios = eigenvalues ./ eigenvalues(end, end);
+
+cutoff = 1e-2;
+
+realP = fliplr(P(:, ratios > cutoff));
+realSigma = flipud(eigenvalues(ratios > cutoff));
+
+r = sum(ratios > cutoff);
+
+assert(r == size(realP, 2));
+
+fprintf('\n');
+for ii=1:n
+  for jj=1:r
+    fprintf("mu_assert(fabs(fabs(matrix_get(P, r, %i, %i)) -\nfabs(%.16f)) < eps,\n\"Incorrect P at %i, %i\");\n", ii-1, jj-1, realP(ii, jj), ii-1, jj-1);
+  end
+end
+
+fprintf('\n');
+for jj=1:r
+  fprintf("mu_assert(fabs(Sigma[%i] - %.16f) < eps,\n\"Incorrect Sigma at %i\");\n", jj-1, realSigma(jj), jj-1);
+end
+
diff --git a/tests/aux/test_kernel_compute.m b/tests/aux/test_kernel_compute.m
new file mode 100644
index 0000000..7cc9388
--- /dev/null
+++ b/tests/aux/test_kernel_compute.m
@@ -0,0 +1,59 @@
+
+function kernel_tests(kerneltype)
+
+rand('state', 123456);
+more off;
+
+n = 10;
+m = 3;
+
+X = rand(n, m);
+Z = [ones(n, 1), X];
+
+for ii=1:n
+  for jj=1:m+1
+    fprintf("matrix_set(data->RAW, data->m+1, %i, %i, %.16f);\n", ii-1, jj-1, Z(ii, jj));
+  end
+end
+
+
+K = zeros(n, n);
+if strcmp(kerneltype, 'poly')
+  # Polynomial kernel
+  # (gamma * <x_1, x_2> + c)^d
+  gamma = 1.5;
+  c = 3.0;
+  d = 1.78;
+  
+  for ii=1:n
+    for jj=1:n
+      K(ii, jj) = (gamma * (X(ii, :) * X(jj, :)') + c)^d;
+    end
+  end
+elseif strcmp(kerneltype, 'rbf')
+  # RBF kernel
+  # exp(-gamma * norm(x1 - x2)^2)
+  gamma = 0.348
+  for ii=1:n
+    for jj=1:n
+      K(ii, jj) = exp(-gamma * sum((X(ii, :) - X(jj, :)).^2));
+    end
+  end
+elseif strcmp(kerneltype, 'sigmoid')
+  # Sigmoid kernel
+  # tanh(gamma * <x_1, x_2> + c)
+  gamma = 1.23;
+  c = 1.6;
+  for ii=1:n
+    for jj=1:n
+      K(ii, jj) = tanh(gamma * (X(ii, :) * X(jj, :)') + c);
+    end
+  end
+end
+
+fprintf('\n');
+for ii=1:n
+  for jj=1:n
+    fprintf("mu_assert(fabs(matrix_get(K, data->n, %i, %i) -\n %.16f) < eps,\n\"Incorrect K at %i, %i\");\n", ii-1, jj-1, K(ii, jj), ii-1, jj-1);
+  end
+end
\ No newline at end of file
diff --git a/tests/aux/test_kernel_cross.m b/tests/aux/test_kernel_cross.m
new file mode 100644
index 0000000..5777afd
--- /dev/null
+++ b/tests/aux/test_kernel_cross.m
@@ -0,0 +1,70 @@
+
+function test_kernel_cross(kerneltype)
+
+clc;
+rand('state', 123456);
+more off;
+
+n_1 = 10;
+n_2 = 5;
+m = 3;
+
+X_1 = rand(n_1, m);
+Z_1 = [ones(n_1, 1), X_1];
+X_2 = rand(n_2, m);
+Z_2 = [ones(n_2, 1), X_2];
+
+for ii=1:n_1
+  for jj=1:m+1
+    fprintf("matrix_set(data_1->RAW, data_1->m+1, %i, %i, %.16f);\n", ii-1, jj-1, Z_1(ii, jj));
+  end
+end
+
+fprintf('\n');
+for ii=1:n_2
+  for jj=1:m+1
+    fprintf("matrix_set(data_2->RAW, data_2->m+1, %i, %i, %.16f);\n", ii-1, jj-1, Z_2(ii, jj));
+  end
+end
+
+
+K = zeros(n_2, n_1);
+if strcmp(kerneltype, 'poly')
+  # Polynomial kernel
+  # (gamma * <x_1, x_2> + c)^d
+  gamma = 1.5;
+  c = 3.0;
+  d = 1.78;
+  
+  for ii=1:n_2
+    for jj=1:n_1
+      K(ii, jj) = (gamma * (X_2(ii, :) * X_1(jj, :)') + c)^d;
+    end
+  end
+elseif strcmp(kerneltype, 'rbf')
+  # RBF kernel
+  # exp(-gamma * norm(x1 - x2)^2)
+  gamma = 0.348
+  for ii=1:n_2
+    for jj=1:n_1
+      K(ii, jj) = exp(-gamma * sum((X_2(ii, :) - X_1(jj, :)).^2));
+    end
+  end
+elseif strcmp(kerneltype, 'sigmoid')
+  # Sigmoid kernel
+  # tanh(gamma * <x_1, x_2> + c)
+  gamma = 1.23;
+  c = 1.6;
+  for ii=1:n_2
+    for jj=1:n_1
+      K(ii, jj) = tanh(gamma * (X_2(ii, :) * X_1(jj, :)') + c);
+    end
+  end
+end
+
+fprintf('\n');
+for ii=1:n_2
+  for jj=1:n_1
+    fprintf("mu_assert(fabs(matrix_get(K2, data_1->n, %i, %i) -\n %.16f) < eps,\n\"Incorrect K2 at %i, %i\");\n", ii-1, jj-1, K(ii, jj), ii-1, jj-1);
+  end
+end
\ No newline at end of file
diff --git a/tests/aux/test_kernel_post.m b/tests/aux/test_kernel_post.m
new file mode 100644
index 0000000..9839566
--- /dev/null
+++ b/tests/aux/test_kernel_post.m
@@ -0,0 +1,62 @@
+
+function test_kernel_post()
+  more off;
+  rand('state', 123456);
+  
+  n_tr = 10;
+  n_tt = 8;
+  m = 5;
+  K = 3;
+  
+  X_tr = rand(n_tr, m);
+  Z_tr = [ones(n_tr, 1), X_tr];
+  set_matrix(Z_tr, "train->RAW", "train->r+1");
+  fprintf("train->Z = train->RAW;\n");
+  
+  X_tt = rand(n_tt, m);
+  Z_tt = [ones(n_tt, 1), X_tt];
+  set_matrix(Z_tt, "test->RAW", "test->m+1");
+  fprintf("test->Z = test->RAW;\n");
+  
+  gamma = 1.132;
+  K2 = zeros(n_tt, n_tr);
+  for ii=1:n_tt
+    for jj=1:n_tr
+      K2(ii, jj) = exp(-gamma * sum((X_tt(ii, :) - X_tr(jj, :)).^2));
+    end
+  end
+  
+  M = X_tr;
+  
+  Sigma = rand(m, 1);
+  set_matrix(Sigma, "train->Sigma", "1");
+  
+  N = K2 * M * diag(Sigma)^(-2);
+  test_Z = [ones(size(N, 1), 1), N];
+  
+  assert_matrix(test_Z, "test->Z", "test->r+1");
+  
+  
+  
+
+end
+
+function set_matrix(A, name, cols)
+  for ii=1:size(A, 1)
+    for jj=1:size(A, 2)
+      fprintf("matrix_set(%s, %s, %i, %i, %.16f);\n", name, cols, ii-1, jj-1, A(ii, jj));
+    end
+  end
+  fprintf("\n");
+end
+
+function assert_matrix(A, name, cols)
+  for ii=1:size(A, 1)
+    for jj=1:size(A, 2)
+          fprintf(["mu_assert(fabs(matrix_get(%s, %s, %i, %i) -\n%.16f) <", ...
+                   " eps,\n\"Incorrect %s at %i, %i\");\n"], name, cols, ...
+                   ii-1, jj-1, A(ii, jj), name, ii-1, jj-1);
+    end
+  end
+  fprintf("\n");
+end
\ No newline at end of file
diff --git a/tests/aux/test_kernel_pre.m b/tests/aux/test_kernel_pre.m
new file mode 100644
index 0000000..a1c8337
--- /dev/null
+++ b/tests/aux/test_kernel_pre.m
@@ -0,0 +1,101 @@
+function test_kernel_pre()
+  
+  kerneltype = 'rbf';
+  rand('state', 123456);
+  n = 10;
+  m = 5;
+  cutoff = 5e-3;
+  
+  X = rand(n, m);
+  Z = [ones(n, 1), X];
+  
+  set_matrix(Z, "data->Z", "data->m+1");
+  
+  K = zeros(n, n);
+  if strcmp(kerneltype, 'poly')
+    # Polynomial kernel
+    # (gamma * <x_1, x_2> + c)^d
+    gamma = 1.5;
+    c = 3.0;
+    d = 1.78;
+    
+    for ii=1:n
+      for jj=1:n
+        K(ii, jj) = (gamma * (X(ii, :) * X(jj, :)') + c)^d;
+      end
+    end
+  elseif strcmp(kerneltype, 'rbf')
+    # RBF kernel
+    # exp(-gamma * norm(x1 - x2)^2)
+    gamma = 0.348
+    for ii=1:n
+      for jj=1:n
+        K(ii, jj) = exp(-gamma * sum((X(ii, :) - X(jj, :)).^2));
+      end
+    end
+  elseif strcmp(kerneltype, 'sigmoid')
+    # Sigmoid kernel
+    # tanh(gamma * <x_1, x_2> + c)
+    gamma = 1.23;
+    c = 1.6;
+    for ii=1:n
+      for jj=1:n
+        K(ii, jj) = tanh(gamma * (X(ii, :) * X(jj, :)') + c);
+      end
+    end
+  end
+  
+  K(1, 2)
+
+  [P, Sigma] = eig(K);
+
+  eigenvalues = diag(Sigma);
+  ratios = eigenvalues ./ eigenvalues(end, end);
+ 
+  realP = fliplr(P(:, ratios > cutoff));
+  realSigma = flipud(eigenvalues(ratios > cutoff));
+  
+  assert_matrix(realSigma, "data->Sigma", "1");
+  
+  r = sum(ratios > cutoff);
+  fprintf("mu_assert(data->r == %i);\n", r);
+  
+  M = realP * diag(realSigma);
+  
+  newZ = [ones(n, 1) M];
+  assert_matrix_abs(newZ, "data->Z", "data->r+1");
+  
+  assert_matrix(Z, "data->RAW", "data->m+1");
+  
+end
+
+function set_matrix(A, name, cols)
+  for ii=1:size(A, 1)
+    for jj=1:size(A, 2)
+      fprintf("matrix_set(%s, %s, %i, %i, %.16f);\n", name, cols, ii-1, jj-1, A(ii, jj));
+    end
+  end
+  fprintf("\n");
+end
+
+function assert_matrix(A, name, cols)
+  for ii=1:size(A, 1)
+    for jj=1:size(A, 2)
+          fprintf(["mu_assert(fabs(matrix_get(%s, %s, %i, %i) -\n%.16f) <", ...
+                   " eps,\n\"Incorrect %s at %i, %i\");\n"], name, cols, ...
+                   ii-1, jj-1, A(ii, jj), name, ii-1, jj-1);
+    end
+  end
+  fprintf("\n");
+end
+
+function assert_matrix_abs(A, name, cols)
+  for ii=1:size(A, 1)
+    for jj=1:size(A, 2)
+          fprintf(["mu_assert(fabs(fabs(matrix_get(%s, %s, %i, %i)) -\nfabs(%.16f)) <", ...
+                   " eps,\n\"Incorrect %s at %i, %i\");\n"], name, cols, ...
+                   ii-1, jj-1, A(ii, jj), name, ii-1, jj-1);
+    end
+  end
+  fprintf("\n");
+end
\ No newline at end of file
diff --git a/tests/aux/test_optimize.m b/tests/aux/test_optimize.m
new file mode 100644
index 0000000..ecd10a3
--- /dev/null
+++ b/tests/aux/test_optimize.m
@@ -0,0 +1,44 @@
+function [V] = test_optimize()
+
+clear;
+more off;
+rand('state', 902183);
+
+n = 8;
+m = 3;
+K = 4;
+
+X = rand(n, m);
+set_matrix(X);
+
+y = [2 1 3 2 3 2 4 1];
+
+set_vector(y);
+
+p = 1.2143;
+kappa = 0.90298;
+lambda = 0.00219038;
+epsilon = 1e-15;
+
+rho = rand(n, 1);
+set_vector(rho);
+
+[W, t] = msvmmaj(X, y, rho, p, kappa, lambda, epsilon);
+
+V = [t'; W];
+
+end
+
+function set_matrix(A)
+  for i=1:size(A, 1)
+    for j=1:size(A, 2)
+      fprintf('matrix_set(A, %i, %i, %i, %.16f);\n', size(A, 2), i-1, j-1, A(i, j));
+    end
+  end
+end
+
+function set_vector(a)
+  for i=1:numel(a)
+    fprintf('A[%i] = %.16f;\n', i-1, a(i));
+  end
+end
\ No newline at end of file
diff --git a/tests/aux/test_testfactor.m b/tests/aux/test_testfactor.m
new file mode 100644
index 0000000..1d6b014
--- /dev/null
+++ b/tests/aux/test_testfactor.m
@@ -0,0 +1,44 @@
+function test_testfactor()
+  rand('state', 123456);
+  more off; # Octave
+  
+  train_n = 10;
+  train_r = 3;
+  test_n = 5;
+  
+  
+  P = rand(train_n, train_r);
+  K2 = rand(test_n, train_n);
+  Sigma = diag(rand(1, train_r));
+  
+  train_Z = [ones(train_n, 1), P];
+  
+  set_matrix(train_Z, "train->Z", "train->r+1");
+  set_matrix(diag(Sigma), "Sigma", "1");
+  set_matrix(K2, "K2", "train->n");
+  
+  test_factor = K2 * P * Sigma^(-2);
+  test_Z = [ones(test_n, 1) test_factor];
+  assert_matrix(test->Z, "test->Z", "test->r+1");
+  
+end
+
+function set_matrix(A, name, cols)
+  for ii=1:size(A, 1)
+    for jj=1:size(A, 2)
+      fprintf("matrix_set(%s, %s, %i, %i, %.16f);\n", name, cols, ii-1, jj-1, A(ii, jj));
+    end
+  end
+  fprintf("\n");
+end
+
+function assert_matrix(A, name, cols)
+  for ii=1:size(A, 1)
+    for jj=1:size(A, 2)
+          fprintf(["mu_assert(fabs(matrix_get(%s, %s, %i, %i) -\n%.16f) <", ...
+                   " eps,\n\"Incorrect %s at %i, %i\");\n"], name, cols, ...
+                   ii-1, jj-1, A(ii, jj), name, ii-1, jj-1);
+    end
+  end
+  fprintf("\n");
+end
diff --git a/tests/aux/test_trainfactor.m b/tests/aux/test_trainfactor.m
new file mode 100644
index 0000000..581a074
--- /dev/null
+++ b/tests/aux/test_trainfactor.m
@@ -0,0 +1,34 @@
+clc;
+more off; # Octave
+rand('state', 123456);
+
+n = 10;
+m = 5;
+K = 3;
+r = 7;
+
+P = rand(n, r);
+Sigma = rand(r, 1);
+
+for ii=1:n
+  for jj=1:r
+    fprintf("matrix_set(P, r, %i, %i, %.16f);\n", ii-1, jj-1, P(ii, jj));
+  end
+end
+
+fprintf('\n');
+for ii=1:r
+  fprintf("Sigma[%i] = %.16f;\n", ii-1, Sigma(ii));
+end
+
+Z = [ones(n, 1), P*diag(Sigma)];
+
+for ii=1:n
+  for jj=1:r+1
+    fprintf("mu_assert(fabs(matrix_get(Z, r+1, %i, %i) -\n%.16f) < eps,\n\"Incorrect Z at %i, %i\");\n", ii-1, jj-1, Z(ii, jj), ii-1, jj-1);
+  end
+end
+
+
+
+  
\ No newline at end of file
diff --git a/tests/aux/update_test.m b/tests/aux/update_test.m
new file mode 100644
index 0000000..ab430e1
--- /dev/null
+++ b/tests/aux/update_test.m
@@ -0,0 +1,272 @@
+function newV = update_test
+  rand('state', 2190382);
+
+  more off;
+  n = 8;
+  m = 3;
+  K = 3;
+  
+  p = 1.1;
+  lambda = 0.123;
+  kappa = 0.5;
+  
+  y = [   2   1   3   2   3   3   1   2]';
+  
+  
+  U = SimplexGen(K);
+  UU = zeros(n, K-1, K);
+  for jj=1:K
+    UU(:, :, jj) = U(y, :) - U(jj*ones(n, 1), :);
+  end
+  
+  R = zeros(n, K);
+  I = eye(K);
+  for i=1:n
+    R(i, :) = I(y(i, :), :);
+  end
+  R = ~logical(R);
+  
+  Z = [ones(n, 1), -1 + 2 * rand(n, m)];
+  V = -1 + 2 * rand(m+1, K-1);
+  rho = ones(n, 1);
+  
+  V
+  
+  newV = getUpdate(Z, y, rho, p, kappa, lambda, UU, R, V, false);
+end
+
+function set_vector(a)
+  for i=1:numel(a)
+    fprintf('A[%i] = %.16f;\n', i-1, a(i));
+  end
+end
+
+function set_matrix(A)
+  for i=1:size(A, 1)
+    for j=1:size(A, 2)
+      fprintf('matrix_set(A, %i, %i, %i, %.16f);\n', size(A, 2), i-1, j-1, A(i, j));
+    end
+  end
+end
+
+% From the original Matlab implementation
+function [newV, oldL] = getUpdate(Z, y, rho, p, kappa, lambda, UU, R, V, debug)
+  % Initialize constants
+  [n, m] = size(Z);
+  m = m - 1;
+  [~, K] = size(R);
+  
+  % Calculate initial errors
+  ZV = Z*V;
+  q = zeros(n, K);
+  for jj = 1:K
+      q(:, jj) = sum(ZV.*UU(:,:,jj), 2);
+  end
+  
+  % Calculate Huber hinge errors
+  G1 = (q <= -kappa);
+  G2 = (q <= 1) & (~G1);
+  H = (1 - q - (kappa+1)/2).*G1 + (1/(2*kappa + 2))*((1 - q).^2).*G2;
+  
+  % Split objects in Category 1 and 2
+  C = sum((H.*R)>0, 2)<=1;
+  C1i = find(C>0); % for some reason this is faster than C==1
+  C2i = find(C<1); % for some reason this is faster than C==0
+  
+  % We are going to do the calculations separately for each category (1,2)
+  % so we separate all important variables as well.
+  Q1 = q(C1i, :);
+  Q2 = q(C2i, :);
+  
+  R1 = R(C1i, :);
+  R2 = R(C2i, :);
+  
+  P1 = rho(C1i, :);
+  P2 = rho(C2i, :);
+  
+  y1 = y(C1i, :);
+  
+  H1 = H(C1i, :);
+  H2 = H(C2i, :);
+  
+  ZV1 = ZV(C1i, :);
+  ZV2 = ZV(C2i, :);
+  
+  UU1 = UU(C1i, :, :);
+  UU2 = UU(C2i, :, :);
+  
+  n1 = length(y1);
+  n2 = n - n1;
+  
+  %% First create all matrices for the Case 1 objects.
+  if debug
+      % Check the first majorization of the Case 1 objects.
+      TT1 = sum((H1.^p).*R1,2).^(1/p);
+      TT2 = sum(H1.*R1,2);
+      if abs(sum(TT1) - sum(TT2))/n1 > eps
+          fprintf('\tFirst Case 1: diff = %15.16f\n', abs(sum(TT1) - sum(TT2))/n1);
+      end
+  end
+  
+  % First do the majorization for the Case 1 objects (p = 1 in Huber maj.)
+  G1 = (Q1 <=-kappa);
+  G2 = (Q1 <= 1) & (~G1);
+  G3 = ~(G1|G2);
+  
+  % calculate dummy variables
+  Phi = 1 - Q1 - (kappa + 1)/2;
+  Psi = (1 - Q1)/sqrt(2*kappa + 2);
+  iPhi = 1./Phi;
+  
+  a1 = 1/4*iPhi.*(G1 - G3) + (1/(2*kappa + 2))*G2;
+  a1(isnan(a1)) = 0; % necessary because Inf*0 = NaN and we need 0
+  
+  b1 = a1.*Q1 + 1/2*G1 + ((Psi.^2)./(1 - Q1)).*G2;
+  
+  B1 = zeros(n1, K-1);
+  for jj=1:K
+      B1 = B1 + ((b1(:, jj) - (a1(:, jj).*Q1(:, jj)))*ones(1, K-1)).*UU1(:,:,jj);
+  end
+  
+  if debug
+      % constant terms in quadratic majorization
+      c1 = a1.*(Q1.^2) + (1-kappa)/2*G1 + ...
+          ((Psi.^2).*(1 + 2.*Q1./(1 - Q1))).*G2;
+  
+      TT1 = a1.*(Q1.^2) - 2*b1.*Q1 + c1;
+      TT2 = H1;
+      D = sum(sum(abs(TT1 - TT2)))/n1;
+      if D > eps
+          fprintf('\tSecond Case 1: diff = %15.16f\n', D);
+      end
+  
+      % Case 1 constant terms in total majorization
+      Gamma1 = 1/n * sum(P1.*sum(c1.*R1,2));
+      Gamma1 = Gamma1 + 1/n * sum(P1.*sum(ZV1.^2,2).*sum(a1.*R1,2));
+      Gamma1 = Gamma1 - 1/n * sum(P1.*sum(a1.*(Q1.^2).*R1,2));
+  
+      clear c1 TT1 TT2 D
+  end
+  
+  %% Now create all matrices for the Case 2 objects.
+  % We can now safely delete a number of matrices from memory
+  clear G1 G2 G3 Phi Psi iPhi b1
+  
+  G1a = (Q2 <= (p+kappa-1)/(p-2));
+  G2a = (Q2 <= 1)&(~G1a);
+  G3a = ~(G1a|G2a);
+  
+  G1b = (Q2 <= -kappa);
+  G2b = (Q2 <= 1) & (~G1b);
+  G3b = ~(G1b|G2b);
+  
+  Phi = 1 - Q2 - (kappa+1)/2;
+  Psi = (1 - Q2)/sqrt(2*kappa + 2);
+  if p~=2
+      Chi = (p*Q2 + kappa - 1)/(p - 2);
+  end
+  
+  omega = (1/p)*(sum((H2.^p).*R2,2)).^(1/p - 1);
+  
+  if debug
+      % First majorization test (p-th root)
+      TT1 = sum((H2.^p).*R2,2).^(1/p);
+      TT2 = omega.*sum((H2.^p).*R2,2) + (1 - 1/p)*(sum((H2.^p).*R2,2)).^(1/p);
+      D = sum(abs(TT1 - TT2))/n2;
+      if D > eps
+          fprintf('\tFirst Case 2: diff = %15.16f\n', D);
+      end
+  end
+  
+  % Some parameters are different when p = 2, we recognize this here.
+  if p~=2
+      a2 = (1/4 * p^2 * Phi.^(p-2)).*G1a + ...
+          (1/4 * p * (2*p - 1) * ((kappa+1)/2)^(p-2)).*G2a + ...
+          (1/4 * p^2 * (p*Phi/(p-2)).^(p-2)).*G3a;
+      a2(isnan(a2)) = 0; % We need Inf*0 = 0.
+  else
+      a2 = 3/2*ones(n2, K);
+  end
+  
+  b2 = (a2.*Q2 + 1/2*p*(Phi.^(p-1))).*G1b + ...
+      (a2.*Q2 + p*(Psi.^(2*p))./(1 - Q2)).*G2b;
+  if p~=2
+      b2 = b2 + (a2.*Chi + 1/2*p*(p*Phi/(p-2)).^(p-1)).*G3b;
+  else
+      b2 = b2 + 3/2*Q2.*G3b;
+  end
+  
+  B2 = zeros(n2, K-1);
+  for jj=1:K
+      B2 = B2 + ((b2(:, jj) - (a2(:, jj).*Q2(:, jj)))*ones(1, K-1)).*UU2(:,:,jj);
+  end
+  
+  if debug
+      c2 = (a2.*(Q2.^2) + Phi.^p + p*Q2.*(Phi.^(p-1))).*G1b + ...
+          (a2.*(Q2.^2) + (Psi.^(2*p)).*(1 + (2*p*Q2)./(1 - Q2))).*G2b;
+      if p~=2
+          c2 = c2 + (a2.*(Chi.^2) + p*Chi.*(p*Phi/(p - 2)).^(p-1) + (p*Phi/(p-2)).^p).*G3b;
+      else
+          c2 = c2 + 3/2*(Q2.^2).*G3b;
+      end
+  
+      TT1 = a2.*(Q2.^2) - 2*b2.*Q2 + c2;
+      TT2 = H2.^p;
+      D = sum(sum(abs(TT1 - TT2)))/n2;
+      if D>eps
+          fprintf('\tSecond Case 2: diff = %15.16f\n', D);
+      end
+  
+      % Case 2 constant terms in majorization
+      Gamma2 = 1/n * (1 - 1/p) * sum(P2.*(sum((H2.^p).*R2,2).^(1/p)));
+      Gamma2 = Gamma2 + 1/n * sum(P2.*omega.*sum(c2.*R2,2));
+      Gamma2 = Gamma2 + 1/n * sum(P2.*omega.*sum(ZV2.^2,2).*sum(a2.*R2,2));
+      Gamma2 = Gamma2 - 1/n * sum(P2.*omega.*sum((Q2.^2).*a2,2));
+  end
+  
+  %% Collect the two classes in a single matrix and calculate update
+  A(C1i, :) = P1.*sum(a1.*R1,2);
+  A(C2i, :) = P2.*omega.*sum(a2.*R2,2);
+  
+  B(C1i, :) = (P1*ones(1, K-1)).*B1;
+  B(C2i, :) = ((P2.*omega)*ones(1, K-1)).*B2;
+  
+  A = 1/n*A;
+  B = 1/n*B;
+  
+  J = eye(m+1); J(1,1) = 0;
+  
+  ZAZ = Z'*((A*ones(1, m+1)).*Z);
+  newV = (ZAZ + lambda*J)\(ZAZ*V + Z'*B);
+  %% temp
+  lhs = ZAZ + lambda*J;
+  rhs = ZAZ*V + Z'*B;
+  fprintf("lhs:\n");
+  disp(lhs);
+  fprintf("rhs:\n");
+  disp(rhs);
+  fprintf("newV:\n");
+  disp(newV);
+  %% endtemp
+  
+  if debug
+      oldL = trace((V - 2*V)'*Z'*diag(A)*Z*V) - 2*trace(B'*Z*V) + Gamma1 + ...
+          Gamma2 + lambda*trace(V'*J*V);
+  else
+      oldL = 0;
+  end
+
+end
+
+function U = SimplexGen(K)
+  U = zeros(K, K-1);
+  for ii=1:K
+      for jj=1:K-1
+          if ii<=jj
+              U(ii,jj) = -1/sqrt(2*(jj^2 + jj));
+          elseif ii==jj+1
+              U(ii,jj) = jj/sqrt(2*(jj^2 + jj));
+          end
+      end
+  end
+end
\ No newline at end of file