#include "pose_graph/occupied_space_cost_function.h"
#include "ceres/cubic_interpolation.h"
#include "common/common.h"
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/Geometry> 

namespace hlzn_slam {
namespace ceres_matching {
namespace {

template<typename T>
T ROUND(T x) // 四舍五入 
{
    return (x > 0.0) ? floor(x + 0.5) : ceil(x - 0.5);
}
// Computes a cost for matching the 'point_cloud' to the 'grid' with
// a 'pose'. The cost increases with poorer correspondence of the grid and the
// point observation (e.g. points falling into less occupied space).
class OccupiedSpaceCostFunction {
 public:
  OccupiedSpaceCostFunction(const double scaling_factor,
                              const common::PointCloud& point_cloud,
                              Map& map)
      : scaling_factor_(scaling_factor),
        point_cloud_(point_cloud),
        map_(map)
        {
          adapter_ = new GridArrayAdapter(map);
          interpolator_ = new ceres::BiCubicInterpolator<GridArrayAdapter>(*adapter_);
        }
        
  ~OccupiedSpaceCostFunction() {
    delete adapter_;
    delete interpolator_;
  }

  template <typename T>
  void mapToWorld(const T& x, const T& y, T& world_x, T& world_y) const {
    const T& x_origin = T(map_.getXorigin());
    const T& y_origin = T(map_.getYorigin());
    const T& reciprocal_resolution = T(map_.getReciprocalResolution());

    world_x = (T)map_.getRows()  - (y + y_origin) * reciprocal_resolution;
    world_y = (x + x_origin) * reciprocal_resolution;
  }

  template <typename T>
  bool operator()(const T* const pose, const T* const quaternion, T* residual) const {
    Eigen::Matrix<T, 3, 1> translation(pose[0], pose[1], pose[2]);
    Eigen::Quaternion<T> Q(quaternion[3], quaternion[0], quaternion[1], quaternion[2]);
    Eigen::Matrix<T, 3, 3> R = Q.toRotationMatrix();

    for (size_t i = 0; i < point_cloud_.data.size(); ++i) {
      // Note that this is a 2D point. The third component is a scaling factor.
      const Eigen::Matrix<T, 3, 1> point(T(point_cloud_.data[i](0)),
                                         T(point_cloud_.data[i](1)),
                                         T(1.));
      const Eigen::Matrix<T, 3, 1> world = R * point + translation;
      T x, y;

      mapToWorld(world[0], world[1], x, y);
      interpolator_->Evaluate(x, y, &residual[i]);
      residual[i] = scaling_factor_ * residual[i];
    }
    return true;
  }

 private:
   class GridArrayAdapter {
   public:
    enum { DATA_DIMENSION = 1 };

    explicit GridArrayAdapter(Map& map) : map_(map) {}

    void GetValue(const int row, const int column, double* const value) const {
      if (row < 0 || column < 0 || row >= NumRows() ||
          column >= NumCols()) {
        *value = 1.;
      } else {
        float probablity = 1.0 - map_.getPixelProbablity(row, column);
        
        *value = static_cast<double>(probablity);
      }
    }

    int NumRows() const {
      return map_.getRows() - 1;
    }

    int NumCols() const {
      return map_.getCols() - 1;
    }

   private:
    Map& map_;
  };

  OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
  OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
      delete;

  const double scaling_factor_;
  const common::PointCloud& point_cloud_;
  Map& map_;
  GridArrayAdapter* adapter_;
  ceres::BiCubicInterpolator<GridArrayAdapter>* interpolator_;
};

}  // namespace

ceres::CostFunction* CreateOccupiedSpaceCostFunction(
    const double scaling_factor, const common::PointCloud& point_cloud,
    Map& map) {
  return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction,
                                         ceres::DYNAMIC /* residuals */,
                                         3 /* pose variables */,
                                         4>(
      new OccupiedSpaceCostFunction(scaling_factor, point_cloud, map),
      point_cloud.data.size());
}

}  // namespace scan_matching
}  // namespace cartographer