// 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.

// Modified by Raúl Mur Artal (2014)
// Added EdgeSE3ProjectXYZ (project using focal_length in x,y directions)
// Modified by Raúl Mur Artal (2016)
// Added EdgeStereoSE3ProjectXYZ (project using focal_length in x,y directions)
// Added EdgeSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose)
// Added EdgeStereoSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose)

#ifndef G2O_SIX_DOF_TYPES_EXPMAP_our
#define G2O_SIX_DOF_TYPES_EXPMAP_our

#include "g2o/g2o/core/base_vertex.h"
#include "g2o/g2o/core/base_binary_edge.h"
#include "g2o/g2o/core/base_unary_edge.h"
//#include "se3_ops.h"
//#include "se3quat.h"
#include "g2o/g2o/types/types_sba.h"
#include "so3.h"
#include <Eigen/Geometry>
#include <Eigen/StdVector>

#include "g2o/g2o/core/factory.h"
#include "g2o/g2o/stuff/macros.h"


#include "g2o/g2o/types/types_sba.h"
#include "g2o/g2o/types/types_six_dof_expmap.h"
#include "g2o/g2o/core/base_edge.h"

#include "g2o/g2o/core/block_solver.h"

namespace g2o {
namespace types_six_dof_expmap {
void init();
}

using namespace Eigen;

typedef Matrix<double, 6, 6> Matrix6d;


/**
 * \brief SE3 Vertex parameterized internally with a transformation matrix
 and externally with its exponential map
 */

    class  VertexMyExpmap : public BaseVertex<6, SE3Quat>{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        VertexMyExpmap();

        bool read(std::istream& is);

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

        virtual void setToOriginImpl() {
            _estimate = SE3Quat();
        }

        virtual void oplusImpl(const double* update_)  {
            Eigen::Map<const Vector6d> update(update_);
            //std::cout<<"VertexSE3Expmap"<<update.transpose()<<std::endl;
            setEstimate(SE3Quat::exp(update)*estimate());
        }
    };


/**
 * \brief SO3 Vertex parameterized internally with a rotation matrix and externally with its exponential map
 */

class VertexSO3Expmap : public BaseVertex<3, SO3>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

public:

  VertexSO3Expmap();
  bool read(std::istream& is);
  bool write(std::ostream& os) const;

  virtual void setToOriginImpl(){
    _estimate = SO3();
  }

  virtual void oplusImpl(const double* update_){
    Eigen::Map<const Vector3d> update(update_);
    SO3 tmp(SO3::exp(update));
    setEstimate(tmp*estimate());      // exp(w + delta_w ) = exp(delta_w ) * exp(w); left multipline
  }
};



class EdgeSE3ProjectPlaneSO3 : public BaseBinaryEdge<3, Vector3d , VertexSO3Expmap , VertexMyExpmap> {

  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

public:

    EdgeSE3ProjectPlaneSO3();

    bool read(std::istream& is);
    bool write(std::ostream& os) const;

    void computeError(){

        VertexMyExpmap * vj = static_cast<VertexMyExpmap *>(_vertices[1]);
        SE3Quat T(vj->estimate());
        VertexSO3Expmap *vi = static_cast<VertexSO3Expmap *>(_vertices[0]);
        SO3 plane_w(vi->estimate());
        Eigen::Matrix3d R = T.to_homogeneous_matrix().block(0,0,3,3);
        Eigen::Vector3d t = T.to_homogeneous_matrix().block(0,3,3,1);
        Eigen::Quaterniond q_m =_measurement_plane_c.unit_quaternion();
        Eigen::Quaterniond q;
        q.vec() = R.transpose()*q_m.vec();
        q.w() = t.transpose()*q_m.vec()+q_m.w();
        SO3 obs_w(q);//T_w_c*_measurement;
        SO3 structure_error = obs_w*plane_w.inverse();
        Eigen::Vector3d u = structure_error.log();
        _error = u;
        int i=0;
/*
      Quaterniond q_m =_measurement_plane_c.unit_quaternion();      // the observation
      Vector4d vec_4;
      vec_4 << q_m.x() , q_m.y() , q_m.z() , q_m.w();
      
      Vector4d _measurement_plane_w = T.to_homogeneous_matrix().transpose().inverse() * vec_4;
      Vector4d q_y = _measurement_plane_w / _measurement_plane_w.norm();
      Quaterniond q_mw(q_y[3] , q_y[0] , q_y[1] , q_y[2]);
      SO3 plane_w_obs(q_mw);
      SO3 structure_error = plane_w_obs * plane_w.inverse();
      Vector3d u = structure_error.log();
      _error = u;
*/
    }

    virtual void linearizeOplus();

    void setParams(const SO3& plane_);

    SO3 _measurement_plane_c;     // the plane coordinates in the current camera frame C
};







} // end namespace

#endif
