//----------------------------------------------------------------------------
//	Copyright (C) 2002, 2004, 2007 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 Monitor.cpp

	\author Michael Fiedler

	\date created at 2009/01/02

	\brief Implementation of class Monitor

	\sa Monitor.h

	\since 3.0
*/

#include <odemx/base/continuous/Monitor.h>
#include <odemx/base/Scheduler.h>
#include <odemx/base/Simulation.h>
#include <typeinfo>

using namespace odemx;
using namespace odemx::base;
using namespace odemx::base::continuous;
using namespace std;

#define MIN_MONITOR_ENLARGE( a ) a > 10 ? a : 10

//----------------------------------------------------------------------------------construction/destruction

Monitor::Monitor(Simulation& s, const data::Label& l, int variablesArraySize, MonitorObserver* o)
:	Process(s, l, o),
	workingMode(intervall),
	searchMode(linearization),
	variablesArraySize(variablesArraySize),
	variablesCount(0),
	variables(0),
	internalTime(0),
	timeLimit(0),
	timeLimitSet(false),
	hasToStop(false),
	diffTime(0.0),
	changed(true)
{
	ODEMX_TRACE << log( "create with sim" ).scope( typeid(Monitor) );

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

Monitor::Monitor(Simulation& s, const data::Label& l, int variablesArraySize, ODESolver* solver, MonitorObserver* o)
:	Process(s, l, o),
	workingMode(intervall),
	searchMode(linearization),
	variablesArraySize(variablesArraySize),
	variablesCount(0),
	variables(0),
	internalTime(0),
	timeLimit(0),
	timeLimitSet(false),
	hasToStop(false),
	diffTime(0.0),
	changed(true)
{
	setODESolver(solver);

	ODEMX_TRACE << log( "create with sim and solver" ).scope( typeid(Monitor) );

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

Monitor::~Monitor(){
	if (variables) {
		delete variables;
	}

	ODEMX_TRACE << log( "destroy" ).scope( typeid(Monitor) );
}

//------------------------------------------------------------------------------stop monitor
void Monitor::setTimeLimit(SimTime time) {
	timeLimitSet = true;
	timeLimit = time;
}

bool Monitor::getTimeLimit(SimTime* time) {
	if (timeLimitSet) {
		*time = timeLimit;
		return true;
	} else {
		return false;
	}
}

void Monitor::removeTimeLimit() {
	timeLimitSet = false;
}

void Monitor::stop() {
	hasToStop = true;
}

//-------------------------------------------------------------------------------eval system
void Monitor::evalF(double time) {
	this->variables->nullDerivatives();
	SimTime evalTime = (SimTime)(internalTime + time);
	std::list<Continuous*>::iterator iter;
	std::list<ODEObject*>::iterator iter2;

	for (iter = this->continuousList.begin(); iter != this->continuousList.end(); iter++)
		for (iter2 = (*iter)->equations.begin(); iter2 != (*iter)->equations.end(); iter2++)
			(*iter2)->derivates(evalTime);
}

void Monitor::evalJacobian(double time) {
	this->variables->nullJacobi();
	this->variables->nullDfDt();
	SimTime evalTime = (SimTime)(internalTime + time);
	std::list<Continuous*>::iterator iter;
	std::list<ODEObject*>::iterator iter2;
	for (iter = this->continuousList.begin(); iter != this->continuousList.end(); iter++)
		for (iter2 = (*iter)->equations.begin(); iter2 != (*iter)->equations.end(); iter2++)
			(*iter2)->jacobi(evalTime);
}

//--------------------------------------------------------------------------handle used solver
void Monitor::setODESolver(ODESolver *solver) {
	if (solver) {
		if (solver->monitor) {
			solver->monitor->setODESolver(0);
		}
		VariableContainer *newVariables = solver->getVariableContainer(variablesArraySize);
		int lv; //loop variable
		if (variables) {
			//copy from old container to new (only variables the rest depend on this)
			for (lv = 0; lv < variablesCount; lv++)
				newVariables->setVariable(lv, variables->getVariable(lv));
			//after copying clean old
			delete variables;
		}
		variables = newVariables;
		this->solver = solver;
		solver->monitor = this;
	} else {
		this->solver = 0;
		//keep variables to keep state
	}
	setChanged();
}

ODESolver* Monitor::getODESolver() {
	return solver;
}

//-----------------------------------------------------------add/remove Continuous and StateEvent
void Monitor::addContinuous(Continuous *continuous) {
	if (continuous != 0) { //only add real objects
		if (continuous->monitor != 0) { //delete from old Monitor
			continuous->monitor->removeContinuous(continuous);
		}
		//add to this Monitor
		continuous->monitor = this;
		continuous->baseIndex = this->variablesCount;
		this->continuousList.push_back(continuous);
		this->variablesCount += continuous->getDimension();
		int diffSize = this->variablesCount - this->variablesArraySize;
		if (diffSize > 0) {
			this->variablesArraySize += MIN_MONITOR_ENLARGE( diffSize );
			if (variables)
				variables->adaptSizeTo(this->variablesArraySize);
		}

		ODEMX_TRACE << log( "add continuous object" ).scope( typeid(Monitor) );

		// observer
		ODEMX_OBS(MonitorObserver, AddContinuous(this, continuous));
	}
	setChanged();
}

void Monitor::removeContinuous(Continuous *continuous) {
	//go to position..
	std::list<Continuous*>::iterator iter = find(this->continuousList.begin(),
												this->continuousList.end(), continuous);
	//..and delete
	if (*iter == continuous) {
		this->continuousList.erase(iter);

		ODEMX_TRACE << log( "remove continuous object" ).scope( typeid(Monitor) );

		// observer
		ODEMX_OBS(MonitorObserver, RemoveContinuous(this, continuous));
	}
	continuous->monitor = 0;
	setChanged();
}

void Monitor::addStateEvent(StateEvent *stateEvent) {
	if (stateEvent) { //only add real objects
		if (stateEvent->monitor) { //delete from old Monitor
			stateEvent->monitor->removeStateEvent(stateEvent);
		}
		//add to this Monitor
		stateEvent->monitor = this;
		this->stateEvents.push_back(stateEvent);
		if (stateEvent->condition(getActualInternalTime())) {
			//condition holds so trigger action
			stateEvent->action();
		}

		ODEMX_TRACE << log( "add StateEvent" ).scope( typeid(Monitor) );

		// observer
		ODEMX_OBS( MonitorObserver, AddStateEvent( this, stateEvent ) );

	}
}

void Monitor::removeStateEvent(StateEvent *stateEvent) {
	//go to position..
	std::list<StateEvent*>::iterator iter = find(this->stateEvents.begin(),
												this->stateEvents.end(), stateEvent);
	//..and delete
	if (*iter == stateEvent) {
		this->stateEvents.erase(iter);

		ODEMX_TRACE << log( "remove StateEvent" ).scope( typeid(Monitor) );

		// observer
		ODEMX_OBS( MonitorObserver, RemoveStateEvent( this, stateEvent ) );
	}
	stateEvent->monitor = 0;
}

//-----------------------------------------------------------add/remove process from synchronisation
void Monitor::addProcessToWaitList(Process *process) {
	if (process) { //only add real objects
		waitForProcesses.push_back(process);

		ODEMX_TRACE << log( "add process to list waitForProcesses" ).detail( "process", process ).scope( typeid(Monitor) );

		// observer
		ODEMX_OBS(MonitorObserver, AddProcessToWaitList(this, process));
	}
}

void Monitor::removeProcessFromWaitList(Process *process) {
	//go to position
	std::list<Process*>::iterator iter = find(waitForProcesses.begin(),
											  waitForProcesses.end(), process);
	//..and delete
	if (*iter == process) {
		waitForProcesses.erase(iter);

		ODEMX_TRACE << log( "remove process from list waitForProcesses" ).detail( "process", process ).scope( typeid(Monitor) );

		// observer
		ODEMX_OBS(MonitorObserver, RemoveProcessFromWaitList(this, process));
	}
}

//-----------------------------------------------------------------------------manage internal time
void Monitor::setInternalTime(SimTime time) {
	setChanged();
	internalTime = time;
	diffTime = 0.0;
	//check StateEvents to be sure conditions are false in this state or trigger action
	std::list<StateEvent*>::iterator iter;
	for (iter = this->stateEvents.begin(); iter != this->stateEvents.end(); iter++) {
		if ((*iter)->condition(time)) {
			(*iter)->action();
		}
	}
}

SimTime Monitor::getInternalTime() {
	return internalTime;
}

SimTime Monitor::getActualInternalTime() {
	return (SimTime)(internalTime + diffTime);
}

void Monitor::adaptInternalTime() {
	setChanged();
	internalTime = (SimTime)(internalTime + diffTime);
	diffTime = 0.0;
}

SimTime Monitor::getStepTimeLimit() {
	SimTime limit = waitForProcesses.front()->getExecutionTime();

	for (std::list<Process*>::iterator iter = waitForProcesses.begin(); iter != waitForProcesses.end(); ++iter) {
		Process* current = *iter;
		SimTime execTime = current->getExecutionTime();
		if(current->isScheduled() && execTime < limit) {
			limit = execTime;
		}
	}

	return limit;
}

//----------------------------------------------------------------------------main and integration
int Monitor::main() {
	while (!hasToStop && (!timeLimitSet || getActualInternalTime() < timeLimit) && solver != 0) {
		SimTime limit = timeLimit;

		// observer
		for (std::list<Continuous*>::iterator iter = this->continuousList.begin(); iter != this->continuousList.end(); ++iter) {
			std::cout << "Monitor::main(), continuousLis_size: " << this->continuousList.size() << endl;
			(*iter)->notifyValidState();
		}

		if (!waitForProcesses.empty()) {
			limit = getStepTimeLimit();
			if (timeLimitSet) {
				limit = limit < timeLimit ? limit : timeLimit;
			}
			integrateUntil(limit);
		} else if (timeLimitSet) {
			integrateUntil(limit);
		} else if (!stateEvents.empty()) { //only stops if an stateEvent occurs
			integrate();
		} else {
			//no limit for integration intervall, would result in infinite long working
			hasToStop = true;
		}
		holdUntil(getActualInternalTime());
	}
	return 0;
}

void Monitor::integrate() {

	if( getProcessState() != Process::CURRENT )
	{
		error << log( "Monitor::integrate(): object is not the current process" )
				.scope( typeid(Monitor) );
	}

	if (solver) {
		if (changed == true) {
			solver->init();
			changed = false;
		}

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

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

		bool testStateEvents = !stateEvents.empty(); //only to testing if necessary
		double old_diffTime, *old_variable_values, old_maxStep = solver->maxStep;
		int lv; //loop variable
		std::list<StateEvent*>::iterator iter;
		if (testStateEvents) {
			//init for first step of while loop
			old_diffTime = diffTime;
			old_variable_values = new double[ variablesCount ];
			for (lv = 0; lv < variablesCount; lv++)
				old_variable_values[lv] = variables->getVariable(lv);
		}
		while (testStateEvents) { //avoid infite loop
			//make an integration step
			solver->makeStep(diffTime);
			diffTime += solver->getStepLength();
			//test state events
			for (iter = this->stateEvents.begin(); iter != this->stateEvents.end(); iter++) {
				if ((*iter)->condition(getActualInternalTime())) {
					//search point of change and trigger action
					searchStateEvent(old_diffTime, old_variable_values);
					//stop integration
					break;
				}
			}
			//init for next step of while loop
			old_diffTime = diffTime;
			for (lv = 0; lv < variablesCount; lv++)
				old_variable_values[lv] = variables->getVariable(lv);

			if (workingMode == stepwise) break;
		}
		solver->setStepLength(solver->minStep, old_maxStep); //correct stepping limits to old
		if (testStateEvents) {
			delete[] old_variable_values;
		}

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

		// observer
		ODEMX_OBS(MonitorObserver, EndIntegrate(this));

	} else {
		//todo throw exception no solver assigned
	}
}

void Monitor::integrateUntil(SimTime time) {

	if( getProcessState() != Process::CURRENT )
	{
		error << log( "Monitor::integrateUntil(): object is not the current process" )
				.scope( typeid(Monitor) );
	}

	if (solver) {
		double expectedDiff = (double)(time - internalTime);
		if (changed == true) {
			solver->init();
			changed = false;
		}

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

		// observer
		ODEMX_OBS(MonitorObserver, BeginIntegrateUntil(this));

		bool testStateEvents = !stateEvents.empty(); //only to testing if necessary
		double old_diffTime, *old_variable_values, old_maxStep = solver->maxStep;
		int lv; //loop variable
		std::list<StateEvent*>::iterator iter;
		if (testStateEvents) {
			//init for first step of while loop
			old_diffTime = diffTime;
			old_variable_values = new double[ variablesCount ];
			for (lv = 0; lv < variablesCount; lv++)
				old_variable_values[lv] = variables->getVariable(lv);
		}
		while (diffTime <= expectedDiff) {
			if (expectedDiff - diffTime <= solver->maxStep) { //make sure intergation does not exceed the limit
				solver->setStepLength(solver->minStep, expectedDiff - diffTime);
			}
			//make an integration step
			solver->makeStep(diffTime);
			diffTime += solver->getStepLength();
			if (testStateEvents) {
				//test state events
				for (iter = this->stateEvents.begin(); iter != this->stateEvents.end(); iter++) {
					if ((*iter)->condition(getActualInternalTime())) {
						//search point of change and trigger action
						searchStateEvent(old_diffTime, old_variable_values);
						//stop integration
						break;
					}
				}
				//init for next step of while loop
				old_diffTime = diffTime;
				for (lv = 0; lv < variablesCount; lv++)
					old_variable_values[lv] = variables->getVariable(lv);
			}
			if (workingMode == stepwise) break;
		}
		solver->setStepLength(solver->minStep, old_maxStep); //correct stepping limits to old
		if (testStateEvents) {
			delete[] old_variable_values;
		}

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

		// observer
		ODEMX_OBS(MonitorObserver, EndIntegrateUntil(this));

	} else {
		//todo throw exception no solver assigned
	}
}

//------------------------------------------------------------------------------------------search occurance of StateEvents
void Monitor::searchStateEvent(double old_diffTime, double *old_variable_values) {
	double left = old_diffTime; //left time bound
	double right = diffTime; //right time bound
	double precision = solver->minStep; //determines how precise the search should be
	double half = (right - left) / 2;
	bool isLeft; //is true if at observed time at least one condition is true
	std::list<StateEvent*>::iterator iter;
	int lv; // loop variable
	//search time of first StateEvent
	switch (searchMode) {
		case direct: {
			while (right-left >= precision) {
				for (lv = 0; lv < variablesCount; lv++) //set initial condition for new shorter step at left
					variables->setVariable(lv, old_variable_values[lv]);
				solver->setStepLength(half, half); //set stepLength to be half
				//errorLimit is proven by greater step done before this function is called
				solver->makeStep(left);
				//check conditions
				isLeft = false;
				for (iter = this->stateEvents.begin(); iter != this->stateEvents.end(); iter++) {
					if ((*iter)->condition((SimTime)(internalTime + left + half)))
						isLeft = true;
				}
				//adapt search intervall
				if (isLeft == true) {
					right = left + half;
				} else {
					left += half;
					for (lv = 0; lv < variablesCount; lv++)
						old_variable_values[lv] = variables->getVariable(lv);
				}
				half = (right - left) / 2;
			}
			//correct diffTime
			diffTime = right;
		} break;
		case linearization:
		default: {
			double *right_values = new double[ variablesCount ];
			for (lv = 0; lv < variablesCount; lv++)
				right_values[lv] = variables->getVariable(lv);
			while (right-left >= precision) {
				for (lv = 0; lv < variablesCount; lv++) { //set linear interpolation value at half
					variables->setVariable(lv, (old_variable_values[lv] + right_values[lv]) / 2);
				}
				//check conditions
				isLeft = false;
				for (iter = this->stateEvents.begin(); iter != this->stateEvents.end(); iter++) {
					if ((*iter)->condition((SimTime)(internalTime + left + half)))
						 isLeft = true;
				}
				//adapt search intervall
				if (isLeft == true) {
					right = left + half;
					for (lv = 0; lv < variablesCount; lv++)
						right_values[lv] = variables->getVariable(lv);
				} else {
					left += half;
					for (lv = 0; lv < variablesCount; lv++)
						old_variable_values[lv] = variables->getVariable(lv);
				}
				half = (right - left) / 2;
			}
			delete[] right_values;
			//correct diffTime
			diffTime = right;
		}
	}
	//trigger events
	for (iter = this->stateEvents.begin(); iter != this->stateEvents.end(); iter++) {
		if ((*iter)->condition(getActualInternalTime())) {
			(*iter)->action();
		}
	}
}

//-------------------------------------------------------------------------------------------------others
void Monitor::setSearchMode(SearchModes mode) {
	searchMode = mode;
}

void Monitor::setChanged() {
	changed = true;
}
