///////////////////////////////////////////////////////////////////////////////////
// (C) Copyright 2007 Anthony Williams                                           //
//                                                                               //
// Distributed under the Boost Software License, Version 1.0.                    //
// See: http://www.boost.org/LICENSE_1_0.txt)                                    //
//                                                                               //
// Original article:                                                             //
// http://www.drdobbs.com/cpp/optimizing-math-intensive-applications-w/207000448 //
//                                                                               //
// Copyright (C) 2018 Edouard Griffiths, F4EXB                                   //
//                                                                               //
// Modified as fully templatized class with variable size and type internal      //
// representation                                                                //
//                                                                               //
// sqrt requires even IntBits                                                    //
//                                                                               //
// This program is free software; you can redistribute it and/or modify          //
// it under the terms of the GNU General Public License as published by          //
// the Free Software Foundation as version 3 of the License, or                  //
//                                                                               //
// This program is distributed in the hope that it will be useful,               //
// but WITHOUT ANY WARRANTY; without even the implied warranty of                //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the                  //
// GNU General Public License V3 for more details.                               //
//                                                                               //
// You should have received a copy of the GNU General Public License             //
// along with this program. If not, see .          //
///////////////////////////////////////////////////////////////////////////////////
#ifndef SDRBASE_UTIL_FIXED_H_
#define SDRBASE_UTIL_FIXED_H_
#include 
#include 
#include 
#include 
#include "fixedtraits.h"
template
class Fixed
{
private:
    IntType m_nVal;
    static int64_t scale_cordic_result(int64_t a);
    static int64_t right_shift(int64_t val,int shift);
    static void perform_cordic_rotation(int64_t&px, int64_t&py, int64_t theta);
    static void perform_cordic_polarization(int64_t& argx, int64_t&argy);
public:
    static const Fixed fixed_max;
    static const Fixed fixed_one;
    static const Fixed fixed_zero;
    static const Fixed fixed_half;
    static const Fixed fixed_pi;
    static const Fixed fixed_two_pi;
    static const Fixed fixed_half_pi;
    static const Fixed fixed_quarter_pi;
    struct internal
    {};
    Fixed():
        m_nVal(0)
    {}
    Fixed(internal, IntType nVal):
        m_nVal(nVal)
    {}
    Fixed(int64_t nVal):
        m_nVal(nVal << FixedTraits::fixed_resolution_shift)
    {}
    Fixed(int nVal):
        m_nVal(int64_t(nVal) << FixedTraits::fixed_resolution_shift)
    {}
    Fixed(short nVal):
        m_nVal(int64_t(nVal) << FixedTraits::fixed_resolution_shift)
    {}
    Fixed(uint64_t nVal):
        m_nVal(nVal << FixedTraits::fixed_resolution_shift)
    {}
    Fixed(unsigned int nVal):
        m_nVal(int64_t(nVal) << FixedTraits::fixed_resolution_shift)
    {}
    Fixed(unsigned short nVal):
        m_nVal(int64_t(nVal) << FixedTraits::fixed_resolution_shift)
    {}
    Fixed(double nVal):
        m_nVal(static_cast(nVal*static_cast(FixedTraits::fixed_resolution)))
    {}
    Fixed(float nVal):
        m_nVal(static_cast(nVal*static_cast(FixedTraits::fixed_resolution)))
    {}
    template
    Fixed& operator=(T other)
    {
        m_nVal = Fixed(other).m_nVal;
        return *this;
    }
    Fixed& operator=(Fixed const& other)
    {
        m_nVal = other.m_nVal;
        return *this;
    }
    friend bool operator==(Fixed const& lhs,Fixed const& rhs)
    {
        return lhs.m_nVal == rhs.m_nVal;
    }
    friend bool operator!=(Fixed const& lhs,Fixed const& rhs)
    {
        return lhs.m_nVal != rhs.m_nVal;
    }
    friend bool operator<(Fixed const& lhs,Fixed const& rhs)
    {
        return lhs.m_nVal < rhs.m_nVal;
    }
    friend bool operator>(Fixed const& lhs,Fixed const& rhs)
    {
        return lhs.m_nVal > rhs.m_nVal;
    }
    friend bool operator<=(Fixed const& lhs,Fixed const& rhs)
    {
        return lhs.m_nVal <= rhs.m_nVal;
    }
    friend bool operator>=(Fixed const& lhs,Fixed const& rhs)
    {
        return lhs.m_nVal >= rhs.m_nVal;
    }
    operator bool() const
    {
        return m_nVal ? true : false;
    }
    inline operator double() const
    {
        return as_double();
    }
    IntType as_internal() const
    {
        return m_nVal;
    }
    float as_float() const
    {
        return m_nVal / (float) FixedTraits::fixed_resolution;
    }
    double as_double() const
    {
        return m_nVal / (double) FixedTraits::fixed_resolution;
    }
    int64_t as_long() const
    {
        return (int64_t) (m_nVal / FixedTraits::fixed_resolution);
    }
    int64_t as_int64() const
    {
        return m_nVal / FixedTraits::fixed_resolution;
    }
    int as_int() const
    {
        return (int) (m_nVal / FixedTraits::fixed_resolution);
    }
    uint64_t as_unsigned_long() const
    {
        return (uint64_t) (m_nVal / FixedTraits::fixed_resolution);
    }
    uint64_t as_unsigned_int64() const
    {
        return (uint64_t) (m_nVal / FixedTraits::fixed_resolution);
    }
    unsigned int as_unsigned_int() const
    {
        return (unsigned int) (m_nVal / FixedTraits::fixed_resolution);
    }
    short as_short() const
    {
        return (short) (m_nVal / FixedTraits::fixed_resolution);
    }
    unsigned short as_unsigned_short() const
    {
        return (unsigned short) (m_nVal / FixedTraits::fixed_resolution);
    }
    Fixed operator++()
    {
        m_nVal += FixedTraits::fixed_resolution;
        return *this;
    }
    Fixed operator--()
    {
        m_nVal -= FixedTraits::fixed_resolution;
        return *this;
    }
    Fixed floor() const;
    Fixed ceil() const;
    Fixed sqrt() const;
    Fixed exp() const;
    Fixed log() const;
    Fixed& operator%=(Fixed const& other);
    Fixed& operator*=(Fixed const& val);
    Fixed& operator/=(Fixed const& val);
    Fixed& operator-=(Fixed const& val)
    {
        m_nVal -= val.m_nVal;
        return *this;
    }
    Fixed& operator+=(Fixed const& val)
    {
        m_nVal += val.m_nVal;
        return *this;
    }
    Fixed& operator*=(double val)
    {
        return (*this) *= Fixed(val);
    }
    Fixed& operator*=(float val)
    {
        return (*this) *= Fixed(val);
    }
    Fixed& operator*=(int64_t val)
    {
        m_nVal *= val;
        return *this;
    }
    Fixed& operator*=(int val)
    {
        m_nVal *= val;
        return *this;
    }
    Fixed& operator*=(short val)
    {
        m_nVal *= val;
        return *this;
    }
    Fixed& operator*=(char val)
    {
        m_nVal *= val;
        return *this;
    }
    Fixed& operator*=(uint64_t val)
    {
        m_nVal *= val;
        return *this;
    }
    Fixed& operator*=(unsigned int val)
    {
        m_nVal *= val;
        return *this;
    }
    Fixed& operator*=(unsigned short val)
    {
        m_nVal *= val;
        return *this;
    }
    Fixed& operator*=(unsigned char val)
    {
        m_nVal *= val;
        return *this;
    }
    Fixed& operator/=(double val)
    {
        return (*this) /= Fixed(val);
    }
    Fixed& operator/=(float val)
    {
        return (*this) /= Fixed(val);
    }
    Fixed& operator/=(int64_t val)
    {
        m_nVal /= val;
        return *this;
    }
    Fixed& operator/=(int val)
    {
        m_nVal /= val;
        return *this;
    }
    Fixed& operator/=(short val)
    {
        m_nVal /= val;
        return *this;
    }
    Fixed& operator/=(char val)
    {
        m_nVal /= val;
        return *this;
    }
    Fixed& operator/=(uint64_t val)
    {
        m_nVal /= val;
        return *this;
    }
    Fixed& operator/=(unsigned int val)
    {
        m_nVal/=val;
        return *this;
    }
    Fixed& operator/=(unsigned short val)
    {
        m_nVal /= val;
        return *this;
    }
    Fixed& operator/=(unsigned char val)
    {
        m_nVal /= val;
        return *this;
    }
    bool operator!() const
    {
        return m_nVal == 0;
    }
    Fixed modf(Fixed* integral_part) const;
    Fixed atan() const;
    static void sin_cos(Fixed const& theta,Fixed* s,Fixed*c);
    static void to_polar(Fixed const& x,Fixed const& y,Fixed* r,Fixed*theta);
    Fixed sin() const;
    Fixed cos() const;
    Fixed tan() const;
    Fixed operator-() const;
    Fixed abs() const;
};
/* why always as double ?
inline std::ostream& operator<<(std::ostream& os,fixed const& value)
{
    return os<
Fixed& Fixed::operator%=(Fixed const& other)
{
    m_nVal = m_nVal%other.m_nVal;
    return *this;
}
template
Fixed& Fixed::operator*=(Fixed const& val)
{
    bool const val_negative = val.m_nVal < 0;
    bool const this_negative = m_nVal < 0;
    bool const negate = val_negative ^ this_negative;
    uint64_t const other = val_negative ? -val.m_nVal : val.m_nVal;
    uint64_t const self = this_negative ? -m_nVal : m_nVal;
    if (uint64_t const self_upper = (self >> 32)) {
        m_nVal = (self_upper*other) << (32 - FixedTraits::fixed_resolution_shift);
    } else {
        m_nVal = 0;
    }
    if (uint64_t const self_lower = (self&0xffffffff))
    {
        uint64_t const other_upper = static_cast(other >> 32);
        uint64_t const other_lower = static_cast(other & 0xffffffff);
        uint64_t const lower_self_upper_other_res = self_lower*other_upper;
        uint64_t const lower_self_lower_other_res = self_lower*other_lower;
        m_nVal += (lower_self_upper_other_res << (32 - FixedTraits::fixed_resolution_shift))
            + (lower_self_lower_other_res >> FixedTraits::fixed_resolution_shift);
    }
    if (negate) {
        m_nVal=-m_nVal;
    }
    return *this;
}
template
Fixed& Fixed::operator/=(Fixed const& divisor)
{
    if( !divisor.m_nVal)
    {
        m_nVal = fixed_max.m_nVal;
    }
    else
    {
        bool const negate_this = (m_nVal < 0);
        bool const negate_divisor = (divisor.m_nVal < 0);
        bool const negate = negate_this ^ negate_divisor;
        uint64_t a = negate_this ? -m_nVal : m_nVal;
        uint64_t b = negate_divisor ? -divisor.m_nVal : divisor.m_nVal;
        uint64_t res = 0;
        uint64_t temp = b;
        bool const a_large = a > b;
        unsigned shift = FixedTraits::fixed_resolution_shift;
        if(a_large)
        {
            uint64_t const half_a = a>>1;
            while (temp < half_a)
            {
                temp <<= 1;
                ++shift;
            }
        }
        uint64_t d = 1LL << shift;
        if (a_large)
        {
            a -= temp;
            res += d;
        }
        while (a && temp && shift)
        {
            unsigned right_shift = 0;
            while(right_shift < shift && (temp > a))
            {
                temp >>= 1;
                ++right_shift;
            }
            d >>= right_shift;
            shift -= right_shift;
            a -= temp;
            res += d;
        }
        m_nVal = (negate ? -(int64_t) res : res);
    }
    return *this;
}
template
Fixed Fixed::sqrt() const
{
    unsigned int const max_shift = 62;
    uint64_t a_squared = 1LL << max_shift;
    unsigned int b_shift = (max_shift + FixedTraits::fixed_resolution_shift) / 2;
    uint64_t a = 1LL << b_shift;
    uint64_t x = m_nVal;
    while (b_shift && (a_squared > x))
    {
        a >>= 1;
        a_squared >>= 2;
        --b_shift;
    }
    uint64_t remainder = x - a_squared;
    --b_shift;
    while (remainder && b_shift)
    {
        uint64_t b_squared = 1LL << (2*b_shift - FixedTraits::fixed_resolution_shift);
        int const two_a_b_shift = b_shift + 1 - FixedTraits::fixed_resolution_shift;
        uint64_t two_a_b = (two_a_b_shift > 0) ? (a << two_a_b_shift) : (a >> -two_a_b_shift);
        while (b_shift && (remainder < (b_squared+two_a_b)))
        {
            b_squared >>= 2;
            two_a_b >>= 1;
            --b_shift;
        }
        uint64_t const delta = b_squared + two_a_b;
        if ((2*remainder) > delta)
        {
            a += (1LL << b_shift);
            remainder -= delta;
            if (b_shift) {
                --b_shift;
            }
        }
    }
    return Fixed(Fixed::internal(), a);
}
template
Fixed Fixed::exp() const
{
    if (m_nVal >= FixedTraits::log_two_power_n_reversed[0])
    {
        return fixed_max;
    }
    if (m_nVal < -FixedTraits::log_two_power_n_reversed[63 - 2*FixedTraits::fixed_resolution_shift])
    {
        return Fixed(internal(), 0);
    }
    if (!m_nVal)
    {
        return Fixed(internal(), FixedTraits::fixed_resolution);
    }
    int64_t res = FixedTraits::fixed_resolution;
    if (m_nVal > 0)
    {
        int power = FixedTraits::max_power;
        int64_t const* log_entry = FixedTraits::log_two_power_n_reversed;
        int64_t temp=m_nVal;
        while (temp && power>(-(int)FixedTraits::fixed_resolution_shift))
        {
            while (!power || (temp<*log_entry))
            {
                if (!power) {
                    log_entry = FixedTraits::log_one_plus_two_power_minus_n;
                } else {
                    ++log_entry;
                }
                --power;
            }
            temp -= *log_entry;
            if (power < 0) {
                res += (res >> (-power));
            } else {
                res <<= power;
            }
        }
    }
    else
    {
        int power = FixedTraits::fixed_resolution_shift;
        int64_t const* log_entry = FixedTraits::log_two_power_n_reversed + (FixedTraits::max_power-power);
        int64_t temp=m_nVal;
        while (temp && (power > (-(int) FixedTraits::fixed_resolution_shift)))
        {
            while (!power || (temp > (-*log_entry)))
            {
                if(!power) {
                    log_entry = FixedTraits::log_one_over_one_minus_two_power_minus_n;
                } else {
                    ++log_entry;
                }
                --power;
            }
            temp += *log_entry;
            if (power <0 ) {
                res -= (res >> (-power));
            } else {
                res >>= power;
            }
        }
    }
    return Fixed(Fixed::internal(), res);
}
template
Fixed Fixed::log() const
{
    if ( m_nVal <= 0) {
        return -fixed_max;
    }
    if (m_nVal == FixedTraits::fixed_resolution) {
        return fixed_zero;
    }
    uint64_t temp = m_nVal;
    int left_shift = 0;
    uint64_t const scale_position = 0x8000000000000000ULL;
    while (temp < scale_position)
    {
        ++left_shift;
        temp <<= 1;
    }
    int64_t res = (left_shift < FixedTraits::max_power) ?
            FixedTraits::log_two_power_n_reversed[left_shift] :
            -FixedTraits::log_two_power_n_reversed[2*FixedTraits::max_power - left_shift];
    unsigned int right_shift = 1;
    uint64_t shifted_temp = temp >> 1;
    while (temp && (right_shift < FixedTraits::fixed_resolution_shift))
    {
        while ((right_shift < FixedTraits::fixed_resolution_shift) && (temp < (shifted_temp + scale_position)))
        {
            shifted_temp >>= 1;
            ++right_shift;
        }
        temp -= shifted_temp;
        shifted_temp = temp >> right_shift;
        res += FixedTraits::log_one_over_one_minus_two_power_minus_n[right_shift-1];
    }
    return Fixed(Fixed::internal(), res);
}
template
int64_t Fixed::scale_cordic_result(int64_t a)
{
    int64_t const cordic_scale_factor = 0x22C2DD1C; /* 0.271572 * 2^31*/
    return (int64_t)((((int64_t)a)*cordic_scale_factor) >> 31);
}
template
int64_t Fixed::right_shift(int64_t val, int shift)
{
    return (shift < 0) ? (val << -shift) : (val >> shift);
}
template
void Fixed::perform_cordic_rotation(int64_t&px, int64_t&py, int64_t theta)
{
    int64_t x = px, y = py;
    int64_t const *arctanptr = FixedTraits::arctantab;
    for (int i = -1; i <= (int) FixedTraits::fixed_resolution_shift; ++i)
    {
        int64_t const yshift = right_shift(y,i);
        int64_t const xshift = right_shift(x,i);
        if (theta < 0)
        {
            x += yshift;
            y -= xshift;
            theta += *arctanptr++;
        }
        else
        {
            x -= yshift;
            y += xshift;
            theta -= *arctanptr++;
        }
    }
    px = scale_cordic_result(x);
    py = scale_cordic_result(y);
}
template
void Fixed::perform_cordic_polarization(int64_t& argx, int64_t&argy)
{
    int64_t theta = 0;
    int64_t x = argx, y = argy;
    int64_t const *arctanptr = FixedTraits::arctantab;
    for (int i = -1; i <= (int) FixedTraits::fixed_resolution_shift; ++i)
    {
        int64_t const yshift = right_shift(y,i);
        int64_t const xshift = right_shift(x,i);
        if(y < 0)
        {
            y += xshift;
            x -= yshift;
            theta -= *arctanptr++;
        }
        else
        {
            y -= xshift;
            x += yshift;
            theta += *arctanptr++;
        }
    }
    argx = scale_cordic_result(x);
    argy = theta;
}
template
void Fixed::sin_cos(Fixed const& theta, Fixed* s, Fixed*c)
{
    int64_t x = theta.m_nVal % FixedTraits::internal_two_pi;
    if (x < 0) {
        x += FixedTraits::internal_two_pi;
    }
    bool negate_cos = false;
    bool negate_sin = false;
    if (x > FixedTraits::internal_pi)
    {
        x = FixedTraits::internal_two_pi - x;
        negate_sin=true;
    }
    if (x > FixedTraits::internal_half_pi)
    {
        x = FixedTraits::internal_pi - x;
        negate_cos=true;
    }
    int64_t x_cos = 1<::fixed_resolution_shift, x_sin = 0;
    perform_cordic_rotation(x_cos, x_sin, (int64_t) x);
    if (s) {
        s->m_nVal = negate_sin ? -x_sin : x_sin;
    }
    if(c) {
        c->m_nVal = negate_cos ? -x_cos : x_cos;
    }
}
template
Fixed Fixed::atan() const
{
    Fixed r, theta;
    to_polar(1, *this, &r, &theta);
    return theta;
}
template
void Fixed::to_polar(Fixed const& x, Fixed const& y, Fixed* r, Fixed *theta)
{
    bool const negative_x = x.m_nVal < 0;
    bool const negative_y = y.m_nVal < 0;
    uint64_t a = negative_x ? -x.m_nVal : x.m_nVal;
    uint64_t b = negative_y ? -y.m_nVal : y.m_nVal;
    unsigned int right_shift = 0;
    unsigned const max_value = 1U << FixedTraits::fixed_resolution_shift;
    while ((a >= max_value) || (b >= max_value))
    {
        ++right_shift;
        a >>= 1;
        b >>= 1;
    }
    int64_t xtemp = (int64_t) a;
    int64_t ytemp = (int64_t) b;
    perform_cordic_polarization(xtemp, ytemp);
    r->m_nVal = int64_t(xtemp) << right_shift;
    theta->m_nVal = ytemp;
    if (negative_x && negative_y) {
        theta->m_nVal -= FixedTraits::internal_pi;
    } else if (negative_x) {
        theta->m_nVal = FixedTraits::internal_pi - theta->m_nVal;
    }
    else if(negative_y) {
        theta->m_nVal = -theta->m_nVal;
    }
}
template
inline Fixed operator-(double a, Fixed const& b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(float a, Fixed const& b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(uint64_t a, Fixed const& b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(int64_t a, Fixed const& b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(unsigned a, Fixed const& b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(int a, Fixed const& b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(unsigned short a, Fixed const& b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(short a, Fixed const& b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(unsigned char a, Fixed const& b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(char a, Fixed const& b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(Fixed const& a, double b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(Fixed const& a, float b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(Fixed const& a,uint64_t b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(Fixed const& a, int64_t b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(Fixed const& a, unsigned b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(Fixed const& a, int b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(Fixed const& a, unsigned short b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(Fixed const& a, short b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(Fixed const& a, unsigned char b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(Fixed const& a, char b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator-(Fixed const& a, Fixed const& b)
{
    Fixed temp(a);
    return temp -= b;
}
template
inline Fixed operator%(double a, Fixed const& b)
{
    Fixed temp(a);
    return temp %= b;
}
template
inline Fixed operator%(float a, Fixed const& b)
{
    Fixed temp(a);
    return temp %= b;
}
template
inline Fixed operator%(uint64_t a, Fixed const& b)
{
    Fixed temp(a);
    return temp %= b;
}
template
inline Fixed operator%(int64_t a, Fixed const& b)
{
    Fixed temp(a);
    return temp %= b;
}
template
inline Fixed operator%(unsigned a, Fixed const& b)
{
    Fixed temp(a);
    return temp %= b;
}
template
inline Fixed operator%(int a, Fixed const& b)
{
    Fixed temp(a);
    return temp %= b;
}
template
inline Fixed operator%(unsigned short a, Fixed const& b)
{
    Fixed temp(a);
    return temp %= b;
}
template
inline Fixed operator%(short a, Fixed const& b)
{
    Fixed temp(a);
    return temp %= b;
}
template
inline Fixed operator%(unsigned char a, Fixed const& b)
{
    Fixed temp(a);
    return temp %= b;
}
template
inline Fixed operator%(char a, Fixed const& b)
{
    Fixed temp(a);
    return temp %= b;
}
template
inline Fixed operator%(Fixed