#ifndef  camera_INC
#define  camera_INC
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <yaml-cpp/yaml.h>

#include "types.h"
#define BINNING 0.5 // set the binning factor
#define YAWCORRECT 1.0
//#define DOYAWCORRECT
using Eigen::Matrix;
using Eigen::Vector4d;
using Eigen::Vector3d;
using Eigen::Quaterniond;
using std::cout;
using std::cerr;
using std::endl;

/*
 * =====================================================================================
 *        Class:  Camera
 *  Description:  Class for the camera.
 * =====================================================================================
 */
class Camera
{
    public:
        /* ====================  LIFECYCLE     ======================================= */
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        Camera (const char *fin);                             /* constructor */

        /* ====================  ACCESSORS     ======================================= */
        Vector4d d() const;
        cv::Vec4d d( int x ) const;
        Matrix<double,3,3> K() const;
        cv::Matx33d K(int x) const;
        Matrix<double,4,4> K4() const;
        Vector3d principalPoint() const;
        Matrix<double,3,3> Rc2b() const;
        Matrix<double,4,4> Rc2b4() const;

        /* ====================  MUTATORS      ======================================= */

        /* ====================  OPERATORS     ======================================= */
        Vector3d img2body(const Vector3d &xi) const;
        Vector3d body2img(const Vector3d &xb) const;
        cv::Mat warpPatch(const cv::Mat &p, const Quaterniond &q0, const Quaterniond &q1) const;
        cv::Mat reflectPatch( const cv::Mat &p, const Quaterniond &q0, const Quaterniond &q1 ) const;

    protected:
        /* ====================  METHODS       ======================================= */
        double roll(const Quaterniond &q) const;
        double pitch(const Quaterniond &q) const;

        /* ====================  DATA MEMBERS  ======================================= */

    private:
        /* ====================  METHODS       ======================================= */

        /* ====================  DATA MEMBERS  ======================================= */
        Matrix<double,3,3> _K;
        Matrix<double,4,4> _T;
        Vector4d _d;

}; /* -----  end of class Camera  ----- */


#endif   /* ----- #ifndef camera_INC  ----- */