// 获取 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();
}

/**
 * 平均分割区间 [start, end] 为num段，num+1 个点，存入sliced
 * start and end 为sliced的首尾元素
 */
template <typename T>
void uniform_slice(const T start, const T end, uint32_t num,
                   std::vector<T>* sliced)
{
  if (!sliced || num == 0)
        return;
  const T delta = (end - start) / num;
  sliced->resize(num + 1);
  T  s = start;
  for (uint32_t i = 0; i < num; ++i, s += delta)
  {
      sliced->at(i) = s;
  }
  sliced->at(num) = end;
}

vector<Point2D> samplePoints(const vector<Point2D>& input_points, int interval)
{
    vector<Point2D> anchor_points;

    int sampling_num = std::max(2, static_cast<int>(input_points.size() / interval + 0.5)  );
    
    vector<double> anchor_id;
    Point2D temp_point;
    uniform_slice(0, input_points.size(), sampling_num - 1, &anchor_id);
    
    for(int i=0; i< anchor_id.size(); i++)
    {
        // cout << anchor_id.at(i) << "   ";
        temp_point = input_points.at(anchor_id.at(i) );
        // cout << "anchor X: " << temp_point.first <<endl;
        // cout << "anchor Y: " << temp_point.second <<endl;
        anchor_points.push_back(temp_point);
    }
    return anchor_points;
}

/**
 * Check if two points u and v are the same point on XY dimension.
 * @param u one point that has member function x() and y().
 * @param v one point that has member function x() and y().
 * @return sqrt((u.x-v.x)^2 + (u.y-v.y)^2) < epsilon, i.e., the Euclid distance
 * on XY dimension.
 */
template <typename U, typename V>
bool SamePointXY(const U& u, const V& v)
{
    static constexpr double kMathEpsilonSqr = 1e-8 * 1e-8;
    return (u.x() - v.x()) * (u.x() - v.x()) < kMathEpsilonSqr &&
         (u.y() - v.y()) * (u.y() - v.y()) < kMathEpsilonSqr;
}

PathPoint GetWeightedAverageOfTwoPathPoints(const PathPoint& p1,
                                            const PathPoint& p2,
                                            const double w1, const double w2)
{
    PathPoint p;
    p.set_x(p1.x() * w1 + p2.x() * w2);
    p.set_y(p1.y() * w1 + p2.y() * w2);
    p.set_z(p1.z() * w1 + p2.z() * w2);
    p.set_theta(p1.theta() * w1 + p2.theta() * w2);
    p.set_kappa(p1.kappa() * w1 + p2.kappa() * w2);
    p.set_dkappa(p1.dkappa() * w1 + p2.dkappa() * w2);
    p.set_ddkappa(p1.ddkappa() * w1 + p2.ddkappa() * w2);
    p.set_s(p1.s() * w1 + p2.s() * w2);
    return p;
}

# 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;
}