#include "mylib/QuatPlane.h"
#include "mylib/g2o_opt_type.h"
/*
    封装的含义：
    1.对属性和方法进行封装
    2.对属性和方法进行访问
    类封装了成员变量和成员函数，使用指针的方法可以用->进行调用
*/


Eigen::Vector4d QuatPlane::CreatePlanebyPoints(Eigen::Vector4d pt1_,Eigen::Vector4d pt2_,Eigen::Vector4d pt3_,Eigen::Vector4d pt4_)
{
    Eigen::Vector3d pt1=pt1_.head(3);Eigen::Vector3d pt2=pt2_.head(3);Eigen::Vector3d pt3=pt3_.head(3);Eigen::Vector3d pt4=pt4_.head(3);
    Eigen::Vector4d plane_homo;
    plane_homo.head(3) =  (pt1-pt3).cross((pt2-pt3));
    plane_homo(3) = -pt3.transpose()*(pt1.cross(pt2));
    Eigen::Vector3d normal;double dis;
    normal =plane_homo.head(3)/plane_homo.head(3).norm();//.normalized();
    dis = -plane_homo(3)/plane_homo.head(3).norm();
    Eigen::Vector4d HessePlane;
    HessePlane.head(3) = normal;
    HessePlane(3) = dis;
    return HessePlane;
}
// popular Hesse notation formed by a unit normal vector n and the orthogonal distance to the origin d.
void QuatPlane::HomogeneousToHesse(const Eigen::Vector4d &homo_para, Eigen::Vector3d &norm, double &dis){
    Eigen::Vector3d n(homo_para[0], homo_para[1], homo_para[2]);

    norm[0] = homo_para[0] / n.norm();
    norm[1] = homo_para[1] / n.norm();
    norm[2] = homo_para[2] / n.norm();

    dis = -1 * homo_para[3] / n.norm();
}
// 只要我们是标准化的，正规化的，我们就可以使用n_norm = 1.从Hesse形式转变为面的齐次形式
void QuatPlane::HesseToHomogenous(const Eigen::Vector3d &norm,const double &dis, Eigen::Vector4d &homo_para){
    double n_norm = 1;
    norm.normalized();
    homo_para[0] = norm[0] * n_norm;
    homo_para[1] = norm[1] * n_norm;
    homo_para[2] = norm[2] * n_norm;
    homo_para[3] = -1 * dis * n_norm;
}

// 这里是平面的四元数表达法，只需要把Hesse形式进行一个小小的变形就可以得到单位四元数化的平面参数化形式，几何意义不明确
Eigen::Quaterniond QuatPlane::HesseToQuaternion(Eigen::Vector3d norm,double d) {
    Eigen::Quaterniond quat;
    norm.normalize();
    quat.x() = norm[0];
    quat.y() = norm[1];
    quat.z() = norm[2];
    quat.w() = -d;
    quat.normalize();
    return quat;
}

// 我们把Hesse单位法向量看成是一个单位圆球上的一点，可以把平面用球坐标参数化
Eigen::Vector3d QuatPlane::SpherToHesse(const double &phi_,const double &theta,const double &dis_s){
    EIGEN_USING_STD(sin)
    EIGEN_USING_STD(cos)
    dis_ = dis_s;
    // [\cos \phi \cos \theta \quad \cos \phi \sin \theta \quad \sin \phi] \mathbf{P}_{\mathrm{f}}-d=0
    normal_[0] = cos(phi_)*cos(theta);
    normal_[1] = cos(phi_)*sin(theta);
    normal_[2] = sin(phi_);
    return normal_;
}



Eigen::Matrix4d QuatPlane::Leftq(const Eigen::Quaterniond q)
{
    Eigen::Matrix4d Rq_l = Eigen::Matrix4d::Zero();

    Rq_l << q.w(), -q.x(), -q.y(), -q.z(),
            q.x(), q.w(), -q.z(), q.y(),
            q.y(), q.z(), q.w(), -q.x(),
            q.z(), -q.y(), q.x(), q.w(); // Hamiltion

    return Rq_l;
}

Eigen::Matrix4d QuatPlane::Rightq(const Eigen::Quaterniond q)
{
    Eigen::Matrix4d Rq_r = Eigen::Matrix4d::Zero();

    Rq_r << q.w(), -q.x(), -q.y(), -q.z(),
            q.x(), q.w(), -q.z(), q.y(),
            q.y(), -q.z(), q.w(), q.x(),
            q.z(), -q.x(), -q.x(), q.w(); // Hamiltion qw,qx,qy,qz

    return Rq_r;
}

Eigen::Quaterniond QuatPlane::Conjugate(const Eigen::Quaterniond q)
{
    Eigen::Quaterniond  q_conj;
    q_conj.x() = -q.x();
    q_conj.y() = -q.y();
    q_conj.z() = -q.z();
    return q_conj;
}

const double SMALL_EPS = 1e-5;

Eigen::Quaterniond QuatPlane::ExpUpDateQuat(const Eigen::Vector3d &omg, const Eigen::Quaterniond q) {
    Eigen::Quaterniond res;
    double theta, half_theta;
    theta = omg.norm();//M_PI / 6;
    half_theta = 0.5 * theta;
    EIGEN_USING_STD(sin)
    EIGEN_USING_STD(cos)
    // if(theta<SMALL_EPS) TODO
    if (theta < SMALL_EPS) {
        res.vec() = omg;
        res.w() = 1;
    } else {
        res.vec() = sin(half_theta) / theta * omg;
        res.w() = cos(half_theta);
    }
    res.normalize();
    return res;
}

Eigen::Vector3d QuatPlane::Rotation2Vector(Eigen::Matrix3d R) {
    double tr = R.trace();
    Eigen::Vector3d w;
    w << 0.5 * (R(2, 1) - R(1, 2)), 0.5 * (R(0, 2) - R(2, 0)), 0.5 * (R(1, 0) - R(0, 1));
    double cos_theta = (tr - 1) / 2;
    if (cos_theta > 1 || cos_theta < -1) { return w; }
    else {
        double theta = acos(cos_theta);
        if (fabs(sin(theta)) < SMALL_EPS) { return w; }
        else {
            return w / sin(theta) * theta;
        }
    }
}

// inverse of the exponential map:log map
Eigen::Vector3d QuatPlane::logQuat(Eigen::Quaterniond q) {
    /*
    Eigen::Vector3d w;
    Eigen::Vector3d q_vec = q.vec();
    double q_norm = q.vec().norm();
    double q_w = q.w();
    if (q_norm < SMALL_EPS) {
        w = q_vec;
    } else {
        w = 2 * acos(q_w) / q_norm * q_vec;
    }
    return w;*/
    q.normalize();
    Eigen::Vector3d w;
    double q_norm = q.vec().norm();
    Eigen::Vector3d q_vec = q.vec();
    double q_w = q.w();
    if (q_norm < SMALL_EPS) {
        w = q_vec;
    } else {
        double q_n_3 = q_norm * q_norm * q_norm;
        double q_w_2 = q_w * q_w;
        w = q_vec / q_norm * (1 - 1 / 3 * q_n_3 / q_w_2);
        //Taylor series
    }
    return w;
}

Eigen::Matrix3d QuatPlane::JacobianR(const Eigen::Vector3d& w)
{
    Eigen::Matrix3d Jr = Eigen::Matrix3d::Identity();
    double theta = w.norm();
    if(theta<0.00001)
    {
        return Jr;// = Matrix3d::Identity();
    }
    else
    {
        Eigen::Vector3d k = w.normalized();  // k - unit direction vector of w
        Eigen::Matrix3d K = g2o::hat(k);
        Jr =   Eigen::Matrix3d::Identity()
               - (1-cos(theta))/theta*K
               + (1-sin(theta)/theta)*K*K;
    }
    return Jr;
}
Eigen::Matrix3d QuatPlane::JacobianRInv(const Eigen::Vector3d& w)
{
    Eigen::Matrix3d Jrinv = Eigen::Matrix3d::Identity();
    double theta = w.norm();

    // very small angle
    if(theta < 0.00001)
    {
        return Jrinv;
    }
    else
    {
        Eigen::Vector3d k = w.normalized();  // k - unit direction vector of w
        Eigen::Matrix3d K = g2o::hat(k);
        Jrinv = Eigen::Matrix3d::Identity()
                + 0.5*g2o::hat(w)
                + ( 1.0 - (1.0+cos(theta))*theta / (2.0*sin(theta)) ) *K*K;
    }

    return Jrinv;
}

// left jacobian inverse
Eigen::Matrix3d QuatPlane::JacobianLInv(const Eigen::Vector3d& w)
{
    Eigen::Matrix3d JlInv_old = JacobianRInv(-w); // original version
    Eigen::Matrix3d JlInv = Eigen::Matrix3d::Identity();

    double theta = w.norm();
    if(theta < 0.00001)
    {
        return JlInv;
    }
    else
    {
        Eigen::Vector3d k = w.normalized();  // k - unit direction vector of w
        Eigen::Matrix3d K_hat = g2o::hat(k);
        double half_theta = theta / 2;
        double cot_half_theta = cos(half_theta) / sin(half_theta);
        JlInv = half_theta * cot_half_theta * Eigen::Matrix3d::Identity()
                + (1 - half_theta * cot_half_theta) * k * k.transpose()
                - half_theta * K_hat;
    }
    return JlInv;
}