//
// Copyright (c) Microsoft. All rights reserved.
// Licensed under the MIT license. See LICENSE.md file in the project root for full license information.
//

#include "stdafx.h"
#include <algorithm>
#include <unordered_map>
#include <random>
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/normal_distribution.hpp>
#include "ImageTransformers.h"
#include "Config.h"
#include "ConcStack.h"
#include "StringUtil.h"
#include "SequenceData.h"
#include "ImageUtil.h"
#include "ImageDeserializerBase.h"

namespace Microsoft { namespace MSR { namespace CNTK
{

// Transforms a single sequence as open cv dense image. Called once per sequence.
SequenceDataPtr ImageTransformerBase::Transform(SequenceDataPtr sequence)
{
    auto inputSequence = dynamic_cast<ImageSequenceData*>(sequence.get());
    if (inputSequence == nullptr)
        RuntimeError("Unexpected sequence provided");

    auto result = std::make_shared<ImageSequenceData>();
    Apply(inputSequence->m_copyIndex, inputSequence->m_image);

    result->m_image = inputSequence->m_image;
    result->m_numberOfSamples = inputSequence->m_numberOfSamples;
    result->m_elementType = GetElementTypeFromOpenCVType(inputSequence->m_image.depth());
    result->m_copyIndex = inputSequence->m_copyIndex;
    result->m_key = inputSequence->m_key;

    ImageDimensions outputDimensions(inputSequence->m_image.cols, inputSequence->m_image.rows, inputSequence->m_image.channels());
    result->m_sampleLayout = std::make_shared<TensorShape>(outputDimensions.AsTensorShape(HWC));
    return result;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
CropTransformer::CropTransformer(const ConfigParameters& config) : ImageTransformerBase(config)
{
    intargvector cropSize = config(L"cropSize", "0");
    m_cropWidth = cropSize[0];
    m_cropHeight = cropSize[1];
    if (m_cropWidth < 0 || m_cropHeight < 0)
    {
        RuntimeError("Invalid cropSize value, must be >= 0");
    }

    m_useSideRatio = true;
    floatargvector sideRatio = config(L"sideRatio", "0.0");
    m_sideRatioMin = sideRatio[0];
    m_sideRatioMax = sideRatio[1];
    if (m_sideRatioMin == 0.0 && m_sideRatioMax == 0.0) // taking default value means not specified
    {
        m_useSideRatio = false;
    }
    else if (!(m_sideRatioMin > 0 && m_sideRatioMax <= 1.0) ||
        m_sideRatioMin > m_sideRatioMax)
    {
        RuntimeError("Invalid sideRatio value, must be > 0 and <= 1. sideMin must <= sideMax");
    }

    m_useAreaRatio = true;
    floatargvector areaRatio = config(L"areaRatio", "0.0");
    m_areaRatioMin = areaRatio[0];
    m_areaRatioMax = areaRatio[1];
    if (m_areaRatioMin == 0.0 && m_areaRatioMax == 0.0) // taking default value means not specified
    {
        m_useAreaRatio = false;
    }
    else if (!(m_areaRatioMin > 0 && m_areaRatioMax <= 1.0) ||
        m_areaRatioMin > m_areaRatioMax)
    {
        RuntimeError("Invalid areaRatio value, must be > 0 and <= 1. areaMin must <= areaMax");
    }

    if (m_useSideRatio && m_useAreaRatio)
        RuntimeError("sideRatio and areaRatio cannot be specified simultaneously");

    floatargvector aspectRatio = config(L"aspectRatio", "1.0");
    m_aspectRatioMin = aspectRatio[0];
    m_aspectRatioMax = aspectRatio[1];
    if (!(m_aspectRatioMin > 0 && m_aspectRatioMax <= 1.0) ||
        m_aspectRatioMin > m_aspectRatioMax)
    {
        RuntimeError("Invalid aspectRatio value, must be > 0 and <= 1. aspectMin must <= aspectMax");
    }

    m_jitterType = ParseJitterType(config(L"jitterType", ""));
    m_cropType = ImageConfigHelper::ParseCropType(config(L"cropType", ""));

    if (!config.ExistsCurrent(L"hflip"))
    {
        m_hFlip = (m_cropType == CropType::RandomSide || m_cropType == CropType::RandomArea);
    }
    else
    {
        m_hFlip = config(L"hflip");
    }

    // for MultiView10 we need to set m_hflip = false, otherwise we might not get 5 unflipped image (see CropTransformer::Apply below)
    if (m_cropType == CropType::MultiView10)
    {
        m_hFlip = false;
    }
}

void CropTransformer::StartEpoch(const EpochConfiguration &config)
{
    ImageTransformerBase::StartEpoch(config);
}

void CropTransformer::Apply(uint8_t copyId, cv::Mat &mat)
{
    auto seed = GetSeed();
    auto rng = m_rngs.pop_or_create([seed]() { return std::make_unique<std::mt19937>(seed); });
    int viewIndex = m_cropType == CropType::MultiView10 ? (int)(copyId % ImageDeserializerBase::NumMultiViewCopies) : 0;

    switch (m_cropType)
    {
    case CropType::Center:
        mat = mat(GetCropRectCenter(mat.rows, mat.cols, *rng));
        break;
    case CropType::RandomSide:
        mat = mat(GetCropRectRandomSide(mat.rows, mat.cols, *rng));
        break;
    case CropType::RandomArea:
        mat = mat(GetCropRectRandomArea(mat.rows, mat.cols, *rng));
        break;
    case CropType::MultiView10:
        mat = mat(GetCropRectMultiView10(viewIndex, mat.rows, mat.cols, *rng));
        break;
    default:
        RuntimeError("Invalid crop type.");
        break;
    }

    // for MultiView10 m_hFlip is false, hence the first 5 will be unflipped, the later 5 will be flipped
    if ((m_hFlip && boost::random::bernoulli_distribution<>()(*rng)) ||
        viewIndex >= 5)
    {
        cv::flip(mat, mat, 1);
    }

    m_rngs.push(std::move(rng));
}

CropTransformer::RatioJitterType
CropTransformer::ParseJitterType(const std::string &src)
{
    if (src.empty() || AreEqualIgnoreCase(src, "none"))
    {
        return RatioJitterType::None;
    }

    if (AreEqualIgnoreCase(src, "uniratio"))
    {
        return RatioJitterType::UniRatio;
    }

    RuntimeError("Invalid jitter type: %s.", src.c_str());
}

double CropTransformer::ApplyRatioJitter(const double minVal, const double maxVal, std::mt19937 &rng)
{
    assert(minVal > 0 && minVal <= maxVal);     // ratio should always be > 0
    switch (m_jitterType)
    {
    case RatioJitterType::None:
        return minVal;
    case RatioJitterType::UniRatio:
        if (minVal == maxVal)
            return minVal;
        else
            return UniRealT(minVal, maxVal)(rng);
    default:
        RuntimeError("Jitter type currently not implemented.");
    }
    return -1;
}

cv::Rect CropTransformer::GetCropRectCenter(int crow, int ccol, std::mt19937 &rng)
{
    assert(crow > 0);
    assert(ccol > 0);
    assert(!(m_useSideRatio && m_useAreaRatio));    // cannot be applied simultaneously

    int cropSizeX=ccol, cropSizeY=crow;
    if (m_cropWidth > 0 && m_cropHeight > 0)    // crop sizes are specified with meaningful values
    {
        cropSizeX = min(ccol, m_cropWidth);
        cropSizeY = min(crow, m_cropHeight);
        int xOff = (ccol - cropSizeX) / 2;
        int yOff = (crow - cropSizeY) / 2;
        return cv::Rect(xOff, yOff, cropSizeX, cropSizeY);
    }

    bool bFound = false;
    int nAttempt = 0;
    while (!bFound && nAttempt < 10)
    {
        if (m_useSideRatio)
        {
            double sideRatio = ApplyRatioJitter(m_sideRatioMin, m_sideRatioMax, rng);
            assert(sideRatio >= m_sideRatioMin && sideRatio <= m_sideRatioMax);
            cropSizeX = cropSizeY = (int)std::round(std::min(crow, ccol) * sideRatio);      // we always crop square shape unless aspectRatio is not 1.0
        }
        else if (m_useAreaRatio)
        {
            double areaRatio = ApplyRatioJitter(m_areaRatioMin, m_areaRatioMax, rng);
            assert(areaRatio >= m_areaRatioMin && areaRatio <= m_areaRatioMax);
            cropSizeX = cropSizeY = (int)std::round(std::sqrt(crow * ccol * areaRatio));    // we always crop square shape unless aspectRatio is not 1.0
        }

        double aspectRatio = ApplyRatioJitter(m_aspectRatioMin, m_aspectRatioMax, rng);
        assert(aspectRatio >= m_aspectRatioMin && aspectRatio <= m_aspectRatioMax);
        if (aspectRatio != 1.0)
        {
            double area = cropSizeX * cropSizeY;
            if (boost::random::bernoulli_distribution<>()(rng))
            {
                cropSizeX = (int)std::sqrt(area * aspectRatio);
                cropSizeY = (int)std::sqrt(area / aspectRatio);
            }
            else
            {
                cropSizeY = (int)std::sqrt(area * aspectRatio);
                cropSizeX = (int)std::sqrt(area / aspectRatio);
            }
        }
        if (cropSizeX <= ccol && cropSizeY <= crow)
        {
            bFound = true;
            break;
        }
        nAttempt++;
    }
    if (bFound)
    {
        int xOff = (ccol - cropSizeX) / 2;
        int yOff = (crow - cropSizeY) / 2;
        return cv::Rect(xOff, yOff, cropSizeX, cropSizeY);
    }
    else
    {   // fall back to return the whole image
        return cv::Rect(0, 0, ccol, crow);
    }
}

cv::Rect CropTransformer::GetCropRectRandomSide(int crow, int ccol, std::mt19937 &rng)
{
    assert(m_useSideRatio);
    cv::Rect rc = GetCropRectCenter(crow, ccol, rng);

    int xOff = UniIntT(0, ccol - rc.width)(rng);
    int yOff = UniIntT(0, crow - rc.height)(rng);
    return cv::Rect(xOff, yOff, rc.width, rc.height);
}

cv::Rect CropTransformer::GetCropRectRandomArea(int crow, int ccol, std::mt19937 &rng)
{
    assert(m_useAreaRatio);
    cv::Rect rc = GetCropRectCenter(crow, ccol, rng);

    int xOff = UniIntT(0, ccol - rc.width)(rng);
    int yOff = UniIntT(0, crow - rc.height)(rng);
    return cv::Rect(xOff, yOff, rc.width, rc.height);
}

cv::Rect CropTransformer::GetCropRectMultiView10(int viewIndex, int crow, int ccol, std::mt19937 &rng)
{
    assert(viewIndex >= 0);
    cv::Rect rc = GetCropRectCenter(crow, ccol, rng);
    viewIndex = viewIndex % 10;

    // 0 - 4: 4 corners + center crop. 5 - 9: same, but with a flip in CropTransformer::Apply().
    int isubView = viewIndex % 5;
    int xOff=-1, yOff=-1, cropSizeX = rc.width, cropSizeY = rc.height;
    switch (isubView)
    {
    case 0: // top-left
        xOff = 0;
        yOff = 0;
        break;
    case 1: // top-right
        xOff = ccol - cropSizeX;
        yOff = 0;
        break;
    case 2: // bottom-left
        xOff = 0;
        yOff = crow - cropSizeY;
        break;
    case 3: // bottom-right
        xOff = ccol - cropSizeX;
        yOff = crow - cropSizeY;
        break;
    case 4: // center
        xOff = (ccol - cropSizeX) / 2;
        yOff = (crow - cropSizeY) / 2;
        break;
    default: // should never happen
        assert(false);
    }

    assert(xOff >= 0 && xOff <= ccol - cropSizeX);
    assert(yOff >= 0 && yOff <= crow - cropSizeY);
    return cv::Rect(xOff, yOff, cropSizeX, cropSizeY);
}

// scaleMode = "fill" (default) - warp the image to the given target size
// scaleMode = "crop" - resize the image's shorter side to the given target size and crops the overlap
// scaleMode = "pad"  - resize the image's larger side to the given target size, center it and pad the rest
ScaleTransformer::ScaleTransformer(const ConfigParameters& config) : ImageTransformerBase(config)
{
    m_imgWidth    = config(L"width");
    m_imgHeight   = config(L"height");
    m_imgChannels = config(L"channels");

    size_t cfeat = m_imgWidth * m_imgHeight * m_imgChannels;
    if (cfeat == 0 || cfeat > SIZE_MAX / 2)
        RuntimeError("Invalid image dimensions.");

    string scaleMode = config(L"scaleMode", "fill");
    if      (scaleMode == "crop") m_scaleMode = ScaleMode::Crop;
    else if (scaleMode == "pad")  m_scaleMode = ScaleMode::Pad;
    else if (scaleMode == "fill") m_scaleMode = ScaleMode::Fill;
    else RuntimeError("Invalid scaleMode value, must be fill, crop or pad (all lower case)");

    // the pad value used for the 'pad' mode. if set to -1 then the border will be replicated.
    m_padValue = config(L"padValue", -1);
    if (m_padValue >= 0)       m_borderType = cv::BORDER_CONSTANT;
    else if (m_padValue == -1) m_borderType = cv::BORDER_REPLICATE;
    else RuntimeError("Invalid padValue value, must be -1 (replicates border) or >= 0 (constant)");

    // for old config options use case-insensitve comparison ...
    string interpolation = config(L"interpolations", "linear");
    if (AreEqualIgnoreCase(interpolation, "nearest"))      m_interp = cv::INTER_NEAREST;
    else if (AreEqualIgnoreCase(interpolation, "cubic"))   m_interp = cv::INTER_CUBIC;
    else if (AreEqualIgnoreCase(interpolation, "lanczos")) m_interp = cv::INTER_LANCZOS4;
    else if (AreEqualIgnoreCase(interpolation, "linear"))  m_interp = cv::INTER_LINEAR;
    else RuntimeError("Invalid interpolations value, must be nearest, cubic, lanczos or linear");
}

// The method describes how input stream is transformed to the output stream. Called once per applied stream.
// Scale transformer transforms the stream so that all samples are of the same size.
StreamDescription ScaleTransformer::Transform(const StreamDescription& inputStream)
{
    TransformBase::Transform(inputStream);
    m_outputStream.m_sampleLayout = std::make_shared<TensorShape>(ImageDimensions(m_imgWidth, m_imgHeight, m_imgChannels).AsTensorShape(HWC));
    return m_outputStream;
}

void ScaleTransformer::Apply(uint8_t, cv::Mat &mat)
{
    if (m_scaleMode == ScaleMode::Fill)
    { // warp the image to the given target size
        cv::resize(mat, mat, cv::Size((int)m_imgWidth, (int)m_imgHeight), 0, 0, m_interp);
    }
    else
    {
        int height = mat.rows;
        int width = mat.cols;

        // which dimension is our scaled one?
        bool scaleW;
        if (m_scaleMode == ScaleMode::Crop)
            scaleW = width < height; // in "crop" mode we resize the smaller side
        else
            scaleW = width > height; // else we resize the larger side

        size_t targetW, targetH;
        if (scaleW)
        {
            targetW = (size_t)m_imgWidth;
            targetH = (size_t)round(height * m_imgWidth / (double)width);
        }
        else
        {
            targetH = (size_t)m_imgHeight;
            targetW = (size_t)round(width * m_imgHeight / (double)height);
        }

        cv::resize(mat, mat, cv::Size((int)targetW, (int)targetH), 0, 0, m_interp);

        if (m_scaleMode == ScaleMode::Crop)
        { // crop the overlap
            size_t xOff = max((size_t)0, (targetW - m_imgWidth) / 2);
            size_t yOff = max((size_t)0, (targetH - m_imgHeight) / 2);
            mat = mat(cv::Rect((int)xOff, (int)yOff, (int)m_imgWidth, (int)m_imgHeight));
        }
        else
        { // ScaleMode::PAD --> center it and pad the rest
            size_t hdiff = max((size_t)0, (m_imgHeight - mat.rows) / 2);
            size_t wdiff = max((size_t)0, (m_imgWidth - mat.cols) / 2);

            size_t top = hdiff;
            size_t bottom = m_imgHeight - top - mat.rows;
            size_t left = wdiff;
            size_t right = m_imgWidth - left - mat.cols;
            cv::copyMakeBorder(mat, mat, (int)top, (int)bottom, (int)left, (int)right, m_borderType, m_padValue);
        }
    }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

MeanTransformer::MeanTransformer(const ConfigParameters& config) : ImageTransformerBase(config)
{
    std::wstring meanFile = config(L"meanFile", L"");
    if (meanFile.empty())
        m_meanImg.release();
    else
    {
        cv::FileStorage fs;
        fs.open(msra::strfun::utf8(meanFile).c_str(), cv::FileStorage::READ);
        if (!fs.isOpened())
            RuntimeError("Could not open file: %ls", meanFile.c_str());
        fs["MeanImg"] >> m_meanImg;
        int cchan;
        fs["Channel"] >> cchan;
        int crow;
        fs["Row"] >> crow;
        int ccol;
        fs["Col"] >> ccol;
        if (cchan * crow * ccol !=
            m_meanImg.channels() * m_meanImg.rows * m_meanImg.cols)
            RuntimeError("Invalid data in file: %ls", meanFile.c_str());
        fs.release();
        m_meanImg = m_meanImg.reshape(cchan, crow);
    }
}

void MeanTransformer::Apply(uint8_t, cv::Mat &mat)
{
    assert(m_meanImg.size() == cv::Size(0, 0) ||
           (m_meanImg.size() == mat.size() &&
           m_meanImg.channels() == mat.channels()));

    if (m_meanImg.size() == mat.size())
    {
        // If matrix has not been converted to the right type, do it now as maen requires floating point type.
        ConvertToFloatingPointIfRequired(mat);
        mat = mat - m_meanImg;
    }
    else
    {
        fprintf(stderr, "WARNING: Mean file does not match the size of the input image, will be ignored.\n"
            "Please remove mean transformation from the config.\n");
    }
}

TransposeTransformer::TransposeTransformer(const ConfigParameters& config) : TransformBase(config),
    m_floatTransform(this), m_doubleTransform(this)
{}

// The method describes how input stream is transformed to the output stream. Called once per applied stream.
// Transpose transformer expects the dense input stream with samples as HWC and outputs CHW.
StreamDescription TransposeTransformer::Transform(const StreamDescription& inputStream)
{
    m_outputStream = TransformBase::Transform(inputStream);

    // Changing from NHWC to NCHW
    m_outputStream.m_elementType = m_precision;
    if (m_inputStream.m_sampleLayout != nullptr)
    {
        ImageDimensions dimensions(*m_inputStream.m_sampleLayout, HWC);
        m_outputStream.m_sampleLayout = std::make_shared<TensorShape>(dimensions.AsTensorShape(CHW));
    }

    return m_outputStream;
}

// Transformation of the sequence.
SequenceDataPtr TransposeTransformer::Transform(SequenceDataPtr sequence)
{
    auto inputSequence = dynamic_cast<ImageSequenceData*>(sequence.get());
    if (inputSequence == nullptr)
        RuntimeError("Currently Transpose transform only works with images.");

    ElementType elementType = m_inputStream.m_elementType != ElementType::tvariant ?
        m_inputStream.m_elementType :
        sequence->m_elementType;

    switch (elementType)
    {
    case ElementType::tdouble:
        if (m_precision == ElementType::tfloat)
            return m_floatTransform.Apply<double>(inputSequence);
        if (m_precision == ElementType::tdouble)
            return m_doubleTransform.Apply<double>(inputSequence);
    case ElementType::tfloat:
        if (m_precision == ElementType::tdouble)
            return m_doubleTransform.Apply<float>(inputSequence);
        if (m_precision == ElementType::tfloat)
            return m_floatTransform.Apply<float>(inputSequence);
    case ElementType::tuchar:
        if (m_precision == ElementType::tdouble)
            return m_doubleTransform.Apply<unsigned char>(inputSequence);
        if (m_precision == ElementType::tfloat)
            return m_floatTransform.Apply<unsigned char>(inputSequence);
    default:
        RuntimeError("Unsupported type. Please apply a cast transform with 'double' or 'float' precision.");
    }
    return nullptr; // Make compiler happy
}

template <class TElementTo>
template<class TElementFrom>
SequenceDataPtr TransposeTransformer::TypedTranspose<TElementTo>::Apply(ImageSequenceData* inputSequence)
{
    TensorShapePtr shape = m_parent->m_inputStream.m_sampleLayout;
    if (shape == nullptr) // Taking the shape from the sequence.
        shape = inputSequence->m_sampleLayout;

    if (!shape)
        RuntimeError("Unknown shape of the sample in stream '%ls'.", m_parent->m_inputStream.m_name.c_str());

    assert(inputSequence->m_numberOfSamples == 1);

    size_t count = shape->GetNumElements();
    auto result = std::make_shared<DenseSequenceWithBuffer<TElementTo>>(m_memBuffers, count);
    result->m_key = inputSequence->m_key;

    ImageDimensions dimensions(*shape, ImageLayoutKind::HWC);
    size_t rowCount = dimensions.m_height * dimensions.m_width;
    size_t channelCount = dimensions.m_numChannels;

    auto dst = result->GetBuffer();

    if (channelCount == 3) // Unrolling for BGR, the most common case.
    {
        size_t nRows = inputSequence->m_image.rows;
        size_t nCols = inputSequence->m_image.cols;

        TElementTo* b = dst;
        TElementTo* g = dst + rowCount;
        TElementTo* r = dst + 2 * rowCount;

        for (size_t i = 0; i < nRows; ++i)
        {
            auto* x = inputSequence->m_image.ptr<TElementFrom>((int)i);
            for (size_t j = 0; j < nCols; ++j)
            {
                auto row = j * 3;
                *b++ = static_cast<TElementTo>(x[row]);
                *g++ = static_cast<TElementTo>(x[row + 1]);
                *r++ = static_cast<TElementTo>(x[row + 2]);
            }
        }
    }
    else
    {
        auto src = reinterpret_cast<const TElementFrom*>(inputSequence->GetDataBuffer());
        for (size_t irow = 0; irow < rowCount; irow++)
        {
            for (size_t icol = 0; icol < channelCount; icol++)
            {
                dst[icol * rowCount + irow] = static_cast<TElementTo>(src[irow * channelCount + icol]);
            }
        }
    }

    result->m_sampleLayout = m_parent->m_outputStream.m_sampleLayout != nullptr ?
        m_parent->m_outputStream.m_sampleLayout :
        std::make_shared<TensorShape>(dimensions.AsTensorShape(CHW));
    result->m_numberOfSamples = inputSequence->m_numberOfSamples;
    return result;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

IntensityTransformer::IntensityTransformer(const ConfigParameters &config) : ImageTransformerBase(config)
{
    m_stdDev = config(L"intensityStdDev", "0.0");
    std::wstring intFile = config(L"intensityFile", L"");
    if (intFile.empty())
    {
        m_eigVal.release();
        m_eigVec.release();
    }
    else
    {
        cv::FileStorage fs;
        fs.open(msra::strfun::utf8(intFile).c_str(), cv::FileStorage::READ);
        if (!fs.isOpened())
            RuntimeError("Could not open file: %ls", intFile.c_str());
        fs["EigVal"] >> m_eigVal;
        if (m_eigVal.rows != 1 || m_eigVal.cols != 3 || m_eigVal.channels() != 1)
            RuntimeError("Invalid EigVal data in file: %ls", intFile.c_str());
        fs["EigVec"] >> m_eigVec;
        if (m_eigVec.rows != 3 || m_eigVec.cols != 3 || m_eigVec.channels() != 1)
            RuntimeError("Invalid EigVec data in file: %ls", intFile.c_str());
        fs.release();
    }
}

void IntensityTransformer::StartEpoch(const EpochConfiguration &config)
{
    ImageTransformerBase::StartEpoch(config);
}

void IntensityTransformer::Apply(uint8_t, cv::Mat &mat)
{
    if (m_eigVal.empty() || m_eigVec.empty() || m_stdDev == 0.0)
        return;

    // Have to convert to float.
    int type = m_precision == ElementType::tfloat ? CV_32F : CV_64F;
    if (mat.type() != type)
        mat.convertTo(mat, type);

    if (mat.type() == CV_64FC(mat.channels()))
        Apply<double>(mat);
    else if (mat.type() == CV_32FC(mat.channels()))
        Apply<float>(mat);
    else
        RuntimeError("Unsupported type");
}

template <typename ElemType>
void IntensityTransformer::Apply(cv::Mat &mat)
{
    auto seed = GetSeed();
    auto rng = m_rngs.pop_or_create([seed]() { return std::make_unique<std::mt19937>(seed); } );

    // Using single precision as EigVal and EigVec matrices are single precision.
    boost::random::normal_distribution<float> d(0, (float)m_stdDev);
    cv::Mat alphas(1, 3, CV_32FC1);
    assert(m_eigVal.rows == 1 && m_eigVec.cols == 3);
    alphas.at<float>(0) = d(*rng) * m_eigVal.at<float>(0);
    alphas.at<float>(1) = d(*rng) * m_eigVal.at<float>(1);
    alphas.at<float>(2) = d(*rng) * m_eigVal.at<float>(2);
    m_rngs.push(std::move(rng));

    assert(m_eigVec.rows == 3 && m_eigVec.cols == 3);

    cv::Mat shifts = m_eigVec * alphas.t();

    // For multi-channel images data is in BGR format.
    size_t cdst = mat.rows * mat.cols * mat.channels();
    ElemType* pdstBase = reinterpret_cast<ElemType*>(mat.data);
    for (ElemType* pdst = pdstBase; pdst < pdstBase + cdst;)
    {
        for (int c = 0; c < mat.channels(); c++)
        {
            float shift = shifts.at<float>(mat.channels() - c - 1);
            *pdst = std::min(std::max(*pdst + shift, (ElemType)0), (ElemType)255);
            pdst++;
        }
    }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

ColorTransformer::ColorTransformer(const ConfigParameters &config) : ImageTransformerBase(config)
{
    m_brightnessRadius = config(L"brightnessRadius", "0.0");
    if (m_brightnessRadius < 0 || m_brightnessRadius > 1.0)
        InvalidArgument("brightnessRadius must be >= 0.0 and <= 1.0");

    m_contrastRadius = config(L"contrastRadius", "0.0");
    if (m_contrastRadius < 0 || m_contrastRadius > 1.0)
        InvalidArgument("contrastRadius must be >= 0.0 and <= 1.0");

    m_saturationRadius = config(L"saturationRadius", "0.0");
    if (m_saturationRadius < 0 || m_saturationRadius > 1.0)
        InvalidArgument("saturationRadius must be >= 0.0 and <= 1.0");
}

void ColorTransformer::StartEpoch(const EpochConfiguration &config)
{
    ImageTransformerBase::StartEpoch(config);
}

void ColorTransformer::Apply(uint8_t, cv::Mat &mat)
{
    if (m_brightnessRadius == 0.0 && m_contrastRadius == 0.0 && m_saturationRadius == 0.0)
        return;

    // Have to convert to float
    ConvertToFloatingPointIfRequired(mat);

    if (mat.type() == CV_64FC(mat.channels()))
        Apply<double>(mat);
    else if (mat.type() == CV_32FC(mat.channels()))
        Apply<float>(mat);
    else
        RuntimeError("Unsupported type");
}

template <typename ElemType>
void ColorTransformer::Apply(cv::Mat &mat)
{
    auto seed = GetSeed();
    auto rng = m_rngs.pop_or_create([seed]() { return std::make_unique<std::mt19937>(seed); });

    if (m_brightnessRadius > 0 || m_contrastRadius > 0)
    {
        // To change brightness and/or contrast the following standard transformation is used:
        // Xij = alpha * Xij + beta, where
        // alpha is a contrast adjustment and beta - brightness adjustment.
        ElemType beta = 0;
        if (m_brightnessRadius > 0)
        {
            UniRealT d(-m_brightnessRadius, m_brightnessRadius);
            // Compute mean value of the image.
            cv::Scalar imgMean = cv::sum(cv::sum(mat));
            // Compute beta as a fraction of the mean.
            beta = (ElemType)(d(*rng) * imgMean[0] / (mat.rows * mat.cols * mat.channels()));
        }

        ElemType alpha = 1;
        if (m_contrastRadius > 0)
        {
            UniRealT d(-m_contrastRadius, m_contrastRadius);
            alpha = (ElemType)(1 + d(*rng));
        }

        // Could potentially use mat.convertTo(mat, -1, alpha, beta)
        // but it does not do range checking for single/double precision matrix. saturate_cast won't work either.
        size_t count = mat.rows * mat.cols * mat.channels();
        ElemType* pbase = reinterpret_cast<ElemType*>(mat.data);
        for (ElemType* p = pbase; p < pbase + count; p++)
        {
            *p = std::min(std::max(*p * alpha + beta, (ElemType)0), (ElemType)255);
        }
    }

    if (m_saturationRadius > 0 && mat.channels() == 3)
    {
        UniRealT d(-m_saturationRadius, m_saturationRadius);
        double ratio = 1.0 + d(*rng);
        assert(0 <= ratio && ratio <= 2);

        auto hsv = m_hsvTemp.pop_or_create([]() { return std::make_unique<cv::Mat>(); });

        // To change saturation, we need to convert the image to HSV format first,
        // the change S channgel and convert the image back to BGR format.
        cv::cvtColor(mat, *hsv, CV_BGR2HSV);
        assert(hsv->rows == mat.rows && hsv->cols == mat.cols);
        size_t count = hsv->rows * hsv->cols * mat.channels();
        ElemType* phsvBase = reinterpret_cast<ElemType*>(hsv->data);
        for (ElemType* phsv = phsvBase; phsv < phsvBase + count; phsv += 3)
        {
            const int HsvIndex = 1;
            phsv[HsvIndex] = std::min((ElemType)(phsv[HsvIndex] * ratio), (ElemType)1);
        }
        cv::cvtColor(*hsv, mat, CV_HSV2BGR);

        m_hsvTemp.push(std::move(hsv));
    }

    m_rngs.push(std::move(rng));
}

CastTransformer::CastTransformer(const ConfigParameters& config) : TransformBase(config), m_floatTransform(this), m_doubleTransform(this)
{
}

StreamDescription CastTransformer::Transform(const StreamDescription& inputStream)
{
    m_outputStream = TransformBase::Transform(inputStream);
    m_outputStream.m_elementType = m_precision;
    return m_outputStream;
}

SequenceDataPtr CastTransformer::Transform(SequenceDataPtr sequence)
{
    if (m_inputStream.m_elementType == m_precision || sequence->m_elementType == m_precision)
    {
        // No need to do anything, exit.
        return sequence;
    }

    SequenceDataPtr result;
    ElementType inputType = m_inputStream.m_elementType != ElementType::tvariant
        ? m_inputStream.m_elementType
        : sequence->m_elementType;

    switch (m_precision)
    {
    case ElementType::tdouble:
        if (inputType == ElementType::tfloat)
            result = m_doubleTransform.Apply<float>(sequence);
        else if (inputType == ElementType::tuchar)
            result = m_doubleTransform.Apply<unsigned char>(sequence);
        else
            RuntimeError("Unsupported type. Please apply a cast transform with 'double' or 'float' precision.");
        break;
    case ElementType::tfloat:
        if (inputType == ElementType::tdouble)
            result = m_floatTransform.Apply<double>(sequence);
        if (inputType == ElementType::tuchar)
            result = m_floatTransform.Apply<unsigned char>(sequence);
        else
            RuntimeError("Unsupported type. Please apply a cast transform with 'double' or 'float' precision.");
        break;
    default:
        RuntimeError("Unsupported type. Please apply a cast transform with 'double' or 'float' precision.");
    }
    result->m_elementType = m_precision;
    return result;
}

template <class TElementTo>
template<class TElementFrom>
SequenceDataPtr CastTransformer::TypedCast<TElementTo>::Apply(SequenceDataPtr sequence)
{
    TensorShapePtr shape = m_parent->m_inputStream.m_sampleLayout;
    if (!shape) // Taking the shape from the sequence.
        shape = sequence->m_sampleLayout;

    if (!shape)
        RuntimeError("Unknown shape of the sample in stream '%ls'.", m_parent->m_inputStream.m_name.c_str());

    auto& inputSequence = static_cast<DenseSequenceData&>(*sequence);
    size_t count = shape->GetNumElements() * sequence->m_numberOfSamples;
    auto result = std::make_shared<DenseSequenceWithBuffer<TElementTo>>(m_memBuffers, count);
    result->m_key = sequence->m_key;

    auto src = reinterpret_cast<const TElementFrom*>(inputSequence.GetDataBuffer());
    auto dst = result->GetBuffer();

    for (size_t i = 0; i < count; i++)
    {
        dst[i] = static_cast<TElementTo>(src[i]);
    }

    result->m_sampleLayout = shape;
    result->m_numberOfSamples = inputSequence.m_numberOfSamples;
    return result;
}

}}}


#include "loki/search.h"
#include <cstdint>

#include <boost/property_tree/ptree.hpp>
#include <unordered_set>

#include "baldr/graphid.h"
#include "baldr/graphreader.h"
#include "baldr/location.h"
#include "baldr/pathlocation.h"
#include "baldr/tilehierarchy.h"
#include "filesystem.h"
#include "midgard/pointll.h"
#include "midgard/vector2.h"
#include "sif/nocost.h"

#include "test.h"

using namespace valhalla::midgard;
using namespace valhalla::baldr;
using namespace valhalla::loki;
namespace vs = valhalla::sif;

#include "mjolnir/directededgebuilder.h"
#include "mjolnir/graphtilebuilder.h"

namespace {

// this is what it looks like
//    b
//    |\
//  1 | \ 0
//    |  \
//  2 |   \ 7
//    |    \
//    a-3-8-d
//    |    /
//  4 |   / 9
//    |  /
//  5 | / 6
//    |/
//    c
const std::string tile_dir = "test/search_tiles";
GraphId tile_id = TileHierarchy::GetGraphId({.125, .125}, 2);
PointLL base_ll = TileHierarchy::get_tiling(tile_id.level()).Base(tile_id.tileid());
std::pair<GraphId, PointLL> b({tile_id.tileid(), tile_id.level(), 0}, {.01, .2});
std::pair<GraphId, PointLL> a({tile_id.tileid(), tile_id.level(), 1}, {.01, .1});
std::pair<GraphId, PointLL> c({tile_id.tileid(), tile_id.level(), 2}, {.01, .01});
std::pair<GraphId, PointLL> d({tile_id.tileid(), tile_id.level(), 3}, {.2, .1});

void clean_tiles() {
  if (filesystem::is_directory(tile_dir)) {
    filesystem::remove_all(tile_dir);
  }
}

void make_tile() {
  using namespace valhalla::mjolnir;
  using namespace valhalla::baldr;

  // make sure that all the old tiles are gone before trying to make new ones.
  clean_tiles();

  // basic tile information
  GraphTileBuilder tile(tile_dir, tile_id, false);
  uint32_t edge_index = 0;

  auto add_node = [&](const std::pair<GraphId, PointLL>& v, const uint32_t edge_count) {
    NodeInfo node_builder;
    node_builder.set_latlng(base_ll, v.second);
    // node_builder.set_road_class(RoadClass::kSecondary);
    node_builder.set_access(kAllAccess);
    node_builder.set_edge_count(edge_count);
    node_builder.set_edge_index(edge_index);
    node_builder.set_timezone(1);
    edge_index += edge_count;
    tile.nodes().emplace_back(node_builder);
  };

  auto add_edge = [&](const std::pair<GraphId, PointLL>& u, const std::pair<GraphId, PointLL>& v,
                      const uint32_t localedgeidx, const uint32_t opp_local_idx, const bool forward) {
    DirectedEdgeBuilder edge_builder({}, v.first, forward, u.second.Distance(v.second) + .5, 1, 1,
                                     Use::kRoad, RoadClass::kMotorway, localedgeidx, false, 0, 0,
                                     false);
    edge_builder.set_opp_index(opp_local_idx); // How is this different from opp_local_idx
    edge_builder.set_opp_local_idx(opp_local_idx);
    edge_builder.set_localedgeidx(localedgeidx);
    edge_builder.set_forwardaccess(kAllAccess);
    edge_builder.set_reverseaccess(kAllAccess);
    edge_builder.set_free_flow_speed(100);
    edge_builder.set_constrained_flow_speed(10);
    std::vector<PointLL> shape = {u.second, u.second.MidPoint(v.second), v.second};
    if (!forward) {
      std::reverse(shape.begin(), shape.end());
    }
    bool added;
    // make more complex edge geom so that there are 3 segments, affine combination doesnt properly
    // handle arcs but who cares
    uint32_t edge_info_offset = tile.AddEdgeInfo(localedgeidx, u.first, v.first, 123, // way_id
                                                 0, 0,
                                                 120, // speed limit in kph
                                                 shape, {std::to_string(localedgeidx)}, 0, added);
    // assert(added);
    edge_builder.set_edgeinfo_offset(edge_info_offset);
    tile.directededges().emplace_back(edge_builder);
  };

  // this is what it looks like
  //    b 0
  //    |\
  //  1 | \ 0
  //    |  \
  //  2 |   \ 7
  //    |    \
  //   1a-3-8-d 3
  //    |    /
  //  4 |   / 9
  //    |  /
  //  5 | / 6
  //    |/
  //    c 2

  // NOTE: edge ids are in the order the edges are added, so b->d is 0, b->a is 1, a->b is 2 and so
  // on

  // B
  {
    add_edge(b, d, 0, 0, true);
    add_edge(b, a, 1, 0, true); // 1
    add_node(b, 2);
  }

  // A
  {
    add_edge(a, b, 0, 1, false); // 2
    add_edge(a, d, 1, 1, true);  // 3
    add_edge(a, c, 2, 0, false); // 4
    add_node(a, 3);
  }

  // C
  {
    add_edge(c, a, 0, 2, true);  // 5
    add_edge(c, d, 1, 2, false); // 6
    add_node(c, 2);
  }

  // D
  {
    add_edge(d, b, 0, 0, false); // 7
    add_edge(d, a, 1, 1, false); // 8
    add_edge(d, c, 2, 1, true);  // 9
    add_node(d, 3);
  }

  // write the tile
  tile.StoreTileData();

  // write the bin data
  GraphTileBuilder::tweeners_t tweeners;
  GraphTile reloaded(tile_dir, tile_id);
  auto bins = GraphTileBuilder::BinEdges(&reloaded, tweeners);
  GraphTileBuilder::AddBins(tile_dir, &reloaded, bins);

  {
    // Verify tiles
    boost::property_tree::ptree conf;
    conf.put("tile_dir", tile_dir);
    valhalla::baldr::GraphReader reader(conf);
    auto tile = reader.GetGraphTile(tile_id);
    ASSERT_EQ(tile->header()->directededgecount(), 10);
  }
}

std::shared_ptr<vs::DynamicCost> create_costing() {
  valhalla::Options options;
  options.set_costing(valhalla::Costing::none_);
  return vs::CreateNoCost(valhalla::Costing::none_, options);
}

void search(valhalla::baldr::Location location,
            bool expected_node,
            const valhalla::midgard::PointLL& expected_point,
            const std::vector<PathLocation::PathEdge>& expected_edges,
            bool exact = false) {
  // make the config file
  boost::property_tree::ptree conf;
  conf.put("tile_dir", tile_dir);
  valhalla::baldr::GraphReader reader(conf);

  // send it to pbf and back just in case something is wrong with that conversion
  valhalla::Location pbf;
  PathLocation::toPBF(location, &pbf, reader);
  location = PathLocation::fromPBF(pbf);
  location.street_side_max_distance_ = 5000;

  const auto costing = create_costing();
  const auto results = Search({location}, reader, costing);
  const auto& p = results.at(location);

  ASSERT_EQ((p.edges.front().begin_node() || p.edges.front().end_node()), expected_node)
      << p.edges.front().begin_node() << ":" << p.edges.front().end_node()
      << (expected_node ? " Should've snapped to node" : " Shouldn't've snapped to node");

  ASSERT_TRUE(p.edges.size()) << "Didn't find any node/edges";
  ASSERT_TRUE(p.edges.front().projected.ApproximatelyEqual(expected_point)) << "Found wrong point";

  valhalla::baldr::PathLocation answer(location);
  for (const auto& expected_edge : expected_edges) {
    answer.edges.emplace_back(
        PathLocation::PathEdge{expected_edge.id, expected_edge.percent_along, expected_point,
                               expected_point.Distance(location.latlng_), expected_edge.sos});
  }
  // note that this just checks that p has the edges that answer has
  // p can have more edges than answer has and that wont fail this check!
  ASSERT_TRUE(answer.shares_edges(p)) << "Did not find expected edges";
  // if you want to enforce that the result didnt have more then expected
  if (exact) {
    ASSERT_EQ(answer.edges.size(), p.edges.size()) << "Got more edges than expected";
  }
}

void search(valhalla::baldr::Location location, size_t result_count, int reachability) {
  // make the config file
  boost::property_tree::ptree conf;
  conf.put("tile_dir", tile_dir);
  valhalla::baldr::GraphReader reader(conf);

  // send it to pbf and back just in case something is wrong with that conversion
  valhalla::Location pbf;
  PathLocation::toPBF(location, &pbf, reader);
  location = PathLocation::fromPBF(pbf);
  location.street_side_max_distance_ = 5000;

  const auto costing = create_costing();

  const auto results = Search({location}, reader, costing);
  if (results.empty() && result_count == 0)
    return;

  const auto& path = results.at(location);

  ASSERT_EQ(path.edges.size(), result_count) << "Wrong number of edges";
  for (const auto& edge : path.edges) {
    ASSERT_GE(edge.outbound_reach, reachability);
    ASSERT_GE(edge.inbound_reach, reachability);
  }
}

TEST(Search, test_edge_search) {
  auto t = a.first.tileid();
  auto l = a.first.level();
  using S = PathLocation::SideOfStreet;
  using PE = PathLocation::PathEdge;
  using ST = Location::StopType;
  using PS = Location::PreferredSide;

  // snap to node searches
  search({a.second}, true, a.second,
         {PE{{t, l, 2}, 0, a.second, 0, S::NONE}, PE{{t, l, 3}, 0, a.second, 0, S::NONE},
          PE{{t, l, 4}, 0, a.second, 0, S::NONE}});
  search({d.second}, true, d.second,
         {PE{{t, l, 7}, 0, d.second, 0, S::NONE}, PE{{t, l, 8}, 0, d.second, 0, S::NONE},
          PE{{t, l, 9}, 0, d.second, 0, S::NONE}});
  // snap to end of edge degenerates to node searches
  search({{d.second.first + .049, d.second.second}}, true, d.second,
         {
             PE{{t, l, 7}, 0, d.second, 0, S::NONE}, PE{{t, l, 8}, 0, d.second, 0, S::NONE},
             PE{{t, l, 9}, 0, d.second, 0, S::NONE}, // leaving edges
             PE{{t, l, 0}, 1, d.second, 0, S::NONE}, PE{{t, l, 3}, 1, d.second, 0, S::NONE},
             PE{{t, l, 6}, 1, d.second, 0, S::NONE} // arriving edges
         });

  // regression test for #2023, displace the point beyond search_cutoff but within node_snap_tolerance
  Location near_node{a.second};
  near_node.latlng_.second += 0.00001; // 1.11 meters
  near_node.search_cutoff_ = 1.0;
  near_node.node_snap_tolerance_ = 2.0;
  search(near_node, true, a.second,
         {PE{{t, l, 2}, 0, a.second, 0, S::NONE}, PE{{t, l, 3}, 0, a.second, 0, S::NONE},
          PE{{t, l, 4}, 0, a.second, 0, S::NONE}, PE{{t, l, 1}, 1, a.second, 0, S::NONE},
          PE{{t, l, 8}, 1, a.second, 0, S::NONE}, PE{{t, l, 5}, 1, a.second, 0, S::NONE}},
         true);

  // snap to node as through location should be all edges
  Location x{a.second, Location::StopType::THROUGH};
  search(x, true, a.second,
         {PE{{t, l, 2}, 0, a.second, 0, S::NONE}, PE{{t, l, 3}, 0, a.second, 0, S::NONE},
          PE{{t, l, 4}, 0, a.second, 0, S::NONE}, PE{{t, l, 1}, 1, a.second, 0, S::NONE},
          PE{{t, l, 8}, 1, a.second, 0, S::NONE}, PE{{t, l, 5}, 1, a.second, 0, S::NONE}},
         true);
  // with a heading should just be a single outgoing
  x.heading_ = 180;
  search(x, true, a.second, {PE{{t, l, 4}, 0, a.second, 0, S::NONE}}, true);

  // mid point search
  auto answer = a.second.MidPoint(d.second);
  search({a.second.MidPoint(d.second)}, false, a.second.MidPoint(d.second),
         {PE{{t, l, 3}, .5f, answer, 0, S::NONE}, PE{{t, l, 8}, .5f, answer, 0, S::NONE}});

  // set a point 40% along the edge runs with the shape direction
  answer = a.second.AffineCombination(.6f, .4f, d.second);
  auto ratio = a.second.Distance(answer) / a.second.Distance(d.second);
  x = {answer};
  search(x, false, answer,
         {PE{{t, l, 3}, ratio, answer, 0, S::NONE}, PE{{t, l, 8}, 1.f - ratio, answer, 0, S::NONE}});

  // with heading
  x.heading_ = 90;
  search(x, false, answer, {PE{{t, l, 3}, ratio, answer, 0, S::NONE}});
  x.heading_ = 0;
  search(x, false, answer,
         {PE{{t, l, 3}, ratio, answer, 0, S::NONE}, PE{{t, l, 8}, 1.f - ratio, answer, 0, S::NONE}});
  x.heading_ = 269;
  search(x, false, answer, {PE{{t, l, 8}, 1.f - ratio, answer, 0, S::NONE}});

  // check for side of street by offsetting the test point from the line orthogonally
  auto ortho = (d.second - a.second).GetPerpendicular(true).Normalize() * .01;
  PointLL test{answer.first + ortho.x(), answer.second + ortho.y()};
  search({test}, false, answer,
         {PE{{t, l, 3}, ratio, answer, 0, S::RIGHT}, PE{{t, l, 8}, 1.f - ratio, answer, 0, S::LEFT}});

  // try getting side of street by setting the display ll instead
  x = {answer};
  x.display_latlng_ = test;
  search(x, false, answer,
         {PE{{t, l, 3}, ratio, answer, 0, S::RIGHT}, PE{{t, l, 8}, 1.f - ratio, answer, 0, S::LEFT}});

  // try display ll on the other side
  x.display_latlng_ = PointLL{answer.first - ortho.x(), answer.second - ortho.y()};
  search(x, false, answer,
         {PE{{t, l, 3}, ratio, answer, 0, S::LEFT}, PE{{t, l, 8}, 1.f - ratio, answer, 0, S::RIGHT}});

  // try nuking display ll by setting it farther away than street_side_max_distance
  x.display_latlng_ = PointLL{answer.first - 5 * ortho.x(), answer.second - 5 * ortho.y()};
  search(x, false, answer,
         {PE{{t, l, 3}, ratio, answer, 0, S::NONE}, PE{{t, l, 8}, 1.f - ratio, answer, 0, S::NONE}});

  // check that the side of street tolerance works
  Location sst_huge(test);
  sst_huge.street_side_tolerance_ = 2000;
  search(sst_huge, false, answer,
         {PE{{t, l, 3}, ratio, answer, 0, S::NONE}, PE{{t, l, 8}, 1.f - ratio, answer, 0, S::NONE}});

  // we only want opposite driving side, tiles are left hand driving
  search({test, ST::BREAK, 0, 0, 0, PS::OPPOSITE}, false, answer,
         {PE{{t, l, 3}, ratio, answer, 0, S::RIGHT}}, true);

  // we only want same driving side, tiles are left hand driving
  search({test, ST::BREAK, 0, 0, 0, PS::SAME}, false, answer,
         {PE{{t, l, 8}, 1.f - ratio, answer, 0, S::LEFT}}, true);

  // set a point 40% along the edge that runs in reverse of the shape
  answer = b.second.AffineCombination(.6f, .4f, d.second);
  ratio = b.second.Distance(answer) / b.second.Distance(d.second);
  search({answer}, false, answer,
         {PE{{t, l, 0}, ratio, answer, 0, S::NONE}, PE{{t, l, 7}, 1.f - ratio, answer, 0, S::NONE}});

  // check for side of street by offsetting the test point from the line orthogonally
  ortho = (d.second - b.second).GetPerpendicular(false).Normalize() * .01;
  test.Set(answer.first + ortho.x(), answer.second + ortho.y());
  search({test}, false, answer,
         {PE{{t, l, 0}, ratio, answer, 0, S::LEFT}, PE{{t, l, 7}, 1.f - ratio, answer, 0, S::RIGHT}});

  // check that the side of street tolerance works
  sst_huge.latlng_ = test;
  search(sst_huge, false, answer,
         {PE{{t, l, 0}, ratio, answer, 0, S::NONE}, PE{{t, l, 7}, 1.f - ratio, answer, 0, S::NONE}});

  // we only want opposite driving side, tiles are left hand driving
  search({test, ST::BREAK, 0, 0, 0, PS::OPPOSITE}, false, answer,
         {PE{{t, l, 7}, 1.f - ratio, answer, 0, S::RIGHT}}, true);

  // we only want same driving side, tiles are left hand driving
  search({test, ST::BREAK, 0, 0, 0, PS::SAME}, false, answer,
         {PE{{t, l, 0}, ratio, answer, 0, S::LEFT}}, true);

  // TODO: add more tests
}

TEST(Search, test_reachability_radius_zero_everything) {
  PointLL ob(b.second.first - .001f, b.second.second - .01f);

  LOGLN_WARN("zero everything should be a single closest result");
  search({ob, Location::StopType::BREAK, 0, 0, 0}, 2, 0);
}

TEST(Search, test_reachability_radius_high) {
  PointLL ob(b.second.first - .001f, b.second.second - .01f);
  unsigned int longest = ob.Distance(d.second);

  LOGLN_WARN("set radius high to get them all");
  search({b.second, Location::StopType::BREAK, 0, 0, longest + 100}, 10, 0);
}

TEST(Search, test_reachability_radius_mid) {
  PointLL ob(b.second.first - .001f, b.second.second - .01f);
  unsigned int shortest = ob.Distance(a.second);

  LOGLN_WARN("set radius mid to get just some");
  search({b.second, Location::StopType::BREAK, 0, 0, shortest - 100}, 4, 0);
}

TEST(Search, test_reachability_radius_reachability_high) {
  PointLL ob(b.second.first - .001f, b.second.second - .001f);

  LOGLN_WARN("set reachability high to see it gets all edges reachable");
  search({ob, Location::StopType::BREAK, 10, 10, 0}, 2, 4);
}

TEST(Search, test_reachability_radius_off_by_one) {
  PointLL ob(b.second.first - .001f, b.second.second - .01f);

  LOGLN_WARN("set reachability right on to see we arent off by one");
  search({ob, Location::StopType::BREAK, 4, 4, 0}, 2, 4);
}

TEST(Search, test_reachability_radius_give_up_early) {
  PointLL ob(b.second.first - .001f, b.second.second - .01f);

  LOGLN_WARN("set reachability lower to see we give up early");
  search({ob, Location::StopType::BREAK, 3, 3, 0}, 2, 3);
}

TEST(Search, test_search_cutoff) {
  // test default cutoff of 35km showing no results
  search({{-77, -77}}, 0, 0);

  // test limits of cut off
  auto t = c.second;
  t.first -= 1;
  t.second -= 1;
  auto dist = t.Distance(c.second);

  // set the cut off just too small and get nothing
  Location x(t);
  x.search_cutoff_ = dist - 1;
  search(x, 0, 0);

  // set the cut off just big enough and get one node snap result with 4 edges at it
  x.search_cutoff_ = dist + 1;
  search(x, 4, 0);

  // lets try an edge snap
  t = c.second;
  t.first -= .00005;
  t.second += .00005;
  dist = t.Distance({0.01, 0.01005});

  // just out of reach
  x.latlng_ = t;
  x.search_cutoff_ = dist - 1;
  search(x, 0, 0);

  // just within reach
  x.search_cutoff_ = dist + 1;
  search(x, 2, 0);
}

} // namespace

// Setup and tearown will be called only once for the entire suite121
class SearchTestSuiteEnv : public ::testing::Environment {
public:
  void SetUp() override {
    make_tile();
  }
  void TearDown() override {
    // todo: think whether we want to clean tile dir after the test
  }
};

int main(int argc, char* argv[]) {
  testing::AddGlobalTestEnvironment(new SearchTestSuiteEnv);
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#ifndef EFANNA_BASE_INDEX_H_
#define EFANNA_BASE_INDEX_H_

#include "general/params.hpp"
#include "general/distance.hpp"
#include "general/matrix.hpp"
#include <boost/dynamic_bitset.hpp>
#include <fstream>
#include <iostream>
#include <omp.h>
#include <unordered_set>
#include <mutex>
#include "boost/smart_ptr/detail/spinlock.hpp"
#include <memory>

//#define BATCH_SIZE 200

namespace efanna{

typedef boost::detail::spinlock Lock;
typedef std::lock_guard<Lock> LockGuard;
struct Point {
	unsigned id;
	float dist;
	bool flag;
	Point () {}
	Point (unsigned i, float d, bool f = true): id(i), dist(d), flag(f) {
	}
	bool operator < (const Point &n) const{
		return this->dist < n.dist;
	}
};

typedef std::vector<Point>  Points;
static inline unsigned InsertIntoKnn (Point *addr, unsigned K, Point nn) {
	// find the location to insert
	unsigned j;
	unsigned i = K;
	while (i > 0) {
		j = i - 1;
		if (addr[j].dist <= nn.dist) break;
		i = j;
	}
	// check for equal ID
	unsigned l = i;
	while (l > 0) {
		j = l - 1;
		if (addr[j].dist < nn.dist) break;
		if (addr[j].id == nn.id) return K + 1;
		l = j;
	}
	// i <= K-1
	j = K;
	while (j > i) {
		addr[j] = addr[j-1];
		--j;
	}
	addr[i] = nn;
	return i;
}
struct Neighbor {
	std::shared_ptr<Lock> lock;
	float radius;
	float radiusM;
	Points pool;
	unsigned L;
	unsigned Range;
	bool found;
	std::vector<unsigned> nn_old;
	std::vector<unsigned> nn_new;
	std::vector<unsigned> rnn_old;
	std::vector<unsigned> rnn_new;

	Neighbor() : lock(std::make_shared<Lock>())
	{
	}

	unsigned insert (unsigned id, float dist) {
		if (dist > radius) return pool.size();
		LockGuard guard(*lock);
		unsigned l = InsertIntoKnn(&pool[0], L, Point(id, dist, true));
		if (l <= L) {
			if (L + 1 < pool.size()) {
				++L;
			}
			else {
				radius = pool[L-1].dist;
			}
		}
		return l;
	}

	template <typename C>
	void join (C callback) const {
		for (unsigned const i: nn_new) {
			for (unsigned const j: nn_new) {
				if (i < j) {
					callback(i, j);
				}
			}
			for (unsigned j: nn_old) {
				callback(i, j);
			}
		}
	}

};

template <typename DataType>
class InitIndex{
public:
	InitIndex(const Matrix<DataType>& features, const Distance<DataType>* d, const IndexParams& params):
		features_(features),
		distance_(d),
		params_(params)
{
}
	virtual ~InitIndex() {};
	virtual void buildTrees(){}

	virtual void buildIndex()
	{
		buildIndexImpl();
	}
	virtual void buildIndexImpl() = 0;
	virtual void loadGraph(char* filename) = 0;
	virtual void outputVisitBucketNum() = 0;

	void saveResults(char* filename){
		std::ofstream out(filename,std::ios::binary);
		std::vector<std::vector<int>>::iterator i;
		//std::cout<<nn_results.size()<<std::endl;
		for(i = nn_results.begin(); i!= nn_results.end(); i++){
			std::vector<int>::iterator j;
			int dim = i->size();
			//std::cout<<dim<<std::endl;
			out.write((char*)&dim, sizeof(int));
			for(j = i->begin(); j != i->end(); j++){
				int id = *j;
				out.write((char*)&id, sizeof(int));
			}
		}
		out.close();
	}
	SearchParams SP;
	void setSearchParams(int epochs, int init_num, int extend_to, int search_method){
		SP.search_epoches = epochs;
		SP.search_init_num = init_num;
		if(extend_to>init_num) SP.extend_to = init_num;
		else  SP.extend_to = extend_to;
		SP.search_method = search_method;
	}

	void nnExpansion_kgraph(size_t K, const DataType* qNow, std::vector<std::pair<float,size_t>>& pool, std::vector<int>& res, bool bSorted){
		unsigned int base_n = features_.get_rows();
		boost::dynamic_bitset<> tbflag(base_n, false);

		std::vector<Point> knn(K + SP.extend_to +1);
		std::vector<Point> results;

		for (unsigned iter = 0; iter < (unsigned)SP.search_epoches; iter++) {

			unsigned L = 0;
			for(unsigned j=0; j < (unsigned)SP.extend_to ; j++){
				if(!tbflag.test(pool[iter*SP.extend_to+j].second)){
					tbflag.set(pool[iter*SP.extend_to+j].second);
					knn[L].id = pool[iter*SP.extend_to+j].second;
					knn[L].dist = pool[iter*SP.extend_to+j].first;
					knn[L].flag = true;
					L++;
				}
			}
			if(~bSorted){
				std::sort(knn.begin(), knn.begin() + L);
			}

			unsigned int k =  0;
			while (k < L) {
				unsigned int nk = L;
				if (knn[k].flag) {
					knn[k].flag = false;
					unsigned n = knn[k].id;

					for(unsigned neighbor=0; neighbor < gs[n].size(); neighbor++){
						unsigned id = gs[n][neighbor];

						if(tbflag.test(id))continue;
						tbflag.set(id);

						float dist = distance_->compare(qNow, features_.get_row(id), features_.get_cols());
						Point nn(id, dist);
						unsigned int r = InsertIntoKnn(&knn[0], L, nn);

						//if ( (r <= L) && (L + 1 < knn.size())) ++L;
						if ( L + 1 < knn.size()) ++L;
						if (r < nk) nk = r;
					}
				}
				if (nk <= k) k = nk;
				else ++k;
			}

			if (L > K) L = K;
			if (results.empty()) {
				results.reserve(K + 1);
				results.resize(L + 1);
				std::copy(knn.begin(), knn.begin() + L, results.begin());
			}
			else {
				for (unsigned int l = 0; l < L; ++l) {
					unsigned r = InsertIntoKnn(&results[0], results.size() - 1, knn[l]);
					if (r < results.size() /* inserted */ && results.size() < (K + 1)) {
						results.resize(results.size() + 1);
					}
				}
			}
		}

		for(size_t i = 0; i < K && i < results.size();i++)
			res.push_back(results[i].id);
	}

	void nnExpansion(size_t K, const DataType* qNow, std::vector<int>& pool, boost::dynamic_bitset<>& tbflag){

		int resultSize = SP.extend_to;
		if (K > (unsigned)SP.extend_to)
			resultSize = K;

		unsigned int base_n = features_.get_rows();
		boost::dynamic_bitset<> newflag(base_n, true);
		newflag.set();

		std::vector<std::pair<float,size_t>> result;
		for(unsigned int i=0; i<pool.size();i++){
			result.push_back(std::make_pair(distance_->compare(qNow, features_.get_row(pool[i]), features_.get_cols()),pool[i]));
		}
		std::partial_sort(result.begin(), result.begin() + resultSize, result.end());
		result.resize(resultSize);
		pool.clear();
		for(int j = 0; j < resultSize; j++)
			pool.push_back(result[j].second);

		int iter=0;
		std::vector<int> ids;
		while(iter++ < SP.search_epoches){
			//the heap is max heap
			ids.clear();
			for(unsigned j = 0; j < SP.extend_to ; j++){
				if(newflag.test( pool[j] )){
					newflag.reset(pool[j]);

					for(unsigned neighbor=0; neighbor < gs[pool[j]].size(); neighbor++){
						unsigned id = gs[pool[j]][neighbor];

						if(tbflag.test(id))continue;
						else tbflag.set(id);

						ids.push_back(id);
					}
				}
			}

			for(size_t j = 0; j < ids.size(); j++){
				result.push_back(std::make_pair(distance_->compare(qNow, features_.get_row(ids[j]), features_.get_cols()),ids[j]));
			}
			std::partial_sort(result.begin(), result.begin() + resultSize, result.end());
			result.resize(resultSize);
			pool.clear();
			for(int j = 0; j < resultSize; j++)
				pool.push_back(result[j].second);
		}

		if(K<(unsigned)SP.extend_to)
			pool.resize(K);

	}

	virtual void knnSearch(int K, const Matrix<DataType>& query){
		getNeighbors(K,query);
	}

	virtual void getNeighbors(size_t K, const Matrix<DataType>& query) = 0;

	virtual void locatPoints(const Matrix<DataType>& query){
		locateNeighbors(query);
	}

	virtual void locateNeighbors(const Matrix<DataType>& query) = 0;

protected:
	const Matrix<DataType> features_;
	const Distance<DataType>* distance_;
	const IndexParams params_;
	std::vector<std::vector<unsigned> > gs;
	std::vector<std::vector<int> > nn_results;
};
#define USING_BASECLASS_SYMBOLS \
		using InitIndex<DataType>::distance_;\
		using InitIndex<DataType>::params_;\
		using InitIndex<DataType>::features_;\
		using InitIndex<DataType>::buildIndex;\
		using InitIndex<DataType>::nn_results;\
		using InitIndex<DataType>::saveResults;\
		using InitIndex<DataType>::SP;\
		using InitIndex<DataType>::nnExpansion;\
		using InitIndex<DataType>::nnExpansion_kgraph;\
		using InitIndex<DataType>::gs;\

}
#endif


#ifndef RACEINFO_INCLUDE
#define RACEINFO_INCLUDE

#include <iostream>
#include <utility>
#include <list>

//-- from player.hpp --//
#include <cstdio>
#include <memory>
#include <thread>
#include <istream>
#include <ostream>
#include <boost/process.hpp>
#include <boost/optional.hpp>
//---------------------

#include <cstdint>
#include <cinttypes>

#if defined(__unix__) || defined(__linux__)
#include <fcntl.h>
#include <cmath>
#endif

//デバッグ用．ステップの数を少なくして確かめる時につかう
#define DEBUG_STEPLIMIT 5

using namespace std;
using namespace rel_ops;

struct IntVec {
  int x, y;
  IntVec operator+(IntVec &another);
  bool operator==(const IntVec &another) const;
  bool operator<(const IntVec &another) const;
  IntVec(int x = 0, int y = 0): x(x), y(y) {};
};

typedef IntVec Position;
typedef IntVec Velocity;
typedef IntVec Acceleration;

struct RaceCourse {
  uint64_t thinkTime;
  int stepLimit;
  int width, length;
  int vision;
};

extern RaceCourse course;

struct Movement {
  Position from, to;
  bool goesOff(RaceCourse &course);
  list <Position> touchedSquares() const;
  Movement() {};
  Movement(Position from, Position to): from(from), to(to) {};
};

//-- from player.hpp
enum PlayerStateCategory {
  RACING, ALREADY_FINISHED, ALREADY_DISQUALIFIED
};

struct PlayerState {
  PlayerStateCategory state;
  Position position;
  Velocity velocity;
  int64_t timeLeft;
  PlayerState(PlayerStateCategory s, Position p, Velocity v,
	      int64_t timeLeft):
    state(s), position(p), velocity(v), timeLeft(timeLeft) {};
  PlayerState() {};
  PlayerState(Position p, Velocity v):
    position(p), velocity(v) {};
};
//-------------------

struct RaceInfo {
  int stepNumber;
  uint64_t timeLeft;
  PlayerState me, opponent;
  char **squares;

};

//---- from player.hpp

struct Option {
  std::shared_ptr<std::ofstream> stdinLogStream;
  std::shared_ptr<std::ofstream> stderrLogStream;
  boost::optional<std::vector<std::string>> pauseCommand;
  boost::optional<std::vector<std::string>> resumeCommand;
};

class Logger {
private:
  thread thrd;
  future<void> ftr;
public:
  shared_ptr<mutex> mtx;
  Logger(unique_ptr<istream> input, shared_ptr<ostream> output, int MAX_SIZE);
  ~Logger();
};

enum ResultCategory {
  NORMAL, FINISHED,		// Normal run
  GONEOFF, OBSTACLED, COLLIDED,	// Ran but with problem
  NOPLAY,			// Already stopped running
  TIMEDOUT, DIED, INVALID	// Problem in response
};

struct Player {
  // Player AI
  string name;
  boost::process::group group;
  PlayerState state;
  Option option;

  unique_ptr<boost::process::child> child;
  unique_ptr<boost::process::opstream> toAI;
  unique_ptr<boost::process::ipstream> fromAI;
  unique_ptr<Logger> stderrLogger;
  // Player state during a race

  Player() {};
  // Player(string command, string name, const RaceCourse &course, int xpos, const Option &opt);
  Player(string command, string name, const RaceCourse &course, Position &mypos, Velocity &myvel, const Option &opt);
  bool playOneStep (int stepNumber, RaceInfo &info, RaceCourse &course);
  ResultCategory plan (int stepNumber, RaceInfo &info, RaceCourse &course, Acceleration &accel, int64_t &timeUsed);
  // void terminate();
};
//---------

istream &operator>>(istream &in, RaceCourse &course);
istream &operator>>(istream &in, PlayerState &ps);
istream &operator>>(istream &in, RaceInfo &ri);

#endif


#line 903 "../../../../libs/parameter/doc/index.rst"
#include <boost/parameter.hpp>

BOOST_PARAMETER_NAME((_graph, graphs) graph)
BOOST_PARAMETER_NAME((_visitor, graphs) visitor)
BOOST_PARAMETER_NAME((_root_vertex, graphs) root_vertex)
BOOST_PARAMETER_NAME((_index_map, graphs) index_map)
BOOST_PARAMETER_NAME((_color_map, graphs) color_map)

using boost::mpl::_;

namespace boost
{
  struct incidence_graph_tag {};
  struct vertex_list_graph_tag {};

  int vertex_index = 0;

  template <class T>
  struct graph_traits
  {
      typedef int traversal_category;
      typedef int vertex_descriptor;
  };

  template <class T>
  struct property_traits
  {
      typedef int value_type;
      typedef int key_type;
  };

  template <class T = int>
  struct dfs_visitor
  {};
}
#line 859 "../../../../libs/parameter/doc/index.rst"
BOOST_PARAMETER_FUNCTION(
    (void), depth_first_search, graphs

  , (required
      (graph
       , *(boost::mpl::and_<
               boost::is_convertible<
                   boost::graph_traits<_>::traversal_category
                 , boost::incidence_graph_tag
               >
             , boost::is_convertible<
                   boost::graph_traits<_>::traversal_category
                 , boost::vertex_list_graph_tag
               >
           >) ))

    (optional
      (visitor, *, boost::dfs_visitor<>()) // not checkable

      (root_vertex
        , (typename boost::graph_traits<graphs::graph::_>::vertex_descriptor)
        , *vertices(graph).first)

      (index_map
        , *(boost::mpl::and_<
              boost::is_integral<
                  boost::property_traits<_>::value_type
              >
            , boost::is_same<
                  typename boost::graph_traits<graphs::graph::_>::vertex_descriptor
                , boost::property_traits<_>::key_type
              >
          >)
        , get(boost::vertex_index,graph))

      (in_out(color_map)
        , *(boost::is_same<
              typename boost::graph_traits<graphs::graph::_>::vertex_descriptor
            , boost::property_traits<_>::key_type
          >)
       , default_color_map(num_vertices(graph), index_map) )
    )
)
#line 940 "../../../../libs/parameter/doc/index.rst"
{}


#include <boost/numeric/odeint/external/nt2/nt2_norm_inf.hpp>


#include "walkandprint.h"
#include "wdict.h"
#include "common.h"
#include "globalVars.h"
#include "kbGraph.h"
#include <string>
#include <iostream>
#include <fstream>

// Random

#include <boost/random/random_device.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/discrete_distribution.hpp>

namespace ukb {

	using namespace std;
	using namespace boost;

	template<class It>
	int rnumber_distribution(It beg, It end) {
		boost::random::discrete_distribution<> dist(beg, end);
		return dist(glVars::rnd::urng);
	}

	// returns a random floating-point value uniformly distributed in the range [0..1)
	static float rnumber_01() {
		boost::random::uniform_01<> dist;
		return dist(glVars::rnd::urng);
	}

	// generate between 0 and b, both inclusive
	static int rnumber(int b) {
		boost::random::uniform_int_distribution<> dist(0, b);
		return dist(glVars::rnd::urng);
	}

	// generate between 0 and b, both inclusive
	static size_t rnumber(size_t b) {
		boost::random::uniform_int_distribution<> dist(0, b);
		return dist(glVars::rnd::urng);
	}

	static float rnumber(float b) {
		boost::random::uniform_real_distribution<> dist(0.0f, b);
		return dist(glVars::rnd::urng);
	}

	///////////////////////////////////////////////
	// vsampling stuff

	struct vsampling_t::sort_t {

		const vector<float> & m_v ;
		sort_t(const vector<float> & score) : m_v(score) {}
		int operator()(const int & i, const int & j) {
			// Descending order
			return m_v[i] > m_v[j];
		}
	};

	void vsampling_t::debug() {
		const vector<float> & sprank = Kb::instance().static_prank();
		cerr << std::setprecision(4) << 1.0f / static_cast<float>(m_bucket_N) << "\n";
		for(size_t i = 0; i < m_intervals.size(); i++) {
			size_t isize = m_intervals[i].second - m_intervals[i].first ;
			cerr << "Interval (" << isize << ")" << i << ": ";
			for (int j = m_intervals[i].first; j != m_intervals[i].second; j++) {
				string v = Kb::instance().get_vertex_name(m_idx[j]);
				float rank = sprank[m_idx[j]];
				cerr << v << ":" << std::setprecision(4) << rank << " ";
			}
			cerr << "\n";
		}
	}

	vsampling_t::vsampling_t(size_t buckets) :
		m_N( Kb::instance().size() ), m_bucket_N(buckets) {
		if (!m_bucket_N) return ;
		init(Kb::instance().static_prank());
	}

	// Create buckets according to ranks vector, which has to be a probability
	// vector

	vsampling_t::vsampling_t(size_t buckets, const vector<float> & ranks) :
		m_N( Kb::instance().size() ), m_bucket_N(buckets) {
		if (!m_bucket_N) return ;
		init(ranks);
	}

	// Init buckets according to ranks vector, which has to be a probability
	// vector

	void vsampling_t::init(const vector<float> & ranks)  {
		vector<int>(m_N).swap(m_idx);
		for(size_t i = 0; i < m_N; i++) m_idx[i] = i;
		// sort idx according to rank vector
		sort(m_idx.begin(), m_idx.end(), sort_t(ranks));
		float factor = 1.0f / static_cast<float>(m_bucket_N) ;
		float accum = 0.0;
		int left = 0;
		for(size_t i = 0; i < m_N; i++) {
			accum += ranks[ m_idx[i] ];
			if (accum > factor) {
				m_intervals.push_back(make_pair(left, i));
				if (m_intervals.size() == m_bucket_N) break;
				left = i;
				accum = 0.0;
			}
		}
		m_intervals.back().second = m_N; // put remaining nodes in last bucket
		m_bucket_N = m_intervals.size(); // safety measure
	}

	// Method to sample a vertex
	// First, decide which bucket, then select one at random from the bucket
	int vsampling_t::sample() {
		if (!m_bucket_N) {
			return rnumber(m_N - 1);
		}
		size_t bucket_i = rnumber(m_bucket_N - 1);
		int left = m_intervals[bucket_i].first;
		int m = m_intervals[bucket_i].second - left;
		assert(m > 0);
		size_t off = rnumber(m - 1);
		return m_idx[left + off];
	}

	///////////////////////////////////////////////
	// sampling according to strong components

	void vsampling_components_t::debug() {
		for(size_t i = 0; i < m_intervals.size(); i++) {
			cerr << "Interval [ " << m_intervals[i].first << ", " << m_intervals[i].second << ") W: "<< std::setprecision(4) << m_compW[i] << " : ";
			// for (int j = m_intervals[i].first; j != m_intervals[i].second; j++) {
			//	string v = Kb::instance().get_vertex_name(m_idx[j]);
			//	cerr << v << " ";
			// }
			cerr << "\n";
		}
	}

	struct vsampling_components_t::sort_t {

		const vector<size_t> & m_components ;
		const vector<int> & m_comp_sizes ;
		sort_t(const vector<size_t> & components, const vector<int> & comp_sizes) : m_components(components), m_comp_sizes(comp_sizes) {}
		int operator()(const int & i, const int & j) {
			// Descending order
			size_t comp_i = m_components[i];
			size_t comp_j = m_components[j];
			int size_delta = m_comp_sizes[ comp_i ] - m_comp_sizes[ comp_j ];
			if (size_delta > 0) return true;  // comp_i is bigger that comp_j
			if (size_delta < 0) return false; // comp_i is smaller that comp_j
			return comp_i < comp_j; // if equal size, sort descending order
		}
	};

	vsampling_components_t::vsampling_components_t() : m_N(0), m_component_N(0) {

		Kb & kb = Kb::instance();
		m_N = kb.size();
		if (!m_N) return ;
		vector<size_t> components;
		m_component_N = kb.components(components);
		if (m_component_N == 1) return;
		vector<int> comp_sizes(m_component_N, 0);
		for(vector<size_t>::const_iterator it = components.begin(), end = components.end();
			it != end; ++it) {
			comp_sizes[*it]++;
		}
		vector<int>(m_N).swap(m_idx);
		for(size_t i = 0; i < m_N; i++) m_idx[i] = i;
		// sort idx according to component
		sort(m_idx.begin(), m_idx.end(), sort_t(components, comp_sizes));

		// calculate component intervals
		// and fill probabilities of components
		float prob_factor = 1.0f / static_cast<float>(m_N);
		size_t current_comp = components[ m_idx[0] ];
		int left = 0;
		for(size_t i = 1; i < m_N; i++) {
			size_t new_comp = components[ m_idx[i] ];
			if (new_comp != current_comp) {
				m_intervals.push_back(make_pair(left, i));
				m_compW.push_back(comp_sizes[current_comp] * prob_factor);
				left = i;
				current_comp = new_comp;
			}
		}
		m_intervals.push_back(make_pair(left, m_N));
		m_compW.push_back(comp_sizes[current_comp] * prob_factor);
		// accumulate component probabilities
		float accum = 0.0;
		for(size_t i = 0; i < m_component_N - 1; i++) {
			accum += m_compW[i];
			m_compW[i] = accum;
		}
		m_compW[m_component_N - 1] = 1.0;
	}

	// Method to sample a vertex
	// First, decide which component (according to size), then select one at random from the component
	int vsampling_components_t::sample() {
		if (m_component_N < 2) {
			return rnumber(m_N - 1);
		}
		float toss = rnumber_01();
		size_t comp_i;
		for(comp_i = 0; comp_i < m_component_N; ++comp_i)
			if (m_compW[comp_i] > toss) break;
		assert(comp_i < m_component_N);

		int left = m_intervals[comp_i].first;
		int m = m_intervals[comp_i].second - left;
		assert(m > 0);
		if (m == 1) return m_idx[left];
		size_t off = rnumber(m - 1);
		return m_idx[left + off];
	}

	static bool emit_word_vertex_mono(Kb::vertex_descriptor current, string & emit_word,
									  vector<float> & vertex2word_tweight) {

		WInvdict_entries words = WDict::instance().words(current);
		if (!words.size()) return false;
		float & total_weight = vertex2word_tweight[current];
		if (total_weight == 0.0f) {
			float T = 0.0f;
			WInvdict_entries::freq_const_iterator freq_it( words.begin() );
			WInvdict_entries::freq_const_iterator freq_end( words.end() );
			for(;freq_it != freq_end; ++freq_it) T += *freq_it;
			total_weight = T;
		}
		if (total_weight == 0.0f) return false; // Should never happen
		float rand_value = rnumber(total_weight);
		float w_accum = 0;
		for(size_t i = 0; i < words.size(); ++i) {
			w_accum += words.get_prob(i);
			emit_word = words.get_word(i);
			if (rand_value < w_accum) break;
		}
		return true;
	}

	static pair<string, bool> xtract_lang(const string & str) {
		size_t m = str.size();
		if (m < 3) return make_pair(string(), false);
		if (str[m-3] != '#')
			return make_pair(string(), false);
		return make_pair(string(str, m - 2), true);
	}

	struct wap_langselect_sort_t {
		wap_langselect_sort_t() {}
		int operator()(const pair<string, float> & a, const pair<string, float> & b) {
			string langA, langB;
			bool ok;
			tie(langB, ok) = xtract_lang(b.first);
			if (!ok) return 1;
			tie(langA, ok) = xtract_lang(a.first);
			if (!ok) return 0;
			return langA < langB;
		}
	};

	// select words of one language at random (languages are equiprobable)
	static pair<vector<pair<string, float> >::iterator,
				vector<pair<string, float> >::iterator>
	subvec_language(WInvdict_entries & words,
					vector<pair<string, float> > & words_mono)
	{
		size_t N = words.size();
		for(size_t i = 0; i < N; ++i) {
			words_mono.push_back(make_pair(words.get_word(i), words.get_prob(i)));
		}
		sort(words_mono.begin(), words_mono.end(), wap_langselect_sort_t());
		vector<pair<string, float> >::iterator it = words_mono.begin(), end = words_mono.end();
		bool ok;
		string stored_lang, current_lang;
		tie(stored_lang, ok) = xtract_lang(it->first);
		if (!ok) return make_pair(words_mono.begin(), end);
		vector<vector<pair<string, float> >::iterator> lang_idx;
		lang_idx.push_back(it);
		++it;
		while(it != end) {
			tie(current_lang, ok) = xtract_lang(it->first);
			if (!ok) return make_pair(words_mono.begin(), end);
			if(stored_lang != current_lang) {
				lang_idx.push_back(it);
				stored_lang = current_lang;
			}
			++it;
		}
		int langN = lang_idx.size();
		if (langN == 1)
			return make_pair(words_mono.begin(), end);
		int idx = rnumber(langN - 1);
		it = lang_idx[idx];
		if (idx == langN - 1)
			end = words_mono.end();
		else
			end = lang_idx[idx + 1];
		return make_pair(it, end);
	}

	static bool emit_word_vertex_multi(Kb::vertex_descriptor current, string & emit_word) {

		WInvdict_entries words = WDict::instance().words(current);
		if (!words.size()) return false;
		vector<pair<string, float> > words_mono;
		vector<pair<string, float> >::iterator beg, end;
		tie(beg, end) = subvec_language(words, words_mono);
		float total_weight = 0.0f;
		for(vector<pair<string, float> >::iterator it = beg;
			it != end; ++it) {
			total_weight += it->second;
		}
		if (total_weight == 0.0f) return false; // Should never happen

		float rand_value = rnumber(total_weight);
		float w_accum = 0;
		for(; beg != end; ++beg) {
			w_accum += beg->second;
			emit_word = beg->first;
			if (rand_value < w_accum) break;
		}
		return true;
	}

	static bool emit_word_vertex(Kb::vertex_descriptor current, string & emit_word,
								 vector<float> & vertex2word_tweight) {
		Kb & kb = Kb::instance();

		float toss = rnumber_01();
		if (toss >= glVars::wap::wemit_prob) {
			emit_word = kb.get_vertex_name(current);
			return true;
		}
		if(glVars::wap::multilang)
			return emit_word_vertex_multi(current, emit_word);
		else
			return emit_word_vertex_mono(current, emit_word, vertex2word_tweight);
	}

	static bool select_next_vertex_eweight(Kb::vertex_descriptor & current,
										   vector<float> & vertex_out_tweight) {

		Kb & kb = Kb::instance();
		Kb::vertex_descriptor previous = current;

		Kb::out_edge_iterator out_it, out_end;

		tie(out_it, out_end) = kb.out_neighbors(previous);
		float & total_weight = vertex_out_tweight[previous];
		if (total_weight == 0.0f) {
			float T = 0.0f;
			for(Kb::out_edge_iterator auxit = out_it; auxit < out_end; ++auxit) {
				T += kb.get_edge_weight(*out_it);
			}
			total_weight = T;
		}
		if (total_weight == 0.0f) return false; // danglink link. The RW is over.
		float rand_value = rnumber(total_weight);
		float w_accum = 0.0f;
		for(; out_it < out_end; ++out_it) {
			w_accum += kb.get_edge_weight(*out_it);
			current = kb.edge_target(*out_it);
			if (rand_value < w_accum) break;
		}
		return true;
	}

	static bool select_next_vertex_degree(Kb::vertex_descriptor & current,
										  vector<float> & vertex_out_tweight) {

		Kb & kb = Kb::instance();
		Kb::boost_graph_t & G = kb.graph();
		Kb::vertex_descriptor previous = current;

		Kb::out_edge_iterator out_it, out_end;

		tie(out_it, out_end) = kb.out_neighbors(previous);
		float & total_weight = vertex_out_tweight[previous];
		if (total_weight == 0.0f) {
			float T = 0.0f;
			for(Kb::out_edge_iterator auxit = out_it; auxit < out_end; ++auxit) {
				Kb::vertex_descriptor uu = kb.edge_target(*auxit);
				T += in_degree(uu, G);
			}
			total_weight = T;
		}
		if (total_weight == 0.0f) return false; // danglink link. The RW is over.
		float rand_value = rnumber(total_weight);
		float w_accum = 0.0f;
		for(; out_it < out_end; ++out_it) {
			Kb::vertex_descriptor uu = kb.edge_target(*out_it);
			w_accum += in_degree(uu, G);
			current = uu;
			if (rand_value < w_accum) break;
		}
		return true;
	}

	static bool select_next_vertex(Kb::vertex_descriptor & current,
								   vector<float> & vertex_out_tweight) {
		if (glVars::wap::prefer_indegree) return select_next_vertex_degree(current,
																		   vertex_out_tweight);
		return select_next_vertex_eweight(current,
										  vertex_out_tweight);
	}

	// perform one complete rw starting from v

	static void do_complete_mc(Kb::vertex_descriptor v,
							   vector<string> & emited_words,
							   vector<float> & vertex2word_tweight,
							   vector<float> & vertex_out_tweight) {

		Kb::vertex_descriptor current = v;  //Start the iteration in the V vertex

		for (float r = rnumber_01(); r <= glVars::prank::damping; r = rnumber_01() ) {
			// emit word from current vertex
			string emit_word;
			if (emit_word_vertex(current, emit_word, vertex2word_tweight))
				emited_words.push_back(emit_word);
			// Select next vertex to jump
			if (!select_next_vertex(current, vertex_out_tweight)) break;
		}
	}

	// perform one complete rw starting from v (fixed path length)

	static void do_complete_length(Kb::vertex_descriptor current,
								   size_t t,
								   vector<string> & emited_words,
								   vector<float> & vertex2word_tweight,
								   vector<float> & vertex_out_tweight) {

		if (!t) return;

		vector<string>().swap(emited_words);

		for (size_t i = 0; i < t; ++i ) {
			// emit word from current vertex
			string emit_word;
			if (emit_word_vertex(current, emit_word, vertex2word_tweight))
				emited_words.push_back(emit_word);
			// Select next vertex to jump
			if (!select_next_vertex(current, vertex_out_tweight)) break;
		}
	}

	// Main algorithm for walk&print

	bool Wap::next(vector<string> & emited_words) {

		vector<string>().swap(emited_words);

		if (m_n && m_i >= m_n) return false;

		int idx = m_vsampler.sample();
		Kb::vertex_descriptor u(idx);
		if (!m_cache_init) {
			if (!glVars::wap::multilang)
				vector<float>(Kb::instance().size(), 0.0f).swap(m_vertex2word_tweight);
			vector<float>(Kb::instance().size(), 0.0f).swap(m_vertex_out_tweight);
			m_cache_init = true;
		}
		do_complete_mc(u, emited_words, m_vertex2word_tweight, m_vertex_out_tweight);
		m_i++;
		return true;
	}

	// Main algorithm for walk&print

	bool WapComponents::next(vector<string> & emited_words) {

		vector<string>().swap(emited_words);

		if (m_n && m_i >= m_n) return false;

		int idx = m_vsampler.sample();
		Kb::vertex_descriptor u(idx);
		if (!m_cache_init) {
			if (!glVars::wap::multilang)
				vector<float>(Kb::instance().size(), 0.0f).swap(m_vertex2word_tweight);
			vector<float>(Kb::instance().size(), 0.0f).swap(m_vertex_out_tweight);
			m_cache_init = true;
		}
		do_complete_mc(u, emited_words, m_vertex2word_tweight, m_vertex_out_tweight);
		m_i++;
		return true;
	}

	// Main algorithm for Walk&Print starting from a word

	bool WapWord::next(vector<string> & emited_words) {

		vector<string>().swap(emited_words);

		if (m_n && m_i >= m_n) return false;
		emited_words.push_back(m_seed);

		static boost::unordered_map<const std::string *, float > dweight_cache;
		boost::unordered_map<const std::string *, float >::iterator dweight_it;
		bool P;

		// select synset at random
		tie(dweight_it, P) = dweight_cache.insert(std::make_pair(&m_seed, 0.0f));
		float & total_weight = dweight_it->second;
		if (P) {
			for(size_t i = 0; i < m_synsets.size(); ++i) {
				total_weight += m_synsets.get_freq(i);
			}
		}
		if (total_weight == 0.0f) return false;; // word has no attached vertices
		float rand_value = rnumber(total_weight);
		float w_accum = 0;
		Kb::vertex_descriptor synset = 0;
		for(size_t i = 0; i < m_synsets.size(); ++i) {
			w_accum += m_synsets.get_freq(i);
			synset = m_synsets.get_entry(i);
			if (rand_value < w_accum) break;
		}
		if (!m_cache_init) {
			if (!glVars::wap::multilang)
				vector<float>(Kb::instance().size(), 0.0f).swap(m_vertex2word_tweight);
			vector<float>(Kb::instance().size(), 0.0f).swap(m_vertex_out_tweight);
			m_cache_init = true;
		}
		do_complete_mc(synset, emited_words, m_vertex2word_tweight, m_vertex_out_tweight);
		m_i++;
		return true;
	}

	bool DeepWalk::next(vector<string> & emited_words) {

		vector<string>().swap(emited_words);

		if (m_g >= m_gamma) return false;

		Kb::vertex_descriptor u(m_i);
		if (!m_cache_init) {
			if (!glVars::wap::multilang)
				vector<float>(Kb::instance().size(), 0.0f).swap(m_vertex2word_tweight);
			vector<float>(Kb::instance().size(), 0.0f).swap(m_vertex_out_tweight);
			m_cache_init = true;
		}
		do_complete_length(u, m_t, emited_words, m_vertex2word_tweight, m_vertex_out_tweight);
		if (++m_i >= m_N) {
			m_i = 0;
			m_g++;
		}
		return true;
	}

	// Not used

	// void wap_do_mc_words(size_t n) {

	//	WDictHeadwords dicthws(WDict::instance());

	//	static boost::unordered_map<const std::string &, float > dweight_cache;
	//	boost::unordered_map<const std::string &, float >::iterator dweight_it;
	//	bool P;
	//	size_t m = dicthws.size();

	//	for(size_t i = 0; i < n; ++i) {
	//		int idx = rnumber((int) m - 1);
	//		const string & hw = dicthws.hw(idx);

	//		// select synset at random
	//		WDict_entries synsets(dicthws.rhs(idx));
	//		tie(dweight_it, P) = dweight_cache.insert(make_pair(hw, 0.0f));
	//		float & total_weight = dweight_it->second;
	//		if (P) {
	//			for(size_t i = 0; i < synsets.size(); ++i) {
	//				total_weight += synsets.get_freq(i);
	//			}
	//		}
	//		float rand_value = rnumber(total_weight);
	//		float w_accum = 0;
	//		Kb::vertex_descriptor synset = 0;
	//		for(size_t i = 0; i < synsets.size(); ++i) {
	//			w_accum += synsets.get_freq(i);
	//			synset = synsets.get_entry(i);
	//			if (rand_value < w_accum) break;
	//		}
	//		// print RW
	//		vector<string> emited_words;
	//		do_mc_complete(synset, emited_words);
	//		// print RW
	//		if(emited_words.size()) {
	//			cout << hw << "\t";
	//			vector<string>::iterator it = emited_words.begin();
	//			vector<string>::iterator end = emited_words.end();
	//			if (it != end) {
	//				--end;
	//				for(;it != end; ++it) {
	//					cout << *it << " ";
	//				}
	//				cout << *end << "\n";
	//			}
	//		}
	//	}
	// }

};


#include <boost/functional/hash.hpp>

#include "memoryrange.hh"

using namespace std;
using namespace boost::accumulators;

std::vector< MemoryRange > MemoryRange::bisect( void ) const
{
  vector< MemoryRange > ret { *this };

  /* bisect in each active axis */
  for ( auto & i : _active_axis ) {
    vector< MemoryRange > doubled;
    for ( const auto &x : ret ) {
      auto ersatz_lower( x._lower ), ersatz_upper( x._upper );
      ersatz_lower.mutable_field( i ) = ersatz_upper.mutable_field( i ) = median( _acc[ i ] );

      if ( x._lower == ersatz_upper ) {
        /* try range midpoint instead */
        ersatz_lower.mutable_field( i ) = ersatz_upper.mutable_field( i ) = (x._lower.field( i ) + x._upper.field( i )) / 2;
      }

      if ( x._lower == ersatz_upper ) {
        assert( !(ersatz_lower == x._upper) );
        assert( x._lower == ersatz_lower );
        /* cannot double on this axis */
        doubled.push_back( x );
      } else {
        doubled.emplace_back( x._lower, ersatz_upper, x._active_axis );
        doubled.emplace_back( ersatz_lower, x._upper, x._active_axis );
      }
    }

    ret = doubled;
  }

  assert( !ret.empty());

  return ret;
}

Memory MemoryRange::range_median( void ) const
{
  Memory median_data( _lower );
  for ( auto & i : _active_axis ) {
    median_data.mutable_field( i ) = (_lower.field( i ) + _upper.field( i )) / 2;
  }
  return median_data;
}

bool MemoryRange::contains( const Memory & query ) const
{
  for ( auto & i : _active_axis ) {
    if (!((query.field(i) >= _lower.field(i)) && (query.field(i) < _upper.field(i)))) { return false; }
  }
  return true;
}

void MemoryRange::track( const Memory & query ) const
{
  /* log it */
  for ( auto & i : _active_axis ) {
    _acc[ i ]( query.field( i ) );
  }
}

bool MemoryRange::operator==( const MemoryRange & other ) const
{
  for ( auto & i : _active_axis ) {
    if (!((_lower.field(i) == other._lower.field(i)) && (_upper.field(i) == other._upper.field(i)))) { return false; }
  }
  return true;
}

string MemoryRange::str( void ) const
{
  char tmp[ 256 ];
  strcpy( tmp, "(lo=< ");
  for ( auto & i : _active_axis ) {
    strcat( tmp, _lower.str( i ).c_str() );
  }
  strcat( tmp, ">, hi=< ");
  for ( auto & i : _active_axis ) {
    strcat( tmp, _upper.str( i ).c_str() );
  }
  strcat( tmp, ">)");
  return tmp;
}

RemyBuffers::MemoryRange MemoryRange::DNA( void ) const
{
  RemyBuffers::MemoryRange ret;

  ret.mutable_lower()->CopyFrom( _lower.DNA() );
  ret.mutable_upper()->CopyFrom( _upper.DNA() );
  for (auto & x : _active_axis) {
    ret.add_active_axis(x);
  }

  return ret;
}

MemoryRange::MemoryRange( const RemyBuffers::MemoryRange & dna )
  : _lower( true, dna.lower() ),
    _upper( false, dna.upper() ),
    _active_axis( ),
    _acc( Memory::datasize ),
    _count( 0 )
{
  for (auto & x : dna.active_axis()) {
    _active_axis.push_back( (Axis) x );
  }
  /* Backward compatibility: old remys don't have active axis; set to default */
  if ( _active_axis.empty() ) {
    _active_axis = vector<Axis>( { RemyBuffers::MemoryRange::SEND_EWMA, RemyBuffers::MemoryRange::REC_EWMA,
                                   RemyBuffers::MemoryRange::RTT_RATIO, RemyBuffers::MemoryRange::SLOW_REC_EWMA } );
  }
}

size_t hash_value( const MemoryRange & mr )
{
  size_t seed = 0;
  boost::hash_combine( seed, mr._lower );
  boost::hash_combine( seed, mr._upper );

  return seed;
}


#ifndef ALJ_SUPPLESEARCH_MEASURE_HPP_
#define ALJ_SUPPLESEARCH_MEASURE_HPP_

#include "database.hpp"
#include "document.hpp"

#include <armadillo>

namespace SuppleSearch {

class Measure {
  public:
    typedef std::shared_ptr<Measure> shared;

    virtual double compare(arma::vec a, arma::vec b) = 0;
    virtual arma::vec compare(arma::mat document, arma::vec query) = 0;
};

}

#endif


/*
 * Filename: c:\Users\alexf\OneDrive\Desktop\projetofinalcurso\FDRV\main.cpp
 * Path: c:\Users\alexf\OneDrive\Desktop\projetofinalcurso\FDRV
 * Created Date: Saturday, November 3rd 2018, 11:31:05 am
 * Author: Alexandre Frazao
 *
 * Copyright (c) 2018
 */

#include <dlib/dnn.h>
#include <dlib/data_io.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <dlib/string.h>
#include <dlib/image_io.h>
#include <dlib/opencv.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/logger.h>

#include <opencv2/core.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/cudacodec.hpp>

#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/filesystem.hpp>

#include <iostream>
#include <stdio.h>
#include <ctime>

using namespace std;
using namespace dlib;
using namespace boost;
using namespace cv;

using std::ofstream;

/******************* Data structures ************************/
struct bookmark
{
    cv::Mat frame;
    int frame_position;
};

class TimeFrame
{
  private:
    bookmark start;
    bookmark end;
    int frames_between;
    double elapsed_seconds;

  public:
    TimeFrame(bookmark starter_marker, bookmark end_marker, int number_between_frames, double second)
    {
        start = starter_marker;
        end = end_marker;
        frames_between = number_between_frames;
        elapsed_seconds = second;
    }
};

struct thread_data
{
    dlib::matrix<dlib::rgb_pixel> *target;
    cv_image<bgr_pixel> *source;
    int id;
};
/************************************************************/

std::vector<filesystem::path> fileSearch(string path);
void threaded_copy(void *arg);

template <long num_filters, typename SUBNET>
using con5d = con<num_filters, 5, 5, 2, 2, SUBNET>;
template <long num_filters, typename SUBNET>
using con5 = con<num_filters, 5, 5, 1, 1, SUBNET>;

template <typename SUBNET>
using downsampler = relu<affine<con5d<32, relu<affine<con5d<32, relu<affine<con5d<16, SUBNET>>>>>>>>>;
template <typename SUBNET>
using rcon5 = relu<affine<con5<45, SUBNET>>>;

using net_type = loss_mmod<con<1, 9, 9, 1, 1, rcon5<rcon5<rcon5<downsampler<input_rgb_image_pyramid<pyramid_down<6>>>>>>>>;

int thread_count = std::thread::hardware_concurrency();
int num = std::thread::hardware_concurrency();
dlib::mutex count_mutex;
dlib::signaler count_signaler(count_mutex);

/**
 * Global logger object
 * */
logger dlog("FDRV_log");

int main(int argc, char **argv)
{
    dlog.set_level(LALL);
    net_type cnn_detector;
    // TODO:  Receber configs como o principal
    deserialize(argv[2]) >> cnn_detector;

    // TODO: Por com CUDA
    cv::VideoCapture video_reader;
    int apiID = cv::CAP_ANY; // 0 = autodetect default API

    // Can be replaced by 0 to show webcam stream
    video_reader.open(argv[1]);

    if (!video_reader.isOpened())
    {
        cerr << "ERROR! Unable to open camera\n";
        return -1;
    }

    //TODO:: Colocar tempo do video no video final
    Mat frame;
    int total_frames = 0;

    std::vector<TimeFrame> identified_frames;

    clock_t begin = clock();

    bookmark s, e;

    cv::VideoWriter video_writer(argv[3],
                                 video_reader.get(CV_CAP_PROP_FOURCC),
                                 video_reader.get(CV_CAP_PROP_FPS),
                                 cv::Size(video_reader.get(CV_CAP_PROP_FRAME_WIDTH),
                                          video_reader.get(CV_CAP_PROP_FRAME_HEIGHT)),
                                 true);
    matrix<rgb_pixel> matrix;

    while (video_reader.read(frame))
    {
        if (frame.empty())
        {
            cerr << "ERROR! blank frame grabbed\n";
            break;
        }

        cv_image<bgr_pixel> image(frame);

        //assign_image(matrix, image);

        matrix.set_size(image.nr(), image.nc());
        //cout << "Starting convertion" << endl;
        std::vector<thread_data> t_data;
        //cout << "Creating threads" << endl;
        for (int t_id = 0; t_id < num; t_id++)
        {
            thread_data da;
            da.target = &matrix;
            da.source = ℑ
            da.id = t_id;
            t_data.push_back(da);
            dlib::create_new_thread(threaded_copy, (void *)&t_data[t_id]);
        }

        dlib::auto_mutex abcd(count_mutex);
        while (thread_count > 0)
        {
            count_signaler.wait();
        }

        thread_count = std::thread::hardware_concurrency();

        std::vector<dlib::mmod_rect> faces_found = cnn_detector(matrix);
        // Write frames with faces found so that we can analise them
        if (faces_found.size() == 0)
        {
            if (s.frame.empty())
            {
                e.frame = frame;
                e.frame_position = total_frames - 1;
                double frame_sec = total_frames / video_reader.get(CV_CAP_PROP_FPS);

                identified_frames.push_back(TimeFrame(s, e, e.frame_position - s.frame_position, frame_sec));
            }
        }
        else
        {
            s.frame_position = total_frames - 1;
            s.frame = frame;
            double frame_sec = total_frames / video_reader.get(CV_CAP_PROP_FPS);

            cv::putText(frame, "Time: " + to_string(frame_sec), cvPoint(50, 60), cv::FONT_HERSHEY_SIMPLEX, 1, cvScalar(255, 255, 250));

            if (true)
            {
                video_writer.write(frame);
            }
        }
        total_frames++;
    }

    // the camera will be deinitialized automatically in VideoCapture destructor

    cout << "Total number of frames: " << total_frames << endl;
    double total_time = double(clock() - begin) / CLOCKS_PER_SEC;
    cout << "Total time (seconds): " << total_time << endl;
    cout << "FPS: " << total_frames / total_time << endl;

    video_reader.release();
    video_writer.release();
    return 0;
}
// TODO: FDXML com tempos, etc...

void create_DFXML()
{
}

void threaded_copy(void *arg)
{
    thread_data *data = (thread_data *)arg;

    int start = data->id;
    int jump = num;

    for (long r = start; r < (*data->source).nr(); r += jump)
    {
        for (long c = 0; c < (*data->source).nc(); c++)
        {
            assign_pixel((*data->target)(r, c), (*data->source)(r, c));
        }
    }
    //cout << "Done my job -- closing" << endl;
    dlib::auto_mutex locker(count_mutex);
    thread_count--;
    // Now we signal this change.  This will cause one thread that is currently waiting
    // on a call to count_signaler.wait() to unblock.
    count_signaler.signal();
}

void parse_config(std::string path)
{
    boost::property_tree::ptree json_tree;
    try
    {
        read_json(path, json_tree);
    }
    catch (std::exception &e)
    {
        cout << "Error parsing json" << endl;
        exit(2);
    }
    /*
    try
    {
        workspace = json_tree.get<string>("workspace");
        imagesToFindPath = json_tree.get<string>("imagesPath");
        positiveImgPath = json_tree.get<string>("wanted_faces");
        detectorPath = json_tree.get_child("paths.").get<string>("0");
        recognitionPath = json_tree.get_child("paths.").get<string>("1");
        shapePredictorPath = json_tree.get_child("paths.").get<string>("2");
        doRecognition = json_tree.get<bool>("doRecognition");
    }
    catch (std::exception &e)
    {
        cout << "Error Initiating variables" << endl;
        exit(3);
    }
    */
}

std::vector<filesystem::path> fileSearch(string path)
{
    std::vector<filesystem::path> images_path;
    std::vector<string> targetExtensions;

    targetExtensions.push_back(".mp4");
    targetExtensions.push_back(".avi");

    if (!filesystem::exists(path))
    {
        exit(4);
    }

    for (filesystem::recursive_directory_iterator end, dir(path); dir != end; ++dir)
    {
        string extension = filesystem::path(*dir).extension().generic_string();
        transform(extension.begin(), extension.end(), extension.begin(), ::toupper);
        if (std::find(targetExtensions.begin(), targetExtensions.end(), extension) != targetExtensions.end())
        {
            images_path.push_back(filesystem::path(*dir));
        }
    }
    return images_path;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_TOUINTS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_TOUINTS_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  touints generic tag

      Represents the touints function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct touints_ : ext::elementwise_<touints_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<touints_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_touints_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::touints_, Site> dispatching_touints_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::touints_, Site>();
    }
    template<class... Args>
    struct impl_touints_;
  }
  /*!
    Convert to unsigned integer by saturated truncation.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    as_integer<T, unsigned> r = touint(x);
    @endcode

    The code is similar to:

    @code
    as_integer<T,unsigned> r = static_cast<as_integer<T,unsigned> >(saturate<as_integer<T,unsigned> (x)))
    @endcode

    @par Notes:

    The Inf, Nan and negative values are treated properly and go respectively to
    Valmax, and Zero of the destination integral type.

    All values superior (resp. less) than Valmax (resp. Valmin) of the return type
    are saturated accordingly.

    @par Alias

    saturated_toint

    @see @funcref{toint}
    @param  a0

    @return      a value of the unsigned integer type associated to the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::touints_, touints, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::touints_, saturated_touint, 1)

} }

#endif