/*
 * Copyright (C) 2020-2021 Aloysius Indrayanto
 *
 * This file is part of the eCxx library, see LICENSE file for the license details.
 */


#ifndef ECXX_CTL_FIXED_POINT_H
#define ECXX_CTL_FIXED_POINT_H


#include <eCxx/CTL/MathUtility.h>
#include <eCxx/CTL/Utility.h>


//
// Uncomment this to use high precision (many not even compile for target platform AVR)
//
// #define ECXX_FIXED_POINT_USE_HIGH_PRECISION


namespace eCxx {


//
// Helper macros
//
#ifndef ECXX_FIXED_POINT_USE_HIGH_PRECISION
    #define ECXX_FIXED_POINT_VALUE_TYPE int32_t
#else
    #define ECXX_FIXED_POINT_VALUE_TYPE int64_t
#endif

#define ECXX_FIXED_POINT_CONST(VAL) ( ONE * VAL + ( (VAL >= 0.0f) ? 0.5f : -0.5f ) )


//
// Internal helper struct
//
namespace internal {


    template <typename ValueT>
    struct getFPSpec;


    template <>
    struct getFPSpec<int32_t> {
        // Intermediary floating-point type
        typedef float IFPType;

        // Number of bits
        static constexpr uint16_t TotalBits      = 24;
        static constexpr uint16_t WholeBits      = 16;
        static constexpr uint16_t FractionalBits =  8;

        // Scaling factor for decomposition to decimal
        static constexpr int32_t FractionalDSF  = 100;
        static constexpr uint8_t FractionalLen  =   2;

        // Number ranges
        static constexpr IFPType S_POSITIVE =      0.01f; // Theoretical
        static constexpr IFPType S_NEGATIVE =     -0.01f; // Theoretical
        static constexpr IFPType L_POSITIVE =  32767.99f; // Theoretical
        static constexpr IFPType L_NEGATIVE = -32768.00f; // Theoretical
    };


    template <>
    struct getFPSpec<int64_t> {
        // Intermediary floating-point type
        typedef double IFPType;

        // Number of bits
        static constexpr uint16_t TotalBits      = 48;
        static constexpr uint16_t WholeBits      = 32;
        static constexpr uint16_t FractionalBits = 16;

        // Scaling factor for decomposition to decimal
        static constexpr int32_t FractionalDSF  = 10000;
        static constexpr uint8_t FractionalLen  =     4;

        // Number ranges
        static constexpr IFPType S_POSITIVE =           0.0001; // Theoretical
        static constexpr IFPType S_NEGATIVE =          -0.0001; // Theoretical
        static constexpr IFPType L_POSITIVE =  2147483647.9999; // Theoretical
        static constexpr IFPType L_NEGATIVE = -2147483648.0000; // Theoretical
    };


} // namespace internal


//
// A simple fixed-point number
//
// # By default it uses 16.8 format (int32_t), hence:
//       + the theoretical positive number range is from +0.01 to +32767.99
//       + the theoretical negative number range is from -0.01 to -32768.00
//
// # There is no support for Inf and NaN (it means underflow and overflow will wrap around).
// # The smaller the number, the larger the error when performing arithmetic operations (it increases non linearly)!
//
// # If ECXX_FIXED_POINT_USE_HIGH_PRECISION is defined the it will use 32.16 format and the error will be reduced.
//
class FixedPoint : public SafeOperatorBool<FixedPoint> {
    public:
        using ValueType              = ECXX_FIXED_POINT_VALUE_TYPE;
        using SizeType               = uint16_t;

        using SignedValueType        = makeSigned      <ValueType            >::Type;
        using UnsignedValueType      = makeUnsigned    <ValueType            >::Type;

        using SignedHalfValueType    = makeHalfSizeType<SignedValueType      >::Type;
        using UnsignedHalfValueType  = makeHalfSizeType<UnsignedValueType    >::Type;

        using SignedQuartValueType   = makeHalfSizeType<SignedHalfValueType  >::Type;
        using UnsignedQuartValueType = makeHalfSizeType<UnsignedHalfValueType>::Type;

        static constexpr SizeType  TotalBits         = internal::getFPSpec<ValueType>::TotalBits;
        static constexpr SizeType  WholeBits         = internal::getFPSpec<ValueType>::WholeBits;
        static constexpr SizeType  FractionalBits    = internal::getFPSpec<ValueType>::FractionalBits;
        static constexpr ValueType FractionalMask    = ( static_cast<ValueType>(1) << FractionalBits ) - 1;

        static constexpr ValueType FractionalDSF     = internal::getFPSpec<ValueType>::FractionalDSF;

        static constexpr ValueType ZERO              = 0;
        static constexpr ValueType ONE               = ( static_cast<ValueType>(1) << FractionalBits );
        static constexpr ValueType HALF              = ( ONE                       >> 1              );
        static constexpr ValueType TWO               = ( ONE                       << 1              );

        static constexpr ValueType S_POSITIVE        = ECXX_FIXED_POINT_CONST(internal::getFPSpec<ValueType>::S_POSITIVE);
        static constexpr ValueType S_NEGATIVE        = ECXX_FIXED_POINT_CONST(internal::getFPSpec<ValueType>::S_NEGATIVE);
        static constexpr ValueType L_POSITIVE        = ECXX_FIXED_POINT_CONST(internal::getFPSpec<ValueType>::L_POSITIVE);
        static constexpr ValueType L_NEGATIVE        = ECXX_FIXED_POINT_CONST(internal::getFPSpec<ValueType>::L_NEGATIVE);

        static constexpr ValueType DEG_TO_RAD        = ECXX_FIXED_POINT_CONST( 0.0174533f);
        static constexpr ValueType RAD_TO_DEG        = ECXX_FIXED_POINT_CONST(57.2957795f);

        static constexpr ValueType QUARTER_PI        = ECXX_FIXED_POINT_CONST( 0.7853982f);
        static constexpr ValueType HALF_PI           = ECXX_FIXED_POINT_CONST( 1.5707963f);

        static constexpr ValueType PI                = ECXX_FIXED_POINT_CONST( 3.1415927f);
        static constexpr ValueType PI_SQUARE         = ECXX_FIXED_POINT_CONST( 9.8696044f);
        static constexpr ValueType SQRT_PI           = ECXX_FIXED_POINT_CONST( 1.7724539f);
        static constexpr ValueType TWO_PI            = ECXX_FIXED_POINT_CONST( 6.2831853f);
        static constexpr ValueType INV_PI            = ECXX_FIXED_POINT_CONST( 0.3183099f);
        static constexpr ValueType INV_PI_SQUARE     = ECXX_FIXED_POINT_CONST( 0.1013212f);
        static constexpr ValueType INV_SQRT_PI       = ECXX_FIXED_POINT_CONST( 0.5641896f);
        static constexpr ValueType INV_TWO_PI        = ECXX_FIXED_POINT_CONST( 0.1591549f);
        static constexpr ValueType TWO_INV_PI        = ECXX_FIXED_POINT_CONST( 0.6366198f);
        static constexpr ValueType TWO_INV_PI_SQUARE = ECXX_FIXED_POINT_CONST( 0.2026424f);
        static constexpr ValueType TWO_INV_SQRT_PI   = ECXX_FIXED_POINT_CONST( 1.1283792f);

        static constexpr ValueType E                 = ECXX_FIXED_POINT_CONST( 2.7182818f);
        static constexpr ValueType E_SQUARE          = ECXX_FIXED_POINT_CONST( 7.3890561f);
        static constexpr ValueType SQRT_E            = ECXX_FIXED_POINT_CONST( 1.6487213f);
        static constexpr ValueType TWO_E             = ECXX_FIXED_POINT_CONST( 5.4365637f);
        static constexpr ValueType INV_E             = ECXX_FIXED_POINT_CONST( 0.3678794f);
        static constexpr ValueType INV_E_SQUARE      = ECXX_FIXED_POINT_CONST( 0.1353353f);
        static constexpr ValueType INV_SQRT_E        = ECXX_FIXED_POINT_CONST( 0.6065307f);
        static constexpr ValueType INV_TWO_E         = ECXX_FIXED_POINT_CONST( 0.1839397f);
        static constexpr ValueType TWO_INV_E         = ECXX_FIXED_POINT_CONST( 0.7357589f);
        static constexpr ValueType TWO_INV_E_SQUARE  = ECXX_FIXED_POINT_CONST( 0.2706706f);
        static constexpr ValueType TWO_INV_SQRT_E    = ECXX_FIXED_POINT_CONST( 1.2130613f);

        static constexpr ValueType LN_PI             = ECXX_FIXED_POINT_CONST( 1.1447299f);
        static constexpr ValueType LN_TWO            = ECXX_FIXED_POINT_CONST( 0.6931472f);
        static constexpr ValueType LN_TEN            = ECXX_FIXED_POINT_CONST( 2.3025851f);
        static constexpr ValueType INV_LN_PI         = ECXX_FIXED_POINT_CONST( 0.8735685f);
        static constexpr ValueType INV_LN_TWO        = ECXX_FIXED_POINT_CONST( 1.4426950f);
        static constexpr ValueType INV_LN_TEN        = ECXX_FIXED_POINT_CONST( 0.4342945f);

        static constexpr ValueType LOG2_PI           = ECXX_FIXED_POINT_CONST( 1.6514961f);
        static constexpr ValueType LOG2_E            = ECXX_FIXED_POINT_CONST( 1.4426950f);
        static constexpr ValueType LOG2_TEN          = ECXX_FIXED_POINT_CONST( 3.3219281f);
        static constexpr ValueType INV_LOG2_PI       = ECXX_FIXED_POINT_CONST( 0.6055116f);
        static constexpr ValueType INV_LOG2_E        = ECXX_FIXED_POINT_CONST( 0.6931472f);
        static constexpr ValueType INV_LOG2_TEN      = ECXX_FIXED_POINT_CONST( 0.3010300f);

        static constexpr ValueType LOG10_PI          = ECXX_FIXED_POINT_CONST( 0.4971499f);
        static constexpr ValueType LOG10_E           = ECXX_FIXED_POINT_CONST( 0.4342945f);
        static constexpr ValueType LOG10_TWO         = ECXX_FIXED_POINT_CONST( 0.3010300f);
        static constexpr ValueType INV_LOG10_PI      = ECXX_FIXED_POINT_CONST( 2.0114659f);
        static constexpr ValueType INV_LOG10_E       = ECXX_FIXED_POINT_CONST( 2.3025851f);
        static constexpr ValueType INV_LOG10_TWO     = ECXX_FIXED_POINT_CONST( 3.3219281f);

        static constexpr ValueType SQRT_TWO          = ECXX_FIXED_POINT_CONST( 1.4142136f);
        static constexpr ValueType SQRT_THREE        = ECXX_FIXED_POINT_CONST( 1.7320508f);
        static constexpr ValueType SQRT_FIVE         = ECXX_FIXED_POINT_CONST( 2.2360680f);
        static constexpr ValueType INV_SQRT_TWO      = ECXX_FIXED_POINT_CONST( 0.7071068f);
        static constexpr ValueType INV_SQRT_THREE    = ECXX_FIXED_POINT_CONST( 0.5773503f);
        static constexpr ValueType INV_SQRT_FIVE     = ECXX_FIXED_POINT_CONST( 0.4472136f);

    public:
        static constexpr ValueType fpConst(float val) noexcept
        { return ECXX_FIXED_POINT_CONST(val); }

    public:
        FixedPoint(       char) = delete;
        FixedPoint(     int8_t) = delete;
        FixedPoint(    uint8_t) = delete;
        FixedPoint(    int16_t) = delete;
        FixedPoint(   uint16_t) = delete;
        FixedPoint(   uint32_t) = delete;
        FixedPoint(   uint64_t) = delete;
        FixedPoint(      float) = delete;
        FixedPoint(     double) = delete;
        FixedPoint(long double) = delete;

        inline FixedPoint(const FixedPoint& ) = default;
        inline FixedPoint(      FixedPoint&&) = default;

        inline FixedPoint& operator=(const FixedPoint& ) = default;
        inline FixedPoint& operator=(      FixedPoint&&) = default;

        inline ~FixedPoint() = default;

    public:
        inline FixedPoint() noexcept;
        inline FixedPoint(ValueType fpValue) noexcept;
        inline FixedPoint(SignedHalfValueType decNumerator, UnsignedQuartValueType decDenumerator) noexcept;

        inline FixedPoint& operator=(ValueType fpValue) noexcept;

        inline int8_t sign() const noexcept;

        inline FixedPoint& abs() noexcept;
        inline FixedPoint& floor() noexcept;
        inline FixedPoint& ceil() noexcept;
        inline FixedPoint& round() noexcept;

        FixedPoint& sqrt() noexcept;
        FixedPoint& exp() noexcept;
        FixedPoint& ln() noexcept;
        FixedPoint& log(uint8_t base = 10) noexcept;
        FixedPoint& pow(uint8_t power) noexcept;

        FixedPoint& sin() noexcept;
        FixedPoint& cos() noexcept;
        FixedPoint& tan() noexcept;

        void decomposeToDecimal(bool& negative, UnsignedHalfValueType& integer, UnsignedQuartValueType& fractionalScaled) const noexcept;

    public:
        inline FixedPoint operator-() const noexcept;
        inline FixedPoint operator+() const noexcept;

        inline bool operator!() const noexcept;

        // Used by SafeOperatorBool<>
        inline bool operatorBool_impl() const;

        inline const FixedPoint& operator+=(const FixedPoint& rhs) noexcept;
        inline const FixedPoint& operator-=(const FixedPoint& rhs) noexcept;
        inline const FixedPoint& operator*=(const FixedPoint& rhs) noexcept;
        inline const FixedPoint& operator/=(const FixedPoint& rhs) noexcept;

        inline friend const FixedPoint operator+(FixedPoint lhs, const FixedPoint& rhs) noexcept;
        inline friend const FixedPoint operator-(FixedPoint lhs, const FixedPoint& rhs) noexcept;
        inline friend const FixedPoint operator*(FixedPoint lhs, const FixedPoint& rhs) noexcept;
        inline friend const FixedPoint operator/(FixedPoint lhs, const FixedPoint& rhs) noexcept;

        inline friend bool operator==(const FixedPoint& lhs, const FixedPoint& rhs) noexcept;
        inline friend bool operator!=(const FixedPoint& lhs, const FixedPoint& rhs) noexcept;
        inline friend bool operator< (const FixedPoint& lhs, const FixedPoint& rhs) noexcept;
        inline friend bool operator> (const FixedPoint& lhs, const FixedPoint& rhs) noexcept;
        inline friend bool operator<=(const FixedPoint& lhs, const FixedPoint& rhs) noexcept;
        inline friend bool operator>=(const FixedPoint& lhs, const FixedPoint& rhs) noexcept;

    private:
        ValueType _data;
};


inline FixedPoint::FixedPoint() noexcept
: _data(0)
{}


inline FixedPoint::FixedPoint(ValueType fpValue) noexcept
: _data(fpValue)
{}


inline FixedPoint::FixedPoint(SignedHalfValueType decNumerator, UnsignedQuartValueType decDenumerator) noexcept
: _data( (ONE * decNumerator + decDenumerator / 2) / decDenumerator )
{}


inline FixedPoint& FixedPoint::operator=(ValueType fpValue) noexcept
{
    _data = fpValue;
    return *this;
}


inline int8_t FixedPoint::sign() const noexcept
{
    if(_data < 0) return -1;
    if(_data > 0) return  1;
    return 0;
}


inline FixedPoint& FixedPoint::abs() noexcept
{
    if(_data < 0) _data = -_data;
    return *this;
}


inline FixedPoint& FixedPoint::floor() noexcept
{
    _data &= ~FractionalMask;
    return *this;
}


inline FixedPoint& FixedPoint::ceil() noexcept
{
    if(_data & ~FractionalMask) _data = (_data + ONE) & ~FractionalMask;
    return *this;
}


inline FixedPoint& FixedPoint::round() noexcept
{
    _data = (_data + HALF) & ~FractionalMask;
    return *this;
}


inline FixedPoint FixedPoint::operator-() const noexcept
{ return FixedPoint(-_data); }


inline FixedPoint FixedPoint::operator+() const noexcept
{ return FixedPoint(_data); }


inline bool FixedPoint::operator!() const noexcept
{ return !_data; }


inline bool FixedPoint::operatorBool_impl() const
{ return !!_data; }


inline const FixedPoint& FixedPoint::operator+=(const FixedPoint& rhs) noexcept
{
    _data = _data + rhs._data;
    return *this;
}


inline const FixedPoint& FixedPoint::operator-=(const FixedPoint& rhs) noexcept
{
    _data = _data - rhs._data;
    return *this;
}


inline const FixedPoint& FixedPoint::operator*=(const FixedPoint& rhs) noexcept
{
    _data = (_data * rhs._data) >> FractionalBits;
    return *this;
}


inline const FixedPoint& FixedPoint::operator/=(const FixedPoint& rhs) noexcept
{
    _data = (_data << FractionalBits) / rhs._data;
    return *this;
}


//
// Inline friend operators of the FixedPoint class
//
inline const FixedPoint operator+(FixedPoint lhs, const FixedPoint& rhs) noexcept
{
    lhs += rhs;
    return lhs;
}


inline const FixedPoint operator-(FixedPoint lhs, const FixedPoint& rhs) noexcept
{
    lhs -= rhs;
    return lhs;
}


inline const FixedPoint operator*(FixedPoint lhs, const FixedPoint& rhs) noexcept
{
    lhs *= rhs;
    return lhs;
}


inline const FixedPoint operator/(FixedPoint lhs, const FixedPoint& rhs) noexcept
{
    lhs /= rhs;
    return lhs;
}


inline bool operator==(const FixedPoint& lhs, const FixedPoint& rhs) noexcept
{ return lhs._data == rhs._data; }


inline bool operator!=(const FixedPoint& lhs, const FixedPoint& rhs) noexcept
{ return lhs._data != rhs._data; }


inline bool operator<(const FixedPoint& lhs, const FixedPoint& rhs) noexcept
{ return lhs._data < rhs._data; }


inline bool operator>(const FixedPoint& lhs, const FixedPoint& rhs) noexcept
{ return lhs._data > rhs._data; }


inline bool operator<=(const FixedPoint& lhs, const FixedPoint& rhs) noexcept
{ return lhs._data <= rhs._data; }


inline bool operator>=(const FixedPoint& lhs, const FixedPoint& rhs) noexcept
{ return lhs._data >= rhs._data; }


//
// Literal operators for fixed-point number
//
namespace fixed_point_literals {


    constexpr FixedPoint::ValueType operator "" _fp(long double val) noexcept
    {
        using IFPType = internal::getFPSpec<FixedPoint::ValueType>::IFPType;

        return FixedPoint::ONE * static_cast<IFPType>(val) + ( (static_cast<IFPType>(val) >= 0.0f) ? 0.5f : -0.5f );
    }


    constexpr FixedPoint::ValueType operator "" _fp(unsigned long long int val) noexcept
    {
        return FixedPoint::ONE * static_cast<FixedPoint::ValueType>(val);
    }


    constexpr FixedPoint::ValueType operator "" _dfp(unsigned long long int val) noexcept
    {
        return (
                   FixedPoint::ONE * static_cast<FixedPoint::ValueType>(val)
                   +
                   ( (static_cast<FixedPoint::ValueType>(val) >= 0) ? 5 : -5 )
               ) / 10;
    }


    constexpr FixedPoint::ValueType operator "" _hfp(unsigned long long int val) noexcept
    {
        return (
                   FixedPoint::ONE * static_cast<FixedPoint::ValueType>(val)
                   +
                   ( (static_cast<FixedPoint::ValueType>(val) >= 0) ? 50 : -50 )
               ) / 100;
    }


    constexpr FixedPoint::ValueType operator "" _kfp(unsigned long long int val) noexcept
    {
        return (
                   FixedPoint::ONE * static_cast<FixedPoint::ValueType>(val)
                   +
                   ( (static_cast<FixedPoint::ValueType>(val) >= 0) ? 500 : -500 )
               ) / 1000;
    }


    constexpr FixedPoint::ValueType operator "" _dkfp(unsigned long long int val) noexcept
    {
        return (
                   FixedPoint::ONE * static_cast<FixedPoint::ValueType>(val)
                   +
                   ( (static_cast<FixedPoint::ValueType>(val) >= 0) ? 5000 : -5000 )
               ) / 10000;
    }


} // namespace fixed_point_literals


//
// Undefine the helper macros
//
#undef ECXX_FIXED_POINT_VALUE_TYPE
#undef ECXX_FIXED_POINT_CONST


} // namespace eCxx


#endif
