/*
 * =====================================================================================
 *
 *       Filename:  camera.cpp
 *
 *    Description:  Class for camera
 *
 *        Version:  1.0
 *        Created:  03/19/2017 09:39:58 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Martin Miller (MHM), miller7@illinois.edu
 *   Organization:  Aerospace Robotics and Controls Lab (ARC)
 *
 * =====================================================================================
 */
#include "camera.h"
#include <iostream>

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Camera
 *      Method:  Camera
 * Description:  constructor
 *--------------------------------------------------------------------------------------
 */
Camera::Camera (const char *fin)
{
    YAML::Node config = YAML::LoadFile(fin);

    _K(0,0) = BINNING*config["camera_matrix"]["data"][0].as<double>();
    _K(0,1) = BINNING*config["camera_matrix"]["data"][1].as<double>();
    _K(0,2) = BINNING*config["camera_matrix"]["data"][2].as<double>();
    _K(1,0) = BINNING*config["camera_matrix"]["data"][3].as<double>();
    _K(1,1) = BINNING*config["camera_matrix"]["data"][4].as<double>();
    _K(1,2) = BINNING*config["camera_matrix"]["data"][5].as<double>();
    _K(2,0) = BINNING*config["camera_matrix"]["data"][6].as<double>();
    _K(2,1) = BINNING*config["camera_matrix"]["data"][7].as<double>();
    _K(2,2) = config["camera_matrix"]["data"][8].as<double>();

    _d[0] = config["distortion_coefficients"]["data"][0].as<double>();
    _d[1] = config["distortion_coefficients"]["data"][1].as<double>();
    _d[2] = config["distortion_coefficients"]["data"][2].as<double>();
    _d[3] = config["distortion_coefficients"]["data"][3].as<double>();

    _T(0,0) = config["T_cam_imu"][0][0].as<double>();
    _T(0,1) = config["T_cam_imu"][0][1].as<double>();
    _T(0,2) = config["T_cam_imu"][0][2].as<double>();
    _T(0,3) = config["T_cam_imu"][0][3].as<double>();
    _T(1,0) = config["T_cam_imu"][1][0].as<double>();
    _T(1,1) = config["T_cam_imu"][1][1].as<double>();
    _T(1,2) = config["T_cam_imu"][1][2].as<double>();
    _T(1,3) = config["T_cam_imu"][1][3].as<double>();
    _T(2,0) = config["T_cam_imu"][2][0].as<double>();
    _T(2,1) = config["T_cam_imu"][2][1].as<double>();
    _T(2,2) = config["T_cam_imu"][2][2].as<double>();
    _T(2,3) = config["T_cam_imu"][2][3].as<double>();
    _T(3,0) = config["T_cam_imu"][3][0].as<double>();
    _T(3,1) = config["T_cam_imu"][3][1].as<double>();
    _T(3,2) = config["T_cam_imu"][3][2].as<double>();
    _T(3,3) = config["T_cam_imu"][3][3].as<double>();

}  /* -----  end of method Camera::Camera  (constructor)  ----- */


/*
 *--------------------------------------------------------------------------------------
 *       Class:  Camera
 *      Method:  Camera :: Rc2b
 * Description:  Returns the rotation matrix that transforms a point from the
 * camera frame to the body frame.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,3,3>
Camera::Rc2b (  ) const
{
#ifdef DOYAWCORRECT
    double yaw;
    yaw = M_PI*( YAWCORRECT )/180.;
    Matrix<double,3,3> Ry;
    Ry = Eigen::AngleAxisd(yaw, Vector3d::UnitZ());
    return Ry*_T.block<3,3>(0,0).transpose();
#else
    return _T.block<3,3>(0,0).transpose();
#endif
}		/* -----  end of method Camera::Rc2b  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Camera
 *      Method:  Camera :: Rc2b4
 * Description:  Returns the 4x4 Rc2b matrix
 *--------------------------------------------------------------------------------------
 */
Matrix<double,4,4>
Camera::Rc2b4 ( ) const
{
#ifdef DOYAWCORRECT
    double yaw;
    yaw = M_PI*( YAWCORRECT )/180.;
    Matrix<double,3,3> Ry;
    Ry = Eigen::AngleAxisd(yaw, Vector3d::UnitZ());

    Matrix<double,4,4> R;
    R = Matrix<double,4,4>::Identity();
    R.block<3,3>(0,0) = Ry*Rc2b();
#else
    Matrix<double,4,4> R;
    R = Matrix<double,4,4>::Identity();
    R.block<3,3>(0,0) = Rc2b();
#endif
    return R ;
}		/* -----  end of method Camera::Rc2b4  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Camera
 *      Method:  Camera :: K4
 * Description:  Returns the 4x4 camera matrix.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,4,4>
Camera::K4 ( ) const
{
    Matrix<double,4,4> K4;
    K4 = Matrix<double,4,4>::Identity();
    K4.block<3,3>(0,0) = K();
    return K4 ;
}		/* -----  end of method Camera::K4  ----- */


/*
 *--------------------------------------------------------------------------------------
 *       Class:  Camera
 *      Method:  Camera :: K
 * Description:  Returns the 3x3 camera matrix.
 *--------------------------------------------------------------------------------------
 */
Matrix<double,3,3>
Camera::K (  ) const
{
    return _K ;
}		/* -----  end of method Camera::K  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Camera
 *      Method:  Camera :: d
 * Description:  Returns the distortion coefficients.
 *--------------------------------------------------------------------------------------
 */
Vector4d
Camera::d ( ) const
{
    return _d;
}		/* -----  end of method Camera::d  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  Camera
 *      Method:  Camera :: img2body
 * Description:  Returns image points (pixel coordinates) in the body frame.
 * That is xi: -> K -> xc -> Rc2b -> xb, the point is tranformed to the camera
 * frame by K and to the body frame by Rc2b.
 *--------------------------------------------------------------------------------------
 */
Vector3d
Camera::img2body ( const Vector3d &xi ) const
{
    Vector3d xc, xb;
    xc = K().inverse()*xi;
    // Normalize
    xc /= xc.norm();
    xb = Rc2b()*xc;
    return xb;
}		/* -----  end of method Camera::img2body  ----- */

Vector3d
Camera::body2img ( const Vector3d &xb ) const
{
    Vector3d xc,xi;
    xc = Rc2b().transpose()*xb;
    xi = K()*xc;
    xi /= xi[2];
    return xi;
}		/* -----  end of method Camera::body2img  ----- */

Vector3d
Camera::principalPoint (  ) const
{
    return _K.col(2);
}		/* -----  end of method Camera::principalPoint  ----- */

cv::Mat
Camera::warpPatch ( const cv::Mat &p, const Quaterniond &q0, const Quaterniond &q1 ) const
{
    cv::Mat w;
    Matrix<double,3,3> R;
    R = Rc2b().transpose() * q1.toRotationMatrix().transpose() *
        q0.toRotationMatrix() * Rc2b();
    cv::Matx23d Rmat;
    Rmat << R(0,0), R(0,1), R(0,2),
            R(1,0), R(1,1), R(1,2);
            //R(2,0), R(2,1), R(2,2);
    cv::Size sz(PATCHSIZE,PATCHSIZE);
    cv::warpAffine(p, w, Rmat, sz);

    return w;
}		/* -----  end of method Camera::warpPatch  ----- */

cv::Mat
Camera::reflectPatch( const cv::Mat &p, const Quaterniond &q0, const Quaterniond &q1 ) const
{
    using Eigen::AngleAxisd;
    cv::Mat w1, w2, w3;
    Matrix<double,3,3> R1, R2;
    Matrix<double,3,3> J;
    Matrix<double,3,3> Rbw, Rb0w;
    double roll0, pitch0;
    double roll1, pitch1;

    roll0 = roll(q0);
    pitch0 = pitch(q0);
    roll1 = roll(q1);
    pitch1 = pitch(q1);

    Rb0w = AngleAxisd(roll0, Vector3d::UnitX())
      *  AngleAxisd(pitch0, Vector3d::UnitY());
    Rbw = AngleAxisd(roll1, Vector3d::UnitX())
      *  AngleAxisd(pitch1, Vector3d::UnitY());

    R1 = Rc2b().transpose() * Rb0w * Rc2b();
    R2 = Rc2b().transpose() * Rbw.transpose() * Rc2b();
    cv::Matx23d Rmat1, Rmat2;

    Rmat1 << R1(0,0), R1(0,1), R1(0,2),
            R1(1,0), R1(1,1), R1(1,2);
    Rmat2 << R2(0,0), R2(0,1), R2(0,2),
            R2(1,0), R2(1,1), R2(1,2);

    cv::Size sz(PATCHSIZE,PATCHSIZE);
    cv::warpAffine(p, w1, Rmat1, sz);
    cv::flip(w1, w2, 0);
    cv::warpAffine(w2, w3, Rmat2, sz);

    return w3;
}		/* -----  end of method Camera::warpPatch  ----- */

double
Camera::roll(const Quaterniond &q) const
{
    return atan2( 2*(q.w()*q.x() + q.y()*q.z()), 1-2*(q.x()*q.x() + q.y()+q.y()));
}

double
Camera::pitch(const Quaterniond &q) const
{
    return asin( 2*(q.w()*q.y() - q.z()*q.x()));
}