skullc-peripherals/Utility/Inc/utility_fixedpoint.hpp

//
// Created by erki on 30.04.21.
//

#ifndef SKULLC_UTILITY_FIXEDPOINT_HPP_
#define SKULLC_UTILITY_FIXEDPOINT_HPP_

#include <cmath>
#include <cstdint>
#include <type_traits>

namespace Utility
{

namespace Details
{

template<typename T>
struct NextRankingInteger
{
};

template<>
struct NextRankingInteger<std::int8_t>
{
  using T = std::int16_t;
};

template<>
struct NextRankingInteger<std::int16_t>
{
  using T = std::int32_t;
};

template<>
struct NextRankingInteger<std::int32_t>
{
  using T = std::int64_t;
};

template<>
struct NextRankingInteger<std::int64_t>
{
  using T = std::int64_t;
};

template<>
struct NextRankingInteger<std::uint8_t>
{
  using T = std::uint16_t;
};

template<>
struct NextRankingInteger<std::uint16_t>
{
  using T = std::uint32_t;
};

template<>
struct NextRankingInteger<std::uint32_t>
{
  using T = std::uint64_t;
};

template<>
struct NextRankingInteger<std::uint64_t>
{
  using T = std::uint64_t;
};

}// namespace Details

template<typename U, std::size_t D>
struct FixedPoint
{
  using underlying = U;
  using isSigned = std::is_signed<underlying>;
  constexpr static std::size_t fractionalBits = D;

  underlying data;

  constexpr FixedPoint()
      : data{0}
  {}

  template<typename T>
  constexpr explicit FixedPoint(const T& value)
  {
    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");
    *this = FixedPoint<U, D>::fromValue(value);
  }

  template<typename T>
  constexpr FixedPoint& operator=(const T& value)
  {
    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");

    *this = FixedPoint<U, D>::fromValue(value);

    return *this;
  }

  constexpr bool operator==(const FixedPoint<U, D>& o) const
  {
    return data == o.data;
  }

  template<typename T>
  constexpr bool operator==(const T& value) const
  {
    static_assert(std::is_arithmetic_v<T>, "T is not an integral type.");
    const auto other = FixedPoint<U, D>(value);
    return *this == other;
  }

  template<typename T>
  constexpr bool operator!=(const T& value) const
  {
    static_assert(std::is_arithmetic_v<T>, "T is not an integral type.");
    const auto other = FixedPoint<U, D>(value);
    return *this != other;
  }

  constexpr bool operator!=(const FixedPoint<U, D>& o) const
  {
    return !operator==(o);
  }

  constexpr bool operator<(const FixedPoint<U, D>& o) const
  {
    return data < o.data;
  }

  constexpr bool operator<=(const FixedPoint<U, D>& o) const
  {
    return data <= o.data;
  }

  constexpr bool operator>(const FixedPoint<U, D>& o) const
  {
    return data > o.data;
  }

  constexpr bool operator>=(const FixedPoint<U, D>& o) const
  {
    return data >= o.data;
  }

  template<typename T>
  constexpr bool operator<(const T& value) const
  {
    const auto o = fromValue(value);
    return data < o.data;
  }

  template<typename T>
  constexpr bool operator<=(const T& value) const
  {
    const auto o = fromValue(value);
    return data <= o.data;
  }

  template<typename T>
  constexpr bool operator>(const T& value) const
  {
    const auto o = fromValue(value);
    return data > o.data;
  }

  template<typename T>
  constexpr bool operator>=(const T& value) const
  {
    const auto o = fromValue(value);
    return data >= o.data;
  }

  friend constexpr FixedPoint<U, D> operator+(const FixedPoint<U, D>& a, const FixedPoint<U, D>& b)
  {
    auto clone = a;
    clone.data += b.data;

    return clone;
  }

  friend constexpr FixedPoint<U, D> operator-(const FixedPoint<U, D>& a, const FixedPoint<U, D>& b)
  {
    auto clone = a;
    clone.data -= b.data;

    return clone;
  }

  template<typename T>
  friend constexpr FixedPoint<U, D> operator+(const FixedPoint<U, D>& a, const T& b)
  {
    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");
    return a + FixedPoint<U, D>(b);
  }

  template<typename T>
  friend constexpr FixedPoint<U, D> operator-(const FixedPoint<U, D>& a, const T& b)
  {
    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");
    return a - FixedPoint<U, D>(b);
  }

  friend constexpr FixedPoint<U, D> operator*(const FixedPoint<U, D>& a, const FixedPoint<U, D>& b)
  {
    // Ref: https://spin.atomicobject.com/2012/03/15/simple-fixed-point-math/
    // Ref: https://en.wikipedia.org/wiki/Q_(number_format)
    using T = typename Details::NextRankingInteger<U>::T;

    const T axb = T(a.data) * T(b.data);
    const T round_value = axb >> (fractionalBits - 1);

    auto dest = FixedPoint<U, D>();
    dest.data = (axb / (underlying(1) << fractionalBits)) + (round_value & 1);
    return dest;
  }

  friend constexpr FixedPoint<U, D> operator/(const FixedPoint<U, D>& a, const FixedPoint<U, D>& b)
  {
    // Ref: https://en.wikipedia.org/wiki/Q_(number_format)
    using T = typename Details::NextRankingInteger<U>::T;

    T temp = T(a.data) << fractionalBits;

    // rounding
    if ((temp >= 0 && b.data >= 0) || (temp < 0 && b.data < 0))
      temp += b.data / 2;
    else
      temp -= b.data / 2;

    auto dest = FixedPoint<U, D>();
    dest.data = temp / b.data;
    return dest;
  }

  template<typename T>
  friend constexpr FixedPoint<U, D> operator*(const FixedPoint<U, D>& a, const T& b)
  {
    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");
    return a * FixedPoint<U, D>(b);
  }

  template<typename T>
  friend constexpr FixedPoint<U, D> operator/(const FixedPoint<U, D>& a, const T& b)
  {
    static_assert(std::is_arithmetic_v<T>, "T is not an arithmetic type.");
    return a / FixedPoint<U, D>(b);
  }

  template<typename T>
  static constexpr FixedPoint<U, D> fromValue(const T& value)
  {
    static_assert(std::is_integral_v<T>, "T is not integral value.");
    auto fp = FixedPoint<U, D>();

    fp.data = value << D;

    return fp;
  }

  constexpr static FixedPoint<U, D> fromValue(const float& value)
  {
    auto fp = FixedPoint<U, D>();

    fp.data = std::round(value * (underlying(1) << fractionalBits));

    return fp;
  }

  constexpr static FixedPoint<U, D> fromValue(const double& value)
  {
    auto fp = FixedPoint<U, D>();

    fp.data = std::round(value * (underlying(1) << fractionalBits));

    return fp;
  }

  template<typename T>
  constexpr T toValue() const
  {
    static_assert(std::is_integral_v<T>, "T is not an integral type.");
    return data >> fractionalBits;
  }

  constexpr explicit operator float() const
  {
    return float(data) / float(underlying(1) << fractionalBits);
  }

  constexpr explicit operator double() const
  {
    return double(data) / double(underlying(1) << fractionalBits);
  }
};

using Q7 = FixedPoint<std::int8_t, 7>;
using Q15 = FixedPoint<std::int16_t, 15>;
using Q31 = FixedPoint<std::int32_t, 31>;

}// namespace Utility

#endif//SKULLC_UTILITY_FIXEDPOINT_HPP_