/** @addtogroup solverCvode
 *
 *  @{
 */
#include <Core/ModelicaDefine.h>
#include <Core/Modelica.h>
#include <Solver/RTEuler/RTEuler.h>
#include <Solver/RTEuler/RTEulerSettings.h>


RTEuler::RTEuler(IMixedSystem* system, ISolverSettings* settings)
    : SolverDefaultImplementation(system, settings)
    , _eulerSettings    (dynamic_cast<ISolverSettings*>(_settings))
    , _z          (NULL)
    , _dimSys        (0)
    , _f          (NULL)
	, _zInit          (NULL)

{
}

RTEuler::~RTEuler()
{
    if(_z)
        delete [] _z;
    if(_f)
        delete [] _f;
    if(_zInit)
      delete [] _zInit;
}


void RTEuler::initialize()
{
    _properties = dynamic_cast<ISystemProperties*>(_system);
    _continuous_system = dynamic_cast<IContinuous*>(_system);
    _event_system =  dynamic_cast<IEvent*>(_system);
    _mixed_system =  dynamic_cast<IMixedSystem*>(_system);
    _time_system =  dynamic_cast<ITime*>(_system);

    _dimSys  = _continuous_system->getDimContinuousStates();



    //(Re-) Initialization of solver -> call default implementation service
	IGlobalSettings* globalsettings = _eulerSettings->getGlobalSettings();
	_h = globalsettings->gethOutput();

	if (_dimSys == 0)
		return;

	SolverDefaultImplementation::initialize();
    // Dimension of the system (number of variables)


    // Check system dimension

    //if(_dimSys <= 0 || !(_properties->isODE()))
    //{
    //    throw std::invalid_argument("Euler::assemble() error");
    //}


        // Allocate state vectors, stages and temporary arrays
	if(_z)        delete [] _z;
	if(_f)        delete [] _f;



	_z        = new double[_dimSys];
	_f         = new double[_dimSys];
	_zInit       = new double[_dimSys];







	memset(_z,0,_dimSys*sizeof(double));     //hier!!!
	memset(_zInit,0,_dimSys*sizeof(double));

	memset(_f,0,_dimSys*sizeof(double));



	_continuous_system->evaluateAll(IContinuous::CONTINUOUS);
	_continuous_system->getContinuousStates(_zInit);

	// Ensures that solver is started with right sign of zero function
	_zeroStatus = UNCHANGED_SIGN;

	memcpy(_z,_zInit,_dimSys*sizeof(double));



}

/// Set start t for numerical solution
void RTEuler::setStartTime(const double& t)
{
    SolverDefaultImplementation::setStartTime(t);
};

/// Set end t for numerical solution
void RTEuler::setEndTime(const double& t)
{
    SolverDefaultImplementation::setEndTime(t);
};

/// Set the initial step size (needed for reinitialization after external zero search)
void RTEuler::setInitStepSize(const double& h)
{
    SolverDefaultImplementation::setInitStepSize(h);
};


/// Provides the status of the solver after returning
ISolver::SOLVERSTATUS RTEuler::getSolverStatus()
{
    return (SolverDefaultImplementation::getSolverStatus());
};

bool RTEuler::stateSelection()
 {
   return SolverDefaultImplementation::stateSelection();
 }

void RTEuler::solve(const SOLVERCALL command)
{

  _continuous_system->stepStarted(_tCurrent);

  if(_dimSys > 0)
  {

  _continuous_system->getContinuousStates(_z);

  doRK1();
    //_totStps++;
    //_accStps++;
    //_tCurrent += _h; //time not required
   _continuous_system->setContinuousStates(_z);
  }


   _tCurrent += _h;
   _time_system->setTime(_tCurrent);
   _continuous_system->evaluateAll();

   _continuous_system->stepCompleted(_tCurrent);
}


void RTEuler::calcFunction(const double& t, const double* z, double* f)
{
    _time_system->setTime(t);
    _continuous_system->setContinuousStates(z);
    _continuous_system->evaluateODE();    // vxworksupdate
    _continuous_system->getRHS(f);
}


void RTEuler::writeSimulationInfo()
{
}

const int RTEuler::reportErrorMessage(ostream& messageStream)
{
    return 1;
}



void RTEuler::doRK1()
{

  calcFunction(_tCurrent, _z, _f);

  for(int i = 0; i < _dimSys; ++i)
    _z[i] += _h * _f[i];
}

void RTEuler::setTimeOut(unsigned int time_out)
{}
void RTEuler::stop()
{}
/** @} */ // end of solverRteuler
