/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. 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.     *
 * * Neither the name of ANU nor the names of its contributors may be         *
 *   used to endorse or promote products derived from this software without   *
 *   specific prior written permission.                                       *
 *                                                                            *
 * 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 ANU OR THE 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 <Eigen/NonLinearOptimization>
#include <Eigen/NumericalDiff>

#include <opengv/absolute_pose/modules/main.hpp>
#include <opengv/absolute_pose/modules/gp3p/modules.hpp>
#include <opengv/absolute_pose/modules/gpnp1/modules.hpp>
#include <opengv/absolute_pose/modules/gpnp2/modules.hpp>
#include <opengv/absolute_pose/modules/gpnp3/modules.hpp>
#include <opengv/absolute_pose/modules/gpnp4/modules.hpp>
#include <opengv/absolute_pose/modules/gpnp5/modules.hpp>
#include <opengv/absolute_pose/modules/upnp2.hpp>
#include <opengv/absolute_pose/modules/upnp4.hpp>
#include <opengv/OptimizationFunctor.hpp>
#include <opengv/math/roots.hpp>
#include <opengv/math/arun.hpp>
#include <opengv/math/cayley.hpp>

void
opengv::absolute_pose::modules::p3p_kneip_main(
    const bearingVectors_t & f,
    const points_t & p,
    transformations_t & solutions )
{
  point_t P1 = p[0];
  point_t P2 = p[1];
  point_t P3 = p[2];

  Eigen::Vector3d temp1 = P2 - P1;
  Eigen::Vector3d temp2 = P3 - P1;

  if( temp1.cross(temp2).norm() == 0)
    return;

  bearingVector_t f1 = f[0];
  bearingVector_t f2 = f[1];
  bearingVector_t f3 = f[2];

  Eigen::Vector3d e1 = f1;
  Eigen::Vector3d e3 = f1.cross(f2);
  e3 = e3/e3.norm();
  Eigen::Vector3d e2 = e3.cross(e1);

  rotation_t T;
  T.row(0) = e1.transpose();
  T.row(1) = e2.transpose();
  T.row(2) = e3.transpose();

  f3 = T*f3;

  if( f3(2,0) > 0)
  {
    f1 = f[1];
    f2 = f[0];
    f3 = f[2];

    e1 = f1;
    e3 = f1.cross(f2);
    e3 = e3/e3.norm();
    e2 = e3.cross(e1);

    T.row(0) = e1.transpose();
    T.row(1) = e2.transpose();
    T.row(2) = e3.transpose();

    f3 = T*f3;

    P1 = p[1];
    P2 = p[0];
    P3 = p[2];
  }

  Eigen::Vector3d n1 = P2-P1;
  n1 = n1/n1.norm();
  Eigen::Vector3d n3 = n1.cross(P3-P1);
  n3 = n3/n3.norm();
  Eigen::Vector3d n2 = n3.cross(n1);

  rotation_t N;
  N.row(0) = n1.transpose();
  N.row(1) = n2.transpose();
  N.row(2) = n3.transpose();

  P3 = N*(P3-P1);

  double d_12 = temp1.norm();
  double f_1 = f3(0,0)/f3(2,0);
  double f_2 = f3(1,0)/f3(2,0);
  double p_1 = P3(0,0);
  double p_2 = P3(1,0);

  double cos_beta = f1.dot(f2);
  double b = 1/( 1 - pow( cos_beta, 2 ) ) - 1;

  if( cos_beta < 0 )
    b = -sqrt(b);
  else
    b = sqrt(b);

  double f_1_pw2 = pow(f_1,2);
  double f_2_pw2 = pow(f_2,2);
  double p_1_pw2 = pow(p_1,2);
  double p_1_pw3 = p_1_pw2 * p_1;
  double p_1_pw4 = p_1_pw3 * p_1;
  double p_2_pw2 = pow(p_2,2);
  double p_2_pw3 = p_2_pw2 * p_2;
  double p_2_pw4 = p_2_pw3 * p_2;
  double d_12_pw2 = pow(d_12,2);
  double b_pw2 = pow(b,2);

  Eigen::Matrix<double,5,1> factors;

  factors(0,0) = -f_2_pw2*p_2_pw4
                 -p_2_pw4*f_1_pw2
                 -p_2_pw4;

  factors(1,0) = 2*p_2_pw3*d_12*b
                 +2*f_2_pw2*p_2_pw3*d_12*b
                 -2*f_2*p_2_pw3*f_1*d_12;

  factors(2,0) = -f_2_pw2*p_2_pw2*p_1_pw2
                 -f_2_pw2*p_2_pw2*d_12_pw2*b_pw2
                 -f_2_pw2*p_2_pw2*d_12_pw2
                 +f_2_pw2*p_2_pw4
                 +p_2_pw4*f_1_pw2
                 +2*p_1*p_2_pw2*d_12
                 +2*f_1*f_2*p_1*p_2_pw2*d_12*b
                 -p_2_pw2*p_1_pw2*f_1_pw2
                 +2*p_1*p_2_pw2*f_2_pw2*d_12
                 -p_2_pw2*d_12_pw2*b_pw2
                 -2*p_1_pw2*p_2_pw2;

  factors(3,0) = 2*p_1_pw2*p_2*d_12*b
                 +2*f_2*p_2_pw3*f_1*d_12
                 -2*f_2_pw2*p_2_pw3*d_12*b
                 -2*p_1*p_2*d_12_pw2*b;

  factors(4,0) = -2*f_2*p_2_pw2*f_1*p_1*d_12*b
                 +f_2_pw2*p_2_pw2*d_12_pw2
                 +2*p_1_pw3*d_12
                 -p_1_pw2*d_12_pw2
                 +f_2_pw2*p_2_pw2*p_1_pw2
                 -p_1_pw4
                 -2*f_2_pw2*p_2_pw2*p_1*d_12
                 +p_2_pw2*f_1_pw2*p_1_pw2
                 +f_2_pw2*p_2_pw2*d_12_pw2*b_pw2;

  std::vector<double> realRoots = math::o4_roots(factors);

  for( int i = 0; i < 4; i++ )
  {
    double cot_alpha =
        (-f_1*p_1/f_2-realRoots[i]*p_2+d_12*b)/
        (-f_1*realRoots[i]*p_2/f_2+p_1-d_12);

    double cos_theta = realRoots[i];
    double sin_theta = sqrt(1-pow(realRoots[i],2));
    double sin_alpha = sqrt(1/(pow(cot_alpha,2)+1));
    double cos_alpha = sqrt(1-pow(sin_alpha,2));

    if (cot_alpha < 0)
      cos_alpha = -cos_alpha;

    translation_t C;
    C(0,0) = d_12*cos_alpha*(sin_alpha*b+cos_alpha);
    C(1,0) = cos_theta*d_12*sin_alpha*(sin_alpha*b+cos_alpha);
    C(2,0) = sin_theta*d_12*sin_alpha*(sin_alpha*b+cos_alpha);

    C = P1 + N.transpose()*C;

    rotation_t R;
    R(0,0) = -cos_alpha;
    R(0,1) = -sin_alpha*cos_theta;
    R(0,2) = -sin_alpha*sin_theta;
    R(1,0) = sin_alpha;
    R(1,1) = -cos_alpha*cos_theta;
    R(1,2) = -cos_alpha*sin_theta;
    R(2,0) = 0.0;
    R(2,1) = -sin_theta;
    R(2,2) = cos_theta;

    R = N.transpose()*R.transpose()*T;

    transformation_t solution;
    solution.col(3) = C;
    solution.block<3,3>(0,0) = R;

    solutions.push_back(solution);
  }
}

void
opengv::absolute_pose::modules::p3p_gao_main(
    const bearingVectors_t & f,
    const points_t & points,
    transformations_t & solutions )
{
  point_t A = points[0];
  point_t B = points[1];
  point_t C = points[2];

  Eigen::Vector3d tempp;
  tempp = A-B;
  double AB = tempp.norm();
  tempp = B-C;
  double BC = tempp.norm();
  tempp = A-C;
  double AC = tempp.norm();

  bearingVector_t f1 = f[0];
  bearingVector_t f2 = f[1];
  bearingVector_t f3 = f[2];

  double cosalpha = f2.transpose()*f3;
  double cosbeta = f1.transpose()*f3;
  double cosgamma = f1.transpose()*f2;

  double a=pow((BC/AB),2);
  double b=pow((AC/AB),2);
  double p=2*cosalpha;
  double q=2*cosbeta;
  double r=2*cosgamma;

  double aSq = a * a;
  double bSq = b * b;
  double pSq = p*p;
  double qSq = q*q;
  double rSq = r*r;

  if ((pSq + qSq + rSq - p*q*r - 1) == 0)
    return;

  Eigen::Matrix<double,5,1> factors;

  factors[0] = -2*b + bSq + aSq + 1 - b*rSq*a + 2*b*a - 2*a;

  if (factors[0] == 0)
    return;

  factors[1] =
      -2*b*q*a - 2*aSq*q + b*rSq*q*a - 2*q + 2*b*q +
      4*a*q + p*b*r + b*r*p*a - bSq*r*p;
  factors[2] =
      qSq + bSq*rSq - b*pSq - q*p*b*r + bSq*pSq - b*rSq*a +
      2 - 2*bSq - a*b*r*p*q + 2*aSq - 4*a - 2*qSq*a + qSq*aSq;
  factors[3] =
      -bSq*r*p + b*r*p*a - 2*aSq*q + q*pSq*b +
      2*b*q*a + 4*a*q + p*b*r - 2*b*q - 2*q;
  factors[4] = 1 - 2*a + 2*b + bSq - b*pSq + aSq - 2*b*a;

  std::vector<double> x_temp = math::o4_roots(factors);
  Eigen::Matrix<double,4,1> x;
  for( size_t i = 0; i < 4; i++ ) x[i] = x_temp[i];

  double temp = (pSq*(a-1+b) + p*q*r - q*a*r*p + (a-1-b)*rSq);
  double b0 = b * temp * temp;

  double rCb = rSq*r;

  Eigen::Matrix<double,4,1> tempXP2;
  tempXP2[0] = x[0]*x[0];
  tempXP2[1] = x[1]*x[1];
  tempXP2[2] = x[2]*x[2];
  tempXP2[3] = x[3]*x[3];
  Eigen::Matrix<double,4,1> tempXP3;
  tempXP3[0] = tempXP2[0]*x[0];
  tempXP3[1] = tempXP2[1]*x[1];
  tempXP3[2] = tempXP2[2]*x[2];
  tempXP3[3] = tempXP2[3]*x[3];

  Eigen::Matrix<double,4,1> ones;
  for( size_t i = 0; i < 4; i++) ones[i] = 1.0;

  Eigen::Matrix<double,4,1> b1_part1 =
      (1-a-b)*tempXP2 + (q*a-q)*x + (1 - a + b)*ones;

  Eigen::Matrix<double,4,1> b1_part2 =
      (aSq*rCb + 2*b*rCb*a - b*rSq*rCb*a - 2*a*rCb + rCb + bSq*rCb
      - 2*rCb*b)*tempXP3
      +(p*rSq + p*aSq*rSq - 2*b*rCb*q*a + 2*rCb*b*q - 2*rCb*q - 2*p*(a+b)*rSq
      + rSq*rSq*p*b + 4*a*rCb*q + b*q*a*rCb*rSq - 2*rCb*aSq*q +2*rSq*p*b*a
      + bSq*rSq*p - rSq*rSq*p*bSq)*tempXP2
      +(rCb*qSq + rSq*rCb*bSq + r*pSq*bSq - 4*a*rCb - 2*a*rCb*qSq + rCb*qSq*aSq
      + 2*aSq*rCb - 2*bSq*rCb - 2*pSq*b*r + 4*p*a*rSq*q + 2*a*pSq*r*b
      - 2*a*rSq*q*b*p - 2*pSq*a*r + r*pSq - b*rSq*rCb*a + 2*p*rSq*b*q
      + r*pSq*aSq -2*p*q*rSq + 2*rCb - 2*rSq*p*aSq*q - rSq*rSq*q*b*p)*x
      +(4*a*rCb*q + p*rSq*qSq + 2*pSq*p*b*a - 4*p*a*rSq - 2*rCb*b*q - 2*pSq*q*r
      - 2*bSq*rSq*p + rSq*rSq*p*b + 2*p*aSq*rSq - 2*rCb*aSq*q - 2*pSq*p*a
      + pSq*p*aSq + 2*p*rSq + pSq*p + 2*b*rCb*q*a + 2*q*pSq*b*r + 4*q*a*r*pSq
      - 2*p*a*rSq*qSq - 2*pSq*aSq*r*q + p*aSq*rSq*qSq - 2*rCb*q - 2*pSq*p*b
      + pSq*p*bSq - 2*pSq*b*r*q*a)*ones;

  Eigen::Matrix<double,4,1> b1;
  b1[0] = b1_part1[0]*b1_part2[0];
  b1[1] = b1_part1[1]*b1_part2[1];
  b1[2] = b1_part1[2]*b1_part2[2];
  b1[3] = b1_part1[3]*b1_part2[3];

  Eigen::Matrix<double,4,1> y=b1/b0;
  Eigen::Matrix<double,4,1> tempYP2;
  tempYP2[0] = pow(y[0],2);
  tempYP2[1] = pow(y[1],2);
  tempYP2[2] = pow(y[2],2);
  tempYP2[3] = pow(y[3],2);

  Eigen::Matrix<double,4,1> tempXY;
  tempXY[0] = x[0]*y[0];
  tempXY[1] = x[1]*y[1];
  tempXY[2] = x[2]*y[2];
  tempXY[3] = x[3]*y[3];

  Eigen::Matrix<double,4,1> v= tempXP2 + tempYP2 - r*tempXY;

  Eigen::Matrix<double,4,1> Z;
  Z[0] = AB/sqrt(v[0]);
  Z[1] = AB/sqrt(v[1]);
  Z[2] = AB/sqrt(v[2]);
  Z[3] = AB/sqrt(v[3]);

  Eigen::Matrix<double,4,1> X;
  X[0] = x[0]*Z[0];
  X[1] = x[1]*Z[1];
  X[2] = x[2]*Z[2];
  X[3] = x[3]*Z[3];

  Eigen::Matrix<double,4,1> Y;
  Y[0] = y[0]*Z[0];
  Y[1] = y[1]*Z[1];
  Y[2] = y[2]*Z[2];
  Y[3] = y[3]*Z[3];

  for( int i = 0; i < 4; i++ )
  {
    //apply arun to find the transformation
    points_t p_cam;
    p_cam.push_back(X[i]*f1);
    p_cam.push_back(Y[i]*f2);
    p_cam.push_back(Z[i]*f3);

    transformation_t solution = math::arun_complete(points,p_cam);
    solutions.push_back(solution);
  }
}

void
opengv::absolute_pose::modules::gp3p_main(
    const Eigen::Matrix3d & f,
    const Eigen::Matrix3d & v,
    const Eigen::Matrix3d & p,
    transformations_t & solutions)
{
  Eigen::Matrix<double,48,85> groebnerMatrix =
      Eigen::Matrix<double,48,85>::Zero();
  gp3p::init(groebnerMatrix,f,v,p);
  gp3p::compute(groebnerMatrix);

  Eigen::Matrix<double,8,8> M = Eigen::Matrix<double,8,8>::Zero();
  M.block<6,8>(0,0) = -groebnerMatrix.block<6,8>(36,77);
  M(6,0) = 1.0;
  M(7,6) = 1.0;

  Eigen::ComplexEigenSolver< Eigen::Matrix<double,8,8> > Eig(M,true);
  Eigen::Matrix<std::complex<double>,8,1> D = Eig.eigenvalues();
  Eigen::Matrix<std::complex<double>,8,8> V = Eig.eigenvectors();

  for( int c = 0; c < V.cols(); c++ )
  {
    std::complex<double> eigValue = D[c];

    if( eigValue.imag() < 0.0001 )
    {
      cayley_t cayley;
      Eigen::Vector3d n;

      for(size_t i = 0; i < 3; i++)
      {
        std::complex<double> cay = V(i+4,c)/V(7,c);
        cayley[2-i] = cay.real();
        std::complex<double> depth = V(i+1,c)/V(7,c);
        n[2-i] = depth.real();
      }

      rotation_t rotation = math::cayley2rot(cayley);
      //the groebner problem was set up to find the transpose!
      rotation.transposeInPlace();

      point_t center_cam = Eigen::Vector3d::Zero();
      point_t center_world = Eigen::Vector3d::Zero();
      for( size_t i = 0; i < (size_t) f.cols(); i++ )
      {
        point_t temp = rotation*(n[i]*f.col(i)+v.col(i));
        center_cam = center_cam + temp;
        center_world = center_world + p.col(i);
      }

      center_cam = center_cam/f.cols();
      center_world = center_world/f.cols();
      translation_t translation = center_world - center_cam;

      transformation_t transformation;
      transformation.block<3,3>(0,0) = rotation;
      transformation.col(3) = translation;
      solutions.push_back(transformation);
    }
  }
}

void
opengv::absolute_pose::modules::gpnp_main(
    const Eigen::Matrix<double,12,1> & a,
    const Eigen::Matrix<double,12,12> & V,
    const points_t & c,
    transformation_t & transformation )
{
  //extracting the nullspace vectors
  Eigen::Matrix<double,12,1> vec_5 = V.col(7);
  Eigen::Matrix<double,12,1> vec_4 = V.col(8);
  Eigen::Matrix<double,12,1> vec_3 = V.col(9);
  Eigen::Matrix<double,12,1> vec_2 = V.col(10);
  Eigen::Matrix<double,12,1> vec_1 = V.col(11);

  point_t c0 = c[0];
  point_t c1 = c[0];
  point_t c2 = c[0];
  point_t c3 = c[0];

  Eigen::Matrix<double,12,1> solution;
  std::vector<double> errors;
  translation_t t;
  translations_t ts;
  rotation_t R;
  rotations_t Rs;
  std::vector<double> factors;

  solution = a;
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //nice, now we just need to find the right combination
  //let's start with trying out the linear combination of the most right
  //null-space vector
  Eigen::Matrix<double,5,3> groebnerMatrix1 =
      Eigen::Matrix<double,5,3>::Zero();
  gpnp1::init(groebnerMatrix1,a,vec_1,c0,c1,c2,c3);
  gpnp1::compute(groebnerMatrix1);
  factors.push_back(-groebnerMatrix1(3,2)/groebnerMatrix1(3,1));
  gpnp_optimize( a, V, c, factors );
  solution = a;
  for(size_t i = 0; i < factors.size(); i++)
    solution += factors[i]*V.col(12-factors.size()+i);
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //now let's compute the solution using two nullspace vectors
  Eigen::Matrix<double,10,6> groebnerMatrix2 =
      Eigen::Matrix<double,10,6>::Zero();
  gpnp2::init(groebnerMatrix2,a,vec_2,vec_1,c0,c1,c2,c3);
  gpnp2::compute(groebnerMatrix2);
  factors[0] = -groebnerMatrix2(8,5)/groebnerMatrix2(8,4);
  factors.push_back(
      -(groebnerMatrix2(7,4)*factors[0]+groebnerMatrix2(7,5))/
      groebnerMatrix2(7,3));
  gpnp_optimize( a, V, c, factors );
  solution = a;
  for(size_t i = 0; i < factors.size(); i++)
    solution += factors[i]*V.col(12-factors.size()+i);
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //now let's compute the solution using three nullspace vectors
  Eigen::Matrix<double,15,18> groebnerMatrix3 =
      Eigen::Matrix<double,15,18>::Zero();
  gpnp3::init(groebnerMatrix3,a,vec_3,vec_2,vec_1,c0,c1,c2,c3);
  gpnp3::compute(groebnerMatrix3);
  factors[0] = -groebnerMatrix3(13,17)/groebnerMatrix3(13,16);
  factors[1] =
      -(groebnerMatrix3(12,16)*factors[0]+groebnerMatrix3(12,17))/
      groebnerMatrix3(12,15);
  factors.push_back(
      -(groebnerMatrix3(11,15)*factors[1]+groebnerMatrix3(11,16)*factors[0]+
      groebnerMatrix3(11,17))/groebnerMatrix3(11,14));
  gpnp_optimize( a, V, c, factors );
  solution = a;
  for(size_t i = 0; i < factors.size(); i++)
    solution += factors[i]*V.col(12-factors.size()+i);
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //now let's compute the solution using four nullspace vectors
  Eigen::Matrix<double,25,37> groebnerMatrix4 =
      Eigen::Matrix<double,25,37>::Zero();
  gpnp4::init(groebnerMatrix4,a,vec_4,vec_3,vec_2,vec_1,c0,c1,c2,c3);
  gpnp4::compute(groebnerMatrix4);
  factors[0] = -groebnerMatrix4(23,36)/groebnerMatrix4(23,35);
  factors[1] =
      -(groebnerMatrix4(22,35)*factors[0]+groebnerMatrix4(22,36))/
      groebnerMatrix4(22,34);
  factors[2] =
      -(groebnerMatrix4(21,34)*factors[1]+groebnerMatrix4(21,35)*factors[0]+
      groebnerMatrix4(21,36))/groebnerMatrix4(21,33);
  factors.push_back(
      -(groebnerMatrix4(20,33)*factors[2]+groebnerMatrix4(20,34)*factors[1]+
      groebnerMatrix4(20,35)*factors[0]+groebnerMatrix4(20,36))/
      groebnerMatrix4(20,32));
  gpnp_optimize( a, V, c, factors );
  solution = a;
  for(size_t i = 0; i < factors.size(); i++)
    solution += factors[i]*V.col(12-factors.size()+i);
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //now let's compute the solution using five nullspace vectors
  Eigen::Matrix<double,44,80> groebnerMatrix5 =
      Eigen::Matrix<double,44,80>::Zero();
  gpnp5::init(groebnerMatrix5,a,vec_5,vec_4,vec_3,vec_2,vec_1,c0,c1,c2,c3);
  gpnp5::compute(groebnerMatrix5);
  factors[0] = -groebnerMatrix5(42,79)/groebnerMatrix5(42,78);
  factors[1] =
      -(groebnerMatrix5(41,78)*factors[0]+groebnerMatrix5(41,79))/
      groebnerMatrix5(41,77);
  factors[2] =
      -(groebnerMatrix5(40,77)*factors[1]+groebnerMatrix5(40,78)*factors[0]+
      groebnerMatrix5(40,79))/groebnerMatrix5(40,76);
  factors[3] =
      -(groebnerMatrix5(39,76)*factors[2]+groebnerMatrix5(39,77)*factors[1]+
      groebnerMatrix5(39,78)*factors[0]+groebnerMatrix5(39,79))/
      groebnerMatrix5(39,75);
  factors.push_back(
      -(groebnerMatrix5(38,75)*factors[3]+groebnerMatrix5(38,76)*factors[1]+
      groebnerMatrix5(38,77)*factors[1]+groebnerMatrix5(38,78)*factors[0]+
      groebnerMatrix5(38,79))/groebnerMatrix5(38,74));
  gpnp_optimize( a, V, c, factors );
  solution = a;
  for(size_t i = 0; i < factors.size(); i++)
    solution += factors[i]*V.col(12-factors.size()+i);
  errors.push_back(gpnp_evaluate(solution,c,t,R));
  ts.push_back(t);
  Rs.push_back(R);

  //find best solution
  double smallestError = errors.at(0);
  int minimumIndex = 0;
  for( int i = 1; i < 6; i++ )
  {
    if( errors.at(i) < smallestError )
    {
      smallestError = errors.at(i);
      minimumIndex = i;
    }
  }

  transformation.col(3) = ts.at(minimumIndex);
  transformation.block<3,3>(0,0) = Rs.at(minimumIndex);
}

double
opengv::absolute_pose::modules::gpnp_evaluate(
    const Eigen::Matrix<double,12,1> & solution,
    const points_t & c,
    translation_t & t,
    rotation_t & R )
{
  points_t ccam;
  for(size_t i = 0; i<4; i++)
    ccam.push_back(solution.block<3,1>(i*3,0));

  transformation_t transformation = math::arun_complete(c,ccam);
  t = transformation.col(3);
  R = transformation.block<3,3>(0,0);

  //transform world points into camera frame and compute the error
  double error = 0.0;
  for(size_t i = 0; i<4; i++)
  {
    point_t ccam_reprojected = R.transpose() * (c[i] - t);
    error +=
        1.0 -
        (ccam_reprojected.dot(ccam[i])/(ccam[i].norm()*ccam_reprojected.norm()));
  }

  return error;
}

namespace opengv
{
namespace absolute_pose
{
namespace modules
{

struct GpnpOptimizationFunctor : OptimizationFunctor<double>
{
  const Eigen::Matrix<double,12,1> & _a;
  const Eigen::Matrix<double,12,12> & _V;
  const points_t & _c;
  size_t _dim;

  GpnpOptimizationFunctor(
      const Eigen::Matrix<double,12,1> & a,
      const Eigen::Matrix<double,12,12> & V,
      const points_t & c,
      size_t dim ) :
      OptimizationFunctor<double>(dim,6),
      _a(a),
      _V(V),
      _c(c),
      _dim(dim) {}

  int operator()(const VectorXd &x, VectorXd &fvec) const
  {
    assert( x.size() == _dim );
    assert( (unsigned int) fvec.size() == 6);

    Eigen::Matrix<double,12,1> solution = _a;
    for(size_t i = 0; i < _dim; i++)
      solution += x[i]*_V.col(12-_dim+i);

    points_t ccam;
    for(size_t i = 0; i<4; i++)
      ccam.push_back(solution.block<3,1>(i*3,0));

    Eigen::Vector3d diffw;
    Eigen::Vector3d diffc;
    size_t index = 0;

    for(size_t i = 0; i<3; i++)
    {
      for(size_t j = i+1; j < 4; j++)
      {
        diffw = _c[i]-_c[j];
        diffc = ccam[i]-ccam[j];
        fvec[index++] = diffw.dot(diffw)-diffc.dot(diffc);
      }
    }

    return 0;
  }
};

}
}
}

void
opengv::absolute_pose::modules::gpnp_optimize(
    const Eigen::Matrix<double,12,1> & a,
    const Eigen::Matrix<double,12,12> & V,
    const points_t & c,
    std::vector<double> & factors )
{
  const int n=factors.size();
  VectorXd x(n);

  for(size_t i = 0; i < factors.size(); i++)
    x[i] = factors[i];

  GpnpOptimizationFunctor functor( a, V, c, factors.size() );
  NumericalDiff<GpnpOptimizationFunctor> numDiff(functor);
  LevenbergMarquardt< NumericalDiff<GpnpOptimizationFunctor> > lm(numDiff);

  lm.resetParameters();
  lm.parameters.ftol = 1.E10*NumTraits<double>::epsilon();
  lm.parameters.xtol = 1.E10*NumTraits<double>::epsilon();
  lm.parameters.maxfev = 1000;
  lm.minimize(x);

  for(size_t i = 0; i < factors.size(); i++)
    factors[i] = x[i];
}

void
opengv::absolute_pose::modules::upnp_fill_s(
    const Eigen::Vector4d & quaternion,
    Eigen::Matrix<double,10,1> & s )
{
  s[0] = quaternion[0] * quaternion[0];
  s[1] = quaternion[1] * quaternion[1];
  s[2] = quaternion[2] * quaternion[2];
  s[3] = quaternion[3] * quaternion[3];
  s[4] = quaternion[0] * quaternion[1];
  s[5] = quaternion[0] * quaternion[2];
  s[6] = quaternion[0] * quaternion[3];
  s[7] = quaternion[1] * quaternion[2];
  s[8] = quaternion[1] * quaternion[3];
  s[9] = quaternion[2] * quaternion[3];
}

//we use this one if the number of correspondences is pretty low (more robust)
void
opengv::absolute_pose::modules::upnp_main(
    const Eigen::Matrix<double,10,10> & M,
    const Eigen::Matrix<double,1,10> & C,
    double gamma,
    std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > > & quaternions )
{
  Eigen::Matrix<double,16,16> Action;
  upnp::setupAction_gj( M, C, gamma, Action );
  Eigen::ComplexEigenSolver< Eigen::Matrix<double,16,16> > Eig( Action, true );
  Eigen::Matrix<std::complex<double>,16,16> V = Eig.eigenvectors();

  //cut the double solutions
  double doubleSolThreshold = 0.00000001;

  for( int i = 0; i < 16; i++ )
  {
    //we decided to drop the test for imaginary part
    //I've noticed that when the number of points is really low, things get a little
    //weary with noise, and complex solutions might actually be pretty good

    Eigen::Vector4d quaternion;
    double norm = 0.0;
    for( int q = 0; q < 4; q++ )
    {
      quaternion[q] = V(11+q,i).real();
      norm += pow(quaternion[q],2.0);
    }
    norm = sqrt(norm);
    if(quaternion[0] < 0) // this here is maybe risky, what if quaternion[0] is very small
      norm *= -1.0;
    for( int q = 0; q < 4; q++ )
      quaternion[q] /= norm;

    bool alreadyThere = false;
    for( size_t s = 0; s < quaternions.size(); s++ )
    {
      Eigen::Vector4d diff = quaternion - quaternions[s].second;
      if( diff.norm() < doubleSolThreshold )
      {
        alreadyThere = true;
        break;
      }
    }

    if( !alreadyThere )
    {
      Eigen::Matrix<double,10,1> s;
      upnp_fill_s(quaternion,s);
      Eigen::Matrix<double,1,1> valueM = s.transpose() * M * s + 2.0 * C * s;
      double value = valueM[0] + gamma;

      std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > >::iterator
          qidx = quaternions.begin();
      while( qidx != quaternions.end() && qidx->first < value )
        qidx++;

      quaternions.insert(qidx,std::pair<double,Eigen::Vector4d>(value,quaternion));
    }
  }
}

//this one is the really fast, symmetric version, that we use in the normal case
void
opengv::absolute_pose::modules::upnp_main_sym(
    const Eigen::Matrix<double,10,10> & M,
    const Eigen::Matrix<double,1,10> & C,
    double gamma,
    std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > > & quaternions )
{
  Eigen::Matrix<double,8,8> Action;
  upnp::setupAction_sym_gj( M, C, gamma, Action );
  Eigen::ComplexEigenSolver< Eigen::Matrix<double,8,8> > Eig( Action, true );
  Eigen::Matrix<std::complex<double>,8,8> V = Eig.eigenvectors();

  //ok, let's cut the imaginary solutions (with a reasonable threshold!)
  double imagThreshold = 0.01;
  std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > > bad_quaternions;

  Eigen::Matrix<std::complex<double>,8,1> D = Eig.eigenvalues();
  for( int i = 0; i < 8; i++ )
  {
    Eigen::Vector4d quaternion;
    quaternion[3] = V(7,i).real();
    quaternion[2] = V(6,i).real();
    quaternion[1] = V(5,i).real();
    quaternion[0] = V(4,i).real();

    double norm = 0.0;
    for( int q = 0; q < 4; q++ )
      norm += pow(quaternion[q],2.0);
    norm = sqrt(norm);
    for( int q = 0; q < 4; q++ )
      quaternion[q] /= norm;

    Eigen::Matrix<double,10,1> s;
    upnp_fill_s(quaternion,s);
    Eigen::Matrix<double,1,1> valueM = s.transpose() * M * s + 2.0 * C * s;
    double value = valueM[0] + gamma;

    if( true )//fabs(D[i].imag()) < imagThreshold ) //use all results for the moment
    {
      std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > >::iterator
          qidx = quaternions.begin();
      while( qidx != quaternions.end() && qidx->first < value )
        qidx++;

      quaternions.insert(qidx,std::pair<double,Eigen::Vector4d>(value,quaternion));
    }
    else
    {
      std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > >::iterator
          qidx = bad_quaternions.begin();
      while( qidx != bad_quaternions.end() && qidx->first < value )
        qidx++;

      bad_quaternions.insert(qidx,std::pair<double,Eigen::Vector4d>(value,quaternion));
    }
  }
  if( quaternions.size() == 0 )
    quaternions = bad_quaternions;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIVCEIL_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIVCEIL_HPP_INCLUDED

#include <boost/simd/toolbox/arithmetic/functions/divceil.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/simd/include/functions/simd/ceil.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/divs.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/constants/one.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                              ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_floating<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return ceil(tofloat(a0)/tofloat(a1)); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                            , ((simd_<unsigned_<A0>,X>))
                              ((simd_<unsigned_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return rdivide(a0+a1-One<A0>(), a1); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                            , ((simd_<int16_<A0>,X>))
                              ((simd_<int16_<A0>,X>))
                            )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type          itype;
      typedef simd::native<itype,X>                       ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::fusion::tie(a0l, a0h) = split(a0);
      boost::fusion::tie(a1l, a1h) = split(a1);
      return bitwise_cast<A0>(group(divceil(a0l, a1l),divceil(a0h, a1h)));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                            , ((simd_<int8_<A0>,X>))
                              ((simd_<int8_<A0>,X>))
                            )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type          itype;
      typedef simd::native<itype, X>                      ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::fusion::tie(a0l, a0h) = split(a0);
      boost::fusion::tie(a1l, a1h) = split(a1);
      return simd::bitwise_cast<A0>(group(divceil(a0l, a1l),divceil(a0h, a1h) ));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return ceil(a0/a1); }
  };
} } }

#endif


/*--
  Open3DMotion
  Copyright (c) 2004-2013.
  All rights reserved.
  See LICENSE.txt for more information.
--*/

#include "Open3DMotion/Biomechanics/Algorithms/MOSHFIT/RigidBodyShape.h"

#ifndef OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN
extern "C"
{
#include <f2clibs/f2c.h>
#include <clapack.h>
}
#else
#include <Eigen/Dense>
#include <Eigen/SVD>
#endif

namespace Open3DMotion
{
  const UInt32 RigidBodyResult::success = 0;
  const UInt32 RigidBodyResult::timesequence_mismatch = 1;
  const UInt32 RigidBodyResult::visibility_disconnected = 2;
  const UInt32 RigidBodyResult::insufficient_points = 3;
  const UInt32 RigidBodyResult::did_not_converge = 4;

	RigidBodyShape::RigidBodyShape()
	{
	}

	RigidBodyShape::RigidBodyShape(const RigidBodyShape& src)
	{
		*this = src;
	}

	const RigidBodyShape& RigidBodyShape::operator=(const RigidBodyShape& src)
	{
		marker.assign( src.marker.begin(), src.marker.end() );
		return *this;
	}

	RigidBodyShape::~RigidBodyShape()
	{
	}

	void RigidBodyShape::AddMarker(const double* position, bool visible)
	{
		marker.resize(NumMarkers()+1);
		marker.back().position = position;
		marker.back().visible = visible ? 1 : 0;
	}

	bool RigidBodyShape::IsVisibilitySupersetOf(const RigidBodyShape& other) const
	{
		if (marker.size() == other.marker.size())
		{
			for (std::vector<RigidBodyMarker>::const_iterator iter_this( marker.begin() ), iter_other( other.marker.begin() );
					 iter_this != marker.end(); iter_this++, iter_other++)
			{
				if (iter_other->visible && !iter_this->visible)
					return false;
			}
			return true;
		}
		else
		{
			return false;
		}
	}

	bool RigidBodyShape::IsVisibilitySubsetOf(const RigidBodyShape& other) const
	{
		if (marker.size() == other.marker.size())
		{
			for (std::vector<RigidBodyMarker>::const_iterator iter_this( marker.begin() ), iter_other( other.marker.begin() );
					 iter_this != marker.end(); iter_this++, iter_other++)
			{
				if (iter_this->visible && !iter_other->visible)
					return false;
			}
			return true;
		}
		else
		{
			return false;
		}
	}

	size_t RigidBodyShape::NumberOfVisibleMarkersInCommonWith(const RigidBodyShape& other) const
	{
		size_t count = 0;
		if (marker.size() == other.marker.size())
		{
			for (std::vector<RigidBodyMarker>::const_iterator iter_this( marker.begin() ), iter_other( other.marker.begin() );
					 iter_this != marker.end(); iter_this++, iter_other++)
			{
				if (iter_this->visible && iter_other->visible)
					++count;
			}
		}
		return count;
	}

	void RigidBodyShape::EvaluateNonsingularity3D(std::vector<double>& s, std::vector<double>& coords)
	{
		long num_points(coords.size() / 3);
    s.resize(3);

#ifndef OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN
    long three(3);
		long lwork(256);
		double work[256];
		long info(0);

		std::vector<double> U(9);
		std::vector<double> VT(num_points*num_points);

		// use lapack routine
		// note coords must be column-major so first 3 elements correspond to first coord
		dgesvd_(
			"N",  // don't actually need U
			"N",  // don't actually need VT
			&three, // rows
			&num_points,      // cols
			&coords[0],   // input/output matrix
			&three, // leading dimension of Acpy
			&s[0],      // singular values
			&U[0],      // left orthonormal matrix
			&three, // leading dimension of left
			&VT[0],      // right orthonormal matrix
			&num_points, // leading dimension of right
			work,   // workspace
			&lwork, // size of workspace
			&info);   // returned error codes
#else
		Eigen::Map< Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> >
      _coords(&coords[0], (int)num_points, 3);
    Eigen::Map< Eigen::Matrix<double, 3, 1> > _s(&s[0], 3, 1);
    Eigen::JacobiSVD< Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> > svd(_coords);
    _s = svd.singularValues();
#endif // OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN
	}

	bool RigidBodyShape::HasUniqueFitWith(const RigidBodyShape& other, double tolerance) const
	{
		if (marker.size() == other.marker.size())
		{
			size_t num_in_common(0);
			Vector3 common_centroid(0.0);
			std::vector<double> common_coords;
			common_coords.reserve(3*NumMarkers());
			for (std::vector<RigidBodyMarker>::const_iterator iter_this( marker.begin() ), iter_other( other.marker.begin() );
					 iter_this != marker.end(); iter_this++, iter_other++)
			{
				if (iter_this->visible && iter_other->visible)
				{
					const Vector3& x = iter_this->position;
					for (size_t j = 0; j < 3; j++)
						common_coords.push_back(x[j]);
					num_in_common++;
					common_centroid += x;
				}
			}

			if (num_in_common >= 3)
			{
				// subract centroid
				common_centroid /= num_in_common;
				double* x = &common_coords[0];
				for (size_t j = 0; j < num_in_common; j++, x+=3)
				{
					Vector3::Sub(x, x, common_centroid);
				}

				// find singular values to estimate how non-colinear these points are
				std::vector<double> s;
				RigidBodyShape::EvaluateNonsingularity3D(s, common_coords);

				// Simplified expression to test points sufficiently non-colinear
				// (condition is sufficient but not always necessary)
				double min_colinearity_allowed = 3.56*sqrt((double)num_in_common)*tolerance;
				if (s[1] > min_colinearity_allowed)
					return true;
			}
		}

		return false;
	}

	UInt32 RigidBodyShape::ComputeFitTo(RigidTransform3& T, const RigidBodyShape& base) const
	{
		size_t num_points = NumMarkers();
		if (num_points == base.NumMarkers())
		{
			// get centroids
			Vector3 centroidA(0,0,0);
			Vector3 centroidB(0,0,0);
			const RigidBodyMarker* iter_A = &base.marker[0];
			const RigidBodyMarker* iter_B = &marker[0];
			size_t ipoint(0);
			size_t num_in_common(0);
			for (ipoint = 0; ipoint < num_points; ipoint++, iter_A++,iter_B++)
			{
				if (iter_A->visible && iter_B->visible)
				{
					Vector3::Add(centroidA.x, centroidA.x, iter_A->position);
					Vector3::Add(centroidB.x, centroidB.x, iter_B->position);
					++num_in_common;
				}
			}

			if (num_in_common >= 3)
			{
				// normalise
				centroidA /= num_in_common;
				centroidB /= num_in_common;

				// correlation matrix
				double sum_correl[9] =
				{ 0.0, 0.0, 0.0,
					0.0, 0.0, 0.0,
					0.0, 0.0, 0.0
				};

				// find correlation matrix
				iter_A = &base.marker[0];
				iter_B = &marker[0];
				for (ipoint = 0; ipoint < num_points; ipoint++, iter_A++, iter_B++)
				{
					if (iter_A->visible && iter_B->visible)
					{
						// input minus centroid
						Vector3 normA;
						Vector3::Sub(normA, iter_A->position, centroidA);

						// cal minus centroid
						Vector3 normB;
						Vector3::Sub(normB, iter_B->position, centroidB);

						// correlation (outer product)
						for (int i = 0; i < 3; i++)
							for (int j = 0; j < 3; j++)
							sum_correl[3*i+j] += normA[i]*normB[j];
					}
				}

				// SVD of correlation to get rotation
				// from mean coords to this frame
				double s[3];
				double U[9], VT[9];
				Matrix3x3::SVD(U, s, VT, sum_correl);

				// force right-handed coord system
				double detU = Matrix3x3::Det(U);
				double detV = Matrix3x3::Det(VT);
				if (detU*detV < 0.0)
				{
					VT[6] *= -1.0;
					VT[7] *= -1.0;
					VT[8] *= -1.0;
				}

				// this is the correct way round
				Matrix3x3::Mul(T.R, U, VT);

				// do post-subtraction of centroid B to get translation vector
				double RcentroidB[3];
				Matrix3x3::MulVec(RcentroidB, T.R, centroidB);
				Vector3::Sub(T.t, centroidA, RcentroidB);

		#if 0
				iter_A = ptA;
				iter_B = ptB;
				for (ipoint = 0; ipoint < npoints; ipoint++, iter_A+=3, iter_B+=3)
				{
					Vector3 Tb;
					RigidTransform3::MulVec(Tb, R, t, iter_B);
					cerr << "x: " << iter_A[0] << " " << Tb[0] << endl;
					cerr << "y: " << iter_A[1] << " " << Tb[1] << endl;
					cerr << "z: " << iter_A[2] << " " << Tb[2] << endl;
					Vector3::Sub(Tb, Tb, iter_A);
					cerr << "Mod: " << Tb.Modulus() << endl;
				}
		#endif

				return RigidBodyResult::success;
			}
		}

		return RigidBodyResult::insufficient_points;
	}

}


#pragma once

#if defined(_OPENMP)
#include <omp.h>
#endif

#include <cmath>
#include <cstdint>
#include <functional>
#include <limits>
#include <iostream>
#include <stdexcept>
#include <utility>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <common_robotics_utilities/openmp_helpers.hpp>

namespace common_robotics_utilities
{
/// Implementation of hierarchical clustering, in both Single-link and
/// Complete-link forms. Unlike some implementations, this does not produce the
/// dendrogram of clusters - it only returns the accumulated clusters up to the
/// provided cluster distance bound. In complete-link clustering, the maximum
/// point-to-point distance in any cluster must be <= distance bound, and
/// clustering is performed by combining two items (each could be a point or an
/// existing cluster) if the maximum distance is less than distance bound.
/// Single-link clustering is produced by combining two items (each could be a
/// point or an existing cluster) if the minimum distance is less than distance
/// bound.
/// Complete-link clustering produces dense clusters, while single-link
/// clustering produces long "thin" clusters.
namespace simple_hierarchical_clustering
{
enum class ClusterStrategy { SINGLE_LINK, COMPLETE_LINK };

/// Storage for a single "item" - either an index to a single value or an index
/// to an existing cluster.
class Item
{
private:
  int64_t index_ = -1;
  bool is_cluster_ = false;

public:
  Item() : index_(-1), is_cluster_(false) {}

  Item(const int64_t index, const bool is_cluster)
      : index_(index), is_cluster_(is_cluster)
  {
    if (index_ < 0)
    {
      throw std::invalid_argument("index < 0");
    }
  }

  int64_t Index() const { return index_; }

  bool IsValue() const { return !is_cluster_; }

  bool IsCluster() const { return is_cluster_; }

  bool IsValid() const { return (index_ >= 0); }
};

/// Storage for a pair of "items" and the distance between them
class ClosestPair
{
private:
  Item first_item_;
  Item second_item_;
  double distance_ = std::numeric_limits<double>::infinity();

public:
  ClosestPair() : distance_(std::numeric_limits<double>::infinity()) {}

  ClosestPair(const Item& first_item, const Item& second_item,
              const double distance)
      : first_item_(first_item), second_item_(second_item), distance_(distance)
  {
    if (distance < 0.0)
    {
      throw std::invalid_argument("distance < 0.0");
    }
    if (!first_item_.IsValid())
    {
      throw std::invalid_argument("first_item is not valid");
    }
    if (!second_item_.IsValid())
    {
      throw std::invalid_argument("second_item is not valid");
    }
    if (first_item_.IsValue() == second_item_.IsValue())
    {
      if (first_item_.Index() == second_item_.Index())
      {
        throw std::invalid_argument("first and second items are the same");
      }
    }
  }

  const Item& FirstItem() const { return first_item_; }

  const Item& SecondItem() const { return second_item_; }

  double Distance() const { return distance_; }

  bool IsValid() const
  {
    return first_item_.IsValid() && second_item_.IsValid();
  }
};

/// Find the closest existing clusters in @param clusters, using the pairwise
/// element-to-element distances in @param distance_matrix and the strategy
/// specified by @param strategy. @return closest pair of clusters.
/// Search is performed in parallel.
inline ClosestPair GetClosestClustersParallel(
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy)
{
  std::vector<ClosestPair> per_thread_closest_clusters(
      openmp_helpers::GetNumOmpThreads(), ClosestPair());
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t first_cluster_idx = 0; first_cluster_idx < clusters.size();
       first_cluster_idx++)
  {
    // Skip empty clusters
    const std::vector<int64_t>& first_cluster = clusters.at(first_cluster_idx);
    if (first_cluster.size() > 0)
    {
      // Only compare against remaining clusters
      for (size_t second_cluster_idx = first_cluster_idx + 1;
           second_cluster_idx < clusters.size();
           second_cluster_idx++)
      {
        // Skip empty clusters
        const std::vector<int64_t>& second_cluster
            = clusters.at(second_cluster_idx);
        if (second_cluster.size() > 0)
        {
          // Compute cluster-cluster distance
          double minimum_distance = std::numeric_limits<double>::infinity();
          double maximum_distance = 0.0;
          for (const int64_t& cluster1_index : first_cluster)
          {
            for (const int64_t& cluster2_index : second_cluster)
            {
              const double distance
                  = distance_matrix(cluster1_index, cluster2_index);
              minimum_distance = std::min(minimum_distance, distance);
              maximum_distance = std::max(maximum_distance, distance);
            }
          }
          const double cluster_distance
              = (strategy == ClusterStrategy::COMPLETE_LINK) ? maximum_distance
                                                             : minimum_distance;
          const int32_t thread_num = openmp_helpers::GetContextOmpThreadNum();
          const double current_closest_distance
              = per_thread_closest_clusters.at(thread_num).Distance();
          if (cluster_distance < current_closest_distance)
          {
            per_thread_closest_clusters.at(thread_num)
                = ClosestPair(Item(first_cluster_idx, true),
                              Item(second_cluster_idx, true),
                              cluster_distance);
          }
        }
      }
    }
  }
  ClosestPair closest_clusters;
  for (const ClosestPair& per_thread_closest_cluster_pair
       : per_thread_closest_clusters)
  {
    if (per_thread_closest_cluster_pair.Distance()
        < closest_clusters.Distance())
    {
      closest_clusters = per_thread_closest_cluster_pair;
    }
  }
  return closest_clusters;
}

/// Find the closest existing clusters in @param clusters, using the pairwise
/// element-to-element distances in @param distance_matrix and the strategy
/// specified by @param strategy. @return closest pair of clusters.
inline ClosestPair GetClosestClustersSerial(
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy)
{
  ClosestPair closest_clusters;
  for (size_t first_cluster_idx = 0; first_cluster_idx < clusters.size();
       first_cluster_idx++)
  {
    // Skip empty clusters
    const std::vector<int64_t>& first_cluster = clusters.at(first_cluster_idx);
    if (first_cluster.size() > 0)
    {
      // Only compare against remaining clusters
      for (size_t second_cluster_idx = first_cluster_idx + 1;
           second_cluster_idx < clusters.size();
           second_cluster_idx++)
      {
        // Skip empty clusters
        const std::vector<int64_t>& second_cluster
            = clusters.at(second_cluster_idx);
        if (second_cluster.size() > 0)
        {
          // Compute cluster-cluster distance
          double minimum_distance = std::numeric_limits<double>::infinity();
          double maximum_distance = 0.0;
          for (const int64_t& cluster1_index : first_cluster)
          {
            for (const int64_t& cluster2_index : second_cluster)
            {
              const double distance
                  = distance_matrix(cluster1_index, cluster2_index);
              minimum_distance = std::min(minimum_distance, distance);
              maximum_distance = std::max(maximum_distance, distance);
            }
          }
          const double cluster_distance
              = (strategy == ClusterStrategy::COMPLETE_LINK) ? maximum_distance
                                                             : minimum_distance;
          if (cluster_distance < closest_clusters.Distance())
          {
            closest_clusters = ClosestPair(Item(first_cluster_idx, true),
                                           Item(second_cluster_idx, true),
                                           cluster_distance);
          }
        }
      }
    }
  }
  return closest_clusters;
}

/// Find the closest existing clusters in @param clusters, using the pairwise
/// element-to-element distances in @param distance_matrix and the strategy
/// specified by @param strategy. @return closest pair of clusters.
/// @param use_parallel selects if the search should be performed in parallel.
inline ClosestPair GetClosestClusters(
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy,
    const bool use_parallel)
{
  if (use_parallel)
  {
    return GetClosestClustersParallel(distance_matrix, clusters, strategy);
  }
  else
  {
    return GetClosestClustersSerial(distance_matrix, clusters, strategy);
  }
}

/// Find the closest value-{value, cluster} pair, using @param datapoint_mask to
/// ignore values that have already been clustered, @param distance_matrix to
/// provide pairwise value-to-value distances, existing clusters provided by
/// @param clusters, and strategy specified by @param strategy.
/// Search is performed in parallel. @return closest pair.
inline ClosestPair GetClosestValueToOtherParallel(
    const std::vector<uint8_t>& datapoint_mask,
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy)
{
  std::vector<ClosestPair> per_thread_closest_value_other(
      openmp_helpers::GetNumOmpThreads(), ClosestPair());
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t value_idx = 0; value_idx < datapoint_mask.size(); value_idx++)
  {
    // Make sure we're not already clustered
    if (datapoint_mask.at(value_idx) == 0x00)
    {
      const int32_t thread_num = openmp_helpers::GetContextOmpThreadNum();
      // Check against other values
      for (size_t other_value_idx = value_idx + 1;
           other_value_idx < datapoint_mask.size(); other_value_idx++)
      {
        // Make sure it's not already clustered
        if (datapoint_mask.at(other_value_idx) == 0x00)
        {
          const double distance = distance_matrix(value_idx, other_value_idx);
          const double current_closest_distance
              = per_thread_closest_value_other.at(thread_num).Distance();
          if (distance < current_closest_distance)
          {
            per_thread_closest_value_other.at(thread_num)
                = ClosestPair(Item(value_idx, false),
                              Item(other_value_idx, false),
                              distance);
          }
        }
      }
      // Check against clusters
      for (size_t cluster_idx = 0; cluster_idx < clusters.size(); cluster_idx++)
      {
        const std::vector<int64_t>& cluster = clusters.at(cluster_idx);
        // Skip empty clusters
        if (cluster.size() > 0)
        {
          // Compute cluster-cluster distance
          double minimum_distance = std::numeric_limits<double>::infinity();
          double maximum_distance = 0.0;
          for (const int64_t& cluster_element_idx : cluster)
          {
            const double distance
                = distance_matrix(value_idx, cluster_element_idx);
            minimum_distance = std::min(minimum_distance, distance);
            maximum_distance = std::max(maximum_distance, distance);
          }
          const double cluster_distance
              = (strategy == ClusterStrategy::COMPLETE_LINK) ? maximum_distance
                                                             : minimum_distance;
          const double current_closest_distance
              = per_thread_closest_value_other.at(thread_num).Distance();
          if (cluster_distance < current_closest_distance)
          {
            per_thread_closest_value_other.at(thread_num)
                = ClosestPair(Item(value_idx, false),
                              Item(cluster_idx, true),
                              cluster_distance);
          }
        }
      }
    }
  }
  ClosestPair closest_value_other;
  for (const ClosestPair& value_other : per_thread_closest_value_other)
  {
    if (value_other.Distance() < closest_value_other.Distance())
    {
      closest_value_other = value_other;
    }
  }
  return closest_value_other;
}

/// Find the closest value-{value, cluster} pair, using @param datapoint_mask to
/// ignore values that have already been clustered, @param distance_matrix to
/// provide pairwise value-to-value distances, existing clusters provided by
/// @param clusters, and strategy specified by @param strategy. @return closest
/// pair.
inline ClosestPair GetClosestValueToOtherSerial(
    const std::vector<uint8_t>& datapoint_mask,
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy)
{
  ClosestPair closest_value_other;
  for (size_t value_idx = 0; value_idx < datapoint_mask.size(); value_idx++)
  {
    // Make sure we're not already clustered
    if (datapoint_mask.at(value_idx) == 0x00)
    {
      // Check against other values
      for (size_t other_value_idx = value_idx + 1;
           other_value_idx < datapoint_mask.size(); other_value_idx++)
      {
        // Make sure it's not already clustered
        if (datapoint_mask.at(other_value_idx) == 0x00)
        {
          const double distance = distance_matrix(value_idx, other_value_idx);
          if (distance < closest_value_other.Distance())
          {
            closest_value_other = ClosestPair(Item(value_idx, false),
                                              Item(other_value_idx, false),
                                              distance);
          }
        }
      }
      // Check against clusters
      for (size_t cluster_idx = 0; cluster_idx < clusters.size(); cluster_idx++)
      {
        const std::vector<int64_t>& cluster = clusters.at(cluster_idx);
        // Skip empty clusters
        if (cluster.size() > 0)
        {
          // Compute cluster-cluster distance
          double minimum_distance = std::numeric_limits<double>::infinity();
          double maximum_distance = 0.0;
          for (const int64_t& cluster_element_idx : cluster)
          {
            const double distance
                = distance_matrix(value_idx, cluster_element_idx);
            minimum_distance = std::min(minimum_distance, distance);
            maximum_distance = std::max(maximum_distance, distance);
          }
          const double cluster_distance
              = (strategy == ClusterStrategy::COMPLETE_LINK) ? maximum_distance
                                                             : minimum_distance;
          if (cluster_distance < closest_value_other.Distance())
          {
            closest_value_other = ClosestPair(Item(value_idx, false),
                                              Item(cluster_idx, true),
                                              cluster_distance);
          }
        }
      }
    }
  }
  return closest_value_other;
}

/// Find the closest value-{value, cluster} pair, using @param datapoint_mask to
/// ignore values that have already been clustered, @param distance_matrix to
/// provide pairwise value-to-value distances, existing clusters provided by
/// @param clusters, and strategy specified by @param strategy.
/// @param use_parallel selects if the search should be performed in parallel.
/// @return closest pair.
inline ClosestPair GetClosestValueToOther(
    const std::vector<uint8_t>& datapoint_mask,
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy,
    const bool use_parallel)
{
  if (use_parallel)
  {
    return GetClosestValueToOtherParallel(
        datapoint_mask, distance_matrix, clusters, strategy);
  }
  else
  {
    return GetClosestValueToOtherSerial(
        datapoint_mask, distance_matrix, clusters, strategy);
  }
}

/// Find the closest {value, cluster}-{value, cluster} pair, using @param
/// datapoint_mask to ignore values that have already been clustered, @param
/// distance_matrix to provide pairwise value-to-value distances, existing
/// clusters provided by @param clusters, and strategy specified by @param
/// strategy. @param use_parallel selects if the search should be performed in
/// parallel. @return closest pair.
inline ClosestPair GetClosestPair(
    const std::vector<uint8_t>& datapoint_mask,
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy,
    const bool use_parallel)
{
  const ClosestPair closest_value_to_other
      = GetClosestValueToOther(datapoint_mask, distance_matrix, clusters,
                               strategy, use_parallel);
  const ClosestPair closest_clusters
      = GetClosestClusters(distance_matrix, clusters, strategy, use_parallel);
  if (closest_value_to_other.IsValid() && closest_clusters.IsValid())
  {
    if (closest_value_to_other.Distance() < closest_clusters.Distance())
    {
      return closest_value_to_other;
    }
    else
    {
      return closest_clusters;
    }
  }
  else if (closest_value_to_other.IsValid())
  {
    return closest_value_to_other;
  }
  else if (closest_clusters.IsValid())
  {
    return closest_clusters;
  }
  else
  {
    return ClosestPair();
  }
}

template<typename DataType, typename Container=std::vector<DataType>>
class ClusteringResult
{
private:
  std::vector<Container> clusters_;
  double final_closest_distance_ = 0.0;

public:
  ClusteringResult() {}

  ClusteringResult(const std::vector<Container>& clusters,
                   const double final_closest_distance)
      : clusters_(clusters), final_closest_distance_(final_closest_distance) {}

  const std::vector<Container>& Clusters() const { return clusters_; }

  double FinalClosestDistance() const { return final_closest_distance_; }
};

using IndexClusteringResult = ClusteringResult<int64_t, std::vector<int64_t>>;

/// Perform hierarchical index clustering of @param data up to @param
/// max_cluster_distance, using @param distance_matrix to provide pairwise
/// value-to-value distance for values in @param data. Strategy to use is
/// specified by @param strategy, and @param use_parallel selects if parallel
/// search should be used internally.
inline IndexClusteringResult IndexClusterWithDistanceMatrix(
    const Eigen::MatrixXd& distance_matrix, const double max_cluster_distance,
    const ClusterStrategy strategy, const bool use_parallel = false)
{
  if (distance_matrix.rows() != distance_matrix.cols())
  {
    throw std::invalid_argument("distance_matrix is not square");
  }
  else if (distance_matrix.rows() == 0)
  {
    throw std::invalid_argument("distance_matrix is empty");
  }
  std::vector<uint8_t> datapoint_mask(distance_matrix.rows(), 0u);
  std::vector<std::vector<int64_t>> cluster_indices;
  double closest_distance = 0.0;
  bool complete = false;
  while (!complete)
  {
    // Get closest pair of items (an element can be a cluster or single value!)
    const ClosestPair closest_element_pair
        = GetClosestPair(datapoint_mask, distance_matrix, cluster_indices,
                         strategy, use_parallel);
    if (closest_element_pair.IsValid()
        && closest_element_pair.Distance() <= max_cluster_distance)
    {
      closest_distance = closest_element_pair.Distance();
      const auto& first_item = closest_element_pair.FirstItem();
      const auto& second_item = closest_element_pair.SecondItem();
      // If both elements are values, create a new cluster
      if (first_item.IsValue() && second_item.IsValue())
      {
        // Add a cluster
        cluster_indices.push_back(std::vector<int64_t>{first_item.Index(),
                                                       second_item.Index()});
        // Mask out the indices (this way we know they are already clustered)
        datapoint_mask.at(static_cast<size_t>(first_item.Index())) = 1u;
        datapoint_mask.at(static_cast<size_t>(second_item.Index())) = 1u;
      }
      // If both elements are clusters, merge the clusters
      else if (first_item.IsCluster() && second_item.IsCluster())
      {
        // Merge the second cluster into the first
        std::vector<int64_t>& first_cluster
            = cluster_indices.at(static_cast<size_t>(first_item.Index()));
        std::vector<int64_t>& second_cluster
            = cluster_indices.at(static_cast<size_t>(second_item.Index()));
        first_cluster.insert(
            first_cluster.end(), second_cluster.begin(), second_cluster.end());
        // Empty the second cluster
        // (we don't remove, because this would trigger a move and reallocation)
        second_cluster.clear();
      }
      // If one of the elements is a cluster and the other is a point, add the
      // point to the existing cluster
      else
      {
        const bool first_is_cluster = first_item.IsCluster();
        const int64_t cluster_index
            = (first_is_cluster) ? first_item.Index() : second_item.Index();
        const int64_t value_index
            = (first_is_cluster) ? second_item.Index() : first_item.Index();
        // Add the element to the cluster
        std::vector<int64_t>& cluster
            = cluster_indices.at(static_cast<size_t>(cluster_index));
        cluster.push_back(value_index);
        // Mask out the index (this way we know it is are already clustered)
        datapoint_mask.at(static_cast<size_t>(value_index)) = 1u;
      }
    }
    else
    {
      complete = true;
    }
  }
  // Get rid of empty clusters left over from cluster-cluster merges
  std::vector<std::vector<int64_t>> index_clusters;
  index_clusters.reserve(cluster_indices.size());
  for (const auto& index_cluster : cluster_indices)
  {
    if (index_cluster.size() > 0)
    {
      index_clusters.push_back(index_cluster);
    }
  }
  // Add any points that we haven't clustered into their own clusters
  for (size_t idx = 0; idx < datapoint_mask.size(); idx++)
  {
    // If an element hasn't been clustered at all yet, make a new cluster
    if (datapoint_mask.at(idx) == 0)
    {
      index_clusters.push_back(
          std::vector<int64_t>(1, static_cast<int64_t>(idx)));
    }
  }
  index_clusters.shrink_to_fit();
  return IndexClusteringResult(index_clusters, closest_distance);
}

template<typename DataType, typename Container=std::vector<DataType>>
inline ClusteringResult<DataType, Container>
MakeElementClusteringFromIndexClustering(
    const Container& data,
    const IndexClusteringResult& index_clustering)
{
  std::vector<Container> clusters(index_clustering.Clusters().size());
  for (size_t idx = 0; idx < clusters.size(); idx++)
  {
    const std::vector<int64_t>& current_index_cluster
        = index_clustering.Clusters().at(idx);
    Container& current_cluster = clusters.at(idx);
    // Use reserve + shrink_to_fit for cases where DataType is not
    // default-constructible.
    current_cluster.reserve(current_index_cluster.size());
    for (const int64_t data_index : current_index_cluster)
    {
      current_cluster.push_back(data.at(static_cast<size_t>(data_index)));
    }
    current_cluster.shrink_to_fit();
  }
  return ClusteringResult<DataType, Container>(
      clusters, index_clustering.FinalClosestDistance());
}

/// Perform hierarchical clustering of @param data up to @param
/// max_cluster_distance, using @param distance_matrix to provide pairwise
/// value-to-value distance for values in @param data. Strategy to use is
/// specified by @param strategy, and @param use_parallel selects if parallel
/// search should be used internally.
template<typename DataType, typename Container=std::vector<DataType>>
inline ClusteringResult<DataType, Container> ClusterWithDistanceMatrix(
    const Container& data, const Eigen::MatrixXd& distance_matrix,
    const double max_cluster_distance, const ClusterStrategy strategy,
    const bool use_parallel = false)
{
  // Safety check the input
  if (data.empty())
  {
    throw std::invalid_argument("data is empty");
  }
  if (static_cast<size_t>(distance_matrix.rows()) != data.size()
      || static_cast<size_t>(distance_matrix.cols()) != data.size())
  {
    throw std::invalid_argument("distance_matrix is the wrong size");
  }
  // Perform index clustering
  const auto index_clustering = IndexClusterWithDistanceMatrix(
      distance_matrix, max_cluster_distance, strategy, use_parallel);
  // Extract the actual cluster data from index clusters
  return MakeElementClusteringFromIndexClustering<DataType, Container>(
      data, index_clustering);
}

/// Perform hierarchical index clustering of @param data up to @param
/// max_cluster_distance, using @param distance_fn to compute pairwise
/// value-to-value distance for values in @param data. Strategy to use is
/// specified by @param strategy, and @param use_parallel selects if parallel
/// search should be used internally.
template<typename DataType, typename Container=std::vector<DataType>>
inline IndexClusteringResult IndexCluster(
    const Container& data,
    const std::function<double(const DataType&, const DataType&)>& distance_fn,
    const double max_cluster_distance, const ClusterStrategy strategy,
    const bool use_parallel = false)
{
  const Eigen::MatrixXd distance_matrix
      = math::BuildPairwiseDistanceMatrix<DataType, Container>(
          data, distance_fn, use_parallel);
  return IndexClusterWithDistanceMatrix(
      distance_matrix, max_cluster_distance, strategy, use_parallel);
}

/// Perform hierarchical clustering of @param data up to @param
/// max_cluster_distance, using @param distance_fn to compute pairwise
/// value-to-value distance for values in @param data. Strategy to use is
/// specified by @param strategy, and @param use_parallel selects if parallel
/// search should be used internally.
template<typename DataType, typename Container=std::vector<DataType>>
inline ClusteringResult<DataType, Container> Cluster(
    const Container& data,
    const std::function<double(const DataType&, const DataType&)>& distance_fn,
    const double max_cluster_distance, const ClusterStrategy strategy,
    const bool use_parallel = false)
{
  const Eigen::MatrixXd distance_matrix
      = math::BuildPairwiseDistanceMatrix<DataType, Container>(
          data, distance_fn, use_parallel);
  return ClusterWithDistanceMatrix<DataType, Container>(
      data, distance_matrix, max_cluster_distance, strategy, use_parallel);
}
}  // namespace simple_hierarchical_clustering
}  // namespace common_robotics_utilities


//
// Created by Hao Wu on 11/10/16.
//

//
// Created by Hao Wu on 11/7/16.
//

#include <iostream>
#include "ROWPlus/ROWPlus.h"
#include <boost/numeric/odeint.hpp>

#include "RxnFunctor.h"

using namespace Eigen;
using namespace ROWPlus;
using namespace boost::numeric::odeint;
using namespace Cantera;
using namespace std;

template<typename vec>
void initX(vec& x, IdealGasMix& gas) {
  x.resize(gas.nSpecies()+1);
  gas.setState_TPX(300.0, OneAtm,
                   "H:1e-3, H2:2.0, O:1e-3, OH:1e-3, H2O:1e-1, O2:1e-3, HO2:1e-3, H2O2:1e-3, "
                       "N2:3.76");
  x[0] = gas.temperature();
  gas.getMassFractions(x.data()+1);
}
int main(const int argc, const char *argv[]) {

  const double t1 = strtod(argv[1], NULL);

  IdealGasMix gas("burke_h2_only.xml");
  gas.setState_TPX(300.0, OneAtm,
                   "H:1e-3, H2:2.0, O:1e-3, OH:1e-3, H2O:1e-1, O2:1e-3, HO2:1e-3, H2O2:1e-3, "
                       "N2:3.76");
  RxnFunctor fun(gas);

  std::cout << "Cantera 0D" << std::endl;

  RxnFunctor::state_type x_ref;
  initX(x_ref, gas);
  typedef runge_kutta_cash_karp54<RxnFunctor::state_type> error_stepper_type;
  integrate_adaptive(make_controlled<error_stepper_type>(1.0e-12, 1.0e-12),
                     fun, x_ref, 0.0, t1, 1e-8 );
  cout << "T = " << x_ref[0] << endl;

  // initialize solution vectors
  RxnFunctor::state_type x_t1;
  VectorXd x_t2;

  fun.checkBound(false);
  // creat solver: runge_kutta4_classic
  runge_kutta4_classic< RxnFunctor::state_type > stepper_rk4;
  // create solver: ROWPlus::rosenbrock4
  ROWPlus::rosenbrock4<RxnFunctor, double> stepper_grk4t;
  stepper_grk4t.makeConstantStepper(&fun);
  // creat solver: rosenbrock_krylov4 ROK4A
  rosenbrock_krylov4<RxnFunctor, double> stepper_rok4a(8);
  ODEOptions<double> _opts = stepper_rok4a.getOptions();
  _opts.TypeScheme = ROK4A;
  stepper_rok4a.makeConstantStepper(&fun);
  // creat solver: rosenbrock_krylov4 ROK4E
  rosenbrock_krylov4<RxnFunctor, double> stepper_rok4e(8);
  stepper_rok4e.makeConstantStepper(&fun);

  //
  double dt = 1e-4;
  while (dt >= 1e-7) {
    ROWPlusSolverSpace::Status ret;
    cout << dt << " ";
    // stepper_rk54
    initX(x_t1, gas);
    integrate_const( stepper_rk4 , fun , x_t1 , 0.0 , t1 , dt );
    Map<VectorXd>(x_t1.data(), x_t1.size()) -= Map<VectorXd>(x_ref.data(), x_ref.size());
    cout << Map<VectorXd>(x_t1.data(), x_t1.size()).stableNorm() /
        Map<VectorXd>(x_ref.data(), x_ref.size()).stableNorm() << " ";
    // stepper_grk4t
    initX(x_t2, gas);
    ret = stepper_grk4t.step(x_t2, 0.0, t1, dt);
    if (ret != ROWPlusSolverSpace::ComputeSucessful) {
      cout << "STAT = " << ret << endl;
      return 1;
    }
    x_t2 -= Map<VectorXd>(x_ref.data(), x_ref.size());
    cout << x_t2.stableNorm() /
        Map<VectorXd>(x_ref.data(), x_ref.size()).stableNorm() << " ";
    // stepper_rok4a
    initX(x_t2, gas);
    ret = stepper_rok4a.step(x_t2, 0.0, t1, dt);
    if (ret != ROWPlusSolverSpace::ComputeSucessful) {
      cout << "STAT = " << ret << endl;
      return 1;
    }
    x_t2 -= Map<VectorXd>(x_ref.data(), x_ref.size());
    cout << x_t2.stableNorm() /
        Map<VectorXd>(x_ref.data(), x_ref.size()).stableNorm() << " ";
    // stepper_rok4e
    initX(x_t2, gas);
    ret = stepper_rok4e.step(x_t2, 0.0, t1, dt);
    if (ret != ROWPlusSolverSpace::ComputeSucessful) {
      cout << "STAT = " << ret << endl;
      return 1;
    }
    x_t2 -= Map<VectorXd>(x_ref.data(), x_ref.size());
    cout << x_t2.stableNorm() /
        Map<VectorXd>(x_ref.data(), x_ref.size()).stableNorm() << " ";

    cout << endl;
    dt /= 2.0;
  }
  return 0;
}


// Copyright (C) 2014 The Regents of the University of California (Regents).
// 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.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_relative_pose.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>
#include <memory>
#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/pose/essential_matrix_utils.h"
#include "theia/sfm/pose/five_point_relative_pose.h"
#include "theia/sfm/pose/util.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

// An estimator for computing the relative pose from 5 feature
// correspondences. The feature correspondences should be normalized
// by the focal length with the principal point at (0, 0).
class RelativePoseEstimator
    : public Estimator<FeatureCorrespondence, RelativePose> {
 public:
  RelativePoseEstimator() {}

  // 5 correspondences are needed to determine an essential matrix and thus a
  // relative pose..
  double SampleSize() const { return 5; }

  // Estimates candidate relative poses from correspondences.
  bool EstimateModel(const std::vector<FeatureCorrespondence>& correspondences,
                     std::vector<RelativePose>* relative_poses) const {
    std::vector<Eigen::Vector2d> image1_points, image2_points;
    image1_points.reserve(correspondences.size());
    image2_points.reserve(correspondences.size());
    for (int i = 0; i < correspondences.size(); i++) {
      image1_points.emplace_back(correspondences[i].feature1.point_);
      image2_points.emplace_back(correspondences[i].feature2.point_);
    }

    std::vector<Matrix3d> essential_matrices;
    if (!FivePointRelativePose(
            image1_points, image2_points, &essential_matrices)) {
      return false;
    }

    relative_poses->reserve(essential_matrices.size() * 4);
    for (const Eigen::Matrix3d& essential_matrix : essential_matrices) {
      RelativePose relative_pose;
      relative_pose.essential_matrix = essential_matrix;

      // The best relative pose decomposition should have at least 4
      // triangulated points in front of the camera. This is because one point
      // may be at infinity.
      const int num_points_in_front_of_cameras =
          GetBestPoseFromEssentialMatrix(essential_matrix,
                                         correspondences,
                                         &relative_pose.rotation,
                                         &relative_pose.position);
      if (num_points_in_front_of_cameras >= 4) {
        relative_poses->push_back(relative_pose);
      }
    }
    return relative_poses->size() > 0;
  }

  // The error for a correspondences given a model. This is the squared sampson
  // error.
  double Error(const FeatureCorrespondence& correspondence,
               const RelativePose& relative_pose) const {
    if (IsTriangulatedPointInFrontOfCameras(
            correspondence, relative_pose.rotation, relative_pose.position)) {
      return SquaredSampsonDistance(relative_pose.essential_matrix,
                                    correspondence.feature1.point_,
                                    correspondence.feature2.point_);
    }
    return std::numeric_limits<double>::max();
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(RelativePoseEstimator);
};

}  // namespace

bool EstimateRelativePose(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence>& normalized_correspondences,
    RelativePose* relative_pose,
    RansacSummary* ransac_summary) {
  RelativePoseEstimator relative_pose_estimator;
  std::unique_ptr<SampleConsensusEstimator<RelativePoseEstimator> > ransac =
      CreateAndInitializeRansacVariant(
          ransac_type, ransac_params, relative_pose_estimator);
  // Estimate the relative pose.
  return ransac->Estimate(
      normalized_correspondences, relative_pose, ransac_summary);
}

}  // namespace theia


/*
 * hartman_skrekovski_color.hpp
 * Author: Aven Bross
 *
 * Implementation of Hartman-Skrekovski path 3-choosing algorithm.
 */

#ifndef __HARTMAN_SKREKOVSKI_COLOR_HPP
#define __HARTMAN_SKREKOVSKI_COLOR_HPP

// STL headers
#include <vector>
#include <utility>
#include <algorithm>

// Basic graph headers
#include <boost/graph/graph_traits.hpp>

// Local project headers
#include "disjoint_set.hpp"
#include "incidence_list_helpers.hpp"

namespace {
    // The three states a vertex may have
    const int INTERIOR_MARK = 0;

    /*
     * set_face_location
     *
     * inputs: a vertex v, the new face location for v, a read-write-able vertex
     *     property map storing the face location of each vertex, a disjoint set
     *     structure to store face location sets.
     *
     * outputs: sets the face location of v to the given face location, if the
     *     given location doesn't exist, that is, the integer new_face_location
     *     is negative, a new face location is created in the disjoint set
     *     structure.
     */
    template<typename vertex_t, typename face_location_map_t>
    inline int set_face_location(
            vertex_t v, int new_face_location,
            face_location_map_t & face_location_map,
            disjoint_set_t & face_location_sets
        )
    {
        if(!face_location_sets.exists(new_face_location)) {
            face_location_map[v] = face_location_sets.make_next();
        }
        else {
            face_location_map[v] = face_location_sets.find(new_face_location);
        }
        return face_location_map[v];
    }

    /*
     * hartman_skrekovski_color
     *
     * assumptions: There is a cycle C=v_0v_1...v_k in the weakly triangulated
     *     being colored. Suppose x, y, and p are in C such that p is between x
     *     and y clockwise around C. Suppose all vertices other than x, y, and p
     *     are uncolored. Suppose vertices x, y, and p have lists of size 1 or
     *     more, other vertices in C have lists of size 2 or more, and vertices
     *     in the subgraph bounded by C have lists of size 3 or more.
     *     Additionally, if p is colored some color c, assume vertices between x
     *     and p clockwise, including x, don't have the color c in their lists.
     *
     * inputs: A weakly triangulated planar graph with vertex indices
     *     (predefined boost property), a valid augmented planar embedding of
     *     the graph (modeling the boost AugmentedEmbedding concept defined in
     *     documentation), a read-write-able vertex property map assigning each
     *     vertex an integer to track face location, a disjoint set structure to
     *     store face location sets, a read-write-able vertex property map
     *     assigning each vertex an integer to track state, a read-able vertex
     *     property map assigning a pair of iterators from its augmented
     *     embedding list to track the range of valid neighbors, a read-write-
     *     able vertex property map to which the coloring will be assigned, the
     *     vertices x, y, and p on the outer cycle of the current subgraph, and
     *     the integers for the current face location marks.
     *
     * outputs: The coloring vertex property will contain a valid path list-
     *     coloring of the subgraph bounded by C such that x, y, and p each
     *     receive at most one same color neighbor. Moreover, if x=y=p then
     *     x,y,p receive no same color neighbors.
     */

    template<
            typename graph_t, typename augmented_embedding_t,
            typename face_location_map_t, typename neighbor_range_map_t,
            typename color_list_map_t, typename vertex_t
                = typename boost::graph_traits<graph_t>::vertex_descriptor
        >
    void hartman_skrekovski_color_recursive(
            const graph_t & graph,
            const augmented_embedding_t & augmented_embedding,
            face_location_map_t & face_location_map,
            disjoint_set_t & face_location_sets,
            neighbor_range_map_t & neighbor_range_map,
            color_list_map_t & color_list_map,
            vertex_t x, vertex_t y, vertex_t p,
            int before_p, int before_y, int before_x
        )
    {
        // If p isn't colored yet we are starting a new path
        if(color_list_map[p].size() > 1) {
            // Color p the first color in its list
            color_list_map[p].erase(
                    ++color_list_map[p].begin(),
                    color_list_map[p].end()
                );
        }

        // Grab p's the color from p's singleton list
        auto p_color = color_list_map[p].front();

        // Track the vertices that will become x and y in the first cycle
        vertex_t new_x = x, new_y = y;

        // Iterate through p's adjacency list
        auto neighbor_iter = neighbor_range_map[p].first;
        do {
            // Wrap adjacency list
            if(neighbor_iter == augmented_embedding[p].end())
                neighbor_iter = augmented_embedding[p].begin();

            // Grab neighbor n, and the iterator to p in n's adjacency list
            vertex_t n = neighbor_iter -> vertex;
            auto back_iter = neighbor_iter -> iterator;
            int n_location = face_location_map[n];

            // Look for p_color in n's color list
            auto color_iter = std::find(
                    color_list_map[n].begin(),
                    color_list_map[n].end(), p_color
                );

            // The case n is not in C
            if(face_location_map[n] == INTERIOR_MARK) {
                // Note that n is between x and p on the first cycle
                before_p = set_face_location(
                        n, before_p, face_location_map, face_location_sets
                    );

                // Initialize n's neighbor range to start at the edge pn
                initialize_neighbor_range(
                        n, back_iter, neighbor_range_map, augmented_embedding
                    );

                // Remove the edge pn from n's neighbor range
                remove_first_neighbor(
                        n, neighbor_range_map, augmented_embedding
                    );

                // Remove the path color from n's list
                if(color_iter != color_list_map[n].end()) {
                    color_list_map[n].erase(color_iter);
                    color_iter = color_list_map[n].end();
                }
            }
            // The case n is immediately counterclockwise to p in C
            else if(neighbor_iter == neighbor_range_map[p].first) {
                // If p has a single neighbor the current subgraph is K_2
                if(neighbor_iter == neighbor_range_map[p].second) {
                    // If is neither x nor y, ensure it doesn't receive p_color
                    if(n != x && n != y) {
                        // Remove the path color from n's list
                        if(color_iter != color_list_map[n].end()) {
                            color_list_map[n].erase(color_iter);
                            color_iter = color_list_map[n].end();
                        }
                    }

                    // Color n with any color remaining in its list
                    if(color_list_map[n].size() > 1) {
                        color_list_map[n].erase(
                                ++color_list_map[n].begin(),
                                color_list_map[n].end()
                            );
                    }

                    break;
                }
                // If n is y and y has the path color, swap p and y so the
                //     path runs clockwise
                else if(n == y && color_iter != color_list_map[n].end()) {
                    if(color_list_map[n].size() > 1) {
                        // Remove all colors other than the path color
                        color_list_map[n].erase(
                                color_list_map[n].begin(),
                                color_iter
                            );
                        color_list_map[n].erase(
                                ++color_list_map[n].begin(),
                                color_list_map[n].end()
                            );
                    }

                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            y, p, y,
                            -1, -1, before_y
                        );

                    break;
                }
                else {
                    // If we are removing x, setup new_x as n
                    if(x == p) {
                        new_x = n;

                        // Note that n is between x and p on the first cycle
                        before_p = set_face_location(
                                n, before_p, face_location_map,
                                face_location_sets
                            );
                    }

                    // Remove path color from n's list
                    if(color_iter != color_list_map[n].end()) {
                        color_list_map[n].erase(color_iter);
                        color_iter = color_list_map[n].end();
                    }

                    // Remove the edge pn from n's adjacency list
                    remove_last_neighbor(
                            n, neighbor_range_map, augmented_embedding
                        );
                }
            }
            else {
                // Divide the neighbor range of n at the edge pn
                auto n_ranges = split_neighbor_range(
                        n, back_iter, neighbor_range_map, augmented_embedding
                    );

                // p has been removed from the first cycle so we may remove
                //     those neighbors from p's neighbor range
                neighbor_range_map[p].first = neighbor_iter;

                // The case we are removing y
                if(p == y) {
                    if(color_iter != color_list_map[n].end()) {
                        color_list_map[n].erase(color_iter);
                        color_iter = color_list_map[n].end();
                    }

                    // If p=x=y then we are removing a single vertex path
                    if(p == x) {
                        // Color the subgraph bounded by the first cycle
                        neighbor_range_map[n] = n_ranges.second;
                        hartman_skrekovski_color_recursive(
                                graph, augmented_embedding,
                                face_location_map, face_location_sets,
                                neighbor_range_map, color_list_map,
                                new_x, n, new_x,
                                -1, before_p, before_y
                            );

                        // If the edge pn is a chord, we must color the rest
                        if(back_iter != n_ranges.first.first) {
                            // Color the subgraph bounded by the second cycle
                            neighbor_range_map[n] = n_ranges.first;
                            hartman_skrekovski_color_recursive(
                                    graph, augmented_embedding,
                                    face_location_map, face_location_sets,
                                    neighbor_range_map, color_list_map,
                                    n, p, p,
                                    -1, -1, before_y
                                );
                        }

                        break;
                    }
                    // The case the edge pn is in the cycle, i.e. not a chord
                    else if(neighbor_iter == neighbor_range_map[p].second) {
                        // Color the subgraph bounded by the first cycle
                        neighbor_range_map[n] = n_ranges.second;
                        hartman_skrekovski_color_recursive(
                                graph, augmented_embedding,
                                face_location_map, face_location_sets,
                                neighbor_range_map, color_list_map,
                                new_x, n, new_x,
                                -1, before_p, before_x
                            );

                        break;
                    }
                    // Otherwise we condition on the location of n as usual

                    // Remember n will be our new y vertex for the first cycle
                    new_y = n;
                }

                // The case n is in C[p,y]
                if(face_location_sets.compare(n_location, before_y) || n == y) {
                    vertex_t new_p = n;

                    // The case n needs to be colored
                    if(color_iter != color_list_map[n].end()
                        && color_list_map[n].size() > 1)
                    {
                        // Remove all colors other than the path color
                        color_list_map[n].erase(
                                color_list_map[n].begin(),
                                color_iter
                            );
                        color_list_map[n].erase(
                                ++color_list_map[n].begin(),
                                color_list_map[n].end()
                            );
                    }
                    // The case n will not be added to the path
                    else if(color_list_map[n].size() > 1
                        || color_list_map[n].front() != p_color)
                    {
                        // Start new path at the new x on the first cycle
                        new_p = new_x;

                        // Combine the before_p and before_y segments
                        before_y = face_location_sets.take_union(
                                before_p, before_y
                            );

                        // Note before_p is gone
                        before_p = -1;
                    }

                    /*
                     * This is the most complicated case.
                     * There are two sub-cases to consider:
                     *
                     * Case 1: p_color in L[n]
                     *   In this case |L[n]|=1. In the first call we will
                     *   continue coloring the path from n. In the second
                     *   call, we will immediately find the edge pn and,
                     *   since it the first edge and in the cycle, we will
                     *   immediately remove the two vertex path np. Thus
                     *   no vertex in the path will recieve a new same color
                     *   neighbor from the second call.
                     *
                     * Case 2: p_color is not in L[n]
                     *   In this case the first call will start a new path
                     *   from the vertex new_x. By removing p_color from all
                     *   neighbors of vertices in the path, we ensure no
                     *   vertices in the path, including n, will recieve any
                     *   new same color neighbors in the first call. In the
                     *   second call we will be continuing the path from p.
                     *   Since p_color is not in L[n] and x=y=n, the vertex
                     *   n will not recieve any new same color neighbors in
                     *   the second call.

                     */

                    // Color the subgraph bounded by the first cycle
                    neighbor_range_map[n] = n_ranges.second;
                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            new_x, new_y, new_p,
                            before_p, before_y, before_x
                        );

                    // If the edge pn is a chord, handle the rest
                    if(back_iter != n_ranges.first.first) {
                        // Color the subgraph bounded by the second cycle
                        neighbor_range_map[n] = n_ranges.first;
                        hartman_skrekovski_color_recursive(
                                graph, augmented_embedding,
                                face_location_map, face_location_sets,
                                neighbor_range_map, color_list_map,
                                n, n, p,
                                -1, before_y, -1
                            );
                    }
                }
                // The case n is in C[y,x], n != y
                else if(face_location_sets.compare(n_location, before_x)) {
                    if(color_iter != color_list_map[n].end()) {
                        color_list_map[n].erase(color_iter);
                        color_iter = color_list_map[n].end();
                    }

                    // Color the subgraph bounded by the first cycle
                    neighbor_range_map[n] = n_ranges.second;
                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            new_x, n, new_x,
                            -1, before_p, before_x
                        );

                    // Color the subgraph bounded by the second cycle
                    neighbor_range_map[n] = n_ranges.first;
                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            n, y, p,
                            -1, before_y, before_x
                        );
                }
                // The case n is in C[x,p]
                else {
                    // Color the subgraph bounded b the second cycle
                    neighbor_range_map[n] = n_ranges.first;
                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            x, y, p,
                            before_p, before_y, before_x
                        );

                    // Color the subgraph bounded by the first cycle
                    neighbor_range_map[n] = n_ranges.second;
                    hartman_skrekovski_color_recursive(
                            graph, augmented_embedding,
                            face_location_map, face_location_sets,
                            neighbor_range_map, color_list_map,
                            n, n, n,
                            -1, before_p, -1
                        );
                }

                break;
            }
        } while(neighbor_iter++ != neighbor_range_map[p].second);
    }
}

/*
 * hartman_skrekovski_color
 *
 * inputs: A weakly triangulated planar graph with vertex indices (predefined
 *     boost property), a valid augmented planar embedding of the graph
 *     (modeling the boost AugmentedEmbedding concept defined in documentation),
 *     a read-able vertex property map assigning a range of colors to each
 *     vertex (each vertex must recieve a range of at least 3 colors if
 *     interior, and at least two colors if on the outer face), a read-write-
 *     able vertex property map to which the coloring will be assigned, and
 *     finally a pair of iterators for the list of vertices on the outer face of
 *     the graph in clockwise order.
 *
 * outputs: The coloring will be a valid assignment of colors from the input
 *     color lists such that each color class induces a disjoint union of paths.
 */

template<
        typename graph_t,
        typename augmented_embedding_t,
        typename color_list_map_t,
        typename neighbor_range_map_t,
        typename face_location_map_t,
        typename face_iterator_t
    >
void hartman_skrekovski_color(
        const graph_t & graph,
        const augmented_embedding_t & augmented_embedding,
        face_iterator_t face_begin, face_iterator_t face_end,
        neighbor_range_map_t & neighbor_range_map,
        face_location_map_t & face_location_map,
        color_list_map_t & color_list_map
    )
{
    // Type definitions
    typedef typename boost::graph_traits<graph_t>::vertex_descriptor vertex_t;

    // Setup face location sets (we will have only the region before_y)
    disjoint_set_t face_location_sets;

    // Make a set for 0, the location given to interior vertices
    face_location_sets.make_next();

    // Intitially all vertices will be
    int before_y = -1;

    // Initialize vertices on outer face
    for(auto face_iter = face_begin; face_iter != face_end; ++face_iter)
    {
        auto next = face_iter;
        if(++next == face_end) next = face_begin;

        vertex_t l = *face_iter;
        vertex_t v = *next;

        before_y = set_face_location(
                v, before_y, face_location_map, face_location_sets
            );

        auto back_iter = find_neighbor_iterator(
                v, l, augmented_embedding, graph
            );
        initialize_neighbor_range(
                v, back_iter, neighbor_range_map, augmented_embedding
            );
    }

    vertex_t x = *face_begin, y = *(--face_end);

    // Construct the path choosing from given color lists
    hartman_skrekovski_color_recursive(
            graph, augmented_embedding,
            face_location_map, face_location_sets,
            neighbor_range_map, color_list_map,
            x, y, x,
            -1, before_y, -1
        );
}

/*
 * A wrapper that automatically construct fast property maps for the
 * face_location_map and neighbor_range_map, but requires that graph_t is some
 * definition of boost::adjacency_list.
 */

template<
        typename graph_t,
        typename augmented_embedding_t,
        typename color_list_map_t,
        typename face_iterator_t
    >
void hartman_skrekovski_color(
        const graph_t & graph,
        const augmented_embedding_t & augmented_embedding,
        face_iterator_t face_begin, face_iterator_t face_end,
        color_list_map_t & color_list_map
    )
{
    // Vertex property map to store vertex marks
    typedef boost::iterator_property_map<
            std::vector<int>::iterator,
            typename boost::property_map<
                    graph_t, boost::vertex_index_t
                >::const_type
        > integer_property_map_t;

    // Vertex property map type for the neighbor ranges of planar_embedding_t
    typedef typename boost::property_traits<augmented_embedding_t>::value_type
            ::const_iterator embedding_iterator_t;
    typedef typename std::vector<
            std::pair<embedding_iterator_t, embedding_iterator_t>
        > neighbor_range_storage_t;
    typedef boost::iterator_property_map<
            typename neighbor_range_storage_t::iterator,
            typename boost::property_map<
                    graph_t, boost::vertex_index_t
                >::const_type
        > neighbor_range_map_t;

    // Construct a vertex property for neighbor ranges
    neighbor_range_storage_t neighbor_range_storage(boost::num_vertices(graph));
    neighbor_range_map_t neighbor_range_map(
            neighbor_range_storage.begin(),
            boost::get(boost::vertex_index, graph)
        );

    // Construct a vertex property map to store face location marks
    std::vector<int> face_location_storage(boost::num_vertices(graph));
    integer_property_map_t face_location_map(
            face_location_storage.begin(),
            boost::get(boost::vertex_index, graph)
        );

    // Call Hartman-Skrekovski with the given cycle and
    hartman_skrekovski_color(
            graph,
            augmented_embedding,
            face_begin, face_end,
            neighbor_range_map,
            face_location_map,
            color_list_map
        );
}

#endif