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

#include <miopen/solver.hpp>

#include <miopen/algorithm.hpp>
#include <miopen/env.hpp>
#include <miopen/gemm_v2.hpp>
#include <miopen/handle.hpp>
#include <miopen/kernel.hpp>
#include <miopen/tensor.hpp>
#include <miopen/tensor_ops.hpp>
#include <miopen/util.hpp>
#include <miopen/conv/data_invoke_params.hpp>

#include <boost/any.hpp>
#include <boost/range/adaptors.hpp>

MIOPEN_DECLARE_ENV_VAR(MIOPEN_CONV_PRECISE_ROCBLAS_TIMING)

// copy from convolution.cpp
// Workaround for issue 1430.
// Vega20 fails to access GPU memory larger than the return value of GetMaxMemoryAllocSize() of
// Vega10
#define MAX_MEM_ALLOC_SZ (std::min(handle.GetMaxMemoryAllocSize(), size_t(7287183769)))

namespace miopen {
namespace solver {

#if MIOPEN_USE_GEMM
#ifdef CPPCHECK
// Keep the value unknown in cppcheck since this can differ between opencl and hip
static bool IsBf16Supported;
static bool IsFp16Supported;
#else
static constexpr const bool IsBf16Supported = (MIOPEN_USE_ROCBLAS || MIOPEN_USE_MIOPENTENSILE);
static constexpr const bool IsFp16Supported = (MIOPEN_USE_ROCBLAS || MIOPEN_USE_MIOPENTENSILE);
#endif

static inline bool IsAnyBufferBF16(const TensorDescriptor& xDesc,
                                   const TensorDescriptor& yDesc,
                                   const TensorDescriptor& wDesc)
{
    return xDesc.GetType() == miopenBFloat16 || yDesc.GetType() == miopenBFloat16 ||
           wDesc.GetType() == miopenBFloat16;
}

static inline bool IsAnyBufferFp16(const TensorDescriptor& xDesc,
                                   const TensorDescriptor& yDesc,
                                   const TensorDescriptor& wDesc)
{
    return xDesc.GetType() == miopenHalf || yDesc.GetType() == miopenHalf ||
           wDesc.GetType() == miopenHalf;
}
#endif

static double
SlowdownFactor(int n_oper, const double oper_factor, const double multiple_oper_factor)
{
    if(n_oper > 0)
    {
        auto rv = oper_factor;
        if(n_oper > 1)
            rv *= multiple_oper_factor;
        return rv;
    }
    else
        return 1.0;
}

bool GemmBwdBase::IsApplicable(const ExecutionContext&,
                               const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    const auto& dyDesc = problem.GetIn();
    const auto& wDesc  = problem.GetWeights();
    const auto& dxDesc = problem.GetOut();
    return problem.GetDirection() == conv::Direction::BackwardData && problem.IsLayoutDefault() &&
           !(IsAnyBufferBF16(dxDesc, dyDesc, wDesc) && !IsBf16Supported) &&
           !(IsAnyBufferFp16(dxDesc, dyDesc, wDesc) && !IsFp16Supported);
#else
    std::ignore = problem;
    return false;
#endif
}

float GemmBwdBase::GetWti(const ExecutionContext&, const conv::ProblemDescription& problem) const
{
    const auto& conv   = problem.GetConv();
    const auto& wDesc  = problem.GetWeights();
    const auto& dxDesc = problem.GetOut();

    int n_SetTensor            = 0;
    int n_transpose_NCHW2CNHW  = 0;
    int n_transpose_CNHW2NCHW  = 0;
    int n_gemm_strided_batched = 1; // not strided-batched by default
    int n_gemm_runs            = 1;
    int n_Col2ImGPU            = 0;

    std::size_t in_n, in_c;
    std::tie(in_n, in_c)    = tie_pick<0, 1>()(dxDesc.GetLengths());
    std::size_t spatial_dim = conv.GetSpatialDimension();
    auto wei_spatial        = boost::adaptors::slice(wDesc.GetLengths(), 2, 2 + spatial_dim);

    // 1x1 does not require col2im
    if(conv.GetSpatialDimension() == 2 &&
       miopen::all_of(wei_spatial, [](auto v) { return v == 1; }) &&
       miopen::all_of(conv.GetConvPads(), [](auto v) { return v == 0; }) &&
       miopen::all_of(conv.GetConvStrides(), [](auto v) { return v == 2; }))
    {
        n_SetTensor            = 1;
        n_transpose_NCHW2CNHW  = 1;
        n_gemm_strided_batched = conv.group_count;
        n_transpose_CNHW2NCHW  = 1;
    }
    // 1x1_stride=1 convolutions use GEMM and zero workspace
    else if(miopen::all_of(wei_spatial, [](auto v) { return v == 1; }) &&
            miopen::all_of(conv.GetConvPads(), [](auto v) { return v == 0; }) &&
            miopen::all_of(conv.GetConvStrides(), [](auto v) { return v == 1; }))
    {
        n_gemm_strided_batched = in_n;
    }
    // if not 1x1
    else
    {
        n_gemm_strided_batched = conv.group_count;
        n_gemm_runs            = in_n;
        n_Col2ImGPU            = in_n;
    }

    auto wti = 1.0;
    wti *= SlowdownFactor(n_SetTensor, 0.95, 0.99);
    wti *= SlowdownFactor(n_transpose_NCHW2CNHW, 0.7, 0.9);
    wti *= SlowdownFactor(n_transpose_CNHW2NCHW, 0.7, 0.9);
    wti *= SlowdownFactor(n_gemm_runs, 0.9, 0.9);
    wti *= SlowdownFactor(n_gemm_strided_batched, 1.0, 0.95);
    wti *= SlowdownFactor(n_Col2ImGPU, 0.4, 0.8);
    return wti;
}

size_t GemmBwd1x1_stride2::GetWorkspaceSize(const ExecutionContext& context,
                                            const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    auto& handle       = context.GetStream();
    const auto& conv   = problem.GetConv();
    const auto& dyDesc = problem.GetIn();
    const auto& dxDesc = problem.GetOut();

    const auto in_n = dxDesc.GetLengths()[0];
    const auto in_c = dxDesc.GetLengths()[1];

    const auto out_spatial =
        boost::adaptors::slice(dyDesc.GetLengths(), 2, 2 + conv.GetSpatialDimension());

    const auto dx_t_size = in_n * in_c *
                           std::accumulate(out_spatial.begin(),
                                           out_spatial.end(),
                                           std::size_t(1),
                                           std::multiplies<std::size_t>()) *
                           GetTypeSize(dxDesc.GetType());

    const auto dy_t_size  = dyDesc.GetElementSize() * GetTypeSize(dyDesc.GetType());
    const auto gemm_trans = dx_t_size + dy_t_size;

    if(gemm_trans > MAX_MEM_ALLOC_SZ)
    {
        MIOPEN_LOG_I2(gemm_trans << " > " << MAX_MEM_ALLOC_SZ);
        return 0;
    }
    return gemm_trans;
#else
    std::ignore = context;
    std::ignore = problem;

    return 0;
#endif
}

bool GemmBwd1x1_stride2::IsApplicable(const ExecutionContext& context,
                                      const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    if(!GemmBwdBase::IsApplicable(context, problem))
        return false;

    const auto& conv  = problem.GetConv();
    const auto& wDesc = problem.GetWeights();

    const auto spatial_dim = conv.GetSpatialDimension();
    const auto wei_spatial = boost::adaptors::slice(wDesc.GetLengths(), 2, 2 + spatial_dim);

    return conv.GetSpatialDimension() == 2 &&
           miopen::all_of(wei_spatial, [](auto v) { return v == 1; }) &&
           miopen::all_of(conv.GetConvPads(), [](auto v) { return v == 0; }) &&
           miopen::all_of(conv.GetConvStrides(), [](auto v) { return v == 2; }) &&
           GetWorkspaceSize(context, problem) > 0;
#else
    std::ignore = context;
    std::ignore = problem;
    return false;
#endif
}

ConvSolution GemmBwd1x1_stride2::GetSolution(const ExecutionContext& context,
                                             const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    const auto& dyDesc = problem.GetIn();
    const auto& wDesc  = problem.GetWeights();
    const auto& dxDesc = problem.GetOut();
    const auto& conv   = problem.GetConv();

    const auto group_count = conv.group_count;

    GemmDescriptor gemm_desc =
        group_count > 1
            ? CreateGemmDescriptorGroupConvCNHWBwdData(wDesc, dyDesc, dxDesc, group_count)
            : CreateGemmDescriptorConvCNHWBwdData(wDesc, dyDesc, dxDesc);

    std::size_t in_n, in_c;
    std::tie(in_n, in_c) = tie_pick<0, 1>()(dxDesc.GetLengths());

    const auto wei_k        = wDesc.GetLengths()[0];
    const auto spatial_dim  = conv.GetSpatialDimension();
    const auto in_spatial_  = boost::adaptors::slice(dxDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto out_spatial_ = boost::adaptors::slice(dyDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto in_spatial   = std::vector<std::size_t>(in_spatial_.begin(), in_spatial_.end());
    const auto out_spatial  = std::vector<std::size_t>(out_spatial_.begin(), out_spatial_.end());
    const auto strides      = conv.GetConvStrides();

    const auto workspace_req = GetWorkspaceSize(context, problem);

    auto solution        = ConvSolution{miopenStatusSuccess};
    solution.workspce_sz = workspace_req;

    solution.invoker_factory = [=](const std::vector<Kernel>&) {
        const bool time_precision = (!IsDisabled(MIOPEN_CONV_PRECISE_ROCBLAS_TIMING{}));

        return [=](const Handle& handle, const AnyInvokeParams& primitive_params) {
            const auto& conv_params    = primitive_params.CastTo<conv::DataInvokeParams>();
            const auto& workspace      = conv_params.workSpace;
            const auto& workspace_size = conv_params.workSpaceSize;
            const auto& dy             = conv_params.tensors.in;
            const auto& dyDesc_        = conv_params.tensors.inDesc;
            const auto& w              = conv_params.tensors.w;
            const auto& dx             = conv_params.tensors.out;
            const auto& dxDesc_        = conv_params.tensors.outDesc;

            if(group_count > 1)
            {
                MIOPEN_LOG_FUNCTION("groupconv, 1x1 u2xv2");
            }
            else
            {
                MIOPEN_LOG_FUNCTION("convolution, 1x1 u2xv2");
            }

            if((workspace_req > 0 && workspace == nullptr) || workspace_size < workspace_req)
                MIOPEN_THROW("Not enough workspace for GemmBwd1x1_stride2. (" +
                             std::to_string(workspace_size) + " < " +
                             std::to_string(workspace_req) + ")");

            // Initialization required for upsampling in bwd direction
            float zero = 0.f;
            SetTensor(handle, dxDesc_, dx, &zero);

            float time_gemm = 0;
            if(handle.IsProfilingEnabled())
                time_gemm = handle.GetKernelTime();

            // dx = CNHW2NCHW(transpose(w) * NCHW2CNHW(dy))
            transpose_NCHW2CNHW(handle,
                                in_n,
                                wei_k,
                                out_spatial[0],
                                out_spatial[1],
                                out_spatial[0],
                                out_spatial[1],
                                dy,
                                workspace,
                                0,
                                0,
                                1,
                                1,
                                dyDesc_.GetType());

            if(handle.IsProfilingEnabled())
                time_gemm += handle.GetKernelTime();

            miopenStatus_t gemm_status;

            if(conv_params.type == InvokeType::Run)
            {
                if(group_count > 1)
                    gemm_status = CallGemmStridedBatched(handle,
                                                         gemm_desc,
                                                         w,
                                                         0,
                                                         workspace,
                                                         0,
                                                         workspace,
                                                         dyDesc_.GetElementSize(),
                                                         nullptr);
                else
                    // tensors.dx = CNHW2NCHW(transpose(tensors.w) * NCHW2CNHW(tensors.dy))
                    gemm_status = CallGemm(handle,
                                           gemm_desc,
                                           w,
                                           0,
                                           workspace,
                                           0,
                                           workspace,
                                           dyDesc_.GetElementSize(),
                                           nullptr);
            }
            else
            {
                gemm_status =
                    CallGemmTimeMeasure(handle,
                                        gemm_desc,
                                        w,
                                        0,
                                        workspace,
                                        0,
                                        workspace,
                                        dyDesc_.GetElementSize(),
                                        nullptr,
                                        time_precision,
                                        group_count > 1 ? callGemmStridedBatched : callGemm);
            }

            if(gemm_status != miopenStatusSuccess)
                MIOPEN_THROW("GemmBwd1x1_stride2 execution failure.");

            if(handle.IsProfilingEnabled())
                time_gemm += handle.GetKernelTime();

            transpose_CNHW2NCHW(handle,
                                in_n,
                                in_c,
                                out_spatial[0],
                                out_spatial[1],
                                in_spatial[0],
                                in_spatial[1],
                                workspace,
                                dx,
                                dyDesc_.GetElementSize(),
                                0,
                                strides[0],
                                strides[1],
                                dyDesc_.GetType());

            if(handle.IsProfilingEnabled())
            {
                time_gemm += handle.GetKernelTime();
                handle.ResetKernelTime();
                handle.AccumKernelTime(time_gemm);
            }
        };
    };

    return solution;
#else
    std::ignore = context;
    std::ignore = problem;
    return {};
#endif
}

size_t GemmBwd1x1_stride1::GetWorkspaceSize(const ExecutionContext&,
                                            const conv::ProblemDescription&) const
{
    return 0;
}

bool GemmBwd1x1_stride1::IsApplicable(const ExecutionContext& context,
                                      const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    if(!GemmBwdBase::IsApplicable(context, problem))
        return false;

    const auto& conv  = problem.GetConv();
    const auto& wDesc = problem.GetWeights();

    const auto spatial_dim = conv.GetSpatialDimension();
    const auto wei_spatial = boost::adaptors::slice(wDesc.GetLengths(), 2, 2 + spatial_dim);

    return miopen::all_of(wei_spatial, [](auto v) { return v == 1; }) &&
           miopen::all_of(conv.GetConvPads(), [](auto v) { return v == 0; }) &&
           miopen::all_of(conv.GetConvStrides(), [](auto v) { return v == 1; });
#else
    std::ignore = context;
    std::ignore = problem;
    return false;
#endif
}

ConvSolution GemmBwd1x1_stride1::GetSolution(const ExecutionContext&,
                                             const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    const auto& dyDesc = problem.GetIn();
    const auto& wDesc  = problem.GetWeights();
    const auto& dxDesc = problem.GetOut();
    const auto& conv   = problem.GetConv();

    const auto group_count = conv.group_count;
    const auto in_n        = dxDesc.GetLengths()[0];

    auto solution        = ConvSolution{miopenStatusSuccess};
    solution.workspce_sz = 0;

    // dx = transpose(w) * dy
    const auto gemm_desc =
        group_count > 1 ? CreateGemmDescriptorGroupConvBwdData(wDesc, dyDesc, dxDesc, group_count)
                        : CreateGemmStridedBatchedDescriptorConv1x1BwdData(wDesc, dyDesc, dxDesc);

    const auto in_c = dxDesc.GetLengths()[1];

    const auto wei_k = wDesc.GetLengths()[0];

    const auto spatial_dim = conv.GetSpatialDimension();
    const auto in_spatial  = boost::adaptors::slice(dxDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto out_spatial = boost::adaptors::slice(dyDesc.GetLengths(), 2, 2 + spatial_dim);

    std::size_t out_spatial_size = std::accumulate(
        out_spatial.begin(), out_spatial.end(), std::size_t(1), std::multiplies<std::size_t>());

    std::size_t in_spatial_size = std::accumulate(
        in_spatial.begin(), in_spatial.end(), std::size_t(1), std::multiplies<std::size_t>());

    solution.invoker_factory = [=](const std::vector<Kernel>&) {
        const bool time_precision = (!IsDisabled(MIOPEN_CONV_PRECISE_ROCBLAS_TIMING{}));

        return [=](const Handle& handle, const AnyInvokeParams& primitive_params) {
            const auto& conv_params = primitive_params.CastTo<conv::DataInvokeParams>();
            const auto& dy          = conv_params.tensors.in;
            const auto& w           = conv_params.tensors.w;
            const auto& dx          = conv_params.tensors.out;

            if(group_count > 1)
            {
                MIOPEN_LOG_FUNCTION("groupconv, 1x1");
            }
            else
            {
                MIOPEN_LOG_FUNCTION("convolution, 1x1");
            }

            miopenStatus_t gemm_status = miopenStatusUnknownError;

            if(conv_params.type == InvokeType::Run)
            {
                if(group_count > 1)
                {
                    float time_0 = 0;
                    for(std::size_t i = 0; i < in_n; i++)
                    {
                        std::size_t out_offset = i * wei_k * out_spatial_size;

                        std::size_t in_offset = i * in_c * in_spatial_size;

                        gemm_status = CallGemmStridedBatched(
                            handle, gemm_desc, w, 0, dy, out_offset, dx, in_offset, nullptr);

                        if(handle.IsProfilingEnabled())
                        {
                            if(i == in_n - 1)
                                handle.AccumKernelTime(time_0);
                            time_0 += handle.GetKernelTime();
                        }
                    }
                }
                else
                {
                    gemm_status =
                        CallGemmStridedBatched(handle, gemm_desc, w, 0, dy, 0, dx, 0, nullptr);
                }
            }
            else
            {
                gemm_status = CallGemmTimeMeasure(handle,
                                                  gemm_desc,
                                                  w,
                                                  0,
                                                  dy,
                                                  0,
                                                  dx,
                                                  0,
                                                  nullptr,
                                                  time_precision,
                                                  callGemmStridedBatched);
            }

            if(gemm_status != miopenStatusSuccess)
                MIOPEN_THROW("GemmBwd1x1_stride1 execution failure.");

            if(handle.IsProfilingEnabled())
            {
                float time_gemm = handle.GetKernelTime();
                if(group_count > 1)
                    time_gemm *= in_n;
                handle.ResetKernelTime();
                handle.AccumKernelTime(time_gemm);
            }
        };
    };

    return solution;
#else
    std::ignore = problem;
    return {};
#endif
}

size_t GemmBwdRest::GetWorkspaceSize(const ExecutionContext& context,
                                     const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    auto& handle       = context.GetStream();
    const auto& conv   = problem.GetConv();
    const auto& dyDesc = problem.GetIn();
    const auto& wDesc  = problem.GetWeights();

    const auto spatial_dim = conv.GetSpatialDimension();

    const auto wei_spatial = boost::adaptors::slice(wDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto out_spatial = boost::adaptors::slice(dyDesc.GetLengths(), 2, 2 + spatial_dim);

    const auto wei_c = wDesc.GetLengths()[1];

    const auto gemm_size = wei_c *
                           std::accumulate(wei_spatial.begin(),
                                           wei_spatial.end(),
                                           std::size_t(1),
                                           std::multiplies<std::size_t>()) *
                           std::accumulate(out_spatial.begin(),
                                           out_spatial.end(),
                                           std::size_t(1),
                                           std::multiplies<std::size_t>()) *
                           GetTypeSize(dyDesc.GetType()) * conv.group_count;

    if(gemm_size > MAX_MEM_ALLOC_SZ)
    {
        MIOPEN_LOG_I2(gemm_size << " > " << MAX_MEM_ALLOC_SZ);
        return 0;
    }
    return gemm_size;
#else
    std::ignore = context;
    std::ignore = problem;

    return 0;
#endif
}

bool GemmBwdRest::IsApplicable(const ExecutionContext& context,
                               const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    if(!GemmBwdBase::IsApplicable(context, problem))
        return false;

    return !GemmBwd1x1_stride2{}.IsApplicable(context, problem) &&
           !GemmBwd1x1_stride1{}.IsApplicable(context, problem) &&
           GetWorkspaceSize(context, problem) > 0;
#else
    std::ignore = context;
    std::ignore = problem;
    return false;
#endif
}

ConvSolution GemmBwdRest::GetSolution(const ExecutionContext& context,
                                      const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    const auto& dyDesc = problem.GetIn();
    const auto& wDesc  = problem.GetWeights();
    const auto& dxDesc = problem.GetOut();
    const auto& conv   = problem.GetConv();

    const auto group_count = conv.group_count;

    const auto spatial_dim  = conv.GetSpatialDimension();
    const auto in_spatial_  = boost::adaptors::slice(dxDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto wei_spatial_ = boost::adaptors::slice(wDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto out_spatial_ = boost::adaptors::slice(dyDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto in_spatial   = std::vector<std::size_t>(in_spatial_.begin(), in_spatial_.end());
    const auto wei_spatial  = std::vector<std::size_t>(wei_spatial_.begin(), wei_spatial_.end());
    const auto out_spatial  = std::vector<std::size_t>(out_spatial_.begin(), out_spatial_.end());

    // dx = transpose(w) * dy
    const auto gemm_desc =
        group_count > 1 ? CreateGemmDescriptorGroupConvBwdData(wDesc, dyDesc, dxDesc, group_count)
                        : CreateGemmDescriptorConvBwdData(wDesc, dyDesc, dxDesc);

    const auto spatial_dims = conv.GetSpatialDimension();
    const auto pads         = conv.GetConvPads();
    const auto strides      = conv.GetConvStrides();
    const auto dilations    = conv.GetConvDilations();

    const auto in_n  = dxDesc.GetLengths()[0];
    const auto in_c  = dxDesc.GetLengths()[1];
    const auto wei_k = wDesc.GetLengths()[0];

    const auto out_spatial_size = std::accumulate(
        out_spatial.begin(), out_spatial.end(), std::size_t(1), std::multiplies<std::size_t>());

    const auto in_spatial_size = std::accumulate(
        in_spatial.begin(), in_spatial.end(), std::size_t(1), std::multiplies<std::size_t>());

    const auto workspace_req = GetWorkspaceSize(context, problem);

    auto solution        = ConvSolution{miopenStatusSuccess};
    solution.workspce_sz = workspace_req;

    solution.invoker_factory = [=](const std::vector<Kernel>&) {
        const bool time_precision = (!IsDisabled(MIOPEN_CONV_PRECISE_ROCBLAS_TIMING{}));

        return [=](const Handle& handle, const AnyInvokeParams& primitive_params) {
            const auto& conv_params    = primitive_params.CastTo<conv::DataInvokeParams>();
            const auto& workspace      = conv_params.workSpace;
            const auto& workspace_size = conv_params.workSpaceSize;
            const auto& dy             = conv_params.tensors.in;
            const auto& dyDesc_        = conv_params.tensors.inDesc;
            const auto& w              = conv_params.tensors.w;
            const auto& dx             = conv_params.tensors.out;

            if(group_count > 1)
            {
                MIOPEN_LOG_FUNCTION("groupconv, non 1x1");
            }
            else
            {
                MIOPEN_LOG_FUNCTION("convolution, non 1x1");
            }

            if((workspace_req > 0 && workspace == nullptr) || workspace_size < workspace_req)
                MIOPEN_THROW("Not enough workspace for GemmBwdRest. (" +
                             std::to_string(workspace_size) + " < " +
                             std::to_string(workspace_req) + ")");

            if(conv_params.type == InvokeType::Run)
            {
                float time_gemm = 0;

                for(std::size_t i = 0; i < in_n; i++)
                {
                    std::size_t out_offset = i * wei_k * out_spatial_size;
                    std::size_t in_offset  = i * in_c * in_spatial_size;

                    miopenStatus_t gemm_status;

                    // tensors.dx = transpose(tensors.w) * tensors.dy
                    if(group_count > 1)
                        gemm_status = CallGemmStridedBatched(
                            handle, gemm_desc, w, 0, dy, out_offset, workspace, 0, nullptr);
                    else
                        gemm_status = CallGemm(handle,
                                               gemm_desc,
                                               w,
                                               0,
                                               dy,
                                               out_offset,
                                               workspace,
                                               0,
                                               nullptr,
                                               GemmBackend_t::miopengemm);

                    if(gemm_status != miopenStatusSuccess)
                        MIOPEN_THROW("GemmBwdRest execution failure.");

                    if(handle.IsProfilingEnabled())
                        time_gemm += handle.GetKernelTime();

                    time_gemm += Col2ImGPU(handle,
                                           spatial_dims,
                                           workspace,
                                           out_spatial,
                                           wei_spatial,
                                           pads,
                                           strides,
                                           dilations,
                                           in_c,
                                           in_spatial,
                                           dx,
                                           in_offset,
                                           dyDesc_.GetType());
                }

                if(handle.IsProfilingEnabled())
                {
                    handle.ResetKernelTime();
                    handle.AccumKernelTime(time_gemm);
                }
            }
            else
            {
                int in_offset = 0;

                miopenStatus_t gemm_status = CallGemmTimeMeasure(
                    handle,
                    gemm_desc,
                    w,
                    0,
                    dy,
                    0,
                    workspace,
                    0,
                    nullptr,
                    time_precision,
                    group_count > 1 ? callGemmStridedBatched : callGemm,
                    group_count > 1 ? GemmBackend_t::miopentensile : GemmBackend_t::miopengemm);

                if(gemm_status != miopenStatusSuccess)
                    MIOPEN_THROW("GemmBwdRest execution failure.");

                float time_gemm = 0;

                if(handle.IsProfilingEnabled())
                    time_gemm = in_n * handle.GetKernelTime();

                const auto time_col2im = Col2ImGPU(handle,
                                                   spatial_dims,
                                                   workspace,
                                                   out_spatial,
                                                   wei_spatial,
                                                   pads,
                                                   strides,
                                                   dilations,
                                                   in_c,
                                                   in_spatial,
                                                   dx,
                                                   in_offset,
                                                   dyDesc_.GetType());

                if(handle.IsProfilingEnabled())
                {
                    time_gemm += in_n * time_col2im;
                    handle.ResetKernelTime();
                    handle.AccumKernelTime(time_gemm);
                }
            }
        };
    };

    return solution;
#else
    std::ignore = context;
    std::ignore = problem;
    return {};
#endif
}

} // namespace solver
} // namespace miopen


#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <armadillo>
#include "model_car_localization.h"

using namespace arma;

int initStateFile(char *file)
{
    std::ofstream out(file,std::ios_base::trunc);

    if (!out)
    {
        perror("output file error\n");
        return -1;
    }

    out.close();

    return 0;
}

void addState(char *file, fmat u)
{
    std::ofstream out(file,std::ios_base::app);
    for(unsigned int i=0; i < u.n_rows; ++i){
        out << u[i] << " ";
    }
    out << std::endl;
    out.close();
}

void extractWindow(cv::Mat* map, cv::Mat* window, float posX, float posY, float phi ,int sizeX, int sizeY){
    float x;
    float y;
    float tempPhi = CV_PI-0.05;
    for (int i = 0; i < sizeY; ++i){
        for (int j=0; j< sizeX;++j){
            x = j - (sizeX/2);
            y = i;
            int xSinPhi = x*sinf(tempPhi);
            int xCosPhi = x*cosf(tempPhi);
            int yCosPhi = y*cosf(tempPhi);
            int ySinPhi = y*sinf(tempPhi);
            window->at<cv::Vec3b>(i,j)[0] = map->at<cv::Vec3b>(posY-xSinPhi+yCosPhi, posX+xCosPhi-ySinPhi)[0];
            window->at<cv::Vec3b>(i,j)[1] = map->at<cv::Vec3b>(posY-xSinPhi+yCosPhi, posX+xCosPhi-ySinPhi)[1];
            window->at<cv::Vec3b>(i,j)[2] = map->at<cv::Vec3b>(posY-xSinPhi+yCosPhi, posX+xCosPhi-ySinPhi)[2];
        }
    }

    //return window;

}

std::vector<float> extractRightLinePositions(cv::Mat* view){
    std::vector<float> spline(view->rows);
    uchar maxval = 0;
    uchar maxPosX = 0;
    bool foundPointInLine = false;

    for (int i = 0; i < view->rows; ++i){
        maxval = 0;
        foundPointInLine = false;
        for (int j=0; j<view->cols;++j){
            if ( maxval < view->at<uchar>(i, j)) {
                foundPointInLine = true;
                maxval = view->at<uchar>(i, j);
                maxPosX = j;
            }

        }
        if (foundPointInLine){
            spline.push_back(maxPosX);
            spline.push_back(i);
        }
    }

    return spline;
}

int main( int argc, char** argv )
{
    /*if (argc<2){
        perror("Error");
        printf("Please call program like:\n");
        printf("./datagen filename_original filename_measurement numberOfStates\n");
        return -1;
    }*/

    int windowWidth = 60;
    int windowHeight = 60;
    int num = 50;

    //char *file_original = argv[1];
    //char *file_out = argv[2];
    num = atoi(argv[1]);

    char* fileAddress_truth=(char*)"track_truth.png";
    cv::Mat map = cv::imread(fileAddress_truth);
    cv::Mat correctMap = cv::Mat::zeros(map.rows,map.cols,CV_8UC3);

    for (int i=0; i< map.rows; i++){
        for(int j=0; j< map.cols; j++){
            correctMap.at<cv::Vec3b>(i,j)[0] = map.at<cv::Vec3b>(map.rows-1-i, j)[0];
            correctMap.at<cv::Vec3b>(i,j)[1] = map.at<cv::Vec3b>(map.rows-1-i, j)[1];
            correctMap.at<cv::Vec3b>(i,j)[2] = map.at<cv::Vec3b>(map.rows-1-i, j)[2];
        }
    }

    cv::Mat particles = cv::Mat::zeros(correctMap.rows,correctMap.cols, CV_8UC3);

    cv::Mat window = cv::Mat::zeros(windowHeight,windowWidth, CV_8UC3);

    cv::Mat lanes = cv::imread("track_truth.png");

    cv::VideoWriter record("simRobotVideo.avi", CV_FOURCC('D','I','V','X'), 30, cv::Size(500,500), true);
    if( !record.isOpened() )
    {
        printf("VideoWriter failed to open!\n");
        return -1;
    }

    cv::VideoWriter positions("posRobotVideo.avi", CV_FOURCC('D','I','V','X'), 30, lanes.size(), true);

    //std::vector<float> spline;

    //float startX = 10;
    //float startY = 10;
    //float startPhi = 0;

    ModelCarLocalization *process = new ModelCarLocalization();
    process->initialize();

    printf("Dimension of state: %d\n",process->getProcessDimension());

    fmat state(process->getProcessDimension(),1);

    //fmat measurement(process->getMeasurementDimension(),1);

    state.zeros();
    state(0) = 428;
    state(1) = 605;
    state(2) = CV_PI-0.05;

    //initStateFile(file_out);	// initialize output file
    //initStateFile(file_original);

    //addState(file_out, state);

    process->speed(0) = 4;
    process->speed(1) = 0;
    process->speed(2) = 0;

    for (int k=0; k<num;k++)
    {
        // generate a new position
        state = process->ffun(&state);
        printf("x: %e  y: %e\n",state(0),state(1));
        //addState(file_original,state);

        float x;
        float y;
        float tempPhi = CV_PI+0.75;
        for (int i = 0; i < windowHeight; ++i){
            for (int j=0; j< windowWidth;++j){
                x = j - (windowWidth/2);
                y = i;
                int xSinPhi = x*sinf(tempPhi);
                int xCosPhi = x*cosf(tempPhi);
                int yCosPhi = y*cosf(tempPhi);
                int ySinPhi = y*sinf(tempPhi);
                window.at<cv::Vec3b>(windowHeight-1-i,j)[0] = correctMap.at<cv::Vec3b>(state(1)-xSinPhi+yCosPhi, state(0)+xCosPhi+ySinPhi)[0];
                window.at<cv::Vec3b>(windowHeight-1-i,j)[1] = correctMap.at<cv::Vec3b>(state(1)-xSinPhi+yCosPhi, state(0)+xCosPhi+ySinPhi)[1];
                window.at<cv::Vec3b>(windowHeight-1-i,j)[2] = correctMap.at<cv::Vec3b>(state(1)-xSinPhi+yCosPhi, state(0)+xCosPhi+ySinPhi)[2];
            }
        }

        imshow("Window", window);
        cvWaitKey(2);

        //extractWindow(&correctMap, &window, float(state(0)), float(state(1)), float(state(2)), windowWidth, windowHeight );

        //spline = extractRightLinePositions(&window);

        //measurement = zeros<fmat>(spline.size());

        //for (unsigned int i = 0; i < spline.size(); ++i){
        //    measurement(i) = spline.at(i);
        //}

        // extract points from map
        //measurement = process->hfun(&state);
        //printf("x: %e  y: %e\n\n",measurement(0),measurement(1));
        //addState(file_out, measurement);

        cv::circle(particles,cv::Point(int(state(0)),particles.rows-1-(state(1))),2,CV_RGB(255,0,0),2);
        positions << (map+particles);
        record << window;

    }

    printf("Done!");

    return 0;

}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_SoftElasticElectronScatteringDistribution.hpp
//! \author Luke Kersting
//! \brief  The soft electron elastic scattering distribution base class
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_SOFT_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP
#define MONTE_CARLO_SOFT_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "MonteCarlo_ElectronState.hpp"
#include "MonteCarlo_ParticleBank.hpp"
#include "MonteCarlo_ElectronScatteringDistribution.hpp"
#include "MonteCarlo_AdjointElectronScatteringDistribution.hpp"
#include "Utility_TabularOneDDistribution.hpp"

namespace MonteCarlo{

//! The soft scattering distribution base class
class SoftElasticElectronScatteringDistribution : public ElectronScatteringDistribution,
                                                  public AdjointElectronScatteringDistribution
{

public:

  //! Typedef for the elastic discrete distribution
  typedef Teuchos::Array<Utility::Pair< double,
		       Teuchos::RCP<const Utility::TabularOneDDistribution> > >
  ElasticDistribution;

  //! Constructor
  SoftElasticElectronScatteringDistribution(
                 const int atomic_number,
                 const double mu_cutoff,
                 const ElasticDistribution& discrete_scattering_distribution );

  //! Destructor
  virtual ~SoftElasticElectronScatteringDistribution()
  { /* ... */ }

  //! Sample an outgoing energy and direction from the distribution
  void sample( const double incoming_energy,
               double& outgoing_energy,
               double& scattering_angle_cosine ) const;

  //! Sample an outgoing energy and direction and record the number of trials
  void sampleAndRecordTrials( const double incoming_energy,
                              double& outgoing_energy,
                              double& scattering_angle_cosine,
                              unsigned& trials ) const;

  //! Randomly scatter the electron
  void scatterElectron( ElectronState& electron,
                        ParticleBank& bank,
                        SubshellType& shell_of_interaction ) const;

  //! Randomly scatter the adjoint electron
  void scatterAdjointElectron( AdjointElectronState& adjoint_electron,
                               ParticleBank& bank,
                               SubshellType& shell_of_interaction ) const;

protected:

   //! Sample an outgoing direction from the distribution
  void sampleAndRecordTrialsImpl( const double incoming_energy,
                                  double& scattering_angle_cosine,
                                  unsigned& trials ) const;

private:

  // elastic discrete scattering distribution
  ElasticDistribution d_discrete_scattering_distribution;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_SOFT_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_ElasticElectronScatteringDistribution.hpp
//---------------------------------------------------------------------------//


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ABS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ABS_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/abs.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/constants/mzero.hpp>
#include <boost/simd/include/constants/maxleftshift.hpp>
#include <boost/simd/include/functions/simd/bitwise_notand.hpp>
#include <boost/simd/include/functions/simd/shri.hpp>
#include <boost/simd/include/functions/simd/negate.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::abs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<signed_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<result_type, signed>::type int_type;
      typedef typename meta::scalar_of<int_type>::type                      sint_type;
      result_type const s = shri(a0, Maxleftshift<sint_type>());
      return (a0-s)^(-s);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::abs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<unsigned_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1) { return a0; }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::abs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return bitwise_notand(Mzero<result_type>(),a0);
    }
  };
} } }

#endif


// ======================================================================
/*!
 * \file NFmiGdalArea.cpp
 * \brief Implementation of class NFmiGdalArea
 */
// ======================================================================

#ifndef DISABLED_GDAL

#include "NFmiGdalArea.h"
#include "NFmiString.h"
#include "NFmiStringTools.h"
#include <boost/functional/hash.hpp>
#include <boost/math/constants/constants.hpp>
#include <gis/CoordinateTransformation.h>
#include <gis/OGR.h>
#include <gis/SpatialReference.h>
#include <macgyver/Exception.h>
#include <cmath>
#include <iomanip>
#include <ogr_spatialref.h>

using namespace std;

// See also NFmiLatLonArea::WKT()
std::string fmiwkt =
    R"(GEOGCS["FMI_Sphere",DATUM["FMI_2007",SPHEROID["FMI_Sphere",6371220,0]],PRIMEM["Greenwich",0],UNIT["Degree",0.0174532925199433]])";

// ----------------------------------------------------------------------
/*!
 * \brief Destructor does nothing special
 */
// ----------------------------------------------------------------------

NFmiGdalArea::~NFmiGdalArea() = default;
// ----------------------------------------------------------------------
/*!
 * \brief Default constructor
 */
// ----------------------------------------------------------------------

NFmiGdalArea::NFmiGdalArea() : NFmiArea(), itsDatum("WGS84") {}

// ----------------------------------------------------------------------
/*!
 * \brief Copy constructor
 */
// ----------------------------------------------------------------------

NFmiGdalArea::NFmiGdalArea(const NFmiGdalArea &theArea)

    = default;

// ----------------------------------------------------------------------
/*!
 * \brief Assignment operator
 */
// ----------------------------------------------------------------------

NFmiGdalArea &NFmiGdalArea::operator=(const NFmiGdalArea &theArea) = default;

// ----------------------------------------------------------------------
/*!
 * \brief Construct from user input
 */
// ----------------------------------------------------------------------

NFmiGdalArea::NFmiGdalArea(const std::string &theDatum,
                           const std::string &theDescription,
                           const NFmiPoint &theBottomLeftLatLon,
                           const NFmiPoint &theTopRightLatLon,
                           const NFmiPoint &theTopLeftXY,
                           const NFmiPoint &theBottomRightXY,
                           bool usePacificView)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsDatum(theDatum),
      itsDescription(theDescription),
      itsBottomLeftLatLon(theBottomLeftLatLon),
      itsTopRightLatLon(theTopRightLatLon),
      itsWorldRect()
{
  try
  {
    init();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Construct from user input
 */
// ----------------------------------------------------------------------

NFmiGdalArea::NFmiGdalArea(const std::string &theDatum,
                           const OGRSpatialReference &theCRS,
                           const NFmiPoint &theBottomLeftLatLon,
                           const NFmiPoint &theTopRightLatLon,
                           const NFmiPoint &theTopLeftXY,
                           const NFmiPoint &theBottomRightXY,
                           bool usePacificView)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsDatum(theDatum),
      itsDescription(),
      itsBottomLeftLatLon(theBottomLeftLatLon),
      itsTopRightLatLon(theTopRightLatLon),
      itsWorldRect()
{
  try
  {
    itsProjStr = Fmi::OGR::exportToProj(theCRS);
    itsSpatialReference = std::make_shared<Fmi::SpatialReference>(itsProjStr);

    // Guess a good value for itsDescription

    const char *auth = theCRS.GetAuthorityName(nullptr);
    if (auth != nullptr)
    {
      itsDescription = std::string(auth);
      const char *code = theCRS.GetAuthorityCode(nullptr);
      if (code != nullptr)
        itsDescription += ":" + std::string(code);
    }
    else
    {
      itsDescription = Fmi::OGR::exportToWkt(theCRS);
    }

    init();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Construct from bounding box
 */
// ----------------------------------------------------------------------

NFmiGdalArea::NFmiGdalArea(const std::string &theDatum,
                           const OGRSpatialReference &theCRS,
                           double theXmin,
                           double theYmin,
                           double theXmax,
                           double theYmax,
                           const NFmiPoint &theTopLeftXY,
                           const NFmiPoint &theBottomRightXY,
                           bool usePacificView)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsDatum(theDatum),
      itsDescription(),
      itsBottomLeftLatLon(),
      itsTopRightLatLon(),
      itsWorldRect(NFmiPoint(theXmin, theYmin), NFmiPoint(theXmax, theYmax))
{
  try
  {
    itsProjStr = Fmi::OGR::exportToProj(theCRS);
    itsSpatialReference = std::make_shared<Fmi::SpatialReference>(itsProjStr);

    // Guess a good value for itsDescription

    const char *auth = theCRS.GetAuthorityName(nullptr);
    if (auth != nullptr)
    {
      itsDescription = std::string(auth);
      const char *code = theCRS.GetAuthorityCode(nullptr);
      if (code != nullptr)
        itsDescription += ":" + std::string(code);
    }
    else
    {
      itsDescription = Fmi::OGR::exportToWkt(theCRS);
    }

    // The needed spatial references

    OGRErr err;
    std::shared_ptr<OGRSpatialReference> datum(new OGRSpatialReference);
    if (itsDatum == "FMI")
      err = datum->SetFromUserInput(fmiwkt.c_str());
    else
      err = datum->SetFromUserInput(itsDatum.c_str());

    if (err != OGRERR_NONE)
      throw Fmi::Exception(BCP, "Failed to set datum: '" + itsDatum + "'");

    // The needed coordinate transformations

    itsWorldXYToLatLonTransformation =
        std::make_shared<Fmi::CoordinateTransformation>(*itsSpatialReference, *datum);
    itsLatLonToWorldXYTransformation =
        std::make_shared<Fmi::CoordinateTransformation>(*datum, *itsSpatialReference);

    // Bottom left and top right coordinates - needed only for geographic projections (Width()
    // calculations)
    // The same data could be extracted from the WorldXYRect too though - and these variables
    // would not be needed.

    double x1 = theXmin;
    double y1 = theYmin;
    double x2 = theXmax;
    double y2 = theYmax;
    itsWorldXYToLatLonTransformation->transform(x1, y1);
    itsWorldXYToLatLonTransformation->transform(x2, y2);
    itsBottomLeftLatLon = NFmiPoint(x1, y1);
    itsTopRightLatLon = NFmiPoint(x2, y2);

    // Initialize itsWKT
    itsWKT = Fmi::OGR::exportToWkt(*itsSpatialReference);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Return class ID
 */
// ----------------------------------------------------------------------

unsigned long NFmiGdalArea::ClassId() const
{
  try
  {
    return kNFmiGdalArea;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Return class name
 */
// ----------------------------------------------------------------------

const char *NFmiGdalArea::ClassName() const
{
  try
  {
    return "kNFmiGdalArea";
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ----------------------------------------------------------------------
/*!
 * \brief Return a clone
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiGdalArea::Clone() const
{
  try
  {
    return new NFmiGdalArea(*this);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ----------------------------------------------------------------------
/*!
 * \brief Area descriptor
 */
// ----------------------------------------------------------------------

const std::string NFmiGdalArea::AreaStr() const
{
  try
  {
    std::ostringstream out;
    out << itsDatum << ':' << itsDescription << "|" << BottomLeftLatLon().X() << ","
        << BottomLeftLatLon().Y() << "," << TopRightLatLon().X() << "," << TopRightLatLon().Y();
    return out.str();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ----------------------------------------------------------------------
/*!
 * \brief Datum
 */
// ----------------------------------------------------------------------

const std::string &NFmiGdalArea::Datum() const
{
  try
  {
    return itsDatum;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ----------------------------------------------------------------------
/*!
 * \brief Return the WKT description
 */
// ----------------------------------------------------------------------

const std::string NFmiGdalArea::WKT() const
{
  try
  {
    return itsWKT;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ----------------------------------------------------------------------
/*!
 * \brief Equality comparison
 */
// ----------------------------------------------------------------------

bool NFmiGdalArea::operator==(const NFmiGdalArea &theArea) const
{
  try
  {
    return (itsBottomLeftLatLon == theArea.itsBottomLeftLatLon &&
            itsTopRightLatLon == theArea.itsTopRightLatLon &&
            itsWorldRect == theArea.itsWorldRect && itsDescription == theArea.itsDescription &&
            itsDatum == theArea.itsDatum);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Inequality comparison
 */
// ----------------------------------------------------------------------

bool NFmiGdalArea::operator!=(const NFmiGdalArea &theArea) const
{
  try
  {
    return !(*this == theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Equality comparison
 */
// ----------------------------------------------------------------------

bool NFmiGdalArea::operator==(const NFmiArea &theArea) const
{
  try
  {
    return *this == static_cast<const NFmiGdalArea &>(theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Inequality comparison
 */
// ----------------------------------------------------------------------

bool NFmiGdalArea::operator!=(const NFmiArea &theArea) const
{
  try
  {
    return !(*this == theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Write the projection definition to a file
 */
// ----------------------------------------------------------------------

std::ostream &NFmiGdalArea::Write(std::ostream &file) const
{
  try
  {
    NFmiString tmp1 = itsDatum;
    NFmiString tmp2 = itsDescription;

    NFmiArea::Write(file);
    file << itsBottomLeftLatLon << itsTopRightLatLon << tmp1 << tmp2;

    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Read new projection definition from the input stream
 */
// ----------------------------------------------------------------------

std::istream &NFmiGdalArea::Read(std::istream &file)
{
  try
  {
    NFmiString tmp1;
    NFmiString tmp2;

    NFmiArea::Read(file);
    file >> itsBottomLeftLatLon >> itsTopRightLatLon >> tmp1 >> tmp2;

    itsDatum = tmp1.CharPtr();
    itsDescription = tmp2.CharPtr();

    init();

    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief XY coordinate to LatLon
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiGdalArea::ToLatLon(const NFmiPoint &theXYPoint) const
{
  try
  {
    double xscale = Width() / itsWorldRect.Width();
    double yscale = Height() / itsWorldRect.Height();

    double worldx = itsWorldRect.Left() + (theXYPoint.X() - Left()) / xscale;
    double worldy = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / yscale;

    return WorldXYToLatLon(NFmiPoint(worldx, worldy));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief LatLon to XY-coordinate
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiGdalArea::ToXY(const NFmiPoint &theLatLonPoint) const
{
  try
  {
    NFmiPoint latlon(FixLongitude(theLatLonPoint.X()), theLatLonPoint.Y());
    NFmiPoint worldxy = LatLonToWorldXY(latlon);

    double xscale = Width() / itsWorldRect.Width();
    double yscale = Height() / itsWorldRect.Height();

    double x = Left() + xscale * (worldxy.X() - itsWorldRect.Left());
    double y = Top() + yscale * (itsWorldRect.Bottom() - worldxy.Y());

    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief XY-coordinate to World coordinates
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiGdalArea::XYToWorldXY(const NFmiPoint &theXYPoint) const
{
  try
  {
    double xscale = Width() / itsWorldRect.Width();
    double yscale = Height() / itsWorldRect.Height();

    double worldx = itsWorldRect.Left() + (theXYPoint.X() - Left()) / xscale;
    double worldy = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / yscale;

    return NFmiPoint(worldx, worldy);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theWorldXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiGdalArea::WorldXYToXY(const NFmiPoint &theWorldXYPoint) const
{
  try
  {
    double xscale = Width() / itsWorldRect.Width();
    double yscale = Height() / itsWorldRect.Height();

    double x = xscale * (theWorldXYPoint.X() - itsWorldRect.Left()) + Left();
    double y = Top() - yscale * (theWorldXYPoint.Y() - itsWorldRect.Bottom());
    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief World coordinates to LatLon
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiGdalArea::WorldXYToLatLon(const NFmiPoint &theXYPoint) const
{
  try
  {
    if (!itsWorldXYToLatLonTransformation)
      throw Fmi::Exception(BCP, "Trying to use an uninitialized GDAL area");

    double x = theXYPoint.X();
    double y = theXYPoint.Y();
    if (!itsWorldXYToLatLonTransformation->transform(x, y))
      return NFmiPoint(kFloatMissing, kFloatMissing);

    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief LatLon to world coordinates
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiGdalArea::LatLonToWorldXY(const NFmiPoint &theLatLonPoint) const
{
  try
  {
    if (!itsLatLonToWorldXYTransformation)
      throw Fmi::Exception(BCP, "Trying to use an uninitialized GDAL area");

    double x = theLatLonPoint.X();
    double y = theLatLonPoint.Y();
    if (!itsLatLonToWorldXYTransformation->transform(x, y))
      return NFmiPoint(kFloatMissing, kFloatMissing);

    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Return the world rectangle
 */
// ----------------------------------------------------------------------

const NFmiRect NFmiGdalArea::WorldRect() const
{
  try
  {
    return itsWorldRect;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiGdalArea::NewArea(const NFmiPoint &theBottomLeftLatLon,
                                const NFmiPoint &theTopRightLatLon,
                                bool allowPacificFix) const
{
  try
  {
    if (allowPacificFix)
    {
      PacificPointFixerData fix =
          NFmiArea::PacificPointFixer(theBottomLeftLatLon, theTopRightLatLon);
      return new NFmiGdalArea(itsDatum,
                              itsDescription,
                              fix.itsBottomLeftLatlon,
                              fix.itsTopRightLatlon,
                              TopLeft(),
                              BottomRight(),
                              fix.fIsPacific);
    }

    return new NFmiGdalArea(itsDatum,
                            itsDescription,
                            theBottomLeftLatLon,
                            theTopRightLatLon,
                            TopLeft(),
                            BottomRight(),
                            PacificView());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Initialize the projection transformation objects
 */
// ----------------------------------------------------------------------

void NFmiGdalArea::init()
{
  try
  {
    // The needed spatial references

    if (!itsSpatialReference)
    {
      itsProjStr = itsDescription;
      itsSpatialReference = std::make_shared<Fmi::SpatialReference>(itsProjStr);
    }

    OGRErr err;
    std::shared_ptr<OGRSpatialReference> datum(new OGRSpatialReference);
    if (itsDatum == "FMI")
      err = datum->SetFromUserInput(fmiwkt.c_str());
    else
      err = datum->SetFromUserInput(itsDatum.c_str());

    if (err != OGRERR_NONE)
      throw Fmi::Exception(BCP, "Failed to set datum: '" + itsDatum + "'");

    // The needed coordinate transformations

    itsWorldXYToLatLonTransformation =
        std::make_shared<Fmi::CoordinateTransformation>(*itsSpatialReference, *datum);
    itsLatLonToWorldXYTransformation =
        std::make_shared<Fmi::CoordinateTransformation>(*datum, *itsSpatialReference);

    // The needed world XY rectangle

    itsWorldRect =
        NFmiRect(LatLonToWorldXY(itsBottomLeftLatLon), LatLonToWorldXY(itsTopRightLatLon));

    // Initialize itsWKT
    itsWKT = Fmi::OGR::exportToWkt(*itsSpatialReference);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

double NFmiGdalArea::WorldXYWidth() const
{
  try
  {
    if (!itsSpatialReference->isGeographic())
      return WorldRect().Width();

    double pi = boost::math::constants::pi<double>();
    double circumference = 2 * pi * 6371220;
    double dlon = itsTopRightLatLon.X() - itsBottomLeftLatLon.X();
    if (dlon < 0)
      dlon += 360;

    double clat = 0.5 * (itsBottomLeftLatLon.Y() + itsTopRightLatLon.Y());
    return dlon / 360 * circumference * cos(clat * pi / 180);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

double NFmiGdalArea::WorldXYHeight() const
{
  try
  {
    if (!itsSpatialReference->isGeographic())
      return WorldRect().Height();

    double pi = boost::math::constants::pi<double>();
    double circumference = 2 * pi * 6371220;
    double dlat = itsTopRightLatLon.Y() - itsBottomLeftLatLon.Y();
    return dlat / 360.0 * circumference;  // angle -> meters
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Hash value
 */
// ----------------------------------------------------------------------

std::size_t NFmiGdalArea::HashValue() const
{
  try
  {
    std::size_t hash = NFmiArea::HashValue();

    // some of these may be redundant:
    boost::hash_combine(hash, boost::hash_value(itsDatum));
    boost::hash_combine(hash, boost::hash_value(itsDescription));
    boost::hash_combine(hash, boost::hash_value(itsWKT));

    boost::hash_combine(hash, itsBottomLeftLatLon.HashValue());
    boost::hash_combine(hash, itsTopRightLatLon.HashValue());
    boost::hash_combine(hash, itsWorldRect.HashValue());

    return hash;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

#endif  // DISABLED_GDAL

// ======================================================================


#include <vector>
#include <string>
#include <iterator>
#include <stdexcept>
#include <type_traits>

#include <boost/format.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/spirit/include/phoenix_stl.hpp>
#include <boost/phoenix/bind/bind_member_function.hpp>
#include <boost/phoenix/statement/throw.hpp>

namespace sched {

namespace qi = boost::spirit::qi;

typedef std::tuple<uint8_t, uint8_t, uint8_t> Item;
typedef std::vector<Item> Field;
typedef std::tuple<Field,Field,Field,Field,Field,Field> Schedule;

template <typename Iterator = std::string::const_iterator>
struct CronGrammar: qi::grammar<Iterator, Schedule()> {

    qi::symbols<char, uint16_t> month_, day_of_week_;

    qi::rule<Iterator, uint8_t()> second_value,
                                  minute_value,
                                  hour_value,
                                  day_of_month_value,
                                  month_value,
                                  day_of_week_value,
                                  step;

    qi::rule<Iterator, Item(), qi::locals<uint8_t, uint8_t, uint8_t>> second_item,
                                                                      minute_item,
                                                                      hour_item,
                                                                      day_of_month_item,
                                                                      month_item,
                                                                      day_of_week_item;

    qi::rule<Iterator, Field()> second_field,
                                minute_field,
                                hour_field,
                                day_of_month_field,
                                month_field,
                                day_of_week_field;

    qi::rule<Iterator, Schedule()> schedule;

    CronGrammar(): CronGrammar::base_type(schedule, "cron grammar"),
        second_value(std::string("second value")),
        minute_value(std::string("minute value")),
        hour_value(std::string("hour value")),
        day_of_month_value(std::string("day of month value")),
        month_value(std::string("month value")),
        day_of_week_value(std::string("day of week value")),
        step(std::string("step value")),

        second_item(std::string("second item")),
        minute_item(std::string("minute item")),
        hour_item(std::string("hour item")),
        day_of_month_item(std::string("day of month item")),
        month_item(std::string("month item")),
        day_of_week_item(std::string("day of week item")),

        second_field(std::string("second field")),
        minute_field(std::string("minute field")),
        hour_field(std::string("hour field")),
        day_of_month_field(std::string("day of month field")),
        month_field(std::string("month field")),
        day_of_week_field(std::string("day of week field"))
    {
        using qi::on_error;
        using qi::fail;
        using namespace qi::labels;
        using boost::spirit::ushort_;
        using boost::spirit::ascii::char_;
        using boost::spirit::ascii::space;
        using boost::spirit::omit;
        using boost::spirit::eoi;
        using boost::spirit::eps;
        using boost::phoenix::push_back;
        using boost::phoenix::construct;
        using boost::phoenix::throw_;

        month_.add
            ("JAN",  1)
            ("FEB",  2)
            ("MAR",  3)
            ("APR",  4)
            ("MAY",  5)
            ("JUN",  6)
            ("JUL",  7)
            ("AUG",  8)
            ("SEP",  9)
            ("OCT", 10)
            ("NOV", 11)
            ("DEC", 12)
        ;

        day_of_week_.add
            ("SUN", 0)
            ("MON", 1)
            ("TUE", 2)
            ("WED", 3)
            ("THU", 4)
            ("FRI", 5)
            ("SAT", 6)
        ;

        second_value        %= ushort_                  [_pass = (0 <= _1 && _1 <= 59)];

        minute_value        %= ushort_                  [_pass = (0 <= _1 && _1 <= 59)];

        hour_value          %= ushort_                  [_pass = (0 <= _1 && _1 <= 23)];

        day_of_month_value  %= ushort_                  [_pass = (1 <= _1 && _1 <= 31)];

        month_value         %= ushort_ | month_         [_pass = (1 <= _1 && _1 <= 12)];

        day_of_week_value   %= ushort_ | day_of_week_   [_pass = (0 <= _1 && _1 <= 06)];

        step                %= ushort_;

        auto define_field_rule = [&] (auto& field, auto& item, auto& value) {

        item =
        (
            (
                (value >> '-' > value)          [_a = _1, _b = _2, _c = 1]
              | value                           [_a = _1, _b = _1, _c = 1]
              | char_('*')                      [_a = -1, _b = -1, _c = 1]
            )  >>
           -(
                '/' > step                      [_c = _1]
            )
        )                                       [_val = construct<Item>(_a, _b, _c)];

        field = item                            [push_back(_val, _1)]
              % ',';

        };

        define_field_rule(second_field,         second_item,        second_value);
        define_field_rule(minute_field,         minute_item,        minute_value);
        define_field_rule(hour_field,           hour_item,          hour_value);
        define_field_rule(day_of_month_field,   day_of_month_item,  day_of_month_value);
        define_field_rule(month_field,          month_item,         month_value);
        define_field_rule(day_of_week_field,    day_of_week_item,   day_of_week_value);

        schedule = eps >
        (
            second_field          > omit[space] >
            minute_field          > omit[space] >
            hour_field            > omit[space] >
            day_of_month_field    > omit[space] >
            month_field           > omit[space] >
            day_of_week_field     > omit[eoi]
        )                                           [_val = construct<Schedule>(_1, _2, _3, _4, _5, _6)];

        on_error<fail>(schedule, boost::phoenix::bind(&CronGrammar::error_handler, this, _1, _2, _3, _4));
    }

    void error_handler(const Iterator first, const Iterator last, const Iterator error_pos, const boost::spirit::info& what)
    {
        std::string tag = what.tag;
        if (tag == "omit") {
            tag = boost::get<boost::spirit::info>(what.value).tag;
        }

        throw std::invalid_argument((boost::format("expecting '%1%' at position %2%") % tag % (std::distance(first, error_pos) + 1)).str());
    }
};

Schedule parse_cron(const std::string& cron_str)
{
    sched::CronGrammar grammar;
    sched::Schedule schedule;

    auto begin = std::begin(cron_str), end = std::end(cron_str);

    if (!parse(begin, end, grammar, schedule)) {
        std::invalid_argument((boost::format("parsing error at position %1%") % (std::distance(std::begin(cron_str), begin) + 1)).str());
    }

    return std::move(schedule);
}

} // namespace sched


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <vw/Math/Matrix.h>
#include <vw/Core/Exception.h>

#include <vw/FileIO/DiskImageResourcePDS.h>
#include <vw/Cartography/GeoReferenceResourcePDS.h>
#include <vw/Cartography/Datum.h>
#include <vw/Cartography/GeoReference.h>

// For boost::split
#include <boost/algorithm/string.hpp>

namespace vw {
namespace cartography {

  bool read_pds_georeference( GeoReference& georef, DiskImageResourcePDS const& resource ) {

    std::string key, value;
    bool failed = false;

    Datum datum;
    datum.name() = "PDS Datum";
    datum.spheroid_name() = "PDS Spheroid";
    // Collect datum information
    key = "A_AXIS_RADIUS";
    if ( resource.query(key,value) ) { datum.set_semi_major_axis(atof(value.c_str()) * 1000); } else {failed = true; }
    key = "B_AXIS_RADIUS";
    if ( resource.query(key,value) ) { datum.set_semi_minor_axis(atof(value.c_str()) * 1000); } else {failed = true; }
    georef.set_datum(datum);

    // Collect projection information
    key = "MAP_PROJECTION_TYPE";
    if ( resource.query(key,value) ) {
      if (value != "\"SIMPLE CYLINDRICAL\"") {
        vw_out(ErrorMessage, "console") << "Unsupported map projection type in PDS header.";
        vw_out(ErrorMessage, "cartography") << "Unsupported map projection type in PDS header.";
        failed = true;
      }
      datum.set_semi_minor_axis(atof(value.c_str()) * 1000);
    } else {failed = true; }

    // Set affine transform
    Matrix<double,3,3> transform;
    transform.set_identity();

    key = "WESTERNMOST_LONGITUDE";
    if ( resource.query(key,value) ) { transform(0,2) = atof(value.c_str()); } else {failed = true; }
    key = "MAXIMUM_LATITUDE";
    if ( resource.query(key,value) ) { transform(1,2) = atof(value.c_str()); } else {failed = true; }
    key = "MAP_RESOLUTION";
    if ( resource.query(key,value) ) {
      transform(0,0) = 1/atof(value.c_str());
      transform(1,1) = -1/atof(value.c_str());
    } else {failed = true; }

    georef.set_transform(transform);

    return !failed;
  }

}} // namespace vw::cartography


#ifndef PARMCB_MPI_SVA_SIGNED_HPP_
#define PARMCB_MPI_SVA_SIGNED_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <cstddef>
#include <functional>
#include <iostream>
#include <iterator>
#include <limits>
#include <set>
#include <vector>

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/detail/tuple_basic.hpp>

#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/collectives.hpp>
#include <boost/mpi/timer.hpp>

#include <tbb/concurrent_vector.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_reduce.h>

#include <parmcb/detail/signed_dijkstra.hpp>
#include <parmcb/mpi/sptrees.hpp>
#include <parmcb/forestindex.hpp>
#include <parmcb/spvecgf2.hpp>
#include <parmcb/util.hpp>

namespace parmcb {

    namespace detail {

        template<class Graph, class WeightMap>
        std::tuple<std::set<typename boost::graph_traits<Graph>::edge_descriptor>,
                typename boost::property_traits<WeightMap>::value_type, bool> find_shortest_odd_cycle_mpi(
                const Graph &g, const WeightMap &weight_map,
                const std::vector<typename boost::graph_traits<Graph>::vertex_descriptor> &allVertices,
                const ForestIndex<Graph> &forest_index,
                const std::set<typename boost::graph_traits<Graph>::edge_descriptor> &signed_edges,
                boost::mpi::communicator &world) {

            typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
            typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
            typedef typename boost::property_traits<WeightMap>::value_type WeightType;

            std::less<WeightType> compare = std::less<WeightType>();
            std::tuple<std::set<Edge>, WeightType, bool> best = std::make_tuple(std::set<Edge> { },
                    (std::numeric_limits<WeightType>::max)(), false);
            typedef std::tuple<std::set<Edge>, WeightType, bool> cycle_t;
            auto cycle_min = [compare](const cycle_t &c1, const cycle_t &c2) {
                if (!std::get<2>(c1) || !std::get<2>(c2)) {
                    if (std::get<2>(c1)) {
                        return c1;
                    } else {
                        return c2;
                    }
                }
                // both valid, compare
                if (!compare(std::get<1>(c2), std::get<1>(c1))) {
                    return c1;
                }
                return c2;
            };

            if (signed_edges.size() == 1) {
                if (world.rank() == 0) {
                    auto se = *signed_edges.begin();
                    auto se_v = boost::source(se, g);
                    auto se_u = boost::target(se, g);
                    auto res = bidirectional_signed_dijkstra(g, weight_map, std::set<Edge> { }, signed_edges, true,
                            se_v, true, se_u, true, std::get<2>(best), std::get<1>(best));
                    if (std::get<2>(res) && std::get<0>(res).find(se) == std::get<0>(res).end()) {
                        std::get<1>(res) += boost::get(weight_map, se);
                        if (!std::get<2>(best) || compare(std::get<1>(res), std::get<1>(best))) {
                            std::get<0>(res).insert(se);
                            best = res;
                            assert(std::get<2>(best));
                        }
                    }
                }
            } else if (signed_edges.size() < boost::num_vertices(g)) {
                /*
                 * Heuristic in case number of signed edges is small compared to the number of vertices.
                 */
                std::map<Edge, std::set<Edge>> hidden_edges_per_edge;
                std::vector<Edge> signed_edges_as_vector;
                std::set<Edge> tmp_signed_edges = signed_edges;
                while (!tmp_signed_edges.empty()) {
                    auto bit = tmp_signed_edges.begin();
                    hidden_edges_per_edge.insert(std::make_pair(*bit, tmp_signed_edges));
                    signed_edges_as_vector.push_back(*bit);
                    tmp_signed_edges.erase(bit);
                }

                std::vector<Edge> local_signed_edges_as_vector;
                std::size_t total = signed_edges_as_vector.size();
                std::size_t stride = ceil((double) total / world.size());
                std::size_t istart = world.rank() * stride;
                std::size_t iend = istart + stride;
                for (std::size_t i = istart; i < iend && i < total; i++) {
                    local_signed_edges_as_vector.push_back(signed_edges_as_vector[i]);
                }

                std::tuple<std::set<Edge>, WeightType, bool> best_local_cycle = tbb::parallel_reduce(
                        tbb::blocked_range<std::size_t>(0, local_signed_edges_as_vector.size()),
                        std::make_tuple(std::set<Edge>(), (std::numeric_limits<WeightType>::max)(), false),
                        [&](tbb::blocked_range<std::size_t> r, auto running_min) {
                            for (std::size_t i = r.begin(); i < r.end(); i++) {
                                auto se = local_signed_edges_as_vector.at(i);
                                auto se_v = boost::source(se, g);
                                auto se_u = boost::target(se, g);
                                auto hidden_edges = hidden_edges_per_edge.at(se);
                                auto res = bidirectional_signed_dijkstra(g, weight_map, signed_edges, hidden_edges,
                                        true, se_v, true, se_u, true, std::get<2>(running_min),
                                        std::get<1>(running_min));
                                if (std::get<2>(res) && std::get<0>(res).find(se) == std::get<0>(res).end()) {
                                    std::get<1>(res) += boost::get(weight_map, se);
                                    if (!std::get<2>(running_min)
                                            || compare(std::get<1>(res), std::get<1>(running_min))) {
                                        std::get<0>(res).insert(se);
                                        running_min = res;
                                    }
                                }
                            }
                            return running_min;
                        },
                        cycle_min);

                std::vector<typename ForestIndex<Graph>::size_type> best_local_cycle_as_indices;
                convert_edges(std::get<0>(best_local_cycle),
                        std::inserter(best_local_cycle_as_indices, best_local_cycle_as_indices.end()), forest_index);
                SerializableMinOddCycle<Graph, WeightMap> local_min_odd_cycle(best_local_cycle_as_indices,
                        std::get<1>(best_local_cycle), std::get<2>(best_local_cycle));
                SerializableMinOddCycle<Graph, WeightMap> global_min_odd_cycle;

                boost::mpi::reduce(world, local_min_odd_cycle, global_min_odd_cycle,
                        SerializableMinOddCycleMinOp<Graph, WeightMap>(), 0);

                convert_edges(global_min_odd_cycle.edges, std::inserter(std::get<0>(best), std::get<0>(best).end()),
                        forest_index);
                std::get<1>(best) = global_min_odd_cycle.weight;
                std::get<2>(best) = global_min_odd_cycle.exists;
            } else {
                // split implicitly all vertices
                std::vector<Vertex> localVertices;
                std::size_t stride = ceil((double) allVertices.size() / world.size());
                std::size_t istart = world.rank() * stride;
                std::size_t iend = istart + stride;
                std::size_t total = allVertices.size();
                for (std::size_t i = istart; i < iend && i < total; i++) {
                    localVertices.push_back(allVertices[i]);
                }

                std::tuple<std::set<Edge>, WeightType, bool> best_local_cycle = tbb::parallel_reduce(
                        tbb::blocked_range<std::size_t>(0, localVertices.size()),
                        std::make_tuple(std::set<Edge>(), (std::numeric_limits<WeightType>::max)(), false),
                        [&](tbb::blocked_range<std::size_t> r, auto running_min) {
                            for (std::size_t i = r.begin(); i < r.end(); i++) {
                                auto v = localVertices[i];
                                const bool use_hidden_edges = false;
                                auto res = bidirectional_signed_dijkstra(g, weight_map, signed_edges,
                                        std::set<Edge> { }, use_hidden_edges, v, true, v, false,
                                        std::get<2>(running_min), std::get<1>(running_min));
                                if (std::get<2>(res)
                                        && (!std::get<2>(running_min)
                                                || compare(std::get<1>(res), std::get<1>(running_min)))) {
                                    running_min = res;
                                }
                            }
                            return running_min;
                        },
                        cycle_min);

                std::vector<typename ForestIndex<Graph>::size_type> best_local_cycle_as_indices;
                convert_edges(std::get<0>(best_local_cycle),
                        std::inserter(best_local_cycle_as_indices, best_local_cycle_as_indices.end()), forest_index);
                SerializableMinOddCycle<Graph, WeightMap> local_min_odd_cycle(best_local_cycle_as_indices,
                        std::get<1>(best_local_cycle), std::get<2>(best_local_cycle));
                SerializableMinOddCycle<Graph, WeightMap> global_min_odd_cycle;

                boost::mpi::reduce(world, local_min_odd_cycle, global_min_odd_cycle,
                        SerializableMinOddCycleMinOp<Graph, WeightMap>(), 0);

                convert_edges(global_min_odd_cycle.edges, std::inserter(std::get<0>(best), std::get<0>(best).end()),
                        forest_index);
                std::get<1>(best) = global_min_odd_cycle.weight;
                std::get<2>(best) = global_min_odd_cycle.exists;
            }

            return best;
        }

    } // detail

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_signed_mpi(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out, boost::mpi::communicator &world, const std::size_t hardware_concurrency_hint = 0) {

        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIt;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        /*
         * Index the graph
         */
        ForestIndex<Graph> forest_index(g);
        auto csd = forest_index.cycle_space_dimension();
        std::vector<Vertex> vertices;
        {
            VertexIt vi, viend;
            for (boost::tie(vi, viend) = boost::vertices(g); vi != viend; ++vi) {
                vertices.push_back(*vi);
            }
        }

        /*
         * Initialize support vectors
         */
        tbb::concurrent_vector<SpVecGF2<std::size_t>> support;
        tbb::parallel_for(tbb::blocked_range<std::size_t>(0, csd), [&](const tbb::blocked_range<std::size_t> &r) {
            for (std::size_t i = r.begin(); i != r.end(); ++i) {
                support.push_back(SpVecGF2<std::size_t> { i });
            }
        });

        boost::mpi::timer total_timer;

        /*
         * Main loop
         */
        WeightType mcb_weight = WeightType();
        for (std::size_t k = 0; k < csd; k++) {
            if (k % 250 == 0) {
                std::cout << "Rank " << world.rank() << " at cycle " << k << std::endl;
            }

            // TODO: check if sparsest support heuristic makes sense here

            // broadcast support vector
            if (world.rank() == 0) {
                boost::mpi::broadcast(world, support[k], 0);
            } else {
                SpVecGF2<std::size_t> received;
                boost::mpi::broadcast(world, received, 0);
                support[k] = received;
            }

            std::set<Edge> signed_edges;
            convert_edges(support[k], std::inserter(signed_edges, signed_edges.end()), forest_index);
            std::tuple<std::set<Edge>, WeightType, bool> best = parmcb::detail::find_shortest_odd_cycle_mpi(g, weight_map,
                    vertices, forest_index, signed_edges, world);

            if (world.rank() == 0) {
                /*
                 * Update support vectors
                 */
                std::set<std::size_t> cyclek;
                convert_edges(std::get<0>(best), std::inserter(cyclek, cyclek.end()), forest_index);
                tbb::parallel_for(tbb::blocked_range<std::size_t>(k + 1, csd),
                        [&](const tbb::blocked_range<std::size_t> &r) {
                            auto e = r.end();
                            for (std::size_t i = r.begin(); i != e; ++i) {
                                if (support[i] * cyclek == 1) {
                                    support[i] += support[k];
                                }
                            }
                        });

                /*
                 * Output cycles
                 */
                std::list<Edge> cyclek_edgelist;
                std::copy(std::get<0>(best).begin(), std::get<0>(best).end(), std::back_inserter(cyclek_edgelist));
                *out++ = cyclek_edgelist;
                mcb_weight += std::get<1>(best);
            }

        }

        if (world.rank() == 0) {
            std::cout << "Total time: " << total_timer.elapsed() << " (sec)" << std::endl;
        }

        return mcb_weight;
    }

} // namespace parmcb

#endif


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017, 2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_VANDG_HPP
#define BOOST_GEOMETRY_PROJECTIONS_VANDG_HPP

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct vandg {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace vandg
    {

            static const double tolerance = 1.e-10;

            template <typename T>
            inline T C2_27() { return .07407407407407407407407407407407; }
            template <typename T>
            inline T PI4_3() { return boost::math::constants::four_thirds_pi<T>(); }
            template <typename T>
            inline T TPISQ() { return 19.739208802178717237668981999752; }
            template <typename T>
            inline T HPISQ() { return 4.9348022005446793094172454999381; }

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_vandg_spheroid
                : public base_t_fi<base_vandg_spheroid<T, Parameters>, T, Parameters>
            {
                inline base_vandg_spheroid(const Parameters& par)
                    : base_t_fi<base_vandg_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T half_pi = detail::half_pi<T>();
                    static const T pi = detail::pi<T>();

                    T  al, al2, g, g2, p2;

                    p2 = fabs(lp_lat / half_pi);
                    if ((p2 - tolerance) > 1.) {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }
                    if (p2 > 1.)
                        p2 = 1.;
                    if (fabs(lp_lat) <= tolerance) {
                        xy_x = lp_lon;
                        xy_y = 0.;
                    } else if (fabs(lp_lon) <= tolerance || fabs(p2 - 1.) < tolerance) {
                        xy_x = 0.;
                        xy_y = pi * tan(.5 * asin(p2));
                        if (lp_lat < 0.) xy_y = -xy_y;
                    } else {
                        al = .5 * fabs(pi / lp_lon - lp_lon / pi);
                        al2 = al * al;
                        g = sqrt(1. - p2 * p2);
                        g = g / (p2 + g - 1.);
                        g2 = g * g;
                        p2 = g * (2. / p2 - 1.);
                        p2 = p2 * p2;
                        xy_x = g - p2; g = p2 + al2;
                        xy_x = pi * (al * xy_x + sqrt(al2 * xy_x * xy_x - g * (g2 - p2))) / g;
                        if (lp_lon < 0.) xy_x = -xy_x;
                        xy_y = fabs(xy_x / pi);
                        xy_y = 1. - xy_y * (xy_y + 2. * al);
                        if (xy_y < -tolerance) {
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        }
                        if (xy_y < 0.)
                            xy_y = 0.;
                        else
                            xy_y = sqrt(xy_y) * (lp_lat < 0. ? -pi : pi);
                    }
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T half_pi = detail::half_pi<T>();
                    static const T pi = detail::pi<T>();
                    static const T pi_sqr = detail::pi_sqr<T>();
                    static const T third = detail::third<T>();
                    static const T two_pi = detail::two_pi<T>();

                    static const T C2_27 = vandg::C2_27<T>();
                    static const T PI4_3 = vandg::PI4_3<T>();
                    static const T TPISQ = vandg::TPISQ<T>();
                    static const T HPISQ = vandg::HPISQ<T>();

                    T t, c0, c1, c2, c3, al, r2, r, m, d, ay, x2, y2;

                    x2 = xy_x * xy_x;
                    if ((ay = fabs(xy_y)) < tolerance) {
                        lp_lat = 0.;
                        t = x2 * x2 + TPISQ * (x2 + HPISQ);
                        lp_lon = fabs(xy_x) <= tolerance ? 0. :
                           .5 * (x2 - pi_sqr + sqrt(t)) / xy_x;
                            return;
                    }
                    y2 = xy_y * xy_y;
                    r = x2 + y2;    r2 = r * r;
                    c1 = - pi * ay * (r + pi_sqr);
                    c3 = r2 + two_pi * (ay * r + pi * (y2 + pi * (ay + half_pi)));
                    c2 = c1 + pi_sqr * (r - 3. *  y2);
                    c0 = pi * ay;
                    c2 /= c3;
                    al = c1 / c3 - third * c2 * c2;
                    m = 2. * sqrt(-third * al);
                    d = C2_27 * c2 * c2 * c2 + (c0 * c0 - third * c2 * c1) / c3;
                    if (((t = fabs(d = 3. * d / (al * m))) - tolerance) <= 1.) {
                        d = t > 1. ? (d > 0. ? 0. : pi) : acos(d);
                        lp_lat = pi * (m * cos(d * third + PI4_3) - third * c2);
                        if (xy_y < 0.) lp_lat = -lp_lat;
                        t = r2 + TPISQ * (x2 - y2 + HPISQ);
                        lp_lon = fabs(xy_x) <= tolerance ? 0. :
                           .5 * (r - pi_sqr + (t <= 0. ? 0. : sqrt(t))) / xy_x;
                    } else {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }
                }

                static inline std::string get_name()
                {
                    return "vandg_spheroid";
                }

            };

            // van der Grinten (I)
            template <typename Parameters>
            inline void setup_vandg(Parameters& par)
            {
                par.es = 0.;
            }

    }} // namespace detail::vandg
    #endif // doxygen

    /*!
        \brief van der Grinten (I) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Example
        \image html ex_vandg.gif
    */
    template <typename T, typename Parameters>
    struct vandg_spheroid : public detail::vandg::base_vandg_spheroid<T, Parameters>
    {
        inline vandg_spheroid(const Parameters& par) : detail::vandg::base_vandg_spheroid<T, Parameters>(par)
        {
            detail::vandg::setup_vandg(this->m_par);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::vandg, vandg_spheroid, vandg_spheroid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class vandg_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<vandg_spheroid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void vandg_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("vandg", new vandg_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_VANDG_HPP