#include <Dmp.h>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/StdVector>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <vector>
#include <cassert>
#include <stdexcept>

namespace Dmp {

/**
 * Determine phase value that corresponds to the current time in the DMP.
 * \param t current time, note that t is allowed to be outside of the range
 *          [start_t, goal_t]
 * \param alpha constant that defines the decay rate of the phase variable
 * \param goal_t time at the end of the DMP
 * \param start_t time at the start of the DMP
 * \return phase value (z)
 */
const double phase(
  const double t,
  const double alpha,
  const double goal_t,
  const double start_t
);

/**
 * Calculates the gradient function for \p in, e.g. the derivation.
 * The returned gradient has the same shape as the input array.
 */
template <typename PosType, typename VelType, typename TimeType>
void gradient(
  const PosType& in,
  VelType& out,
  const TimeType& time,
  const bool allow_final_velocity
)
{
  assert(in.cols() > 1);
  out.resize(in.rows(), in.cols());

  // Special case for first element: assume gradient to be zero
  out.col(0).setZero();

  const int end = in.cols();
  for(int i = 1; i < end; ++i)
  {// Difference quotient for each following element
    out.col(i) = (in.col(i) - in.col(i - 1)) / (time(i) - time(i - 1));
  }
  if(!allow_final_velocity)
    out.col(end - 1).setZero();
}

/**
 * Compute axis-angle representation from quaternion (logarithmic map).
 */
Eigen::Array3d qLog(const Eigen::Quaterniond& q);

/**
 * Compute quaternion from axis-angle representation (exponential map).
 */
Eigen::Quaterniond vecExp(const Eigen::Vector3d& input);

typedef std::vector<Eigen::Quaternion<double>, Eigen::aligned_allocator<Eigen::Quaternion<double> > > QuaternionVector;

template <typename VelType, typename TimeType>
void quaternionGradient(
  const QuaternionVector& rotations,
  VelType& velocities,
  const TimeType& time,
  bool allow_final_velocity)
{
  assert(velocities.rows() == 3);
  assert((size_t) velocities.cols() == rotations.size());
  assert(rotations.size() >= 2);

  // Special case for first element: assume gradient to be zero
  velocities.col(0).setZero();

  // Backward difference quotient
  const int end = (int) rotations.size();
  for(int i = 1; i < end; ++i)
  {// Difference quotient for each following element
    const Eigen::Quaterniond& q0 = rotations[i - 1];
    const Eigen::Quaterniond& q1 = rotations[i];
    const double dt = time(i) - time(i - 1);
    velocities.col(i) = 2 * qLog(q1 * q0.conjugate()) / dt;
  }
  if(!allow_final_velocity)
    velocities.col(end - 1).setZero();
}

/**
 * Solve for 6 position, velocity, and acceleration constraints.
 */
void solveConstraints(
  const double t0,
  const double t1,
  const Eigen::ArrayXd y0,
  const Eigen::ArrayXd y0d,
  const Eigen::ArrayXd y0dd,
  const Eigen::ArrayXd y1,
  const Eigen::ArrayXd y1d,
  const Eigen::ArrayXd y1dd,
  std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1> > >& coefficients
);

const Eigen::MatrixXd rbfDesignMatrix(
  const Eigen::ArrayXd& T,
  const double alpha_z,
  const Eigen::ArrayXd& widths,
  const Eigen::ArrayXd& centers
);

/**
 * Linear regression with L2 regularization.
 *
 * \param X design matrix, each column contains a sample
 * \param targets each column contains a sample
 * \param regularization_coefficient
 * \param weights resulting weights (will be updated)
 */
void ridgeRegression(
  const Eigen::MatrixXd& X,
  const Eigen::ArrayXXd& targets,
  const double regularization_coefficient,
  Eigen::Map<Eigen::ArrayXXd>& weights
);

/**
 * Apply 6 position, velocity, and acceleration constraints.
 */
void applyConstraints(
  const double t, const Eigen::ArrayXd& goal_y, const double goal_t,
  const std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1> > >& coefficients,
  Eigen::ArrayXd& g, Eigen::ArrayXd& gd, Eigen::ArrayXd& gdd
);

/**
 * Determine accelerating forces of the forcing term during the demonstrated
 * trajectory.
 */
void determineForces(
  const Eigen::ArrayXd& T,
  const Eigen::ArrayXXd& Y,
  Eigen::ArrayXXd& F,
  const double alpha_y,
  const double beta_y,
  bool allow_final_velocity
);

const Eigen::ArrayXd rbfActivations(
  const double z,
  const Eigen::ArrayXd& widths,
  const Eigen::ArrayXd& centers,
  const bool normalized = true
);

const Eigen::ArrayXd forcingTerm(
  const double z,
  const Eigen::ArrayXXd& weights,
  const Eigen::ArrayXd& widths,
  const Eigen::ArrayXd& centers
);

double calculateAlpha(
  const double goal_z,
  const double goal_t,
  const double start_t
)
{
  if(goal_z <= 0.0)
    throw std::invalid_argument("Final phase must be > 0!");
  if(start_t >= goal_t)
    throw std::invalid_argument("Goal must be chronologically after start!");

  const double int_dt = 0.001;
  const double execution_time = goal_t - start_t;
  const int num_phases = (int)(execution_time / int_dt) + 1;
  // assert that the execution_time is approximately divisible by int_dt
  assert(abs(((num_phases -1) * int_dt) - execution_time) < 0.05);
  return (1.0 - pow(goal_z, 1.0 / (num_phases - 1))) * (num_phases - 1);
}

void initializeRbf(
  double* widths,
  int num_widths,
  double* centers,
  int num_centers,
  const double goal_t,
  const double start_t,
  const double overlap,
  const double alpha
)
{
  const int num_weights_per_dim = num_widths;
  if(num_widths <= 1)
    throw std::invalid_argument("The number of weights per dimension must be > 1!");

  if(start_t >= goal_t)
    throw std::invalid_argument("Goal must be chronologically after start!");
  const double execution_time = goal_t - start_t;

  assert(num_weights_per_dim == num_widths);
  assert(num_weights_per_dim == num_centers);
  Eigen::Map<Eigen::ArrayXd> widths_array(widths, num_widths);
  Eigen::Map<Eigen::ArrayXd> centers_array(centers, num_centers);

  const double step = execution_time / (num_weights_per_dim - 1); // -1 because we want the last entry to be execution_time
  const double logOverlap = -std::log(overlap);
  // do first iteration outside loop because we need access to i and i - 1 in loop
  double t = start_t;
  centers_array(0) = phase(t, alpha, goal_t, start_t);
  for(int i = 1; i < num_weights_per_dim; ++i)
  {
    // Alternatively Eigen::LinSpaced can be used, however it does exactly the same calculation
    t = i * step; // normally lower_border + i * step but lower_border is 0
    centers_array(i) = phase(t, alpha, goal_t, start_t);
    // Choose width of RBF basis functions automatically so that the
    // RBF centered at one center has value overlap at the next center
    const double diff = centers_array(i) - centers_array(i - 1);
    widths_array(i - 1) = logOverlap / (diff * diff);
  }
  // Width of last gausian cannot be calculated, just use the same width as the one before
  widths_array(num_weights_per_dim - 1) = widths_array(num_weights_per_dim - 2);
}

void imitate(
  const double* T,
  int num_T,
  const double* Y,
  int num_steps,
  int num_task_dims,
  double* weights,
  int num_weights_per_dim,
  int num_weight_dims,
  const double* widths,
  int num_widths,
  const double* centers,
  int num_centers,
  const double regularization_coefficient,
  const double alpha_y,
  const double beta_y,
  const double alpha_z,
  bool allow_final_velocity
)
{
  assert(num_steps == num_T);
  assert(num_weights_per_dim == num_widths);
  assert(num_weights_per_dim == num_centers);
  assert(num_task_dims == num_weight_dims);

  if(regularization_coefficient < 0.0)
  {
    throw std::invalid_argument("Regularization coefficient must be >= 0!");
  }

  Eigen::Map<const Eigen::ArrayXd> widths_array(widths, num_widths);
  Eigen::Map<const Eigen::ArrayXd> centers_array(centers, num_centers);

  Eigen::ArrayXd T_array = Eigen::Map<const Eigen::ArrayXd>(T, num_T);
  Eigen::ArrayXXd Y_array = Eigen::Map<const Eigen::ArrayXXd>(Y, num_task_dims, num_steps);
  Eigen::ArrayXXd F(num_task_dims, num_steps);
  determineForces(T_array, Y_array, F, alpha_y, beta_y, allow_final_velocity);

  const Eigen::MatrixXd X = rbfDesignMatrix(T_array, alpha_z, widths_array, centers_array);

  Eigen::Map<Eigen::ArrayXXd> weights_array(weights, num_task_dims, num_weights_per_dim);
  ridgeRegression(X, F, regularization_coefficient, weights_array);
}

void dmpStep(
  const double last_t,
  const double t,
  const double* last_y,
  int num_last_y,
  const double* last_yd,
  int num_last_yd,
  const double* last_ydd,
  int num_last_ydd,
  double* y,
  int num_y,
  double* yd,
  int num_yd,
  double* ydd,
  int num_ydd,
  const double* goal_y,
  int num_goal_y,
  const double* goal_yd,
  int num_goal_yd,
  const double* goal_ydd,
  int num_goal_ydd,
  const double* start_y,
  int num_start_y,
  const double* start_yd,
  int num_start_yd,
  const double* start_ydd,
  int num_start_ydd,
  const double goal_t,
  const double start_t,
  const double* weights,
  int num_weights_per_dim,
  int num_weight_dims,
  const double* widths,
  int num_widths,
  const double* centers,
  int num_centers,
  const double alpha_y,
  const double beta_y,
  const double alpha_z,
  const double integration_dt
)
{
  const int num_dimensions = num_last_y;
  if(start_t >= goal_t)
    throw std::invalid_argument("Goal must be chronologically after start!");

  assert(num_dimensions == num_last_y);
  assert(num_dimensions == num_last_yd);
  assert(num_dimensions == num_last_ydd);
  Eigen::Map<const Eigen::ArrayXd> last_y_array(last_y, num_last_y);
  Eigen::Map<const Eigen::ArrayXd> last_yd_array(last_yd, num_last_yd);
  Eigen::Map<const Eigen::ArrayXd> last_ydd_array(last_ydd, num_last_ydd);

  assert(num_dimensions == num_y);
  assert(num_dimensions == num_yd);
  assert(num_dimensions == num_ydd);
  Eigen::Map<Eigen::ArrayXd> y_array(y, num_y);
  Eigen::Map<Eigen::ArrayXd> yd_array(yd, num_yd);
  Eigen::Map<Eigen::ArrayXd> ydd_array(ydd, num_ydd);

  assert(num_dimensions == num_goal_y);
  assert(num_dimensions == num_goal_yd);
  assert(num_dimensions == num_goal_ydd);
  Eigen::Map<const Eigen::ArrayXd> goal_y_array(goal_y, num_goal_y);
  Eigen::Map<const Eigen::ArrayXd> goal_yd_array(goal_yd, num_goal_yd);
  Eigen::Map<const Eigen::ArrayXd> goal_ydd_array(goal_ydd, num_goal_ydd);

  assert(num_dimensions == num_start_y);
  assert(num_dimensions == num_start_yd);
  assert(num_dimensions == num_start_ydd);
  Eigen::Map<const Eigen::ArrayXd> start_y_array(start_y, num_start_y);
  Eigen::Map<const Eigen::ArrayXd> start_yd_array(start_yd, num_start_yd);
  Eigen::Map<const Eigen::ArrayXd> start_ydd_array(start_ydd, num_start_ydd);

  if(t <= start_t)
  {
    y_array = start_y_array;
    yd_array = start_yd_array;
    ydd_array = start_ydd_array;
  }
  else
  {
    const double execution_time = goal_t - start_t;

    // TODO only recompute iff start or goal state changes
    std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1> > > coefficients;
    solveConstraints(
        start_t, goal_t,
        start_y_array, start_yd_array, start_ydd_array,
        goal_y_array, goal_yd_array, goal_ydd_array,
        coefficients);

    y_array = last_y_array;
    yd_array = last_yd_array;
    ydd_array = last_ydd_array;
    Eigen::ArrayXd g(num_y);
    Eigen::ArrayXd gd(num_y);
    Eigen::ArrayXd gdd(num_y);

    assert(num_weights_per_dim == num_widths);
    assert(num_weights_per_dim == num_centers);
    assert(num_weight_dims == num_dimensions);
    Eigen::Map<const Eigen::ArrayXXd> weights_array(
        weights, num_dimensions, num_weights_per_dim);
    Eigen::Map<const Eigen::ArrayXd> widths_array(widths, num_widths);
    Eigen::Map<const Eigen::ArrayXd> centers_array(centers, num_centers);

    // We use multiple integration steps to improve numerical precision
    double current_t = last_t;
    while(current_t < t)
    {
      double dt_int = integration_dt;
      if(t - current_t < dt_int)
        dt_int = t - current_t;

      current_t += dt_int;

      const double z = phase(current_t, alpha_z, goal_t, start_t);
      const Eigen::ArrayXd f = forcingTerm(z, weights_array, widths_array, centers_array);

      applyConstraints(current_t, goal_y_array, goal_t, coefficients, g, gd, gdd);
      const double execution_time_squared = execution_time * execution_time;
      ydd_array = (alpha_y
                   * (beta_y * (g - y_array)
                      + execution_time * gd
                      - execution_time * yd_array)
                   + gdd * execution_time_squared + f)
                  / execution_time_squared;
      y_array += dt_int * yd_array;
      yd_array += dt_int * ydd_array;
    }
    assert(current_t == t);
  }
}

const double phase(
  const double t,
  const double alpha,
  const double goal_t,
  const double start_t
)
{
  const double int_dt = 0.001;
  const double execution_time = goal_t - start_t;
  const double b = std::max(1 - alpha * int_dt / execution_time, 1e-10);
  return pow(b, (t - start_t) / int_dt);
}

void solveConstraints(
  const double t0,
  const double t1,
  const Eigen::ArrayXd y0,
  const Eigen::ArrayXd y0d,
  const Eigen::ArrayXd y0dd,
  const Eigen::ArrayXd y1,
  const Eigen::ArrayXd y1d,
  const Eigen::ArrayXd y1dd,
  std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1> > >& coefficients
)
{
  const double t02 = t0 * t0;
  const double t03 = t02 * t0;
  const double t04 = t03 * t0;
  const double t05 = t04 * t0;
  const double t12 = t1 * t1;
  const double t13 = t12 * t1;
  const double t14 = t13 * t1;
  const double t15 = t14 * t1;

  Eigen::Matrix<double, 6, 6> M;
  M << 1,   t0,      t02,      t03,       t04,        t05,
       0,   1,   2 * t0,   3 * t02,   4 * t03,    5 * t04,
       0,   0,       2,    6 * t0,   12 * t02,   20 * t03,
       1,   t1,      t12,      t13,       t14,        t15,
       0,   1,   2 * t1,   3 * t12,   4 * t13,    5 * t14,
       0,   0,       2,    6 * t1,   12 * t12,   20 * t13;

  // Solve M*b = y for b in each DOF separately
  Eigen::PartialPivLU<Eigen::Matrix<double, 6, 6> > luOfM(M);
  coefficients.clear();
  coefficients.reserve(y0.size());
  Eigen::Matrix<double, 6, 1> x;
  for(unsigned i = 0; i < y0.size(); ++i)
  {
    x << y0[i], y0d[i], y0dd[i], y1[i], y1d[i], y1dd[i];
    coefficients.push_back(luOfM.solve(x));
  }
}

void applyConstraints(
  const double t, const Eigen::ArrayXd& goal_y, const double goal_t,
  const std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1> > >& coefficients,
  Eigen::ArrayXd& g, Eigen::ArrayXd& gd, Eigen::ArrayXd& gdd
)
{
  if(t > goal_t)
  {
    /**For t > goal_t the polynomial should always 'pull' to the goal position.
     * But velocity and acceleration should be zero.
     * This is done to avoid diverging from the goal if the dmp is executed
     * longer than expected. */
    g = goal_y;
    gd.setZero();
    gdd.setZero();
  }
  else
  {
    Eigen::Matrix<double, 1, 6> pos;
    Eigen::Matrix<double, 1, 6> vel;
    Eigen::Matrix<double, 1, 6> acc;
    const double t2 = t * t;
    const double t3 = t2 * t;
    const double t4 = t3 * t;
    const double t5 = t4 * t;
    pos << 1,   t,      t2,       t3,        t4,        t5;
    vel << 0,   1,   2 * t,   3 * t2,    4 * t3,    5 * t4;
    acc << 0,   0,       2,    6 * t,   12 * t2,   20 * t3;

    for(int i = 0; i < g.size(); ++i)
    {
      g[i] = pos * coefficients[i];
      gd[i] = vel * coefficients[i];
      gdd[i] = acc * coefficients[i];
    }
  }
}

void determineForces(
  const Eigen::ArrayXd& T,
  const Eigen::ArrayXXd& Y,
  Eigen::ArrayXXd& F,
  const double alpha_y,
  const double beta_y,
  bool allow_final_velocity
)
{
  assert(T.rows() == Y.cols());
  assert(T.rows() == F.cols());
  assert(Y.rows() == F.rows());
  const int num_dim = Y.rows();
  const int num_steps = Y.cols();

  Eigen::ArrayXXd Yd(num_dim, num_steps);
  gradient(Y, Yd, T, allow_final_velocity);
  Eigen::ArrayXXd Ydd(num_dim, num_steps);
  // The final acceleration is needs to be zero for the imitation learning to
  // work properly in Muelling DMPs.
  gradient(Yd, Ydd, T, false);

  //following code is equation (9) from [Muelling2012]
  Eigen::VectorXd start_y(Y.col(0));
  Eigen::VectorXd start_yd(Yd.col(0));
  Eigen::VectorXd start_ydd(Ydd.col(0));
  Eigen::VectorXd goal_y(Y.col(num_steps - 1));
  Eigen::VectorXd goal_yd(Yd.col(num_steps - 1));
  Eigen::VectorXd goal_ydd(Ydd.col(num_steps - 1));
  const double start_t = T(0);
  const double goal_t = T(num_steps - 1);

  std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1> > > coefficients;
  solveConstraints(start_t, goal_t, start_y, start_yd, start_ydd,
                   goal_y, goal_yd, goal_ydd, coefficients);

  Eigen::ArrayXd g(num_dim);
  Eigen::ArrayXd gd(num_dim);
  Eigen::ArrayXd gdd(num_dim);
  const double t = goal_t - start_t;
  const double t2 = t * t;

  for(int i = 0; i < num_steps; ++i)
  {
    applyConstraints(T(i), goal_y, goal_t, coefficients, g, gd, gdd);
    F.col(i) = t2 * Ydd.col(i)
               - alpha_y * (beta_y * (g - Y.col(i))
                            + gd * t
                            - Yd.col(i) * t)
               - t2 * gdd;
  }
}

const Eigen::MatrixXd rbfDesignMatrix(
  const Eigen::ArrayXd& T,
  const double alpha_z,
  const Eigen::ArrayXd& widths,
  const Eigen::ArrayXd& centers
)
{
  Eigen::MatrixXd X(centers.rows(), T.rows());
  for(int i = 0; i < T.rows(); i++)
  {
    const double z = phase(T(i), alpha_z, T(T.rows() - 1), T(0));
    X.col(i) = z * rbfActivations(z, widths, centers, true);
  }
  return X;
}

// pseudo inverse from http://eigen.tuxfamily.org/bz/show_bug.cgi?id=257
Eigen::MatrixXd pseudoInverse(const Eigen::MatrixXd& a, double epsilon = std::numeric_limits<double>::epsilon())
{
  Eigen::JacobiSVD<Eigen::MatrixXd> svd = a.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);

  double tolerance = epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff();

  return svd.matrixV() * Eigen::MatrixXd( (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().
      array().inverse(), 0) ).asDiagonal() * svd.matrixU().adjoint();
}

void ridgeRegression(
  const Eigen::MatrixXd& X,
  const Eigen::ArrayXXd& targets,
  const double regularization_coefficient,
  Eigen::Map<Eigen::ArrayXXd>& weights
)
{
  const int num_outputs = weights.rows();
  const int num_features = weights.cols();
  for(int i = 0; i < num_outputs; i++)
    weights.row(i) =
        (pseudoInverse(X * X.transpose()
          + regularization_coefficient * Eigen::MatrixXd::Identity(num_features, num_features)
          )
         * X * targets.row(i).transpose().matrix()
         ).transpose();
}

const Eigen::ArrayXd rbfActivations(
  const double z,
  const Eigen::ArrayXd& widths,
  const Eigen::ArrayXd& centers,
  const bool normalized
)
{
  Eigen::ArrayXd activations = (-widths * (z - centers).pow(2)).exp();
  if(normalized)
    activations /= activations.sum();
  return activations;
}

const Eigen::ArrayXd forcingTerm(
  const double z,
  const Eigen::ArrayXXd& weights,
  const Eigen::ArrayXd& widths,
  const Eigen::ArrayXd& centers
)
{
  const Eigen::ArrayXd activations = rbfActivations(z, widths, centers, true);
  return (z * weights.matrix() * activations.matrix()).array();
}

Eigen::Array3d qLog(const Eigen::Quaterniond& q)
{
  const double len = q.vec().norm();
  if(len == 0.0)
    return Eigen::Array3d::Zero();
  return q.vec().array() / len * acos(q.w());
}

Eigen::Quaterniond vecExp(const Eigen::Vector3d& input)
{
  const double len = input.norm();
  if(len != 0)
  {
    const Eigen::Array3d vec = sin(len) * input / len;
    return Eigen::Quaterniond(cos(len), vec.x(), vec.y(), vec.z());
  }
  else
  {
    return Eigen::Quaterniond::Identity();
  }
}

void quaternionDetermineForces(
  const Eigen::ArrayXd& T,
  const QuaternionVector& R,
  Eigen::ArrayXXd& F,
  const double alpha_y,
  const double beta_y,
  bool allow_final_velocity
)
{
  assert((size_t) T.rows() == R.size());
  assert(T.rows() == F.cols());
  assert((size_t) F.cols() == R.size());
  const int num_steps = T.rows();

  Eigen::ArrayXXd Rd(3, num_steps);
  quaternionGradient(R, Rd, T, allow_final_velocity);
  Eigen::ArrayXXd Rdd(3, num_steps);
  gradient(Rd, Rdd, T, false);

  const double t = T(num_steps - 1) - T(0);
  const double t2 = t * t;

  // Following code is equation (16) from [Ude2014] rearranged to $f_0$
  for(size_t i = 0; i < R.size(); ++i)
  {
    F.col(i) = t2 * Rdd.col(i)
               - (alpha_y * (beta_y * 2 * qLog(R.back() * R[i].conjugate())
                             - t * Rd.col(i)));
  }
}

void quaternionImitate(
  const double* T,
  int num_T,
  const double* R,
  int num_steps,
  int num_task_dims,
  double* weights,
  int num_weights_per_dim,
  int num_weight_dims,
  const double* widths,
  int num_widths,
  const double* centers,
  int num_centers,
  const double regularization_coefficient,
  const double alpha_r,
  const double beta_r,
  const double alpha_z,
  bool allow_final_velocity
)
{
  assert(num_steps == num_T);
  assert(num_task_dims == 4);
  assert(num_weights_per_dim == num_widths);
  assert(num_weights_per_dim == num_centers);
  assert(num_weight_dims == 3);

  if(regularization_coefficient < 0.0)
  {
    throw std::invalid_argument("Regularization coefficient must be >= 0!");
  }

  Eigen::Map<const Eigen::ArrayXd> widths_array(widths, num_widths);
  Eigen::Map<const Eigen::ArrayXd> centers_array(centers, num_centers);

  Eigen::ArrayXd T_array = Eigen::Map<const Eigen::ArrayXd>(T, num_T);
  Eigen::ArrayXXd R_array = Eigen::Map<const Eigen::ArrayXXd>(R, num_task_dims, num_steps);
  QuaternionVector rotationsVector;
  for(int i = 0; i < num_steps; ++i)
  {
    Eigen::Quaterniond q(R_array(0, i), R_array(1, i), R_array(2, i), R_array(3, i));
    q.normalize(); // has to be done to avoid nans
    rotationsVector.push_back(q);
  }
  Eigen::ArrayXXd F(num_weight_dims, num_steps);
  quaternionDetermineForces(T_array, rotationsVector, F, alpha_r, beta_r, allow_final_velocity);

  const Eigen::MatrixXd X = rbfDesignMatrix(T_array, alpha_z, widths_array, centers_array);

  Eigen::Map<Eigen::ArrayXXd> weights_array(weights, num_weight_dims, num_weights_per_dim);
  ridgeRegression(X, F, regularization_coefficient, weights_array);
}

void quaternionDmpStep(
  const double last_t,
  const double t,
  const double* last_r,
  int num_last_r,
  const double* last_rd,
  int num_last_rd,
  const double* last_rdd,
  int num_last_rdd,
  double* r,
  int num_r,
  double* rd,
  int num_rd,
  double* rdd,
  int num_rdd,
  const double* goal_r,
  int num_goal_r,
  const double* goal_rd,
  int num_goal_rd,
  const double* goal_rdd,
  int num_goal_rdd,
  const double* start_r,
  int num_start_r,
  const double* start_rd,
  int num_start_rd,
  const double* start_rdd,
  int num_start_rdd,
  const double goal_t,
  const double start_t,
  const double* weights,
  int num_weights_per_dim,
  int num_weight_dims,
  const double* widths,
  int num_widths,
  const double* centers,
  int num_centers,
  const double alpha_r,
  const double beta_r,
  const double alpha_z,
  const double integration_dt
)
{
  if(start_t >= goal_t)
    throw std::invalid_argument("Goal must be chronologically after start!");

  assert(4 == num_last_r);
  assert(3 == num_last_rd);
  assert(3 == num_last_rdd);
  const Eigen::Quaterniond last_r_array(last_r[0], last_r[1], last_r[2], last_r[3]);
  Eigen::Map<const Eigen::ArrayXd> last_rd_array(last_rd, num_last_rd);
  Eigen::Map<const Eigen::ArrayXd> last_rdd_array(last_rdd, num_last_rdd);

  assert(4 == num_r);
  assert(3 == num_rd);
  assert(3 == num_rdd);
  Eigen::Quaterniond r_array(r[0], r[1], r[2], r[3]);
  Eigen::Map<Eigen::ArrayXd> rd_array(rd, num_rd);
  Eigen::Map<Eigen::ArrayXd> rdd_array(rdd, num_rdd);

  assert(4 == num_goal_r);
  assert(3 == num_goal_rd);
  assert(3 == num_goal_rdd);
  const Eigen::Quaterniond goal_r_array(goal_r[0], goal_r[1], goal_r[2], goal_r[3]);
  Eigen::Map<const Eigen::ArrayXd> goal_rd_array(goal_rd, num_goal_rd);
  Eigen::Map<const Eigen::ArrayXd> goal_rdd_array(goal_rdd, num_goal_rdd);

  assert(4 == num_start_r);
  assert(3 == num_start_rd);
  assert(3 == num_start_rdd);
  const Eigen::Quaterniond start_r_array(start_r[0], start_r[1], start_r[2], start_r[3]);
  Eigen::Map<const Eigen::ArrayXd> start_rd_array(start_rd, num_start_rd);
  Eigen::Map<const Eigen::ArrayXd> start_rdd_array(start_rdd, num_start_rdd);

  if(t <= start_t)
  {
    r_array = start_r_array;
    rd_array = start_rd_array;
    rdd_array = start_rdd_array;
  }
  else
  {
    const double execution_time = goal_t - start_t;

    r_array = last_r_array;
    rd_array = last_rd_array;
    rdd_array = last_rdd_array;

    assert(num_weights_per_dim == num_widths);
    assert(num_weights_per_dim == num_centers);
    assert(num_weight_dims == 3);
    Eigen::Map<const Eigen::ArrayXXd> weights_array(
        weights, num_weight_dims, num_weights_per_dim);
    Eigen::Map<const Eigen::ArrayXd> widths_array(widths, num_widths);
    Eigen::Map<const Eigen::ArrayXd> centers_array(centers, num_centers);

    // We use multiple integration steps to improve numerical precision
    double current_t = last_t;
    while(current_t < t)
    {
      double dt_int = integration_dt;
      if(t - current_t < dt_int)
        dt_int = t - current_t;

      current_t += dt_int;

      const double z = phase(current_t, alpha_z, goal_t, start_t);
      const Eigen::ArrayXd f = forcingTerm(z, weights_array, widths_array, centers_array);

      const double execution_time_squared = execution_time * execution_time;

      rdd_array = (alpha_r * (beta_r * 2.0 * qLog(goal_r_array * r_array.conjugate())
                              - execution_time * rd_array)
                   + f)
                  / execution_time_squared;
      r_array = vecExp(dt_int / 2.0 * rd_array) * r_array;
      rd_array += dt_int * rdd_array;
    }
    assert(current_t == t);
  }

  r[0] = r_array.w();
  r[1] = r_array.x();
  r[2] = r_array.y();
  r[3] = r_array.z();
}

namespace internal
{

void compute_gradient(
  const double* in,
  int num_in_steps,
  int num_in_dims,
  double* out,
  int num_out_steps,
  int num_out_dims,
  const double* time,
  int num_time,
  bool allow_final_velocity
)
{
  assert(num_in_steps == num_time);
  assert(num_out_steps == num_time);
  assert(num_in_dims == num_out_dims);
  Eigen::Map<const Eigen::ArrayXXd> in_array(in, num_in_dims, num_time);
  Eigen::Map<Eigen::ArrayXXd> out_array(out, num_out_dims, num_time);
  Eigen::Map<const Eigen::ArrayXd> time_array(time, num_time);
  gradient(in_array, out_array, time_array, allow_final_velocity);
}

void compute_quaternion_gradient(
  const double* in,
  int num_in_steps,
  int num_in_dims,
  double* out,
  int num_out_steps,
  int num_out_dims,
  const double* time,
  int num_time,
  bool allow_final_velocity
)
{
  assert(num_in_steps == num_time);
  assert(num_out_steps == num_time);
  assert(num_in_dims == 4);
  assert(num_out_dims == 3);

  Eigen::Map<const Eigen::ArrayXXd> in_array(in, num_in_dims, num_time);
  QuaternionVector rotationsVector;
  for(int i = 0; i < num_time; ++i)
  {
    Eigen::Quaterniond q(in_array(0, i), in_array(1, i), in_array(2, i), in_array(3, i));
    q.normalize(); // has to be done to avoid nans
    rotationsVector.push_back(q);
  }
  Eigen::Map<Eigen::ArrayXXd> out_array(out, num_out_dims, num_time);
  Eigen::Map<const Eigen::ArrayXd> time_array(time, num_time);
  quaternionGradient(rotationsVector, out_array, time_array, allow_final_velocity);
}

} // namespace internal

} // namespace Dmp


/*
 * Copyright (c) 2019, ETH Zurich
 */

#include <boost/math/common_factor.hpp>

#include <algorithm>
#include <cassert>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <set>
#include <vector>

#include "Access.h"
#include "Timer.h"

#include "barvinok/isl.h"
#include "isl-helpers.h"
#include "op.h"

void Access::initAccess(std::vector<NamedLong> ParameterValues, isl::set Parameters) {
  // set the parametes
  ParameterValues_ = ParameterValues;
  Parameters_ = Parameters;
  // compute the number of cache levels
  long CacheLevels = MachineModel_.CacheSizes.size();
  // number of cache misses
  Result_.CompulsoryMisses = 0;
  Result_.CapacityMisses = std::vector<long>(CacheLevels, 0);
  Result_.Counted = 0;
  // compute the domain size
  Result_.Total = isl::cardinality(Domain_);
  // clear the stack distances
  Misses_ = 0;
  Affine_.clear();
  NonAffine_.clear();
  Constant_.clear();
}

void Access::countCompulsoryMisses(isl::union_map First) {
  Timer::startTimer("CountCompulsoryMisses");
  // extract access first map
  First = First.intersect_domain(Domain_);
  if (!First.is_empty()) {
    // intersect with the parameter values
    if (!Parameters_.is_null())
      First = First.intersect_params(Parameters_);
    // simplify map
    First = First.coalesce();
    First = First.detect_equalities();
    // count the individual sets
    auto countCompulsory = [&](isl::set Set) {
      Result_.CompulsoryMisses += isl::cardinality(Set);
      return isl::stat::ok();
    };
    First.range().foreach_set(countCompulsory);
  }
  Timer::stopTimer("CountCompulsoryMisses");
}

void Access::computeStackDistances(isl::union_map BetweenMap) {
  Timer::startTimer("ComputeStackDistances");
  // extract access between map
  BetweenMap = BetweenMap.intersect_domain(Domain_);
  if (!BetweenMap.is_empty()) {
    if (!Parameters_.is_null())
      BetweenMap = BetweenMap.intersect_params(Parameters_);
    // simplify between maps
    BetweenMap = BetweenMap.detect_equalities();
    // compute stack distance bounds
    if (ModelOptions_.ComputeBounds) {
      // compute the set of obvious cache misses
      Timer::startTimer("ComputeBounds");
      long Limit = *std::max_element(MachineModel_.CacheSizes.begin(), MachineModel_.CacheSizes.end());
      Limit /= MachineModel_.CacheLineSize;
      isl::union_set Misses = BetweenMap.domain().empty(BetweenMap.domain().get_space());
      isl::union_set Domain = BetweenMap.domain();
      // compute number of maps
      auto countMap = [&](isl::map Map) {
        auto countBasicMap = [&](isl::basic_map BasicMap) {
          // check if the map is already part of the miss set
          if (!Domain.intersect(BasicMap.domain()).is_empty()) {
            // compute the lower bound for the map
            auto Map = isl::map(BasicMap);
            auto Norm = Map.sum(Map.lexmin().neg());
            long UpperBound = isl::cardinality(Norm.range());
            if (UpperBound > Limit) {
              auto Complement = Norm.complement().intersect_domain(Map.domain());
              long LowerBound = isl::cardinality(Complement.range().complement().intersect(Norm.range()));
              if (LowerBound > Limit) {
                Misses = Misses.unite(BasicMap.domain());
                Domain = Domain.subtract(BasicMap.domain());
              }
            }
          }
          return isl::stat::ok();
        };
        Map.foreach_basic_map(countBasicMap);
        return isl::stat::ok();
      };
      BetweenMap.foreach_map(countMap);
      // count the domains with stack distance bound larger than the maximal cache size
      if (!Misses.is_empty()) {
        // count the misses
        auto countMisses = [&](isl::set Set) {
          long Count = isl::cardinality(Set);
          Result_.Counted += Count;
          Misses_ += Count;
          return isl::stat::ok();
        };
        Misses.foreach_set(countMisses);
        // update the between map
        BetweenMap = BetweenMap.subtract_domain(Misses);
      }
      Timer::stopTimer("ComputeBounds");
    }
    // compute the stack distance for the remaining points of the iteration domain
    Timer::startTimer("CountBetweenMap");
    Expression_.clear();
    auto Count = isl::manage(isl_union_map_card(isl::union_map(BetweenMap).release()));
    Count = Count.intersect_domain(BetweenMap.domain());
    auto countMapDomain = [&](isl::set Set) {
      Result_.Counted += isl::cardinality(Set);
      return isl::stat::ok();
    };
    isl::union_map(BetweenMap).domain().foreach_set(countMapDomain);
    Timer::stopTimer("CountBetweenMap");
    Timer::stopTimer("ComputeStackDistances");
    Timer::startTimer("CountCapacityMisses");
    // extract the cache miss expression
    extractStackDistanceExpression(Count);
    // count affine pieces immediately
    storeAffinePieces();
#ifdef EQUALIZATION
    // perform floor term equalization
    applyEqualization();
    // count affine pieces again
    storeAffinePieces();
#endif
#ifdef RASTERIZATION
    // perform floor term rasterization
    applyRasterization();
    // count affine pieces again
    storeAffinePieces();
#endif
    // process all non affine pieces
    for (auto &Piece : Expression_) {
      enumerateNonAffineDimensions(Piece);
    }
    Timer::stopTimer("CountCapacityMisses");
  }
}

void Access::countCapacityMisses() {
  // compute the capacity misses for the machine
  Result_.CapacityMisses = countCapacityMisses(MachineModel_.CacheSizes);
}

std::vector<long> Access::countCapacityMisses(std::vector<long> CacheSizes) {
  Timer::startTimer("CountCapacityMisses");
  if (ModelOptions_.ComputeBounds) {
    // verify the cache sizes do not exceed the maximum cache size of the machine
    // (the bounds were computed for the maximal cache size)
    if (*std::max_element(MachineModel_.CacheSizes.begin(), MachineModel_.CacheSizes.end()) <
        *std::max_element(CacheSizes.begin(), CacheSizes.end())) {
      printf("-> exit(-1) cache size exceeds maximum cache size of the machine\n");
      exit(-1);
    }
  }
  // initializes the limit and result vectors
  std::vector<long> Results;
  std::vector<long> Limits;
  for (auto Size : CacheSizes) {
    Results.push_back(Misses_);
    Limits.push_back(Size / MachineModel_.CacheLineSize);
  }
  // compute the cache misses for the constant domains
  for (auto &Domain : Constant_) {
    for (int i = 0; i < Limits.size(); ++i) {
      Results[i] += Domain.second.gt(isl::val(Domain.second.get_ctx(), Limits[i])) ? Domain.first : 0;
    }
  }
  // compute the cache misses for the affine pieces
  for (auto &Piece : Affine_) {
    auto Misses = countAffineDimensions(Piece, Limits);
    std::transform(Results.begin(), Results.end(), Misses.begin(), Results.begin(), std::plus<long>());
  }
  // compute the cache misses for the non-affine pieces
  for (auto &Piece : NonAffine_) {
    auto Misses = enumerateNonAffinePoints(Piece, Limits);
    std::transform(Results.begin(), Results.end(), Misses.begin(), Results.begin(), std::plus<long>());
  }
  Timer::stopTimer("CountCapacityMisses");
  return Results;
}

void Access::storeAffinePieces() {
  // count and remove all affine pieces
  std::vector<piece> NonAffine;
  for (auto &Piece : Expression_) {
    if (!Piece.Expression.is_null()) {
      long All = getPieceSize(Piece);
      Affine_.push_back(Piece);
    } else {
      NonAffine.push_back(Piece);
    }
  }
  std::swap(Expression_, NonAffine);
}

std::vector<long> Access::enumerateNonAffinePoints(piece Piece, std::vector<long> Limits) const {
  Timer::startTimer("enumerateNonAffinePoints");
  std::vector<long> Results(Limits.size(), 0);
  auto countMisses = [&](isl::point Point) {
    isl::val StackDistance = Piece.Polynomial.eval(Point);
    for (int i = 0; i < Limits.size(); ++i) {
      if (isl::get_value(StackDistance) > Limits[i])
        Results[i]++;
    }
    return isl::stat::ok();
  };
  Piece.Domain.foreach_point(countMisses);
  Timer::stopTimer("enumerateNonAffinePoints");
  return Results;
}

std::vector<long> Access::countAffineDimensions(piece Piece, std::vector<long> Limits) const {
  Timer::startTimer("countAffineDimensions");
  std::vector<long> Results(Limits.size());
  isl::pw_aff LHS = Piece.Expression;
  for (int i = 0; i < Limits.size(); ++i) {
    isl::pw_aff RHS(Piece.Domain, isl::val(Piece.Domain.get_ctx(), Limits[i]));
    isl::set Misses = LHS.gt_set(RHS);
    auto Variable = isl::manage(isl_set_card(Misses.release()));
    assert(isl::get_value(Variable.min()) == isl::get_value(Variable.max()));
    Results[i] = isl::get_value(Variable.max());
  }
  Timer::stopTimer("countAffineDimensions");
  return Results;
}

std::vector<int> Access::findNonAffineDimensions(piece Piece) const {
  // compute the non-affine dimensions and the conflicting dimensions
  std::set<int> NonAffine;
  std::map<int, std::set<int>> Conflicts;
  // first analyze all terms
  auto analyzeTerms = [&](isl::term Term) {
    // compute the parameter and variable exponent vectors
    std::vector<int> Parameters(Term.dim(isl::dim::param), 0);
    std::vector<int> Variables(Term.dim(isl::dim::set), 0);
    // count the parameter exponents
    for (int i = 0; i < Term.dim(isl::dim::param); i++) {
      Parameters[i] += Term.get_exp(isl::dim::param, i);
    }
    // count the variable exponents
    for (int i = 0; i < Term.dim(isl::dim::set); i++) {
      Variables[i] += Term.get_exp(isl::dim::set, i);
    }
    // get the divisor variable
    for (int i = 0; i < Term.dim(isl::dim::div); i++) {
      if (Term.get_exp(isl::dim::div, i) > 0) {
        isl::aff Divisor = Term.get_div(i);
        // extract the parameter and variable information
        for (int j = 0; j < Divisor.dim(isl::dim::param); ++j) {
          isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::param, j);
          if (!Coefficient.is_zero())
            Parameters[j] += Term.get_exp(isl::dim::div, i);
        }
        for (int j = 0; j < Divisor.dim(isl::dim::in); ++j) {
          isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::in, j);
          if (!Coefficient.is_zero())
            Variables[j] += Term.get_exp(isl::dim::div, i);
        }
      }
    }
    // we do not support parameters
    assert(std::accumulate(Parameters.begin(), Parameters.end(), 0) == 0);
    // update the non-affine variables
    std::vector<int> All;
    for (int i = 0; i < Variables.size(); ++i) {
      if (Variables[i] > 1)
        NonAffine.insert(i);
      if (Variables[i] > 0)
        All.push_back(i);
    }
    // update the conflicts
    for (auto Variable : All) {
      for (auto Conflict : All) {
        if (Conflict != Variable)
          Conflicts[Variable].insert(Conflict);
      }
    }
    return isl::stat::ok();
  };
  Piece.Polynomial.foreach_term(analyzeTerms);
  // compute the minimal set of non-affine Dimensions
  std::vector<int> Dimensions;
  auto storeAndRemoveFromConflicts = [&](int Dimension) {
    // remove from conflicts
    Conflicts.erase(Dimension);
    for (auto Iter = Conflicts.begin(); Iter != Conflicts.end();) {
      Iter->second.erase(Dimension);
      if (Iter->second.empty()) {
        Conflicts.erase(Iter++);
      } else {
        ++Iter;
      }
    }
    // store the dimension
    Dimensions.push_back(Dimension);
  };
  // always add all non-affine dimensions
  for (auto Dimension : NonAffine) {
    storeAndRemoveFromConflicts(Dimension);
  }
  // eliminate all conflicts
  while (!Conflicts.empty()) {
    int Maximum = 0;
    int Dimension = 0;
    for (auto Conflict : Conflicts) {
      if (Conflict.second.size() > Maximum) {
        Maximum = Conflict.second.size();
        Dimension = Conflict.first;
      }
    }
    assert(Maximum != 0);
    // store the dimension and
    storeAndRemoveFromConflicts(Dimension);
  }
  // return the sorted dimensions
  std::sort(Dimensions.begin(), Dimensions.end());
  return Dimensions;
}

void Access::enumerateNonAffineDimensions(piece Piece) {
  Timer::startTimer("enumerateNonAffineDimensions");
  // count total number of accesses
  long All = getPieceSize(Piece);
#ifdef ENUMERATE_POINTS
  // count manually if only the memory access dimension is non-affine
  NonAffine_.push_back(Piece);
#else
  // compute the dimensions
  std::vector<int> NonAffine = findNonAffineDimensions(Piece);
  std::vector<int> Affine;
  for (int i = 0; i < Piece.Domain.dim(isl::dim::set); ++i) {
    auto Iter = std::find(NonAffine.begin(), NonAffine.end(), i);
    if (Iter == NonAffine.end())
      Affine.push_back(i);
  }
  // enumerate and count the affine subdomains
  if (Affine.size() <= 1 || All == 0) {
    // count manually if only the memory access dimension is non-affine
    NonAffine_.push_back(Piece);
  } else {
    // compute the enumeration domain
    isl::set Enumeration = Piece.Domain;
    for (auto Iter = Affine.rbegin(); Iter != Affine.rend(); Iter++) {
      Enumeration = Enumeration.project_out(isl::dim::set, *Iter, 1);
    }
    // check if there is enough work for barvinok
    if (All < 16 || All / isl::cardinality(Enumeration) < 16) {
      NonAffine_.push_back(Piece);
    } else {
      // compute the piecewise polynomial
      auto countCacheMisses = [&](isl::basic_set All) {
        isl::set Enumeration = All;
        for (auto Iter = Affine.rbegin(); Iter != Affine.rend(); Iter++) {
          Enumeration = Enumeration.project_out(isl::dim::set, *Iter, 1);
        }
        // enumerate the points of the enumeration domain
        auto countSubdomain = [&](isl::point Point) {
          // compute the updated domain of the piece
          std::map<int, long> Values;
          auto Domain = All;
          for (int i = 0; i < Point.get_space().dim(isl::dim::set); ++i) {
            auto Value = Point.get_coordinate_val(isl::dim::set, i);
            Domain = Domain.fix_val(isl::dim::set, NonAffine[i], Value);
            Values[NonAffine[i]] = isl::get_value(Value);
          }
          auto Expression = extractAffineExpression(Piece.Polynomial, Domain, Values);
          // handle constant expressions efficiently
          if (Expression.is_cst()) {
            long Size = isl::cardinality(Domain);
            auto Constant = Expression.get_constant_val();
            Constant_.push_back(std::make_pair(Size, Constant));
          } else {
            piece Piece;
            Piece.Domain = Domain;
            Piece.Expression = Expression;
            Affine_.push_back(Piece);
          }
          return isl::stat::ok();
        };
        Enumeration.foreach_point(countSubdomain);
        return isl::stat::ok();
      };
      // we evaluate the basic sets separately since otherwise foreach counts some points multiple times
      Piece.Domain = isl::manage(isl_set_make_disjoint(Piece.Domain.release()));
      Piece.Domain.foreach_basic_set(countCacheMisses);
    }
  }
#endif
  Timer::stopTimer("enumerateNonAffineDimensions");
}

void Access::applyEqualization() {
  Timer::startTimer("applyEqualization");
  // convert the terms to to piecewise aff expressions
  std::vector<piece> Expression;
  for (auto Piece : Expression_) {
    // try to eliminate non affine terms
    std::vector<piece> Pieces = {Piece};
    while (!Pieces.empty()) {
      // compute possible candidates and select the one with the highest exponent
      // (doe not split polynoms with a single divisor term)
      auto Candidates = findEqualizationCandidates(Pieces.back());
      std::vector<std::vector<long>> Splits;
      std::vector<int> Indexes;
      for (auto Candidate : Candidates) {
        Splits.push_back(computeSplits(Candidate, Pieces.back()));
      }
      for (int i = 0; i < Candidates.size(); ++i) {
        Indexes.push_back(i);
      }
      std::sort(Indexes.begin(), Indexes.end(), [&](int I1, int I2) { return Splits[I1].size() < Splits[I2].size(); });
      // classify the updates
      std::vector<piece> Done;
      std::vector<piece> Empty;
      std::vector<piece> Improved;
      // try to eliminate all the candidates and stop on success
      for (auto Index : Indexes) {
        std::vector<piece> Updates = equalizeCandidate(Candidates[Index], Splits[Index], Pieces.back());
        // store updates if the exponent improves
        if (!Updates.empty()) {
          for (auto Update : Updates) {
            if (Update.Polynomial.is_null()) {
              Empty.push_back(Update);
            } else if (isPieceAffine(Update)) {
              Update.Expression = extractAffineExpression(Update);
              Done.push_back(Update);
            } else {
              Improved.push_back(Update);
            }
          }
          break;
        }
      }
      // keep the updates or discard everything
      if (Improved.empty() && Done.empty() && Empty.empty()) {
        Expression.push_back(Pieces.back());
        Pieces.pop_back();
      } else {
        Pieces.pop_back();
        Pieces.insert(Pieces.end(), Improved.begin(), Improved.end());
        Expression.insert(Expression.end(), Done.begin(), Done.end());
      }
    }
  }
  // update the expression
  std::swap(Expression_, Expression);
  Timer::stopTimer("applyEqualization");
}

void Access::applyRasterization() {
  Timer::startTimer("applyRasterization");
  // compute the extent of the domain
  std::vector<long> Sizes;
  for (int i = 0; i < Domain_.dim(isl::dim::set); ++i) {
    long LowerBound = isl::computeMinimum(Domain_, i);
    long UpperBound = isl::computeMaximum(Domain_, i);
    Sizes.push_back(1 + UpperBound - LowerBound);
  }
  // raster the dimensions to eliminate even more terms
  std::vector<piece> Expression;
  for (auto &Piece : Expression_) {
    // compute the raster information
    std::vector<int> Dimensions = findRasterDimensions(Piece);
    std::vector<isl::val> Multipliers = computeMultipliers(Dimensions, Piece);
    // filter dimensions with multiplier larger than size
    for (int i = Dimensions.size() - 1; i >= 0; --i) {
      if (Sizes[Dimensions[i]] <= isl::get_value(Multipliers[i])) {
        Dimensions.erase(Dimensions.begin() + i);
        Multipliers.erase(Multipliers.begin() + i);
      }
    }
    // oder the dimensions by the multiplier values
    std::vector<int> Indexes;
    for (int i = 0; i < Dimensions.size(); ++i) {
      Indexes.push_back(i);
    }
    std::sort(Indexes.begin(), Indexes.end(), [&](int I1, int I2) { return Multipliers[I1].lt(Multipliers[I2]); });
    // compute the domain size
    long All = getPieceSize(Piece);
    // raster the pieces dimension by dimension
    std::vector<piece> Current = {Piece};
    std::vector<piece> Next;
    // try to make all dimensions affine
    bool Updated = false;
    for (auto Index : Indexes) {
      // skip the dimension if the average subdomain would be small
      if (All / (Current.size() * isl::get_value(Multipliers[Index])) < 16)
        continue;
      // try to raster the dimension
      Next.clear();
      bool IsSuccess = true;
      for (auto Piece : Current) {
        std::vector<piece> Updates = rasterDimension(Dimensions[Index], Multipliers[Index], Piece);
        if (Updates.empty()) {
          IsSuccess = false;
          break;
        }
        Next.insert(Next.end(), Updates.begin(), Updates.end());
      }
      // swap the containers if successfull
      if (IsSuccess) {
        std::swap(Current, Next);
        Updated = true;
      }
    }
    // if the domain was split preprocess the affine pieces
    if (Updated) {
      for (auto &Piece : Current) {
        if (Piece.Expression.is_null() && isPieceAffine(Piece))
          Piece.Expression = extractAffineExpression(Piece);
      }
    }
    // store the updated pieces
    Expression.insert(Expression.end(), Current.begin(), Current.end());
  }
  // update the expression
  std::swap(Expression_, Expression);
  Timer::stopTimer("applyRasterization");
}

int Access::computeExponent(piece Piece) const {
  int Result = 0;
  // check all terms
  auto analyzeTerms = [&](isl::term Term) {
    int Exponent = 0;
    // count the parameter exponents
    for (int i = 0; i < Term.dim(isl::dim::param); i++) {
      Exponent += Term.get_exp(isl::dim::param, i);
    }
    // count the variable exponents
    for (int i = 0; i < Term.dim(isl::dim::set); i++) {
      Exponent += Term.get_exp(isl::dim::set, i);
    }
    // get the divisor variable
    for (int i = 0; i < Term.dim(isl::dim::div); i++) {
      Exponent += Term.get_exp(isl::dim::div, i);
    }
    Result = std::max(Result, Exponent);
    return isl::stat::ok();
  };
  Piece.Polynomial.foreach_term(analyzeTerms);
  return Result;
}

int Access::computeDimensionExponent(int Dimension, piece Piece) const {
  int Result = 0;
  // check all terms
  auto analyzeTerms = [&](isl::term Term) {
    int Exponent = 0;
    if (Dimension < Term.dim(isl::dim::set))
      Exponent += Term.get_exp(isl::dim::set, Dimension);
    // get the divisor variable
    for (int i = 0; i < Term.dim(isl::dim::div); i++) {
      if (Term.get_exp(isl::dim::div, i) > 0) {
        isl::aff Divisor = Term.get_div(i);
        if (Dimension < Divisor.dim(isl::dim::in) && !Divisor.get_coefficient_val(isl::dim::in, Dimension).is_zero())
          Exponent += Term.get_exp(isl::dim::div, i);
      }
    }
    Result = std::max(Result, Exponent);
    return isl::stat::ok();
  };
  Piece.Polynomial.foreach_term(analyzeTerms);
  return Result;
}

isl::qpolynomial Access::computeReplacement(std::map<int, isl::qpolynomial> Replacements, piece Piece) const {
  isl::space Space = Piece.Polynomial.get_domain_space();
  isl::qpolynomial Polynomial = isl::qpolynomial::zero_on_domain(Space);
  auto updatePolynomial = [&](isl::term Term) {
    bool Replace = false;
    for (int i = 0; i < Term.dim(isl::dim::div); i++) {
      if (Term.get_exp(isl::dim::div, i) > 0 && Replacements.count(i) == 1)
        Replace = true;
    }
    // compute replacement term
    if (Replace) {
      // compute the update term
      isl::qpolynomial Update = isl::qpolynomial::val_on_domain(Space, Term.get_coefficient_val());
      // multiply with the parameters
      for (int i = 0; i < Term.dim(isl::dim::param); ++i) {
        int Exponent = Term.get_exp(isl::dim::param, i);
        Update = Update.mul(isl::qpolynomial::var_on_domain(Space, isl::dim::param, i).pow(Exponent));
      }
      // multiply with variables
      for (int i = 0; i < Term.dim(isl::dim::set); ++i) {
        int Exponent = Term.get_exp(isl::dim::set, i);
        Update = Update.mul(isl::qpolynomial::var_on_domain(Space, isl::dim::set, i).pow(Exponent));
      }
      // compute the divisors
      for (int i = 0; i < Term.dim(isl::dim::div); ++i) {
        int Exponent = Term.get_exp(isl::dim::div, i);
        if (Exponent > 0) {
          if (Replacements.count(i) == 1) {
            Update = Update.mul(Replacements[i].pow(Exponent));
          } else
            Update = Update.mul(isl::qpolynomial::from_aff(Term.get_div(i).floor()).pow(Exponent));
        }
      }
      // update the polynomial
      Polynomial = Polynomial.add(Update);
    } else {
      Polynomial = Polynomial.add(isl::qpolynomial::from_term(Term));
    }
    return isl::stat::ok();
  };
  Piece.Polynomial.foreach_term(updatePolynomial);
  return Polynomial;
}

std::vector<int> Access::findRasterDimensions(piece Piece) const {
  // count the dimension exponent pairs for all and divisor terms
  std::map<std::pair<int, int>, int> Counts;
  std::map<std::pair<int, int>, int> DCounts;
  auto analyzeTerms = [&](isl::term Term) {
    // compute the variable exponents
    int Parameters = 0;
    std::map<int, int> Variables;
    std::map<int, int> DVariables;
    // count the parameter exponents
    for (int i = 0; i < Term.dim(isl::dim::param); i++) {
      Parameters += Term.get_exp(isl::dim::param, i);
    }
    // count the variable exponents
    for (int i = 0; i < Term.dim(isl::dim::set); i++) {
      Variables[i] += Term.get_exp(isl::dim::set, i);
    }
    // get the divisor variable
    for (int i = 0; i < Term.dim(isl::dim::div); i++) {
      if (Term.get_exp(isl::dim::div, i) > 0) {
        isl::aff Divisor = Term.get_div(i);
        // extract the parameter and variable information
        for (int j = 0; j < Divisor.dim(isl::dim::param); ++j) {
          isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::param, j);
          if (!Coefficient.is_zero())
            Parameters += Term.get_exp(isl::dim::div, i);
        }
        for (int j = 0; j < Divisor.dim(isl::dim::in); ++j) {
          isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::in, j);
          if (!Coefficient.is_zero()) {
            Variables[j] += Term.get_exp(isl::dim::div, i);
            DVariables[j] += Term.get_exp(isl::dim::div, i);
          }
        }
      }
    }
    // update the counts for parameter free terms
    if (Parameters == 0) {
      for (auto Variable : Variables) {
        Counts[Variable]++;
        if (DVariables[Variable.first] > 0)
          DCounts[Variable]++;
      }
    }
    return isl::stat::ok();
  };
  Piece.Polynomial.foreach_term(analyzeTerms);
  // compute the candidate dimensions
  std::vector<int> Dimensions;
  for (auto Count : Counts) {
    // select the dimensions that appear multiple times with exponent > 1
    // only consider dimensions with divisors
    if (Count.second > 1 && Count.first.second > 1 && DCounts[Count.first] > 0)
      Dimensions.push_back(Count.first.first);
  }
  // make dimensions unique
  std::sort(Dimensions.begin(), Dimensions.end());
  Dimensions.erase(std::unique(Dimensions.begin(), Dimensions.end()), Dimensions.end());
  return Dimensions;
}

std::vector<isl::val> Access::computeMultipliers(std::vector<int> Dimensions, piece Piece) const {
  // compute the least common multipliers for all dimensions
  std::vector<long> LCM(Piece.Polynomial.dim(isl::dim::in), 1);
  // process every divisor exactly once
  std::set<int> Divisors;
  auto findMultipliers = [&](isl::term Term) {
    // get the divisor variable
    for (int i = 0; i < Term.dim(isl::dim::div); i++) {
      if (Divisors.count(i) == 0 && Term.get_exp(isl::dim::div, i) > 0) {
        Divisors.insert(i);
        isl::aff Divisor = Term.get_div(i);
        // count parameters and find variables
        std::map<int, isl::val> Variables;
        // extract the variable information
        for (int j = 0; j < Divisor.dim(isl::dim::in); ++j) {
          isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::in, j);
          if (!Coefficient.is_zero())
            Variables[j] = Coefficient;
        }
        // compute the multipliers for all variables of the term
        for (auto Dimension : Dimensions) {
          if (Variables.count(Dimension) == 1) {
            long Coefficient = isl::get_value(Variables[Dimension].mul(Divisor.get_denominator_val()));
            long Denominator = isl::get_value(Divisor.get_denominator_val());
            Denominator = Denominator / boost::math::gcd(Coefficient, Denominator);
            LCM[Dimension] = boost::math::lcm(LCM[Dimension], Denominator);
          }
        }
      }
    }
    return isl::stat::ok();
  };
  Piece.Polynomial.foreach_term(findMultipliers);
  // compute the multipliers
  std::vector<isl::val> Multipliers;
  for (auto Dimension : Dimensions) {
    Multipliers.push_back(isl::val(Piece.Domain.get_ctx(), LCM[Dimension]));
  }
  return Multipliers;
}

std::vector<piece> Access::rasterDimension(int Dimension, isl::val Multiplier, piece Piece) const {
  isl::local_space DS = isl::local_space(Piece.Domain.get_space());
  isl::local_space PS = isl::local_space(Piece.Polynomial.get_domain_space());
  // result vector
  std::vector<piece> Pieces;
  // raster the domain with all possible modulo value
  for (int Current = 0; Current < isl::get_value(Multiplier); ++Current) {
    // compute the domain
    isl::set Domain = Piece.Domain;
    isl::pw_aff Modulo = isl::pw_aff::var_on_domain(DS, isl::dim::set, Dimension) % Multiplier;
    isl::set Element = Modulo.eq_set(isl::aff(DS, isl::val(Domain.get_ctx(), Current)));
    Domain = Domain.intersect(Element);
    // compute the polynomial
    isl::qpolynomial Polynomial;
    if (!Domain.is_empty()) {
      // compute replacement divisors
      std::map<int, isl::qpolynomial> Replacements;
      std::set<int> Divisors;
      auto findReplacement = [&](isl::term Term) {
        // does the term contain the critical divisors
        for (int i = 0; i < Term.dim(isl::dim::div); i++) {
          if (Divisors.count(i) == 0 && Term.get_exp(isl::dim::div, i) > 0) {
            Divisors.insert(i);
            isl::aff Divisor = Term.get_div(i);
            // compute the variable information
            int Parameters = 0;
            std::vector<int> Variables;
            std::vector<isl::val> Coefficients;
            // extract the parameter and variable information
            for (int j = 0; j < Divisor.dim(isl::dim::param); ++j) {
              isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::param, j);
              if (!Coefficient.is_zero())
                Parameters++;
            }
            for (int j = 0; j < Divisor.dim(isl::dim::in); ++j) {
              isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::in, j);
              if (!Coefficient.is_zero()) {
                Variables.push_back(j);
                Coefficients.push_back(Coefficient);
              }
            }
            // search the dimension
            int Index = -1;
            auto Iter = std::find(Variables.begin(), Variables.end(), Dimension);
            if (Iter != Variables.end())
              Index = std::distance(Variables.begin(), Iter);
            // replace the divisor if the variable matches
            if (Parameters == 0 && Index >= 0) {
              // compute the replacement
              isl::aff Remainder = isl::aff(PS, isl::val(Domain.get_ctx(), Current));
              isl::aff New = isl::aff(PS, Divisor.get_constant_val());
              // add all other variables
              for (int j = 0; j < Variables.size(); ++j) {
                if (j != Index) {
                  isl::aff Var = isl::aff::var_on_domain(PS, isl::dim::set, Variables[j]);
                  New = New.add(isl::aff(PS, Coefficients[j]).mul(Var));
                }
              }
              // add the remainder and compute the floor
              New = New.add(isl::aff(PS, Coefficients[Index]).mul(Remainder)).floor();
              // add the division outside of the floor
              isl::aff Var = isl::aff::var_on_domain(PS, isl::dim::set, Variables[Index]).sub(Remainder);
              New = New.add(isl::aff(PS, Coefficients[Index]).mul(Var));
              Replacements[i] = isl::qpolynomial::from_aff(New);
            }
          }
        }
        return isl::stat::ok();
      };
      Piece.Polynomial.foreach_term(findReplacement);
      // compute the polynomail
      Polynomial = computeReplacement(Replacements, Piece);
      // store the result
      Pieces.push_back(createPiece(Domain, Polynomial));
    }
  }
  // verify that at least one piece is affine along the dimension
  bool IsAffine = false;
  for (auto Piece : Pieces) {
    if (computeDimensionExponent(Dimension, Piece) <= 1) {
      IsAffine = true;
      break;
    }
  }
  if (!IsAffine)
    return {};
#ifdef VERIFY_RESULT
  // verify the correctness of the elimination
  bool Success = verifySplit(Piece, Pieces);
  assert(Success);
#endif
  return Pieces;
}

std::vector<std::vector<std::tuple<int, long, long>>> Access::findEqualizationCandidates(piece Piece) const {
  // count the number of divisors that differ only by constant
  std::map<std::vector<std::tuple<int, long, long>>, int> Counts;
  std::vector<std::vector<std::tuple<int, long, long>>> Candidates;
  // process every divisor exactly once
  std::set<int> Divisors;
  // analyze the terms
  auto analyzeTerms = [&](isl::term Term) {
    // get the divisor variable
    for (int i = 0; i < Term.dim(isl::dim::div); i++) {
      if (Divisors.count(i) == 0 && Term.get_exp(isl::dim::div, i) > 0) {
        Divisors.insert(i);
        isl::aff Divisor = Term.get_div(i);
        int Parameters = 0;
        std::vector<std::tuple<int, long, long>> Variables;
        isl::val Denominator = Divisor.get_denominator_val();
        // extract the parameter and variable information
        for (int j = 0; j < Divisor.dim(isl::dim::param); ++j) {
          isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::param, j);
          if (!Coefficient.is_zero())
            Parameters++;
        }
        for (int j = 0; j < Divisor.dim(isl::dim::in); ++j) {
          isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::in, j);
          if (!Coefficient.is_zero())
            Variables.push_back(
                std::make_tuple(j, isl::get_value(Coefficient.mul(Denominator)), isl::get_value(Denominator)));
        }
        // count the single variate divisors with coefficient one
        if (Parameters == 0) {
          Counts[Variables]++;
        }
      }
    }
    return isl::stat::ok();
  };
  Piece.Polynomial.foreach_term(analyzeTerms);
  // convert the divisors that apply multiple times
  for (auto Count : Counts) {
    if (Count.second > 1) {
      Candidates.push_back(Count.first);
    }
  }
  return Candidates;
}

std::vector<long> Access::computeSplits(std::vector<std::tuple<int, long, long>> Candidate, piece Piece) const {
  // compute the splits for the candidate variable
  std::vector<long> Splits;
  std::set<int> Divisors;
  auto computeSplits = [&](isl::term Term) {
    // get the divisor variable
    for (int i = 0; i < Term.dim(isl::dim::div); i++) {
      if (Divisors.count(i) == 0 && Term.get_exp(isl::dim::div, i) > 0) {
        Divisors.insert(i);
        isl::aff Divisor = Term.get_div(i);
        int Parameters = 0;
        std::vector<int> Variables;
        std::vector<isl::val> Coefficients;
        isl::val Denominator = Divisor.get_denominator_val();
        // extract the parameter and variable information
        for (int j = 0; j < Divisor.dim(isl::dim::param); ++j) {
          isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::param, j);
          if (!Coefficient.is_zero())
            Parameters++;
        }
        for (int j = 0; j < Divisor.dim(isl::dim::in); ++j) {
          isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::in, j);
          if (!Coefficient.is_zero()) {
            Variables.push_back(j);
            Coefficients.push_back(Coefficient);
          }
        }
        // make sure the term matches the candidate
        if (Parameters == 0 && Variables.size() == Candidate.size()) {
          bool IsMatch = true;
          for (int i = 0; i < Variables.size(); ++i) {
            if (Variables[i] != std::get<0>(Candidate[i]) ||
                isl::get_value(Coefficients[i].mul(Denominator)) != std::get<1>(Candidate[i]) ||
                isl::get_value(Denominator) != std::get<2>(Candidate[i])) {
              IsMatch = false;
              break;
            }
          }
          // if all variables match compute the splits
          if (IsMatch) {
            isl::val Constant = Divisor.get_constant_val().mul(Denominator);
            Splits.push_back(isl::get_value(Denominator.sub(Constant).mod(Denominator)));
          }
        }
      }
    }
    return isl::stat::ok();
  };
  Piece.Polynomial.foreach_term(computeSplits);
  // make the splits unique
  std::sort(Splits.begin(), Splits.end());
  Splits.erase(std::unique(Splits.begin(), Splits.end()), Splits.end());
  return Splits;
}

std::vector<piece> Access::equalizeCandidate(std::vector<std::tuple<int, long, long>> Candidate,
                                             std::vector<long> Splits, piece Piece) const {
  isl::local_space DS = isl::local_space(Piece.Domain.get_space());
  isl::local_space PS = isl::local_space(Piece.Polynomial.get_domain_space());
  // result vector
  std::vector<piece> Pieces;
  assert(!Splits.empty());
  // extract the candidate information
  isl::val Denominator = isl::val(Piece.Domain.get_ctx(), std::get<2>(Candidate[0]));
  isl::aff Dimension = isl::aff(DS, isl::val(Piece.Domain.get_ctx(), 0));
  for (auto Variable : Candidate) {
    Dimension = Dimension.add(isl::aff::var_on_domain(DS, isl::dim::set, std::get<0>(Variable))
                                  .mul(isl::aff(DS, isl::val(Piece.Domain.get_ctx(), std::get<1>(Variable)))));
  }
  // compute the equalization offset
  // (allows us to avoid split at zero)
  isl::val Offset = isl::val(Piece.Domain.get_ctx(), -Splits[0]).mod(Denominator);
  // add the correction terms for each interval
  long Start = Splits.back();
  for (int i = 0; i < Splits.size(); ++i) {
    // compute the range
    long Stop = Splits[i];
    bool Single = Stop - Start == 1 || Stop - Start == 1 - isl::get_value(Denominator);
    // compute the domain
    isl::set Domain = Piece.Domain;
    if (Start != Stop) {
      isl::aff Modulo = Dimension.mod(Denominator);
      isl::set LowerBound = Modulo.ge_set(isl::aff(DS, isl::val(Domain.get_ctx(), Start)));
      isl::set UpperBound = Modulo.lt_set(isl::aff(DS, isl::val(Domain.get_ctx(), Stop)));
      // handle intervals that wrap around
      if (Start < Stop) {
        Domain = Domain.intersect(LowerBound).intersect(UpperBound);
      } else {
        Domain = Domain.intersect(LowerBound).unite(Domain.intersect(UpperBound));
      }
    }
    // compute the polynomial if the domain is not empty
    isl::qpolynomial Polynomial;
    if (!Domain.is_empty()) {
      // compute replacement divisors
      std::map<int, isl::qpolynomial> Replacements;
      std::set<int> Divisors;
      auto findReplacement = [&](isl::term Term) {
        // does the term contain the critical divisors
        for (int i = 0; i < Term.dim(isl::dim::div); i++) {
          if (Divisors.count(i) == 0 && Term.get_exp(isl::dim::div, i) > 0) {
            Divisors.insert(i);
            isl::aff Divisor = Term.get_div(i);
            int Parameters = 0;
            std::vector<int> Variables;
            std::vector<isl::val> Coefficients;
            isl::val Denominator = Divisor.get_denominator_val();
            // extract the parameter and variable information
            for (int j = 0; j < Divisor.dim(isl::dim::param); ++j) {
              isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::param, j);
              if (!Coefficient.is_zero())
                Parameters++;
            }
            for (int j = 0; j < Divisor.dim(isl::dim::in); ++j) {
              isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::in, j);
              if (!Coefficient.is_zero()) {
                Variables.push_back(j);
                Coefficients.push_back(Coefficient);
              }
            }
            // check if we match the candidate
            if (Parameters == 0 && Variables.size() == Candidate.size()) {
              bool IsMatch = true;
              for (int i = 0; i < Variables.size(); ++i) {
                if (Variables[i] != std::get<0>(Candidate[i]) ||
                    isl::get_value(Coefficients[i].mul(Denominator)) != std::get<1>(Candidate[i]) ||
                    isl::get_value(Denominator) != std::get<2>(Candidate[i])) {
                  IsMatch = false;
                  break;
                }
              }
              // if all variables match compute the splits
              if (IsMatch) {
                isl::val Constant = Divisor.get_constant_val().mul(Denominator);
                isl::val Variable = isl::val(Term.get_ctx(), Start);
                isl::val Old = Variable.add(Constant).div(Denominator).floor();
                // compute the replacement divisor
                // (no floor necessary if for single elements)
                if (Single) {
                  isl::val New = Variable.add(Offset).div(Denominator);
                  Replacements[i] = isl::qpolynomial::from_aff(Dimension.add(isl::aff(PS, Offset))
                                                                   .div(isl::aff(PS, Denominator))
                                                                   .add(isl::aff(PS, Old.sub(New))));
                } else {
                  isl::val New = Variable.add(Offset).div(Denominator).floor();
                  Replacements[i] = isl::qpolynomial::from_aff(Dimension.add(isl::aff(PS, Offset))
                                                                   .div(isl::aff(PS, Denominator))
                                                                   .floor()
                                                                   .add(isl::aff(PS, Old.sub(New))));
                }
              }
            }
          }
        }
        return isl::stat::ok();
      };
      Piece.Polynomial.foreach_term(findReplacement);
      // update the polynomial
      Polynomial = computeReplacement(Replacements, Piece);
    }
    // create the piece
    Pieces.push_back(createPiece(Domain, Polynomial));
    Start = Stop;
  }
  // check if the number of divisors improves
  bool Improved = false;
  auto Exponent = computeExponent(Piece);
  for (auto Piece : Pieces) {
    if (!Piece.Polynomial.is_null()) {
      if (computeExponent(Piece) < Exponent) {
        Improved = true;
        break;
      }
    } else {
      Improved = true;
      break;
    }
  }
  if (!Improved) {
    return {};
  }
#ifdef VERIFY_RESULT
  // verify the correctness of the elimination
  bool Success = verifySplit(Piece, Pieces);
  assert(Success);
#endif
  return Pieces;
}

bool Access::verifySplit(piece Piece, std::vector<piece> Updates) const {
  // sum the domain sizes of the pieces and compare the the original piece
  long All = isl::cardinality(Piece.Domain);
  long Total = std::accumulate(Updates.begin(), Updates.end(), 0,
                               [](long Total, piece Current) { return Total + isl::cardinality(Current.Domain); });
  if (All != Total) {
    printf("-> domain split failed (%ld!=%ld)!\n", All, Total);
    Piece.Domain.dump();
    printf("transformed to");
    for (auto Update : Updates) {
      Update.Domain.dump();
    }
    return false;
  }
  // verify the polynomials
  for (auto Update : Updates) {
    bool Match = true;
    if (!Update.Polynomial.is_null()) {
      // compare the polynomials on all points of the domain
      auto comparePolynomials = [&](isl::point Point) {
        isl::val Before = Piece.Polynomial.eval(Point);
        isl::val After = Update.Polynomial.eval(Point);
        if (!Before.eq(After)) {
          Match = false;
        }
        return isl::stat::ok();
      };
      Update.Domain.foreach_point(comparePolynomials);
      if (!Match) {
        printf("-> polynomial split failed!\n");
        Piece.Polynomial.dump();
        Piece.Domain.dump();
        printf("transformed to\n");
        Update.Polynomial.dump();
        Update.Domain.dump();
        printf("Updates.size()==%ld\n", Updates.size());
        return false;
      }
    }
  }
  return true;
}

piece Access::createPiece(isl::set Domain, isl::qpolynomial Count) const {
  // analyze the next piece
  piece Piece;
  Piece.Domain = Domain;
  Piece.Size = -1;
  Piece.Polynomial = Count;
  // analyze the terms
  auto analyzeTerms = [&](isl::term Term) {
    // analyze the term
    term Result;
    // get the coefficient
    Result.Coefficient = Term.get_coefficient_val();
    // get the parameter exponents
    for (int i = 0; i < Term.dim(isl::dim::param); ++i) {
      Result.Parameters.push_back(Term.get_exp(isl::dim::param, i));
    }
    // get the input exponents
    for (int i = 0; i < Term.dim(isl::dim::set); i++) {
      Result.Variables.push_back(Term.get_exp(isl::dim::set, i));
    }
    // get the divisors exponents
    for (int i = 0; i < Term.dim(isl::dim::div); i++) {
      Result.Divisors.push_back(Term.get_exp(isl::dim::div, i));
    }
    Result.Polynomial = isl::qpolynomial::from_term(Term);
    Piece.Terms.push_back(Result);
    return isl::stat::ok();
  };
  Count.foreach_term(analyzeTerms);
  return Piece;
}

void Access::extractStackDistanceExpression(isl::union_pw_qpolynomial Count) {
  // analyze the polynomials
  auto analyzePolynomial = [&](isl::pw_qpolynomial Polynomial) {
    // try to minimize the number of pieces
    Polynomial = Polynomial.coalesce();
    // analyze th individual piece
    auto analyzePiece = [&](isl::set Domain, isl::qpolynomial Polynomial) {
      // analyze the next piece
      auto Piece = createPiece(Domain, Polynomial);
      if (isPieceAffine(Piece)) {
        Piece.Expression = extractAffineExpression(Piece);
      }
      Expression_.push_back(Piece);
      return isl::stat::ok();
    };
    Polynomial.foreach_piece(analyzePiece);
    return isl::stat::ok();
  };
  Count.foreach_pw_qpolynomial(analyzePolynomial);
}

bool Access::isPieceAffine(piece Piece) const {
  // sum the exponents the determine if the piece is affine
  // (note that this function does not consider domains with constant dimensions)
  for (auto Term : Piece.Terms) {
    // count the factors
    int Factors = 0;
    // check the parameters
    for (int i = 0; i < Term.Parameters.size(); ++i) {
      if (Term.Parameters[i] > 0) {
        Factors += Term.Parameters[i];
        if (Factors > 1)
          return false;
      }
    }
    // multiply with variable
    for (int i = 0; i < Term.Variables.size(); ++i) {
      if (Term.Variables[i] > 0) {
        Factors += Term.Variables[i];
        if (Factors > 1)
          return false;
      }
    }
    // multiply with divisor
    for (int i = 0; i < Term.Divisors.size(); ++i) {
      if (Term.Divisors[i] > 0) {
        Factors += Term.Divisors[i];
        if (Factors > 1)
          return false;
      }
    }
  }
  return true;
}

long Access::getPieceSize(piece &Piece) const {
  if (Piece.Size == -1) {
    Piece.Size = isl::cardinality(Piece.Domain);
  }
  return Piece.Size;
}

isl::pw_aff Access::extractAffineExpression(piece Piece) const {
  Timer::startTimer("extractAffineExpression");
  isl::local_space DS = isl::local_space(Piece.Domain.get_space());
  // initialize the expression with the neutral element
  isl::val Zero = isl::val::zero(Piece.Domain.get_ctx());
  isl::pw_aff Sum(Piece.Domain, Zero);
  for (auto &Term : Piece.Terms) {
    // compute the product
    isl::pw_aff Product(Piece.Domain, Term.Coefficient);
    // multiply with parameter
    for (int i = 0; i < Term.Parameters.size(); ++i) {
      if (Term.Parameters[i] > 0) {
        assert(Term.Parameters[i] <= 1);
        isl::pw_aff Parameter = isl::pw_aff::var_on_domain(DS, isl::dim::param, i);
        Product = Product.mul(Parameter);
      }
    }
    // multiply with variable
    for (int i = 0; i < Term.Variables.size(); ++i) {
      if (Term.Variables[i] > 0) {
        assert(Term.Variables[i] <= 1);
        isl::pw_aff Variable;
        Variable = isl::pw_aff::var_on_domain(DS, isl::dim::set, i);
        Product = Product.mul(Variable);
      }
    }
    // multiply with divisor
    auto multiplyTerms = [&](isl::term Term) {
      for (int i = 0; i < Term.dim(isl::dim::div); i++) {
        if (Term.get_exp(isl::dim::div, i) > 0) {
          assert(Term.get_exp(isl::dim::div, i) == 1);
          Product = Product.mul(Term.get_div(i).floor());
        }
      }
      return isl::stat::ok();
    };
    Term.Polynomial.foreach_term(multiplyTerms);
    Sum = Sum.add(Product);
  }
  Timer::stopTimer("extractAffineExpression");
  return Sum;
}

isl::aff Access::extractAffineExpression(isl::qpolynomial Polynomial, isl::set Domain,
                                         std::map<int, long> Values) const {
  Timer::startTimer("extractAffineExpression");
  // buffer the fixed divisors
  std::map<int, isl::aff> Divisors;
  // accumulate the values of all terms
  isl::local_space DS = isl::local_space(Domain.get_space());
  isl::val Zero = isl::val::zero(Domain.get_ctx());
  isl::aff Sum(DS, Zero);
  auto analyzeTerms = [&](isl::term Term) {
    auto Coefficient = Term.get_coefficient_val();
    isl::aff Product(DS, Coefficient);
    // get the parameter exponents
    for (int i = 0; i < Term.dim(isl::dim::param); ++i) {
      int Exponent = Term.get_exp(isl::dim::param, i);
      assert(Exponent <= 1);
      if (Exponent == 1) {
        auto Parameter = isl::aff::var_on_domain(DS, isl::dim::param, i);
        Product = Product.mul(Parameter);
      }
    }
    // get the input exponents
    for (int i = 0; i < Term.dim(isl::dim::set); i++) {
      int Exponent = Term.get_exp(isl::dim::set, i);
      if (Exponent > 0) {
        if (Values.count(i)) {
          long Coefficient = compute_power(Values[i], Exponent);
          isl::aff Value(DS, isl::val(Domain.get_ctx(), Coefficient));
          Product = Product.mul(Value);
        } else {
          assert(Exponent == 1);
          auto Variable = isl::aff::var_on_domain(DS, isl::dim::set, i);
          Product = Product.mul(Variable);
        }
      }
    }
    // get the divisors exponents
    for (int i = 0; i < Term.dim(isl::dim::div); i++) {
      int Exponent = Term.get_exp(isl::dim::div, i);
      if (Exponent > 0) {
        // update the divisor if we don't have it yet
        if (Divisors.count(i) == 0) {
          auto Divisor = Term.get_div(i);
          auto Replacement = isl::aff(DS, Divisor.get_constant_val());
          for (int j = 0; j < Divisor.dim(isl::dim::param); ++j) {
            isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::param, j);
            if (!Coefficient.is_zero()) {
              Replacement = Replacement.add_coefficient_val(isl::dim::param, j, Coefficient);
            }
          }
          for (int j = 0; j < Divisor.dim(isl::dim::in); ++j) {
            isl::val Coefficient = Divisor.get_coefficient_val(isl::dim::in, j);
            if (!Coefficient.is_zero()) {
              if (Values.count(j)) {
                Replacement = Replacement.add_constant_val(Coefficient.mul_ui(Values[j]));
              } else {
                Replacement = Replacement.add_coefficient_val(isl::dim::in, j, Coefficient);
              }
            }
          }
          Divisors[i] = Replacement;
        }
        // distinguish cases
        if (Divisors[i].is_cst()) {
          long Base = isl::get_value(Divisors[i].floor().get_constant_val());
          long Coefficient = compute_power(Base, Exponent);
          isl::aff Value(DS, isl::val(Domain.get_ctx(), Coefficient));
          Product = Product.mul(Value);
        } else {
          assert(Exponent == 1);
          Product = Product.mul(Divisors[i].floor());
        }
      }
    }
    Sum = Sum.add(Product);
    return isl::stat::ok();
  };
  Polynomial.foreach_term(analyzeTerms);
  Timer::stopTimer("extractAffineExpression");
  return Sum;
}