// This file is part of libigl, a simple c++ geometry processing library.
// 
// Copyright (C) 2019 Alec Jacobson <alecjacobson@gmail.com>
// 
// This Source Code Form is subject to the terms of the Mozilla Public License 
// v. 2.0. If a copy of the MPL was not distributed with this file, You can 
// obtain one at http://mozilla.org/MPL/2.0/.
#ifndef IGL_ITERATIVE_CLOSEST_POINT_H
#define IGL_ITERATIVE_CLOSEST_POINT_H
#include "igl_inline.h"
#include <Eigen/Core>
#include "AABB.h"

namespace igl
{
  // Solve for the rigid transformation that places mesh X onto mesh Y using the
  // iterative closest point method. In particular, optimize:
  //
  // min      ∫_X inf ‖x*R+t - y‖² dx
  // R∈SO(3)      y∈Y
  // t∈R³
  //
  // Typically optimization strategies include using Gauss Newton
  // ("point-to-plane" linearization) and stochastic descent (sparse random
  // sampling each iteration).
  //
  // Inputs:
  //   VX  #VX by 3 list of mesh X vertices
  //   FX  #FX by 3 list of mesh X triangle indices into rows of VX
  //   VY  #VY by 3 list of mesh Y vertices
  //   FY  #FY by 3 list of mesh Y triangle indices into rows of VY
  //   num_samples  number of random samples to use (larger --> more accurate,
  //     but also more suceptible to sticking to local minimum)
  // Outputs:
  //   R  3x3 rotation matrix so that (VX*R+t,FX) ~~ (VY,FY)
  //   t  1x3 translation row vector
  template <
    typename DerivedVX,
    typename DerivedFX,
    typename DerivedVY,
    typename DerivedFY,
    typename DerivedR,
    typename Derivedt
    >
  IGL_INLINE void iterative_closest_point(
    const Eigen::MatrixBase<DerivedVX> & VX,
    const Eigen::MatrixBase<DerivedFX> & FX,
    const Eigen::MatrixBase<DerivedVY> & VY,
    const Eigen::MatrixBase<DerivedFY> & FY,
    const int num_samples,
    const int max_iters,
    Eigen::PlainObjectBase<DerivedR> & R,
    Eigen::PlainObjectBase<Derivedt> & t);
  // Inputs:
  //   Ytree  precomputed AABB tree for accelerating closest point queries
  //   NY  #FY by 3 list of precomputed unit face normals
  template <
    typename DerivedVX,
    typename DerivedFX,
    typename DerivedVY,
    typename DerivedFY,
    typename DerivedNY,
    typename DerivedR,
    typename Derivedt
    >
  IGL_INLINE void iterative_closest_point(
    const Eigen::MatrixBase<DerivedVX> & VX,
    const Eigen::MatrixBase<DerivedFX> & FX,
    const Eigen::MatrixBase<DerivedVY> & VY,
    const Eigen::MatrixBase<DerivedFY> & FY,
    const igl::AABB<DerivedVY,3> & Ytree, 
    const Eigen::MatrixBase<DerivedNY> & NY,
    const int num_samples,
    const int max_iters,
    Eigen::PlainObjectBase<DerivedR> & R,
    Eigen::PlainObjectBase<Derivedt> & t);
}

#ifndef IGL_STATIC_LIBRARY
#  include "iterative_closest_point.cpp"
#endif

#endif