function prepareFeatureVectorInAdvance(typeData)
% 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.



perceptionParameters;

if nargin < 1
  typeData = 'tea';
end

imrows = 48;
imcols = 64;
gridSize = 10;

global syntheticDataDirectory nDimForGrasp nNeighbors nDimNeighbors scratchDataDirectory
directoryName = [syntheticDataDirectory '/' typeData '/']

nFiles = length(dir([directoryName 'graspPriorityWidth_' typeData '*.png']))
if strcmp(typeData, 'emptyDishwasher') || strcmp(typeData, 'emptyDishwasherZoomed')
%  nFiles = 32;
  gammaVal = 1;
else
  gammaVal = 2.19;
end

%% For each test pic
disp('Creating feature vector..');

testFlag = 0;

batchVector = 0:50:min(nFiles+50,600);

if (testFlag == 1)
  batchVector = 0:1:min(nFiles+50,1);
end

for batch = 1:(length(batchVector)-1)

  featureVector = zeros(0, 0);%nNeighbors*nDimForGrasp);
  targetVector = zeros(0, 1);
  targetVectorTableEdge = zeros(0, 1);

  numOfZeros = 0;
  powersOfTen = 10;
  while (nFiles > powersOfTen - 1);
    powersOfTen = powersOfTen * 10;
    numOfZeros = numOfZeros + 1;
  end;

  if strcmp(typeData, 'emptyDishwasher')
    numOfZeros = 3;
  end

  for pic=batchVector(batch):min(nFiles-1,batchVector(batch+1)-1 )  %(1:min(batch,nFiles-1)        %cannot cope with more than 1000 files
    if mod(pic,20) == 0
      disp([directoryName '              Calculating features for Image number ' num2str(pic)]);
    end

    %============ Read files ========================================
    if pic<10
      numOfZerosToAppend = numOfZeros;
    elseif pic<100
      numOfZerosToAppend = numOfZeros - 1;
    elseif pic<1000
      numOfZerosToAppend = numOfZeros - 2;
    elseif pic<10000
      numOfZerosToAppend = numOfZeros - 3;
    end
    zeroString = repmat('0', 1, numOfZerosToAppend);
    filename = [typeData zeroString num2str(pic) '.png'];
  
    A = (imread([directoryName filename], 'png'));
    targetImage = double(imread([directoryName 'graspPriorityWidth_' filename ], 'png'));
    depthImage = double(imread([directoryName 'depth_' filename ], 'png')) .^ gammaVal;
    depthMask = (depthImage(:, :, 1) + depthImage(:, :, 2) + depthImage(:, :, 3)) / (3*255);
    validDepthMask = depthMask > 0;

    depthRescaleValue = 1;
    if strcmp(typeData, 'cerealbowl')
      depthRescaleValue = 40 / 100;
    elseif strcmp(typeData, 'thickpencil')
      depthRescaleValue = 50 / 100;
    elseif strcmp(typeData, 'martini')
      depthRescaleValue = 80 / 100;
    elseif strcmp(typeData, 'teaTwo')
      depthRescaleValue = 55 / 100;
    elseif strcmp(typeData, 'mug')
      depthRescaleValue = 90 / 100;
    elseif strcmp(typeData, 'eraser')
      depthRescaleValue = 40 / 100;
    end

    depthMask = depthMask * depthRescaleValue;

    if strcmp(typeData, 'emptyDishwasher')
      points = load([directoryName 'xyz_points' sprintf('%02d', pic) '.txt']);
    elseif strcmp(typeData, 'emptyDishwasherZoomed')
      points = load([directoryName 'xyz_points' sprintf('%03d', pic) '.txt']);
    else
      points = getPointsFromDepth(depthMask);
    end
    % Create discrete depth mask here
    discreteDepthMask = makeDiscreteDepthMask(depthMask, imrows, imcols, gridSize);
    
    % Flag invlaid depth areas here
    invalidDepthMask = ~validDepthMask .* (-1 * ones(size(depthMask)));
    depthMask = validDepthMask .* depthMask + invalidDepthMask;
    discreteDepthMask = validDepthMask .* discreteDepthMask + invalidDepthMask;

    %targetImage = (targetImageTableEdge(:,:,1) == 127);
    H = rgb2hsv(A);
    objectMask = H(:,:,2) > mean(mean( H(:,:,2) ));
    %subplot(2,2,1), imagesc(A);
    %subplot(2,2,2), imagesc(H(:,:,2));
    %subplot(2,2,3), imagesc(targetImage);
    %subplot(2,2,4), imagesc( objectMask );
    %figure, imagesc( targetImage(:,:,1) );
    %targetImageTableEdge(241,221,2)


    %% Scale target image
    targetImageScaled = zeros(imrows,imcols);
    obkectMaskScaled = zeros(imrows,imcols);
    for y=1:imrows
      for x=1:imcols
        %targetImageScaled(y, x) = sum(sum(targetImage(10*y-9:y*10, 10*x-9:x*10)));
        possibleGrasps = targetImage(10*y-9:y*10, 10*x-9:x*10, 1);
        possibleGrasps = reshape(possibleGrasps, 100, 1);
        possibleGrasps = sort(possibleGrasps);
        bestVal = 0;
        bestNum = 0;
        numProcessed = 0;
        while numProcessed < 100;
          thisVal = possibleGrasps(numProcessed+1);
          thisNum = sum(possibleGrasps == thisVal);
          numProcessed = numProcessed + thisNum;
          if (thisVal ~= 0) && (thisNum > bestNum)
            bestNum = thisNum;
            bestVal = thisVal;
          end
        end
        %if (bestVal ~= 0)
        %  targetImage(10*y-9:10*y, 10*x-9:10*x, 1)
        %  bestVal
        %end
        targetImageScaled(y, x) = bestVal;
        objectMaskScaled(y, x) = mean(mean(objectMask(10*y-9:y*10, 10*x-9:x*10))) > 0.5;
        %targetImageTableEdgeScaled(y,x) = sum(sum(targetImageTableEdge(10*y-9:y*10, 10*x-9:x*10,3)));
      end
    end

    %targetImageScaled = targetImageScaled ./ 255; %max(targetImageScaled, ones(imrows,imcols));
    %targetImageTableEdgeScaled = targetImageTableEdgeScaled ./ max(targetImageTableEdgeScaled, ones(imrows,imcols));
  
    %targetImageScaled1 = (imresize(targetImage,.1) > 0);
    %sum(abs( targetImageScaled(:) - targetImageScaled1(:)))
  
    %% Vectorize data and label
    gridImage = zeros(imrows,imcols);
    configurationFile(A, gridImage);
    %size(A), imshow(A)
    %fV = [];%makeJustinFeatureVector(A);
    fV = makeJustinFeatureVector(A);

    %  Make depth feature vectors here
    depthMask = repmat(depthMask, [1 1 3]);
    discreteDepthMask = repmat(discreteDepthMask, [1 1 3]);
    fVDepth = makeDepthFeatureVector(depthMask);
    fVDepthDiscrete = makeDepthFeatureVector(discreteDepthMask);
    %fVDepth = [  ]; %makeDepthFeatureVector(depthMask);
    %fVDepthDiscrete = [  ]; %makeDepthFeatureVector(discreteDepthMask);

    %  Make 3D feature vectors here
    %disp('Starting 3D Features');
    fV3DPoints = make3DFeatureVector(depthMask(:,:,1), points);
    %disp('Finished 3D Features');
  
    %adding neighbors here
    fV_old = fV;
    for r = -2:2
      for c= -2:2
        if (r~=0 || c~=0)
          tempFV = [zeros(imrows,max(0,-c),nDimNeighbors), fV_old(:,max(1,c+1):(end+min(c,0)),1:nDimNeighbors), zeros(imrows,max(0,c),nDimNeighbors)];
          tempFV2 = [zeros(max(0,-r),imcols,nDimNeighbors); tempFV(max(1,r+1):(end+min(r,0)),:,1:nDimNeighbors); zeros(max(0,r),imcols,nDimNeighbors)];
          fV = cat(3, fV, tempFV2);
        end
      end
    end
    % adding depth here
    fV = cat(3, fV, fVDepth);
    for r = -1:1
      for c= -1:1
        if (r*r ~= c*c) && (r~=0 || c~=0)
          tempFV = [zeros(imrows,max(0,-c),nDimNeighbors), fVDepth(:,max(1,c+1):(end+min(c,0)),1:nDimNeighbors), zeros(imrows,max(0,c),nDimNeighbors)];
          tempFV2 = [zeros(max(0,-r),imcols,nDimNeighbors); tempFV(max(1,r+1):(end+min(r,0)),:,1:nDimNeighbors); zeros(max(0,r),imcols,nDimNeighbors)];
          fV = cat(3, fV, tempFV2);
        end
      end
    end
    fV = cat(3, fV, fVDepthDiscrete);
    for r = -1:1
      for c= -1:1
        if (r*r ~= c*c) && (r~=0 || c~=0)
          tempFV = [zeros(imrows,max(0,-c),nDimNeighbors), fVDepthDiscrete(:,max(1,c+1):(end+min(c,0)),1:nDimNeighbors), zeros(imrows,max(0,c),nDimNeighbors)];
          tempFV2 = [zeros(max(0,-r),imcols,nDimNeighbors); tempFV(max(1,r+1):(end+min(r,0)),:,1:nDimNeighbors); zeros(max(0,r),imcols,nDimNeighbors)];
          fV = cat(3, fV, tempFV2);
        end
      end
    end
    
    % adding 3d points here
    fV = cat(3, fV, fV3DPoints);

    %% Pruning the examples
    trainFeatureVectorCut = reshape(fV, [imrows*imcols, size(fV,3)]); %featureVector;
    trainTargetVectorCut = reshape(targetImageScaled, [imrows*imcols 1]);   %targetVector;
    objectMaskVector = reshape( objectMaskScaled, [imrows*imcols 1]);   %targetVector;
    %trainTargetVectorTableEdgeCut = reshape( targetImageTableEdgeScaled, [imrows*imcols 1]);

    RelNumNegExamples = 1.1;

    idealNumElements = floor(RelNumNegExamples*sum(trainTargetVectorCut) + 2);
    if strcmp(typeData, 'emptyDishwasher')
      idealNumElements = 2 * idealNumElements; %length(trainTargetVectorCut);
    end

    negInd = find( (trainTargetVectorCut == 0) & (objectMaskVector == 1) );
    posInd = find(trainTargetVectorCut);

    tmpRnd = cumsum(rand(idealNumElements,1));
    randInd = 1 + floor( (size(negInd,1)-1) * tmpRnd/tmpRnd(end) );		%ERROR: some might be repeated
    randInd = randInd( find(filter([1 -1], 1, randInd) ~= 0) );
   
    trainFeatureVectorCut = trainFeatureVectorCut([posInd; negInd(randInd)],:);
    trainTargetVectorCut = trainTargetVectorCut([posInd; negInd(randInd)]);
    %trainTargetVectorTableEdgeCut = trainTargetVectorTableEdgeCut([posInd; negInd(randInd)]);
   
    %% Add current image info to training data
    featureVector = [featureVector; trainFeatureVectorCut];
    targetVector = [targetVector; trainTargetVectorCut];
    %targetVectorTableEdge = [targetVectorTableEdge; trainTargetVectorTableEdgeCut];
  end

  if (testFlag ~= 1)
    save([scratchDataDirectory '/Features/' typeData '_' num2str(batch) '.mat'], 'featureVector', 'targetVector', 'targetVectorTableEdge');
  end

end
