/**************************************************************************
	Description: Программа моделирует работу процессора - осуществляет
	вызов функций init28335, detcoeff, isr.
	Также моделирует различные периферийные устройства процессора
	TMS320F28335/TMS320F28379D (ADC, PWM, QEP и т.д.).

	Автор: Улитовский Д.И.
	Дата последнего обновления: 2021.11.08
**************************************************************************/


#include "simstruc.h"
#include "controller.h"
#include "init28335.h"
#include "param.h"


UMotorMeasure motor; 
double dt;

// Для имитации обмена с ПУ
int j;
unsigned short paramNo;
unsigned short paramNew[PAR_NUMBER];
void processSFunctionIfChanged(SimStruct *S, int_T *iW) {

	// обрабатываем параметры S-Function каждый раз, когда они изменились
	if ( iW[0] == 1 ) {
		iW[0] = 0;
		int kkk = 0;
		for (int lll = 0; lll < NPARAMS; lll++ ) {
			// определяем кол-во элементов в параметре
			int dimen = mxGetNumberOfElements(ssGetSFcnParam(S,lll));
			// обрабатываем параметр в зависимости от его размера
			if ( dimen > LEN_PARAM_MATR*2 ) {
				ssSetErrorStatus(S,"В параметре-массиве слишком много элементов");
				return;
			}
			else if ( dimen > 1 ) {
				// запоминаем кол-во элементов параметра-матрицы
				paramMatrDimen = dimen;
				// запоминаем значения элементов параметра-матрицы
				for (int mmm = 0; mmm < dimen; mmm++ )
					paramMatr[mmm] = mxGetPr(ssGetSFcnParam(S,lll))[mmm];
			}
			else {
				// запоминаем значения параметров-скаляров
				paramScal[kkk++] = mxGetPr(ssGetSFcnParam(S,lll))[0];
			}
		}
		// ПАРАМЕТРЫ (begin)
		int nn = 0;
		dt = paramScal[nn++];//шаг дискретизации (всегда должен передаваться в S-function последним!)
		// ПАРАМЕТРЫ (end)
	} //if ( iW[0] == 1 )

}

void initialisationOnStart(int_T *iW) {

	// кое-что выполняем один раз при запуске модели
	if ( iW[1] == 1 ) {
		iW[1] = 0;
		Init_Timers();
		init28335();
	} //if ( iW[1] == 1 )


}


void update_norm_ADC_array()
{
			//// Udc1
			//if ( udc1_ml > UDC_SENS_MAX )
			//	udc1_ml = UDC_SENS_MAX;
			//else if ( udc1_ml < -UDC_SENS_MAX )
			//	udc1_ml = -UDC_SENS_MAX;
			//iq_norm_ADC[0] = _IQ(udc1_ml/NORMA_ACP);
			//// Udc2
			//if ( udc2_ml > UDC_SENS_MAX )
			//	udc2_ml = UDC_SENS_MAX;
			//else if ( udc2_ml < -UDC_SENS_MAX )
			//	udc2_ml = -UDC_SENS_MAX;
			//iq_norm_ADC[1] = _IQ(udc2_ml/NORMA_ACP);
			//// Udc3
			//if ( udc3_ml > UDC_SENS_MAX )
			//	udc3_ml = UDC_SENS_MAX;
			//else if ( udc3_ml < -UDC_SENS_MAX )
			//	udc3_ml = -UDC_SENS_MAX;
			//iq_norm_ADC[7] = _IQ(udc3_ml/NORMA_ACP);
			//// Udc4
			//if ( udc4_ml > UDC_SENS_MAX )
			//	udc4_ml = UDC_SENS_MAX;
			//else if ( udc4_ml < -UDC_SENS_MAX )
			//	udc4_ml = -UDC_SENS_MAX;
			//iq_norm_ADC[8] = _IQ(udc4_ml/NORMA_ACP);
			//// Idc1
			//if ( idc1_ml > IDC_SENS_MAX )
			//	idc1_ml = IDC_SENS_MAX;
			//else if ( idc1_ml < -IDC_SENS_MAX )
			//	idc1_ml = -IDC_SENS_MAX;
			//iq_norm_ADC[2] = _IQ(idc1_ml/NORMA_ACP);
			//// Idc2
			//if ( idc2_ml > IDC_SENS_MAX )
			//	idc2_ml = IDC_SENS_MAX;
			//else if ( idc2_ml < -IDC_SENS_MAX )
			//	idc2_ml = -IDC_SENS_MAX;
			//iq_norm_ADC[3] = _IQ(idc2_ml/NORMA_ACP);
			//// Idc3
			//if ( idc3_ml > IDC_SENS_MAX )
			//	idc3_ml = IDC_SENS_MAX;
			//else if ( idc3_ml < -IDC_SENS_MAX )
			//	idc3_ml = -IDC_SENS_MAX;
			//iq_norm_ADC[9] = _IQ(idc3_ml/NORMA_ACP);
			//// Idc4
			//if ( idc4_ml > IDC_SENS_MAX )
			//	idc4_ml = IDC_SENS_MAX;
			//else if ( idc4_ml < -IDC_SENS_MAX )
			//	idc4_ml = -IDC_SENS_MAX;
			//iq_norm_ADC[10] = _IQ(idc4_ml/NORMA_ACP);

			//// Ia1
			//if ( ia1_ml > IAC_SENS_MAX )
			//	ia1_ml = IAC_SENS_MAX;
			//else if ( ia1_ml < -IAC_SENS_MAX )
			//	ia1_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[4] = _IQ(ia1_ml/NORMA_ACP);

			//// Ib1
			//if ( ib1_ml > IAC_SENS_MAX )
			//	ib1_ml = IAC_SENS_MAX;
			//else if ( ib1_ml < -IAC_SENS_MAX )
			//	ib1_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[5] = _IQ(ib1_ml/NORMA_ACP);

			//// Ic1
			//if ( ic1_ml > IAC_SENS_MAX )
			//	ic1_ml = IAC_SENS_MAX;
			//else if ( ic1_ml < -IAC_SENS_MAX )
			//	ic1_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[6] = _IQ(ic1_ml/NORMA_ACP);


			//// Ia2
			//if ( ia2_ml > IAC_SENS_MAX )
			//	ia2_ml = IAC_SENS_MAX;
			//else if ( ia2_ml < -IAC_SENS_MAX )
			//	ia2_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[11] = _IQ(ia2_ml/NORMA_ACP);

			//// Ib2
			//if ( ib2_ml > IAC_SENS_MAX )
			//	ib2_ml = IAC_SENS_MAX;
			//else if ( ib2_ml < -IAC_SENS_MAX )
			//	ib2_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[12] = _IQ(ib2_ml/NORMA_ACP);

			//// Ic2
			//if ( ic2_ml > IAC_SENS_MAX )
			//	ic2_ml = IAC_SENS_MAX;
			//else if ( ic2_ml < -IAC_SENS_MAX )
			//	ic2_ml = -IAC_SENS_MAX;
			//iq_norm_ADC[13] = _IQ(ic2_ml/NORMA_ACP);



			//vect_control.off_curr_pi = mst.off_curr_pi;
			//vect_control.only_one_km = mst.only_one_km;
			//vect_control.enable_compens_iq1_iq2 = mst.enable_compens_iq1_iq2;
}

void simulateAdcAndCallIsr() {

	///* Моделируем преобразования измеряемых величин датчиками,
	//операционниками и АЦП (с помощью nAdc учитываем сдвиг по времени
	//между АЦП разных сигналов) */
	//if ( tAdc < Tadc ) {
	//	tAdc++;
	//}
	//else {

	//	tAdc = 1;
	//	timers_adc++;
	//	if (timers_adc>=FREQ_ADC_TIMER)
	//	   timers_adc = 0;

	//	update_norm_ADC_array();
	//	// после завершения серии АЦП вызываем isr()
	//	acp_Handler();
	//	//isr();


	//	// nAdc++;
	//	// switch ( nAdc ) {
	//	// case 5:
	//
	//	// 	break;
	//	// case 6:

	//	// 	break;

	//	// case 7:
	//	// 	// как бы с ПУ
	//	// 	for ( j = FIRST_WRITE_PAR_NUM; j < paramNo; j++ ) {
	//	// 		if ( paramNew[j] != param[j] ) {
	//	// 			input_param((short)j, paramNew[j]);
	//	// 			break;
	//	// 		}
	//	// 	}
	//	// 	// после завершения серии АЦП вызываем isr()
	//	// 	isr();
	//	// 	break;
	//	// } //switch ( nAdc )

	//} //tAdc

	//		if (calcAlgUpr) {
	//			// реализует алгоритм управления
	//			upr();
	//			calcAlgUpr = 0;
	//			timers_pwm++;
	//			if (timers_pwm>=FREQ_PWM_TIMER)
	//			   timers_pwm = 0;
	//		}


}



void controller(SimStruct *S, const real_T *u, real_T *xD, real_T *rW, int_T *iW) {


	readInputParameters(u);
	processSFunctionIfChanged(S, iW);
	initialisationOnStart(iW);

	Simulate_Timers();

	
	simulateAdcAndCallIsr();



	writeOutputParameters(xD);

} //void controller(SimStruct ...
