%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function faceDetection()
%   z.li, 11-10-2003.
%   face detection from local facial features
% function dependency:
%   - n/a
% input:
%   lEyes   - 6xn:[(1)prob, (2)scale, (3)x, (4)y, (5)w, (6)h]
%   rEyes   - 6xn:[(1)prob, (2)scale, (3)x, (4)y, (5)w, (6)h]
%   mouths  - 5xn:[(1)prob, (2)scale, (3)x, (4)y, (5)theta]
%   thres   - face prob thres
%   pcolor  - plot color for the detection results
% output:
%   faces   - 7xn:[(1)prob, (2)scale, (3)x, (4)y, (5)w, (6)h, (7)theta]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [faces] = faceDetection(lEyes, rEyes, mouths, thres, pcolor)
function [faces] = faceDetection(lEyes, rEyes, mouths, thres, pcolor)
  dbg = 'n';
  % input parameters
  if dbg=='y'
    lEyes = [0.5, 2, 30, 40, 20, 16;
             0.9, 2, 40, 78, 20, 16;
             0.8, 3, 30, 50, 20, 16]';
    rEyes = [0.8, 2, 50, 40, 20, 16;
             0.5, 3, 50, 50, 20, 16;
             0.6, 2, 46, 90, 20, 16;
             0.8, 3, 50, 50, 20, 16]';
    mouths = [0.7, 1, 40, 90, pi/4;
              0.8, 2, 60, 40, pi/2]';
    thres  = 0.5;
    pcolor = 'b';
  end

  % const
  kScaleDiffExp = 0.1;               % const for mapping scale diff to prob
  kAngleDiffExp = 0.2;               % const for mapping angle diff to prob
  kSymetryDiffExp = 2;
  thresFaceProb = thres;

  % var
  nFace     = 0;
  nEyePair  = 0;
  nLEye     = length(lEyes(1,:));            
  nREye     = length(rEyes(1,:));            
  nMouth    = length(mouths(1,:));
  
  % compute feasible eye pairs set
  for j=1:nLEye
     for k=1:nREye
        nEyePair = nEyePair + 1;
        % build feasible eye pairs set (feps)
        % feps = [(1)log prob,(2)scaleDiff,(3)lEye=j,(4)rEye=k,(5)w,(6)theta]
        scaleDiff = abs(lEyes(2,j)-rEyes(2,k));
        feps(1,nEyePair) = log(lEyes(1,j))+log(rEyes(1,k));
        feps(2,nEyePair) = scaleDiff;
        feps(3,nEyePair) = j;       % left eye = lEyes(j)
        feps(4,nEyePair) = k;       % right eye = rEyes(k)
        pv = lEyes(3:4,j)-rEyes(3:4,k);     % eye pair vector
        feps(5,nEyePair) = norm(pv);
        if pv(1) == 0   % x=0
            feps(6,nEyePair) = sign(pv(2))*pi/2;
        else
            feps(6,nEyePair) = atan(pv(2)/pv(1));
        end
     end %for k
  end %for j

  % pruning eye pairs
  pFeps = feps;

  % compute feasible face set with feps and mouths
  nFace = 0;
  for i=1:nEyePair
    for t=1:nMouth
        % compute feasible eyes-mouth set
        % fems = [(1)log prob, (2)max scale diff, (3)theta diff 
        %         (4)lEye (5)rEye (6)mouth]
        nFace = nFace + 1;
        j = pFeps(3, i);     %left eye index
        k = pFeps(4, i);     %right eye index
        fems(1, nFace) = pFeps(1,i)+log(mouths(1,t));
        % max scale difference
        maxscaleDiff = max([feps(2,i), abs(mouths(2,t)-lEyes(2,j)), abs(mouths(2,t)-rEyes(2,k))]);
        fems(2, nFace) = maxscaleDiff;                 % max scale diff
        fems(3, nFace) = abs(pFeps(6,i)-mouths(5,t));   % angle diff
        % eyes, mouth location
        fems(4, nFace) = j;
        fems(5, nFace) = k;
        fems(6, nFace) = t;
    end %for t
  end %for i

  % pruning the feasible eyes-mouth sets, resolve the conflict.
  % faces=[(1)prob, (2)scale, (3-8)lx,ly,rx,ry,mx,my, (9)theta]
  for i=1:nFace     
     j = fems(4,i);
     k = fems(5,i);
     t = fems(6,i);
     % adjust for scale diff
     faces(1,i) = fems(1,i)-kScaleDiffExp*fems(2,i);      
     % adjust for angle diff
     faces(1,i) = faces(1,i)-kAngleDiffExp*fems(3,i);
     % adjust for symetry diff
     lenLR = norm(lEyes(3:4,j) - rEyes(3:4,k));
     lenLM = norm(lEyes(3:4,j) - mouths(3:4,t));
     lenRM = norm(rEyes(3:4,k) - mouths(3:4,t));
     symDiff(i) = abs((lenLM-lenRM)/(lenLM+lenRM));    
     faces(1,i) = faces(1,i)-kSymetryDiffExp*symDiff(i); 
     % convert from log prob to prob
     faces(1,i) = exp(faces(1,i));
     scales = [lEyes(2,j), rEyes(2,k), mouths(2,t)];
     faces(2,i) = mean(scales);
     faces(3,i) = lEyes(3,j);   % left eye xy
     faces(4,i) = lEyes(4,j);
     faces(5,i) = rEyes(3,k);   % right eye xy
     faces(6,i) = rEyes(4,k);
     faces(7,i) = mouths(3,t);  % mouth xy
     faces(8,i) = mouths(4,t);
  end %for i 

  % overlapping conflict resolving

  noplot = 'n';
  if noplot == 'y'
    return;
  end

  hold on;
  allx = [lEyes(3,:) rEyes(3,:) mouths(3,:)];
  ally = [lEyes(4,:) rEyes(4,:) mouths(4,:)];
  xmin = min(allx);
  xmax = max(allx);
  ymin = min(ally);
  ymax = max(ally);

  axis([xmin-5 xmax+5 ymin-5 ymax+5]);

  for j=1:nLEye
    plot(lEyes(3,j), lEyes(4,j), 'Or');
    plot(lEyes(3,j), lEyes(4,j), '+r');
  end

  for k=1:nREye
    plot(rEyes(3,k), rEyes(4,k), 'Ob');
    plot(rEyes(3,k), rEyes(4,k), '*b');
  end

  for t=1:nMouth
    plot(mouths(3,t), mouths(4,t), '^k');
  end

  styl = sprintf(':%s', pcolor);
  for i=1:nFace
    if faces(1,i) > thresFaceProb
       % plot eye pair line
       px=[faces(3,i) faces(5,i)];py=[faces(4,i) faces(6,i)];
       plot(px, py, '.', px, py, styl);
       % plot left eye-mouth line
       px=[faces(3,i) faces(7,i)];py=[faces(4,i) faces(8,i)];
       plot(px, py, '.', px, py, styl);
       % plot right eye-mouth line
       px=[faces(5,i) faces(7,i)];py=[faces(6,i) faces(8,i)];
       plot(px, py, '.', px, py, styl);    
    end
  end

  return;