// 求解器一般有两种, TEB用的是 Levenberg
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 3>> Block; // 每个误差项优化变量维度为3，误差值维度为1

Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver) );

g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr) );

g2o::SparseOptimizer optimizer;
// setAlgorithm()需要传递一个优化算法
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);
optimizer->initMultiThreading();

// ......
optimizer.optimize(maxIterations); 	//开始优化，设置迭代次数

// 加载几组 solver
G2O_USE_OPTIMIZATION_LIBRARY(pcg)
G2O_USE_OPTIMIZATION_LIBRARY(cholmod)
G2O_USE_OPTIMIZATION_LIBRARY(csparse)
G2O_USE_OPTIMIZATION_LIBRARY(dense)
G2O_USE_OPTIMIZATION_LIBRARY(eigen)


g2o::OptimizationAlgorithmFactory *solver_factory = g2o::OptimizationAlgorithmFactory::instance();
// 记录所有可用的 solver
std::ofstream fileout("/home/user/test_nodes/solvers.txt");
solver_factory->listSolvers(fileout);
// or lm_var_cparse
g2o::OptimizationAlgorithmProperty solver_property;
g2o::OptimizationAlgorithm *solver = solver_factory->construct("lm_var", solver_property);
std::unique_ptr<g2o::SparseOptimizer> optimizer(new g2o::SparseOptimizer());
optimizer->setAlgorithm(solver);

class EdgeVelocity : public BaseTebMultiEdge<2, double>
{
	public:  
	EdgeVelocity()
	{
		this->resize(3); // from a g2o::BaseMultiEdge, set the desired number of vertices
	}
	// 代价函数
void computeError()
{
    ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
    const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
    // 两点的向量差
    const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
    
    double dist = deltaS.norm();
    const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());
    // 使用实际弧长，而不是两点的直线距离
    if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
    {
        double radius =  dist/(2*sin(angle_diff/2));
        dist = fabs( angle_diff * radius ); // actual arg length!
    }
    double vel = dist / deltaT->estimate();  // 线速度

    // fast_sigmoid函数是保证速度连续可微
    // vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
    vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
    const double omega = angle_diff / deltaT->estimate();  // 角速度
    // 线速度需要限制在最大和最大倒退的范围内
    _error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon);
    // 角速度只需限制在最大内
    _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta, cfg_->optim.penalty_epsilon);

    ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
}

 /*  Linear penalty function for bounding  var to the interval   $-a < var < a $
 * var       The scalar that should be bounded
 * a         lower and upper absolute bound
 * epsilon   safty margin (move bound to the interior of the interval)
 *           penaltyBoundToIntervalDerivative
 *  return Penalty / cost value that is nonzero if the constraint is not satisfied
 */

inline double penaltyBoundToInterval(const double& var,const double& a,const double& epsilon)
{
  if (var < -a+epsilon)
  {
    return (-var - (a - epsilon));
  }
  // 怀疑这里应该是 >=
  if (var <= a-epsilon)
  {
    return 0.;
  }
  else
  {
    return (var - (a - epsilon));
  }
}

/**
 *  Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$
 *  var The scalar that should be bounded
 *  a       lower bound
 *  b       upper bound
 *  epsilon safty margin (move bound to the interior of the interval)
 *  penaltyBoundToIntervalDerivative
 *  return Penalty / cost value that is nonzero if the constraint is not satisfied
 */
inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon)
{
  if (var < a+epsilon)
  {
    return (-var + (a + epsilon));
  }
  if (var <= b-epsilon)
  {
    return 0.;
  }
  else
  {
    return (var - (b - epsilon));
  }
}

_jacobianOplus[0].resize(2,3); // conf1
_jacobianOplus[1].resize(2,3); // conf2
_jacobianOplus[2].resize(2,1); // deltaT

// 获取 num 在 vector 中的 index
int findIndex(const vector<int>& vector, int num);

int main(int argc, char *argv[])
{
    vector<int> v;
    for(int i=0; i<9; i++)
    {
        v.emplace_back();
        auto& num = v.back();
        num = i*2;
        cout << "  v.back: "<< v.back();
    }
    int id = findIndex(v, 6) ;
    if(id < 0 )
        cout << "no such number !" <<endl;
    else
        cout << "index of number is : "<< id <<endl;
    return 0;
}

int findIndex(const vector<int>& vector, int num)
{
    Q_ASSERT(vector.size() >0 );
    int size = vector.size();
    if(vector.at(0) ==num )    return 0;
    if(size==1 && vector.at(0) !=num )    return -1;
    if(num == vector.back() )   return size-1;

    int begin, mid, end;
    begin =0; end = vector.size()-1;
    mid = (begin + end) / 2;
    int count = 0;
    while(num != vector.at(mid) )
    {
        count++;
        if(count > log2(size) )
        {
            return -1;
            break;
        }
        // 比较mid和num, 按情况只取一段进行递归比较，这样才降低复杂度
        if( num > vector.at(mid) )
        {
            begin = mid; end = end;
            mid = (begin + end) / 2;
        }
        else
        {
            begin = begin; end = mid;
            mid = (begin + end) / 2;
        }
    }
    return mid;
}

// 省略不重要部分
class ReferencePoint : public hdmap::MapPathPoint
{
     public:
      common::PathPoint ToPathPoint(double s) const;

      double kappa() const;
      double dkappa() const;

      static void RemoveDuplicates(std::vector<ReferencePoint>* points);

     private:
      double kappa_ = 0.0;
      double dkappa_ = 0.0;
};

struct LaneWaypoint {
  LaneWaypoint() = default;
  LaneWaypoint(LaneInfoConstPtr lane, const double s)
      : lane(CHECK_NOTNULL(lane)), s(s) {}
  LaneWaypoint(LaneInfoConstPtr lane, const double s, const double l)
      : lane(CHECK_NOTNULL(lane)), s(s), l(l) {}

  LaneInfoConstPtr lane = nullptr;
  double s = 0.0;
  double l = 0.0;

  std::string DebugString() const;
};

// lane_info_ptr->accumulate_s(); 的来源
for (size_t i = 0; i + 1 < points_.size(); ++i)
{
    segments_.emplace_back(points_[i], points_[i + 1]);
    accumulated_s_.push_back(s);
    unit_directions_.push_back(segments_.back().unit_direction());
    s += segments_.back().length();
}

# include <coin-or/IpIpoptApplication.hpp>
# include <coin-or/IpTNLP.hpp>

#include <cppad/ipopt/solve.hpp>
#include <iostream>
//#include <mpi.h>
namespace
{
    using CppAD::AD;
    class FG_eval {
    public:
        typedef CPPAD_TESTVECTOR( AD<double> ) ADvector;
        void operator()(ADvector& fg, const ADvector& x)
        {     assert( fg.size() == 3 );
            assert( x.size()  == 4 );

            // Fortran style indexing
            AD<double> x1 = x[0];
            AD<double> x2 = x[1];
            AD<double> x3 = x[2];
            AD<double> x4 = x[3];
            // f(x)
            fg[0] = x1 * x4 * (x1 + x2 + x3) + x3;
            // g_1 (x)
            fg[1] = x1 * x2 * x3 * x4;
            // g_2 (x)
            fg[2] = x1 * x1 + x2 * x2 + x3 * x3 + x4 * x4;
            return;
        }
    };
}

bool get_started(void)
{     
    bool ok = true;
    size_t i;
    typedef CPPAD_TESTVECTOR( double ) Dvector;
    // number of independent variables (domain dimension for f and g)
    size_t nx = 4;
    // number of constraints (range dimension for g)
    size_t ng = 2;
    // x[i] 设置第i个变量的初始迭代值
    Dvector xi(nx);
    xi[0] = 1.0;
    xi[1] = 1.0;
    xi[2] = 1.0;
    xi[3] = 1.0;

    // x_l[i]设置xi的下界值, x_u[i]设置xi的上界值
    Dvector xl(nx), xu(nx);
    for(i = 0; i < nx; i++)
    {     
        xl[i] = 1.0;
        xu[i] = 5.0;
    }
    // g_l[i]设置约束i的下界值, g_u[i]设置约束i的上界值
    Dvector gl(ng), gu(ng);
    gl[0] = 25.0;     gu[0] = 1.0e19;
    gl[1] = 40.0;     gu[1] = 40.0;

    // object that computes objective and constraints
    FG_eval fg_eval;

    // options
    std::string options;
    // turn off any printing
    options += "Integer print_level  0\n";
    options += "String  sb           yes\n";
    // maximum number of iterations
    options += "Integer max_iter     100\n";
    // approximate accuracy in first order necessary conditions;
    // see Mathematical Programming, Volume 106, Number 1,
    // Pages 25-57, Equation (6)
    options += "Numeric tol          1e-6\n";
    // derivative testing
    options += "String  derivative_test            second-order\n";
    // maximum amount of random pertubation; e.g.,
    // when evaluation finite diff
    options += "Numeric point_perturbation_radius  0.\n";

    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval>(
            options, xi, xl, xu, gl, gu, fg_eval, solution
    );

    // Check some of the solution values
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
    double rel_tol = 1e-6;  // relative tolerance
    double abs_tol = 1e-6;  // absolute tolerance
    std::cout << "solution x: "<< std::endl;
    for(i = 0; i < nx; i++)
    {     
        std::cout << solution.x[i] << std::endl;
    }
    return ok;
}

int main(int argc, const char** argv) {
    //MPI_Init();
    get_started();
    return 0;
}