// ----------------------------------------------------------------------------------------
//
//  common/util/fixedpoint.inl
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//    
//  This file provides a fixed-point numbers for 8.8 and 16.16 variants.
//
//  Copyright (C) 2008 Bob Cook
//
//      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; either version 2 of the License, or (at your
//      option) any later version.
//    
//      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 
//      for more details.
//    
//      You should have received a copy of the GNU General Public License along
//      with this program; if not, write to the Free Software Foundation, Inc., 
//      51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
//
//  This implementation is best used with a compiler that intelligently de-inlines some
//  functions based on size. GCC 4.1.2 (WinAVR 20070525) with -Os is acceptable.
//
//  The specializations for 32-bit multiplication using only 32 bits was from an article
//  at Embedded.com available here:
//
//      http://www.embedded.com/showArticle.jhtml?articleID=15201575
//
//  The specialization for 32-bit division using only 32 bits was inspired by an article
//  at DSPRelated.com available here:
//
//      http://www.dsprelated.com/showmessage/42769/1.php
//
// ----------------------------------------------------------------------------------------


// ----------------------------------------------------------------------------------------

//template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
//fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >
//const T_BASE_TYPE fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::max_value
//	= ( 1 << ( sizeof( T_BASE_TYPE ) * 8 - T_DENOMINATOR_SIZE - 1 ) );
//		const T_BASE_TYPE min_value;

// ----------------------------------------------------------------------------------------

template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >
inline fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::fromRaw( T_BASE_TYPE raw )
{
    fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE > result;
    result.m_value = raw;
    return result;
}


// ----------------------------------------------------------------------------------------

template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
inline fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::fixedpoint()
    :   m_value()
{
}

        
// ----------------------------------------------------------------------------------------

template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
inline fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::fixedpoint( T_BASE_TYPE integer )
{
    m_value = integer << T_DENOMINATOR_SIZE;
}


// ----------------------------------------------------------------------------------------

template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
inline fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE > 
fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::operator+( const fixedpoint& value )
{
    return fromRaw( m_value + value.m_value );
}


// ----------------------------------------------------------------------------------------

template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
inline fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE > 
fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::operator-( const fixedpoint& value )
{
    return fromRaw( m_value - value.m_value );
}


// ----------------------------------------------------------------------------------------
//  specialization for fixedpoint< int16, 8 >
//

template<>
inline fixedpoint< int16_t, 8 > 
fixedpoint< int16_t, 8 >::operator*( const fixedpoint& value )
{
    int32_t result = m_value;
    result *= value.m_value;
    result >>= 8;
    return fromRaw( static_cast< int16_t >( result & 0x0ffff ) );
}


// ----------------------------------------------------------------------------------------
//  specialization for fixedpoint< int32, 16 >
//

template<>
inline fixedpoint< int32_t, 16 > 
fixedpoint< int32_t, 16 >::operator*( const fixedpoint& value )
{
    int32_t a = m_value;
    int8_t  sign = 1;
    
    if (a < 0)
    {
        sign = -1;
        a *= -1;
    }

    int32_t b = value.m_value;
    
    if (b < 0)
    {
        sign *= -1;
        b *= -1;
    }

    int32_t result;

    result = ( a & 0x0ffff ) * ( b & 0x0ffff );
    result += static_cast< int32_t >( 0x01 ) << 15; // T_DENOMINATOR_SIZE - 1
    result >>= 16; // T_DENOMINATOR_SIZE

    result += ( a >> 16 ) * ( b & 0x0ffff );
    // result += 0x01 << -1; // ( 16 - ( 32 - T_DENOMINATOR_SIZE ) ) - 1
    // result >>= 0;        // 16 - ( 32 - T_DENOMINATOR_SIZE )

    result += ( b >> 16 ) * ( a & 0x0ffff );
    // result += 0x01 << -1; // ( 16 - ( 32 - T_DENOMINATOR_SIZE ) ) - 1
    // result >>= 0;        // 16 - ( 32 - T_DENOMINATOR_SIZE )

    result += ( ( a >> 16 ) * ( b >> 16 ) ) << 16; // 32 - T_DENOMINATOR_SIZE

    return fromRaw( result * sign );
}


// ----------------------------------------------------------------------------------------
//  specialization for fixedpoint< int16, 8 >
//

template<>
inline fixedpoint< int16_t, 8 > 
fixedpoint< int16_t, 8 >::operator/( const fixedpoint& value )
{
    int32_t result = m_value;
    result <<= 8;
    result /= value.m_value;
    return fromRaw( static_cast< int16_t >( result & 0x0ffff ) );
}


// ----------------------------------------------------------------------------------------
//  specialization for fixedpoint< int32, 16 >
//

template<>
inline fixedpoint< int32_t, 16 > 
fixedpoint< int32_t, 16 >::operator/( const fixedpoint& value )
{
    int8_t   sign = 1;

    uint32_t divisor_hi;
    uint32_t divisor_lo = 0;

    if ( value.m_value < 0 )
    {
        divisor_hi = -1 * value.m_value;
        sign = -1;
    }
    else
    {
        divisor_hi = value.m_value;
    }

    uint32_t remainder_hi = 0;
    uint32_t remainder_lo;

    if ( m_value < 0 )
    {
        remainder_lo = -1 * m_value;
        sign *= -1;
    }
    else
    {
        remainder_lo = m_value;
    }

    uint32_t answer_hi = 0;
    uint32_t answer_lo  = 0;

    for ( int8_t n = 0; n < 32; n++ )
    {
        divisor_lo >>= 1;
        if ( divisor_hi & 0x01 )
        {
            divisor_lo |= 0x80000000;
        }
        divisor_hi >>= 1;

        if ( ( remainder_hi > divisor_hi )
            || ( ( remainder_hi == divisor_hi ) && ( remainder_lo >= divisor_lo ) ) )
        {
            answer_hi |= static_cast< uint32_t >( 1 ) << ( 31 - n );

            uint32_t diff = remainder_hi - divisor_hi;
            remainder_hi -= divisor_hi;
            remainder_lo -= divisor_lo;
            remainder_lo -= diff;
        }
    }

    for ( int8_t n = 0; n < 32; n++ )
    {
        remainder_hi <<= 1;
        if ( remainder_lo & 0x80000000 )
        {
            remainder_hi |= 0x01;
        }
        remainder_lo <<= 1;

        if ( ( remainder_hi > divisor_hi )
            || ( ( remainder_hi == divisor_hi ) && ( remainder_lo >= divisor_lo ) ) )
        {
            answer_lo |= static_cast< uint32_t >( 1 ) << ( 31 - n );

            uint32_t diff = remainder_hi - divisor_hi;
            remainder_hi -= divisor_hi;
            remainder_lo -= divisor_lo;
            remainder_lo -= diff;
        }
    }

    answer_lo >>= 16;
    answer_hi <<= 16;
    answer_lo |= answer_hi;

    int32_t answer = static_cast< int32_t >( answer_lo );
    answer *= sign;

    return fromRaw( answer );
}

        
// ----------------------------------------------------------------------------------------

template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
inline fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >& 
fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::operator+=( const fixedpoint& value )
{
	m_value += value.m_value;
	return *this;
}


// ----------------------------------------------------------------------------------------

template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
inline fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >& 
fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::operator-=( const fixedpoint& value )
{
	m_value -= value.m_value;
	return *this;
}


// ----------------------------------------------------------------------------------------

template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
inline fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >& 
fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::operator*=( const fixedpoint& value )
{
	m_value = fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >( *this * value ).asRaw();
	return *this;
}


// ----------------------------------------------------------------------------------------

template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
inline fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >& 
fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::operator/=( const fixedpoint& value )
{
	m_value = fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >( *this / value ).asRaw();
	return *this;
}


// ----------------------------------------------------------------------------------------

template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
inline T_BASE_TYPE fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::asInteger() const
{
    return ( m_value >> T_DENOMINATOR_SIZE );
}


// ----------------------------------------------------------------------------------------
//  specialization for fixedpoint< int16_t, 8 >
//

template<>
inline int16_t fixedpoint< int16_t, 8 >::asFractional() const
{
    return m_value & 0x0ff;
}


// ----------------------------------------------------------------------------------------
//  specialization for fixedpoint< int32_t, 16 >
//

template<>
inline int32_t fixedpoint< int32_t, 16 >::asFractional() const
{
    return m_value & 0x0ffff;
}


// ----------------------------------------------------------------------------------------

template< typename T_BASE_TYPE, unsigned T_DENOMINATOR_SIZE >
inline T_BASE_TYPE fixedpoint< T_BASE_TYPE, T_DENOMINATOR_SIZE >::asRaw() const
{
    return m_value;
}


// ----------------------------------------------------------------------------------------
//  end of fixedpoint.inl
// ----------------------------------------------------------------------------------------

