#ifndef _Fast_Math_H__
#define _Fast_Math_H__
#include <arm_math.h>
#include "libs/utils.h"
// Constants
#define ONE_BY_SQRT3			(0.57735026919f) // 1/sqrt(3)
#define TWO_BY_SQRT3			(2.0f * 0.57735026919f)
#define SQRT3_BY_2				(0.86602540378f)
#define SQRT3                   (1.73205080757f)
#define SQRT2_BY_SQRT3          (0.8164966f)
#define	TWO_BY_THREE            (0.66667f)
#define M_PI (3.14159265f)


#define ONE_BY_SQRT3_Q14  (9459L) //0.57735026919 * 16384.0F
#define SQRT3_BY_2_Q14    (14189L)//0.86602540378 * 16384.0F
#define TWO_BY_SQRT3_Q14  (18918L)

#ifndef SQ
#define SQ(x) ((x)*(x))
#endif

#define NORM2_f(x,y)		(sqrtf(SQ(x) + SQ(y)))
// nan and infinity check for floats
#define UTILS_IS_INF(x)		((x) == (1.0F / 0.0F) || (x) == (-1.0F / 0.0F))
#define UTILS_IS_NAN(x)		((x) != (x))
#define UTILS_NAN_ZERO(x)	(x = UTILS_IS_NAN(x) ? 0.0F : x)

void fast_sincos(float angle, float *sin, float *cos);
void arm_sin_cos(float angle, float *s, float *c);

static __INLINE int32_t sclamp(int32_t v, int32_t minv, int32_t maxv) {
	if (v < minv) {
		return minv;
	}else if (v > maxv) {
		return maxv;
	}
	return v;
}

static __INLINE float fclamp(float v, float minv, float maxv) {
	if (v < minv) {
		return minv;
	}else if (v > maxv) {
		return maxv;
	}
	return v;
}


static void fast_norm_angle(float *angle) {
	*angle = fmodf(*angle, 360.0f);

	if (*angle < 0.0f) {
		*angle += 360.0f;
	}
}


static __INLINE float f_map(float x, float in_min, float in_max, float out1, float out2) {
	if (out1 > out2) { /* 递增map */
		return out1 - (x - in_min) * (out1 - out2) / (in_max - in_min);
	}else { /* 递减map */
		return (x - in_min) * (out2 - out1) / (in_max - in_min) + out1;
	}
}


static __INLINE void step_towards(float *value, float goal, float step) {
    if (*value < goal) {
        if ((*value + step) < goal) {
            *value += step;
        } else {
            *value = goal;
        }
    } else if (*value > goal) {
        if ((*value - step) > goal) {
            *value -= step;
        } else {
            *value = goal;
        }
    }
}

static __INLINE void step_towards_s16(s16 *value, s16 goal, s16 step) {
    if (*value < goal) {
        if ((*value + step) < goal) {
            *value += step;
        } else {
            *value = goal;
        }
    } else if (*value > goal) {
        if ((*value - step) > goal) {
            *value -= step;
        } else {
            *value = goal;
        }
    }
}

static __INLINE s16 line_intp(s16 x, s16 x_l, s16 x_h, s16 y_l, s16 y_h) {
	float r = (float)(x - x_l) /(float)(x_h - x_l);
	return (s16)(r * (y_h - y_l)) + y_l;
}

/**
 * Fast atan2
 *
 * See http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
 *
 * @param y
 * y
 *
 * @param x
 * x
 *
 * @return
 * The angle in radians
 */
static __INLINE float fast_atan2(float y, float x) {
	float abs_y = fabsf(y) + 1e-20f; // kludge to prevent 0/0 condition
	float angle;

	if (x >= 0) {
		float r = (x - abs_y) / (x + abs_y);
		float rsq = r * r;
		angle = ((0.1963f * rsq) - 0.9817f) * r + (M_PI / 4.0f);
	} else {
		float r = (x + abs_y) / (abs_y - x);
		float rsq = r * r;
		angle = ((0.1963f * rsq) - 0.9817f) * r + (3.0f * M_PI / 4.0f);
	}

	UTILS_NAN_ZERO(angle);

	if (y < 0) {
		return(-angle);
	} else {
		return(angle);
	}
}

static __INLINE float fast_atan_2(float y, float x) {
    // a := min (|x|, |y|) / max (|x|, |y|)
    float abs_y = ABS(y);
    float abs_x = ABS(x);
    // inject FLT_MIN in denominator to avoid division by zero
    float a = min(abs_x, abs_y) / (MAX(abs_x, abs_y) + 1e-20f);
    // s := a * a
    float s = a * a;
    // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
    float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
    // if |y| > |x| then r := 1.57079637 - r
    if (abs_y > abs_x)
        r = 1.57079637f - r;
    // if x < 0 then r := 3.14159274 - r
    if (x < 0.0f)
        r = 3.14159274f - r;
    // if y < 0 then r := -r
    if (y < 0.0f)
        r = -r;

    return r;
}

static __INLINE void saturate_vector_2d(float *x, float *y, float max) {
	float mag = NORM2_f(*x, *y);
	max = fabsf(max);

	if (mag < 1e-4f) {
		mag = 1e-4f;
	}
	if (mag > max) {
		const float f = max / mag;
		*x *= f;
		*y *= f;
	}
}


static void normal_sincosf(float angle, float *sin, float *cos) {
	*sin = arm_sin_f32(angle);
	*cos = arm_cos_f32(angle);
}
#define degree_2_pi(d) ((float)(d) * M_PI / 180.0f)
#define pi_2_degree(d) ((float)(d) * 180.0f / M_PI)

#define INVALID_ANGLE 0x3DFF

#define SIGN(x)				(((x) < 0.0f) ? -1.0f : 1.0f)

#define norm_angle_rad(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};


/**
 * A simple low pass filter.
 *
 * @param value
 * The filtered value.
 *
 * @param sample
 * Next sample.
 *
 * @param filter_constant
 * Filter constant. Range 0.0 to 1.0, where 1.0 gives the unfiltered value.
 */
/* 前向差分离散化 */
#define LowPass_Filter(value, sample, filter_constant)	(value = ((float)sample - (float)value) * filter_constant + value)
/* 后向差分离散化 */
#define do_lpf(value, sample, filter_constant)	((sample * filter_constant + value)/(1.0f + filter_constant))

#endif /* _Fast_Math_H__ */