// g2o - General Graph Optimization
// Copyright (C) 2011 Kurt Konolige
// 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.

#ifndef G2O_TYPES_ICP
#define G2O_TYPES_ICP

#define GICP_ANALYTIC_JACOBIANS
//#define SCAM_ANALYTIC_JACOBIANS

#include <Eigen/Geometry>
#include <iostream>

#include "g2o/core/base_binary_edge.h"
#include "g2o/core/base_variable_sized_edge.h"
#include "g2o/core/base_vertex.h"
#include "g2o/types/sba/types_sba.h"
#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o_types_icp_api.h"

namespace g2o {

namespace types_icp {
void init();
}
//
// GICP-type edges
// Each measurement is between two rigid points on each 6DOF vertex
//

//
// class for edges between two points rigidly attached to vertices
//

class G2O_TYPES_ICP_API EdgeGICP {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  // point positions
  Vector3 pos0, pos1;

  // unit normals
  Vector3 normal0, normal1;

  // rotation matrix for normal
  Matrix3 R0, R1;

  // initialize an object
  EdgeGICP() {
    pos0.setZero();
    pos1.setZero();
    normal0 << 0, 0, 1;
    normal1 << 0, 0, 1;
    // makeRot();
    R0.setIdentity();
    R1.setIdentity();
  }

  // set up rotation matrix for pos0
  void makeRot0() {
    Vector3 y;
    y << 0, 1, 0;
    R0.row(2) = normal0;
    y = y - normal0(1) * normal0;
    y.normalize();  // need to check if y is close to 0
    R0.row(1) = y;
    R0.row(0) = normal0.cross(R0.row(1));
    //      cout << normal.transpose() << endl;
    //      cout << R0 << endl << endl;
    //      cout << R0*R0.transpose() << endl << endl;
  }

  // set up rotation matrix for pos1
  void makeRot1() {
    Vector3 y;
    y << 0, 1, 0;
    R1.row(2) = normal1;
    y = y - normal1(1) * normal1;
    y.normalize();  // need to check if y is close to 0
    R1.row(1) = y;
    R1.row(0) = normal1.cross(R1.row(1));
  }

  // returns a precision matrix for point-plane
  Matrix3 prec0(number_t e) {
    makeRot0();
    Matrix3 prec;
    prec << e, 0, 0, 0, e, 0, 0, 0, 1;
    return R0.transpose() * prec * R0;
  }

  // returns a precision matrix for point-plane
  Matrix3 prec1(number_t e) {
    makeRot1();
    Matrix3 prec;
    prec << e, 0, 0, 0, e, 0, 0, 0, 1;
    return R1.transpose() * prec * R1;
  }

  // return a covariance matrix for plane-plane
  Matrix3 cov0(number_t e) {
    makeRot0();
    Matrix3 cov;
    cov << 1, 0, 0, 0, 1, 0, 0, 0, e;
    return R0.transpose() * cov * R0;
  }

  // return a covariance matrix for plane-plane
  Matrix3 cov1(number_t e) {
    makeRot1();
    Matrix3 cov;
    cov << 1, 0, 0, 0, 1, 0, 0, 0, e;
    return R1.transpose() * cov * R1;
  }
};

// 3D rigid constraint
//    3 values for position wrt frame
//    3 values for normal wrt frame, not used here
// first two args are the measurement type, second two the connection classes
class G2O_TYPES_ICP_API EdgeVVGicp
    : public BaseBinaryEdge<3, EdgeGICP, VertexSE3, VertexSE3> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  EdgeVVGicp() = default;
  explicit EdgeVVGicp(const EdgeVVGicp *e);

  // switch to go between point-plane and plane-plane
  bool pl_pl = false;
  Matrix3 cov0, cov1;

  // I/O functions
  bool read(std::istream &is) override;
  bool write(std::ostream &os) const override;

  // return the error estimate as a 3-vector
  void computeError() override {
    // from <ViewPoint> to <Point>
    const VertexSE3 *vp0 = vertexXnRaw<0>();
    const VertexSE3 *vp1 = vertexXnRaw<1>();

    // get vp1 point into vp0 frame
    // could be more efficient if we computed this transform just once
    Vector3 p1;

#if 0
      if (_cnum >= 0 && 0)      // using global cache
        {
          if (_tainted[_cnum])  // set up transform
            {
              _transforms[_cnum] = vp0->estimate().inverse() * vp1->estimate();
              _tainted[_cnum] = 0;
              cout << _transforms[_cnum] << endl;
            }
          p1 = _transforms[_cnum].map(measurement().pos1); // do the transform
        }
      else
#endif
    {
      p1 = vp1->estimate() * measurement().pos1;
      p1 = vp0->estimate().inverse() * p1;
    }

    //      cout << endl << "Error computation; points are: " << endl;
    //      cout << p0.transpose() << endl;
    //      cout << p1.transpose() << endl;

    // get their difference
    // this is simple Euclidean distance, for now
    error_ = p1 - measurement().pos0;

#if 0
      cout << "vp0" << endl << vp0->estimate() << endl;
      cout << "vp1" << endl << vp1->estimate() << endl;
      cout << "e Jac Xj" << endl <<  jacobianOplusXj_ << endl << endl;
      cout << "e Jac Xi" << endl << jacobianOplusXi_ << endl << endl;
#endif

    if (!pl_pl) return;

    // re-define the information matrix
    // topLeftCorner<3,3>() is the rotation()
    const Matrix3 transform = (vp0->estimate().inverse() * vp1->estimate())
                                  .matrix()
                                  .topLeftCorner<3, 3>();
    information() = (cov0 + transform * cov1 * transform.transpose()).inverse();
  }

  // try analytic jacobians
#ifdef GICP_ANALYTIC_JACOBIANS
  void linearizeOplus() override;
#endif

  // global derivative matrices
  static Matrix3 dRidx_;
  static Matrix3 dRidy_;
  static Matrix3 dRidz_;  // differential quat matrices
};

/**
 * \brief Stereo camera vertex, derived from SE3 class.
 * Note that we use the actual pose of the vertex as its parameterization,
 * rather than the transform from RW to camera coords. Uses static vars for
 * camera params, so there is a single camera setup.
 */
class G2O_TYPES_ICP_API VertexSCam : public VertexSE3 {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // I/O
  bool read(std::istream &is) override;
  bool write(std::ostream &os) const override;

  // capture the update function to reset aux transforms
  void oplusImpl(const VectorX::MapType& update) override {
    VertexSE3::oplusImpl(update);
    setAll();
  }

  // camera matrix and stereo baseline
  static Matrix3 kcam_;
  static number_t baseline_;

  // transformations
  Eigen::Matrix<number_t, 3, 4, Eigen::ColMajor>
      w2n;  // transform from world to node coordinates
  Eigen::Matrix<number_t, 3, 4, Eigen::ColMajor>
      w2i;  // transform from world to image coordinates

  // Derivatives of the rotation matrix transpose wrt quaternion xyz, used for
  // calculating Jacobian wrt pose of a projection.
  Matrix3 dRdx, dRdy, dRdz;

  // transforms
  static void transformW2F(Eigen::Matrix<number_t, 3, 4, Eigen::ColMajor> &m,
                           const Vector3 &trans, const Quaternion &qrot) {
    m.block<3, 3>(0, 0) = qrot.toRotationMatrix().transpose();
    m.col(3).setZero();  // make sure there's no translation
    Vector4 tt;
    tt.head(3) = trans;
    tt[3] = 1.0;
    m.col(3) = -m * tt;
  }

  static void transformF2W(Eigen::Matrix<number_t, 3, 4, Eigen::ColMajor> &m,
                           const Vector3 &trans, const Quaternion &qrot) {
    m.block<3, 3>(0, 0) = qrot.toRotationMatrix();
    m.col(3) = trans;
  }

  // set up camera matrix
  static void setKcam(number_t fx, number_t fy, number_t cx, number_t cy,
                      number_t tx) {
    kcam_.setZero();
    kcam_(0, 0) = fx;
    kcam_(1, 1) = fy;
    kcam_(0, 2) = cx;
    kcam_(1, 2) = cy;
    kcam_(2, 2) = 1.0;
    baseline_ = tx;
  }

  // set transform from world to cam coords
  void setTransform() {
    w2n = estimate().inverse().matrix().block<3, 4>(0, 0);
    // transformW2F(w2n,estimate().translation(), estimate().rotation());
  }

  // Set up world-to-image projection matrix (w2i), assumes camera parameters
  // are filled.
  void setProjection() { w2i = kcam_ * w2n; }

  // sets angle derivatives
  void setDr() {
    // inefficient, just for testing
    // use simple multiplications and additions for production code in
    // calculating dRdx,y,z for dS'*R', with dS the incremental change
    dRdx = dRidx_ * w2n.block<3, 3>(0, 0);
    dRdy = dRidy_ * w2n.block<3, 3>(0, 0);
    dRdz = dRidz_ * w2n.block<3, 3>(0, 0);
  }

  // set all aux transforms
  void setAll() {
    setTransform();
    setProjection();
    setDr();
  }

  // calculate stereo projection
  void mapPoint(Vector3 &res, const Vector3 &pt3) {
    Vector4 pt;
    pt.head<3>() = pt3;
    pt(3) = cst(1.0);
    Vector3 p1 = w2i * pt;
    Vector3 p2 = w2n * pt;
    Vector3 pb(baseline_, 0, 0);

    number_t invp1 = cst(1.0) / p1(2);
    res.head<2>() = p1.head<2>() * invp1;

    // right camera px
    p2 = kcam_ * (p2 - pb);
    res(2) = p2(0) / p2(2);
  }

  static Matrix3 dRidx_;
  static Matrix3 dRidy_;
  static Matrix3 dRidz_;
};

/**
 * \brief Point vertex, XYZ, is in types_sba
 */

// stereo projection
// first two args are the measurement type, second two the connection classes
class G2O_TYPES_ICP_API EdgeXyzVsc
    : public BaseBinaryEdge<3, Vector3, VertexPointXYZ, VertexSCam> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

  // return the error estimate as a 2-vector
  void computeError() override {
    // from <Point> to <Cam>
    VertexPointXYZ *point = vertexXnRaw<0>();
    VertexSCam *cam = vertexXnRaw<1>();
    // cam->setAll();

    // calculate the projection
    Vector3 kp;
    cam->mapPoint(kp, point->estimate());

    // std::cout << std::endl << "CAM   " << cam->estimate() << std::endl;
    // std::cout << "POINT " << pt.transpose() << std::endl;
    // std::cout << "PROJ  " << p1.transpose() << std::endl;
    // std::cout << "PROJ  " << p2.transpose() << std::endl;
    // std::cout << "CPROJ " << kp.transpose() << std::endl;
    // std::cout << "MEAS  " << measurement_.transpose() << std::endl;

    // error, which is backwards from the normal observed - calculated
    // measurement_ is the measured projection
    error_ = kp - measurement_;
  }
#ifdef SCAM_ANALYTIC_JACOBIANS
  // jacobian
  virtual void linearizeOplus();
#endif
};

}  // namespace g2o

#endif  // TYPES_ICP
