/*******************************************************************************
 *
 * 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 <limits>
#include <cassert>
#include <miopen/solver.hpp>
#include <miopen/env.hpp>
#include <miopen/gcn_asm_utils.hpp>
#include <miopen/tensor.hpp>
#include <miopen/gemm_v2.hpp>
#include <miopen/stringutils.hpp>
#include <miopen/miopen.h>
#include <miopen/generic_search.hpp>
#include <miopen/conv/invokers/impl_gemm.hpp>

#include <boost/any.hpp>
#include <miopen/conv/data_invoke_params.hpp>

#if MIOPEN_BACKEND_HIP

#define WORKAROUND_SWDEV_203031 1 // See also issues #2075, #2067
#define WORKAROUND_ISSUE_1146 1   // check asm solver applicability for gfx90a
#endif

#define WORKAROUND_SWDEV_257202 1 // For SSD convergence issue.

#if WORKAROUND_SWDEV_257202
// Workaround, solver disabled by default.
#define IS_DISABLED(expr) !miopen::IsEnabled(expr)
#else
// Normal behavior (solver enabled by default).
#define IS_DISABLED(expr) miopen::IsDisabled(expr)
#endif

namespace miopen {
namespace solver {
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_F2X3)
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_F3X3)
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_F4X3)
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_F5X3)
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_F6X3)

MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_XDLOPS_WINOGRAD_F2X3)
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_XDLOPS_WINOGRAD_F3X3)
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_XDLOPS_WINOGRAD_F4X3)
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_XDLOPS_WINOGRAD_F5X3)
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_XDLOPS_WINOGRAD_F6X3)

MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_WORKSPACE_MAX)

MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_EXPEREMENTAL_FP16_TRANSFORM)

MIOPEN_DECLARE_ENV_VAR(MIOPEN_CONV_PRECISE_ROCBLAS_TIMING)

// Introduces a number of shader-specific aliases (names) in the current scope at zero cost.
// These names represent shader parameters, e.g. shader C is batch_size etc and useful for
// programming.
#define DEFINE_GETXFORMHWSIZE(params)                                                        \
    const auto                                                                               \
        wino_xform_h =                                                                       \
            solver::ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>:: \
                GetSolverWinoXformHWSize(),                                                  \
        wino_xform_w =                                                                       \
            solver::ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>:: \
                GetSolverWinoXformHWSize();

#define DEFINE_SHADER_ALIASES(params)                       \
    const auto& group_cnt = (params).group_counts;          \
    const auto& N         = (params).batch_sz;              \
    const int K           = (params).n_outputs / group_cnt; \
    const int C           = (params).n_inputs / group_cnt;  \
    const auto& R         = (params).kernel_size_h;         \
    const auto& S         = (params).kernel_size_w;         \
    const auto& H         = (params).in_height;             \
    const auto& W         = (params).in_width;              \
    const auto& out_H     = (params).out_height;            \
    const auto& out_W     = (params).out_width;

#if MIOPEN_BACKEND_HIP
#define GENERATE_MAIN_OPTIONS(options)                                     \
    GenerateClangDefsym((options), "acc_type", 1);                         \
    GenerateClangDefsym((options), "ROCM_METADATA_VERSION", 5);            \
    GenerateClangDefsym((options), "xformx_o_size", WinoDataW);            \
    GenerateClangDefsym((options), "xformy_o_size", WinoDataH);            \
    GenerateClangDefsym((options), "xformx_d_size", wino_xform_w);         \
    GenerateClangDefsym((options), "xformy_d_size", wino_xform_h);         \
    GenerateClangDefsym((options), "xformx_f_size", WinoFilterW);          \
    GenerateClangDefsym((options), "xformy_f_size", WinoFilterH);          \
    GenerateClangDefsym((options), "fdilation_w", params.kernel_stride_w); \
    GenerateClangDefsym((options), "fdilation_h", params.kernel_stride_h);

struct WinoOffsets
{
    const size_t in, out, wei;
    WinoOffsets(size_t in_size, size_t out_size) : in(0), out(in_size), wei(in_size + out_size) {}
};
#endif

template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
WinogradBufferInfo<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>
GetWinoBuffer(const ConvolutionContext& params,
              const ConvWinoBuffType buff_type,
              const miopenDataType_t transform_data_type)
{
    DEFINE_GETXFORMHWSIZE(params)
    DEFINE_SHADER_ALIASES(params)

    WinogradBufferInfo<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW> Transform_info(
        N,
        K,
        C,
        group_cnt,
        out_H,
        out_W,
        R,
        S,
        (MemLayout_t::GCNHW),
        (ConvWinoXformType::N_GXhXw_C_Th_Tw),
        GetTypeSize(transform_data_type),
        buff_type,
        wino_xform_h,
        wino_xform_w);

    (void)H;
    (void)W;
    return Transform_info;
}

template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
inline bool IsApplicableGEMM(const ConvolutionContext& params)
{
#if(MIOPEN_BACKEND_HIP && (MIOPEN_USE_ROCBLAS || MIOPEN_USE_MIOPENTENSILE))

    const miopenDataType_t transform_data_type =
        miopen::IsEnabled(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_EXPEREMENTAL_FP16_TRANSFORM{})
            ? params.in_data_type
            : miopenFloat;

    // int offset for Workspace buffers.
    return !(((GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
                   params, ConvWinoBuffType::Input, transform_data_type))
                      .buff_info.total_byte_size /
                  GetTypeSize(transform_data_type) +
              (GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
                   params, ConvWinoBuffType::Output, transform_data_type))
                      .buff_info.total_byte_size /
                  GetTypeSize(transform_data_type)) >= (1LL << 31));
#else
    (void)params;
    return false;
#endif
}

template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
inline bool IsApplicableTransform(const ConvolutionContext& params)
{
#if MIOPEN_BACKEND_HIP
    if(!params.use_asm_kernels)
        return false;
    if(!params.rmv.IsV3())
        return false;
    if(!params.Is2d())
        return false;
    if(!(params.direction.IsForward() || params.direction.IsBackwardData()))
        return false;
    if(!(params.IsFp32() || params.IsFp16()))
        return false;

    const auto target = params.GetStream().GetTargetProperties();
    if(target.Xnack() && *target.Xnack())
        return false;

    const std::string name = params.GetStream().GetDeviceName();
    if(!StartsWith(name, "gfx9"))
        return false;
#if WORKAROUND_ISSUE_1146
    if(name == "gfx90a")
        return false;
#endif

    {
        std::size_t limit = miopen::Value(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_WORKSPACE_MAX{});
#if WORKAROUND_SWDEV_203031
        if(limit == 0)
        {
            if(name == "gfx900" ||
               (name == "gfx906" && params.GetStream().GetMaxComputeUnits() <= 60))
                limit = 2000000000ULL; // ~1.862 GiB
            else
                limit = std::numeric_limits<std::size_t>::max();
        }
#else
        if(limit == 0)
            limit = std::numeric_limits<std::size_t>::max();
#endif
        if(limit != std::numeric_limits<std::size_t>::max())
        {
            const auto required =
                ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>{}
                    .GetWorkspaceSize(params);
            MIOPEN_LOG_I2("Workspace required: " << required << ", limit: " << limit);
            if(required > limit)
                return false;
        }
    }

    if(!params.IsLayoutDefault())
    {
        return false;
    }

    {
        unsigned int const waves_in_group = 512 / wave_size;
        unsigned int const tiles_per_wave = 8;
        auto const tiles_per_group        = waves_in_group * tiles_per_wave / 2;
        auto const n_groups               = params.GetStream().GetMaxComputeUnits();
        auto const tiles_step             = tiles_per_group * n_groups;
        if(tiles_step >= std::pow(2, 16))
            return false;
    }
    DEFINE_SHADER_ALIASES(params)
    {
        const miopenDataType_t transform_data_type =
            miopen::IsEnabled(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_EXPEREMENTAL_FP16_TRANSFORM{})
                ? params.in_data_type
                : miopenFloat;

        BuffInfo in_buff(GetGroupConvLayout(GetMemLayout_t(params.in_layout), true),
                         N,
                         C,
                         H,
                         W,
                         group_cnt,
                         GetTypeSize(params.in_data_type)),
            // cppcheck-suppress unreadVariable
            out_buff(GetGroupConvLayout(GetMemLayout_t(params.out_layout), true),
                     N,
                     K,
                     out_H,
                     out_W,
                     group_cnt,
                     GetTypeSize(params.out_data_type)),
            // cppcheck-suppress unreadVariable
            wei_buff(GetGroupConvLayout(params.direction.IsForward()
                                            ? (MemLayout_t::NCHW)
                                            : GetSwappedNCLayout(MemLayout_t::NCHW),
                                        false),
                     K,
                     C,
                     R,
                     S,
                     group_cnt,
                     GetTypeSize(params.weights_data_type));

        auto wino_in = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
            params, ConvWinoBuffType::Input, transform_data_type);
        auto wino_out = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
            params, ConvWinoBuffType::Output, transform_data_type);
        auto wino_wei = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
            params, ConvWinoBuffType::Weight, transform_data_type);

        if(in_buff.total_byte_size > std::pow(2, 31) ||
           wei_buff.total_byte_size > std::pow(2, 31) ||
           out_buff.total_byte_size > std::pow(2, 31) ||
           wino_in.buff_info.total_byte_size > std::pow(2, 31) ||
           wino_out.buff_info.total_byte_size > std::pow(2, 31) ||
           wino_wei.buff_info.total_byte_size > std::pow(2, 31))
            return false;
    }

    // clang-format off
    bool ok = (
        (params.kernel_size_w == WinoFilterW
            && params.kernel_size_h == WinoFilterH)
        && (params.kernel_stride_w == 1)
        && params.kernel_stride_h == params.kernel_stride_w
        && params.kernel_dilation_w == 1
        && params.kernel_dilation_h == 1
        && N < std::pow(2, 16)
        && C < std::pow(2, 16)
        && K < std::pow(2, 16)
        && out_H < std::pow(2, 16)
        && out_W < std::pow(2, 16)
        && group_cnt < std::pow(2, 16)
        && params.bias == 0
        && params.in_layout == "NCHW");
    // clang-format on
    return ok;
#else
    (void)params;
    return false;
#endif
}

template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
bool ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::IsApplicable(
    const ConvolutionContext& params) const
{
    // HIP backend required for sending ptr (buffer + offset)
    // ROCBLAS for GEMM step

    if(!params.IsLayoutDefault())
    {
        return false;
    }

    if(!IsApplicableGEMM<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(params))
        return false;

    static const int wino_data_tile   = std::max(WinoDataH, WinoDataW);
    static const int wino_filter_tile = std::max(WinoFilterH, WinoFilterW);

    if(wino_data_tile == 6 && wino_filter_tile == 3)
        if(IS_DISABLED(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_F6X3{}))
            return false;
    if(wino_data_tile == 5 && wino_filter_tile == 3)
        if(IS_DISABLED(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_F5X3{}))
            return false;
    if(wino_data_tile == 4 && wino_filter_tile == 3)
        if(IS_DISABLED(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_F4X3{}))
            return false;
    if(wino_data_tile == 3 && wino_filter_tile == 3)
        if(IS_DISABLED(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_F3X3{}))
            return false;
    if(wino_data_tile == 2 && wino_filter_tile == 3)
        if(IS_DISABLED(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_F2X3{}))
            return false;

    return IsApplicableTransform<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(params);
}

template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
size_t ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::GetWorkspaceSize(
    const ConvolutionContext& params) const
{
    const miopenDataType_t transform_data_type =
        miopen::IsEnabled(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_EXPEREMENTAL_FP16_TRANSFORM{})
            ? params.in_data_type
            : miopenFloat;

    return (GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
                params, ConvWinoBuffType::Input, transform_data_type))
               .buff_info.total_byte_size +
           (GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
                params, ConvWinoBuffType::Output, transform_data_type))
               .buff_info.total_byte_size +
           (GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
                params, ConvWinoBuffType::Weight, transform_data_type))
               .buff_info.total_byte_size;
}

template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
InvokerFactory MakeWinogradInvokerFactory(const ConvolutionContext& params,
                                          InvokerFactory xdlops_factory = InvokerFactory(),
                                          bool isXdlops                 = false)
{
#if MIOPEN_BACKEND_HIP
    const int pad_H    = params.direction.IsForward() ? params.pad_h : params.GetBackwardPadH();
    const int pad_W    = params.direction.IsForward() ? params.pad_w : params.GetBackwardPadW();
    const int n_groups = params.GetStream().GetMaxComputeUnits();
    DEFINE_SHADER_ALIASES(params)
    DEFINE_GETXFORMHWSIZE(params)
    BuffInfo in_buff(GetGroupConvLayout(GetMemLayout_t(params.in_layout), true),
                     N,
                     C,
                     H,
                     W,
                     group_cnt,
                     GetTypeSize(params.in_data_type)),
        // cppcheck-suppress unreadVariable
        out_buff(GetGroupConvLayout(GetMemLayout_t(params.out_layout), true),
                 N,
                 K,
                 out_H,
                 out_W,
                 group_cnt,
                 GetTypeSize(params.out_data_type)),
        // cppcheck-suppress unreadVariable
        weights_buff(GetGroupConvLayout(params.direction.IsForward()
                                            ? (MemLayout_t::NCHW)
                                            : GetSwappedNCLayout(MemLayout_t::NCHW),
                                        false),
                     K,
                     C,
                     R,
                     S,
                     group_cnt,
                     GetTypeSize(params.weights_data_type));

    const miopenDataType_t transform_data_type =
        miopen::IsEnabled(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_EXPEREMENTAL_FP16_TRANSFORM{})
            ? params.in_data_type
            : miopenFloat;
    auto wino_in = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
        params, ConvWinoBuffType::Input, transform_data_type);
    auto wino_out = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
        params, ConvWinoBuffType::Output, transform_data_type);
    auto wino_wei = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
        params, ConvWinoBuffType::Weight, transform_data_type);

    int reserved       = 0;
    void* reserved_ptr = nullptr;
    int unused         = 0;
    const WinoOffsets transform_offset(wino_in.buff_info.total_byte_size,
                                       wino_out.buff_info.total_byte_size);

    InvokerFactory gemm_conv_factory;
    std::string gemm_conv_kernel_name;

    auto zeroDesc = TensorDescriptor();
    if(isXdlops)
    {
        gemm_conv_kernel_name = "XDLOPS_CONV: ";
        gemm_conv_factory     = xdlops_factory;
    }
    else
    {
#if MIOPEN_USE_ROCBLAS || MIOPEN_USE_MIOPENTENSILE
        // GEMM
        gemm_conv_kernel_name = "WRW_WINO_GEMM: ";

        int m = K, k = C,
            n   = wino_in.buff_info.size.nk * wino_in.buff_info.size.w * wino_in.buff_info.size.h;
        int lda = m, ldb = n, ldc = n;
        int batch_count       = wino_xform_h * wino_xform_w * group_cnt;
        long long int strideA = m * k * 1LL, strideB = k * n * 1LL, strideC = m * n * 1LL;
        float alpha = 1., beta = 0.0;
        const bool isColMajor = false, transA = true, transB = false;
        // clang-format off
        GemmDescriptor wino_gemm_desc{isColMajor,transA,transB,m,n,k,
            lda,ldb,ldc,batch_count,strideA,strideB,
            strideC,alpha,beta,transform_data_type};
// clang-format on
#else
        (void)wino_xform_w;
        (void)wino_xform_h;
#endif

        gemm_conv_factory = [=](const std::vector<Kernel>&) {
            return [=](const Handle& handle, const AnyInvokeParams& ctx) {
#if MIOPEN_USE_ROCBLAS || MIOPEN_USE_MIOPENTENSILE
                const auto& data_ctx = ctx.CastTo<conv::DataInvokeParams>();
                Data_t workSpace     = data_ctx.workSpace;
                CallGemmStridedBatched(
                    handle,
                    wino_gemm_desc,
                    workSpace,
                    static_cast<int>(transform_offset.wei / wino_wei.buff_info.element_size),
                    workSpace,
                    static_cast<int>(transform_offset.in / wino_in.buff_info.element_size),
                    workSpace,
                    static_cast<int>(transform_offset.out / wino_out.buff_info.element_size),
                    nullptr,
                    GemmBackend_t::rocblas);
#else
                (void)handle;
                (void)ctx;
                MIOPEN_THROW(miopenStatusBadParm, "ConvMPBidirectWinograd is not supported ");
#endif
            };
        };
    }

    return [=](const std::vector<Kernel>& kernels) {
        const std::vector<Kernel> transform_kernels =
            std::vector<Kernel>{kernels[0], kernels[1], kernels[2]};

        const std::vector<Kernel> conv_kernels =
            isXdlops ? std::vector<Kernel>{kernels[3]} : std::vector<Kernel>{};

        auto gemm_conv_invoker = gemm_conv_factory(conv_kernels);

        return [=](const Handle& handle, const AnyInvokeParams& ctx) {
            const auto& data_ctx = ctx.CastTo<conv::DataInvokeParams>();
            const auto tensors   = data_ctx.tensors;
            Data_t workSpace     = data_ctx.workSpace;
            auto workSpaceSize   = data_ctx.workSpaceSize;
            float total_time     = 0;
            auto wino_in_ptr =
                static_cast<void*>(reinterpret_cast<char*>(workSpace) + transform_offset.in);
            auto wino_w_ptr =
                static_cast<void*>(reinterpret_cast<char*>(workSpace) + transform_offset.wei);
            auto wino_out_ptr =
                static_cast<void*>(reinterpret_cast<char*>(workSpace) + transform_offset.out);

            for(int i = 0, cur = 0; i < 4; i++)
            {
                std::string kernel_name;
                if(i == 2) // GEMM
                {
                    // rocblas_gemm use workSpace pointer and constant offset
                    // xdlops_conv use tensors.in, tensors.w, tensors.out
                    ConvDataTensors xdlops_tensor = ConvDataTensors(ConvFwdTensors{
                        zeroDesc, wino_in_ptr, zeroDesc, wino_w_ptr, zeroDesc, wino_out_ptr});
                    const auto invoke_params      = conv::DataInvokeParams{
                        xdlops_tensor, workSpace, workSpaceSize, data_ctx.gfx90aFp16alt};

                    gemm_conv_invoker(handle, invoke_params);
                    kernel_name = gemm_conv_kernel_name;
                }
                else
                {
                    const auto kernel     = handle.Run(transform_kernels[cur++]);
                    const BuffInfo* d_buf = nullptr;
                    const BuffInfo* o_buf = nullptr;
                    void* buff_out_addr   = nullptr;

                    auto const_buff_in_adr = tensors.in;
                    auto buff_in_adr       = wino_out_ptr;
                    bool const_input       = false;
                    kernel_name            = kernel.GetName();

                    if(i == 0) // Input
                    {          // Transform
                        d_buf             = &in_buff;
                        o_buf             = &(wino_in.buff_info);
                        const_buff_in_adr = tensors.in;
                        buff_out_addr     = wino_in_ptr;
                        const_input       = true;
                    }
                    else if(i == 1) // filter
                    {               // Transform
                        d_buf             = &weights_buff;
                        o_buf             = &(wino_wei.buff_info);
                        const_buff_in_adr = tensors.w;
                        buff_out_addr     = wino_w_ptr;
                        const_input       = true;
                    }
                    else if(i == 3)
                    { // Output
                        d_buf         = &(wino_out.buff_info);
                        o_buf         = &(out_buff);
                        buff_in_adr   = wino_out_ptr;
                        buff_out_addr = tensors.out;
                        const_input   = false;
                    }

                    const auto input_ptr =
                        static_cast<const void*>(const_input ? const_buff_in_adr : buff_in_adr);
                    const auto output_ptr = buff_out_addr;
                    // clang-format off
                    MIOPEN_LOG_I2(" N=" << N << " G=" << group_cnt << " C=" << C << " H=" << H << " W=" << W << " K=" << K
                        << " n_groups=" << n_groups << " R=" << R << " S=" << S
                        << " pad_H=" << pad_H << " pad_W=" << pad_W << " out_H=" << out_H << " out_W=" << out_W
                        << " d_buf.byte_stride.nk=" << d_buf->byte_stride.nk << " d_buf->.byte_stride.c=" << d_buf->byte_stride.c
                        << " d_buf->.byte_stride.h=" << d_buf->byte_stride.h << " d_buf->.byte_stride.w=" << d_buf->byte_stride.w
                        << " o_buf->byte_stride.nk=" << o_buf->byte_stride.nk << " o_buf->byte_stride.c=" << o_buf->byte_stride.c
                        << " o_buf.byte_stride.h="  << o_buf->byte_stride.h <<  " o_buf->byte_stride.w=" << o_buf->byte_stride.w
                        << " d_buf->.byte_stride.g=" << d_buf->byte_stride.g  << " o_buf->byte_stride.g="  << o_buf->byte_stride.g);
                    // clang-format on
                    kernel(N,
                           C,
                           H,
                           W,
                           K,
                           n_groups,
                           unused,
                           reserved,
                           input_ptr,
                           reserved_ptr,
                           output_ptr,
                           reserved_ptr, // Unused return_addr.
                           R,
                           S,
                           pad_H, // Like Fwd wino.
                           pad_W,
                           out_H,
                           out_W,
                           reserved_ptr, // Unused bias_addr.
                           reserved,     // Unused relu_alpha.
                           d_buf->byte_stride.nk,
                           d_buf->byte_stride.c,
                           d_buf->byte_stride.h,
                           d_buf->byte_stride.w,
                           unused,
                           unused,
                           unused,
                           unused,
                           o_buf->byte_stride.nk,
                           o_buf->byte_stride.c,
                           o_buf->byte_stride.h,
                           o_buf->byte_stride.w,
                           group_cnt,
                           d_buf->byte_stride.g,
                           unused,
                           o_buf->byte_stride.g);
                }
                if(handle.IsProfilingEnabled())
                {
                    float cur_time = handle.GetKernelTime();
                    MIOPEN_LOG_I2(kernel_name << ": " << cur_time);

                    if(i < 3)
                        total_time += cur_time;
                    else
                        handle.AccumKernelTime(total_time);
                }
            }
        };
    };
#else
    (void)params;
    (void)xdlops_factory;
    (void)isXdlops;
    MIOPEN_THROW(miopenStatusBadParm, "ConvMPBidirectWinograd is not supported ");
    return nullptr;
#endif
}

template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
ConvSolution ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::GetSolution(
    const ConvolutionContext& params) const
{
    ConvSolution result;
    result.workspce_sz = GetWorkspaceSize(params);
#if MIOPEN_BACKEND_HIP

    const int n_groups = params.GetStream().GetMaxComputeUnits();
    DEFINE_GETXFORMHWSIZE(params)
    const std::vector<size_t> l_wk{512, 1, 1};
    const size_t g_wk_0 = n_groups * l_wk[0];
    const std::vector<size_t> g_wk{g_wk_0, 1, 1};
    const miopenDataType_t transform_data_type =
        miopen::IsEnabled(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_EXPEREMENTAL_FP16_TRANSFORM{})
            ? params.in_data_type
            : miopenFloat;
    std::ostringstream options_in;
    GENERATE_MAIN_OPTIONS(options_in)
    GenerateClangDefsym(options_in, "xform_mirror", 0);
    GenerateClangDefsym(options_in, "in_type", (params.IsFp32() ? 1 : 2));
    GenerateClangDefsym(options_in, "out_type", (transform_data_type == miopenFloat ? 1 : 2));

    std::ostringstream options_filter;
    GENERATE_MAIN_OPTIONS(options_filter)
    GenerateClangDefsym(options_filter, "xform_mirror", params.direction.IsBackwardData());
    GenerateClangDefsym(options_filter, "in_type", (params.IsFp32() ? 1 : 2));
    GenerateClangDefsym(options_filter, "out_type", (transform_data_type == miopenFloat ? 1 : 2));

    std::ostringstream options_out;
    GENERATE_MAIN_OPTIONS(options_out)
    GenerateClangDefsym(options_out, "xform_mirror", 0);
    GenerateClangDefsym(options_out, "in_type", (transform_data_type == miopenFloat ? 1 : 2));
    GenerateClangDefsym(options_out, "out_type", (params.IsFp32() ? 1 : 2));

    KernelInfo InTransform{
        options_in.str(),
        l_wk,
        g_wk,
        ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::GetSolverFileNames(
            0),
        ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::
            GetSolverKernelNames(0),
    };

    KernelInfo FilterTransform{
        options_filter.str(),
        l_wk,
        g_wk,
        ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::GetSolverFileNames(
            1),
        ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::
            GetSolverKernelNames(1),
    };

    KernelInfo OutTransform{
        options_out.str(),
        l_wk,
        g_wk,
        ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::GetSolverFileNames(
            2),
        ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::
            GetSolverKernelNames(2),
    };

    result.construction_params.push_back(InTransform);
    result.construction_params.push_back(FilterTransform);
    result.construction_params.push_back(OutTransform);

    result.invoker_factory =
        MakeWinogradInvokerFactory<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(params);

    return result;
#else
    (void)params;
    MIOPEN_THROW(miopenStatusBadParm, "ConvMPBidirectWinograd is not supported ");
#endif
}
template struct ConvMPBidirectWinograd<2, 3>;
template struct ConvMPBidirectWinograd<3, 3>;
template struct ConvMPBidirectWinograd<4, 3>;
template struct ConvMPBidirectWinograd<5, 3>;
template struct ConvMPBidirectWinograd<6, 3>;

// context transformation
// for winograd buffers calculation using xdlops_convolution
template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
ConvolutionContext ConvMPBidirectWinograd_xdlops<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::
    GetTransformedConvContext(const ConvolutionContext& ctx) const
{
    DEFINE_GETXFORMHWSIZE(ctx)
    int batch_count = wino_xform_h * wino_xform_w * ctx.group_counts;
    const miopenDataType_t transform_data_type =
        miopen::IsEnabled(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_EXPEREMENTAL_FP16_TRANSFORM{})
            ? ctx.in_data_type
            : miopenFloat;

    WinogradBufferInfo<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>
        wino_in = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
            ctx, ConvWinoBuffType::Input, transform_data_type),
        wino_out = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
            ctx, ConvWinoBuffType::Output, transform_data_type),
        wino_wei = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
            ctx, ConvWinoBuffType::Weight, transform_data_type);

    // GNCHW -> GCNHW
    TensorDescriptor in, wei, out;
    miopenSet4dTensorDescriptor(&in,
                                transform_data_type,
                                1,
                                wino_in.buff_info.size.c * batch_count,
                                1,
                                wino_in.buff_info.size.w * wino_in.buff_info.size.h *
                                    wino_in.buff_info.size.nk);

    miopenSet4dTensorDescriptor(&wei,
                                transform_data_type,
                                wino_wei.buff_info.size.nk * batch_count,
                                wino_wei.buff_info.size.c,
                                wino_wei.buff_info.size.h,
                                wino_wei.buff_info.size.w);

    miopenSet4dTensorDescriptor(&out,
                                transform_data_type,
                                1,
                                wino_out.buff_info.size.c * batch_count,
                                1,
                                wino_out.buff_info.size.w * wino_out.buff_info.size.h *
                                    wino_out.buff_info.size.nk);

    // default conv_desc.
    // pads{0,0}, stride{1,1}, dilation {1, 1}
    // trans_output_pads = {0, 0},  group_count = gem_batch_count
    ConvolutionDescriptor conv_desc({0, 0}, {1, 1}, {1, 1}, {0, 0}, batch_count);

    auto dir = conv::Direction::Forward;

    ConvolutionContext transformed_ctx(in, wei, out, conv_desc, dir, 0);
    transformed_ctx.ExecutionContext::operator=(ctx);

    transformed_ctx.SetupFloats();
    return transformed_ctx;
}

// must be same as invoke_params in Invoker
template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
conv::DataInvokeParams GetTransformedInvokeContext(const ConvolutionContext& ctx,
                                                   const AnyInvokeParams& invoke_ctx)
{
#if MIOPEN_BACKEND_HIP
    const miopenDataType_t transform_data_type =
        miopen::IsEnabled(MIOPEN_DEBUG_AMD_MP_BD_WINOGRAD_EXPEREMENTAL_FP16_TRANSFORM{})
            ? ctx.in_data_type
            : miopenFloat;
    WinogradBufferInfo<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>
        wino_in = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
            ctx, ConvWinoBuffType::Input, transform_data_type),
        wino_out = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
            ctx, ConvWinoBuffType::Output, transform_data_type),
        wino_wei = GetWinoBuffer<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
            ctx, ConvWinoBuffType::Weight, transform_data_type);

    // There are no unread variables, but the compiler still shows a warning.
    // cppcheck-suppress unreadVariable
    const WinoOffsets transform_offset(wino_in.buff_info.total_byte_size,
                                       wino_out.buff_info.total_byte_size);

    const auto& data_ctx = invoke_ctx.CastTo<conv::DataInvokeParams>();

    auto workSpace = data_ctx.workSpace;

    const auto wino_in_ptr =
        static_cast<void*>(reinterpret_cast<char*>(workSpace) + transform_offset.in);
    const auto wino_w_ptr =
        static_cast<void*>(reinterpret_cast<char*>(workSpace) + transform_offset.wei);
    const auto wino_out_ptr =
        static_cast<void*>(reinterpret_cast<char*>(workSpace) + transform_offset.out);

    const auto transform_workSpaceSize = wino_in.buff_info.total_byte_size +
                                         wino_wei.buff_info.total_byte_size +
                                         wino_out.buff_info.total_byte_size;

    const auto gemm_workSpaceSize = data_ctx.workSpaceSize - transform_workSpaceSize;
    const auto gemm_workSpace =
        static_cast<void*>(reinterpret_cast<char*>(workSpace) + transform_workSpaceSize);
    const auto zeroDesc           = TensorDescriptor();
    ConvDataTensors xdlops_tensor = ConvDataTensors(
        ConvFwdTensors{zeroDesc, wino_in_ptr, zeroDesc, wino_w_ptr, zeroDesc, wino_out_ptr});
    return conv::DataInvokeParams{
        xdlops_tensor, gemm_workSpace, gemm_workSpaceSize, data_ctx.gfx90aFp16alt};
#else
    (void)invoke_ctx;
    (void)ctx;
    MIOPEN_THROW(miopenStatusBadParm, "ConvMPBidirectWinograd is not supported ");
#endif
}

template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
bool ConvMPBidirectWinograd_xdlops<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::IsApplicable(
    const ConvolutionContext& ctx) const
{

    static const int wino_data_tile   = std::max(WinoDataH, WinoDataW);
    static const int wino_filter_tile = std::max(WinoFilterH, WinoFilterW);

    if(wino_data_tile == 6 && wino_filter_tile == 3)
        if(IS_DISABLED(MIOPEN_DEBUG_AMD_MP_BD_XDLOPS_WINOGRAD_F6X3{}))
            return false;
    if(wino_data_tile == 5 && wino_filter_tile == 3)
        if(IS_DISABLED(MIOPEN_DEBUG_AMD_MP_BD_XDLOPS_WINOGRAD_F5X3{}))
            return false;
    if(wino_data_tile == 4 && wino_filter_tile == 3)
        if(IS_DISABLED(MIOPEN_DEBUG_AMD_MP_BD_XDLOPS_WINOGRAD_F4X3{}))
            return false;
    if(wino_data_tile == 3 && wino_filter_tile == 3)
        if(IS_DISABLED(MIOPEN_DEBUG_AMD_MP_BD_XDLOPS_WINOGRAD_F3X3{}))
            return false;
    if(wino_data_tile == 2 && wino_filter_tile == 3)
        if(IS_DISABLED(MIOPEN_DEBUG_AMD_MP_BD_XDLOPS_WINOGRAD_F2X3{}))
            return false;

    return IsApplicableTransform<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(ctx) &&
           ConvHipImplicitGemmForwardV4R4Xdlops().IsApplicable(GetTransformedConvContext(ctx));
}

template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
ConvSolution
ConvMPBidirectWinograd_xdlops<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::GetSolution(
    const ConvolutionContext& ctx,
    const PerformanceImplicitGemmForwardV4R4Xdlops& config,
    bool) const
{
    ConvSolution wino_transform =
        ConvMPBidirectWinograd<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>{}.GetSolution(ctx);

    const ConvolutionContext xdlops_conv_ctx = GetTransformedConvContext(ctx);

    ConvSolution xdlops_conv =
        ConvHipImplicitGemmForwardV4R4Xdlops{}.GetSolution(xdlops_conv_ctx, config);

    ConvSolution result;
    result.workspce_sz = wino_transform.workspce_sz + xdlops_conv.workspce_sz;

    assert(xdlops_conv.construction_params.size() == 1);

    // change transform layout
    // GCNHW -> GNCHW
    std::ostringstream additional_options_wei;
    GenerateClangDefsym(additional_options_wei, "swap_filter_layout_KC", 1);
    wino_transform.construction_params[1].comp_options += additional_options_wei.str();

    result.construction_params.push_back(wino_transform.construction_params[0]);
    result.construction_params.push_back(wino_transform.construction_params[1]);
    result.construction_params.push_back(wino_transform.construction_params[2]);
    result.construction_params.push_back(xdlops_conv.construction_params[0]);

    result.invoker_factory =
        MakeWinogradInvokerFactory<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(
            ctx, xdlops_conv.invoker_factory.value(), true);

    return result;
}

template <int WinoDataH, int WinoFilterH, int WinoDataW, int WinoFilterW>
PerformanceImplicitGemmForwardV4R4Xdlops
ConvMPBidirectWinograd_xdlops<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>::Search(
    const ConvolutionContext& ctx, const AnyInvokeParams& invoke_ctx) const
{
    const auto transformed_invoke_ctx =
        GetTransformedInvokeContext<WinoDataH, WinoFilterH, WinoDataW, WinoFilterW>(ctx,
                                                                                invoke_ctx);
    return ConvHipImplicitGemmForwardV4R4Xdlops().Search(GetTransformedConvContext(ctx),
                                                         transformed_invoke_ctx);
}

template struct ConvMPBidirectWinograd_xdlops<2, 3>;
template struct ConvMPBidirectWinograd_xdlops<3, 3>;
template struct ConvMPBidirectWinograd_xdlops<4, 3>;
template struct ConvMPBidirectWinograd_xdlops<5, 3>;
template struct ConvMPBidirectWinograd_xdlops<6, 3>;

} // namespace solver
} // namespace miopen


#include "test_rank_knowledge_of_valid_opening.h"
#include "test_params.h"
#include "test_functions.h"
#include <rank/knowledge_of_valid_opening/knowledge_of_valid_opening.h>
#include <rank/rank_commitment/rank_commitment.h>
#include <NTL/GF2XFactoring.h>
#include <utils/utils.h>

static void generate_keys(
        rank::knowledge_of_valid_opening::private_key_t *private_key,
        rank::knowledge_of_valid_opening::public_key_t *public_key
) {

    ::rank::knowledge_of_valid_opening::generate_private_key(
            private_key);
    ::rank::knowledge_of_valid_opening::generate_public_key(
            public_key,
            private_key);
}

static void generate_commitments(
        rank::knowledge_of_valid_opening::commitments_t *commitments,
        rank::knowledge_of_valid_opening::responses_t *responses,
        rank::knowledge_of_valid_opening::revealed_values_t *revealed_values,
        rank::knowledge_of_valid_opening::private_key_t *private_key,
        rank::knowledge_of_valid_opening::public_key_t *public_key) {

    ::rank::knowledge_of_valid_opening::random_values_t random_values;

    ::rank::knowledge_of_valid_opening::generate_random_values(
            &random_values);

    ::rank::knowledge_of_valid_opening::generate_revealed_values(
            revealed_values);

    ::rank::knowledge_of_valid_opening::generate_commitments_and_responses(
            commitments,
            responses,
            &random_values,
            revealed_values,
            private_key,
            public_key);
}

int test::rank::knowledge_of_valid_opening::test_verify_0() {
    ::rank::rank_commitment::context_t context;
    ::rank::knowledge_of_valid_opening::private_key_t private_key;
    ::rank::knowledge_of_valid_opening::public_key_t public_key;
    ::rank::knowledge_of_valid_opening::revealed_values_t revealed_values;
    ::rank::knowledge_of_valid_opening::commitments_t commitments;
    ::rank::knowledge_of_valid_opening::responses_t responses;

    ::rank::rank_commitment::init(&context);

    generate_keys(
            &private_key,
            &public_key);

    generate_commitments(
            &commitments,
            &responses,
            &revealed_values,
            &private_key,
            &public_key);

    return ::rank::knowledge_of_valid_opening::verify_0(
            commitments.c0,
            commitments.s0,
            commitments.c1,
            commitments.s1,
            responses.r1,
            responses.r2,
            revealed_values.P,
            revealed_values.Q,
            public_key.G);
}

int test::rank::knowledge_of_valid_opening::test_verify_1() {
    ::rank::rank_commitment::context_t context;
    ::rank::knowledge_of_valid_opening::private_key_t private_key;
    ::rank::knowledge_of_valid_opening::public_key_t public_key;
    ::rank::knowledge_of_valid_opening::revealed_values_t revealed_values;
    ::rank::knowledge_of_valid_opening::commitments_t commitments;
    ::rank::knowledge_of_valid_opening::responses_t responses;

    ::rank::rank_commitment::init(&context);

    generate_keys(
            &private_key,
            &public_key);

    generate_commitments(
            &commitments,
            &responses,
            &revealed_values,
            &private_key,
            &public_key);

    return ::rank::knowledge_of_valid_opening::verify_1(
            commitments.c0,
            commitments.s0,
            commitments.c2,
            commitments.s2,
            responses.r1,
            responses.r3,
            public_key.commitment,
            revealed_values.P,
            revealed_values.Q,
            public_key.G);
}

int test::rank::knowledge_of_valid_opening::test_verify_2() {
    ::rank::rank_commitment::context_t context;
    ::rank::knowledge_of_valid_opening::private_key_t private_key;
    ::rank::knowledge_of_valid_opening::public_key_t public_key;
    ::rank::knowledge_of_valid_opening::revealed_values_t revealed_values;
    ::rank::knowledge_of_valid_opening::commitments_t commitments;
    ::rank::knowledge_of_valid_opening::responses_t responses;

    ::rank::rank_commitment::init(&context);

    generate_keys(
            &private_key,
            &public_key);

    generate_commitments(
            &commitments,
            &responses,
            &revealed_values,
            &private_key,
            &public_key);

    return ::rank::knowledge_of_valid_opening::verify_2(
            commitments.c1,
            commitments.s1,
            commitments.c2,
            commitments.s2,
            responses.r2,
            responses.r3,
            public_key.G);
}

static void test_perf_random_matrices() {

    ::rank::rank_commitment::context_t context;
    ::rank::rank_commitment::init(&context);

    ::rank::knowledge_of_valid_opening::revealed_values_t revealed_values;

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::rank::knowledge_of_valid_opening::generate_revealed_values(
                    &revealed_values);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_random_matrices",
            total_time_in_seconds,
            operation_per_second);
}

static void test_perf_response_encode_commit_0() {

    ::rank::rank_commitment::context_t context;
    ::rank::rank_commitment::init(&context);

    ::rank::knowledge_of_valid_opening::responses_t responses;
    ::rank::knowledge_of_valid_opening::commitments_t commitments;
    ::rank::knowledge_of_valid_opening::public_key_t public_key;
    ::rank::knowledge_of_valid_opening::private_key_t private_key;
    ::rank::knowledge_of_valid_opening::revealed_values_t revealed_values;
    ::rank::knowledge_of_valid_opening::random_values_t random_values;

    ::rank::knowledge_of_valid_opening::generate_private_key(
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_public_key(
            &public_key,
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_revealed_values(
            &revealed_values);

    ::rank::knowledge_of_valid_opening::generate_random_values(
            &random_values);

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::rank::knowledge_of_valid_opening::generate_response_0(
                    responses.r1,
                    random_values.u,
                    random_values.f,
                    public_key.G);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_response_0",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;
    NTL::vec_GF2 encoded;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::rank::knowledge_of_valid_opening::encode(
                    encoded,
                    revealed_values.P,
                    revealed_values.Q,
                    responses.r1);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_encode_0",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::rank::knowledge_of_valid_opening::commit(
                    commitments.c0,
                    commitments.s0,
                    encoded,
                    public_key.G);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_commit_0",
            total_time_in_seconds,
            operation_per_second);
}

static void test_perf_response_encode_commit_1() {

    ::rank::rank_commitment::context_t context;
    ::rank::rank_commitment::init(&context);

    ::rank::knowledge_of_valid_opening::responses_t responses;
    ::rank::knowledge_of_valid_opening::commitments_t commitments;
    ::rank::knowledge_of_valid_opening::public_key_t public_key;
    ::rank::knowledge_of_valid_opening::private_key_t private_key;
    ::rank::knowledge_of_valid_opening::revealed_values_t revealed_values;
    ::rank::knowledge_of_valid_opening::random_values_t random_values;

    ::rank::knowledge_of_valid_opening::generate_private_key(
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_public_key(
            &public_key,
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_revealed_values(
            &revealed_values);

    ::rank::knowledge_of_valid_opening::generate_random_values(
            &random_values);

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::rank::knowledge_of_valid_opening::generate_response_1(
                    responses.r2,
                    random_values.f,
                    revealed_values.P,
                    revealed_values.Q);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_response_1",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;
    NTL::vec_GF2 encoded;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::rank::knowledge_of_valid_opening::encode(
                    encoded,
                    responses.r2);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_encode_1",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::rank::knowledge_of_valid_opening::commit(
                    commitments.c1,
                    commitments.s1,
                    encoded,
                    public_key.G);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_commit_1",
            total_time_in_seconds,
            operation_per_second);
}

static void test_perf_response_encode_commit_2() {

    ::rank::rank_commitment::context_t context;
    ::rank::rank_commitment::init(&context);

    ::rank::knowledge_of_valid_opening::responses_t responses;
    ::rank::knowledge_of_valid_opening::commitments_t commitments;
    ::rank::knowledge_of_valid_opening::public_key_t public_key;
    ::rank::knowledge_of_valid_opening::private_key_t private_key;
    ::rank::knowledge_of_valid_opening::revealed_values_t revealed_values;
    ::rank::knowledge_of_valid_opening::random_values_t random_values;

    ::rank::knowledge_of_valid_opening::generate_private_key(
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_public_key(
            &public_key,
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_revealed_values(
            &revealed_values);

    ::rank::knowledge_of_valid_opening::generate_random_values(
            &random_values);

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::rank::knowledge_of_valid_opening::generate_response_2(
                    responses.r3,
                    private_key.e,
                    random_values.f,
                    revealed_values.P,
                    revealed_values.Q);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_response_2",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;
    NTL::vec_GF2 encoded;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::rank::knowledge_of_valid_opening::encode(
                    encoded,
                    responses.r3);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_encode_2",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::rank::knowledge_of_valid_opening::commit(
                    commitments.c2,
                    commitments.s2,
                    encoded,
                    public_key.G);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_commit_2",
            total_time_in_seconds,
            operation_per_second);
}

static void test_perf_verify_0() {

    ::rank::rank_commitment::context_t context;
    ::rank::rank_commitment::init(&context);

    ::rank::knowledge_of_valid_opening::responses_t responses;
    ::rank::knowledge_of_valid_opening::commitments_t commitments;
    ::rank::knowledge_of_valid_opening::public_key_t public_key;
    ::rank::knowledge_of_valid_opening::private_key_t private_key;
    ::rank::knowledge_of_valid_opening::revealed_values_t revealed_values;
    ::rank::knowledge_of_valid_opening::random_values_t random_values;

    ::rank::knowledge_of_valid_opening::generate_private_key(
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_public_key(
            &public_key,
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_revealed_values(
            &revealed_values);

    ::rank::knowledge_of_valid_opening::generate_random_values(
            &random_values);

    ::rank::knowledge_of_valid_opening::generate_commitments_and_responses(
            &commitments,
            &responses,
            &random_values,
            &revealed_values,
            &private_key,
            &public_key);

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            auto _r1 = utils::encode(revealed_values.P, revealed_values.Q, responses.r1, MI);
            ::rank::rank_commitment::verify_proof(
                    commitments.c0,
                    commitments.s0,
                    _r1,
                    public_key.G);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_verify_0 verification 1",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;
    NTL::vec_GF2 encoded;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            auto _r2 = utils::encode_2(responses.r2, MI);
            ::rank::rank_commitment::verify_proof(
                    commitments.c1,
                    commitments.s1,
                    _r2,
                    public_key.G);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_verify_0 verification 2",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            NTL::vec_GF2E pi_inv;
            ::rank::knowledge_of_valid_opening::calculate_pi_inv(
                    pi_inv,
                    revealed_values.P,
                    revealed_values.Q,
                    responses.r2);

            auto sum = responses.r1 + pi_inv;

            NTL::vec_GF2E result;
            utils::solve_equation(
                    result,
                    public_key.G,
                    sum);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_verify_0 verification 3",
            total_time_in_seconds,
            operation_per_second);
}

static void test_perf_verify_1() {

    ::rank::rank_commitment::context_t context;
    ::rank::rank_commitment::init(&context);

    ::rank::knowledge_of_valid_opening::responses_t responses;
    ::rank::knowledge_of_valid_opening::commitments_t commitments;
    ::rank::knowledge_of_valid_opening::public_key_t public_key;
    ::rank::knowledge_of_valid_opening::private_key_t private_key;
    ::rank::knowledge_of_valid_opening::revealed_values_t revealed_values;
    ::rank::knowledge_of_valid_opening::random_values_t random_values;

    ::rank::knowledge_of_valid_opening::generate_private_key(
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_public_key(
            &public_key,
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_revealed_values(
            &revealed_values);

    ::rank::knowledge_of_valid_opening::generate_random_values(
            &random_values);

    ::rank::knowledge_of_valid_opening::generate_commitments_and_responses(
            &commitments,
            &responses,
            &random_values,
            &revealed_values,
            &private_key,
            &public_key);

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            auto _r1 = utils::encode(revealed_values.P, revealed_values.Q, responses.r1, MI);
            ::rank::rank_commitment::verify_proof(
                    commitments.c0,
                    commitments.s0,
                    _r1,
                    public_key.G);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_verify_1 verification 1",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;
    NTL::vec_GF2 encoded;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            auto _r3 = utils::encode_2(responses.r3, MI);
            ::rank::rank_commitment::verify_proof(
                    commitments.c2,
                    commitments.s2,
                    _r3,
                    public_key.G);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_verify_1 verification 2",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            NTL::vec_GF2E pi_inv_r3;
            ::rank::knowledge_of_valid_opening::calculate_pi_inv(
                    pi_inv_r3,
                    revealed_values.P,
                    revealed_values.Q,
                    responses.r3);

            auto sum = responses.r1 + pi_inv_r3 + public_key.commitment;

            NTL::vec_GF2E result;
            utils::solve_equation(
                    result,
                    public_key.G,
                    sum);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_verify_1 verification 3",
            total_time_in_seconds,
            operation_per_second);
}

static void test_perf_verify_2() {

    ::rank::rank_commitment::context_t context;
    ::rank::rank_commitment::init(&context);

    ::rank::knowledge_of_valid_opening::responses_t responses;
    ::rank::knowledge_of_valid_opening::commitments_t commitments;
    ::rank::knowledge_of_valid_opening::public_key_t public_key;
    ::rank::knowledge_of_valid_opening::private_key_t private_key;
    ::rank::knowledge_of_valid_opening::revealed_values_t revealed_values;
    ::rank::knowledge_of_valid_opening::random_values_t random_values;

    ::rank::knowledge_of_valid_opening::generate_private_key(
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_public_key(
            &public_key,
            &private_key);

    ::rank::knowledge_of_valid_opening::generate_revealed_values(
            &revealed_values);

    ::rank::knowledge_of_valid_opening::generate_random_values(
            &random_values);

    ::rank::knowledge_of_valid_opening::generate_commitments_and_responses(
            &commitments,
            &responses,
            &random_values,
            &revealed_values,
            &private_key,
            &public_key);

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            auto _r2 = utils::encode_2(responses.r2, MI);
            ::rank::rank_commitment::verify_proof(
                    commitments.c1,
                    commitments.s1,
                    _r2,
                    public_key.G);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_verify_2 verification 1",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            auto _r3 = utils::encode_2(responses.r3, MI);
            ::rank::rank_commitment::verify_proof(
                    commitments.c2,
                    commitments.s2,
                    _r3,
                    public_key.G);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_verify_2 verification 2",
            total_time_in_seconds,
            operation_per_second);

    total_time = 0;
    iterations = 0x100;
    total_iterations = iterations;

    do {
        start = clock();
        for (i = 0; i < iterations; i++) {
            auto r2_plus_r3 = responses.r2 + responses.r3;
            utils::rank_of_vector(r2_plus_r3);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Rank",
            "KVO test_perf_verify_2 verification 3",
            total_time_in_seconds,
            operation_per_second);
}

void test::rank::knowledge_of_valid_opening::test_perf() {
    test_perf_random_matrices();
    test_perf_response_encode_commit_0();
    test_perf_response_encode_commit_1();
    test_perf_response_encode_commit_2();
    test_perf_verify_0();
    test_perf_verify_1();
    test_perf_verify_2();
}


/**
 * @file utils.cpp
 *
 * This file serves to seperate utility functions from the main omnicore.cpp
 * and omnicore.h files.
 */

#include "omnicore/utils.h"

#include "base58.h"
#include "utilstrencodings.h"

// TODO: use crypto/sha256 instead of openssl
#include "openssl/sha.h"

#include "omnicore/log.h"
#include "omnicore/script.h"

#include <boost/algorithm/string.hpp>

#include <assert.h>
#include <string.h>
#include <string>
#include <vector>

/**
 * Generates hashes used for obfuscation via ToUpper(HexStr(SHA256(x))).
 *
 * It is expected that the seed has a length of less than 128 characters.
 *
 * @see The class B transaction encoding specification:
 * https://github.com/mastercoin-MSC/spec#class-b-transactions-also-known-as-the-multisig-method
 *
 * @param strSeed[in]      A seed used for the obfuscation
 * @param hashCount[in]    How many hashes to generate (number of packets to debofuscate)
 * @param vstrHashes[out]  The generated hashes
 */
void PrepareObfuscatedHashes(const std::string& strSeed, int hashCount, std::string(&vstrHashes)[1+MAX_SHA256_OBFUSCATION_TIMES])
{
    unsigned char sha_input[128];
    unsigned char sha_result[128];
    std::vector<unsigned char> vec_chars;

    assert(strSeed.size() < sizeof(sha_input));
    strcpy((char *)sha_input, strSeed.c_str());

    if (hashCount > MAX_SHA256_OBFUSCATION_TIMES) hashCount = MAX_SHA256_OBFUSCATION_TIMES;

    // Do only as many re-hashes as there are data packets, 255 per specification
    for (int j = 1; j <= hashCount; ++j)
    {
        SHA256(sha_input, strlen((const char *)sha_input), sha_result);
        vec_chars.resize(32);
        memcpy(&vec_chars[0], &sha_result[0], 32);
        vstrHashes[j] = HexStr(vec_chars);
        boost::to_upper(vstrHashes[j]); // Convert to upper case characters

        assert(vstrHashes[j].size() < sizeof(sha_input));
        strcpy((char *)sha_input, vstrHashes[j].c_str());
    }
}

#if 0
std::string HashToAddress(unsigned char version, const uint160& hash)
{
    CBitcoinAddress address;
    if (version == Params().Base58Prefix(CChainParams::PUBKEY_ADDRESS)[0]) {
        CKeyID keyId = hash;
        address.Set(keyId);
        return address.ToString();
    } else if (version == Params().Base58Prefix(CChainParams::SCRIPT_ADDRESS)[0]) {
        CScriptID scriptId = hash;
        address.Set(scriptId);
        return address.ToString();
    }

    return "";
}
#endif

std::vector<unsigned char> AddressToBytes(const std::string& address)
{
    std::vector<unsigned char> addressBytes;
    bool success = DecodeBase58(address, addressBytes);
    if (!success) {
        PrintToLog("ERROR: failed to decode address %s.\n", address);
    }
    if (addressBytes.size() == 25) {
        addressBytes.resize(21); // truncate checksum
    } else {
        PrintToLog("ERROR: unexpected size from DecodeBase58 when decoding address %s.\n", address);
    }

    return addressBytes;
}


/**
 * Copyright (c) 2019 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef threedtiles_3dtiles_hpp_included_
#define threedtiles_3dtiles_hpp_included_

#include <algorithm>
#include <iosfwd>

#include <vector>
#include <boost/any.hpp>
#include <boost/optional.hpp>
#include <boost/variant.hpp>

#include "utility/enum-io.hpp"
#include "utility/openmp.hpp"

#include "math/geometry_core.hpp"

namespace threedtiles {

using OptString = boost::optional<std::string>;
using ExtensionList = std::vector<std::string>;
using Extension = boost::any;
using Extensions = std::map<std::string, Extension>;

UTILITY_GENERATE_ENUM_CI(Refinement,
                         ((replace)("REPLACE"))
                         ((add)("ADD"))
                         )

struct CommonBase {
    Extensions extensions;
    boost::any extras;
};

struct Box : CommonBase {
    math::Point3 center;
    math::Point3 x;
    math::Point3 y;
    math::Point3 z;

    void update(const Box &other);
};

struct Region : CommonBase {
    math::Extents3 extents;

    Region() : extents(math::InvalidExtents{}) {}

    void update(const Region &other);
};

struct Sphere : CommonBase {
    math::Point3 center;
    double radius = 0.0;

    void update(const Sphere &other);
};

/** Bounding volume. Blank when not filled yet. Tile with blank bounding volume
 *  cannot be serialized.
 *
 *  boost::blank must stay first template argument otherwise
 *  valid(BoundingVolume) would stop working
 */
using BoundingVolume = boost::variant<boost::blank, Box, Region, Sphere>;

inline bool valid(const BoundingVolume &bv) { return bv.which(); }

bool inside(const BoundingVolume &inner, const BoundingVolume &outer);

std::ostream& operator<<(std::ostream &os, const Box &b);
std::ostream& operator<<(std::ostream &os, const Region &r);
std::ostream& operator<<(std::ostream &os, const Sphere &s);
std::ostream& operator<<(std::ostream &os, const BoundingVolume &bv);

struct Property : CommonBase {
    double minimum;
    double maximum;

    Property() : minimum(), maximum() {}
};

using Properties = std::map<std::string, Property>;

struct TileContent : CommonBase {
    /** Bounding volume can be invalid.
     */
    BoundingVolume boundingVolume;
    std::string uri;
};

struct Tile : CommonBase {
    typedef std::unique_ptr<Tile> pointer;
    typedef std::vector<pointer> list;

    BoundingVolume boundingVolume;
    BoundingVolume viewerRequestVolume;
    double geometricError = 0.0;
    boost::optional<Refinement> refine;
    boost::optional<math::Matrix4> transform; // serialize as column major!
    boost::optional<TileContent> content;
    Tile::list children;

    /** Return number of tiles in subtree rooted in this tile.
     */
    std::size_t subtreeSize() const;

    /** Return depth of subtree. No children -> 0.
     */
    std::size_t subtreeDepth() const;
};

struct Asset : CommonBase {
    std::string version;
    OptString tilesetVersion;

    Asset() : version("1.0") {}
};

struct Tileset : CommonBase {
    Asset asset;
    Properties properties;
    double geometricError = 0.0;
    Tile::pointer root;

    ExtensionList extensionsUsed;
    ExtensionList extensionsRequired;
};

/** Write a tileset JSON file to an output stream.
 */
void write(std::ostream &os, const Tileset &tileset);

/** Write a tileset JSON file to an output file.
 */
void write(const boost::filesystem::path &path, const Tileset &tileset);

struct TilesetWithUri {
    typedef std::vector<TilesetWithUri> list;

    std::string uri;
    Tileset tileset;

    TilesetWithUri(const std::string &uri) : uri(uri) {}
};

/** Splits tileset tree into multiple sub-trees.
 *
 * Tile limit is only indicative, subtree is always full pyramid unless there
 * are leaf nodes.
 *
 * NB: original tileset tree is modified!
 *
 * \param tileset tileset to split
 * \param tileLimit (soft) tile limit
 * \returns list of sub trees
 */
TilesetWithUri::list split(Tileset &tileset, std::size_t tileLimit);

/** Updates one volume from the other.
 *  If updated is valid then it has to be of the same type as the updater.
 */
void update(BoundingVolume &updated, const BoundingVolume &updater);

/** Read tileset JSON file from an output stream.
 */
void read(std::istream &is, Tileset &tileset
          , const boost::filesystem::path &path = "");

/** Resolves all relative URIs using provided base URI and distributes various
 * inherited data to all tiles. Used to make tileset consistent after load.
 *
 *  \param tileset tileset to update
 *  \param baseUri base URI
 *  \param relaxed relaxed validity check
 *  \return tileset itself, to allow call chaining
 */
Tileset& absolutize(Tileset &ts, const std::string &baseUri
                    , bool relaxed = false);

/** Path in tile tree.
 */
struct TilePath {
    std::vector<int> path;
    TilePath() { path.reserve(32); /* guesstimage */ }
    int depth() const { return path.size(); }
};

/** Recursively traverses the tile tree starting at root and calls
 *      op(const Tile &tile, const TilePath &path)
 *  for every encountered tile.
 */
template <typename Op>
void traverse(const Tile &root, Op op);

/** Recursively traverses the tile tree starting at root and calls
 *      op(const Tile &tile, const TilePath &path)
 *  for every encountered tile.
 *
 * Explicitly const version.
 */
template <typename Op>
void ctraverse(const Tile &root, Op op);

/** Recursively traverses the tile tree starting at root and calls
 *      op(Tile &tile, int depth)
 *  for every encountered tile.
 */
template <typename Op>
void traverse(Tile &root, Op op);

/** Deep copy, when needed.
 */
Tileset clone(const Tileset &ts);

/** Dumps tileset metadata in human readable format.
 */
void dumpMetadata(std::ostream &os, const Tileset &ts);

// inlines

namespace detail {

template <typename TileT, typename Op>
void traverse(TileT &root, Op op) {
    struct Traverser {
        Op op;
        Traverser(Op op) : op(op) {}
        void process(TileT &tile, TilePath &path) {
            op(tile, path);

            int i(0);
            for (const auto &child : tile.children) {
                path.path.push_back(i++);
                process(*child, path);
                path.path.pop_back();
            }
        }
    } t(op);

    TilePath path;
    t.process(root, path);
}

} // namespace detail

template <typename Op> void traverse(const Tile &root, Op op)
{
    detail::traverse(root, op);
}

template <typename Op> void ctraverse(const Tile &root, Op op)
{
    detail::traverse(root, op);
}

template <typename Op> void traverse(Tile &root, Op op)
{
    detail::traverse(root, op);
}

inline std::size_t Tile::subtreeSize() const {
    std::size_t count(0);
    traverse(*this, [&count](const Tile&, const TilePath&) { ++count; });
    return count;
}

inline std::size_t Tile::subtreeDepth() const {
    std::size_t depth(0);
    traverse(*this, [&depth](const Tile&, const TilePath &path) {
                        depth = std::max(depth, std::size_t(path.depth()));
                    });
    return depth;
}

} // namespace threedtiles

#endif // 3dtiles_3dtiles_hpp_included_


// Prepare for G2O-------------------------------------------------

#include <Eigen/StdVector>
#include <iostream>

#include <stdint.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/stuff/sampler.h>

#include "targetTypes6D.hpp"
// #include "continuous_to_discrete.h"
// #include "targetTypes3D.hpp"
using namespace Eigen;
using namespace std;
using namespace g2o;

// Prepare for ROS-------------------------------------------------
#include <ros/ros.h>
#include <ros/time.h>
#include "sensor_msgs/Imu.h"
// #include "sensor_msgs/MagneticField.h"
// #include "sensor_msgs/Temperature.h"
// #include "sensor_msgs/FluidPressure.h"
// #include "sensor_msgs/Joy.h"
// #include "sensor_msgs/NavSatFix.h"
// #include "sensor_msgs/Range.h"

// #include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/PoseArray.h"
// #include "geometry_msgs/PoseStamped.h"
// #include "geometry_msgs/TwistWithCovarianceStamped.h"
// #include <geometry_msgs/Vector3Stamped.h>
// #include <sensor_fusion_comm/ExtEkf.h>
#include <sensor_fusion_comm/DoubleArrayStamped.h>

// #include "nav_msgs/Odometry.h"
// #include "crossover_nav/Ack.h"
// #include "crossover_nav/Navdata.h"
// #include "crossover_nav/odom_data.h"
// #include "crossover_nav/Status.h"

#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Quaternion.h>

// #include <dynamic_reconfigure/BoolParameter.h>
// #include <dynamic_reconfigure/Reconfigure.h>
// #include <dynamic_reconfigure/Config.h>

// #include <termios.h>

//function convert arduino to ros
//------------ROS version-------
// #include <iostream>
inline long millis() {
  return (1e3 * ros::Time::now().sec + 1e-6 * ros::Time::now().nsec);
}
inline long micros() {
  return (1e6 * ros::Time::now().sec + 1e-3 * ros::Time::now().nsec);
}
//not use millis() it give me online floating point behind sec
//go use ros::Time::now();
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

#define delay(x) ros::Duration(x/1000.0).sleep()
#define say(x) (std::cout << x)
#define sayend(x) (std::cout << x << std::endl)
#define saytab(x) (std::cout << x << "\t")
#define saycomma(x) (std::cout << x << " ,")
#define Min(a,b) ((a)<(b)?(a):(b))
#define Max(a,b) ((a)>(b)?(a):(b))
#define isfinite(X) std::isfinite(X)
#define wrap_180(x) (x < -180 ? x+360 : (x > 180 ? x - 360: x))
#define wrap_pi(x) (x < -3.14 ? x+6.28 : (x > 3.14 ? x - 6.28: x))
#define HALF_M_PI 1.570796

sensor_msgs::Imu imu;
geometry_msgs::PoseWithCovarianceStamped gps_pose , poseop_msgs;
sensor_fusion_comm::DoubleArrayStamped state_out_msf;
#define NUM_ITERATE 1

Vector3d gps[5];
Vector6d msf_state[17];
Vector3d acc[17];
double dtime[200];
int Iterate = 0;
Vector6d state;
int Numimu = 0;
int Numstate = 0;

int num_state = 0; //round max = 17
float GPS_lOOCKUP[5][2] = {{0,0},
                         {3,0},
                         {3,-3},
                         {0,-3},
                         {0,0}
                          };

float STATE_LOOCKUP[17][2];

// void imucallback(const sensor_msgs::Imu::ConstPtr &data)
// {
//   // imu = *data;
//   sensor_msgs::Imu imu_bf = *data;

//   static ros::Time st = ros::Time::now();
//   static ros::Duration ct = ros::Time::now()-st;
//   static ros::Duration pt= ct;
//   ct = ros::Time::now()-st;
//   dtime[Numimu] = (ct - pt).toSec();

//   //transform bf to ef
//   tf::Quaternion qimu(imu_bf.linear_acceleration.x,imu_bf.linear_acceleration.y,imu_bf.linear_acceleration.z,0);
//   tf::Quaternion q(imu_bf.orientation.x,imu_bf.orientation.y,imu_bf.orientation.z,imu_bf.orientation.w);

//   qimu = q*qimu*q.inverse() - tf::Quaternion(0,0,9.81,0);

//   imu.linear_acceleration.x = qimu.x();
//   imu.linear_acceleration.y = qimu.y();
//   imu.linear_acceleration.z = qimu.z();
//   imu.orientation = imu_bf.orientation;

//   acc[Numimu][0] = imu.linear_acceleration.x;
//   acc[Numimu][1] = imu.linear_acceleration.y;
//   acc[Numimu][2] = imu.linear_acceleration.z;

//   Numimu++;
//   // tf::Quaternion q(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w);
//   // tf::Matrix3x3 m(q);
//   // m.getRPY(roll, pitch, yaw);
//   //ROS_INFO("ax:[%f]", yaw);
// }
// void gpscallback(/*const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& data*/) {
//   // gps_pose = *data;

//   if(Iterate>=NUM_ITERATE) return;

//   gps[num_state][0] = GPS_lOOCKUP[0][0];
//   gps[num_state][1] = gps_pose.pose.pose.position.y;
//   gps[num_state][2] = 0;

//   Iterate++;

// }
// void state_out_callback(/*const sensor_fusion_comm::DoubleArrayStamped::ConstPtr& data*/) {

//   state_out_msf = *data;

//   for(int i =0;i<6;i++)
//     msf_state[Numstate][i] = state_out_msf.data[i];
//   Numstate++;
// }

void Optimize_test(double dt);

int main( int argc, char** argv )
{
  ros::init(argc, argv, "gps_tester");
  ros::NodeHandle n;
  ros::Rate r(200);

  // ros::Subscriber imu_sub           = n.subscribe<sensor_msgs::Imu>("/imu_max", 2, imucallback);
  // ros::Subscriber gps_sub           = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/imu_max/pose", 2, gpscallback);
  // ros::Subscriber state_out_msf_sub = n.subscribe<sensor_fusion_comm::DoubleArrayStamped>("/msf_core/state_out", 2, state_out_callback);
  ros::Publisher  poseop_pub        = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("/optimized", 10);

  static ros::Time start_time = ros::Time::now();
  static ros::Duration cur_time = ros::Time::now()-start_time;
  static ros::Duration prev_time= cur_time;
  static double dt = 0.25;

  poseop_msgs.header.frame_id = "odom";

  state_out_msf.data.resize(36);
  // state.setZero();
  // Iterate over the simulation steps

  state.setZero();
  for (int k = 0; k < 3; k++)
    {
      state[k] = 0;
    }

  STATE_LOOCKUP[0][0] = 0;
  STATE_LOOCKUP[0][1] = 0;

  for(int i=1;i<=4;i++) {
    /*x*/STATE_LOOCKUP[i][0] = (3.0/4.0)*i;
    /*y*/STATE_LOOCKUP[i][1] = 0;
    if(i==1 || i==4) {
      acc[i] = {16*3/3,0,0};
      if(i==4) acc[i]*=-1;
    }else
      acc[i] = {0,0,0};
  }
  for(int i=1;i<=4;i++) {
    /*x*/STATE_LOOCKUP[i+4][0] = 3.0+(1.0/4.0)*i;
    /*y*/STATE_LOOCKUP[i+4][1] = -i;
    if(i==1 || i==4) {
      acc[i+4] = {16*1/3,-16*4/3,0};
      if(i==4) acc[i+4]*=-1;
    }else
      acc[i+4] = {0,0,0};
  }
  for(int i=1;i<=4;i++) {
    /*x*/STATE_LOOCKUP[i+8][0] = 4.0+ (-3.0/4.0)*i;
    /*y*/STATE_LOOCKUP[i+8][1] = -4.0 + (1.0/4.0)*i;
    if(i==1 || i==4) {
      acc[i+8] = {-16*3/3,16*1/3,0};
      if(i==4) acc[i+8]*=-1;
    }else
      acc[i+8] = {0,0,0};
  }
  for(int i=1;i<=4;i++) {
    /*x*/STATE_LOOCKUP[i+12][0] = 1.0;
    /*y*/STATE_LOOCKUP[i+12][1] = -3.0+i;
    if(i==1 || i==4) {
      acc[i+12] = {16*0/3,16*4/3,0};
      if(i==4) acc[i+12]*=-1;
    }else
      acc[i+12] = {0,0,0};
  }

  for(int i=0;i<17;i++) {
   saytab(acc[i][0]); sayend(acc[i][1]);
 }

  while(ros::ok() && Iterate <= 3)
    {

        gps[Iterate+1][0] = GPS_lOOCKUP[Iterate+1][0];
        gps[Iterate+1][1] = GPS_lOOCKUP[Iterate+1][1];
        gps[Iterate+1][2] = 0;

        for(int i = 1;i<=4;i++) {
          msf_state[Iterate*4+i][0] = STATE_LOOCKUP[Iterate*4+i][0];
          msf_state[Iterate*4+i][1] = STATE_LOOCKUP[Iterate*4+i][1];
          msf_state[Iterate*4+i][2] = 0;
          msf_state[Iterate*4+i][3] = 0;
          msf_state[Iterate*4+i][4] = 0;
          msf_state[Iterate*4+i][5] = 0;
          // Numstate++;
        }

        Iterate ++;
      }
        saytab("START Iterate = ");sayend(Iterate);
        Optimize_test(dt);
        sayend("END");

}

void Optimize_test(double dt) {
 // Set up the parameters of the simulation
  int numberOfTimeSteps = NUM_ITERATE;
  const double processNoiseSigma = 0.05;
  const double accelerometerNoiseSigma = 0.01;
  const double gpsNoiseSigma = 0.001;
  // const double dt = 1;

  // Set up the optimiser and block solver
  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  typedef BlockSolver< BlockSolverTraits<6, 6> > BlockSolver;
  BlockSolver::LinearSolverType * linearSolver
      = new LinearSolverCholmod<BlockSolver::PoseMatrixType>();
  BlockSolver* blockSolver = new BlockSolver(linearSolver);
  OptimizationAlgorithm* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
  optimizer.setAlgorithm(optimizationAlgorithm);

  // Sample the start location of the target

    state = msf_state[0];

  // Construct the first vertex; this corresponds to the initial
  // condition and register it with the optimiser
  saytab("add vertex state ");sayend(0);
  VertexPositionVelocity3D* stateNode = new VertexPositionVelocity3D();
  stateNode->setEstimate(state);
  stateNode->setId(0);
  optimizer.addVertex(stateNode);

  // Construct the GPS observation
  Vector3d gpsMeasurement = gps[0];
  saytab("add edge gps ");sayend(0);
  // Add the GPS observation
  GPSObservationEdgePositionVelocity3D* goe = new GPSObservationEdgePositionVelocity3D(gpsMeasurement, gpsNoiseSigma);
  goe->setMeasurement(gpsMeasurement);
  goe->setVertex(0, stateNode);
  optimizer.addEdge(goe);

  // Construct the msf observation
  Vector3d msfMeasurement(0,0,0);

  saytab("add edge statemsf ");sayend(0);
  // Add the msf observation
  GPSObservationEdgePositionVelocity3D* goe2 = new GPSObservationEdgePositionVelocity3D(msfMeasurement, 0.1);
  goe2->setMeasurement(msfMeasurement);
  goe2->setVertex(0, stateNode);
  optimizer.addEdge(goe2);

  // Set up last estimate
  VertexPositionVelocity3D* lastStateNode = stateNode;

  // Iterate over the simulation steps
  for (int k = 0; k < Iterate; ++k)
    {
      for(int j = 1; j <= 4/*Numstate per round*/; j++)
      {
        // saytab(j);saytab(Numstate);sayend(Iterate);

          // state = msf_state[k*4+j];

          // Construct vertex which corresponds to the current state of the target
          VertexPositionVelocity3D* stateNode = new VertexPositionVelocity3D();

          stateNode->setId(k*4+j);
          stateNode->setEstimate(state);
          stateNode->setMarginalized(false);
          optimizer.addVertex(stateNode);

          saytab("add vertex state ");sayend(k*4+j);

          // // Construct the accelerometer measurement
          Vector3d accelerometerMeasurement/*(msf_state[k*4+j][0],msf_state[k*4+j][1],0);//*/ = acc[k*4+j];

          TargetOdometry3DEdge* toe = new TargetOdometry3DEdge(dt, accelerometerNoiseSigma);
          toe->setVertex(0, lastStateNode);
          toe->setVertex(1, stateNode);
          VertexPositionVelocity3D* vPrev= dynamic_cast<VertexPositionVelocity3D*>(lastStateNode);
          VertexPositionVelocity3D* vCurr= dynamic_cast<VertexPositionVelocity3D*>(stateNode);
          toe->setMeasurement(accelerometerMeasurement);
          optimizer.addEdge(toe);
          // compute the initial guess via the odometry
          g2o::OptimizableGraph::VertexSet vPrevSet;
          vPrevSet.insert(vPrev);
          toe->initialEstimate(vPrevSet,vCurr);

          lastStateNode = stateNode;
      }
                // Construct the msf observation
          Vector3d gpsMeasurement2 = gps[k+1];

          // Add the msf observation
          GPSObservationEdgePositionVelocity3D* goe2 = new GPSObservationEdgePositionVelocity3D(gpsMeasurement2, 0.1);
          goe2->setMeasurement(gpsMeasurement2);
          goe2->setVertex(0, stateNode);
          optimizer.addEdge(goe2);

        // Construct the GPS observation
        Vector3d gpsMeasurement = gps[k+1];

      saytab("add edge gps ");sayend(k+1);
      // Add the GPS observation
      GPSObservationEdgePositionVelocity3D* goe = new GPSObservationEdgePositionVelocity3D(gpsMeasurement, gpsNoiseSigma);
      goe->setMeasurement(gpsMeasurement);
      goe->setVertex(0, stateNode);
      optimizer.addEdge(goe);
    }

  // Configure and set things going
  optimizer.initializeOptimization();
  optimizer.setVerbose(true);
  optimizer.optimize(5);
  cerr << "number of vertices:" << optimizer.vertices().size() << endl;
  cerr << "number of edges:" << optimizer.edges().size() << endl;

  // Print the results

  // cout << "state=\n" << state << endl;

#if 0
  for (int k = 0; k < numberOfTimeSteps; k++)
    {
      cout << "computed estimate " << k << "\n"
           << dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find(k)->second)->estimate() << endl;
       }
#endif

  // Vector6d v1 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-2,0))->second)->estimate();
  // Vector6d v2 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-1,0))->second)->estimate();
  // cout << "v1=\n" << v1 << endl;
  // cout << "v2=\n" << v2 << endl;
  // cout << "delta state=\n" << v2-v1 << endl;
       // Vector6d v[4];
       Vector6d v[17];
       for(int i=0;i<17;i++)
          v[i] = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find(i)->second)->estimate();
      for(int i=0;i<17;i++)
       cout << v[i][0] << "\t" << v[i][1] <<endl;

      cout << endl;
      cout << endl;
      cout << endl;
      for(int i=0;i<17;i++)
       cout << msf_state[i][0] << "\t" << msf_state[i][1] <<endl;

      cout << endl;
      cout << endl;
      cout << endl;
      for(int i=0;i<5;i++)
        cout << gps[i][0] << "\t" << gps[i][1] <<endl;
       // poseop_msgs.header.stamp = ros::Time::now();
       // poseop_msgs.pose.pose.position.x = v[0];
       // poseop_msgs.pose.pose.position.y = v[1];
       // poseop_msgs.pose.pose.position.z = v[2];

       // poseop_msgs.pose.pose.orientation = imu.orientation;

      SparseBlockMatrix<MatrixXd> spinv;

      optimizer.computeMarginals(spinv, stateNode);

      //optimizer.solver()->computeMarginals();

      // covariance
      //
      cout << "covariance\n" << spinv << endl;

      // for(int u =0;u<6;u++)
      //   for(int v=0;v<6;v++)
      //     poseop_msgs.pose.covariance[6*u+v] = spinv(u, v);
      poseop_msgs.pose.covariance[0]=0.1;
      poseop_msgs.pose.covariance[7]=0.1;
      poseop_msgs.pose.covariance[14]=0.1;
      poseop_msgs.pose.covariance[21]=0.1;
      poseop_msgs.pose.covariance[28]=0.1;
      poseop_msgs.pose.covariance[35]=0.1;

}

// #ifndef G2O_TARGET_TYPES_6D_HPP_
// #define G2O_TARGET_TYPES_6D_HPP_

// #include <g2o/core/base_vertex.h>
// #include <g2o/core/base_binary_edge.h>
// #include <g2o/core/base_unary_edge.h>
// #include <Eigen/Core>

// using namespace g2o;

// typedef Eigen::Matrix<double,6,1> Vector6d;
// typedef Eigen::Matrix<double,6,6> Matrix6d;

// // This header file specifies a set of types for the different
// // tracking examples; note that

// class VertexPosition3D : public g2o::BaseVertex<3, Eigen::Vector3d>
// {
// public:
//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//   VertexPosition3D()
//   {
//   }

//   virtual void setToOriginImpl() {
//     _estimate.setZero();
//   }

//   virtual void oplusImpl(const double* update)
//   {
//     _estimate[0] += update[0];
//     _estimate[1] += update[1];
//     _estimate[2] += update[2];
//   }

//   virtual bool read(std::istream& /*is*/)
//   {
//     return false;
//   }

//   virtual bool write(std::ostream& /*os*/) const
//   {
//     return false;
//   }

// };

// class PositionVelocity3DEdge
// {
// };

// class VertexPositionVelocity3D : public g2o::BaseVertex<6, Vector6d>
// {
// public:
//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//   VertexPositionVelocity3D()
//   {
//   }

//   virtual void setToOriginImpl() {
//     _estimate.setZero();
//   }

//   virtual void oplusImpl(const double* update)
//   {
//     for (int k = 0; k < 6; k++)
//       _estimate[k] += update[k];
//   }

//   virtual bool read(std::istream& /*is*/)
//   {
//     return false;
//   }

//   virtual bool write(std::ostream& /*os*/) const
//   {
//     return false;
//   }

// };

// // The odometry which links pairs of nodes together
// class TargetOdometry3DEdge : public g2o::BaseBinaryEdge<6, Eigen::Vector3d, VertexPositionVelocity3D, VertexPositionVelocity3D>
// {
// public:
//   TargetOdometry3DEdge(double dt, double noiseSigma)
//   {
//     _dt = dt;

//     double q = noiseSigma * noiseSigma;
//     double dt2 = dt * dt;

//     // Process noise covariance matrix; this assumes an "impulse"
//     // noise model; we add a small stabilising term on the diagonal to make it invertible
//     Matrix6d Q=Matrix6d::Zero();
//     Q(0, 0) = Q(1,1) = Q(2,2) = dt2*dt2*q/4 + 1e-4;
//     Q(0, 3) = Q(1, 4) = Q(2, 5) = dt*dt2*q/2;
//     Q(3, 3) = Q(4,4) = Q(5,5) = dt2 * q + 1e-4;
//     Q(3, 0) = Q(4, 1) = Q(5, 2) = dt*dt2*q/2;

//     setInformation(Q.inverse());
//   }

//   /** set the estimate of the to vertex, based on the estimate of the from vertex in the edge. */
//   virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from, g2o::OptimizableGraph::Vertex* to){
//     assert(from.size() == 1);
//     const VertexPositionVelocity3D* vi = static_cast<const VertexPositionVelocity3D*>(*from.begin());
//     VertexPositionVelocity3D* vj = static_cast<VertexPositionVelocity3D*>(to);
//     Vector6d viEst=vi->estimate();
//     Vector6d vjEst=viEst;

//     for (int m = 0; m < 3; m++)
//     {
//       vjEst[m] += _dt * (vjEst[m+3] + 0.5 * _dt * _dt* _measurement[m]);
//     }

//     for (int m = 0; m < 3; m++)
//     {
//       vjEst[m+3] =0;//+= _dt * _measurement[m];
//     }

//     vjEst[0] = _measurement[0];
//     vjEst[1] = _measurement[1];
//     vjEst[2] = 0;
//     vj->setEstimate(vjEst);
//   }

//   /** override in your class if it's not possible to initialize the vertices in certain combinations */
//   virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& from, g2o::OptimizableGraph::Vertex* to) {
//     //only works on sequential vertices
//     const VertexPositionVelocity3D* vi = static_cast<const VertexPositionVelocity3D*>(*from.begin());
//     return (to->id() - vi->id() == 1) ? 1.0 : -1.0;
//   }

//   void computeError()
//   {
//     const VertexPositionVelocity3D* vi = static_cast<const VertexPositionVelocity3D*>(_vertices[0]);
//     const VertexPositionVelocity3D* vj = static_cast<const VertexPositionVelocity3D*>(_vertices[1]);

//     for (int k = 0; k < 3; k++)
//       {
//         _error[k] = vi->estimate()[k] /*+ _dt * (vi->estimate()[k+3] + 0.5 * _dt * _measurement[k]) - vj->estimate()[k]*/-_measurement[k];
//       }
//     for (int k = 3; k < 6; k++)
//       {
//         _error[k] = 0;//vi->estimate()[k] + _dt * _measurement[k-3]- vj->estimate()[k];
//       }
//   }

//   virtual bool read(std::istream& /*is*/)
//   {
//     return false;
//   }

//   virtual bool write(std::ostream& /*os*/) const
//   {
//     return false;
//   }

// private:
//   double _dt;
// };

// // The GPS
// class GPSObservationEdgePositionVelocity3D : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPositionVelocity3D>
// {
// public:
//   GPSObservationEdgePositionVelocity3D(const Eigen::Vector3d& measurement, double noiseSigma)
//   {
//     setMeasurement(measurement);
//     setInformation(Eigen::Matrix3d::Identity() / (noiseSigma*noiseSigma));
//   }

//   void computeError()
//   {
//     const VertexPositionVelocity3D* v = static_cast<const VertexPositionVelocity3D*>(_vertices[0]);
//     for (int k = 0; k < 3; k++)
//       {
//         _error[k] = v->estimate()[k] - _measurement[k];
//       }
//   }

//   virtual bool read(std::istream& /*is*/)
//   {
//     return false;
//   }

//   virtual bool write(std::ostream& /*os*/) const
//   {
//     return false;
//   }
// };

// #endif //  __TARGET_TYPES_6D_HPP__