#ifndef __MY_MATH_H
#define __MY_MATH_H

#include <stdbool.h>
#include <stdint.h>

#define M_PI_F 3.14159265358979f //圆周率

#ifndef M_2PI_F
#define M_2PI_F (2 * M_PI_F)
#endif

#define EARTH_R 6378100 //地球半径（m）

// acceleration due to gravity in m/s/s
#define GRAVITY_MSS 9.80665f
// acceleration due to gravity in cm/s/s
#define GRAVITY_CMSS 980.665f

// Single precision conversions
#define DEG_TO_RAD 0.017453292519943295769236907684886f // 度换算弧度
#define RAD_TO_DEG 57.295779513082320876798154814105f   // 弧度换算度

// convert a longitude or latitude point to meters or centimeteres.
// Note: this does not include the longitude scaling which is dependent upon
// location
#define LONLAT_TO_M 0.01113195f
#define LONLAT_TO_CM 1.113195f

#define max(a, b)                                                              \
    ({                                                                         \
        typeof(a) _a = (a);                                                    \
        typeof(b) _b = (b);                                                    \
        _a > _b ? _a : _b;                                                     \
    })

#define min(a, b)                                                              \
    ({                                                                         \
        typeof(a) _a = (a);                                                    \
        typeof(b) _b = (b);                                                    \
        _a < _b ? _a : _b;                                                     \
    })

void buf2float(float *tfloat, unsigned char *buf);
void buf2long(long *tfloat, unsigned char *buf);
void buf2int(int *tint, unsigned char *buf);
void buf2short(short *tshort, unsigned char *buf);
void float2buf(unsigned char *buf, float *tfloat);
void int2buf(unsigned char *buf, int *tint);
void short2buf(unsigned char *buf, short *tshort);
int brinv(float a[], int n);

// constrain a int16_t value
int16_t constrain_int16(int16_t amt, int16_t low, int16_t high);
// constrain a int32_t value
int32_t constrain_int32(int32_t amt, int32_t low, int32_t high);

unsigned char constrain_uint8(unsigned char value, unsigned char min,
                              unsigned char max);
short math_constrain(short value, short min, short max);

short abs_int16(short value);

float min_float(float value1, float value2);
int min_int32(int value1, int value2);
short min_int16(short value1, short value2);
float max_float(float value1, float value2);
int max_int32(int value1, int value2);
short max_int16(short value1, short value2);

bool is_zero(const float fVal1);

float no_zero_float(float Val1);
int no_zero_int32(int Val1);

float sq(float v);
float get_norm(float a, float b);

// float stdev(short num[], int n);

void insert_sort(int arr[], int n);

#endif