/******************************************************************************
 * 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 <math.h>
#include <vector>
#include <Eigen/NonLinearOptimization>
#include <Eigen/NumericalDiff>

#include <opengv/relative_pose/modules/main.hpp>
#include <opengv/relative_pose/modules/fivept_nister/modules.hpp>
#include <opengv/relative_pose/modules/fivept_stewenius/modules.hpp>
#include <opengv/relative_pose/modules/fivept_kneip/modules.hpp>
#include <opengv/relative_pose/modules/eigensolver/modules.hpp>
#include <opengv/relative_pose/modules/sixpt/modules.hpp>
#include <opengv/relative_pose/modules/ge/modules.hpp>
#include <opengv/OptimizationFunctor.hpp>
#include <opengv/math/arun.hpp>
#include <opengv/math/cayley.hpp>
#include <opengv/math/Sturm.hpp>

#include <stdio.h>
#include <iostream>

void
opengv::relative_pose::modules::fivept_stewenius_main(
    const Eigen::Matrix<double,9,4> & EE,
    complexEssentials_t & complexEssentials )
{
  Eigen::Matrix<double,10,20> A;
  fivept_stewenius::composeA(EE,A);

  Eigen::Matrix<double,10,10> A1 = A.block(0,0,10,10);
  Eigen::Matrix<double,10,10> A2 = A.block(0,10,10,10);

  Eigen::FullPivLU< Eigen::Matrix<double,10,10> > luA1(A1);
  Eigen::Matrix<double,10,10> A3 = luA1.inverse()*A2;

  Eigen::Matrix<double,10,10> M;
  M.block(0,0,3,10) = -A3.block(0,0,3,10);
  M.block(3,0,2,10) = -A3.block(4,0,2,10);
  M.row(5) = -A3.row(7);
  M.block(6,0,4,10) = Eigen::Matrix<double,4,10>::Zero();
  M(6,0) = 1.0;
  M(7,1) = 1.0;
  M(8,3) = 1.0;
  M(9,6) = 1.0;

  Eigen::EigenSolver< Eigen::Matrix<double,10,10> > Eig(M,true);

  //Eigen::Matrix<std::complex<double>,10,1> D = Eig.eigenvalues();
  Eigen::Matrix<std::complex<double>,10,10> V = Eig.eigenvectors();

  Eigen::Matrix<std::complex<double>,3,10> V1;
  V1 = V.block(6,0,3,10);

  Eigen::Matrix<std::complex<double>,3,10> V2;
  V2.row(0) = V.row(9);
  V2.row(1) = V.row(9);
  V2.row(2) = V.row(9);

  Eigen::Matrix<std::complex<double>,4,10> SOLS;

  for( int r = 0; r < 3; r++ )
  {
    for( int c = 0; c < 10; c++ )
      SOLS(r,c) = V1(r,c)/V2(r,c);
  }

  SOLS.row(3) = Eigen::Matrix<std::complex<double>,1,10>::
      Constant(std::complex<double>(1.0,0.0));

  Eigen::Matrix<std::complex<double>,9,10> Evec = EE * SOLS;

  Eigen::Matrix<std::complex<double>,1,10> norms;
  for( int c = 0; c < 10; c++ )
  {
    norms(0,c) = std::complex<double>(0.0,0.0);

    for( int r = 0; r < 9; r++ )
      norms(0,c) += pow( Evec(r,c), 2 );

    norms(0,c) = sqrt(norms(0,c));
  }

  Eigen::Matrix<std::complex<double>,9,10> EvecNorms;
  for( int i = 0; i < 9; i++ )
    EvecNorms.row(i) = norms;

  for( int r = 0; r < 9; r++ )
  {
    for( int c = 0; c < 10; c++ )
      Evec(r,c) = Evec(r,c) / EvecNorms(r,c);
  }

  for( int c = 0; c < 10; c++ )
  {
    complexEssential_t complexEssential;

    complexEssential.row(0) = Evec.block(0,c,3,1).transpose();
    complexEssential.row(1) = Evec.block(3,c,3,1).transpose();
    complexEssential.row(2) = Evec.block(6,c,3,1).transpose();

    complexEssentials.push_back(complexEssential);
  }
}

void
opengv::relative_pose::modules::fivept_nister_main(
    const Eigen::Matrix<double,9,4> & EE,
    essentials_t & essentials )
{
  Eigen::Matrix<double,10,20> A;
  fivept_nister::composeA(EE,A);

  Eigen::Matrix<double,10,10> A1 = A.block(0,0,10,10);
  Eigen::Matrix<double,10,10> A2 = A.block(0,10,10,10);

  Eigen::FullPivLU< Eigen::Matrix<double,10,10> > luA1(A1);
  Eigen::Matrix<double,10,10> A3 = luA1.inverse()*A2;

  Eigen::Matrix<double,1,4> b11_part1 = Eigen::Matrix<double,1,4>::Zero();
  b11_part1.block<1,3>(0,1) = A3.block<1,3>(4,0);
  Eigen::Matrix<double,1,4> b11_part2 = Eigen::Matrix<double,1,4>::Zero();
  b11_part2.block<1,3>(0,0) = A3.block<1,3>(5,0);
  Eigen::Matrix<double,1,4> b11 = b11_part1 - b11_part2;

  Eigen::Matrix<double,1,4> b21_part1 = Eigen::Matrix<double,1,4>::Zero();
  b21_part1.block<1,3>(0,1) = A3.block<1,3>(6,0);
  Eigen::Matrix<double,1,4> b21_part2 = Eigen::Matrix<double,1,4>::Zero();
  b21_part2.block<1,3>(0,0) = A3.block<1,3>(7,0);
  Eigen::Matrix<double,1,4> b21 = b21_part1 - b21_part2;

  Eigen::Matrix<double,1,4> b31_part1 = Eigen::Matrix<double,1,4>::Zero();
  b31_part1.block<1,3>(0,1) = A3.block<1,3>(8,0);
  Eigen::Matrix<double,1,4> b31_part2 = Eigen::Matrix<double,1,4>::Zero();
  b31_part2.block<1,3>(0,0) = A3.block<1,3>(9,0);
  Eigen::Matrix<double,1,4> b31 = b31_part1 - b31_part2;

  Eigen::Matrix<double,1,4> b12_part1 = Eigen::Matrix<double,1,4>::Zero();
  b12_part1.block<1,3>(0,1) = A3.block<1,3>(4,3);
  Eigen::Matrix<double,1,4> b12_part2 = Eigen::Matrix<double,1,4>::Zero();
  b12_part2.block<1,3>(0,0) = A3.block<1,3>(5,3);
  Eigen::Matrix<double,1,4> b12 = b12_part1 - b12_part2;

  Eigen::Matrix<double,1,4> b22_part1 = Eigen::Matrix<double,1,4>::Zero();
  b22_part1.block<1,3>(0,1) = A3.block<1,3>(6,3);
  Eigen::Matrix<double,1,4> b22_part2 = Eigen::Matrix<double,1,4>::Zero();
  b22_part2.block<1,3>(0,0) = A3.block<1,3>(7,3);
  Eigen::Matrix<double,1,4> b22 = b22_part1 - b22_part2;

  Eigen::Matrix<double,1,4> b32_part1 = Eigen::Matrix<double,1,4>::Zero();
  b32_part1.block<1,3>(0,1) = A3.block<1,3>(8,3);
  Eigen::Matrix<double,1,4> b32_part2 = Eigen::Matrix<double,1,4>::Zero();
  b32_part2.block<1,3>(0,0) = A3.block<1,3>(9,3);
  Eigen::Matrix<double,1,4> b32 = b32_part1 - b32_part2;

  Eigen::Matrix<double,1,5> b13_part1 = Eigen::Matrix<double,1,5>::Zero();
  b13_part1.block<1,4>(0,1) = A3.block<1,4>(4,6);
  Eigen::Matrix<double,1,5> b13_part2 = Eigen::Matrix<double,1,5>::Zero();
  b13_part2.block<1,4>(0,0) = A3.block<1,4>(5,6);
  Eigen::Matrix<double,1,5> b13 = b13_part1 - b13_part2;

  Eigen::Matrix<double,1,5> b23_part1 = Eigen::Matrix<double,1,5>::Zero();
  b23_part1.block<1,4>(0,1) = A3.block<1,4>(6,6);
  Eigen::Matrix<double,1,5> b23_part2 = Eigen::Matrix<double,1,5>::Zero();
  b23_part2.block<1,4>(0,0) = A3.block<1,4>(7,6);
  Eigen::Matrix<double,1,5> b23 = b23_part1 - b23_part2;

  Eigen::Matrix<double,1,5> b33_part1 = Eigen::Matrix<double,1,5>::Zero();
  b33_part1.block<1,4>(0,1) = A3.block<1,4>(8,6);
  Eigen::Matrix<double,1,5> b33_part2 = Eigen::Matrix<double,1,5>::Zero();
  b33_part2.block<1,4>(0,0) = A3.block<1,4>(9,6);
  Eigen::Matrix<double,1,5> b33 = b33_part1 - b33_part2;

  Eigen::Matrix<double,1,8> p1_part1;
  fivept_nister::computeSeventhOrderPolynomial(b23,b12,p1_part1);
  Eigen::Matrix<double,1,8> p1_part2;
  fivept_nister::computeSeventhOrderPolynomial(b13,b22,p1_part2);
  Eigen::Matrix<double,1,8> p1 = p1_part1 - p1_part2;
  Eigen::Matrix<double,1,8> p2_part1;
  fivept_nister::computeSeventhOrderPolynomial(b13,b21,p2_part1);
  Eigen::Matrix<double,1,8> p2_part2;
  fivept_nister::computeSeventhOrderPolynomial(b23,b11,p2_part2);
  Eigen::Matrix<double,1,8> p2 = p2_part1 - p2_part2;
  Eigen::Matrix<double,1,7> p3_part1;
  fivept_nister::computeSixthOrderPolynomial(b11,b22,p3_part1);
  Eigen::Matrix<double,1,7> p3_part2;
  fivept_nister::computeSixthOrderPolynomial(b12,b21,p3_part2);
  Eigen::Matrix<double,1,7> p3 = p3_part1 - p3_part2;

  Eigen::Matrix<double,1,11> p_order10_part1;
  fivept_nister::computeTenthOrderPolynomialFrom73(p1,b31,p_order10_part1);
  Eigen::Matrix<double,1,11> p_order10_part2;
  fivept_nister::computeTenthOrderPolynomialFrom73(p2,b32,p_order10_part2);
  Eigen::Matrix<double,1,11> p_order10_part3;
  fivept_nister::computeTenthOrderPolynomialFrom64(p3,b33,p_order10_part3);
  Eigen::Matrix<double,1,11> p_order10 =
      p_order10_part1 + p_order10_part2 + p_order10_part3;

  math::Sturm sturmSequence(p_order10);
  std::vector<double> roots = sturmSequence.findRoots();

  Eigen::MatrixXd Evec(9,roots.size());

  for( size_t i = 0; i < roots.size(); i++ )
  {
    double z = roots[i];
    double x = fivept_nister::polyVal(p1,z)/fivept_nister::polyVal(p3,z);
    double y = fivept_nister::polyVal(p2,z)/fivept_nister::polyVal(p3,z);

    //pollishing here
    fivept_nister::pollishCoefficients(A,x,y,z);

    Evec.col(i) = x*EE.col(0) + y*EE.col(1) + z*EE.col(2) + EE.col(3);
  }

  Eigen::MatrixXd norms(1,roots.size());
  for( size_t c = 0; c < roots.size(); c++ )
  {
    norms(0,c) = 0.0;

    for( int r = 0; r < 9; r++ )
      norms(0,c) += pow( Evec(r,c), 2 );

    norms(0,c) = sqrt(norms(0,c));
  }

  Eigen::MatrixXd EvecNorms(9,roots.size());
  for( size_t i = 0; i < 9; i++ )
    EvecNorms.row(i) = norms;

  for( size_t r = 0; r < 9; r++ )
  {
    for( size_t c = 0; c < roots.size(); c++ )
      Evec(r,c) = Evec(r,c) / EvecNorms(r,c);
  }

  for( size_t c = 0; c < roots.size(); c++ )
  {
    essential_t essential;

    essential.row(0) = Evec.block<3,1>(0,c).transpose();
    essential.row(1) = Evec.block<3,1>(3,c).transpose();
    essential.row(2) = Evec.block<3,1>(6,c).transpose();

    essentials.push_back(essential);
  }
}

void
opengv::relative_pose::modules::fivept_kneip_main(
    const Eigen::Matrix<double,3,5> & f1,
    const Eigen::Matrix<double,3,5> & f2,
    rotations_t & rotations )
{
  Eigen::Matrix<double,66,197> groebnerMatrix =
      Eigen::Matrix<double,66,197>::Zero();
  Eigen::Matrix3d temp1 = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d temp2 = Eigen::Matrix3d::Zero();
  std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> > c_1;
  c_1.push_back(temp1);
  c_1.push_back(temp1);
  c_1.push_back(temp1);
  std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> > c_2;
  c_2.push_back(temp2);
  c_2.push_back(temp2);
  c_2.push_back(temp2);

  int currentRow = 0;

  for( int firstFeat = 0; firstFeat < 5; firstFeat++ )
  {
    for( int secondFeat = firstFeat + 1; secondFeat < 5; secondFeat++ )
    {
      temp1 = f1.col(firstFeat)*f1.col(secondFeat).transpose();
      temp2 = f2.col(firstFeat)*f2.col(secondFeat).transpose();

      for( int thirdFeat = secondFeat + 1; thirdFeat < 5; thirdFeat++ )
      {
        c_1[0] = temp1 * f1(0,thirdFeat);
        c_1[1] = temp1 * f1(1,thirdFeat);
        c_1[2] = temp1 * f1(2,thirdFeat);
        c_2[0] = temp2 * f2(0,thirdFeat);
        c_2[1] = temp2 * f2(1,thirdFeat);
        c_2[2] = temp2 * f2(2,thirdFeat);

        groebnerMatrix.row(currentRow++) =
            fivept_kneip::initEpncpRowR( c_1, c_2 );
      }
    }
  }

  fivept_kneip::initMatrix(groebnerMatrix);
  fivept_kneip::computeBasis(groebnerMatrix);

  Eigen::Matrix<double,20,20> M = Eigen::Matrix<double,20,20>::Zero();
  M.block<10,20>(0,0) = -groebnerMatrix.block<10,20>(51,177);
  M(10,1) = 1.0;
  M(11,2) = 1.0;
  M(12,3) = 1.0;
  M(13,4) = 1.0;
  M(14,5) = 1.0;
  M(15,6) = 1.0;
  M(16,7) = 1.0;
  M(17,8) = 1.0;
  M(18,9) = 1.0;
  M(19,18) = 1.0;

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

  //Eigen::Matrix<std::complex<double>,9,1> tempVector;
  rotation_t finalRotation = Eigen::Matrix3d::Zero();
  for( unsigned int i = 0; i < 20; i++ )
  {
    std::complex<double> tempp;
    tempp = D[i];

    //check if we have a real solution
    if( fabs(tempp.imag()) < 0.1 )
    {
      tempp = V(18,i)/V(19,i);
      finalRotation(0,0) = tempp.real();// tempVector[0] = tempp;
      tempp = V(17,i)/V(19,i);
      finalRotation(0,1) = tempp.real();// tempVector[1] = tempp;
      tempp = V(16,i)/V(19,i);
      finalRotation(0,2) = tempp.real();// tempVector[2] = tempp;
      tempp = V(15,i)/V(19,i);
      finalRotation(1,0) = tempp.real();// tempVector[3] = tempp;
      tempp = V(14,i)/V(19,i);
      finalRotation(1,1) = tempp.real();// tempVector[4] = tempp;
      tempp = V(13,i)/V(19,i);
      finalRotation(1,2) = tempp.real();// tempVector[5] = tempp;
      tempp = V(12,i)/V(19,i);
      finalRotation(2,0) = tempp.real();// tempVector[6] = tempp;
      tempp = V(11,i)/V(19,i);
      finalRotation(2,1) = tempp.real();// tempVector[7] = tempp;
      tempp = V(10,i)/V(19,i);
      finalRotation(2,2) = tempp.real();// tempVector[8] = tempp;

      double tempNorm = finalRotation.row(1).norm();
      finalRotation.row(1) = finalRotation.row(1) / tempNorm;
      tempNorm = finalRotation.row(2).norm();
      finalRotation.row(2) = finalRotation.row(2) / tempNorm;

      //check if the normalized rotation matrix has determinant close enough to 1
      if( fabs( finalRotation.determinant() - 1.0 ) < 0.1 )
      {
        Eigen::Matrix3d eval;
        double totalEval = 0.0;
        eval.col(0) = f1.col(0).cross(finalRotation*f2.col(0));
        eval.col(1) = f1.col(1).cross(finalRotation*f2.col(1));
        eval.col(2) = f1.col(2).cross(finalRotation*f2.col(2));
        totalEval += fabs(eval.determinant());
        eval.col(0) = f1.col(0).cross(finalRotation*f2.col(0));
        eval.col(1) = f1.col(1).cross(finalRotation*f2.col(1));
        eval.col(2) = f1.col(3).cross(finalRotation*f2.col(3));
        totalEval += fabs(eval.determinant());
        eval.col(0) = f1.col(0).cross(finalRotation*f2.col(0));
        eval.col(1) = f1.col(1).cross(finalRotation*f2.col(1));
        eval.col(2) = f1.col(4).cross(finalRotation*f2.col(4));
        totalEval += fabs(eval.determinant());
        eval.col(0) = f1.col(0).cross(finalRotation*f2.col(0));
        eval.col(1) = f1.col(2).cross(finalRotation*f2.col(2));
        eval.col(2) = f1.col(3).cross(finalRotation*f2.col(3));
        totalEval += fabs(eval.determinant());
        eval.col(0) = f1.col(0).cross(finalRotation*f2.col(0));
        eval.col(1) = f1.col(2).cross(finalRotation*f2.col(2));
        eval.col(2) = f1.col(4).cross(finalRotation*f2.col(4));
        totalEval += fabs(eval.determinant());
        eval.col(0) = f1.col(0).cross(finalRotation*f2.col(0));
        eval.col(1) = f1.col(3).cross(finalRotation*f2.col(3));
        eval.col(2) = f1.col(4).cross(finalRotation*f2.col(4));
        totalEval += fabs(eval.determinant());
        eval.col(0) = f1.col(1).cross(finalRotation*f2.col(1));
        eval.col(1) = f1.col(2).cross(finalRotation*f2.col(2));
        eval.col(2) = f1.col(3).cross(finalRotation*f2.col(3));
        totalEval += fabs(eval.determinant());
        eval.col(0) = f1.col(1).cross(finalRotation*f2.col(1));
        eval.col(1) = f1.col(2).cross(finalRotation*f2.col(2));
        eval.col(2) = f1.col(4).cross(finalRotation*f2.col(4));
        totalEval += fabs(eval.determinant());
        eval.col(0) = f1.col(1).cross(finalRotation*f2.col(1));
        eval.col(1) = f1.col(3).cross(finalRotation*f2.col(3));
        eval.col(2) = f1.col(4).cross(finalRotation*f2.col(4));
        totalEval += fabs(eval.determinant());
        eval.col(0) = f1.col(2).cross(finalRotation*f2.col(2));
        eval.col(1) = f1.col(3).cross(finalRotation*f2.col(3));
        eval.col(2) = f1.col(4).cross(finalRotation*f2.col(4));
        totalEval += fabs(eval.determinant());
        totalEval += fabs(
            finalRotation(0,0)*finalRotation(0,0)+
            finalRotation(0,1)*finalRotation(0,1)+
            finalRotation(0,2)*finalRotation(0,2)-1);
        totalEval += fabs(
            finalRotation(0,0)*finalRotation(1,0)+
            finalRotation(0,1)*finalRotation(1,1)+
            finalRotation(0,2)*finalRotation(1,2));
        totalEval += fabs(
            finalRotation(0,0)*finalRotation(2,0)+
            finalRotation(0,1)*finalRotation(2,1)+
            finalRotation(0,2)*finalRotation(2,2));
        totalEval += fabs(
            finalRotation(1,0)*finalRotation(1,0)+
            finalRotation(1,1)*finalRotation(1,1)+
            finalRotation(1,2)*finalRotation(1,2)-1);
        totalEval += fabs(
            finalRotation(1,0)*finalRotation(2,0)+
            finalRotation(1,1)*finalRotation(2,1)+
            finalRotation(1,2)*finalRotation(2,2));
        totalEval += fabs(
            finalRotation(2,0)*finalRotation(2,0)+
            finalRotation(2,1)*finalRotation(2,1)+
            finalRotation(2,2)*finalRotation(2,2)-1);
        totalEval += fabs(
            finalRotation(1,0)*finalRotation(2,1)-
            finalRotation(2,0)*finalRotation(1,1)-finalRotation(0,2));
        totalEval += fabs(
            finalRotation(2,0)*finalRotation(0,1)-
            finalRotation(0,0)*finalRotation(2,1)-finalRotation(1,2));
        totalEval += fabs(
            finalRotation(0,0)*finalRotation(1,1)-
            finalRotation(1,0)*finalRotation(0,1)-finalRotation(2,2));

        //check if the initial constraints are fullfilled to a sufficient extend
        if( totalEval < 0.001 )
        {
          Eigen::Matrix<double,3,5> normalVectors;
          normalVectors.col(0) = f1.col(0).cross(finalRotation * f2.col(0));
          normalVectors.col(1) = f1.col(1).cross(finalRotation * f2.col(1));
          normalVectors.col(2) = f1.col(2).cross(finalRotation * f2.col(2));
          normalVectors.col(3) = f1.col(3).cross(finalRotation * f2.col(3));
          normalVectors.col(4) = f1.col(4).cross(finalRotation * f2.col(4));

          Eigen::Vector3d trans01 =
              normalVectors.col(0).cross(normalVectors.col(1));
          Eigen::Vector3d trans02 =
              normalVectors.col(0).cross(normalVectors.col(2));
          Eigen::Vector3d trans03 =
              normalVectors.col(0).cross(normalVectors.col(3));
          Eigen::Vector3d trans04 =
              normalVectors.col(0).cross(normalVectors.col(4));
          Eigen::Vector3d trans12 =
              normalVectors.col(1).cross(normalVectors.col(2));
          Eigen::Vector3d trans13 =
              normalVectors.col(1).cross(normalVectors.col(3));
          Eigen::Vector3d trans14 =
              normalVectors.col(1).cross(normalVectors.col(4));
          Eigen::Vector3d trans23 =
              normalVectors.col(2).cross(normalVectors.col(3));
          Eigen::Vector3d trans24 =
              normalVectors.col(2).cross(normalVectors.col(4));
          Eigen::Vector3d trans34 =
              normalVectors.col(3).cross(normalVectors.col(4));

          Eigen::Vector3d tempVector1;
          Eigen::Vector3d tempVector2;

          bool positive = true;

          for( int j = 0; j < 5; j++ )
          {
            tempVector1 = trans01.cross( f1.col(j) );
            tempVector2 = trans01.cross( finalRotation * f2.col(j) );
            if( tempVector1.dot(tempVector2) < 0 )
            {
              positive = false;
              break;
            }
            tempVector1 = trans02.cross( f1.col(j) );
            tempVector2 = trans02.cross( finalRotation * f2.col(j) );
            if( tempVector1.dot(tempVector2) < 0 )
            {
              positive = false;
              break;
            }
            tempVector1 = trans03.cross( f1.col(j) );
            tempVector2 = trans03.cross( finalRotation * f2.col(j) );
            if( tempVector1.dot(tempVector2) < 0 )
            {
              positive = false;
              break;
            }
            tempVector1 = trans04.cross( f1.col(j) );
            tempVector2 = trans04.cross( finalRotation * f2.col(j) );
            if( tempVector1.dot(tempVector2) < 0 )
            {
              positive = false;
              break;
            }
            tempVector1 = trans12.cross( f1.col(j) );
            tempVector2 = trans12.cross( finalRotation * f2.col(j) );
            if( tempVector1.dot(tempVector2) < 0 )
            {
              positive = false;
              break;
            }
            tempVector1 = trans13.cross( f1.col(j) );
            tempVector2 = trans13.cross( finalRotation * f2.col(j) );
            if( tempVector1.dot(tempVector2) < 0 )
            {
              positive = false;
              break;
            }
            tempVector1 = trans14.cross( f1.col(j) );
            tempVector2 = trans14.cross( finalRotation * f2.col(j) );
            if( tempVector1.dot(tempVector2) < 0 )
            {
              positive = false;
              break;
            }
            tempVector1 = trans23.cross( f1.col(j) );
            tempVector2 = trans23.cross( finalRotation * f2.col(j) );
            if( tempVector1.dot(tempVector2) < 0 )
            {
              positive = false;
              break;
            }
            tempVector1 = trans24.cross( f1.col(j) );
            tempVector2 = trans24.cross( finalRotation * f2.col(j) );
            if( tempVector1.dot(tempVector2) < 0 )
            {
              positive = false;
              break;
            }
            tempVector1 = trans34.cross( f1.col(j) );
            tempVector2 = trans34.cross( finalRotation * f2.col(j) );
            if( tempVector1.dot(tempVector2) < 0 )
            {
              positive = false;
              break;
            }
          }

          //finally, check if the cheiriality constraint is fullfilled to
          //sufficient extend
          if( positive )
            rotations.push_back(finalRotation);
        }
      }
    }
  }
}

namespace opengv
{
namespace relative_pose
{
namespace modules
{

struct Eigensolver_step : OptimizationFunctor<double>
{
  const Eigen::Matrix3d & _xxF;
  const Eigen::Matrix3d & _yyF;
  const Eigen::Matrix3d & _zzF;
  const Eigen::Matrix3d & _xyF;
  const Eigen::Matrix3d & _yzF;
  const Eigen::Matrix3d & _zxF;

  Eigensolver_step(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF ) :
    OptimizationFunctor<double>(3,3),
    _xxF(xxF),_yyF(yyF),_zzF(zzF),_xyF(xyF),_yzF(yzF),_zxF(zxF) {}

  int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
  {
    cayley_t cayley = x;
    Eigen::Matrix<double,1,3> jacobian;
    eigensolver::getSmallestEVwithJacobian(
        _xxF,_yyF,_zzF,_xyF,_yzF,_zxF,cayley,jacobian);

    fvec[0] = jacobian(0,0);
    fvec[1] = jacobian(0,1);
    fvec[2] = jacobian(0,2);
    return 0;
  }
};

}
}
}

void
opengv::relative_pose::modules::eigensolver_main(
  const Eigen::Matrix3d & xxF,
  const Eigen::Matrix3d & yyF,
  const Eigen::Matrix3d & zzF,
  const Eigen::Matrix3d & xyF,
  const Eigen::Matrix3d & yzF,
  const Eigen::Matrix3d & zxF,
  eigensolverOutput_t & output )
{
  const int n=3;
  VectorXd x(n);

  x = math::rot2cayley(output.rotation);
  Eigensolver_step functor(xxF,yyF,zzF,xyF,yzF,zxF );
  NumericalDiff<Eigensolver_step> numDiff(functor);
  LevenbergMarquardt< NumericalDiff<Eigensolver_step> > lm(numDiff);

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

  cayley_t cayley = x;
  rotation_t R = math::cayley2rot(cayley);

  Eigen::Matrix3d M = eigensolver::composeM(xxF,yyF,zzF,xyF,yzF,zxF,cayley);
  Eigen::EigenSolver< Eigen::Matrix3d > Eig(M,true);
  Eigen::Matrix<std::complex<double>,3,1> D_complex = Eig.eigenvalues();
  Eigen::Matrix<std::complex<double>,3,3> V_complex = Eig.eigenvectors();
  eigenvalues_t D;
  eigenvectors_t V;
  for(size_t i = 0; i < 3; i++)
  {
    D[i] = D_complex[i].real();
    for(size_t j = 0; j < 3; j++)
      V(i,j) = V_complex(i,j).real();
  }

  double translationMagnitude = sqrt(pow(D[1],2) + pow(D[2],2));
  translation_t t = translationMagnitude * V.col(0);

  output.translation = t;
  output.rotation = R;
  output.eigenvalues = D;
  output.eigenvectors = V;

}

void
opengv::relative_pose::modules::sixpt_main(
  Eigen::Matrix<double,6,6> & L1,
  Eigen::Matrix<double,6,6> & L2,
  rotations_t & solutions)
{

  //create vectors of Pluecker coordinates
  std::vector< Eigen::Matrix<double,6,1>, Eigen::aligned_allocator<Eigen::Matrix<double,6,1> > > L1vec;
  std::vector< Eigen::Matrix<double,6,1>, Eigen::aligned_allocator<Eigen::Matrix<double,6,1> > > L2vec;
  for(int i = 0; i < 6; i++ )
  {
    L1vec.push_back( L1.col(i) );
    L2vec.push_back( L2.col(i) );
  }

  //setup the action matrix
  Eigen::Matrix<double,64,64> Action = Eigen::Matrix<double,64,64>::Zero();
  sixpt::setupAction( L1vec, L2vec, Action );

  //finally eigen-decompose the action-matrix and obtain the solutions
  Eigen::EigenSolver< Eigen::Matrix<double,64,64> > Eig(Action,true);
  Eigen::Matrix<std::complex<double>,64,64> EV = Eig.eigenvectors();

  solutions.reserve(64);
  for( int c = 0; c < 64; c++ )
  {
    cayley_t solution;
    for( int r = 0; r < 3; r++ )
    {
      std::complex<double> temp = EV(60+r,c)/EV(63,c);
      solution[r] = temp.real();
    }

    solutions.push_back(math::cayley2rot(solution).transpose());
  }
}

namespace opengv
{
namespace relative_pose
{
namespace modules
{

struct Ge_step : OptimizationFunctor<double>
{
  const Eigen::Matrix3d & _xxF;
  const Eigen::Matrix3d & _yyF;
  const Eigen::Matrix3d & _zzF;
  const Eigen::Matrix3d & _xyF;
  const Eigen::Matrix3d & _yzF;
  const Eigen::Matrix3d & _zxF;
  const Eigen::Matrix<double,3,9> & _x1P;
  const Eigen::Matrix<double,3,9> & _y1P;
  const Eigen::Matrix<double,3,9> & _z1P;
  const Eigen::Matrix<double,3,9> & _x2P;
  const Eigen::Matrix<double,3,9> & _y2P;
  const Eigen::Matrix<double,3,9> & _z2P;
  const Eigen::Matrix<double,9,9> & _m11P;
  const Eigen::Matrix<double,9,9> & _m12P;
  const Eigen::Matrix<double,9,9> & _m22P;

  Ge_step(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    const Eigen::Matrix<double,3,9> & x1P,
    const Eigen::Matrix<double,3,9> & y1P,
    const Eigen::Matrix<double,3,9> & z1P,
    const Eigen::Matrix<double,3,9> & x2P,
    const Eigen::Matrix<double,3,9> & y2P,
    const Eigen::Matrix<double,3,9> & z2P,
    const Eigen::Matrix<double,9,9> & m11P,
    const Eigen::Matrix<double,9,9> & m12P,
    const Eigen::Matrix<double,9,9> & m22P ) :
    OptimizationFunctor<double>(3,3),
    _xxF(xxF),_yyF(yyF),_zzF(zzF),_xyF(xyF),_yzF(yzF),_zxF(zxF),
    _x1P(x1P),_y1P(y1P),_z1P(z1P),_x2P(x2P),_y2P(y2P),_z2P(z2P),
    _m11P(m11P),_m12P(m12P),_m22P(m22P) {}

  int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
  {
    cayley_t cayley = x;
    Eigen::Matrix<double,1,3> jacobian;
    ge::getCostWithJacobian(
        _xxF,_yyF,_zzF,_xyF,_yzF,_zxF,
        _x1P,_y1P,_z1P,_x2P,_y2P,_z2P,_m11P,_m12P,_m22P,cayley,jacobian,1);

    fvec[0] = jacobian(0,0);
    fvec[1] = jacobian(0,1);
    fvec[2] = jacobian(0,2);

    return 0;
  }
};

}
}
}

void
opengv::relative_pose::modules::ge_main(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    const Eigen::Matrix<double,3,9> & x1P,
    const Eigen::Matrix<double,3,9> & y1P,
    const Eigen::Matrix<double,3,9> & z1P,
    const Eigen::Matrix<double,3,9> & x2P,
    const Eigen::Matrix<double,3,9> & y2P,
    const Eigen::Matrix<double,3,9> & z2P,
    const Eigen::Matrix<double,9,9> & m11P,
    const Eigen::Matrix<double,9,9> & m12P,
    const Eigen::Matrix<double,9,9> & m22P,
    const cayley_t & startingPoint,
    geOutput_t & output )
{
  //this one doesn't work, probably because of double numerical differentiation
  //use ge_main2, which is an implementation of gradient descent
  const int n=3;
  VectorXd x(n);

  x = startingPoint;
  Ge_step functor(xxF,yyF,zzF,xyF,yzF,zxF,
      x1P,y1P,z1P,x2P,y2P,z2P,m11P,m12P,m22P);
  NumericalDiff<Ge_step> numDiff(functor);
  LevenbergMarquardt< NumericalDiff<Ge_step> > lm(numDiff);

  lm.resetParameters();
  lm.parameters.ftol = 0.000001;//1.E1*NumTraits<double>::epsilon();
  lm.parameters.xtol = 1.E1*NumTraits<double>::epsilon();
  lm.parameters.maxfev = 100;
  lm.minimize(x);

  cayley_t cayley = x;
  rotation_t R = math::cayley2rot(cayley);

  Eigen::Matrix4d G = ge::composeG(xxF,yyF,zzF,xyF,yzF,zxF,
      x1P,y1P,z1P,x2P,y2P,z2P,m11P,m12P,m22P,cayley);

  Eigen::EigenSolver< Eigen::Matrix4d > Eig(G,true);
  Eigen::Matrix<std::complex<double>,4,1> D_complex = Eig.eigenvalues();
  Eigen::Matrix<std::complex<double>,4,4> V_complex = Eig.eigenvectors();
  Eigen::Vector4d D;
  Eigen::Matrix4d V;
  for(size_t i = 0; i < 4; i++)
  {
    D[i] = D_complex[i].real();
    for(size_t j = 0; j < 4; j++)
      V(i,j) = V_complex(i,j).real();
  }

  double factor = V(3,0);
  Eigen::Vector4d t = (1.0/factor) * V.col(0);

  output.translation = t;
  output.rotation = R;
  output.eigenvalues = D;
  output.eigenvectors = V;
}

void
opengv::relative_pose::modules::ge_main2(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    const Eigen::Matrix<double,3,9> & x1P,
    const Eigen::Matrix<double,3,9> & y1P,
    const Eigen::Matrix<double,3,9> & z1P,
    const Eigen::Matrix<double,3,9> & x2P,
    const Eigen::Matrix<double,3,9> & y2P,
    const Eigen::Matrix<double,3,9> & z2P,
    const Eigen::Matrix<double,9,9> & m11P,
    const Eigen::Matrix<double,9,9> & m12P,
    const Eigen::Matrix<double,9,9> & m22P,
    const cayley_t & startingPoint,
    geOutput_t & output )
{
  //todo: the optimization strategy is something that can possibly be improved:
  //-one idea is to check the gradient at the new sampling point, if that derives
  // too much, we have to stop
  //-another idea consists of having linear change of lambda, instead of exponential (safer, but slower)

  double lambda = 0.01;
  double maxLambda = 0.08;
  double modifier = 2.0;
  int maxIterations = 50;
  double min_xtol = 0.00001;
  bool disablingIncrements = true;
  bool print = false;

  cayley_t cayley;

  double disturbanceAmplitude = 0.3;
  bool found = false;
  int randomTrialCount = 0;

  while( !found && randomTrialCount < 5 )
  {
    if(randomTrialCount > 2)
      disturbanceAmplitude = 0.6;

    if( randomTrialCount == 0 )
      cayley = startingPoint;
    else
    {
      cayley = startingPoint;
      Eigen::Vector3d disturbance;
      disturbance[0] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0*disturbanceAmplitude;
      disturbance[1] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0*disturbanceAmplitude;
      disturbance[2] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0*disturbanceAmplitude;
      cayley += disturbance;
    }

    lambda = 0.01;
    int iterations = 0;
    double smallestEV = ge::getCost(xxF,yyF,zzF,xyF,yzF,zxF,
        x1P,y1P,z1P,x2P,y2P,z2P,m11P,m12P,m22P,cayley,1);

    while( iterations < maxIterations )
    {
      Eigen::Matrix<double,1,3> jacobian;
      ge::getQuickJacobian(xxF,yyF,zzF,xyF,yzF,zxF,
          x1P,y1P,z1P,x2P,y2P,z2P,m11P,m12P,m22P,cayley,smallestEV,jacobian,1);

      double norm = sqrt(pow(jacobian[0],2.0) + pow(jacobian[1],2.0) + pow(jacobian[2],2.0));
      cayley_t normalizedJacobian = (1/norm) * jacobian.transpose();

      cayley_t samplingPoint = cayley - lambda * normalizedJacobian;
      double samplingEV = ge::getCost(xxF,yyF,zzF,xyF,yzF,zxF,
          x1P,y1P,z1P,x2P,y2P,z2P,m11P,m12P,m22P,samplingPoint,1);

      if(print)
      {
        std::cout << iterations << ": " << samplingPoint.transpose();
        std::cout << " lambda: " << lambda << " EV: " << samplingEV << std::endl;
      }

      if( iterations == 0 || !disablingIncrements )
      {
        while( samplingEV < smallestEV )
        {
          smallestEV = samplingEV;
          if( lambda * modifier > maxLambda )
            break;
          lambda *= modifier;
          samplingPoint = cayley - lambda * normalizedJacobian;
          samplingEV = ge::getCost(xxF,yyF,zzF,xyF,yzF,zxF,
              x1P,y1P,z1P,x2P,y2P,z2P,m11P,m12P,m22P,samplingPoint,1);

          if(print)
          {
            std::cout << iterations << ": " << samplingPoint.transpose();
            std::cout << " lambda: " << lambda << " EV: " << samplingEV << std::endl;
          }
        }
      }

      while( samplingEV > smallestEV )
      {
        lambda /= modifier;
        samplingPoint = cayley - lambda * normalizedJacobian;
        samplingEV = ge::getCost(xxF,yyF,zzF,xyF,yzF,zxF,
            x1P,y1P,z1P,x2P,y2P,z2P,m11P,m12P,m22P,samplingPoint,1);

        if(print)
        {
          std::cout << iterations << ": " << samplingPoint.transpose();
          std::cout << " lambda: " << lambda << " EV: " << samplingEV << std::endl;
        }
      }

      //apply update
      cayley = samplingPoint;
      smallestEV = samplingEV;

      //stopping condition (check if the update was too small)
      if( lambda < min_xtol )
        break;

      iterations++;
    }

    //try to see if we can robustly identify each time we enter up in the wrong minimum
    if( cayley.norm() < 0.01 )
    {
      //we are close to the origin, test the EV 2
      double ev2 = ge::getCost(xxF,yyF,zzF,xyF,yzF,zxF,
            x1P,y1P,z1P,x2P,y2P,z2P,m11P,m12P,m22P,cayley,0);
      if( ev2 > 0.001 )
        randomTrialCount++;
      else
        found = true;
    }
    else
      found = true;
  }

  Eigen::Matrix4d G = ge::composeG(xxF,yyF,zzF,xyF,yzF,zxF,
      x1P,y1P,z1P,x2P,y2P,z2P,m11P,m12P,m22P,cayley);

  Eigen::EigenSolver< Eigen::Matrix4d > Eig(G,true);
  Eigen::Matrix<std::complex<double>,4,1> D_complex = Eig.eigenvalues();
  Eigen::Matrix<std::complex<double>,4,4> V_complex = Eig.eigenvectors();
  Eigen::Vector4d D;
  Eigen::Matrix4d V;
  for(size_t i = 0; i < 4; i++)
  {
    D[i] = D_complex[i].real();
    for(size_t j = 0; j < 4; j++)
      V(i,j) = V_complex(i,j).real();
  }

  double factor = V(3,0);
  Eigen::Vector4d t = (1.0/factor) * V.col(0);

  output.translation = t;
  output.rotation = math::cayley2rot(cayley);
  output.eigenvalues = D;
  output.eigenvectors = V;
}

void
opengv::relative_pose::modules::ge_plot(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    const Eigen::Matrix<double,3,9> & x1P,
    const Eigen::Matrix<double,3,9> & y1P,
    const Eigen::Matrix<double,3,9> & z1P,
    const Eigen::Matrix<double,3,9> & x2P,
    const Eigen::Matrix<double,3,9> & y2P,
    const Eigen::Matrix<double,3,9> & z2P,
    const Eigen::Matrix<double,9,9> & m11P,
    const Eigen::Matrix<double,9,9> & m12P,
    const Eigen::Matrix<double,9,9> & m22P,
    geOutput_t & output )
{
  cayley_t cayley = math::rot2cayley(output.rotation);

  ge::getEV(xxF,yyF,zzF,xyF,yzF,zxF,
      x1P,y1P,z1P,x2P,y2P,z2P,m11P,m12P,m22P,cayley,output.eigenvalues);

  output.eigenvectors = ge::composeG(xxF,yyF,zzF,xyF,yzF,zxF,
      x1P,y1P,z1P,x2P,y2P,z2P,m11P,m12P,m22P,cayley);
}


#ifndef FAST_GICP_FAST_VGICP_CUDA_IMPL_HPP
#define FAST_GICP_FAST_VGICP_CUDA_IMPL_HPP

#include <atomic>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <pcl/registration/registration.h>

#include <sophus/so3.hpp>
#include <fast_gicp/gicp/fast_vgicp_cuda.hpp>
#include <fast_gicp/cuda/fast_vgicp_cuda.cuh>

namespace fast_gicp {

template<typename PointSource, typename PointTarget>
FastVGICPCuda<PointSource, PointTarget>::FastVGICPCuda() {
  reg_name_ = "FastVGICPCuda";
  max_iterations_ = 64;
  k_correspondences_ = 20;
  rotation_epsilon_ = 2e-3;
  transformation_epsilon_ = 5e-4;

  voxel_resolution_ = 1.0;
  regularization_method_ = PLANE;
  neighbor_search_method_ = CPU_PARALLEL_KDTREE;

  vgicp_cuda.reset(new FastVGICPCudaCore());
  vgicp_cuda->set_max_iterations(max_iterations_);
  vgicp_cuda->set_rotation_epsilon(rotation_epsilon_);
  vgicp_cuda->set_transformation_epsilon(transformation_epsilon_);
  vgicp_cuda->set_resolution(voxel_resolution_);
}

template<typename PointSource, typename PointTarget>
FastVGICPCuda<PointSource, PointTarget>::~FastVGICPCuda() {}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setRotationEpsilon(double eps) {
  rotation_epsilon_ = eps;
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setResolution(double resolution) {
  voxel_resolution_ = resolution;
  vgicp_cuda->set_resolution(voxel_resolution_);
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setCorrespondenceRandomness(int k) {
  k_correspondences_ = k;
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setRegularizationMethod(RegularizationMethod method) {
  regularization_method_ = method;
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setNearesetNeighborSearchMethod(NearestNeighborMethod method) {
  neighbor_search_method_ = method;
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::swapSourceAndTarget() {
  vgicp_cuda->swap_source_and_target();
  input_.swap(target_);
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::clearSource() {
  input_.reset();
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::clearTarget() {
  target_.reset();
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
  // the input cloud is the same as the previous one
  if(cloud == input_) {
    return;
  }

  pcl::Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);

  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> points(cloud->size());
  std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointSource& pt) { return pt.getVector3fMap(); });

  vgicp_cuda->set_source_cloud(points);
  switch(neighbor_search_method_) {
    case CPU_PARALLEL_KDTREE: {
      std::vector<int> neighbors = find_neighbors_parallel_kdtree(k_correspondences_, cloud, source_kdtree);
      vgicp_cuda->set_source_neighbors(k_correspondences_, neighbors);
    }
      break;
    case GPU_BRUTEFORCE:
      vgicp_cuda->find_source_neighbors(k_correspondences_);
      break;
  }
  vgicp_cuda->calculate_source_covariances(regularization_method_);
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
  // the input cloud is the same as the previous one
  if(cloud == target_) {
    return;
  }

  pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);

  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> points(cloud->size());
  std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointTarget& pt) { return pt.getVector3fMap(); });

  vgicp_cuda->set_target_cloud(points);
  switch(neighbor_search_method_) {
    case CPU_PARALLEL_KDTREE: {
      std::vector<int> neighbors = find_neighbors_parallel_kdtree(k_correspondences_, cloud, target_kdtree);
      vgicp_cuda->set_target_neighbors(k_correspondences_, neighbors);
    } break;
    case GPU_BRUTEFORCE:
      vgicp_cuda->find_target_neighbors(k_correspondences_);
      break;
  }
  vgicp_cuda->calculate_target_covariances(regularization_method_);
  vgicp_cuda->create_target_voxelmap();
}

template<typename PointSource, typename PointTarget>
template<typename PointT>
std::vector<int> FastVGICPCuda<PointSource, PointTarget>::find_neighbors_parallel_kdtree(int k, const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree) const {
  kdtree.setInputCloud(cloud);
  std::vector<int> neighbors(cloud->size() * k);

#pragma omp parallel for
  for(int i = 0; i < cloud->size(); i++) {
    std::vector<int> k_indices;
    std::vector<float> k_sq_distances;
    kdtree.nearestKSearch(cloud->at(i), k, k_indices, k_sq_distances);

    std::copy(k_indices.begin(), k_indices.end(), neighbors.begin() + i * k);
  }

  return neighbors;
}

template<typename PointSource, typename PointTarget>
void FastVGICPCuda<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
  Eigen::Isometry3f initial_guess(guess);
  Eigen::Isometry3f estimated = Eigen::Isometry3f::Identity();

  vgicp_cuda->set_max_iterations(max_iterations_);
  vgicp_cuda->set_rotation_epsilon(rotation_epsilon_);
  vgicp_cuda->set_transformation_epsilon(transformation_epsilon_);
  vgicp_cuda->set_resolution(voxel_resolution_);

  converged_ = vgicp_cuda->optimize(initial_guess, estimated);

  final_transformation_ = estimated.matrix();
  pcl::transformPointCloud(*input_, output, final_transformation_);
}

}  // namespace fast_gicp

#endif


// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "camera.h"

#include <iostream>
#include <Eigen/LU>

Frame Frame::lerp(float alpha, const Frame& other) const
{
    return Frame((1.f-alpha)*position + alpha * other.position,
                 orientation.slerp(alpha,other.orientation));
}

Eigen::Matrix4f Frame::getMatrix() const
{
    Eigen::Matrix3f rotation = orientation.toRotationMatrix();
    Eigen::Translation3f translation(position);
    Eigen::Affine3f transformation = translation * rotation;
    return transformation.matrix();
}

Camera::Camera()
    : mViewIsUptodate(false), mProjIsUptodate(false)
{
    mViewMatrix.setIdentity();

    mFovY = M_PI/3.;
    mNearDist = 0.1;
    mFarDist = 50000.;

    mVpX = 0;
    mVpY = 0;

    lookAt(Vector3f::Constant(100.), Vector3f::Zero(), Vector3f::UnitZ());

    mSampleCount = 1;
    mIsInitialized = false;
}

Camera::Camera(const PropertyList &propList)
    : mViewIsUptodate(false), mProjIsUptodate(false)
{
    mViewMatrix.setIdentity();
    mVpX = 0;
    mVpY = 0;

    /* Width and height in pixels.*/
    mVpWidth = propList.getInteger("width", 512);
    mVpHeight = propList.getInteger("height", 512);

    /* Specifies a camera-to-world transformation. Default: none */
    Transform mCameraToWorld = propList.getTransform("toWorld", Transform());

    mFrame.position = mCameraToWorld.getMatrix().topRightCorner<3,1>();
    mFrame.orientation = Eigen::Quaternionf(mCameraToWorld.getMatrix().topLeftCorner<3,3>());

    /* Horizontal field of view in degrees */
    mFovY = degToRad(propList.getFloat("fieldOfView", 30.0f));

    /* Near and far clipping planes in world-space units */
    mNearDist = propList.getFloat("nearClip", 1e-4f);
    mFarDist = propList.getFloat("farClip", 1e4f);

    /* Number of samples per pixel */
    mSampleCount = propList.getInteger("samples", 1);

    updateViewMatrix();
    updateProjectionMatrix();

    mIsInitialized = false;
}

Camera& Camera::operator=(const Camera& other)
{
    mViewIsUptodate = false;
    mProjIsUptodate = false;

    mVpX = other.mVpX;
    mVpY = other.mVpY;
    mVpWidth = other.mVpWidth;
    mVpHeight = other.mVpHeight;

    mTarget = other.mTarget;
    mFovY = other.mFovY;
    mNearDist = other.mNearDist;
    mFarDist = other.mFarDist;

    mViewMatrix = other.mViewMatrix;
    mProjectionMatrix = other.mProjectionMatrix;

    mFrame = other.mFrame;
    mSampleCount = other.mSampleCount;

    mIsInitialized = false;

    return *this;
}

Camera::Camera(const Camera& other)
{
    *this = other;
}

Camera::~Camera()
{
}

void Camera::setPerspective(float fovY, float nearp, float farp)
{
    mFovY = fovY;
    mNearDist = nearp;
    mFarDist = farp;
    mProjIsUptodate = false;
}

void Camera::setViewport(uint offsetx, uint offsety, uint width, uint height)
{
    mVpX = offsetx;
    mVpY = offsety;
    mVpWidth = width;
    mVpHeight = height;

    mProjIsUptodate = false;
}

void Camera::setViewport(uint width, uint height)
{
    mVpWidth = width;
    mVpHeight = height;

    mProjIsUptodate = false;
}

void Camera::setFovY(float value)
{
    mFovY = value;
    mProjIsUptodate = false;
}

Vector3f Camera::direction() const
{
    updateViewMatrix();
    return -mViewMatrix.linear().row(2);
}

Vector3f Camera::up() const
{
    updateViewMatrix();
    return mViewMatrix.linear().row(1);
}

Vector3f Camera::right() const
{
    updateViewMatrix();
    return mViewMatrix.linear().row(0);
}

void Camera::lookAt(const Point3f& position, const Point3f& target, const Point3f& up)
{
    mTarget = target;
    mFrame.position = position;
    Eigen::Matrix3f R;
    R.col(2) = (position-target).normalized();
    R.col(0) = up.cross(R.col(2)).normalized();
    R.col(1) = R.col(2).cross(R.col(0));
    setOrientation(Eigen::Quaternionf(R));
    mViewIsUptodate = false;
}

void Camera::setPosition(const Point3f &p)
{
    mFrame.position = p;
    mViewIsUptodate = false;
}

void Camera::setOrientation(const Eigen::Quaternionf& q)
{
    mFrame.orientation = q;
    mViewIsUptodate = false;
}

void Camera::setFrame(const Frame& f)
{
    mFrame = f;
    mViewIsUptodate = false;
}

void Camera::rotateAroundTarget(const Eigen::Quaternionf& q)
{
    Eigen::Matrix4f mrot, mt, mtm;

    // update the transform matrix
    updateViewMatrix();
    Vector3f t = mViewMatrix * mTarget;

    mViewMatrix = Eigen::Translation3f(t)
            * q
            * Eigen::Translation3f(-t)
            * mViewMatrix;

    Eigen::Quaternionf qa(mViewMatrix.linear());
    qa = qa.conjugate();
    setOrientation(qa);
    setPosition(- (qa * mViewMatrix.translation()) );

    mViewIsUptodate = true;
}

void Camera::localRotate(const Eigen::Quaternionf& q)
{
    float dist = (position() - mTarget).norm();
    setOrientation(orientation() * q);
    mTarget = position() + dist * direction();
    mViewIsUptodate = false;
}

void Camera::zoom(float d)
{
    float dist = (position() - mTarget).norm();
    if(dist > d)
    {
        setPosition(position() + direction() * d);
        mViewIsUptodate = false;
    }
}

void Camera::localTranslate(const Point3f &t)
{
    Vector3f trans = orientation() * t;
    setPosition( position() + trans );
    mTarget += trans;

    mViewIsUptodate = false;
}

void Camera::updateViewMatrix() const
{
    if(!mViewIsUptodate)
    {
        Eigen::Quaternionf q = orientation().conjugate();
        mViewMatrix.linear() = q.toRotationMatrix();
        mViewMatrix.translation() = - (mViewMatrix.linear() * position());

        mViewIsUptodate = true;
    }
}

const Eigen::Affine3f& Camera::viewMatrix() const
{
    updateViewMatrix();
    return mViewMatrix;
}

void Camera::updateProjectionMatrix() const
{
    if(!mProjIsUptodate)
    {
        mProjectionMatrix.setIdentity();
        float aspect = float(mVpWidth)/float(mVpHeight);
        float theta = mFovY*0.5;
        float range = mFarDist - mNearDist;
        float invtan = 1./tan(theta);

        mProjectionMatrix(0,0) = invtan / aspect;
        mProjectionMatrix(1,1) = invtan;
        mProjectionMatrix(2,2) = -(mNearDist + mFarDist) / range;
        mProjectionMatrix(3,2) = -1;
        mProjectionMatrix(2,3) = -2 * mNearDist * mFarDist / range;
        mProjectionMatrix(3,3) = 0;

        mProjIsUptodate = true;
    }
}

const Eigen::Matrix4f& Camera::projectionMatrix() const
{
    updateProjectionMatrix();
    return mProjectionMatrix;
}

Point3f Camera::unProject(const Vector2f& uv, float depth) const
{
    Eigen::Matrix4f inv = mViewMatrix.inverse().matrix();
    return unProject(uv, depth, inv);
}

Point3f Camera::unProject(const Vector2f& uv, float depth, const Eigen::Matrix4f& invModelview) const
{
    updateViewMatrix();
    updateProjectionMatrix();

    Vector3f a(2.*uv.x()/float(mVpWidth)-1., 2.*uv.y()/float(mVpHeight)-1., 1.);
    a.x() *= depth/mProjectionMatrix(0,0);
    a.y() *= depth/mProjectionMatrix(1,1);
    a.z() = -depth;

    Vector3f b = a.x() * right() + a.y() * up() - a.z() * direction() + position();

    return Point3f(b.x(), b.y(), b.z());
}

void Camera::draw(nanogui::GLShader* prg)
{
    if(!mIsInitialized)
    {
        mIsInitialized = true;
        mPoints.clear();

        // grille
        float ym = tan(mFovY*0.5);
        float xm = ((float)mVpWidth)*(ym*1.0/mVpHeight);
        float zm = 0.75f;
        for(uint x=1; x<mVpWidth; ++x){
            mPoints.push_back(Point3f(xm*(x*2.0/mVpWidth-1.0),ym,-zm));
            mPoints.push_back(Point3f(xm*(x*2.0/mVpWidth-1.0),-ym,-zm));
        }
        for(uint y=1; y<mVpHeight; ++y){
            mPoints.push_back(Point3f(xm,ym*(y*2.0/mVpHeight-1.0),-zm));
            mPoints.push_back(Point3f(-xm,ym*(y*2.0/mVpHeight-1.0),-zm));
        }

        //pyramide
        mPoints.push_back(Point3f(0.0,0.0,0.0));
        mPoints.push_back(Point3f(xm,ym,-zm));
        mPoints.push_back(Point3f(xm,ym,-zm));
        mPoints.push_back(Point3f(xm,-ym,-zm));
        mPoints.push_back(Point3f(xm,-ym,-zm));
        mPoints.push_back(Point3f(0.0,0.0,0.0));
        mPoints.push_back(Point3f(0.0,0.0,0.0));
        mPoints.push_back(Point3f(-xm,ym,-zm));
        mPoints.push_back(Point3f(-xm,ym,-zm));
        mPoints.push_back(Point3f(-xm,-ym,-zm));
        mPoints.push_back(Point3f(-xm,-ym,-zm));
        mPoints.push_back(Point3f(0.0,0.0,0.0));
        mPoints.push_back(Point3f(0.0,0.0,0.0));
        mPoints.push_back(Point3f(xm,ym,-zm));
        mPoints.push_back(Point3f(xm,ym,-zm));
        mPoints.push_back(Point3f(-xm,ym,-zm));
        mPoints.push_back(Point3f(-xm,ym,-zm));
        mPoints.push_back(Point3f(0.0,0.0,0.0));
        mPoints.push_back(Point3f(0.0,0.0,0.0));
        mPoints.push_back(Point3f(xm,-ym,-zm));
        mPoints.push_back(Point3f(xm,-ym,-zm));
        mPoints.push_back(Point3f(-xm,-ym,-zm));
        mPoints.push_back(Point3f(-xm,-ym,-zm));
        mPoints.push_back(Point3f(0.0,0.0,0.0));

        glGenBuffers(1,&mVertexBufferId);
        glBindBuffer(GL_ARRAY_BUFFER, mVertexBufferId);
        glBufferData(GL_ARRAY_BUFFER, sizeof(Point3f)*mPoints.size(), mPoints[0].data(), GL_STATIC_DRAW);

        glGenVertexArrays(1,&mVertexArrayId);
    }

    // bind the vertex array
    glBindVertexArray(mVertexArrayId);

    glBindBuffer(GL_ARRAY_BUFFER, mVertexBufferId);

    int vertex_loc = prg->attrib("vtx_position");
    glVertexAttribPointer(vertex_loc, 3, GL_FLOAT, GL_FALSE, 0, (void*)0);
    glEnableVertexAttribArray(vertex_loc);

    glUniformMatrix4fv(prg->uniform("mat_obj"), 1, GL_FALSE, mFrame.getMatrix().data());

    glDrawArrays(GL_LINES,0,mPoints.size());

    glDisableVertexAttribArray(vertex_loc);

    // release the vertex array
    glBindVertexArray(0);
}

void Camera::convertClickToLine(const Point2i &p, Point3f &orig, Vector3f& dir) const
{
    orig = position();
    dir = Vector3f( ((2.0 * p[0] / vpWidth()) - 1.0) * tan(fovY()/2.0) * vpWidth() / vpHeight(),
                    ((2.0 * (vpHeight() - p[1]) / vpHeight()) - 1.0) * tan(fovY()/2.0),
                    -1.0 );
    Eigen::Matrix3f rotation = mFrame.orientation.toRotationMatrix();
    dir = rotation * dir + mFrame.position;
    dir = dir - orig;
    dir.normalize();
}

/// Return a human-readable summary
std::string Camera::toString() const {
    std::ostringstream oss;
    oss << mFrame.getMatrix().format(Eigen::IOFormat(4, 0, ", ", ";\n", "", "", "[", "]"));
    return tfm::format(
        "Camera[\n"
        "  frame = %s,\n"
        "  outputSize = %f x %f,\n"
        "  samples = %f,\n"
        "  fov = %f,\n"
        "  clip = [%f, %f],\n"
        "]",
        indent(oss.str(), 10),
        mVpWidth, mVpHeight,
        mSampleCount,
        mFovY,
        mNearDist,
        mFarDist
    );
}

REGISTER_CLASS(Camera, "perspective");


/*
 * Copyright (C) 2019-2020 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

#include <yield_plugin/yield_plugin.h>
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <carma_wm/CARMAWorldModel.h>
#include <math.h>
#include <tf/LinearMath/Vector3.h>
#include <boost/property_tree/json_parser.hpp>
#include <carma_wm/WMTestLibForGuidance.h>

#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_io/Io.h>
#include <lanelet2_io/io_handlers/Factory.h>
#include <lanelet2_io/io_handlers/Writer.h>
#include <lanelet2_projection/UTM.h>
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_core/Attribute.h>
#include <lanelet2_core/primitives/Traits.h>
#include <lanelet2_extension/traffic_rules/CarmaUSTrafficRules.h>
#include <lanelet2_extension/utility/query.h>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <lanelet2_extension/io/autoware_osm_parser.h>
#include <carma_wm/MapConformer.h>
#include <carma_wm/CARMAWorldModel.h>
#include <ros/console.h>
#include <unsupported/Eigen/Splines>
#include <carma_utils/containers/containers.h>
#include <tf/LinearMath/Vector3.h>

using namespace yield_plugin;

TEST(YieldPluginTest, test_polynomial_calc)
{
  YieldPluginConfig config;
  std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  YieldPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {});

  std::vector<double> coeff;
  coeff.push_back(2.0);
  coeff.push_back(2.0);
  coeff.push_back(2.0);
  coeff.push_back(2.0);
  coeff.push_back(2.0);
  coeff.push_back(2.0);

  double result = plugin.polynomial_calc(coeff, 0);
  EXPECT_EQ(2, result);

  result = plugin.polynomial_calc(coeff, 1);
  EXPECT_EQ(12, result);

  result = plugin.polynomial_calc(coeff, 2);
  EXPECT_EQ(126, result);

  result = plugin.polynomial_calc(coeff, 3);
  EXPECT_EQ(728, result);
}

TEST(YieldPluginTest, test_polynomial_calc_derivative)
{
  YieldPluginConfig config;
  std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  YieldPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {});

  std::vector<double> coeff;
  coeff.push_back(2.0);
  coeff.push_back(2.0);
  coeff.push_back(2.0);
  coeff.push_back(2.0);
  coeff.push_back(2.0);
  coeff.push_back(2.0);

  double result = plugin.polynomial_calc_d(coeff, 0);
  EXPECT_EQ(2, result);

  result = plugin.polynomial_calc_d(coeff, 1);
  EXPECT_EQ(30, result);

  result = plugin.polynomial_calc_d(coeff, 2);
  EXPECT_EQ(258, result);

  result = plugin.polynomial_calc_d(coeff, 3);
  EXPECT_EQ(1094, result);
}

TEST(YieldPluginTest, MaxTrajectorySpeed)
{
  YieldPluginConfig config;
  std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  YieldPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {});

  std::vector<cav_msgs::TrajectoryPlanPoint> trajectory_points;

  ros::Time startTime(1.0);

  cav_msgs::TrajectoryPlanPoint point_1;
  point_1.x = 0.0;
  point_1.y = 0.0;
  point_1.target_time = startTime;
  point_1.lane_id = "1";
  trajectory_points.push_back(point_1);

  cav_msgs::TrajectoryPlanPoint point_2;
  point_2.x = 5.0;
  point_2.y = 0.0;
  point_2.target_time = startTime + ros::Duration(1);
  point_2.lane_id = "1";
  trajectory_points.push_back(point_2);

  cav_msgs::TrajectoryPlanPoint point_3;
  point_3.x = 10.0;
  point_3.y = 0.0;
  point_3.target_time = startTime + ros::Duration(2);
  point_3.lane_id = "1";
  trajectory_points.push_back(point_3);

  cav_msgs::TrajectoryPlanPoint point_4;
  point_4.x = 15.0;
  point_4.y = 0.0;
  point_4.target_time = startTime + ros::Duration(3);
  point_4.lane_id = "1";
  trajectory_points.push_back(point_4);

  cav_msgs::TrajectoryPlanPoint point_5;
  point_5.x = 20.0;
  point_5.y = 0.0;
  point_5.target_time = startTime + ros::Duration(4);
  point_5.lane_id = "1";
  trajectory_points.push_back(point_5);

  cav_msgs::TrajectoryPlanPoint point_6;
  point_6.x = 25.0;
  point_6.y = 0.0;
  point_6.target_time = startTime + ros::Duration(5);
  point_6.lane_id = "1";
  trajectory_points.push_back(point_6);

  cav_msgs::TrajectoryPlanPoint point_7;
  point_7.x = 40.0;
  point_7.y = 0.0;
  point_7.target_time = startTime + ros::Duration(6);
  point_7.lane_id = "1";
  trajectory_points.push_back(point_7);

  double result = plugin.max_trajectory_speed(trajectory_points);
  EXPECT_EQ(5, result);

}

TEST(YieldPluginTest, test_update_traj)
{
  std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  auto map = carma_wm::test::buildGuidanceTestMap(100,100);

  wm->setMap(map);

  YieldPluginConfig config;
  config.vehicle_length = 4;
  config.vehicle_width = 2;
  config.vehicle_height = 1;

  // std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  YieldPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {});

  cav_msgs::RoadwayObstacleList rwol;
  cav_msgs::TrajectoryPlan tp;

  ros::Time startTime(1.0);

  cav_msgs::TrajectoryPlanPoint trajectory_point_1;
  cav_msgs::TrajectoryPlanPoint trajectory_point_2;
  cav_msgs::TrajectoryPlanPoint trajectory_point_3;
  cav_msgs::TrajectoryPlanPoint trajectory_point_4;
  cav_msgs::TrajectoryPlanPoint trajectory_point_5;
  cav_msgs::TrajectoryPlanPoint trajectory_point_6;
  cav_msgs::TrajectoryPlanPoint trajectory_point_7;

  trajectory_point_1.x = 1.0;
  trajectory_point_1.y = 1.0;
  trajectory_point_1.target_time = ros::Time(0);

  trajectory_point_2.x = 10.0;
  trajectory_point_2.y = 20.0;
  trajectory_point_2.target_time = ros::Time(0, 1);

  trajectory_point_3.x = 10.0;
  trajectory_point_3.y = 30.0;
  trajectory_point_3.target_time = ros::Time(0, 2);

  trajectory_point_4.x = 10.0;
  trajectory_point_4.y = 40.0;
  trajectory_point_4.target_time = ros::Time(0, 3);

  trajectory_point_5.x = 10.0;
  trajectory_point_5.y = 50.0;
  trajectory_point_5.target_time = ros::Time(0, 4);

  trajectory_point_6.x = 10.0;
  trajectory_point_6.y = 60.0;
  trajectory_point_6.target_time = ros::Time(0, 5);

  trajectory_point_7.x = 10.0;
  trajectory_point_7.y = 70.0;
  trajectory_point_7.target_time = ros::Time(0, 6);

  tp.trajectory_points = {trajectory_point_1, trajectory_point_2, trajectory_point_3, trajectory_point_4, trajectory_point_5, trajectory_point_6, trajectory_point_7};

  cav_msgs::RoadwayObstacle rwo_1;

  tf2::Quaternion tf_orientation;
  tf_orientation.setRPY(0, 0, 1.5708);

  rwo_1.object.pose.pose.position.x = 60;
  rwo_1.object.pose.pose.position.y = 50;
  rwo_1.object.pose.pose.position.z = 0;

  rwo_1.object.pose.pose.orientation.x = tf_orientation.getX();
  rwo_1.object.pose.pose.orientation.y = tf_orientation.getY();
  rwo_1.object.pose.pose.orientation.z = tf_orientation.getZ();
  rwo_1.object.pose.pose.orientation.w = tf_orientation.getW();

  rwo_1.object.size.x = 1;
  rwo_1.object.size.y = 1;
  rwo_1.object.size.z = 1;

  cav_msgs::PredictedState ps_1;
  ps_1.header.stamp.nsec = 1000;

  ps_1.predicted_position.position.x = 10;
  ps_1.predicted_position.position.y = 10;
  ps_1.predicted_position.position.z = 0;

  ps_1.predicted_position.orientation.x = tf_orientation.getX();
  ps_1.predicted_position.orientation.y = tf_orientation.getY();
  ps_1.predicted_position.orientation.z = tf_orientation.getZ();
  ps_1.predicted_position.orientation.w = tf_orientation.getW();

  cav_msgs::PredictedState ps_2;
  ps_2.header.stamp.nsec = 2000;

  ps_2.predicted_position.position.x = 10;
  ps_2.predicted_position.position.y = 20;
  ps_2.predicted_position.position.z = 0;

  ps_2.predicted_position.orientation.x = tf_orientation.getX();
  ps_2.predicted_position.orientation.y = tf_orientation.getY();
  ps_2.predicted_position.orientation.z = tf_orientation.getZ();
  ps_2.predicted_position.orientation.w = tf_orientation.getW();

  cav_msgs::PredictedState ps_3;
  ps_3.header.stamp.nsec = 3000;

  ps_3.predicted_position.position.x = 10;
  ps_3.predicted_position.position.y = 30;
  ps_3.predicted_position.position.z = 0;

  ps_3.predicted_position.orientation.x = tf_orientation.getX();
  ps_3.predicted_position.orientation.y = tf_orientation.getY();
  ps_3.predicted_position.orientation.z = tf_orientation.getZ();
  ps_3.predicted_position.orientation.w = tf_orientation.getW();

  rwo_1.object.predictions = {ps_1,ps_2,ps_3};
  rwo_1.object.velocity.twist.linear.x = 5.0;

  rwol.roadway_obstacles = {rwo_1};

  std::vector<cav_msgs::RoadwayObstacle> rw_objs;

  rw_objs.push_back(rwo_1);

  wm->setRoadwayObjects(rw_objs);

  cav_msgs::TrajectoryPlan tp_new = plugin.update_traj_for_object(tp, 10.0);

  for (size_t i = 1; i < tp_new.trajectory_points.size(); i++) {
    std::cout << tp_new.trajectory_points[i] << std::endl;
  }

  EXPECT_EQ(7, tp.trajectory_points.size());

}

TEST(YieldPluginTest, test_update_traj2)
{
  YieldPluginConfig config;
  std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  YieldPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {});

  cav_msgs::TrajectoryPlan original_tp;

  cav_msgs::TrajectoryPlanPoint trajectory_point_1;
  cav_msgs::TrajectoryPlanPoint trajectory_point_2;
  cav_msgs::TrajectoryPlanPoint trajectory_point_3;
  cav_msgs::TrajectoryPlanPoint trajectory_point_4;
  cav_msgs::TrajectoryPlanPoint trajectory_point_5;
  cav_msgs::TrajectoryPlanPoint trajectory_point_6;
  cav_msgs::TrajectoryPlanPoint trajectory_point_7;

  trajectory_point_1.x = 1.0;
  trajectory_point_1.y = 1.0;
  trajectory_point_1.target_time = ros::Time(0);

  trajectory_point_2.x = 5.0;
  trajectory_point_2.y = 1.0;
  trajectory_point_2.target_time = ros::Time(1);

  trajectory_point_3.x = 10.0;
  trajectory_point_3.y = 1.0;
  trajectory_point_3.target_time = ros::Time(2);

  trajectory_point_4.x = 15.0;
  trajectory_point_4.y = 1.0;
  trajectory_point_4.target_time = ros::Time(3);

  trajectory_point_5.x = 20.0;
  trajectory_point_5.y = 1.0;
  trajectory_point_5.target_time = ros::Time(4);

  trajectory_point_6.x = 25.0;
  trajectory_point_6.y = 1.0;
  trajectory_point_6.target_time = ros::Time(5);

  trajectory_point_7.x = 30.0;
  trajectory_point_7.y = 1.0;
  trajectory_point_7.target_time = ros::Time(6);

  original_tp.trajectory_points = {trajectory_point_1, trajectory_point_2, trajectory_point_3, trajectory_point_4, trajectory_point_5, trajectory_point_6, trajectory_point_7};

  double initial_pos = 0.0;
  double goal_pos = 35.0;
  double current_speed_ = 10.0;
  double goal_velocity = 5.0;
  double initial_accel = 0.0;
  double goal_accel = 0.0;
  double initial_time = 0.0;
  double tp = 5;

  std::vector<double> values = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos,
                                                                                goal_pos,
                                                                                current_speed_,
                                                                                goal_velocity,
                                                                                initial_accel,
                                                                                goal_accel,
                                                                                initial_time,
                                                                                tp);

  std::vector<cav_msgs::TrajectoryPlanPoint> new_trajectory_points;
  new_trajectory_points.push_back(original_tp.trajectory_points[0]);

  std::vector<double> original_traj_downtracks = plugin.get_relative_downtracks(original_tp);

  std::vector<double> new_speeds;
  std::vector<double> distance_travelled;

  for(size_t i = 1; i < original_tp.trajectory_points.size() ; i++ )
  {
      double traj_target_time = i * tp / original_tp.trajectory_points.size();
      double dt_dist = plugin.polynomial_calc(values, traj_target_time);
      distance_travelled.push_back(dt_dist);
      double dv = plugin.polynomial_calc_d(values, traj_target_time);
      new_speeds.push_back(dv);
      cav_msgs::TrajectoryPlanPoint new_tpp;
      new_tpp.x = original_tp.trajectory_points[i].x;
      new_tpp.y = original_tp.trajectory_points[i].y;
      new_tpp.target_time = new_trajectory_points[0].target_time + ros::Duration(original_traj_downtracks[i]/dv);
      new_trajectory_points.push_back(new_tpp);
  }

  // speeds are decreasing
  EXPECT_EQ(6, new_speeds.size());
  EXPECT_TRUE(new_speeds[5] <= new_speeds[4]);
  EXPECT_TRUE(new_speeds[3] <= new_speeds[1]);

  EXPECT_EQ(6, distance_travelled.size());
  // did not exceed goal position
  EXPECT_TRUE(distance_travelled[5] < goal_pos );
}

TEST(YieldPluginTest, test_update_traj_stop)
{
  YieldPluginConfig config;
  std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  YieldPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {});

  cav_msgs::TrajectoryPlan original_tp;

  cav_msgs::TrajectoryPlanPoint trajectory_point_1;
  cav_msgs::TrajectoryPlanPoint trajectory_point_2;
  cav_msgs::TrajectoryPlanPoint trajectory_point_3;
  cav_msgs::TrajectoryPlanPoint trajectory_point_4;
  cav_msgs::TrajectoryPlanPoint trajectory_point_5;
  cav_msgs::TrajectoryPlanPoint trajectory_point_6;
  cav_msgs::TrajectoryPlanPoint trajectory_point_7;

  trajectory_point_1.x = 1.0;
  trajectory_point_1.y = 1.0;
  trajectory_point_1.target_time = ros::Time(0);

  trajectory_point_2.x = 5.0;
  trajectory_point_2.y = 1.0;
  trajectory_point_2.target_time = ros::Time(1);

  trajectory_point_3.x = 10.0;
  trajectory_point_3.y = 1.0;
  trajectory_point_3.target_time = ros::Time(2);

  trajectory_point_4.x = 15.0;
  trajectory_point_4.y = 1.0;
  trajectory_point_4.target_time = ros::Time(3);

  trajectory_point_5.x = 20.0;
  trajectory_point_5.y = 1.0;
  trajectory_point_5.target_time = ros::Time(4);

  trajectory_point_6.x = 25.0;
  trajectory_point_6.y = 1.0;
  trajectory_point_6.target_time = ros::Time(5);

  trajectory_point_7.x = 30.0;
  trajectory_point_7.y = 1.0;
  trajectory_point_7.target_time = ros::Time(6);

  original_tp.trajectory_points = {trajectory_point_1, trajectory_point_2, trajectory_point_3, trajectory_point_4, trajectory_point_5, trajectory_point_6, trajectory_point_7};

  // When the lead vehicle is stopped

  double initial_pos = 0.0;
  double goal_pos = 15.0;
  double current_speed_ = 10.0;
  double goal_velocity = 0.0;
  double initial_accel = 0.0;
  double goal_accel = 0.0;
  double initial_time = 0.0;
  double tp = 5;

  std::vector<double> values = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos,
                                                                                goal_pos,
                                                                                current_speed_,
                                                                                goal_velocity,
                                                                                initial_accel,
                                                                                goal_accel,
                                                                                initial_time,
                                                                                tp);

  std::vector<cav_msgs::TrajectoryPlanPoint> new_trajectory_points;
  new_trajectory_points.push_back(original_tp.trajectory_points[0]);
  std::vector<double> original_traj_downtracks = plugin.get_relative_downtracks(original_tp);

  std::vector<double> new_speeds;
  std::vector<double> distance_travelled;
  for(size_t i = 1; i < original_tp.trajectory_points.size() ; i++ )
  {
      double traj_target_time = i * tp / original_tp.trajectory_points.size();
      double dt_dist = plugin.polynomial_calc(values, traj_target_time);
      double dv = plugin.polynomial_calc_d(values, traj_target_time);
      cav_msgs::TrajectoryPlanPoint new_tpp;

      if (dv >= 1.0)
        {
          ROS_WARN_STREAM("target speed is positive");
          if (dv >= current_speed_){
            dv = current_speed_;
          }
          // trajectory point is copied to move all the available information, then its target time is updated
          new_tpp = original_tp.trajectory_points[i];
          new_tpp.target_time = new_trajectory_points[i-1].target_time + ros::Duration(original_traj_downtracks[i]/dv);
          new_trajectory_points.push_back(new_tpp);
        }
        else
        {
          ROS_WARN_STREAM("target speed is zero");
          new_tpp = new_trajectory_points[i-1];
          new_tpp.target_time = new_trajectory_points[0].target_time + ros::Duration(traj_target_time);
          new_trajectory_points.push_back(new_tpp);
        }
      new_speeds.push_back(dv);
  }
  EXPECT_EQ(original_tp.trajectory_points.size(), new_trajectory_points.size());
  // Trajectory point location same as previous point
  EXPECT_EQ(new_trajectory_points[5].x, new_trajectory_points[4].x);
  // Trajectory point time is greater than previous point
  EXPECT_TRUE(new_trajectory_points[5].target_time > new_trajectory_points[4].target_time);
}

TEST(YieldPluginTest, jmt_traj)
{
  YieldPluginConfig config;
  std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  YieldPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {});

  cav_msgs::TrajectoryPlan original_tp;

  cav_msgs::TrajectoryPlanPoint trajectory_point_1;
  cav_msgs::TrajectoryPlanPoint trajectory_point_2;
  cav_msgs::TrajectoryPlanPoint trajectory_point_3;
  cav_msgs::TrajectoryPlanPoint trajectory_point_4;
  cav_msgs::TrajectoryPlanPoint trajectory_point_5;
  cav_msgs::TrajectoryPlanPoint trajectory_point_6;
  cav_msgs::TrajectoryPlanPoint trajectory_point_7;

  trajectory_point_1.x = 1.0;
  trajectory_point_1.y = 1.0;
  trajectory_point_1.target_time = ros::Time(0);

  trajectory_point_2.x = 5.0;
  trajectory_point_2.y = 1.0;
  trajectory_point_2.target_time = ros::Time(1);

  trajectory_point_3.x = 10.0;
  trajectory_point_3.y = 1.0;
  trajectory_point_3.target_time = ros::Time(2);

  trajectory_point_4.x = 15.0;
  trajectory_point_4.y = 1.0;
  trajectory_point_4.target_time = ros::Time(3);

  trajectory_point_5.x = 20.0;
  trajectory_point_5.y = 1.0;
  trajectory_point_5.target_time = ros::Time(4);

  trajectory_point_6.x = 25.0;
  trajectory_point_6.y = 1.0;
  trajectory_point_6.target_time = ros::Time(5);

  trajectory_point_7.x = 30.0;
  trajectory_point_7.y = 1.0;
  trajectory_point_7.target_time = ros::Time(6);

  original_tp.trajectory_points = {trajectory_point_1, trajectory_point_2, trajectory_point_3, trajectory_point_4, trajectory_point_5, trajectory_point_6, trajectory_point_7};

  // When the lead vehicle is stopped

  double initial_pos = 0.0;
  double goal_pos = 35.0;
  double initial_velocity = 10.0;
  double goal_velocity = 5.0;
  double initial_accel = 0.0;
  double goal_accel = 0.0;
  double initial_time = 0.0;
  double tp = 5;

  cav_msgs::TrajectoryPlan jmt_traj = plugin.generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, tp);

  EXPECT_EQ(jmt_traj.trajectory_points.size(), original_tp.trajectory_points.size());
  EXPECT_LE(jmt_traj.trajectory_points[2].target_time, original_tp.trajectory_points[2].target_time);

}

TEST(YieldPluginTest, min_digital_gap)
{
  std::shared_ptr<carma_wm::CARMAWorldModel> wm = std::make_shared<carma_wm::CARMAWorldModel>();
  auto map = carma_wm::test::buildGuidanceTestMap(3,3);

  auto ll_1200 = map->laneletLayer.get(1200);

  double min_gap = 12;

  auto regulatory_element = std::make_shared<lanelet::DigitalMinimumGap>(lanelet::DigitalMinimumGap::buildData(lanelet::utils::getId(), min_gap, {ll_1200}, { },
                                                     { lanelet::Participants::VehicleCar }));
  ll_1200.addRegulatoryElement(regulatory_element);
  map->add(regulatory_element);
  wm->setMap(map);

  YieldPluginConfig config;
  YieldPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {});

  cav_msgs::TrajectoryPlan original_tp;

  cav_msgs::TrajectoryPlanPoint trajectory_point_1;
  cav_msgs::TrajectoryPlanPoint trajectory_point_2;
  cav_msgs::TrajectoryPlanPoint trajectory_point_3;
  cav_msgs::TrajectoryPlanPoint trajectory_point_4;
  cav_msgs::TrajectoryPlanPoint trajectory_point_5;
  cav_msgs::TrajectoryPlanPoint trajectory_point_6;
  cav_msgs::TrajectoryPlanPoint trajectory_point_7;

  trajectory_point_1.x = 1.0;
  trajectory_point_1.y = 1.0;
  trajectory_point_1.target_time = ros::Time(0);

  trajectory_point_2.x = 1.5;
  trajectory_point_2.y = 1.0;
  trajectory_point_2.target_time = ros::Time(1);

  trajectory_point_3.x = 2.0;
  trajectory_point_3.y = 1.0;
  trajectory_point_3.target_time = ros::Time(2);

  trajectory_point_4.x = 2.5;
  trajectory_point_4.y = 1.0;
  trajectory_point_4.target_time = ros::Time(3);

  original_tp.trajectory_points = {trajectory_point_1, trajectory_point_2, trajectory_point_3, trajectory_point_4};

  double gap = plugin.check_traj_for_digital_min_gap(original_tp);

  EXPECT_EQ(gap, min_gap);

}

// Run all the tests
int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    ros::Time::init();
    ROSCONSOLE_AUTOINIT;
    if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) {
      ros::console::notifyLoggerLevelsChanged();
    }
    return RUN_ALL_TESTS();
}