/**
**************************************************************************
* @file MCU.c
* @brief Исходный код S-Function.
**************************************************************************
@details
Данный файл содержит функции S-Function, который вызывает MATLAB.
**************************************************************************
@note
Описание функций по большей части сгенерировано MATLAB'ом, поэтому на английском
**************************************************************************/

/**
  * @addtogroup		WRAPPER_SFUNC    S-Function funtions
  * @ingroup		MCU_WRAPPER
  * @brief 			Дефайны и функции блока S-Function
  * @details		Здесь собраны функции, с которыми непосредственно работает S-Function
  *	@note			Описание функций по большей части сгенерировано MATLAB'ом, поэтому на английском
  * @{
  */

#define S_FUNCTION_NAME MCU
#define S_FUNCTION_LEVEL    2

#include "mcu_wrapper_conf.h"

#include <windows.h>
#include <process.h>
#include <intrin.h>
#include "mex.h"


static unsigned long long get_timer_frequency(void)
{
#ifdef USE_CPU_TIMER
	HKEY hKey;
	DWORD frequency = 0;
	DWORD size = sizeof(DWORD);

	if (RegOpenKeyExA(HKEY_LOCAL_MACHINE, "HARDWARE\\DESCRIPTION\\System\\CentralProcessor\\0",
		0, KEY_READ, &hKey) == ERROR_SUCCESS) {
		if (RegQueryValueExA(hKey, "~MHz", NULL, NULL, (LPBYTE)&frequency, &size) == ERROR_SUCCESS) {
			RegCloseKey(hKey);
			return (unsigned long long)frequency * 1000000ULL; // MHz -> Hz
		}
		RegCloseKey(hKey);
	}
#elif defined(USE_QPF_TIMER)
	LARGE_INTEGER frequency;
	QueryPerformanceFrequency(&frequency);
	return frequency.QuadPart;
#endif
}

static double ticksToNanoseconds(unsigned long long start, unsigned long long end)
{
#if defined(USE_CPU_TIMER) || defined(USE_QPF_TIMER)
	unsigned long long elapsed = end - start;
	unsigned long long frequency = get_timer_frequency();
	return (double)elapsed * 1e9 / (double)frequency;
#endif
}

static void InitializeHighPrecisionTimer(void)
{
#ifdef USE_QPF_TIMER
	QueryPerformanceFrequency(&hmcu.dTimer.Frequency);
	hmcu.dTimer.TimerResolutionNs = 1e9 / (double)(hmcu.dTimer.Frequency);
#endif
}

static unsigned long long read_timer(void)
{
#ifdef USE_CPU_TIMER
	return __rdtsc();
#elif defined(USE_QPF_TIMER)
	LARGE_INTEGER counter;
	QueryPerformanceCounter(&counter);
	return counter.QuadPart;
#endif
}


#define MDL_UPDATE ///< для подключения mdlUpdate()
  /**
   *	@brief		Update S-Function at every step of simulation
   *	@param		S - pointer to S-Function (library struct from "simstruc.h")
   *	@details	Abstract:
   *    This function is called once for every major integration time step.
   *    Discrete states are typically updated here, but this function is useful
   *    for performing any tasks that should only take place once per
   *    integration step.
   */
static void mdlUpdate(SimStruct* S, int_T tid)
{
	// Расчет периода (время между вызовами)
	if (hmcu.dTimer.call_count > 0 && hmcu.dTimer.SFuncPrevTime != 0) {
		hmcu.dSFuncPeriod = ticksToNanoseconds(
			hmcu.dTimer.SFuncPrevTime,
			hmcu.dTimer.SFuncStartTime) / 1000.0; // в микросекундах
	}

	// Сохраняем текущее время для следующего расчета периода
	hmcu.dTimer.SFuncPrevTime = hmcu.dTimer.SFuncStartTime;
	// Текущий вызов становится началом S-Function
	hmcu.dTimer.SFuncStartTime = read_timer();

	// get time of simulation
	time_T TIME = ssGetT(S);

	//---------------SIMULATE MCU---------------
    // Измерение времени выполнения MCU_Step_Simulation
    hmcu.dTimer.MCUStepStartTime = read_timer();
	MCU_Step_Simulation(S, TIME); // SIMULATE MCU
	hmcu.dTimer.MCUStepEndTime = read_timer();
	//------------------------------------------

	// Расчет времени выполнения в микросекундах
	hmcu.dMCUStepTime = ticksToNanoseconds(
		hmcu.dTimer.MCUStepStartTime,
		hmcu.dTimer.MCUStepEndTime) / 1000.0;
}//end mdlUpdate

/**
 *	@brief		Writting outputs of S-Function
 *	@param		S - pointer to S-Function (library struct from "simstruc.h")
 *	@details	Abstract:
 *    In this function, you compute the outputs of your S-function
 *    block. Generally outputs are placed in the output vector(s),
 *    ssGetOutputPortSignal.
 */
static void mdlOutputs(SimStruct* S, int_T tid)
{
	SIM_writeOutputs(S);

	// Измерение времени окончания выполнения mdlOutputs
	hmcu.dTimer.SFuncEndTime = read_timer();

	// Общее время выполнения S-Function в микросекундах
	if (hmcu.dTimer.SFuncStartTime)
	{
		hmcu.dSFuncTime = ticksToNanoseconds(
			hmcu.dTimer.SFuncStartTime,
			hmcu.dTimer.SFuncEndTime) / 1000.0;
	}

	// Накопление статистики
	hmcu.dTimer.call_count++;
}//end mdlOutputs

#define MDL_CHECK_PARAMETERS   /* Change to #undef to remove function */
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
static void mdlCheckParameters(SimStruct* S)
{
	int i;

	// Проверяем и принимаем параметры и разрешаем или запрещаем их менять
	// в процессе моделирования
	for (i = 0; i < 1; i++)
	{
		// Input parameter must be scalar or vector of type double
		if (!mxIsDouble(ssGetSFcnParam(S, i)) || mxIsComplex(ssGetSFcnParam(S, i)) ||
			mxIsEmpty(ssGetSFcnParam(S, i)))
		{
			ssSetErrorStatus(S, "Input parameter must be of type double");
			return;
		}
		// Параметр м.б. только скаляром, вектором или матрицей
		if (mxGetNumberOfDimensions(ssGetSFcnParam(S, i)) > 2)
		{
			ssSetErrorStatus(S, "Параметр м.б. только скаляром, вектором или матрицей");
			return;
		}
		//         sim_dt = mxGetPr(ssGetSFcnParam(S,0))[0];
				// Parameter not tunable
		//		ssSetSFcnParamTunable(S, i, SS_PRM_NOT_TUNABLE);
				// Parameter tunable (we must create a corresponding run-time parameter)
		ssSetSFcnParamTunable(S, i, SS_PRM_TUNABLE);
		// Parameter tunable only during simulation
//		ssSetSFcnParamTunable(S, i, SS_PRM_SIM_ONLY_TUNABLE);

	}//for (i=0; i<NPARAMS; i++)

}//end mdlCheckParameters
#endif //MDL_CHECK_PARAMETERS
static void mdlInitializeSizes(SimStruct* S)
{
	ssSetNumSFcnParams(S, 1);
	// Кол-во ожидаемых и фактических параметров должно совпадать
	if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S))
	{
		// Проверяем и принимаем параметры
		mdlCheckParameters(S);
	}
	else
	{
		return;// Parameter mismatch will be reported by Simulink
	}

	// set up discrete states
	ssSetNumContStates(S, 0);				// number of continuous states
	ssSetNumDiscStates(S, DISC_STATES_WIDTH);	// number of discrete states

	// set up input port
	if (!ssSetNumInputPorts(S, IN_PORT_NUMB)) return;
	for (int i = 0; i < IN_PORT_NUMB; i++)
	{
		ssSetInputPortWidth(S, i, inLengths[i]);
		ssSetInputPortDirectFeedThrough(S, i, 0);
		ssSetInputPortRequiredContiguous(S, i, 1);
	}

	// set up output port
	if (!ssSetNumOutputPorts(S, OUT_PORT_NUMB)) return;
	for (int i = 0; i < OUT_PORT_NUMB; i++)
		ssSetOutputPortWidth(S, i, outLengths[i]);


	ssSetNumSampleTimes(S, 1);


	ssSetNumRWork(S, 5);	// number of real work vector elements
	ssSetNumIWork(S, 5);	// number of integer work vector elements
	ssSetNumPWork(S, 0);	// number of pointer work vector elements
	ssSetNumModes(S, 0);	// number of mode work vector elements
	ssSetNumNonsampledZCs(S, 0);	// number of nonsampled zero crossings


	ssSetRuntimeThreadSafetyCompliance(S, RUNTIME_THREAD_SAFETY_COMPLIANCE_TRUE);
	/* Take care when specifying exception free code - see sfuntmpl.doc */
	ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);


}

#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START)
/**
 *	@brief		Initialize S-Function at start of simulation
 *	@param		S - pointer to S-Function (library struct from "simstruc.h")
 *	@details	Abstract:
 *    This function is called once at start of model execution. If you
 *    have states that should be initialized once, this is the place
 *    to do it.
 */
static void mdlStart(SimStruct* S)
{
	// Инициализация высокоточного таймера
	InitializeHighPrecisionTimer();

	SIM_Initialize_Simulation(S);
}
#endif // MDL_START

/**
 *	@brief		Initialize Sample Time of Simulation
 *	@param		S - pointer to S-Function (library struct from "simstruc.h")
 *	@details	Abstract:
 *    This function is used to specify the sample time(s) for your
 *    S-function. You must register the same number of sample times as
 *    specified in ssSetNumSampleTimes.
 */
static void mdlInitializeSampleTimes(SimStruct* S)
{
	// Шаг дискретизации
	hmcu.sSimSampleTime = mxGetPr(ssGetSFcnParam(S, NPARAMS - 1))[0];

	// Register one pair for each sample time
	ssSetSampleTime(S, 0, hmcu.sSimSampleTime);
	ssSetOffsetTime(S, 0, 0.0);
}

/**
 *	@brief		Terminate S-Function at the end of simulation
 *	@param		S - pointer to S-Function (library struct from "simstruc.h")
 *	@details	Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was
 *    allocated in mdlStart, this is the place to free it.
 */
static void mdlTerminate(SimStruct* S)
{
	hmcu.fMCU_Stop = 1;
#ifdef RUN_APP_MAIN_FUNC_THREAD
	if (hmcu.hMCUThread != NULL) {
		ResumeThread(hmcu.hMCUThread);

		//// Простая версия - ждем завершения потока

		//// Ждем до 5 секунд
		//for (int i = 0; i < 50; i++) {
		//	// Проверяем, завершился ли поток (упрощенная проверка)
		//	DWORD exitCode;
		//	if (GetExitCodeThread(hmcu.hMCUThread, &exitCode) && exitCode != STILL_ACTIVE) {
		//		break;  // поток завершился
		//	}
		//	Sleep(100);  // ждем 100ms
		//}


		// Даем потоку шанс завершиться нормально
		DWORD waitResult = WaitForSingleObject(hmcu.hMCUThread, 10000);
		if (waitResult == WAIT_TIMEOUT) {
			// Поток не ответил - завершаем принудительно
			TerminateThread(hmcu.hMCUThread, 0);
		}

		CloseHandle(hmcu.hMCUThread);
		hmcu.hMCUThread = NULL;
	}
#endif
	SIM_deInitialize_Simulation(S);
	mexUnlock();
}


/** WRAPPER_SFUNC
	* @}
	*/

#ifdef MATLAB_MEX_FILE    /* Is this file being compiled as a 
							 MEX-file? */
#include "simulink.c"     /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"      /* Code generation registration 
							 function */
#endif