#include "transformations.h"

/*! Decomposition of rotation matrix in Euler angles ZYZ:
 * \f[ R = R_z(\theta_1) R_y(\rho) R_z(\theta_2)\f]
 * \param[in] R rotation matrix
 * \param[out] t1 \f$\theta_1\f$
 * \param[out] rho \f$\rho\f$
 * \param[out] t2 \f$\theta_2\f$
 */
void rotmat::decomposeEulerZYZ(const cv::Mat &R, double &t1, double &rho, double &t2){
    cv::Matx33d rotmat = R;

    std::cout << rotmat;
    if (rotmat(2,2) < 1){
        if (rotmat(2,2) > -1){
            rho = acos(rotmat(2,2));
            t1  = atan2(rotmat(1,2),rotmat(0,2));
            t2  = atan2(rotmat(2,1),-rotmat(2,0));
        }
        else{
            rho = PI;
            t1  = -atan2(rotmat(1,0),rotmat(1,1));
            t2  = 0;
        }
    }
    else{
        rho = 0;
        t1  = atan2(rotmat(1,0),rotmat(1,1));
        t2  = 0;
    }
}

/// Get rotation matrix from affine camera matrix \f$P_{3\times4}\f$
cv::Mat rotmat::fromCameraMatrixAffine(const cv::Mat &P){

    P.col(1).row(2) = 5;

    if (P.rows != 3 || P.cols != 4)
        std::runtime_error("assert failed (P.rows == 3 && P.cols == 4)");

    cv::Mat K,R;
    cv::Mat temp;
    temp = P.colRange(0,0+3).clone();
    cv::Point3d p0(temp.row(0).clone());
    cv::Point3d p1(temp.row(1).clone());
    temp.row(2) = cv::Mat(p0.cross(p1)).t();

    cv::RQDecomp3x3(temp,K,R);
    if (K.at<double>(2,2) < 0){
        K = -K;
        R = -R;
    }
    return R;
}

/*! Transform a vector \f$(\theta_1,\rho,\theta_2)\f$ reprensenting the euler (Rzyz) angles
 *  into a rotation matrix:
 *  \f[ R = R_z(\theta_1) R_y(\rho) R_z(\theta_2)\f]
 */
cv::Mat rotmat::fromEulerZYZ(double t1, double rho, double t2){
    double c0,c1,c2,s0,s1,s2;

    c0 = cos(t1);
    c1 = cos(rho);
    c2 = cos(t2);
    s0 = sin(t1);
    s1 = sin(rho);
    s2 = sin(t2);

    cv::Matx33d R;
    R(0,0) = c0*c1*c2 - s0*s2;
    R(0,1) = -c0*c1*s2 - s0*c2;
    R(0,2) = c0*s1;
    R(1,0) = s0*c1*c2+c0*s2 ;
    R(1,1) = -s0*c1*s2 + c0*c2 ;
    R(1,2) = s0*s1;
    R(2,0) = -s1*c2;
    R(2,1) = s1*s2;
    R(2,2) = c1;
    return ((cv::Mat)R).clone();
}