#include <iostream>
#include "cppoptlib/meta.h"
#include "cppoptlib/problem.h"
#include "cppoptlib/solver/bfgssolver.h"
#include <cppoptlib/solver/neldermeadsolver.h>
#include "Eigen/Dense"
#include "time.h"
#include "polytraj.h"
#include "fstream"
#include "lib/src/shoot.h"
#include "plot2d.hpp"
#include <QApplication>

using namespace cppoptlib;
using namespace Eigen;
/*
 * liang rongmin 2021.8.24
 * module variable interval between [-π，π)
 */
double mod(double x){
    if( x>= -M_PI && x<M_PI)
        return x;
    else if( x < -M_PI ){
        return x+( int(x/(2*M_PI)) + 1.0 )*2*M_PI;
    }
    else if( x>= M_PI)
        return x-( int(x/(2*M_PI)) + 1.0 )*2*M_PI;

}

int main(int argc, char* argv[]){
    QApplication app(argc, argv);

    using namespace polytraj::path;

    State xs,xe;

    xs << 0,0,15.0/16.0*2*M_PI,0; //(x,y,theta,curvature);
    xe << -7,2,0,0;
    if( abs(xe(ST) - xs(ST))>M_PI){
        xs(ST) = mod(xs(ST));
        xe(ST) = mod(xe(ST));
    }

    clock_t start,end;
    start = clock();
//    Path path = generate(xs, xe, 100);        //只有边界条件
    Path path = generateMinE(xs,xe,100);    //最小能量指标+边界约束
    end = clock();
    std::cout << "time cost is:" << double(end-start)/CLOCKS_PER_SEC << "\n";

    Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");

    std::cout << "Start Posture: " << xs.transpose().format(CleanFmt)
              << std::endl;
    std::cout << "End Posture:   " << xe.transpose().format(CleanFmt)
              << std::endl;
    std::cout << std::endl;

    // Pretty print the path. Each row is a state.
    // The first row is xs. The final row is (nearly) xe.
    std::cout << "Generated Path: [x, y, theta, curvature]" << std::endl;
    std::cout << path.transpose().format(CleanFmt) << std::endl;

    ArrayXXf px = path.transpose().cast<float>().array();
    ArrayXXf py = path.transpose().cast<float>().array();
    auto plx = px.col(0);
    auto ply = py.col(1);

    plx.minCoeff();

    Plot2D plt;
    plt.grid(true);
    plt.plot(plx,ply,marker="-",linewidth=1);
    plt.axis(plx.minCoeff()-2,plx.maxCoeff()+2,ply.minCoeff()-2,ply.maxCoeff()+2);
    plt.show();

//    std::ofstream fout("matrix.txt");
//    fout << path.transpose() ;
//    fout.close();

//    FILE *pipe = _popen("gnuplot","w");
//    if(pipe == NULL)
//    {
//        exit(-1);
//    }
//    //use gnuplot to show the path
//    fprintf(pipe,"set title 'trajectory'\n");
//    fprintf(pipe,"set xlabel 'x'\n");
//    fprintf(pipe,"set ylabel 'y'\n");
////    fprintf(pipe,"set xrange[-2:12]\n");
////    fprintf(pipe,"set yrange[-2:12]\n");
////  fprintf(pipe,"set size 2 2\n");
//    fprintf(pipe,"set grid\n");
//    fprintf(pipe,"plot 'matrix.txt' title 'path' with lines\n");
//    fprintf(pipe,"pause mouse\n");
//    _pclose(pipe);
}


//class Rosenbrock : public Problem<double> {   //测试迭代器的使用方法
//public:
//    double value(const TVector &x) {
//        const double t1 = (1 - x[0]);
//        const double t2 = (x[1] - x[0] * x[0]);
//        return   t1 * t1 + 100 * t2 * t2;
//    }
////    void gradient(const TVector &x, TVector &grad) {
////        grad[0]  = -2 * (1 - x[0]) + 200 * (x[1] - x[0] * x[0]) * (-2 * x[0]);
////        grad[1]  = 200 * (x[1] - x[0] * x[0]);
////    }
//};
//
//Eigen::VectorXd intCos(double s, Eigen::VectorXd &xs){
//    Eigen::VectorXd xdot = xs;
//    xdot << cos(s), sin(s);
//    return xdot;
//}
//
////int main(){
////    Eigen::Matrix<double, 2, 1> xd;
////    xd.setZero();
////    Eigen::MatrixXd path = polytraj::shootSimpson(intCos,xd, M_PI,100);
////
////}

//int main(int argc, char const *argv[]) {
//    clock_t start,finish;
//    start = clock();
//    Rosenbrock f;
//    Problem<double>::TVector x(2); x << 0, 0;
//    NelderMeadSolver<Rosenbrock> solver;    //花费0.004秒
////    BfgsSolver<Rosenbrock> solver;        //花费0.009秒
//    solver.minimize(f, x);
//    finish = clock();
//    std::cout << "time cost is:" << double(finish - start) / CLOCKS_PER_SEC << "\n";
//    std::cout << "argmin      " << x.transpose() << std::endl;
//    std::cout << "f in argmin " << f(x) << std::endl;
//    return 0;
//}