// g2o - General Graph Optimization
// Copyright (C) 2011 H. Strasdat
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "mylib/types_six_dof_expmap_ours.h"
#include "mylib/g2o_opt_type.h"

namespace g2o {

using namespace std;
/*************************/
    VertexMyExpmap::VertexMyExpmap() : BaseVertex<6, SE3Quat>() {
    }

    bool VertexMyExpmap::read(std::istream& is) {
        Vector7d est;
        for (int i=0; i<7; i++)
            is  >> est[i];
        SE3Quat cam2world;
        cam2world.fromVector(est);
        setEstimate(cam2world.inverse());
        return true;
    }

    bool VertexMyExpmap::write(std::ostream& os) const {
        SE3Quat cam2world(estimate().inverse());
        for (int i=0; i<7; i++)
            os << cam2world[i] << " ";
        return os.good();
    }

/**************************/



Vector2d project2d(const Vector3d& v)  {
  Vector2d res;
  res(0) = v(0)/v(2);
  res(1) = v(1)/v(2);
  return res;
}

Vector3d unproject2d(const Vector2d& v)  {
  Vector3d res;
  res(0) = v(0);
  res(1) = v(1);
  res(2) = 1;
  return res;
}


VertexSO3Expmap::VertexSO3Expmap() : BaseVertex<3, SO3>(){

}

bool VertexSO3Expmap::read(std::istream& is){
  Vector3d est;
  for(int i = 0 ;i < 3 ; i ++){
    is >> est[i];

    SO3 plane(SO3::exp(est));
    setEstimate(plane);
    return true;
  }
}

bool VertexSO3Expmap::write(std::ostream& os) const{
  SO3 plane(estimate());
  Vector3d angle = plane.log();
  for(int i= 0; i < 3; i ++){
    os << angle[i] << " ";
  }
  return os.good();
}

EdgeSE3ProjectPlaneSO3::EdgeSE3ProjectPlaneSO3() : BaseBinaryEdge<3, Vector3d , VertexSO3Expmap , VertexMyExpmap>(){

}

bool EdgeSE3ProjectPlaneSO3::read(std::istream& is){
  for(int i=0; i <= 2; i ++){
    is >> _measurement[i];
  }
  for (int i=0; i<=2; i++)
    for (int j=i; j<=2; j++) {
      is >> information()(i,j);
      if (i!=j)
        information()(j,i)=information()(i,j);
    }

  return true;  
}

bool EdgeSE3ProjectPlaneSO3::write(std::ostream& os) const{
    for (int i=0; i<=2; i++){
    os << measurement()[i] << " ";
  }

  for (int i=0; i<=2; i++)
    for (int j=i; j<=2; j++){
      os << " " <<  information()(i,j);
    }
  return os.good();
}

void EdgeSE3ProjectPlaneSO3::linearizeOplus(){
    VertexMyExpmap * vj = static_cast<VertexMyExpmap *>(_vertices[1]);
    SE3Quat T(vj->estimate());
    VertexSO3Expmap *vi = static_cast<VertexSO3Expmap *>(_vertices[0]);
    SO3 plane_opt_w(vi->estimate());
    Eigen::Vector3d plane =plane_opt_w.log();
    Eigen::Quaterniond q(1,0,0,0);
    Eigen::Quaterniond plane_obs_c = QuatPlane::ExpUpDateQuat(_measurement_plane_c.log(),q).normalized();  // the observation
    SO3 plane_obs_c_so3(plane_obs_c);
    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]);
    SO3 plane_obs_w_so3(plane_obs_w);
    //Eigen::Quaterniond plane_opt_w = QuatPlane::ExpUpDateQuat(plane,q);
    Eigen::Quaterniond structure_error1 = plane_obs_w * QuatPlane::ExpUpDateQuat(plane,q).inverse();
    SO3 structure_error = plane_obs_w_so3*plane_opt_w.inverse();
    Eigen::Vector3d u = QuatPlane::logQuat(structure_error1);
    //std::cout<<"QuatPlane::logQuat(structure_error1) is "<<u.transpose()<<std::endl;
    Eigen::Vector3d u2 = structure_error.log();
    //std::cout<<"structure_error.log() is "<<u2.transpose()<<std::endl;
    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); // Jr(u)^(-1)
    _jacobianOplusXj = jacobian_E_u * jacobian_u_phi * jacobian_phi_y * jacobian_y_pose; // 3 x 6 matrix
    std::cout<<"--------finish Jacobian calculation-------"<<std::endl;
/*
  Quaterniond pi_x = _measurement_plane_c.unit_quaternion();      // the observation
  Vector4d pi_x_4d;
  pi_x_4d << pi_x.x() , pi_x.y() , pi_x.z() , pi_x.w();
  
  Vector4d y = T.to_homogeneous_matrix().transpose() * pi_x_4d;
  Vector4d q_y = y / y.norm();
  Quaterniond n_q_y(q_y[3] , q_y[0] , q_y[1] , q_y[2]);
  SO3 q_T(n_q_y);
  SO3 q_u = q_T * plane.inverse();
  Vector3d u = q_u.log();

  Vector3d q_y_v = n_q_y.vec();
  double q_y_v_norm = q_y_v.norm();
  double q_y_v_norm_3 = q_y_v_norm * q_y_v_norm * q_y_v_norm;
  double q4 = n_q_y.w();

  Eigen::Matrix3d J_l_u_inv = SO3::JacobianLInv(u);

  Eigen::Matrix<double ,3,4> M;
  M.block<3,3>(0,0) = 2 * acos(q4) * ( Eigen::Matrix3d::Identity() / q_y_v_norm - q_y_v * q_y_v.transpose()/q_y_v_norm_3);
  M.block<3,1>(0,3) = -2.0 * q_y_v / (q_y_v_norm * sqrt(1.0 - q4 * q4 + 0.0001))/q_y_v_norm;

  Eigen::Matrix<double, 4,4> Q;
  double y_norm = y.norm();
  double y_norm_3 = y_norm * y_norm * y_norm;
  Q = Eigen::Matrix4d::Identity() / y_norm - y * y.transpose() / y_norm_3;

  Vector3d pi_x_v;
  pi_x_v << pi_x.x() , pi_x.y() , pi_x.z();
  Eigen::Matrix<double, 4,6> H = Eigen::Matrix<double, 4,6>::Zero();
  H.block<3,3>(0,0) = T.rotation().toRotationMatrix().transpose() * SO3::hat(pi_x_v);
  H.block<1,3>(3,0) = T.translation().transpose() * SO3::hat(pi_x_v);
  H.block<1,3>(3,3) = pi_x_v.transpose();


  // ok, finish
  _jacobianOplusXi = SO3::JacobianRInv(u); // Jr(u)^(-1)
  _jacobianOplusXj = J_l_u_inv * M * Q * H; // 3 x 6 matrix

*/
}
void EdgeSE3ProjectPlaneSO3::setParams(const SO3& plane_){
    _measurement_plane_c = plane_;
}

} // end namespace
