void collocationInterpolators(const std::vector<double> & tau_root,
                                std::vector< std::vector<double> > &C, std::vector< double > &D) {

    // Find the degree of the interpolation
    int deg = tau_root.size()-1;


    // Allocate storage space for resulting coefficients
    C.resize(deg+1);
    for (int i=0;i<deg+1;++i) {
      C[i].resize(deg+1);
    }
    D.resize(deg+1);

    // Collocation point
    SX tau = SX::sym("tau");

    // For all collocation points
    for (int j=0; j<deg+1; ++j) {
      // Construct Lagrange polynomials to get the polynomial basis at the collocation point
      SX L = 1;
      for (int j2=0; j2<deg+1; ++j2) {
        if (j2 != j) {
          L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]);
        }
      }

      Function lfcn("lfcn", {tau}, {L});

      // Evaluate the polynomial at the final time to get the
      // coefficients of the continuity equation
      D[j] = lfcn(vector<DM>{1.}).at(0)->front();

      // Evaluate the time derivative of the polynomial at all collocation points to
      // get the coefficients of the continuity equation
      Function tfcn = lfcn.tangent();
      for (int j2=0; j2<deg+1; ++j2) {
        C[j2][j] =  tfcn(vector<DM>{tau_root[j2]}).at(0)->front();
      }
    }
  }
void DirectCollocationInternal::init(){
  // Initialize the base classes
  OCPSolverInternal::init();
  
  // Free parameters currently not supported
  casadi_assert_message(np_==0, "Not implemented");

  // Legendre collocation points
  double legendre_points[][6] = {
    {0},
    {0,0.500000},
    {0,0.211325,0.788675},
    {0,0.112702,0.500000,0.887298},
    {0,0.069432,0.330009,0.669991,0.930568},
    {0,0.046910,0.230765,0.500000,0.769235,0.953090}};

  // Radau collocation points
  double radau_points[][6] = {
    {0},
    {0,1.000000},
    {0,0.333333,1.000000},
    {0,0.155051,0.644949,1.000000},
    {0,0.088588,0.409467,0.787659,1.000000},
    {0,0.057104,0.276843,0.583590,0.860240,1.000000}};

  // Read options
  bool use_radau;
  if(getOption("collocation_scheme")=="radau"){
    use_radau = true;
  } else if(getOption("collocation_scheme")=="legendre"){
    use_radau = false;
  }

  // Interpolation order
  deg_ = getOption("interpolation_order");

  // All collocation time points
  double* tau_root = use_radau ? radau_points[deg_] : legendre_points[deg_];

  // Size of the finite elements
  double h = tf_/nk_;

  // Coefficients of the collocation equation
  vector<vector<MX> > C(deg_+1,vector<MX>(deg_+1));

  // Coefficients of the collocation equation as DMatrix
  DMatrix C_num = DMatrix(deg_+1,deg_+1,0);

  // Coefficients of the continuity equation
  vector<MX> D(deg_+1);

  // Coefficients of the collocation equation as DMatrix
  DMatrix D_num = DMatrix(deg_+1,1,0);

  // Collocation point
  SXMatrix tau = ssym("tau");

  // For all collocation points
  for(int j=0; j<deg_+1; ++j){
    // Construct Lagrange polynomials to get the polynomial basis at the collocation point
    SXMatrix L = 1;
    for(int j2=0; j2<deg_+1; ++j2){
      if(j2 != j){
        L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]);
      }
    }

    SXFunction lfcn(tau,L);
    lfcn.init();

    // Evaluate the polynomial at the final time to get the coefficients of the continuity equation
    lfcn.setInput(1.0);
    lfcn.evaluate();
    D[j] = lfcn.output();
    D_num(j) = lfcn.output();

    // Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
    for(int j2=0; j2<deg_+1; ++j2){
      lfcn.setInput(tau_root[j2]);
      lfcn.setFwdSeed(1.0);
      lfcn.evaluate(1,0);
      C[j][j2] = lfcn.fwdSens();
      C_num(j,j2) = lfcn.fwdSens();
    }
  }

  C_num(std::vector<int>(1,0),ALL) = 0;
  C_num(0,0)   = 1;

  // All collocation time points
  vector<vector<double> > T(nk_);
  for(int k=0; k<nk_; ++k){
	  T[k].resize(deg_+1);
	  for(int j=0; j<=deg_; ++j){
		  T[k][j] = h*(k + tau_root[j]);
	  }
  }

  // Total number of variables
  int nlp_nx = 0;
  nlp_nx += nk_*(deg_+1)*nx_;   // Collocated states
  nlp_nx += nk_*nu_;            // Parametrized controls
  nlp_nx += nx_;               	// Final state

  // NLP variable vector
  MX nlp_x = msym("x",nlp_nx);
  int offset = 0;

  // Get collocated states and parametrized control
  vector<vector<MX> > X(nk_+1);
  vector<MX> U(nk_);
  for(int k=0; k<nk_; ++k){
    // Collocated states
	X[k].resize(deg_+1);
    for(int j=0; j<=deg_; ++j){
        // Get the expression for the state vector
        X[k][j] = nlp_x[Slice(offset,offset+nx_)];
        offset += nx_;
    }

    // Parametrized controls
    U[k] = nlp_x[Slice(offset,offset+nu_)];
    offset += nu_;
  }

  // State at end time
  X[nk_].resize(1);
  X[nk_][0] = nlp_x[Slice(offset,offset+nx_)];
  offset += nx_;
  casadi_assert(offset==nlp_nx);

  // Constraint function for the NLP
  vector<MX> nlp_g;

  // Objective function
  MX nlp_j = 0;

  // For all finite elements
  for(int k=0; k<nk_; ++k){

    // For all collocation points
    for(int j=1; j<=deg_; ++j){

        // Get an expression for the state derivative at the collocation point
        MX xp_jk = 0;
        for(int r=0; r<=deg_; ++r){
            xp_jk += C[r][j]*X[k][r];
        }

        // Add collocation equations to the NLP
        MX fk = ffcn_.call(daeIn("x",X[k][j],"p",U[k]))[DAE_ODE];
        nlp_g.push_back(h*fk - xp_jk);
    }

    // Get an expression for the state at the end of the finite element
    MX xf_k = 0;
    for(int r=0; r<=deg_; ++r){
        xf_k += D[r]*X[k][r];
    }

    // Add continuity equation to NLP
    nlp_g.push_back(X[k+1][0] - xf_k);

    // Add path constraints
    if(nh_>0){
      MX pk = cfcn_.call(daeIn("x",X[k+1][0],"p",U[k])).at(0);
      nlp_g.push_back(pk);
    }

    // Add integral objective function term
	//    [Jk] = lfcn.call([X[k+1,0], U[k]])
	//    nlp_j += Jk
  }

  // Add end cost
  MX Jk = mfcn_.call(mayerIn("x",X[nk_][0])).at(0);
  nlp_j += Jk;

  // Objective function of the NLP
  F_ = MXFunction(nlp_x, nlp_j);

  // Nonlinear constraint function
  G_ = MXFunction(nlp_x, vertcat(nlp_g));

  // Get the NLP creator function
  NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
  
  // Allocate an NLP solver
  nlp_solver_ = nlp_solver_creator(F_,G_,FX(),FX());
  
  // Pass options
  if(hasSetOption("nlp_solver_options")){
    const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
    nlp_solver_.setOption(nlp_solver_options);
  }
  
  // Initialize the solver
  nlp_solver_.init();
}
void NLPSolverInternal::init(){
  // Read options
  verbose_ = getOption("verbose");
  gauss_newton_ = getOption("gauss_newton");
  
  // Initialize the functions
  casadi_assert_message(!F_.isNull(),"No objective function");
  if(!F_.isInit()){
    F_.init();
    log("Objective function initialized");
  }
  if(!G_.isNull() && !G_.isInit()){
    G_.init();
    log("Constraint function initialized");
  }

  // Get dimensions
  n_ = F_.input(0).numel();
  m_ = G_.isNull() ? 0 : G_.output(0).numel();

  parametric_ = getOption("parametric");
  
  if (parametric_) {
    casadi_assert_message(F_.getNumInputs()==2, "Wrong number of input arguments to F for parametric NLP. Must be 2, but got " << F_.getNumInputs());
  } else {
    casadi_assert_message(F_.getNumInputs()==1, "Wrong number of input arguments to F for non-parametric NLP. Must be 1, but got " << F_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option.");
  }

  // Basic sanity checks
  casadi_assert_message(F_.getNumInputs()==1 || F_.getNumInputs()==2, "Wrong number of input arguments to F. Must be 1 or 2");
  
  if (F_.getNumInputs()==2) parametric_=true;
  casadi_assert_message(getOption("ignore_check_vec") || gauss_newton_ || F_.input().size2()==1,
     "To avoid confusion, the input argument to F must be vector. You supplied " << F_.input().dimString() << endl <<
     " We suggest you make the following changes:" << endl <<
     "   -  F is an SXFunction:  SXFunction([X],[rhs]) -> SXFunction([vec(X)],[rhs])" << endl <<
     "             or            F -                   ->  F = vec(F) " << 
     "   -  F is an MXFunction:  MXFunction([X],[rhs]) -> " <<  endl <<
     "                                     X_vec = MX(\"X\",vec(X.sparsity())) " << endl <<
     "                                     F_vec = MXFunction([X_flat],[F.call([X_flat.reshape(X.sparsity())])[0]]) " << endl <<
     "             or            F -                   ->  F = vec(F) " << 
     " You may ignore this warning by setting the 'ignore_check_vec' option to true." << endl
  );
  
  casadi_assert_message(F_.getNumOutputs()>=1, "Wrong number of output arguments to F");
  casadi_assert_message(gauss_newton_  || F_.output().scalar(), "Output argument of F not scalar.");
  casadi_assert_message(F_.output().dense(), "Output argument of F not dense.");
  casadi_assert_message(F_.input().dense(), "Input argument of F must be dense. You supplied " << F_.input().dimString());
  
  if(!G_.isNull()) {
    if (parametric_) {
      casadi_assert_message(G_.getNumInputs()==2, "Wrong number of input arguments to G for parametric NLP. Must be 2, but got " << G_.getNumInputs());
    } else {
      casadi_assert_message(G_.getNumInputs()==1, "Wrong number of input arguments to G for non-parametric NLP. Must be 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option.");
    }
    casadi_assert_message(G_.getNumOutputs()>=1, "Wrong number of output arguments to G");
    casadi_assert_message(G_.input().numel()==n_, "Inconsistent dimensions");
    casadi_assert_message(G_.input().sparsity()==F_.input().sparsity(), "F and G input dimension must match. F " << F_.input().dimString() << ". G " << G_.input().dimString());
  }
  
  // Find out if we are to expand the objective function in terms of scalar operations
  bool expand_f = getOption("expand_f");
  if(expand_f){
    log("Expanding objective function");
    
    // Cast to MXFunction
    MXFunction F_mx = shared_cast<MXFunction>(F_);
    if(F_mx.isNull()){
      casadi_warning("Cannot expand objective function as it is not an MXFunction");
    } else {
      // Take use the input scheme of G if possible (it might be an SXFunction)
      vector<SXMatrix> inputv;
      if(!G_.isNull() && F_.getNumInputs()==G_.getNumInputs()){
        inputv = G_.symbolicInputSX();
      } else {
        inputv = F_.symbolicInputSX();
      }
      
      // Try to expand the MXFunction
      F_ = F_mx.expand(inputv);
      F_.setOption("number_of_fwd_dir",F_mx.getOption("number_of_fwd_dir"));
      F_.setOption("number_of_adj_dir",F_mx.getOption("number_of_adj_dir"));
      F_.init();
    }
  }
  
  
  // Find out if we are to expand the constraint function in terms of scalar operations
  bool expand_g = getOption("expand_g");
  if(expand_g){
    log("Expanding constraint function");
    
    // Cast to MXFunction
    MXFunction G_mx = shared_cast<MXFunction>(G_);
    if(G_mx.isNull()){
      casadi_warning("Cannot expand constraint function as it is not an MXFunction");
    } else {
      // Take use the input scheme of F if possible (it might be an SXFunction)
      vector<SXMatrix> inputv;
      if(F_.getNumInputs()==G_.getNumInputs()){
        inputv = F_.symbolicInputSX();
      } else {
        inputv = G_.symbolicInputSX();
      }
      
      // Try to expand the MXFunction
      G_ = G_mx.expand(inputv);
      G_.setOption("number_of_fwd_dir",G_mx.getOption("number_of_fwd_dir"));
      G_.setOption("number_of_adj_dir",G_mx.getOption("number_of_adj_dir"));
      G_.init();
    }
  }
  
  // Find out if we are to expand the constraint function in terms of scalar operations
  bool generate_hessian = getOption("generate_hessian");
  if(generate_hessian && H_.isNull()){
    casadi_assert_message(!gauss_newton_,"Automatic generation of Gauss-Newton Hessian not yet supported");
    log("generating hessian");
    
    // Simple if unconstrained
    if(G_.isNull()){
      // Create Hessian of the objective function
      FX HF = F_.hessian();
      HF.init();
      
      // Symbolic inputs of HF
      vector<MX> HF_in = F_.symbolicInput();
      
      // Lagrange multipliers
      MX lam("lam",0);
      
      // Objective function scaling
      MX sigma("sigma");
      
      // Inputs of the Hessian function
      vector<MX> H_in = HF_in;
      H_in.insert(H_in.begin()+1, lam);
      H_in.insert(H_in.begin()+2, sigma);

      // Get an expression for the Hessian of F
      MX hf = HF.call(HF_in).at(0);
      
      // Create the scaled Hessian function
      H_ = MXFunction(H_in, sigma*hf);
      log("Unconstrained Hessian function generated");
      
    } else { // G_.isNull()
      
      // Check if the functions are SXFunctions
      SXFunction F_sx = shared_cast<SXFunction>(F_);
      SXFunction G_sx = shared_cast<SXFunction>(G_);
      
      // Efficient if both functions are SXFunction
      if(!F_sx.isNull() && !G_sx.isNull()){
        // Expression for f and g
        SXMatrix f = F_sx.outputSX();
        SXMatrix g = G_sx.outputSX();
        
        // Numeric hessian
        bool f_num_hess = F_sx.getOption("numeric_hessian");
        bool g_num_hess = G_sx.getOption("numeric_hessian");
        
        // Number of derivative directions
        int f_num_fwd = F_sx.getOption("number_of_fwd_dir");
        int g_num_fwd = G_sx.getOption("number_of_fwd_dir");
        int f_num_adj = F_sx.getOption("number_of_adj_dir");
        int g_num_adj = G_sx.getOption("number_of_adj_dir");
        
        // Substitute symbolic variables in f if different input variables from g
        if(!isEqual(F_sx.inputSX(),G_sx.inputSX())){
          f = substitute(f,F_sx.inputSX(),G_sx.inputSX());
        }
        
        // Lagrange multipliers
        SXMatrix lam = ssym("lambda",g.size1());

        // Objective function scaling
        SXMatrix sigma = ssym("sigma");        
        
        // Lagrangian function
        vector<SXMatrix> lfcn_in(parametric_? 4: 3);
        lfcn_in[0] = G_sx.inputSX();
        lfcn_in[1] = lam;
        lfcn_in[2] = sigma;
        if (parametric_) lfcn_in[3] = G_sx.inputSX(1);
        SXFunction lfcn(lfcn_in, sigma*f + inner_prod(lam,g));
        lfcn.setOption("verbose",getOption("verbose"));
        lfcn.setOption("numeric_hessian",f_num_hess || g_num_hess);
        lfcn.setOption("number_of_fwd_dir",std::min(f_num_fwd,g_num_fwd));
        lfcn.setOption("number_of_adj_dir",std::min(f_num_adj,g_num_adj));
        lfcn.init();
        
        // Hessian of the Lagrangian
        H_ = static_cast<FX&>(lfcn).hessian();
        H_.setOption("verbose",getOption("verbose"));
        log("SX Hessian function generated");
        
      } else { // !F_sx.isNull() && !G_sx.isNull()
        // Check if the functions are SXFunctions
        MXFunction F_mx = shared_cast<MXFunction>(F_);
        MXFunction G_mx = shared_cast<MXFunction>(G_);
        
        // If they are, check if the arguments are the same
        if(!F_mx.isNull() && !G_mx.isNull() && isEqual(F_mx.inputMX(),G_mx.inputMX())){
          casadi_warning("Exact Hessian calculation for MX is still experimental");
          
          // Expression for f and g
          MX f = F_mx.outputMX();
          MX g = G_mx.outputMX();
          
          // Lagrange multipliers
          MX lam("lam",g.size1());
      
          // Objective function scaling
          MX sigma("sigma");

          // Inputs of the Lagrangian function
          vector<MX> lfcn_in(parametric_? 4:3);
          lfcn_in[0] = G_mx.inputMX();
          lfcn_in[1] = lam;
          lfcn_in[2] = sigma;
          if (parametric_) lfcn_in[3] = G_mx.inputMX(1);

          // Lagrangian function
          MXFunction lfcn(lfcn_in,sigma*f+ inner_prod(lam,g));
          lfcn.init();
	  log("SX Lagrangian function generated");
          
/*          cout << "countNodes(lfcn.outputMX()) = " << countNodes(lfcn.outputMX()) << endl;*/
      
          bool adjoint_mode = true;
          if(adjoint_mode){
          
            // Gradient of the lagrangian
            MX gL = lfcn.grad();
            log("MX Lagrangian gradient generated");

            MXFunction glfcn(lfcn_in,gL);
            glfcn.init();
            log("MX Lagrangian gradient function initialized");
//           cout << "countNodes(glfcn.outputMX()) = " << countNodes(glfcn.outputMX()) << endl;

            // Get Hessian sparsity
            CRSSparsity H_sp = glfcn.jacSparsity();
            log("MX Lagrangian Hessian sparsity determined");
            
            // Uni-directional coloring (note, the hessian is symmetric)
            CRSSparsity coloring = H_sp.unidirectionalColoring(H_sp);
            log("MX Lagrangian Hessian coloring determined");

            // Number of colors needed is the number of rows
            int nfwd_glfcn = coloring.size1();
            log("MX Lagrangian gradient function number of sensitivity directions determined");

            glfcn.setOption("number_of_fwd_dir",nfwd_glfcn);
            glfcn.updateNumSens();
            log("MX Lagrangian gradient function number of sensitivity directions updated");
            
            // Hessian of the Lagrangian
            H_ = glfcn.jacobian();
          } else {

            // Hessian of the Lagrangian
            H_ = lfcn.hessian();
            
          }
          log("MX Lagrangian Hessian function generated");
          
        } else {
          casadi_assert_message(0, "Automatic calculation of exact Hessian currently only for F and G both SXFunction or MXFunction ");
        }
      } // !F_sx.isNull() && !G_sx.isNull()
    } // G_.isNull()
  } // generate_hessian && H_.isNull()
  if(!H_.isNull() && !H_.isInit()) {
    H_.init();
    log("Hessian function initialized");
  }

  // Create a Jacobian if it does not already exists
  bool generate_jacobian = getOption("generate_jacobian");
  if(generate_jacobian && !G_.isNull() && J_.isNull()){
    log("Generating Jacobian");
    J_ = G_.jacobian();
    
    // Use live variables if SXFunction
    if(!shared_cast<SXFunction>(J_).isNull()){
      J_.setOption("live_variables",true);
    }
    log("Jacobian function generated");
  }
    
  if(!J_.isNull() && !J_.isInit()){
    J_.init();
    log("Jacobian function initialized");
  }

  
  if(!H_.isNull()) {
    if (parametric_) {
      casadi_assert_message(H_.getNumInputs()>=2, "Wrong number of input arguments to H for parametric NLP. Must be at least 2, but got " << G_.getNumInputs());
    } else {
      casadi_assert_message(H_.getNumInputs()>=1, "Wrong number of input arguments to H for non-parametric NLP. Must be at least 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option.");
    }
    casadi_assert_message(H_.getNumOutputs()>=1, "Wrong number of output arguments to H");
    casadi_assert_message(H_.input(0).numel()==n_,"Inconsistent dimensions");
    casadi_assert_message(H_.output().size1()==n_,"Inconsistent dimensions");
    casadi_assert_message(H_.output().size2()==n_,"Inconsistent dimensions");
  }

  if(!J_.isNull()){
    if (parametric_) {
      casadi_assert_message(J_.getNumInputs()==2, "Wrong number of input arguments to J for parametric NLP. Must be at least 2, but got " << G_.getNumInputs());
    } else {
      casadi_assert_message(J_.getNumInputs()==1, "Wrong number of input arguments to J for non-parametric NLP. Must be at least 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option.");
    }
    casadi_assert_message(J_.getNumOutputs()>=1, "Wrong number of output arguments to J");
    casadi_assert_message(J_.input().numel()==n_,"Inconsistent dimensions");
    casadi_assert_message(J_.output().size2()==n_,"Inconsistent dimensions");
  }

  if (parametric_) {
    sp_p = F_->input(1).sparsity();
    
    if (!G_.isNull()) casadi_assert_message(sp_p == G_->input(G_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while G has got " << G_->input(G_->getNumInputs()-1).dimString());
    if (!H_.isNull()) casadi_assert_message(sp_p == H_->input(H_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while H has got " << H_->input(H_->getNumInputs()-1).dimString());
    if (!J_.isNull()) casadi_assert_message(sp_p == J_->input(J_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while J has got " << J_->input(J_->getNumInputs()-1).dimString());
  }
  
  // Infinity
  double inf = numeric_limits<double>::infinity();
  
  // Allocate space for inputs
  input_.resize(NLP_NUM_IN - (parametric_? 0 : 1));
  input(NLP_X_INIT)      = DMatrix(n_,1,0);
  input(NLP_LBX)         = DMatrix(n_,1,-inf);
  input(NLP_UBX)         = DMatrix(n_,1, inf);
  input(NLP_LBG)         = DMatrix(m_,1,-inf);
  input(NLP_UBG)         = DMatrix(m_,1, inf);
  input(NLP_LAMBDA_INIT) = DMatrix(m_,1,0);
  if (parametric_) input(NLP_P) = DMatrix(sp_p,0);
  
  // Allocate space for outputs
  output_.resize(NLP_NUM_OUT);
  output(NLP_X_OPT)      = DMatrix(n_,1,0);
  output(NLP_COST)       = DMatrix(1,1,0);
  output(NLP_LAMBDA_X)   = DMatrix(n_,1,0);
  output(NLP_LAMBDA_G)   = DMatrix(m_,1,0);
  output(NLP_G)          = DMatrix(m_,1,0);
  
  if (hasSetOption("iteration_callback")) {
   callback_ = getOption("iteration_callback");
   if (!callback_.isNull()) {
     if (!callback_.isInit()) callback_.init();
     casadi_assert_message(callback_.getNumOutputs()==1, "Callback function should have one output, a scalar that indicates wether to break. 0 = continue");
     casadi_assert_message(callback_.output(0).size()==1, "Callback function should have one output, a scalar that indicates wether to break. 0 = continue");
     casadi_assert_message(callback_.getNumInputs()==NLP_NUM_OUT, "Callback function should have the output scheme of NLPSolver as input scheme. i.e. " <<NLP_NUM_OUT << " inputs instead of the " << callback_.getNumInputs() << " you provided." );
     for (int i=0;i<NLP_NUM_OUT;i++) {
       casadi_assert_message(callback_.input(i).sparsity()==output(i).sparsity(),
         "Callback function should have the output scheme of NLPSolver as input scheme. " << 
         "Input #" << i << " (" << getSchemeEntryEnumName(SCHEME_NLPOutput,i) <<  " aka '" << getSchemeEntryName(SCHEME_NLPOutput,i) << "') was found to be " << callback_.input(i).dimString() << " instead of expected " << output(i).dimString() << "."
       );
       callback_.input(i).setAll(0);
     }
   }
  }
  
  callback_step_ = getOption("iteration_callback_step");

  // Call the initialization method of the base class
  FXInternal::init();
}
void DirectCollocationInternal::init(){
  // Initialize the base classes
  OCPSolverInternal::init();
  
  // Free parameters currently not supported
  casadi_assert_message(np_==0, "Not implemented");

  // Interpolation order
  deg_ = getOption("interpolation_order");

  // All collocation time points
  std::vector<double> tau_root = collocationPoints(deg_,getOption("collocation_scheme"));

  // Size of the finite elements
  double h = tf_/nk_;

  // Coefficients of the collocation equation
  vector<vector<MX> > C(deg_+1,vector<MX>(deg_+1));

  // Coefficients of the collocation equation as DMatrix
  DMatrix C_num = DMatrix::zeros(deg_+1,deg_+1);

  // Coefficients of the continuity equation
  vector<MX> D(deg_+1);

  // Coefficients of the collocation equation as DMatrix
  DMatrix D_num = DMatrix::zeros(deg_+1);

  // Collocation point
  SX tau = SX::sym("tau");

  // For all collocation points
  for(int j=0; j<deg_+1; ++j){
    // Construct Lagrange polynomials to get the polynomial basis at the collocation point
    SX L = 1;
    for(int j2=0; j2<deg_+1; ++j2){
      if(j2 != j){
        L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]);
      }
    }

    SXFunction lfcn(tau,L);
    lfcn.init();

    // Evaluate the polynomial at the final time to get the coefficients of the continuity equation
    lfcn.setInput(1.0);
    lfcn.evaluate();
    D[j] = lfcn.output();
    D_num(j) = lfcn.output();

    // Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
    Function tfcn = lfcn.tangent();
    tfcn.init();
    for(int j2=0; j2<deg_+1; ++j2){
      tfcn.setInput(tau_root[j2]);
      tfcn.evaluate();
      C[j][j2] = tfcn.output();
      C_num(j,j2) = tfcn.output();
    }
  }

  C_num(std::vector<int>(1,0),ALL) = 0;
  C_num(0,0)   = 1;

  // All collocation time points
  vector<vector<double> > T(nk_);
  for(int k=0; k<nk_; ++k){
          T[k].resize(deg_+1);
          for(int j=0; j<=deg_; ++j){
                  T[k][j] = h*(k + tau_root[j]);
          }
  }

  // Total number of variables
  int nlp_nx = 0;
  nlp_nx += nk_*(deg_+1)*nx_;   // Collocated states
  nlp_nx += nk_*nu_;            // Parametrized controls
  nlp_nx += nx_;                       // Final state

  // NLP variable vector
  MX nlp_x = MX::sym("x",nlp_nx);
  int offset = 0;

  // Get collocated states and parametrized control
  vector<vector<MX> > X(nk_+1);
  vector<MX> U(nk_);
  for(int k=0; k<nk_; ++k){
    // Collocated states
        X[k].resize(deg_+1);
    for(int j=0; j<=deg_; ++j){
        // Get the expression for the state vector
        X[k][j] = nlp_x[Slice(offset,offset+nx_)];
        offset += nx_;
    }

    // Parametrized controls
    U[k] = nlp_x[Slice(offset,offset+nu_)];
    offset += nu_;
  }

  // State at end time
  X[nk_].resize(1);
  X[nk_][0] = nlp_x[Slice(offset,offset+nx_)];
  offset += nx_;
  casadi_assert(offset==nlp_nx);

  // Constraint function for the NLP
  vector<MX> nlp_g;

  // Objective function
  MX nlp_j = 0;

  // For all finite elements
  for(int k=0; k<nk_; ++k){

    // For all collocation points
    for(int j=1; j<=deg_; ++j){

        // Get an expression for the state derivative at the collocation point
        MX xp_jk = 0;
        for(int r=0; r<=deg_; ++r){
            xp_jk += C[r][j]*X[k][r];
        }

        // Add collocation equations to the NLP
        MX fk = ffcn_.call(daeIn("x",X[k][j],"p",U[k]))[DAE_ODE];
        nlp_g.push_back(h*fk - xp_jk);
    }

    // Get an expression for the state at the end of the finite element
    MX xf_k = 0;
    for(int r=0; r<=deg_; ++r){
        xf_k += D[r]*X[k][r];
    }

    // Add continuity equation to NLP
    nlp_g.push_back(X[k+1][0] - xf_k);

    // Add path constraints
    if(nh_>0){
      MX pk = cfcn_.call(daeIn("x",X[k+1][0],"p",U[k])).at(0);
      nlp_g.push_back(pk);
    }

    // Add integral objective function term
        //    [Jk] = lfcn.call([X[k+1,0], U[k]])
        //    nlp_j += Jk
  }

  // Add end cost
  MX Jk = mfcn_.call(mayerIn("x",X[nk_][0])).at(0);
  nlp_j += Jk;

  // Objective function of the NLP
  nlp_ = MXFunction(nlpIn("x",nlp_x), nlpOut("f",nlp_j,"g",vertcat(nlp_g)));

  // Get the NLP creator function
  NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
  
  // Allocate an NLP solver
  nlp_solver_ = nlp_solver_creator(nlp_);
  
  // Pass options
  if(hasSetOption("nlp_solver_options")){
    const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
    nlp_solver_.setOption(nlp_solver_options);
  }
  
  // Initialize the solver
  nlp_solver_.init();
}
void CollocationIntegratorInternal::init(){
  
  // Call the base class init
  IntegratorInternal::init();
  
  // Legendre collocation points
  double legendre_points[][6] = {
    {0},
    {0,0.500000},
    {0,0.211325,0.788675},
    {0,0.112702,0.500000,0.887298},
    {0,0.069432,0.330009,0.669991,0.930568},
    {0,0.046910,0.230765,0.500000,0.769235,0.953090}};
    
  // Radau collocation points
  double radau_points[][6] = {
    {0},
    {0,1.000000},
    {0,0.333333,1.000000},
    {0,0.155051,0.644949,1.000000},
    {0,0.088588,0.409467,0.787659,1.000000},
    {0,0.057104,0.276843,0.583590,0.860240,1.000000}};

  // Read options
  bool use_radau;
  if(getOption("collocation_scheme")=="radau"){
    use_radau = true;
  } else if(getOption("collocation_scheme")=="legendre"){
    use_radau = false;
  }
  
  // Hotstart?
  hotstart_ = getOption("hotstart");
  
  // Number of finite elements
  int nk = getOption("number_of_finite_elements");
  
  // Interpolation order
  int deg = getOption("interpolation_order");

  // Assume explicit ODE
  bool explicit_ode = f_.input(DAE_XDOT).size()==0;
  
  // All collocation time points
  double* tau_root = use_radau ? radau_points[deg] : legendre_points[deg];

  // Size of the finite elements
  double h = (tf_-t0_)/nk;
  
  // MX version of the same
  MX h_mx = h;
    
  // Coefficients of the collocation equation
  vector<vector<MX> > C(deg+1,vector<MX>(deg+1));

  // Coefficients of the continuity equation
  vector<MX> D(deg+1);

  // Collocation point
  SXMatrix tau = ssym("tau");

  // For all collocation points
  for(int j=0; j<deg+1; ++j){
    // Construct Lagrange polynomials to get the polynomial basis at the collocation point
    SXMatrix L = 1;
    for(int j2=0; j2<deg+1; ++j2){
      if(j2 != j){
        L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]);
      }
    }
    
    SXFunction lfcn(tau,L);
    lfcn.init();
  
    // Evaluate the polynomial at the final time to get the coefficients of the continuity equation
    lfcn.setInput(1.0);
    lfcn.evaluate();
    D[j] = lfcn.output();

    // Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
    for(int j2=0; j2<deg+1; ++j2){
      lfcn.setInput(tau_root[j2]);
      lfcn.setFwdSeed(1.0);
      lfcn.evaluate(1,0);
      C[j][j2] = lfcn.fwdSens();
    }
  }
  
  // Initial state
  MX X0("X0",nx_);
  
  // Parameters
  MX P("P",np_);
  
  // Backward state
  MX RX0("RX0",nrx_);
  
  // Backward parameters
  MX RP("RP",nrp_);
  
  // Collocated differential states and algebraic variables
  int nX = (nk*(deg+1)+1)*(nx_+nrx_);
  int nZ = nk*deg*(nz_+nrz_);
  
  // Unknowns
  MX V("V",nX+nZ);
  int offset = 0;
  
  // Get collocated states, algebraic variables and times
  vector<vector<MX> > X(nk+1);
  vector<vector<MX> > RX(nk+1);
  vector<vector<MX> > Z(nk);
  vector<vector<MX> > RZ(nk);
  coll_time_.resize(nk+1);
  for(int k=0; k<nk+1; ++k){
    // Number of time points
    int nj = k==nk ? 1 : deg+1;
    
    // Allocate differential states expressions at the time points
    X[k].resize(nj);
    RX[k].resize(nj);
    coll_time_[k].resize(nj);

    // Allocate algebraic variable expressions at the collocation points
    if(k!=nk){
      Z[k].resize(nj-1);
      RZ[k].resize(nj-1);
    }

    // For all time points
    for(int j=0; j<nj; ++j){
      // Get expressions for the differential state
      X[k][j] = V[range(offset,offset+nx_)];
      offset += nx_;
      RX[k][j] = V[range(offset,offset+nrx_)];
      offset += nrx_;
      
      // Get the local time
      coll_time_[k][j] = h*(k + tau_root[j]);
      
      // Get expressions for the algebraic variables
      if(j>0){
        Z[k][j-1] = V[range(offset,offset+nz_)];
        offset += nz_;
        RZ[k][j-1] = V[range(offset,offset+nrz_)];
        offset += nrz_;
      }
    }
  }
  
  // Check offset for consistency
  casadi_assert(offset==V.size());

  // Constraints
  vector<MX> g;
  g.reserve(2*(nk+1));
  
  // Quadrature expressions
  MX QF = MX::zeros(nq_);
  MX RQF = MX::zeros(nrq_);
  
  // Counter
  int jk = 0;
  
  // Add initial condition
  g.push_back(X[0][0]-X0);
  
  // For all finite elements
  for(int k=0; k<nk; ++k, ++jk){
  
    // For all collocation points
    for(int j=1; j<deg+1; ++j, ++jk){
      // Get the time
      MX tkj = coll_time_[k][j];
      
      // Get an expression for the state derivative at the collocation point
      MX xp_jk = 0;
      for(int j2=0; j2<deg+1; ++j2){
        xp_jk += C[j2][j]*X[k][j2];
      }
      
      // Add collocation equations to the NLP
      vector<MX> f_in(DAE_NUM_IN);
      f_in[DAE_T] = tkj;
      f_in[DAE_P] = P;
      f_in[DAE_X] = X[k][j];
      f_in[DAE_Z] = Z[k][j-1];
      
      vector<MX> f_out;
      if(explicit_ode){
        // Assume equation of the form ydot = f(t,y,p)
        f_out = f_.call(f_in);
        g.push_back(h_mx*f_out[DAE_ODE] - xp_jk);
      } else {
        // Assume equation of the form 0 = f(t,y,ydot,p)
        f_in[DAE_XDOT] = xp_jk/h_mx;
        f_out = f_.call(f_in);
        g.push_back(f_out[DAE_ODE]);
      }
      
      // Add the algebraic conditions
      if(nz_>0){
        g.push_back(f_out[DAE_ALG]);
      }
      
      // Add the quadrature
      if(nq_>0){
        QF += D[j]*h_mx*f_out[DAE_QUAD];
      }
      
      // Now for the backward problem
      if(nrx_>0){
        
        // Get an expression for the state derivative at the collocation point
        MX rxp_jk = 0;
        for(int j2=0; j2<deg+1; ++j2){
          rxp_jk += C[j2][j]*RX[k][j2];
        }
        
        // Add collocation equations to the NLP
        vector<MX> g_in(RDAE_NUM_IN);
        g_in[RDAE_T] = tkj;
        g_in[RDAE_X] = X[k][j];
        g_in[RDAE_Z] = Z[k][j-1];
        g_in[RDAE_P] = P;
        g_in[RDAE_RP] = RP;
        g_in[RDAE_RX] = RX[k][j];
        g_in[RDAE_RZ] = RZ[k][j-1];
        
        vector<MX> g_out;
        if(explicit_ode){
          // Assume equation of the form xdot = f(t,x,p)
          g_out = g_.call(g_in);
          g.push_back(h_mx*g_out[RDAE_ODE] - rxp_jk);
        } else {
          // Assume equation of the form 0 = f(t,x,xdot,p)
          g_in[RDAE_XDOT] = xp_jk/h_mx;
          g_in[RDAE_RXDOT] = rxp_jk/h_mx;
          g_out = g_.call(g_in);
          g.push_back(g_out[RDAE_ODE]);
        }
        
        // Add the algebraic conditions
        if(nrz_>0){
          g.push_back(g_out[RDAE_ALG]);
        }
        
        // Add the backward quadrature
        if(nrq_>0){
          RQF += D[j]*h_mx*g_out[RDAE_QUAD];
        }
      }
    }
    
    // Get an expression for the state at the end of the finite element
    MX xf_k = 0;
    for(int j=0; j<deg+1; ++j){
      xf_k += D[j]*X[k][j];
    }

    // Add continuity equation to NLP
    g.push_back(X[k+1][0] - xf_k);
    
    if(nrx_>0){
      // Get an expression for the state at the end of the finite element
      MX rxf_k = 0;
      for(int j=0; j<deg+1; ++j){
        rxf_k += D[j]*RX[k][j];
      }

      // Add continuity equation to NLP
      g.push_back(RX[k+1][0] - rxf_k);
    }
  }
  
  // Add initial condition for the backward integration
  if(nrx_>0){
    g.push_back(RX[nk][0]-RX0);
  }
  
  // Constraint expression
  MX gv = vertcat(g);
    
  // Make sure that the dimension is consistent with the number of unknowns
  casadi_assert_message(gv.size()==V.size(),"Implicit function unknowns and equations do not match");

  // Nonlinear constraint function input
  vector<MX> gfcn_in(1+INTEGRATOR_NUM_IN);
  gfcn_in[0] = V;
  gfcn_in[1+INTEGRATOR_X0] = X0;
  gfcn_in[1+INTEGRATOR_P] = P;
  gfcn_in[1+INTEGRATOR_RX0] = RX0;
  gfcn_in[1+INTEGRATOR_RP] = RP;

  vector<MX> gfcn_out(1+INTEGRATOR_NUM_OUT);
  gfcn_out[0] = gv;
  gfcn_out[1+INTEGRATOR_XF] = X[nk][0];
  gfcn_out[1+INTEGRATOR_QF] = QF;
  gfcn_out[1+INTEGRATOR_RXF] = RX[0][0];
  gfcn_out[1+INTEGRATOR_RQF] = RQF;
  
  // Nonlinear constraint function
  FX gfcn = MXFunction(gfcn_in,gfcn_out);
  
  // Expand f?
  bool expand_f = getOption("expand_f");
  if(expand_f){
    gfcn.init();
    gfcn = SXFunction(shared_cast<MXFunction>(gfcn));
  }
  
  // Get the NLP creator function
  implicitFunctionCreator implicit_function_creator = getOption("implicit_solver");
  
  // Allocate an NLP solver
  implicit_solver_ = implicit_function_creator(gfcn);
  
  // Pass options
  if(hasSetOption("implicit_solver_options")){
    const Dictionary& implicit_solver_options = getOption("implicit_solver_options");
    implicit_solver_.setOption(implicit_solver_options);
  }
  
  // Initialize the solver
  implicit_solver_.init();
  
  if(hasSetOption("startup_integrator")){
    
    // Create the linear solver
    integratorCreator startup_integrator_creator = getOption("startup_integrator");
    
    // Allocate an NLP solver
    startup_integrator_ = startup_integrator_creator(f_,g_);
    
    // Pass options
    startup_integrator_.setOption("number_of_fwd_dir",0); // not needed
    startup_integrator_.setOption("number_of_adj_dir",0); // not needed
    startup_integrator_.setOption("t0",coll_time_.front().front());
    startup_integrator_.setOption("tf",coll_time_.back().back());
    if(hasSetOption("startup_integrator_options")){
      const Dictionary& startup_integrator_options = getOption("startup_integrator_options");
      startup_integrator_.setOption(startup_integrator_options);
    }
    
    // Initialize the startup integrator
    startup_integrator_.init();
  }

  // Mark the system not yet integrated
  integrated_once_ = false;
}