/*
 * Copyright 2016 The Cartographer Authors
 *
 * 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.
 */

#ifndef CARTOGRAPHER_SENSOR_POINT_CLOUD_H_
#define CARTOGRAPHER_SENSOR_POINT_CLOUD_H_

#include <vector>

#include "Eigen/Core"
#include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/sensor/rangefinder_point.h"
#include "cartographer/transform/rigid_transform.h"
#include "glog/logging.h"

namespace cartographer {
namespace sensor {

// Stores 3D positions of points.
// For 2D points, the third entry is 0.f.
using PointCloud = std::vector<RangefinderPoint>;

// Stores 3D positions of points with their relative measurement time in the
// fourth entry. Time is in seconds, increasing and relative to the moment when
// the last point was acquired. So, the fourth entry for the last point is 0.f.
// If timing is not available, all fourth entries are 0.f. For 2D points, the
// third entry is 0.f (and the fourth entry is time).
using TimedPointCloud = std::vector<TimedRangefinderPoint>;

struct PointCloudWithIntensities {
  TimedPointCloud points;
  std::vector<float> intensities;
};

// Transforms 'point_cloud' according to 'transform'.
PointCloud TransformPointCloud(const PointCloud& point_cloud,
                               const transform::Rigid3f& transform);

// Transforms 'point_cloud' according to 'transform'.
TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
                                         const transform::Rigid3f& transform);

// Returns a new point cloud without points that fall outside the region defined
// by 'min_z' and 'max_z'.
PointCloud CropPointCloud(const PointCloud& point_cloud, float min_z,
                          float max_z);

// Returns a new point cloud without points that fall outside the region defined
// by 'min_z' and 'max_z'.
TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud,
                                    float min_z, float max_z);

///////////////////////////////////////////////////////////////////////////
// Copy from LOAM

struct LoamPoint;

#define LOAM_MAX_RING 64
#define LOAM_LIDAR_TYPE_32E 0
#define LOAM_LIDAR_TYPE_40P 1

class LoamProjection {
 public:
  LoamProjection(int lidar_type);
  ~LoamProjection();

  int FilterCloud(TimedPointCloud& laserCloud, const float highResRange);

 private:
  void ProjectPointCloud(const TimedPointCloud& laserCloudIn);
  void GroundRemoval();
  void LabelComponents(int row, int col);

 private:
  const float sensorMountAngle = 0.0;           // installation angle
  std::vector<std::pair<int8_t, int8_t>> neighbors;

  int size;
  LoamPoint* projCloud;
  uint16_t* queueRow;      // breadth-first search process of segmentation
  uint16_t* queueCol;
  int labelCount;

  size_t N_SCAN, HORIZON_SCAN, N_GROUND_SCAN;
  const float* N_SCAN_ANGLE;
  float HORIZON_ANGLE;
  float N_SCAN_RAD[LOAM_MAX_RING];
  float HORIZON_RAD;

  const float SCAN_ANGLE_32E[LOAM_MAX_RING] = {
      -30.67, -29.33, -28.00, -26.67, -25.33, -24.00, -22.67, -21.33,
      -20.00, -18.67, -17.33, -16.00, -14.67, -13.33, -12.00, -10.67,
      -9.33,  -8.00,  -6.67,  -5.33,  -4.00,  -2.67,  -1.33,  0.00,
      1.33,   2.67,   4.00,   5.33,   6.67,   8.00,   9.33,   10.67};
  const float SCAN_ANGLE_40P[LOAM_MAX_RING] = {
      -25,   -19,   -14,   -13,   -12,   -11,   -10,   -9,    -8,    -7,
      -6,    -5.67, -5.33, -5,    -4.67, -4.33, -4,    -3.67, -3.33, -3,
      -2.67, -2.33, -2,    -1.67, -1.33, -1,    -0.67, -0.33, 0,     0.33,
      0.67,  1,     1.33,  1.67,  2,     3,     5,     8,     11,    15};

  const float SENSOR_MIN_RANGE = 1.0;
  const float SEGMENT_GROUND_ANGLE = 6;                   // default is 10
  const float SEGMENT_HORIZON_PLANE = 10.0 / 180.0 * M_PI;// default is 10
  const float SEGMENT_SAME_LABEL = 50.0 / 180.0 * M_PI;   // default is 60
  const int SEGMENT_VALID_NUM = 50;
  const int SEGMENT_VALID_MIN_WIDTH = 10;
  const int SEGMENT_VALID_MIN_LINE = 5;
};

}  // namespace sensor
}  // namespace cartographer

#endif  // CARTOGRAPHER_SENSOR_POINT_CLOUD_H_