#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <cmath>
#include <iomanip>
#include <stdlib.h>

Eigen::MatrixXd Euler2A(double fi, double teta, double psi);
std::pair<Eigen::VectorXd, double> AxisAngle(Eigen::Matrix3d A);
Eigen::MatrixXd Rodrigez(Eigen::Vector3d p, double fi);
std::tuple<double, double, double> Auler2E(Eigen::MatrixXd A);
Eigen::Vector4d AxisAngle2Q(Eigen::Vector3d p, double fi);
std::pair<Eigen::VectorXd, double> Q2AxisAngle(Eigen::Vector4d q);

int main() {

    std::cout << std::fixed;
    std::cout << std::setprecision(6);

    Eigen::MatrixXd A = Euler2A(3.14/3, 3.14/2, -3.14/4);

    std::cout << "Euler2A" << std::endl;
    std::cout << A << std::endl;

    std::pair<Eigen::VectorXd, double> pfi = AxisAngle(A);
    std::cout << "AxisAngle" << std::endl;
    std::cout << pfi.first << std::endl;
    std::cout << pfi.second << std::endl;

    Eigen::MatrixXd Rp = Rodrigez(pfi.first, pfi.second);
    std::cout << "Rodrigez" << std::endl;
    std::cout << Rp << std::endl;

    std::tuple<double, double, double> auler2e = Auler2E(A);
    std::cout << "A2Euler" << std::endl;
    std::cout << "fi: " << std::get<0>(auler2e) << std::endl;
    std::cout << "teta: " << std::get<1>(auler2e) << std::endl;
    std::cout << "psi: " << std::get<2>(auler2e) << std::endl;

    Eigen::Vector4d q = AxisAngle2Q(pfi.first, pfi.second);
    std::cout << "AxisAngle2Q" << std::endl;
    std::cout << q << std::endl;

    std::pair<Eigen::VectorXd, double> q2fi = Q2AxisAngle(q);
    std::cout << "Q2AxisAngle" << std::endl;
    std::cout << "p: " << std::endl << q2fi.first << std::endl;
    std::cout << "fi: " << q2fi.second << std::endl;

    return 0;
}

Eigen::MatrixXd Euler2A(double fi, double teta, double psi){
    Eigen::MatrixXd m1(3, 3), m2(3, 3), m3(3, 3);
    m1 << 1.0, 0.0,       0.0,
          0.0, cos(fi), -sin(fi),
          0.0, sin(fi), cos(fi);

    m2 << cos(teta),  0.0, sin(teta),
          0.0      ,  1.0, 0.0,
          -sin(teta), 0.0, cos(teta);

    m3 << cos(psi), -sin(psi), 0.0,
          sin(psi), cos(psi),  0.0,
          0.0,        0.0,     1.0;

    return m3*m2*m1;
}

std::pair<Eigen::VectorXd, double> AxisAngle(Eigen::Matrix3d A){

    Eigen::MatrixXd e(3, 3);
    e.setIdentity();

    float detA = round(A.determinant());


    Eigen::Matrix3d AAT = A*A.transpose();
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            AAT(i, j) = round(abs(AAT(i, j)));
        }
    }

    if (detA != 1 || AAT != e) {
        std::cout << "Matrix is invalid!" << std::endl;
        exit(1);
    }

    Eigen::Matrix3d Ae;
    Ae = A - e;

    Eigen::Vector3d p = Ae.row(0).cross(Ae.row(1));
    p.normalize();

    Eigen::Vector3d n = {0, -(p(2)/p(1)), 1};
    Eigen::Vector3d n1 = A * n;

    double phi = std::acos(n.dot(n1) / (n.norm() * n1.norm()));

    return std::pair<Eigen::VectorXd, double>(p, phi);
}

Eigen::MatrixXd Rodrigez(Eigen::Vector3d p, double fi){

    Eigen::MatrixXd e(3, 3);
    e.setIdentity();

    Eigen::MatrixXd px(3, 3);
    px << 0, -p(2), p(1),
          p(2), 0, -p(0),
          -p(1), p(0), 0;

    Eigen::MatrixXd Rp = p*p.transpose() + cos(fi)*(e - p*p.transpose()) + sin(fi) * px;

    return Rp;
}

std::tuple<double, double, double> Auler2E(Eigen::MatrixXd A){

    double fi, teta, psi;

    if(A(2, 0) < 1){
        if(A(2, 0) > -1){
            psi = atan2(A(1, 0), A(0, 0));
            teta = asin(-A(2, 0));
            fi = atan2(A(2, 1), A(2, 2));
        } else{
            fi = atan2(-A(0, 1), A(1, 1));
            teta = M_PI/2;
            fi = 0;
        }
    }else{
        fi = atan2(-A(0, 1), A(1, 1));
        teta = -M_PI/2;
        fi = 0;
    }

    return std::tuple<double, double, double>(fi, teta, psi);
}

Eigen::Vector4d AxisAngle2Q(Eigen::Vector3d p, double fi){

    double w = cos(fi/2);

    p.normalize();
    Eigen::Vector3d sp = sin(fi/2) * p;

    Eigen::Vector4d q;
    q << sp, w;

    return q;
}

std::pair<Eigen::VectorXd, double> Q2AxisAngle(Eigen::Vector4d q){

    q.normalize();
    if(q(3) < 0) q = -q;

    double fi = 2*acos(q(3));

    Eigen::Vector3d p, q3 = {q(0), q(1), q(2)};
    q3.normalize();

    if(abs(q(3)) == 1) p = {1, 0, 0};
    else p = q3;

    return std::pair<Eigen::VectorXd, double>(p, fi);
}