text: #include <acado_code_generation.hpp>
  
  const int controlHorizon = 50;
  
  using namespace std;
  
  #define G 9.81
  #define TR 1.8
  
  #define RW(v_ego, v_l) (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G))
  #define NORM_RW_ERROR(v_ego, v_l, p) ((RW(v_ego, v_l) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1))
  
  int main( )
  {
    USING_NAMESPACE_ACADO
  
  
    DifferentialEquation f;
  
    DifferentialState x_ego, v_ego, a_ego;
    DifferentialState x_l, v_l, a_l;
  
    OnlineData lambda;
  
    Control j_ego;
  
    auto desired = 4.0 + RW(v_ego, v_l);
    auto d_l = x_l - x_ego;
  
    // Equations of motion
    f << dot(x_ego) == v_ego;
    f << dot(v_ego) == a_ego;
    f << dot(a_ego) == j_ego;
  
    f << dot(x_l) == v_l;
    f << dot(v_l) == a_l;
    f << dot(a_l) == -lambda * a_l;
  
    // Running cost
    Function h;
    h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
    h << (d_l - desired) / (0.1 * v_ego + 0.5);
    h << a_ego * (1.0 + v_ego / 10.0);
    h << j_ego * (1.0 + v_ego / 10.0);
  
    // Weights are defined in mpc.
    BMatrix Q(4,4); Q.setAll(true);
  
    // Terminal cost
    Function hN;
    hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
    hN << (d_l - desired) / (0.1 * v_ego + 0.5);
    hN << a_ego * (1.0 + v_ego / 10.0);
  
    // Weights are defined in mpc.
    BMatrix QN(3,3); QN.setAll(true);
  
    // Non uniform time grid
    // First 5 timesteps are 0.2, after that it's 0.6
    DMatrix numSteps(20, 1);
    for (int i = 0; i < 5; i++){
      numSteps(i) = 1;
    }
    for (int i = 5; i < 20; i++){
      numSteps(i) = 3;
    }
  
    // Setup Optimal Control Problem
    const double tStart = 0.0;
    const double tEnd   = 10.0;
  
    OCP ocp( tStart, tEnd, numSteps);
    ocp.subjectTo(f);
  
    ocp.minimizeLSQ(Q, h);
    ocp.minimizeLSQEndTerm(QN, hN);
  
    ocp.subjectTo( 0.0 <= v_ego);
    ocp.setNOD(1);
  
    OCPexport mpc(ocp);
    mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
    mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
    mpc.set( INTEGRATOR_TYPE, INT_RK4 );
    mpc.set( NUM_INTEGRATOR_STEPS, controlHorizon);
    mpc.set( MAX_NUM_QP_ITERATIONS, 500);
    mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
  
    mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
    mpc.set( QP_SOLVER, QP_QPOASES );
    mpc.set( HOTSTART_QP, YES );
    mpc.set( GENERATE_TEST_FILE, NO);
    mpc.set( GENERATE_MAKE_FILE, NO );
    mpc.set( GENERATE_MATLAB_INTERFACE, NO );
    mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
  
    if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN)
      exit( EXIT_FAILURE );
  
    mpc.printDimensionsQP( );
  
    return EXIT_SUCCESS;
  }
  