/*
 * main.c
 *
 *  Created on: 13. 4. 2018
 *      Author: Lukas Malik
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
/* C API headers */

#include <unistd.h>
#include "NiFpga_FPGA.h"
//#include <stdio.h>

/* FMI2 standard headers */
#include "fmi2Functions.h"
#include "fmi2FunctionTypes.h"
#include "fmi2TypesPlatform.h"
#include "fmu2_model_interface.h"

/* User defined variables*/
fmi2ValueReference vr_i_input [3] = {0,0,0};
fmi2Integer i_input [3] = {0,0,0};
int i = 0;
fmi2Real startTime, currMasterTime = 0;		// simulation start time, current master time
fmi2Real stopTime = 1;		// simulation stop time
fmi2Real h = 0.1;			// default communication step size
fmi2Status simStat;			// actual simulation state
fmi2Boolean boolVal;		// auxiliary variable
bool terminateSimulation = false;	// slaves variable indicates that he want terminate simulation

/* User defined functions */
const char * get_status_string( fmi2Status status )
{
	switch ( status ) {
		case fmi2OK:
			return( "fmi2OK" );
		case fmi2Warning:
			return( "fmi2Warning" );
		case fmi2Discard:
			return( "fmi2Discard" );
		case fmi2Error:
			return( "fmi2Error" );
		case fmi2Fatal:
			return( "fmi2Fatal" );
		case fmi2Pending:
			return( "fmi2Pending" );
	}
	return( "Unknown" );
}

void simple_logger(
   fmi2ComponentEnvironment env,
   fmi2String               instance_name,
   fmi2Status               status,
   fmi2String               category_name,
   fmi2String               message,
                            ...            )
{
   /* Declare and initialize the variable arguments list: valist. */
   va_list valist;
   va_start( valist, message );

   printf( "FMU Model: %s : %s : %s : ", instance_name, get_status_string(status), category_name     );
   vprintf( message, valist );
   printf( "\n" );

   /* We're done with valist. */
   va_end(valist);

   return;
}

int main(int argc, char *argv[]){
	printf("\n********** HERE IS YOUR FMU **********\n");
	printf("Platform of Header Files: %s\n",fmi2GetTypesPlatform());
	printf("Version Number of Header Files: %s\n", fmi2GetVersion());

///////////////////////////////////////////////////////////////////////////////////////////////
	printf("C API\n");

	NiFpga_Session session;

	/* Load FPGA interface library */
	NiFpga_Status status = NiFpga_Initialize();

	/* Open a session with FPGA, download and run the bitstream on the FPGA and store any error info in "status" */
	NiFpga_MergeStatus(&status, NiFpga_Open("/home/admin/ProjectsMalik/NiFpga_FPGA.lvbitx",
				NiFpga_FPGA_Signature,
				"RIO0",
				0,
				&session));

	/* Initialize variables that control the application behavior */
	NiFpga_Bool button_pressed = 0;		/* button is not pushed */
	NiFpga_Bool LED = 0;				/* LED is OFF */


	NiFpga_MergeStatus(&status, NiFpga_WriteBool(	session,
				NiFpga_FPGA_ControlBool_Boolean,
				LED));
	/* users code */
	usleep(1000000); /* 1 second */
	LED = 1;
	NiFpga_MergeStatus(&status, NiFpga_WriteBool(	session,
				NiFpga_FPGA_ControlBool_Boolean,
				LED));
	usleep(1000000); /* 1 second */
	LED = 0;
	NiFpga_MergeStatus(&status, NiFpga_WriteBool(	session,
				NiFpga_FPGA_ControlBool_Boolean,
				LED));
	usleep(1000000); /* 1 second */

	/* while loop checking the error status */
	while(NiFpga_IsNotError(status)){

		NiFpga_MergeStatus(&status, NiFpga_ReadBool(session,
				NiFpga_FPGA_IndicatorBool_Buttonoutput,
				&button_pressed));

		if (button_pressed){
			LED = 1;
		}
		else {
			LED = 0;
		}

		NiFpga_MergeStatus(&status, NiFpga_WriteBool(	session,
					NiFpga_FPGA_ControlBool_Boolean,
					LED));
		usleep(100000); /* 100ms delay */
	}

	/* close the FPGA session */
	NiFpga_MergeStatus(&status, NiFpga_Close(session, 0));

	/* unloading C API library - call it before exiting and after closing FPGA session */
	NiFpga_MergeStatus(&status, NiFpga_Finalize());
	printf("C API END\n");

///////////////////////////////////////////////////////////////////////////////////////////////
	printf("----------- Initialization sub-phase -------------\n");
	printf("Instantiation slave\n");
	fmi2CallbackFunctions cbf ={
			.logger = simple_logger,
			.allocateMemory = calloc,
			.freeMemory = free,
			.stepFinished = NULL 		//synchronous execution
	};
	fmi2Component s1 = fmi2Instantiate(	"Sumator",
										fmi2CoSimulation,
										"{1c302156-9f89-4e47-989e-82f4cd9069f8}",
										"",
										&cbf,
										fmi2True,
										fmi2True);	// creation new FMU instance
	if(s1 == NULL){
		printf("Instantiation failed!\n");
		return -1;
	}
	else
		printf("Slave was successfully instantiated\n");

	printf("Initializing slave\n");

	// Simulation Start and Stop time
	//startTime = 0;
	//stopTime = 0.2;

	// Communication step size other then default 100ms
	//h = 0.01;

	// Set all variable start values (of "ScalarVariable / <type> / start")

	// Initialize slave
	fmi2SetupExperiment(s1, fmi2True, 0.0, startTime, fmi2True, stopTime);

	// Enter initialization mode
	fmi2EnterInitializationMode(s1);

	// Set the input values at time = startTime
	for (i = 0; i<2; i++){
		vr_i_input[i] = i;
		i_input[i] = i+1;
		fmi2SetInteger(s1, &vr_i_input[i], 1, &i_input[i]);
	}
	// Get the output values at time = startTime
	for (i = 2; i<3; i++){
		vr_i_input[i] = i;
		fmi2GetInteger(s1, &vr_i_input[i], 1, &i_input[i]);
	}

	fmi2ExitInitializationMode(s1);
///////////////////////////////////////////////////////////////////////////////////////////////
	printf("----------- Simulation sub-phase -------------\n");

/*
printf("toto to obsahuje\n");
	vr_i_input[0] = 0;
	vr_i_input[1] = 1;
	vr_i_input[2] = 2;
	fmi2GetInteger(s1, &vr_i_input, 3, &i_input);
	printf("toto tam zapisuju\n");
	i_input[0] = 2;
	i_input[1] = 3;
	i_input[2] = 0;
	fmi2SetInteger(s1,&vr_i_input, 2, &i_input);
	printf("toto jsem tam zapsal\n");
	fmi2GetInteger(s1, &vr_i_input[2], 1, &i_input[2]);
*/

	currMasterTime = startTime;
	i = 0;
	while((currMasterTime < stopTime) && (simStat == fmi2OK)){
		i++;
		if (i == 3) // 1,2,3
			i = 0;

		// Retrieve outputs
		vr_i_input[2] = 2;	// position in field
		fmi2GetInteger(s1, &vr_i_input[2], 1, &i_input[2]);

		// Set inputs
		vr_i_input[1] = 1;	// position in field
		i_input[1] = i+1; 	// value on position in field
		fmi2SetInteger(s1, &vr_i_input[1], 1, &i_input[1]);

		// Calling slave 1 and returned status check
		simStat = fmi2DoStep(s1, currMasterTime, h, fmi2False);
		switch(simStat){
		case fmi2OK:
			break;
		case fmi2Warning:
			printf("Some warning occurred.\n");
			break;
		case fmi2Pending:
			printf("State pending.\n");
			break;
		case fmi2Discard:
			fmi2GetBooleanStatus(s1, fmi2Terminated, &boolVal);
			if(boolVal == fmi2True)
				printf("Slave s1 wants to terminate simulation.\n");
			terminateSimulation = true;
			break;
		case fmi2Error:
			printf("State Error.\n");
			terminateSimulation = true;
			break;
		case fmi2Fatal:
			printf("State Fatal.\n");
			terminateSimulation = true;
			break;
		default:
			printf("Undefined slave state.\n");
			terminateSimulation = true;
			break;
		}

		if (terminateSimulation)
			break;

		// Other slaves call + status check

		// Increment master time
		currMasterTime += h;
	}

///////////////////////////////////////////////////////////////////////////////////////////////
	printf("----------- Shutdown sub-phase -------------\n");

	if ((simStat != fmi2Error) && (simStat != fmi2Fatal)){
		fmi2Terminate(s1);
	}

	if (simStat != fmi2Fatal){
		fmi2FreeInstance(s1);
	}

	printf("********** End of FMU **********\n\n");
	return 0;
}
