#ifndef _Fast_Math_H__
#define _Fast_Math_H__
#include <arm_math.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 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
// 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);
float fast_arctan2(float y, float x);
void SinCos_Lut(float angle, float *s, float *c);

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

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

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)


/**
 * 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 = value * (1.0f - filter_constant) + sample * filter_constant)
#define LowPass_Filter(value, sample, filter_constant)	(value = ((float)sample - (float)value) * filter_constant + value)


static float limitRPM(float vel_limit, float vel_estimate, float vel_gain, float torque) {
    float Tmax = (vel_limit - vel_estimate) * vel_gain;
    float Tmin = (-vel_limit - vel_estimate) * vel_gain;
	if (torque < Tmin) {
		return Tmin;
	}
	if (Tmax > torque) {
		return torque;
	}
	return Tmax;
}

#define MAX_H 1.1F
static float lp_compestion(float w, float wc) {
	float comp = sqrtf(1.0f + SQ(w/wc));
	if (comp > MAX_H) {
		comp = MAX_H;
	}else if (comp < -MAX_H) {
		comp = -MAX_H;
	}
	return comp;
}


typedef struct {                         /* '<Root>/in' */
  float time;                         /* '<Root>/time' */
  float target;
  float diff;
  float Integrator;
  float Integrator1;
  float DT;
}TD_t;

void TD_run(TD_t *td, float in);
void TD_Init(TD_t *td, float wc, float DT);
#endif /* _Fast_Math_H__ */