#include "camodocal/camera_models/Camera.h"
#include "camodocal/camera_models/ScaramuzzaCamera.h"

#include <opencv2/calib3d/calib3d.hpp>

namespace camodocal {

Camera::Parameters::Parameters(ModelType modelType) : m_modelType(modelType), m_imageWidth(0), m_imageHeight(0) {
  switch (modelType) {
    case KANNALA_BRANDT:
      m_nIntrinsics = 8;
      break;
    case PINHOLE:
      m_nIntrinsics = 8;
      break;
    case SCARAMUZZA:
      m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;
      break;
    case MEI:
    default:
      m_nIntrinsics = 9;
  }
}

Camera::Parameters::Parameters(ModelType modelType, const std::string& cameraName, int w, int h)
    : m_modelType(modelType), m_cameraName(cameraName), m_imageWidth(w), m_imageHeight(h) {
  switch (modelType) {
    case KANNALA_BRANDT:
      m_nIntrinsics = 8;
      break;
    case PINHOLE:
      m_nIntrinsics = 8;
      break;
    case SCARAMUZZA:
      m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS;
      break;
    case MEI:
    default:
      m_nIntrinsics = 9;
  }
}

Camera::ModelType& Camera::Parameters::modelType(void) { return m_modelType; }

std::string& Camera::Parameters::cameraName(void) { return m_cameraName; }

int& Camera::Parameters::imageWidth(void) { return m_imageWidth; }

int& Camera::Parameters::imageHeight(void) { return m_imageHeight; }

Camera::ModelType Camera::Parameters::modelType(void) const { return m_modelType; }

const std::string& Camera::Parameters::cameraName(void) const { return m_cameraName; }

int Camera::Parameters::imageWidth(void) const { return m_imageWidth; }

int Camera::Parameters::imageHeight(void) const { return m_imageHeight; }

int Camera::Parameters::nIntrinsics(void) const { return m_nIntrinsics; }

cv::Mat& Camera::mask(void) { return m_mask; }

const cv::Mat& Camera::mask(void) const { return m_mask; }

void Camera::estimateExtrinsics(const std::vector<cv::Point3f>& objectPoints,
                                const std::vector<cv::Point2f>& imagePoints,
                                cv::Mat& rvec,
                                cv::Mat& tvec) const {
  std::vector<cv::Point2f> Ms(imagePoints.size());
  for (size_t i = 0; i < Ms.size(); ++i) {
    Eigen::Vector3d P;
    liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);

    P /= P(2);

    Ms.at(i).x = P(0);
    Ms.at(i).y = P(1);
  }

  // assume unit focal length, zero principal point, and zero distortion
  cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec);
}

double Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const {
  Eigen::Vector2d p1, p2;

  spaceToPlane(P1, p1);
  spaceToPlane(P2, p2);

  return (p1 - p2).norm();
}

double Camera::reprojectionError(const std::vector<std::vector<cv::Point3f> >& objectPoints,
                                 const std::vector<std::vector<cv::Point2f> >& imagePoints,
                                 const std::vector<cv::Mat>& rvecs,
                                 const std::vector<cv::Mat>& tvecs,
                                 cv::OutputArray _perViewErrors) const {
  int imageCount = objectPoints.size();
  size_t pointsSoFar = 0;
  double totalErr = 0.0;

  bool computePerViewErrors = _perViewErrors.needed();
  cv::Mat perViewErrors;
  if (computePerViewErrors) {
    _perViewErrors.create(imageCount, 1, CV_64F);
    perViewErrors = _perViewErrors.getMat();
  }

  for (int i = 0; i < imageCount; ++i) {
    size_t pointCount = imagePoints.at(i).size();

    pointsSoFar += pointCount;

    std::vector<cv::Point2f> estImagePoints;
    projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), estImagePoints);

    double err = 0.0;
    for (size_t j = 0; j < imagePoints.at(i).size(); ++j) {
      err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j));
    }

    if (computePerViewErrors) {
      perViewErrors.at<double>(i) = err / pointCount;
    }

    totalErr += err;
  }

  return totalErr / pointsSoFar;
}

double Camera::reprojectionError(const Eigen::Vector3d& P,
                                 const Eigen::Quaterniond& camera_q,
                                 const Eigen::Vector3d& camera_t,
                                 const Eigen::Vector2d& observed_p) const {
  Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t;

  Eigen::Vector2d p;
  spaceToPlane(P_cam, p);

  return (p - observed_p).norm();
}

void Camera::projectPoints(const std::vector<cv::Point3f>& objectPoints,
                           const cv::Mat& rvec,
                           const cv::Mat& tvec,
                           std::vector<cv::Point2f>& imagePoints) const {
  // project 3D object points to the image plane
  imagePoints.reserve(objectPoints.size());

  // double
  cv::Mat R0;
  cv::Rodrigues(rvec, R0);

  Eigen::MatrixXd R(3, 3);
  R << R0.at<double>(0, 0), R0.at<double>(0, 1), R0.at<double>(0, 2), R0.at<double>(1, 0), R0.at<double>(1, 1),
      R0.at<double>(1, 2), R0.at<double>(2, 0), R0.at<double>(2, 1), R0.at<double>(2, 2);

  Eigen::Vector3d t;
  t << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);

  for (size_t i = 0; i < objectPoints.size(); ++i) {
    const cv::Point3f& objectPoint = objectPoints.at(i);

    // Rotate and translate
    Eigen::Vector3d P;
    P << objectPoint.x, objectPoint.y, objectPoint.z;

    P = R * P + t;

    Eigen::Vector2d p;
    spaceToPlane(P, p);

    imagePoints.push_back(cv::Point2f(p(0), p(1)));
  }
}

}  // namespace camodocal
