# include <cppad/ipopt/solve.hpp>

namespace {
     using CppAD::AD;

     // value of a during simulation a[0], a[1], a[2]
     double a_[] =                   {2.0,  1.0, 0.5};
     // number of components in a
     size_t na_ = sizeof(a_) / sizeof(a_[0]);

     // function used to simulate data
     double yone(double t)
     {     return
               a_[0]*a_[1] * (exp(-a_[2]*t) - exp(-a_[1]*t)) / (a_[1] - a_[2]);
     }

     // time points were we have data (no data at first point)
     double s_[] = {0.0,   0.5,        1.0,          1.5,         2.0 };

     // Simulated data for case with no noise (first point is not used)
     double z_[] = {yone(s_[1]), yone(s_[2]), yone(s_[3]), yone(s_[4])};
     size_t nz_  = sizeof(z_) / sizeof(z_[0]);

     // number of trapozoidal approximation points per measurement interval
     size_t np_  = 40;


     class FG_eval
     {
     private:
     public:
          // derived class part of constructor
          typedef CPPAD_TESTVECTOR( AD<double> ) ADvector;

          // Evaluation of the objective f(x), and constraints g(x)
          void operator()(ADvector& fg, const ADvector& x)
          {     CPPAD_TESTVECTOR( AD<double> ) a(na_);
               size_t i, j, k;

               // extract the vector a
               for(i = 0; i < na_; i++)
                    a[i] = x[i];

               // compute the object f(x)
               fg[0] = 0.0;
               for(i = 0; i < nz_; i++)
               {     k = (i + 1) * np_;
                    AD<double> y_1 = x[na_ + 2 * k + 1];
                    AD<double> dif = z_[i] - y_1;
                    fg[0]         += dif * dif;
               }

               // constraint corresponding to initial value y(0, a)
               // Note that this constraint is invariant with size of dt
               fg[1] = x[na_+0] - a[0];
               fg[2] = x[na_+1] - 0.0;

               // constraints corresponding to trapozoidal approximation
               for(i = 0; i < nz_; i++)
               {     // spacing between grid point
                    double dt = (s_[i+1] - s_[i]) / static_cast<double>(np_);
                    for(j = 1; j <= np_; j++)
                    {     k = i * np_ + j;
                         // compute derivative at y^k
                         AD<double> y_0  = x[na_ + 2 * k + 0];
                         AD<double> y_1  = x[na_ + 2 * k + 1];
                         AD<double> G_0  = - a[1] * y_0;
                         AD<double> G_1  = + a[1] * y_0 - a[2] * y_1;

                         // compute derivative at y^{k-1}
                         AD<double> ym_0  = x[na_ + 2 * (k-1) + 0];
                         AD<double> ym_1  = x[na_ + 2 * (k-1) + 1];
                         AD<double> Gm_0  = - a[1] * ym_0;
                         AD<double> Gm_1  = + a[1] * ym_0 - a[2] * ym_1;

                         // constraint should be zero
                         fg[1 + 2*k ] = y_0  - ym_0 - dt*(G_0 + Gm_0)/2.;
                         fg[2 + 2*k ] = y_1  - ym_1 - dt*(G_1 + Gm_1)/2.;

                         // scale g(x) so it has similar size as f(x)
                         fg[1 + 2*k ] /= dt;
                         fg[2 + 2*k ] /= dt;
                    }
               }
          }
     };
}
bool ode_inverse(void)
{     bool ok = true;
     size_t i;
     typedef CPPAD_TESTVECTOR( double ) Dvector;

     // number of components in the function g
     size_t ng = (np_ * nz_ + 1) * 2;
     // number of independent variables
     size_t nx = na_ + ng;
     // initial vlaue for the variables we are optimizing w.r.t
     Dvector xi(nx), xl(nx), xu(nx);
     for(i = 0; i < nx; i++)
     {     xi[i] =   0.0; // initial value
          xl[i] = -1e19; // no lower limit
          xu[i] = +1e19; // no upper limit
     }
     for(i = 0; i < na_; i++)
          xi[0] = 1.5;   // initial value for a

     // all the difference equations are constrainted to be zero
     Dvector gl(ng), gu(ng);
     for(i = 0; i < ng; i++)
     {     gl[i] = 0.0;
          gu[i] = 0.0;
     }
     // object defining both f(x) and g(x)
     FG_eval fg_eval;

     // options
     std::string options;
     // Use sparse matrices for calculation of Jacobians and Hessians
     // with forward mode for Jacobian (seems to be faster for this case).
     options += "Sparse  true        forward\n";
     // turn off any printing
     options += "Integer print_level 0\n";
     options += "String  sb        yes\n";
     // maximum number of iterations
     options += "Integer max_iter    30\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";

     // 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-4;  // relative tolerance
     double abs_tol    = 1e-4;  // absolute tolerance
     for(i = 0; i < na_; i++)
          ok &= CppAD::NearEqual( a_[i],  solution.x[i],   rel_tol, abs_tol);

     return ok;
}