% results = computePointOrientations(results)
%
%  Computes the principle orientation at all of the feature-points.
%
%  This function drops the points near the edge of the image, and
%  duplicates point if there are multiple primary orientations at a given
%  point.  So the index of a point before and after calling this function
%  will be different!
%
%  Input:
%     results.s -- The number of scale-space levels per octave (not the same as the number of levels
%                  in a stack)
%     results.pyramidBlurred{ind} -- The blurred pyramid.
%     results.points
%        results.points(ind).iPyr -- the i-index in the pyramid stack where the feature was detected.
%        results.points(ind).jPyr -- the j-index in the pyramid stack where the feature was detected.
%        results.points(ind).kPyr -- the k-index in the pyramid stack where the feature was detected.
%        results.points(ind).indLevel -- the level in the pyramid where the feature was detected
%           You could find the original point again using
%               results.pyramidBlurred{indLevel}(iPyr,jPyr,kPyr)
%
%  Output: 
%     results.points               -- this structure is edited to
%                                     add/remove points. See the comments
%                                     above
%        results.points(ind).theta -- the angle, in radians, that the
%                                     image patch should be rotated CCW
%                                     around the interest-point
%                                     to produce the desired bounding-box
%                                     for the SIFT feature.

% Author: Josiah Yoder
% Term: Summer 2014
% Course: CS498 - Computer Vision
% Assignment: Lab 4 - SIFT feature description

function results = computePointOrientations(results)

if ~isfield(results,'dIdx')
    error('msoe:illegalArgument','results must contain a field called dIdx, which is a pyramid of dIdx image-stacks')
end
if ~isfield(results,'dIdy')
    error('msoe:illegalArgument','results must contain a field called dIdy, which is a pyramid of dIdy image-stacks')
end
if ~isfield(results,'points')
    error('msoe:illegalArgument','results must contain a field called points, which is a struct array')
end
if isempty(results.points)
    warning('msoe:unexpectedArgument','No points provided.');
end
if isempty(results.s)
    error('msoe:illegalArgument','results must contain a field called s, which is the number of scales per octave (not the number of planes in an octave stack.)');
end
if ~isempty(results.points) && ~isfield(results.points(1),'iPyr')
    error('msoe:illegalArgument','results.points must contain the fields iPyr, jPyr, kPyr, and indLevel. (At least iPyr is missing)');
end

if ~isempty(results.points) && ~isfield(results.points(1),'jPyr')
    error('msoe:illegalArgument','results.points must contain the fields iPyr, jPyr, kPyr, and indLevel. (At least jPyr is missing)');
end

if ~isempty(results.points) && ~isfield(results.points(1),'kPyr') 
    error('msoe:illegalArgument','results.points must contain the fields iPyr, jPyr, kPyr, and indLevel. (At least kPyr is missing)');
end

if ~isempty(results.points) && ~isfield(results.points(1),'indLevel')
    error('msoe:illegalArgument','results.points must contain the fields iPyr, jPyr, kPyr, and indLevel. (At least indLevel is missing)');
end

for indLevel = 1:length(results.dIdx)
    dIdx = results.dIdx{indLevel};
    dIdy = results.dIdy{indLevel};
    
    m = sqrt(dIdx.^2 + dIdy.^2);
    theta = atan2(-dIdx,dIdy); % Note: This is not the typical way of using atan2.
                               % My goal here is to compute the angle
                               % between the maximum slope direction
                               % and a vertical line, measured CCW
                               % to the slope. Thus "rise" is -dIdx, and
                               % "run" is dIdy.                               
                               
    pyramidM{indLevel} = m;
    pyramidTheta{indLevel} = theta;
end

newPoints = [];
for indPoint = 1:length(results.points)
    point = results.points(indPoint);
    iPyr = point.iPyr;
    jPyr = point.jPyr;
    kPyr = point.kPyr;
    indLevel = point.indLevel;
    
    halfSize = ceil(3*1.5*2^((kPyr-1)/results.s));
    if iPyr+halfSize <= size(results.dIdx{indLevel},1) && ...
        iPyr-halfSize >= 1 && ...
        jPyr+halfSize <= size(results.dIdx{indLevel},2) && ...
        jPyr-halfSize >= 1

        w = pyramidM{indLevel}(iPyr-halfSize:iPyr+halfSize,jPyr-halfSize:jPyr+halfSize,kPyr);
        sampTheta = pyramidTheta{indLevel}(iPyr-halfSize:iPyr+halfSize,jPyr-halfSize:jPyr+halfSize,kPyr);
        gw = createGaussianWeighting(halfSize,kPyr,results.s);
        w = gw.*w;

        h = histAngles(sampTheta,w);
        indMaxes = find(h>max(h)*0.8);
        if isempty(indMaxes)
            warning('msoe:unexpectedLostPoint',['No maxes found for feature at ' ...
                   'indLevel: ' num2str(indLevel) ...
                   'iPyr: ' num2str(iPyr) ...
                   'jPyr: ' num2str(jPyr) ...
                   'kPyr: ' num2str(kPyr) ...
                   '.  This is surprising.  Could the patch be in a featureless area of the image?']);
        end
        for indMax = indMaxes(:)';
            point.theta = thetaFromValidDirection(indMax);
            newPoints = [newPoints point];
        end
    end
end
results.points = newPoints;

end

% Creating the weighting for __determining orientation__.
%
% Creates a square matrix with gaussian weights for determing orientation.
%
% halfSize -- half the size of the window to create along one side.
%             (techically, the full size is 2*halfSize+1)
% k - level in stack
% s - number of scale-space levels per octave
%
% See createGaussianWeightingForFeatureSample
function result = createGaussianWeighting(halfSize,k,s)
    oneSide = pdf('norm',(-halfSize:halfSize),0,1.5*2^((k-1)/s));
    result = oneSide' * oneSide;
end

%
% Compute an orientation histogram with the desired number of bins
%
function h = histAngles(a,w,bins)
if nargin < 3
    bins = 36;
end
a = a(:);
w = w(:);
h = zeros(1,bins);
for i = 1:length(a)
    destInd = computeAngleIndex(a(i),bins);
    h(destInd) = h(destInd) + w(i);
end
end

%
% Compute the index needed for a given angle...
%
function destInd = computeAngleIndex(a,bins)
    destInd = ceil(a/pi*(bins/2)+(bins/2)+0.5);
    if destInd == bins+1
        destInd = 1;
    end
end

%
% Find the angle theta when given a valid direction.
% (May give wrong results if the direction is invalid.)
%
function theta = thetaFromValidDirection(direction,numAngles)
    if nargin < 2
        numAngles = 36;
    end

    theta = ((direction-1)*2*pi/numAngles)-pi;
end
