/*create by user 18/06/2021 Jacobian error*/
#include "mylib/g2o_opt_type.h"
const double SMALL_EPS = 1e-5;

/**
 * \brief Plane vertex, omg(XYZ)
 */
namespace g2o
{
//    virtual bool read(std::istream &is);

//    virtual bool write(std::ostream &os) const;

    void VertexSBAPlaneomg::setToOriginImpl() {
        _estimate.fill(0.1);
    }

    void VertexSBAPlaneomg::oplusImpl(const double *update) {

        //Eigen::Map<const Vector3d> omg(update);
        Eigen::Vector3d omg;
        omg << update[0],update[1],update[2];
       /* double theta, half_theta;
        theta = omg.norm();//M_PI / 6;
        omg = omg/theta;
        half_theta = 0.5 * theta;
        EIGEN_USING_STD(sin)
        EIGEN_USING_STD(cos)*/
        // if(theta<SMALL_EPS) TODO
        Eigen::Quaterniond res;
        Eigen::Quaterniond q(1,0,0,0),q_tmp;
        q_tmp = QuatPlane::ExpUpDateQuat(_estimate,q);
        res = QuatPlane::ExpUpDateQuat(omg, q_tmp);
        Eigen::Vector3d v = QuatPlane::logQuat(res);
        _estimate += v;


    }

    void EdgeSE3ProjectPlane::computeError()
    {
        const VertexSE3Expmap* vj = static_cast<const VertexSE3Expmap*>(_vertices[1]);
        g2o::SE3Quat T(vj->estimate());
        Eigen::Matrix4d  T_w_c= T.to_homogeneous_matrix().transpose();
        const VertexSBAPlaneomg* vi = static_cast<const VertexSBAPlaneomg*>(_vertices[0]);
        Eigen::Vector3d opt_plane = vi->estimate();

        Eigen::Quaterniond  q(1,0,0,0),qt;
        double dism = _measurement.norm();
        Eigen::Vector3d normalm = _measurement/dism;
        Eigen::Vector4d vec4;
        QuatPlane::HesseToHomogenous(normalm,dism,vec4);
        Eigen::Vector4d vec4_c;
        vec4_c = T_w_c*vec4;
        qt.vec() = vec4_c.head(3);
        qt.w() = vec4_c(3);
        qt.normalize();
        Eigen::Vector3d obs_plane_w =QuatPlane::logQuat(qt);
        Eigen::Quaterniond  structure_err = QuatPlane::ExpUpDateQuat(obs_plane_w.normalized(),q)*QuatPlane::ExpUpDateQuat(opt_plane.normalized(),q).inverse();
        structure_err.normalize();
        _error = QuatPlane::logQuat(structure_err);
        std::cout<<"error is : "<<_error.transpose()<<std::endl;
        /*****************************************************************************/

    }

    void EdgeSE3ProjectPlane::linearizeOplus()
    {
        VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
        SE3Quat T(vj->estimate());
        VertexSBAPlaneomg *vi = static_cast<VertexSBAPlaneomg *>(_vertices[0]);
        Eigen::Vector3d plane = vi->estimate();
        Eigen::Quaterniond q(1,0,0,0);
        Eigen::Quaterniond plane_obs_c = QuatPlane::ExpUpDateQuat(_measurement,q).normalized();  // the observation
        Eigen::Vector4d homo_c_4d;
        homo_c_4d << plane_obs_c.x() , plane_obs_c.y() , plane_obs_c.z() , plane_obs_c.w();
        Eigen::Vector4d homo_w_4d = T.to_homogeneous_matrix().transpose() * homo_c_4d;
        Eigen::Vector4d q_y = homo_w_4d / homo_w_4d.norm();
        Eigen::Quaterniond plane_obs_w(q_y[3] , q_y[0] , q_y[1] , q_y[2]);
        Eigen::Quaterniond plane_opt_w = QuatPlane::ExpUpDateQuat(plane,q);
        Eigen::Quaterniond structure_error = plane_obs_w * plane_opt_w.inverse();
        Eigen::Vector3d u = QuatPlane::logQuat(structure_error);
        Eigen::Matrix3d jacobian_E_u = QuatPlane::JacobianLInv(u);
        Eigen::Matrix<double, 3, 4> jacobian_u_phi;
        double q_n = plane_obs_w.norm();
        double q_n_3 = q_n*q_n*q_n;
        jacobian_u_phi.block<3, 3>(0, 0) = 2 * acos(plane_obs_w.w()) * (Eigen::Matrix3d::Identity() / plane_obs_w.vec().norm() -
                plane_obs_w.vec() * plane_obs_w.vec().transpose() / sqrt(q_n_3));
        jacobian_u_phi.block<3, 1>(0, 3) = -1 * 2.0 / (plane_obs_w.vec().norm() * sqrt(1.0 - plane_obs_w.w() * plane_obs_w.w() + 0.0001)) * plane_obs_w.vec();
        double q_y_n = q_y.norm();
        double q_y_n3 = q_y_n*q_y_n*q_y_n;
        Eigen::Matrix4d jacobian_phi_y = Eigen::Matrix4d::Identity()/q_y.norm()-q_y*q_y.transpose()/ sqrt(q_y_n3);
        Eigen::Matrix<double, 4, 6> jacobian_y_pose = Eigen::Matrix<double, 4, 6>::Zero();
        jacobian_y_pose.block<3, 3>(0, 0) = T.rotation().toRotationMatrix().transpose() * g2o::hat(plane_obs_w.vec());
        jacobian_y_pose.block<1, 3>(3, 0) = T.translation().transpose() * g2o::hat(plane_obs_w.vec());
        jacobian_y_pose.block<1, 3>(3, 3) = plane_obs_w.vec().transpose();

        _jacobianOplusXi = QuatPlane::JacobianRInv(u);
        _jacobianOplusXj = jacobian_E_u * jacobian_u_phi * jacobian_phi_y * jacobian_y_pose; // 3 x 6 matrix
    }
    Eigen::Matrix3d hat(const Eigen::Vector3d &v) {
        Eigen::Matrix3d m;
        m << 0.0, -v[2], v[1],
                v[2], 0.0, -v[0],
                -v[1], v[0], 0.0;
        return m;
    }
};