% initialize your output
gray_img = rgb2gray(img);

% parameters
num_angles = 8;
num_bins = 4;
num_samples = num_bins * num_bins;
patchSize = 16;

% initialize descriptors to zero
descriptors = zeros(size(keypoints,1), num_samples * num_angles);

% for all patches
for i=1:size(keypoints,1)

    H = fspecial('gaussian',10, keypoints(i,3));
    scaled_img = imfilter(gray_img,H,'replicate');

    G_X = [-1 0 1];
    G_Y = [-1 0 1]';
    I_X = filter2(G_X, scaled_img, 'same'); % vertical edges
    I_Y = filter2(G_Y, scaled_img, 'same'); % horizontal edges
    I_mag = sqrt(I_X.^2 + I_Y.^2); % gradient magnitude
    I_theta = mod(atan2(I_Y,I_X) - keypoints(i,4),2*pi); %rotation invariance!
    I_theta(find(isnan(I_theta))) = 0; 
%    I_mag_result = I_mag;
%    I_theta_result = I_theta;
    angle_step = 2 * pi / num_angles;
    angles = 0:angle_step:2*pi;

    % find coordinates of sample points (bin centers)
    sample_x_t = round(keypoints(i,1));
    sample_y_t = round(keypoints(i,2));
    
    % find window of pixels that contributes to this descriptor
    x_lo = sample_x_t - patchSize/2;
    x_hi = sample_x_t + patchSize/2 - 1;
    y_lo = sample_y_t - patchSize/2;
    y_hi = sample_y_t + patchSize/2 - 1;

    try
    patch_mag = I_mag(y_lo:y_hi,x_lo:x_hi) .* fspecial('gaussian',patchSize, patchSize/2);
    patch_ang = I_theta(y_lo:y_hi,x_lo:x_hi);
    catch
       continue; %skip the point that is too close to the image boundary
    end
%    patch_mag_result = patch_mag;
%    patch_ang_result = patch_ang;
    feature = [];
    
    for x = 1:4:patchSize-3
        for y = 1:4:patchSize-3
            smallhist = zeros(1,8); %the histgram for one 4x4 region
            
            %find the correct angle bin and increment it by the magnitude
            %loop around all points inside each 4x4 section
            for x_off = 0:3
                for y_off = 0:3
                    if patch_mag(y+y_off,x+x_off) > 0
                        %If the magnitude is zero, then there will be no
                        %addition to the orientation histogram
                        for a = 1:8 %across the angles
                            if a == 8 %the angle bin must be the last one
                                smallhist(a) = smallhist(a) + patch_mag(y+y_off,x+x_off); 
                                break;
                            end
                            if patch_ang(y+y_off,x+x_off)<angles(a+1)
                                smallhist(a) = smallhist(a) + patch_mag(y+y_off,x+x_off);
                                break; 
                            end
                        end
                    end
                end
            end
            
            feature = [feature smallhist];
        end
    end
    %The computed feature is now the completed descriptor for that
    %particular interest point!
    
    descriptors(i,:) = feature;
end

%Normalization step:

% find indices of descriptors to be normalized (those whose norm is larger than 1)
tmp = sqrt(sum(descriptors.^2, 2));
normalize_ind = find(tmp > 1);

descriptors_norm = descriptors(normalize_ind,:);
descriptors_norm = descriptors_norm ./ repmat(tmp(normalize_ind,:), [1 size(descriptors,2)]);

% suppress large gradients
descriptors_norm(find(descriptors_norm > 0.2)) = 0.2;

% finally, renormalize to unit length
tmp = sqrt(sum(descriptors_norm.^2, 2));
descriptors_norm = descriptors_norm ./ repmat(tmp, [1 size(descriptors,2)]);

descriptors(normalize_ind,:) = descriptors_norm;

