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

/*
 * ----------------------------------------------------------------------------------
 * Most of the code in this file is copied (with a few modifications) from:
 *     https://github.com/andrewray/mirage-kfreebsd/blob/master/fixpt/fixedptc.h
 * ----------------------------------------------------------------------------------
 * Copyright (C) 2010-2012 Ivan Voras   < ivoras@freebsd.org >
 * Copyright (C) 2012      Tim Hartrick < tim@edgecast.com   >
 * Copyright (C) 2013      Gabor Pali   < pgj@freebsd.org    >
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *     1. Redistributions of source code must retain the above copyright notice,
 *        this list of conditions and the following disclaimer.
 *     2. Redistributions in binary form must reproduce the above copyright notice,
 *        this list of conditions and the following disclaimer in the documentation
 *        and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
 * SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 * ----------------------------------------------------------------------------------
 */


#include <eCxx/CTL/FixedPoint.h>


namespace eCxx {


#if 0
//
// Sign extension
//
int32_t signExtend24To32(int32_t input)
{
    const int32_t value = 0x00FFFFFFL & input;
    const int32_t mask  = 0x00800000L;

    return (mask & input) ? (value + 0xFF000000L) : value;
}

uint64_t signExtend48To64(uint64_t input)
{
    const uint64_t value = 0x0000FFFFFFFFFFFFLL & input;
    const uint64_t mask  = 0x0000800000800000LL;

    return (mask & input) ? (value + 0xFFFF000000000000LL) : value;
}
#endif


//
// A simple fixed-point number
//
void FixedPoint::decomposeToDecimal(bool& negative, UnsignedHalfValueType& integer, UnsignedQuartValueType& fractionalScaled) const noexcept
{
    // Sign extend the number
    ValueType value = eCxx::signExtend<ValueType, TotalBits>(_data);

    // Check if negative
    negative = (value < 0);
    if(negative) value = -value;

    // Decompose
    integer          = value >> FractionalBits;
    fractionalScaled = ( ( value - (integer << FractionalBits) ) * FractionalDSF ) >> FractionalBits;

#if 1
    // Increase the precision a bit
    const UnsignedQuartValueType tmp = ( ( value - (integer << FractionalBits) ) * FractionalDSF * 10 ) >> FractionalBits;
    if( (tmp % 10) >= 5 ) {
        if(fractionalScaled < FractionalDSF - 1) {
            ++fractionalScaled;
        }
        else {
            fractionalScaled = 0;
            ++integer;
        }
    }
#endif
}


FixedPoint& FixedPoint::sqrt() noexcept
{
    // Return error for negative numbers
    if(_data < 0) {
        _data = -1;
        return *this;
    }

    // Check for zero and one
    if( !_data || _data == ONE ) return *this;

    // Invert the value as needed
    const bool invert = (_data < ONE) && (_data > 6);

    if(invert) _data = (ONE << FractionalBits) / _data;

    // Calculate the optimal number of iterations
    SizeType numOfIterations = FractionalBits;

    if(_data > ONE) {
        numOfIterations = 0;
        for(ValueType i = _data; i > 0; i = i >> 2) ++numOfIterations;
    }

    // Execute Newton-Raphson method
    ValueType result = (_data >> 1) + 1;

    for(SizeType i = 0; i < numOfIterations; ++i) {
        result = ( result + (_data << FractionalBits) / result ) >> 1;
    }

    // Invert the value as needed
    _data = invert ? ( (ONE << FractionalBits) / result )
                   : result;

    // Done
    return *this;
}


FixedPoint& FixedPoint::exp() noexcept
{
    // Lookup table
    constexpr ValueType ep[5] = {
        fpConst( 1.6666667e-01),
        fpConst(-2.7777778e-03),
        fpConst( 6.6137563e-05),
        fpConst(-1.6533902e-06),
        fpConst( 4.1381368e-08)
    };

    // Check for zero
    if( !_data ) {
        _data = ONE;
        return *this;
    }

    // Calculate the exponentiation
    ValueType k = ( ( (_data < 0) ? -_data : _data ) * INV_LN_TWO) >> FractionalBits;

    k += HALF;
    k &= ~FractionalMask;
    if(_data < 0) k = -k;

    _data -= (k * LN_TWO) >> FractionalBits;

    k = (k < 0) ? ( ONE >> (-k >> FractionalBits) )
                : ( ONE << ( k >> FractionalBits) );

    const ValueType s = (_data * _data) >> FractionalBits;
    const ValueType a = (s * ep[4]    ) >> FractionalBits;
    const ValueType b = (s * ep[3] + a) >> FractionalBits;
    const ValueType c = (s * ep[2] + b) >> FractionalBits;
    const ValueType d = (s * ep[1] + c) >> FractionalBits;
    const ValueType e = (s * ep[0] + d) >> FractionalBits;
    const ValueType f = ONE + ( (_data * TWO) & ~FractionalMask ) / (TWO + e - _data);

    _data = (k * f) >> FractionalBits;

    // Done
    return *this;
}


FixedPoint& FixedPoint::ln() noexcept
{
    // Lookup table
    constexpr ValueType lg[7] = {
        fpConst(6.6666667e-01),
        fpConst(3.9999999e-01),
        fpConst(2.8571429e-01),
        fpConst(2.2222198e-01),
        fpConst(1.8183572e-01),
        fpConst(1.5313838e-01),
        fpConst(1.4798199e-01)
    };

    // Return error for negative numbers and zero
    if(_data <= 0) {
        _data = -1;
        return *this;
    }

    // Calculate the natural logarithm
    ValueType log2 = 0;
    ValueType xi   = _data;

    while(xi > TWO) {
        xi = xi >> 1;
        ++log2;
    }

    const ValueType x = xi - ONE;
    const ValueType s = (x << FractionalBits) / (TWO + x);
    const ValueType z = (s * s) >> FractionalBits;
    const ValueType w = (z * z) >> FractionalBits;

    const ValueType a = (w *   lg[5]       ) >> FractionalBits;
    const ValueType b = (w * ( lg[3] + a ) ) >> FractionalBits;
    const ValueType c = (w * ( lg[1] + b ) ) >> FractionalBits;
    const ValueType d = (w *   lg[6]       ) >> FractionalBits;
    const ValueType e = (w * ( lg[4] + d ) ) >> FractionalBits;
    const ValueType f = (w * ( lg[2] + e ) ) >> FractionalBits;
    const ValueType g = (w * ( lg[0] + f ) ) >> FractionalBits;

    _data = ( ( LN_TWO * (log2 << FractionalBits) ) >> FractionalBits ) + x - ( ( s * (x - (c + g) ) ) >> FractionalBits );

    // Done
    return *this;
}


FixedPoint& FixedPoint::log(uint8_t base) noexcept
{
    _data = ( ln()._data << FractionalBits )
          / FixedPoint( static_cast<ValueType>(base) << FractionalBits ).ln()._data;

    return *this;
}


FixedPoint& FixedPoint::pow(uint8_t power) noexcept
{
    // Check for zero
    if( !power ) {
        _data = ONE;
        return *this;
    }

    // Calculate the pow
    const ValueType value = _data;

    for(uint8_t i = 1; i < power; ++i) {
        _data = (_data * value) >> FractionalBits;
    }

    // Done
    return *this;
}


FixedPoint& FixedPoint::sin() noexcept
{
    // Lookup table
    constexpr ValueType sk[2] = {
        fpConst(7.6100e-03),
        fpConst(1.6605e-01)
    };

    // Calculate the sine
    bool negative = false;

    _data = _data % (2 * PI);

    if(_data < 0) _data = PI * 2 + _data;

    if( (_data > HALF_PI) && (_data <= HALF_PI) ) {
        _data = PI - _data;
    }
    else if( (_data > PI) && ( _data <= (PI + HALF_PI) ) ) {
        _data    = _data - PI;
        negative = true;
    }
    else if( _data > (PI + HALF_PI) ) {
        _data    = (PI << 1) - _data;
        negative = true;
    }

    const ValueType sqr = ( (_data * _data) >> FractionalBits );
    const ValueType a   = ( (sk[0] * sqr  ) >> FractionalBits ) - sk[1];
    const ValueType b   = ( (a     * sqr  ) >> FractionalBits ) + ONE;
    const ValueType c   = ( (b     * _data) >> FractionalBits );

    _data = negative ? -c : c;

    // Done
    return *this;
}


FixedPoint& FixedPoint::cos() noexcept
{
    _data = HALF_PI - _data;

    return sin();
}


FixedPoint& FixedPoint::tan() noexcept
{
    _data = ( FixedPoint(_data).sin() / FixedPoint(_data).cos() )._data;

    return *this;
}


} // namespace eCxx
