#ifndef SPACE_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_
#define SPACE_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_

#include <memory>
#include <vector>

#include "Eigen/Core"
#include "ceres/ceres.h"

#include "map/map.h"
#include "common/common.h"
#include "ceres_matcher/occupied_space_cost_function_2d.h"
#include "ceres_matcher/rotation_delta_cost_functor_2d.h"
#include "ceres_matcher/translation_delta_cost_functor_2d.h"

namespace hlzn_slam {
namespace ceres_matching {

// Align scans with an existing map using Ceres.
class CeresScanMatcher2D {
 public:
  explicit CeresScanMatcher2D(const double& occupied_space_weight, const double& translation_weight,
                                const double& rotation_weight, const double& self_stab_weight);
  virtual ~CeresScanMatcher2D();

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

  // Aligns 'point_cloud' within the 'grid' given an
  // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
  // 'summary'.
  void Match(const common::Rigid2f& target_pose,
             const common::Rigid2f& initial_pose_estimate,
             const common::PointCloud& point_cloud, 
             Map& map,
             common::Rigid2f* pose_estimate,
             ceres::Solver::Summary* summary) const;

 private:
  ceres::Solver::Options ceres_solver_options_;
  double occupied_space_weight_;
  double translation_weight_;
  double rotation_weight_;
  double self_stab_weight_;
};

}  // namespace scan_matching
}  // namespace cartographer

#endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_