function qp_h_rwm4_tb
% preconditioned algorithm with the FGM variables x,v,q un-scaled

%% load data

load alg_data_scaling3.mat   % alg_data QP_reform MPC_0s  ! non-scaled matrices, scaling done here
load SaveData_scaledcplex.mat 
%load alg_data_scaling_noprecond.mat   % alg_data QP_reform MPC_0s
%load SaveData_scaling_noprecond.mat
% load alg_data_scaling2.mat
% load SaveData_scaling_KxI.mat
load beta1000.mat


% clear SaveData
% load SaveData.mat
% for no_samp = 1 : 667,
%   if mod(no_samp, 20) == 0, disp(no_samp), end
%   [SaveData(no_samp).U10, iter] =  qp_m_rwm(alg_data,SaveData(no_samp).x0, 10);
%   [SaveData(no_samp).U20, iter] =  qp_m_rwm(alg_data,SaveData(no_samp).x0, 20);
%   [SaveData(no_samp).U50, iter] =  qp_m_rwm(alg_data,SaveData(no_samp).x0, 50);
% end%for
% save SaveData_scaling_noprecond.mat SaveData

% alg_data_scaling3.mat: matrices/vectors are not scaled by E / Einv
alg_data.H = full(alg_data.H);
alg_data.B = full(alg_data.B); 
alg_data.C = full(alg_data.C); 
alg_data.R = full(alg_data.R); 
alg_data.r2 = full(alg_data.r2); 
alg_data.E = full(alg_data.E); 
alg_data.Einv = full(alg_data.Einv); 

% Passed to the solver AND PATCHED FOR MODIFIED NUMERICAL IMPLEMENTATION!!!
% Preconditioned alg but opt variables y,v,x no longer scaled with Einv
% The matrices are not consistent for J calculation!
% Einv2 added for the computation of "test" restart condition
clear alg_data4
alg_data4.H = alg_data.E * alg_data.E * alg_data.H; 
alg_data4.Gb = alg_data.E *alg_data.E * alg_data.Gb; 
alg_data4.Gg = alg_data.E *alg_data.E * alg_data.Gg;  
alg_data4.R = alg_data.R; 
alg_data4.h.Lb = alg_data.h.Lb; 
alg_data4.h.Ub = alg_data.h.Ub; 
alg_data4.beta_vec = beta_vec;
alg_data4.vdim = size(alg_data.H,1);
alg_data4.Einv2 = alg_data.Einv*alg_data.Einv;     % match the original preconditioned alg
%alg_data4.Einv2 = eye(size(alg_data.Einv*alg_data.Einv));    % test
%alg_data4.Einv = alg_data.Einv;   %debug
%alg_data4.E = alg_data.E;   %debug

H = alg_data4.H; 
Gb = alg_data4.Gb; 
Gg = alg_data4.Gg;  
R = alg_data4.R; 
h = alg_data4.h; 
vdim = alg_data4.vdim;
Einv2 = alg_data4.Einv2;
save alg_data4.mat H Gb Gg R h beta_vec vdim Einv2

% reconstruct scaled data (for cost function) - original QPgen preconditioned alg
alg_data0.H = alg_data.E * alg_data.H * alg_data.E; 
alg_data0.Gb = alg_data.E * alg_data.Gb; 
alg_data0.Gg = alg_data.E * alg_data.Gg; 
alg_data0.R = alg_data.R * alg_data.E; 
alg_data0.h.Lb = alg_data.Einv * alg_data.h.Lb; 
alg_data0.h.Ub = alg_data.Einv * alg_data.h.Ub; 
alg_data0.beta_vec = beta_vec;
alg_data0.vdim = size(alg_data.H,1);

alg_data0.Einv2 = eye(size(alg_data.Einv*alg_data.Einv));   % test match
%alg_data0.Einv2 = alg_data.Einv*alg_data.Einv; 


%% test 

tic
for no_samp = 1 : 667, 
  if mod(no_samp, 20) == 0, disp(no_samp), end
%  [SaveData(no_samp).U10m, iter] =  qp_m_rwm1(alg_data1,SaveData(no_samp).x0, 10);
  [SaveData(no_samp).U20m, iter] =  qp_h_rwm4(SaveData(no_samp).x0, 20);
 %%%%% [SaveData(no_samp).U20m, iter] =  qp_m_rwm1(alg_data0,SaveData(no_samp).x0, 20);
%  [SaveData(no_samp).U50m, iter] =  qp_m_rwm1(alg_data1,SaveData(no_samp).x0, 50);
%   [SaveData(no_samp).U10m, iter] = qp_mex(SaveData(no_samp).x0, 10);
%   [SaveData(no_samp).U20m, iter] = qp_mex(SaveData(no_samp).x0, 20);
%   [SaveData(no_samp).U50m, iter] = qp_mex(SaveData(no_samp).x0, 50);
end%for  
toc
  
%% evaluate

dimU = size(SaveData(1).U, 1);
Urange = sqrt(1/dimU) * (QP_reform.QP.h.Ub - QP_reform.QP.h.Lb);

invR = inv(alg_data0.R);    %original QPgen preconditioned alg
for no_samp = 1:667
  %  if mod(no_samp, 20) == 0, disp(no_samp), end
  
  % U norm difference from QPgen C code, double
  % normalized by U range and the number of signals (MSE style)
  %  nnorm20(no_samp) = norm( (SaveData(no_samp).U20m - SaveData(no_samp).U20) ./ Urange, 2);
  nnorm20(no_samp) = norm( (double(SaveData(no_samp).U20m) - SaveData(no_samp).U20) ./ Urange, 2);
  
  % cost J difference from QPgen C code, double
  x_0 = SaveData(no_samp).x0;
  sol = SaveData(no_samp).U20;
  SaveData(no_samp).J20 = 0.5*(invR*sol)'*alg_data0.H*(invR*sol) + ...
    (alg_data0.Gb*x_0 + alg_data0.Gg)'*(invR*sol);
  sol = double(SaveData(no_samp).U20m);
  SaveData(no_samp).J20m = 0.5*(invR*sol)'*alg_data0.H*(invR*sol) + ...
    (alg_data0.Gb*x_0 + alg_data0.Gg)'*(invR*sol);
  diffJ20(no_samp) = abs(SaveData(no_samp).J20 - double(SaveData(no_samp).J20m));
end%for

invR = inv(alg_data.R);  % compare with un-scaled cost (alg_data_scaling3.mat) 
                         % (ad4 is not consistent!)
for no_samp = 1:667
  % cost J difference from QPgen C code, double
  x_0 = SaveData(no_samp).x0;
  sol = SaveData(no_samp).U20;
  SaveData(no_samp).J20a = 0.5*(invR*sol)'*alg_data.H*(invR*sol) + ...
    (alg_data.Gb*x_0 + alg_data.Gg)'*(invR*sol);
  sol = double(SaveData(no_samp).U20m);
  SaveData(no_samp).J20ma = 0.5*(invR*sol)'*alg_data.H*(invR*sol) + ...
    (alg_data.Gb*x_0 + alg_data.Gg)'*(invR*sol);
  diffJ20a(no_samp) = abs(SaveData(no_samp).J20a - double(SaveData(no_samp).J20ma));
end%for

% disp(['nnorm max 10 20 50: ', num2str(max(nnorm10)),' ',num2str(max(nnorm20)),' ',num2str(max(nnorm50))])
% disp(['nnorm avg 10 20 50: ', num2str(mean(nnorm10)),' ',num2str(mean(nnorm20)),' ',num2str(mean(nnorm50))])
% disp(['diffJ max 10 20 50: ', num2str(max(diffJ10)),' ',num2str(max(diffJ20)),' ',num2str(max(diffJ50))])
% disp(['diffJ avg 10 20 50: ', num2str(mean(diffJ10)),' ',num2str(mean(diffJ20)),' ',num2str(mean(diffJ50))])
disp(['nnorm max 20: ', num2str(max(nnorm20)),'  diffJad0 max 20: ', num2str(max(diffJ20))])
disp(['nnorm max 20: ', num2str(max(nnorm20)),'  diffJad max 20: ', num2str(max(diffJ20a))])





