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


% ******* Initialize voting parameters **************************************
rmax = 9;		%maximum radius of the cell
phi_deg = 25.1;		%half the angular range of the voting area
phi = phi_deg * pi / 180;
iter = 5;	%number of voting iterations
t0 = 1.2; %threshold color
sigma = [4, 4, 2];
final_t = 75;


% ******** Testing parameters ******************************************
p = [50, 20, 50] ;
ps = [100, 100, 50] - 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 = 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);

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


subplot(3, 3, 1),
imshow( squeeze( I(:, :, ceil(size(I, 3)/2)) ) ./ 255 );
%colormap(gray);

subplot(3, 3, 2),
imshow( squeeze( I(:, ceil(size(I, 2)/2), :) ) ./ 255 );

subplot(3, 3, 3),
imshow( squeeze( I(ceil(size(I, 1)/2), :, :) ) ./ 255 );

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

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

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

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

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

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

%
% 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;
S(:, 1:rmax, :) = 0;
S(:, It_y - rmax:It_y,:) = 0;
S(:, :, 1:rmax) = 0;
S(:,:, It_z - rmax:It_z) = 0;

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

nV = nnz(It);


% create a meshgrid describing coordinates relative to the voter position
range = -rmax: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;

pxPerRow = size(I_blur,1);
pxPerCol = size(I_blur,2);
validPoints = zeros(nV,1); 

g_v_prime = zeros(nV, ceil(rmax^2*phi/3)); 

%%

disp('done.');
%% vote
for itr = 1 : iter
    Ivote = zeros(size(Iblur));
    for v = 1: nV
       % coordinates of the current voter
       vx = Itx(v);
       vy = Ity(v);
       vz = Itz(v);

       vgrad = [Isub_x(vx,vy,vz), Isub_y(vx,vy,vz), Isub_z(vx,vy,vz)];
       %determine the gradient magnitude at the current voter location
       vmag = Imag(vx,vy,vz);
       ang_diff = 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
                   a  = [mx(i,j,k), my(i,j,k), mz(i,j,k)];
                   c = dot(vgrad, a);
                   ang_diff(i,j,k) = acos(c/(m_mag(i,j,k)* vmag ));
               end
           end
       end

        M_diff = min(2*pi - ang_diff,ang_diff)<phi;
        %compute the final mask
        M = (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 + (vx_prime - (rmax + 1)) + (vy + (vy_prime - (rmax + 1))-1).*pxPerRow + (vz + (vz_prime - (rmax + 1))-1).*pxPerRow*pxPerCol; 

        for n=1: npts
    %             
                  Ivote( g_v_prime(v,n)) = Ivote( g_v_prime(v,n)) + vmag;
        end
        
    end
    
disp('voting done.');
    %% update the voting direction

    for v = 1: nV
       % coordinates of the current voter
       vx = sx(v);
       vy = sy(v);
       vz = sz(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
         new_vx = gx - vx;
         new_vy = gy - vy;
         new_vz = gz - vz;
         Igrad_x(vx,vy,vz) = new_vx;
         Igrad_y(vx,vy,vz) = new_vy;
         Igrad_z(vx,vy,vz) = new_vz;
    end
end
cell_center = Ivote;
fido = fopen('out\outImg.vol', 'w+');
fwrite(fido, cell_center);
fclose(fido);
t1 = toc