#ifndef EQUIDISTANTCAMERA_H
#define EQUIDISTANTCAMERA_H

#include <opencv2/core/core.hpp>
#include <string>

#include "Camera.h"
#include "ceres/rotation.h"

namespace camodocal {

/**
 * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method
 * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006
 */

class EquidistantCamera : public Camera {
 public:
  class Parameters : public Camera::Parameters {
   public:
    Parameters();
    Parameters(const std::string& cameraName,
               int w,
               int h,
               double k2,
               double k3,
               double k4,
               double k5,
               double mu,
               double mv,
               double u0,
               double v0);

    double& k2(void);
    double& k3(void);
    double& k4(void);
    double& k5(void);
    double& mu(void);
    double& mv(void);
    double& u0(void);
    double& v0(void);

    double k2(void) const;
    double k3(void) const;
    double k4(void) const;
    double k5(void) const;
    double mu(void) const;
    double mv(void) const;
    double u0(void) const;
    double v0(void) const;

    bool readFromYamlFile(const std::string& filename);
    void writeToYamlFile(const std::string& filename) const;

    Parameters& operator=(const Parameters& other);
    friend std::ostream& operator<<(std::ostream& out, const Parameters& params);

   private:
    // projection
    double m_k2;
    double m_k3;
    double m_k4;
    double m_k5;

    double m_mu;
    double m_mv;
    double m_u0;
    double m_v0;
  };

  EquidistantCamera();

  /**
   * \brief Constructor from the projection model parameters
   */
  EquidistantCamera(const std::string& cameraName,
                    int imageWidth,
                    int imageHeight,
                    double k2,
                    double k3,
                    double k4,
                    double k5,
                    double mu,
                    double mv,
                    double u0,
                    double v0);
  /**
   * \brief Constructor from the projection model parameters
   */
  EquidistantCamera(const Parameters& params);

  Camera::ModelType modelType(void) const;
  const std::string& cameraName(void) const;
  int imageWidth(void) const;
  int imageHeight(void) const;

  void estimateIntrinsics(const cv::Size& boardSize,
                          const std::vector<std::vector<cv::Point3f> >& objectPoints,
                          const std::vector<std::vector<cv::Point2f> >& imagePoints);

  // Lift points from the image plane to the sphere
  virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
  //%output P

  // Lift points from the image plane to the projective space
  void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
  //%output P

  // Projects 3D points to the image plane (Pi function)
  void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;
  //%output p

  // Projects 3D points to the image plane (Pi function)
  // and calculates jacobian
  void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix<double, 2, 3>& J) const;
  //%output p
  //%output J

  void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;
  //%output p

  template <typename T>
  static void spaceToPlane(const T* const params,
                           const T* const q,
                           const T* const t,
                           const Eigen::Matrix<T, 3, 1>& P,
                           Eigen::Matrix<T, 2, 1>& p);

  void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;
  cv::Mat initUndistortRectifyMap(cv::Mat& map1,
                                  cv::Mat& map2,
                                  float fx = -1.0f,
                                  float fy = -1.0f,
                                  cv::Size imageSize = cv::Size(0, 0),
                                  float cx = -1.0f,
                                  float cy = -1.0f,
                                  cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;

  int parameterCount(void) const;

  const Parameters& getParameters(void) const;
  void setParameters(const Parameters& parameters);

  void readParameters(const std::vector<double>& parameterVec);
  void writeParameters(std::vector<double>& parameterVec) const;

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

  std::string parametersToString(void) const;

 private:
  template <typename T>
  static T r(T k2, T k3, T k4, T k5, T theta);

  void fitOddPoly(const std::vector<double>& x, const std::vector<double>& y, int n, std::vector<double>& coeffs) const;

  void backprojectSymmetric(const Eigen::Vector2d& p_u, double& theta, double& phi) const;

  Parameters mParameters;

  double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
};

typedef boost::shared_ptr<EquidistantCamera> EquidistantCameraPtr;
typedef boost::shared_ptr<const EquidistantCamera> EquidistantCameraConstPtr;

template <typename T>
T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) {
  // k1 = 1
  return theta + k2 * theta * theta * theta + k3 * theta * theta * theta * theta * theta +
         k4 * theta * theta * theta * theta * theta * theta * theta +
         k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta;
}

template <typename T>
void EquidistantCamera::spaceToPlane(const T* const params,
                                     const T* const q,
                                     const T* const t,
                                     const Eigen::Matrix<T, 3, 1>& P,
                                     Eigen::Matrix<T, 2, 1>& p) {
  T P_w[3];
  P_w[0] = T(P(0));
  P_w[1] = T(P(1));
  P_w[2] = T(P(2));

  // Convert quaternion from Eigen convention (x, y, z, w)
  // to Ceres convention (w, x, y, z)
  T q_ceres[4] = {q[3], q[0], q[1], q[2]};

  T P_c[3];
  ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);

  P_c[0] += t[0];
  P_c[1] += t[1];
  P_c[2] += t[2];

  // project 3D object point to the image plane;
  T k2 = params[0];
  T k3 = params[1];
  T k4 = params[2];
  T k5 = params[3];
  T mu = params[4];
  T mv = params[5];
  T u0 = params[6];
  T v0 = params[7];

  T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);
  T theta = acos(P_c[2] / len);
  T phi = atan2(P_c[1], P_c[0]);

  Eigen::Matrix<T, 2, 1> p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix<T, 2, 1>(cos(phi), sin(phi));

  p(0) = mu * p_u(0) + u0;
  p(1) = mv * p_u(1) + v0;
}

}  // namespace camodocal

#endif
