//=========================================================================
//
// Copyright 2018 Kitware, Inc.
// Author: Guilbert Pierre (spguilbert@gmail.com)
// Date: 03-27-2018
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//=========================================================================

// This slam algorithm is inspired by the LOAM algorithm:
// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.

// The algorithm is composed of three sequential steps:
//
// - Keypoints extraction: this step consists of extracting keypoints over
// the points clouds. To do that, the laser lines / scans are trated indepently.
// The laser lines are projected onto the XY plane and are rescale depending on
// their vertical angle. Then we compute their curvature and create two class of
// keypoints. The edges keypoints which correspond to points with a hight curvature
// and planar points which correspond to points with a low curvature.
//
// - Ego-Motion: this step consists of recovering the motion of the lidar
// sensor between two frames (two sweeps). The motion is modelized by a constant
// velocity and angular velocity between two frames (i.e null acceleration). 
// Hence, we can parameterize the motion by a rotation and translation per sweep / frame
// and interpolate the transformation inside a frame using the timestamp of the points.
// Since the points clouds generated by a lidar are sparses we can't design a
// pairwise match between keypoints of two successive frames. Hence, we decided to use
// a closest-point matching between the keypoints of the current frame
// and the geometrics features derived from the keypoints of the previous frame.
// The geometrics features are lines or planes and are computed using the edges keypoints
// and planar keypoints of the previous frame. Once the matching is done, a keypoint
// of the current frame is matched with a plane / line (depending of the
// nature of the keypoint) from the previous frame. Then, we recover R and T by
// minimizing the function f(R, T) = sum(d(point, line)^2) + sum(d(point, plane)^2).
// Which can be writen f(R, T) = sum((R*X+T-P).t*A*(R*X+T-P)) where:
// - X is a keypoint of the current frame
// - P is a point of the corresponding line / plane
// - A = (n*n.t) with n being the normal of the plane
// - A = (I - n*n.t).t * (I - n*n.t) with n being a director vector of the line
// Since the function f(R, T) is a non-linear mean square error function
// we decided to use the Levenberg-Marquardt algorithm to recover its argmin.
//
// - Mapping: This step consists of refining the motion recovered in the Ego-Motion
// step and to add the new frame in the environment map. Thanks to the ego-motion
// recovered at the previous step it is now possible to estimate the new position of
// the sensor in the map. We use this estimation as an initial point (R0, T0) and we
// perform an optimization again using the keypoints of the current frame and the matched
// keypoints of the map (and not only the previous frame this time!). Once the position in the
// map has been refined from the first estimation it is then possible to update the map by
// adding the keypoints of the current frame into the map.
//
// In the following programs : "vtkSlam.h" and "vtkSlam.cxx" the lidar
// coordinate system {L} is a 3D coordinate system with its origin at the
// geometric center of the lidar. The world coordinate system {W} is a 3D
// coordinate system which coinciding with {L] at the initial position. The
// points will be denoted by the ending letter L or W if they belong to
// the corresponding coordinate system

#ifndef VTK_SLAM_H
#define VTK_SLAM_H

// VTK
#include <vtkPolyDataAlgorithm.h>
#include <vtkSmartPointer.h>

// EIGEN
#include <Eigen/Dense>

// LOCAL
#include "vtkPCLConversions.h"
#include "Slam.h"
#include "KalmanFilter.h"
#include "vtkTemporalTransforms.h"

// This custom macro is needed to make the SlamManager time agnostic
// The SlamManager need to know when RequestData is call, if it's due
// to a new timestep been requested or due to Slam parameters been changed.
// By keeping track of the last time the parameters been modified there is
// no ambiguty anymore. This mecanimsm is similar to the one usedby the paraview filter
// PlotDataOverTime
#define vtkCustomSetMacro(name,type) \
virtual void Set##name (type _arg) \
{ \
  vtkDebugMacro(<< this->GetClassName() << " (" << this << "): setting " #name " to " << _arg); \
  if (this->SlamAlgo.Get##name() != _arg) \
  { \
    this->SlamAlgo.Set##name(_arg); \
    this->Modified(); \
    this->ParametersModificationTime.Modified(); \
  } \
}

#define vtkCustomGetMacro(name,type) \
virtual type Get##name () { \
  vtkDebugMacro(<< this->GetClassName() << " (" << this << "): returning " << #name " of " << this->SlamAlgo.Get##name() ); \
  return this->SlamAlgo.Get##name(); \
}

class vtkSpinningSensorKeypointExtractor;
class vtkTable;

class VTK_EXPORT vtkSlam : public vtkPolyDataAlgorithm
{
public:
  static vtkSlam *New();
  vtkTypeMacro(vtkSlam, vtkPolyDataAlgorithm)
  void PrintSelf(ostream& os, vtkIndent indent);

  // Get/Set General
  vtkGetMacro(DisplayMode, bool)
  vtkSetMacro(DisplayMode, bool)

  vtkCustomGetMacro(MaxDistBetweenTwoFrames, double)
  vtkCustomSetMacro(MaxDistBetweenTwoFrames, double)

  vtkCustomGetMacro(MaxDistanceForICPMatching, double)
  vtkCustomSetMacro(MaxDistanceForICPMatching, double)

  vtkCustomGetMacro(FastSlam, bool)
  vtkCustomSetMacro(FastSlam, bool)

  vtkCustomGetMacro(Undistortion, bool)
  vtkCustomSetMacro(Undistortion, bool)

  vtkGetObjectMacro(KeyPointsExtractor, vtkSpinningSensorKeypointExtractor)
  virtual void SetKeyPointsExtractor(vtkSpinningSensorKeypointExtractor *);

  // Set RollingGrid Parameters
  void SetVoxelGridLeafSizeEdges(double size);
  void SetVoxelGridLeafSizePlanes(double size);
  void SetVoxelGridLeafSizeBlobs(double size);
  void SetVoxelGridSize(unsigned int size);
  void SetVoxelGridResolution(double resolution);

  // Get/Set EgoMotion
  vtkCustomGetMacro(EgoMotionLMMaxIter, unsigned int)
  vtkCustomSetMacro(EgoMotionLMMaxIter, unsigned int)

  vtkCustomGetMacro(EgoMotionICPMaxIter, unsigned int)
  vtkCustomSetMacro(EgoMotionICPMaxIter, unsigned int)

  vtkCustomGetMacro(EgoMotionLineDistanceNbrNeighbors, unsigned int)
  vtkCustomSetMacro(EgoMotionLineDistanceNbrNeighbors, unsigned int)

  vtkCustomGetMacro(EgoMotionMinimumLineNeighborRejection, unsigned int)
  vtkCustomSetMacro(EgoMotionMinimumLineNeighborRejection, unsigned int)

  vtkCustomGetMacro(EgoMotionLineDistancefactor, double)
  vtkCustomSetMacro(EgoMotionLineDistancefactor, double)

  vtkCustomGetMacro(EgoMotionPlaneDistanceNbrNeighbors, unsigned int)
  vtkCustomSetMacro(EgoMotionPlaneDistanceNbrNeighbors, unsigned int)

  vtkCustomGetMacro(EgoMotionPlaneDistancefactor1, double)
  vtkCustomSetMacro(EgoMotionPlaneDistancefactor1, double)

  vtkCustomGetMacro(EgoMotionPlaneDistancefactor2, double)
  vtkCustomSetMacro(EgoMotionPlaneDistancefactor2, double)

  vtkCustomGetMacro(EgoMotionMaxLineDistance, double)
  vtkCustomSetMacro(EgoMotionMaxLineDistance, double)

  vtkCustomGetMacro(EgoMotionMaxPlaneDistance, double)
  vtkCustomSetMacro(EgoMotionMaxPlaneDistance, double)

  // Get/Set Mapping
  vtkCustomGetMacro(MappingLMMaxIter, unsigned int)
  vtkCustomSetMacro(MappingLMMaxIter, unsigned int)

  vtkCustomGetMacro(MappingICPMaxIter, unsigned int)
  vtkCustomSetMacro(MappingICPMaxIter, unsigned int)

  vtkCustomGetMacro(MappingLineDistanceNbrNeighbors, unsigned int)
  vtkCustomSetMacro(MappingLineDistanceNbrNeighbors, unsigned int)

  vtkCustomGetMacro(MappingMinimumLineNeighborRejection, unsigned int)
  vtkCustomSetMacro(MappingMinimumLineNeighborRejection, unsigned int)

  vtkCustomGetMacro(MappingLineDistancefactor, double)
  vtkCustomSetMacro(MappingLineDistancefactor, double)

  vtkCustomGetMacro(MappingPlaneDistanceNbrNeighbors, unsigned int)
  vtkCustomSetMacro(MappingPlaneDistanceNbrNeighbors, unsigned int)

  vtkCustomGetMacro(MappingPlaneDistancefactor1, double)
  vtkCustomSetMacro(MappingPlaneDistancefactor1, double)

  vtkCustomGetMacro(MappingPlaneDistancefactor2, double)
  vtkCustomSetMacro(MappingPlaneDistancefactor2, double)

  vtkCustomGetMacro(MappingMaxLineDistance, double)
  vtkCustomSetMacro(MappingMaxLineDistance, double)

  vtkCustomGetMacro(MappingMaxPlaneDistance, double)
  vtkCustomSetMacro(MappingMaxPlaneDistance, double)

  vtkCustomGetMacro(MappingLineMaxDistInlier, double)
  vtkCustomSetMacro(MappingLineMaxDistInlier, double)

protected:
  vtkSlam();
  void Reset();

  int FillInputPortInformation(int port, vtkInformation* info) override;
  int RequestData(vtkInformation *, vtkInformationVector **, vtkInformationVector *) override;

  // Keeps track of the time the parameters have been modified
  // This will enable the SlamManager to be time-agnostic
  // MTime is a much more general mecanism so we can't rely on it
  vtkTimeStamp ParametersModificationTime;

  Slam SlamAlgo;
  vtkSpinningSensorKeypointExtractor* KeyPointsExtractor = nullptr;

private:
  vtkSlam(const vtkSlam&) = delete;
  void operator = (const vtkSlam&) = delete;

  // Polydata which represents the trajectory computed
  vtkSmartPointer<vtkTemporalTransforms> Trajectory;

  std::vector<size_t> GetLaserIdMapping(vtkTable *calib);

  // Indicate if we are in display mode or not
  // Display mode will add arrays showing some
  // results of the slam algorithm such as
  // the keypoints extracted, curvature etc
  bool DisplayMode = false;
};

#endif // VTK_SLAM_H
