clc;
clear;
disp('***************** NEW RUN *********************');
total = tic;


% ******* Initialize voting parameters **************************************
rmax = 9;		%maximum radius of the cell
ang_deg = 15.1;		%half the angular range of the voting area
ang = ang_deg * pi / 180;
iter = 1;	%number of voting iterations
t0 = 1.2; %threshold color
sigma = [4, 4, 2];
% t = 10;
d_ang= ang / (iter);

% ******** Testing parameters ******************************************
p = [100, 50, 100] ;
ps = [50, 50, 25] - 1;
% p = [100, 100, 100] ;
% ps = [300, 300, 150] - 1;

volfile = 'img\nissl-rat.vol';
fid = fopen(volfile); % open the file that include the image
S = fread(fid, 3, 'int32');
X = S(1);
Y = S(2);
Z = S(3);

% load the VOL data into a 2D matrix
I = fread(fid,[X Y*Z], 'uint8');
fclose(fid);

%change this to a 3D matrix
I = single(reshape(I, [X, Y, Z]));

% invert the intensity
I = 255 - I;

%perform a gaussian blur
I_blur = gauss_blur3d(I, sigma);

% compute the gradient
[Igrad_x, Igrad_y, Igrad_z] = gradient(I_blur);

%%crop out a small subregion of I
Isub_x = Igrad_x(p(1):p(1)+ps(1), p(2):p(2)+ps(2), p(3):p(3)+ps(3));
Isub_y = Igrad_y(p(1):p(1)+ps(1), p(2):p(2)+ps(2), p(3):p(3)+ps(3));
Isub_z = Igrad_z(p(1):p(1)+ps(1), p(2):p(2)+ps(2), p(3):p(3)+ps(3));

%calculate the gradient magnitude
Imag = sqrt(Isub_x .^ 2 + Isub_y .^ 2 + Isub_z .^2);
I_theta = acos(Isub_z./Imag);
I_phi = atan(Isub_y./Isub_x);


%set a threshold for the gradient magnitude
It = Imag > t0;

%
% thresholding
It_x = size(It,1);
It_y = size(It,2);
It_z = size(It,3);
%
It(1:rmax, :, :) = 0;
It(It_x - rmax:It_x, :,:) = 0;
It(:, 1:rmax, :) = 0;
It(:, It_y - rmax:It_y,:) = 0;
It(:, :, 1:rmax) = 0;
It(:,:, It_z - rmax:It_z) = 0;

%
[Itx,Ity,Itz] = (ind2sub(size(It),find(It)));
Itx = single(Itx);
Ity = single(Ity);
Itz = single(Itz);
nV = nnz(It);


% create a meshgrid describing coordinates relative to the voter position
range = single(-rmax):single(rmax);
[mx, my, mz] = (meshgrid(range, range, range));
m_mag = sqrt(mx.^2 + my.^2 + mz.^2);

% create a mask for the voting area
M_dist =  mx.^2 + my.^2 + mz.^2 < rmax^2;
M_theta = acos(mz./m_mag);
M_phi = atan(my./mx);
M_phi (my ==0 & mx ==0) = 0;
pxPerRow = single(size(It,1));
pxPerCol = single(size(It,2));
validPoints = single(zeros(nV,1)); 

% g_v_prime = zeros(nV, ceil(rmax^2*ang/3)); 
g_v_prime = single(zeros(nV, (rmax^3))); 



disp('first part done.');
%% vote
tic;
  for itr = 1 : iter
     
    Ivote = single(zeros(size(It)));
     for v = 1: 1

%
        % coordinates of the current voter
        vx = Itx(v);
        vy = Ity(v);
        vz = Itz(v);
%
        vtheta= I_theta(vx,vy,vz);
        vphi = I_phi(vx,vy,vz);
        vmag = Imag(vx,vy,vz);
%         ang_diff = single(zeros([2*rmax+1 2*rmax+1 2*rmax+1]));
%         for i=1: 2*rmax +1
%             for j=1: 2*rmax+1
%                 for k = 1:2*rmax+1
%                     ang_diff(i,j,k) = acos(dot([Isub_x(vx,vy,vz) Isub_y(vx,vy,vz) Isub_z(vx,vy,vz)], [mx(i,j,k) my(i,j,k) mz(i,j,k)])/(vmag*norm([mx(i,j,k) my(i,j,k) mz(i,j,k)])));
%                 end
%             end
%         end
        ang_diff = acos(((sin(M_theta).*sin(vtheta)).*cos(vphi - M_phi)) + cos(M_theta).*cos(vtheta));
        M_diff = min(2*pi - ang_diff,ang_diff)<ang;
%         M_diff = abs(ang_diff)<ang;
        %compute the final mask
        M = single(M_dist .* M_diff);
%
        % get the coordinates of each pixel in the final mask
        [vx_prime,vy_prime,vz_prime] = ind2sub(size(M),find(M));

        %transform the local coordinates of the pixels in the voting region
        %to the global coordinates of the image

        npts =  numel(vx_prime);
        validPoints(v) = npts; 

        g_v_prime(v,1:npts) = vx + (single(vx_prime) - single(rmax + 1)) + (vy + (single(vy_prime) - single(rmax + 1))-1).*single(pxPerRow) + (vz + (single(vz_prime) - single(rmax + 1))-1).*single(pxPerRow)*single(pxPerCol); 

        for n=1: npts
        %             
              Ivote( g_v_prime(v,n)) = Ivote( single(g_v_prime(v,n))) + vmag;
        end

    end
    
  
    t_v1 = toc;   
    disp(['voting done.  time =',num2str(t_v1)]);
    
    
    % update the voting direction
    tic;
    for v = 1: nV
       % coordinates of the current voter
       vx = single(Itx(v));
       vy = single(Ity(v));
       vz = single(Itz(v));

       %get the local value of the voting image
        local_Ivote = Ivote(g_v_prime(v,1:validPoints(v)));

        %find the index of the maximum value
        [~, local_max_idx] = max(local_Ivote);

        %convert this into a global subscript
         gz = ceil(g_v_prime(v,local_max_idx)/(pxPerRow*pxPerCol)) ;
         gy = ceil((g_v_prime(v,local_max_idx)-(gz - 1)*pxPerRow*pxPerCol)/pxPerRow);
         gx = g_v_prime(v,local_max_idx)- pxPerRow*((gy - 1) + (gz - 1) * pxPerCol);

         %compute the vector from the voter position to this position
         up_vx = gx - vx;
         up_vy = gy - vy;
         up_vz = gz - vz;
         I_theta(vx,vy,vz) = acos(up_vz/sqrt(up_vx^2 + up_vy^2 + up_vz^2));
         I_phi(vx,vy,vz) = atan(up_vy/up_vx);
    end
    
    tdir1 = toc;
    display (['updating dir done.  time = ', num2str(tdir1)]);
    ang = ang - d_ang;
 end
 

%%
out1(:,:,:,1) = mat2gray( I(p(1):p(1)+ps(1), p(2):p(2)+ps(2), p(3):p(3)+ps(3)));
out1(:,:,:,2) = mat2gray(Ivote);
out1(:,:,:,3) =  mat2gray(I(p(1):p(1)+ps(1), p(2):p(2)+ps(2), p(3):p(3)+ps(3)));

subplot(3, 3, 1),
imagesc( squeeze( out1(:, :, ceil(size(out1, 3)/2),:) ) );

subplot(3, 3, 2),
imagesc( squeeze( out1(:, ceil(size(out1, 2)/2), :,:) ) );

subplot(3, 3, 3),
imagesc( squeeze( out1(ceil(size(out1, 1)/2), :, :,:) ) );

t = 10;
cell_center = Ivote;
cell_center(cell_center<t) =0;
cell_center(cell_center>=t) = 255;
% cell_center = imregionalmax(cell_center);

out2(:,:,:,1) = mat2gray( I(p(1):p(1)+ps(1), p(2):p(2)+ps(2), p(3):p(3)+ps(3)));
out2(:,:,:,2) = mat2gray(cell_center);
out2(:,:,:,3) =  mat2gray(I(p(1):p(1)+ps(1), p(2):p(2)+ps(2), p(3):p(3)+ps(3)));


subplot(3, 3, 4),
imagesc( squeeze( out2(:, :, ceil(size(out2, 3)/2),:) ) );

subplot(3, 3, 5),
imagesc( squeeze( out2(:, ceil(size(out2, 2)/2), :,:) ) );

subplot(3, 3, 6),
imagesc( squeeze( out2(ceil(size(out2, 1)/2), :, :,:) ) );

colormap(gray);
subplot(3, 3, 7),
imagesc( squeeze( Imag(:, :, ceil(size(Imag, 3)/2),:) ) );

subplot(3, 3, 8),
imagesc( squeeze( Imag(:, ceil(size(Imag, 2)/2), :,:) ) );

subplot(3, 3, 9),
imagesc( squeeze( Imag(ceil(size(Imag, 1)/2), :, :,:) ) );