#ifndef STAN_MCMC_HMC_INTEGRATORS_IMPL_LEAPFROG_HPP
#define STAN_MCMC_HMC_INTEGRATORS_IMPL_LEAPFROG_HPP

#include <Eigen/Dense>
#include <stan/mcmc/hmc/integrators/base_leapfrog.hpp>

namespace stan {
  namespace mcmc {

    template <typename Hamiltonian>
    class impl_leapfrog: public base_leapfrog<Hamiltonian> {
    public:
      impl_leapfrog(): base_leapfrog<Hamiltonian>(),
                       max_num_fixed_point_(10),
                       fixed_point_threshold_(1e-8) {}

      void begin_update_p(typename Hamiltonian::PointType& z,
                          Hamiltonian& hamiltonian,
                          double epsilon,
                          callbacks::writer& info_writer,
                          callbacks::writer& error_writer) {
        hat_phi(z, hamiltonian, epsilon, info_writer, error_writer);
        hat_tau(z, hamiltonian, epsilon, this->max_num_fixed_point_,
                info_writer, error_writer);
      }

      void update_q(typename Hamiltonian::PointType& z,
                    Hamiltonian& hamiltonian,
                    double epsilon,
                    callbacks::writer& info_writer,
                    callbacks::writer& error_writer) {
        // hat{T} = dT/dp * d/dq
        Eigen::VectorXd q_init = z.q + 0.5 * epsilon * hamiltonian.dtau_dp(z);
        Eigen::VectorXd delta_q(z.q.size());

        for (int n = 0; n < this->max_num_fixed_point_; ++n) {
          delta_q = z.q;
          z.q.noalias() = q_init + 0.5 * epsilon * hamiltonian.dtau_dp(z);
          hamiltonian.update_metric(z, info_writer, error_writer);

          delta_q -= z.q;
          if (delta_q.cwiseAbs().maxCoeff() < this->fixed_point_threshold_)
            break;
        }
        hamiltonian.update_gradients(z, info_writer, error_writer);
      }

      void end_update_p(typename Hamiltonian::PointType& z,
                        Hamiltonian& hamiltonian,
                        double epsilon,
                        callbacks::writer& info_writer,
                        callbacks::writer& error_writer) {
        hat_tau(z, hamiltonian, epsilon, 1, info_writer, error_writer);
        hat_phi(z, hamiltonian, epsilon, info_writer, error_writer);
      }

      // hat{phi} = dphi/dq * d/dp
      void hat_phi(typename Hamiltonian::PointType& z,
                   Hamiltonian& hamiltonian,
                   double epsilon,
                   callbacks::writer& info_writer,
                   callbacks::writer& error_writer) {
        z.p -= epsilon * hamiltonian.dphi_dq(z, info_writer, error_writer);
      }

      // hat{tau} = dtau/dq * d/dp
      void hat_tau(typename Hamiltonian::PointType& z,
                   Hamiltonian& hamiltonian,
                   double epsilon,
                   int num_fixed_point,
                   callbacks::writer& info_writer,
                   callbacks::writer& error_writer) {
        Eigen::VectorXd p_init = z.p;
        Eigen::VectorXd delta_p(z.p.size());

        for (int n = 0; n < num_fixed_point; ++n) {
          delta_p = z.p;
          z.p.noalias() = p_init
                          - epsilon
                          * hamiltonian.dtau_dq(z, info_writer, error_writer);
          delta_p -= z.p;
          if (delta_p.cwiseAbs().maxCoeff() < this->fixed_point_threshold_)
            break;
        }
      }

      int max_num_fixed_point() {
        return this->max_num_fixed_point_;
      }

      void set_max_num_fixed_point(int n) {
        if (n > 0) this->max_num_fixed_point_ = n;
      }

      double fixed_point_threshold() {
        return this->fixed_point_threshold_;
      }

      void set_fixed_point_threshold(double t) {
        if (t > 0) this->fixed_point_threshold_ = t;
      }

    private:
      int max_num_fixed_point_;
      double fixed_point_threshold_;
    };

  }  // mcmc
}  // stan

#endif
