/*******************************************************************************
 *
 * MIT License
 *
 * Copyright (c) 2020 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/rocm_features.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)

/// MIOpenGEMM issues with ROCm 3.7, most likely related to the
/// issues in the OpenCL compiler. Not reproducible in ROCm 4.0.
#define WORKAROUND_MIOPENGEMM_ROCM37 \
    (MIOPEN_USE_MIOPENGEMM && HIP_PACKAGE_VERSION_MAJOR == 3 && HIP_PACKAGE_VERSION_MINOR == 7)

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

bool GemmFwdBase::IsApplicable(const ExecutionContext&,
                               const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    const auto& xDesc = problem.GetIn();
    const auto& wDesc = problem.GetWeights();
    const auto& yDesc = problem.GetOut();
    return problem.GetDirection() == conv::Direction::Forward && problem.IsLayoutDefault() &&
           !(IsAnyBufferBF16(xDesc, yDesc, wDesc) && !IsBf16Supported) &&
           !(IsAnyBufferFp16(xDesc, yDesc, wDesc) && !IsFp16Supported);
#else
    std::ignore = problem;
    return false;
#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;
}

float GemmFwdBase::GetWti(const ExecutionContext&, const conv::ProblemDescription& problem) const
{
    decltype(auto) conv  = problem.GetConv();
    decltype(auto) wDesc = problem.GetWeights();
    decltype(auto) xDesc = problem.GetIn();
    decltype(auto) yDesc = problem.GetOut();

    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_transpose_packed_MN2NM = 0;
    int n_CastTensor             = 0;
    int n_Im2ColGPU              = 0;

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

    // Use transpose path 1x1, stride=2
    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_transpose_NCHW2CNHW = 1;
        if(wDesc.GetType() == miopenInt8)
            n_transpose_packed_MN2NM = 1;
        n_gemm_strided_batched = conv.group_count;
        n_transpose_CNHW2NCHW  = 1;
        if((wDesc.GetType() == miopenInt8 || wDesc.GetType() == miopenInt8x4) &&
           yDesc.GetType() != miopenInt32)
            n_CastTensor = 1;
    }
    // 1x1_stride=1 with 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; }))
    {

        if(wDesc.GetType() == miopenInt8)
        {
            n_transpose_packed_MN2NM = in_n;
            n_gemm_runs              = in_n;
        }
        else
        {
            n_gemm_strided_batched = conv.group_count;
            n_gemm_runs            = in_n;
        }
        if((wDesc.GetType() == miopenInt8 || wDesc.GetType() == miopenInt8x4) &&
           yDesc.GetType() != miopenInt32)
            n_CastTensor = 1;
    }
    else // not 1x1
    {
        n_Im2ColGPU = in_n;
        if(wDesc.GetType() == miopenInt8)
            n_transpose_packed_MN2NM = in_n;
        n_gemm_strided_batched = conv.group_count;
        n_gemm_runs            = in_n;
        if((wDesc.GetType() == miopenInt8 || wDesc.GetType() == miopenInt8x4) &&
           yDesc.GetType() != miopenInt32)
            n_CastTensor = 1;
    }

    auto wti = 1.0;
    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_transpose_packed_MN2NM, 0.7, 0.9);
    wti *= SlowdownFactor(n_CastTensor, 0.95, 0.9);
    wti *= SlowdownFactor(n_Im2ColGPU, 0.4, 0.8);
    return wti;
}

// 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)))

size_t GemmFwd1x1_0_2::GetWorkspaceSize(const ExecutionContext& context,
                                        const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    decltype(auto) handle = context.GetStream();
    decltype(auto) conv   = problem.GetConv();
    decltype(auto) xDesc  = problem.GetIn();
    decltype(auto) yDesc  = problem.GetOut();

    std::size_t in_n, in_c;
    std::tie(in_n, in_c) = miopen::tie_pick<0, 1>{}(xDesc.GetLengths());

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

    const auto x_t_size = in_n * in_c * (xDesc.GetType() == miopenInt8 ? 2 : 1) *
                          std::accumulate(out_spatial.begin(),
                                          out_spatial.end(),
                                          std::size_t(1),
                                          std::multiplies<std::size_t>()) *
                          GetTypeSize(xDesc.GetType());

    const auto y_t_size   = yDesc.GetElementSize() * GetTypeSize(yDesc.GetType());
    const auto gemm_trans = x_t_size + y_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 GemmFwd1x1_0_2::IsApplicable(const ExecutionContext& context,
                                  const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    if(!GemmFwdBase::IsApplicable(context, problem))
        return false;

    decltype(auto) conv  = problem.GetConv();
    decltype(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 GemmFwd1x1_0_2::GetSolution(const ExecutionContext& context,
                                         const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    decltype(auto) conv  = problem.GetConv();
    decltype(auto) xDesc = problem.GetIn();
    decltype(auto) wDesc = problem.GetWeights();
    decltype(auto) yDesc = problem.GetOut();

    const GemmDescriptor gemm_desc =
        conv.group_count > 1
            ? CreateGemmDescriptorGroupConvCNHWFwd(wDesc, xDesc, yDesc, conv.group_count)
            : CreateGemmDescriptorConvCNHWFwd(wDesc, xDesc, yDesc);

    const auto workspace_req = GetWorkspaceSize(context, problem);

    const std::size_t spatial_dim = conv.GetSpatialDimension();
    const auto& in_spatial_       = boost::adaptors::slice(xDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto& out_spatial_      = boost::adaptors::slice(yDesc.GetLengths(), 2, 2 + spatial_dim);

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

    const std::size_t wei_k = wDesc.GetLengths()[0];

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

    const auto group_count  = conv.group_count;
    const auto lowp_quant   = conv.lowp_quant;
    const auto conv_strides = conv.GetConvStrides();

    solution.invoker_factory = [=](const std::vector<Kernel>&) {
        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 std::size_t out_spatial_size = std::accumulate(
            out_spatial.begin(), out_spatial.end(), std::size_t(1), std::multiplies<std::size_t>());

        const bool time_precision = context.GetStream().IsProfilingEnabled() &&
                                    (!IsDisabled(MIOPEN_CONV_PRECISE_ROCBLAS_TIMING{}));

        return [=](const Handle& handle, const AnyInvokeParams& primitive_params) {
            float time_gemm          = 0;
            const auto& conv_params  = primitive_params.CastTo<conv::DataInvokeParams>();
            const auto& workSpace    = conv_params.workSpace;
            const auto workSpaceSize = conv_params.workSpaceSize;
            const auto x             = conv_params.tensors.in;
            const auto w             = conv_params.tensors.w;
            const auto y             = conv_params.tensors.out;

            if((workSpace == nullptr && workspace_req > 0) || workSpaceSize < workspace_req)
                MIOPEN_THROW("Not enough workspace for GEMM (" + std::to_string(workSpaceSize) +
                             " provided, " + std::to_string(workspace_req) + " required)");

            const std::string name = group_count > 1 ? "groupconv" : "convolution";
            MIOPEN_LOG_FUNCTION(name + ", 1x1 u2xv2");

            // y = CNHW2NCHW(w * NCHW2CNHW(x))
            transpose_NCHW2CNHW(handle,
                                in_n,
                                in_c,
                                in_spatial[0],
                                in_spatial[1],
                                out_spatial[0],
                                out_spatial[1],
                                x,
                                workSpace,
                                0,
                                0,
                                conv_strides[0],
                                conv_strides[1],
                                xDesc.GetType());
            if(handle.IsProfilingEnabled())
                time_gemm = handle.GetKernelTime();

            std::size_t x_t_size = in_n * in_c * out_spatial_size;

            std::size_t wksp_offset = 0;
            if(wDesc.GetType() == miopenInt8)
            {
                wksp_offset = x_t_size;
                transpose_packed_MN2NM(handle,
                                       in_c,
                                       static_cast<int>(in_n * out_spatial_size),
                                       0,
                                       wksp_offset,
                                       workSpace,
                                       workSpace,
                                       xDesc.GetType());

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

                x_t_size *= 2;
            }

            if(wDesc.GetType() == miopenInt8 || wDesc.GetType() == miopenInt8x4)
            {
                const auto xts = GetTypeSize(xDesc.GetType());
                if(xts > 0)
                {
                    const auto yts_div_xts = GetTypeSize(yDesc.GetType()) / xts;
                    if(yts_div_xts > 0)
                        x_t_size /= yts_div_xts;
                }
            }

            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,
                                                         x_t_size,
                                                         nullptr,
                                                         GemmBackend_t::miopentensile,
                                                         conv_params.gfx90aFp16alt);
                }
                else
                {
                    // tensors.y = CNHW2NCHW(tensors.w * NCHW2CNHW(tensors.x))
                    gemm_status = CallGemm(handle,
                                           gemm_desc,
                                           w,
                                           0,
                                           workSpace,
                                           wksp_offset,
                                           workSpace,
                                           x_t_size,
                                           nullptr,
                                           GemmBackend_t::miopentensile,
                                           conv_params.gfx90aFp16alt);
                }
            }
            else
            {
                gemm_status =
                    CallGemmTimeMeasure(handle,
                                        gemm_desc,
                                        w,
                                        0,
                                        workSpace,
                                        wksp_offset,
                                        workSpace,
                                        x_t_size,
                                        nullptr,
                                        time_precision,
                                        group_count > 1 ? callGemmStridedBatched : callGemm,
                                        GemmBackend_t::miopentensile,
                                        conv_params.gfx90aFp16alt);
            }

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

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

            transpose_CNHW2NCHW(handle,
                                in_n,
                                wei_k,
                                out_spatial[0],
                                out_spatial[1],
                                out_spatial[0],
                                out_spatial[1],
                                workSpace,
                                y,
                                x_t_size,
                                0,
                                1,
                                1,
                                yDesc.GetType());
            if(handle.IsProfilingEnabled())
                time_gemm += handle.GetKernelTime();

            if((wDesc.GetType() == miopenInt8 || wDesc.GetType() == miopenInt8x4) &&
               yDesc.GetType() != miopenInt32)
            {
                TensorDescriptor ygemmDesc(miopenInt32, yDesc.GetLengths(), yDesc.GetStrides());

                CastTensor(handle, &lowp_quant, ygemmDesc, y, yDesc, y, 0, 0);
                if(handle.IsProfilingEnabled())
                    time_gemm += handle.GetKernelTime();
            }

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

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

size_t GemmFwd1x1_0_1_int8::GetWorkspaceSize(const ExecutionContext& context,
                                             const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    decltype(auto) handle = context.GetStream();
    decltype(auto) conv   = problem.GetConv();
    decltype(auto) wDesc  = problem.GetWeights();
    decltype(auto) yDesc  = problem.GetOut();

    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(yDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto wei_c       = wDesc.GetLengths()[1];

    const auto ws_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(wDesc.GetType()) * conv.group_count;

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

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

    decltype(auto) conv  = problem.GetConv();
    decltype(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; }) &&
           wDesc.GetType() == miopenInt8 && conv.group_count == 1 &&
           GetWorkspaceSize(context, problem) > 0;
#else
    std::ignore = context;
    std::ignore = problem;
    return false;
#endif
}

ConvSolution GemmFwd1x1_0_1_int8::GetSolution(const ExecutionContext& context,
                                              const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    decltype(auto) conv  = problem.GetConv();
    decltype(auto) xDesc = problem.GetIn();
    decltype(auto) wDesc = problem.GetWeights();
    decltype(auto) yDesc = problem.GetOut();

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

    const std::size_t wei_k       = wDesc.GetLengths()[0];
    const std::size_t spatial_dim = conv.GetSpatialDimension();

    // This ones do not store data directly, so they should be copied to vectors for invokers.
    const auto& in_spatial_  = boost::adaptors::slice(xDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto& out_spatial_ = boost::adaptors::slice(yDesc.GetLengths(), 2, 2 + spatial_dim);

    const auto workspace_req = GetWorkspaceSize(context, problem);

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

    TensorDescriptor ygemmDesc(miopenInt32, yDesc.GetLengths(), yDesc.GetStrides());
    const GemmDescriptor gemm_desc = CreateGemmDescriptorConvFwd(wDesc, xDesc, yDesc);
    const auto x_type              = xDesc.GetType();
    const auto lowp_quant          = conv.lowp_quant;

    solution.invoker_factory = [=](const std::vector<Kernel>&) {
        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 std::size_t in_spatial_size = std::accumulate(
            in_spatial.begin(), in_spatial.end(), std::size_t(1), std::multiplies<std::size_t>());

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

        const bool time_precision = context.GetStream().IsProfilingEnabled() &&
                                    (!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 workSpaceSize = conv_params.workSpaceSize;
            const auto x             = conv_params.tensors.in;
            const auto w             = conv_params.tensors.w;
            const auto y             = conv_params.tensors.out;

            MIOPEN_LOG_FUNCTION("convolution, 1x1");

            if((workSpace == nullptr && workspace_req > 0) || workSpaceSize < workspace_req)
                MIOPEN_THROW("Not enough workspace for GEMM (" + std::to_string(workSpaceSize) +
                             " provided, " + std::to_string(workspace_req) + " required)");

            // y = w * x
            miopenStatus_t gemm_status = miopenStatusNotInitialized;
            float time                 = 0;
            const auto runs            = conv_params.type == InvokeType::Run ? in_n : 1;

            for(std::size_t i = 0; i < runs; i++)
            {
                std::size_t out_offset = i * wei_k * out_spatial_size;

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

                transpose_packed_MN2NM(
                    handle, in_c, in_spatial_size, in_offset, 0, x, workSpace, x_type);
                if(handle.IsProfilingEnabled())
                    time += handle.GetKernelTime();

                if(conv_params.type == InvokeType::Run)
                {
                    gemm_status = CallGemm(handle,
                                           gemm_desc,
                                           w,
                                           0,
                                           workSpace,
                                           0,
                                           y,
                                           out_offset,
                                           nullptr,
                                           GemmBackend_t::miopentensile,
                                           conv_params.gfx90aFp16alt);
                }
                else
                {
                    gemm_status = CallGemmTimeMeasure(handle,
                                                      gemm_desc,
                                                      w,
                                                      0,
                                                      workSpace,
                                                      0,
                                                      y,
                                                      out_offset,
                                                      nullptr,
                                                      time_precision,
                                                      callGemm,
                                                      GemmBackend_t::miopentensile,
                                                      conv_params.gfx90aFp16alt);
                }

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

                if(handle.IsProfilingEnabled())
                    time += handle.GetKernelTime();
            }

            if(conv_params.type != InvokeType::Run)
                time *= in_n;

            CastTensor(handle, &lowp_quant, ygemmDesc, y, conv_params.tensors.outDesc, y, 0, 0);

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

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

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

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

    decltype(auto) conv  = problem.GetConv();
    decltype(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; }) &&
           wDesc.GetType() != miopenInt8;
#else
    std::ignore = context;
    std::ignore = problem;
    return false;
#endif
}

ConvSolution GemmFwd1x1_0_1::GetSolution(const ExecutionContext& context,
                                         const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    decltype(auto) conv  = problem.GetConv();
    decltype(auto) xDesc = problem.GetIn();
    decltype(auto) wDesc = problem.GetWeights();
    decltype(auto) yDesc = problem.GetOut();

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

    const std::size_t wei_k       = wDesc.GetLengths()[0];
    const std::size_t spatial_dim = conv.GetSpatialDimension();

    // This ones do not store data directly, so they should be copied to vectors for invokers.
    const auto& in_spatial_  = boost::adaptors::slice(xDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto& out_spatial_ = boost::adaptors::slice(yDesc.GetLengths(), 2, 2 + spatial_dim);

    auto solution = ConvSolution{miopenStatusSuccess};

    const auto group_count = conv.group_count;
    const auto lowp_quant  = conv.lowp_quant;

    if(group_count > 1)
    {
        GemmDescriptor gemm_desc =
            CreateGemmDescriptorGroupConvFwd(wDesc, xDesc, yDesc, group_count);

        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 std::size_t in_spatial_size = std::accumulate(
            in_spatial.begin(), in_spatial.end(), std::size_t(1), std::multiplies<std::size_t>());

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

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

            MIOPEN_LOG_FUNCTION("groupconv, 1x1");

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

                const std::string name = group_count > 1 ? "groupconv" : "convolution";
                MIOPEN_LOG_FUNCTION(name + ", 1x1");

                // y = w * x
                miopenStatus_t gemm_status = miopenStatusNotInitialized;

                const auto runs =
                    group_count <= 1 ? 1 : conv_params.type == InvokeType::Run ? in_n : 1;

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

                    if(conv_params.type == InvokeType::Run)
                    {
                        gemm_status = CallGemmStridedBatched(handle,
                                                             gemm_desc,
                                                             w,
                                                             0,
                                                             x,
                                                             in_offset,
                                                             y,
                                                             out_offset,
                                                             nullptr,
                                                             GemmBackend_t::miopentensile,
                                                             conv_params.gfx90aFp16alt);
                    }
                    else
                    {
                        gemm_status = CallGemmTimeMeasure(handle,
                                                          gemm_desc,
                                                          w,
                                                          0,
                                                          x,
                                                          in_offset,
                                                          y,
                                                          out_offset,
                                                          nullptr,
                                                          time_precision,
                                                          callGemmStridedBatched,
                                                          GemmBackend_t::miopentensile,
                                                          conv_params.gfx90aFp16alt);
                    }

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

                    if(handle.IsProfilingEnabled())
                    {
                        const auto time = handle.GetKernelTime();
                        if(group_count > 1 && conv_params.type != InvokeType::Run)
                            time_gemm += time * in_n;
                    }
                }

                if(wDesc.GetType() == miopenInt8x4 && yDesc.GetType() != miopenInt32)
                {
                    TensorDescriptor ygemmDesc(miopenInt32, yDesc.GetLengths(), yDesc.GetStrides());
                    CastTensor(handle, &lowp_quant, ygemmDesc, y, yDesc, y, 0, 0);
                    if(handle.IsProfilingEnabled())
                        time_gemm += handle.GetKernelTime();
                }

                if(handle.IsProfilingEnabled())
                {
                    handle.ResetKernelTime();
                    handle.AccumKernelTime(time_gemm);
                }
            };
        };
    }
    else
    {
        // tensors.y = tensors.w * tensors.x
        GemmDescriptor gemm_desc =
            CreateGemmStridedBatchedDescriptorConv1x1Fwd(wDesc, xDesc, yDesc);

        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());

        solution.invoker_factory = [=](const std::vector<Kernel>&) {
            MIOPEN_LOG_FUNCTION("convolution, 1x1");

            const bool time_precision = context.GetStream().IsProfilingEnabled() &&
                                        (!IsDisabled(MIOPEN_CONV_PRECISE_ROCBLAS_TIMING{}));

            return [=](const Handle& handle, const AnyInvokeParams& primitive_params) {
                float time                 = 0;
                decltype(auto) conv_params = primitive_params.CastTo<conv::DataInvokeParams>();
                const auto& tensors        = conv_params.tensors;
                const auto& x              = tensors.in;
                const auto& w              = tensors.w;
                const auto& y              = tensors.out;

                MIOPEN_LOG_FUNCTION("convolution, 1x1");

                // tensors.y = tensors.w * tensors.x
                miopenStatus_t gemm_status;
                if(conv_params.type == InvokeType::Run)
                {
                    gemm_status = CallGemmStridedBatched(handle,
                                                         gemm_desc,
                                                         w,
                                                         0,
                                                         x,
                                                         0,
                                                         y,
                                                         0,
                                                         nullptr,
                                                         GemmBackend_t::miopentensile,
                                                         conv_params.gfx90aFp16alt);
                }
                else
                {
                    gemm_status = CallGemmTimeMeasure(handle,
                                                      gemm_desc,
                                                      w,
                                                      0,
                                                      x,
                                                      0,
                                                      y,
                                                      0,
                                                      nullptr,
                                                      time_precision,
                                                      callGemmStridedBatched,
                                                      GemmBackend_t::miopentensile,
                                                      conv_params.gfx90aFp16alt);
                }

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

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

                if(wDesc.GetType() == miopenInt8x4 && yDesc.GetType() != miopenInt32)
                {
                    TensorDescriptor ygemmDesc(miopenInt32, yDesc.GetLengths(), yDesc.GetStrides());
                    CastTensor(handle, &lowp_quant, ygemmDesc, y, yDesc, y, 0, 0);
                    if(handle.IsProfilingEnabled())
                        time += handle.GetKernelTime();
                }

                if(handle.IsProfilingEnabled())
                {
                    handle.ResetKernelTime();
                    handle.AccumKernelTime(time);
                }
            };
        };
    }

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

size_t GemmFwdRest::GetWorkspaceSize(const ExecutionContext& context,
                                     const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    decltype(auto) handle = context.GetStream();
    decltype(auto) conv   = problem.GetConv();
    decltype(auto) wDesc  = problem.GetWeights();
    decltype(auto) yDesc  = problem.GetOut();

    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(yDesc.GetLengths(), 2, 2 + spatial_dim);
    const auto wei_c       = wDesc.GetLengths()[1];

    const auto workspace_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(wDesc.GetType()) * conv.group_count;

    const auto ws_sz = (wDesc.GetType() == miopenInt8 ? 2 * workspace_size : workspace_size);

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

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

#if WORKAROUND_MIOPENGEMM_ROCM37
    {
        decltype(auto) conv  = problem.GetConv();
        decltype(auto) xDesc = problem.GetIn();
        decltype(auto) wDesc = problem.GetWeights();

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

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

        if(conv.GetSpatialDimension() == 2 && conv.group_count == 4 && in_c == 4 &&
           in_spatial[0] == 161 && in_spatial[1] == 700 && wDesc.GetLengths()[0] == 32 &&
           wDesc.GetLengths()[1] == 1 && wei_spatial[0] == 5 && wei_spatial[1] == 20 &&
           miopen::all_of(conv.GetConvPads(), [](auto v) { return v == 0; }) &&
           miopen::all_of(conv.GetConvStrides(), [](auto v) { return v == 2; }) &&
           miopen::all_of(conv.GetConvDilations(), [](auto v) { return v == 1; }))
            return false;
    }
#endif
#if WORKAROUND_MIOPENGEMM_SINCE_ROCM41
    {
        decltype(auto) conv  = problem.GetConv();
        decltype(auto) xDesc = problem.GetIn();
        decltype(auto) wDesc = problem.GetWeights();

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

        if(miopen::any_of(in_spatial, [](auto v) { return v >= 161; }) &&
           miopen::any_of(wei_spatial, [](auto v) { return v >= 7; }))
            return false;
    }
#endif

    // Todo: This is a rest-of kind of logic. Should be revised later.
    if(GemmFwd1x1_0_1{}.IsApplicable(context, problem))
        return false;
    if(GemmFwd1x1_0_1_int8{}.IsApplicable(context, problem))
        return false;
    if(GemmFwd1x1_0_2{}.IsApplicable(context, problem))
        return false;

    return GetWorkspaceSize(context, problem) > 0;
#else
    std::ignore = context;
    std::ignore = problem;
    return false;
#endif
}

ConvSolution GemmFwdRest::GetSolution(const ExecutionContext& context,
                                      const conv::ProblemDescription& problem) const
{
#if MIOPEN_USE_GEMM
    decltype(auto) conv  = problem.GetConv();
    decltype(auto) xDesc = problem.GetIn();
    decltype(auto) wDesc = problem.GetWeights();
    decltype(auto) yDesc = problem.GetOut();

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

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

    const auto workspace_req = GetWorkspaceSize(context, problem);

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

    solution.invoker_factory = [=](const std::vector<Kernel>&) {
        const auto gemm_desc =
            conv.group_count > 1
                ? CreateGemmDescriptorGroupConvFwd(wDesc, xDesc, yDesc, conv.group_count)
                : CreateGemmDescriptorConvFwd(wDesc, xDesc, yDesc);

        decltype(auto) in_spatial_ = boost::adaptors::slice(xDesc.GetLengths(), 2, 2 + spatial_dim);
        decltype(auto) out_spatial_ =
            boost::adaptors::slice(yDesc.GetLengths(), 2, 2 + spatial_dim);
        decltype(auto) wei_spatial_ =
            boost::adaptors::slice(wDesc.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 wei_spatial = std::vector<std::size_t>(wei_spatial_.begin(), wei_spatial_.end());

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

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

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

        const bool time_precision = (!IsDisabled(MIOPEN_CONV_PRECISE_ROCBLAS_TIMING{}));

        return [=](const Handle& handle, const AnyInvokeParams& primitive_params) {
            float time_gemm          = 0;
            const auto& conv_params  = primitive_params.CastTo<conv::DataInvokeParams>();
            const auto& workSpace    = conv_params.workSpace;
            const auto workSpaceSize = conv_params.workSpaceSize;
            const auto x             = conv_params.tensors.in;
            const auto w             = conv_params.tensors.w;
            const auto y             = conv_params.tensors.out;

            const std::string name = conv.group_count > 1 ? "groupconv" : "convolution";
            MIOPEN_LOG_FUNCTION(name + ", non 1x1");

            if((workSpace == nullptr && workspace_req > 0) || workSpaceSize < workspace_req)
                MIOPEN_THROW("Not enough workspace for GemmFwdRest (" +
                             std::to_string(workSpaceSize) + " provided, " +
                             std::to_string(workspace_req) + " required)");

            const auto runs = conv_params.type == InvokeType::Run ? in_n : 1;

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

                iteration_time += Im2ColGPU(handle,
                                            spatial_dim,
                                            x,
                                            in_offset,
                                            in_c,
                                            in_spatial,
                                            wei_spatial,
                                            out_spatial,
                                            conv.GetConvPads(),
                                            conv.GetConvStrides(),
                                            conv.GetConvDilations(),
                                            workSpace,
                                            xDesc.GetType());

                std::size_t wksp_offset = 0;
                if(wDesc.GetType() == miopenInt8)
                {
                    wksp_offset = in_c * wei_spatial_size * out_spatial_size;

                    transpose_packed_MN2NM(handle,
                                           static_cast<int>(in_c * wei_spatial_size),
                                           out_spatial_size,
                                           0,
                                           wksp_offset,
                                           workSpace,
                                           workSpace,
                                           xDesc.GetType());

                    if(handle.IsProfilingEnabled())
                        iteration_time += handle.GetKernelTime();
                }

                miopenStatus_t gemm_status = miopenStatusNotInitialized;

                // tensors.y = tensors.w * Im2Col(tensors.x)
                if(conv_params.type != InvokeType::Run)
                {
                    gemm_status = CallGemmTimeMeasure(
                        handle,
                        gemm_desc,
                        w,
                        0,
                        workSpace,
                        wksp_offset,
                        y,
                        0,
                        nullptr,
                        time_precision,
                        conv.group_count > 1 ? callGemmStridedBatched : callGemm,
                        (conv.group_count > 1 || wDesc.GetType() == miopenInt8 ||
                         wDesc.GetType() == miopenInt8x4 || wDesc.GetType() == miopenBFloat16)
                            ? GemmBackend_t::miopentensile
                            : GemmBackend_t::miopengemm,
                        conv_params.gfx90aFp16alt);
                }
                else
                {
                    if(conv.group_count > 1)
                        gemm_status = CallGemmStridedBatched(handle,
                                                             gemm_desc,
                                                             w,
                                                             0,
                                                             workSpace,
                                                             0,
                                                             y,
                                                             out_offset,
                                                             nullptr,
                                                             GemmBackend_t::miopentensile,
                                                             conv_params.gfx90aFp16alt);
                    else
                        gemm_status = CallGemm(
                            handle,
                            gemm_desc,
                            w,
                            0,
                            workSpace,
                            wksp_offset,
                            y,
                            out_offset,
                            nullptr,
                            (wDesc.GetType() == miopenInt8 || wDesc.GetType() == miopenInt8x4)
                                ? GemmBackend_t::rocblas
                                : GemmBackend_t::miopengemm,
                            conv_params.gfx90aFp16alt);
                }

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

                // Update times for both kernels
                if(handle.IsProfilingEnabled())
                {
                    iteration_time += handle.GetKernelTime();
                    if(conv_params.type != InvokeType::Run)
                        iteration_time *= in_n;
                    time_gemm += iteration_time;
                }
            }

            if((wDesc.GetType() == miopenInt8 || wDesc.GetType() == miopenInt8x4) &&
               yDesc.GetType() != miopenInt32)
            {
                TensorDescriptor ygemmDesc(miopenInt32, yDesc.GetLengths(), yDesc.GetStrides());

                CastTensor(handle, &conv.lowp_quant, ygemmDesc, y, yDesc, y, 0, 0);

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

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

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

} // namespace solver
} // namespace miopen


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>

#include "Tudat/Astrodynamics/Propagators/stateTransitionMatrixInterface.h"

namespace tudat
{

namespace propagators
{

//! Function to reset the state transition and sensitivity matrix interpolators
void SingleArcCombinedStateTransitionAndSensitivityMatrixInterface::updateMatrixInterpolators(
        const std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > >
        stateTransitionMatrixInterpolator,
        const std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > >
<<<<<<< HEAD
        sensitivityMatrixInterpolator )
=======
        sensitivityMatrixInterpolator,
        const std::vector< std::pair< int, int > >& statePartialAdditionIndices )
>>>>>>> origin/master
{
    stateTransitionMatrixInterpolator_ = stateTransitionMatrixInterpolator;
    sensitivityMatrixInterpolator_ = sensitivityMatrixInterpolator;
    statePartialAdditionIndices_ = statePartialAdditionIndices;
}

//! Function to get the concatenated state transition and sensitivity matrix at a given time.
Eigen::MatrixXd SingleArcCombinedStateTransitionAndSensitivityMatrixInterface::getCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime )
{
    combinedStateTransitionMatrix_.setZero( );

    // Set Phi and S matrices.
    combinedStateTransitionMatrix_.block( 0, 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ) =
            stateTransitionMatrixInterpolator_->interpolate( evaluationTime );

    if( sensitivityMatrixSize_ > 0 )
    {
        combinedStateTransitionMatrix_.block( 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_, sensitivityMatrixSize_ ) =
                sensitivityMatrixInterpolator_->interpolate( evaluationTime );
    }

    for( unsigned int i = 0; i < statePartialAdditionIndices_.size( ); i++ )
    {
        combinedStateTransitionMatrix_.block( statePartialAdditionIndices_.at( i ).first, 0, 6, stateTransitionMatrixSize_ + sensitivityMatrixSize_ ) +=
                combinedStateTransitionMatrix_.block( statePartialAdditionIndices_.at( i ).second, 0, 6, stateTransitionMatrixSize_ + sensitivityMatrixSize_ );
    }

    return combinedStateTransitionMatrix_;
}

//! Constructor
MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::MultiArcCombinedStateTransitionAndSensitivityMatrixInterface(
        const std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > >
        stateTransitionMatrixInterpolators,
        const std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > >
        sensitivityMatrixInterpolators,
        const std::vector< double >& arcStartTimes,
        const std::vector< double >& arcEndTimes,
        const int numberOfInitialDynamicalParameters,
        const int numberOfParameters,
        const std::vector< std::vector< std::pair< int, int > > >& statePartialAdditionIndices ):
    CombinedStateTransitionAndSensitivityMatrixInterface( numberOfInitialDynamicalParameters, numberOfParameters ),
    stateTransitionMatrixInterpolators_( stateTransitionMatrixInterpolators ),
    sensitivityMatrixInterpolators_( sensitivityMatrixInterpolators ),
    arcStartTimes_( arcStartTimes ),
    arcEndTimes_( arcEndTimes ),
    statePartialAdditionIndices_( statePartialAdditionIndices )
{
    if( arcStartTimes_.size( ) != arcEndTimes_.size( ) )
    {
        throw std::runtime_error( "Error when making MultiArcCombinedStateTransitionAndSensitivityMatrixInterface, incompatible time lists" );
    }
    numberOfStateArcs_ = arcStartTimes_.size( );

    sensitivityMatrixSize_ = numberOfParameters - numberOfStateArcs_ * stateTransitionMatrixSize_;

    if( stateTransitionMatrixInterpolators_.size( ) != sensitivityMatrixInterpolators_.size( ) ||
            stateTransitionMatrixInterpolators_.size( ) != static_cast< unsigned int >( numberOfStateArcs_ ) )
    {
        throw std::runtime_error(
                    "Error when making multi arc state transition and sensitivity interface, vector sizes are inconsistent" );
    }

    std::vector< double > arcSplitTimes = arcStartTimes_;
    arcSplitTimes.push_back(  std::numeric_limits< double >::max( ));
    lookUpscheme_ = std::make_shared< interpolators::HuntingAlgorithmLookupScheme< double > >(
                arcSplitTimes );
}

//! Function to reset the state transition and sensitivity matrix interpolators
void MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::updateMatrixInterpolators(
        const std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > >
        stateTransitionMatrixInterpolators,
        const std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::MatrixXd > > >
        sensitivityMatrixInterpolators,
        const std::vector< double >& arcStartTimes,
        const std::vector< double >& arcEndTimes,
        const std::vector< std::vector< std::pair< int, int > > >& statePartialAdditionIndices )
{
    stateTransitionMatrixInterpolators_ = stateTransitionMatrixInterpolators;
    sensitivityMatrixInterpolators_ = sensitivityMatrixInterpolators;
    arcStartTimes_ =  arcStartTimes;
    arcEndTimes_ = arcEndTimes;
    statePartialAdditionIndices_ = statePartialAdditionIndices;

    if( stateTransitionMatrixInterpolators_.size( ) != sensitivityMatrixInterpolators_.size( ) ||
            stateTransitionMatrixInterpolators_.size( ) != static_cast< unsigned int >( numberOfStateArcs_ ) )
    {
        throw std::runtime_error(
                    "Error when resetting multi arc state transition and sensitivity interface, vector sizes are inconsistent." );
    }

    std::vector< double > arcSplitTimes = arcStartTimes_;
    arcSplitTimes.push_back( std::numeric_limits< double >::max( ) );

    lookUpscheme_ = std::make_shared< interpolators::HuntingAlgorithmLookupScheme< double > >(
                arcSplitTimes );
}

//! Function to get the concatenated single-arc state transition and sensitivity matrix at a given time.
Eigen::MatrixXd MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime,
        const bool addCentralBodySensitivity )
{
    Eigen::MatrixXd combinedStateTransitionMatrix = Eigen::MatrixXd::Zero(
                stateTransitionMatrixSize_, stateTransitionMatrixSize_ + sensitivityMatrixSize_ );

    int currentArc = getCurrentArc( evaluationTime ).first;

    // Set Phi and S matrices.
    if( currentArc >= 0 )
    {
        combinedStateTransitionMatrix.block( 0, 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ) =
                stateTransitionMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );
        combinedStateTransitionMatrix.block( 0, stateTransitionMatrixSize_, stateTransitionMatrixSize_, sensitivityMatrixSize_ ) =
                sensitivityMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );

        for( unsigned int i = 0; i < statePartialAdditionIndices_.at( currentArc ).size( ); i++ )
        {
            int indicesToAdd = addCentralBodySensitivity ?
                        ( stateTransitionMatrixSize_ + sensitivityMatrixSize_ ) : stateTransitionMatrixSize_;
            combinedStateTransitionMatrix.block(
                        statePartialAdditionIndices_.at( currentArc ).at( i ).first, 0, 6, indicesToAdd ) +=
                    combinedStateTransitionMatrix.block(
                        statePartialAdditionIndices_.at( currentArc ).at( i ).second, 0, 6, indicesToAdd );
        }
    }
    return combinedStateTransitionMatrix;
}

//! Function to get the concatenated single-arc state transition and sensitivity matrix at a given time.
Eigen::MatrixXd MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime )
{
    return getCombinedStateTransitionAndSensitivityMatrix( evaluationTime, true );
}

//! Function to get the concatenated state transition matrices for each arc and sensitivity matrix at a given time.
Eigen::MatrixXd MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getFullCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime,
        const bool addCentralBodySensitivity )
{
    Eigen::MatrixXd combinedStateTransitionMatrix = getCombinedStateTransitionAndSensitivityMatrix(
                evaluationTime, addCentralBodySensitivity );
    Eigen::MatrixXd fullCombinedStateTransitionMatrix = Eigen::MatrixXd::Zero(
                stateTransitionMatrixSize_, numberOfStateArcs_ * stateTransitionMatrixSize_ + sensitivityMatrixSize_ );

    int currentArc = getCurrentArc( evaluationTime ).first;

    // Set Phi and S matrices of current arc.
<<<<<<< HEAD
    combinedStateTransitionMatrix.block( 0, currentArc * stateTransitionMatrixSize_, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ) =
            stateTransitionMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );

    combinedStateTransitionMatrix.block(
                0, numberOfStateArcs_ * stateTransitionMatrixSize_, stateTransitionMatrixSize_, sensitivityMatrixSize_ ) =
            sensitivityMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );
=======
    if( currentArc >= 0 )
    {
        fullCombinedStateTransitionMatrix.block(
                    0, currentArc * stateTransitionMatrixSize_, stateTransitionMatrixSize_, stateTransitionMatrixSize_ ) =
                stateTransitionMatrixInterpolators_.at( currentArc )->interpolate( evaluationTime );
        fullCombinedStateTransitionMatrix.block(
                    0, numberOfStateArcs_ * stateTransitionMatrixSize_, stateTransitionMatrixSize_, sensitivityMatrixSize_ ) =
                combinedStateTransitionMatrix.block( 0, 0, stateTransitionMatrixSize_, sensitivityMatrixSize_ );
    }
    return fullCombinedStateTransitionMatrix;
}
>>>>>>> origin/master

//! Function to get the concatenated state transition matrices for each arc and sensitivity matrix at a given time.
Eigen::MatrixXd MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getFullCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime )
{
    return getFullCombinedStateTransitionAndSensitivityMatrix( evaluationTime, true );
}

//! Function to retrieve the current arc for a given time
std::pair< int, double > MultiArcCombinedStateTransitionAndSensitivityMatrixInterface::getCurrentArc( const double evaluationTime )
{

    int currentArc =  lookUpscheme_->findNearestLowerNeighbour( evaluationTime );
    if( evaluationTime < arcEndTimes_.at( currentArc ) && evaluationTime > arcStartTimes_.at( currentArc ) )
    {
        return std::make_pair( currentArc, arcStartTimes_.at( currentArc ) );
    }
    else
    {
        return std::make_pair( -1, TUDAT_NAN );
    }
}

//! Function to get the concatenated state transition and sensitivity matrix at a given time.
Eigen::MatrixXd HybridArcCombinedStateTransitionAndSensitivityMatrixInterface::getCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime )
{
    Eigen::MatrixXd combinedStateTransitionMatrix = Eigen::MatrixXd::Zero(
                stateTransitionMatrixSize_, stateTransitionMatrixSize_ + sensitivityMatrixSize_ );

    // Get single-arc matrices
    Eigen::MatrixXd singleArcStateTransition = singleArcInterface_->getCombinedStateTransitionAndSensitivityMatrix(
                evaluationTime );

    // Get multi-arc matrices
    Eigen::MatrixXd multiArcStateTransition = multiArcInterface_->getCombinedStateTransitionAndSensitivityMatrix(
                evaluationTime, false );
    std::pair< int, double >  currentArc = multiArcInterface_->getCurrentArc( evaluationTime );

    // Set single-arc block
    combinedStateTransitionMatrix.block( 0, 0, singleArcStateSize_, singleArcStateSize_ ) =
            singleArcStateTransition.block( 0, 0, singleArcStateSize_, singleArcStateSize_ );

    // Set single-arc sensitivity block
    combinedStateTransitionMatrix.block( 0, multiArcStateSize_, singleArcStateSize_, sensitivityMatrixSize_ ) =
            singleArcStateTransition.block( 0, singleArcStateSize_, singleArcStateSize_, sensitivityMatrixSize_ );

    if( !( currentArc.first < 0 ) )
    {
        // Set multi-arc block
        combinedStateTransitionMatrix.block(
                    singleArcStateSize_, singleArcStateSize_, originalMultiArcStateSize_, originalMultiArcStateSize_ ) =
                multiArcStateTransition.block(
                    singleArcStateSize_, singleArcStateSize_, originalMultiArcStateSize_, originalMultiArcStateSize_ );

        // Get single-arc matrices at current arc start
        Eigen::MatrixXd singleArcStateTransitionAtArcStart = singleArcInterface_->getCombinedStateTransitionAndSensitivityMatrix(
                    currentArc.second );

        // Set coupled block
        combinedStateTransitionMatrix.block(
                    singleArcStateSize_, 0, originalMultiArcStateSize_, singleArcStateSize_ ) =
                multiArcStateTransition.block(
                    singleArcStateSize_, 0, originalMultiArcStateSize_, singleArcStateSize_ ) *
                singleArcStateTransitionAtArcStart.block(
                    0, 0, singleArcStateSize_, singleArcStateSize_ );

        // Set multi-arc sensitivity block
        combinedStateTransitionMatrix.block(
                    singleArcStateSize_, multiArcStateSize_, originalMultiArcStateSize_, sensitivityMatrixSize_ ) =
                multiArcStateTransition.block(
                    singleArcStateSize_, multiArcStateSize_, originalMultiArcStateSize_, sensitivityMatrixSize_ );

        std::vector< std::pair< int, int > > statePartialAdditionIndices =
                multiArcInterface_->getStatePartialAdditionIndices( currentArc.first );

        for( unsigned int i = 0; i < statePartialAdditionIndices.size( ); i++ )
        {
            combinedStateTransitionMatrix.block(
                        statePartialAdditionIndices.at( i ).first, multiArcStateSize_,
                        6, sensitivityMatrixSize_ ) +=
                    combinedStateTransitionMatrix.block(
                        statePartialAdditionIndices.at( i ).second, multiArcStateSize_,
                        6, sensitivityMatrixSize_ );

        }
    }

    return combinedStateTransitionMatrix;

}

//! Function to get the concatenated state transition matrices for each arc and sensitivity matrix at a given time.
Eigen::MatrixXd HybridArcCombinedStateTransitionAndSensitivityMatrixInterface::getFullCombinedStateTransitionAndSensitivityMatrix(
        const double evaluationTime )
{
    Eigen::MatrixXd combinedStateTransitionMatrix = getCombinedStateTransitionAndSensitivityMatrix( evaluationTime );
    Eigen::MatrixXd fullCombinedStateTransitionMatrix = Eigen::MatrixXd::Zero(
                stateTransitionMatrixSize_, singleArcStateSize_ + numberOfMultiArcs_ * originalMultiArcStateSize_ + sensitivityMatrixSize_ );

    fullCombinedStateTransitionMatrix.block(
                0, 0, singleArcStateSize_, singleArcStateSize_ ) =
            combinedStateTransitionMatrix.block(
                0, 0, singleArcStateSize_, singleArcStateSize_ );

    fullCombinedStateTransitionMatrix.block(
                0, singleArcStateSize_ + numberOfMultiArcs_ * originalMultiArcStateSize_, singleArcStateSize_, sensitivityMatrixSize_ ) =
            combinedStateTransitionMatrix.block(
                0, multiArcStateSize_, singleArcStateSize_, sensitivityMatrixSize_ );

    int currentArc = multiArcInterface_->getCurrentArc( evaluationTime ).first;

    // Set Phi and S matrices of current arc.
    if( currentArc >= 0 )
    {

        // Set multi-arc block
        fullCombinedStateTransitionMatrix.block(
                    singleArcStateSize_, singleArcStateSize_ + currentArc * originalMultiArcStateSize_,
                    originalMultiArcStateSize_, originalMultiArcStateSize_ ) =
                combinedStateTransitionMatrix.block(
                    singleArcStateSize_, singleArcStateSize_, originalMultiArcStateSize_, originalMultiArcStateSize_ );

<<<<<<< HEAD
    // Set sensitivity block
    combinedStateTransitionMatrix.block(
                0, singleArcStateSize_ + numberOfMultiArcs_ * originalMultiArcStateSize_,
                singleArcStateSize_, sensitivityMatrixSize_ ) =
            singleArcStateTransition.block( 0, singleArcStateSize_, singleArcStateSize_, sensitivityMatrixSize_ );
    combinedStateTransitionMatrix.block(
                singleArcStateSize_, singleArcStateSize_ + numberOfMultiArcs_ * originalMultiArcStateSize_,
                originalMultiArcStateSize_, sensitivityMatrixSize_ ) =
            multiArcStateTransition.block(
                singleArcStateSize_, multiArcStateSize_, originalMultiArcStateSize_, sensitivityMatrixSize_ );
=======
        // Set coupled block
        fullCombinedStateTransitionMatrix.block(
                    singleArcStateSize_, 0, originalMultiArcStateSize_, singleArcStateSize_ ) =
                combinedStateTransitionMatrix.block(
                    singleArcStateSize_, 0, originalMultiArcStateSize_, singleArcStateSize_ );
>>>>>>> origin/master

        // Set multi-arc sensitivity block
        fullCombinedStateTransitionMatrix.block(
                    singleArcStateSize_, singleArcStateSize_ + numberOfMultiArcs_ * originalMultiArcStateSize_,
                    originalMultiArcStateSize_, sensitivityMatrixSize_ ) =
                combinedStateTransitionMatrix.block(
                    singleArcStateSize_, multiArcStateSize_, originalMultiArcStateSize_, sensitivityMatrixSize_ );

    }

    return fullCombinedStateTransitionMatrix;
}

}

}


//todo: frontier classifaction
//time:0.01s
#include <functional>
#include <mutex>
#include <thread>
#include <string>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <nav_msgs/OccupancyGrid.h>
#include <queue>
#include <unordered_map>
// #include <boost>
#include <time.h>
#include <random>
#include <stack>
#include <sampling_based_frontier_detection.h>
#include <boost/foreach.hpp>
#include <costmap_tools.h>
#include <chrono>

#define INF INT_MAX
using namespace std;
namespace frontier_detection
{
    auto start = std::chrono::high_resolution_clock::now();
    int count=0;
    frontierdetection::frontierdetection(ros::NodeHandle private_nh)
    {
        getCostmap = false;
        NODE = private_nh;
        costmap_sub_global_ = private_nh.subscribe<nav_msgs::OccupancyGrid>("/map", 1, &frontierdetection::map_sub, this);
        costmap_pub_temp_=private_nh.advertise<nav_msgs::OccupancyGrid>("/origin_map", 1);
        costmap_sub_temp_ = private_nh.subscribe<nav_msgs::OccupancyGrid>("/origin_map", 1, &frontierdetection::CostmapSubCallback_global, this);
        frontier_map_pub_=private_nh.advertise<nav_msgs::OccupancyGrid>("/frontier_map", 1);

        // frontier_pub_=private_nh.advertise<geometry_msgs::PoseStamped>("/frontier/goal", 1);
        // robot_position_pub_=private_nh.advertise<geometry_msgs::PoseStamped>("/robot_position", 1);
    }

    void frontierdetection::map_sub(const nav_msgs::OccupancyGrid::ConstPtr& map)
    {
        cout<<"get map"<<endl;
        nav_msgs::OccupancyGrid map_temp=*map;
        while(1)
        {
            costmap_pub_temp_.publish(map_temp);
            sleep(1);
        }
    }

    void frontierdetection::CostmapSubCallback_global(const nav_msgs::OccupancyGrid::ConstPtr& map)
    {
        nav_msgs::OccupancyGrid map_temp=*map;
        for(int i=0;i<map_temp.info.height;i++)
        {
            if(i%y_width==0)
            {
            for(int j=0;j<map_temp.info.width;j++)
                {
                    if(j%x_width==0&&map_temp.data.at(i*map_temp.info.width+j)==0)
                    {
                        map_temp.data.at(i*map_temp.info.width+j)=particle_value;
                        particle_indexes.push_back(i*map_temp.info.width+j);
                    }
                }
            }
        }
        auto start = std::chrono::high_resolution_clock::now();
        while(count<3)
        {
            particle_move(map_temp,particle_indexes,step);
            cout<<"comparing frontier num:"<<frontier_index_sum_temp<<" "<<frontier_index_sum.size()<<endl;
            if(frontier_index_sum_temp==frontier_index_sum.size())
                count++;
            else
                continue;
        }
        vector <int>().swap(particle_indexes);
        count=0;
        for(auto i:frontier_index_sum)
        {
            map_temp.data.at(i)=frontier_value;
        }
        frontier_map_pub_.publish(map_temp);
        auto end = std::chrono::high_resolution_clock::now();
        auto timespan=std::chrono::duration<double>(end - start);
        cout<<"                consuming time sum:"<<timespan.count()<<endl;

        // for(auto x:frontier_index_sum)
        //     cout<<x<<endl;
    }
    void frontierdetection::particle_move(nav_msgs::OccupancyGrid& map, std::vector<int> indexes,int step)
    {

        frontier_index_sum_temp=frontier_index_sum.size();
        auto start2 = std::chrono::high_resolution_clock::now();
        for(auto index:indexes)
        {
            auto start1 = std::chrono::high_resolution_clock::now();
            int mx,my;//store the (x,y) of index in map
            int temp_x,temp_y,temp_index;//store the temp point from index in map
            indexTomap(map,index,mx,my);
            // std::cout<<"mx:"<<mx<<std::endl;
            // std::cout<<"my:"<<my<<std::endl;
            float k_x=rand() % (N + 1) / (float)(N + 1)-0.5;

            float k_y=rand() % (N + 1) / (float)(N + 1)-0.5;
            // std::cout<<"k_x:"<<k_x<<"k_y:"<<k_y<<std::endl;
            std::vector<int> frontier;
            // std::cout<<"random:"<< rand() % (N + 1) / (float)(N + 1)-0.5<<std::endl;
            for(int i=0;i<step;i++)
            {
                temp_x=mx+i*k_x;
                temp_y=my+i*k_y;
                temp_index=temp_y*map.info.width+temp_x;
                if(map.data.at(temp_index)==free_value)
                    // continue;
                    map.data.at(temp_index)=particle_value;
                else if(map.data.at(temp_index)==unexplored_value)
                {
                    // map.data.at(temp_index)=frontier_value;
                    // std::cout<<"enter frontier_find1"<<endl;
                    if(!frontier_index_exist(temp_index))
                    {
                        // std::cout<<"enter frontier_find2"<<endl;
                        frontier_find(map,temp_index);
                    }
                    // std::vector<unsigned int> x=nhood4(temp_index,map);
                    break;
                }
            }
            auto end1 = std::chrono::high_resolution_clock::now();
            auto timespan=std::chrono::duration<double>(end1 - start1);
            cout<<"per loop consuming time:"<<timespan.count()<<endl;
        }
            auto end2 = std::chrono::high_resolution_clock::now();
            auto timespan=std::chrono::duration<double>(end2 - start2);
            cout<<"loop consuming time sum:"<<timespan.count()<<endl;

        // vector <int>().swap(frontier_index_sum);//check the single epid of frontiers
    }

    void frontierdetection::frontier_find(const nav_msgs::OccupancyGrid& map,const int frontier_point_index)
    {
        // std::cout<<"enter frontier_find"<<endl;
        std::queue<int> bfs;
        bfs.push(frontier_point_index);
        while (!bfs.empty())
        {
            int idx = bfs.front();
            bfs.pop();
            // std::cout<<"frontier_exist(idx,map):"<<frontier_exist(idx,map)<<endl;
            // std::cout<<"frontier_index_exist(idx)"<<frontier_index_exist(idx)<<endl;
            if(frontier_exist(idx,map)&&!frontier_index_exist(idx))
            {
                // std::cout<<"frontier_exist"<<std::endl;
                // std::cout<<"idx_value"<<map.data.at(idx)<<std::endl;
                frontier_index_sum.push_back(idx);
                frontier.push_back(idx);
                BOOST_FOREACH(int nbr, nhood8(idx, map))
                {
                    if(map.data.at(nbr)==unexplored_value&&!frontier_index_exist(nbr))
                        bfs.push(nbr);
                }
            }
            // else
            //     continue;
        }
            if (frontier.size() > min_frontier_size_)
            {
                frontier_list.push_back(frontier);
            }
        // std::cout<<"frontier_find_end"<<std::endl;
    }

    bool frontierdetection::frontier_index_exist(const int frontier_point_index)
    {
        auto exist=find(frontier_index_sum.begin(),frontier_index_sum.end(),frontier_point_index);
        if(exist==frontier_index_sum.end())
        {
            return false;
        }
        else
            return true;
    }
    bool frontierdetection::frontier_exist(const int candidate_frontier_index,const nav_msgs::OccupancyGrid& map)
    {
        BOOST_FOREACH(int nbr, nhood8(candidate_frontier_index, map))
        {
            // std::cout<<"nbr:"<<nbr<<std::endl;
            if(map.data.at(nbr)==free_value&&map.data.at(candidate_frontier_index)==unexplored_value)
                return true;
        }
        return false;
    }
    void frontierdetection::collion_check()
    {

    }
    void frontierdetection::indexTomap(const nav_msgs::OccupancyGrid& map,int index,int& x,int& y)
    {
        x=index%map.info.width;
        y=index/map.info.width;
    }
}

//         void GetRobotpose(std::string iden_frame_id,geometry_msgs::PoseStamped& global_pose, ros::Time timestamp)
//         {
//         tf::StampedTransform transform;
//         geometry_msgs::PoseStamped iden_pose;
//         iden_pose.header.frame_id = iden_frame_id;
//         iden_pose.header.stamp = ros::Time::now();
//         iden_pose.pose.orientation.w = 1;
//         tf_listener_.waitForTransform("/robot0/map",iden_frame_id, ros::Time(0), ros::Duration(2.0));
//         tf_listener_.lookupTransform( "/robot0/map",iden_frame_id, ros::Time(0), transform);
//         global_pose.pose.position.x=transform.getOrigin().x();
//         global_pose.pose.position.y=transform.getOrigin().y();
//         global_pose.pose.position.z=transform.getOrigin().z();
//         }

int main(int argc, char** argv)
{
    ros::init(argc, argv, "frontier_detection");
    ros::NodeHandle private_nh1("~");
    frontier_detection::frontierdetection ta(private_nh1);
    ros::MultiThreadedSpinner spinner;
    spinner.spin();
    return 0;
}


// Copyright (c) 2017-2019 The papara developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <boost/test/unit_test.hpp>

#include <vector>
#include <cstdlib>

#include "masternode-tiers.h"
#include "primitives/transaction.h"
#include "spork.h"

using namespace std;

BOOST_AUTO_TEST_SUITE(tier_tests)

namespace
{
    void TierFromOutputTest(unsigned int* tiers, bool* isMastermodeOutput, int blockHeight)
    {
        int nextLockTime = 0;
        int nInput = 0;

        for (auto i = 0; i < MasternodeTiers::TIER_NONE; i++) {
            CMutableTransaction tx;
            tx.nLockTime = nextLockTime++;
            tx.vout.resize(nInput + 1);
            tx.vout[nInput].nValue = MASTERNODE_TIER_COINS[i] * COIN;

            BOOST_CHECK_EQUAL(IsMasternodeOutput(tx.vout[nInput].nValue, blockHeight), isMastermodeOutput[i]);
            BOOST_CHECK_EQUAL(GetMasternodeTierFromOutput(tx.vout[nInput].nValue, blockHeight), tiers[i]);
        }
        CMutableTransaction txNotMn;
        txNotMn.nLockTime = nextLockTime++;
        txNotMn.vout.resize(nInput + 1);
        txNotMn.vout[nInput].nValue = 1234 * COIN;

        BOOST_CHECK(!IsMasternodeOutput(txNotMn.vout[nInput].nValue, blockHeight));
        BOOST_CHECK_EQUAL(GetMasternodeTierFromOutput(txNotMn.vout[nInput].nValue, blockHeight), MasternodeTiers::TIER_NONE);
    }
}

BOOST_AUTO_TEST_CASE(test_tier_from_outputs_old_tiers)
{
    unsigned int tiers[MasternodeTiers::TIER_NONE] = {MasternodeTiers::TIER_NONE, MasternodeTiers::TIER_NONE, MasternodeTiers::TIER_1K,
                                                      MasternodeTiers::TIER_3K, MasternodeTiers::TIER_10K, MasternodeTiers::TIER_30K,
                                                      MasternodeTiers::TIER_100K, MasternodeTiers::TIER_NONE, MasternodeTiers::TIER_NONE,
                                                      MasternodeTiers::TIER_NONE, MasternodeTiers::TIER_NONE};
    bool isMastermodeOutput[MasternodeTiers::TIER_NONE] = {false, false, true, true, true, true, true, false, false, false, false};
    TierFromOutputTest(tiers, isMastermodeOutput, TIER_BLOCK_HEIGHT);
}

BOOST_AUTO_TEST_CASE(test_tier_from_outputs_new_tiers)
{
    unsigned int tiers[MasternodeTiers::TIER_NONE] = {MasternodeTiers::TIER_NONE, MasternodeTiers::TIER_NONE, MasternodeTiers::TIER_NONE,
                                                      MasternodeTiers::TIER_NONE, MasternodeTiers::TIER_NONE, MasternodeTiers::TIER_NONE,
                                                      MasternodeTiers::TIER_100K, MasternodeTiers::TIER_300K, MasternodeTiers::TIER_1KK,
                                                      MasternodeTiers::TIER_3KK, MasternodeTiers::TIER_10KK};
    bool isMastermodeOutput[MasternodeTiers::TIER_NONE] = {false, false, false, false, false, false, true, true, true, true, true};
    TierFromOutputTest(tiers, isMastermodeOutput, SPORK_17_MASTERNODE_PAYMENT_CHECK_DEFAULT);
}

BOOST_AUTO_TEST_CASE(test_tier_from_outputs_new_spork21_tiers)
{
    unsigned int tiers[MasternodeTiers::TIER_NONE] = {MasternodeTiers::TIER_100, MasternodeTiers::TIER_300,  MasternodeTiers::TIER_1K,
                                                      MasternodeTiers::TIER_3K, MasternodeTiers::TIER_10K, MasternodeTiers::TIER_NONE,
                                                      MasternodeTiers::TIER_NONE, MasternodeTiers::TIER_NONE, MasternodeTiers::TIER_NONE,
                                                      MasternodeTiers::TIER_NONE, MasternodeTiers::TIER_NONE};
    bool isMastermodeOutput[MasternodeTiers::TIER_NONE] = {true, true, true, true, true, false, false, false, false, false, false};
    TierFromOutputTest(tiers, isMastermodeOutput, SPORK_21_SUPERBLOCK_START_DEFAULT);
}

BOOST_AUTO_TEST_CASE(test_masternode_coins)
{
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_100), 100);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_300), 300);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_1K), 1000);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_3K), 3000);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_10K), 10000);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_30K), 30000);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_100K), 100000);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_300K), 300000);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_1KK), 1000000);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_3KK), 3000000);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_10KK), 10000000);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(MasternodeTiers::TIER_NONE), 0);
    BOOST_CHECK_EQUAL(GetMastenodeTierCoins(-2), 0);
}

BOOST_AUTO_TEST_CASE(test_obfuscation_value)
{
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_100) == 99.99, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_300) == 299.99, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_1K) == 999.99, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_3K) == 2999.99, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_10K) == 9999.99, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_30K) == 29999.99, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_100K) == 99999.99, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_300K) == 299999.99, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_1KK) == 999999.99, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_3KK) == 2999999.99, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_10KK) == 9999999.99, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(MasternodeTiers::TIER_NONE) == 0, boost::test_tools::tolerance(0.0001));
    BOOST_TEST(GetObfuscationValueForTier(-2) == 0, boost::test_tools::tolerance(0.0001));
}

BOOST_AUTO_TEST_CASE(test_intial_distribution)
{
    std::vector<size_t> vecTierSizes;
    std::vector<int> vecBlockNumbers = {0, 3, 10, 24,
                                        43, 45, 111, 143,
                                        145, 345, 443, 444,
                                        1443, 1444, 14443, 14444,
                                        144443, 144444, 144445, 144448};
    std::vector<unsigned int> vecTiers = {MasternodeTiers::TIER_100, MasternodeTiers::TIER_300, MasternodeTiers::TIER_1K, MasternodeTiers::TIER_3K,
                                          MasternodeTiers::TIER_3K, MasternodeTiers::TIER_10K, MasternodeTiers::TIER_10K, MasternodeTiers::TIER_10K,
                                          MasternodeTiers::TIER_30K, MasternodeTiers::TIER_30K, MasternodeTiers::TIER_30K, MasternodeTiers::TIER_100K,
                                          MasternodeTiers::TIER_100K, MasternodeTiers::TIER_300K, MasternodeTiers::TIER_1KK, MasternodeTiers::TIER_3KK,
                                          MasternodeTiers::TIER_10KK, MasternodeTiers::TIER_100, MasternodeTiers::TIER_300, MasternodeTiers::TIER_1K};

    for (auto i = 0; i < MasternodeTiers::TIER_NONE; i++) {
        vecTierSizes.push_back(1);
    }

    for (auto j = 0; j < vecBlockNumbers.size(); j++) {
        uint256 tmpHash(vecBlockNumbers[j]);
        auto nTier = CalculateWinningTier(vecTierSizes, tmpHash);
        BOOST_CHECK_EQUAL(nTier, vecTiers[j]);
    }
}

BOOST_AUTO_TEST_CASE(test_single_tier)
{
    std::vector<int> vecBlockNumbers = {0, 1, 3, 4, 13, 14, 43, 44, 143, 144, 312, 322, 1024, 1234, 3003, 8888, 10321, 11111};
    unsigned int tiers[MasternodeTiers::TIER_NONE] = {MasternodeTiers::TIER_100, MasternodeTiers::TIER_300, MasternodeTiers::TIER_1K,
                                                      MasternodeTiers::TIER_3K, MasternodeTiers::TIER_10K, MasternodeTiers::TIER_30K,
                                                      MasternodeTiers::TIER_100K, MasternodeTiers::TIER_300K, MasternodeTiers::TIER_1KK,
                                                      MasternodeTiers::TIER_3KK, MasternodeTiers::TIER_10KK};

    for (auto tier = 0; tier < MasternodeTiers::TIER_NONE; tier++) {
        std::vector<size_t> vecTierSizes(MasternodeTiers::TIER_NONE, 0);
        vecTierSizes[tier] = 10;

        for (auto i = 0; i < vecBlockNumbers.size(); i++) {
            uint256 tmpHash(vecBlockNumbers[i]);
            BOOST_CHECK_EQUAL(CalculateWinningTier(vecTierSizes, tmpHash), tiers[tier]);
        }
    }
}

void WinningTierTest(const std::vector<size_t>& vecTierSizes,
                     const std::vector<int>& vecBlockNumbers,
                     const std::vector<unsigned int>& vecTiers)
{
    for (auto j = 0; j < vecBlockNumbers.size(); j++) {
        uint256 tmpHash(vecBlockNumbers[j]);
        auto nTier = CalculateWinningTier(vecTierSizes, tmpHash);
        BOOST_CHECK_EQUAL(nTier, vecTiers[j]);
    }
}

BOOST_AUTO_TEST_CASE(test_winning_tier)
{
    std::vector<size_t> vecTierSizes = {120, 50, 30, 20, 10, 5, 0, 0, 0, 0, 0};
    std::vector<int> vecBlockNumbers = {0, 9, 12,
                                        15, 17, 32,
                                        33, 68, 69,
                                        142, 143, 443,
                                        444};
    std::vector<unsigned int> vecTiers = {MasternodeTiers::TIER_100, MasternodeTiers::TIER_100, MasternodeTiers::TIER_100,
                                           MasternodeTiers::TIER_300, MasternodeTiers::TIER_300, MasternodeTiers::TIER_300,
                                           MasternodeTiers::TIER_1K, MasternodeTiers::TIER_1K, MasternodeTiers::TIER_3K,
                                           MasternodeTiers::TIER_10K, MasternodeTiers::TIER_10K, MasternodeTiers::TIER_30K,
                                           MasternodeTiers::TIER_100};
    WinningTierTest(vecTierSizes, vecBlockNumbers, vecTiers);
}

BOOST_AUTO_TEST_CASE(test_winning_tier2)
{
    std::vector<size_t> vecTierSizes = {100, 20, 10, 5, 2, 0, 0, 0, 0, 0, 0};
    std::vector<int> vecBlockNumbers = {0, 10, 24,
                                        36, 38, 60,
                                        93, 97, 139,
                                        144};
    std::vector<unsigned int> vecTiers = {MasternodeTiers::TIER_100, MasternodeTiers::TIER_100, MasternodeTiers::TIER_300,
                                           MasternodeTiers::TIER_300, MasternodeTiers::TIER_1K, MasternodeTiers::TIER_1K,
                                           MasternodeTiers::TIER_3K, MasternodeTiers::TIER_10K, MasternodeTiers::TIER_10K,
                                           MasternodeTiers::TIER_100};
    WinningTierTest(vecTierSizes, vecBlockNumbers, vecTiers);
}

BOOST_AUTO_TEST_CASE(test_winning_tier3)
{
    std::vector<size_t> vecTierSizes = {100, 40, 0, 5, 1, 1, 1, 0, 0, 0, 0};
    std::vector<int> vecBlockNumbers = {0, 81, 178,
                                        301, 381, 623,
                                        1430, 1433, 1434,
                                        1534};
    std::vector<unsigned int> vecTiers = {MasternodeTiers::TIER_100, MasternodeTiers::TIER_300, MasternodeTiers::TIER_3K,
                                           MasternodeTiers::TIER_10K, MasternodeTiers::TIER_30K, MasternodeTiers::TIER_30K,
                                           MasternodeTiers::TIER_100K, MasternodeTiers::TIER_100K, MasternodeTiers::TIER_100,
                                           MasternodeTiers::TIER_300};
    WinningTierTest(vecTierSizes, vecBlockNumbers, vecTiers);
}

BOOST_AUTO_TEST_CASE(test_winning_tier_new_tiers)
{
    std::vector<size_t> vecTierSizes = {0, 0, 0, 0, 0, 0, 100, 20, 10, 5, 1};
    std::vector<int> vecBlockNumbers = {328, 15444, 28235,
                                        45175, 45176, 73410,
                                        73411, 115763, 115764,
                                        144000};
    std::vector<unsigned int> vecTiers = {MasternodeTiers::TIER_100K, MasternodeTiers::TIER_100K,  MasternodeTiers::TIER_300K,
                                           MasternodeTiers::TIER_300K, MasternodeTiers::TIER_1KK, MasternodeTiers::TIER_1KK,
                                           MasternodeTiers::TIER_3KK, MasternodeTiers::TIER_3KK, MasternodeTiers::TIER_10KK,
                                           MasternodeTiers::TIER_100K};
    WinningTierTest(vecTierSizes, vecBlockNumbers, vecTiers);
}

BOOST_AUTO_TEST_SUITE_END()