/**
 * relocgui: RelocaliserApplication.cpp
 * Copyright (c) Torr Vision Group, University of Oxford, 2017. All rights reserved.
 */

#include "RelocaliserApplication.h"

#include <iostream>
#include <stdexcept>

#include <boost/make_shared.hpp>

#include <Eigen/Geometry>

#include <grove/relocalisation/ScoreRelocaliserFactory.h>

#include <ITMLib/Engines/ViewBuilding/ITMViewBuilderFactory.h>
#include <ITMLib/Objects/Camera/ITMCalibIO.h>

#include <orx/base/MemoryBlockFactory.h>
#include <orx/persistence/PosePersister.h>

#include <tvgutil/filesystem/PathFinder.h>
#include <tvgutil/timing/AverageTimer.h>
#include <tvgutil/timing/TimeUtil.h>

namespace bf = boost::filesystem;

using namespace ITMLib;
using namespace ORUtils;

using namespace grove;
using namespace itmx;
using namespace orx;
using namespace tvgutil;

namespace relocgui {

//#################### ANONYMOUS FREE FUNCTIONS ####################

namespace {

/**
 * \brief Computes the angular separation between two rotation matrices.
 *
 * \param r1 The first rotation matrix.
 * \param r2 The second rotation matrix.
 *
 * \return The angle of the transformation mapping r1 to r2.
 */
float angular_separation(const Eigen::Matrix3f &r1, const Eigen::Matrix3f &r2)
{
  // First calculate the rotation matrix which maps r1 to r2.
  Eigen::Matrix3f dr = r2 * r1.transpose();

  Eigen::AngleAxisf aa(dr);
  return aa.angle();
}

/**
 * \brief Check whether a pose matrix is similar enough to a ground truth pose matrix.
 *
 * \param gtPose               The ground truth pose matrix.
 * \param testPose             The candidate pose matrix.
 * \param translationMaxError  The maximum difference (in m) between the translation components of the two matrices.
 * \param angleMaxError        The maximum angular difference between the two rotation components (when converted to the
 *                             axis-angle representation).
 *
 * \return Whether or not the two poses differ for less or equal than translationMaxError and angleMaxError.
 */
bool pose_matches(const Matrix4f &gtPose, const Matrix4f &testPose, float translationMaxError, float angleMaxError)
{
  // Both our Matrix type and Eigen's are column major, so we can just use Map here.
  const Eigen::Map<const Eigen::Matrix4f> gtPoseEigen(gtPose.m);
  const Eigen::Map<const Eigen::Matrix4f> testPoseEigen(testPose.m);

  const Eigen::Matrix3f gtR = gtPoseEigen.block<3, 3>(0, 0);
  const Eigen::Matrix3f testR = testPoseEigen.block<3, 3>(0, 0);
  const Eigen::Vector3f gtT = gtPoseEigen.block<3, 1>(0, 3);
  const Eigen::Vector3f testT = testPoseEigen.block<3, 1>(0, 3);

  const float translationError = (gtT - testT).norm();
  const float angleError = angular_separation(gtR, testR);

  return translationError <= translationMaxError && angleError <= angleMaxError;
}

/**
 * \brief Read a 4x4 pose matrix from a text file.
 *
 * \param fileName Path to the text file.
 *
 * \return The pose matrix.
 *
 * \throws std::runtime_error if the file is missing or has the wrong format.
 */
Matrix4f read_pose_from_file(const bf::path &fileName)
{
  if(!bf::is_regular(fileName)) throw std::runtime_error("File not found: " + fileName.string());

  std::ifstream in(fileName.string().c_str());

  Matrix4f res;
  in >> res(0, 0) >> res(1, 0) >> res(2, 0) >> res(3, 0);
  in >> res(0, 1) >> res(1, 1) >> res(2, 1) >> res(3, 1);
  in >> res(0, 2) >> res(1, 2) >> res(2, 2) >> res(3, 2);
  in >> res(0, 3) >> res(1, 3) >> res(2, 3) >> res(3, 3);

  if(!in) throw std::runtime_error("Error reading a pose matrix from: " + fileName.string());

  return res;
}
}

//#################### CONSTRUCTOR ####################

RelocaliserApplication::RelocaliserApplication(const std::string &calibrationPath,
                                               const std::string &trainingPath,
                                               const std::string &testingPath,
                                               const SettingsContainer_CPtr &settings)
  : m_calibrationFilePath(calibrationPath)
  , m_saveRelocalisedPoses(true)
  , m_settingsContainer(settings)
  , m_testSequencePath(testingPath)
  , m_trainSequencePath(trainingPath)
{
  // Read camera calibration parameters.
  if(!ITMLib::readRGBDCalib(m_calibrationFilePath.string().c_str(), m_cameraCalibration))
  {
    throw std::invalid_argument("Couldn't read calibration parameters.");
  }

  // Check that training and testing folders exist.
  if(!bf::is_directory(m_trainSequencePath))
  {
    throw std::invalid_argument("The specified training path does not exist.");
  }

  if(!bf::is_directory(m_testSequencePath))
  {
    throw std::invalid_argument("The specified testing path does not exist.");
  }

  // Setup the image masks.
  m_depthImageMask = m_settingsContainer->get_first_value<std::string>("depthImageMask", "frame-%06d.depth.png");
  m_poseFileMask = m_settingsContainer->get_first_value<std::string>("poseFileMask", "frame-%06d.pose.txt");
  m_rgbImageMask = m_settingsContainer->get_first_value<std::string>("rgbImageMask", "frame-%06d.color.png");

  // Create the folder that will store the relocalised poses.
  if(m_saveRelocalisedPoses)
  {
    const std::string experimentTag =
        m_settingsContainer->get_first_value<std::string>("experimentTag", TimeUtil::get_iso_timestamp());
    m_outputPosesPath = find_subdir_from_executable("reloc_poses") / experimentTag;

    std::cout << "Saving output poses in: " << m_outputPosesPath << '\n';
    if(!bf::is_directory(m_outputPosesPath))
    {
      bf::create_directories(m_outputPosesPath);
    }
  }

  // Set up the path generators.
  m_outputPosesPathGenerator = boost::make_shared<SequentialPathGenerator>(m_outputPosesPath);
  m_testingSequencePathGenerator = boost::make_shared<SequentialPathGenerator>(m_testSequencePath);
  m_trainingSequencePathGenerator = boost::make_shared<SequentialPathGenerator>(m_trainSequencePath);

  // We try to run everything on the GPU.
  const DeviceType deviceType = DEVICE_CUDA;

  // Setup the relocaliser.
  m_relocaliser = ScoreRelocaliserFactory::make_score_relocaliser("forest", "ScoreRelocaliser.", m_settingsContainer, deviceType);

  // Create a ViewBuilder to convert the depth image to float (might to it with OpenCV as well but this is easier).
  m_viewBuilder.reset(ITMViewBuilderFactory::MakeViewBuilder(m_cameraCalibration, deviceType));

  // Allocate the ITM Images used to train/test the relocaliser (empty for now, will be resized later).
  const MemoryBlockFactory& mbf = MemoryBlockFactory::instance();
  m_currentDepthImage = mbf.make_image<float>();
  m_currentRawDepthImage = mbf.make_image<short>();
  m_currentRgbImage = mbf.make_image<Vector4u>();
}

void RelocaliserApplication::run()
{
  boost::optional<RelocalisationExample> currentExample;

  std::cout << "Start training.\n";

  // First of all, train the relocaliser processing each image from the training folder.
  AverageTimer<boost::chrono::milliseconds> trainingTimer("Training Timer");
  while((currentExample = read_example(m_trainingSequencePathGenerator)))
  {
    trainingTimer.start_sync();

    prepare_example_images(*currentExample);

    // Now train the relocaliser.
    m_relocaliser->train(m_currentRgbImage.get(),
                         m_currentDepthImage.get(),
                         m_cameraCalibration.intrinsics_d.projectionParamsSimple.all,
                         currentExample->cameraPose);

    // Finally, increment the index.
    m_trainingSequencePathGenerator->increment_index();

    // Stop the timer before the visualization calls.
    trainingTimer.stop_sync();

    // Update UI
    show_example(*currentExample);
  }

  std::cout << "Training done, processed " << m_trainingSequencePathGenerator->get_index() << " RGB-D image pairs.\n";

  // Now test the relocaliser accumulating the number of successful relocalisations.
  uint32_t successfulExamples = 0;
  AverageTimer<boost::chrono::milliseconds> testingTimer("Testing Timer");
  while((currentExample = read_example(m_testingSequencePathGenerator)))
  {
    testingTimer.start_sync();
    prepare_example_images(*currentExample);

    // Now relocalise.
    std::vector<Relocaliser::Result> relocaliserResults =
        m_relocaliser->relocalise(m_currentRgbImage.get(),
                                  m_currentDepthImage.get(),
                                  m_cameraCalibration.intrinsics_d.projectionParamsSimple.all);

    // This will store the relocalised pose (if we were successful).
    Matrix4f inverseCameraPose;
    inverseCameraPose.setValues(std::numeric_limits<float>::quiet_NaN());

    if(!relocaliserResults.empty())
    {
      inverseCameraPose = relocaliserResults[0].pose.GetInvM();
    }

    // Save the pose if required.
    if(m_saveRelocalisedPoses)
    {
      PosePersister::save_pose_on_thread(inverseCameraPose,
                                         m_outputPosesPathGenerator->make_path("pose-%06i.reloc.txt"));
    }

    // Check whether the relocalisation succeeded or not by looking at the ground truth pose (using the 7-scenes
    // 5cm/5deg criterion).
    static const float translationMaxError = 0.05f;
    static const float angleMaxError = 5.f * static_cast<float>(M_PI) / 180.f;
    const bool relocalisationSucceeded =
        pose_matches(currentExample->cameraPose.GetInvM(), inverseCameraPose, translationMaxError, angleMaxError);

    // Update stats.
    successfulExamples += relocalisationSucceeded;

    // Increment path generator indices.
    m_testingSequencePathGenerator->increment_index();
    m_outputPosesPathGenerator->increment_index();

    // Stop the timer before the visualization calls.
    testingTimer.stop_sync();

    // Show the example and print whether the relocalisation succeeded or not.
    show_example(*currentExample, relocalisationSucceeded ? "Relocalisation OK" : "Relocalisation Failed");
  }

  const int testedExamples = m_testingSequencePathGenerator->get_index();
  std::cout << "Testing done.\n\nEvaluated " << testedExamples << " RGBD frames.\n";
  std::cout << successfulExamples
            << " frames were relocalised correctly (<=5cm translational and <=5deg angular error).\n";
  const float accuracy =
      100.f * (testedExamples > 0 ? static_cast<float>(successfulExamples) / static_cast<float>(testedExamples) : 0.0f);
  std::cout << "Overall accuracy: " << accuracy << "%\n";
  std::cout << trainingTimer << '\n';
  std::cout << testingTimer << '\n';
}

//#################### PRIVATE MEMBER FUNCTIONS ####################

boost::optional<RelocaliserApplication::RelocalisationExample>
    RelocaliserApplication::read_example(const RelocaliserApplication::SequentialPathGenerator_Ptr &pathGenerator) const
{
  bf::path currentDepthPath = pathGenerator->make_path(m_depthImageMask);
  bf::path currentRgbPath = pathGenerator->make_path(m_rgbImageMask);
  bf::path currentPosePath = pathGenerator->make_path(m_poseFileMask);

  if(bf::is_regular(currentDepthPath) && bf::is_regular(currentRgbPath) && bf::is_regular(currentPosePath))
  {
    RelocalisationExample example;

    // Read the images.
    example.depthImage = cv::imread(currentDepthPath.string().c_str(), cv::IMREAD_ANYDEPTH);
    example.rgbImage = cv::imread(currentRgbPath.string().c_str());

    // The files store the inverse camera pose.
    example.cameraPose.SetInvM(read_pose_from_file(currentPosePath));

    // Convert from BGR to RGBA.
    cv::cvtColor(example.rgbImage, example.rgbImage, CV_BGR2RGBA);

    return example;
  }

  return boost::none;
}

void RelocaliserApplication::prepare_example_images(const RelocaliserApplication::RelocalisationExample &currentExample)
{
  const Vector2i depthImageDims(currentExample.depthImage.cols, currentExample.depthImage.rows);
  const Vector2i rgbImageDims(currentExample.rgbImage.cols, currentExample.rgbImage.rows);

  // Copy the Mats into the ITM images (the depth image needs conversion to float according to the calibration).

  // Resize them (usually NOOP).
  m_currentRawDepthImage->ChangeDims(depthImageDims);
  m_currentDepthImage->ChangeDims(depthImageDims);
  m_currentRgbImage->ChangeDims(rgbImageDims);

  // Perform copy using a Mat wrapper.
  currentExample.depthImage.convertTo(
      cv::Mat(depthImageDims.y, depthImageDims.x, CV_16SC1, m_currentRawDepthImage->GetData(MEMORYDEVICE_CPU)), CV_16S);
  currentExample.rgbImage.copyTo(
      cv::Mat(rgbImageDims.y, rgbImageDims.x, CV_8UC4, m_currentRgbImage->GetData(MEMORYDEVICE_CPU)));

  // Update them on the device.
  m_currentRawDepthImage->UpdateDeviceFromHost();
  m_currentRgbImage->UpdateDeviceFromHost();

  // Use the viewBuilder to prepare the depth image.
  m_viewBuilder->ConvertDepthAffineToFloat(
      m_currentDepthImage.get(), m_currentRawDepthImage.get(), m_cameraCalibration.disparityCalib.GetParams());
}

void RelocaliserApplication::show_example(const RelocalisationExample &example, const std::string &uiText) const
{
  static const std::string WINDOW_NAME = "Relocalisation GUI";

  // Setup a named window.
  cv::namedWindow(WINDOW_NAME, CV_WINDOW_AUTOSIZE);

  // Allocate a canvas big enough to hold the colour and depth image side by side.
  cv::Mat canvas = cv::Mat::zeros(std::max(example.depthImage.rows, example.rgbImage.rows),
                                  example.depthImage.cols + example.rgbImage.cols,
                                  CV_8UC3);

  // Copy the colour image to its location on the canvas (converting it into BGR format since that's what OpenCV uses
  // for visualization).
  cv::cvtColor(example.rgbImage, canvas(cv::Rect(0, 0, example.rgbImage.cols, example.rgbImage.rows)), CV_RGBA2BGR);

  // Normalize the depth image (black is very close, white is far away) and copy it to its location on the canvas.
  cv::Mat processedDepth;
  cv::normalize(example.depthImage, processedDepth, 0, 255, cv::NORM_MINMAX, CV_8U);
  cv::cvtColor(processedDepth,
               canvas(cv::Rect(example.rgbImage.cols, 0, example.depthImage.cols, example.depthImage.rows)),
               CV_GRAY2BGR);

  // Draw the text in the top-left corner, if required.
  if(!uiText.empty())
  {
    const double fontSize = 1.5;
    const int thickness = 2;

    // Compute the text's bounding box (we actually only care about its height).
    int baseLine = 0;
    cv::Size textSize = cv::getTextSize(uiText, cv::FONT_HERSHEY_SIMPLEX, fontSize, thickness, &baseLine);

    // Write the text on the image applying a "Poor man's shadow effect".
    cv::putText(canvas,
                uiText,
                cv::Point(12, 12 + textSize.height),
                cv::FONT_HERSHEY_SIMPLEX,
                fontSize,
                cv::Scalar::all(0),
                thickness);
    cv::putText(canvas,
                uiText,
                cv::Point(10, 10 + textSize.height),
                cv::FONT_HERSHEY_SIMPLEX,
                fontSize,
                cv::Scalar::all(255),
                thickness);
  }

  // Actualy show the image.
  cv::imshow(WINDOW_NAME, canvas);
  cv::waitKey(1);
}
}


#include "random_tree_image.h"

#include <boost/format.hpp>
#include <map>
#include <math.h>
#include <set>
#include <tbb/mutex.h>
#include <tbb/parallel_for_each.h>
#include <thrust/gather.h>
#include <thrust/sort.h>

#include "ndarray_ops.h"
#include "random_tree_image_gpu.h"
#include "random_tree.h"
#include "score.h"

namespace curfil {

// for statistics
extern ImageCache imageCache;

class FeatureEvaluationCPU {

public:

    FeatureEvaluationCPU(size_t numClasses,
            size_t numFeatures,
            size_t numThresholds,
            const std::vector<const PixelInstance*>& samples,
            const ImageFeaturesAndThresholds<cuv::host_memory_space>& features,
            tbb::concurrent_vector<cuv::ndarray<WeightType, cuv::host_memory_space>>& perClassHistograms) :
            numClasses(numClasses),
                    numFeatures(numFeatures),
                    numThresholds(numThresholds),
                    samples(samples), features(features), perClassHistograms(perClassHistograms) {

        assert(!samples.empty());
    }

    // must be a const-method for TBB
    void operator()(const tbb::blocked_range<size_t>& range) const {

        cuv::ndarray<WeightType, cuv::host_memory_space> perClassHistogram(
                cuv::extents[numClasses][numFeatures][numThresholds][2]);

        for (size_t i = 0; i < perClassHistogram.size(); i++) {
            perClassHistogram[i] = 0;
        }

        std::vector<ImageFeatureFunction> featureFunctions(numFeatures);
        for (size_t featureNr = 0; featureNr < numFeatures; ++featureNr) {
            featureFunctions[featureNr] = features.getFeatureFunction(featureNr);
        }

        // transpose the thresholds
        std::vector<std::vector<float> > thresholds;
        for (size_t featureNr = 0; featureNr < numFeatures; ++featureNr) {
            std::vector<float> thresholdsPerFeature;
            for (size_t threshNr = 0; threshNr < numThresholds; ++threshNr) {
                thresholdsPerFeature.push_back(features.getThreshold(threshNr, featureNr));
            }
            thresholds.push_back(thresholdsPerFeature);
        }

        assert(perClassHistogram.ndim() == 4);
        const unsigned int labelStride = perClassHistogram.stride(0);
        const unsigned int featureStride = perClassHistogram.stride(1);
        const unsigned int thresholdsStride = perClassHistogram.stride(2);

        for (size_t s = range.begin(); s != range.end(); s++) {
            const PixelInstance* sample = samples[s];
            assert(sample);
            const LabelType label = sample->getLabel();
            const WeightType weight = sample->getWeight();

            const unsigned int labelOffset = label * labelStride;

            for (size_t featureNr = 0; featureNr < numFeatures; ++featureNr) {
                double value = featureFunctions[featureNr].calculateFeatureResponse(*sample);
                const std::vector<float>& thresholdsPerFeature = thresholds[featureNr];

                const unsigned int featureOffset = labelOffset + featureNr * featureStride;

                for (size_t threshNr = 0; threshNr < numThresholds; ++threshNr) {
                    const float threshold = thresholdsPerFeature[threshNr];
                    assert(!isnan(threshold));

                    // avoid the if () else () branch here by casting the compare into 0 or 1
                    // important: (!(x<=y)) is not the same as (x>y) because of NaNs!
                    int offset = static_cast<int>(!(value <= threshold));
                    assert(offset == ((value <= threshold) ? 0 : 1));

                    // offset stride must be 1
                    assert(perClassHistogram.stride(3) == 1);
                    unsigned int idx = featureOffset + threshNr * thresholdsStride + offset;
                    perClassHistogram.ptr()[idx] += weight;
                    assert(perClassHistogram(label, featureNr, threshNr, offset) == perClassHistogram.ptr()[idx]);
                }
            }
        }

        perClassHistograms.push_back(perClassHistogram);
    }

private:
    const size_t numClasses;
    const size_t numFeatures;
    const size_t numThresholds;
    const std::vector<const PixelInstance*>& samples;
    const ImageFeaturesAndThresholds<cuv::host_memory_space>& features;
    tbb::concurrent_vector<cuv::ndarray<WeightType, cuv::host_memory_space> >& perClassHistograms;
};

bool ImageFeatureFunction::operator==(const ImageFeatureFunction& other) const {
    if (featureType != other.featureType)
        return false;
    if (offset1 != other.offset1)
        return false;
    if (region1 != other.region1)
        return false;
    if (channel1 != other.channel1)
        return false;
    if (offset2 != other.offset2)
        return false;
    if (region2 != other.region2)
        return false;
    if (channel2 != other.channel2)
        return false;

    return true;

}

template<>
cuv::ndarray<ScoreType, cuv::host_memory_space> ImageFeatureEvaluation::calculateScores(
        const cuv::ndarray<WeightType, cuv::host_memory_space>& counters,
        const ImageFeaturesAndThresholds<cuv::host_memory_space>& featuresAndThresholds,
        const cuv::ndarray<WeightType, cuv::host_memory_space>& histogram) {

    const unsigned int numFeatures = configuration.getFeatureCount();
    const unsigned int numThresholds = configuration.getThresholds();

    cuv::ndarray<ScoreType, cuv::host_memory_space> scores(numThresholds, numFeatures, scoresAllocator);

    utils::Profile profile("calculateScores");

    const size_t numLabels = histogram.size();
    assert(numLabels > 0);

    for (size_t featureNr = 0; featureNr < numFeatures; featureNr++) {

        for (size_t threshNr = 0; threshNr < numThresholds; ++threshNr) {

            assert(!isnan(featuresAndThresholds.getThreshold(threshNr, featureNr)));

            ScoreType summedLeft = 0;
            ScoreType summedRight = 0;

            WeightType perClassLeftCounter[numLabels];
            WeightType perClassRightCounter[numLabels];
            const unsigned int leftRightStride = 1; // consecutive in memory

            for (size_t classNr = 0; classNr < numLabels; classNr++) {

                const WeightType& left = counters(classNr, featureNr, threshNr, 0);
                const WeightType& right = counters(classNr, featureNr, threshNr, 1);

                perClassLeftCounter[classNr] = left;
                perClassRightCounter[classNr] = right;

                summedLeft += left;
                summedRight += right;
            }

            ScoreType score = NormalizedInformationGainScore::calculateScore(numLabels, perClassLeftCounter,
                    perClassRightCounter, leftRightStride, histogram.ptr(), summedLeft, summedRight);

            scores(threshNr, featureNr) = score;
        }
    }

    return scores;
}

std::vector<SplitFunction<PixelInstance, ImageFeatureFunction> > ImageFeatureEvaluation::evaluateBestSplits(
        RandomSource& randomSource,
        const std::vector<std::pair<boost::shared_ptr<RandomTree<PixelInstance, ImageFeatureFunction> >,
                std::vector<const PixelInstance*> > >& samplesPerNode) {

    std::vector<SplitFunction<PixelInstance, ImageFeatureFunction> > bestSplits(samplesPerNode.size());

    ImageFeaturesAndThresholds<cuv::host_memory_space> featuresAndThresholdsCPU(configuration.getFeatureCount(),
            configuration.getThresholds(), featuresAllocator);

    ImageFeaturesAndThresholds<cuv::dev_memory_space> featuresAndThresholdsGPU(configuration.getFeatureCount(),
            configuration.getThresholds(), featuresAllocator);

    size_t totalTransferTimeMicrosecondsStart = imageCache.getTotalTransferTimeMircoseconds();

    const AccelerationMode accelerationMode = configuration.getAccelerationMode();

    utils::Timer generatingRandomFeaturesTimer;

    {
        std::set<const RGBDImage*> images;
        std::vector<const PixelInstance*> allSamples;
        unsigned int numSkipped = 0;
        for (size_t nodeId = 0; nodeId < samplesPerNode.size(); nodeId++) {

            const std::vector<const PixelInstance*>& samples = samplesPerNode[nodeId].second;
            for (size_t s = 0; s < samples.size(); s++) {
                const PixelInstance* sample = samples[s];
                if (accelerationMode == GPU_ONLY || accelerationMode == GPU_AND_CPU_COMPARE) {
                    if (images.size() == static_cast<size_t>(configuration.getImageCacheSize())) {
                        if (images.find(sample->getRGBDImage()) == images.end()) {
                            // skip sample. image does not fit in cache
                            numSkipped++;
                            continue;
                        }
                    }
                }
                images.insert(sample->getRGBDImage());
                allSamples.push_back(sample);
            }

        }

        if (numSkipped > 0) {
            CURFIL_INFO("randomFeatureGeneration: skipped " << numSkipped << " samples");
        }

        const int seed = randomSource.uniformSampler(0xFFFFFF).getNext();

        if (accelerationMode == CPU_ONLY || accelerationMode == GPU_AND_CPU_COMPARE) {
            featuresAndThresholdsCPU = generateRandomFeatures(allSamples, seed, true, cuv::host_memory_space());
        }
        if (accelerationMode == GPU_ONLY || accelerationMode == GPU_AND_CPU_COMPARE) {
            featuresAndThresholdsGPU = generateRandomFeatures(allSamples, seed, true, cuv::dev_memory_space());
        }
    }

    CURFIL_INFO("generating random features: " << generatingRandomFeaturesTimer.format(2));

    tbb::mutex cpuEvaluationMutex;

    size_t grainSize = 1;
    if (accelerationMode != CPU_ONLY) {
        // GPU: use two threads
        grainSize = ceil(samplesPerNode.size() / 2.0);
    }
    tbb::parallel_for(tbb::blocked_range<size_t>(0, samplesPerNode.size(), grainSize),
            [&](const tbb::blocked_range<size_t>& range) {
                for(size_t nodeNr = range.begin(); nodeNr != range.end(); nodeNr++) {

                    const std::pair<boost::shared_ptr<RandomTree<PixelInstance, ImageFeatureFunction> >,
                    std::vector<const PixelInstance*> >& nodeSamples = samplesPerNode[nodeNr];

                    utils::Timer timerEvaluateBestSplit;

                    RandomTree<PixelInstance, ImageFeatureFunction>& currentNode = *(nodeSamples.first);
                    const std::vector<const PixelInstance*>& samples = nodeSamples.second;

                    const size_t numLabels = currentNode.getNumClasses();
                    assert(numLabels >= 2 && numLabels < 256);
                    assert(!samples.empty());

                    SplitFunction<PixelInstance, ImageFeatureFunction> bestFeatureCPU;
                    SplitFunction<PixelInstance, ImageFeatureFunction> bestFeatureGPU;

                    cuv::ndarray<ScoreType, cuv::host_memory_space> scoresCPU;
                    cuv::ndarray<ScoreType, cuv::host_memory_space> scoresGPU;

                    size_t transferTimeStart = imageCache.getTotalTransferTimeMircoseconds();

                    if (accelerationMode == CPU_ONLY || accelerationMode == GPU_AND_CPU_COMPARE) {

                        CURFIL_DEBUG("prepare batches");

                        std::vector<std::vector<const PixelInstance*> > batches = prepare(samples, currentNode,
                                cuv::host_memory_space());

                        tbb::concurrent_vector<cuv::ndarray<WeightType, cuv::host_memory_space> > perClassHistograms;
                        utils::Timer timeEvaluate;

                        int numThreads = configuration.getNumThreads();

                        assert(numThreads > 0);

                        size_t grainSize = std::max(100ul, samples.size() / numThreads);

                        CURFIL_DEBUG("start evaluation");

                        cuv::ndarray<WeightType, cuv::host_memory_space> countersCPU(
                                cuv::extents[numLabels][configuration.getFeatureCount()][configuration.getThresholds()][2]);
                        for (size_t i = 0; i < countersCPU.size(); i++) {
                            countersCPU[i] = 0;
                        }

                        {
                            // tbb::mutex::scoped_lock cpuEvaluationLock(cpuEvaluationMutex);

                            utils::Profile profile("feature evaluation CPU");

                            FeatureEvaluationCPU evaluation(numLabels, configuration.getFeatureCount(),
                                    configuration.getThresholds(),
                                    samples, featuresAndThresholdsCPU, perClassHistograms);

                            tbb::parallel_for(tbb::blocked_range<size_t>(0, samples.size(), grainSize), evaluation);
                            currentNode.setTimerValue("featureEvaluation", profile.getSeconds());

                            assert(!perClassHistograms.empty());

                            utils::Profile reaggregateHistograms("reaggregateHistograms");
                            for (size_t i = 0; i < perClassHistograms.size(); i++) {
                                countersCPU += perClassHistograms[i];
                            }
                            currentNode.setTimerValue("reaggregateHistograms", reaggregateHistograms.getSeconds());
                        }

                        CURFIL_DEBUG("calculate scores");

                        {
                            utils::Profile profile("calculateScores");
                            scoresCPU = calculateScores(countersCPU, featuresAndThresholdsCPU, currentNode.getHistogram());
                            currentNode.setTimerValue("calculateScores", profile.getSeconds());
                        }

                        currentNode.setTimerValue("evaluationTime", timeEvaluate);
                    }

                    if (accelerationMode == GPU_ONLY || accelerationMode == GPU_AND_CPU_COMPARE) {

                        std::vector<std::vector<const PixelInstance*> > batches = prepare(samples, currentNode,
                                cuv::dev_memory_space());

                        if (accelerationMode == GPU_AND_CPU_COMPARE) {
                            featuresAndThresholdsGPU = featuresAndThresholdsCPU;
                        }

                        utils::Timer featureResponsesAndHistograms;

                        cuv::ndarray<WeightType, cuv::dev_memory_space> counters = calculateFeatureResponsesAndHistograms(
                                currentNode, batches, featuresAndThresholdsGPU);

                        currentNode.setTimerValue("featureResponsesAndHistograms", featureResponsesAndHistograms);

                        utils::Timer calculateScoresTimer;

                        cuv::ndarray<WeightType, cuv::dev_memory_space> histogram = currentNode.getHistogram();
                        scoresGPU = calculateScores(counters, featuresAndThresholdsGPU, histogram);

                        currentNode.setTimerValue("calculateScores", calculateScoresTimer);
                    }

                    size_t transferTimeEnd = imageCache.getTotalTransferTimeMircoseconds();

                    currentNode.setTimerValue("transferImages", (transferTimeEnd - transferTimeStart) / static_cast<double>(1e6));

                    currentNode.setTimerValue("evaluateBestSplit", timerEvaluateBestSplit);

                    if (accelerationMode == GPU_AND_CPU_COMPARE) {
                        if (scoresCPU.shape() != scoresGPU.shape()) {
                            throw std::runtime_error("different shapes");
                        }
                        for (size_t i = 0; i < scoresCPU.size(); i++) {
                            ScoreType diff = abs(static_cast<ScoreType>(scoresCPU[i] - scoresGPU[i]));
                            if (diff > 1e-10) {
                                const std::string message = boost::str(
                                        boost::format("different scores [%d]: %.10f vs. %.10f, diff: %.10f")
                                        % i
                                        % scoresCPU[i]
                                        % scoresGPU[i]
                                        % diff);
                                throw std::runtime_error(message);
                            }
                        }
                    }

                    cuv::ndarray<ScoreType, cuv::host_memory_space> scores;
                    if (accelerationMode == CPU_ONLY || accelerationMode == GPU_AND_CPU_COMPARE) {
                        scores = scoresCPU;
                    } else {
                        scores = scoresGPU;
                    }

                    assert(scores.ndim() == 2);
                    assert(scores.shape(0) == configuration.getThresholds());
                    assert(scores.shape(1) == configuration.getFeatureCount());
                    ScoreType bestScore = -std::numeric_limits<ScoreType>::infinity();
                    uint16_t bestThresh = 0;
                    unsigned int bestFeat = 0;
                    for (uint16_t thresh = 0; thresh < configuration.getThresholds(); thresh++) {
                        for (unsigned int feat = 0; feat < configuration.getFeatureCount(); feat++) {
                            const ScoreType score = scores(thresh, feat);
                            if (isnan(bestScore) || detail::isScoreBetter(bestScore, score, feat)) {
                                bestFeat = feat;
                                bestThresh = thresh;
                                bestScore = score;
                            }
                        }
                    }

                    assert(bestScore > 0.0);

                    ImageFeatureFunction feature;
                    float threshold;
                    if (accelerationMode == CPU_ONLY) {
                        feature = featuresAndThresholdsCPU.getFeatureFunction(bestFeat);
                        threshold = featuresAndThresholdsCPU.getThreshold(bestThresh, bestFeat);
                    } else {
                        feature = featuresAndThresholdsGPU.getFeatureFunction(bestFeat);
                        threshold = featuresAndThresholdsGPU.getThreshold(bestThresh, bestFeat);
                    }

                    SplitFunction<PixelInstance, ImageFeatureFunction> bestFeature(bestFeat, feature, threshold, bestScore);

                    CURFIL_DEBUG("tree " << currentNode.getTreeId() << ", node " << currentNode.getNodeId() <<
                            ", best score: " << bestScore << ", " << feature);

                    bestSplits[nodeNr]= bestFeature;
                }
            });

    size_t totalTransferTimeMicrosecondsEnd = imageCache.getTotalTransferTimeMircoseconds();
    assert(totalTransferTimeMicrosecondsEnd >= totalTransferTimeMicrosecondsStart);
    double transferTime = (totalTransferTimeMicrosecondsEnd - totalTransferTimeMicrosecondsStart)
            / static_cast<double>(1e6);

    if (transferTime > 0) {
        CURFIL_INFO((boost::format("image cache transfer time: %.3f s") % transferTime).str());
    }

    return bestSplits;
}

template<>
void ImageFeatureEvaluation::sortFeatures(
        ImageFeaturesAndThresholds<cuv::host_memory_space>& featuresAndThresholds,
        const cuv::ndarray<int, cuv::host_memory_space>& keysIndices) const {

    const size_t numFeatures = configuration.getFeatureCount();
    assert(featuresAndThresholds.m_features.shape(1) == numFeatures);

    ImageFeaturesAndThresholds<cuv::host_memory_space> tmpFeaturesAndThresholds = featuresAndThresholds.copy();

    int* keys = keysIndices[cuv::indices[0][cuv::index_range()]].ptr();
    int* indices = keysIndices[cuv::indices[1][cuv::index_range()]].ptr();

    thrust::sort_by_key(keys, keys + numFeatures, indices);

    cuv::ndarray<int8_t, cuv::host_memory_space> features = tmpFeaturesAndThresholds.features();
    cuv::ndarray<int8_t, cuv::host_memory_space> sortedFeatures = featuresAndThresholds.features();

    assert(features.shape() == sortedFeatures.shape());

    const size_t dim = features.shape(0);
    assert(dim == 11);
    for (size_t d = 0; d < dim; d++) {
        int8_t* ptr = features[cuv::indices[d][cuv::index_range()]].ptr();
        int8_t* sortedPtr = sortedFeatures[cuv::indices[d][cuv::index_range()]].ptr();

        assert(ptr != sortedPtr);
        thrust::gather(indices, indices + numFeatures, ptr, sortedPtr);
    }

#ifndef NDEBUG
    for (size_t feat = 0; feat < numFeatures; feat++) {
        const ImageFeatureFunction feature1 = tmpFeaturesAndThresholds.getFeatureFunction(indices[feat]);
        const ImageFeatureFunction feature2 = featuresAndThresholds.getFeatureFunction(feat);
        assert(feature1 == feature2);
    }
#endif

#ifndef NDEBUG
    for (size_t feat = 1; feat < numFeatures; feat++) {
        const ImageFeatureFunction feature1 = featuresAndThresholds.getFeatureFunction(feat - 1);
        const ImageFeatureFunction feature2 = featuresAndThresholds.getFeatureFunction(feat);

        const int key1 = feature1.getSortKey();
        const int key2 = feature2.getSortKey();
        assert(key1 <= key2);
    }
#endif

    assert(featuresAndThresholds.thresholds().ndim() == 2);
    assert(featuresAndThresholds.thresholds().shape(0) == configuration.getThresholds());
    assert(featuresAndThresholds.thresholds().shape(1) == numFeatures);

    for (size_t thresh = 0; thresh < configuration.getThresholds(); thresh++) {
        float* thresholdsPtr =
                tmpFeaturesAndThresholds.m_thresholds[cuv::indices[thresh][cuv::index_range()]].ptr();
        float* sortedThresholdsPtr =
                featuresAndThresholds.m_thresholds[cuv::indices[thresh][cuv::index_range()]].ptr();

        thrust::gather(indices, indices + numFeatures, thresholdsPtr, sortedThresholdsPtr);
    }

}

std::vector<std::vector<const PixelInstance*> > ImageFeatureEvaluation::prepare(
        const std::vector<const PixelInstance*>& samples,
        RandomTree<PixelInstance, ImageFeatureFunction>& node,
        cuv::host_memory_space) {
    std::vector<std::vector<const PixelInstance*> > batches;
    batches.push_back(samples);
    return batches;
}

ImageFeaturesAndThresholds<cuv::host_memory_space> ImageFeatureEvaluation::generateRandomFeatures(
        const std::vector<const PixelInstance*>& samples, int seed, const bool sort, cuv::host_memory_space) {

    utils::Profile profile("generateRandomFeatures host");

    unsigned int numFeatures = configuration.getFeatureCount();
    unsigned int numThresholds = configuration.getThresholds();

    ImageFeaturesAndThresholds<cuv::host_memory_space> featuresAndThresholds(numFeatures, numThresholds,
            featuresAllocator);

    assert(!samples.empty());

    RandomSource randomSource(seed);

    Sampler sampleGen = randomSource.uniformSampler(samples.size());

    size_t generatedFeatures = 0;

    cuv::ndarray<int, cuv::host_memory_space> keysIndices(2, numFeatures, keysIndicesAllocator);

    int maxLoops = 3 * numFeatures;

    while (generatedFeatures < numFeatures) {
        const ImageFeatureFunction feature = sampleFeature(randomSource, samples);

        if (--maxLoops <= 0) {
            throw std::runtime_error("failed to generate random features. max loops exceeded");
        }

        if (!feature.isValid()) {
            continue;
        }

        bool isValid = true;
        for (size_t thresh = 0; thresh < configuration.getThresholds(); thresh++) {
            size_t maxTries = 10;
            float threshold;
            do {
                const PixelInstance* sample = samples.at(sampleGen.getNext());
                assert(sample);
                threshold = feature.calculateFeatureResponse(*sample);
            } while (isnan(threshold) && --maxTries > 0);

            if (isnan(threshold)) {
                isValid = false;
                break;
            }

            featuresAndThresholds.thresholds()(thresh, generatedFeatures) = threshold;
            assert(featuresAndThresholds.getThreshold(thresh, generatedFeatures) == threshold);
        }

        if (!isValid) {
            continue;
        }

        featuresAndThresholds.setFeatureFunction(generatedFeatures, feature);

        keysIndices(0, generatedFeatures) = feature.getSortKey();
        keysIndices(1, generatedFeatures) = generatedFeatures;

        generatedFeatures++;
    }

    if (sort)
        sortFeatures(featuresAndThresholds, keysIndices);

    return featuresAndThresholds;
}

const ImageFeatureFunction ImageFeatureEvaluation::sampleFeature(RandomSource& randomSource,
        const std::vector<const PixelInstance*>&) const {
    // Random feature type
    Sampler rgen_ft = randomSource.uniformSampler(2);

    uint16_t boxRadius = configuration.getBoxRadius();
    uint16_t regionSize = configuration.getRegionSize();

    // (x,y) offsets
    Sampler rgen_xy = randomSource.uniformSampler(-boxRadius, boxRadius);

    // (w,h) regions
    Sampler rgen_region = randomSource.uniformSampler(1, regionSize);

    // Color channel [0-2]
    Sampler rgen_cc = randomSource.uniformSampler(3);

    // Generate a feature that is non-constant
    Offset offset1(rgen_xy.getNext(), rgen_xy.getNext());

    // Second, disjoint pixel
    Offset offset2;
    do {
        offset2 = Offset(rgen_xy.getNext(), rgen_xy.getNext());
    } while (offset1 == offset2);

    Region region1(rgen_region.getNext(), rgen_region.getNext());
    Region region2(rgen_region.getNext(), rgen_region.getNext());

    assert(region1.getX() >= 0);
    assert(region1.getY() >= 0);
    assert(region2.getX() >= 0);
    assert(region2.getY() >= 0);

    switch (rgen_ft.getNext()) {
        case 0:
            return ImageFeatureFunction(DEPTH,
                    offset1,
                    region1,
                    0,
                    offset2,
                    region2,
                    0);
        case 1:
            return ImageFeatureFunction(COLOR,
                    offset1,
                    region1,
                    rgen_cc.getNext(),
                    offset2,
                    region2,
                    rgen_cc.getNext());
        default:
            assert(false);
            break;
    }
    throw std::runtime_error("");
}

RandomTreeImage::RandomTreeImage(int id, const TrainingConfiguration& configuration) :
        finishedTraining(false), id(id), configuration(configuration),
                tree(), classLabelPriorDistribution() {
}

RandomTreeImage::RandomTreeImage(boost::shared_ptr<RandomTree<PixelInstance, ImageFeatureFunction> > tree,
        const TrainingConfiguration& configuration,
        const cuv::ndarray<WeightType, cuv::host_memory_space>& classLabelPriorDistribution) :
        finishedTraining(true), id(tree->getNodeId()), configuration(configuration),
                tree(tree), classLabelPriorDistribution(classLabelPriorDistribution) {
}

void RandomTreeImage::doTrain(RandomSource& randomSource, size_t numClasses,
        std::vector<const PixelInstance*>& subsamples) {

    RandomTreeTrain<PixelInstance, ImageFeatureEvaluation, ImageFeatureFunction> treeTrain(getId(), numClasses,
            configuration);

    tree = boost::make_shared<RandomTree<PixelInstance, ImageFeatureFunction> >(getId(), 1, subsamples, numClasses); // no parent
    assert(tree->isRoot());

    std::vector<
            std::pair<boost::shared_ptr<RandomTree<PixelInstance, ImageFeatureFunction> >,
                    std::vector<const PixelInstance*> > > samplesPerNode;
    samplesPerNode.push_back(std::make_pair(tree, subsamples));

    ImageFeatureEvaluation featureEvaluation(tree->getTreeId(), configuration);
    treeTrain.train(featureEvaluation, randomSource, samplesPerNode, getId());
}

void RandomTreeImage::normalizeHistograms(const double histogramBias) {
    tree->normalizeHistograms(classLabelPriorDistribution, histogramBias);
}

bool RandomTreeImage::shouldIgnoreLabel(const LabelType& label) const {
    const RGBColor color = LabelImage::decodeLabel(label);
    for (const std::string colorString : configuration.getIgnoredColors()) {
        if (color == RGBColor(colorString)) {
            return true;
        }
    }
    return false;
}

void RandomTreeImage::train(const std::vector<LabeledRGBDImage>& trainLabelImages,
        RandomSource& randomSource, size_t subsampleCount) {

    assert(subsampleCount > 0);
    assert(finishedTraining == false);
    assert(tree == NULL);
    assert(!trainLabelImages.empty());

    calculateLabelPriorDistribution(trainLabelImages);

    // Subsample training set
    std::vector<PixelInstance> subsamples;

    if (configuration.getSubsamplingType() == "pixelUniform") {
        subsamples = subsampleTrainingDataPixelUniform(trainLabelImages, randomSource, subsampleCount);
    } else if (configuration.getSubsamplingType() == "classUniform") {
        subsamples = subsampleTrainingDataClassUniform(trainLabelImages, randomSource, subsampleCount);
    } else {
        throw std::runtime_error(
                boost::str(boost::format("unknown subsamplingType: %d") % configuration.getSubsamplingType()));
    }

    CURFIL_INFO("sorting " << subsamples.size() << " samples");

    utils::Timer sortTimer;

    // interesting fact: sorting the concrete instances is faster than sorting with pointers!
    // sort by imageid, class id (to improve CPU caching)

    const int SORT_BLOCK_SIZE = 200000;
    auto iterator = subsamples.begin();
    auto end = std::min(subsamples.end(), iterator + SORT_BLOCK_SIZE);
    while (iterator < end) {
        std::sort(iterator, end, [](const PixelInstance& a, const PixelInstance& b) {
            if (a.getRGBDImage() != b.getRGBDImage()) {
                return(a.getRGBDImage() < b.getRGBDImage());
            }

            if (a.getLabel() != b.getLabel()) {
                return (a.getLabel() < b.getLabel());
            }

            // FIXME optimize magic value
            const int quantisation = 10;
            if (a.getY()/quantisation != b.getY()/quantisation) {
                return (a.getY()/quantisation < b.getY()/quantisation);
            }
            if (a.getX()/quantisation != b.getX()/quantisation) {
                return (a.getX()/quantisation < b.getX()/quantisation);
            }
            return false;
        });
        iterator += SORT_BLOCK_SIZE;
        end += SORT_BLOCK_SIZE;
        end = std::min(subsamples.end(), end);
    }

    sortTimer.stop();

    CURFIL_INFO("sorted in " << sortTimer.format(2));

    std::vector<const PixelInstance*> subsamplePointers;
    subsamplePointers.reserve(subsamples.size());
    for (const PixelInstance& sample : subsamples) {
        subsamplePointers.push_back(&sample);
    }

    utils::Timer trainTimer;

    assert(tree == NULL);

    const size_t numClasses = classLabelPriorDistribution.size();

    doTrain(randomSource, numClasses, subsamplePointers);
    assert(tree != NULL);
    finishedTraining = true;

    CURFIL_INFO("trained tree " << getId() << " in " << trainTimer.format(2));
}

void RandomTreeImage::test(const RGBDImage* image, LabelImage& prediction) const {
    assert(finishedTraining);
    assert(image->getWidth() == prediction.getWidth());
    assert(image->getHeight() == prediction.getHeight());

    // Classify one pixel at a time (this may be inefficient)
    for (int y = 0; y < image->getHeight(); ++y) {
        for (int x = 0; x < image->getWidth(); ++x) {
            PixelInstance pixel(image, 0, x, y);
            prediction.setLabel(x, y, tree->classify(pixel));
        }
    }
}

std::vector<PixelInstance> RandomTreeImage::subsampleTrainingDataPixelUniform(
        const std::vector<LabeledRGBDImage>& trainLabelImages,
        RandomSource& randomSource,
        size_t subsampleCount) const {
    // Random across imags type [0..(n-1)]
    auto rgen_image = randomSource.uniformSampler(trainLabelImages.size());

    std::vector<PixelInstance> subsamples;
    for (size_t n = 0; n < subsampleCount * trainLabelImages.size(); ++n) {
        unsigned int image_id = rgen_image.getNext();
        assert(image_id < trainLabelImages.size());

        // Sample a random (x,y) coordinate for the image
        const auto image = trainLabelImages[image_id].getLabelImage();

        auto rgen_x = randomSource.uniformSampler(image.getWidth());
        auto rgen_y = randomSource.uniformSampler(image.getHeight());

        do {
            uint16_t x = static_cast<uint16_t>(rgen_x.getNext());
            uint16_t y = static_cast<uint16_t>(rgen_y.getNext());
            assert(x < image.getWidth());
            assert(y < image.getHeight());

            LabelType label = image.getLabel(x, y);

            if (shouldIgnoreLabel(label)) {
                continue;
            }

            // Append to sample list
            subsamples.push_back(PixelInstance(&(trainLabelImages[image_id].getRGBDImage()), label, x, y));
            break;
        } while (true);
    }

    return subsamples;
}

void RandomTreeImage::calculateLabelPriorDistribution(const std::vector<LabeledRGBDImage>& trainLabelImages) {

    std::map<LabelType, size_t> priorDistribution;
    for (const auto& trainLabelImage : trainLabelImages) {

        const auto& img = trainLabelImage.getLabelImage();

        for (int y = 0; y < img.getHeight(); ++y) {
            for (int x = 0; x < img.getWidth(); ++x) {
                LabelType label = img.getLabel(x, y);
                assert(label >= 0);
                priorDistribution[label]++;
            }
        }
    }

    classLabelPriorDistribution.resize(priorDistribution.size());
    for (LabelType label = 0; label < priorDistribution.size(); label++) {
        classLabelPriorDistribution[label] = priorDistribution[label];
    }

}

std::vector<PixelInstance> RandomTreeImage::subsampleTrainingDataClassUniform(
        const std::vector<LabeledRGBDImage>& trainLabelImages,
        RandomSource& randomSource,
        size_t subsampleCount) const {

    if (trainLabelImages.empty()) {
        throw std::runtime_error("got no label images");
    }

    utils::Timer samplingTimer;

    size_t numLabels = classLabelPriorDistribution.size();

    std::map<RGBColor, LabelType> colorLabelMap;
    for (LabelType label = 0; label < numLabels; label++) {
        colorLabelMap[LabelImage::decodeLabel(label)] = label;
    }

    for (const auto& color : configuration.getIgnoredColors()) {
        if (colorLabelMap.find(color) == colorLabelMap.end()) {
            throw std::runtime_error((boost::format("color to ignore was not found: RGB(%s) (numLabels: %d)")
                    % color % classLabelPriorDistribution.size()).str());
        }
        assert(numLabels >= 1);
        numLabels--;
    }

    // Number of samples per class, rounded up
    const size_t samplesPerClass = static_cast<size_t>(
            ceil(trainLabelImages.size() * static_cast<double>(subsampleCount) /
                    static_cast<double>(numLabels)));

    std::vector<PixelInstance> allSubsamples;

    CURFIL_INFO("sampling " << numLabels << " classes. " << samplesPerClass << " samples per class with "
            << configuration.getNumThreads() << " threads from " << trainLabelImages.size() << " images");

    tbb::concurrent_vector<std::map<LabelType, ReservoirSampler<PixelInstance> > > samplersPerLabel;

    int grainSize = ceil(trainLabelImages.size() / static_cast<double>(configuration.getNumThreads()));

    const int randomSeed = randomSource.uniformSampler(0xFFFFFF).getNext();

    std::set<LabelType> labelsToIgnore;
    for (const std::string colorString : configuration.getIgnoredColors()) {
        LabelType label = std::numeric_limits<LabelType>::max();
        label = getOrAddColorId(RGBColor(colorString), label);
        if (label == std::numeric_limits<LabelType>::max()) {
            throw std::runtime_error(std::string("unknown color to ignore: ") + colorString);
        }
        labelsToIgnore.insert(label);
    }

    // Parallel Reservoir Sampling
    tbb::parallel_for(tbb::blocked_range<size_t>(0, trainLabelImages.size(), grainSize),
            [&](const tbb::blocked_range<size_t>& range) {

#ifndef NDEBUG
            size_t numImages = range.end() - range.begin();
            assert(numImages > 0);
            assert(numImages <= trainLabelImages.size());
#endif

            std::map<LabelType, ReservoirSampler<PixelInstance> > reservoirSamplers;

            for (LabelType label = 0; label < classLabelPriorDistribution.size(); label++) {
                reservoirSamplers[label] = ReservoirSampler<PixelInstance>(samplesPerClass);
            }

            const int randMax = 0x7FFFFFFF;
            RandomSource randomSource(randomSeed);
            Sampler sampler = randomSource.uniformSampler(randMax);

            for(unsigned int imageNr = range.begin(); imageNr != range.end(); imageNr++) {

                const auto image = trainLabelImages[imageNr].getLabelImage();
                for(int y=0; y < image.getHeight(); y++) {
                    for(int x=0; x < image.getWidth(); x++) {
                        const LabelType label = image.getLabel(x, y);

                        if (labelsToIgnore.find(label) != labelsToIgnore.end()) {
                            continue;
                        }

                        PixelInstance sample(&(trainLabelImages[imageNr].getRGBDImage()), label, x, y);
                        if (!sample.getDepth().isValid()) {
                            continue;
                        }

                        reservoirSamplers[label].sample(sampler, sample);
                    }
                }

            }

            samplersPerLabel.push_back(reservoirSamplers);

        });

    const int randMax = 0x7FFFFFFF;
    Sampler sampler = randomSource.uniformSampler(randMax);

    for (LabelType label = 0; label < classLabelPriorDistribution.size(); label++) {

        ReservoirSampler<PixelInstance> reservoirSampler(samplesPerClass);

        for (auto& samplers : samplersPerLabel) {
            const auto& samples = samplers[label].getReservoir();

            if (labelsToIgnore.find(label) != labelsToIgnore.end()) {
                if (!samples.empty()) {
                    throw std::runtime_error("sampled pixels for ignored class. must not happen");
                }
                continue;
            }

            for (const PixelInstance& sample : samples) {
                reservoirSampler.sample(sampler, sample);
            }
        }

        const std::vector<PixelInstance>& reservoir = reservoirSampler.getReservoir();

        auto color = LabelImage::decodeLabel(label);
        CURFIL_INFO((boost::format("sampled %d pixels of class '%d' RGB(%s)")
                % reservoir.size()
                % static_cast<int>(label)
                % color.toString()).str());

        allSubsamples.insert(allSubsamples.begin(), reservoir.begin(), reservoir.end());
    }

    samplingTimer.stop();

    CURFIL_INFO("sampled " << allSubsamples.size() << " pixels for "
            << numLabels << " classes (" << samplesPerClass << " samples/class)"
            << " in " << samplingTimer.format(4));

    if (allSubsamples.size() != samplesPerClass * numLabels) {
        throw std::runtime_error("failed to sample enough pixels");
    }
    return allSubsamples;
}

}

std::ostream& operator<<(std::ostream& os,
        const curfil::RandomTreeImage& tree) {
    const auto rtree = tree.getTree();
    os << rtree->countNodes() << " nodes, "
            << rtree->countLeafNodes() << " leaves, "
            << rtree->getNumClasses() << " classes, "
            << rtree->getTreeDepth() << " levels";
    return (os);
}

std::ostream& operator<<(std::ostream& os,
        const curfil::ImageFeatureFunction& featureFunction) {
    os << "ImageFeatureFunction[";
    os << "type: " << featureFunction.getTypeString();
    os << ", offset1: " << featureFunction.getOffset1();
    os << ", region1: " << featureFunction.getRegion1();
    os << ", channel1: " << static_cast<int>(featureFunction.getChannel1());
    os << ", offset2: " << featureFunction.getOffset2();
    os << ", region2: " << featureFunction.getRegion2();
    os << ", channel2: " << static_cast<int>(featureFunction.getChannel2());
    os << "]";
    return (os);
}

std::ostream& operator<<(std::ostream& os,
        const curfil::XY& xy) {
    os << "[" << xy.getX() << "," << xy.getY() << "]";
    return (os);
}


#ifndef RESNIK_SIMILARITY
#define RESNIK_SIMILARITY

#include <cmath>
#include <string>

#include <ggtk/TermSimilarityInterface.hpp>
#include <ggtk/TermInformationContentMap.hpp>
#include <ggtk/GoGraph.hpp>

#include <boost/unordered_set.hpp>

/*! \class ResnikSimilarity
	\brief A class to calculate resnik similarity between 2 terms

	This class calculates Resnik similarity.
	Philip Resnik (1995). "Using information content to evaluate semantic similarity in a taxonomy".
	  In Chris S. Mellish (Ed.). Proceedings of the 14th international joint conference on Artificial intelligence (IJCAI'95)

    P. W. Lord, R. D. Stevens, A. Brass, and C. A. Goble,
	 "Semantic similarity measures as tools for exploring the gene ontology,"
	 Pac Symp Biocomput, pp. 601-12, 2003.

	maximun information content of all shared ancestors
	IC(MICA)

*/
class ResnikSimilarity: public TermSimilarityInterface{

public:

	//! A constructor
	/*!
		Creates the default(empty) StandardRelationshipPolicy
	*/
	inline ResnikSimilarity(GoGraph* goGraph,TermInformationContentMap &icMap){
		_goGraph = goGraph;
		_icMap = icMap;
	}

	//! A method for calculating term-to-term similarity for GO terms using Resnik similarity
	/*!
		This method returns the Resnik similarity or the information content of the most informative common ancestor.
	*/
	inline double calculateTermSimilarity(std::string goTermA, std::string goTermB){
		//if the terms do not exit return 0.0 similarity
		if(!_icMap.hasTerm(goTermA) || !_icMap.hasTerm(goTermB)){
			return 0.0;
		}

		//if not from same ontology, return 0;
		if(_goGraph->getTermOntology(goTermA) != _goGraph->getTermOntology(goTermB)){
			return 0.0;
		}

		//create 2 sets
		boost::unordered_set<std::string> ancestorsA = _goGraph->getAncestorTerms(goTermA);
		ancestorsA.insert(goTermA);
		boost::unordered_set<std::string> ancestorsB = _goGraph->getAncestorTerms(goTermB);
		ancestorsB.insert(goTermB);

		//if either set is empty, return 0
		if(ancestorsA.size() == 0 || ancestorsB.size() == 0){
			return 0.0;
		}

		//get the MICA
		std::string mica = getMICA(ancestorsA,ancestorsB);

		//return the inforamtion content of the mica
		return _icMap[mica];
	}

	//! A method for calculating term-to-term similarity for GO terms using Normalized Resnik similarity
	/*!
		This method returns the Resnik similarity divided by the maximum possible similarity
	*/
	inline double calculateNormalizedTermSimilarity(std::string goTermA, std::string goTermB){
		//if the terms do not exit return 0.0 similarity
		if (!_icMap.hasTerm(goTermA) || !_icMap.hasTerm(goTermB)){
			return 0.0;
		}

		//call base similarity
		double resnik = calculateTermSimilarity(goTermA,goTermB);

		double maxIC;

		//select the correct ontology normalization factor
		GO::Onto ontoType = _goGraph->getTermOntology(goTermA);
		if(ontoType == GO::BP){
			maxIC = -std::log(_icMap.getMinBP());
		}else if(ontoType == GO::MF){
			maxIC = -std::log(_icMap.getMinMF());
		}else{
			maxIC = -std::log(_icMap.getMinCC());
		}

		return resnik/maxIC;
	}

	//! A method for calculating the most informative common ancestor
	/*!
		This method searches the sets to determine the most informatics ancestor.
	*/
	inline std::string getMICA(boost::unordered_set<std::string> &ancestorsA,boost::unordered_set<std::string> &ancestorsB){
		//get the first term as a start
		std::string mica = *ancestorsA.begin();
		double max = -1.0;

		//always traverse the shortest list
		if(ancestorsA.size() > ancestorsB.size()){
			ancestorsA.swap(ancestorsB);
		}

		//loop over shorter list
		boost::unordered_set<std::string>::iterator iter;
		for(iter = ancestorsA.begin(); iter != ancestorsA.end(); ++iter){
			//current string
			std::string currentTerm = *iter;

			if(ancestorsB.find(currentTerm) == ancestorsB.end()){
				//continue, not a shared ancestor term
				continue;
			}

			//if new max, update
			if(_icMap[currentTerm] > max){
				mica = currentTerm;
				max = _icMap[currentTerm];
			}
		}

		return mica;
	}

private:

	GoGraph* _goGraph;
	TermInformationContentMap _icMap;

};
#endif


// Boost.Geometry
// Unit Test

// Copyright (c) 2016-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include "test_get_turns.hpp"
#include <boost/geometry/geometries/geometries.hpp>

#include <algorithms/overlay/overlay_cases.hpp>

template <typename T>
void test_all()
{
    typedef bg::model::point<T, 2, bg::cs::spherical_equatorial<bg::degree> > pt;
    //typedef bg::model::ring<pt> ring;
    typedef bg::model::polygon<pt> poly;
    //typedef bg::model::multi_polygon<poly> mpoly;

    test_geometry<poly, poly>(case_1[0], case_1[1],
                              expected("iiu")("iui")("iiu")("iui")("iiu")("iui"));
    test_geometry<poly, poly>(case_2[0], case_2[1],
                              expected("iui")("iiu")("iui")("iiu")("iui")("iiu")("iui")("iiu"));
    test_geometry<poly, poly>(case_3_sph[0], case_3_sph[1],
                              expected("miu")("miu")("miu")("miu"));
    test_geometry<poly, poly>(case_4[0], case_4[1],
                              expected("iiu")("iui")("iiu")("iui")("iiu")("iui")("iiu")("iui")("iiu")("iui")("iiu")("iui"));
    test_geometry<poly, poly>(case_5[0], case_5[1],
                              expected("iiu")("iui")("iiu")("iui")("iiu")("iui")("iiu")("iui")("iiu")("iui")("iiu")("iui")("iiu")("iui")("iiu")("iui"));
    test_geometry<poly, poly>(case_6_sph[0], case_6_sph[1],
                              expected("ccc")("eui")("mcc"));

    test_geometry<poly, poly>(case_7[0], case_7[1],
                              expected("txu")("tux"));
    test_geometry<poly, poly>(case_8_sph[0], case_8_sph[1],
                              expected("mux")("cxu"));
    test_geometry<poly, poly>(case_9_sph[0], case_9_sph[1],
                              expected("muu"));
    test_geometry<poly, poly>(case_10_sph[0], case_10_sph[1],
                              expected("cxu")("mux")("txx"));
    test_geometry<poly, poly>(case_11_sph[0], case_11_sph[1],
                              expected("mui"));
    test_geometry<poly, poly>(case_12[0], case_12[1],
                              expected("iiu")("iui")("iiu")("iui")("iiu")("iui")("iiu")("iui"));

    test_geometry<poly, poly>(case_13_sph[0], case_13_sph[1],
                              expected("mxu")("mux"));
    test_geometry<poly, poly>(case_14_sph[0], case_14_sph[1],
                              expected("cxu")("mux"));
    test_geometry<poly, poly>(case_15_sph[0], case_15_sph[1],
                              expected("cxu")("mux"));
    test_geometry<poly, poly>(case_16_sph[0], case_16_sph[1],
                              expected("txx")("txx")("tux")("cxu"));
    test_geometry<poly, poly>(case_17_sph[0], case_17_sph[1],
                              expected("mcc")("cui"));
    test_geometry<poly, poly>(case_18_sph[0], case_18_sph[1],
                              expected("mcc")("ccc")("ccc")("cui"));

    test_geometry<poly, poly>("POLYGON((16 15,-132 10,-56 89,67 5,16 15))",
                              "POLYGON((101 49,12 40,-164 10,117 0,101 49))",
                              expected("iiu")("iui"));
}

int test_main(int, char* [])
{
    test_all<float>();
    test_all<double>();

//#if ! defined(_MSC_VER)
//    test_all<long double>();
//#endif

    return 0;
}


#include <boost/test/unit_test.hpp>
#include "algorithms/dp/ways_to_get_target_sum.hpp"

BOOST_AUTO_TEST_SUITE(TestWaysToGetTargetSum)

BOOST_AUTO_TEST_CASE(empty_vector)
{
    const std::vector<int> values;
    const int targetSum = 0;
    const int expected = 0;
    BOOST_CHECK(expected == Algo::DP::WaysToGetTargetSum::calc(values, targetSum));
}

BOOST_AUTO_TEST_CASE(vector_wih_one_element_valid_target)
{
    const std::vector<int> values = {1};
    const int targetSum = 1;
    const int expected = 1;
    BOOST_CHECK(expected == Algo::DP::WaysToGetTargetSum::calc(values, targetSum));
}

BOOST_AUTO_TEST_CASE(vector_wih_one_element_invalid_target)
{
    const std::vector<int> values = {10};
    const int targetSum = 1;
    const int expected = 0;
    BOOST_CHECK(expected == Algo::DP::WaysToGetTargetSum::calc(values, targetSum));
}

// TODO fix
//BOOST_AUTO_TEST_CASE(vector_wih_several_element_valid_target)
//{
//    const std::vector<int> values = {1, 1, 1, 1, 1};
//    const int targetSum = 3;
//    const int expected = 5;
//    BOOST_CHECK(expected == Algo::DP::WaysToGetTargetSum::calc(values, targetSum));
//}
//
//BOOST_AUTO_TEST_CASE(vector_wih_several_element_invalid_target)
//{
//    const std::vector<int> values = {1, 1, 1, 1, 1};
//    const int targetSum = 80;
//    const int expected = 0;
//    BOOST_CHECK(expected == Algo::DP::WaysToGetTargetSum::calc(values, targetSum));
//}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file SparseMatrixHelper.hpp
 * @author Giulio Romualdi
 * @copyright  Released under the terms of the BSD 3-Clause License
 * @date 2018
 */

#ifndef SPARSE_MATRIX_HPP
#define SPARSE_MATRIX_HPP

// std
#include <iostream>

// eigen
#include <Eigen/Sparse>

// OSQP
#include <osqp.h>

/**
 * OsqpEigen namespace.
 */
namespace OsqpEigen
{
    /**
     * SparseMatrixHelper namespace is a namspace that contains helper function to handle osqp matrix.
     * Use it to create ot update or manage an osqp sparse matrix.
     * osqp sparse matrix in [compressed-column](https://people.sc.fsu.edu/~jburkardt/data/cc/cc.html)
     * or triplet form.
     */
    namespace SparseMatrixHelper
    {

        /**
         * Allocate an osqpSparseMatrix struct.
         * NOTE: c_malloc function is used to allocate memory please call
         * c_free to deallcate memory.
         * @return a const point to the csc struct.
         */
        template <typename Derived>
        bool createOsqpSparseMatrix(const Eigen::SparseCompressedBase<Derived>& eigenSparseMatrix,
                                    csc*& osqpSparseMatrix);

        /**
         * Convert an osqp sparse matrix into an eigen sparse matrix.
         * @param osqpSparseMatrix is  a constant pointer to a constant csc struct;
         * @param eigenSparseMatrix is the eigen sparse matrix object.
         * @return a const point to the csc struct.
         */
        template <typename T>
        bool osqpSparseMatrixToEigenSparseMatrix(const csc* const & osqpSparseMatrix,
                                                 Eigen::SparseMatrix<T> &eigenSparseMatrix);

        /**
         * Convert an osqp sparse matrix into a eigen triplet list.
         * @param osqpSparseMatrix is reference to a constant pointer to a constant csc struct;
         * @param tripletList is a std::vector containing the triplet.
         * @return a const point to the csc struct.
         */
        template <typename T>
        bool osqpSparseMatrixToTriplets(const csc* const & osqpSparseMatrix,
                                        std::vector<Eigen::Triplet<T>> &tripletList);

        /**
         * Convert an eigen sparse matrix into a eigen triplet list.
         * @param eigenSparseMatrix is the eigen sparse matrix object;
         * @param tripletList is a std::vector containing the triplet.
         * @return a const point to the csc struct.
         */
        template<typename Derived, typename T>
        bool eigenSparseMatrixToTriplets(const Eigen::SparseCompressedBase<Derived> &eigenSparseMatrix,
                                         std::vector<Eigen::Triplet<T>> &tripletList);
    };
}

#include <OsqpEigen/SparseMatrixHelper.tpp>

#endif


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

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

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

#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/function/is_not_equal.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/seladd.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( oneplus_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_<bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return seladd(is_not_equal(a0, Valmax<A0>()),
                    a0,
                    One<A0>());
    }
  };

  BOOST_DISPATCH_OVERLOAD ( oneplus_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return a0+One<A0>();
    }
  };
} } }

#endif


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <qle/instruments/deposit.hpp>

#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/simplecashflow.hpp>

#include <boost/make_shared.hpp>

using namespace QuantLib;

namespace QuantExt {

Deposit::Deposit(const Real nominal, const Rate rate, const Period& tenor, const Natural fixingDays,
                 const Calendar& calendar, const BusinessDayConvention convention, const bool endOfMonth,
                 const DayCounter& dayCounter, const Date& tradeDate, const bool isLong, const Period forwardStart) {

    leg_.resize(3);
    index_ = boost::make_shared<IborIndex>("despoit-helper-index", tenor, fixingDays, Currency(), calendar, convention,
                                           endOfMonth, dayCounter);
    // move to next good day
    Date referenceDate = calendar.adjust(tradeDate);
    startDate_ = index_->valueDate(referenceDate);
    fixingDate_ = index_->fixingDate(startDate_);
    maturityDate_ = index_->maturityDate(startDate_);
    Real w = isLong ? 1.0 : -1.0;
    leg_[0] = boost::make_shared<Redemption>(-w * nominal, startDate_);
    leg_[1] =
        boost::make_shared<FixedRateCoupon>(maturityDate_, w * nominal, rate, dayCounter, startDate_, maturityDate_);
    leg_[2] = boost::make_shared<Redemption>(w * nominal, maturityDate_);
}

bool Deposit::isExpired() const { return detail::simple_event(maturityDate_).hasOccurred(); }

void Deposit::setupExpired() const {
    Instrument::setupExpired();
    fairRate_ = Null<Real>();
}

void Deposit::setupArguments(PricingEngine::arguments* args) const {

    Deposit::arguments* arguments = dynamic_cast<Deposit::arguments*>(args);
    QL_REQUIRE(arguments, "wrong argument type in deposit");
    arguments->leg = leg_;
    arguments->index = index_;
    arguments->maturityDate = maturityDate_;
}

void Deposit::fetchResults(const PricingEngine::results* r) const {

    Instrument::fetchResults(r);
    const Deposit::results* results = dynamic_cast<const Deposit::results*>(r);
    QL_REQUIRE(results, "wrong result type");
    fairRate_ = results->fairRate;
}

void Deposit::arguments::validate() const {
    QL_REQUIRE(leg.size() == 3,
               "deposit arugments: unexpected number of cash flows (" << leg.size() << "), should be 3");
}

void Deposit::results::reset() {

    Instrument::results::reset();
    fairRate = Null<Real>();
}
} // namespace QuantExt


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2015-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifdef UNITTEST

#include <algorithm>
#include <iostream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"

#include "fem/assemble.hh"
#include "fem/istlinterface.hh"
#include "fem/functional_aux.hh"
#include "fem/fixdune.hh"
#include "fem/variables.hh"
#include "fem/gridmanager.hh"
#include "fem/lagrangespace.hh"
#include "fem/spaces.hh"
#include "fem/variables.hh"
#include "io/vtk.hh"
#include "linalg/direct.hh"
#include "linalg/jacobiPreconditioner.hh"
#include "linalg/threadedMatrix.hh"
#include "mg/additiveMultigrid.hh"
#include "mg/multigrid.hh"
#include "mg/multiplicativeMultigrid.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare
#include "utilities/linalg/scalarproducts.hh"

template <class RType, class VarSet>
class HeatFunctional : public Kaskade::FunctionalBase<Kaskade::VariationalFunctional>
{
public:
  using Scalar = RType;
  using OriginVars = VarSet;
  using AnsatzVars = VarSet;
  using TestVars = VarSet;

  using Cell = typename AnsatzVars::Grid::template Codim<0>::Entity;

  static constexpr int dim = AnsatzVars::Grid::dimension;
  static constexpr int uIdx = 0;
  static constexpr int uSpaceIdx = boost::fusion::result_of::value_at_c<typename AnsatzVars::Variables,
                                             uIdx>::type::spaceIndex;

  class DomainCache : public Kaskade::CacheBase<HeatFunctional,DomainCache>
  {
  public:
    DomainCache(HeatFunctional const& f,
                typename AnsatzVars::VariableSet const& vars_,
                int flags=7):
      data(vars_), sigmaExterior(f.sigmaExterior)
    {}

    void moveTo(Cell const& cell_)
    {
      cell = &cell_;
    }

    template <class Position, class Evaluators>
    void evaluateAt(Position const& xi, Evaluators const& evaluators)
    {
      u  = boost::fusion::at_c<uIdx>(data.data).value(boost::fusion::at_c<uSpaceIdx>(evaluators));
      du = boost::fusion::at_c<uIdx>(data.data).derivative(boost::fusion::at_c<uSpaceIdx>(evaluators));
      f = 1.0;

      // generate jumping coefficients for challenging multigrid
      auto x = cell->geometry().global(xi);
      x -= 0.5;
      if (x.two_norm()<0.4 && x.two_norm()>0.2)
        sigma = 1;
      else
        sigma = sigmaExterior;
    }

    Scalar
    d0() const
    {
      return sigma*sp(du,du)/2 - f*u;
    }

    template<int row>
    Scalar d1_impl(Kaskade::VariationalArg<Scalar,dim,TestVars::template Components<row>::m> const& arg) const
    {
      return sigma*sp(du,arg.derivative) - f*arg.value;
    }

    template<int row, int col>
    Scalar d2_impl(Kaskade::VariationalArg<Scalar,dim,TestVars::template Components<row>::m> const &arg1,
                   Kaskade::VariationalArg<Scalar,dim,AnsatzVars::template Components<row>::m> const &arg2) const
    {
      return sigma*sp(arg1.derivative,arg2.derivative);
    }

  private:
    typename AnsatzVars::VariableSet const& data;
    Dune::FieldVector<Scalar,AnsatzVars::template Components<uIdx>::m> u, f;
    Dune::FieldMatrix<Scalar,AnsatzVars::template Components<uIdx>::m,dim> du;
    Kaskade::LinAlg::EuclideanScalarProduct sp;
    Cell const* cell;
    double sigma, sigmaExterior;
  };

/// \class BoundaryCache
///
  class BoundaryCache : public Kaskade::CacheBase<HeatFunctional,BoundaryCache>
  {
  public:
    BoundaryCache(HeatFunctional<RType,AnsatzVars> const&,
                  typename AnsatzVars::VariableSet const& vars_,
                  int flags=7):
      data(vars_), penalty(1e9), u(0.), uDirichletBoundaryValue(0.)
    {}

    template <class Position, class Evaluators>
    void evaluateAt(Position const&, Evaluators const& evaluators)
    {
      u = boost::fusion::at_c<uIdx>(data.data).value(boost::fusion::at_c<uSpaceIdx>(evaluators));
    }

    Scalar d0() const
    {
      return penalty*(u-uDirichletBoundaryValue)*(u-uDirichletBoundaryValue)/2;
    }

    template<int row>
    Scalar d1_impl(Kaskade::VariationalArg<Scalar,dim> const& arg) const
    {
      return penalty*(u-uDirichletBoundaryValue)*arg.value;
    }

    template<int row, int col>
    Scalar d2_impl(Kaskade::VariationalArg<Scalar,dim> const &arg1,
                   Kaskade::VariationalArg<Scalar,dim> const &arg2) const
    {
      return penalty*arg1.value*arg2.value;
    }

  private:
    typename AnsatzVars::VariableSet const& data;
    Scalar penalty;
    Dune::FieldVector<Scalar,AnsatzVars::template Components<uIdx>::m> u, uDirichletBoundaryValue;
  };

/// \struct D2
///
  template <int row>
  struct D1: public Kaskade::FunctionalBase<Kaskade::VariationalFunctional>::D1<row>
  {
    static bool const present   = true;
    static bool const constant  = false;

  };

  template <int row, int col>
  struct D2: public Kaskade::FunctionalBase<Kaskade::VariationalFunctional>::D2<row,col>
  {
    static bool const present = true;
    static bool const symmetric = true;
    static bool const lumped = false;
  };

  HeatFunctional(double sExt)
  : sigmaExterior(sExt) {}

  template <class Cell>
  int integrationOrder(Cell const& /* cell */, int shapeFunctionOrder, bool boundary) const
  {
    if (boundary)
      return 2*shapeFunctionOrder;
    else
    {
      int stiffnessMatrixIntegrationOrder = 2*(shapeFunctionOrder-1);
      int sourceTermIntegrationOrder = shapeFunctionOrder;        // as rhs f is constant, i.e. of order 0

      return std::max(stiffnessMatrixIntegrationOrder,sourceTermIntegrationOrder);
    }
  }

  double sigmaExterior;
};

template <class CoefficientVector, class Matrix, class Multigrid, class Function>
void runMultiGrid(Matrix const& A, CoefficientVector const& b, Multigrid& mg, Function& u, int order, std::string base, int steps, bool verbose=false)
{
  using namespace Kaskade;

  auto r = b;
  auto dx = b;
  auto solution = b;
  solution = 0;
  std::vector<double> norms;

  Timings& timer = Timings::instance();

  int i = 0;
  for (i=0; i<steps && (norms.empty() || norms.back()>1e-8); ++i)
  {
    // compute residual r = b-A*u
    r = b;
    timer.start("matrix-vector");
    A.usmv(-1.0,solution,r);
    timer.stop("matrix-vector");

    auto res = r;
    double normr = r.two_norm();
    norms.push_back(normr);
    if (verbose)
      std::cerr << "euclidean residual norm |r|=" << normr << "\n";

    // compute preconditioner
    dx = 0.0;
    timer.start("MG application");
    mg.apply(dx,r);
    timer.stop("MG application");

    // compute step length, using r as temporary
    A.mv(dx,r);
    double omega = (res*dx) / (dx*r);
    dx *= omega;
    solution += dx;

    // output
    boost::fusion::at_c<0>(u.data) = solution;
    if (verbose) writeVTKFile(u,base+"-sol-"+paddedString(i),IoOptions().setOrder(order).setPrecision(8));
    boost::fusion::at_c<0>(u.data) = dx;
    if (verbose) writeVTKFile(u,base+"-cor-"+paddedString(i),IoOptions().setOrder(order).setPrecision(7));
  }

  if (norms.size()>2)
  {
    std::cout << "contraction factor: " << std::sqrt(norms.back()/norms[norms.size()-3]) << "\n";
    std::cout << "residual history: ";
    for (double nrm: norms)
      std::cout << nrm << " ";
    std::cout << "\n";
  }
}

void checkP1(int refinements, int steps)
{
  using namespace Kaskade;

  constexpr int dim = 2;

  using Grid = Dune::UGGrid<dim>;
  using Spaces = boost::fusion::vector<H1Space<Grid> const*>;
  using VariableDescriptions = boost::fusion::vector<Variable<SpaceIndex<0>,Components<1>,VariableId<0>>>;
  using VariableSetDesc = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = HeatFunctional<double,VariableSetDesc>;
  using CoefficientVectors = VariableSetDesc::CoefficientVectorRepresentation<0,1>::type;

  std::cerr << "=================================================================\nh multigrid with j=" << refinements << "\n";

  boost::timer::cpu_timer timer;
  GridManager<Grid> gridManager( createUnitSquare<Grid>(0.25) );
  std::cout << "grid creation:   " << timer.format();
  timer.start();
  gridManager.globalRefine(refinements);
  std::cout << "grid refinement: " << timer.format();
  gridManager.enforceConcurrentReads(true);

  // construction of finite element space for the scalar solution u.
  timer.start();
  H1Space<Grid> temperatureSpace(gridManager,gridManager.grid().leafGridView(),1);
  std::cout << "p1space creation:" << timer.format();
  Spaces spaces(&temperatureSpace);
  VariableSetDesc variableSetDesc(spaces,{ "u" });

  Functional F(1e0); // 1e0: constant coefficients, nice problem.
                     // 1e2: jumping coefficients, bad problem

  //construct Galerkin representation
  VariationalFunctionalAssembler<LinearizationAt<Functional>> assembler(spaces);
  VariableSetDesc::VariableSet u(variableSetDesc);
  auto du = u;
  timer.start();
  assembler.assemble(linearization(F,u));
  std::cout << "assembly:        " << timer.format();
  using Matrix = NumaBCRSMatrix<Dune::FieldMatrix<double,1,1>>;
  timer.start();
  auto A = assembler.get<Matrix>(false);
  std::cout << "matrix extract:  " << timer.format();
  using Vector = Dune::BlockVector<Dune::FieldVector<double,1>>;

  // Perform a couple of steps of the
  Vector solution(temperatureSpace.degreesOfFreedom());
  CoefficientVectors rhs(assembler.rhs());
  Vector dx = solution;
  Vector r = boost::fusion::at_c<0>(rhs.data);

  std::cout << "Degrees of freedom: " << temperatureSpace.degreesOfFreedom() << "\n";

  Timings& tmr = Timings::instance();
  tmr.clear();
  // Create multigrid preconditioner
  std::cerr << "\nmultiplicative Jacobi multigrid j=" << refinements << "\n";
  tmr.start("MG Jacobi 2,2 construction");
  auto multMG = makeJacobiMultiGrid(A,gridManager,2,2);
  tmr.stop("MG Jacobi 2,2 construction");
  tmr.start("solution");
  runMultiGrid(A,boost::fusion::at_c<0>(rhs.data),multMG,u,1,"/tmp/mul-",steps);
  tmr.stop("solution");
  std::cout << tmr;
  tmr.clear();

  std::cout << "\nmultiplicative Jacobi multigrid j=" << refinements << " with linesearch\n";
  multMG.setLinesearch(true);
  tmr.start("solution");
  runMultiGrid(A,boost::fusion::at_c<0>(rhs.data),multMG,u,1,"/tmp/mul-",steps);
  tmr.stop("solution");
  std::cout << tmr;
  tmr.clear();

  // Create multigrid preconditioner
  std::cerr << "\nadditive BPX multigrid j=" << refinements << "\n";
  tmr.start("BPX construction");
  auto addMG = makeBPX(A,gridManager);
  tmr.stop("BPX construction");
  tmr.start("solution");
  runMultiGrid(A,boost::fusion::at_c<0>(rhs.data),addMG,u,1,"/tmp/add-",steps);
  tmr.stop("solution");
  std::cout << tmr;
  tmr.clear();

  // Create default multigrid preconditioner
  std::cerr << "\ndefault multigrid j=" << refinements << "\n";
  tmr.start("MG default construction");
  auto defMG = makeMultigrid(A,temperatureSpace);
  tmr.stop("MG default construction");
  tmr.start("solution");
  runMultiGrid(A,boost::fusion::at_c<0>(rhs.data),*defMG,u,1,"/tmp/add-",steps);
  tmr.stop("solution");
  std::cout << tmr;
  tmr.clear();

  // Create algebraic multigrid preconditioner
  std::cerr << "\nalgebraic multigrid j=" << refinements << "\n";
  tmr.start("algebraic MG construction");
  tmr.start("MG stack creation");
  auto mgStack = makeAlgebraicMultigridStack(duplicate(A));
  std::cout << "nr levels: " << mgStack.levels() << "\n";
  //~ std::cout << mgStack << "\n";
  tmr.stop("MG stack creation");
  tmr.start("direct solver creation");
  auto coarsePreconditioner = makeDirectPreconditioner(std::move(mgStack.coarseGridMatrix()));
  tmr.stop("direct solver creation");
  tmr.start("MG creation");
  auto amg = makeMultiplicativeMultiGrid(std::move(mgStack),MakeJacobiSmoother(),moveUnique(std::move(coarsePreconditioner)),3,3);
  tmr.stop("MG creation");
  amg.setSmootherStepSize(0.5);
  tmr.stop("algebraic MG construction");
  tmr.start("solution");
  runMultiGrid(A,boost::fusion::at_c<0>(rhs.data),amg,u,1,"/tmp/add-",steps);
  tmr.stop("solution");
  std::cout << tmr;
  tmr.clear();

  // compare with direct solver
  std::cout << "\ndirect solver j=" << refinements << "\n";
  timer.start();
  DirectSolver<Vector,Vector> direct(A,DirectType::MUMPS);
  std::cout << "Factorization: " << timer.format();
  timer.start();
  direct.apply(dx,r);
  std::cout << "Solve: " << timer.format() << "\n";
}

void checkPp(int refinements, int steps, int order)
{
  using namespace Kaskade;

  std::cout << "=======================================\n";
  std::cout << "running hp multigrid with p=" << order << "\n";
  constexpr int dim = 2;

  using Grid = Dune::UGGrid<dim>;
  using Spaces = boost::fusion::vector<H1Space<Grid> const*>;
  using VariableDescriptions = boost::fusion::vector<Variable<SpaceIndex<0>,Components<1>,VariableId<0>>>;
  using VariableSetDesc = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = HeatFunctional<double,VariableSetDesc>;
  using CoefficientVectors = VariableSetDesc::CoefficientVectorRepresentation<0,1>::type;

  Timings& timer = Timings::instance();
  timer.start("grid creation");
  GridManager<Grid> gridManager( createUnitSquare<Grid>(1.0) );
  timer.stop("grid creation");
  timer.start("refinement");
  gridManager.globalRefine(refinements);
  timer.stop("refinement");
  gridManager.enforceConcurrentReads(true);

  // construction of finite element space for the scalar solution u.
  timer.start("P1 space creation");
  H1Space<Grid> p1Space(gridManager,gridManager.grid().leafGridView(),1);
  timer.stop("P1 space creation");
  timer.start("P? space creation");
  H1Space<Grid> temperatureSpace(gridManager,gridManager.grid().leafGridView(),order);
  timer.stop("P? space creation");

  std::cout << "Degrees of freedom: " << temperatureSpace.degreesOfFreedom() << "\n";

  auto assembleMatrix = [&](auto const& space)
  {
    Spaces spaces(&space);
    VariableSetDesc variableSetDesc(spaces,{ "u" });
    Functional F(1);
    VariationalFunctionalAssembler<LinearizationAt<Functional>> assembler(spaces);
    VariableSetDesc::VariableSet u(variableSetDesc);

    assembler.assemble(linearization(F,u));
    using Matrix = NumaBCRSMatrix<Dune::FieldMatrix<double,1,1>>;
    CoefficientVectors r(assembler.rhs());
    return std::make_pair(assembler.get<Matrix>(false),r);
  };

  Spaces spaces(&temperatureSpace);
  VariableSetDesc variableSetDesc(spaces,{ "u" });
  VariableSetDesc::VariableSet u(variableSetDesc);

  //construct Galerkin representation
  timer.start("assembly");
  auto mr = assembleMatrix(temperatureSpace);
  auto A = mr.first;
  auto r = boost::fusion::at_c<0>(mr.second.data);
  timer.stop("assembly");

  std::cout << "Setup:" << timer;
  timer.clear();

  // Create multiplicative multigrid preconditioner
  std::cerr << "\nmultiplicative P multigrid with Jacobi smoother\n";
  timer.start("MG creation");
  auto multMG = makeJacobiPMultiGrid(A,temperatureSpace,2,2);
  timer.stop("MG creation");
  timer.start("solution");
  runMultiGrid(A,r,multMG,u,2,"/tmp/mulp1-"+paddedString(order),steps);
  timer.stop("solution");
  std::cout << timer;
  timer.clear();

  // Create multiplicative multigrid preconditioner
  std::cerr << "\nmultiplicative P multigrid with Schwarz smoother\n";
  timer.start("MG creation");
  auto multMG2 = makeBlockJacobiPMultiGrid(A,temperatureSpace,2,2);
  timer.stop("MG creation");
  timer.start("solution");
  runMultiGrid(A,r,multMG2,u,2,"/tmp/mulp2-"+paddedString(order),steps);
  timer.stop("solution");
  std::cout << timer;
  timer.clear();

  // Create multiplicative multigrid preconditioner
  std::cerr << "\nmultiplicative P multigrid with Schwarz smoother (P1 assembly)\n";
  timer.start("MG creation");
  timer.start("assembly");
  auto cA = assembleMatrix(p1Space).first;
  timer.stop("assembly");
  auto multMG3 = makeBlockJacobiPMultiGrid(A,temperatureSpace,cA,p1Space,2,2);
  timer.stop("MG creation");
  timer.start("solution");
  runMultiGrid(A,r,multMG3,u,2,"/tmp/mulp3-"+paddedString(order),steps);
  timer.stop("solution");
  std::cout << timer;
  timer.clear();

  // Create multigrid preconditioner
  std::cerr << "\nadditive PBPX multigrid\n";
  timer.start("MG creation");
  auto addMG = makePBPX(A,temperatureSpace,p1Space);
  timer.stop("MG creation");
  timer.start("solution");
  runMultiGrid(A,r,addMG,u,2,"/tmp/addp-"+paddedString(order),steps);
  timer.stop("solution");
  std::cout << timer;
  timer.clear();

  // Create default multigrid preconditioner
  std::cerr << "\ndefault multigrid\n";
  timer.start("MG creation");
  auto defMG = makeMultigrid(A,temperatureSpace);
  timer.stop("MG creation");
  timer.start("solution");
  runMultiGrid(A,r,*defMG,u,1,"/tmp/add-",steps);
  timer.stop("solution");
  std::cout << timer;
  timer.clear();

  // compare with direct solver
  std::cout << "\ndirect solver j=" << refinements << "\n";
  timer.start("factorization");
  using Vector = Dune::BlockVector<Dune::FieldVector<double,1>>;
  DirectSolver<Vector,Vector> direct(A,DirectType::MUMPS);
  timer.stop("factorization");
  timer.start("solution");
  Vector dx(temperatureSpace.degreesOfFreedom());
  direct.apply(dx,r);
  timer.stop("solution");
  std::cout << timer;
  timer.clear();
}

int main()
{
  int maxSteps = 100;
  std::cout << "Start multigrid test program" << std::endl;
  for (int j=1; j<12; ++j)
    checkP1(j,maxSteps);
  //~ checkPp(9,maxSteps,2);
  //~ checkPp(8,maxSteps,3);
  //~ checkPp(8,maxSteps,4);
  //~ checkPp(7,maxSteps,5);
  //~ checkPp(7,maxSteps,6);
  return 0;
}

#endif


//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//  Adaptation to Boost of the libcxx
//  Copyright 2010 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

// ToDuration shall be an instantiation of duration.

#include <boost/chrono/chrono.hpp>

void test()
{
    typedef boost::chrono::system_clock Clock;
    typedef boost::chrono::time_point<Clock, boost::chrono::milliseconds> FromTimePoint;
    typedef boost::chrono::time_point<Clock, boost::chrono::minutes> ToTimePoint;
    boost::chrono::time_point_cast<ToTimePoint>(FromTimePoint(boost::chrono::milliseconds(3)));
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *        Wakker, K.F. "Astrodynamics I, AE4-874", Delft University of Technology, 2007.
 *        Howell, K.C. Three-dimensional, periodic, 'Halo' orbits, Celestial Mechanics, 32. 53-71,
 *          1984.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/bind.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKutta4Integrator.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Astrodynamics/Propagators/stateDerivativeCircularRestrictedThreeBodyProblem.h"

namespace tudat
{
namespace unit_tests
{

using namespace propagators;
using namespace orbital_element_conversions;

//! Test if state derivative for circular restricted three-body problem is computed correctly.
BOOST_AUTO_TEST_CASE( testStateDerivativeCircularRestrictedThreeBodyProblem )
{

    // Test 1: test state derivative at L1.
    {
        // Set mass parameter for Earth-moon system. Value from Table 3.1 (Wakker, 2007).
        double massParameter = 0.01215;

        // Initialize position L1, from Table 3.4 (Wakker, 2007).
        Eigen::VectorXd stateAtL1 = Eigen::VectorXd::Zero( 6 );
        stateAtL1( xCartesianPositionIndex ) = 0.836914;

        // Declare state derivative object.
        StateDerivativeCircularRestrictedThreeBodyProblem stateDerivative( massParameter );

        // Compute state derivative at L1.
        Eigen::VectorXd stateDerivativeAtL1 = stateDerivative.computeStateDerivative(
                    0.0, stateAtL1 );

        // Check if expected state derivative (all zeros) matches computed.
        TUDAT_CHECK_MATRIX_BASE( Eigen::VectorXd::Zero( 6 ), stateDerivativeAtL1 )
            BOOST_CHECK_SMALL( stateDerivativeAtL1.coeff( row, col ), 1.0e-4 );
    }

    // Test 2: test Halo orbit around L1. Note that this is actually a system test, since it
    //         requires that the Runge-Kutta 4 numerical integrator is functioning correctly.
    {
        // Set mass parameter. Value from Table I (Howell, 1984).
        double massParameter = 0.04;

        // Initialize position on Halo orbit around L1 (Howell, 1984).
        Eigen::VectorXd initialStateOnHaloOrbit = Eigen::VectorXd::Zero( 6 );
        initialStateOnHaloOrbit( xCartesianPositionIndex ) = 0.723268;
        initialStateOnHaloOrbit( zCartesianPositionIndex ) = 0.04;
        initialStateOnHaloOrbit( yCartesianVelocityIndex ) = 0.198019;

        // Declare state derivative object.
        StateDerivativeCircularRestrictedThreeBodyProblem stateDerivative( massParameter );

        // Declare Runge-Kutta 4 integrator.
        numerical_integrators::RungeKutta4IntegratorXd rungeKutta4Integrator(
                    std::bind(
                        &StateDerivativeCircularRestrictedThreeBodyProblem::computeStateDerivative,
                        &stateDerivative, std::placeholders::_1, std::placeholders::_2 ),
                    0.0, initialStateOnHaloOrbit );

        // Integrate Halo orbit over one period.
        Eigen::VectorXd finalStateOnHaloOrbit = rungeKutta4Integrator.integrateTo(
                    2.0 * 1.300177, 0.1 );

        // Check if expected final state (same as initial) matches computed.
        BOOST_CHECK_CLOSE_FRACTION( initialStateOnHaloOrbit( 0 ),
                                    finalStateOnHaloOrbit( 0 ), 1.0e-3 );
        BOOST_CHECK_SMALL( finalStateOnHaloOrbit( 1 ), 1.0e-3 );
        BOOST_CHECK_CLOSE_FRACTION( initialStateOnHaloOrbit( 2 ),
                                    finalStateOnHaloOrbit( 2 ), 1.0e-3 );
        BOOST_CHECK_SMALL( finalStateOnHaloOrbit( 3 ), 1.0e-2 );
        BOOST_CHECK_CLOSE_FRACTION( initialStateOnHaloOrbit( 4 ),
                                    finalStateOnHaloOrbit( 4 ), 1.0e-2 );
        BOOST_CHECK_SMALL( finalStateOnHaloOrbit( 5 ), 1.0e-3 );
    }
}

} // namespace unit_tests
} // namespace tudat


/* +------------------------------------------------------------------------+
|                     Mobile Robot Programming Toolkit (MRPT)            |
|                          https://www.mrpt.org/                         |
|                                                                        |
| Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
| See: https://www.mrpt.org/Authors - All rights reserved.               |
| Released under BSD License. See: https://www.mrpt.org/License          |
+------------------------------------------------------------------------+ */

#include "math-precomp.h"  // Precompiled headers

#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/math/CVectorDynamic.h>
#include <Eigen/Dense>

using namespace mrpt::math;

template <typename T>
CMatrixDynamic<float> CMatrixDynamic<T>::cast_float() const
{
	CMatrixDynamic<float> r(rows(), cols());
	r.asEigen() = asEigen().template cast<float>();
	return r;
}
template <typename T>
CMatrixDynamic<double> CMatrixDynamic<T>::cast_double() const
{
	CMatrixDynamic<double> r(rows(), cols());
	r.asEigen() = asEigen().template cast<double>();
	return r;
}

template <typename T>
CVectorDynamic<T> CMatrixDynamic<T>::llt_solve(const CVectorDynamic<T>& b) const
{
	auto ret = CVectorDynamic<T>(asEigen().llt().solve(b.asEigen()));
	return ret;
}
template <typename T>
CVectorDynamic<T> CMatrixDynamic<T>::lu_solve(const CVectorDynamic<T>& b) const
{
	auto ret = CVectorDynamic<T>(asEigen().lu().solve(b.asEigen()));
	return ret;
}

// Template instantiation:
#define DO_MATDYN_INSTANTIATION(T_) \
	template class mrpt::math::CMatrixDynamic<T_>;

DO_MATDYN_INSTANTIATION(float)
DO_MATDYN_INSTANTIATION(double)


#ifndef STAN_MATH_PRIM_PROB_BERNOULLI_LOGIT_RNG_HPP
#define STAN_MATH_PRIM_PROB_BERNOULLI_LOGIT_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/fun/inv_logit.hpp>
#include <stan/math/prim/err.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a Bernoulli random variate with logit-parameterized chance of success
 * using the specified random number generator.
 *
 * t can be a scalar or a one-dimensional container.
 *
 * @tparam T_t type of logit-parameterized chance of success parameter
 * @tparam RNG type of random number generator
 * @param t (Sequence of) logit-parameterized chance of success parameter(s)
 * @param rng random number generator
 * @return (Sequence of) Bernoulli random variate(s)
 * @throw std::domain_error if logit-parameterized chance of success parameter
 * is not finite
 */
template <typename T_t, class RNG>
inline typename VectorBuilder<true, int, T_t>::type bernoulli_logit_rng(
    const T_t& t, RNG& rng) {
  using boost::bernoulli_distribution;
  using boost::variate_generator;

  check_finite("bernoulli_logit_rng", "Logit transformed probability parameter",
               t);

  scalar_seq_view<T_t> t_vec(t);
  size_t N = size(t);
  VectorBuilder<true, int, T_t> output(N);

  for (size_t n = 0; n < N; ++n) {
    variate_generator<RNG&, bernoulli_distribution<> > bernoulli_rng(
        rng, bernoulli_distribution<>(inv_logit(t_vec[n])));
    output[n] = bernoulli_rng();
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif