#include <cmath>
#include <cassert>
#include <iostream>
#include <fenv.h>

#include "Float.h"

const float Float::FINF = std::numeric_limits<float>::infinity();
const double Float::DINF = std::numeric_limits<double>::infinity();

Float::Float( float fv, double dv, double l, double u ):
	fvalue( fv ),
	dvalue( dv ),
	lower_bound( l ),
	upper_bound( u )
{
}

Float::Float( float x ):
	fvalue( x ),
	dvalue( static_cast<double>( x ) )
{
	int expon = static_cast<int>( std::floor( log2( x ) ) );

	double delta = pow( 2.0, expon - 24 );

	lower_bound = dvalue - delta;
	upper_bound = dvalue + delta;
}

Float::Float( double x ):
	fvalue( static_cast<float>( x ) ),
	dvalue( static_cast<double>( static_cast<float>( x ) ) )
{
	int expon = static_cast<int>( std::floor( log2( x ) ) );

	double delta = pow( 2.0, expon - 24 );

	lower_bound = dvalue - delta;
	upper_bound = dvalue + delta;
}

Float::Float( int x ):
	fvalue( static_cast<float>( x ) ),
	dvalue( static_cast<double>( x ) ),
	lower_bound( static_cast<double>( x ) ),
	upper_bound( static_cast<double>( x ) )
{
	// empty
}

Float::Float( const Float &x ):
	fvalue( x.dvalue ),
	dvalue( x.dvalue ),
	lower_bound( x.lower_bound ),
	upper_bound( x.upper_bound )
{
	// empty
}

Float &Float::operator = ( const Float & x ) {
	fvalue = x.dvalue;
	dvalue = x.dvalue;
	lower_bound = x.lower_bound;
	upper_bound = x.upper_bound;

	return *this;
}

Float::operator double() const {
	return dvalue;
}

Float::operator float() const {
	return fvalue;
}

double Float::lower() const {
	return lower_bound;
}

double Float::upper() const {
	return upper_bound;
}

Float Float::operator + ( const Float & x ) const {
	int cround = fegetround();

	int set_ok = fesetround( FE_DOWNWARD );
	assert( set_ok == 0 );
	double new_lower_bound = lower_bound + x.lower_bound;

	set_ok = fesetround( FE_UPWARD );
	assert( set_ok == 0 );
	double new_upper_bound = upper_bound + x.upper_bound;

	set_ok = fesetround( cround );
	assert( set_ok == 0 );

	return Float(
		fvalue + x.fvalue,
		dvalue + x.dvalue,
		new_lower_bound,
		new_upper_bound
	);
}

Float &Float::operator += ( const Float & x ) {
	*this = *this + x;

	return *this;
}

Float Float::operator - ( const Float & x ) const {
	int cround = fegetround();

	int set_ok = fesetround( FE_DOWNWARD );
	assert( set_ok == 0 );
	double new_lower_bound = lower_bound - x.upper_bound;

	set_ok = fesetround( FE_UPWARD );
	assert( set_ok == 0 );
	double new_upper_bound = upper_bound - x.lower_bound;

	set_ok = fesetround( cround );
	assert( set_ok == 0 );

	return Float(
		fvalue - x.fvalue,
		dvalue - x.dvalue,
		new_lower_bound,
		new_upper_bound
	);
}

Float &Float::operator -= ( const Float & x ) {
	*this = *this - x;

	return *this;
}

Float Float::operator - () const {
	return Float(
		-fvalue,
		-dvalue,
		-upper_bound,
		-lower_bound
	);
}

Float Float::operator * ( const Float & x ) const {
	int cround = fegetround();
	int set_ok;

	double new_lower_bound, new_upper_bound;

	if ( lower_bound > 0 && x.lower_bound > 0 ) { 
		//   | [L, U]
		//   | [xL, xU] 
		//   | [L*xL, U*xU]

		set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = lower_bound * x.lower_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = upper_bound * x.upper_bound;
	} else if ( upper_bound < 0 && x.upper_bound < 0 ) { 
		//       [L, U] |
		//     [xL, xU] | 
		//              | [U*xU, L*xL]

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = upper_bound * x.upper_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = lower_bound * x.lower_bound;
	} else if ( lower_bound > 0 && x.upper_bound < 0 ) { 
		//              | [L, U]
		//     [xL, xU] | 
		// [U*xL, L*xU] |

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = upper_bound * x.lower_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = lower_bound * x.upper_bound;
	} else if ( upper_bound < 0 && x.lower_bound > 0 ) { 
		//       [L, U] | 
		//              | [xL, xU]
		// [L*xU, U*xL] |

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = lower_bound * x.upper_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = upper_bound * x.lower_bound;
	} else if ( lower_bound > 0 ) {
		//          | [L, U]
		//      [xL | xU]
		//    [U*xL | U*xU]

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = upper_bound * x.upper_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = upper_bound * x.lower_bound;
	} else if ( upper_bound < 0 ) {
		//   [L, U] |
		//      [xL | xU]
		//    [L*xU | L*xL]

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = lower_bound * x.upper_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = lower_bound * x.lower_bound;
	} else if ( x.lower_bound > 0 ) {
		//       [L | U]
		//          | [xL, xU]
		//    [L*xU | U*xU]

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = lower_bound * x.upper_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = upper_bound * x.upper_bound;
	} else if ( x.upper_bound < 0 ) {
		//       [L | U]
		// [xL, xU] |
		//    [U*xL | L*xL]

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = upper_bound * x.lower_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = lower_bound * x.lower_bound;
	} else if ( lower_bound < 0 && upper_bound > 0 && x.lower_bound < 0 && x.upper_bound > 0 ) {
		//       [L | U]
		//      [xL | xU]
		//    [U*xL | L*xL]

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = std::min( upper_bound * x.lower_bound, lower_bound * x.upper_bound );

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_lower_bound = std::min( upper_bound * x.upper_bound, lower_bound * x.lower_bound );
	} else {
		// Not yet implemented...
		// Need to deal correctly with +0 and -0
		new_lower_bound = DINF;
		new_upper_bound = DINF;
	}

	set_ok = fesetround( cround );
	assert( set_ok == 0 );

	return Float(
		fvalue * x.fvalue,
		dvalue * x.dvalue,
		new_lower_bound,
		new_upper_bound
	);
}

Float &Float::operator *= ( const Float & x ) {
	*this = *this * x;

	return *this;
}

Float Float::operator / ( const Float & x ) const {
	int cround = fegetround();
	int set_ok;

	double new_lower_bound, new_upper_bound;

	if ( lower_bound > 0 && x.lower_bound > 0 ) { 
		//   | [L, U]
		//   | [xL, xU] 
		//   | [L/xU, U/xL]

		set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = lower_bound / x.upper_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = upper_bound / x.lower_bound;
	} else if ( upper_bound < 0 && x.upper_bound < 0 ) { 
		//       [L, U] |
		//     [xL, xU] | 
		//              | [U/xL, L/xU]

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = upper_bound / x.lower_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = lower_bound / x.upper_bound;
	} else if ( lower_bound > 0 && x.upper_bound < 0 ) { 
		//              | [L, U]
		//     [xL, xU] | 
		// [U/xU, L/xL] |

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = upper_bound / x.upper_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = lower_bound / x.lower_bound;
	} else if ( upper_bound < 0 && x.lower_bound > 0 ) {
		//       [L, U] | 
		//              | [xL, xU]
		// [L*xL, U/xU] |

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = lower_bound / x.lower_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = upper_bound / x.upper_bound;
	} else if ( lower_bound > 0 ) {
		//          | [L, U]
		//      [xL | xU]
		//   (-infty, infty) 

		new_lower_bound = DINF;
		new_upper_bound = DINF;
	} else if ( upper_bound < 0 ) {
		//   [L, U] |
		//      [xL | xU]
		//   (-infty, infty) 

		new_lower_bound = DINF;
		new_upper_bound = DINF;
	} else if ( x.lower_bound > 0 ) {
		//       [L | U]
		//          | [xL, xU]
		//    [L/xL | U*/xL]

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = lower_bound / x.lower_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = upper_bound / x.lower_bound;
	} else if ( x.upper_bound < 0 ) {
		//       [L | U]
		// [xL, xU] |
		//    [U/xU | L*xU]

		int set_ok = fesetround( FE_DOWNWARD );
		assert( set_ok == 0 );
		new_lower_bound = upper_bound / x.upper_bound;

		set_ok = fesetround( FE_UPWARD );
		assert( set_ok == 0 );
		new_upper_bound = lower_bound * x.upper_bound;
	} else if ( lower_bound < 0 && upper_bound > 0 && x.lower_bound < 0 && x.upper_bound > 0 ) {
		//       [L | U]
		//      [xL | xU]
		//   (-infty, infty) 

		new_lower_bound = DINF;
		new_upper_bound = DINF;
	} else {
		// Not yet implemented...
		// Need to deal correctly with +0 and -0

		new_lower_bound = DINF;
		new_upper_bound = DINF;
	}

	set_ok = fesetround( cround );
	assert( set_ok == 0 );

	return Float(
		fvalue / x.fvalue,
		dvalue / x.dvalue,
		new_lower_bound,
		new_upper_bound
	);
}

Float &Float::operator /= ( const Float & x ) {
	*this = *this / x;

	return *this;
}

std::ostream &operator << ( std::ostream &out, const Float &x ) {
	out << x.fvalue << " ~ " << x.dvalue << " (" << x.lower_bound << ", " << x.upper_bound << ")";

	return out;
}