% Created by: Dima Al Damen
% June 21st 2007
% last modified: July 20th 2007

% note: this is the working code while edgeCorrelationICP does not work
% properly. This is why it was deleted.

% This function is responsible for aligning two boundaries of the person
% and using ICP - Iternative Closest Point. It uses a downloaded function
% from the net (icp.m).

% It's though important to notice that this implementation ignores the
% rotation found by ICP and only uses the translation.

% uses functions: - cleanBlob
%                 - getBoundaryOnly
%                 - icp

function [medians, footLoc] = edgeCorrelationICP_f (setNo, beginFrame, endFrame, bbFile, newMedianFile, rescaleFactor, params)

   % parameters
   personWidth = params.Results.personWidth;
   personHeight = params.Results.personHeight;
   backgroundColour = params.Results.backgroundColour;
   median = params.Results.median;
   writeIntermediateResults = params.Results.writeIntermediateResults;
   imgtype = params.Results.imgtype;
   
   medians = zeros (0,5);
   
   % The first image for comparison is that representing the first image
   fileName = strcat ('Set', num2str(setNo), '\', num2str(beginFrame), '.', imgtype);
   while (~exist(fileName))
       beginFrame = beginFrame + 1;
       fileName = strcat ('Set', num2str(setNo), '\', num2str(beginFrame), '.', imgtype);
   end;
   prev = imread (fileName);
   % The image is transformed into BW and then converted
   if (backgroundColour == 0)
      prev = cleanBlob(prev);
   else
      prev = invertAndCleanBlob (prev);
   end;
   [m,n] = find (bbFile(:,1) == beginFrame);
   prev = maskAndResize (prev, bbFile(m,2:5), rescaleFactor);
   
   [m,n] = find (prev);
   if (median == true)
      medianX = median(n);
      medianY = median(m);
   else
      medianX = round(mean(n));
      medianY = round(mean(m));
   end;
   prevRowNo = 1;
   medians (1,1:5) = [beginFrame, medianX, medianY, medianX, medianY];
   footLoc (1,1:3) = [beginFrame, mean(n), max(m)];
   counter = 1;
   boundary2 = centreAroundMedian (prev, medianX, medianY, personWidth, personHeight);
   % Loops through all images
   for i=beginFrame+1:endFrame
     %  i
      fileName2 = strcat ('Set', num2str(setNo), '\', num2str(i), '.', imgtype);
      if (exist(fileName2))
          counter = counter + 1;
          rowNo = counter;
          current = imread (fileName2);
          if (backgroundColour == 0)
               current = cleanBlob(current);
          else
               current = invertAndCleanBlob (current);
          end;
          [m,n] = find (bbFile(:,1) == i);
        %  figure; imshow (current);
        %  size (current)
          if (size(m,1) == 0)
              continue;
          end;
          current = maskAndResize (current, bbFile(m,2:5), rescaleFactor);
        %  figure; imshow (current);
          
          [m,n] = find (current);
          if (median == true)
            medianX = median(n);
            medianY = median(m);
          else
            medianX = round(mean(n));
            medianY = round(mean(m));
          end;
          currentCentred = centreAroundMedian (current, medianX, medianY, personWidth, personHeight);
          boundary1 = centreAroundMedian (boundary2, medians(prevRowNo, 4)-medians(prevRowNo,2), medians(prevRowNo, 5)-medians(prevRowNo,3), personWidth, personHeight);
          boundary2 = getBoundaryOnly (currentCentred);

          diffOptimal = 1000;
          optShiftX = 0;
          optShiftY = 0;
          for xShift = -5:1:5
              currentCentredX = circshift (currentCentred, [0, xShift]);
              for yShift = -5:1:5
                  current2 = circshift (currentCentredX, [yShift, 0]);
                  boundary2X = getBoundaryOnly(current2);
                  diffXY = sum(sum(abs(boundary1 - boundary2X)));
                  if (diffXY < diffOptimal)
                      diffOptimal = diffXY;
                      optShiftX = xShift;
                      optShiftY = yShift;
                  end;
              end;
          end;
         medians(rowNo, 1:5) = [i, medianX, medianY, round(medianX-optShiftX), round(medianY-optShiftY)];
         footLoc (counter, 1:3) = [i, mean(n), max(m)];
         prev = current;
         prevRowNo = rowNo;
      end
   end
   % writes the resulting medians file
   if (writeIntermediateResults == true)
       dlmwrite (newMedianFile , medians, 'precision', '%g');
   end;
end