//=========================================================================
//
// 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"

#include "LidarCoreModule.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 LIDARCORE_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)

  vtkCustomGetMacro(EgoMotionInitLossScale, double)
  vtkCustomSetMacro(EgoMotionInitLossScale, double)

  vtkCustomGetMacro(EgoMotionFinalLossScale, double)
  vtkCustomSetMacro(EgoMotionFinalLossScale, 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)

  vtkCustomGetMacro(MappingInitLossScale, double)
  vtkCustomSetMacro(MappingInitLossScale, double)

  vtkCustomGetMacro(MappingFinalLossScale, double)
  vtkCustomSetMacro(MappingFinalLossScale, 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 = true;
};

template <typename T>
std::vector<size_t> sortIdx(const std::vector<T> &v);

void PointCloudFromPolyData(vtkPolyData* poly, pcl::PointCloud<Slam::Point>::Ptr pc);

#endif // VTK_SLAM_H
