/*****************************************************************************/
/*  Copyright (c) 2015, Karl Pauwels                                         */
/*  All rights reserved.                                                     */
/*                                                                           */
/*  Redistribution and use in source and binary forms, with or without       */
/*  modification, are permitted provided that the following conditions       */
/*  are met:                                                                 */
/*                                                                           */
/*  1. Redistributions of source code must retain the above copyright        */
/*  notice, this list of conditions and the following disclaimer.            */
/*                                                                           */
/*  2. Redistributions in binary form must reproduce the above copyright     */
/*  notice, this list of conditions and the following disclaimer in the      */
/*  documentation and/or other materials provided with the distribution.     */
/*                                                                           */
/*  3. Neither the name of the copyright holder nor the names of its         */
/*  contributors may be used to endorse or promote products derived from     */
/*  this software without specific prior written permission.                 */
/*                                                                           */
/*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS      */
/*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT        */
/*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR    */
/*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT     */
/*  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,   */
/*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT         */
/*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,    */
/*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY    */
/*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT      */
/*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE    */
/*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.     */
/*****************************************************************************/

#include <numeric>
#include <algorithm>
#include <cstdlib>
#include <math.h>
#include <cstring>
#include <cuda_profiler_api.h>
#include <stdexcept>
#include <Eigen/Dense>
#include <d_multiple_rigid_poses.h>
#include <utility_kernels_pose.h>
#include <multiple_rigid_pose_kernels.h>
#include <utilities.h>

namespace pose {

D_MultipleRigidPoses::D_MultipleRigidPoses(int n_cols, int n_rows,
                                           float nodal_point_x,
                                           float nodal_point_y,
                                           float focal_length_x,
                                           float focal_length_y, float baseline,
                                           Parameters parameters)
    : parameters_{ parameters }, _n_cols{ n_cols }, _n_rows{ n_rows },
      _nodal_point_x{ nodal_point_x }, _nodal_point_y{ nodal_point_y },
      _focal_length_x{ focal_length_x }, _focal_length_y{ focal_length_y },
      _baseline{ baseline }, _currentSparseObject{ 0 },
      _multipleRigidModelsOgre{
        std::unique_ptr<MultipleRigidModelsOgre>{ new MultipleRigidModelsOgre(
            _n_cols, _n_rows, _focal_length_x, _focal_length_y, _nodal_point_x,
            _nodal_point_y, parameters_.near_plane_, parameters_.far_plane_) }
      },
      cub_radix_sorter_{
        std::unique_ptr<util::CubRadixSorter<unsigned int, int> >{
          new util::CubRadixSorter<unsigned int, int>(
              _n_rows * _n_cols * _N_FLOWS, 0, parameters_.getKeyBits())
        }
      },
      _n_objects{ 0 }, _running{ true }, render_state_changed_{ true } {

  camera_pose_.setValid(true);
  previous_camera_pose_.setValid(false);

  /*******************************************************/
  /* Pre-allocate (and initiate )all device space needed */
  /* with maximal possible size                          */
  /* to avoid re-allocation and re-computation           */
  /*******************************************************/

  // linear index to store pixel locations
  // goes from 0 to n_rows*n_cols*n_flows
  // used to identify flow source
  std::vector<int> h_linear_ind(_n_rows * _n_cols * _N_FLOWS);
  std::iota(h_linear_ind.begin(), h_linear_ind.end(), 0);
  d_linear_ind_ =
      util::Device1D<int>::make_unique(_n_rows * _n_cols * _N_FLOWS);
  d_linear_ind_->copyFrom(h_linear_ind);

  // 0,1,2,...,n_objects valid locations
  d_valid_flow_Zbuffer_ =
      util::Device1D<unsigned int>::make_unique(_n_rows * _n_cols * _N_FLOWS);
  d_valid_disparity_Zbuffer_ =
      util::Device1D<unsigned int>::make_unique(_n_rows * _n_cols);
  d_valid_flow_Zbuffer_sub_ =
      util::Device1D<unsigned int>::make_unique(_n_rows * _n_cols * _N_FLOWS);
  d_valid_disparity_Zbuffer_sub_ =
      util::Device1D<unsigned int>::make_unique(_n_rows * _n_cols);
  d_extra_disparity_buffer_ =
      util::Device1D<unsigned int>::make_unique(_n_rows * _n_cols);

  // index of valid locations
  d_ind_flow_Zbuffer_ =
      util::Device1D<int>::make_unique(_n_rows * _n_cols * _N_FLOWS);
  d_ind_disparity_Zbuffer_ =
      util::Device1D<int>::make_unique(_n_rows * _n_cols);
  d_ind_flow_Zbuffer_sub_ =
      util::Device1D<int>::make_unique(_n_rows * _n_cols * _N_FLOWS);
  d_ind_disparity_Zbuffer_sub_ =
      util::Device1D<int>::make_unique(_n_rows * _n_cols);

  // starting index of sorted segments
  d_seg_start_inds_ =
      util::Device1D<int>::make_unique(parameters_.max_objects_);

  // Gathering
  d_flow_compact_ =
      util::Device1D<float2>::make_unique(_n_cols * _n_rows * _N_FLOWS);
  d_Zbuffer_flow_compact_ =
      util::Device1D<float>::make_unique(_n_cols * _n_rows * _N_FLOWS);
  d_disparity_compact_ = util::Device1D<float>::make_unique(_n_cols * _n_rows);
  d_Zbuffer_normals_compact_ =
      util::Device1D<float4>::make_unique(_n_cols * _n_rows);
  d_n_values_flow_ = util::Device1D<int>::make_unique(parameters_.max_objects_);
  d_start_ind_flow_ =
      util::Device1D<int>::make_unique(parameters_.max_objects_);
  d_n_values_disparity_ =
      util::Device1D<int>::make_unique(parameters_.max_objects_);
  d_start_ind_disparity_ =
      util::Device1D<int>::make_unique(parameters_.max_objects_);

  // Normal equations
  d_CO_ = util::Device1D<float>::make_unique(
      4 * _MAX_N_VAL_ACCUM * _N_CON_FLOW * parameters_.max_objects_);
  d_CO_reduced_ = util::Device1D<float>::make_unique(_N_CON_FLOW *
                                                     parameters_.max_objects_);
  d_CD_ = util::Device1D<float>::make_unique(
      4 * _MAX_N_VAL_ACCUM * _N_CON_DISP * parameters_.max_objects_);
  d_CD_reduced_ = util::Device1D<float>::make_unique(_N_CON_DISP *
                                                     parameters_.max_objects_);
  d_abs_res_ =
      util::Device1D<float>::make_unique((_N_FLOWS + 1) * _n_rows * _n_cols);

  h_CO_reduced_.resize(_N_CON_FLOW * parameters_.max_objects_);
  h_CD_reduced_.resize(_N_CON_DISP * parameters_.max_objects_);
  segment_normal_eqs_.resize(parameters_.max_objects_);

  // Segment starting indices in single residual structure
  d_offset_ind_res_flow_ =
      util::Device1D<int>::make_unique(parameters_.max_objects_);
  d_offset_ind_res_disparity_ =
      util::Device1D<int>::make_unique(parameters_.max_objects_);
  d_dTR_ = util::Device1D<float>::make_unique(6 * parameters_.max_objects_);
  d_delta_T_accum_ =
      util::Device1D<float>::make_unique(3 * parameters_.max_objects_);
  d_delta_Rmat_accum_ =
      util::Device1D<float>::make_unique(9 * parameters_.max_objects_);
  d_segment_translation_table_ =
      util::Device1D<int>::make_unique(parameters_.max_objects_);

  // Approximate median
  // pre-generate maximum number of random numbers required (uniform between
  // [0,1[ , used for sampling (with replacement) of absolute residuals
  std::vector<float> h_random_numbers(_n_rows * _n_cols * (_N_FLOWS + 1));
  srand(0);
  for (auto &it : h_random_numbers)
    it = (float)((double)rand() / double(RAND_MAX));
  d_random_numbers_ =
      util::Device1D<float>::make_unique(_n_rows * _n_cols * (_N_FLOWS + 1));
  d_random_numbers_->copyFrom(h_random_numbers);
  int pp = int(ceil(double(_n_rows * _n_cols * (_N_FLOWS + 1)) /
                    243.0)); // second stage reduction
  d_median_tmp_ = util::Device1D<float>::make_unique(pp);
  d_abs_res_scales_ =
      util::Device1D<float>::make_unique(parameters_.max_objects_);

  // temps used inside median calculation
  d_median_n_in_ = util::Device1D<int>::make_unique(parameters_.max_objects_);
  d_median_start_inds_ =
      util::Device1D<int>::make_unique(parameters_.max_objects_);

  // Initial Zbuffer
  d_init_Z_ = util::Device1D<float>::make_unique(_n_rows * _n_cols);

  // Residual flow
  d_res_flowx_ = util::Device1D<float>::make_unique(_n_rows * _n_cols);
  d_res_flowy_ = util::Device1D<float>::make_unique(_n_rows * _n_cols);

  // Residual ar flow
  d_res_ar_flowx_ = util::Device1D<float>::make_unique(_n_rows * _n_cols);
  d_res_ar_flowy_ = util::Device1D<float>::make_unique(_n_rows * _n_cols);
}

void D_MultipleRigidPoses::addModel(const char *obj_filename, float obj_scale,
                                    TranslationRotation3D initial_pose) {
  if (_n_objects > (parameters_.max_objects_ - 2))
    throw std::runtime_error("D_MultipleRigidPoses::addModel: Max objects "
                             "exceeded, increase key_bits\n");

  _n_objects++;
  _currentPoses.push_back(initial_pose);
  setSparsePose(initial_pose, _n_objects - 1);

  _multipleRigidModelsOgre->addModel(obj_filename);
}

void D_MultipleRigidPoses::removeAllModels() {
  _multipleRigidModelsOgre->removeAllModels();
  _currentPoses.clear();
  _currentSparseObject = 0;
  _n_objects = 0;
}

void D_MultipleRigidPoses::update(const util::Device1D<float> &d_flowx,
                                  const util::Device1D<float> &d_flowy,
                                  const util::Device1D<float> &d_ar_flowx,
                                  const util::Device1D<float> &d_ar_flowy,
                                  const util::Device2D<float> &d_disparity,
                                  std::bitset<32> segments_to_update) {
// small inaccuracy here is that the optical flow (frame 1 -> frame 2) is
// segmented according to the region estimated at frame 2
// this is necessary in order to update the residual flow according to the
// updated model depth
// have shown (in simulation) that the resulting pose estimates are a lot more
// accurate than those based on initial depth

#ifdef TIME_STEPS
  // Setup timers
  cudaEvent_t start, end;
  float elapsed_time;
  cudaEventCreate(&start);
  cudaEventCreate(&end);
#endif

  if (_running) {

    // reset timers
    int n_timers = 14;
    _compTimes.clear();
    _compTimes.assign(n_timers, 0.0);

    /* 0 preprocess-total
     * 1 ols-total
     * 2 robust-total
     * 3 mark valids
     * 4 radix sort
     * 5 gather
     * 6 compose normal equations
     * 7 reduce normal equations
     * 8 solve normal equations (cpu)
     * 9 compute absolute residuals
     * 10 median absolute residuals
     * 11 render
     * 12 pose accumulation (cpu)
     * 13 residual flow
     */

    std::vector<TranslationRotation3D> delta_poses_accum(
        _n_objects, TranslationRotation3D());

    std::vector<TranslationRotation3D> explained_delta_poses(
        _n_objects, TranslationRotation3D());

#ifdef TIME_STEPS
    cudaEventRecord(start, 0);
#endif

    // first rendering
    render(_currentPoses);

#ifdef TIME_STEPS
    cudaThreadSynchronize();
    cudaEventRecord(end, 0);
    cudaEventSynchronize(end);
    cudaEventElapsedTime(&elapsed_time, start, end);
    _compTimes.at(11) += (double)elapsed_time;
#endif

#ifdef TIME_STEPS
    cudaEventRecord(start, 0);
#endif

    // store initial Zbuffer as initial model for residual flow
    convertZbufferToZ(d_init_Z_->data(), _multipleRigidModelsOgre->getZBuffer(),
                      _n_cols, _n_rows, _nodal_point_x, _nodal_point_y,
                      parameters_.near_plane_, parameters_.far_plane_);

#ifdef TIME_STEPS
    cudaThreadSynchronize();
    cudaEventRecord(end, 0);
    cudaEventSynchronize(end);
    cudaEventElapsedTime(&elapsed_time, start, end);
    _compTimes.at(13) += (double)elapsed_time;
#endif

    // start iterations
    std::vector<float> dT_accum(3 * _n_objects);
    std::vector<float> dR_accum(9 * _n_objects);
    std::vector<TranslationRotation3D> poses_accum = _currentPoses;

    for (int it = 1; it <= parameters_.n_icp_outer_it_; it++) {

// update residual flow
#ifdef TIME_STEPS
      cudaEventRecord(start, 0);
#endif

      for (int o = 0; o < _n_objects; o++) {
        auto delta_pose =
            poses_accum.at(o) * _currentPoses.at(o).inverseTransform();
        if (previous_camera_pose_.isValid()) {
          explained_delta_poses.at(o) = camera_pose_.inverseTransform() *
                                        delta_pose * previous_camera_pose_;
        } else {
          explained_delta_poses.at(o) = delta_pose;
        }
      }

      for (int o = 0; o < _n_objects; o++) {
        explained_delta_poses.at(o).getT(&dT_accum[3 * o]);
        explained_delta_poses.at(o).getR_mat(&dR_accum[9 * o]);
      }
      d_delta_T_accum_->copyFrom(dT_accum, 3 * _n_objects);
      d_delta_Rmat_accum_->copyFrom(dR_accum, 9 * _n_objects);

#ifdef TIME_STEPS
      cudaThreadSynchronize();
      cudaEventRecord(end, 0);
      cudaEventSynchronize(end);
      cudaEventElapsedTime(&elapsed_time, start, end);
      _compTimes.at(12) += (double)elapsed_time;
#endif

#ifdef TIME_STEPS
      cudaEventRecord(start, 0);
#endif
      computeResidualFlow(
          d_res_flowx_->data(), d_res_flowy_->data(), d_res_ar_flowx_->data(),
          d_res_ar_flowy_->data(), d_flowx.data(), d_flowy.data(),
          d_ar_flowx.data(), d_ar_flowy.data(), d_delta_T_accum_->data(),
          d_delta_Rmat_accum_->data(), d_init_Z_->data(),
          _multipleRigidModelsOgre->getSegmentIND(), _n_cols, _n_rows,
          _nodal_point_x, _nodal_point_y, _focal_length_x, _focal_length_y);

#ifdef TIME_STEPS
      cudaThreadSynchronize();
      cudaEventRecord(end, 0);
      cudaEventSynchronize(end);
      cudaEventElapsedTime(&elapsed_time, start, end);
      _compTimes.at(13) += (double)elapsed_time;
#endif

      robustPoseUpdates(d_res_flowx_->data(), d_res_flowy_->data(),
                        d_res_ar_flowx_->data(), d_res_ar_flowy_->data(),
                        d_disparity.data(), d_disparity.pitch(),
                        (int)segments_to_update.to_ulong());

      // accumulate delta poses
      for (int s = 0; s < _segment_info.size(); s++) {
        SegmentINFO &tmp = _segment_info.at(s);
        delta_poses_accum.at(tmp.segment_ind) =
            _robustDeltaPoses.at(s) * delta_poses_accum.at(tmp.segment_ind);
      }

#ifdef TIME_STEPS
      cudaEventRecord(start, 0);
#endif

      // update poses, invalidating if magnitude T too large
      poses_accum = _currentPoses;

      // transform to camera frame
      for (auto &it : poses_accum)
        it = camera_pose_.inverseTransform() * it;

      for (int o = 0; o < _n_objects; o++)
        if (delta_poses_accum.at(o).normT2() <
            parameters_.max_t_update_norm_squared_)
          poses_accum.at(o) = delta_poses_accum.at(o) * poses_accum.at(o);
        else // invalidate
          poses_accum.at(o).setValid(false);

      // transform back to world frame
      for (auto &it : poses_accum)
        it = camera_pose_ * it;

#ifdef TIME_STEPS
      cudaThreadSynchronize();
      cudaEventRecord(end, 0);
      cudaEventSynchronize(end);
      cudaEventElapsedTime(&elapsed_time, start, end);
      _compTimes.at(12) += (double)elapsed_time;
#endif

#ifdef TIME_STEPS
      cudaEventRecord(start, 0);
#endif

      // render at updated pose (unless last iteration)
      if (it < parameters_.n_icp_outer_it_)
        render(poses_accum);

#ifdef TIME_STEPS
      cudaThreadSynchronize();
      cudaEventRecord(end, 0);
      cudaEventSynchronize(end);
      cudaEventElapsedTime(&elapsed_time, start, end);
      _compTimes.at(11) += (double)elapsed_time;
#endif
    }

    // update current pose
    _currentPoses = poses_accum;
  }
}

void D_MultipleRigidPoses::updateNormalEquations(
    const util::Device1D<float> &d_flowx, const util::Device1D<float> &d_flowy,
    const util::Device1D<float> &d_ar_flowx,
    const util::Device1D<float> &d_ar_flowy,
    const util::Device2D<float> &d_disparity,
    const std::vector<TranslationRotation3D> &explained_delta_poses,
    std::bitset<32> segments_to_update) {
  // ensure rendering is up-to-date
  render(_currentPoses);

  // store initial Zbuffer as initial model for residual flow
  convertZbufferToZ(d_init_Z_->data(), _multipleRigidModelsOgre->getZBuffer(),
                    _n_cols, _n_rows, _nodal_point_x, _nodal_point_y,
                    parameters_.near_plane_, parameters_.far_plane_);

  // initialize residual flow (will improve later)
  d_res_flowx_->copyFrom(d_flowx);
  d_res_flowy_->copyFrom(d_flowy);
  d_res_ar_flowx_->copyFrom(d_ar_flowx);
  d_res_ar_flowy_->copyFrom(d_ar_flowy);

  // subtract camera-motion-induced flow
  std::vector<float> dT_accum(3 * _n_objects);
  std::vector<float> dR_accum(9 * _n_objects);

  for (int o = 0; o < _n_objects; o++) {
    explained_delta_poses.at(o).getT(&dT_accum[3 * o]);
    explained_delta_poses.at(o).getR_mat(&dR_accum[9 * o]);
  }
  d_delta_T_accum_->copyFrom(dT_accum, 3 * _n_objects);
  d_delta_Rmat_accum_->copyFrom(dR_accum, 9 * _n_objects);

  computeResidualFlow(
      d_res_flowx_->data(), d_res_flowy_->data(), d_res_ar_flowx_->data(),
      d_res_ar_flowy_->data(), d_flowx.data(), d_flowy.data(),
      d_ar_flowx.data(), d_ar_flowy.data(), d_delta_T_accum_->data(),
      d_delta_Rmat_accum_->data(), d_init_Z_->data(),
      _multipleRigidModelsOgre->getSegmentIND(), _n_cols, _n_rows,
      _nodal_point_x, _nodal_point_y, _focal_length_x, _focal_length_y);

  // reset normal equations
  for (auto &it : segment_normal_eqs_)
    it.reset();

  // perform robust iteration
  robustPoseUpdates(d_res_flowx_->data(), d_res_flowy_->data(),
                    d_res_ar_flowx_->data(), d_res_ar_flowy_->data(),
                    d_disparity.data(), d_disparity.pitch(),
                    (int)segments_to_update.to_ulong());

  // prepare normal equations for (external) constrained update
  int min_samples = 1000;
  for (auto &it : _segment_info) {
    // reset if not enough samples or update norm too large
    if (((it.n_values_flow + it.n_values_disparity) < min_samples) &&
        (segment_normal_eqs_.at(it.segment_ind).squaredNormDeltaT() >=
         parameters_.max_t_update_norm_squared_))
      segment_normal_eqs_.at(it.segment_ind).reset();
  }
}

void D_MultipleRigidPoses::setWeights(float w_flow, float w_ar_flow,
                                      float w_disp) {
  parameters_.w_flow_ = w_flow;
  parameters_.w_ar_flow_ = w_ar_flow;
  parameters_.w_disp_ = w_disp;
}

void D_MultipleRigidPoses::getWeights(float &w_flow, float &w_ar_flow,
                                      float &w_disp) const {
  w_flow = parameters_.w_flow_;
  w_ar_flow = parameters_.w_ar_flow_;
  w_disp = parameters_.w_disp_;
}

cudaArray *D_MultipleRigidPoses::getTexture() {
  render(_currentPoses);
  return (_multipleRigidModelsOgre->getTexture());
}

cudaArray *D_MultipleRigidPoses::getSegmentIND() {
  render(_currentPoses);
  return (_multipleRigidModelsOgre->getSegmentIND());
}

cudaArray *D_MultipleRigidPoses::getZbuffer() {
  render(_currentPoses);
  return _multipleRigidModelsOgre->getZBuffer();
}

cudaArray *D_MultipleRigidPoses::getNormalX() {
  render(_currentPoses);
  return _multipleRigidModelsOgre->getNormalX();
}

cudaArray *D_MultipleRigidPoses::getNormalY() {
  render(_currentPoses);
  return _multipleRigidModelsOgre->getNormalY();
}

cudaArray *D_MultipleRigidPoses::getNormalZ() {
  render(_currentPoses);
  return _multipleRigidModelsOgre->getNormalZ();
}

void D_MultipleRigidPoses::setCameraParameters(float focal_length_x,
                                               float focal_length_y,
                                               float nodal_point_x,
                                               float nodal_point_y) {
  render_state_changed_ = true;
  _nodal_point_x = nodal_point_x;
  _nodal_point_y = nodal_point_y;
  _focal_length_x = focal_length_x;
  _focal_length_y = focal_length_y;
  _multipleRigidModelsOgre->updateProjectionMatrix(
      _focal_length_x, _focal_length_y, _nodal_point_x, _nodal_point_y,
      parameters_.near_plane_, parameters_.far_plane_);
}

void
D_MultipleRigidPoses::setCameraPose(const TranslationRotation3D &camera_pose) {
  render_state_changed_ = true;

  // if this is the first camera pose received, make previous and current equal
  previous_camera_pose_ =
      previous_camera_pose_.isValid() ? camera_pose_ : camera_pose;

  camera_pose_ = camera_pose;
  _multipleRigidModelsOgre->updateCameraPose(camera_pose_.rotateX180());
}

bool D_MultipleRigidPoses::isDenseWinner() {

  bool denseWinner = true;
  double thres = parameters_.reliability_threshold_;

  // make sure that sparse does not negatively affect any object's proportion ar
  // valid
  // add some margin to allow minor decrease in reliability (due to noise)
  double margin = parameters_.sparse_intro_allowed_reliability_decrease_;
  bool sparseRelDecrease = false;
  for (int o = 0; o < _n_objects; o++)
    if (_S_ar_flow_prop_valid[o] < (_D_ar_flow_prop_valid[o] - margin))
      sparseRelDecrease = true;

  if (!sparseRelDecrease) {

    // compare reliability at the current sparse object
    int o = _currentSparseObject;

    // we require a pretty high reliability for sparse introduction
    double intro_thres = parameters_.sparse_intro_reliability_threshold_;
    if ((_S_ar_flow_prop_valid[o] > (_D_ar_flow_prop_valid[o] + intro_thres)) ||
        ((_S_ar_flow_prop_valid[o] > intro_thres) &&
         (_D_ar_flow_prop_valid[o] < thres)))
      denseWinner = false;
  }

  return (denseWinner);
}

void D_MultipleRigidPoses::render(
    const std::vector<TranslationRotation3D> &renderPoses) {
  if ((renderPoses != _lastPosesRendered) || (render_state_changed_)) {

    //    util::TimerGPU render_timer;
    _multipleRigidModelsOgre->render(renderPoses);
    //    std::cout << "render time: " << render_timer.read() << " ms\n";
    _lastPosesRendered = renderPoses;
    render_state_changed_ = false;
  }
}

void
D_MultipleRigidPoses::getSegmentLengths(std::vector<int> &lengths,
                                        const std::vector<int> &starting_inds,
                                        int n_segments, int total_length) {

  for (int i = 0; i < (n_segments - 1); i++) {
    int s = starting_inds.at(i);
    if (s >= 0) {

      // find end-point
      int e = starting_inds.at(i + 1);
      for (int j = (i + 2); ((e < 0) && (j < n_segments)); j++)
        e = starting_inds.at(j);

      if (e < 0)
        e = total_length;

      lengths.at(i) = e - s;

    } else

      lengths.at(i) = 0;
  }

  // final segment
  int s = starting_inds.at(n_segments - 1);
  if (s >= 0)
    lengths.at(n_segments - 1) = total_length - s;
  else
    lengths.at(n_segments - 1) = 0;
}

int D_MultipleRigidPoses::getNextSelectedSparseObject(bool dense) {

  std::vector<double> probabilities =
      dense ? _D_ar_flow_prop_valid : _S_ar_flow_prop_valid;

  double sum = 0.0;
  for (int o = 0; o < _n_objects; o++) {
    probabilities.at(o) = 1.0 - probabilities.at(o);
    sum += probabilities.at(o);
  }

  for (int o = 0; o < _n_objects; o++)
    probabilities.at(o) /= sum;

  double r = ((double)rand() / double(RAND_MAX));

  int object = 0;
  double prob_ulim = probabilities.at(object);

  while (r > prob_ulim) {
    object++;
    prob_ulim += probabilities.at(object);
  }

  return (object);
}

void D_MultipleRigidPoses::evaluateARFlowPoseError(
    bool dense, const util::Device1D<float> &d_ar_flowx,
    std::vector<TranslationRotation3D> &poses) {
  if (dense)
    computeARFlowPoseError(d_ar_flowx, poses, _D_ar_flow_prop_valid);
  else
    computeARFlowPoseError(d_ar_flowx, poses, _S_ar_flow_prop_valid);
}

void D_MultipleRigidPoses::computeARFlowPoseError(
    const util::Device1D<float> &d_ar_flowx,
    std::vector<TranslationRotation3D> &poses,
    std::vector<double> &ar_flow_prop_valid) {
  std::vector<double> ar_flow_abs_valid; // discarded
  computeARFlowPoseError(d_ar_flowx, poses, ar_flow_prop_valid,
                         ar_flow_abs_valid);
}

void D_MultipleRigidPoses::computeARFlowPoseError(
    const util::Device1D<float> &d_ar_flowx,
    std::vector<TranslationRotation3D> &poses,
    std::vector<double> &ar_flow_prop_valid,
    std::vector<double> &ar_flow_abs_valid) {
  ar_flow_prop_valid.clear();
  ar_flow_abs_valid.clear();

  render(poses);

  // re-purposing some already allocated data
  auto d_valid_ar_flow_Zbuffer = d_valid_disparity_Zbuffer_.get();
  auto d_valid_Zbuffer = d_valid_disparity_Zbuffer_sub_.get();
  markValidFlowZbufferAndZbufferZeroBased(
      d_valid_ar_flow_Zbuffer->data(), d_valid_Zbuffer->data(),
      d_ar_flowx.data(), _multipleRigidModelsOgre->getSegmentIND(), _n_cols,
      _n_rows, _n_objects);

  // additional buffers required for sorting
  auto d_value = d_ind_disparity_Zbuffer_.get();
  auto d_value_buf = d_ind_disparity_Zbuffer_sub_.get();
  auto d_key_buf = d_extra_disparity_buffer_.get();

  // Radix sort all the indices using the valid marks
  // indices could be ignored here to speed up sorting (but diff is minimal)
  cub_radix_sorter_->sort(*d_valid_ar_flow_Zbuffer, *d_value, *d_key_buf,
                          *d_value_buf);
  pose::extractLabelStartingIndices(d_seg_start_inds_->data(),
                                    d_valid_ar_flow_Zbuffer->data(),
                                    _n_cols * _n_rows, _n_objects);
  std::vector<int> h_seg_start_inds_flow_Zbuffer(_n_objects + 1);
  d_seg_start_inds_->copyTo(h_seg_start_inds_flow_Zbuffer, _n_objects + 1);

  cub_radix_sorter_->sort(*d_valid_Zbuffer, *d_value, *d_key_buf, *d_value_buf);
  pose::extractLabelStartingIndices(d_seg_start_inds_->data(),
                                    d_valid_Zbuffer->data(), _n_cols * _n_rows,
                                    _n_objects);
  std::vector<int> h_seg_start_inds_Zbuffer(_n_objects + 1);
  d_seg_start_inds_->copyTo(h_seg_start_inds_Zbuffer, _n_objects + 1);

  //  for(int o=0;o<(_n_objects+1);o++)
  //    printf("obj %d - flow+z %09d - z
  // %09d\n",o,h_seg_start_inds_flow_Zbuffer[o],h_seg_start_inds_Zbuffer[o]);

  int n_valid_flow_Zbuffer = (h_seg_start_inds_flow_Zbuffer.at(_n_objects) >= 0)
                                 ? h_seg_start_inds_flow_Zbuffer.at(_n_objects)
                                 : (_n_cols * _n_rows);

  _seg_lengths_flow_Zbuffer.assign(_n_objects, 0);

  getSegmentLengths(_seg_lengths_flow_Zbuffer, h_seg_start_inds_flow_Zbuffer,
                    _n_objects, n_valid_flow_Zbuffer);

  int n_valid_Zbuffer = (h_seg_start_inds_Zbuffer.at(_n_objects) >= 0)
                            ? h_seg_start_inds_Zbuffer.at(_n_objects)
                            : (_n_cols * _n_rows);

  _seg_lengths_Zbuffer.assign(_n_objects, 0);
  getSegmentLengths(_seg_lengths_Zbuffer, h_seg_start_inds_Zbuffer, _n_objects,
                    n_valid_Zbuffer);

  //  for(int o=0;o<(_n_objects+1);o++)
  //    printf("obj %d - start+z %09d - length+z %09d - start %09d - length
  // %09d\n",o,h_seg_start_inds_flow_Zbuffer[o],seg_lengths_flow_Zbuffer[o],h_seg_start_inds_Zbuffer[o],seg_lengths_Zbuffer[o]);

  // save proportion valid for each object
  auto bounding_boxes =
      _multipleRigidModelsOgre->getBoundingBoxesInCameraImage(poses);

  for (int o = 0; o < _n_objects; o++) {

    double absolute_count = (double)_seg_lengths_flow_Zbuffer[o];
    double prop = (_seg_lengths_Zbuffer[o] > 0)
                      ? (double)_seg_lengths_flow_Zbuffer[o] /
                            (double)_seg_lengths_Zbuffer[o]
                      : 0.0;

    if (parameters_.check_reliability_) {
      // does the proportion valid AR flow exceed the threshold?
      bool valid = prop > parameters_.reliability_threshold_;

      // is the projected shape 'sufficiently two-dimensional'?
      // the AR flow-based reliability measure fails on near-edge-like object
      // projections
      Eigen::Map<Eigen::Matrix<double, 2, 8> > bb_pixel(
          bounding_boxes.at(o).data());

      // subtract mean
      Eigen::Vector2d mn = bb_pixel.rowwise().mean();
      bb_pixel.colwise() -= mn;

      // covariance
      double ratio = parameters_.max_proportion_projected_bounding_box_;
      Eigen::Matrix2d cov = bb_pixel * bb_pixel.transpose();
      // check validity
      if (std::isfinite(cov(0, 0)) && std::isfinite(cov(0, 1)) &&
          std::isfinite(cov(1, 0)) && std::isfinite(cov(1, 1))) {
        Eigen::EigenSolver<Eigen::Matrix2d> eigen_solver(cov);
        // rotate bounding box
        Eigen::Matrix2d real_eigenvectors = eigen_solver.eigenvectors().real();
        //        Eigen::VectorXd w = eigen_solver.eigenvalues().real();
        bb_pixel = real_eigenvectors.transpose() * bb_pixel;
        // find extent
        auto extent =
            bb_pixel.rowwise().maxCoeff() - bb_pixel.rowwise().minCoeff();
        ratio = extent.maxCoeff() / extent.minCoeff();
      }
      valid =
          valid && (ratio < parameters_.max_proportion_projected_bounding_box_);

      poses.at(o).setValid(valid);
    }

    ar_flow_prop_valid.push_back(prop);
    ar_flow_abs_valid.push_back(absolute_count);
  }
}

void D_MultipleRigidPoses::robustPoseUpdates(
    const float *d_flowx, const float *d_flowy, const float *d_ar_flowx,
    const float *d_ar_flowy, const float *d_disparity, size_t d_disparity_pitch,
    int segments_to_update) {

  cudaArray *d_ZbufferArray = _multipleRigidModelsOgre->getZBuffer();
  cudaArray *d_normalXArray = _multipleRigidModelsOgre->getNormalX();
  cudaArray *d_normalYArray = _multipleRigidModelsOgre->getNormalY();
  cudaArray *d_normalZArray = _multipleRigidModelsOgre->getNormalZ();
  cudaArray *d_segmentINDArray = _multipleRigidModelsOgre->getSegmentIND();

#ifdef TIME_STEPS
  // Setup timers
  cudaEvent_t start, end, start_sub, end_sub;
  float elapsed_time;
  cudaEventCreate(&start);
  cudaEventCreate(&end);
  cudaEventCreate(&start_sub);
  cudaEventCreate(&end_sub);
#endif
#ifdef TIME_STEPS
  cudaEventRecord(start, 0); // preprocessing
#endif

  // initialize normal equations to zero
  std::fill(h_CO_reduced_.begin(), h_CO_reduced_.end(), 0);
  std::fill(h_CD_reduced_.begin(), h_CD_reduced_.end(), 0);
  cudaMemset(d_CO_reduced_->data(), 0,
             _N_CON_FLOW * parameters_.max_objects_ * sizeof(float));
  cudaMemset(d_CD_reduced_->data(), 0,
             _N_CON_DISP * parameters_.max_objects_ * sizeof(float));

  // Determine Zbuffer conversion constants
  // depth = Z_conv1/(Zbuffer+Z_conv2)
  float Z_conv1, Z_conv2;
  get_GL_conv_constants(Z_conv1, Z_conv2, parameters_.far_plane_,
                        parameters_.near_plane_);

// Mark valid locations with segment index
// If a cue's weight equals 0 mark everything invalid
// No initialization required

#ifdef TIME_STEPS
  cudaEventRecord(start_sub, 0);
#endif

  mark_with_zero_based_segmentIND(
      d_valid_flow_Zbuffer_->data(), d_valid_disparity_Zbuffer_->data(),
      d_flowx, d_ar_flowx, (const char *)d_disparity, d_segmentINDArray,
      _n_cols, _n_rows, _n_objects, d_disparity_pitch, parameters_.w_flow_ > 0,
      parameters_.w_ar_flow_ > 0, parameters_.w_disp_ > 0, segments_to_update);

#ifdef TIME_STEPS
  cudaThreadSynchronize();
  cudaEventRecord(end_sub, 0);
  cudaEventSynchronize(end_sub);
  cudaEventElapsedTime(&elapsed_time, start_sub, end_sub);
  _compTimes.at(3) += (double)elapsed_time;
#endif

#ifdef TIME_STEPS
  cudaEventRecord(start_sub, 0);
#endif

  // Radix sort all the indices using the valid marks
  // the _sub_ buffers are only used for double-buffering (ping-pong)
  d_ind_flow_Zbuffer_->copyFrom(*d_linear_ind_);
  d_ind_disparity_Zbuffer_->copyFrom(*d_linear_ind_, _n_cols * _n_rows);

  //  util::TimerGPU sort_timer;
  cub_radix_sorter_->sort(*d_valid_flow_Zbuffer_, *d_ind_flow_Zbuffer_,
                          *d_valid_flow_Zbuffer_sub_, *d_ind_flow_Zbuffer_sub_);
  //  std::cout << "flow sort time: " << sort_timer.read();
  //  sort_timer.reset();
  cub_radix_sorter_->sort(
      *d_valid_disparity_Zbuffer_, *d_ind_disparity_Zbuffer_,
      *d_valid_disparity_Zbuffer_sub_, *d_ind_disparity_Zbuffer_sub_);
//  std::cout << " disp sort time: " << sort_timer.read() << std::endl;

#ifdef TIME_STEPS
  cudaThreadSynchronize();
  cudaEventRecord(end_sub, 0);
  cudaEventSynchronize(end_sub);
  cudaEventElapsedTime(&elapsed_time, start_sub, end_sub);
  _compTimes.at(4) += (double)elapsed_time;
#endif

  // Get starting indices
  pose::extractLabelStartingIndices(d_seg_start_inds_->data(),
                                    d_valid_flow_Zbuffer_->data(),
                                    _n_cols * _n_rows * _N_FLOWS, _n_objects);
  std::vector<int> h_seg_start_inds_flow(_n_objects + 1);
  d_seg_start_inds_->copyTo(h_seg_start_inds_flow, _n_objects + 1);
  pose::extractLabelStartingIndices(d_seg_start_inds_->data(),
                                    d_valid_disparity_Zbuffer_->data(),
                                    _n_cols * _n_rows, _n_objects);
  std::vector<int> h_seg_start_inds_disparity(_n_objects + 1);
  d_seg_start_inds_->copyTo(h_seg_start_inds_disparity, _n_objects + 1);

  //  printf("before sub\n");
  //  for(int i=0;i<(_n_objects+1);i++)
  //    printf("se %d f %+07d d
  // %+07d\n",i,h_seg_start_inds_flow[i],h_seg_start_inds_disparity[i]);

  // Regularly subsample flow and disparity indices if required when a maximum
  // number of samples has been enforced
  // Relative proportion flow/disp will be maintained approximately

  int n_valid_flow_Zbuffer = (h_seg_start_inds_flow.at(_n_objects) >= 0)
                                 ? h_seg_start_inds_flow.at(_n_objects)
                                 : (_n_cols * _n_rows * _N_FLOWS);
  int n_valid_disparity_Zbuffer =
      (h_seg_start_inds_disparity.at(_n_objects) >= 0)
          ? h_seg_start_inds_disparity.at(_n_objects)
          : (_n_cols * _n_rows);

  //  printf("before sub %s\n",cudaGetErrorString(cudaGetLastError()));
  //  printf("val flow %d val disp
  // %d\n",n_valid_flow_Zbuffer,n_valid_disparity_Zbuffer);

  int n_valid_total = n_valid_flow_Zbuffer + n_valid_disparity_Zbuffer;

  if (n_valid_total > parameters_.max_samples_) {

    // proportionally subsample flow and disparity
    double sub_factor =
        (double)parameters_.max_samples_ / (double)n_valid_total;
    double inv_sub_factor = 1.0 / sub_factor;

    // flow
    int n_valid_flow_Zbuffer_sub =
        (int)floor((double)n_valid_flow_Zbuffer * sub_factor);

    if (n_valid_flow_Zbuffer_sub > 0) {
      subsample_ind_and_labels(
          d_ind_flow_Zbuffer_sub_->data(), d_ind_flow_Zbuffer_->data(),
          d_valid_flow_Zbuffer_sub_->data(), d_valid_flow_Zbuffer_->data(),
          n_valid_flow_Zbuffer_sub, (float)inv_sub_factor);

      // update all regular variables to account for subsampling
      // swap full and subsampled storage!
      n_valid_flow_Zbuffer = n_valid_flow_Zbuffer_sub;
      d_ind_flow_Zbuffer_->swap(*d_ind_flow_Zbuffer_sub_);
      d_valid_flow_Zbuffer_->swap(*d_valid_flow_Zbuffer_sub_);

      // update segment starting indices
      pose::extractLabelStartingIndices(d_seg_start_inds_->data(),
                                        d_valid_flow_Zbuffer_->data(),
                                        n_valid_flow_Zbuffer, _n_objects);
      d_seg_start_inds_->copyTo(h_seg_start_inds_flow, _n_objects + 1);
      //      printf("valsub flow %d\n",n_valid_flow_Zbuffer);
    }

    // disparity
    int n_valid_disparity_Zbuffer_sub =
        (int)floor((double)n_valid_disparity_Zbuffer * sub_factor);

    if (n_valid_disparity_Zbuffer_sub > 0) {
      subsample_ind_and_labels(d_ind_disparity_Zbuffer_sub_->data(),
                               d_ind_disparity_Zbuffer_->data(),
                               d_valid_disparity_Zbuffer_sub_->data(),
                               d_valid_disparity_Zbuffer_->data(),
                               n_valid_disparity_Zbuffer_sub,
                               (float)inv_sub_factor);

      // update all regular variables to account for subsampling
      // swap full and subsampled storage!
      n_valid_disparity_Zbuffer = n_valid_disparity_Zbuffer_sub;
      d_ind_disparity_Zbuffer_->swap(*d_ind_disparity_Zbuffer_sub_);
      d_valid_disparity_Zbuffer_->swap(*d_valid_disparity_Zbuffer_sub_);

      // update segment starting indices
      pose::extractLabelStartingIndices(d_seg_start_inds_->data(),
                                        d_valid_disparity_Zbuffer_->data(),
                                        n_valid_disparity_Zbuffer, _n_objects);
      d_seg_start_inds_->copyTo(h_seg_start_inds_disparity, _n_objects + 1);
      //      printf("valsub disp %d\n",n_valid_disparity_Zbuffer);
    }

    //    printf("after sub\n");
    //    for(int i=0;i<(_n_objects+1);i++)
    //      printf("se %d f %+07d d
    // %+07d\n",i,h_seg_start_inds_flow[i],h_seg_start_inds_disparity[i]);

  } // subsample

  // compute segment lengths
  std::vector<int> seg_lengths_flow(_n_objects);
  getSegmentLengths(seg_lengths_flow, h_seg_start_inds_flow, _n_objects,
                    n_valid_flow_Zbuffer);
  std::vector<int> seg_lengths_disparity(_n_objects);
  getSegmentLengths(seg_lengths_disparity, h_seg_start_inds_disparity,
                    _n_objects, n_valid_disparity_Zbuffer);

//  printf("after subsampling\n");
//  printf("n_f %07d n_d %07d\n", n_valid_flow_Zbuffer,
//         n_valid_disparity_Zbuffer);
//  for (int i = 0; i < _n_objects; i++)
//    printf("se %d f %+07d (%07d) d %+07d (%07d)\n", i,
// h_seg_start_inds_flow[i],
//           seg_lengths_flow[i], h_seg_start_inds_disparity[i],
//           seg_lengths_disparity[i]);

#ifdef TIME_STEPS
  cudaEventRecord(start_sub, 0);
#endif

  // Gather flow and disparity
  if (n_valid_flow_Zbuffer > 0)
    gather_valid_flow_Zbuffer(
        d_flow_compact_->data(), d_Zbuffer_flow_compact_->data(), d_flowx,
        d_flowy, d_ar_flowx, d_ar_flowy, d_ind_flow_Zbuffer_->data(),
        d_ZbufferArray, n_valid_flow_Zbuffer, _n_cols, _n_rows, Z_conv1,
        Z_conv2);

  if (n_valid_disparity_Zbuffer > 0)
    gather_valid_disparity_Zbuffer(
        d_disparity_compact_->data(), d_Zbuffer_normals_compact_->data(),
        (const char *)d_disparity, d_ind_disparity_Zbuffer_->data(),
        d_ZbufferArray, d_normalXArray, d_normalYArray, d_normalZArray,
        n_valid_disparity_Zbuffer, _n_cols, _n_rows, Z_conv1, Z_conv2,
        d_disparity_pitch);

#ifdef TIME_STEPS
  cudaThreadSynchronize();
  cudaEventRecord(end_sub, 0);
  cudaEventSynchronize(end_sub);
  cudaEventElapsedTime(&elapsed_time, start_sub, end_sub);
  _compTimes.at(5) += (double)elapsed_time;
#endif

  // Gather index information
  _segment_info.clear();

  for (int o = 0; o < _n_objects; o++) {
    int nf = seg_lengths_flow[o];
    int nd = seg_lengths_disparity[o];
    if ((nf > 0) || (nd > 0)) {
      SegmentINFO tmp;
      tmp.segment_ind = o;
      tmp.n_values_flow = nf;
      tmp.n_values_disparity = nd;
      tmp.start_ind_flow = h_seg_start_inds_flow[o];
      tmp.start_ind_disparity = h_seg_start_inds_disparity[o];
      _segment_info.push_back(tmp);
    }
  }

//    printf("------------------------------------\n");
//    for(int o=0;o<_segment_info.size();o++) {
//      SegmentINFO &tmp = _segment_info.at(o);
//      printf("seg %d f_start %06d f_size %06d d_start %06d d_size
// %06d\n",tmp.segment_ind,tmp.start_ind_flow,tmp.n_values_flow,tmp.start_ind_disparity,tmp.n_values_disparity);
//    }
//    printf("------------------------------------\n");

#ifdef TIME_STEPS
  cudaThreadSynchronize();
  cudaEventRecord(end, 0);
  cudaEventSynchronize(end);
  cudaEventElapsedTime(&elapsed_time, start, end);
  //  printf("preproc time : %2.4f ms\n",elapsed_time);
  _compTimes.at(0) += (double)elapsed_time;
#endif

  // preparatory book-keeping

  int n_segments = _segment_info.size();

  if (n_segments > 0) {

    std::vector<int> n_values_flow(n_segments);
    std::vector<int> start_ind_flow(n_segments);
    std::vector<int> n_values_disparity(n_segments);
    std::vector<int> start_ind_disparity(n_segments);
    std::vector<int> start_ind_res_flow(n_segments);
    std::vector<int> start_ind_res_disparity(n_segments);

    for (int s = 0; s < n_segments; s++) {
      SegmentINFO &tmp = _segment_info.at(s);
      n_values_flow.at(s) = tmp.n_values_flow;
      start_ind_flow.at(s) = tmp.start_ind_flow;
      n_values_disparity.at(s) = tmp.n_values_disparity;
      start_ind_disparity.at(s) = tmp.start_ind_disparity;

      start_ind_res_flow.at(s) = (s > 0) ? (start_ind_res_disparity.at(s - 1) +
                                            n_values_disparity.at(s - 1))
                                         : 0;
      start_ind_res_disparity.at(s) =
          start_ind_res_flow.at(s) + n_values_flow.at(s);
    }

    // Compute offset between residual input and output indices. Residuals are
    // stored as [seg1_flow seg1_disp seg2_flow seg2_disp ... ] to facilitate
    // median extraction

    std::vector<int> offset_ind_res_flow(n_segments);
    std::vector<int> offset_ind_res_disparity(n_segments);

    for (int s = 0; s < n_segments; s++) {
      offset_ind_res_flow.at(s) =
          start_ind_res_flow.at(s) - start_ind_flow.at(s);
      offset_ind_res_disparity.at(s) =
          start_ind_res_disparity.at(s) - start_ind_disparity.at(s);
    }

    // Create segment translation table (map zero-based original segment indices
    // to compressed segment indices)

    std::vector<int> segment_translation_table(_n_objects);
    for (int s = 0; s < n_segments; s++)
      segment_translation_table.at(_segment_info.at(s).segment_ind) = s;

    d_segment_translation_table_->copyFrom(segment_translation_table,
                                           _n_objects);
    d_n_values_flow_->copyFrom(n_values_flow, n_segments);
    d_start_ind_flow_->copyFrom(start_ind_flow, n_segments);
    d_n_values_disparity_->copyFrom(n_values_disparity, n_segments);
    d_start_ind_disparity_->copyFrom(start_ind_disparity, n_segments);
    d_offset_ind_res_flow_->copyFrom(offset_ind_res_flow, n_segments);
    d_offset_ind_res_disparity_->copyFrom(offset_ind_res_disparity, n_segments);
  }

  // take care here since _d_CO and _d_CD's maximum sizes are not exactly
  // determined in this way (especially the times 4)

  dim3 threadBlock_normal(64, 1, 1);
  int gridDim_x_normal_equations =
      (n_segments > 0)
          ? divUp(_MAX_N_VAL_ACCUM, n_segments * threadBlock_normal.x) * 4
          : 0;
  dim3 blockGrid_normal(gridDim_x_normal_equations, n_segments);
  //  dim3 threadBlock_reduce_64(64,1);
  //  dim3 blockGrid_reduce_flow(_N_CON_FLOW, n_segments);
  //  dim3 blockGrid_reduce_disparity(_N_CON_DISP, n_segments);

  dim3 threadBlock_reduce_64_mult(64, 4);
  dim3 blockGrid_reduce_flow_mult(divUp(_N_CON_FLOW, 4), n_segments);
  dim3 blockGrid_reduce_disparity_mult(divUp(_N_CON_DISP, 4), n_segments);

/****************/
/* OLS Estimate */
/****************/

#ifdef TIME_STEPS
  cudaEventRecord(start, 0); // ols-time
#endif

  if (n_segments > 0) {

#ifdef TIME_STEPS
    cudaEventRecord(start_sub, 0);
#endif

    normal_eqs_flow(blockGrid_normal, threadBlock_normal, d_CO_->data(),
                    d_flow_compact_->data(), d_Zbuffer_flow_compact_->data(),
                    d_ind_flow_Zbuffer_->data(), _focal_length_x,
                    _focal_length_y, _nodal_point_x, _nodal_point_y, _n_rows,
                    _n_cols, d_n_values_flow_->data(),
                    d_start_ind_flow_->data());
    normal_eqs_disparity(blockGrid_normal, threadBlock_normal, d_CD_->data(),
                         d_disparity_compact_->data(),
                         d_Zbuffer_normals_compact_->data(),
                         d_ind_disparity_Zbuffer_->data(), _focal_length_x,
                         _focal_length_y, _nodal_point_x, _nodal_point_y,
                         _baseline, _n_cols, d_n_values_disparity_->data(),
                         d_start_ind_disparity_->data(), parameters_.w_disp_);

#ifdef TIME_STEPS
    cudaThreadSynchronize();
    cudaEventRecord(end_sub, 0);
    cudaEventSynchronize(end_sub);
    cudaEventElapsedTime(&elapsed_time, start_sub, end_sub);
    _compTimes.at(6) += (double)elapsed_time;
#endif

#ifdef TIME_STEPS
    cudaEventRecord(start_sub, 0);
#endif

    reduce_normal_eqs_64_mult_constr(blockGrid_reduce_flow_mult,
                                     threadBlock_reduce_64_mult,
                                     d_CO_reduced_->data(), d_CO_->data(),
                                     gridDim_x_normal_equations, _N_CON_FLOW);
    reduce_normal_eqs_64_mult_constr(blockGrid_reduce_disparity_mult,
                                     threadBlock_reduce_64_mult,
                                     d_CD_reduced_->data(), d_CD_->data(),
                                     gridDim_x_normal_equations, _N_CON_DISP);

//    reduce_normal_eqs_64_GPU<<<blockGrid_reduce_flow,threadBlock_reduce_64>>>(_d_CO_reduced,
// _d_CO, gridDim_x_normal_equations);
//    reduce_normal_eqs_64_GPU<<<blockGrid_reduce_disparity,threadBlock_reduce_64>>>(_d_CD_reduced,
// _d_CD, gridDim_x_normal_equations);

#ifdef TIME_STEPS
    cudaThreadSynchronize();
    cudaEventRecord(end_sub, 0);
    cudaEventSynchronize(end_sub);
    cudaEventElapsedTime(&elapsed_time, start_sub, end_sub);
    _compTimes.at(7) += (double)elapsed_time;
#endif
  }

  d_CO_reduced_->copyTo(h_CO_reduced_);
  d_CD_reduced_->copyTo(h_CD_reduced_);

#ifdef TIME_STEPS
  cudaEventRecord(start_sub, 0);
#endif

  // Solve systems
  std::vector<float> dTdR(6 * n_segments);
  for (int s = 0; s < n_segments; s++) {
    int curr_part = _segment_info.at(s).segment_ind;
    segment_normal_eqs_.at(curr_part).compose(
        &h_CO_reduced_.at(_N_CON_FLOW * s), &h_CD_reduced_.at(_N_CON_DISP * s));
    segment_normal_eqs_.at(curr_part).solve(&dTdR.at(6 * s));
  }

#ifdef TIME_STEPS
  cudaThreadSynchronize();
  cudaEventRecord(end_sub, 0);
  cudaEventSynchronize(end_sub);
  cudaEventElapsedTime(&elapsed_time, start_sub, end_sub);
  _compTimes.at(8) += (double)elapsed_time;
#endif

  // Copy to device memory
  d_dTR_->copyFrom(dTdR, 6 * n_segments);

  //  for(int s=0;s<n_segments;s++) {
  //    printf("seg %d - ",_segment_info.at(s).segment_ind);
  //    for(int i=0;i<6;i++)
  //      printf("%+03.4f ",dTdR[s*6+i]);
  //    printf("\n");
  //  }

  // Store OLS estimates
  _OLSDeltaPoses.clear();
  for (int i = 0; i < n_segments; i++)
    _OLSDeltaPoses.push_back(TranslationRotation3D(&dTdR[i * 6]));

#ifdef TIME_STEPS
  cudaThreadSynchronize();
  cudaEventRecord(end, 0);
  cudaEventSynchronize(end);
  cudaEventElapsedTime(&elapsed_time, start, end);
  //  printf("OLS time : %2.4f ms\n",elapsed_time);
  _compTimes.at(1) += (double)elapsed_time;
#endif

/****************/
/* M-estimation */
/****************/

#ifdef TIME_STEPS
  cudaEventRecord(start, 0); // robust-time
#endif

  for (int it = 0; it < parameters_.n_icp_inner_it_; it++) {

    // Compute flow and disparity residuals

    if (n_segments > 0) {

#ifdef TIME_STEPS
      cudaEventRecord(start_sub, 0);
#endif

      dim3 threadBlock_res(256, 1);

      if (n_valid_flow_Zbuffer > 0) {
        dim3 blockGrid_res(divUp(n_valid_flow_Zbuffer, threadBlock_res.x), 1);
        flow_absolute_residual_scalable(
            blockGrid_res, threadBlock_res, d_abs_res_->data(),
            d_flow_compact_->data(), d_Zbuffer_flow_compact_->data(),
            d_ind_flow_Zbuffer_->data(), d_valid_flow_Zbuffer_->data(),
            _focal_length_x, _focal_length_y, _nodal_point_x, _nodal_point_y,
            _n_rows, _n_cols, n_valid_flow_Zbuffer,
            d_offset_ind_res_flow_->data(),
            d_segment_translation_table_->data(), parameters_.w_flow_,
            parameters_.w_ar_flow_, d_dTR_->data());
      }

      if (n_valid_disparity_Zbuffer > 0) {
        dim3 blockGrid_res(divUp(n_valid_disparity_Zbuffer, threadBlock_res.x),
                           1);
        disp_absolute_residual_scalable(
            blockGrid_res, threadBlock_res, d_abs_res_->data(),
            d_disparity_compact_->data(), d_Zbuffer_normals_compact_->data(),
            d_ind_disparity_Zbuffer_->data(),
            d_valid_disparity_Zbuffer_->data(), _focal_length_x,
            _focal_length_y, _nodal_point_x, _nodal_point_y, _baseline, _n_cols,
            n_valid_disparity_Zbuffer, d_offset_ind_res_disparity_->data(),
            d_segment_translation_table_->data(), parameters_.w_disp_,
            d_dTR_->data());
      }

#ifdef TIME_STEPS
      cudaThreadSynchronize();
      cudaEventRecord(end_sub, 0);
      cudaEventSynchronize(end_sub);
      cudaEventElapsedTime(&elapsed_time, start_sub, end_sub);
      _compTimes.at(9) += (double)elapsed_time;
#endif

#ifdef TIME_STEPS
      cudaEventRecord(start_sub, 0);
#endif

      // Median absolute residuals
      std::vector<float> abs_res_scales(n_segments);
      int pp[n_segments];
      for (int s = 0; s < n_segments; s++) {
        SegmentINFO &tmp = _segment_info.at(s);
        pp[s] = tmp.n_values_flow + tmp.n_values_disparity;
      }
      approx_multiple_medians_shuffle_cuda(
          abs_res_scales.data(), d_abs_res_->data(), d_median_tmp_->data(),
          d_random_numbers_->data(), pp, n_segments, d_median_n_in_->data(),
          d_median_start_inds_->data());

      for (int s = 0; s < n_segments; s++)
        abs_res_scales.at(s) *= 6.9460f;

      d_abs_res_scales_->copyFrom(abs_res_scales, n_segments);

#ifdef TIME_STEPS
      cudaThreadSynchronize();
      cudaEventRecord(end_sub, 0);
      cudaEventSynchronize(end_sub);
      cudaEventElapsedTime(&elapsed_time, start_sub, end_sub);
      _compTimes.at(10) += (double)elapsed_time;
#endif

//  for(int s=0;s<n_segments;s++)
//    printf("%2.5f ",abs_res_scales[s]);
//  printf("\n");

// Weighted constraints

#ifdef TIME_STEPS
      cudaEventRecord(start_sub, 0);
#endif

      normal_eqs_flow_weighted(
          blockGrid_normal, threadBlock_normal, d_CO_->data(),
          d_flow_compact_->data(), d_Zbuffer_flow_compact_->data(),
          d_ind_flow_Zbuffer_->data(), _focal_length_x, _focal_length_y,
          _nodal_point_x, _nodal_point_y, _n_rows, _n_cols,
          d_n_values_flow_->data(), d_start_ind_flow_->data(),
          d_abs_res_scales_->data(), parameters_.w_flow_,
          parameters_.w_ar_flow_, d_dTR_->data());

      normal_eqs_disparity_weighted(
          blockGrid_normal, threadBlock_normal, d_CD_->data(),
          d_disparity_compact_->data(), d_Zbuffer_normals_compact_->data(),
          d_ind_disparity_Zbuffer_->data(), _focal_length_x, _focal_length_y,
          _nodal_point_x, _nodal_point_y, _baseline, _n_cols,
          d_n_values_disparity_->data(), d_start_ind_disparity_->data(),
          d_abs_res_scales_->data(), parameters_.w_disp_, d_dTR_->data());

#ifdef TIME_STEPS
      cudaThreadSynchronize();
      cudaEventRecord(end_sub, 0);
      cudaEventSynchronize(end_sub);
      cudaEventElapsedTime(&elapsed_time, start_sub, end_sub);
      _compTimes.at(6) += (double)elapsed_time;
#endif

#ifdef TIME_STEPS
      cudaEventRecord(start_sub, 0);
#endif

      reduce_normal_eqs_64_mult_constr(blockGrid_reduce_flow_mult,
                                       threadBlock_reduce_64_mult,
                                       d_CO_reduced_->data(), d_CO_->data(),
                                       gridDim_x_normal_equations, _N_CON_FLOW);
      reduce_normal_eqs_64_mult_constr(blockGrid_reduce_disparity_mult,
                                       threadBlock_reduce_64_mult,
                                       d_CD_reduced_->data(), d_CD_->data(),
                                       gridDim_x_normal_equations, _N_CON_DISP);

//      reduce_normal_eqs_64_GPU<<<blockGrid_reduce_flow,threadBlock_reduce_64>>>(_d_CO_reduced,
// _d_CO, gridDim_x_normal_equations);
//      reduce_normal_eqs_64_GPU<<<blockGrid_reduce_disparity,threadBlock_reduce_64>>>(_d_CD_reduced,
// _d_CD, gridDim_x_normal_equations);

#ifdef TIME_STEPS
      cudaThreadSynchronize();
      cudaEventRecord(end_sub, 0);
      cudaEventSynchronize(end_sub);
      cudaEventElapsedTime(&elapsed_time, start_sub, end_sub);
      _compTimes.at(7) += (double)elapsed_time;
#endif
    }

    d_CO_reduced_->copyTo(h_CO_reduced_);
    d_CD_reduced_->copyTo(h_CD_reduced_);

#ifdef TIME_STEPS
    cudaEventRecord(start_sub, 0);
#endif

    // Solve systems
    for (int s = 0; s < n_segments; s++) {
      int curr_part = _segment_info.at(s).segment_ind;
      segment_normal_eqs_.at(curr_part)
          .compose(&h_CO_reduced_.at(_N_CON_FLOW * s),
                   &h_CD_reduced_.at(_N_CON_DISP * s));
      segment_normal_eqs_.at(curr_part).solve(&dTdR.at(6 * s));
    }

#ifdef TIME_STEPS
    cudaThreadSynchronize();
    cudaEventRecord(end_sub, 0);
    cudaEventSynchronize(end_sub);
    cudaEventElapsedTime(&elapsed_time, start_sub, end_sub);
    _compTimes.at(8) += (double)elapsed_time;
#endif

    // Copy to device memory
    d_dTR_->copyFrom(dTdR, 6 * n_segments);

    //    printf("ROBUST ITERATION %d\n",it);
    //    printf("-------------------\n");
    //    for(int s=0;s<n_segments;s++) {
    //      printf("seg %d - ",_segment_info.at(s).segment_ind);
    //      for(int i=0;i<6;i++)
    //        printf("%+03.4f ",dTdR[s*6+i]);
    //      printf("\n");
    //    }
  }

  // Store robust estimates
  _robustDeltaPoses.clear();
  for (int i = 0; i < n_segments; i++)
    _robustDeltaPoses.push_back(TranslationRotation3D(&dTdR[i * 6]));

#ifdef TIME_STEPS
  cudaThreadSynchronize();
  cudaEventRecord(end, 0);
  cudaEventSynchronize(end);
  cudaEventElapsedTime(&elapsed_time, start, end);
  //  printf("robust time : %2.4f ms\n",elapsed_time);
  _compTimes.at(2) += (double)elapsed_time;
#endif
}
}


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

    @copyright 2016 NumScale SAS

  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_FUNCTION_IFIX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IFIX_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object convert to integer by saturated truncation.

    This is an alias of saturated_(toint) (See @ref toint)

    @par Header <boost/simd/function/ifix.hpp>

    @par Example:

      @snippet ifix.cpp ifix

    @par Possible output:

      @snippet ifix.txt ifix

  **/
  Value ifix(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/ifix.hpp>
#include <boost/simd/function/simd/ifix.hpp>

#endif


#ifndef DART_NEURAL_MAPPING_HPP_
#define DART_NEURAL_MAPPING_HPP_

#include <memory>
#include <optional>

#include <Eigen/Dense>

#include "dart/math/MathTypes.hpp"

namespace dart {

namespace simulation {
class World;
}

namespace neural {

class Mapping
{
public:
  Mapping();

  virtual ~Mapping();

  virtual int getPosDim() = 0;
  virtual int getVelDim() = 0;
  virtual int getControlForceDim() = 0;
  virtual int getMassDim() = 0;

  virtual void setPositions(
      std::shared_ptr<simulation::World> world,
      const Eigen::Ref<Eigen::VectorXs>& positions)
      = 0;
  virtual void setVelocities(
      std::shared_ptr<simulation::World> world,
      const Eigen::Ref<Eigen::VectorXs>& velocities)
      = 0;
  virtual void setControlForces(
      std::shared_ptr<simulation::World> world,
      const Eigen::Ref<Eigen::VectorXs>& forces)
      = 0;
  virtual void setMasses(
      std::shared_ptr<simulation::World> world,
      const Eigen::Ref<Eigen::VectorXs>& masses)
      = 0;

  virtual void getPositionsInPlace(
      std::shared_ptr<simulation::World> world,
      /* OUT */ Eigen::Ref<Eigen::VectorXs> positions)
      = 0;
  virtual void getVelocitiesInPlace(
      std::shared_ptr<simulation::World> world,
      /* OUT */ Eigen::Ref<Eigen::VectorXs> velocities)
      = 0;
  virtual void getControlForcesInPlace(
      std::shared_ptr<simulation::World> world,
      /* OUT */ Eigen::Ref<Eigen::VectorXs> forces)
      = 0;
  virtual void getMassesInPlace(
      std::shared_ptr<simulation::World> world,
      /* OUT */ Eigen::Ref<Eigen::VectorXs> masses)
      = 0;

  Eigen::VectorXs getPositions(std::shared_ptr<simulation::World> world);
  Eigen::VectorXs getVelocities(std::shared_ptr<simulation::World> world);
  Eigen::VectorXs getControlForces(std::shared_ptr<simulation::World> world);
  Eigen::VectorXs getMasses(std::shared_ptr<simulation::World> world);

  /// Check if a Jacobian is equal
  void equalsOrCrash(
      Eigen::MatrixXs analytical,
      Eigen::MatrixXs bruteForce,
      std::shared_ptr<simulation::World> world,
      const std::string& name);

  /// This gets a Jacobian relating the changes in the inner positions (the
  /// "real" positions) to the corresponding outer positions (the "mapped"
  /// positions)
  virtual Eigen::MatrixXs getRealPosToMappedPosJac(
      std::shared_ptr<simulation::World> world)
      = 0;

  Eigen::MatrixXs finiteDifferenceRealPosToMappedPosJac(
      std::shared_ptr<simulation::World> world, bool useRidders = true);

  Eigen::MatrixXs finiteDifferenceRiddersRealPosToMappedPosJac(
      std::shared_ptr<simulation::World> world);

  /// This gets a Jacobian relating the changes in the inner velocities (the
  /// "real" velocities) to the corresponding outer positions (the "mapped"
  /// positions)
  virtual Eigen::MatrixXs getRealVelToMappedPosJac(
      std::shared_ptr<simulation::World> world)
      = 0;

  Eigen::MatrixXs finiteDifferenceRealVelToMappedPosJac(
      std::shared_ptr<simulation::World> world, bool useRidders = true);

  Eigen::MatrixXs finiteDifferenceRiddersRealVelToMappedPosJac(
      std::shared_ptr<simulation::World> world);

  /// This gets a Jacobian relating the changes in the inner velocity (the
  /// "real" velocity) to the corresponding outer velocity (the "mapped"
  /// velocity)
  virtual Eigen::MatrixXs getRealVelToMappedVelJac(
      std::shared_ptr<simulation::World> world)
      = 0;

  Eigen::MatrixXs finiteDifferenceRealVelToMappedVelJac(
      std::shared_ptr<simulation::World> world, bool useRidders = true);

  Eigen::MatrixXs finiteDifferenceRiddersRealVelToMappedVelJac(
      std::shared_ptr<simulation::World> world);

  /// This gets a Jacobian relating the changes in the inner position (the
  /// "real" position) to the corresponding outer velocity (the "mapped"
  /// velocity)
  virtual Eigen::MatrixXs getRealPosToMappedVelJac(
      std::shared_ptr<simulation::World> world)
      = 0;

  Eigen::MatrixXs finiteDifferenceRealPosToMappedVelJac(
      std::shared_ptr<simulation::World> world, bool useRidders = true);

  Eigen::MatrixXs finiteDifferenceRiddersRealPosToMappedVelJac(
      std::shared_ptr<simulation::World> world);

  /// This gets a Jacobian relating the changes in the inner force (the
  /// "real" force) to the corresponding outer force (the "mapped"
  /// force)
  virtual Eigen::MatrixXs getRealForceToMappedForceJac(
      std::shared_ptr<simulation::World> world)
      = 0;

  /// This gets a Jacobian relating the changes in the inner mass (the
  /// "real" mass) to the corresponding outer mass (the "mapped"
  /// mass)
  virtual Eigen::MatrixXs getRealMassToMappedMassJac(
      std::shared_ptr<simulation::World> world)
      = 0;

  virtual Eigen::VectorXs getPositionLowerLimits(
      std::shared_ptr<simulation::World> world);
  virtual Eigen::VectorXs getPositionUpperLimits(
      std::shared_ptr<simulation::World> world);
  virtual Eigen::VectorXs getVelocityLowerLimits(
      std::shared_ptr<simulation::World> world);
  virtual Eigen::VectorXs getVelocityUpperLimits(
      std::shared_ptr<simulation::World> world);
  virtual Eigen::VectorXs getControlForceLowerLimits(
      std::shared_ptr<simulation::World> world);
  virtual Eigen::VectorXs getControlForceUpperLimits(
      std::shared_ptr<simulation::World> world);
  virtual Eigen::VectorXs getMassLowerLimits(
      std::shared_ptr<simulation::World> world);
  virtual Eigen::VectorXs getMassUpperLimits(
      std::shared_ptr<simulation::World> world);
};

} // namespace neural
} // namespace dart

#endif


// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_OPERATORS_PROLONGATIONS_HH
#define DUNE_GDT_OPERATORS_PROLONGATIONS_HH

#include <vector>
#include <limits>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/dynmatrix.hh>

#include <dune/stuff/common/type_utils.hh>
#include <dune/stuff/common/vector.hh>
#include <dune/stuff/functions/interfaces.hh>
#include <dune/stuff/grid/boundaryinfo.hh>
#include <dune/stuff/grid/intersection.hh>
#include <dune/stuff/la/container.hh>
#include <dune/stuff/la/solver.hh>

#include <dune/geometry/quadraturerules.hh>

#include <dune/stuff/grid/search.hh>

#include <dune/gdt/exceptions.hh>
#include <dune/gdt/discretefunction/default.hh>
#include <dune/gdt/spaces/cg/fem.hh>
#include <dune/gdt/spaces/cg/pdelab.hh>

namespace Dune {
namespace GDT {

namespace Spaces {
namespace DG {
template<class GridPartImp, int polynomialOrder, class RangeFieldImp, size_t rangeDim, size_t rangeDimCols>
class FemBased;
}
template<class SpaceImp>
class Block;
}

namespace Operators {

/**
 *  \note The automatic detection of the right integration order might fail, so you might want to specify
 *        over_integrate. The reason is that in order to locally evaluate the source we first have to create a
 *        quadrature, the correct order of wich we guess by taking the sources order on the first entity.
 *  \note We would have liked to do something like this and match on implementations of SpaceInterface:\code
template< class T, class VS, class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
void apply(const ConstDiscreteFunction< SpaceInterface< T >, VS >& source,
           DiscreteFunction< Spaces::DG::FemBased< GPR, pR, RR, rR, rCR >, VR >& range) const
{
  static_assert(Dune::AlwaysFalse< T >::value, "Not implemented for this combination of source and range!");
}\endcode
 *        but that gave compile errors (the compiler just could not match the first argument for whatever reason). This
 *        is why we need all combinations of spaces below which are just compile time checks and forwards.
 *
 *  \todo Create a Redirect class templatized with space/vector to check that its a const/discrete function with a
 *        method which extracts the correct space if needed. This should give better compile errors.
 */
template< class GridViewType >
class L2Prolongation
{
  typedef typename GridViewType::template Codim< 0 >::Entity EntityType;
  typedef typename GridViewType::ctype DomainFieldType;
  static const size_t dimDomain = GridViewType::dimension;

public:
  L2Prolongation(const GridViewType& grid_view)
    : grid_view_(grid_view)
  {}

  // Source: Spaces::CG::FemBased
  // Range:  Spaces::DG::FemBased

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  void apply(const ConstDiscreteFunction< Spaces::CG::FemBased< GPS, pS, RS, rS, rCS >, VS >& /*source*/,
             DiscreteFunction< Spaces::DG::FemBased< GPR, pR, RR, rR, rCR >, VR >& /*range*/) const
  {
    static_assert(Dune::AlwaysFalse< GPS >::value, "Not implemented for this combination of source and range!");
  }

  template< class GPS, int pS, class R, size_t r, size_t rC, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction< Spaces::CG::FemBased< GPS, pS, R, r, rC >, VS >& source,
                    DiscreteFunction< Spaces::DG::FemBased< GPR, pR, R, r, rC >, VR >&
                      range) const
  {
    prolong_onto_dg_fem_localfunctions_wrapper(source, range);
  }

  // Source: Spaces::DG::FemBased
  // Range:  Spaces::DG::FemBased

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  void apply(const ConstDiscreteFunction< Spaces::DG::FemBased< GPS, pS, RS, rS, rCS >, VS >& /*source*/,
             DiscreteFunction< Spaces::DG::FemBased< GPR, pR, RR, rR, rCR >, VR >& /*range*/) const
  {
    static_assert(Dune::AlwaysFalse< GPS >::value, "Not implemented for this combination of source and range!");
  }

  template< class GPS, int pS, class R, size_t r, size_t rC, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction
                      < Spaces::DG::FemBased< GPS, pS, R, r, rC >, VS >& source,
                    DiscreteFunction< Spaces::DG::FemBased< GPR, pR, R, r, rC >, VR >&
                      range) const
  {
    prolong_onto_dg_fem_localfunctions_wrapper(source, range);
  }

  template< class GPS, int pS, class R, size_t r, size_t rC, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction
                      < Spaces::Block< Spaces::DG::FemBased< GPS, pS, R, r, rC > >, VS >& source,
                    DiscreteFunction< Spaces::Block< Spaces::DG::FemBased< GPR, pR, R, r, rC > >, VR >&
                      range) const
  {
    prolong_onto_dg_fem_localfunctions_wrapper(source, range);
  }

  template< class GPS, int pS, class R, size_t r, size_t rC, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction
                      < Spaces::Block< Spaces::DG::FemBased< GPS, pS, R, r, rC > >, VS >& source,
                    DiscreteFunction< Spaces::DG::FemBased< GPR, pR, R, r, rC >, VR >& range) const
  {
    prolong_onto_dg_fem_localfunctions_wrapper(source, range);
  }

private:
  template< class SourceFunctionType, class RangeFunctionType >
  void prolong_onto_dg_fem_localfunctions_wrapper(const SourceFunctionType& source, RangeFunctionType& range) const
  {
    typedef typename RangeFunctionType::DomainType DomainType;
    typedef typename RangeFunctionType::RangeType RangeType;
    typedef typename RangeFunctionType::RangeFieldType RangeFieldType;
    typedef typename Stuff::LA::Container< RangeFieldType, Stuff::LA::default_dense_backend >::MatrixType
        LocalMatrixType;
    typedef typename Stuff::LA::Container< RangeFieldType, Stuff::LA::default_dense_backend >::VectorType
        LocalVectorType;
    // clear
    range.vector() *= 0.0;
    // create search in the source grid part
    typedef typename SourceFunctionType::SpaceType::GridViewType SourceGridViewType;
    typedef Stuff::Grid::EntityInlevelSearch< SourceGridViewType > EntitySearch;
    EntitySearch entity_search(source.space().grid_view());
    // guess the polynomial order of the source by hoping that they are the same for all entities
    const size_t source_order = source.local_function(*source.space().grid_view().template begin< 0 >())->order();
    // walk the grid
    RangeType source_value(0);
    std::vector< RangeType > basis_values(range.space().mapper().maxNumDofs());
    const auto entity_it_end = grid_view_.template end< 0 >();
    for (auto entity_it = grid_view_.template begin< 0 >(); entity_it != entity_it_end; ++entity_it) {
      // prepare
      const auto& entity = *entity_it;
      const auto local_basis = range.space().base_function_set(entity);
      auto local_range = range.local_discrete_function(entity);
      LocalMatrixType local_matrix(local_basis.size(), local_basis.size(), RangeFieldType(0));
      LocalVectorType local_vector(local_basis.size(), RangeFieldType(0));
      LocalVectorType local_DoFs(local_basis.size(), RangeFieldType(0));
      // create quadrature
      const auto integrand_order = std::max(source_order, local_basis.order()) + local_basis.order();
      const auto& quadrature = QuadratureRules< DomainFieldType, dimDomain >::rule(entity.type(),
                                                                                boost::numeric_cast< int >(integrand_order));
      // get global quadrature points
      std::vector< DomainType > quadrature_points;
      for (const auto& quadrature_point : quadrature)
        quadrature_points.emplace_back(entity.geometry().global(quadrature_point.position()));
      // get source entities
      const auto source_entity_ptr_unique_ptrs = entity_search(quadrature_points);
      assert(source_entity_ptr_unique_ptrs.size() >= quadrature_points.size());
      // loop over all quadrature points
      size_t pp = 0;
      for (const auto& quadrature_point : quadrature) {
        const auto local_point = quadrature_point.position();
        const auto quadrature_weight = quadrature_point.weight();
        const auto integration_element = entity.geometry().integrationElement(local_point);
        // evaluate source
        const auto& source_entity_ptr_unique_ptr = source_entity_ptr_unique_ptrs[pp];
        if (source_entity_ptr_unique_ptr) {
          const auto source_entity_ptr = *source_entity_ptr_unique_ptr;
          const auto& source_entity = *source_entity_ptr;
          const auto local_source = source.local_function(source_entity);
          local_source->evaluate(source_entity.geometry().local(entity.geometry().global(local_point)), source_value);
        } else
          source_value *= 0.0;
        // evaluate
        local_basis.evaluate(local_point, basis_values);
        // compute integrals
        for (size_t ii = 0; ii < local_basis.size(); ++ii) {
          local_vector[ii] += integration_element * quadrature_weight * (source_value * basis_values[ii]);
          for (size_t jj = 0; jj < local_basis.size(); ++jj) {
            local_matrix.add_to_entry(ii,
                                      jj,
                                      integration_element * quadrature_weight * (basis_values[ii] * basis_values[jj]));
          }
        }
        ++pp;
      } // loop over all quadrature points
      // compute local DoFs
      try {
        Stuff::LA::Solver< LocalMatrixType >(local_matrix).apply(local_vector, local_DoFs);
      } catch (Stuff::Exceptions::linear_solver_failed& ee) {
        DUNE_THROW(Exceptions::prolongation_error,
                   "L2 prolongation failed because a local matrix could not be inverted!\n\n"
                   << "This was the original error: " << ee.what());
      }
      // set local DoFs
      auto local_range_vector = local_range->vector();
      assert(local_range_vector.size() == local_DoFs.size());
      for (size_t ii = 0; ii < local_range_vector.size(); ++ii)
        local_range_vector.set(ii, local_DoFs.get_entry(ii));
    } // walk the grid
  } // ... prolong_onto_dg_fem_localfunctions_wrapper(...)

  const GridViewType& grid_view_;
}; // class L2Prolongation

/**
 *  \note We would have liked to do something like this and match on implementations of SpaceInterface:\code
template< class T, class VS, class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
void apply(const ConstDiscreteFunction< SpaceInterface< T >, VS >& source,
           DiscreteFunction< Spaces::CG::FemBased< GPR, pR, RR, rR, rCR >, VR >& range) const
{
  static_assert(Dune::AlwaysFalse< T >::value, "Not implemented for this combination of source and range!");
}\endcode
 *        but that gave compile errors (the compiler just could not match the first argument for whatever reason). This
 *        is why we need all combinations of spaces below which are just compile time checks and forwards.
 */
template< class GridViewType >
class LagrangeProlongation
{
public:
  typedef typename GridViewType::ctype DomainFieldType;
  static const size_t dimDomain = GridViewType::dimension;

  LagrangeProlongation(const GridViewType& grid_view)
    : grid_view_(grid_view)
  {}

  // Source: Spaces::CG::FemBased
  // Range:  Spaces::CG::FemBased

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  void apply(const ConstDiscreteFunction< Spaces::CG::FemBased< GPS, pS, RS, rS, rCS >, VS >& /*source*/,
             DiscreteFunction< Spaces::CG::FemBased< GPR, pR, RR, rR, rCR >, VR >& /*range*/) const
  {
    static_assert(Dune::AlwaysFalse< GPS >::value, "Not implemented for this combination of source and range!");
  }

  template< class GPS, int pS, class R, size_t r, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction< Spaces::CG::FemBased< GPS, pS, R, r, 1 >, VS >& source,
                    DiscreteFunction< Spaces::CG::FemBased< GPR, pR, R, r, 1 >, VR >& range) const
  {
    redirect_to_appropriate_apply(source, range);
  }

  // Source: Spaces::DG::FemBased
  // Range:  Spaces::CG::FemBased

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  void apply(const ConstDiscreteFunction< Spaces::DG::FemBased< GPS, pS, RS, rS, rCS >, VS >& /*source*/,
             DiscreteFunction< Spaces::CG::FemBased< GPR, pR, RR, rR, rCR >, VR >& /*range*/) const
  {
    static_assert(Dune::AlwaysFalse< GPS >::value, "Not implemented for this combination of source and range!");
  }

  template< class GPS, int pS, class R, size_t r, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction
                      < Spaces::DG::FemBased< GPS, pS, R, r, 1 >, VS >& source,
                    DiscreteFunction< Spaces::CG::FemBased< GPR, pR, R, r, 1 >, VR >& range) const
  {
    redirect_to_appropriate_apply(source, range);
  }

  // Source: Spaces::CG::PdelabBased
  // Range:  Spaces::CG::PdelabBased

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  void apply(const ConstDiscreteFunction< Spaces::CG::PdelabBased< GPS, pS, RS, rS, rCS >, VS >& /*source*/,
             DiscreteFunction< Spaces::CG::PdelabBased< GPR, pR, RR, rR, rCR >, VR >& /*range*/) const
  {
    static_assert(Dune::AlwaysFalse< GPS >::value, "Not implemented for this combination of source and range!");
  }

  template< class GPS, int pS, class R, size_t r, size_t rC, class VS, class GPR, class VR >
  inline void apply(const ConstDiscreteFunction < Spaces::CG::PdelabBased< GPS, pS, R, r, rC >, VS >&
                      source,
                    DiscreteFunction< Spaces::CG::PdelabBased< GPR, 1, R, r, rC >, VR >& range) const
  {
    redirect_to_appropriate_apply(source, range);
  }

private:
  template< class SourceType, class RangeType >
  void redirect_to_appropriate_apply(const SourceType& source, RangeType& range) const
  {
    // create search in the source grid part
    typedef typename SourceType::SpaceType::GridViewType SourceGridViewType;
    typedef Stuff::Grid::EntityInlevelSearch< SourceGridViewType > EntitySearch;
    EntitySearch entity_search(source.space().grid_view());
    // set all range dofs to infinity
    const auto infinity = std::numeric_limits< typename RangeType::RangeFieldType >::infinity();
    for (size_t ii = 0; ii < range.vector().size(); ++ii)
      range.vector().set_entry(ii, infinity);
    // walk the grid
    const auto entity_it_end = grid_view_.template end< 0 >();
    for (auto entity_it = grid_view_.template begin< 0 >();
         entity_it != entity_it_end;
         ++entity_it) {
      const auto& entity = *entity_it;
      // get global lagrange point coordinates
      const auto lagrange_point_set = range.space().lagrange_points(entity);
      typedef FieldVector< typename SourceGridViewType::ctype, SourceGridViewType::dimension > DomainType;
      std::vector< DomainType > lagrange_points(lagrange_point_set.size());
      for (size_t ii = 0; ii < lagrange_point_set.size(); ++ii)
        lagrange_points[ii] = entity.geometry().global(lagrange_point_set[ii]);
      // get source entities
      const auto source_entity_ptrs = entity_search(lagrange_points);
      assert(source_entity_ptrs.size() == lagrange_points.size());
      // get range
      auto local_range = range.local_discrete_function(entity);
      auto local_range_DoF_vector = local_range->vector();
      // do the actual work (see below)
      apply_local(source, lagrange_points, source_entity_ptrs, local_range_DoF_vector);
    } // walk the grid
  } // ... redirect_to_appropriate_apply(...)

  template< class SourceType, class LagrangePointsType, class EntityPointers, class LocalDoFVectorType >
  void apply_local(const SourceType& source,
                   const LagrangePointsType& lagrange_points,
                   const EntityPointers& source_entity_ptr_unique_ptrs,
                   LocalDoFVectorType& range_DoF_vector) const
  {
    static const size_t dimRange = SourceType::dimRange;
    size_t kk = 0;
    assert(source_entity_ptr_unique_ptrs.size() >= lagrange_points.size());
    for (size_t ii = 0; ii < lagrange_points.size(); ++ii) {
      if (std::isinf(range_DoF_vector.get(kk))) {
        const auto& global_point = lagrange_points[ii];
        // evaluate source function
        const auto& source_entity_ptr_unique_ptr = source_entity_ptr_unique_ptrs[ii];
        if (source_entity_ptr_unique_ptr) {
          const auto source_entity_ptr = *source_entity_ptr_unique_ptr;
          const auto& source_entity = *source_entity_ptr;
          const auto local_source_point = source_entity.geometry().local(global_point);
          const auto local_source = source.local_function(source_entity);
          const auto source_value = local_source->evaluate(local_source_point);
          for (size_t jj = 0; jj < dimRange; ++jj, ++kk)
            range_DoF_vector.set(kk, source_value[jj]);
        } else
          for (size_t jj = 0; jj < dimRange; ++jj, ++kk)
            range_DoF_vector.set(kk, 0.0);
      }
      else
        kk += dimRange;
    }
  } // ... apply_local(...)

  const GridViewType& grid_view_;
}; // class LagrangeProlongation

template< class GridViewType >
class Prolongation
{
public:
  typedef typename GridViewType::ctype DomainFieldType;
  static const size_t dimDomain = GridViewType::dimension;

  Prolongation(const GridViewType& grid_view)
    : l2_prolongation_operator_(grid_view)
    , lagrange_prolongation_operator_(grid_view)
  {}

  template< class SourceType, class RangeType >
  void apply(const SourceType& source, RangeType& range) const
  {
    redirect_to_appropriate_operator(source, range);
  }

private:
  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction< Spaces::CG::FemBased
                                                  < GPS, pS, RS, rS, rCS >, VS >& source,
                                               DiscreteFunction< Spaces::DG::FemBased
                                                  < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    l2_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::DG::FemBased
                                                    < GPS, pS, RS, rS, rCS >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::DG::FemBased
                                                    < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    l2_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::Block< Spaces::DG::FemBased
                                                    < GPS, pS, RS, rS, rCS > >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::Block< Spaces::DG::FemBased
                                                    < GPR, pR, RR, rR, rCR > >, VR >& range) const
  {
    l2_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::Block< Spaces::DG::FemBased
                                                    < GPS, pS, RS, rS, rCS > >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::DG::FemBased
                                                    < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    l2_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::CG::FemBased
                                                    < GPS, pS, RS, rS, rCS >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::CG::FemBased
                                                    < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    lagrange_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::DG::FemBased
                                                    < GPS, pS, RS, rS, rCS >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::CG::FemBased
                                                    < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    lagrange_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::CG::PdelabBased
                                                    < GPS, pS, RS, rS, rCS >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::CG::PdelabBased
                                                    < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    lagrange_prolongation_operator_.apply(source, range);
  }

  const L2Prolongation< GridViewType > l2_prolongation_operator_;
  const LagrangeProlongation< GridViewType > lagrange_prolongation_operator_;
}; // class Prolongation

template< class GridViewType, class SourceType, class RangeType >
void prolong(const GridViewType& grid_view, const SourceType& source, RangeType& range)
{
  const Prolongation< GridViewType > prolongation_operator(grid_view);
  prolongation_operator.apply(source, range);
}

template< class SourceType, class RangeType >
void prolong(const SourceType& source, RangeType& range)
{
  const Prolongation< typename RangeType::SpaceType::GridViewType > prolongation_operator(range.space().grid_view());
  prolongation_operator.apply(source, range);
}

} // namespace Operators
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_OPERATORS_PROLONGATIONS_HH