%================================= pts3D =================================
%
% @class pts3D
%
% @brief Encapsulation of a 3D point cloud, with additional functionality.
%
%  Implements a class that simply stores 3D points.  Usually such a
%  description is associated to a 3D point cloud.  The point here is to
%  make certain operations on point clouds easier to do.
%
%  myPts = pts3D(); <BR>
%  myPts = pts3D( ptsMatrix ); <BR>
%  myPts = pts3D( pointCloud ); <BR>
%  myPts = pts3D( ptsMatrix, theAx); <BR>
%  myPts = pts3D( pointCloud, theAx); <BR>
%
%================================= pts3D =================================

%
%
%  @file    pts3D.m
%
%  @author  Patricio A. Vela,       pvela@gatech.edu
%
%  @date    2017/02/04              [created]
%
%  @note
%    Indent is 2 spaces. <BR>
%    Tabstop is 4 spaces (with conversion to spaces).
%
%  Table of Contents:
%   - Member Variables.
%   - Constructor + Basic Info
%   - Changing Points in Cloud (Add, Remove, etc.)
%   - Utility Functions (copy)
%   - Operations on the Point Cloud
%   - Operations between Point Clouds
%   - Display Functions 
%   - Input/Output Functions
%   
%================================= pts3D =================================
classdef pts3D < handle
  
%============================ Constant Values ============================
%
properties (Constant)
  thresh_SVDcard = 15;    %< Minimal cardinality of points to perform SVD.
end

%============================ Member Variables ===========================
%
properties (Access = protected)
  pts = [];         %< The actual points (pointCloud object).
  ax  = [];         %< The axes to plot into, if specified.
end
  
properties (Access = public)
  plotRad = 20;     %< The radius to plot the data at.
end
  
%)
%
%============================= Public Methods ============================
%
%(
  
methods
    
%----------------------- Constructor + Basic Info ------------------------
%
%(

  %=============================== pts3D ===============================
  %
  % @fn pts3D pts3D(pointSet thePts, axis theAx)
  %
  % @brief pts3D constructor
  %
  % @param thePts Set of points to use.
  % @param theAx  Axes to plot to [optional].
  %
  function this = pts3D(thePts, theAx)

  if (nargin >= 1)
    if ~isempty(thePts)
      if (isa(thePts, 'pointCloud'))    % If point cloud, use.
        this.pts = thePts;
      else                              % Else, instantiate as point cloud.
        this.pts = pointCloud(transpose(thePts));
      end
    end
  end

  if (nargin >= 2)
    this.ax = theAx;
  end

  end

  %================================ size ===============================
  %
  % @fn     int theSize size()
  %
  % @brief  Return the amount of points in the 3D point set.
  %
  function theSize = size(this)

  if (isempty(this.pts))
    theSize = 0;
  else
    theSize = 0;                    %! TODO: Week #1.
  end

  end

  %============================= numPoints =============================
  %
  % @fn     int nump numPoints()
  %
  % @brief  Return the amount of points in the 3D point set.
  %
  % Same as asking for the size.
  %
  function numP = numPoints(this)
  
  numP = this.size();

  end

%)
%
%------------------------ Changing Points in Cloud -----------------------
%
%(

  %=============================== clear ===============================
  %
  % @fn void clear()
  %
  % @brief Remove all points from the set.
  %
  function clear(this)

  this.pts = [];

  end

  %================================= add ===============================
  %
  % @fn     void add(pts3D morePts)
  % @fnalt  void add(pointCloud morePts)
  % @fnalt  void add(matrix morePts)
  %
  % @brief      Add points to point set/point cloud object.
  %
  % @param[in]  morePts     A set of points. Can be of various types
  %                         (pts3D, pointCloud, 3xN matrix).
  %
  function add(this, morePts)

  if (isa(morePts, 'pts3D'))
    if (isempty(morePts.pts))
      return;
    end

    if (isempty(this.pts))
      this.pts = morePts.pts.copy();
    else
      this.pts = pointCloud([this.pts.Location ; morePts.pts.Location]);
    end
  elseif (isa(morePts,'pointCloud'))
    if (isempty(this.pts))
      this.pts = morePts.copy();
    else
      this.pts = pointCloud([this.pts.Location ; morePts.Location]);
    end
  elseif (size(morePts,1) == 3)
    if (isempty(this.pts))
      this.pts = pointCloud(transpose(morePts));
    else
      this.pts = pointCloud([this.pts.Location ; transpose(morePts)]);
    end
  end

  end
  
  %============================= removeInds ============================
  %
  % @fn  pts3D remPts removeInds(array inds)
  %
  % @brief Remove points specified by indices.
  %
  % @param[in]   inds        Indices of points to remove.
  %
  % @param[out]  remPts      The points that were removed.
  %
  function remPts = removeInds(this, inds)

  if (isempty(inds))
    remPts = pts3D();
    return;
  end

  % TODO: Week #1 -- All of the code below is wrong.  Fix each line.
  %
  remPts = [];  %! Get points to remove. 

  keepInds = [1:this.pts.Count];   %! Get set of indices of points to keep. 
  this.pts = [];    %! Replace points with the keep subset.

  end

  %============================ keepOnlyInds ===========================
  %
  % @fn     pts3D remPts keepOnlyInds(array inds)
  %
  % @brief  Keep only the points specified by indices.  Rest go.
  %
  % @param[in]  kinds       Indices of points to keep.
  % @param[out] remPts      The points that were removed.
  %
  function remPts = keepOnlyInds(this, kinds)

  allInds = [1:this.size()];        %! Get set of all point indices.

  %TODO: Week #1: Get set of allInds without kinds
  remInds = [];    %! Get subset of points to remove.
  if (isempty(remInds))
    remPts = pts3D();
  else
    rempPts = this.removeInds(remInds);
  end

  end

  %=========================== normalizePose =============================
  %
  % @brief  Create pose normalized version of data using SVD.
  %
  %  "Normalize" the point cloud's pose so that the shape is shifted and
  %  aligned with axes at origin. Most likely not be robus to noise, so
  %  there should probably be a (model-based) denoising process run
  %  first that hopefully removes "outliers."
  %
  % @param[out] g   The SE(3) matrix defining the rotation and
  %                 translation that gives back the original data, after
  %                 being applied.
  %
  % @ntoe
  %   The set of points is modified to be "normalized."
  %
  function g = normalizePose(this)

  if (this.size() < this.thresh_SVDcard)
    g = eye(4);             % Not computable, do nothing, return identity.
    return;
  end

  %TODO: Week #1.
  g = [];
  normPts = [];

  this.pts = pointCloud( transpose(normPts) );
  end

  end


%)
%
%--------------------------- Utility Functions ---------------------------
%
%(

  %================================ copy ===============================
  %
  % @brief  Return a depp copy.
  %
  function ptsCopy = copy(this)

  if (isempty(this.pts))
    ptsCopy = pts3D();
  else
    ptsCopy = pts3D(this.pts.copy(), this.ax);
  end

  end

  %============================== subinds ==============================
  %
  % @brief  Get subset by indices.
  %
  % @param[in]  theInds     List of indices to grab from point cloud.
  %
  %
  function subPts = subinds(this, theInds)

  subPts = pts3D(transpose(this.pts.Location(theInds,:)));

  end


%)
%
%--------------------- Operations on the Point Cloud ---------------------
%
%(

  %============================= transform =============================
  %
  % @fn void transform(SE3 g)
  %
  % @brief Transform points under an SE(3) Lie group action.
  %
  % @param[in]  g       SE3 element to apply to points.
  %
  function transform(this, g)

  %TODO: Week #2.
  this.pts = [];

  end

  %=============================== mapTo ===============================
  %
  % @fn matrix mPts mapTo(matrix mA)
  %
  % @brief   Apply a linear mapping to the data.
  %
  % @param[in]   mA     A matrix reflecting the linear mapping to apply.
  %
  % @param[out]  mPts   The points mapped under the transformation. The
  %                     output need not be a set of 3D points.
  %
  function mPts = mapTo(this, mA)

  %TODO: Not Sure When.
  mPts = [];

  end

  %============================= mapAffine =============================
  %
  % @fn     matrix aCoords mapAffine(matrix affM)
  %
  % @brief  Apply affine mapping to the data.
  %
  % @param[in]  affM    A matrix reflecting the affine mapping to apply.
  %
  %
  %@param[out]  aCoords The points mapped under the affine transformation. 
  %                     Output need not be a set of 3D points.
  %
  function aCoords = mapAffine(this, affM)

  %TODO: Not Sure When.
  aCoords = [];

  end

  %========================== distanceToPoints =========================
  %
  % @brief  Compute the distance(s) to the specified point(s) from the
  %         point cloud.
  %
  % @param[in]  testPts     Set of points to test against.
  %                         Can be a matrix, a pts3D, or a pointCloud.
  %
  % @param[out] minDist     Distances (minimum to point cloud).
  % @param[out] minInd      Index of nearest point. 
  %
  function [minDist, minInd] = distanceToPoints(this, testPts)

  if (isa(testPts,'pts3D'))
    testPts = testPts.pts.Location;
  elseif (isa(testPts, 'pointCloud'))
    testPts = testPts.Location;
  elseif (size(testPts,1) == 3)
    testPts = transpose(testPts);
  end

  if (nargout == 1)
    minDist = []; %TODO: Not Sure When.
  else
    [minDist, minInd] = notSureWhen();  % TODO: Not Sure When.
  end

  end

  %========================= connectivityMatrix ========================
  %
  % @brief  Compute and return the connectivity matrix of the point cloud.
  %
  % @param[in]  radP    Proximity radius for determining connectivity.
  %
  % @note
  %  Depending on memory in system, there is a maximum size matrix that
  %  can be instantiated.    This function does not test for that since
  %  it is memory dependent.  Just note, that there is a limit to how
  %  big the point cloud can be to compute this particular matrix.
  %  Probably it is best to not go over a few GB in size, so that would
  %  mean roughly 20k is a good upper limit.  It can go higher, but then
  %  there will not be room for additional computations.
  %
  function cM = connectivityMatrix(this, radP)

  %TODO: Week #2. Compute pairwise distances and return binary matrix of
  %TODO:   connectivity relative to neihhborhood proximity radius radP.
  cM = [];

  end

  %============================== deNoise ==============================
  %
  % @fn     void deNoise(int numNN, double thresh)
  % @brief  Remove outliers from point cloud
  %
  % @param  numNN   Number of nearest naighbors to use [optional].
  % @param  thresh  Distance threshold on neighbors [optional].
  %
  function deNoise(this, numNN, thresh)

  if ( (nargin <= 1) || isempty(numNN) )
    numNN = 4;
  end

  if (nargin <= 2)
    thresh = 1;
  end

  if ~isempty(this.pts)
    this.pts = [];  %TODO: Not Sure When.
  end

  end


  %========================= clusterByProximity ========================
  %
  % @brief  Identify the different clusters using connected components
  %         based on a proximity radius.
  %
  % @param[in]  radP        Proximity radius for connectivity.
  %
  % @param[out] indLabels   The list of labels for the points.  In order
  %                         of points stored in the point cloud.
  %
  % @note   The colors for the points are set based on the cluster.
  %
  function indLabels = clusterByProximity(this, radP)

  %TODO: Week #2.

  % Lots of missing code here.  Use connectivity matrix.

  numClusters = 5;   % Clearly wrong, fix and delete this comment.
  indLabels = [1];   % Same here.

  colorSet = hsv2rgb([linspace(0,0.7,numClusters); ...
                     1*ones(1,100); 1*ones(1,100)]');

  theColors = [];   % Fix me and delete this comment. Use label to index
                    %   colorSet correctly.
  this.setColor(theColors);

  end

  %====================== clusterBySurfaceNormals ======================
  %
  % @brief  Identify the different clusters using connected components
  %         based on a proximity radius and alignment of normals.
  %
  % @param[in]  radP        Proximity radius for connectivity.
  % @param[in]  threshA     Threshold on angle agreement between normals.
  %
  % @param[out] indLabels   The list of labels for the points.  In order
  %                         of points stored in the point cloud.
  %
  % @note   The colors for the points are set based on the cluster.
  %
  function indLabels = clusterByProximity(this, radP)

  %TODO: Week #4.

  % Lots of missing code here.  Use connectivity matrix.

  numClusters = 5;   % Clearly wrong, fix and delete this comment.
  indLabels = [1];   % Same here.

  colorSet = hsv2rgb([linspace(0,0.7,numClusters); ...
                     1*ones(1,100); 1*ones(1,100)]');

  theColors = [];   % Fix me and delete this comment. Use label to index
                    %   colorSet correctly.
  this.setColor(theColors);

  end

  %============================== createMesh =============================
  %
  % @brief  Create a mesh from the point cloud.
  %
  % @brief[in]  meshGroups  Optional argument specifying whether point
  %                         cloud should be broken into smaller clusters
  %                         that are then meshed.  In which case,
  %                         theMesh would return a list of meshes.
  %
  function theMesh = createMesh(this, meshGroups)

  % TODO: Week #5.

  end

%)
%
%-------------------- Operations between Point Clouds --------------------
%
%(

  %=========================== findAdjacenct ===========================
  %
  % @fn     pts3D adjPts findAdjacenct(pts3D pc2, double Tdist)
  %
  % @brief  Find the subset of points adjacent to point set.
  %
  %  Returns list of adjacent points between the two point clouds, as
  %  determined by being within the specified distance threshold (in
  %  Euclidean sense).  The calling point cloud is the reference, while
  %  the argument point cloud is the query set used to identify the
  %  adjacent subset.
  %
  % @param[in]  pc2     The point cloud to test for adjacency.
  % @param[in]  Tdist   Threshold on Eculidean distance for adjacency.  %
  % @param[out] adjPts  The set of points considered adjacent to point set.
  %
  function [adjPts, adjInds] = findAdjacent(this, pc2, Tdist)

  if ( (this.size() <= 0) || (pc2.size() <= 0) )
    adjPts = pts3D();
    adjInds = [];
    return;
  end

  %! Get the points from each cloud.
  %! Find the minimal distance of points in tsPts to those in myPts.
  %! Apply threshold to identify those adjacent to myPts.
  %! TODO: Not sure when.

  %! Instantiate return argument.
  if (isempty(adjInds))
    adjPts = pts3D();
    return;
  else
    adjPts = []; % Fill in.
  end

  end

%)
%
%-------------------------- Display Functions --------------------------
%
%(

  %============================= assignAxes ============================
  %
  % @brief   Set the axes to plot the point data to.
  %
  % @param[in]   theAx   The new set of axes to plot to.
  %
  function assignAxes(this, theAx)

  this.ax = theAx;

  end

  %============================= setColor ============================
  %
  % @fn     void setColor(covector labCols)
  % @fnalt  void setColor(matrix labCols)
  % @brief  Set the color to plot the point data with.
  %
  % @param[in]   labCols    Row vector specifying color of all points, or
  %                         row-wise matrix specifying color of each point.
  %
  function setColor(this, labCols)

  if (isempty(this.pts) || (this.pts.Count == 0))
    return;
  end

  if size(labCols, 1) == 1
    this.pts.Color = repmat(uint8(labCols), size(this.pts.Location, 1), 1);
  elseif size(labCols, 1) == size(this.pts.Location, 1)
    this.pts.Color = uint8(labCols);
  else
    disp('setColor: Unmatched size between point cloud and color vector!');
  end

  end


  %================================ plot ===============================
  %
  % @fn void plot(pts3D this, double ptSize, extras varargin)
  %
  % @brief   Plot the list of points in 3D space.
  %
  function plot(this, ptSize)

  % IF empty cloud, nothing to plot.
  if ( this.size() == 0) 
    return;
  end

  % Parse arguments to determine radius of points when plotting.
  if ( (nargin < 2) || isempty(ptSize) )
    ptSize = this.plotRad;
  end

  % Check to see what axis should be plotted to, store for later use.
  if (isempty(this.ax))
    plotAx = gca;
  else
    plotAx = this.ax;
  end

  % TODO: Week #1.
  % Plot point cloud in proper axis with proper radius.

  end

%)
%
%------------------------- Input/Output Functions ------------------------
%
%(

  %============================= saveToFile ============================
  %
  % @fn  void saveToFile(string fileName)
  %
  % @brief   Save the point cloud.
  %
  %  Save the point cloud as well as the segmentation labels to ply
  %  file, with color-coded segmentation labels.  During loading, the
  %  color-coded segmentation labels can be parsed to recreate the
  %  labels.
  %
  % @param[in]   fileName    Name of file to save to.
  %
  function saveToFile(this, fileName, varargin)

  if (nargin < 3)
    varargin = {'PLYFormat', 'binary'}
  end

  pcwrite(this, fileName, varargin{:});

  end

  %============================ loadFromFile ===========================
  %
  % @fn  void loadFromFile(string fileName)
  %
  % @brief   Load point cloud from PLY file into current object.
  %
  %  Loads a point cloud from a PLY file and in the process destroys any
  %  pre-existing points stored here.  To instantiate a new object from
  %  a file see: pts3D.readFromFile.  If the filename does not exist,
  %  then the points are cleared and an empty point set results.
  %
  % @param[in]   fileName    The name of the PLY file to load.
  %
  function loadFromFile(this, fileName)

  if (exist(fileName,'file'))
    this.pts = pcread(fileName);
  else
    this.pts = [];
  end

  end

%)
%-------------------------------------------------------------------------
    
end 

%)
%
%============================= Static Methods ============================
%
%(

methods(Static)

  %============================ readFromFile ===========================
  %
  % @fn     ptCloud readFromFile(string fileName)
  % @brief  Reads a point cloud data file and instantiates pts3D object.
  %
  %  Read the point cloud from a ply file.  Ignore any
  %  information embedded in the data (this would be color information).
  %  It might still be there, but it won't be used to decipher any
  %  meaning from the point cloud.
  %
  % @param[in]  fileName    The filename to load.
  %
  % @return     A pts3D object (it will be empty if file doesn't exist).
  %
  function ptCloud = readFromFile(fileName)

  if (exist(fileName,'file'))
    pts = pcread(fileName);
    ptCloud = pts3D(pts);
  else
    ptCloud = pts3D();
  end

  end

end
%)
  
end %) Class definition.

%
%================================= pts3D =================================