#ifndef CAMERACALIBRATION_H
#define CAMERACALIBRATION_H

#include <opencv2/core/core.hpp>

#include "camodocal/camera_models/Camera.h"

namespace camodocal {

class CameraCalibration {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  CameraCalibration();

  CameraCalibration(Camera::ModelType modelType,
                    const std::string& cameraName,
                    const cv::Size& imageSize,
                    const cv::Size& boardSize,
                    float squareSize);

  void clear(void);

  void addChessboardData(const std::vector<cv::Point2f>& corners);

  bool calibrate(void);

  int sampleCount(void) const;
  std::vector<std::vector<cv::Point2f> >& imagePoints(void);
  const std::vector<std::vector<cv::Point2f> >& imagePoints(void) const;
  std::vector<std::vector<cv::Point3f> >& scenePoints(void);
  const std::vector<std::vector<cv::Point3f> >& scenePoints(void) const;
  CameraPtr& camera(void);
  const CameraConstPtr camera(void) const;

  Eigen::Matrix2d& measurementCovariance(void);
  const Eigen::Matrix2d& measurementCovariance(void) const;

  cv::Mat& cameraPoses(void);
  const cv::Mat& cameraPoses(void) const;

  void drawResults(std::vector<cv::Mat>& images) const;

  void writeParams(const std::string& filename) const;

  bool writeChessboardData(const std::string& filename) const;
  bool readChessboardData(const std::string& filename);

  void setVerbose(bool verbose);

 private:
  bool calibrateHelper(CameraPtr& camera, std::vector<cv::Mat>& rvecs, std::vector<cv::Mat>& tvecs) const;

  void optimize(CameraPtr& camera, std::vector<cv::Mat>& rvecs, std::vector<cv::Mat>& tvecs) const;

  template <typename T>
  void readData(std::ifstream& ifs, T& data) const;

  template <typename T>
  void writeData(std::ofstream& ofs, T data) const;

  cv::Size m_boardSize;
  float m_squareSize;

  CameraPtr m_camera;
  cv::Mat m_cameraPoses;

  std::vector<std::vector<cv::Point2f> > m_imagePoints;
  std::vector<std::vector<cv::Point3f> > m_scenePoints;

  Eigen::Matrix2d m_measurementCovariance;

  bool m_verbose;
};

}  // namespace camodocal

#endif
