//------------------------------------------------------------------------------
//	Copyright (C) 2003, 2004, 2009 Humboldt-Universitaet zu Berlin
//
//	This library is free software; you can redistribute it and/or
//	modify it under the terms of the GNU Lesser General Public
//	License as published by the Free Software Foundation; either
//	version 2.1 of the License, or (at your option) any later version.
//
//	This library 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
//	Lesser General Public License for more details.
//
//	You should have received a copy of the GNU Lesser General Public
//	License along with this library; if not, write to the Free Software
//	Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//------------------------------------------------------------------------------

/**
 * @file base/Continuous.cpp
 * @author Ralf Gerstenberger
 * @date created at 2003/06/15
 * @brief Implementation of odemx::base::Continuous
 * @sa Continuous.h
 * @since 1.0
 */

#include <odemx/base/Continuous.h>

#ifdef ODEMX_USE_CONTINUOUS

#include <fstream>
#include <iostream>
#include <iomanip>
#include <cmath>
#include <cassert>

namespace odemx {
namespace base {

Continuous::Continuous( Simulation& sim, const data::Label& label, int dimension,
		ContinuousObserver* obs )
:	Process( sim, label, obs )
,	data::Observable< ContinuousObserver >( obs )
,	state( dimension )
,	rate( dimension )
,	error_vector( dimension )
,	initial_state( dimension )
,	slope_1( dimension )
,	slope_2( dimension )
,	slope_3( dimension )
, dimension( dimension )
, stepLength( 0.01 )
, minStepLength( 0.01 )
, maxStepLength( 0.1 )
, relative( 0 )
, errorLimit( 0.1 )
, stopped( 0 )
, stopTime( HUGE_VAL )
, stopCond( 0 )
{
	ODEMX_TRACE << log( "create" ).scope( typeid(Continuous) );

	// observer
	ODEMX_OBS(ContinuousObserver, Create(this));
}


Continuous::~Continuous()
{
	ODEMX_TRACE << log( "destroy" );
}

unsigned int Continuous::getDimension() const
{
	return dimension;
}

void Continuous::interrupt()
{
	// stop integration
	stopped = 2;

	Process::interrupt();
}

void Continuous::setStepLength(double min, double max)
{
	if( ( min <= 0 ) || ( max <= 0 ) || ( min >= max ) )
	{
		error << log( "Continuous::setStepLength(): invalid step length boundaries" )
				.scope( typeid(Continuous) );

		if( min < 0 ) min = -min;
		if( max < 0 ) max = -max;
		if( min == 0 ) min = 0.01;
		if( max == 0 ) max = 0.1;
		if( min >= max) max *= 10.0;
	}

	minStepLength = min;
	maxStepLength = max;
}

double Continuous::getStepLength() const
{
	return stepLength;
}

void Continuous::setErrorlimit( int type, double limit )
{
	relative = type;
	errorLimit = limit;
}

void Continuous::addPeer( Process* newPeer )
{
	assert( newPeer );

	peers.push_back( newPeer );

	ODEMX_TRACE << log( "add peer" ).detail( "process", newPeer ).scope( typeid(Continuous) );

	// observer
	ODEMX_OBS(ContinuousObserver, AddPeer(this, newPeer));
}

void Continuous::delPeer( Process* peer )
{
	assert( peer );

	peers.remove( peer );

	ODEMX_TRACE << log( "remove peer" ).detail( "process", peer ).scope( typeid(Continuous) );

	// observer
	ODEMX_OBS(ContinuousObserver, RemovePeer(this, peer));
}

int Continuous::integrate( SimTime timeEvent, Condition stateEvent )
{
	if( getProcessState() != Process::CURRENT )
	{
		error << log( "Continuous::integrate(): object is not the current process" )
				.scope( typeid(Continuous) );
	}

	ODEMX_TRACE << log( "begin integrate" ).scope( typeid(Continuous) );

	// observer
	ODEMX_OBS(ContinuousObserver, BeginIntegrate(this));

	// initialise stop condition
	stopTime = timeEvent;
	if( stopTime == 0 )
	{
		stopTime = HUGE_VAL;
	}
	stopCond = stateEvent;

	stopped = 0;
	stepLength = maxStepLength;
	time = getTime();

	// reduce priority
	setPriority( getPriority() - 1 );

	while( ! stopIntegrate()
		   && getTime() < stopTime
		   && ! stopped )
	{
		// save initial state
		initial_state = state;

		// set step length
		if( stepLength > maxStepLength )
		{
			stepLength = maxStepLength;
		}

		// synchronise with timeEvent
		SimTime stepTime = getTime() + stepLength;
		if( stepTime > stopTime )
		{
			stepLength = stopTime - stepTime;
			stepTime = stopTime;
		}

		// synchronise with peers (reduce stepLength)
		ProcessList::iterator i;
		for( i = peers.begin(); i != peers.end(); ++i )
		{
			Process* p = *i;
			if( p->getProcessState() == Process::RUNNABLE )
			{
				SimTime et = p->getExecutionTime();
				if( stepTime > et )
				{
					stepTime = et;
				}
			}

			if ( stepTime < getTime() ) {
					error << log( "Continuous::integrate(): stepTime is less than current time. Setting to current time." )
							.scope( typeid (Continuous) );
					stepTime = getTime();
			}
		}
		stepLength = stepTime - getTime();

		// integrate
		takeAStep(stepLength);

		// handle integration errors
		while( reduce() )
		{
			// while integration error to great:

			// reduce stepLength
			stepLength /= 2.0;

			// reset state
			state = initial_state;

			// try again
			takeAStep( stepLength );
		}

		// handle state events
		if( stopIntegrate() )
		{
			// state event->Binary search for event time
			binSearch();
		}
		else
		{
			// waiting time
			time += stepLength;
			holdFor( stepLength );

			ODEMX_TRACE << log( "new valid state" ).scope( typeid(Continuous) );

			// observer
			ODEMX_OBS(ContinuousObserver, NewValidState(this));

			// try to increase stepLength
			stepLength *= 2.0;
		}
	} // end main while loop

	// reset priority
	setPriority( getPriority() + 1 );

	ODEMX_TRACE << log( "end integrate" ).scope( typeid(Continuous) );

	// observer
	ODEMX_OBS(ContinuousObserver, EndIntegrate(this, stopped));

	return stopped;
}

void Continuous::binSearch()
{
	// Binary search (BS)
	double sl = stepLength / 2.0; // step length for BS

	if( sl < minStepLength )
	{
		// step length already to small
		//->accept results
		stopped = 1;

		// waiting time
		time += stepLength;
		holdFor( stepLength );

		ODEMX_TRACE << log( "new valid state" ).scope( typeid(Continuous) );

		// observer
		ODEMX_OBS(ContinuousObserver, NewValidState(this));
	}
	else
	{
		state = initial_state; // reset state
		while( sl >= minStepLength )
		{
			// integrate
			takeAStep( sl );

			if( stopIntegrate() )
			{
				// step length still to long
				// reduce sl and try again
				sl /= 2.0;

				// reset state
				state = initial_state;
			}
			else
			{
				// accept step
				time += sl;
				holdFor( sl );

				ODEMX_TRACE << log( "new valid state" ).scope( typeid(Continuous) );

				// observer
				ODEMX_OBS(ContinuousObserver, NewValidState(this));

				initial_state = state;

				// and try to get closer to the state event
				sl /= 2.0;
				
			}
		}
		// final steps if conditions not reached
		// take several minimal steps until condition reached
		sl = minStepLength;
		while (!stopIntegrate()){
			takeAStep( sl );	
			time += sl;
			holdFor( sl );
			ODEMX_TRACE << log( "new valid state" ).scope( typeid(Continuous) );
			
			// observer
			ODEMX_OBS(ContinuousObserver, NewValidState(this));

			initial_state = state;			
		}
		stopped = 1;
	}
}

double Continuous::errorNorm()
{
	unsigned int i;
	double keep_value = 0.0;

	for( i = 0; i <= getDimension() - 1; i++ )
	{
		if( error_vector[i] > keep_value )
		{
			keep_value = error_vector[i];
		}
	}

	return keep_value;
}

int Continuous::reduce()
{
	error_value = errorNorm();

	if( error_value <= errorLimit )
	{
		return 0;
	}
	else
	{
		if( stepLength < ( 2.0 * minStepLength ) )
		{
			error << log( "Continuous::reduce(): error over limit, but can't reduce step length further" )
					.scope( typeid(Continuous) );

			return 0;
		}
		else
		{
			return 1;
		}
	}
	return 0;
}

void Continuous::takeAStep( double h )
{
	double hdiv2, hdiv3, hdiv6, hdiv8;
	int i;
	int n = getDimension() - 1;
	hdiv2 = h / 2.0;
	hdiv3 = h / 3.0;
	hdiv6 = h / 6.0;
	hdiv8 = h / 8.0;

	derivatives( time );
	for( i = 0; i <= n; i++ )
	{
		slope_1[i] = rate[i];
		state[i] = initial_state[i] + hdiv3 * slope_1[i];
	}

	derivatives( time + hdiv3 );
	for( i = 0; i <= n; i++ )
	{
		slope_2[i] = rate[i];
		state[i] = initial_state[i] + hdiv6 * ( slope_1[i] + slope_2[i] );
	}

	derivatives( time + hdiv3 );
	for( i = 0; i <= n; i++ )
	{
		slope_2[i] = rate[i];
		state[i] = initial_state[i] + hdiv8 * ( slope_1[i] + 3.0 * slope_2[i] );
	}

	derivatives( time + hdiv2 );
	for( i = 0; i <= n; i++ )
	{
		slope_3[i] = rate[i];
		state[i] = initial_state[i] + hdiv2 * ( slope_1[i] - 3.0 * slope_2[i] + 4.0 * slope_3[i] );
		error_vector[i]= state[i];
	}

	derivatives( time + h );
	for( i = 0; i <= n; i++ )
	{
		slope_2[i] = rate[i];
		state[i] = initial_state[i] + hdiv6 * ( slope_1[i] + 4.0 * slope_3[i] + slope_2[i] );

		if( relative )
		{
			if( error_vector[i] )
			{
				error_vector[i] = fabs( 0.2 * ( 1.0 - state[i] / error_vector[i] ) );
			}
		}
		else
		{
			error_vector[i] = fabs( 0.2 * ( error_vector[i] - state[i] ) );
		}
	}
}

bool Continuous::stopIntegrate()
{
	bool result = false;
	if( stopCond != 0 )
	{
		result = ( this->*stopCond )();
	}

	return result;
}

//--------------------------------------------------------------------ContuTrace

ContuTrace::ContuTrace( Continuous* contu, const std::string& fileName )
:	out( 0 )
,	firstTime( true )
{
	if( contu != 0 )
	{
		contu->data::Observable< ContinuousObserver >::addObserver( this );
	}

	if( ! fileName.empty() )
	{
		openFile( fileName );
	}
	else if( contu != 0 )
	{
		std::string fn = contu->getLabel();
		fn += "_trace.txt";
		openFile( fn );
	}
}

ContuTrace::~ContuTrace()
{
	if( out == 0 || out == &std::cout )
	{
		return;
	}

	static_cast< std::ofstream* >( out )->close();
	delete out;
}

void ContuTrace::onNewValidState( Continuous* sender )
{
	using namespace std;

	assert( sender != 0 );

	unsigned int i = 0;

	if( firstTime )
	{
		if( out == 0 )
		{
			std::string fn = sender->getLabel();
			fn += "_trace.txt";
			openFile( fn );
		}

		*out << endl;
		*out << setw(60) << "T R A C E   F I L E   F O R  " << sender->getLabel() << endl << endl;
		*out << setw(13) << "Time";
		*out << setw(13) << "Steplength";
		*out << setw(13) << "Error";

		for( i = 0; i < sender->getDimension(); ++i )
		{
			*out << setw(15) << "state[" << i << "]";
		}

		for( i = 0; i < sender->getDimension(); ++i )
		{
			*out << setw(15) << "rate[" << i << "]";
		}

		*out << endl;

		firstTime=false;
	}

	( *out ).setf( std::ios::showpoint );
	( *out ).setf( std::ios::fixed );
	*out << setw( 13 ) << setprecision( 7 ) << sender->getTime();
	*out << setw( 13 ) << setprecision( 7 ) << sender->getStepLength();
	*out << setw( 13 ) << setprecision( 7 ) << sender->error_value;

	for( i = 0; i < sender->getDimension(); i++ )
	{
		*out << setw( 17 ) << setprecision( 7 ) << sender->state[ i ];
	}

	for( i = 0; i < sender->getDimension(); i++ )
	{
		*out << setw(17) << setprecision(7) << sender->rate[i];
	}

	*out << endl;
}

void ContuTrace::openFile( const std::string& fileName )
{
	out = new std::ofstream( fileName.c_str() );
	if( out == 0 || !( *out ) )
	{
		// Error: cannot open file
		std::cerr << "ContuTrace::openFile(): cannot open file; sending output to stdout." << std::endl;
		out = &std::cout;
	}
}

} } // namespace odemx::base

#endif /* ODEMX_USE_CONTINUOUS */
