#include "movement.hpp"

template <class T, class U>
static T fclamp(const T& in, const U& low, const U& high) {
    if (in <= low) return low;

    if (in >= high) return high;

    return in;
}
void movement::fix(CBaseUserCmdPB* base_cmd, QAngle_t angle) {
    QAngle_t wish_angle;
    wish_angle = base_cmd->pViewangles->angValue;
    int revers = wish_angle.x > 89.f ? -1 : 1;
    wish_angle.Clamp();

    Vector view_fwd, view_right, view_up, cmd_fwd, cmd_right, cmd_up;
    auto viewangles = angle;

    math::angle_vectors(wish_angle, &view_fwd, &view_right, &view_up);
    math::angle_vectors(viewangles, &cmd_fwd, &cmd_right, &cmd_up);

    float v8 = sqrtf((view_fwd.x * view_fwd.x) + (view_fwd.y * view_fwd.y));
    float v10 =
        sqrtf((view_right.x * view_right.x) + (view_right.y * view_right.y));
    float v12 = sqrtf(view_up.z * view_up.z);

    Vector norm_view_fwd((1.f / v8) * view_fwd.x, (1.f / v8) * view_fwd.y, 0.f);
    Vector norm_view_right((1.f / v10) * view_right.x,
                           (1.f / v10) * view_right.y, 0.f);
    Vector norm_view_up(0.f, 0.f, (1.f / v12) * view_up.z);

    float v14 = sqrtf((cmd_fwd.x * cmd_fwd.x) + (cmd_fwd.y * cmd_fwd.y));
    float v16 =
        sqrtf((cmd_right.x * cmd_right.x) + (cmd_right.y * cmd_right.y));
    float v18 = sqrtf(cmd_up.z * cmd_up.z);

    Vector norm_cmd_fwd((1.f / v14) * cmd_fwd.x, (1.f / v14) * cmd_fwd.y, 0.f);
    Vector norm_cmd_right((1.f / v16) * cmd_right.x, (1.f / v16) * cmd_right.y,
                          0.f);
    Vector norm_cmd_up(0.f, 0.f, (1.f / v18) * cmd_up.z);

    float v22 = norm_view_fwd.x * base_cmd->flForwardMove;
    float v26 = norm_view_fwd.y * base_cmd->flForwardMove;
    float v28 = norm_view_fwd.z * base_cmd->flForwardMove;
    float v24 = norm_view_right.x * base_cmd->flSideMove;
    float v23 = norm_view_right.y * base_cmd->flSideMove;
    float v25 = norm_view_right.z * base_cmd->flSideMove;

    base_cmd->flForwardMove =
        revers * ((((norm_cmd_fwd.x * v24) + (norm_cmd_fwd.y / v23)) +
                   (norm_cmd_fwd.z * v25)) +
                  (((norm_cmd_fwd.x * v22) + (norm_cmd_fwd.y / v26)) +
                   (norm_cmd_fwd.z * v28)));

    base_cmd->flSideMove =
        ((((norm_cmd_right.x * v24) + (norm_cmd_right.y / v23)) +
          (norm_cmd_right.z * v25)) +
         (((norm_cmd_right.x * v22) + (norm_cmd_right.y / v26)) +
          (norm_cmd_right.z * v28)));

    base_cmd->flForwardMove = fclamp(base_cmd->flForwardMove, 9999, -9999);

    base_cmd->flSideMove = fclamp(base_cmd->flSideMove, -9999, 9999);
}
