diff --git a/mbdynFMI.cc b/mbdynFMI.cc
index cb3a1bb..c006928 100644
--- a/mbdynFMI.cc
+++ b/mbdynFMI.cc
@@ -114,12 +114,12 @@ void fmu2::setCallBackFunction(){
 	callBackFunctions.componentEnvironment = 0;
 }
 
-void fmu2::ImportCreateDLL(int type){
+void fmu2::ImportCreateDLL(){
 
-	if(type == 1){
+	if(simType == 1){
 		jmstatus = fmi2_import_create_dllfmu(fmu, fmi2_fmu_kind_me, &callBackFunctions);
 	}
-	else if (type == 0){
+	else if (simType == 0){
 		jmstatus = fmi2_import_create_dllfmu(fmu, fmi2_fmu_kind_cs, &callBackFunctions);
 	}
 	else{
@@ -173,7 +173,7 @@ void fmu2::Initialize(double dTol, double time, double rTol){
 
 }
 
-bool fmu2::SupportsDirectionalDerivatives(int simType){
+bool fmu2::SupportsDirectionalDerivatives(){
 	unsigned int capability;
 
 	if(simType == 0){
@@ -195,7 +195,7 @@ bool fmu2::SupportsDirectionalDerivatives(int simType){
 
 }
 
-void fmu2::GetDirectionalDerivatives(double** jacobian, int* inputStatesVRef, int inputLength, double* seedVector){
+void fmu2::GetDirectionalDerivatives(FullMatrixHandler jacobian, int* inputStatesVRef, int inputLength, double* seedVector){
 //Seed Vector (the state value)
 
 	fmi2_import_variable_list_t* derivativeList = fmi2_import_get_derivatives_list(fmu);
@@ -224,7 +224,7 @@ void fmu2::GetDirectionalDerivatives(double** jacobian, int* inputStatesVRef, in
 		STATUSCHECK(fmistatus);
 
 		for (int j=0; j<numOfContStates + inputLength; j++){
-			jacobian[i][j] = output[j];
+			jacobian.PutCoef(i,j,output[j]);
 		}
 	}
 	delete[] derList;
@@ -295,6 +295,18 @@ fmu2::~fmu2(void){
 
 	fmi_import_free_context(context);
 
+	if (simType ==1 ){
+
+		fmi2_import_free_instance(fmu);
+		fmi2_import_free(fmu);
+
+	} else {
+
+		fmistatus = fmi2_import_terminate(fmu);
+		STATUSCHECK(fmistatus);
+		fmi2_import_free_instance(fmu);
+
+	}
 
 }
 
@@ -373,15 +385,9 @@ void fmu2::SetValuesByVariable(const std::string s, double value){
         STATUSCHECK(fmistatus);
 }
 
-void fmu2::Terminate(void){
-	fmi2_import_free_instance(fmu);
-
-	fmi2_import_free_instance(fmu);
-	fmi2_import_free(fmu);
-
-}
 
 void fmu2::InitializeAsSlave(const char* location, double tstart, double tend){
+
 	std::stringstream resourceLocation;
 	std::string strLocation = location;
 
@@ -390,7 +396,12 @@ void fmu2::InitializeAsSlave(const char* location, double tstart, double tend){
 	jmstatus = fmi2_import_instantiate(fmu, "Model for CS", fmi2_cosimulation, resourceLocation.str().c_str(), visible);
 	STATUSCHECK(jmstatus);
 
-	fmistatus = fmi2_import_setup_experiment(fmu, fmi2_true, relativeTolerance, tstart, fmi2_true, tend);
+	if (tend == std::numeric_limits<double>::max()){
+		fmistatus = fmi2_import_setup_experiment(fmu, fmi2_true, relativeTolerance, tstart, fmi2_false, 0);
+	} else {
+		fmistatus = fmi2_import_setup_experiment(fmu, fmi2_true, relativeTolerance, tstart, fmi2_true, tend);
+	}
+
 	STATUSCHECK(fmistatus);
 
         fmistatus = fmi2_import_enter_initialization_mode(fmu);
@@ -406,20 +417,36 @@ void fmu2::CSPropogate(double tcur, double dt){
 	STATUSCHECK(fmistatus);
 }
 
-void fmu2::TerminateSlave(void){
-	fmistatus = fmi2_import_terminate(fmu);
-	STATUSCHECK(fmistatus);
-        fmi2_import_free_instance(fmu);
-
-}
 
 fmu::~fmu(void){
 	NO_OP;
 }
 
 fmu1::~fmu1(void){
-
 	fmi_xml_free_context(context);
+
+	if(simType == 1){
+
+		fmi1_capi_free_dll(fmu->capi);
+
+		delete[] eventIndicators;
+		delete[] eventIndicatorsPrev;
+		//	delete[] currStates;
+
+		delete[] deriv;
+		delete[] vrs;
+
+		fmi1_import_free_model_instance(fmu);
+		fmi1_import_destroy_dllfmu(fmu);
+		fmi1_import_free(fmu);
+
+	} else {
+
+		fmistatus = fmi1_import_terminate_slave(fmu);
+		STATUSCHECK(fmistatus);
+		fmi1_import_free_slave_instance(fmu);
+
+	}
 }
 
 
@@ -440,7 +467,7 @@ void fmu1::setCallBackFunction(){
 	silent_cout("Callback Functions Set.\n");
 }
 
-void fmu1::ImportCreateDLL(int type){
+void fmu1::ImportCreateDLL(void){
 	jmstatus = fmi1_import_create_dllfmu(fmu, callBackFunctions, 1);
 	STATUSCHECK(jmstatus);
 	
@@ -604,11 +631,11 @@ double fmu1::GetStateFromRefValue(unsigned int i){
 	return realValue[0];
 } 
 
-bool fmu1::SupportsDirectionalDerivatives(int simType){
+bool fmu1::SupportsDirectionalDerivatives(){
 	return false;
 }
 
-void fmu1::GetDirectionalDerivatives(double** jacobian, int* vector, int vectorLength, double* seedVector){
+void fmu1::GetDirectionalDerivatives(FullMatrixHandler jacobian, int* vector, int vectorLength, double* seedVector){
 	NO_OP;
 }
 
@@ -637,35 +664,6 @@ void fmu1::CSPropogate(double tcur, double dt){
 }
 
 
-void fmu1::Terminate(void){
-
-
-	fmi1_capi_free_dll(fmu->capi);
-
-        delete[] eventIndicators;
-        delete[] eventIndicatorsPrev;
-//	delete[] currStates;
-
-	delete[] deriv;
-	delete[] vrs;
-
-        fmi1_import_free_model_instance(fmu);
-	fmi1_import_destroy_dllfmu(fmu);
-	fmi1_import_free(fmu);
-
-
-
-
-}
-
-void fmu1::TerminateSlave(void){
-
-        fmistatus = fmi1_import_terminate_slave(fmu);
-	STATUSCHECK(fmistatus);
-        fmi1_import_free_slave_instance(fmu);
-
-}
-
 void fmu1::GetStates(double* states){
 
 	fmistatus = fmi1_import_get_continuous_states(fmu, states, numOfContStates);
diff --git a/mbdynFMI.h b/mbdynFMI.h
index dfb9e88..d81fb80 100644
--- a/mbdynFMI.h
+++ b/mbdynFMI.h
@@ -24,6 +24,10 @@
 #include <FMI2/fmi2_import.h>
 #include <FMI/fmi_import_context.h>
 
+#include "mbconfig.h"
+#include "stepsol.h"
+ 
+
 //#include <fmi1_import_impl.h>
 //#include <fmi1_import_variable_list_impl.h>
 
@@ -32,7 +36,7 @@ class fmu {
         virtual void parseXML(fmi_import_context_t* context, const char* dirPath) = 0;
         virtual void setCallBackFunction() = 0;
 
-	virtual void ImportCreateDLL(int) = 0;
+	virtual void ImportCreateDLL(void) = 0;
 
 	virtual bool CheckInput(const std::string) = 0;
 
@@ -52,15 +56,14 @@ class fmu {
 	virtual double GetStateFromRefValue(unsigned int i) = 0;
 	virtual void GetStateDerivatives(double*) = 0;
 	virtual void GetStates(double*) = 0;
-	virtual void GetDirectionalDerivatives(double** ,int*, int, double *) = 0;
-	virtual bool SupportsDirectionalDerivatives(int) = 0;
+	virtual void GetDirectionalDerivatives(FullMatrixHandler ,int*, int, double *) = 0;
+	virtual bool SupportsDirectionalDerivatives() = 0;
 
 	virtual bool CheckInterrupts(double, double*) = 0;
-	virtual void Terminate(void) = 0;
 
 	virtual void InitializeAsSlave(const char*, double, double) = 0;
 	virtual void CSPropogate(double tcur, double dt) = 0;
-	virtual void TerminateSlave(void) = 0;
+
 
 	virtual ~fmu(void);
 };
@@ -84,24 +87,23 @@ class fmu1 :public fmu{
 	fmi1_import_variable_t* v;
 
 	int numOfContStates;
-	fmi1_real_t* currStates;
-	fmi1_real_t* currStatesDer;
 	fmi1_real_t* deriv;
         fmi1_value_reference_t *vrs;
 
 	fmi1_boolean_t intermediateResults;
 	fmi1_import_variable_list_t* vl;
 
-	int type; //1: IMPORT 2:COSIM
+	int simType; //1: IMPORT 2:COSIM
     public:
         void parseXML(fmi_import_context_t* context, const char* dirPath);
         void setCallBackFunction();
 
-	fmu1(fmi_import_context_t* text){
-		context= text;
+	fmu1(fmi_import_context_t* text, int type){
+		context = text;
+		simType    = type;
 	}
 
-	void ImportCreateDLL(int);
+	void ImportCreateDLL(void);
 
 	bool CheckInput(const std::string);
 
@@ -121,15 +123,13 @@ class fmu1 :public fmu{
 	double GetStateFromRefValue(unsigned int i);
 	void GetStateDerivatives(double*);
 	void GetStates(double *);
-	void GetDirectionalDerivatives(double** , int*, int, double*);
-	bool SupportsDirectionalDerivatives(int);
+	void GetDirectionalDerivatives(FullMatrixHandler, int*, int, double*);
+	bool SupportsDirectionalDerivatives();
 
 	bool CheckInterrupts(double, double*);
-	void Terminate(void);
 
 	void InitializeAsSlave(const char* location, double tstart, double tend);
 	void CSPropogate(double tcur, double dt);
-	void TerminateSlave(void);
 
 	virtual ~fmu1(void);
 
@@ -156,25 +156,23 @@ class fmu2 : public fmu{
 	fmi2_boolean_t terminateSimulation;
 
 	int numOfContStates;
-	fmi2_real_t* currStates;
-	fmi2_real_t* currStatesDer;
         fmi2_value_reference_t *vrs;
 
 	fmi2_boolean_t intermediateResults;
 	fmi2_import_variable_list_t* vl;
 
-	
+	int simType; //1: IMPORT 2:COSIM
 
-	int type; //1: IMPORT 2:COSIM
     public:
         void parseXML(fmi_import_context_t* context, const char* dirPath);
         void setCallBackFunction();
 
-	fmu2(fmi_import_context_t* text){
+	fmu2(fmi_import_context_t* text, int type){
 		context= text;
+		simType = type;
 	}
 
-	void ImportCreateDLL(int);
+	void ImportCreateDLL(void);
 
 	bool CheckInput(const std::string);
 
@@ -194,15 +192,13 @@ class fmu2 : public fmu{
 	double GetStateFromRefValue(unsigned int i);
 	void GetStateDerivatives(double*);
 	void GetStates(double *);
-	void GetDirectionalDerivatives(double** , int*, int, double*);
-	bool SupportsDirectionalDerivatives(int);
+	void GetDirectionalDerivatives(FullMatrixHandler , int*, int, double*);
+	bool SupportsDirectionalDerivatives();
 
 	bool CheckInterrupts(double, double*);
-	void Terminate(void);
 
 	void InitializeAsSlave(const char* , double, double);
 	void CSPropogate(double tcur, double dt);
-	void TerminateSlave(void);
 
 	virtual ~fmu2(void);
 };
diff --git a/mbdynFMI.o b/mbdynFMI.o
index 76dc84f..368a3f4 100644
Binary files a/mbdynFMI.o and b/mbdynFMI.o differ
diff --git a/module-FMU.cc b/module-FMU.cc
index 517631e..f410e20 100644
--- a/module-FMU.cc
+++ b/module-FMU.cc
@@ -92,24 +92,24 @@ pDM(pDM)
         version = fmi_import_get_fmi_version(context, FMUlocation, UClocation.c_str());
 
 	if(version == 1){
-		model = new fmu1(context);
+		model = new fmu1(context, SIMTYPE);
 	} else if (version == 2){
-		model = new fmu2(context);
+		model = new fmu2(context, SIMTYPE);
 	}
 
 	silent_cout("Version"<<version);
         model->parseXML(context, UClocation.c_str());
         model->setCallBackFunction();
-	model->ImportCreateDLL(SIMTYPE);
+	model->ImportCreateDLL();
 
 	currTime    = pDM->dGetTime();
 	initialTime = currTime;
-	endTime     = (pDM->GetSolver())->GetDFinalTime();
+	endTime     = (pDM->GetSolver())->dGetFinalTime();
 /// SIMTYPE specific work
 
 	if(SIMTYPE == IMPORT){
 
-		double dTol = pDM->GetSolver()->pGetStepIntegrator()->GetIntegratorDTol();
+		double dTol = pDM->GetSolver()->dGetTolerance();
 		model->Initialize(dTol, currTime, relativeTolerance);
 		model->EventIndicatorInit();
 		numOfContinousStates = model->GetNumOfContinousStates();
@@ -125,7 +125,7 @@ pDM(pDM)
 
 	currState           = new double[numOfContinousStates];
 	stateDerivatives    = new double[numOfContinousStates];
-	directionalFlag     = model->SupportsDirectionalDerivatives(SIMTYPE);
+	directionalFlag     = model->SupportsDirectionalDerivatives();
 	jacobianInputVector = new int[drivesContainer.size()];
 	privDriveLength = 0;
 	int k = 0;
@@ -152,39 +152,32 @@ pDM(pDM)
 	}
 	
 	if (directionalFlag){
-		jacobian = new double*[numOfContinousStates];
-		jacobian[0] = new double[(numOfContinousStates + privDriveLength)*numOfContinousStates];
-
-
 		seedVector = new double[numOfContinousStates + privDriveLength];
 	}
+
+	Jacobian.Resize(numOfContinousStates,numOfContinousStates + privDriveLength);
+	Jacobian.Reset();
+
 }
 
 ModuleFMU::~ModuleFMU(void)
 {
-	if (SIMTYPE == IMPORT){
-		model->Terminate();
-	}
-	else if (SIMTYPE == SIMTYPE){
-		model->TerminateSlave();
-	}
+	delete model;	
 
 	if(directionalFlag){
 
-		delete[] jacobian[0];
-		delete[] jacobian;
-
 		delete[] seedVector;
 	}
 
 
 	delete[] jacobianInputVector;
-	delete model;	
+
 	delete[] currState;
 	delete[] stateDerivatives;
 
 	for (strDriveCon::iterator i = drivesContainer.begin(); i != drivesContainer.end(); i++){
-		delete[] (i->second);
+		delete (i->second);
+		drivesContainer.erase(i->first);
 	}
 
 	drivesContainer.clear();
@@ -260,20 +253,20 @@ ModuleFMU::AssJac(VariableSubMatrixHandler& WorkMat,
 				seedVector[i+numOfContinousStates] = privDrivesIndex[i]->dGet();
 			}
 
-			model->GetDirectionalDerivatives(jacobian, jacobianInputVector, privDriveLength, seedVector);
+			model->GetDirectionalDerivatives(Jacobian, jacobianInputVector, privDriveLength, seedVector);
 			for (int i=0; i<numOfContinousStates; i++){
 				WM.IncCoef(i,i, 1);
 				for(int j=0; j<numOfContinousStates; j++){
-					WM.IncCoef(i,j, -dCoef*jacobian[i][j]);
+					WM.IncCoef(i,j, -dCoef*Jacobian.dGetCoef(i,j));
 				}
 				
 				for(int j=numOfContinousStates; j<numOfContinousStates + privDriveLength; j++){
 					if(privDrivesIndex[j-numOfContinousStates]->iGetSE()->
 						GetDofType(privDrivesIndex[i-numOfContinousStates]->iGetIndex()) 
 							== DofOrder::DIFFERENTIAL)
-						WM.IncCoef(i,j, -dCoef*jacobian[i][j]);
+						WM.IncCoef(i,j, -dCoef*Jacobian.dGetCoef(i,j));
 					else
-						WM.IncCoef(i,j, -jacobian[i][j]);
+						WM.IncCoef(i,j, -Jacobian.dGetCoef(i,j));
 				}
 			}
 
diff --git a/module-FMU.h b/module-FMU.h
index 12fba65..50fe500 100644
--- a/module-FMU.h
+++ b/module-FMU.h
@@ -39,12 +39,7 @@ class ModuleFMU
         jm_callbacks callbacks;
         jm_status_enu_t status;
 
-	enum SimulationTypes{
-		COSIM,
-		IMPORT
-	} SIMTYPE;
-
-
+	
         typedef std::map<std::string, const DriveCaller *> strDriveCon;
         typedef std::map<int, const PrivDriveCaller*> intDriveCon;
         strDriveCon drivesContainer;
@@ -61,8 +56,7 @@ class ModuleFMU
 	bool* privDriveArray;
 	int privDriveLength;
 
-	bool directionalFlag;
-	double **jacobian;
+	bool directionalFlag;	
 	double *seedVector;	
 
 public:
@@ -102,5 +96,11 @@ class ModuleFMU
 	DofOrder::Order GetDofType(unsigned int i) const;
 	DofOrder::Order GetEqType(unsigned int i) const;
 
+	enum SimulationTypes{
+		COSIM,
+		IMPORT
+	} SIMTYPE;
+
+	FullMatrixHandler Jacobian;
 };
 
diff --git a/module-FMU.o b/module-FMU.o
index e775b81..cf5d6ba 100644
Binary files a/module-FMU.o and b/module-FMU.o differ