#ifndef __MATHS_H
#define __MATHS_H

#include "math.h"
#include "main.h"

#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))


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

#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

// Use floating point M_PI instead explicitly.
#define M_PIf       3.14159265358979323846f
#define M_LN2f      0.69314718055994530942f
#define M_Ef        2.71828182845904523536f

#define DEG2RAD		0.017453293f	/* 度转弧度 π/180 */
#define RAD2DEG		57.29578f		/* 弧度转度 180/π */

#define RAD    (M_PIf / 180.0f)

#define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100)
#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100)

#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10)
#define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10)

#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)
#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10)

#define DEGREES_PER_DEKADEGREE 10
#define DEGREES_TO_DEKADEGREES(angle) ((angle) / DEGREES_PER_DEKADEGREE)
#define DEKADEGREES_TO_DEGREES(angle) ((angle) * DEGREES_PER_DEKADEGREE)

#define DEGREES_TO_RADIANS(angle) ((angle) * RAD)
#define RADIANS_TO_DEGREES(angle) ((angle) / RAD)
#define DECIDEGREES_TO_RADIANS(angle) (((angle) / 10.0f) * RAD)
#define RADIANS_TO_DECIDEGREES(angle) (((angle) * 10.0f) / RAD)

#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
#define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD)

#define MIN(a, b) 	(((a) < (b)) ? (a) : (b))
#define MAX(a, b) 	(((a) > (b)) ? (a) : (b))

#define ABS(x) 		(((x) < 0) ? (-x) : (x))


typedef enum {
    X = 0,
    Y,
    Z
} axis_e;

typedef struct stdev_s
{
    float m_oldM, m_newM, m_oldS, m_newS;
    int m_n;
} stdev_t;


// Floating point Euler angles.
// Be carefull, could be either of degrees or radians.
typedef struct fp_angles {
    float roll;
    float pitch;
    float yaw;
} fp_angles_def;


typedef union {
    float raw[3];
    fp_angles_def angles;
} fp_angles_t;



typedef struct {
    float XtY[4];
    float XtX[4][4];
} sensorCalibrationState_t;


float constrainFloat(float data,float min,float max);
int16_t constrainInt16_t(int16_t data,int16_t min,int16_t max);
uint16_t constrainUint16_t(uint16_t data,uint16_t min,uint16_t max);


void sensorCalibrationResetState(sensorCalibrationState_t * state);
void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3]);	
void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
float sin_approx(float x);
float cos_approx(float x);
float atan2_approx(float y, float x);
float acos_approx(float x);
void devClear(stdev_t *dev);
void devPush(stdev_t *dev, float x);
float devStandardDeviation(stdev_t *dev);
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3]);
float map(long x, long in_min, long in_max, float out_min, float out_max);
int32_t applyDeadband(int32_t value, int32_t deadband);
float applyDeadbandF(float value, float deadband);
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max);

#endif