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

#include <miopen/solver.hpp>

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

#include <boost/any.hpp>

#include <tuple>

MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_WINOGRAD_RXS_F2X3)
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_WINOGRAD_RXS_F2X3_PERF_VALS)
MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_AMD_WINOGRAD_RXS_F2X3_G1)

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

/// \todo The model is well-defined in for filters sized up to 5.
/// However, it seems producing valid results without this limitation,
/// when used against simple GEMM WTI model (to select the fastest solver).
/// This needs to be re-tested/re-considered when we have WTI
/// models for other solvers, OR when GEMM WTI model is improved.
/// --atamazov 2020-11-07.
#define WTI_MODEL_ALLOW_ANY_RS 1

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;
}

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>{}, &PerformanceConfigConvBinWinogradRxSf2x3::n_groups)
        );
    }
// clang-format on

/// \todo Consider re-using code from RxS.
inline bool IsShaderContraintsMet(const int R,
                                  const int S,
                                  const int,
                                  const int,
                                  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();
    if(!params.IsLayoutDefault())
    {
        return false;
    }

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

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

void PerformanceConfigConvBinWinogradRxSf2x3::EuristicInit(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 PerformanceConfigConvBinWinogradRxSf2x3::SetNextValue()
{
    return !PerfFieldRules().Next(*this);
}

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

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

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

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

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

PerformanceConfigConvBinWinogradRxSf2x3
ConvBinWinogradRxSf2x3::GetPerformanceConfig(const ConvolutionContext& params) const
{
    PerformanceConfigConvBinWinogradRxSf2x3 pp;
    pp.EuristicInit(params);
    MIOPEN_LOG_I(pp.ToString());
    return pp;
}

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

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

class ShaderModel : public UnifiedDescriptionConv2d
{
    const size_t DATATYPE_BITS;    // S
    const size_t n_groups;         // BQ ~compute units
    const bool out_of_model_scope; // Shader model produces unreliable results.

    public:
    ShaderModel(const ConvolutionContext& ctx)
        : UnifiedDescriptionConv2d(ctx),
          DATATYPE_BITS(ctx.IsFp16() ? 16 : 32),
          n_groups(ctx.GetStream()
                       .GetMaxHardwareComputeUnits()),   /// \todo Take n_groups from PerfConfig.
          out_of_model_scope(!(ctx.group_counts == 1) || //
                             !(U == 1) ||                //
                             !(V == 1) ||                //
                             !(input_stride_h == 1) ||   //
                             !(input_stride_w == 1) ||   //
                             !(filter_stride_h == 1) ||  //
                             !(filter_stride_w == 1) ||  //
#if !WTI_MODEL_ALLOW_ANY_RS
                             !(R <= 5) || //
                             !(S <= 5) || //
#endif
                             !(C >= 16) || //
                             !(K >= 16))
    {
        // Computations do not support negative padding.
        // Negative padding is not applicable, so let use simple assert here.
        assert(pad_h >= 0 && pad_w >= 0);
    }

    double ComputeWti() const
    {
        if(out_of_model_scope)
            return -1.0; // Shader model produces unreliable results.

        const auto direct_convolution_macs =
            static_cast<double>(C * N * K) / 1e+6 *
            static_cast<double>(RoundUpToMultiple(S * out_w / input_stride_w, 1)) *
            static_cast<double>(RoundUpToMultiple(R * out_h / input_stride_h, 1)); // AK

        constexpr size_t TILE_S = WINOFILTER; // AL
        constexpr size_t TILE_R = WINOFILTER; // AO
        assert(!(U > 2 && V > 2));
        const auto granulated_S =
            (U == 1 && input_stride_w == 1 && filter_stride_w == 1 && S <= TILE_S)
                ? TILE_S
                : RoundUpToMultiple(S, 2 * TILE_S); // AM
        const auto granulated_R = RoundUpToMultiple(
            R,
            (((V == 1 && input_stride_h == 1 && filter_stride_h == 1) || (R % (2 * TILE_R) == 1))
                 ? TILE_R
                 : 2 * TILE_R)); // AP

        constexpr size_t TILE_OUT_W = WINODATA; // AR
        constexpr size_t TILE_OUT_H = WINODATA; // AU
        const auto granulated_out_w =
            RoundUpToMultiple(out_w + ((input_stride_w == 2 && (pad_w % 2 != 0)) ? 1 : 0),
                              TILE_OUT_W * input_stride_w); // AS
        const auto granulated_out_h =
            RoundUpToMultiple(out_h + ((input_stride_h == 2 && (pad_h % 2 != 0)) ? 1 : 0),
                              TILE_OUT_H * input_stride_h); // AV

        constexpr size_t GRANULARITY_NHW_TILES = 32; // AY$2
        constexpr size_t GRANULARITY_K         = 32; // BC$2

        const auto NWH_tiles =
            granulated_out_w * granulated_out_h * N / TILE_OUT_H / TILE_OUT_W; // AX

        const auto granulated_NWH_tiles = RoundUpToMultiple(
            NWH_tiles,
            GRANULARITY_NHW_TILES * ((input_stride_w == 2 && input_stride_h == 2) ? 2 : 1)); // AY

        const auto granulated_C =
            RoundUpToMultiple(C,
                              ((U == 1 && S <= 3) ? 2 : 1) * 32 / DATATYPE_BITS); // BA

        const auto granulated_K = RoundUpToMultiple(
            K,
            GRANULARITY_K / ((input_stride_w == 2 && input_stride_h == 2) ? 2 : 1)); // BC

        const auto NKWH_tiles = granulated_NWH_tiles * granulated_K; // BE

        const auto granulated_NKWH_tiles =
            RoundUpToMultiple(NKWH_tiles,
                              n_groups * GRANULARITY_NHW_TILES * GRANULARITY_K); // BR

        const auto works_per_CU = granulated_NKWH_tiles / 32 / n_groups; // BY

        constexpr size_t MIN_FE_PER_WORK = 20; // BZ$2

        const auto fe_per_work =
            std::max(MIN_FE_PER_WORK,
                     granulated_S * granulated_R * granulated_C * DATATYPE_BITS / 32); // BZ

        const auto phases   = fe_per_work * works_per_CU; // CA
        const auto fe_calls = phases;                     // CC
        const auto be_calls = works_per_CU;               // CD

        constexpr double C0      = 43283;                                       // CB$2
        constexpr double C1      = 1.012;                                       // CC$2
        constexpr double C2      = 134.14;                                      // CD$2
        const auto GUI_predicted = (C0 + C1 * fe_calls + C2 * be_calls) / 1e+6; // CE

        if(GUI_predicted <= 0.1)
            return -1.0; // Unreliable, too small work to do for the shader.

        const auto N_MACS_PER_CU_PER_CLOCK = 64 * 32 / DATATYPE_BITS;
        const auto WTI_predicted           = direct_convolution_macs /
                                   static_cast<double>(N_MACS_PER_CU_PER_CLOCK) /
                                   static_cast<double>(n_groups) / GUI_predicted; // similar to BW
        return WTI_predicted;
    }
};

static float GetWtiBase(const ConvolutionContext& params)
{
    constexpr auto WTI_UNKNOWN = -2.0;
    const auto rv              = ShaderModel(params).ComputeWti();
    return rv < 0 ? WTI_UNKNOWN : rv;
}

float ConvBinWinogradRxSf2x3::GetWti(const ConvolutionContext& params) const
{
    return GetWtiBase(params);
}

static bool IsApplicableBase(const ConvolutionContext& params)
{
    if(!params.Is2d())
        return false;
    if(!(params.IsFp32() || params.IsFp16()))
        return false;
    if(!params.use_asm_kernels)
        return false;
    if(!params.rmv.IsV3())
        return false;

    const auto name = params.GetStream().GetDeviceName();
    if(!(StartsWith(name, "gfx9") || StartsWith(name, "gfx10")))
        return false;
    if(params.IsFp16() &&
       !(StartsWith(name, "gfx906") || StartsWith(name, "gfx908") || StartsWith(name, "gfx1011") ||
         StartsWith(name, "gfx1012") || StartsWith(name, "gfx103")))
        return false;

    // clang-format off
    if (! ( (params.kernel_stride_w == 1 || params.kernel_stride_w == 2)
        && params.kernel_stride_w == params.kernel_stride_h
        && params.kernel_dilation_w == 1
        && params.kernel_dilation_h == 1
        && params.bias == 0
        && params.in_layout == "NCHW"))
        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())
    {
        if(params.kernel_stride_w == 2)
            return false;
        return IsShaderContraintsMet(params.in_height,
                                     params.in_width,
                                     params.kernel_dilation_h,
                                     params.kernel_dilation_w,
                                     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,
                                     params.kernel_stride_h,
                                     params.kernel_stride_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);
    }
}

bool ConvBinWinogradRxSf2x3::IsApplicable(const ConvolutionContext& params) const
{
    if(miopen::IsDisabled(MIOPEN_DEBUG_AMD_WINOGRAD_RXS_F2X3{}))
        return false;
    return IsApplicableBase(params) && params.group_counts > 1;
}

ConvSolution
ConvBinWinogradRxSf2x3::GetSolution(const ConvolutionContext& params,
                                    const PerformanceConfigConvBinWinogradRxSf2x3& config,
                                    const bool disableConfigOverrideFromEnv) const
{
    const auto n_groups = config.n_groups;
    static bool IsWarned;
    if(!IsWarned)
    {
        if(params.GetStream().GetMaxHardwareComputeUnits() > MAX_CU_LIMIT)
            MIOPEN_LOG_WE(SolverDbId(*this) << ": GPU has "
                                            << params.GetStream().GetMaxHardwareComputeUnits()
                                            << "CUs, but this solver supports max "
                                            << MAX_CU_LIMIT
                                            << "and thus may show sub-optimal performance.");
        IsWarned = true;
    }

    ConvSolution result;

    const PerformanceConfigConvBinWinogradRxSf2x3* pcfg = &config;
    PerformanceConfigConvBinWinogradRxSf2x3 fromEnv;

    if(!disableConfigOverrideFromEnv)
    {
        std::string s;
        const auto p_asciz = miopen::GetStringEnv(MIOPEN_DEBUG_AMD_WINOGRAD_RXS_F2X3_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_F2X3_PERF_VALS: "
                                 "Bad format or invalid for the problem config: "
                                 << s);
                }
                else
                {
                    MIOPEN_LOG_I("Overridden from env: " << fromEnv.ToString());
                    pcfg = &fromEnv;
                }
            }
        }
    }

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

    KernelInfo kernel;

    kernel.g_wk.push_back(wg_size * pcfg->GetNGroups() * 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() ? "_fp32" : "_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";
    }
    else if(params.kernel_stride_w == 2 && !params.direction.IsBackwardData())
    {
        kernel_postfix += "_stride2";
    }
    else // if(params.kernel_dilation_h == 2)
    {
        kernel_postfix += "_dilation2";
    }

    if(params.group_counts != 1 || params.direction.IsBackwardWrW())
        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, // 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);
            };
        };
    }

    return result;
}

bool ConvBinWinogradRxSf2x3g1::IsApplicable(const ConvolutionContext& params) const
{
    if(miopen::IsDisabled(MIOPEN_DEBUG_AMD_WINOGRAD_RXS_F2X3_G1{}))
        return false;
    return IsApplicableBase(params) && params.group_counts == 1;
}

float ConvBinWinogradRxSf2x3g1::GetWti(const ConvolutionContext& params) const
{
    return GetWtiBase(params);
}

ConvSolution ConvBinWinogradRxSf2x3g1::GetSolution(const ConvolutionContext& params) const
{
    const auto tunable = ConvBinWinogradRxSf2x3{};
    return tunable.GetSolution(params, tunable.GetPerformanceConfig(params), false);
}

} // namespace solver
} // namespace miopen

ext: cpp


text:
////////////////////////////////////////////////////////////////////////////////
// Name:      cmdline.cpp
// Purpose:   Implementation of wex::cmdline class
// Author:    Anton van Wezenbeek
// Copyright: (c) 2021-2022 Anton van Wezenbeek
////////////////////////////////////////////////////////////////////////////////

#include <boost/algorithm/string.hpp>
#include <wex/core/cmdline.h>
#include <wex/core/config.h>
#include <wex/core/core.h>
#include <wex/core/log.h>
#include <wex/core/regex.h>

#include "cmdline-imp.h"

#include <algorithm>
#include <iostream>
#include <numeric>

namespace wex
{
const std::string def_option(const std::vector<std::string>& v)
{
  return v.size() > 2 ? v.back() : std::string();
}

const std::string def_switch(const std::vector<std::string>& v)
{
  return v.size() > 2 ? v.back() : std::string("0");
}

template <typename T>
const auto find_if(const T& t, const std::vector<std::string>& v)
{
  return std::find_if(
    t.begin(),
    t.end(),
    [v](auto const& i)
    {
      return v[0] == find_before(i.first[0], ",");
    });
}

std::string get_option(
  const std::pair<
    const std::vector<std::string>,
    std::pair<cmdline::option_t, std::function<void(const std::any& any)>>>& p,
  config* cfg)
{
  const auto& name(find_before(p.first[0], ","));
  const auto& key(p.first[1]);

  try
  {
    switch (p.second.first)
    {
      case cmdline::FLOAT:
        return name + "=" +
               std::to_string(cfg->item(key).get(
                 static_cast<float>(std::stod(def_option(p.first))))) +
               "\n";

      case cmdline::INT:
        return name + "=" +
               std::to_string(
                 cfg->item(key).get(std::stoi(def_option(p.first)))) +
               "\n";

      case cmdline::STRING:
        return name + "=" + cfg->item(key).get(def_option(p.first)) + "\n";

      default:
        assert(0);
    }
  }
  catch (std::exception& e)
  {
    log(e) << "get_option:" << name << key;
  }

  return std::string();
}

std::string get_switch(
  const std::pair<const std::vector<std::string>, std::function<void(bool on)>>&
          p,
  config* cfg)
{
  const auto& name(find_before(p.first[0], ","));
  const auto& key(p.first[1]);

  try
  {
    return !cfg->item(key).get(
             static_cast<bool>(std::stoi(def_switch(p.first)))) ?
             "no" + name + "\n" :
             name + "\n";
  }
  catch (std::exception& e)
  {
    log(e) << "get_switch:" << name << key;
  }

  return std::string();
}
}; // namespace wex

#define ADD(TYPE, CONV)                        \
  if (!def_option(it.first).empty())           \
    m_parser->m_desc.add_options()(            \
      it.first[p_n].c_str(),                   \
      po::value<TYPE>()->implicit_value(CONV), \
      it.first[p_d].c_str());                  \
  else                                         \
    m_parser->m_desc.add_options()(            \
      it.first[p_n].c_str(),                   \
      po::value<TYPE>(),                       \
      it.first[p_d].c_str());

wex::cmdline::cmdline(
  const cmd_switches_t& s,
  const cmd_options_t&  o,
  const cmd_params_t&   p,
  bool                  add_standard_options,
  const std::string&    prefix)
  : m_cfg(new config(prefix))
  , m_options(o)
  , m_params(p)
  , m_switches(s)
  , m_add_standard_options(add_standard_options)
{
  if (!prefix.empty())
  {
    m_cfg->child_start();
  }
}

wex::cmdline::~cmdline()
{
  delete m_cfg;
  delete m_parser;
}

void wex::cmdline::get_all(std::string& help) const
{
  help += std::accumulate(
            m_options.begin(),
            m_options.end(),
            std::string(),
            [this](const std::string& a, const auto& b)
            {
              return a + get_option(b, m_cfg);
            }) +
          std::accumulate(
            m_switches.begin(),
            m_switches.end(),
            std::string(),
            [this](const std::string& a, const auto& b)
            {
              return a + get_switch(b, m_cfg);
            });
}

std::string wex::cmdline::get_output()
{
  return cmdline_imp::m_output;
}

std::string wex::cmdline::get_scriptout()
{
  return cmdline_imp::m_scriptout;
}

bool wex::cmdline::get_single(
  const std::vector<std::string>& v,
  std::string&                    help) const
{
  if (const auto& it = find_if(m_options, v); it != m_options.end())
  {
    help += get_option(*it, m_cfg);
    return true;
  }

  if (const auto& it = find_if(m_switches, v); it != m_switches.end())
  {
    help += get_switch(*it, m_cfg);
    return true;
  }

  return false;
}

bool wex::cmdline::is_echo()
{
  return cmdline_imp::m_is_echo;
}

bool wex::cmdline::is_output()
{
  return cmdline_imp::m_is_output;
}

void wex::cmdline::init()
{
  try
  {
    const size_t p_n{0}, p_d{1}; // par name, description

    std::for_each(
      m_switches.begin(),
      m_switches.end(),
      [this](const auto& it)
      {
        m_parser->m_desc.add_options()(
          it.first[p_n].c_str(),
          po::bool_switch(),
          it.first[p_d].c_str());
        m_parser->add_function(it.first[p_n], it.second);
      });

    std::for_each(
      m_options.begin(),
      m_options.end(),
      [this](const auto& it)
      {
        m_parser->add_function(
          it.first[p_n],
          {it.second.second, it.second.first});

        switch (it.second.first)
        {
          case FLOAT:
            ADD(float, (float)std::stod(def_option(it.first)));
            break;
          case INT:
            ADD(int, std::stoi(def_option(it.first)));
            break;
          case STRING:
            ADD(std::string, def_option(it.first));
            break;
          default:
            assert(0);
        }
      });

    if (!m_params.first.first.empty())
    {
      m_parser->m_desc.add_options()(
        m_params.first.first.c_str(),
        po::value<std::vector<std::string>>(),
        m_params.first.second.c_str());
      m_parser->m_pos_desc.add(m_params.first.first.c_str(), -1);
      m_parser->add_function(m_params.first.first, m_params.second);
    }
  }
  catch (std::exception& e)
  {
    log(e) << "cmdline";
  }
}

bool wex::cmdline::parse(data::cmdline& data)
{
  try
  {
    if (m_parser == nullptr)
    {
      m_parser = new cmdline_imp(m_add_standard_options, *m_cfg);
      init();
    }

    return m_parser->parse(data);
  }
  catch (std::exception& e)
  {
    if (data.av() != nullptr)
    {
      std::cout << e.what() << "\n";
    }
    else
    {
      data.help(e.what());
    }
  }
  catch (...)
  {
    log("unsupported flags");
  }

  return false;
}

bool wex::cmdline::parse_set(data::cmdline& data) const
{
  // [option[=[value]] ...][nooption ...][option? ...][all]
  /*
    When no arguments are specified, write the value of the term edit
    option and those options whose values have been changed from the default
    settings; when the argument all is specified, write all of the option
    values.

    Giving an option name followed by the character '?' shall cause the
    current value of that option to be written.
    The '?' can be separated from the option name by zero or more <blank>
    characters. The '?' shall be necessary only for Boolean valued options.
    Boolean options can be given values by the form set option to turn
    them on or set no option to turn them off; string and numeric options
    can be assigned by the form set option= value.
    Any <blank> characters in strings can be included as is by preceding
    each <blank> with an escaping <backslash>.
    More than one option can be set or listed by a single set command
    by specifying multiple arguments, each separated from the next by one
    or more <blank> characters.
    */
  bool  found = false;
  regex r(
    {{"all",
      [&, this](const regex::match_t& m)
      {
        std::string help;
        get_all(help);
        data.help(help);
      }},
     // [nooption ...]
     {"no([a-z0-9]+)(.*)",
      [&, this](const regex::match_t& m)
      {
        if (set_no_option(m, data.save()))
          found = true;
      }},
     // [option? ...]
     {"([a-z0-9]+)[ \t]*\\?(.*)",
      [&, this](const regex::match_t& m)
      {
        std::string help;
        if (get_single(m, help))
        {
          data.help(help);
          found = true;
        }
      }},
     // [option[=[value]] ...]
     {"([a-z0-9]+)(=[a-z0-9]+)?(.*)",
      [&, this](const regex::match_t& m)
      {
        if (set_option(m, data.save()))
          found = true;
      }}});

  try
  {
    for (auto line(boost::algorithm::trim_copy(data.string())); !line.empty();
         line = r.matches().back())
    {
      switch (r.search(line); r.which_no())
      {
        case -1:
          data.help("unmatched cmdline: " + line);
          return false;

        case 0:
          return true;
      }
    }
  }
  catch (std::exception& e)
  {
    data.help(e.what());
  }

  return found;
}

bool wex::cmdline::set_no_option(const std::vector<std::string>& v, bool save)
  const
{
  return set_option_check(v, save, false);
}

bool wex::cmdline::set_option(const std::vector<std::string>& v, bool save)
  const
{
  if (v.size() > 1 && v[1][0] == '=')
  {
    // skip the =
    const auto val(v[1].substr(1));

    for (const auto& it : m_options)
    {
      if (v[0] == find_before(it.first[0], ","))
      {
        switch (it.second.first)
        {
          case wex::cmdline::FLOAT:
            m_cfg->item(it.first[1]).set(std::stof(val));
            if (it.second.second != nullptr)
              it.second.second(std::stof(val));
            return true;

          case wex::cmdline::INT:
            m_cfg->item(it.first[1]).set(std::stoi(val));
            if (it.second.second != nullptr)
              it.second.second(std::stoi(val));
            return true;

          case wex::cmdline::STRING:
            m_cfg->item(it.first[1]).set(val);
            if (it.second.second != nullptr)
              it.second.second(val);
            return true;

          default:
            assert(0);
        }
      }
    }
  }
  else
  {
    return set_option_check(v, save, true);
  }

  return false;
}

bool wex::cmdline::set_option_check(
  const std::vector<std::string>& v,
  bool                            save,
  bool                            check) const
{
  if (const auto& it = find_if(m_switches, v); it != m_switches.end())
  {
    if (save)
    {
      m_cfg->item(it->first[1]).set(check);
    }

    if (it->second != nullptr)
    {
      it->second(check);
    }

    return true;
  }

  return false;
}

bool wex::cmdline::use_events()
{
  return cmdline_imp::m_use_events;
}

max_stars_repo_name: antonvw/wxExtension

max_issues_repo_name: antonvw/wxExtension

max_forks_repo_name: antonvw/wxExtension


text:
#include "Test.h"
#include <Eigen/Dense>
#include "Robot.h"
#include "Body/Body.h"
#include "Utils.h"
#include "SimEnvGenerator.h"
#include "Joint/Joint.h"
#include "Body/BodyCuboid.h"
#include "Body/BodySphere.h"
#include "Joint/JointFixed.h"
#include "Joint/JointSphericalExp.h"
#include "Force/ForceGeneralPrimitiveContact.h"

using namespace redmax;

Eigen::VectorXd Test::random_generated_vector(int len, Eigen::VectorXd lower_bound, Eigen::VectorXd upper_bound) {
    Eigen::VectorXd res(len);
    for (int i = 0;i < len;++i) {
        double a = (double)rand() / RAND_MAX;
        res[i] = a * lower_bound[i] + (1.0 - a) * upper_bound[i];
    }
    return res;
}

Eigen::VectorXd Test::random_generated_vector(int len, double lower_bound, double upper_bound) {
    Eigen::VectorXd res(len);
    for (int i = 0;i < len;++i) {
        double a = (double)rand() / RAND_MAX;
        res[i] = a * lower_bound + (1.0 - a) * upper_bound;
    }
    return res;
}

Eigen::MatrixXd Test::random_generated_matrix(int n, int m, double lower_bound, double upper_bound) {
    Eigen::MatrixXd res(n, m);
    for (int i = 0;i < n;++i)
        for (int j = 0;j < m;++j) {
            double a = (double)rand() / RAND_MAX;
            res(i, j) = a * lower_bound + (1.0 - a) * upper_bound;
        }
    return res;
}

void Test::test_xml_reader() {
    Simulation* sim = new Simulation("../../assets/finger_torque.xml");
    // Simulation* sim = new Simulation("../../assets/rss_two_finger_assemble.xml");

    sim->_viewer_options->_loop = true;
    sim->_viewer_options->_infinite = true;

    sim->init();

    sim->print_ctrl_info();

    sim->print_design_params_info();

    auto qdot_init = sim->get_qdot_init();
    qdot_init[sim->_ndof_r - 2] = 5.;
    sim->set_qdot_init(qdot_init);

    sim->reset(false);

    int t0 = clock();
    sim->forward(300, false, false);

    int t1 = clock();

    std::cerr << "time = " << (t1 - t0) / 1000.0 << "ms" << std::endl;

    sim->replay();
}

void Test::test_speed() {
    Simulation* sim = new Simulation("../../assets/rss_finger_flip.xml");

    sim->init();

    sim->reset();

    int t0 = clock();
    for (int i = 0;i < 5000;i++)
        sim->forward(1, false, false);

    int t1 = clock();

    std::cerr << "time = " << (t1 - t0) / 1000.0 << "ms" << std::endl;
}

// for passive simulation without actuators
void Test::test_forward() {
    // Simulation* sim = SimEnvGenerator::createSinglePendulumTest();
    // Simulation* sim = SimEnvGenerator::createMultiPendulumTest(4);
    // Simulation* sim = SimEnvGenerator::createPrismaticTest();
    // Simulation* sim = SimEnvGenerator::createFree2DTest();
    // Simulation* sim = SimEnvGenerator::createGroundContactTest("BDF1");
    // Simulation* sim = SimEnvGenerator::createBoxContactTest("BDF1");
    // Simulation* sim = SimEnvGenerator::createSinglePendulumObjTest();
    // Simulation* sim = SimEnvGenerator::createSphereGroundContactTest("BDF2");
    // Simulation* sim = SimEnvGenerator::createFree3DEulerTest();
    // Simulation* sim = SimEnvGenerator::createFree3DExpTest();
    Simulation* sim = SimEnvGenerator::createTorqueFingerFlickDemo();

    sim->_viewer_options->_loop = true;
    sim->_viewer_options->_infinite = true;

    sim->init();

    int num_steps = 1000;

    int t0 = clock();
    sim->reset();
    sim->forward(num_steps, false);
    int t1 = clock();

    std::cerr << "time = " << (t1 - t0) / 1000.0 << "ms" << std::endl;

    std::cerr << "q = " << sim->get_q().transpose() << ", qdot = " << sim->get_qdot().transpose() << std::endl;

    sim->replay();
}

void Test::test_derivative() {
    // Simulation* sim = SimEnvGenerator::createSinglePendulumTest();
    // Simulation* sim = SimEnvGenerator::createMultiPendulumTest(4);
    // Simulation* sim = SimEnvGenerator::createPrismaticTest();
    // Simulation* sim = SimEnvGenerator::createFree2DTest();
    // Simulation* sim = SimEnvGenerator::createGroundContactTest();
    // Simulation* sim = SimEnvGenerator::createBoxContactTest();
    Simulation* sim = new Simulation("/home/eanswer/Workspace/Projects/RoboticHand/code/RoboticHandDesign/RedMax/assets/finger_mesh.xml");

    sim->init();

    // srand(time(0));
    // sim->set_q(Test::random_generated_vector(sim->_ndof_r, -1., 1.));
    // sim->set_qdot(Test::random_generated_vector(sim->_ndof_r, -1., 1.));

    // sim->test_derivative();

    // test JointSphericalExp Derivatives
    {
        JointSphericalExp* joint = new JointSphericalExp(sim, 0, nullptr, Matrix3::Identity(), Vector3::Zero());
        joint->test_derivatives();
        delete joint;
    }

    // test BodySphere Derivatives
    {
        JointFixed* joint = new JointFixed(sim, 0, nullptr, Matrix3::Identity(), Vector3::Zero());
        BodySphere* body_sphere = new BodySphere(nullptr, joint, 2., Matrix3::Identity(), Vector3::Zero(), 1.);
        body_sphere->test_collision_derivatives();
        delete joint;
        delete body_sphere;
    }

    // test BodyCuboid Derivatives
    {
        JointFixed* joint = new JointFixed(sim, 0, nullptr, Matrix3::Identity(), Vector3::Zero());
        BodyCuboid* body_cuboid = new BodyCuboid(nullptr, joint, Vector3(1., 1., 1.), Matrix3::Identity(), Vector3::Zero(), 1.);
        body_cuboid->test_collision_derivatives();
        delete joint;
        delete body_cuboid;
    }

    // test general-primitive contact Derivatives
    // {
    //     Simulation* sim = new Simulation("/home/eanswer/Workspace/Projects/RoboticHand/code/RoboticHandDesign/RedMax/assets/finger_mesh.xml");

    //     dynamic_cast<ForceGeneralPrimitiveContact*>(sim->_robot->_forces[2])->test_derivatives();

    //     delete sim;
    // }
}

void Test::test_design_derivative() {
    // Simulation* sim = new Simulation("/home/eanswer/Workspace/Projects/RoboticHand/code/RoboticHandDesign/RedMax/assets/double_pendulum_design_1.xml");
    // Simulation* sim = new Simulation("/home/eanswer/Workspace/Projects/RoboticHand/code/RoboticHandDesign/RedMax/assets/double_pendulum_design.xml");
    // Simulation* sim = new Simulation("/home/eanswer/Workspace/Projects/RoboticHand/code/RoboticHandDesign/RedMax/assets/double_pendulum_contact_design.xml");
    Simulation* sim = new Simulation("/home/eanswer/Workspace/Projects/RoboticHand/code/RoboticHandDesign/RedMax/assets/abstract_finger.xml");
    // Simulation* sim = new Simulation("/home/eanswer/Workspace/Projects/RoboticHand/code/RoboticHandDesign/RedMax/assets/new_finger.xml");

    sim->_viewer_options->_loop = true;
    sim->_viewer_options->_infinite = true;

    sim->init();

    sim->print_ctrl_info();

    sim->print_design_params_info();

    VectorX design_params = sim->get_design_params();

    sim->set_design_params(design_params);

    sim->reset(true, true);

    int t0 = clock();
    dtype f = 0.;
    int num_steps = 100;
    for (int ii = 0;ii < num_steps;ii ++) {
        sim->forward(1, false, true);
        VectorX var = sim->get_variables();
        f += var.sum();
    }
    sim->_backward_info.set_flags(false, false, true, false);
    sim->_backward_info._df_dq0 = VectorX::Zero(sim->_ndof_r);
    sim->_backward_info._df_dqdot0 = VectorX::Zero(sim->_ndof_r);
    sim->_backward_info._df_dq = VectorX::Zero(sim->_ndof_r * num_steps);
    sim->_backward_info._df_dvar = VectorX::Ones(sim->_ndof_var * num_steps);
    sim->_backward_info._df_dp = VectorX::Zero(sim->_ndof_p);
    sim->backward();
    int t1 = clock();

    std::cerr << "time = " << (t1 - t0) / 1000.0 << "ms" << std::endl;

    design_params = sim->get_design_params();

    std::cerr << "design params = " << design_params.transpose() << std::endl;
    std::cerr << "f = " << f << std::endl;
    std::cerr << "df_dp = " << sim->_backward_results._df_dp.transpose() << std::endl;
    VectorX df_dp = sim->_backward_results._df_dp;

    // dtype eps = 1e-2;
    // for (int trial = 0;trial < 10;trial++) {
    //     VectorX df_dp_fd = VectorX::Zero(sim->_ndof_p);
    //     for (int i = 0;i < sim->_ndof_p;i++) {
    //         // if (i % 12 < 9) {
    //         //     df_dp(i) = 0;
    //         //     continue;
    //         // }
    //         VectorX design_params_pos = design_params;
    //         design_params_pos[i] += eps;
    //         sim->set_design_params(design_params_pos);
    //         sim->reset(false);
    //         dtype f_pos = 0.;
    //         for (int ii = 0;ii < num_steps;ii++) {
    //             sim->forward(1, false, false);
    //             VectorX var = sim->get_variables();
    //             f_pos += var.sum();
    //         }
    //         df_dp_fd(i) = (f_pos - f) / eps;
    //     }

    //     // std::cerr << "df_dp_fd = " << df_dp_fd.transpose() << std::endl;

    //     print_error_full("df_dp", df_dp, df_dp_fd);
    //     eps /= 10.;
    // }

    sim->replay();
}

void Test::test_control_derivative() {
    Simulation* sim = new Simulation("/home/eanswer/Workspace/Projects/RoboticHand/code/RoboticHandDesign/RedMax/assets/abstract_finger.xml");

    sim->_viewer_options->_loop = true;
    sim->_viewer_options->_infinite = true;

    sim->init();

    sim->print_ctrl_info();

    sim->reset(true, false);

    int t0 = clock();
    dtype f = 0.;
    int num_steps = 1;
    VectorX action = VectorX::Zero(num_steps * sim->_ndof_u);
    action << -3.65349773,  2.90075299,  2.12093891,  2.06099197;
    VectorX u = action;
    for (int i = 0;i < u.size();i++)
        u(i) = tanh(u(i));

    for (int ii = 0;ii < num_steps;ii ++) {
        sim->set_u(u.segment(ii * sim->_ndof_u, (ii + 1) * sim->_ndof_u));
        sim->forward(1, false, false);
        VectorX var = sim->get_variables();
        f += var.sum();
    }
    sim->_backward_info.set_flags(false, false, false, true);
    sim->_backward_info._df_dq = VectorX::Zero(sim->_ndof_r * num_steps);
    sim->_backward_info._df_du = VectorX::Zero(sim->_ndof_u * num_steps);
    sim->_backward_info._df_dvar = VectorX::Ones(sim->_ndof_var * num_steps);
    // sim->_backward_info._df_du = VectorX::Ones(sim->_ndof_u * num_steps);
    // sim->_backward_info._df_dvar = VectorX::Zero(sim->_ndof_var * num_steps);
    sim->backward();
    int t1 = clock();

    std::cerr << "time = " << (t1 - t0) / 1000.0 << "ms" << std::endl;

    std::cerr << "f = " << f << std::endl;

    VectorX df_du = sim->_backward_results._df_du;
    VectorX df_daction = df_du;
    for (int i = 0;i < df_daction.size();i++) {
        df_daction[i] *= (1. - tanh(action(i)) * tanh(action(i)));
    }

    std::cerr << "df_du: " << df_du.transpose() << std::endl;
    {
        dtype eps = 1e-2;
        for (int trial = 0;trial < 10;trial++) {
            VectorX df_du_fd = VectorX::Zero(sim->_ndof_u * num_steps);
            for (int i = 0;i < num_steps * sim->_ndof_u;i++) {
                VectorX u_pos = u;
                u_pos(i) += eps;

                sim->set_u(u_pos);
                sim->reset(false, false);
                dtype f_pos = 0.;
                for (int ii = 0;ii < num_steps;ii++) {
                    sim->set_u(u_pos.segment(ii * sim->_ndof_u, (ii + 1) * sim->_ndof_u));
                    sim->forward(1, false, false);
                    VectorX var = sim->get_variables();
                    f_pos += var.sum();
                }
                // f_pos = u_pos.sum();
                df_du_fd(i) = (f_pos - f) / eps;
            }
            print_error_full("eps = " + to_string(eps) + ", df_du", df_du, df_du_fd);
            eps /= 10.;
        }
    }

    std::cerr << "df_daction: " << df_daction.transpose() << std::endl;

    {
        dtype eps = 1e-2;
        for (int trial = 0;trial < 10;trial++) {
            VectorX df_daction_fd = VectorX::Zero(sim->_ndof_u * num_steps);
            for (int i = 0;i < num_steps * sim->_ndof_u;i++) {
                VectorX action_pos = action;
                action_pos(i) += eps;
                VectorX u_pos = action_pos;
                for (int j = 0;j < u_pos.size();j++)
                    u_pos(j) = tanh(u_pos(j));

                sim->set_u(u_pos);
                sim->reset(false, false);
                dtype f_pos = 0.;
                for (int ii = 0;ii < num_steps;ii++) {
                    sim->set_u(u_pos.segment(ii * sim->_ndof_u, (ii + 1) * sim->_ndof_u));
                    sim->forward(1, false, false);
                    VectorX var = sim->get_variables();
                    f_pos += var.sum();
                }
                // f_pos = u_pos.sum();
                df_daction_fd(i) = (f_pos - f) / eps;
            }
            print_error_full("eps = " + to_string(eps) + ", df_daction", df_daction, df_daction_fd);
            eps /= 10.;
        }
    }

    sim->replay();
}

void Test::test_torque_finger() {
    Simulation* sim = SimEnvGenerator::createTorqueFingerDemo("BDF1");

    sim->init();

    int num_steps = 50;
    int ndof_r = sim->_ndof_r;
    int ndof_u = sim->_ndof_u;

    VectorX ctrl_min, ctrl_max;
    sim->get_ctrl_range(ctrl_min, ctrl_max);

    Vector3 q_goal(0., constants::pi / 2., constants::pi / 4.);
    Vector3 P_q = Vector3(10., 1.5, 1.5) * 1e5;

    int t0 = clock();
    sim->reset(false);

    for (int i = 0;i < num_steps;i++) {
        VectorX q = sim->get_q();
        VectorX error = q_goal - q;
        VectorX u = error.cwiseProduct(P_q);

        sim->set_u(u);

        sim->forward(1, false);
    }

    int t1 = clock();

    std::cerr << "time = " << (t1 - t0) / 1000.0 << "ms" << std::endl;

    std::cerr << "q = " << sim->get_q().transpose() << ", qdot = " << sim->get_qdot().transpose() << std::endl;

    sim->replay();
}

void Test::test_torque_finger_flick() {
    Simulation* sim = SimEnvGenerator::createTorqueFingerFlickDemo("BDF1");

    sim->init();

    int num_steps = 1000;
    int ndof_r = sim->_ndof_r;
    int ndof_u = sim->_ndof_u;

    VectorX ctrl_min, ctrl_max;
    sim->get_ctrl_range(ctrl_min, ctrl_max);

    Vector3 q_goal(0., 0., 0.);
    Vector3 P_q = Vector3(10., 1.5, 1.5) * 1e5;

    int t0 = clock();
    Vector6 q0 = (Vector6() << 0., constants::pi / 2., constants::pi / 4., 0., 0., 0.).finished();
    sim->set_q_init(q0);

    sim->reset(false);

    for (int i = 0;i < num_steps;i++) {
        VectorX q = sim->get_q();
        VectorX error = q_goal - q.head(3);
        VectorX u = error.cwiseProduct(P_q);

        sim->set_u(u);

        sim->forward(1, false);
    }

    int t1 = clock();

    std::cerr << "time = " << (t1 - t0) / 1000.0 << "ms" << std::endl;

    std::cerr << "q = " << sim->get_q().transpose() << ", qdot = " << sim->get_qdot().transpose() << std::endl;

    sim->replay();
}

void Test::test_pendulum_optimization() {
    Simulation* sim = SimEnvGenerator::createMultiPendulumTest(2, "BDF2");

    sim->init();

    sim->_viewer_options->_loop = false;
    sim->_viewer_options->_infinite = true;
    bool test_gradient = false;

    int num_steps = 100;

    Vector2 q_goal(constants::pi / 2., constants::pi / 4.);

    Vector2 q0(0., 0.);
    Vector2 qdot0(0., 0.);
    for (int opt_iter = 0;opt_iter < 2000;opt_iter ++) {
        sim->set_state_init(q0, qdot0);

        sim->reset(true);
        sim->forward(num_steps, false);

        VectorX q = sim->get_q();
        VectorX qdot = sim->get_qdot();

        if (opt_iter == 0) { // render the initial guess
            sim->replay();
        }

        // objective f = |q - q_goal|^2
        dtype f = (q - q_goal).dot(q - q_goal);
        std::cerr << "iter = " << opt_iter << ", f = " << f << std::endl;

        // set backward info
        sim->_backward_info.set_flags(true, true, false, false);

        int ndof_r = sim->_ndof_r;

        // set terminal derivatives
        sim->_backward_info._df_dq0 = VectorX::Zero(ndof_r);
        sim->_backward_info._df_dqdot0 = VectorX::Zero(ndof_r);
        sim->_backward_info._df_dq = VectorX::Zero(ndof_r * num_steps);
        sim->_backward_info._df_dq.tail(ndof_r) = 2. * (q - q_goal);

        // backward
        sim->backward();

        // test gradient
        if (test_gradient) {
            dtype eps = 0.01;
            for (int ii = 0;ii < 10;ii ++) {
                VectorX df_dq0_fd(ndof_r);
                for (int i = 0;i < ndof_r;i++) {
                    VectorX q0_pos = q0;
                    q0_pos(i) += eps;
                    sim->set_state_init(q0_pos, qdot0);
                    sim->reset(false);
                    sim->forward(num_steps, false);
                    VectorX q_pos = sim->get_q();
                    dtype f_pos = (q_pos - q_goal).dot(q_pos - q_goal);

                    VectorX q0_neg = q0;
                    q0_neg(i) -= eps;
                    sim->set_state_init(q0_neg, qdot0);
                    sim->reset(false);
                    sim->forward(num_steps, false);
                    VectorX q_neg = sim->get_q();
                    dtype f_neg = (q_neg - q_goal).dot(q_neg - q_goal);

                    df_dq0_fd(i) = (f_pos - f_neg) / eps / 2.;
                }
                VectorX df_dqdot0_fd(ndof_r);
                for (int i = 0;i < ndof_r;i++) {
                    VectorX qdot0_pos = qdot0;
                    qdot0_pos(i) += eps;
                    sim->set_state_init(q0, qdot0_pos);
                    sim->reset(false);
                    sim->forward(num_steps, false);
                    VectorX q_pos = sim->get_q();
                    dtype f_pos = (q_pos - q_goal).dot(q_pos - q_goal);

                    VectorX qdot0_neg = qdot0;
                    qdot0_neg(i) -= eps;
                    sim->set_state_init(q0, qdot0_neg);
                    sim->reset(false);
                    sim->forward(num_steps, false);
                    VectorX q_neg = sim->get_q();
                    dtype f_neg = (q_neg - q_goal).dot(q_neg - q_goal);

                    df_dqdot0_fd(i) = (f_pos - f_neg) / eps / 2.;
                }

                std::cerr << "eps = " << eps << std::endl;
                print_error("df_dq0", sim->_backward_results._df_dq0, df_dq0_fd);
                print_error("df_dqdot0", sim->_backward_results._df_dqdot0, df_dqdot0_fd);

                eps /= 2.;
            }
        }

        dtype step = 0.02;
        q0 -= step * sim->_backward_results._df_dq0;
        qdot0 -= step * sim->_backward_results._df_dqdot0;
    }

    sim->set_state_init(q0, qdot0);

    sim->reset(false);
    sim->forward(num_steps, false);

    std::cerr << "q0 = " << q0.transpose() << ", qdot0 = " << qdot0.transpose() << std::endl;
    std::cerr << "q = " << sim->get_q().transpose() << ", qdot = " << sim->get_qdot().transpose() << std::endl;

    sim->replay(); // render the optimized values
}

void Test::test_torque_finger_optimization() {
    Simulation* sim = SimEnvGenerator::createTorqueFingerDemo("BDF2");

    sim->init();

    sim->_viewer_options->_loop = true;
    sim->_viewer_options->_infinite = true;
    bool test_gradient = false;

    int num_steps = 50;
    int ndof_r = sim->_ndof_r;
    int ndof_u = sim->_ndof_u;

    VectorX ctrl_min, ctrl_max;
    sim->get_ctrl_range(ctrl_min, ctrl_max);

    Vector3 q_goal(0., constants::pi / 2., constants::pi / 4.);

    VectorX u = VectorX::Zero(ndof_u * num_steps);

    int t0 = clock();
    for (int opt_iter = 0;opt_iter < 1000;opt_iter ++) {
        sim->reset(true);
        std::vector<VectorX> qs;
        qs.clear();
        for (int i = 0;i < num_steps;i++) {
            sim->set_u(u.segment(ndof_u * i, ndof_u));
            sim->forward(1, false);
            qs.push_back(sim->get_q());
        }

        if (opt_iter == 0) { // render the initial guess
            sim->replay();
        }

        // objective f = sum(|q(i) - q_goal|^2)
        dtype f = 0.;
        for (int i = 0;i < num_steps;i++) {
            f += (qs[i] - q_goal).dot(qs[i] - q_goal);
        }
        std::cerr << "iter = " << opt_iter << ", f = " << f << std::endl;

        // set backward info
        sim->_backward_info.set_flags(false, false, false, true);

        // set terminal derivatives
        sim->_backward_info._df_du = VectorX::Zero(ndof_u * num_steps);
        sim->_backward_info._df_dq = VectorX::Zero(ndof_r * num_steps);
        for (int i = 0;i < num_steps;i++) {
            sim->_backward_info._df_dq.segment(ndof_r * i, ndof_r) = 2. * (qs[i] - q_goal);
        }

        // backward
        sim->backward();

        // test gradient
        if (test_gradient) {
            // dtype eps = 1e-7;
            dtype eps = 0.01;
            for (int ii = 0;ii < 10;ii++) {
                VectorX df_du_fd(ndof_u * num_steps);
                for (int k = 0;k < ndof_u * num_steps;k++) {
                    VectorX u_pos = u;
                    u_pos(k) += eps;
                    sim->reset(false);
                    dtype f_pos = 0.;
                    for (int i = 0;i < num_steps;i++) {
                        sim->set_u(u_pos.segment(ndof_u * i, ndof_u));
                        sim->forward(1, false);
                        VectorX q = sim->get_q();
                        f_pos += (q - q_goal).dot(q - q_goal);
                    }
                    df_du_fd(k) = (f_pos - f) / eps;
                }

                std::cerr << "eps = " << eps << std::endl;
                print_error("df_du", sim->_backward_results._df_du, df_du_fd);

                eps /= 2.;
            }
        }

        dtype step = 0.05;
        u -= step * sim->_backward_results._df_du;
        // for (int i = 0;i < num_steps;i++)
        //     for (int j = 0;j < ndof_u;j++)
        //         u(i * ndof_u + j) = min(ctrl_max(j), max(ctrl_min(j), u(i * ndof_u + j)));
    }

    int t1 = clock();
    std::cerr << "time = " << (t1 - t0) / 1000.0 << "ms" << std::endl;

    sim->reset(false);
    for (int i = 0;i < num_steps;i++) {
        sim->set_u(u.segment(ndof_u * i, ndof_u));
        sim->forward(1, false);
    }

    sim->replay();
}

void Test::test_torque_finger_flick_optimization() {
    Simulation* sim = SimEnvGenerator::createTorqueFingerFlickDemo("BDF2");

    sim->init();

    sim->_viewer_options->_loop = true;
    sim->_viewer_options->_infinite = true;
    bool test_gradient = false;

    int num_steps = 1000;
    int ndof_r = sim->_ndof_r;
    int ndof_u = sim->_ndof_u;

    VectorX ctrl_min, ctrl_max;
    sim->get_ctrl_range(ctrl_min, ctrl_max);

    Vector6 q0 = (Vector6() << 0., constants::pi / 2., constants::pi / 4., 0., 0., 0.).finished();
    sim->set_q_init(q0);

    dtype x_goal = 5.5;

    VectorX u = VectorX::Zero(ndof_u * num_steps);

    // initial guess
    Vector3 q_goal(0., 0., 0.);
    Vector3 P_q = Vector3(10., 2.0, 3.0);

    sim->reset(false);

    for (int i = 0;i < num_steps;i++) {
        VectorX q = sim->get_q();
        VectorX error = q_goal - q.head(3);
        VectorX ui = error.cwiseProduct(P_q);
        // ui.setZero();
        // for (int j = 0;j < ui.size();j++)
        //     ui[j] = min(1., max(ui[j], -1.));

        u.segment(ndof_u * i, ndof_u) = ui;

        sim->set_u(ui);

        sim->forward(1, false);
    }

    std::cerr << "u = " << u.head(10).transpose() << std::endl;
    std::cerr << u.maxCoeff() << ", " << u.minCoeff() << std::endl;
    std::cerr << "u.norm = " << u.norm() << std::endl;

    std::cerr << "q = " << sim->get_q().transpose() << ", qdot = " << sim->get_qdot().transpose() << std::endl;
    int t0 = clock();
    for (int opt_iter = 0;opt_iter < 1;opt_iter ++) {
        sim->reset(true);
        for (int i = 0;i < num_steps;i++) {
            sim->set_u(u.segment(ndof_u * i, ndof_u));
            sim->forward(1, false);
        }

        VectorX q = sim->get_q();

        if (opt_iter == 0) { // render the initial guess
            sim->replay();
        }

        // objective f = sum(|q(3) - x_goal|^2)
        dtype f = (q(3) - x_goal) * (q(3) - x_goal);

        std::cerr << "iter = " << opt_iter << ", f = " << f << std::endl;

        // set backward info
        sim->_backward_info.set_flags(false, false, false, true);

        // set terminal derivatives
        sim->_backward_info._df_du = VectorX::Zero(ndof_u * num_steps);
        sim->_backward_info._df_dq = VectorX::Zero(ndof_r * num_steps);
        sim->_backward_info._df_dq(ndof_r * (num_steps - 1) + 3) = 2. * (q(3) - x_goal);

        // backward
        sim->backward();

        // // test gradient
        // if (test_gradient) {
        //     dtype eps = 1e-7;
        //     VectorX df_du_fd(ndof_u * num_steps);
        //     for (int k = 0;k < ndof_u * num_steps;k++) {
        //         VectorX u_pos = u;
        //         u_pos(k) += eps;
        //         sim->reset(false);
        //         dtype f_pos = 0.;
        //         for (int i = 0;i < num_steps;i++) {
        //             sim->set_u(u_pos.segment(ndof_u * i, ndof_u));
        //             sim->forward(1, false);
        //             VectorX q_pos = sim->get_q();
        //             f_pos += (q_pos(3) - x_goal) * (q_pos(3) - x_goal);
        //         }
        //         df_du_fd(k) = (f_pos - f) / eps;
        //     }

        //     // print_error("df_du", sim->_backward_results._df_du, df_du_fd);
        //     std::cerr << "df_du_fd = " << df_du_fd.head(10).transpose() << std::endl;
        // }

        std::cerr << "df_du = " << sim->_backward_results._df_du.head(10).transpose() << std::endl;
        std::cerr << "df_du = " << sim->_backward_results._df_du.norm() << std::endl;
        dtype step = 0.2;
        u -= step * sim->_backward_results._df_du;
        // for (int i = 0;i < num_steps;i++)
        //     for (int j = 0;j < ndof_u;j++)
        //         u(i * ndof_u + j) = min(1., max(-1., u(i * ndof_u + j)));
    }

    int t1 = clock();
    std::cerr << "time = " << (t1 - t0) / 1000.0 << "ms" << std::endl;

    sim->reset(false);
    for (int i = 0;i < num_steps;i++) {
        sim->set_u(u.segment(ndof_u * i, ndof_u));
        sim->forward(1, false);
    }

    std::cerr << "q = " << sim->get_q().transpose() << ", qdot = " << sim->get_qdot().transpose() << std::endl;

    sim->replay();
}

void Test::test_ground_contact_optimization() {
    Simulation* sim = SimEnvGenerator::createGroundContactTest("BDF2");

    // target x,y position
    Vector2 pos_goal(1.0, 0.5);

    // Add virtual goal
    JointFixed* joint = new JointFixed(sim, 10, nullptr, Matrix3::Identity(), Vector3(pos_goal(0), pos_goal(1), 0.));
    BodyCuboid* target = new BodyCuboid(sim, joint, Vector3(0.2, 0.2, 0.2), Matrix3::Identity(), Vector3::Zero(), 1.);
    target->set_color(Vector3(0., 1., 0.));

    sim->_robot->_root_joints.push_back(joint);
    sim->_robot->init();

    // start optimization
    sim->init();

    sim->_viewer_options->_loop = true;
    sim->_viewer_options->_infinite = true;
    bool test_gradient = false;

    int num_steps = 1000;
    int ndof_r = sim->_ndof_r;

    // optimize the initial state for cube 1 (i.e. qdot[0]~qdot[2])
    Vector3 q0 = (Vector3() << -2, 2, 0).finished();
    Vector3 qdot0 = (Vector3() << 5, 50, 50).finished();

    int t0 = clock();
    for (int opt_iter = 0;opt_iter < 200;opt_iter ++) {
        sim->set_state_init(q0, qdot0);

        sim->reset(true);
        sim->forward(num_steps, false);

        VectorX q = sim->get_q();

        if (opt_iter == 0) {// render the initial guess
            sim->replay();
        }

        // objective f = |q[0:2] - pos_goal|^2
        dtype f = (q.head(2) - pos_goal).dot(q.head(2) - pos_goal);
        std::cerr << "iter = " << opt_iter << ", f = " << f << std::endl;

        // set backward info
        sim->_backward_info.set_flags(false, true, false, false);

        int ndof_r = sim->_ndof_r;

        // set terminal derivatives
        sim->_backward_info._df_dqdot0 = VectorX::Zero(ndof_r);
        sim->_backward_info._df_dq = VectorX::Zero(ndof_r * num_steps);
        sim->_backward_info._df_dq.segment(ndof_r * (num_steps - 1), 2) = 2. * (q.head(2) - pos_goal);

        // backward
        sim->backward();

        // test gradient
        if (test_gradient) {
            dtype eps = 1e-7;
            VectorX df_dqdot0_fd(ndof_r);
            for (int i = 0;i < ndof_r;i++) {
                VectorX qdot0_pos = qdot0;
                qdot0_pos(i) += eps;
                sim->set_state_init(q0, qdot0_pos);
                sim->reset(false);
                sim->forward(num_steps, false);
                VectorX q_pos = sim->get_q();
                dtype f_pos = (q_pos.head(2) - pos_goal).dot(q_pos.head(2) - pos_goal);
                df_dqdot0_fd(i) = (f_pos - f) / eps;
            }

            print_error("df_dqdot0", sim->_backward_results._df_dqdot0, df_dqdot0_fd);
        }

        dtype step = 0.002;
        qdot0.head(3) -= step * sim->_backward_results._df_dqdot0.head(3);

        qdot0(0) = min((dtype)50., max((dtype)-50., qdot0(0)));
        qdot0(1) = min((dtype)50., max((dtype)-50., qdot0(1)));
        qdot0(2) = min((dtype)50., max((dtype)-50., qdot0(2)));

        std::cerr << "df_dqdot0 = " << sim->_backward_results._df_dqdot0.transpose() << std::endl;
        std::cerr << "qdot0 = " << qdot0.head(3).transpose() << std::endl;
    }

    int t1 = clock();
    std::cerr << "time = " << (t1 - t0) / 1000.0 << "ms" << std::endl;

    sim->set_state_init(q0, qdot0);

    sim->reset(false);
    sim->forward(num_steps, false);

    std::cerr << "qdot0 = " << qdot0.transpose() << std::endl;
    std::cerr << "q = " << sim->get_q().transpose() << std::endl;

    sim->replay();
}

void Test::test_box_contact_optimization() {
    Simulation* sim = SimEnvGenerator::createBoxContactTest("BDF2");

    // target x position for the cube 2 (i.e. target q[3])
    dtype x_goal = 3.5;

    // Add virtual goal
    JointFixed* joint = new JointFixed(sim, 10, nullptr, Matrix3::Identity(), Vector3(x_goal + 3., 0.5, 0.));
    BodyCuboid* target = new BodyCuboid(sim, joint, Vector3(0.2, 0.2, 0.2), Matrix3::Identity(), Vector3::Zero(), 1.);
    target->set_color(Vector3(0., 1., 0.));

    sim->_robot->_root_joints.push_back(joint);
    sim->_robot->init();

    sim->init();

    sim->_viewer_options->_loop = false;
    sim->_viewer_options->_infinite = true;
    bool test_gradient = false;

    int num_steps = 400;
    int ndof_r = sim->_ndof_r;

    // optimize the initial state for cube 1 (i.e. qdot[0]~qdot[2])
    Vector6 q0 = (Vector6() << 0., 0., 0., 0., 0., 0.).finished();
    Vector6 qdot0 = (Vector6() << 35., 0., 0., 0., 0., 0.).finished();
    for (int opt_iter = 0;opt_iter < 200;opt_iter ++) {
        sim->set_state_init(q0, qdot0);

        sim->reset(true);
        sim->forward(num_steps, false);

        VectorX q = sim->get_q();

        // if (opt_iter == 0) {// render the initial guess
        //     std:cerr << "q = " << q.transpose() << std::endl;
        //     sim->replay();
        // }

        // objective f = |q[3] - x_goal|^2
        dtype f = (q[3] - x_goal) * (q[3] - x_goal);
        std::cerr << "iter = " << opt_iter << ", f = " << f << std::endl;

        // set backward info
        sim->_backward_info.set_flags(false, true, false, false);

        int ndof_r = sim->_ndof_r;

        // set terminal derivatives
        sim->_backward_info._df_dq0 = VectorX::Zero(ndof_r);
        sim->_backward_info._df_dqdot0 = VectorX::Zero(ndof_r);
        sim->_backward_info._df_dq = VectorX::Zero(ndof_r * num_steps);
        sim->_backward_info._df_dq[ndof_r * (num_steps - 1) + 3] = 2. * (q[3] - x_goal);

        // backward
        sim->backward();

        // test gradient
        if (test_gradient) {
            dtype eps = 1e-7;
            VectorX df_dq0_fd(ndof_r);
            for (int i = 0;i < ndof_r;i++) {
                VectorX q0_pos = q0;
                q0_pos(i) += eps;
                sim->set_state_init(q0_pos, qdot0);
                sim->reset(false);
                sim->forward(num_steps, false);
                VectorX q_pos = sim->get_q();
                dtype f_pos = (q_pos[3] - x_goal) * (q_pos[3] - x_goal);
                df_dq0_fd(i) = (f_pos - f) / eps;
            }
            VectorX df_dqdot0_fd(ndof_r);
            for (int i = 0;i < ndof_r;i++) {
                VectorX qdot0_pos = qdot0;
                qdot0_pos(i) += eps;
                sim->set_state_init(q0, qdot0_pos);
                sim->reset(false);
                sim->forward(num_steps, false);
                VectorX q_pos = sim->get_q();
                dtype f_pos = (q_pos[3] - x_goal) * (q_pos[3] - x_goal);
                df_dqdot0_fd(i) = (f_pos - f) / eps;
            }

            // print_error("df_dq0", sim->_backward_results._df_dq0, df_dq0_fd);
            print_error("df_dqdot0", sim->_backward_results._df_dqdot0, df_dqdot0_fd);
        }

        dtype step = 0.5;
        qdot0(0) -= step * sim->_backward_results._df_dqdot0(0);
        qdot0(1) -= step * sim->_backward_results._df_dqdot0(1);

        // qdot0(0) = min(60., max(30., qdot0(0)));

        std::cerr << "df_dqdot0 = " << sim->_backward_results._df_dqdot0.head(3).transpose() << std::endl;
        std::cerr << "qdot0 = " << qdot0.head(3).transpose() << std::endl;
    }

    sim->set_state_init(q0, qdot0);

    sim->reset(false);
    sim->forward(num_steps, false);

    std::cerr << "q0 = " << q0.transpose() << ", qdot0 = " << qdot0.transpose() << std::endl;
    std::cerr << "q = " << sim->get_q().transpose() << ", qdot = " << sim->get_qdot().transpose() << std::endl;

    sim->replay();
}

max_stars_repo_name: eanswer/DiffHand

max_issues_repo_name: eanswer/DiffHand

max_forks_repo_name: eanswer/DiffHand


text:
#pragma once

#include <vector>
#include <boost/threadpool.hpp>
#include <thread>
#include "core.hpp"
#include "multithreading.hpp"
#include "boost/threadpool.hpp"

namespace mhcpp
{
	namespace objectives
	{
		class Evaluations
		{
		private:
			Evaluations() {}

			template<typename T>
			class EvaluateScoresTask
			{
			public:
				EvaluateScoresTask(const IObjectiveEvaluator<T>& evaluator, std::vector<T>& population) : population(population)
				{
					this->evaluator = evaluator.Clone();
				}

				std::vector<T>& population;
				IObjectiveEvaluator<T>* evaluator = nullptr;
				std::vector<IObjectiveScores<T>> result;
				~EvaluateScoresTask()
				{
					if (evaluator != nullptr)
					{
						delete evaluator;
						evaluator = nullptr;
					}
				}
				void EvaluateScores()
				{
					for (size_t i = 0; i < population.size(); i++)
						result.push_back(evaluator->EvaluateScore(population[i]));
				}
			};

		public:
			~Evaluations() {}

			template<typename T>
			static vector<vector<T>> MakeBins(vector<T> population, int numBins)
			{
				int div = population.size() / numBins;
				int remainder = population.size() % numBins;
				vector<vector<T>> result;
				int offset = 0;
				for (int i = 0; i < numBins; i++)
				{
					int len = (i < remainder ? div + 1 : div);
					result.push_back(vector<T>(len));
					std::copy(population.begin() + offset, population.begin() + offset + len, result[i].begin());
					offset += len;
				}
				return result;
			}

			template<typename T>
			static std::vector<IObjectiveScores<T>> EvaluateScores(IObjectiveEvaluator<T>* evaluator, const std::vector<T>& population, bool useMultiThreading, int nThreads)
			{
				std::vector<IObjectiveScores<T>> result;

				if (useMultiThreading && evaluator->IsCloneable())
				{
					int nParallel = nThreads;
					vector<vector<T>> subPop = MakeBins(population, nParallel);

					vector<EvaluateScoresTask<T>*> taskPkgs;
					vector<std::function<void()>> tasks;
					for (size_t i = 0; i < subPop.size(); i++)
					{
						auto eval = new EvaluateScoresTask<T> (*evaluator, subPop.at(i));
						taskPkgs.push_back(eval);
						std::function<void()> evalFunc =
							[=]()
						{
							eval->EvaluateScores();
						};
						tasks.push_back(evalFunc);
					}

					CrossThreadExceptions<std::function<void()>> cte(tasks);
					vector<exception_ptr> exceptions;
					try {
						cte.ExecuteTasks(nParallel);
						for (size_t i = 0; i < taskPkgs.size(); i++)
						{
							auto r = taskPkgs[i]->result;
							for (size_t j = 0; j < r.size(); j++)
								result.push_back(r[j]);
						}
					}
					catch (const std::exception&) {
						exceptions.push_back(current_exception());
					}
					for (size_t i = 0; i < taskPkgs.size(); i++)
						delete taskPkgs[i];

					if(exceptions.size() > 0)
						rethrow_exception(exceptions[0]);
				}
				else
				{
					for (size_t i = 0; i < population.size(); i++)
						result.push_back(evaluator->EvaluateScore(population[i]));
				}

				return result;
			}
		};
	}
}

ext: hpp

max_stars_repo_name: csiro-hydroinformatics/wila

max_forks_repo_name: csiro-hydroinformatics/wila


text:
/**
 * @author Wei Cao
 * @date 2009-10-15
 */
#include "boost/tuple/tuple.hpp"
#include <boost/test/unit_test.hpp>

#include <ir/index_manager/index/MockIndexReader.h>
#include <search-manager/WildcardPhraseDocumentIterator.h>

using namespace sf1r;
using namespace izenelib::ir::indexmanager;

BOOST_AUTO_TEST_SUITE( WildcardPhraseDocumentIterator_suite )

static boost::tuple<std::vector<uint32_t>, std::vector<size_t>, std::vector<size_t> > make_query(const std::string& query)
{
    std::vector<uint32_t> tl;
    std::vector<size_t> asterisk_pos;
    std::vector<size_t> question_mark_pos;

    uint32_t t;
    char ch;
    std::stringstream ss(query);
    while(!ss.eof()) {
        ss >> ch;
        if( ch == '*') {
            asterisk_pos.push_back(tl.size());
        } else if( ch == '?' ) {
            question_mark_pos.push_back(tl.size());
        } else {
                ss.putback(ch);
                ss >> t;
                tl.push_back(t);
        }
    }

    return boost::make_tuple(tl, asterisk_pos, question_mark_pos);
}

BOOST_AUTO_TEST_CASE(simple)
{
    MockIndexReaderWriter indexer;
    indexer.insertDoc(0, "title", "1 2 3 4 5 6");
    indexer.insertDoc(1, "title", "1 2 3 4 5 8 9 10");
    indexer.insertDoc(2, "title", "5 6 7 8 9 10 11");
    indexer.insertDoc(3, "title", "9 10 10 10 13 14 15 16 10");
    indexer.insertDoc(4, "title", "3 5 8 7 9");
    indexer.insertDoc(5, "title", "3 5 8 10 9");
    vector<unsigned> termIndexes;

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("3 *")),
            termIndexes, boost::get<1>(make_query("3 *")), boost::get<2>(make_query("3 *")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 0U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 1U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 4U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 5U);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("* 3")),
            termIndexes, boost::get<1>(make_query("* 3")), boost::get<2>(make_query("* 3")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 0U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 1U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 4U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 5U);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("1 * 2")),
            termIndexes, boost::get<1>(make_query("1 * 2")), boost::get<2>(make_query("1 * 2")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 0U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 1U);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("8 * 9")),
            termIndexes, boost::get<1>(make_query("8 * 9")), boost::get<2>(make_query("8 * 9")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 1U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 2U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 4U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 5U);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("1 * 3")),
            termIndexes, boost::get<1>(make_query("1 * 3")), boost::get<2>(make_query("1 * 3")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 0U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 1U);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("1 * 4")),
            termIndexes, boost::get<1>(make_query("1 * 4")), boost::get<2>(make_query("1 * 4")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 0U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 1U);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("1 ? 3")),
            termIndexes, boost::get<1>(make_query("1 ? 3")), boost::get<2>(make_query("1 ? 3")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 0U);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 1U);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("1 ? 4")),
            termIndexes, boost::get<1>(make_query("1 ? 4")), boost::get<2>(make_query("1 ? 4")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("1 2 3 * 6")),
            termIndexes, boost::get<1>(make_query("1 2 3 * 6")), boost::get<2>(make_query("1 2 3 * 6")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 0U);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("1 3 * 6")),
            termIndexes, boost::get<1>(make_query("1 3 * 6")), boost::get<2>(make_query("1 3 * 6")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("1 3")),
            termIndexes, boost::get<1>(make_query("1 3")), boost::get<2>(make_query("1 3")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("5 * 9 ? 11")),
            termIndexes, boost::get<1>(make_query("5 * 9 ? 11")), boost::get<2>(make_query("5 * 9 ? 11")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 2U);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

    {
        WildcardPhraseDocumentIterator_<MockIndexReaderWriter> iter( boost::get<0>(make_query("9 * 13 ? 15 * 10")),
            termIndexes, boost::get<1>(make_query("9 * 13 ? 15 * 10")), boost::get<2>(make_query("9 * 13 ? 15 * 10")), 0U, &indexer, "title",1);
        BOOST_CHECK_EQUAL(iter.next(), true );
        BOOST_CHECK_EQUAL(iter.doc(), 3U);
        BOOST_CHECK_EQUAL(iter.next(), false );
    }

}

BOOST_AUTO_TEST_SUITE_END()


text:
/*
 * Defines the class template boost::iostreams::detail::filter_adapter,
 * a convenience base class for filter adapters.
 *
 * File:        boost/iostreams/detail/adapter/filter_adapter.hpp
 * Date:        Mon Nov 26 14:35:48 MST 2007
 * Copyright:   2007-2008 CodeRage, LLC
 * Author:      Jonathan Turkanis
 * Contact:     turkanis at coderage dot com
 *
 * 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.)
 *
 * See http://www.boost.org/libs/iostreams for documentation.
 */

#ifndef BOOST_IOSTREAMS_DETAIL_FILTER_ADAPTER_HPP_INCLUDED
#define BOOST_IOSTREAMS_DETAIL_FILTER_ADAPTER_HPP_INCLUDED

#include <boost/iostreams/categories.hpp>
#include <boost/iostreams/detail/call_traits.hpp>
#include <boost/iostreams/detail/ios.hpp>
#include <boost/iostreams/operations.hpp>
#include <boost/iostreams/traits.hpp>
#include <boost/static_assert.hpp>

namespace boost { namespace iostreams { namespace detail {

template<typename T>
class filter_adapter {
private:
    typedef typename detail::value_type<T>::type value_type;
    typedef typename detail::param_type<T>::type param_type;
public:
    explicit filter_adapter(param_type t) : t_(t) { }
    T& component() { return t_; }

    template<typename Device>
    void close(Device& dev)
    {
        detail::close_all(t_, dev);
    }

    template<typename Device>
    void close(Device& dev, BOOST_IOS::openmode which)
    {
        iostreams::close(t_, dev, which);
    }

    template<typename Device>
    void flush(Device& dev)
    {
        return iostreams::flush(t_, dev);
    }

    template<typename Locale> // Avoid dependency on <locale>
    void imbue(const Locale& loc) { iostreams::imbue(t_, loc); }

    std::streamsize optimal_buffer_size() const
    { return iostreams::optimal_buffer_size(t_); }
public:
    value_type t_;
};

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

} } } // End namespaces detail, iostreams, boost.

#endif // #ifndef BOOST_IOSTREAMS_DETAIL_FILTER_ADAPTER_HPP_INCLUDED