function [featureVector, targetVector] = makeFeatureVector(images, depthImage, discreteDepthImage, points, calc3dFeats, displayFlag)
% 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 realWorldFlag neighborFlag nDimForGrasp nDimNeighbors

%% 459 is a magic number here
featureVector = zeros(0, 459);
targetVector = zeros(0, 1);

imrows = 48;
imcols = 64;

%% For each training pic
if (displayFlag)
  disp('Creating feature vector..');
end
for pic=1:size(images,4)

  %% Scale target image
  targetImageScaled = zeros(48,64);

  %% Vectorize data and label
  gridImage = zeros(48,64);
  configurationFile(images(:,:,:,pic), gridImage);

  if realWorldFlag
    if(size(images(:,:,:,pic),1) == 240)
      fV = makeJustinFeatureVector( imresize(images(:,:,:,pic),2));%, 'bilinear') );
    else
      fV = makeJustinFeatureVector( images(:,:,:,pic));%, 'bilinear') );
    end
  else
      fV = makeJustinFeatureVector( images(:,:,:,pic)  );
  end

  %  Make depth feature vectors here
  if (displayFlag)
    disp('Creating depth feature vector..');
  end
  depthMask = repmat(depthImage, [1 1 3 1]);
  discreteDepthMask = repmat(discreteDepthImage, [1 1 3 1]);
  fVDepth = makeDepthFeatureVector(depthMask(:,:,:,pic));
  fVDepthDiscrete = makeDepthFeatureVector(discreteDepthMask(:,:,:,pic));

  %  Make 3D feature vectors here
  if (displayFlag)
    disp('Creating 3D feature vector..');
  end
  if (calc3dFeats == 1)
    fV3DPoints = make3DFeatureVector(depthMask(:,:,1,pic), points);
  else
    fV3DPoints = zeros(size(fV, 1), size(fV, 2), 232);
  end

  if (displayFlag)
    disp('Feature vector created.');
  end
  %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);
  
  %% Add current image info to training data
  
  if size(featureVector, 1) == 0
    featureVector = reshape(fV, [48*64, size(fV,3)]);
    targetVector = reshape(targetImageScaled, [48*64 1]);
  else
    featureVector = [featureVector; ];
    targetVector = [targetVector; ];
  end
  if (displayFlag)
    disp(['Calculating features for testing. Image number' num2str(pic)]);
  end
end


  return;
