fprintf('Testing consistency between MATLAB and C++ implementations\n');

points1 = h3d_test_shape('pyramid');

R = rot_z(deg2rad(135)) * rot_y(deg2rad(25));

points2 = R * points1;

fprintf('Testing point generation and rotations ...');

p1loaded = load('saved_pyramid_points');
p2loaded = load('saved_pyramid_points2');


m = [max(max(abs(p1loaded'-points1))) max(max(abs(p2loaded'-points2)))];
check_result(max(m));

p = h3d_params;

fprintf('Testing Hough spectrum computation ... ');

base1 = h3d_create_struct(p.angular_cell_size_deg, p.rho_cell_size, -p.max_norm, p.max_norm);
ht1 = h3d_compute_cube_ht(p,base1,points1);

htcomputed = ht1.cube_hs;

htloaded = zeros(size(ht1.cube_hs));

htloaded(1,:,:) = load('saved_spectrum1');
htloaded(2,:,:) = load('saved_spectrum2');
htloaded(3,:,:) = load('saved_spectrum3');
htloaded(4,:,:) = load('saved_spectrum4');
htloaded(5,:,:) = load('saved_spectrum5');
htloaded(6,:,:) = load('saved_spectrum6');

check_result(max(max(max(abs(htloaded-htcomputed)))));

fprintf('Testing upsampling ... ');

ht1u = h3d_upsample(ht1, p.upsample);
htupsample_computed = ht1u.cube_hs;

htupsample_loaded = zeros(size(htupsample_computed));
htupsample_loaded(1,:,:) = load('saved_upsample1');
htupsample_loaded(2,:,:) = load('saved_upsample2');
htupsample_loaded(3,:,:) = load('saved_upsample3');
htupsample_loaded(4,:,:) = load('saved_upsample4');
htupsample_loaded(5,:,:) = load('saved_upsample5');
htupsample_loaded(6,:,:) = load('saved_upsample6');

check_result(max(max(max(abs(htupsample_loaded-htupsample_computed)))));

fprintf('Testing local maxima detection ... ');

res = zeros(1,6);

for face = 1:6

    f = squeeze(ht1u.cube_hs(face,:,:));
    [coords,val] = h3d_matrix_local_maxima(f);
    coords = coords;
    peaks = [val' coords'];
    loaded_peaks = load(sprintf('saved_maxima_face_%d',face));
    loaded_peaks = [loaded_peaks(:,1) loaded_peaks(:,3:4)];
    res(face)= max(max(abs(loaded_peaks-peaks)));  
end

check_result(max(res));

fprintf('Testing detection of maxima in Hough spectrum ... ');

computed_hspeaks = h3d_find_hs_peak(ht1u)';	
loaded_hspeaks = load('saved_hs_peaks');

check_result(max(max(abs(computed_hspeaks-loaded_hspeaks))));

fprintf('Testing coordinates projection on cylinder ... ');

computed_cyl = h3d_project_hs_on_cylinder(ht1u,[0.6973 0.7098 0.996]',[0 0.1390 -0.9903]',p.cyl_num_alpha_steps,p.cyl_num_beta_steps,2.6180,1,2);

loaded_cyl = load('saved_cylinder');

check_result(max(max(abs(computed_cyl-loaded_cyl))));
     

fprintf('Testing rotation guessing ...\n');

base2 = h3d_create_struct(p.angular_cell_size_deg, p.rho_cell_size, -p.max_norm, p.max_norm);
ht2 = h3d_compute_cube_ht(p, base2, points2);

[Rs,values] = h3d_guess_rotation(p,ht1,ht2);

loaded_rotations = load('saved_rotation_hypothesis');

check_result(max(abs(loaded_rotations'-values)));