function featureVector = make3DFeatureVector(depthValidMask, points, scales)
% depthValidMask --- binary image which tells depths at which points are
% valid.
% points = (x,y,X,Y,Z) ---- output from Bubmblee.
% scales = unused

% This code is copyrighted by Stanford University.
% Author: Ashutosh Saxena, Justin Drieymeyer
% Code cannot be used for commercial purposes.
% Use of the code, if used with permission from authors, must acknowledge:
% Learning to Grasp Novel Objects using Vision,
% Ashutosh Saxena, Justin Driemeyer, Justin Kearns, Chioma Osondu, Andrew Y. Ng. In 10th International Symposium on Experimental Robotics, ISER, 2006.
% Robotic Grasping of Novel Obects,
% Ashutosh Saxena, Justing Driemeyer, Justin Kearns, Andrew Y. Ng, In Neural Information Processing Systems (NIPS), 2006.
% More details at: http://ai.stanford.edu/~asaxena/learninggrasp/
% Version 0.1:   Aug 2006.

global columnWidth nGridCol nGridRow rowWidth
nGCol = nGridCol;
nGRow = nGridRow;

pointsBox = [-2, -2, -2;
             -2, -2,  0;
             -2, -2,  2;
             -2,  0, -2;
             -2,  0,  0;
             -2,  0,  2;
             -2,  2, -2;
             -2,  2,  0;
             -2,  2,  2;
              0, -2, -2;
              0, -2,  0;
              0, -2,  2;
              0,  0, -2;
              0,  0,  0;
              0,  0,  2;
              0,  2, -2;
              0,  2,  0;
              0,  2,  2;
              2, -2, -2;
              2, -2,  0;
              2, -2,  2;
              2,  0, -2;
              2,  0,  0;
              2,  0,  2;
              2,  2, -2;
              2,  2,  0;
              2,  2,  2];


%pointsBox = [0, 0, 0; -2, 0, 0; 2, 0, 0; 0, -2, 0; 0, 2, 0; 0, 0, -2; 0, 0, 2];
pointsBox = [0, 0, 0; -2, 0, 0; 2, 0, 0; 0, -2, 0; 0, 2, 0; 0, 0, -2; 0, 0, 2; -2, -2, 0; -2, 2, 0; 2, -2, 0; 2, 2, 0; -2, 0, -2; -2, 0, 2; 2, 0, -2; 2, 0, 2; 0, -2, -2; 0, 2, -2; 0, -2, 2; 0, 2, 2;];

basePCACubeTopCorners = [-3, -3, -3;
                         -3, -3, -1;
                         -3, -3,  1;
                         -3, -1, -3;
                         -3, -1, -1;
                     -3, -1,  1;
                     -3,  1, -3;
                     -3,  1, -1;
                     -3,  1,  1;
                     -1, -3, -3;
                     -1, -3, -1;
                     -1, -3,  1;
                     -1, -1, -3;
                     -1, -1, -1;
                     -1, -1,  1;
                     -1,  1, -3;
                     -1,  1, -1;
                     -1,  1,  1;
                      1, -3, -3;
                      1, -3, -1;
                      1, -3,  1;
                      1, -1, -3;
                      1, -1, -1;
                      1, -1,  1;
                      1,  1, -3;
                      1,  1, -1;
                      1,  1,  1];

basePCACubeBotCorners = basePCACubeTopCorners + 2;
basePCACubeTopCorners = basePCACubeTopCorners * 1/3;
basePCACubeBotCorners = basePCACubeBotCorners * 1/3;

neighborGridSize = 10;

pointsGrid = zeros(size(depthValidMask, 1), size(depthValidMask, 2), 3);
for pointsRow = 1:size(points, 1)
  pointsGrid(points(pointsRow, 1), points(pointsRow, 2), :) = reshape(points(pointsRow, 3:5), [1, 1, 3]);
end
reductionFactor = 3;
validMask = zeros(size(depthValidMask));
numActiveRows = ceil(size(validMask, 1) / reductionFactor);
numActiveCols = ceil(size(validMask, 2) / reductionFactor);
rowStep = (size(validMask, 1) - 1) / numActiveRows;
colStep = (size(validMask, 2) - 1) / numActiveCols;
pickedRows = round(1:rowStep:size(validMask, 1));
pickedCols = round(1:colStep:size(validMask, 2));

validMask(pickedRows, pickedCols) = 1;
validMask = validMask & (depthValidMask);

%pointsGrid2 = cat(3, repmat((1:size(validMask, 2)), [size(validMask, 1), 1]), pointsGrid); 
%pointsGrid2 = cat(3, repmat((1:size(validMask, 1))', [1, size(validMask, 2)]), pointsGrid2); 
%points = reshape(pointsGrid2, [size(pointsGrid2, 1) * size(pointsGrid2, 2), size(pointsGrid2, 3)]);
%validVec = reshape(validMask, [size(validMask, 1) * size(validMask, 2), 1]);
%points = points(find(validVec), :);

pointsGrid = cat(3, (depthValidMask), pointsGrid);

totalFeatureVectorOverlay = zeros(size(depthValidMask, 1), size(depthValidMask, 2), 0);

numMomentFeats = 16;
numPCAMomentFeats = 16;
numPercentageFeats = size(pointsBox, 1) * 3;
numPCAPercentFeats = size(basePCACubeTopCorners, 1);

featStartPercentage = numMomentFeats + numPCAMomentFeats + 1;
featStartPCAPercent = featStartPercentage + numPercentageFeats;

totalNumFeatPerRad = featStartPCAPercent + numPCAPercentFeats - 1;

radVec = [0.025, 0.05];

for rad = radVec
  radSquared = rad * rad;
  
  PCACubeTopCorners = basePCACubeTopCorners * rad;
  PCACubeBotCorners = basePCACubeBotCorners * rad;

  thisPointsBox = rad * pointsBox;

  featureVectorOverlay = zeros(size(depthValidMask, 1), size(depthValidMask, 2), totalNumFeatPerRad);
  for pointsRow = 1:size(points, 1);
    if validMask(points(pointsRow, 1), points(pointsRow, 2))
      %[points(pointsRow, 1) points(pointsRow, 2)]
      % select reasonable number of points from 2-d image
      goodRows = max(1, points(pointsRow, 1)-neighborGridSize):min(size(depthValidMask, 1), points(pointsRow, 1)+neighborGridSize);
      goodCols = max(1, points(pointsRow, 2)-neighborGridSize):min(size(depthValidMask, 2), points(pointsRow, 2)+neighborGridSize);
      interestingPoints = reshape(pointsGrid(goodRows, goodCols, :), [length(goodRows) * length(goodCols), size(pointsGrid, 3)]);
      % now use validity bit to remove invalid points and trim to just xyz coords
      interestingPoints = interestingPoints(find(interestingPoints(:, 1)), 2:4);
      moreInterestingPoints = interestingPoints;

      distToThisPoint = zeros(size(interestingPoints));
      distToThisPoint(:, 1) = interestingPoints(:, 1) - points(pointsRow, 3);
      distToThisPoint(:, 2) = interestingPoints(:, 2) - points(pointsRow, 4);
      distToThisPoint(:, 3) = interestingPoints(:, 3) - points(pointsRow, 5);
      %distToThisPoint = interestingPoints - repmat(points(pointsRow, 3:5), [size(interestingPoints, 1), 1]);
      distToThisPointSquared = (distToThisPoint .* distToThisPoint) * ones(3, 1);
      neighborRows = find(distToThisPointSquared <= radSquared);
      neighboringDistances = distToThisPoint(neighborRows, :);
      neighboringPoints = interestingPoints(neighborRows, :);
      numOfPoints = size(neighboringPoints, 1);

      cubeNeighborRows = find((distToThisPoint(:, 1) <= rad) & ...
                              (distToThisPoint(:, 2) <= rad) & ...
                              (distToThisPoint(:, 3) <= rad));
      cubeNeighborDist = distToThisPoint(cubeNeighborRows, :);
      cubeNeighborPoints = interestingPoints(cubeNeighborRows, :);
      cubeNumOfPoints = size(cubeNeighborDist, 1);
      [U, S, V] = svd(cubeNeighborDist);
      newCubeNeighborDist = cubeNeighborDist * V;

      thisR = points(pointsRow, 1);
      thisC = points(pointsRow, 2);

      % Moment Feature 1 & 2
      totalDistInXYZ = ones(1, size(neighboringPoints, 1)) * abs(neighboringDistances);
      totalDivisor = (totalDistInXYZ * ones(3, 1));
      if totalDivisor == 0
        totalDivisor = 1;
      end
      varInXYZ = totalDistInXYZ / numOfPoints;
      percentageXYZ = totalDistInXYZ / totalDivisor;

      totalDistInXYZSq = ones(1, size(neighboringPoints, 1)) * (neighboringDistances .* neighboringDistances);
      totalDivisorSq = (totalDistInXYZSq * ones(3, 1));
      if totalDivisorSq == 0
        totalDivisorSq = 1;
      end
      varInXYZSq = totalDistInXYZSq / numOfPoints;
      percentageXYZSq = totalDistInXYZSq / totalDivisorSq;

      featureVectorOverlay(thisR, thisC,  1) = varInXYZ(1);
      featureVectorOverlay(thisR, thisC,  2) = varInXYZ(2);
      featureVectorOverlay(thisR, thisC,  3) = varInXYZ(3);
      featureVectorOverlay(thisR, thisC,  4) = max(varInXYZ);
      featureVectorOverlay(thisR, thisC,  5) = min(varInXYZ);
      featureVectorOverlay(thisR, thisC,  6) = varInXYZ * ones(3, 1);
      featureVectorOverlay(thisR, thisC,  7) = max(percentageXYZ);
      featureVectorOverlay(thisR, thisC,  8) = min(percentageXYZ);
      featureVectorOverlay(thisR, thisC,  9) = varInXYZSq(1);
      featureVectorOverlay(thisR, thisC, 10) = varInXYZSq(2);
      featureVectorOverlay(thisR, thisC, 11) = varInXYZSq(3);
      featureVectorOverlay(thisR, thisC, 12) = max(varInXYZSq);
      featureVectorOverlay(thisR, thisC, 13) = min(varInXYZSq);
      featureVectorOverlay(thisR, thisC, 14) = varInXYZSq * ones(3, 1);
      featureVectorOverlay(thisR, thisC, 15) = max(percentageXYZSq);
      featureVectorOverlay(thisR, thisC, 16) = min(percentageXYZSq);

      % PCA Moment Feature 1 & 2
      totalDistInXYZ = ones(1, size(newCubeNeighborDist, 1)) * abs(newCubeNeighborDist);
      totalDivisor = (totalDistInXYZ * ones(3, 1));
      if totalDivisor == 0
        totalDivisor = 1;
      end
      varInXYZ = totalDistInXYZ / cubeNumOfPoints;
      percentageXYZ = totalDistInXYZ / totalDivisor;

      totalDistInXYZSq = ones(1, size(newCubeNeighborDist, 1)) * (newCubeNeighborDist .* newCubeNeighborDist);
      totalDivisorSq = (totalDistInXYZSq * ones(3, 1));
      if totalDivisorSq == 0
        totalDivisorSq = 1;
      end
      varInXYZSq = totalDistInXYZSq / cubeNumOfPoints;
      percentageXYZSq = totalDistInXYZSq / totalDivisorSq;

      featureVectorOverlay(thisR, thisC, 17) = varInXYZ(1);
      featureVectorOverlay(thisR, thisC, 18) = varInXYZ(2);
      featureVectorOverlay(thisR, thisC, 19) = varInXYZ(3);
      featureVectorOverlay(thisR, thisC, 20) = max(varInXYZ);
      featureVectorOverlay(thisR, thisC, 21) = min(varInXYZ);
      featureVectorOverlay(thisR, thisC, 22) = varInXYZ * ones(3, 1);
      featureVectorOverlay(thisR, thisC, 23) = max(percentageXYZ);
      featureVectorOverlay(thisR, thisC, 24) = min(percentageXYZ);
      featureVectorOverlay(thisR, thisC, 25) = varInXYZSq(1);
      featureVectorOverlay(thisR, thisC, 26) = varInXYZSq(2);
      featureVectorOverlay(thisR, thisC, 27) = varInXYZSq(3);
      featureVectorOverlay(thisR, thisC, 28) = max(varInXYZSq);
      featureVectorOverlay(thisR, thisC, 29) = min(varInXYZSq);
      featureVectorOverlay(thisR, thisC, 30) = varInXYZSq * ones(3, 1);
      featureVectorOverlay(thisR, thisC, 31) = max(percentageXYZSq);
      featureVectorOverlay(thisR, thisC, 32) = min(percentageXYZSq);

      numOfPlanes = 3;
      percentages = zeros(size(thisPointsBox, 1), numOfPlanes);

      for boxRow = 1:size(thisPointsBox, 1);
        distToThisPoint = zeros(size(moreInterestingPoints));
        distToThisPoint(:, 1) = moreInterestingPoints(:, 1) - thisPointsBox(boxRow, 1);
        distToThisPoint(:, 2) = moreInterestingPoints(:, 2) - thisPointsBox(boxRow, 2);
        distToThisPoint(:, 3) = moreInterestingPoints(:, 3) - thisPointsBox(boxRow, 3);
        %distToThisPoint = moreInterestingPoints - repmat(thisPointsBox(boxRow, :), [size(moreInterestingPoints, 1), 1]);
        distToThisPointSquared = (distToThisPoint .* distToThisPoint) * ones(3, 1);
        neighborRows = find(distToThisPointSquared <= radSquared);
        neighboringDistances = distToThisPoint(neighborRows, :);
        % neighboringPoints = moreInterestingPoints(neighborRows, :);
        numOfPoints = size(neighboringDistances, 1);
      
        if numOfPoints > 0
          XYZPlaneSides = neighboringDistances > 0;
          %  For now, just use the three basic planes - YZ, XZ, and XY
          percentages(boxRow, 1:3) = ones(1, size(XYZPlaneSides, 1)) * XYZPlaneSides;
          percentages = percentages / numOfPoints;
        end
      end

      featureVectorOverlay(thisR, thisC, featStartPercentage:(featStartPCAPercent - 1)) = reshape(percentages, [1, 1, numPercentageFeats]);

      PCAFeatIndexes = featStartPCAPercent:totalNumFeatPerRad;
      if (cubeNumOfPoints > 0);
        for boxRow = 1:size(PCACubeTopCorners, 1);
          topCorner = PCACubeTopCorners(boxRow, :);
          botCorner = PCACubeBotCorners(boxRow, :);
          inThisBox = ((newCubeNeighborDist(:, 1) >= topCorner(1, 1)) & ...
                       (newCubeNeighborDist(:, 1) <  botCorner(1, 1)) & ...
                       (newCubeNeighborDist(:, 2) >= topCorner(1, 2)) & ...
                       (newCubeNeighborDist(:, 2) <  botCorner(1, 2)) & ...
                       (newCubeNeighborDist(:, 3) >= topCorner(1, 3)) & ...
                       (newCubeNeighborDist(:, 3) <  botCorner(1, 3)));
          numPointsInThisBox = sum(inThisBox);
        
          featureVectorOverlay(thisR, thisC, PCAFeatIndexes(boxRow)) = numPointsInThisBox / cubeNumOfPoints;
        end;
      else
        featureVectorOverlay(thisR, thisC, PCAFeatIndexes) = zeros([1, 1, size(PCACubeTopCorners, 1)]);
      end;
    end
  end
  totalFeatureVectorOverlay = cat(3, totalFeatureVectorOverlay, featureVectorOverlay);
end

% Convert point features to patch features
% Assume that the image is correctly oriented.
imheight = size(depthValidMask, 1);
imwidth  = size(depthValidMask, 2);

% Step sizes in x and y
stepwidth  = imwidth/nGridCol;
stepheight = imheight/nGridRow;

featureVector = zeros( nGRow, nGCol, size(totalFeatureVectorOverlay, 3));

for g = 1:nGRow
  for c = 1:nGCol
    gridLeft  = round( (g - 1)*stepheight + 1);
    gridRight = round( g*stepheight );
    gridTop = round( (c - 1)*stepwidth + 1);
    gridBot = round( c*stepwidth );
    numPoints = sum( sum( validMask(gridLeft:gridRight, gridTop:gridBot)));
    if numPoints > 0
      % totalFeatureVectorOverlay(gridLeft:gridRight, gridTop:gridBot, :)
      % numPoints
      % sum( sum( totalFeatureVectorOverlay(gridLeft:gridRight, gridTop:gridBot, :), 2), 1)
      featureVector(g,c,:) = sum( sum( totalFeatureVectorOverlay(gridLeft:gridRight, gridTop:gridBot, :), 2), 1) / numPoints;
    end
  end
end
return;