/******************************************************************************
 *                                                                            *
 *                       (c) 2007-2024 Jaroslav fojtik                        *
 *                  This code could be used under LGPL licence                *
 *                                                                            *
 ******************************************************************************/
/** @file
 *  Floating point related macros and inlines.
 * Compiler: IAR, Keil, GCC, MSVC, Borland Turbo C
 *
 * $HeadURL: https://217.160.184.146:4433/svn/lib_datapool/lib/lib_datapool/arithmetic/arithmetic.h $
 * $Revision: 221 $
 * $Date: 2020-07-19 10:49:35 +0200 (ne, 19 7 2020) $
 * $Author: escjfo $ */
#ifndef __ARITHETIC_H__
#define __ARITHETIC_H__

#include "typedfs.h"


#define MARSH_FLOAT(ptr,val) memcpy(ptr,&val,4)
#define MARSH_DOUBLE(ptr,val) memcpy(ptr,&val,8)

#define MARSH_SHTIME(ptr,val) ST_UINT32(ptr,val)
#define MARSH_FULLTIME(ptr,pval) memcpy(ptr,pval,6)

#define DEMARSH_FLOAT(val,ptr) memcpy(&val,ptr,4)
#define DEMARSH_DOUBLE(val,ptr) memcpy(&val,ptr,8)



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

#ifdef __GNUC__
 #ifndef PACKED
  #define PACKED __attribute__((packed))     // TODO: Tohle na GCC nefunguje, nutno delat jinak.
 #endif
 #if __GNUC__<3
  #define sqrtf(_value) sqrt(_value)
 #endif
#else
 #ifndef PACKED
  #define PACKED
 #endif
 #if defined(_MSC_VER) || defined(__BORLANDC__) || defined(ewarm)
  #pragma pack(push, 1)
 #endif
#endif

typedef struct
   {
   unsigned Mantisa:23;
   unsigned Exponent:8;
   unsigned s:1;
   } PACKED FloatStruct;

typedef struct
{
    unsigned Mantisa2;
    unsigned Mantisa1: 20;
    unsigned Exponent: 11;
    unsigned s: 1;
} PACKED DoubleStruct;

#if defined(_MSC_VER) || defined(__BORLANDC__) || defined(ewarm)
 #pragma pack(pop)
#endif


#define SIGN_MASK     0x80000000
#define EXPONENT_MASK 0x7F800000
#define MANTISA_MASK  0x007FFFFF

#define IsFiniteF(floatptr) (((*(uint32_t *)(floatptr)) & EXPONENT_MASK)!=EXPONENT_MASK)
#define IsInfF(floatptr) (((*(uint32_t *)(floatptr)) & EXPONENT_MASK)==EXPONENT_MASK && ((*(uint32_t*)(floatptr)) & MANTISA_MASK)==0)
#define MakeNaNF(floatptr) *(uint32_t *)(floatptr) = 0xFFC00001
#define MakeInfF(floatptr) *(uint32_t *)(floatptr) = 0x7F800000
//#define MakeNaNF(FloatPtr) (((FloatStruct *)(FloatPtr))->Exponent=0xFF); ((FloatStruct *)(FloatPtr))->Mantisa=1
//#define MakeInfF(FloatPtr) (((FloatStruct *)(FloatPtr))->Exponent=0xFF); ((FloatStruct *)(FloatPtr))->Mantisa=0


#define SIGN_MASKdHi     0x80000000
#define EXPONENT_MASKdHi 0x7FF00000
#define MANTISA_MASKdHi  0x000FFFFF

#define IsFiniteD(dblptr) (((*((uint32_t *)(dblptr)+1)) & EXPONENT_MASKdHi)!=EXPONENT_MASKdHi)
#define IsInfD(dblptr) (((*((uint32_t *)(dblptr)+1)) & EXPONENT_MASKdHi)==EXPONENT_MASKdHi && ((*((uint32_t*)(dblptr)+1)) & MANTISA_MASKdHi)==0)
#define MakeInfD(DblPtr) (((DoubleStruct *)(DblPtr))->Exponent=0x7FF); ((DoubleStruct *)(DblPtr))->Mantisa1=0; ((DoubleStruct *)(DblPtr))->Mantisa2=0
#define MakeNaND(DblPtr) (((DoubleStruct *)(DblPtr))->Exponent=0x7FF); ((DoubleStruct *)(DblPtr))->Mantisa1=1; ((DoubleStruct *)(DblPtr))->Mantisa2=0


#define FLOAT_TOLERANCE    1e-5        // for comparison using relative difference


#ifndef M_PI
	/* Constants rounded for 21 decimals. */
 #define M_E         2.71828182845904523536
 #define M_LOG2E     1.44269504088896340736
 #define M_LOG10E    0.434294481903251827651
 #define M_LN2       0.693147180559945309417
 #define M_LN10      2.30258509299404568402
 #define M_PI        3.14159265358979323846
 #define M_PI_2      1.57079632679489661923
 #define M_PI_4      0.785398163397448309616
 #define M_1_PI      0.318309886183790671538
 #define M_2_PI      0.636619772367581343076
 #define M_1_SQRTPI  0.564189583547756286948
 #define M_2_SQRTPI  1.12837916709551257390
 #define M_SQRT2     1.41421356237309504880
 #define M_SQRT_2    0.707106781186547524401
#endif



#ifdef __cplusplus	// ************ C++ ONLY ************

 inline float DEG2RAD(const float x) {return((x)*(float)M_PI/180.0f);}
 inline double DEG2RAD(const double x) {return((x)*M_PI/180.0);}
 inline float RAD2DEG(const float x) {return((x)*180.0f/(float)M_PI);}
 inline double RAD2DEG(const double x) {return((x)*180.0/M_PI);}

 #if (__GNUC__ == 4 && __GNUC_MINOR__ >= 4) || (__GNUC__ > 4)
  #define NAN_SUPPORT_IN_CMATH		// GCC>=4.4.1 https://bugs.launchpad.net/ubuntu/+source/gcc-4.3/+bug/512741
 #endif
 #if (_MSC_VER>=1800)			// VS2013 and above.
  #define NAN_SUPPORT_IN_CMATH
 #endif
 
 #ifdef NAN_SUPPORT_IN_CMATH
  #include <cmath>
 #endif

inline bool IsFinite(const float *f)
{
#ifdef NAN_SUPPORT_IN_CMATH
    return std::isfinite(*f);
#else
    return IsFiniteF(f);
#endif
}

inline bool IsFinite(const float f)
{
#ifdef NAN_SUPPORT_IN_CMATH
    return std::isfinite(f);
#else
    return IsFiniteF(&f);
#endif
}


inline bool IsFinite(const double *d)
{
#ifdef NAN_SUPPORT_IN_CMATH
    return std::isfinite(*d);
#else
    return IsFiniteD(d);
#endif
}

inline bool IsFinite(const double d)
{
#ifdef NAN_SUPPORT_IN_CMATH
    return std::isfinite(d);
#else
    return IsFiniteD(&d);
#endif
}

 inline double sqr(const double d) {return(d*d);}
 inline float sqr(const float f) {return(f*f);}

 inline bool IsInf(const float *f) {return IsInfF(f);}
 inline bool IsInf(const double *d) {return IsInfD(d);}
 inline bool IsInf(const float f) {return IsInfF(&f);}
 inline bool IsInf(const double d) {return IsInfD(&d);}


 inline void MakeNaN(float *f) {MakeNaNF(f);}
 inline void MakeNaN(float &f) {MakeNaNF(&f);}
 inline void MakeNaN(double *d) {MakeNaND(d);}
 inline void MakeNaN(double &d) {MakeNaND(&d);}
 inline void MakeInf(float *f) {MakeInfF(f);}
 inline void MakeInf(double *d) {MakeInfD(d);}

 extern "C" {
#else			// ************ C ONLY ************
 #define IsFinite(realtptr) ((sizeof(*realtptr)==4) ? IsFiniteF(realtptr) : IsFiniteD(realtptr))
 #define IsInf(realtptr) ((sizeof(*realtptr)==4) ? IsInfF(realtptr) : IsInfD(realtptr))
 #define MakeNaN(realtptr) if(sizeof(*realtptr)==4) {MakeNaNF(realtptr);} else {MakeNaND(realtptr);}
 #define MakeInf(realtptr) if(sizeof(*realtptr)==4) {MakeInfF(realtptr);} else {MakeInfD(realtptr);}
 #ifndef DEG2RAD
   #define DEG2RAD(x) ((x)*M_PI/180.0)
 #endif
 #ifndef RAD2DEG
   #define RAD2DEG(x) ((x)*180.0/M_PI)
 #endif
#endif		/* __cplusplus */

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

#ifdef __BORLANDC__
 double SafeAtan2(double x, double y);
 #ifndef atan2
  #define atan2(_x,_y) SafeAtan2(_x,_y)
 #endif
#endif

int Round(float f);
float DiffAngle90(float num1, float num2);
float NormAngle(float num1);
float NormAngle90(float num1);
float InvSqrt(float x);

char *ftoa(float f, char *buffer);

int isEqualF(const float value1, const float value2, const float RelativeTolerance);
int isEqualD(const double value1, const double value2, const double RelativeTolerance);

#ifdef __cplusplus
}

inline bool isEqual(const float value1, const float value2, const float RelativeTolerance=FLOAT_TOLERANCE)
{return isEqualF(value1,value2,RelativeTolerance);}

inline bool isEqual(const double value1, const double value2, const double RelativeTolerance=FLOAT_TOLERANCE)
{return isEqualD(value1,value2,RelativeTolerance);}
#endif


#endif // __ARITHETIC_H__
