%--------------------------------------------------------------------
   % Define algorithm Name, Description, and UserDirections.
   properties(Constant)

       %Name: Algorithm Name
       %   Character vector specifying name of algorithm.
       Name = 'Vehicle Detection and Distance Estimation';

       % Description: Provide a one-line description for your algorithm.
       Description = 'Detect vehicles using a pretrained ACF vehicle detector and compute distance of detected vehicles from camera.';

       % UserDirections: Provide a set of directions that are displayed
       %                 when this algorithm is invoked. The directions
       %                 are to be provided as a cell array of character
       %                 vectors, with each element of the cell array
       %                 representing a step in the list of directions.
       UserDirections = {...
           'Define a rectangle ROI Label to label vehicles.',...
           'For the label definition created, define an Attribute with name Distance, type Numeric Value and default value 0.', ...
           'Run the algorithm',...
           'Manually inspect and modify results if needed'};
   end

   %--------------------------------------------------------------------
   % Vehicle Detector Properties
   %--------------------------------------------------------------------
   properties
       %SelectedLabelName Selected label name
       %   Name of selected label. Vehicles detected by the algorithm will
       %   be assigned this variable name.
       SelectedLabelName

       %Detector Detector
       %   Pretrained vehicle detector, an object of class
       %   acfObjectDetector.
       Detector

       %VehicleModelName Vehicle detector model name
       %   Name of pretrained vehicle detector model.
       VehicleModelName = 'full-view';

       %OverlapThreshold Overlap threshold
       %   Threshold value used to eliminate overlapping bounding boxes
       %   around the reference bounding box, between 0 and 1. The
       %   bounding box overlap ratio denominator, 'RatioType' is set to
       %   'Min'
       OverlapThreshold = 0.65;

       %ScoreThreshold Classification Score Threshold
       %   Threshold value used to reject detections with low detection
       %   scores.
       ScoreThreshold = 30;

       %ConfigureDetector Boolean value to decide on configuring the detector 
       %   Boolean value which decides if the detector is configured using
       %   monoCamera sensor.
       ConfigureDetector = true;

       %SensorObj monoCamera sensor
       %   Monocular Camera Sensor object used to configure the detector.
       %   A configured detector will run faster and can potentially
       %   result in better detections.
       SensorObj = [];

       %SensorStr monoCamera sensor variable name
       %   Monocular Camera Sensor object variable name used to configure
       %   the detector.
       SensorStr = '';

       %VehicleWidth Vehicle Width
       %   Vehicle Width used to configure the detector, specified as
       %   [minWidth, maxWidth] describing the approximate width of the
       %   object in world units.
       VehicleWidth = [1.5 2.5];

       %VehicleLength Vehicle Length
       %   Vehicle Length used to configure the detector, specified as
       %   [minLength, maxLength] describing the approximate length of the
       %   object in world units.
       VehicleLength = [ ];    
   end

   %--------------------------------------------------------------------
   % Attribute automation Properties
   %--------------------------------------------------------------------    
   properties (Constant, Access = private)

       % Flag to enable Distance attribute estimation automation
       AutomateDistanceAttribute = true;

       % Supported Distance attribute name. 
       % The label must have an attribute with the name specified.
       SupportedDistanceAttribName = 'Distance';
   end

   properties (Access = private)

       % Actual attribute name for distance
       DistanceAttributeName;

       % Flag to check if attribute specified is a valid distance
       % attribute
       HasValidDistanceAttribute = false;
   end

   %--------------------------------------------------------------------
   % Initialize sensor, detector and other relevant properties.
   function initialize(algObj, ~)

       % Store the name of the selected label definition. Use this
       % name to label the detected vehicles.
       algObj.SelectedLabelName = algObj.SelectedLabelDefinitions.Name;

       % Initialize the vehicle detector with a pretrained model.
       algObj.Detector = vehicleDetectorACF(algObj.VehicleModelName);

       % Initialize parameters to compute vehicle distance
       if algObj.AutomateDistanceAttribute
           initializeAttributeParams(algObj);
       end            
   end

   function initializeAttributeParams(algObj)
       % Initialize properties relevant to attribute automation.

       % The label must have an attribute with name Distance and type
       % Numeric Value.
       hasAttribute = isfield(algObj.ValidLabelDefinitions, 'Attributes') && ...
                      isstruct(algObj.ValidLabelDefinitions.Attributes);
       if hasAttribute           
          attributeNames = fieldnames(algObj.ValidLabelDefinitions.Attributes);
          idx = find(contains(attributeNames, algObj.SupportedDistanceAttribName));
          if ~isempty(idx)
             algObj.DistanceAttributeName = attributeNames{idx};  
             algObj.HasValidDistanceAttribute = validateDistanceType(algObj);
          end
       end  
   end  

   function tf = validateDistanceType(algObj)
       % Validate the attribute type. 

       tf = isfield(algObj.ValidLabelDefinitions.Attributes, algObj.DistanceAttributeName) && ...
            isfield(algObj.ValidLabelDefinitions.Attributes.(algObj.DistanceAttributeName), 'DefaultValue') && ...
            isnumeric(algObj.ValidLabelDefinitions.Attributes.(algObj.DistanceAttributeName).DefaultValue);
   end

   %--------------------------------------------------------------------
   function autoLabels = run(algObj, I)

       autoLabels = [];

       % Configure the detector.
       if algObj.ConfigureDetector && ~isa(algObj.Detector,'acfObjectDetectorMonoCamera')
           vehicleSize = [algObj.VehicleWidth;algObj.VehicleLength];
           algObj.Detector = configureDetectorMonoCamera(algObj.Detector, algObj.SensorObj, vehicleSize);                
       end

       % Detect vehicles using the initialized vehicle detector.
       [bboxes, scores] = detect(algObj.Detector, I,...
           'SelectStrongest', false);

       [selectedBbox, selectedScore] = selectStrongestBbox(bboxes, scores, ...
           'RatioType', 'Min', 'OverlapThreshold', algObj.OverlapThreshold);

       % Reject detections with detection score lower than
       % ScoreThreshold.
       detectionsToKeepIdx = (selectedScore > algObj.ScoreThreshold);
       selectedBbox = selectedBbox(detectionsToKeepIdx,:);

       if ~isempty(selectedBbox)
           % Add automated labels at bounding box locations detected
           % by the vehicle detector, of type Rectangle having name of
           % the selected label.
           autoLabels.Name     = algObj.SelectedLabelName;
           autoLabels.Type     = labelType.Rectangle;
           autoLabels.Position = selectedBbox;

           if (algObj.AutomateDistanceAttribute && algObj.HasValidDistanceAttribute)
               attribName = algObj.DistanceAttributeName;
               % Attribute value is of type 'Numeric Value'
               autoLabels.Attributes = computeVehicleDistances(algObj, selectedBbox, attribName);
           end                  
       else
           autoLabels = [];
       end
   end

   function midPts = helperFindBottomMidpoint(bboxes)
      % Find midpoint of bottom edge of the bounding box.

      xBL = bboxes(:,1);
      yBL = bboxes(:,2);

      xM = xBL + bboxes(:,3)/2;
      yM = yBL + + bboxes(:,4)./2;
      midPts = [xM yM];

   end

   function distances= computeDistances(algObj, bboxes)
      % Helper function to compute vehicle distance.

      midPts = helperFindBottomMidpoint(bboxes);
      xy = algObj.SensorObj.imageToVehicle(midPts);
      distances  = sqrt(xy(:,1).^2 + xy(:,2).^2);

   end

   function attribS = computeVehicleDistances(algObj, bboxes, attribName)
       % Compute vehicle distance.

       numCars = size(bboxes, 1);
       attribS = repmat(struct(attribName, 0), [numCars, 1]);

       for i=1:numCars       
           distanceVal = computeDistances(algObj, bboxes(i,:));
           attribS(i).(attribName) = distanceVal;
       end
   end

     mkdir('+vision/+labeler');
     copyfile(fullfile(matlabroot,'examples','driving','main','VehicleDetectionAndDistanceEstimation.m'),'+vision/+labeler');

    load('FCWDemoMonoCameraSensor.mat', 'sensor')

     groundTruthLabeler 05_highway_lanechange_25s.mp4