#include "kernels.h"
#include <math.h>

double native_dry_friction(double velocity, double static_share, double coulomb,
                           double stiction, double stribeck_velocity, int stribeck,
                           double mode) {
    if (fabs(mode)<.5) return -static_share;
    double resistance=coulomb;
    if (stribeck) resistance+=(stiction-coulomb)*exp(-3*fabs(velocity)/stribeck_velocity);
    return mode>0 ? -resistance : resistance;
}

double native_contact(double penetration, double velocity, double stiffness,
                      double damping, double pdis, int signed_force) {
    if (penetration <= 0) return 0;
    double fraction = pdis > 0 ? -expm1(-penetration / pdis) : 1;
    double force = stiffness * penetration + fraction * damping * velocity;
    return signed_force == 1 ? force : fmax(force, 0);
}

void native_stop_motion(double x, double v, double lower, double upper,
                        double *acceleration, double *velocity) {
    double vt = 1e-12 * fmax(fabs(v), 1);
    if ((x <= lower + 1e-12*fmax(fabs(lower),1) && v <= vt && *acceleration <= 0) ||
        (x >= upper - 1e-12*fmax(fabs(upper),1) && v >= -vt && *acceleration >= 0)) {
        *acceleration = 0;
        *velocity = 0;
    }
}

double native_limit_force(double penetration, double velocity, double stiffness,
                          double damping, double depth, int signed_force) {
    return native_contact(penetration, velocity, stiffness, damping, depth, signed_force);
}