Finding the proper image shift from phase correlation peak location

I have developed a script in MATLAB which performs a phase correlation of two images, and successfully produces a delta peak. I'm confused on a foolproof method to extracting the peak location as a shift vector. I have tried the following check, however it breaks in some situations:

if X>=1 && X<N/2 || X<1
    dx=-X;
    else
        dx=-X+N+1;
end
 

if Y>=1 && Y<N/2 || Y<1
    dy=-Y+1;
    else
        dy=-Y+N+1;
end

This website describes a bit more background information, but it is non-specific about how to get the shift vector.

Here is the script I use to perform the correlation so you could check:

clear;clc;close all

%WHEN PHASE CORR IMAGE SHOWS USE BRACKETS TO ZOOM, AFTER CLICKING PEAK HIT
%ENTER

a = imread('cameraman.tif'); %load image
b = imtranslate(a,[33 30]); %shift image for example
imcell=cell(1,2);
imcell{1}=a;
imcell{2}=b;
[m,n] = size(a);
xcenter = n/2;
ycenter = m/2;
sigma = n*.65; %choose window termination point

%create window function

for i=1:m
    for j=1:n
        r = sqrt( (j-xcenter)^2 + (i-ycenter)^2);
        if r<=sigma
            g(i,j) = .5 + .5*cos((pi*r)/sigma); %hanning
        else
            g(i,j) = 0;
        end 
    end
end

g=(g/max(max(g)));

fwa = im2uint8(g.*im2double(a));
fwb = im2uint8(g.*im2double(b));

N = round(1.5*length(a)); %choose a multiple of 16 greater than m,n.
m0=round((N-m)/2);
n0=round((N-n)/2);
fca = zeros(N,N);
fcb = zeros(N,N); %make big blank square image 

% center images in blank square

for i=1:N
    for j=1:N 
        if (j>n0 && j<(n0+n-1) && i>m0 && i<(m0+m-1))
             fca(i,j) = fwa(i-m0,j-n0);
             fcb(i,j) = fwb(i-m0,j-n0);
        else
            fca(i,j) = 0;
            fcb(i,j) = 0;
        end
    end
end

imcell=cell(1,2);
imcell{1}=fca;
imcell{2}=fcb;

%% Correlate
Ga = fft2(fca);
Gb = fft2(fcb);
Aga = abs(Ga);
Agb = abs(Gb);
p=.0001*max(Aga,Agb);
q=.0001*max(Aga,Agb);

 
%Rs = (Ga.*conj(Gb))./sqrt((Ga)^2 + (conj(Gb))^2); %normalized cross power spectrum

Rs = (Ga.*conj(Gb))./((Aga+p) + (Agb+q)); %seminormalized cross power spectrum
[etamax,numax] = size(Rs);

%parameters of weighted highpasslowpass filter

r1 = .4;
sigma1 = .2;
r2 = 0.9;
sigma2 = .25;
 
%high pass weight function
for eta=1:etamax
    for nu=1:numax
       p = ((eta-N/2)^2 + (nu-N/2)^2);      
       if 4/N^2 * p < (r1-sigma1)^2
           Hr1s1(eta,nu) = 0;     
       elseif  (r1-sigma1)^2 <= 4/N^2 * p && 4/N^2 * p<r1^2
           Hr1s1(eta,nu) = .5+.5*cos(pi*(r1-(2/N)*sqrt(p))/sigma1);
       else 
            Hr1s1(eta,nu) = 1;
       end    
    end
end
    
%low pass weight function
for eta=1:etamax
    for nu=1:numax
       p = ((eta-N/2)^2 + (nu-N/2)^2);      
       if 4/N^2 * p < r2^2
           Hr2s2(eta,nu) = 1;     
       elseif  r2^2 <= 4/N^2 * p && 4/N^2 * p<(r2+sigma2)^2
           Hr2s2(eta,nu) = .5+.5*cos(pi*(r2-(2/N)*sqrt(p))/sigma2);
       else 
            Hr2s2(eta,nu) = 0;
       end    
    end
end
 

Hr2s2r1s1 = Hr2s2.*Hr1s1;
Rsfilt = Hr2s2r1s1.*Rs;
PCfilt = ifft2(Rsfilt);
PC = ifft2(Rs);

B = imshow(PCfilt,[]);
X = []; Y = [];

while 0<1
    [x,y,b] = ginput(1); 
    if isempty(b)
        break;
    elseif b==91
        ax = axis; width=ax(2)-ax(1); height=ax(4)-ax(3);
        axis([x-width/2 x+width/2 y-height/2 y+height/2]);
        zoom(1/2);
    elseif b==93
        ax = axis; width=ax(2)-ax(1); height=ax(4)-ax(3);
        axis([x-width/2 x+width/2 y-height/2 y+height/2]);
        zoom(2);    
    else
        X=[X;x];
        Y=[Y;y];
    end
end
 
close all

X;
Y;

if X>=1 && X<N/2 || X<1
    dx=-X;
    else
        dx=-X+N+1;
end
 

if Y>=1 && Y<N/2 || Y<1
    dy=-Y+1;
    else
        dy=-Y+N+1;
end 

dx %output shifts
dy