/**************************************************************************
	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);

	mcu_simulate_step();
} //void controller(SimStruct ...