%_______________________________________________________________________
% I'm not sure where this ciruclar correlation algorithm came from.  I
% found it on the internet sometime during late summer of '04.  Nothing
% too formal was presented but I have never found any other analysis or
% implementation done on it.  So, here it is.  Works wonderfully.  If you
% can find where this method is described, please let me know so I can
% properly cite it. ~ Isaac Gerg
%
% This script runs the circular correlation algorithm.
% Inpur Parameters:
%   1. radii - The radii which you wish you search over.
%   2. g - granularity of template.  This is the number of points you want
%   to have in your template.
%   3. base_radius - For really fast searching, you can generate the
%   'signals' for the image ones, and then compare mutiplate templates
%   against them. This is the radius you want the image 'signals' to be
%   created at.  A range somewhere in the middle of your search is probably
%   sufficient
%_______________________________________________________________________
close all; clear;
warning off MATLAB:divideByZero
set(0,'DefaultFigureRenderer','zbuffer');

%Parameters
    radius = 15:16;
    g = 128;
    base_radius = 10;
    img = imread('aerial.bmp');
    template = imread('aerial_template.bmp');

    %fftw('measure', g);    
    
template = double(rgb2gray(template));
img = double(rgb2gray(img));
minR = min(radius);
templateSize = floor(size(template)/2);

% Template modificaitons
    template = imresize(template, 1.56, 'bicubic');
    template = imrotate(template, 226, 'bilinear');
    template = template ./ max(max(template));    
    imwrite(template, 'template.png');   
    %template = imnoise(template, 'gaussian');  
    img = img ./ max(max(img));
    noise = zeros(size(img,1), size(img, 2));
    %noise = imnoise(noise, 'gaussian', 0, 0.1);
    %noise = noise - mean2(noise);
    noise = imnoise(noise, 'salt & pepper');
    
% Compute SNR of image.    
    np = mean2(fft(noise).^2);
    sp = mean2(fft(img).^2);
    img = img + noise;
    SNR = 10*log10(sp/np);
    disp(sprintf('SNR: %f dB', SNR))

% Preallocate memory
    output = 0.001*ones(size(img,1), size(img, 2));
    tmp = 0.001*ones(size(img,1), size(img, 2));

% Generate template 'signals' at different radii.
tic	
templates = [];
for r=radius
	% Construct 'signal' of template.
	template_signal = [];
	t = [];
	center = floor(size(template) ./ 2);
    m = template;
    m(center(1),:) = 0;
    m(:,center(2)) = 0;
	for theta=0:(2*pi/g):(2*pi-(1/g))
        pt = center + [r*cos(theta)  r*sin(theta)];    
        % Do billinear interpolation
        x=pt(1);
        y=pt(2);
        d = floor(x); c = floor(y); a = x - d;  b = y - c;
        newSignalValue = (1-a)*(1-b)*(template(d,c)) + (a)*(1-b)*(template(d+1, c)) + (b)*(1-a)*(template(d, c+1)) + (a)*(b)*(template(d+1, c+1));
        template_signal = [template_signal newSignalValue];
	end
    templates(r - minR + 1, :) = template_signal;
end

% Generate image 'signals' at each point.
base_signals = ones(g, prod([ length(base_radius+1:(size(img, 1)-base_radius-2)) , length( base_radius+1:(size(img, 2)-base_radius-2))]));
count = 1;
for x=base_radius+1:(size(img, 1)-base_radius-2)
    for y=base_radius+1:(size(img, 2)-base_radius-2)
        image_signal = [];
        center = [x y];
        for theta=0:(2*pi/g):(2*pi-(1/g))
            pt = center + [base_radius*cos(theta)  base_radius*sin(theta)];
            % Linear Interpolation
            x1=pt(1);
            y1=pt(2);
            d = floor(x1); c = floor(y1); a = x1 - d;  b = y1 - c;
            newSignalValue = (1-a)*(1-b)*(img(d,c)) + (a)*(1-b)*(img(d+1, c)) + (b)*(1-a)*(img(d, c+1)) + (a)*(b)*(img(d+1, c+1));	
            image_signal = [image_signal; newSignalValue];
        end
        base_signals(:, count) = image_signal;
        count = count + 1;
    end
end
templateCreationTime = toc;
disp(sprintf('Template creation time: %f', templateCreationTime))

tic
% Do cross correlation at each radius across whole image.
sizeX = length(base_radius+1:(size(img, 1)-base_radius-2));
sizeY = length(base_radius+1:(size(img, 2)-base_radius-2));
resultsTable = zeros(length(radius), 5);
for r = radius
    for i = 1:length(base_signals)                     
        [a,b] = cxcorr(base_signals(:,i)', templates(r-minR+1,:));
        a2 = base_signals(:,i)';
        a1 = templates(r-minR+1,:);
        d = floor(i/sizeY) + 1 + base_radius+1;
        c = mod(i-1, sizeY) + 1 + base_radius+1;
        output(d, c, r-minR + 1) = max(b);   
    end
    
    % Display results.
    tmp = img;
    [Q, y] =max(max(output(:,:,r-minR + 1)));
    resultsTable(r-min(radius) + 1, 3) = Q;
    [Q, x] =max(output(:,y, r-minR + 1));
    resultsTable(r-min(radius) + 1, 4) = x;
    resultsTable(r-min(radius) + 1, 5) = y;
    % Draw crosshairs and target box on image.
    tmp(:, y) = 1;
    tmp(x,:) = 1;
    if ((x-templateSize(1)) > 0 & (y-templateSize(2)) > 0)
        tmp((x-templateSize(1)) : (x+templateSize(1)), y-templateSize(2)) = 1;
        tmp((x-templateSize(1)) : (x+templateSize(1)), y+templateSize(2)) = 1;
        tmp(x-templateSize(1), y-templateSize(2):y+templateSize(2)) = 1;
        tmp(x+templateSize(1), y-templateSize(2):y+templateSize(2)) = 1;
    end
    % Display results.
    %figure; imagesc(tmp); colormap(gray);
    imwrite(tmp, sprintf('image_%d.png', r));
        %output(x,y,r-minR+1) = 0.99;
    tmp = exp(output(:,:,r-minR + 1).*100);
    tmp = tmp ./ max(max(tmp));
    tmp = 1- tmp;
    imwrite(tmp, sprintf('correlation_%d.png', r));  
    %figure; imagesc(tmp);
    
    i =  ((x-1-base_radius-1)*sizeY) + (y-base_radius-1);
    [a,b] = cxcorr(base_signals(:,i)', templates(r-minR + 1,:));
    [a,b] = max(b);
    disp(sprintf('Radius: %d   Angle: %f   Confidence: %f', r, (1-(b/g)) * 360, a));
end
searchTime = toc;
disp(sprintf('Search Time: %f', searchTime));

    
    