#include <alglib/ap.h>
#include <alglib/interpolation.h>

#include "spline-2d.hpp"

#define This Spline2d

namespace perceive
{
using std::cout;
using std::endl;

// ----------------------------------------------------------------------- Pimpl

class This::Pimpl
{
 public:
   Pimpl()
       : is_init(false)
       , n_control_points(0)
       , approx_length(dNAN)
   {}

   Vector2 range_hint{0.0, 1.0};
   bool is_init;
   uint n_control_points;
   double approx_length;
   alglib::spline1dinterpolant spline_x;
   alglib::spline1dinterpolant spline_y;
   alglib::spline1dfitreport rep_x;
   alglib::spline1dfitreport rep_y;
};

// ---------------------------------------------------------------- Construction

This::Spline2d()
    : pimpl(make_unique<Pimpl>())
{}

This::Spline2d(const Spline2d& rhs)
    : Spline2d()
{
   *this = rhs;
}
This::Spline2d(Spline2d&& rhs)
    : pimpl{nullptr}
{
   *this = std::move(rhs);
}

This::Spline2d(const std::vector<Vector2>& ps, double rho, int M_factor)
    : Spline2d()
{
   init(ps, rho, M_factor);
}
This::Spline2d(const std::vector<Vector2r>& ps, double rho, int M_factor)
    : Spline2d()
{
   init(ps, rho, M_factor);
}
This::Spline2d(const std::deque<Vector2>& ps, double rho, int M_factor)
    : Spline2d()
{
   init(ps, rho, M_factor);
}
This::Spline2d(const std::deque<Vector2r>& ps, double rho, int M_factor)
    : Spline2d()
{
   init(ps, rho, M_factor);
}

This::~Spline2d() = default;

Spline2d& This::operator=(const Spline2d& rhs)
{
   if(this != &rhs) *pimpl = *rhs.pimpl;
   return *this;
}

Spline2d& This::operator=(Spline2d&& rhs) = default;

// ------------------------------------------------------------------- Accessors

bool This::is_init() const { return pimpl->is_init; }
uint This::n_control_points() const { return pimpl->n_control_points; }
double This::approx_length() const { return pimpl->approx_length; }

double This::rms_error_x() const { return pimpl->rep_x.rmserror; }
double This::avg_error_x() const { return pimpl->rep_x.avgerror; }
double This::avg_rel_error_x() const { return pimpl->rep_x.avgrelerror; }
double This::max_error_x() const { return pimpl->rep_x.maxerror; }

double This::rms_error_y() const { return pimpl->rep_y.rmserror; }
double This::avg_error_y() const { return pimpl->rep_y.avgerror; }
double This::avg_rel_error_y() const { return pimpl->rep_y.avgrelerror; }
double This::max_error_y() const { return pimpl->rep_y.maxerror; }

Vector2& This::range_hint() { return pimpl->range_hint; }
const Vector2& This::range_hint() const { return pimpl->range_hint; }

// -------------------------------------------------------------------- p-spline

/*************************************************************************
Fitting by penalized cubic spline.

Equidistant grid with M nodes on [min(x,xc),max(x,xc)] is  used  to  build
basis functions. Basis functions are cubic splines with  natural  boundary
conditions. Problem is regularized by  adding non-linearity penalty to the
usual least squares penalty function:

    S(x) = arg min { LS + P }, where
    LS   = SUM { w[i]^2*(y[i] - S(x[i]))^2 } - least squares penalty
    P    = C*10^rho*integral{ S''(x)^2*dx } - non-linearity penalty
    rho  - tunable constant given by user
    C    - automatically determined scale parameter,
           makes penalty invariant with respect to scaling of X, Y, W.

INPUT PARAMETERS:
    X   -   points, array[0..N-1].
    Y   -   function values, array[0..N-1].
    N   -   number of points (optional):
            * N>0
            * if given, only first N elements of X/Y are processed
            * if not given, automatically determined from X/Y sizes
    M   -   number of basis functions ( = number_of_nodes), M>=4.
    Rho -   regularization  constant  passed   by   user.   It   penalizes
            nonlinearity in the regression spline. It  is  logarithmically
            scaled,  i.e.  actual  value  of  regularization  constant  is
            calculated as 10^Rho. It is automatically scaled so that:
            * Rho=2.0 corresponds to moderate amount of nonlinearity
            * generally, it should be somewhere in the [-8.0,+8.0]
            If you do not want to penalize nonlineary,
            pass small Rho. Values as low as -15 should work.

OUTPUT PARAMETERS:
    Info-   same format as in LSFitLinearWC() subroutine.
            * Info>0    task is solved
            * Info<=0   an error occured:
                        -4 means inconvergence of internal SVD or
                           Cholesky decomposition; problem may be
                           too ill-conditioned (very rare)
    S   -   spline interpolant.
    Rep -   Following fields are set:
            * RMSError      rms error on the (X,Y).
            * AvgError      average error on the (X,Y).
            * AvgRelError   average relative error on the non-zero Y
            * MaxError      maximum error
                            NON-WEIGHTED ERRORS ARE CALCULATED

IMPORTANT:
    this subroitine doesn't calculate task's condition number for K<>0.

NOTE 1: additional nodes are added to the spline outside  of  the  fitting
interval to force linearity when x<min(x,xc) or x>max(x,xc).  It  is  done
for consistency - we penalize non-linearity  at [min(x,xc),max(x,xc)],  so
it is natural to force linearity outside of this interval.

NOTE 2: function automatically sorts points,  so  caller may pass unsorted
array.

  -- ALGLIB PROJECT --
     Copyright 18.08.2009 by Bochkanov Sergey
*************************************************************************/

template<typename U>
bool init_pspline(Spline2d& curve,
                  const U& container,
                  double rho,
                  unsigned M_factor)
{
   curve.pimpl->n_control_points = 0;
   curve.pimpl->is_init          = false;
   curve.pimpl->approx_length    = dNAN;
   curve.pimpl->range_hint       = Vector2(0.0, 1.0);

   const uint32_t len = uint32_t(container.size());
   if(len < 5) return false;

   alglib::real_1d_array T;
   alglib::real_1d_array X;
   alglib::real_1d_array Y;
   auto extra = 0u;
   T.setlength(len + extra);
   X.setlength(len + extra);
   Y.setlength(len + extra);

   uint counter = 0;
   double t     = 0.0;
   double dt    = 1.0 / double(len - 1);
   for(const auto& p : container) {
      T[counter] = t;
      X[counter] = p(0);
      Y[counter] = p(1);
      t += dt;
      counter++;
   }

   // Add the last point in five more times... to get boundary
   for(auto i = 0u; i < extra; ++i) {
      T[len + i] = 1.0;
      X[len + i] = container.back()(0);
      Y[len + i] = container.back()(1);
   }

   alglib::ae_int_t info_x, info_y;
   alglib::ae_int_t M = std::max<int>(4, int(M_factor));

   alglib::spline1dfitpenalized(
       T, X, M, rho, info_x, curve.pimpl->spline_x, curve.pimpl->rep_x);
   alglib::spline1dfitpenalized(
       T, Y, M, rho, info_y, curve.pimpl->spline_y, curve.pimpl->rep_y);

   curve.pimpl->n_control_points = unsigned(M);

   if(info_x > 0 || info_y > 0) {
      curve.pimpl->is_init       = true;
      curve.pimpl->approx_length = 0.0;
      double dt                  = 0.1 / double(M - 1);
      Vector2 p                  = curve.evaluate(0.0);
      for(double t = dt; t <= 1.0; t += dt) {
         Vector2 q = curve.evaluate(t);
         curve.pimpl->approx_length += (p - q).norm();
         p = q;
      }
   }

   return curve.pimpl->is_init;
}

// ------------------------------------------------------------------------ Init

bool This::init(const std::vector<Vector2>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::deque<Vector2>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::vector<Vector2r>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::deque<Vector2r>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

// -------------------------------------------------------------------- Evaluate

Vector2 This::evaluate(double t) const
{
   // assert(t >= 0.0 && t <= 1.0);

   return is_init() ? Vector2(alglib::spline1dcalc(pimpl->spline_x, t),
                              alglib::spline1dcalc(pimpl->spline_y, t))
                    : Vector2::nan();
}

This::GradientResult This::gradient(double t) const
{
   GradientResult x;

   if(is_init()) {
      double s, ds, ds2;
      double r, dr, dr2;
      alglib::spline1ddiff(pimpl->spline_x, t, s, ds, ds2);
      alglib::spline1ddiff(pimpl->spline_y, t, r, dr, dr2);
      x.x(0) = s;
      x.x(1) = r;
      x.g(0) = -dr;
      x.g(1) = ds;
      x.g.normalise();
   } else {
      x.x = x.g = Vector2::nan();
   }

   return x;
}

// ---------------------------------------------------------------------- Unpack

std::vector<array<double, 12>> This::unpack() const
{
   alglib::ae_int_t nx{0};
   alglib::ae_int_t ny{0};
   alglib::real_2d_array tbl_x, tbl_y;
   spline1dunpack(pimpl->spline_x, nx, tbl_x);
   spline1dunpack(pimpl->spline_y, ny, tbl_y);

   if(nx != ny)
      FATAL(format("How did this happen? nx = {} != {} = ny", nx, ny));

   auto N = tbl_x.rows();

   // INFO(format("nx = {}, len = ({}, {})", nx, tbl_x.rows(),tbl_x.cols()));

   std::vector<array<double, 12>> out;
   out.resize(size_t(N));
   for(alglib::ae_int_t ind = 0; ind < N; ++ind) {
      auto& coeff = out[size_t(ind)];
      auto ptr    = &coeff[0];
      for(unsigned i = 0; i < 6; ++i) *ptr++ = tbl_x(ind, i);
      for(unsigned i = 0; i < 6; ++i) *ptr++ = tbl_y(ind, i);
   }

   return out;
}

void This::init(const std::vector<array<double, 12>>& coefficients) const
{
   using alglib::spline1dinterpolant;

   auto make_interpolant = [&](bool is_x, spline1dinterpolant& interpolant) {
      unsigned offset = (is_x) ? 0 : 6;

      const unsigned dilate   = 1; // number of point per cooef set
      const double dilate_inv = 1.0 / double(dilate);
      const unsigned n        = unsigned(coefficients.size() * dilate + 1);
      alglib::real_1d_array x, y, d; // (x, f(x), f'(x))
      x.setlength(n);
      y.setlength(n);
      d.setlength(n);
      unsigned pos = 0; // write position

      auto write = [&](double t,
                       double X0,
                       double X1,
                       double C0,
                       double C1,
                       double C2,
                       double C3) {
         x(pos) = X0 + t * (X1 - X0);
         y(pos) = C0 + C1 * t + C2 * t * t + C3 * t * t * t;
         d(pos) = C1 + 2.0 * C2 * t + 3.0 * C3 * t * t;
         pos++;
      };

      for(unsigned ind = 0; ind < coefficients.size(); ++ind) {
         const auto& CC = coefficients[ind];
         auto X0        = CC[0 + offset];
         auto X1        = CC[1 + offset];
         auto C0        = CC[2 + offset];
         auto C1        = CC[3 + offset];
         auto C2        = CC[4 + offset];
         auto C3        = CC[5 + offset];
         for(unsigned i = 0; i < dilate; ++i)
            write(dilate_inv * double(i), X0, X1, C0, C1, C2, C3);

         // Write that final coefficient
         if(ind == coefficients.size() - 1) write(1.0, X0, X1, C0, C1, C2, C3);
      }

      assert(pos == n);

      alglib::spline1dbuildhermite(x, y, d, interpolant);
   };

   spline1dinterpolant spline_x, spline_y;
   make_interpolant(true, spline_x);
   make_interpolant(false, spline_y);

   pimpl->spline_x         = spline_x;
   pimpl->spline_y         = spline_y;
   pimpl->is_init          = true;
   pimpl->n_control_points = unsigned(coefficients.size());
   pimpl->rep_x.rmserror = pimpl->rep_y.rmserror = 0.0;
   pimpl->rep_x.avgerror = pimpl->rep_y.avgerror = 0.0;
   pimpl->rep_x.maxerror = pimpl->rep_y.maxerror = 0.0;
   pimpl->rep_x.avgrelerror = pimpl->rep_y.avgrelerror = 0.0;
}

// ----------------------------------------------------------------- Interpolate

void This::interpolate(unsigned n,
                       std::vector<Vector2>& out,
                       double min_t,
                       double max_t)
{
   assert(min_t >= 0.0 && min_t <= 1.0);
   assert(max_t >= 0.0 && max_t <= 1.0);
   assert(min_t <= max_t);

   out.reserve(n);

   if(n == 0)
      return;
   else if(n == 1)
      out.push_back(evaluate(max_t));
   else {
      double t  = min_t;
      double dt = (max_t - min_t) / double(n - 1);
      for(unsigned i = 0; i < n - 1; ++i) {
         out.push_back(evaluate(clamp(t, min_t, max_t)));
         t += dt;
      }
      out.push_back(evaluate(max_t));
   }

   assert(out.size() == n);
}

// ---------------------------------------------------------------------- Smooth

bool This::smooth(std::vector<Vector2>& path, double rho, int M_factor)
{
   if(path.size() < 5) return false;
   Spline2d spline;
   if(!spline.init(path, rho, M_factor)) return false;
   std::vector<Vector2> t;
   spline.interpolate(unsigned(path.size()), t);
   if(!(t.size() == path.size()))
      FATAL(format("Container size mismatch: expected {}, but got {}",
                   path.size(),
                   t.size()));
   auto src = t.begin();
   auto dst = path.begin();
   while(src != t.end()) *src++ = *dst++;
   return true;
}

// ---------------------------------------------------------------------- Find-t

double find_t(const Spline2d& spline,
              double y_value,
              double start_t,
              bool forward,
              double epsilon)
{
   if(!spline.is_init()) return double(NAN);

   uint n_cps = spline.n_control_points();
   assert(n_cps > 0);
   assert(start_t >= 0 && start_t <= 1.0);
   double step = (forward ? 1.0 : -1.0) * 0.5 / double(n_cps);

   double last_t  = start_t;
   Vector2 last_v = spline.evaluate(start_t);
   double next_t  = last_t + step;
   if(next_t <= 0.0) next_t = 0.0;
   if(next_t >= 1.0) next_t = 1.0;
   Vector2 next_v = spline.evaluate(next_t);

   auto is_between = [&]() {
      return (last_v.y < next_v.y)
                 ? inclusive_between<double>(last_v.y, y_value, next_v.y)
                 : inclusive_between<double>(next_v.y, y_value, last_v.y);
   };

   while(inclusive_between(0.0, next_t, 1.0) && !is_between()) {
      last_v = next_v;
      last_t = next_t;
      next_t += step;
      if(inclusive_between(0.0, next_t, 1.0)) next_v = spline.evaluate(next_t);
   }

   if(!is_between()) return dNAN;

   // Okay now we have to do a binary search to find 't'
   struct S
   {
      S()
          : y(dNAN)
          , t(dNAN)
      {}
      S(double y_, double t_)
          : y(y_)
          , t(t_)
      {}
      double y;
      double t;
   };
   S upper(last_v.y, last_t);
   S lower(next_v.y, next_t);
   if(upper.y < lower.y) { // swap
      S tmp(upper);
      upper = lower;
      lower = tmp;
   }

   double error = 1.0;
   uint counter = 0;
   S mid;
   while(error > epsilon) {
      if(counter++ > 100) FATAL(format("Failed to find 't' value!"));

      mid.t = 0.5 * (upper.t + lower.t);
      mid.y = spline.evaluate(mid.t).y;
      error = fabs(mid.y - y_value);

      bool go_low = inclusive_between(lower.y, y_value, mid.y)
                    || inclusive_between(mid.y, y_value, lower.y);
      bool go_high = inclusive_between(upper.y, y_value, mid.y)
                     || inclusive_between(mid.y, y_value, upper.y);
      if((go_low && go_high) || (!go_low && !go_high))
         LOG_ERR(format("t escaped"));

      if(go_low) upper = mid;
      if(go_high) lower = mid;
   }
   // INFO(format("counter = {}") % counter);
   return mid.t;
}

// --------------------------------------------------------------- Intersections

Vector2 spline_spline_intersection(const Spline2d& a,
                                   const Spline2d& b,
                                   const real threshold) // pixels
{
   // Have to find two 't' values (ta, tb) such that
   // a.evaluate(ta) == b.evaluate(tb)

   array<double, 3> tas, tbs;
   array<Vector2, 3> vas, vbs;

   // This _could_ be more efficient: two*golden-section at worst
   // However, 'intersections' is not the bottle-neck for this
   // cost-function. (Refitting the splines is.)
   auto update = [&](double ta, double tb, double range) {
      tas[0] = clamp(ta - 0.5 * range, 0.0, 1.0);
      tas[1] = ta;
      tas[2] = clamp(ta + 0.5 * range, 0.0, 1.0);
      tbs[0] = clamp(tb - 0.5 * range, 0.0, 1.0);
      tbs[1] = tb;
      tbs[2] = clamp(tb + 0.5 * range, 0.0, 1.0);
      for(unsigned i = 0; i < tas.size(); ++i) {
         vas[i] = a.evaluate(tas[i]);
         vbs[i] = b.evaluate(tbs[i]);
      }

      // which are the closest?
      unsigned best_i{0}, best_j{0};
      real best_dist = std::numeric_limits<real>::max();
      for(unsigned i = 0; i < tas.size(); ++i) {
         for(unsigned j = 0; j < tbs.size(); ++j) {
            auto dist = vas[i].quadrance(vbs[j]);
            if(dist < best_dist) {
               best_dist = dist;
               best_i    = i;
               best_j    = j;
            }
         }
      }

      auto ret = Vector3(tas[best_i], tbs[best_j], best_dist);

      if(false) {
         INFO("REPORT");
         cout << format("tas = [{}]", implode(tas.begin(), tas.end(), ", "))
              << endl;
         cout << format("tbs = [{}]", implode(tbs.begin(), tbs.end(), ", "))
              << endl;
         cout << format("vas = [{}]", implode(vas.begin(), vas.end(), ", "))
              << endl;
         cout << format("vbs = [{}]", implode(vbs.begin(), vbs.end(), ", "))
              << endl;
         cout << format("ret = {}", str(ret)) << endl;

         fgetc(stdin);
      }

      return ret;
   };

   double range = 1.0;
   double ta{0.0}, tb{0.0};
   Vector3 val(0.5, 0.5, std::numeric_limits<real>::max());
   unsigned counter = 0;
   while(val.z > (threshold * threshold)) {
      if(counter++ > 100) { break; }
      val = update(val.x, val.y, range);
      range *= 0.5;
   }

   return 0.5 * (a.evaluate(val.x) + b.evaluate(val.y));
}

} // namespace perceive


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_NEXTPOW2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_NEXTPOW2_HPP_INCLUDED

#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/abss.hpp>
#include <boost/simd/function/scalar/ffs.hpp>
#include <boost/simd/function/scalar/frexp.hpp>
#include <boost/simd/function/scalar/minusone.hpp>
#include <boost/simd/function/scalar/reversebits.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( nextpow2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_ < bd::unsigned_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      if (!a0) return a0;
      return sizeof(A0)*8-ffs(reversebits(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( nextpow2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_ < bd::signed_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      if (!a0) return a0;
      return sizeof(A0)*8-ffs(reversebits(abss(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( nextpow2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_ < bd::floating_<A0> >
                          )
  {
    using result_t = bd::as_integer_t<A0, signed>;
    BOOST_FORCEINLINE result_t operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      A0 m;
      result_t p;
      simd::frexp(simd::abs(a0), m, p);
      return (m == Half<A0>())  ? minusone(p) :  p;
    }
  };
} } }

#endif


/**
 * @file tests/recurrent_network_test.cpp
 * @author Marcus Edel
 *
 * Tests the recurrent network.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <ensmallen.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/loss_functions/mean_squared_error.hpp>
#include <mlpack/methods/ann/rnn.hpp>
#include <mlpack/methods/ann/brnn.hpp>
#include <mlpack/core/data/binarize.hpp>
#include <mlpack/core/math/random.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"
#include "serialization.hpp"
#include "custom_layer.hpp"

using namespace mlpack;
using namespace mlpack::ann;
using namespace ens;
using namespace mlpack::math;

BOOST_AUTO_TEST_SUITE(RecurrentNetworkTest);

/**
 * Construct a 2-class dataset out of noisy sines.
 *
 * @param data Input data used to store the noisy sines.
 * @param labels Labels used to store the target class of the noisy sines.
 * @param points Number of points/features in a single sequence.
 * @param sequences Number of sequences for each class.
 * @param noise The noise factor that influences the sines.
 */
void GenerateNoisySines(arma::cube& data,
                        arma::mat& labels,
                        const size_t points,
                        const size_t sequences,
                        const double noise = 0.3)
{
  arma::colvec x =  arma::linspace<arma::colvec>(0, points - 1, points) /
      points * 20.0;
  arma::colvec y1 = arma::sin(x + arma::as_scalar(arma::randu(1)) * 3.0);
  arma::colvec y2 = arma::sin(x / 2.0 + arma::as_scalar(arma::randu(1)) * 3.0);

  data = arma::zeros(1 /* single dimension */, sequences * 2, points);
  labels = arma::zeros(2 /* 2 classes */, sequences * 2);

  for (size_t seq = 0; seq < sequences; seq++)
  {
    arma::vec sequence = arma::randu(points) * noise + y1 +
        arma::as_scalar(arma::randu(1) - 0.5) * noise;
    for (size_t i = 0; i < points; ++i)
      data(0, seq, i) = sequence[i];

    labels(0, seq) = 1;

    sequence = arma::randu(points) * noise + y2 +
        arma::as_scalar(arma::randu(1) - 0.5) * noise;
    for (size_t i = 0; i < points; ++i)
      data(0, sequences + seq, i) = sequence[i];

    labels(1, sequences + seq) = 1;
  }
}

/**
 * Train the BRNN on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(SequenceClassificationBRNNTest)
{
  // Using same test for RNN below.
  size_t successes = 0;
  const size_t rho = 10;

  for (size_t trial = 0; trial < 6; ++trial)
  {
    // Generate 12 (2 * 6) noisy sines. A single sine contains rho
    // points/features.
    arma::cube input;
    arma::mat labelsTemp;
    GenerateNoisySines(input, labelsTemp, rho, 6);

    arma::cube labels = arma::zeros<arma::cube>(1, labelsTemp.n_cols, rho);
    for (size_t i = 0; i < labelsTemp.n_cols; ++i)
    {
      const int value = arma::as_scalar(arma::find(
          arma::max(labelsTemp.col(i)) == labelsTemp.col(i), 1)) + 1;
      labels.tube(0, i).fill(value);
    }

    Add<> add(4);
    Linear<> lookup(1, 4);
    SigmoidLayer<> sigmoidLayer;
    Linear<> linear(4, 4);
    Recurrent<>* recurrent = new Recurrent<>(
        add, lookup, linear, sigmoidLayer, rho);

    BRNN<> model(rho);
    model.Add<IdentityLayer<> >();
    model.Add(recurrent);
    model.Add<Linear<> >(4, 5);

    StandardSGD opt(0.1, 1, 500 * input.n_cols, -100);
    model.Train(input, labels, opt);
    BOOST_TEST_CHECKPOINT("Training over");
    arma::cube prediction;
    model.Predict(input, prediction);
    BOOST_TEST_CHECKPOINT("Prediction over");

    size_t error = 0;
    for (size_t i = 0; i < prediction.n_cols; ++i)
    {
      const int predictionValue = arma::as_scalar(arma::find(
          arma::max(prediction.slice(rho - 1).col(i)) ==
          prediction.slice(rho - 1).col(i), 1) + 1);

      const int targetValue = arma::as_scalar(arma::find(
          arma::max(labelsTemp.col(i)) == labelsTemp.col(i), 1)) + 1;

      if (predictionValue == targetValue)
      {
        error++;
      }
    }

    double classificationError = 1 - double(error) / prediction.n_cols;
    BOOST_TEST_CHECKPOINT(classificationError);
    if (classificationError <= 0.2)
    {
      ++successes;
      break;
    }
  }

  BOOST_REQUIRE_GE(successes, 1);
}

/**
 * Train the vanilla network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(SequenceClassificationTest)
{
  // It isn't guaranteed that the recurrent network will converge in the
  // specified number of iterations using random weights. If this works 1 of 6
  // times, I'm fine with that. All I want to know is that the network is able
  // to escape from local minima and to solve the task.
  size_t successes = 0;
  const size_t rho = 10;

  for (size_t trial = 0; trial < 6; ++trial)
  {
    // Generate 12 (2 * 6) noisy sines. A single sine contains rho
    // points/features.
    arma::cube input;
    arma::mat labelsTemp;
    GenerateNoisySines(input, labelsTemp, rho, 6);

    arma::cube labels = arma::zeros<arma::cube>(1, labelsTemp.n_cols, rho);
    for (size_t i = 0; i < labelsTemp.n_cols; ++i)
    {
      const int value = arma::as_scalar(arma::find(
          arma::max(labelsTemp.col(i)) == labelsTemp.col(i), 1)) + 1;
      labels.tube(0, i).fill(value);
    }

    /**
     * Construct a network with 1 input unit, 4 hidden units and 10 output
     * units. The hidden layer is connected to itself. The network structure
     * looks like:
     *
     *  Input         Hidden        Output
     * Layer(1)      Layer(4)      Layer(10)
     * +-----+       +-----+       +-----+
     * |     |       |     |       |     |
     * |     +------>|     +------>|     |
     * |     |    ..>|     |       |     |
     * +-----+    .  +--+--+       +-----+
     *            .     .
     *            .     .
     *            .......
     */
    Add<> add(4);
    Linear<> lookup(1, 4);
    SigmoidLayer<> sigmoidLayer;
    Linear<> linear(4, 4);
    Recurrent<>* recurrent = new Recurrent<>(
        add, lookup, linear, sigmoidLayer, rho);

    RNN<> model(rho);
    model.Add<IdentityLayer<> >();
    model.Add(recurrent);
    model.Add<Linear<> >(4, 10);
    model.Add<LogSoftMax<> >();

    StandardSGD opt(0.1, 1, 500 * input.n_cols, -100);
    model.Train(input, labels, opt);

    arma::cube prediction;
    model.Predict(input, prediction);

    size_t error = 0;
    for (size_t i = 0; i < prediction.n_cols; ++i)
    {
      const int predictionValue = arma::as_scalar(arma::find(
          arma::max(prediction.slice(rho - 1).col(i)) ==
          prediction.slice(rho - 1).col(i), 1) + 1);

      const int targetValue = arma::as_scalar(arma::find(
          arma::max(labelsTemp.col(i)) == labelsTemp.col(i), 1)) + 1;

      if (predictionValue == targetValue)
      {
        error++;
      }
    }

    double classificationError = 1 - double(error) / prediction.n_cols;
    if (classificationError <= 0.2)
    {
      ++successes;
      break;
    }
  }

  BOOST_REQUIRE_GE(successes, 1);
}

/**
 * Generate a random Reber grammar.
 *
 * For more information, see the following thesis.
 *
 * @code
 * @misc{Gers2001,
 *   author = {Felix Gers},
 *   title = {Long Short-Term Memory in Recurrent Neural Networks},
 *   year = {2001}
 * }
 * @endcode
 *
 * @param transitions Reber grammar transition matrix.
 * @param reber The generated Reber grammar string.
 */
void GenerateReber(const arma::Mat<char>& transitions, std::string& reber)
{
  size_t idx = 0;
  reber = "B";

  do
  {
    const int grammerIdx = rand() % 2;
    reber += arma::as_scalar(transitions.submat(idx, grammerIdx, idx,
        grammerIdx));

    idx = arma::as_scalar(transitions.submat(idx, grammerIdx + 2, idx,
        grammerIdx + 2)) - '0';
  } while (idx != 0);
}

/**
 * Generate a random recursive Reber grammar.
 *
 * @param transitions Recursive Reber grammar transition matrix.
 * @param averageRecursion Average recursive depth of the reber grammar.
 * @param maxRecursion Maximum recursive depth of reber grammar.
 * @param reber The generated embedded Reber grammar string.
 * @param addEnd Add ending 'E' to the generated grammar.
 */
void GenerateRecursiveReber(const arma::Mat<char>& transitions,
                            size_t averageRecursion,
                            size_t maxRecursion,
                            std::string& reber,
                            bool addEnd = true)
{
  char c = (rand() % averageRecursion) == 1 ? 'P' : 'T';

  if (maxRecursion == 1 || c == 'T')
  {
    c = 'T';
    GenerateReber(transitions, reber);
  }
  else
  {
    GenerateRecursiveReber(transitions, averageRecursion, --maxRecursion,
        reber, false);
  }

  reber = c + reber + c;

  if (addEnd)
  {
    reber = "B" + reber + "E";
  }
}

/**
 * Convert a unit vector to a Reber symbol.
 *
 * @param translation The unit vector to be converted.
 * @param symbol The converted unit vector stored as Reber symbol.
 */
template<typename MatType>
void ReberReverseTranslation(const MatType& translation, char& symbol)
{
  arma::Col<char> symbols;
  symbols << 'B' << 'T' << 'S' << 'X' << 'P' << 'V' << 'E' << arma::endr;
  const int idx = arma::as_scalar(arma::find(translation == 1, 1, "first"));

  symbol = symbols(idx);
}

/**
 * Convert a Reber symbol to a unit vector.
 *
 * @param symbol Reber symbol to be converted.
 * @param translation The converted symbol stored as unit vector.
 */
void ReberTranslation(const char symbol, arma::colvec& translation)
{
  arma::Col<char> symbols;
  symbols << 'B' << 'T' << 'S' << 'X' << 'P' << 'V' << 'E' << arma::endr;
  const int idx = arma::as_scalar(arma::find(symbols == symbol, 1, "first"));

  translation = arma::zeros<arma::colvec>(7);
  translation(idx) = 1;
}

/**
 * Given a Reber string, return a Reber string with all reachable next symbols.
 *
 * @param transitions The Reber transistion matrix.
 * @param reber The Reber string used to generate all reachable next symbols.
 * @param nextReber All reachable next symbols.
 */
void GenerateNextReber(const arma::Mat<char>& transitions,
                       const std::string& reber, std::string& nextReber)
{
  size_t idx = 0;

  for (size_t grammer = 1; grammer < reber.length(); grammer++)
  {
    const int grammerIdx = arma::as_scalar(arma::find(
        transitions.row(idx) == reber[grammer], 1, "first"));

    idx = arma::as_scalar(transitions.submat(idx, grammerIdx + 2, idx,
        grammerIdx + 2)) - '0';
  }

  nextReber = arma::as_scalar(transitions.submat(idx, 0, idx, 0));
  nextReber += arma::as_scalar(transitions.submat(idx, 1, idx, 1));
}

/**
 * Given a recursive Reber string, return a Reber string with all
 * reachable next symbols.
 *
 * @param transitions The Reber transistion matrix.
 * @param reber The Reber string used to generate all reachable next symbols.
 * @param nextReber All reachable next symbols.
 */
void GenerateNextRecursiveReber(const arma::Mat<char>& transitions,
                                const std::string& reber,
                                std::string& nextReber)
{
  size_t state = 0;
  size_t numPs = 0;

  for (size_t cIndex = 0; cIndex < reber.length(); cIndex++)
  {
    char c = reber[cIndex];

    if (c == 'B' && state == 0)
    {
      state = 1;
    }
    else if (c == 'P' && state == 1)
    {
      numPs++;
      state = 1;
    }
    else if (c == 'T' && state == 1)
    {
      state = 2;
    }
    else if (c == 'B' && state == 2)
    {
      size_t pos = reber.find('E');
      if (pos != std::string::npos)
      {
        cIndex = pos;
        state = 4;
      }
      else
      {
        GenerateNextReber(transitions, reber.substr(cIndex), nextReber);
        state = 3;
      }
    }
    else if (c == 'T' && state == 4)
    {
      state = 5;
    }
    else if (c == 'P' && state == 5)
    {
      numPs--;
      state = 5;
    }
  }

  if (state == 0 || state == 2)
  {
    nextReber = "B";
  }
  else if (state == 1)
  {
    nextReber = "PT";
  }
  else if (state == 4)
  {
    nextReber = "T";
  }
  else if (state == 5)
  {
    if (numPs == 0)
    {
      nextReber = "E";
    }
    else
    {
      nextReber = "P";
    }
  }
}

/**
 * @brief Creates the reber grammar data for tests.
 *
 * @param trainInput The train data
 * @param trainLabels The train labels
 * @param testInput The test input
 * @param recursive whether recursive Reber
 * @param trainReberGrammarCount The number of training set
 * @param testReberGrammarCount The number of test set
 * @param averageRecursion Average recursion
 * @param maxRecursion Max recursion
 * @return arma::Mat<char> The Reber state translation to be used.
 */
arma::Mat<char> GenerateReberGrammarData(
                              arma::field<arma::mat>& trainInput,
                              arma::field<arma::mat>& trainLabels,
                              arma::field<arma::mat>& testInput,
                              bool recursive = false,
                              const size_t trainReberGrammarCount = 700,
                              const size_t testReberGrammarCount = 250,
                              const size_t averageRecursion = 3,
                              const size_t maxRecursion = 5)
{
  // Reber state transition matrix. (The last two columns are the indices to the
  // next path).
  arma::Mat<char> transitions;
  transitions << 'T' << 'P' << '1' << '2' << arma::endr
              << 'X' << 'S' << '3' << '1' << arma::endr
              << 'V' << 'T' << '4' << '2' << arma::endr
              << 'X' << 'S' << '2' << '5' << arma::endr
              << 'P' << 'V' << '3' << '5' << arma::endr
              << 'E' << 'E' << '0' << '0' << arma::endr;

  std::string trainReber, testReber;

  arma::colvec translation;

  // Generate the training data.
  for (size_t i = 0; i < trainReberGrammarCount; ++i)
  {
    if (recursive)
      GenerateRecursiveReber(transitions, 3, 5, trainReber);
    else
      GenerateReber(transitions, trainReber);

    for (size_t j = 0; j < trainReber.length() - 1; ++j)
    {
      ReberTranslation(trainReber[j], translation);
      trainInput(0, i) = arma::join_cols(trainInput(0, i), translation);

      ReberTranslation(trainReber[j + 1], translation);
      trainLabels(0, i) = arma::join_cols(trainLabels(0, i), translation);
    }
  }

  // Generate the test data.
  for (size_t i = 0; i < testReberGrammarCount; ++i)
  {
    if (recursive)
      GenerateRecursiveReber(transitions, averageRecursion, maxRecursion,
          testReber);
    else
      GenerateReber(transitions, testReber);

    for (size_t j = 0; j < testReber.length() - 1; ++j)
    {
      ReberTranslation(testReber[j], translation);
      testInput(0, i) = arma::join_cols(testInput(0, i), translation);
    }
  }

  return transitions;
}

/**
 * Train the specified network and the construct a Reber grammar dataset.
 */
template<typename ModelType>
void ReberGrammarTestNetwork(ModelType& model,
                             const bool recursive = false,
                             const size_t averageRecursion = 3,
                             const size_t maxRecursion = 5,
                             const size_t iterations = 10,
                             const size_t trials = 5)
{
  const size_t trainReberGrammarCount = 700;
  const size_t testReberGrammarCount = 250;

  arma::field<arma::mat> trainInput(1, trainReberGrammarCount);
  arma::field<arma::mat> trainLabels(1, trainReberGrammarCount);
  arma::field<arma::mat> testInput(1, testReberGrammarCount);

  arma::Mat<char> transitions =
                  GenerateReberGrammarData(trainInput,
                                           trainLabels,
                                           testInput,
                                           recursive,
                                           trainReberGrammarCount,
                                           testReberGrammarCount,
                                           averageRecursion,
                                           maxRecursion);

  /*
   * Construct a network with 7 input units, layerSize hidden units and 7 output
   * units. The hidden layer is connected to itself. The network structure looks
   * like:
   *
   *  Input         Hidden        Output
   * Layer(7)  Layer(layerSize)   Layer(7)
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |    ..>|     |       |     |
   * +-----+    .  +--+--+       +-- ---+
   *            .     .
   *            .     .
   *            .......
   */
  // It isn't guaranteed that the recurrent network will converge in the
  // specified number of iterations using random weights. If this works 1 of 5
  // times, I'm fine with that. All I want to know is that the network is able
  // to escape from local minima and to solve the task.
  size_t successes = 0;
  size_t offset = 0;
  const size_t inputSize = 7;
  for (size_t trial = 0; trial < trials; ++trial)
  {
    // Reset model before using for next trial.
    model.Reset();
    MomentumSGD opt(0.06, 50, 2, -50000);

    arma::cube inputTemp, labelsTemp;
    for (size_t iteration = 0; iteration < (iterations + offset); iteration++)
    {
      for (size_t j = 0; j < trainReberGrammarCount; ++j)
      {
        // Each sequence may be a different length, so we need to extract them
        // manually.  We will reshape them into a cube with each slice equal to
        // a time step.
        inputTemp = arma::cube(trainInput.at(0, j).memptr(), inputSize, 1,
            trainInput.at(0, j).n_elem / inputSize, false, true);
        labelsTemp = arma::cube(trainLabels.at(0, j).memptr(), inputSize, 1,
            trainInput.at(0, j).n_elem / inputSize, false, true);

        model.Rho() = inputTemp.n_elem / inputSize;
        model.Train(inputTemp, labelsTemp, opt);
        opt.ResetPolicy() = false;
      }
    }

    double error = 0;

    // Ask the network to predict the next Reber grammar in the given sequence.
    for (size_t i = 0; i < testReberGrammarCount; ++i)
    {
      arma::cube prediction;
      arma::cube input(testInput.at(0, i).memptr(), inputSize, 1,
          testInput.at(0, i).n_elem / inputSize, false, true);

      model.Rho() = input.n_elem / inputSize;
      model.Predict(input, prediction);

      const size_t reberGrammerSize = 7;
      std::string inputReber = "";

      size_t reberError = 0;

      for (size_t j = 0; j < (prediction.n_elem / reberGrammerSize); ++j)
      {
        char predictedSymbol, inputSymbol;
        std::string reberChoices;

        arma::umat output = (prediction.slice(j) == (arma::ones(
            reberGrammerSize, 1) *
            arma::as_scalar(arma::max(prediction.slice(j)))));

        ReberReverseTranslation(output, predictedSymbol);
        ReberReverseTranslation(input.slice(j), inputSymbol);
        inputReber += inputSymbol;

        if (recursive)
          GenerateNextRecursiveReber(transitions, inputReber, reberChoices);
        else
          GenerateNextReber(transitions, inputReber, reberChoices);

        if (reberChoices.find(predictedSymbol) != std::string::npos)
          reberError++;
      }

      if (reberError != (prediction.n_elem / reberGrammerSize))
        error += 1;
    }

    error /= testReberGrammarCount;
    if (error <= 0.3)
    {
      ++successes;
      break;
    }

    offset += 3;
  }

  BOOST_REQUIRE_GE(successes, 1);
}

/**
 * Train the specified networks on an embedded Reber grammar dataset.
 */
BOOST_AUTO_TEST_CASE(LSTMReberGrammarTest)
{
  RNN<MeanSquaredError<> > model(5);
  model.Add<Linear<> >(7, 10);
  model.Add<LSTM<> >(10, 10);
  model.Add<Linear<> >(10, 7);
  model.Add<SigmoidLayer<> >();
  ReberGrammarTestNetwork(model, false);
}

/**
 * Train the specified networks on an embedded Reber grammar dataset.
 */
BOOST_AUTO_TEST_CASE(FastLSTMReberGrammarTest)
{
  RNN<MeanSquaredError<> > model(5);
  model.Add<Linear<> >(7, 8);
  model.Add<FastLSTM<> >(8, 8);
  model.Add<Linear<> >(8, 7);
  model.Add<SigmoidLayer<> >();
  ReberGrammarTestNetwork(model, false);
}

/**
 * Train the specified networks on an embedded Reber grammar dataset.
 */
BOOST_AUTO_TEST_CASE(GRURecursiveReberGrammarTest)
{
  RNN<MeanSquaredError<> > model(5);
  model.Add<Linear<> >(7, 16);
  model.Add<GRU<> >(16, 16);
  model.Add<Linear<> >(16, 7);
  model.Add<SigmoidLayer<> >();
  ReberGrammarTestNetwork(model, true, 3, 5, 10, 7);
}

/**
 * Train BLSTM on an embedded Reber grammar dataset.
 */
BOOST_AUTO_TEST_CASE(BRNNReberGrammarTest)
{
  BRNN<MeanSquaredError<>, AddMerge<>, SigmoidLayer<> > model(5);
  model.Add<Linear<> >(7, 10);
  model.Add<LSTM<> >(10, 10);
  model.Add<Linear<> >(10, 7);
  ReberGrammarTestNetwork(model, false, 3, 5, 1);
}

/*
 * This sample is a simplified version of Derek D. Monner's Distracted Sequence
 * Recall task, which involves 10 symbols:
 *
 * Targets: must be recognized and remembered by the network.
 * Distractors: never need to be remembered.
 * Prompts: direct the network to give an answer.
 *
 * A single trial consists of a temporal sequence of 10 input symbols. The first
 * 8 consist of 2 randomly chosen target symbols and 6 randomly chosen
 * distractor symbols in an random order. The remaining two symbols are two
 * prompts, which direct the network to produce the first and second target in
 * the sequence, in order.
 *
 * For more information, see the following paper.
 *
 * @code
 * @misc{Monner2012,
 *   author = {Monner, Derek and Reggia, James A},
 *   title = {A generalized LSTM-like training algorithm for second-order
 *   recurrent neural networks},
 *   year = {2012}
 * }
 * @endcode
 *
 * @param input The generated input sequence.
 * @param input The generated output sequence.
 */
void GenerateDistractedSequence(arma::mat& input, arma::mat& output)
{
  input = arma::zeros<arma::mat>(10, 10);
  output = arma::zeros<arma::mat>(3, 10);

  arma::uvec index = arma::shuffle(arma::linspace<arma::uvec>(0, 7, 8));

  // Set the target in the input sequence and the corresponding targets in the
  // output sequence by following the correct order.
  for (size_t i = 0; i < 2; ++i)
  {
    size_t idx = rand() % 2;
    input(idx, index(i)) = 1;
    output(idx, index(i) > index(i == 0) ? 9 : 8) = 1;
  }

  for (size_t i = 2; i < 8; ++i)
    input(2 + rand() % 6, index(i)) = 1;

  // Set the prompts which direct the network to give an answer.
  input(8, 8) = 1;
  input(9, 9) = 1;

  input.reshape(input.n_elem, 1);
  output.reshape(output.n_elem, 1);
}

/**
 * Train the specified network and the construct distracted sequence recall
 * dataset.
 */
template<typename RecurrentLayerType>
void DistractedSequenceRecallTestNetwork(
    const size_t cellSize, const size_t hiddenSize)
{
  const size_t trainDistractedSequenceCount = 600;
  const size_t testDistractedSequenceCount = 300;

  arma::field<arma::mat> trainInput(1, trainDistractedSequenceCount);
  arma::field<arma::mat> trainLabels(1, trainDistractedSequenceCount);
  arma::field<arma::mat> testInput(1, testDistractedSequenceCount);
  arma::field<arma::mat> testLabels(1, testDistractedSequenceCount);

  // Generate the training data.
  for (size_t i = 0; i < trainDistractedSequenceCount; ++i)
    GenerateDistractedSequence(trainInput(0, i), trainLabels(0, i));

  // Generate the test data.
  for (size_t i = 0; i < testDistractedSequenceCount; ++i)
    GenerateDistractedSequence(testInput(0, i), testLabels(0, i));

  /*
   * Construct a network with 10 input units, layerSize hidden units and 3
   * output units. The hidden layer is connected to itself. The network
   * structure looks like:
   *
   *  Input        Recurrent      Hidden       Output
   * Layer(10)  Layer(cellSize)   Layer(3)     Layer(3)
   * +-----+       +-----+       +-----+       +-----+
   * |     |       |     |       |     |       |     |
   * |     +------>|     +------>|     |------>|     |
   * |     |    ..>|     |       |     |       |     |
   * +-----+    .  +--+--+       +-----+       +-----+
   *            .     .
   *            .     .
   *            .......
   */
  const size_t outputSize = 3;
  const size_t inputSize = 10;
  const size_t rho = trainInput.at(0, 0).n_elem / inputSize;

  // It isn't guaranteed that the recurrent network will converge in the
  // specified number of iterations using random weights. If this works 1 of 5
  // times, I'm fine with that. All I want to know is that the network is able
  // to escape from local minima and to solve the task.
  size_t successes = 0;
  size_t offset = 0;
  for (size_t trial = 0; trial < 5; ++trial)
  {
    RNN<MeanSquaredError<> > model(rho);
    model.Add<IdentityLayer<> >();
    model.Add<Linear<> >(inputSize, cellSize);
    model.Add<RecurrentLayerType>(cellSize, hiddenSize);
    model.Add<Linear<> >(hiddenSize, outputSize);
    model.Add<SigmoidLayer<> >();

    StandardSGD opt(0.1, 50, 2, -50000);

    // We increase the number of iterations (training) if the first run didn't
    // pass.
    arma::cube inputTemp, labelsTemp;
    for (size_t iteration = 0; iteration < (9 + offset); iteration++)
    {
      for (size_t j = 0; j < trainDistractedSequenceCount; ++j)
      {
        inputTemp = arma::cube(trainInput.at(0, j).memptr(), inputSize, 1,
            trainInput.at(0, j).n_elem / inputSize, false, true);
        labelsTemp = arma::cube(trainLabels.at(0, j).memptr(), outputSize, 1,
            trainLabels.at(0, j).n_elem / outputSize, false, true);

        model.Train(inputTemp, labelsTemp, opt);
      }
    }

    double error = 0;

    // Ask the network to predict the targets in the given sequence at the
    // prompts.
    for (size_t i = 0; i < testDistractedSequenceCount; ++i)
    {
      arma::cube output;
      arma::cube input(testInput.at(0, i).memptr(), inputSize, 1,
          testInput.at(0, i).n_elem / inputSize, false, true);

      model.Predict(input, output);
      for (size_t j = 0; j < output.n_slices; ++j)
      {
        arma::mat outputSlice = output.slice(j);
        data::Binarize(outputSlice, outputSlice, 0.5);
        output.slice(j) = outputSlice;
      }

      arma::cube label(testLabels.at(0, i).memptr(), outputSize, 1,
          testLabels.at(0, i).n_elem / outputSize, false, true);
      if (arma::accu(arma::abs(label - output)) != 0)
        error += 1;
    }

    error /= testDistractedSequenceCount;
    // Can we reproduce the results from the paper. They provide an 95% accuracy
    // on a test set of 1000 randomly selected sequences.
    // Ensure that this is within tolerance, which is at least as good as the
    // paper's results (plus a little bit for noise).
    if (error <= 0.3)
    {
      ++successes;
      break;
    }

    offset += 2;
  }

  BOOST_REQUIRE_GE(successes, 1);
}

/**
 * Train the specified networks on the Derek D. Monner's distracted sequence
 * recall task.
 */
BOOST_AUTO_TEST_CASE(LSTMDistractedSequenceRecallTest)
{
  DistractedSequenceRecallTestNetwork<LSTM<> >(4, 8);
}

/**
 * Train the specified networks on the Derek D. Monner's distracted sequence
 * recall task.
 */
BOOST_AUTO_TEST_CASE(FastLSTMDistractedSequenceRecallTest)
{
  DistractedSequenceRecallTestNetwork<FastLSTM<> >(4, 8);
}

/**
 * Train the specified networks on the Derek D. Monner's distracted sequence
 * recall task.
 */
BOOST_AUTO_TEST_CASE(GRUDistractedSequenceRecallTest)
{
  DistractedSequenceRecallTestNetwork<GRU<> >(4, 8);
}

/**
 * Create a simple recurrent neural network for the noisy sines task, and
 * require that it produces the exact same network for a few batch sizes.
 */
template<typename RecurrentLayerType>
void BatchSizeTest()
{
  const size_t rho = 10;

  // Generate 12 (2 * 6) noisy sines. A single sine contains rho
  // points/features.
  arma::cube input;
  arma::mat labelsTemp;
  GenerateNoisySines(input, labelsTemp, rho, 6);

  arma::cube labels = arma::zeros<arma::cube>(1, labelsTemp.n_cols, rho);
  for (size_t i = 0; i < labelsTemp.n_cols; ++i)
  {
    const int value = arma::as_scalar(arma::find(
        arma::max(labelsTemp.col(i)) == labelsTemp.col(i), 1)) + 1;
    labels.tube(0, i).fill(value);
  }

  RNN<> model(rho);
  model.Add<Linear<>>(1, 10);
  model.Add<SigmoidLayer<>>();
  model.Add<RecurrentLayerType>(10, 10);
  model.Add<SigmoidLayer<>>();
  model.Add<Linear<>>(10, 10);
  model.Add<SigmoidLayer<>>();

  model.Reset();
  arma::mat initParams = model.Parameters();

  StandardSGD opt(1e-5, 1, 5, -100, false);
  model.Train(input, labels, opt);

  // This is trained with one point.
  arma::mat outputParams = model.Parameters();

  model.Reset();
  model.Parameters() = initParams;
  opt.BatchSize() = 2;
  model.Train(input, labels, opt);

  CheckMatrices(outputParams, model.Parameters(), 1);

  model.Parameters() = initParams;
  opt.BatchSize() = 5;
  model.Train(input, labels, opt);

  CheckMatrices(outputParams, model.Parameters(), 1);
}

/**
 * Ensure LSTMs work with larger batch sizes.
 */
BOOST_AUTO_TEST_CASE(LSTMBatchSizeTest)
{
  BatchSizeTest<LSTM<>>();
}

/**
 * Ensure fast LSTMs work with larger batch sizes.
 */
BOOST_AUTO_TEST_CASE(FastLSTMBatchSizeTest)
{
  BatchSizeTest<FastLSTM<>>();
}

/**
 * Ensure GRUs work with larger batch sizes.
 */
BOOST_AUTO_TEST_CASE(GRUBatchSizeTest)
{
  BatchSizeTest<GRU<>>();
}

/**
 * Make sure the RNN can be properly serialized.
 */
BOOST_AUTO_TEST_CASE(SerializationTest)
{
  const size_t rho = 10;

  // Generate 12 (2 * 6) noisy sines. A single sine contains rho
  // points/features.
  arma::cube input;
  arma::mat labelsTemp;
  GenerateNoisySines(input, labelsTemp, rho, 6);

  arma::cube labels = arma::zeros<arma::cube>(1, labelsTemp.n_cols, rho);
  for (size_t i = 0; i < labelsTemp.n_cols; ++i)
  {
    const int value = arma::as_scalar(arma::find(
        arma::max(labelsTemp.col(i)) == labelsTemp.col(i), 1)) + 1;
    labels.tube(0, i).fill(value);
  }

  /**
   * Construct a network with 1 input unit, 4 hidden units and 10 output
   * units. The hidden layer is connected to itself. The network structure
   * looks like:
   *
   *  Input         Hidden        Output
   * Layer(1)      Layer(4)      Layer(10)
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |    ..>|     |       |     |
   * +-----+    .  +--+--+       +-----+
   *            .     .
   *            .     .
   *            .......
   */
  Add<> add(4);
  Linear<> lookup(1, 4);
  SigmoidLayer<> sigmoidLayer;
  Linear<> linear(4, 4);
  Recurrent<>* recurrent = new Recurrent<>(add, lookup, linear,
      sigmoidLayer, rho);

  RNN<> model(rho);
  model.Add<IdentityLayer<> >();
  model.Add(recurrent);
  model.Add<Linear<> >(4, 10);
  model.Add<LogSoftMax<> >();

  StandardSGD opt(0.1, 1, input.n_cols /* 1 epoch */, -100);
  model.Train(input, labels, opt);

  // Serialize the network.
  RNN<> xmlModel(1), textModel(3), binaryModel(5);
  SerializeObjectAll(model, xmlModel, textModel, binaryModel);

  // Take predictions, check the output.
  arma::cube prediction, xmlPrediction, textPrediction, binaryPrediction;
  model.Predict(input, prediction);
  xmlModel.Predict(input, xmlPrediction);
  textModel.Predict(input, textPrediction);
  binaryModel.Predict(input, binaryPrediction);

  CheckMatrices(prediction, xmlPrediction, textPrediction, binaryPrediction);
}

/**
 * Test RNN with a custom layer.
 */
void ReberGrammarTestCustomNetwork(const size_t hiddenSize = 4,
                                   const bool recursive = false,
                                   const size_t iterations = 10)
{
  const size_t trainReberGrammarCount = 700;
  const size_t testReberGrammarCount = 250;

  arma::field<arma::mat> trainInput(1, trainReberGrammarCount);
  arma::field<arma::mat> trainLabels(1, trainReberGrammarCount);
  arma::field<arma::mat> testInput(1, testReberGrammarCount);

  arma::Mat<char> transitions =
                  GenerateReberGrammarData(trainInput,
                                           trainLabels,
                                           testInput,
                                           recursive,
                                           trainReberGrammarCount,
                                           testReberGrammarCount);

  /*
   * Construct a network with 7 input units, layerSize hidden units and 7 output
   * units. The hidden layer is connected to itself. The network structure looks
   * like:
   *
   *  Input         Hidden        Output
   * Layer(7)  Layer(layerSize)   Layer(7)
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |    ..>|     |       |     |
   * +-----+    .  +--+--+       +-- ---+
   *            .     .
   *            .     .
   *            .......
   */
  // It isn't guaranteed that the recurrent network will converge in the
  // specified number of iterations using random weights. If this works 1 of 10
  // times, I'm fine with that. All I want to know is that the network is able
  // to escape from local minima and to solve the task.
  size_t successes = 0;
  size_t offset = 0;
  for (size_t trial = 0; trial < 10; ++trial)
  {
    const size_t outputSize = 7;
    const size_t inputSize = 7;

    RNN<MeanSquaredError<>, RandomInitialization, CustomLayer<> > model(5);
    model.Add<Linear<> >(inputSize, hiddenSize);
    model.Add<GRU<> >(hiddenSize, hiddenSize);
    model.Add<Linear<> >(hiddenSize, outputSize);
    model.Add<CustomLayer<> >();
    MomentumSGD opt(0.06, 50, 2, -50000);

    arma::cube inputTemp, labelsTemp;
    for (size_t iteration = 0; iteration < (iterations + offset); iteration++)
    {
      for (size_t j = 0; j < trainReberGrammarCount; ++j)
      {
        // Each sequence may be a different length, so we need to extract them
        // manually.  We will reshape them into a cube with each slice equal to
        // a time step.
        inputTemp = arma::cube(trainInput.at(0, j).memptr(), inputSize, 1,
            trainInput.at(0, j).n_elem / inputSize, false, true);
        labelsTemp = arma::cube(trainLabels.at(0, j).memptr(), inputSize, 1,
            trainInput.at(0, j).n_elem / inputSize, false, true);

        model.Rho() = inputTemp.n_elem / inputSize;
        model.Train(inputTemp, labelsTemp, opt);
        opt.ResetPolicy() = false;
      }
    }

    double error = 0;

    // Ask the network to predict the next Reber grammar in the given sequence.
    for (size_t i = 0; i < testReberGrammarCount; ++i)
    {
      arma::cube prediction;
      arma::cube input(testInput.at(0, i).memptr(), inputSize, 1,
          testInput.at(0, i).n_elem / inputSize, false, true);

      model.Rho() = input.n_elem / inputSize;
      model.Predict(input, prediction);

      const size_t reberGrammerSize = 7;
      std::string inputReber = "";

      size_t reberError = 0;

      for (size_t j = 0; j < (prediction.n_elem / reberGrammerSize); ++j)
      {
        char predictedSymbol, inputSymbol;
        std::string reberChoices;

        arma::umat output = (prediction.slice(j) == (arma::ones(
            reberGrammerSize, 1) *
            arma::as_scalar(arma::max(prediction.slice(j)))));

        ReberReverseTranslation(output, predictedSymbol);
        ReberReverseTranslation(input.slice(j), inputSymbol);
        inputReber += inputSymbol;

        if (recursive)
          GenerateNextRecursiveReber(transitions, inputReber, reberChoices);
        else
          GenerateNextReber(transitions, inputReber, reberChoices);

        if (reberChoices.find(predictedSymbol) != std::string::npos)
          reberError++;
      }

      if (reberError != (prediction.n_elem / reberGrammerSize))
        error += 1;
    }

    error /= testReberGrammarCount;
    if (error <= 0.35)
    {
      ++successes;
      break;
    }

    offset += 3;
  }

  BOOST_REQUIRE_GE(successes, 1);
}

/**
 * Train the specified networks on an embedded Reber grammar dataset.
 */
BOOST_AUTO_TEST_CASE(CustomRecursiveReberGrammarTest)
{
  ReberGrammarTestCustomNetwork(16, true);
}

/**
 * @brief Generates noisy sine wave and outputs the data and the labels that
 *        can be used directly for training and testing with RNN.
 *
 * @param data The data points as output
 * @param labels The expected values as output
 * @param rho The size of the sequence of each data point
 * @param outputSteps How many output steps to consider for every rho inputs
 * @param dataPoints  The number of generated data points. The actual generated
 *        data points may be more than this to adjust to the outputSteps. But at
 *        the minimum these many data points will be generated.
 * @param gain The gain on the amplitude
 * @param freq The frquency of the sine wave
 * @param phase The phase shift if any
 * @param noisePercent The percent noise to induce
 * @param numCycles How many full size wave cycles required. All the data
 *        points will be fit into these cycles.
 * @param normalize Whether to normalise the data. This may be required for some
 *        layers like LSTM. Default is true.
 */
void GenerateNoisySinRNN(arma::cube& data,
                         arma::cube& labels,
                         size_t rho,
                         size_t outputSteps = 1,
                         const int dataPoints = 100,
                         const double gain = 1.0,
                         const int freq = 10,
                         const double phase = 0,
                         const int noisePercent = 20,
                         const double numCycles = 6.0,
                         const bool normalize = true)
{
  int points = dataPoints;
  int r = dataPoints % rho;

  if (r == 0)
  {
    points += outputSteps;
  }
  else
  {
    points += rho - r + outputSteps;
  }

  arma::colvec x(points);
  int i = 0;
  double interval = numCycles / freq / points;

  x.for_each([&i, gain, freq, phase, noisePercent, interval]
    (arma::colvec::elem_type& val) {
    double t = interval * (++i);
    val = gain * ::sin(2 * M_PI * freq * t + phase) +
        (noisePercent * gain / 100 * Random(0.0, 0.1));
  });

  arma::colvec y = x;
  if (normalize)
    y = arma::normalise(x);

  // Now break this into columns of rho size slices.
  size_t numColumns = y.n_elem / rho;
  data = arma::cube(1, numColumns, rho);
  labels = arma::cube(outputSteps, numColumns, 1);

  for (size_t i = 0; i < numColumns; ++i)
  {
    data.tube(0, i) = y.rows(i * rho, i * rho + rho - 1);
    labels.subcube(0, i, 0, outputSteps - 1, i, 0) =
        y.rows(i * rho + rho, i * rho + rho + outputSteps - 1);
  }
}

/**
 * @brief RNNSineTest Test a simple RNN using noisy sine. Use single output
 *        for multiple inputs.
 * @param hiddenUnits No of units in the hiddenlayer.
 * @param rho The input sequence length.
 * @param numEpochs The number of epochs to run.
 * @return The mean squared error of the prediction.
 */
double RNNSineTest(size_t hiddenUnits, size_t rho, size_t numEpochs = 100)
{
  RNN<MeanSquaredError<> > net(rho, true);
  net.Add<LinearNoBias<> >(1, hiddenUnits);
  net.Add<LSTM<> >(hiddenUnits, hiddenUnits);
  net.Add<LinearNoBias<> >(hiddenUnits, 1);

  RMSProp opt(0.005, 100, 0.9, 1e-08, 50000, 1e-5);

  // Generate data
  arma::cube data;
  arma::cube labels;
  GenerateNoisySinRNN(data, labels, rho, 1, 2000, 20.0, 200, 0.0, 45, 20);

  // Break into training and test sets. Simply split along columns.
  size_t trainCols = data.n_cols * 0.8; // Take 20% out for testing.
  size_t testCols = data.n_cols - trainCols;
  arma::cube testData = data.subcube(0, data.n_cols - testCols, 0,
      data.n_rows - 1, data.n_cols - 1, data.n_slices - 1);
  arma::cube testLabels = labels.subcube(0, labels.n_cols - testCols, 0,
      labels.n_rows - 1, labels.n_cols - 1, labels.n_slices - 1);

  for (size_t i = 0; i < numEpochs; ++i)
  {
    net.Train(data.subcube(0, 0, 0, data.n_rows - 1, trainCols - 1,
        data.n_slices - 1), labels.subcube(0, 0, 0, labels.n_rows - 1,
        trainCols - 1, labels.n_slices - 1), opt);
  }
  // Well now it should be trained. Do the test here.
  arma::cube prediction;
  net.Predict(testData, prediction);

  // The prediction must really follow the test data. So convert both the test
  // data and the pediction to vectors and compare the two.
  arma::colvec testVector = arma::vectorise(testData);
  arma::colvec predVector = arma::vectorise(prediction);

  // Adjust the vectors for comparison, as the prediction is one step ahead.
  testVector = testVector.rows(1, testVector.n_rows - 1);
  predVector = predVector.rows(0, predVector.n_rows - 2);
  double error = std::sqrt(arma::sum(arma::square(testVector - predVector))) /
      testVector.n_rows;

  return error;
}

/**
 * Test RNN using multiple timestep input and single output.
 */
BOOST_AUTO_TEST_CASE(MultiTimestepTest)
{
  double err = RNNSineTest(4, 10, 20);
  BOOST_REQUIRE_LE(err, 0.025);
}

/**
 * Test that RNN::Train() returns finite objective value.
 */
BOOST_AUTO_TEST_CASE(RNNTrainReturnObjective)
{
  const size_t rho = 10;

  // Generate 12 (2 * 6) noisy sines. A single sine contains rho
  // points/features.
  arma::cube input;
  arma::mat labelsTemp;
  GenerateNoisySines(input, labelsTemp, rho, 6);

  arma::cube labels = arma::zeros<arma::cube>(1, labelsTemp.n_cols, rho);
  for (size_t i = 0; i < labelsTemp.n_cols; ++i)
  {
    const int value = arma::as_scalar(arma::find(
        arma::max(labelsTemp.col(i)) == labelsTemp.col(i), 1)) + 1;
    labels.tube(0, i).fill(value);
  }

  /**
   * Construct a network with 1 input unit, 4 hidden units and 10 output
   * units. The hidden layer is connected to itself. The network structure
   * looks like:
   *
   *  Input         Hidden        Output
   * Layer(1)      Layer(4)      Layer(10)
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |    ..>|     |       |     |
   * +-----+    .  +--+--+       +-----+
   *            .     .
   *            .     .
   *            .......
   */
  Add<> add(4);
  Linear<> lookup(1, 4);
  SigmoidLayer<> sigmoidLayer;
  Linear<> linear(4, 4);
  Recurrent<>* recurrent = new Recurrent<>(add, lookup, linear,
      sigmoidLayer, rho);

  RNN<> model(rho);
  model.Add<IdentityLayer<> >();
  model.Add(recurrent);
  model.Add<Linear<> >(4, 10);
  model.Add<LogSoftMax<> >();

  StandardSGD opt(0.1, 1, input.n_cols /* 1 epoch */, -100);
  double objVal = model.Train(input, labels, opt);

  BOOST_REQUIRE_EQUAL(std::isfinite(objVal), true);
}

/**
 * Test that BRNN::Train() returns finite objective value.
 */
BOOST_AUTO_TEST_CASE(BRNNTrainReturnObjective)
{
  const size_t rho = 10;

  arma::cube input;
  arma::mat labelsTemp;
  GenerateNoisySines(input, labelsTemp, rho, 6);

  arma::cube labels = arma::zeros<arma::cube>(1, labelsTemp.n_cols, rho);
  for (size_t i = 0; i < labelsTemp.n_cols; ++i)
  {
    const int value = arma::as_scalar(arma::find(
        arma::max(labelsTemp.col(i)) == labelsTemp.col(i), 1)) + 1;
    labels.tube(0, i).fill(value);
  }

  Add<> add(4);
  Linear<> lookup(1, 4);
  SigmoidLayer<> sigmoidLayer;
  Linear<> linear(4, 4);
  Recurrent<>* recurrent = new Recurrent<>(
      add, lookup, linear, sigmoidLayer, rho);

  BRNN<> model(rho);
  model.Add<IdentityLayer<> >();
  model.Add(recurrent);
  model.Add<Linear<> >(4, 5);

  StandardSGD opt(0.1, 1, 500 * input.n_cols, -100);
  double objVal = model.Train(input, labels, opt);
  BOOST_TEST_CHECKPOINT("Training over");

  // Test that BRNN::Train() returns finite objective value.
  BOOST_REQUIRE_EQUAL(std::isfinite(objVal), true);
}

/**
 * Test that RNN::Train() does not give an error for large rho.
 */
BOOST_AUTO_TEST_CASE(LargeRhoValueRnnTest)
{
  // Setting rho value greater than sequence length which is 17.
  const size_t rho = 100;
  const size_t hiddenSize = 128;
  const size_t numLetters = 256;
  using MatType = arma::cube;
  std::vector<std::string>trainingData = { "THIS IS THE INPUT 0" ,
                                           "THIS IS THE INPUT 1" ,
                                           "THIS IS THE INPUT 3"};

  RNN<> model(rho);
  model.Add<IdentityLayer<>>();
  model.Add<LSTM<>>(numLetters, hiddenSize, rho);
  model.Add<Dropout<>>(0.1);
  model.Add<Linear<>>(hiddenSize, numLetters);

  const auto makeInput = [numLetters](const char *line) -> MatType
  {
    const auto strLen = strlen(line);
    // Rows: number of dimensions.
    // Cols: number of sequences/points.
    // Slices: number of steps in sequences.
    MatType result(numLetters, 1, strLen, arma::fill::zeros);
    for (size_t i = 0; i < strLen; ++i)
    {
      result.at(static_cast<arma::uword>(line[i]), 0, i) = 1.0;
    }
    return result;
  };

  const auto makeTarget = [] (const char *line) -> MatType
  {
    const auto strLen = strlen(line);
    // Responses for NegativeLogLikelihood should be
    // non-one-hot-encoded class IDs (from 1 to num_classes).
    MatType result(1, 1, strLen, arma::fill::zeros);
    // The response is the *next* letter in the sequence.
    for (size_t i = 0; i < strLen - 1; ++i)
    {
      result.at(0, 0, i) = static_cast<arma::uword>(line[i + 1]) + 1.0;
    }
    // The final response is empty, so we set it to class 0.
    result.at(0, 0, strLen - 1) = 1.0;
    return result;
  };

  std::vector<MatType> inputs(trainingData.size());
  std::vector<MatType> targets(trainingData.size());
  for (size_t i = 0; i < trainingData.size(); ++i)
  {
    inputs[i] = makeInput(trainingData[i].c_str());
    targets[i] = makeTarget(trainingData[i].c_str());
  }
  ens::SGD<> opt(0.01, 1, 100);
  model.Train(inputs[0], targets[0], opt);
  BOOST_TEST_CHECKPOINT("Training over");
}

BOOST_AUTO_TEST_SUITE_END();


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__INTERP__BEZIER_HPP_
#define SMOOTH__INTERP__BEZIER_HPP_

/**
 * @file
 * @brief bezier splines on lie groups.
 */

#include <ranges>

#include <Eigen/Sparse>
#include <Eigen/SparseLU>

#include "smooth/concepts.hpp"
#include "smooth/internal/utils.hpp"

#include "common.hpp"

namespace smooth {

/**
 * @brief Bezier curve on [0, 1].
 * @tparam N Polonimial degree of curve.
 * @tparam G LieGroup type.
 *
 * The curve is defined by
 * \f[
 *  g(t) = g_0 * \exp(\tilde B_1(t) v_1) * ... \exp(\tilde B_N(t) v_N)
 * \f]
 * where \f$\tilde B_i(t)\f$ are cumulative Bernstein basis functions and
 * \f$v_i = g_i \ominus g_{i-1}\f$ are the control point differences.
 */
template<std::size_t N, LieGroup G>
class Bezier
{
public:
  /**
   * @brief Default constructor creates a constant curve on [0, 1] equal to identity.
   */
  Bezier() : g0_(G::Identity()) { vs_.fill(G::Tangent::Zero()); }

  /**
   * @brief Create curve from rvalue parameter values.
   *
   * @param g0 starting value
   * @param vs differences [v_1, ..., v_n] between control points
   */
  Bezier(G && g0, std::array<typename G::Tangent, N> && vs) : g0_(std::move(g0)), vs_(std::move(vs))
  {}

  /**
   * @brief Create curve from parameter values.
   *
   * @tparam Rv range containing control point differences
   * @param g0 starting value
   * @param vs differences [v_1, ..., v_n] between control points
   *
   * @note Range value type of \p Rv must be the tangent type of \p G.
   */
  template<std::ranges::range Rv>
  Bezier(const G & g0, const Rv & vs) : g0_(g0)
  {
    if (std::ranges::size(vs) != N) { throw std::runtime_error("Wrong number of control points"); }
    std::copy(std::ranges::begin(vs), std::ranges::end(vs), vs_.begin());
  }

  /// @brief Copy constructor
  Bezier(const Bezier &) = default;
  /// @brief Move constructor
  Bezier(Bezier &&) = default;
  /// @brief Copy assignment
  Bezier & operator=(const Bezier &) = default;
  /// @brief Move assignment
  Bezier & operator=(Bezier &&) = default;
  /// @brief Destructor
  ~Bezier() = default;

  /**
   * @brief Evaluate Bezier curve.
   *
   * @param[in] t time point to evaluate at
   * @param[out] vel output body velocity at evaluation time
   * @param[out] acc output body acceleration at evaluation time
   * @return spline value at time t
   *
   * @note Input \p t is clamped to interval [0, 1]
   */
  G eval(double t, detail::OptTangent<G> vel = {}, detail::OptTangent<G> acc = {}) const
  {
    double tc = std::clamp<double>(t, 0, 1);

    constexpr auto Mstatic = detail::cum_coefmat<CSplineType::BEZIER, double, N>().transpose();
    Eigen::Map<const Eigen::Matrix<double, N + 1, N + 1, Eigen::RowMajor>> M(Mstatic[0].data());

    return cspline_eval<N>(g0_, vs_, M, tc, vel, acc);
  }

private:
  G g0_;
  std::array<typename G::Tangent, N> vs_;
};

/**
 * @brief Piecewise curve built from Bezier segments.
 *
 * The curve is given by
 * \f[
 *  \mathbf{x}(t) = p_i \left( \frac{t - t_i}{t_{i+1} - t_{i}} \right)
 * \f]
 * for \f$ t \in [t_i, t_{i+1}]\f$
 * where \f$p_i\f$ is a Bezier curve on \f$[0, 1]\f$.
 */
template<std::size_t N, LieGroup G>
class PiecewiseBezier
{
public:
  /**
   * @brief Default constructor creates a constant curve defined on [0, 1] equal to identity.
   */
  PiecewiseBezier() : knots_{0, 1}, segments_{Bezier<N, G>{}} {}

  /**
   * @brief Create a PiecewiseBezier from knot times and Bezier segments.
   *
   * @param knots points \f$ t_i \f$
   * @param segments Bezier curves \f$ p_i \f$
   */
  PiecewiseBezier(std::vector<double> && knots, std::vector<Bezier<N, G>> && segments)
      : knots_(std::move(knots)), segments_(std::move(segments))
  {}

  /**
   * @brief Create a PiecewiseBezier from knot times and Bezier segments.
   *
   * @param knots points \f$ t_i \f$
   * @param segments Bezier curves \f$ p_i \f$
   */
  template<std::ranges::range Rt, std::ranges::range Rs>
  PiecewiseBezier(const Rt & knots, const Rs & segments)
      : knots_(std::ranges::begin(knots), std::ranges::end(knots)),
        segments_(std::ranges::begin(segments), std::ranges::end(segments))
  {}

  /// @brief Copy constructor
  PiecewiseBezier(const PiecewiseBezier &) = default;
  /// @brief Move constructor
  PiecewiseBezier(PiecewiseBezier &&) = default;
  /// @brief Copy assignment
  PiecewiseBezier & operator=(const PiecewiseBezier &) = default;
  /// @brief Move assignment
  PiecewiseBezier & operator=(PiecewiseBezier &&) = default;
  /// @brief Destructor
  ~PiecewiseBezier() = default;

  /// @brief Minimal time where curve is defined.
  double t_min() const { return knots_.front(); }

  /// @brief Maximal time where curve is defined.
  double t_max() const { return knots_.back(); }

  /**
   * @brief Evalauate PiecewiseBezier curve.
   *
   * @param[in] t time point to evaluate at
   * @param[out] vel output body velocity at evaluation time
   * @param[out] acc output body acceleration at evaluation time
   * @return curve value at time t
   */
  G eval(double t, detail::OptTangent<G> vel = {}, detail::OptTangent<G> acc = {}) const
  {
    /// find index
    // TODO binary search
    std::size_t istar = 0;
    while (istar + 2 < knots_.size() && knots_[istar + 1] <= t) { ++istar; }

    double T = knots_[istar + 1] - knots_[istar];

    const double u = (t - knots_[istar]) / T;

    G g = segments_[istar].eval(u, vel, acc);

    if (vel.has_value()) { vel.value() /= T; }
    if (acc.has_value()) { acc.value() /= (T * T); }

    return g;
  }

private:
  std::vector<double> knots_;
  std::vector<Bezier<N, G>> segments_;
};

/**
 * @brief Fit a linear PiecewiseBezier curve to data.
 *
 * The resulting curve passes through the data points and has piecewise
 * constant velocity.
 *
 * @warning Result has discontinuous derivatives at knot points
 *
 * @tparam Rt, Rg range types
 * @param tt interpolation times
 * @param gg interpolation values
 */
template<std::ranges::range Rt, std::ranges::range Rg>
PiecewiseBezier<1, std::ranges::range_value_t<Rg>> fit_linear_bezier(const Rt & tt, const Rg & gg)
{
  if (std::ranges::size(tt) < 2 || std::ranges::size(gg) < 2) {
    throw std::runtime_error("Not enough points");
  }
  using G = std::ranges::range_value_t<Rg>;

  const std::size_t N = std::min<std::size_t>(std::ranges::size(tt), std::ranges::size(gg)) - 1;

  std::vector<Bezier<1, G>> segments(N);

  auto it_g = std::ranges::begin(gg);
  auto it_t = std::ranges::begin(tt);

  for (auto i = 0u; i != N; ++i, ++it_t, ++it_g) {
    segments[i] = Bezier<1, G>(*it_g, std::array<typename G::Tangent, 1>{(*(it_g + 1) - *it_g)});
  }

  auto take_view = tt | std::views::take(N + 1);
  std::vector<double> knots(std::ranges::begin(take_view), std::ranges::end(take_view));

  return PiecewiseBezier<1, G>(std::move(knots), std::move(segments));
}

/**
 * @brief Fit a quadratic PiecewiseBezier curve to data.
 *
 * The resulting curve passes through the data points and has
 * continuous derivatives.
 *
 * @warning Result may exhibit oscillatory behavior since second derivative
 * is free.
 *
 * @tparam Rt, Rg range types
 * @param tt interpolation times
 * @param gg interpolation values
 */
template<std::ranges::range Rt, std::ranges::range Rg>
PiecewiseBezier<2, std::ranges::range_value_t<Rg>> fit_quadratic_bezier(
  const Rt & tt, const Rg & gg)
{
  if (std::ranges::size(tt) < 2 || std::ranges::size(gg) < 2) {
    throw std::runtime_error("Not enough points");
  }
  using G             = std::ranges::range_value_t<Rg>;
  const std::size_t N = std::min<std::size_t>(std::ranges::size(tt), std::ranges::size(gg)) - 1;

  std::vector<Bezier<2, G>> segments(N);

  auto it_g = std::ranges::begin(gg);
  auto it_t = std::ranges::begin(tt);

  typename G::Tangent v0 = (*(it_g + 1) - *it_g) / (*(it_t + 1) - *it_t);

  for (auto i = 0u; i != N; ++i, ++it_t, ++it_g) {
    const double dt = *(it_t + 1) - *it_t;

    // scaled velocity
    const typename G::Tangent va = v0 * dt;

    // create segment
    const typename G::Tangent v1 = va / 2;
    const typename G::Tangent v2 = *(it_g + 1) - (*it_g * G::exp(va / 2));

    segments[i] = Bezier<2, G>(*it_g, std::array<typename G::Tangent, 2>{v1, v2});

    // unscaled end velocity for interval
    v0 = v2 * 2 / dt;
  }

  auto take_view = tt | std::views::take(N + 1);
  std::vector<double> knots(std::ranges::begin(take_view), std::ranges::end(take_view));

  return PiecewiseBezier<2, G>(std::move(knots), std::move(segments));
}

/**
 * @brief Fit a cubic PiecewiseBezier curve to data.
 *
 * The resulting curve passes through the data points, has continuous first
 * derivatives, and approximately continuous second derivatives.
 *
 * @tparam Rt, Rg range types
 * @param tt interpolation times
 * @param gg interpolation values
 */
template<std::ranges::range Rt, std::ranges::range Rg>
PiecewiseBezier<3, std::ranges::range_value_t<Rg>> fit_cubic_bezier(const Rt & tt, const Rg & gg)
{
  if (std::ranges::size(tt) < 2 || std::ranges::size(gg) < 2) {
    throw std::runtime_error("Not enough points");
  }
  using G = std::ranges::range_value_t<Rg>;

  // number of intervals
  const std::size_t N = std::min<std::size_t>(std::ranges::size(tt), std::ranges::size(gg)) - 1;

  std::size_t NumVars = G::Dof * 3 * N;

  Eigen::SparseMatrix<typename G::Scalar> lhs;
  lhs.resize(NumVars, NumVars);
  Eigen::Matrix<int, -1, 1> nnz = Eigen::Matrix<int, -1, 1>::Constant(NumVars, 3);
  nnz.head(G::Dof).setConstant(2);
  nnz.tail(G::Dof).setConstant(2);
  lhs.reserve(nnz);

  Eigen::Matrix<typename G::Scalar, -1, 1> rhs(NumVars);
  rhs.setZero();

  // variable layout:
  //
  // [ v_{1, 0}; v_{2, 0}; v_{3, 0}; v_{1, 1}; v_{2, 1}; v_{3, 1}; ...]
  //
  // where v_ji is a Dof-length vector

  const auto idx = [&](int j, int i) { return 3 * G::Dof * i + G::Dof * (j - 1); };

  std::size_t row_counter = 0;

  //// LEFT END POINT  ////

  // zero second derivative at start:
  // v_{1, 0} = v_{2, 0}
  const std::size_t v10_start = idx(1, 0);
  const std::size_t v20_start = idx(2, 0);
  for (auto n = 0u; n != G::Dof; ++n) {
    lhs.insert(row_counter + n, v10_start + n) = 1;
    lhs.insert(row_counter + n, v20_start + n) = -1;
  }
  row_counter += G::Dof;

  //// INTERIOR END POINT  ////

  auto it_t = std::ranges::begin(tt);
  auto it_g = std::ranges::begin(gg);

  for (auto i = 0u; i + 1 < N; ++i, ++it_t, ++it_g) {
    const std::size_t v1i_start = idx(1, i);
    const std::size_t v2i_start = idx(2, i);
    const std::size_t v3i_start = idx(3, i);

    const std::size_t v1ip_start = idx(1, i + 1);
    const std::size_t v2ip_start = idx(2, i + 1);

    // segment lengths
    const typename G::Scalar Ti  = *(it_t + 1) - *it_t;
    const typename G::Scalar Tip = *(it_t + 2) - *(it_t + 1);

    // pass through control points
    // v_{1, i} + v_{2, i} + v_{3, i} = x_{i+1} - x_i
    for (auto n = 0u; n != G::Dof; ++n) {
      lhs.insert(row_counter + n, v1i_start + n) = 1;
      lhs.insert(row_counter + n, v2i_start + n) = 1;
      lhs.insert(row_counter + n, v3i_start + n) = 1;
    }
    rhs.segment(row_counter, G::Dof) = *(it_g + 1) - *(it_g);
    row_counter += G::Dof;

    // velocity continuity
    // v_{3, i} = v_{1, i+1}
    for (auto n = 0u; n != G::Dof; ++n) {
      lhs.insert(row_counter + n, v3i_start + n)  = 1 * Tip;
      lhs.insert(row_counter + n, v1ip_start + n) = -1 * Ti;
    }
    row_counter += G::Dof;

    // acceleration continuity (approximate for Lie groups)
    // v_{2, i} - v_{3, i} = v_{2, i+1} - v_{1, i+1}
    for (auto n = 0u; n != G::Dof; ++n) {
      lhs.insert(row_counter + n, v2i_start + n)  = 1 * (Tip * Tip);
      lhs.insert(row_counter + n, v3i_start + n)  = -1 * (Tip * Tip);
      lhs.insert(row_counter + n, v1ip_start + n) = -1 * (Ti * Ti);
      lhs.insert(row_counter + n, v2ip_start + n) = 1 * (Ti * Ti);
    }
    row_counter += G::Dof;
  }

  //// RIGHT END POINT  ////

  const std::size_t v1_nm_start = idx(1, N - 1);
  const std::size_t v2_nm_start = idx(2, N - 1);
  const std::size_t v3_nm_start = idx(3, N - 1);

  // end at last control point
  // v_{1, n-1} + v_{2, n-1} v_{3, n-1} = x_{n} - x_{n-1}
  for (auto n = 0u; n != G::Dof; ++n) {
    lhs.insert(row_counter + n, v1_nm_start + n) = 1;
    lhs.insert(row_counter + n, v2_nm_start + n) = 1;
    lhs.insert(row_counter + n, v3_nm_start + n) = 1;
  }
  rhs.segment(row_counter, G::Dof) = *(it_g + 1) - *it_g;
  row_counter += G::Dof;

  // zero second derivative at end:
  // v_{2, n-1} = v_{3, n-1}
  for (auto n = 0u; n != G::Dof; ++n) {
    lhs.insert(row_counter + n, v2_nm_start + n) = 1;
    lhs.insert(row_counter + n, v3_nm_start + n) = -1;
  }

  //// DONE FILLING SPARSE MATRIX ////

  lhs.makeCompressed();

  //// SOLVE SYSTEM ////

  Eigen::SparseLU<decltype(lhs), Eigen::COLAMDOrdering<int>> solver(lhs);
  Eigen::VectorXd result = solver.solve(rhs);

  //// EXTRACT SOLUTION ////

  std::vector<Bezier<3, G>> segments;
  segments.reserve(N);

  it_g = std::ranges::begin(gg);

  for (auto i = 0u; i != N; ++i, ++it_g) {
    const std::size_t v1i_start = idx(1, i);
    const std::size_t v3i_start = idx(3, i);

    typename G::Tangent v1 = result.template segment<G::Dof>(v1i_start);
    typename G::Tangent v3 = result.template segment<G::Dof>(v3i_start);
    // re-compute v2 to compensate for linearization
    // this ensures points are interpolated, but the cost is
    // potential non-continuity of the second derivative
    typename G::Tangent v2 = *(it_g + 1) * G::exp(-v3) - *(it_g)*G::exp(v1);

    segments.emplace_back(
      *it_g, std::array<typename G::Tangent, 3>{std::move(v1), std::move(v2), std::move(v3)});
  }

  auto take_view = tt | std::views::take(N + 1);
  std::vector<double> knots(std::ranges::begin(take_view), std::ranges::end(take_view));

  return PiecewiseBezier<3, G>(std::move(knots), std::move(segments));
}

}  // namespace smooth

#endif  // SMOOTH__INTERP__BEZIER_HPP_


#include "CubicInterpolation/BicubicSplines.h"
#include "CubicInterpolation/InterpolantBuilder.h"

#include <Eigen/Dense>
#include <boost/math/differentiation/finite_difference.hpp>
#include <boost/math/interpolators/cardinal_cubic_b_spline.hpp>
#include <boost/serialization/access.hpp>
#include <cmath>
#include <vector>

#include <chrono>
#include <iostream>

namespace cubic_splines {
namespace detail {

template <typename T> auto calculate_node(T val, unsigned int n_max) {
  auto n = std::floor(val);
  if (n < 0)
    return 0u;
  if (static_cast<unsigned int>(n) > n_max - 2u)
    return n_max - 2u;
  return static_cast<unsigned int>(n);
}

template <typename T> auto exponent_vector(T x) {
  auto x_vec = ::Eigen::Matrix<T, 4, 1>(4);
  for (size_t i = 0; i < 4; ++i) {
    x_vec(i) = 1;
    for (size_t j = 1; j <= i; ++j)
      x_vec(i) *= x;
  }
  return x_vec;
}
} // namespace detail

namespace detail {}

template <typename T> struct BicubicSplines<T>::RuntimeData {
  using MatrixX = ::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic>;
  using Matrix4 = ::Eigen::Matrix<T, 4, 4>;

  MatrixX y, dydx1, dydx2, d2ydx1dx2;
  Matrix4 m =
      (Matrix4() << 1, 0, -3, 2, 0, 0, 3, -2, 0, 1, -2, 1, 0, 0, -1, 1).finished();

  RuntimeData() = default;

  RuntimeData(size_t n1, size_t n2)
      : y(n1, n2), dydx1(n1, n2), dydx2(n1, n2), d2ydx1dx2(n1, n2){};

  template <typename T1>
  RuntimeData(T1 &&_y, T1 &&_dydx1, T1 &&_dydx2, T1 &&_d2ydx1dx2)
      : y(std::forward<T1>(_y)), dydx1(std::forward<T1>(_dydx1)),
        dydx2(std::forward<T1>(_dydx2)), d2ydx1dx2(std::forward<T1>(_d2ydx1dx2)){};

  template <typename T1> static ::std::vector<T> to_vector(T1 m) {
    return ::std::vector<T>(m.data(), m.data() + m.rows() * m.cols());
  }

  auto get_dimensions() const {
    return std::array<long int, 2>{static_cast<long int>(y.rows()),
                                   static_cast<long int>(y.cols())};
  }

  StorageData to_storage_data() const;
};

template <typename T> struct BicubicSplines<T>::StorageData {
  using MatrixX = ::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic>;

  ::std::array<long int, 2> size;
  ::std::vector<T> y, dydx1, dydx2, d2ydx1dx2;

  friend class boost::serialization::access;
  template <class Archive> void serialize(Archive &ar, const unsigned int) {
    ar &size;
    ar &y;
    ar &dydx1;
    ar &dydx2;
    ar &d2ydx1dx2;
  }

  template <typename V> inline auto to_matrix(V v) const {
    return ::Eigen::Map<MatrixX>(v.data(), size[0], size[1]);
  }

public:
  StorageData() = default;

  template <typename T1>
  StorageData(std::array<long int, 2> _size, T1 &&_y, T1 &&_dydx1, T1 &&_dydx2,
              T1 &&_d2ydx1dx2)
      : size(std::move(_size)), y(std::forward<T1>(_y)), dydx1(std::forward<T1>(_dydx1)),
        dydx2(std::forward<T1>(_dydx2)), d2ydx1dx2(std::forward<T1>(_d2ydx1dx2)){};

  auto to_runtime_data() const {
    return RuntimeData(to_matrix(y), to_matrix(dydx1), to_matrix(dydx2),
                       to_matrix(d2ydx1dx2));
  }
};

template <typename T>
typename BicubicSplines<T>::StorageData
BicubicSplines<T>::RuntimeData::to_storage_data() const {
  return StorageData(get_dimensions(), to_vector(y), to_vector(dydx1), to_vector(dydx2),
                     to_vector(d2ydx1dx2));
}

template <typename T>
BicubicSplines<T>::BicubicSplines(BicubicSplines::RuntimeData _data)
    : data(::std::make_unique<BicubicSplines::RuntimeData>(_data)) {}

template <typename T>
BicubicSplines<T>::BicubicSplines(Definition const &def, std::string path,
                                  std::string filename) {
  try {
    auto storage_data = load<BicubicSplines>(path, filename);
    *this = BicubicSplines(storage_data.to_runtime_data());
  } catch (std::system_error const &ex) {
    if (ex.code().value() != ENOENT)
      throw(ex);
    *this = BicubicSplines(def);
    save(data->to_storage_data(), path, filename);
  }
}

template <typename T>
std::tuple<T, T> BicubicSplines<T>::back_transform(Definition const &def,
                                                   unsigned long n1,
                                                   unsigned long n2) const {
  auto x1 = def.axis[0]->back_transform(n1);
  auto x2 = def.axis[1]->back_transform(n2);
  return std::make_tuple(x1, x2);
}

template <typename T>
template <typename T1>
std::array<T, 2>
BicubicSplines<T>::_prime(T1 func, std::array<std::unique_ptr<Axis<T>>, 2> const &axis,
                          unsigned int n1, unsigned int n2) {
  using boost::math::differentiation::finite_difference_derivative;
  auto f_x1 = [&func, ax = axis[0].get(), x2 = axis[1]->back_transform(n2)](T t1) {
    return func(ax->back_transform(t1), x2);
  };
  auto f_x2 = [&func, ax = axis[1].get(), x1 = axis[0]->back_transform(n1)](T t2) {
    return func(x1, ax->back_transform(t2));
  };
  auto dydx = std::array<T, 2>();
  dydx[0] = finite_difference_derivative(f_x1, static_cast<T>(n1));
  dydx[1] = finite_difference_derivative(f_x2, static_cast<T>(n2));
  return dydx;
}

/* template <typename T> */
/* template <typename T1> */
/* std::vector<std::tuple<unsigned int, T>> */
/* BicubicSplines<T>::_prime(std::vector<T> const &yi, T1 func, unsigned int n_max) { */
/*   auto spline = boost::math::interpolators::cardinal_cubic_b_spline<T>( */
/*       yi.data(), yi.size(), 0, 1, func(0u), func(n_max)); */
/*   auto diff = std::vector<std::tuple<unsigned int, T>>(); */
/*   for (auto i = 0u; i < n_max; ++i) */
/*     diff.emplace_back(i, spline.prime(i)); */
/*   return diff; */
/* } */

template <typename T>
template <typename T1>
T BicubicSplines<T>::_double_prime(Definition const &def, T1 func, unsigned int n1,
                                   unsigned int n2) {
  using boost::math::differentiation::finite_difference_derivative;
  auto f_x1 = [&func, ax = def.axis[0].get()](T t1, T t2) {
    return func(ax->back_transform(t1), t2);
  };
  auto dydx1 = [&f_x1, n1](T t2) {
    return finite_difference_derivative([&f_x1, t2](T t1) { return f_x1(t1, t2); },
                                        static_cast<T>(n1));
  };
  auto dydx1_x2 = [&dydx1, ax = def.axis[1].get()](T t2) {
    return dydx1(ax->back_transform(t2));
  };
  return finite_difference_derivative(dydx1_x2, static_cast<T>(n2));
}

template <typename T>
BicubicSplines<T>::BicubicSplines(Definition const &def)
    : data(std::make_shared<RuntimeData>(def.axis[0]->required_nodes(),
                                         def.axis[1]->required_nodes())) {
  using boost::math::differentiation::finite_difference_derivative;
  auto func = [&def](T x1, T x2) {
    if (def.f_trafo)
      return def.f_trafo->transform(def.f(x1, x2));
    return def.f(x1, x2);
  };

  for (auto n1 = 0u; n1 < data->y.rows(); ++n1) {
    for (auto n2 = 0u; n2 < data->y.cols(); ++n2) {
      auto x = back_transform(def, n1, n2);
      data->y(n1, n2) = func(std::get<0>(x), std::get<1>(x));
    }
  }

  auto n_rows = data->y.rows();
  auto n_cols = data->y.cols();
  if (def.approx_derivates) {
    for (auto n1 = 0u; n1 < n_cols; ++n1) {
      auto yi = RuntimeData::to_vector(data->y.col(n1));
      auto diff = [this, &def, &func, n1](unsigned int n) {
        return _prime(func, def.axis, n, n1)[0];
      };
      auto spline = boost::math::interpolators::cardinal_cubic_b_spline<T>(
          yi.data(), yi.size(), 0, 1, diff(0u), diff(n_rows - 1));
      for (auto row = 0; row < n_rows; ++row)
        data->dydx1(n1, row) = spline.prime(row);
    }
    data->dydx1.transposeInPlace();

    ::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic> y_rowise = data->y;
    y_rowise.transposeInPlace();
    for (auto n2 = 0u; n2 < n_rows; ++n2) {
      auto yi = RuntimeData::to_vector(y_rowise.col(n2));
      auto diff = [this, &def, &func, n2](unsigned int n) {
        return _prime(func, def.axis, n2, n)[1];
      };
      auto spline = boost::math::interpolators::cardinal_cubic_b_spline<T>(
          yi.data(), yi.size(), 0, 1, diff(0u), diff(n_cols - 1));
      for (auto col = 0; col < n_cols; ++col)
        data->dydx2(col, n2) = spline.prime(col);
    }
    data->dydx2.transposeInPlace();
  } else {
    for (auto n1 = 0u; n1 < n_rows; ++n1) {
      for (auto n2 = 0u; n2 < n_cols; ++n2) {
        auto dydx = _prime(func, def.axis, n1, n2);
        data->dydx1(n1, n2) = dydx[0];
        data->dydx2(n1, n2) = dydx[1];
      }
    }
  }
  if (def.approx_derivates) {
    ::Eigen::Matrix<T, ::Eigen::Dynamic, ::Eigen::Dynamic> dydx1_rowise = data->dydx1;
    dydx1_rowise.transposeInPlace();
    for (auto n2 = 0u; n2 < n_rows; ++n2) {
      auto yi = RuntimeData::to_vector(dydx1_rowise.col(n2));
      auto diff = [this, &def, &func, n2](unsigned int n) {
        return _double_prime(def, func, n2, n);
      };
      auto spline = boost::math::interpolators::cardinal_cubic_b_spline<T>(
          yi.data(), yi.size(), 0, 1, diff(0u), diff(n_cols - 1));
      for (auto col = 0; col < n_cols; ++col)
        data->d2ydx1dx2(col, n2) = spline.prime(col);
    }
    data->d2ydx1dx2.transposeInPlace();
  } else {
    for (auto n1 = 0u; n1 < n_rows; ++n1) {
      for (auto n2 = 0u; n2 < n_cols; ++n2)
        data->d2ydx1dx2(n1, n2) = _double_prime(def, func, n1, n2);
    }
  }
}

template <typename T> T BicubicSplines<T>::evaluate(T x0, T x1) const {
  auto n0 = detail::calculate_node(x0, data->y.cols());
  auto n1 = detail::calculate_node(x1, data->y.rows());
  auto temp = ::Eigen::Matrix<T, 4, 4>(4, 4);
  temp.template block<2, 2>(0, 0) = data->y.block(n0, n1, 2, 2);
  temp.template block<2, 2>(2, 0) = data->dydx1.block(n0, n1, 2, 2);
  temp.template block<2, 2>(0, 2) = data->dydx2.block(n0, n1, 2, 2);
  temp.template block<2, 2>(2, 2) = data->d2ydx1dx2.block(n0, n1, 2, 2);
  auto v1 = detail::exponent_vector(x0 - n0);
  auto v2 = detail::exponent_vector(x1 - n1);
  return v1.dot((data->m.transpose() * (temp * data->m)) * v2);
}

template <typename T> std::array<T, 2> BicubicSplines<T>::prime(T x0, T x1) const {
  using boost::math::differentiation::finite_difference_derivative;
  auto grad = std::array<T, 2>();
  grad[0] =
      finite_difference_derivative([this, x1](T x_0) { return evaluate(x_0, x1); }, x0);
  grad[1] =
      finite_difference_derivative([this, x0](T x_1) { return evaluate(x0, x_1); }, x1);
  return grad;
}

template <typename T> T BicubicSplines<T>::double_prime(T x0, T x1) const {
  using boost::math::differentiation::finite_difference_derivative;
  return finite_difference_derivative(
      [this, x1](T x_1) {
        return finite_difference_derivative(
            [this, x_1](T x_2) { return evaluate(x_1, x_2); }, x1);
      },
      x0);
}
} // namespace cubic_splines

template class cubic_splines::BicubicSplines<double>;
template class cubic_splines::BicubicSplines<float>;