#ifndef SIM_RK4SOLVER_H
#define SIM_RK4SOLVER_H

/// \cond
// C headers
// C++ headers
#include <functional>
#include <limits>

// 3rd party headers

/// \endcond

// qtrocket headers
#include "Logger.h"

/**
 * @brief Runge-Kutta 4th order coupled ODE solver.
 * @note This was written outside of the context of QtRocket, and it is very generic. There are
 *       some features of this solver that are note used by QtRocket, for example, it can solve
 *       and arbitrarily large system of coupled ODEs, but QtRocket only makes use of a system
 *       of size 6 (x, y, z, xDot, yDot, zDot) at a time. 
 * 
 * @tparam Ts 
 */
template<typename T>
class RK4Solver
{
public:

   RK4Solver(std::function<std::pair<T, T>(T&, T&)> func, double ts=0.1)
   {
//      static_assert(std::is_same<T, Vector3>::value,
//                    "You can only use Vector3 or Quaternion valued functions in RK4Solver");
      
      odes = func;
      setTimeStep(ts);
   }
   ~RK4Solver() {}

   void setTimeStep(double inTs) { dt = inTs;  halfDT = dt / 2.0; }

   std::pair<T, T> step(T& state, T& rate)
   {
      std::pair<T, T> res;
      if(dt == std::numeric_limits<double>::quiet_NaN())
      {
         Logger::getInstance()->error("Calling RK4Solver without setting dt first is an error");
         return res;
      }

      std::tie(k1State, k1Rate) = odes(state, rate);
      // compute k2 values. This involves stepping the current values forward a half-step
      // based on k1, so we do the stepping first
      std::tie(tempState, tempRate) = std::make_pair(state + k1State*halfDT, rate + k1Rate*halfDT);
      std::tie(k2State, k2Rate) = odes(tempState, tempRate);

      std::tie(tempState, tempRate) = std::make_pair(state + k2State*halfDT, rate + k2Rate*halfDT);
      std::tie(k3State, k3Rate) = odes(tempState, tempRate);

      std::tie(tempState, tempRate) = std::make_pair(state + k3State*dt, rate + k3Rate*dt);
      std::tie(k4State, k4Rate) = odes(tempState, tempRate);

      res = std::make_pair(state + (dt / 6.0)*(k1State + 2.0*k2State + 2.0*k3State + k4State),
                           rate  + (dt / 6.0)*(k1Rate +  2.0*k2Rate  + 2.0*k3Rate  + k4Rate));

      return res;
   }

private:
   std::function<std::pair<T, T>(T&, T&)> odes;

   T k1State;
   T k2State;
   T k3State;
   T k4State;
   T k1Rate;
   T k2Rate;
   T k3Rate;
   T k4Rate;

   T tempState;
   T tempRate;

   double dt = std::numeric_limits<double>::quiet_NaN();
   double halfDT = 0.0;

};

#endif // SIM_RK4SOLVER_H