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

#include <miopen/solver.hpp>

#include <miopen/env.hpp>
#include <miopen/kernel.hpp>
#include <miopen/kernel_build_params.hpp>
#include <miopen/stringutils.hpp>
#include <miopen/conv/compiled_in_parameters.hpp>
#include <miopen/conv/data_invoke_params.hpp>
#include <miopen/conv/wrw_invoke_params.hpp>
#include <miopen/generic_search.hpp>
#include <miopen/sequences.hpp>

#include <boost/any.hpp>

MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_WINOGRAD_RXS_F3X2)
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_WINOGRAD_RXS_F3X2_PERF_VALS)

#define WINODATA 3
#define WINOFILTER 2
#define MAX_CU_LIMIT 512

static inline size_t Ceil(const size_t v, const size_t m)
{
    assert(m > 0);
    return (v + m - 1) / m;
}

static inline size_t RoundUpToMultiple(size_t val, size_t factor)
{
    return Ceil(val, factor) * factor;
}

/// \todo Consider re-using code from RxS_f2x3.
static inline int GetBestNGroupParam(const int R,
                                     const int S,
                                     const int R_stride,
                                     const int S_stride,
                                     const int C,
                                     const int K,
                                     const int OH,
                                     const int OW,
                                     const int pad_H,
                                     const int pad_W,
                                     const int N,
                                     const int idilation_w,
                                     const int idilation_h,
                                     const int n_groups,
                                     const int G)
{
    int o_tile     = WINODATA;
    int f_tile     = WINOFILTER;
    int r_factor   = f_tile * 2;
    int s_factor   = r_factor;
    int c_factor   = 2;
    int k_factor   = 32;
    int nwh_factor = 32;
    int w_factor   = o_tile * idilation_w * S_stride;
    int h_factor   = o_tile * idilation_h * R_stride;

    if(S_stride == 1 && idilation_w == 1 && S <= f_tile)
        s_factor = f_tile;
    if((R_stride == 1 && idilation_h == 1) || (R % (f_tile * 2)) == 1)
        r_factor = f_tile;
    if(S_stride == 2 || R_stride == 2 || idilation_w == 2 || idilation_h == 2)
        c_factor = 1;

    size_t g_s = RoundUpToMultiple(S, s_factor);
    size_t g_r = RoundUpToMultiple(R, r_factor);
    size_t g_c = RoundUpToMultiple(C, c_factor);
    size_t g_k = RoundUpToMultiple(K, k_factor);
    size_t g_w = OW;
    size_t g_h = OH;

    if((pad_W % 2 == 0) && (idilation_w > 1 || S_stride > 1))
        g_w += 1;
    if((pad_H % 2 == 1) && (idilation_h > 1 || R_stride > 1))
        g_h += 1;

    g_w            = RoundUpToMultiple(g_w, w_factor);
    g_h            = RoundUpToMultiple(g_h, h_factor);
    size_t g_n_w_h = RoundUpToMultiple(g_w * g_h * N, nwh_factor * w_factor * h_factor);

    int best_n_groups_cnt = 1;
    double min_param      = 0;
    for(auto i = 1; i < n_groups; ++i)
    {
        size_t g_n_w_h_k =
            RoundUpToMultiple(g_n_w_h * g_k, nwh_factor * w_factor * h_factor * k_factor * i);
        size_t granulated_mac_count = g_n_w_h_k * g_c * g_s * g_r;
        size_t n_groups_per_cu      = Ceil(i * G, n_groups);
        double perf_metric = static_cast<double>(n_groups_per_cu) * granulated_mac_count / i;
        if(static_cast<double>(granulated_mac_count) / i > 1.0e+7)
            perf_metric *= (1 + i * 0.003);
        else
            perf_metric *= (1 + i * 0.04);
        if(i == 1)
            min_param = perf_metric;
        if(min_param > perf_metric)
        {
            best_n_groups_cnt = i;
            min_param         = perf_metric;
        }
    }
    return best_n_groups_cnt;
}

namespace miopen {
namespace solver {

namespace {
// clang-format off
auto PerfFieldRules()
{
    return seq::MakeRuleSet(
        std::make_tuple(seq::Span<int, 1, MAX_CU_LIMIT>{}, &PerformanceConfigConvBinWinogradRxSf3x2::n_groups)
    );
}
// clang-format on

/// \todo Consider re-using code from RxS_f2x3.
inline bool IsShaderContraintsMet(const int R,
                                  const int S,
                                  const int C,
                                  const int K,
                                  const int H,
                                  const int W,
                                  const int OH,
                                  const int OW,
                                  const int N,
                                  const ConvolutionContext& params)
{
    // Padding for bwd data shall not be negative.
    /// \todo Either remove WrW related code or re-use function from RxS
    if(params.direction.IsBackwardData())
    {
        if(!(0 <= params.GetBackwardPadW() && params.GetBackwardPadW() < std::pow(2, 16)))
            return false;
        if(!(0 <= params.GetBackwardPadH() && params.GetBackwardPadH() < std::pow(2, 16)))
            return false;
    }

    const auto grid_workgroup_count_x = params.GetStream().GetMaxHardwareComputeUnits();

    // clang-format off
    // Check implementation limits.
    return N < std::pow(2, 16)
        && C < std::pow(2, 16)
        && K < std::pow(2, 16)
        && H < std::pow(2, 16)
        && W < std::pow(2, 16)
        && OH < std::pow(2, 16)
        && OW < std::pow(2, 16)
        && params.pad_w < std::pow(2, 16)
        && params.pad_h < std::pow(2, 16)
        && S < std::pow(2, 16)
        && R < std::pow(2, 16)
        && grid_workgroup_count_x < std::pow(2, 16)
        && (C * H * W) <= std::pow(2, 28)
        && (OH * OW) <= std::pow(2, 23)
        && (K * OH * OW) <= std::pow(2, 28)
        && (K * R * S) <= std::pow(2, 28)
        && (C * R * S) <= std::pow(2, 28);
    // clang-format on
}

} // namespace

PerformanceConfigConvBinWinogradRxSf3x2::PerformanceConfigConvBinWinogradRxSf3x2(int n_groups_)
    : n_groups(n_groups_)
{
}

void PerformanceConfigConvBinWinogradRxSf3x2::HeuristicInit(const ConvolutionContext& config)
{
    const auto n_inputs_per_group  = config.n_inputs / config.group_counts,
               n_outputs_per_group = config.n_outputs / config.group_counts;
    if(config.group_counts == 1)
    {
        n_groups = config.GetStream().GetMaxHardwareComputeUnits();
        return;
    }

    if(config.direction.IsBackwardWrW())
    {
        n_groups = GetBestNGroupParam(config.in_height,
                                      config.in_width,
                                      config.kernel_dilation_h,
                                      config.kernel_dilation_w,
                                      config.batch_sz,    // N
                                      n_inputs_per_group, // K
                                      config.kernel_size_h,
                                      config.kernel_size_w,
                                      config.pad_w,
                                      config.pad_h,
                                      n_outputs_per_group, // C
                                      config.kernel_stride_h,
                                      config.kernel_stride_w,
                                      config.GetStream().GetMaxHardwareComputeUnits(),
                                      config.group_counts);
    }
    else
    {
        n_groups = GetBestNGroupParam(config.kernel_size_h, // RxS
                                      config.kernel_size_w,
                                      config.kernel_stride_h,
                                      config.kernel_stride_w,
                                      n_inputs_per_group,  // C
                                      n_outputs_per_group, // K
                                      config.out_height,   // OHxOW
                                      config.out_width,
                                      config.pad_w,
                                      config.pad_h,
                                      config.batch_sz, // N
                                      config.kernel_dilation_h,
                                      config.kernel_dilation_w,
                                      config.GetStream().GetMaxHardwareComputeUnits(),
                                      config.group_counts);
    }
}

bool PerformanceConfigConvBinWinogradRxSf3x2::SetNextValue()
{
    return !PerfFieldRules().Next(*this);
}

bool PerformanceConfigConvBinWinogradRxSf3x2::IsValidValue() const
{
    return PerfFieldRules().IsIn(*this);
}

bool PerformanceConfigConvBinWinogradRxSf3x2::IsValid(const ConvolutionContext& config) const
{
    if(config.GetStream().GetMaxHardwareComputeUnits() < n_groups)
        return false;

    if(!IsValidValue())
        return false;
    return true;
}

inline bool PerformanceConfigConvBinWinogradRxSf3x2::
operator==(const PerformanceConfigConvBinWinogradRxSf3x2& other) const
{
    return n_groups == other.n_groups;
}

std::string PerformanceConfigConvBinWinogradRxSf3x2::ToString() const
{
    std::ostringstream ss;
    Serialize(ss);
    return ss.str();
}

PerformanceConfigConvBinWinogradRxSf3x2
ConvBinWinogradRxSf3x2::GetPerformanceConfig(const ConvolutionContext& params) const
{
    PerformanceConfigConvBinWinogradRxSf3x2 pp;
    pp.HeuristicInit(params);
    MIOPEN_LOG_I(pp.ToString());
    return pp;
}

bool ConvBinWinogradRxSf3x2::IsValidPerformanceConfig(
    const ConvolutionContext& problem, const PerformanceConfigConvBinWinogradRxSf3x2& c) const
{
    return c.IsValidValue() && c.IsValid(problem);
}

PerformanceConfigConvBinWinogradRxSf3x2
ConvBinWinogradRxSf3x2::Search(const ConvolutionContext& context,
                               const AnyInvokeParams& invoke_ctx) const
{
    return GenericSearch(*this, context, invoke_ctx);
}

bool ConvBinWinogradRxSf3x2::IsApplicable(const ConvolutionContext& params) const
{
    if(!params.Is2d())
        return false;
    if(!params.IsFp32())
        return false;
    if(miopen::IsDisabled(MIOPEN_DEBUG_AMD_WINOGRAD_RXS_F3X2{}))
        return false;
    if(!params.use_asm_kernels)
        return false;
    if(!params.rmv.IsV3())
        return false;
    if(!params.IsLayoutDefault())
        return false;

    const auto max_cu = params.GetStream().GetMaxHardwareComputeUnits();
    if(max_cu > MAX_CU_LIMIT)
        return false;

    const auto name = params.GetStream().GetDeviceName();
    if(!(StartsWith(name, "gfx9") || StartsWith(name, "gfx10")) || name == "gfx90a")
        return false;

    // clang-format off
    if (! (params.kernel_stride_w == 1
        && params.kernel_stride_w == params.kernel_stride_h
        && params.kernel_dilation_w == 1
        && params.kernel_dilation_h == 1
        && params.bias == 0))
        return false;
    // clang-format on

    const auto n_inputs_per_group  = params.n_inputs / params.group_counts,
               n_outputs_per_group = params.n_outputs / params.group_counts;

    if(params.direction.IsBackwardWrW())
    {
        return IsShaderContraintsMet(params.in_height,
                                     params.in_width,
                                     params.batch_sz,    // N
                                     n_inputs_per_group, // K
                                     params.out_height,
                                     params.out_width,
                                     params.kernel_size_h,
                                     params.kernel_size_w,
                                     n_outputs_per_group, // C
                                     params);
    }
    else
    {
        return IsShaderContraintsMet(params.kernel_size_h, // RxS
                                     params.kernel_size_w,
                                     n_inputs_per_group,  // C
                                     n_outputs_per_group, // K
                                     params.in_height,    // HxW
                                     params.in_width,
                                     params.out_height, // OHxOW
                                     params.out_width,
                                     params.batch_sz, // N
                                     params);
    }
}

/// \todo Consider re-using code from RxS_f2x3.
ConvSolution
ConvBinWinogradRxSf3x2::GetSolution(const ConvolutionContext& params,
                                    const PerformanceConfigConvBinWinogradRxSf3x2& config,
                                    const bool disableConfigOverrideFromEnv) const
{
    const PerformanceConfigConvBinWinogradRxSf3x2* pcfg = &config;
    PerformanceConfigConvBinWinogradRxSf3x2 fromEnv;
    if(!disableConfigOverrideFromEnv)
    {
        std::string s;
        const auto p_asciz = miopen::GetStringEnv(MIOPEN_DEBUG_AMD_WINOGRAD_RXS_F3X2_PERF_VALS{});
        if(p_asciz != nullptr)
        {
            s = std::string(p_asciz);
            if(!s.empty()) // else nothing to parse.
            {
                if(!fromEnv.Deserialize(s) || !fromEnv.IsValid(params))
                {
                    MIOPEN_LOG_E("MIOPEN_DEBUG_AMD_WINOGRAD_RXS_F3X2_PERF_VALS: "
                                 "Bad format or invalid for the problem config: "
                                 << s);
                }
                else
                {
                    MIOPEN_LOG_I("Overridden from env: " << fromEnv.ToString());
                    pcfg = &fromEnv;
                }
            }
        }
    }

    ConvSolution result;
    KernelInfo kernel;

    const auto n_groups = pcfg->GetNGroups();
    const auto name     = params.GetStream().GetDeviceName();
    const auto is_gfx9  = StartsWith(name, "gfx9");
    size_t wg_size      = is_gfx9 ? 512 : 256;

    kernel.g_wk.push_back(wg_size * n_groups * params.group_counts);
    kernel.g_wk.push_back(1);
    kernel.g_wk.push_back(1);

    kernel.l_wk.push_back(wg_size);
    kernel.l_wk.push_back(1);
    kernel.l_wk.push_back(1);

    KernelBuildParameters options{
        {"ROCM_METADATA_VERSION", 5},
    };
    kernel.comp_options = options.GenerateFor(kbp::GcnAsm{});

    std::string kernel_name    = "miopenSp3AsmConv_v21_1_2";
    std::string kernel_file    = "Conv_Winograd_v21_1_2";
    std::string kernel_postfix = params.IsFp32() ? "_f3x2_fp32" : "_f3x2_fp16_dot2_edc";

    if(is_gfx9)
    {
        kernel_name += "_gfx9";
    }
    else // if(StartsWith(name, "gfx10"))
    {
        kernel_name += "_gfx10";
        kernel.comp_options += std::string(" -mcumode -mwavefrontsize64");
    }

    if(params.kernel_stride_w == 1)
    {
        kernel_postfix += "_stride1";
    }

    kernel_postfix += "_group";
    kernel.kernel_name = kernel_name + kernel_postfix;
    kernel.kernel_file = kernel_file + kernel_postfix + ".s";

    result.construction_params.push_back(kernel);

    if(!params.direction.IsBackwardWrW())
    {
        const bool is_forward     = params.direction.IsForward();
        constexpr int F_REVERSE_R = 1 << 0;
        constexpr int F_REVERSE_S = 1 << 1;
        constexpr int F_FLIP_K_C  = 1 << 2;
        // These are not used yet. Nevertheless let's keep as a shader documentation.
        // constexpr int F_FLIP_DATA_N_C = 1 << 3; // Unsupported in f3x2.
        // constexpr int F_FLIP_OUT_N_K = 1 << 4; // Unsupported in f3x2.
        // constexpr int L_F_ADDR_INDIRECT  = 1 << 6;
        // constexpr int L_F_BIAS  = 1 << 7;
        // constexpr int L_F_LEAKY_RELU  = 1 << 8;
        constexpr int L_F_NKC_STRIDES   = 1 << 9;
        constexpr int L_F_GROUP_STRIDES = 1 << 10;
        // constexpr int L_F_FORCE_FILTER_TRAVERSE_MODE  = 1 << 11;
        // constexpr int L_F_FILTER_TRAVERSE_DUAL  = 1 << 12;
        // constexpr int L_F_TENSOR_OFFSETS  = 1 << 13;
        // constexpr int L_F_USE_EXTENDED_FLAGS_64  = 1 << 15;
        int reserved             = 0;
        uint64_t reserved_offset = 0;
        int* reserved_ptr        = nullptr;
        int ignore;

        int N, C, H, W, K, out_H, out_W, R, S, pad_H, pad_W;
        GetCompiledInParameters(
            params, &N, &C, &H, &W, &K, &ignore, &out_H, &out_W, &R, &S, &pad_H, &pad_W);
        const auto group_cnt = params.group_counts;
        C                    = C / group_cnt;
        K                    = K / group_cnt;
        int flags            = is_forward ? 0 : F_REVERSE_R + F_REVERSE_S + F_FLIP_K_C;
        flags |= L_F_NKC_STRIDES + L_F_GROUP_STRIDES;

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

        result.invoker_factory = [=](std::vector<Kernel> kernels) {
            return [=](const Handle& handle, const AnyInvokeParams& primitive_params) {
                const auto k         = handle.Run(kernels[0]);
                const auto& data_ctx = primitive_params.CastTo<conv::DataInvokeParams>();
                const auto& tensors  = data_ctx.tensors;

                // clang-format off
                MIOPEN_LOG_I2(" N=" << N << " G=" << group_cnt << " C=" << C << " H=" << H << " W=" << W << " K=" << K
                    << " n_groups=" << n_groups << " flags=" << flags << " 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
                    << " f_buf.byte_stride.nk=" << f_buf.byte_stride.nk << " f_buf.byte_stride.c=" << f_buf.byte_stride.c
                    << " f_buf.byte_stride.h=" << f_buf.byte_stride.h << " f_buf.byte_stride.w=" << f_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
                    << " f_buf.byte_stride.g=" << f_buf.byte_stride.g); // clang-format on

                k(N,
                  C,
                  H,
                  W,
                  K,
                  n_groups,
                  flags,
                  reserved,
                  tensors.in,
                  tensors.w,
                  tensors.out,
                  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.
                  reserved,        // Unused reserved2.
                  reserved_offset, // Unused d_offset.
                  reserved_offset, // Unused f_offset.
                  reserved_offset, // Unused o_offset.
                  reserved_offset, // Unused b_offset.
                  d_buf.byte_stride.nk,
                  d_buf.byte_stride.c,
                  d_buf.byte_stride.h,
                  d_buf.byte_stride.w,
                  f_buf.byte_stride.nk,
                  f_buf.byte_stride.c,
                  f_buf.byte_stride.h,
                  f_buf.byte_stride.w,
                  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,
                  f_buf.byte_stride.g,
                  o_buf.byte_stride.g);
            };
        };
    }
    else
    {
        int unused = 0;
        int N, C, H, W, K, out_H, out_W, R, S;
        GetCompiledInParameters(
            params, &C, &K, &R, &S, &N, &unused, &H, &W, &out_H, &out_W, &unused, &unused);
        const auto group_cnt             = params.group_counts;
        static const int F_NKC_STRIDES   = 1 << 9;
        static const int F_GROUP_STRIDES = 1 << 10;
        int flags                        = F_NKC_STRIDES + F_GROUP_STRIDES;
        N                                = N / group_cnt;
        K                                = K / group_cnt;
        int pad_H                        = params.conv_problem.GetConv().GetConvPads()[0];
        int pad_W                        = params.conv_problem.GetConv().GetConvPads()[1];

        BuffInfo d_buf(
            GetGroupConvLayout(GetSwappedNCLayout(GetMemLayout_t(params.in_layout)), true),
            N,
            C,
            H,
            W,
            group_cnt,
            GetTypeSize(params.in_data_type)),
            o_buf(GetGroupConvLayout(GetSwappedNCLayout(GetMemLayout_t(params.out_layout)), false),
                  N,
                  K,
                  out_H,
                  out_W,
                  group_cnt,
                  GetTypeSize(params.out_data_type)),
            f_buf(GetGroupConvLayout(GetSwappedNCLayout(MemLayout_t::NCHW), true),
                  K,
                  C,
                  R,
                  S,
                  group_cnt,
                  GetTypeSize(params.weights_data_type));

        decltype(auto) batch_sz = params.batch_sz;
        decltype(auto) n_inputs = params.n_inputs;

        result.invoker_factory = [=](std::vector<Kernel> kernels) {
            return [=](const Handle& handle, const AnyInvokeParams& primitive_params) {
                decltype(auto) invoke_params = primitive_params.CastTo<conv::WrWInvokeParams>();
                const auto& tensors          = invoke_params.tensors;

                // clang-format off
                MIOPEN_LOG_I2(" N=" << N << " G=" << group_cnt << " C=" << C << " H=" << H << " W=" << W << " K=" << K
                    << " n_groups=" << n_groups << " flags=" << flags << " 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
                    << " f_buf.byte_stride.nk=" << f_buf.byte_stride.nk << " f_buf.byte_stride.c=" << f_buf.byte_stride.c
                    << " f_buf.byte_stride.h=" << f_buf.byte_stride.h << " f_buf.byte_stride.w=" << f_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
                    << " f_buf.byte_stride.g=" << f_buf.byte_stride.g); // clang-format on
                MIOPEN_LOG_I2(" ctx.batch_sz=" << batch_sz << "ctx.n_inputs=" << n_inputs);

                int reserved             = 0;
                uint64_t reserved_offset = 0;
                int* reserved_ptr        = nullptr;

                handle.Run(kernels[0])(N,
                                       C,
                                       H,
                                       W,
                                       K,
                                       n_groups,
                                       flags,
                                       reserved,
                                       tensors.x,
                                       tensors.dy,
                                       tensors.dw,
                                       reserved_ptr, // Unused return_addr.
                                       R,
                                       S,
                                       pad_H,
                                       pad_W,
                                       out_H,
                                       out_W,
                                       reserved_ptr,    // Unused bias_addr.
                                       reserved,        // Unused relu_alpha.
                                       reserved,        // Unused reserved2.
                                       reserved_offset, // Unused d_offset.
                                       reserved_offset, // Unused f_offset.
                                       reserved_offset, // Unused o_offset.
                                       reserved_offset, // Unused b_offset.
                                       d_buf.byte_stride.nk,
                                       d_buf.byte_stride.c,
                                       d_buf.byte_stride.h,
                                       d_buf.byte_stride.w,
                                       f_buf.byte_stride.nk,
                                       f_buf.byte_stride.c,
                                       f_buf.byte_stride.h,
                                       f_buf.byte_stride.w,
                                       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,
                                       f_buf.byte_stride.g,
                                       o_buf.byte_stride.g);
            };
        };
    }

    return result;
}

} // namespace solver
} // namespace miopen


// Boost.Geometry (aka GGL, Generic Geometry Library)

// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DIFFERENCE_BOX_BOX_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DIFFERENCE_BOX_BOX_HPP
#include <algorithm>
#include <boost/foreach.hpp>
#include <boost/geometry/util/range.hpp>
#include <boost/geometry/algorithms/is_valid.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace difference
{

template
<
    typename OutBox,
    int corner,
    size_t dim,
    size_t end,
    size_t i
>
struct set_for_each {
    typedef typename traits::point_type<OutBox>::type PointOut;
    typedef typename traits::coordinate_type<PointOut>::type CoordinatesOut;
        static inline void impl(
            OutBox arr[2 * dim],
            const CoordinatesOut &val)
        {
            set<corner, dim>(arr[i], val);
            set_for_each<OutBox, corner, dim, end, i + 1>::impl(arr, val);
        }
    };

template
<
    typename OutBox,
    int corner,
    size_t dim,
    size_t end
>
struct set_for_each<OutBox, corner, dim, end, end> {
    typedef typename traits::point_type<OutBox>::type PointOut;
    typedef typename traits::coordinate_type<PointOut>::type CoordinatesOut;
        static inline void impl(
            OutBox arr[2*dim],
            const CoordinatesOut &val) { }
};

template
<
    typename Box1,
    typename Box2,
    typename OutBox,
    int coord_dim,
    int dim
>
struct box_diff_rec {
    static inline void
        impl(const Box1 &minuend, const Box2 &subtrahend,
            OutBox (&candidates)[2*dim]) {
    typedef typename traits::point_type<Box1>::type Point1;
    typedef typename traits::point_type<Box2>::type Point2;
    typedef typename traits::coordinate_type<Point1>::type Coordinates1;
    typedef typename traits::coordinate_type<Point2>::type Coordinates2;
    const Coordinates1 minuend_min = get<min_corner, coord_dim>(minuend);
    const Coordinates1 minuend_max = get<max_corner, coord_dim>(minuend);
    const Coordinates2 subtrahend_min = get<min_corner, coord_dim>(subtrahend);
    const Coordinates2 subtrahend_max = get<max_corner, coord_dim>(subtrahend);

    set_for_each<OutBox, min_corner, coord_dim, 2 * coord_dim, 0>::impl(candidates, minuend_min);
    set_for_each<OutBox, max_corner, coord_dim, 2 * coord_dim, 0>::impl(candidates, minuend_max);
    set<min_corner, coord_dim>(candidates[2 * coord_dim], minuend_min);
    set<max_corner, coord_dim>(candidates[2 * coord_dim], subtrahend_min);
    set<min_corner, coord_dim>(candidates[2 * coord_dim+1], subtrahend_max);
    set<max_corner, coord_dim>(candidates[2 * coord_dim+1], minuend_max);
    set_for_each<OutBox, min_corner, coord_dim, 2 * dim, 2 * (coord_dim + 1)>::impl(candidates,
        std::max(subtrahend_min, minuend_min));
    set_for_each<OutBox, max_corner, coord_dim, 2 * dim, 2 * (coord_dim + 1)>::impl(candidates,
    std::min(subtrahend_max, minuend_max));
        box_diff_rec<Box1, Box2, OutBox, coord_dim + 1,dim>::impl(minuend, subtrahend, candidates);
    }
};

template <
    typename Box1,
    typename Box2,
    typename OutBox,
    int dim
>
struct box_diff_rec<Box1, Box2, OutBox, dim, dim>
    {
        static inline void
            impl(const Box1 &minuend, const Box2 &subtrahend,
                OutBox (&candidates)[2 * dim])
        {
        }
    };

template
<
    typename Box1,
    typename Box2,
    typename BoxCollection
>
struct difference_box_box {
    typedef typename BoxCollection::value_type OutBox;
    typedef typename traits::point_type<Box1>::type Point1;
    typedef typename traits::point_type<Box2>::type Point2;
    typedef typename traits::point_type<OutBox>::type PointOut;
    typedef typename traits::coordinate_type<Point1>::type Coordinates1;
    typedef typename traits::coordinate_type<Point2>::type Coordinates2;
    static const std::size_t dim = dimension<PointOut>::type::value;

    static inline void apply(Box1 const& minuend,
        Box2 const& subtrahend,
        BoxCollection& output_collection)
    {
        range::back_insert_iterator<BoxCollection> out(output_collection);
        if (boost::geometry::disjoint(minuend, subtrahend))
        {
            OutBox m;
            boost::geometry::transform(minuend, m);
            out=m;
        }
        else
        {
            OutBox candidates[2 * dim];
            box_diff_rec<Box1, Box2, OutBox, 0, dim>::impl(minuend, subtrahend, candidates);
            BOOST_FOREACH(OutBox& candidate, candidates)
            {
                if (boost::geometry::is_valid(candidate))
                    *out++ = candidate;
            }
        }
    }
};

}} // namespace detail::difference
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DIFFERENCE_BOX_BOX_HPP


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  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 OWNER 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.

#include <cmath>
#include <Eigen/Geometry>
#include <comma/math/compare.h>
#include "range_bearing_elevation.h"

namespace snark {

Eigen::Vector2d bearing::to_cartesian( const double radians, const double radius )
{
    return Eigen::Vector2d( radius * std::sin( radians ), radius * std::cos( radians ) );
}

double bearing::from_cartesian( const double x, const double y )
{
    double radians = ( M_PI / 2 ) - std::atan2( y, x );
    return radians < 0 ? radians + 2 * M_PI : radians;
}

bearing_elevation::bearing_elevation() : bearing_( 0 ), elevation_( 0 ) {}

bearing_elevation::bearing_elevation( double b, double e ) : bearing_( bearing( b ) ), elevation_( elevation( e ) ) {}

double bearing_elevation::bearing() const { return bearing_; }

double bearing_elevation::elevation() const { return elevation_; }

double bearing_elevation::b() const { return bearing(); }

double bearing_elevation::e() const { return elevation(); }

double bearing_elevation::b( double b ) { return bearing( b ); }

double bearing_elevation::e( double e ) { return elevation( e ); }

static double mod_( double t, double m ) // quick and dirty, because std::fmod is ridiculously slow
{
    int r = std::abs( t / m );
    return comma::math::less( t, 0 ) ? ( t + m * r ) : ( t - m * r );
}

double bearing_elevation::bearing( double t )
{
    double b( mod_( t, ( double )( M_PI * 2 ) ) ); //double b( std::fmod( t, ( double )( M_PI * 2 ) ) );
    if( !comma::math::less( b, M_PI ) ) { b -= ( M_PI * 2 ); }
    else if( comma::math::less( b, -M_PI ) ) { b += ( M_PI * 2 ); }
    bearing_ = b;
    return bearing_;
}

double bearing_elevation::elevation( double t )
{
    double e( mod_( t, ( double )( M_PI * 2 ) ) ); //double e( std::fmod( t, ( double )( M_PI * 2 ) ) );
    if( comma::math::less( e, 0 ) ) { e += M_PI * 2; }
    if( !comma::math::less( e, M_PI / 2 ) )
    {
        if( !comma::math::less( e, M_PI ) )
        {
            if( !comma::math::less( e, M_PI * 3 / 2 ) )
            {
                e = e - M_PI * 2;
            }
            else
            {
                e = M_PI - e;
                bearing( bearing() + M_PI );
            }
        }
        else
        {
            e = M_PI - e;
            bearing( bearing() + M_PI );
        }
    }
    elevation_ = e;
    return elevation_;
}

range_bearing_elevation::range_bearing_elevation() { range( 0 ); }

range_bearing_elevation::range_bearing_elevation( double r, double b, double e )
{
    bearing_elevation_.bearing( b );
    bearing_elevation_.elevation( e );
    range( r );
}

double range_bearing_elevation::range( double t )
{
    if( !comma::math::less( t, 0 ) )
    {
        range_ = t;
    }
    else
    {
        range_ = -t;
        bearing_elevation_.bearing( -bearing_elevation_.bearing() );
        bearing_elevation_.elevation( -bearing_elevation_.elevation() );
    }
    return range_;
}

Eigen::Vector3d range_bearing_elevation::to_cartesian() const
{
    double xy_projection( range() * std::cos( elevation() ) );
    return ::Eigen::Matrix< double, 3, 1 >( xy_projection * std::cos( bearing() )
                                    , xy_projection * std::sin( bearing() )
                                    , range() * std::sin( elevation() ) );
}

const range_bearing_elevation& range_bearing_elevation::from_cartesian( double x, double y, double z )
{
    return from_cartesian( Eigen::Vector3d( x, y, z ) );
}

const range_bearing_elevation& range_bearing_elevation::from_cartesian( const Eigen::Vector3d& v )
{
    range_ = v.norm();
    if( comma::math::equal( range_, 0 ) ) { bearing_elevation_ = snark::bearing_elevation( 0, 0 ); return *this; }
    const Eigen::AngleAxis< double >& a =  Eigen::AngleAxis< double >( Eigen::Quaternion< double >::FromTwoVectors( v, Eigen::Vector3d( 0, 0, 1 ) ) );
    double e = M_PI / 2 - a.angle();
    Eigen::AngleAxis< double > c( -e, a.axis() );
    const Eigen::Matrix3d& r = c.toRotationMatrix();
    Eigen::AngleAxis< double > d( Eigen::Quaternion< double >::FromTwoVectors( Eigen::Vector3d( 1, 0, 0 ), r * v ) );
    bearing_elevation_.bearing( d.angle() * ( d.axis().z() < 0 ? -1 : 1 ) );
    bearing_elevation_.elevation( e );
    return *this;
}

// const range_bearing_elevation& range_bearing_elevation::from_cartesian( const Eigen::Vector3d& xyz )
// {
//     return from_cartesian( xyz[0], xyz[1], xyz[2] );
// }
//
// const range_bearing_elevation& range_bearing_elevation::from_cartesian( double x, double y, double z )
// { // todo: use rotation matrices instead!
//     long double projection_square ( x * x +  y * y );
//     if ( comma::math::equal( projection_square, 0 ) )
//     {
//         if ( comma::math::equal( z, 0 ) )
//         {
//             range_ = 0;
//             bearing_elevation_.bearing( 0 );
//             bearing_elevation_.elevation( 0 );
//             return *this;
//         }
//         range_ = std::abs( z );
//         bearing_elevation_.bearing( 0 );
//         elevation( comma::math::less( z, 0 ) ? -M_PI / 2 : M_PI / 2 );
//         return *this;
//     }
//     long double range_square( projection_square + z * z );
//     long double lr( std::sqrt( range_square ) );
//     long double elevation_ = 0;
//     if ( !comma::math::equal( z, 0 ) )
//     {
//         long double r = z / lr;
//         if ( comma::math::less( ( long double )( 1.0 ), r ) ) { r = 1; }
//         else if ( comma::math::less( r, ( long double ) ( -1.0 ) ) ) { r = -1; }
//         elevation_ = std::asin( r );
//     }
//     long double r = x / std::sqrt( projection_square );
//     if ( comma::math::less( ( long double )( 1.0 ), r ) ) { r = 1; }
//     else if ( comma::math::less( r, ( long double ) ( -1.0 ) ) ) { r = -1; }
//     long double bearing_ = std::acos( r );
//     if ( comma::math::less( y, 0 ) ) { bearing_ = M_PI * 2 - bearing_; }
//     range_ = lr;
//     bearing( bearing_ );
//     elevation( elevation_ );
//     return *this;
// }

Eigen::AngleAxis< double > great_circle_angle_axis( const bearing_elevation& lhs, const bearing_elevation& rhs )
{
    Eigen::Vector3d a = rbe( 1, lhs.bearing(), lhs.elevation() ).to_cartesian();
    Eigen::Vector3d b = rbe( 1, rhs.bearing(), rhs.elevation() ).to_cartesian();
    return Eigen::AngleAxis< double >( Eigen::Quaternion< double >::FromTwoVectors( a, b ) );
}

} // namespace snark {


//
// Object to cluster straw hits, used in background removal and track fitting.  This
// implementation uses the 2-level distance scheme, where hits are associated if their
// distance to an existing cluster is below the first threshold, and are used to seed
// a new cluster if their distance to every other existing cluster is above the 2nd threshold
//
//  David Brown (LBNL) 2013
//
#ifndef TLTClusterer_HH
#define TLTClusterer_HH

// base class
#include "TrkReco/inc/BkgClusterer.hh"
// framework
#include "fhiclcpp/ParameterSet.h"
// root
#include "TTree.h"
// boost
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/mean.hpp>

namespace mu2e
{

  class TLTClusterer : public BkgClusterer
  {
  public:
  enum PosAlgorithm { median=0, mean}; // algorithm for comuting cluster position
// parameter set should be passed in on construction
  explicit TLTClusterer(fhicl::ParameterSet const&);
  virtual ~TLTClusterer();
  virtual void init();
  //  base class function: given the straw hits and associated data, find the clusters.
  virtual void findClusters(BkgClusterCollection& clusters,
      ComboHitCollection const& chcol);
  private:
  // functions to define the distance between a hit and cluster
  // this is the variable against which the clustering thresholds are applied
  float distance(BkgCluster const&, ComboHit const& ch) const;
  // merge overlapping clusters
  unsigned mergeClusters(BkgClusterCollection& bkgcol,
      float dt, float drho2,
      ComboHitCollection const& chcol ) const;
 // compress out empties
  void cleanClusters(BkgClusterCollection& bkgcol ) const;
  // initialize clusters.  Different strategeis can be tried
  void initClusters( BkgClusterCollection& clusters, ComboHitCollection const& chcol) const;
  // assign hits to the given clusters.  Hits already assigned
  // are tested first, for efficiency
  void assignHits(BkgClusterCollection& bkgcol, ComboHitCollection const& chcol) const;
  // compute the total distance between hits and clusters
  float hitDistance(BkgClusterCollection const& bkgcol, ComboHitCollection const& chcol) const;
  // update the cluster state (position, distances)
  void updateClusters(BkgClusterCollection& clusters, ComboHitCollection const& chcol) const;
  //
  void updateCluster(BkgCluster& cluster, ComboHitCollection const& chcol) const;
  //
  void countClusters(BkgClusterCollection const& clusters, unsigned& nclu, unsigned& nchits, float& tdist) const;
  void refineClusters(BkgClusterCollection& clusters, ComboHitCollection const& chcol) const;
  void refineCluster(BkgCluster& cluster, ComboHitCollection const& chcol ) const;

  // configuration parameters
  int _diag,_debug;
  PosAlgorithm _palg;
  bool _testflag;
  StrawHitFlag _bkgmask; // mask for hits to ignore
  StrawHitFlag _sigmask; // mask for hits to use
  StrawHitFlag _stereo; // mask for selecting stereo hits
  float _dseed; // Minimum separation to seed a new cluster
  float _dhit; // Maximum separation to include a hit in a cluster
  float _dd; // cluster diameter
  float _dd2; // cluster diameter squared, cached for efficiency
  float _dt; // natural time spread
  float _maxdt; // maximum time difference
  float _trms2; // time RMS
  float _md2; // // cached square of maximum distance
  float _maxdsum; // maximum total distance change to consider 'converged'
  unsigned _maxniter; // maximum number of iterations
  bool _stereoinit; // initialize using only stereo hits
  float _maxwt; // maximum weight to give a single hit
  float _minerr; // minimum error to give a single hit

  // diagnostics
  mutable TTree* _idiag; // iteration diagnostics
  mutable unsigned _niter, _nmerge, _nclu, _nhits, _nchits;
  mutable float _odist, _tdist;
  };
}
#endif


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <mmtbx/bulk_solvent/bulk_solvent.h>
#include <mmtbx/f_model/f_model.h>

#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>

namespace mmtbx { namespace bulk_solvent {
namespace {

  void init_module()
  {
    using namespace boost::python;
    using boost::python::arg;
    typedef return_value_policy<return_by_value> rbv;

    class_<ls_kbp_sol_u_star<> >(
      "ls_kbp_sol_u_star")
      .def(init<
           f_model::core<double, std::complex<double> > const&,
           af::const_ref<double> const&,
           double,
           bool const&,
           bool const&,
           bool const&,
           bool const&,
           bool const& >(
             (arg("f_model"),
              arg("f_obs"),
              arg("scale"),
              arg("kb_sol_grad"),
              arg("p_sol_grad"),
              arg("u_star_grad"),
              arg("kb_sol_curv"),
              arg("p_sol_curv"))))
      .def("target",      &ls_kbp_sol_u_star<>::target)
      .def("grad_u_star", &ls_kbp_sol_u_star<>::grad_u_star)
      .def("grad_k_sols", &ls_kbp_sol_u_star<>::grad_k_sols)
      .def("grad_b_sols", &ls_kbp_sol_u_star<>::grad_b_sols)
      .def("grad_p_sols", &ls_kbp_sol_u_star<>::grad_p_sols)
      .def("curv_k_sols", &ls_kbp_sol_u_star<>::curv_k_sols)
      .def("curv_b_sols", &ls_kbp_sol_u_star<>::curv_b_sols)
      .def("curv_p_sols", &ls_kbp_sol_u_star<>::curv_p_sols)
   ;

   class_<ls_u_star<> >(
      "ls_u_star")
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<cctbx::miller::index<> > const&,
           af::const_ref<double> const& >(
             (arg("f_model_abs_no_k_total"),
              arg("f_obs"),
              arg("miller_indices"),
              arg("k_anisotropic"))))
      .def("target",      &ls_u_star<>::target)
      .def("grad_u_star", &ls_u_star<>::grad_u_star)
   ;

   class_<f_kb_scaled<> >(
      "f_kb_scaled")
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<double> const& >(
             (arg("f1"),
              arg("f2"),
              arg("b_range"),
              arg("ss"))))
      .def("k", &f_kb_scaled<>::k)
      .def("b", &f_kb_scaled<>::b)
      .def("scaled", &f_kb_scaled<>::scaled)
   ;

   class_<complex_f_kb_scaled<> >(
      "complex_f_kb_scaled")
      .def(init<
           af::const_ref<std::complex<double> > const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<double> const&,
           af::const_ref<double> const& >(
             (arg("f1"),
              arg("f2"),
              arg("b_range"),
              arg("ss"))))
      .def("k", &complex_f_kb_scaled<>::k)
      .def("b", &complex_f_kb_scaled<>::b)
      .def("scaled", &complex_f_kb_scaled<>::scaled)
   ;

   class_<k_sol_b_sol_k_anisotropic_scaler_twin<> >(
      "k_sol_b_sol_k_anisotropic_scaler_twin")
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<double> const&,
           double const&,
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<cctbx::miller::index<> > const&,
           cctbx::uctbx::unit_cell const&,
           double const&>(
             (arg("f_obs"),
              arg("f_calc_1"),
              arg("f_calc_2"),
              arg("f_mask_1"),
              arg("f_mask_2"),
              arg("ss"),
              arg("twin_fraction"),
              arg("k_sol_range"),
              arg("b_sol_range"),
              arg("miller_indices"),
              arg("unit_cell"),
              arg("r_ref"))))
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           double const&>(
             (arg("f_obs"),
              arg("f_calc"),
              arg("f_mask"),
              arg("k_total"),
              arg("ss"),
              arg("k_sol_range"),
              arg("b_sol_range"),
              arg("r_ref"))))
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<cctbx::miller::index<> > const&,
           double const&>(
             (arg("f_obs"),
              arg("f_calc"),
              arg("f_mask"),
              arg("ss"),
              arg("k_sol_range"),
              arg("b_sol_range"),
              arg("miller_indices"),
              arg("r_ref"))))
      .def("r", &k_sol_b_sol_k_anisotropic_scaler_twin<>::r)
      .def("k_sol", &k_sol_b_sol_k_anisotropic_scaler_twin<>::k_sol)
      .def("b_sol", &k_sol_b_sol_k_anisotropic_scaler_twin<>::b_sol)
      .def("k_mask", &k_sol_b_sol_k_anisotropic_scaler_twin<>::k_mask)
      .def("k_anisotropic", &k_sol_b_sol_k_anisotropic_scaler_twin<>::k_anisotropic)
      .def("updated", &k_sol_b_sol_k_anisotropic_scaler_twin<>::updated)
      .def("u_star", &k_sol_b_sol_k_anisotropic_scaler_twin<>::u_star)
   ;

   class_<overall_and_bulk_solvent_scale_coefficients_analytical<> >(
      "overall_and_bulk_solvent_scale_coefficients_analytical")
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<bool> const& >(
             (arg("f_obs"),
              arg("f_calc"),
              arg("f_mask"),
              arg("selection"))))
      .add_property("x", make_getter(
        &overall_and_bulk_solvent_scale_coefficients_analytical<>::x, rbv()))
      .add_property("r", make_getter(
        &overall_and_bulk_solvent_scale_coefficients_analytical<>::r, rbv()))
      .add_property("x_best", make_getter(
        &overall_and_bulk_solvent_scale_coefficients_analytical<>::x_best, rbv()))
      .add_property("r_best", make_getter(
        &overall_and_bulk_solvent_scale_coefficients_analytical<>::r_best, rbv()))
   ;

   class_<bulk_solvent_scale_coefficients_analytical<> >(
      "bulk_solvent_scale_coefficients_analytical")
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<std::complex<double> > const&,
           af::const_ref<bool> const& >(
             (arg("f_obs"),
              arg("f_calc"),
              arg("f_mask"),
              arg("selection"))))
      .add_property("x", make_getter(
        &bulk_solvent_scale_coefficients_analytical<>::x, rbv()))
      .add_property("r", make_getter(
        &bulk_solvent_scale_coefficients_analytical<>::r, rbv()))
      .add_property("x_best", make_getter(
        &bulk_solvent_scale_coefficients_analytical<>::x_best, rbv()))
      .add_property("r_best", make_getter(
        &bulk_solvent_scale_coefficients_analytical<>::r_best, rbv()))
   ;

   class_<aniso_u_scaler<> >("aniso_u_scaler")
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<cctbx::miller::index<> > const& >(
             (arg("f_model_abs"),
              arg("f_obs"),
              arg("miller_indices"))))
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<cctbx::miller::index<> > const&,
           af::const_ref<double, af::mat_grid> const& >(
             (arg("f_model_abs"),
              arg("f_obs"),
              arg("miller_indices"),
              arg("adp_constraint_matrix"))))
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<cctbx::miller::index<> > const&,
           cctbx::uctbx::unit_cell const& >(
             (arg("f_model_abs"),
              arg("f_obs"),
              arg("miller_indices"),
              arg("unit_cell"))))
      .add_property("u_star_independent",
        make_getter(&aniso_u_scaler<>::u_star_independent, rbv()))
      .add_property("a", make_getter(&aniso_u_scaler<>::a, rbv()))
      .add_property("u_star", make_getter(&aniso_u_scaler<>::u_star, rbv()))
   ;

    def("k_mask_and_k_overall_grid_search",
      (af::tiny<double, 2>(*)
        (af::const_ref<double>                const& f_obs,
         af::const_ref<std::complex<double> > const& f_calc,
         af::const_ref<std::complex<double> > const& f_mask,
         af::const_ref<double>                const& k_mask_range,
         af::const_ref<bool>                  const& selection
         )) k_mask_and_k_overall_grid_search);
   ;

   def("fit_k_exp_b_to_k_total",
      (af::tiny<double, 3>(*)
        (af::const_ref<double> const& data,
         af::const_ref<double> const& ss,
         double                       k_start,
         double                       b_start
         )) fit_k_exp_b_to_k_total);
   ;

   def("ksol_bsol_grid_search",
      (af::shared<double>(*)
        (af::const_ref<double>   const&,
         af::const_ref<std::complex<double> > const&,
         af::const_ref<std::complex<double> > const&,
         af::const_ref<double>   const&,
         af::const_ref<double>   const&,
         af::const_ref<double>   const&,
         double                  const&,
         af::const_ref<double>   const&,
         af::const_ref<double>   const&,
         double                  const&)) ksol_bsol_grid_search);
   ;

   def("complex_f_minus_f_kb_scaled",
      (af::shared<std::complex<double> >(*)
        (af::const_ref<std::complex<double> > const&,
         af::const_ref<std::complex<double> > const&,
         af::const_ref<double>   const&,
         af::const_ref<double>   const&)) complex_f_minus_f_kb_scaled);
   ;

   def("set_k_mask_to_cubic_polynom",
      (af::shared<double>(*)
        (af::const_ref<double> const& ss,
         double                const& ss_cutoff,
         af::tiny<double, 4>   const& coeffs)) set_k_mask_to_cubic_polynom);
   ;

   def("set_to_linear_interpolated",
      (af::shared<double>(*)
        (af::const_ref<double> const& ss,
         double                const& k,
         double                const& b,
         af::const_ref<bool>   const& selection,
         af::shared<double>           data)) set_to_linear_interpolated);
   ;

    def("r_factor",
      (double(*)
        (af::const_ref<double> const&,
         af::const_ref< std::complex<double> > const&)) r_factor);
   ;
   def("r_factor",
      (double(*)
        (af::const_ref<double> const&,
         af::const_ref< std::complex<double> > const&,
         af::const_ref<bool> const&)) r_factor);
   ;
   def("r_factor",
      (double(*)
        (af::const_ref<double> const&,
         af::const_ref< std::complex<double> > const&,
         af::const_ref< std::complex<double> > const&,
         double const&)) r_factor);
   ;
   def("r_factor",
      (double(*)
        (af::const_ref<double> const&,
         af::const_ref< std::complex<double> > const&,
         double const&)) r_factor);
   ;
   def("r_factor",
      (double(*)
        (af::const_ref<double> const&,
         af::const_ref< std::complex<double> > const&,
         af::const_ref<bool> const&,
         double const&)) r_factor);
   ;
   def("r_factor",
      (double(*)
        (af::const_ref<double> const&,
         af::const_ref<double> const&,
         double const&)) r_factor);
   ;
   def("r_factor",
      (double(*)
        (af::const_ref<double> const&,
         af::const_ref<double> const&)) r_factor);
   ;
   def("r_factor",
      (double(*)
        (af::const_ref<double> const&,
         af::const_ref< std::complex<double> > const&,
         af::const_ref< std::complex<double> > const&,
         double const&,
         double const&)) r_factor);
   ;
   def("scale",
      (double(*)
        (af::const_ref<double> const&,
         af::const_ref< std::complex<double> > const&)) scale);
   ;
   def("scale",
      (double(*)
        (af::const_ref<double> const&,
         af::const_ref< std::complex<double> > const&,
         af::const_ref<bool> const&)) scale);
   ;
   def("scale",
      (double(*)
        (af::const_ref<double> const&,
         af::const_ref< std::complex<double> > const&,
         af::const_ref< std::complex<double> > const&,
         double const&)) scale);
   ;
    def("fb_cart",fb_cart)
   ;
   def("symmetrize_mask",
      (void(*)
        (af::ref<int, af::c_grid<3> > const&,
         af::const_ref<long, af::c_grid<3> > const&)) symmetrize_mask);
  }

} // namespace <anonymous>
}} // namespace mmtbx::bulk_solvent

BOOST_PYTHON_MODULE(mmtbx_bulk_solvent_ext)
{
  mmtbx::bulk_solvent::init_module();
}


//
// Created by pengpeng on 11/2/20.
//

//
// Created by pengpeng on 10/29/20.
//

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/OpticalFlowRad.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <trajectory_msgs/JointTrajectoryPoint.h>

#include <iostream>
#include <string>
#include <fstream>
#include <cmath>
#include <Eigen/Eigen>
#include <algorithm>
#include <chrono>
#include <cstdint>

#define PI 3.1415926
#define number 10
using namespace Eigen;

mavros_msgs::State current_state;

Matrix<double, number, 3> planned_p;

Matrix<double, number, 3> planned_v;

Vector3d planned_a(0,0,0);
Matrix<double,number,1> planned_yaw;
ros::Publisher zuan_quan_point_pub;

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void set_zuan_quan_point(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw=0.0)
{

    trajectory_msgs::JointTrajectoryPoint zuan_quan_setpoint;

    zuan_quan_setpoint.positions.push_back(p(0)); //x
    zuan_quan_setpoint.positions.push_back(p(1)); //y
    zuan_quan_setpoint.positions.push_back(p(2)); //z
    zuan_quan_setpoint.positions.push_back(yaw);
    //ROS_INFO("P(0)  %f P(1) %f P(2) %f",p(0),p(1),p(2));

    zuan_quan_setpoint.velocities.push_back(v(0));
    zuan_quan_setpoint.velocities.push_back(v(1));
    zuan_quan_setpoint.velocities.push_back(v(2));

    zuan_quan_setpoint.accelerations.push_back(a(0));
    zuan_quan_setpoint.accelerations.push_back(a(1));
    zuan_quan_setpoint.accelerations.push_back(a(2));

    zuan_quan_point_pub.publish(zuan_quan_setpoint);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pub_zuan_quan");
    ros::NodeHandle nh;

    //ROS_INFO("QQQQ");

    planned_p << -2.0,1.0,1.6,
            -3.0,1.0,1.6,
            -4.7,2.5,1.6,
            -5.7,2.5,1.6,
            -6.7,1.5,1.6,
            -7.7,1.5,1.6,
            -9.0,-2.5,1.6,
            -10.0,-2.5,1.6,
            -13.1,-2.25,1.6,
            -13.9,-1.75,1.6;

    //ROS_INFO("planned_p(0):%f",planned_p(0));
    planned_v<<-1.0,0,0,
            -1.0,0.2,0,
            -1.0,0,0,
            -1.0,-0.3,0,
            -1.0,0,0,
            -0.3,-0.3,0,
            -1.0,0,0,
            -0,0,0,
            -0,0,0,
            0,0,0;

    planned_yaw<<180.0/180.0*PI,
                180.0/180*PI,
                180.0/180*PI,
                180.0/180*PI,
                180.0/180*PI,
                180.0/180*PI,
                180.0/180*PI,
                180.0/180*PI,
                150.0/180*PI,
                150.0/180*PI;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 1, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 1);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

    zuan_quan_point_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/zuan_quan_setpoint", 1);

    ros::Rate rate(40.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }

    //send a few setpoints before starting
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z =0.5;
    double theta=180.0/180.0*PI;
    pose.pose.orientation.w=cos(theta/2);
    pose.pose.orientation.x=0;
    pose.pose.orientation.y=0;
    pose.pose.orientation.z=sin(theta/2);

    int quan_num=0;
    nh.param<int>("int_param",quan_num, 0);
    nh.setParam("int_param", 0);

    for(int i = 100; ros::ok() && i > 0; --i)
    {
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    ros::Time last_request = ros::Time::now();

    while(ros::ok())
    {
        if (current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        }
        else
        {
            if (!current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
            {
                if (arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

       // static unsigned int count=0;

//        count++;
//        if(count>1000)
//        {
//            break;
//        }
        //local_pos_pub.publish(pose);

        nh.getParam("int_param", quan_num);
       // ROS_INFO("quan_num:%d",quan_num);

        set_zuan_quan_point(planned_p.row(quan_num),planned_v.row(quan_num),planned_a,planned_yaw(quan_num));

        ros::spinOnce();
        rate.sleep();

        //local_pos_pub.publish(pose);
    }

}


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

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

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

#include <boost/simd/config.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG(ext, divfix_, boost::dispatch::elementwise_<divfix_>);
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, divfix_);
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::divfix_,divfix);

} }

#endif


// ----------------------------------------------------------------------------
//
//
// SubWorld -- SubMarine Game
//
// Copyright (c) 2020, F.Lainard
// Original author: F.Lainard
//
// 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.
//
//
// ----------------------------------------------------------------------------

#define NOMINMAX
#include "PathFinder.h"

#include "GameLevel.h"
#include "GameWorld.h"
#include "GameLevel.h"
#include "GameNode.h"
#include <UnigineGame.h>
#include <UnigineWorld.h>
#include <UnigineVisualizer.h>
#include <sstream>
#include <iomanip>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/planners/rrt/LazyRRT.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/objectives/StateCostIntegralObjective.h>
#include <ompl/base/objectives/MaximizeMinClearanceObjective.h>
#include <ompl/util/PPM.h>
#include <string>
#include <ompl/config.h>
#include <boost/filesystem.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/math/interpolators/catmull_rom.hpp>
#include <iostream>
#include <functional>
#include <algorithm>
#include <array>
#include "Converter.h"

// ----------------------------------------------------------------------------

using namespace SubWorld;
using namespace Unigine;
using namespace boost;
namespace ob = ompl::base;
namespace og = ompl::geometric;

// ----------------------------------------------------------------------------

PathFinder::PathFinder(GameLevel* level)
	: _level(level),_debug(false)
{
}

// ----------------------------------------------------------------------------

PathFinder::~PathFinder()
{
}

// ----------------------------------------------------------------------------

void PathFinder::init(const char * path, int terrainSize,bool debug)
{
	_debug = debug;
	_mapImage = Image::create();
	bool loaded = _mapImage->load(path);
	if (!loaded)
	{
		Log::message("\nPathFinder : Map does not exists: %s ", path);
		return;
	}
	_heightmap_path = path;
	_terrainSize = terrainSize;

	_maxWidth = _mapImage->getWidth()-1;
	_maxHeight = _mapImage->getHeight()-1;
	_scale = (float)_maxWidth / (float)_terrainSize;
	buildDebugImage();

	auto space(std::make_shared<ob::RealVectorStateSpace>());
	space->addDimension(0.0, _maxWidth);
	space->addDimension(0.0, _maxHeight);
//	space->addDimension(0.0, _maxAltitude);

	ss_ = std::make_shared<og::SimpleSetup>(space);
	// set state validity checking for this space
	ss_->setStateValidityChecker([this](const ob::State *state) { return isStateValid(state); });
	space->setup();

	//og::RRTConnect* planner = new og::RRTConnect(ss_->getSpaceInformation());

	//og::RRTstar* planner = new og::RRTstar(ss_->getSpaceInformation());
	og::LazyRRT* planner = new og::LazyRRT(ss_->getSpaceInformation());

	ss_->setPlanner(ob::PlannerPtr(planner));

	if (_debug && !_pathImage)
	{
		OMPL_INFORM("_pathImage is null !!!!!!!");
	}

}

// ----------------------------------------------------------------------------

OpenSteer::PolylinePathway* PathFinder::circular_plan(float radius, int nodex, int nodey, int nodez, int centerx, int centery, int centerz, int circle_radius,int circle_z)
{
	std::vector< OpenSteer::PolylinePathway*> segments;
	bool error = false;
	int nbItems = 0;

	int old_goal_x = (float)( centerx + Unigine::Math::cos((float)0) * circle_radius);
	int old_goal_y = (float)(centery+ Unigine::Math::sin((float)0) * circle_radius);

	// move to the border
	OpenSteer::PolylinePathway* p = plan(radius, nodex, nodey, nodez, old_goal_x, old_goal_y, circle_z);
	if (!p) return nullptr;
	segments.push_back(p);
	nbItems += p->pointCount;

	// travel along the border
	 for (float i = 0; i < 6.28318530718; i += 0.2)
	{
		int  goal_x = centerx + Unigine::Math::cos(i) * circle_radius;
		int  goal_y = centery + Unigine::Math::sin(i) * circle_radius;
		//printf("\n plan: %d %d -> %d %d\n", old_goal_x, old_goal_y, goal_x, goal_y);

	 	p = plan(radius, old_goal_x, old_goal_y, centerz, goal_x, goal_y, circle_z);
		if (p)
		{
			segments.push_back(p);
			nbItems += p->pointCount;
		}
		else
		{
			error = true;
			break;
		}
		 old_goal_x = goal_x;
		 old_goal_y = goal_y;

	}

	OpenSteer::Vec3* arr = new OpenSteer::Vec3[nbItems];
	int ip = 0;
	for (OpenSteer::PolylinePathway* ps : segments)
	{
		memcpy(&arr[ip], ps->points, ps->pointCount*sizeof(OpenSteer::Vec3));
		ip += ps->pointCount;
		delete ps;
	}

	OpenSteer::PolylinePathway* path = new OpenSteer::PolylinePathway((const int)nbItems, arr, _pathRadius, false);
	return path;

}

// ----------------------------------------------------------------------------

OpenSteer::PolylinePathway* PathFinder::plan(float radius, int x, int y, int z, int goal_x, int goal_y, int goal_z)
{
	if (!ss_)
	{
		OMPL_INFORM("PathFinder bad config");
		return nullptr;
	}

	_sampling_radius = radius * _scale;
	_goal_z = goal_z;
	ob::ScopedState<> start(ss_->getStateSpace());
	WorldToMap(x, y);
	start[0] = x;
	start[1] = y;
	//start[2] = z;
	ob::ScopedState<> goal(ss_->getStateSpace());
	WorldToMap(goal_x, goal_y);
	goal[0] = goal_x;
	goal[1] = goal_y;
	//goal[2] = goal_z;
	ss_->setStartAndGoalStates(start, goal);
	if (ss_->getPlanner())
			ss_->getPlanner()->clear();

	if (_debug && !_pathImage)
	{
		OMPL_INFORM("_pathImage is null !!!!!!!");
	}

	ss_->solve();
	std::size_t ns = ss_->getProblemDefinition()->getSolutionCount();
	if (_debug)
	{
		OMPL_INFORM("Found %d solutions", (int)ns);
	}
	if (ss_->haveSolutionPath())
	{
		return recordSolution();
	}
	saveDebugImage();
	return nullptr;
}

// ----------------------------------------------------------------------------

void setPixel(Image::Pixel& p, int r, int g, int b)
{
	p.i.r = r; p.i.g = g;  p.i.b = b;

}

// ----------------------------------------------------------------------------

bool PathFinder::isStateValid(const ob::State *state) const
{
	int w = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[0], _maxWidth);
	int h = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[1], _maxHeight);

	if (w < 0) return false;
	if (h < 0) return false;
	Image::Pixel pixel = _mapImage->get2D(w, h);

	bool valid = pixel.i.r <= 0;

	// the location is valid, but with want to test the zone around
	if (valid)
	{
		const float  PI_F = 3.14159265358979f;

		float pconv = PI_F / 180.0f;
		for (float i = 0.0; i < 360.0; i += 15.0)
		{
			float angle = i * pconv;
			int x = std::min((int)(w + _sampling_radius * std::cos(angle)), _maxWidth);
			int y = std::min((int)(h + _sampling_radius * std::sin(angle)), _maxHeight);
			Image::Pixel pixelExtern = _mapImage->get2D(x, y);
			valid = pixelExtern.i.r <= 0;
			if (!valid) break;
		}

	}

	if (_debug)
	{
		if (_debug && !_pathImage)
		{
			OMPL_INFORM("_pathImage is null !!!!!!!");
		}
		if (!valid)
		{
			//	printf("\npt invalid %d %d = %d",h,w, pixel.i.r);
			setPixel(pixel, 255, 80, 10);
			_pathImage->set2D(w, h, pixel);
		}
		else
		{
			setPixel(pixel, 255, 0, 100);
			_pathImage->set2D(w, h, pixel);
		}
	}

	return valid;
}

// ----------------------------------------------------------------------------

OpenSteer::PolylinePathway*  PathFinder::recordSolution()
{
	if (!ss_ || !ss_->haveSolutionPath())
		return nullptr;

	ss_->simplifySolution();
	og::PathGeometric &p = ss_->getSolutionPath();
	ss_->getPathSimplifier()->simplifyMax(p);
	ss_->getPathSimplifier()->smoothBSpline(p);
	p.interpolate();
	if (_debug)
	{
		OMPL_INFORM("Path %d points", (int)p.getStateCount());
	}
	std::vector< OpenSteer::Vec3> pointList;
	for (std::size_t i = 0; i < p.getStateCount(); ++i)
	{
		int w = std::min(_maxWidth, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[0]);
		int h = std::min(_maxHeight, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[1]);
		//int z = std::min(_maxHeight, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[2]);
		int x = w;
		int y = h;
		MapToWorld(x, y);
		pointList.push_back(OpenSteer::Vec3((float)x, (float)y, (float)_goal_z));
		// origin of the map is left bottom
		h -= _maxHeight;

		if (_debug)
		{
			//	OMPL_INFORM("Path %d %d ", (int)w, h);

			Image::Pixel pixel;
			setPixel(pixel, 0, 254, 0);
			_pathImage->set2D(w, h, pixel);
		}
	}

	// save debug image
	saveDebugImage();

	// catmull rom smoother
	//Smooth(pointList);
	// build 	PolylinePathway
	OpenSteer::Vec3* arr = new OpenSteer::Vec3[pointList.size()];
	std::copy(pointList.begin(), pointList.end(), arr);
	OpenSteer::PolylinePathway* path = new OpenSteer::PolylinePathway((const int)pointList.size(), arr, _pathRadius, false);
	// convert to openSteer coordinate reference
	for (int i = 0; i < path->pointCount; i++)
	{
		std::swap(path->points[i].y, path->points[i].z);
	}

	return path;
}

// ----------------------------------------------------------------------------

void PathFinder::Smooth(std::vector< OpenSteer::Vec3>& pointList)
{
	std::vector< OpenSteer::Vec3> smoothedPoints;
	if (pointList.size() < 4) return;
	std::vector<std::array<float, 3>> points;
	// remove points wich are to close
	for (int i = 0; i < pointList.size(); ++i)
	{
		OpenSteer::Vec3 n1 = pointList.at(i);
		std::array<float, 3> c = { n1.x, n1.y, n1.z };

		if (i > 0 && i < pointList.size() - 1)
		{
			std::array<float, 3> back = points.back();
			OpenSteer::Vec3  fd = pointList.at(i);
			OpenSteer::Vec3 vn1(fd.x, fd.y, fd.z);
			OpenSteer::Vec3 vn2(back[0], back[1], back[2]);
			float dd = (vn2 - vn1).length();
			if (dd < 2)
				continue;

		}
		points.push_back(c);
	}
	if (points.size() < 4) return;
	try
	{

		boost::math::catmull_rom<std::array<float, 3>> cr(std::move(points), false, 1);
		OpenSteer::Vec3 firstPt = pointList.front();
		OpenSteer::Vec3 lastPt = pointList.back();
		pointList.clear();
		//pointList.push_back(firstPt);
		for (float p = 0; p < cr.max_parameter(); p += 1)
		{
			std::array<float, 3> pt = cr(p);
			pointList.push_back(OpenSteer::Vec3(pt[0], pt[1], pt[2]));
		}
		pointList.push_back(lastPt);

	}
	catch (std::domain_error& e)
	{
		printf("PathFinder::Smooth domain_error! %s", e.what());
	}

}

// ----------------------------------------------------------------------------

void PathFinder::WorldToMap(int& x, int& y)
{
	x = (int)(x * _scale);
	y = (int)(y * _scale);
	y = _maxHeight - y;
}

// ----------------------------------------------------------------------------

void PathFinder::MapToWorld(int& x, int& y)
{
	x = (int)(x / _scale);
	y = (int)+(_maxHeight - y);
	y = (int)(y / _scale);
}

// ----------------------------------------------------------------------------

void PathFinder::saveDebugImage()
{
	if (_debug)
	{
		std::string path = _heightmap_path + "debug.png";
		_pathImage->save(path.c_str());
	}
}

void PathFinder::buildDebugImage()
{
	if (_debug && !_pathImage)
	{
		_pathImage = Image::create();
		_pathImage->create2D(_maxWidth + 1, _maxHeight + 1, Image::FORMAT_RGBA8);

		if (_debug && !_pathImage)
		{
			OMPL_INFORM("_pathImage is null !!!!!!!");
		}

		for (int x = 0; x < _maxWidth; x++)
			for (int y = 0; y < _maxHeight; y++)
			{
				Image::Pixel pixel;
				pixel = _mapImage->get2D(x, y);
				int height = pixel.i.r;

				if (height > 10)
				{
					setPixel(pixel, 255, 255, 255);
				}
				else
				{
					setPixel(pixel, 0, 0, 100);
				}
				_pathImage->set2D(x, y, pixel);
			}
	}

}


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

//Distributed under the Boost Software License, Version 1.0. (See accompanying
//file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/qvm/mat_operations.hpp>
#include "test_qvm_matrix.hpp"
#include "gold.hpp"

namespace
    {
    template <class T>
    void
    test_perspective_lh( T fov_y, T aspect_ratio, T z_near, T z_far )
        {
        using namespace boost::qvm;
        test_qvm::matrix<M1,4,4> const m=perspective_lh(fov_y,aspect_ratio,z_near,z_far);
        test_qvm::matrix_perspective_lh(m.b,fov_y,aspect_ratio,z_near,z_far);
        BOOST_QVM_TEST_CLOSE(m.a,m.b,0.000001f);
        }

    template <class T>
    void
    test_perspective_rh( T fov_y, T aspect_ratio, T z_near, T z_far )
        {
        using namespace boost::qvm;
        test_qvm::matrix<M1,4,4> const m=perspective_rh(fov_y,aspect_ratio,z_near,z_far);
        test_qvm::matrix_perspective_rh(m.b,fov_y,aspect_ratio,z_near,z_far);
        BOOST_QVM_TEST_CLOSE(m.a,m.b,0.000001f);
        }
    }

int
main()
    {
    test_perspective_lh(0.5f,1.3f,0.1f,2000.0f);
    test_perspective_rh(0.5f,1.3f,0.1f,2000.0f);
    return boost::report_errors();
    }


// copyright (c) 2015 the moorecoin core developers
// distributed under the mit/x11 software license, see the accompanying
// file copying or http://www.opensource.org/licenses/mit-license.php.

#include "main.h"
#include "pow.h"
#include "util.h"
#include "test/test_moorecoin.h"

#include <boost/test/unit_test.hpp>

using namespace std;

boost_fixture_test_suite(pow_tests, basictestingsetup)

/* test calculation of next difficulty target with no constraints applying */
boost_auto_test_case(get_next_work)
{
    selectparams(cbasechainparams::main);
    const consensus::params& params = params().getconsensus();

    int64_t nlastretargettime = 1261130161; // block #30240
    cblockindex pindexlast;
    pindexlast.nheight = 32255;
    pindexlast.ntime = 1262152739;  // block #32255
    pindexlast.nbits = 0x1d00ffff;
    boost_check_equal(calculatenextworkrequired(&pindexlast, nlastretargettime, params), 0x1d00d86a);
}

/* test the constraint on the upper bound for next work */
boost_auto_test_case(get_next_work_pow_limit)
{
    selectparams(cbasechainparams::main);
    const consensus::params& params = params().getconsensus();

    int64_t nlastretargettime = 1231006505; // block #0
    cblockindex pindexlast;
    pindexlast.nheight = 2015;
    pindexlast.ntime = 1233061996;  // block #2015
    pindexlast.nbits = 0x1d00ffff;
    boost_check_equal(calculatenextworkrequired(&pindexlast, nlastretargettime, params), 0x1d00ffff);
}

/* test the constraint on the lower bound for actual time taken */
boost_auto_test_case(get_next_work_lower_limit_actual)
{
    selectparams(cbasechainparams::main);
    const consensus::params& params = params().getconsensus();

    int64_t nlastretargettime = 1279008237; // block #66528
    cblockindex pindexlast;
    pindexlast.nheight = 68543;
    pindexlast.ntime = 1279297671;  // block #68543
    pindexlast.nbits = 0x1c05a3f4;
    boost_check_equal(calculatenextworkrequired(&pindexlast, nlastretargettime, params), 0x1c0168fd);
}

/* test the constraint on the upper bound for actual time taken */
boost_auto_test_case(get_next_work_upper_limit_actual)
{
    selectparams(cbasechainparams::main);
    const consensus::params& params = params().getconsensus();

    int64_t nlastretargettime = 1263163443; // note: not an actual block time
    cblockindex pindexlast;
    pindexlast.nheight = 46367;
    pindexlast.ntime = 1269211443;  // block #46367
    pindexlast.nbits = 0x1c387f6f;
    boost_check_equal(calculatenextworkrequired(&pindexlast, nlastretargettime, params), 0x1d00e1fd);
}

boost_auto_test_case(getblockproofequivalenttime_test)
{
    selectparams(cbasechainparams::main);
    const consensus::params& params = params().getconsensus();

    std::vector<cblockindex> blocks(10000);
    for (int i = 0; i < 10000; i++) {
        blocks[i].pprev = i ? &blocks[i - 1] : null;
        blocks[i].nheight = i;
        blocks[i].ntime = 1269211443 + i * params.npowtargetspacing;
        blocks[i].nbits = 0x207fffff; /* target 0x7fffff000... */
        blocks[i].nchainwork = i ? blocks[i - 1].nchainwork + getblockproof(blocks[i - 1]) : arith_uint256(0);
    }

    for (int j = 0; j < 1000; j++) {
        cblockindex *p1 = &blocks[getrand(10000)];
        cblockindex *p2 = &blocks[getrand(10000)];
        cblockindex *p3 = &blocks[getrand(10000)];

        int64_t tdiff = getblockproofequivalenttime(*p1, *p2, *p3, params);
        boost_check_equal(tdiff, p1->getblocktime() - p2->getblocktime());
    }
}

boost_auto_test_suite_end()


#include "teleop_tracking/mesh.h"

#include <ros/ros.h>

#include <geometry_msgs/Point.h>
#include <boost/ref.hpp>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/PoseStamped.h>
#include <eigen_conversions/eigen_msg.h>

const std::string marker_ns = "ns";
const int source_id = 0;
const int latch_id = 1;

double scale = 0.1;

visualization_msgs::Marker makeMarker(const Eigen::Vector3d& pt, int marker_id)
{
  visualization_msgs::Marker marker;
  marker.action = visualization_msgs::Marker::ADD;
  marker.header.frame_id = "world";
  marker.header.stamp = ros::Time::now();
  marker.ns = marker_ns;
  marker.id = marker_id;
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.pose.position.x = pt(0);
  marker.pose.position.y = pt(1);
  marker.pose.position.z = pt(2);
  marker.pose.orientation.x = 0.0;
  marker.pose.orientation.y = 0.0;
  marker.pose.orientation.z = 0.0;
  marker.pose.orientation.w = 1.0;
  marker.scale.x = scale;
  marker.scale.y = scale;
  marker.scale.z = scale;
  marker.color.r = (marker_id == source_id ? 0.0f : 1.0f);
  marker.color.g = (marker_id == source_id ? 1.0f : 0.0f);
  marker.color.b = 0.0f;
  marker.color.a = 1.0;
  marker.lifetime = ros::Duration();
  return marker;
}

geometry_msgs::PoseStamped makeStampedPose(const Eigen::Affine3d& pose)
{
  geometry_msgs::PoseStamped stamped;
  stamped.header.frame_id = "world";
  stamped.header.stamp = ros::Time::now();
  tf::poseEigenToMsg(pose, stamped.pose);
  return stamped;
}

void pointCallback(const geometry_msgs::Point::ConstPtr& pt,
                   const teleop_tracking::Mesh& mesh,
                   ros::Publisher& pub,
                   ros::Publisher& pose_pub)
{
  using teleop_tracking::TrianglePose;
  using teleop_tracking::TrianglePosition;

  static TrianglePose current_tpose = mesh.closestPose(Eigen::Vector3d(0.05,0.1,-0.5));

  ROS_WARN("Moving %f %f %f", pt->x, pt->y, pt->z);

  if (pt->z > 0.0)
  {
    current_tpose = mesh.closestPose(current_tpose.pose.translation());
    geometry_msgs::PoseStamped gpose = makeStampedPose(current_tpose.pose);
    std::cout << current_tpose.pose.matrix() << '\n';
    if (current_tpose.pose.matrix()(0,0) != current_tpose.pose.matrix()(0,0)) exit(1);
    pose_pub.publish(gpose);
    return;
  }

  if (pt->z < 0)
  {
    Eigen::AngleAxisd twist (0.1, Eigen::Vector3d::UnitZ());
    current_tpose.pose *= twist;
  }

  Eigen::Vector3d v (pt->x, pt->y, pt->z);

  Eigen::Affine3d new_pose = mesh.walkTriangle2(current_tpose.pose,
                                                current_tpose.index,
                                                current_tpose.index,
                                                Eigen::Vector2d(v(0), v(1)));

  std::cout << "Current Indx: " << current_tpose.index << "\n";

  current_tpose.pose = new_pose;
  geometry_msgs::PoseStamped gpose1 = makeStampedPose(current_tpose.pose);

  Eigen::Vector3d int_point = new_pose.translation();
//  visualization_msgs::Marker m_pt = makeMarker(int_point, latch_id);

//  pub.publish(m_pt);
  pose_pub.publish(gpose1);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "mesh_tracking_test");
  ros::NodeHandle pnh ("~");

  std::string mesh_file;
  pnh.param<std::string>("mesh_file", mesh_file, "");

  pnh.param<double>("scale", scale, 0.1);

  if (mesh_file.empty())
  {
    ROS_FATAL("Mesh Tracking Test requires the 'mesh_file' param be set.");
    return -1;
  }

  std::cout << "Loading mesh...\n";

  teleop_tracking::Mesh mesh(mesh_file);

  std::cout << "Done loading mesh\n";

  mesh.debugInfo();

  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<visualization_msgs::Marker>("markers", 1000);
  ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("poses", 1000);
  ros::Subscriber sub = nh.subscribe<geometry_msgs::Point>("points", 1, boost::bind(pointCallback, _1, boost::cref(mesh), boost::ref(pub), boost::ref(pose_pub)));

  ros::spin();

  return 0;
}


//  Created by jimmy on 2016-08-28.
//  Copyright (c) 2016 Nowhere Planet. All rights reserved.
//

#ifndef __cvx_wht__
#define __cvx_wht__

// Walsh Hadamard transform (WHT) feature

#include <stdio.h>
#include <opencv2/core/core.hpp>
#include <opencv2/core/core_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/highgui/highgui_c.h>
#include <vector>
#include <Eigen/Dense>

using std::vector;
using cv::Mat;

class CvxWalshHadamardTransform
{
public:

    // WHT feature without the first pattern which has all positive component
    // points:    locations in image
    // patch_size: unit pixel, 2^{2,3,4,5}
    // kernel_num: feature dimension in single channel
    // features: output, feature dimension == (kernel_num - 1) * 3
    static
    bool generateWHFeatureWithoutFirstPattern(const cv::Mat & rgb_image,
                                              const vector<cv::Point2d> & points,
                                              const int patch_size,
                                              const int kernel_num,
                                              vector<Eigen::VectorXf> & features);
private:
    // rgb_image: input image
    // points:    locations in image
    // patch_size: unit pixel, 2^{2,3,4,5}
    // kernel_num: feature dimension in single channel
    // features: output, feature dimension == kernel_num * 3
    static
    bool generateWHFeature(const cv::Mat & rgb_image,
                           const vector<cv::Point2d> & points,
                           const int patch_size,
                           const int kernel_num,
                           vector<Eigen::VectorXf> & features);

};

#endif /* defined(__cvx_wht__) */


#include <boost/range/counting_range.hpp>
#include <boost/range/algorithm_ext/push_back.hpp>
#include <boost/range.hpp>

#include <queue>
#include <stack>

#include "../parallel/tbb.h"
#include "../../def_debug_ws.h"

template<class T>
hera::ws::dnn::KDTree<T>::
KDTree(const Traits& traits, HandleContainer&& handles, double _wassersteinPower):
    traits_(traits), tree_(std::move(handles)), wassersteinPower(_wassersteinPower)
{ assert(wassersteinPower >= 1.0); init(); }

template<class T>
template<class Range>
hera::ws::dnn::KDTree<T>::
KDTree(const Traits& traits, const Range& range, double _wassersteinPower):
    traits_(traits), wassersteinPower(_wassersteinPower)
{
    assert( wassersteinPower >= 1.0);
    init(range);
}

template<class T>
template<class Range>
void
hera::ws::dnn::KDTree<T>::
init(const Range& range)
{
    size_t sz = std::distance(std::begin(range), std::end(range));
    tree_.reserve(sz);
    weights_.resize(sz, 0);
    subtree_weights_.resize(sz, 0);
    for (PointHandle h : range)
        tree_.push_back(h);
    init();
}

template<class T>
void
hera::ws::dnn::KDTree<T>::
init()
{
    if (tree_.empty())
        return;

#if defined(TBB)
    task_group g;
    g.run(OrderTree(tree_.begin(), tree_.end(), 0, traits()));
    g.wait();
#else
    OrderTree(tree_.begin(), tree_.end(), 0, traits()).serial();
#endif

    for (size_t i = 0; i < tree_.size(); ++i)
        indices_[tree_[i]] = i;
}

template<class T>
struct
hera::ws::dnn::KDTree<T>::OrderTree
{
                OrderTree(HCIterator b_, HCIterator e_, size_t i_, const Traits& traits_):
                    b(b_), e(e_), i(i_), traits(traits_)            {}

    void        operator()() const
    {
        if (e - b < 1000)
        {
            serial();
            return;
        }

        HCIterator m = b + (e - b)/2;
        CoordinateComparison cmp(i, traits);
        std::nth_element(b,m,e, cmp);
        size_t next_i = (i + 1) % traits.dimension();

        task_group g;
        if (b < m - 1)  g.run(OrderTree(b,   m, next_i, traits));
        if (e > m + 2)  g.run(OrderTree(m+1, e, next_i, traits));
        g.wait();
    }

    void        serial() const
    {
        std::queue<KDTreeNode> q;
        q.push(KDTreeNode(b,e,i));
        while (!q.empty())
        {
            HCIterator b, e; size_t i;
            std::tie(b,e,i) = q.front();
            q.pop();
            HCIterator m = b + (e - b)/2;

            CoordinateComparison cmp(i, traits);
            std::nth_element(b,m,e, cmp);
            size_t next_i = (i + 1) % traits.dimension();

            // Replace with a size condition instead?
            if (b < m - 1)  q.push(KDTreeNode(b,   m, next_i));
            if (e - m > 2)  q.push(KDTreeNode(m+1, e, next_i));
        }
    }

    HCIterator      b, e;
    size_t          i;
    const Traits&   traits;
};

template<class T>
template<class ResultsFunctor>
void
hera::ws::dnn::KDTree<T>::
search(PointHandle q, ResultsFunctor& rf) const
{
    typedef         typename HandleContainer::const_iterator        HCIterator;
    typedef         std::tuple<HCIterator, HCIterator, size_t>      KDTreeNode;

    if (tree_.empty())
        return;

    DistanceType    D  = std::numeric_limits<DistanceType>::max();

    // TODO: use tbb::scalable_allocator for the queue
    std::queue<KDTreeNode>  nodes;

    nodes.push(KDTreeNode(tree_.begin(), tree_.end(), 0));

    while (!nodes.empty())
    {
        HCIterator b, e; size_t i;
        std::tie(b,e,i) = nodes.front();
        nodes.pop();

        CoordinateComparison cmp(i, traits());
        i = (i + 1) % traits().dimension();

        HCIterator   m    = b + (e - b)/2;

        DistanceType dist = (wassersteinPower == 1.0) ?  traits().distance(q, *m) + weights_[m - tree_.begin()] : std::pow(traits().distance(q, *m), wassersteinPower) + weights_[m - tree_.begin()];

        D = rf(*m, dist);

        // we are really searching w.r.t L_\infty ball; could prune better with an L_2 ball
        Coordinate diff = cmp.diff(q, *m);     // diff returns signed distance

        DistanceType diffToWasserPower = (wassersteinPower == 1.0) ? diff : ((diff > 0 ? 1.0 : -1.0) * std::pow(fabs(diff), wassersteinPower));

        size_t       lm   = m + 1 + (e - (m+1))/2 - tree_.begin();
        if (e > m + 1 && diffToWasserPower - subtree_weights_[lm] >= -D) {
            nodes.push(KDTreeNode(m+1, e, i));
        }

        size_t       rm   = b + (m - b) / 2 - tree_.begin();
        if (b < m && diffToWasserPower + subtree_weights_[rm] <= D) {
            nodes.push(KDTreeNode(b,   m, i));
        }
    }
}

template<class T>
void
hera::ws::dnn::KDTree<T>::
adjust_weights(DistanceType delta)
{
    for(auto& w : weights_)
        w -= delta;

    for(auto& sw : subtree_weights_)
        sw -= delta;
}

template<class T>
void
hera::ws::dnn::KDTree<T>::
change_weight(PointHandle p, DistanceType w)
{
    size_t idx = indices_[p];

    if ( weights_[idx] == w ) {
        return;
    }

    bool weight_increases = ( weights_[idx] < w );
    weights_[idx] = w;

    typedef     std::tuple<HCIterator, HCIterator>      KDTreeNode;

    // find the path down the tree to this node
    // not an ideal strategy, but // it's not clear how to move up from the node in general
    std::stack<KDTreeNode>  s;
    s.push(KDTreeNode(tree_.begin(),tree_.end()));

    do
    {
        HCIterator b,e;
        std::tie(b,e) = s.top();

        size_t im = b + (e - b)/2 - tree_.begin();

        if (idx == im)
            break;
        else if (idx < im)
            s.push(KDTreeNode(b, tree_.begin() + im));
        else    // idx > im
            s.push(KDTreeNode(tree_.begin() + im + 1, e));
    } while(1);

    // update subtree_weights_ on the path to the root
    DistanceType min_w = w;
    while (!s.empty())
    {
        HCIterator b,e;
        std::tie(b,e) = s.top();
        HCIterator m  = b + (e - b)/2;
        size_t     im = m - tree_.begin();
        s.pop();

        // left and right children
        if (b < m)
        {
            size_t lm = b + (m - b)/2 - tree_.begin();
            if (subtree_weights_[lm] < min_w)
                min_w = subtree_weights_[lm];
        }

        if (e > m + 1)
        {
            size_t rm = m + 1 + (e - (m+1))/2 - tree_.begin();
            if (subtree_weights_[rm] < min_w)
                min_w = subtree_weights_[rm];
        }

        if (weights_[im] < min_w) {
            min_w = weights_[im];
        }

        if (weight_increases) {

            if (subtree_weights_[im] < min_w )   // increase weight
                subtree_weights_[im] = min_w;
            else
                break;

        } else {

            if (subtree_weights_[im] > min_w )   // decrease weight
                subtree_weights_[im] = min_w;
            else
                break;

        }
    }
}

template<class T>
typename hera::ws::dnn::KDTree<T>::HandleDistance
hera::ws::dnn::KDTree<T>::
find(PointHandle q) const
{
    hera::ws::dnn::NNRecord<HandleDistance> nn;
    search(q, nn);
    return nn.result;
}

template<class T>
typename hera::ws::dnn::KDTree<T>::Result
hera::ws::dnn::KDTree<T>::
findR(PointHandle q, DistanceType r) const
{
    hera::ws::dnn::rNNRecord<HandleDistance> rnn(r);
    search(q, rnn);
    std::sort(rnn.result.begin(), rnn.result.end());
    return rnn.result;
}

template<class T>
typename hera::ws::dnn::KDTree<T>::Result
hera::ws::dnn::KDTree<T>::
findK(PointHandle q, size_t k) const
{
    hera::ws::dnn::kNNRecord<HandleDistance> knn(k);
    search(q, knn);
    std::sort(knn.result.begin(), knn.result.end());
    return knn.result;
}

template<class T>
struct hera::ws::dnn::KDTree<T>::CoordinateComparison
{
                CoordinateComparison(size_t i, const Traits& traits):
                    i_(i), traits_(traits)                              {}

    bool        operator()(PointHandle p1, PointHandle p2) const        { return coordinate(p1) < coordinate(p2); }
    Coordinate  diff(PointHandle p1, PointHandle p2) const              { return coordinate(p1) - coordinate(p2); }

    Coordinate  coordinate(PointHandle p) const                         { return traits_.coordinate(p, i_); }
    size_t      axis() const                                            { return i_; }

    private:
        size_t          i_;
        const Traits&   traits_;
};

template<class T>
void
hera::ws::dnn::KDTree<T>::
printWeights(void)
{
#ifndef FOR_R_TDA
    std::cout << "weights_:" << std::endl;
    for(const auto ph : indices_) {
        std::cout << "idx = " << ph.second << ": (" << (ph.first)->at(0) << ", " << (ph.first)->at(1) << ") weight = " << weights_[ph.second] << std::endl;
    }
    std::cout << "subtree_weights_:" << std::endl;
    for(size_t idx = 0; idx < subtree_weights_.size(); ++idx) {
        std::cout << idx << " : " << subtree_weights_[idx] << std::endl;
    }
#endif
}