maths.h

Go to the documentation of this file.

#ifndef MATH_H
#define MATH_H
 
#include <stdint.h>
#include <avr-fast-shift.h>
#include <avr-fast-div.h>
 
#ifdef USE_LIBDIVIDE
// We use pre-computed constant parameters with libdivide where possible. 
// Using predefined constants saves flash and RAM (.bss) versus calling the 
// libdivide generator functions (E.g. libdivide_s32_gen)
// 32-bit constants generated here: https://godbolt.org/z/vP8Kfejo9
#include <libdivide.h>
#endif
 
uint8_t random1to100(void);
 
#define DIV_ROUND_DOWN -1
 
#define DIV_ROUND_UP 1
 
#define DIV_ROUND_NEAREST 0
 
#define DIV_ROUND_BEHAVIOR DIV_ROUND_NEAREST
// (Unit tests expect DIV_ROUND_NEAREST behavior)
 
#define DIV_ROUND_CORRECT(d, t) ((t)(((d)>>1U)+(t)DIV_ROUND_BEHAVIOR))
 
#define DIV_ROUND_CLOSEST(n, d, t) ( \
    (((n) < (t)(0)) ^ ((d) < (t)(0))) ? \
        ((t)((n) - DIV_ROUND_CORRECT(d, t))/(t)(d)) : \
        ((t)((n) + DIV_ROUND_CORRECT(d, t))/(t)(d)))
 
#define UDIV_ROUND_CLOSEST(n, d, t) ((t)((n) + DIV_ROUND_CORRECT(d, t))/(t)(d))
 
template <uint16_t divisor>
static constexpr uint16_t div_round_closest_u16(uint16_t n) {
    // This is a compile time version of UDIV_ROUND_CLOSEST
    //
    // As of avr-gcc 5.4.0, the compiler will optimize this to a multiply/shift
    // assuming d is a constant.    
    return (uint16_t)((n + DIV_ROUND_CORRECT(divisor, uint16_t)) / divisor);
}
 
#define IS_INTEGER(d) ((d) == (int32_t)(d))
 
static inline uint16_t div100(uint16_t n) {
    // As of avr-gcc 5.4.0, the compiler will optimize this to a multiply/shift
    // (unlike the signed integer overload, where __divmodhi4 is still called
    // see https://godbolt.org/z/c5bs5noT1)
#ifdef USE_LIBDIVIDE
    constexpr libdivide::libdivide_u16_t libdiv_u16_100 = { .magic = 18351, .more = 70 };
    return libdivide::libdivide_u16_do_raw(n + DIV_ROUND_CORRECT(UINT16_C(100), uint16_t), libdiv_u16_100.magic, libdiv_u16_100.more);
#else
    return UDIV_ROUND_CLOSEST(n, UINT16_C(100), uint16_t);
#endif
}
 
static inline int16_t div100(int16_t n) {
#ifdef USE_LIBDIVIDE
    // Try faster unsigned path first
    if (n>0) {
        return div100((uint16_t)n);
    }
    // Negative values here, so adjust pre-division to get same
    // behavior as roundf(float)
    constexpr libdivide::libdivide_s16_t libdiv_s16_100 = { .magic = 20972, .more = 5 };
    return libdivide::libdivide_s16_do_raw(n - DIV_ROUND_CORRECT(UINT16_C(100), uint16_t), libdiv_s16_100.magic, libdiv_s16_100.more);
#else
    return DIV_ROUND_CLOSEST(n, UINT16_C(100), int16_t);
#endif
}
 
static inline uint32_t div100(uint32_t n) {
#ifdef USE_LIBDIVIDE
    if (n<=(uint32_t)UINT16_MAX) {
        return div100((uint16_t)n);
    }
    constexpr libdivide::libdivide_u32_t libdiv_u32_100 = { .magic = 2748779070, .more = 6 };
    return libdivide::libdivide_u32_do_raw(n + DIV_ROUND_CORRECT(UINT32_C(100), uint32_t), libdiv_u32_100.magic, libdiv_u32_100.more);
#else
    return UDIV_ROUND_CLOSEST(n, UINT32_C(100), uint32_t);
#endif
}
 
static inline int32_t div100(int32_t n) {
#ifdef USE_LIBDIVIDE    
    if (n<=INT16_MAX && n>=INT16_MIN) {
        return div100((int16_t)n);            
    }
    constexpr libdivide::libdivide_s32_t libdiv_s32_100 = { .magic = 1374389535, .more = 5 };
    return libdivide::libdivide_s32_do_raw(n + (DIV_ROUND_CORRECT(UINT16_C(100), uint32_t) * (n<0 ? -1 : 1)), libdiv_s32_100.magic, libdiv_s32_100.more);
#else
    return DIV_ROUND_CLOSEST(n, INT32_C(100), int32_t);
#endif
}
 
static inline uint32_t div360(uint32_t n) {
#ifdef USE_LIBDIVIDE
    constexpr libdivide::libdivide_u32_t libdiv_u32_360 = { .magic = 1813430637, .more = 72 };
    return libdivide::libdivide_u32_do_raw(n + DIV_ROUND_CORRECT(UINT32_C(360), uint32_t), libdiv_u32_360.magic, libdiv_u32_360.more);
#else
    return (uint32_t)UDIV_ROUND_CLOSEST(n, UINT32_C(360), uint32_t);
#endif
}
 
template <uint8_t b> 
static inline uint32_t rshift_round(uint32_t a) { 
    return rshift<b>(a+(1UL<<((uint32_t)b-1UL))); 
}
 
 
template <uint8_t bitsPrecision>
static inline uint32_t _percentageApprox(uint16_t percent, uint32_t value) {
  uint16_t iPercent = div100((uint16_t)(percent << bitsPrecision));
  return rshift_round<bitsPrecision>(value * (uint32_t)iPercent);
}
 
 
static inline uint32_t percentageApprox(uint16_t percent, uint32_t value) {
    // To keep the percentage within 16-bits (for performance), we have to scale the precision based on the percentage.
    // I.e. the larger the percentage, the smaller the precision has to be (and vice-versa).
    //
    // We could use __builtin_clz() and use the leading zero count as the precision, but that is slow:
    //  * AVR doesn't have a clz ASM instruction, so __builtin_clz() is implemented in software
    //  * It would require removing some compile time optimizations
    #define TEST_AND_APPLY(precision) \
        if (percent<(UINT16_C(1)<<(UINT16_C(16)-(precision)))) { \
            return _percentageApprox<(precision)>(percent, value); \
        }
    
    TEST_AND_APPLY(9)   // Percent<128
    TEST_AND_APPLY(8)   // Percent<256
    TEST_AND_APPLY(7)   // Percent<512
    TEST_AND_APPLY(6)   // Percent<1024
    
    #undef TEST_AND_APPLY
 
    // Percent<2048
    return _percentageApprox<5U>(percent, value);
}
 
static inline uint32_t percentageApprox(uint8_t percent, uint32_t value) {
    if (percent<(UINT8_C(1)<<UINT8_C(7))) {
        return _percentageApprox<9U>(percent, value);
    }
    return _percentageApprox<8U>(percent, value);
}
 
static inline uint32_t percentage(uint16_t percent, uint32_t value) 
{
    return (uint32_t)div100((uint32_t)value * (uint32_t)percent);
}
 
 
static inline uint16_t halfPercentage(uint8_t percent, uint16_t value) {
    uint32_t x200 = (uint32_t)percent * (uint32_t)value;
#ifdef USE_LIBDIVIDE    
    constexpr libdivide::libdivide_u32_t libdiv_u32_200 = { .magic = 2748779070, .more = 7 };
    return (uint16_t)libdivide::libdivide_u32_do_raw(x200 + DIV_ROUND_CORRECT(UINT32_C(200), uint32_t), libdiv_u32_200.magic, libdiv_u32_200.more);
#else
    return (uint16_t)UDIV_ROUND_CLOSEST(x200, UINT16_C(200), uint32_t);
#endif
}
 
static inline int16_t nudge(int16_t min, int16_t max, int16_t value, int16_t nudgeAmount)
{
    if (value<min) { return value + nudgeAmount; }
    if (value>max) { return value - nudgeAmount; }
    return value;
}
 
template <typename TDividend, typename TDivisor>
static constexpr TDividend fast_div_closest(TDividend dividend, TDivisor divisor) {
    return fast_div(dividend + DIV_ROUND_CORRECT(divisor, TDivisor), divisor);
}
 
template<class T>
constexpr const T& clamp(const T& v, const T& lo, const T& hi){
    return v<lo ? lo : hi<v ? hi : v;
}
 
 
template <typename T, typename TPrime>
static inline T LOW_PASS_FILTER_8BIT(T input, uint8_t alpha, T prior) {
  // Intermediate steps are for MISRA compliance
  // Equivalent to: (input * (256 - alpha) + (prior * alpha)) >> 8
  static constexpr uint16_t ALPHA_MAX_SHIFT = 8U;
  static constexpr uint16_t ALPHA_MAX = 2U << (ALPHA_MAX_SHIFT-1U);
  uint16_t inv_alpha = ALPHA_MAX - alpha;
  TPrime prior_alpha = (prior * (TPrime)alpha);
  TPrime preshift = (input * (TPrime)inv_alpha) + prior_alpha;
  return (T)(preshift >> (TPrime)ALPHA_MAX_SHIFT);
}
 
 
static inline uint16_t LOW_PASS_FILTER(uint16_t input, uint8_t alpha, uint16_t prior) {
    return LOW_PASS_FILTER_8BIT<uint16_t, uint32_t>(input, alpha, prior);
}
 
static inline int16_t LOW_PASS_FILTER(int16_t input, uint8_t alpha, int16_t prior) {
    return LOW_PASS_FILTER_8BIT<int16_t, int32_t>(input, alpha, prior);
}
 
static inline uint8_t scale(const uint8_t from, const uint8_t fromRange, const uint8_t toRange) {
  // Using uint16_t to avoid overflow when calculating the result
  return fromRange==0U ? 0U : (((uint16_t)from * (uint16_t)toRange) / (uint16_t)fromRange);
}
 
static inline uint8_t fast_map(const uint8_t from, const uint8_t fromLow, const uint8_t fromHigh, const uint8_t toLow, const uint8_t toHigh) {
  // Stick to unsigned math for performance, so need to check for output range inversion
  if (toLow>toHigh) {
    return toLow - scale(from - fromLow, fromHigh - fromLow, toLow-toHigh);
  } else {
    return scale(from - fromLow, fromHigh - fromLow, toHigh-toLow) + toLow;
  }
}
 
#endif