#include "DSP2833x_Device.h"     // DSP2833x Headerfile Include File
#include "DSP2833x_SWPrioritizedIsrLevels.h" 
#include "filter_bat2.h"
#include "package.h"

#include "measure.h"
#include "package.h"

#include "peripher.h"
#include "ADC.h"
#include "DAC.h"

#include "RS485.h"
#include "message.h"
#include "log_to_mem.h"

#include <math.h>	//	Это чтобы мерить амплитуду! sqrt без этого будет крив!!!

unsigned int CanPowse=CANPOWSE,CanGO=0;

unsigned int Maska[2][8]; 

int TPL_CANS=0;	// Количество температурных каналов 
int tpl_cans=0;
int cal_addr=0;

int period_ready, period_blink, period_dac, time_dac;

FLAG chk,sig;
long time_1_5sec, time_5msec, time_5sec;

int 	READY = 0;

unsigned long WAKEpowse;

unsigned long STOPpowse;

long err_count[6];
float lev_count[6];
float zer_count[4];

int sens_type[16];
int sens_pair[16];
long din_count[32];
int preCur[4];
int cntCur[4];
unsigned int Caliber_time = 0;
int Curr_Edge;

float powK[] = 
{
	0.0,
	0.1033,
	0.1027,
	0.0076,	0.0076,	0.0076, 0.0076, 1
};

FILTERBAT def_FILTERBAT = DEF_FILTERBAT;
FILTERBAT adc_filter[24];
FILTERBAT out_filter[24];

long sens_count[24];

float K100,K_50;
float K_T1,K_T2;

float curK[4];

#define cur_K	0.75

interrupt void cpu_timer1_isr_SENS(void);

/********************************************************************/    
/* Расчет модула тока из показаний двух фаз */                                
/********************************************************************/
float im_calc(float ia,float ib)
{
  float isa,isb;
  
	isa  = - 1.5 * (ia + ib);
	isb  = COSPi6 * (ia - ib);
	return  (2*sqrt(isa*isa+isb*isb)/3);
}

void calc_temper_koef()
{
	K_T1 = 100.0/(K400_1 - K300_1);
	K_T2 = 100.0/(K400_2 - K300_2);

	powK[1] = K380_1; powK[1]/=10000.0;
	powK[2] = K380_2; powK[2]/=10000.0;
}

int er_anal(int term, long * count, int edge, int pre)
{                                 
	if (term)
	{
		if((*count)>=edge)	return 1;
		(*count)++;			return pre;
	} 
	if( (*count) == 0 ) 	return 0;
	(*count)--;				return pre;
}   

interrupt void cpu_timer1_isr_SENS(void)
{
  static unsigned int
  count_blink=0, count_bright=0, count_mode, count_dac=0,count_load=0,
  blink_over, blink_alarm, work_lamp, heat_lamp, power_lamp, x;

  float sinus;
  int kod;

  int TST;

#define err_lamp	work_lamp	//	это дла шкапа,
#define alm_lamp	heat_lamp	//	чтоб было понатнее

	EALLOW;
	CpuTimer1.InterruptCount++;
	IER |= MINT13;                 // Set "global" priority
	EINT;
  	EDIS;    // This is needed to disable write to EALLOW protected registers


	if(++CanPowse >= CANPOWSE)
	{
		CanPowse = 0;	CanGO = 1;
	}

	TST = cTestLamp;//|bTestLamp;

	if(!sig.bit.Error|TST)	toggle_READY();
	else					clear_READY();			

	if(!cReset)	ServiceDog();

	if(Desk==dsk_LOAD)	
	{
		if(STOPpowse)STOPpowse--;
		if(++count_dac >= period_dac)
		{
			count_dac=0;

			if(WAKEpowse)
			{
				if(WAKEpowse < ADC_FREQ*2)	
				{
					Init_DAC();
			}	}

			else
			{
				if(cUMPstart)
				{
					if(++count_load > time_dac)	count_load = time_dac; 
if(count_load > (time_dac/2))count_load++;
					sinus = (14.0/*7.0*/ / 16.0 * count_load) + (2.0 / 16.0 * time_dac);
				}			
				else 
				{	count_load = 0; 
					sinus=0;
			}	}
			
			x = (x+1)&0xFFF;
			if(!x || cInitDac)
			{
				Init_DAC(); cInitDac=0;
			}
			else
			{
				kod = (int)sinus;

	   			Anal_output(kod, time_dac);
		}	}

		return;
	}
	
	if(++count_bright == maximum_bright)
	{
		count_bright = 0 ;

		if( ((Desk==dsk_SHKF) && power_lamp)	|| 
			((Desk==dsk_COMM) && heat_lamp)	)	set_RES_OUT_1();
		else									clear_RES_OUT_1();
	
		if(work_lamp)	set_LED_OUT_1(); //	Она же err_lamp для шкапа
		else			clear_LED_OUT_1();
		if(heat_lamp)	set_LED_OUT_2(); //	Она же alm_lamp для шкапа
		else			clear_LED_OUT_2();
	}

	if(count_bright == Brightness)
	{
		clear_LED_OUT_1();
		clear_LED_OUT_2();
		clear_RES_OUT_1();
	}

	if(++count_blink >= BLINK_TIME)
	{
		count_blink=0;
		count_mode++;
		blink_over  = (count_mode & 1)?1:0;
		blink_alarm = (count_mode & 7)?1:0;

		if(cTestLamp|bTestLamp)
		{
			heat_lamp = blink_over;
			work_lamp = blink_over;
			power_lamp= blink_over;
		}
		else
		{
			if(Mode==adr_SHKF)
			{
				power_lamp= 1;
				if(sig.bit.Alarm){//	power_lamp= blink_alarm;
										alm_lamp  = blink_over;	}
				else					alm_lamp  = 0;

				if(sig.bit.Error){		power_lamp= blink_over;
										err_lamp  = blink_over;	}
				else					err_lamp  = 0;          
			}
			else
			{
//					 if(sig.bit.Error)		work_lamp  = blink_over;
//				else if(sig.bit.Alarm)		work_lamp  = blink_alarm;
//				else						
				work_lamp  = 1;

				if(bSecretBt|cSecretBt)		work_lamp  = blink_alarm;

					 if(sig.bit.OverHeat)	heat_lamp  = 1;
				else if(sig.bit.SubHeat)	heat_lamp  = blink_over;
				else if(sig.bit.OutHeat)	heat_lamp  = !blink_alarm;
				else						heat_lamp  = 0;
}	}	}	}

void Init_sensors()
{
  int i;
  
	WAKEpowse = 5L * ADC_FREQ;

	KeyPressed.all=0;

	calc_temper_koef();

	period_dac	= READY_FREQ / DAC_FREQ;
	time_dac	= LOAD_TIME * DAC_FREQ;

	time_1_5sec	= (3 * ADC_FREQ) / 2;
	time_5msec	= (5 * ADC_FREQ) / 1000;
	time_5sec	= (5 * ADC_FREQ);

	for(i=0;i<16;i++) 
	{
		sens_type[i]=0;
		sens_pair[i]=i;
	}

	if(Desk==dsk_LOAD)
	{
		sens_type[0]=VOLTAGE;	sens_pair[0]=1;
		sens_type[1]=VOLTAGE;	sens_pair[1]=0;
		sens_type[2]=CURRENT;	sens_pair[2]=3;
		sens_type[3]=CURRENT;	sens_pair[3]=2;

		if(Mode==adr_LOA1)
		{
			curK[0] =  0.386;//0.492;
			curK[1] =  0.386;//0.492;
			curK[2] = 4.82;//1.783;//3.288;
			curK[3] = 4.82;//1.783;//3.288;
		}

		if(Mode==adr_LOA2)
		{
			curK[0] = 0.386;//0.489;
			curK[1] = 0.386;//0.489;
			curK[2] = 4.82;//3.288;
			curK[3] = 4.82;//3.288;
		}
		Curr_Edge = 100;
	}

	if((Mode==adr_POW1) || (Mode==adr_POW2))
	{
		Curr_Edge = 40;
		for(i=0;i<4; i++)
		{
			sens_type[i]=VOLTAGE;
			curK[i] = 0.402;//0.75;
	}	}

	if(Mode==adr_SHKF)
	{
		sens_type[0] = POWER_380; sens_pair[0]=1;
		sens_type[1] = POWER_38O; sens_pair[1]=0;
		sens_type[2] = POWER_31; sens_pair[2]=3;
		sens_type[3] = POWER_31; sens_pair[3]=2;
		sens_type[4] = POWER_31; sens_pair[4]=5;
		sens_type[5] = POWER_31; sens_pair[5]=4;
		sens_type[6] = POWER_24;
		sens_type[7] = POWER_27;
		sens_type[8] = POWER_24;
		sens_type[9] = POWER_15;
		sens_type[10] = POWER_24;
		sens_type[11] = POWER_24;
		sens_type[12] = POWER_24;
		sens_type[13] = TERMO_AD;
		sens_type[14] = TERMO_AD;
	}

	for(i=0;i<4; i++) err_count[i] = 0;
	for(i=0;i<6; i++) lev_count[i] = 0;;
	for(i=0;i<28;i++) sens_count[i] = 0;
	for(i=0;i<32;i++) din_count[i] = 0; 

	for(i=0;i<24;i++)
	{	adc_filter[i] = def_FILTERBAT;
		out_filter[i] = def_FILTERBAT;
	}

	switch(Mode)
	{
		case adr_TRN1:
		case adr_TRN2:	
			TPL_CANS = TPL_TRN;	tpl_cans = TPL_TRN*2;	cal_addr = tpl_cans;	break;

		case adr_POW1:
		case adr_POW2:	
			TPL_CANS = TPL_POW;	tpl_cans = TPL_POW*2;	cal_addr = tpl_cans;	break;

		case adr_ENG1:	
			TPL_CANS = TPL_ENG;	tpl_cans = TPL_ENG;		cal_addr = tpl_cans;	break;

		case adr_SHKF:			
								tpl_cans = 15;			cal_addr = tpl_cans-2;	break;
}	}

void Init_sensors_more()
{
  int i;

	for(i=0;i<DATASTART;i++) Modbus[i].all &= NOER;

	for(i=0;i<6; i++) lev_count[i] = 0;
	for(i=0;i<4; i++) zer_count[i] = Zero_lev[i];

 	if(Mode==adr_POW1 || Mode==adr_POW2)
	{
		Modbus[tpl_cans+0].bit.bitE = 1;	//	Ignore
		Modbus[tpl_cans+1].bit.bitE = 1;	//	Ignore
		Modbus[tpl_cans+2].bit.bitE = 1;	//	Ignore
		Modbus[tpl_cans+3].bit.bitE = 1;	//	Ignore
	}

	if(Mode==adr_LOA1 || Mode==adr_LOA2)
	{
		Modbus[0].bit.bitE = 1;	//	Ignore
		Modbus[1].bit.bitE = 1;	//	Ignore
	}


	if(TermoAD) calc_temper_koef();

}

void Init_packMask()
{
  int i,j,can=0;

	can = tpl_cans;

	if(Currentoz)	can+= 4;

	for(i=0;i<2;i++)
	for(j=0;j<8;j++) { Maska[i][j]=0; }

	switch(Desk)
	{
		case dsk_PULT:	
		case dsk_EPLT:	
			Maska[m_SLOW][0]|= 0x000F;						//	Полученное
			Maska[m_SLOW][6]|= 0x01FC;						//	Яркость ламп и одиночество
			break;

		default:
		 	for(i=0;i<can;i++)
			{
				Maska[m_FAST][ i    /16]|=(1<<( i    %16));	//	Диагностика
				Maska[m_FAST][(i+24)/16]|=(1<<((i+24)%16));	//	Показания
			}
			for(i=0;i<3; i++)
				Maska[m_SLOW][i+3] = Maska[m_FAST][i];		//	Уставки

			if(Desk==dsk_BKSD)
								Maska[m_SLOW][7]|= 0x0300;	//	Калибровка термодатчиков
			else
								Maska[m_SLOW][7]|= 0x0F00;	//	Калибровка термодатчиков

			if(Currentoz)		Maska[m_SLOW][7]|= 0x000F;	//	Ноль датчиков напряжения

			if(Desk==dsk_LOAD)
			{					Maska[m_SLOW][7]|=0x0030;	//	Калибровка ЦАП
								Maska[m_FAST][7]|=0xE000;	//	Дублировать команды
	}		}

	Maska[m_FAST][1]|= 0x0001;								//	Дискретные входы
	Maska[m_SLOW][6]|= 0x0007;								//	Яркость ламп и периоды посылок
	Maska[m_SLOW][7]|= 0xE000;								//	Адрес, команды, и чтоб не вылазило
	
	Maska[1][6] = 0xF3FF;
	Maska[1][7] = 0xC03F;
}

void reset_errs(int sens, ERROR er)
{
//  unsigned long report;
  unsigned int set;
  ERROR err;

	err=er;

	set = sens_error[sens].all & NOER;
	sens_error[sens].all = err.all | set;

	sens_error[sens].bit.Ready = !(err.bit.Stop && (!sens_error[sens].bit.Ignor));
	chk.bit.Error|= !(sens_error[sens].bit.Ready);
}


ERROR control_ADC(int sens, int number, int zero)
{
  ERROR err;
  int erwait;

  	err.all = 0;

	if(TermoSW) erwait = SENS_ERR_WAIT;
	else		erwait = ADC_FREQ;

//	Канал оборван
	if(er_anal(((number < zero)||(number > (0x0FFF-zero))),
				&sens_count[sens],erwait,
				sens_error[sens].bit.Tear))
	{		
		err.bit.Tear  = 1;
	}
/*
//	АЦП залип
	if(er_anal( (sens_prev[sens] == number),
				&sens_count[sens][1],ADC_FREQ,
				sens_error[sens].bit.Stick))
	{		
		err.bit.Stick = 1;
	}
	sens_prev[sens] = number;
*/
	return err;
}

void Current_count(int chan)
{
  float Numb,Current,fLev,Level;
  static float aCurrent,Amplitude,ffLev;
  int ignor, sens, pair, ist, thrd;

  ERROR error;

	if(Desk == dsk_LOAD)
	if(!chan)
	{
		sig.all = chk.all;
		chk.all = 0;
	}

	error.all = 0;

	sens = tpl_cans + chan;
	pair = sens_pair[chan];
	ist	= !(chan & 1);
	thrd=  (chan >>1) + 4;

	if(sens_error[sens].bit.Bypas)
	{
		sens_error[sens].all = 0;		
		sens_error[sens].bit.Bypas = 1;
		Modbus[sens+DATASTART].all = 0;
		return;
	}

	Numb = adc_table_lem[chan];

	zer_count[chan] += (Numb-zer_count[chan])/(5.0 * ADC_FREQ);
	Zero_lev[chan] = (int)(filterbat(&out_filter[chan],zer_count[chan]));

	Current = (Numb - Zero_lev[chan]) * curK[chan];

modbus[0x64+chan]=Numb;

//	if(Desk == dsk_COMM)
//	if(ist)	Current = -Current;

	lev_count[chan] += (fabs(Current)-lev_count[chan])/(ADC_FREQ/5);

//	Запомним
	if(ist)	
	{
		aCurrent = Current;	// Запомнили мгновенное значение - дла амплитуды
	}
	else	
	{
//		Вычисление амплитуды
		Amplitude = im_calc(Current,aCurrent);
		Level = Amplitude/RADIX2;
		fLev = filterbat(&out_filter[thrd],Level);
		if(fLev<Curr_Edge)fLev=0;

		if((sens_type[chan]==CURRENT)||(Desk==dsk_COMM))
		{
			ffLev += (fLev-ffLev)/(ADC_FREQ/5);
			modbus[sens+DATASTART-1] = ffLev;
		}
		else
			modbus[sens+DATASTART-1] = (int)(fLev);

		modbus[sens+DATASTART] = RADIX2 * modbus[sens+DATASTART-1];

//		Третья фаза для проверок
		if(Desk==dsk_COMM)	Numb = Current-aCurrent;
		else				Numb =-Current-aCurrent;

		lev_count[thrd] += (fabs(Numb)-lev_count[thrd])/(ADC_FREQ/5);
			
Modbus[0x68 + (chan >>1)*3 + 0].all = lev_count[chan];
Modbus[0x68 + (chan >>1)*3 + 1].all = lev_count[pair];
Modbus[0x68 + (chan >>1)*3 + 2].all = lev_count[thrd];

		}

//	Зашиты!

	ignor = sens_error[sens].bit.Ignor;

								Numb = lev_count[chan];
	if(Numb<lev_count[pair])	Numb = lev_count[pair];
	if(Numb<lev_count[thrd])	Numb = lev_count[thrd];

	if(er_anal( ((Numb-lev_count[chan])/Numb > 0.2) && (Numb>Curr_Edge),
				&err_count[chan],time_1_5sec,0))
	{
		error.bit.Wry = 1;
		if(!ignor)
		error.bit.Stop = 1;
	}

	if(er_anal( ((Numb-lev_count[thrd])/Numb > 0.2) && (Numb>Curr_Edge),
				&err_count[thrd],time_1_5sec,0))
	{
		error.bit.Wry = 1;
		if(!ignor)
		error.bit.Stop = 1;
	}

	if(Desk == dsk_LOAD)
	if(!ist)
	{
		if(Level > sens_hi_edge[sens])
		{
			error.bit.Hyper	= 1;
			if(!ignor)
			error.bit.Stop = 1;
		}

		if(sens_type[chan]==VOLTAGE)
		if(Level < sens_lo_edge[sens])
		{
			error.bit.Out	= 1;
			if(!ignor)
			error.bit.Stop = 1;
		}

		if(sens_type[chan]==CURRENT)
		if(Level > sens_lo_edge[sens])
		{
			error.bit.Over	= 1;
	}	}

	reset_errs(sens,error);
}

void Temper_count(int chan)
{
  float Numb;
  static int Temper;
  int kun, ignor;
  ERROR error;

	if(!chan)
	{
		sig.all = chk.all;
		chk.all = 0;
	}

	if(sens_error[chan].bit.Bypas)
	if(chan<tpl_cans)
	{
		sens_error[chan].all = 0;		
		sens_error[chan].bit.Bypas = 1;
		Modbus[chan+DATASTART].all = 0;
		return;
	}

	Numb = adc_table_tpl[chan];

	kun = chan - cal_addr;

	if(TermoAD)
	{
		if(cTermoCal|bTermoCal)
		{
			if(Desk==dsk_SHKF)
			if(Numb>2200) kun|=2;

			if(kun>=0)
			{
				Caliber[kun] = Numb;
				calc_temper_koef();
				return;
		}	}

		if(kun&1)	Numb = (Numb-K300_2)*K_T2+ZERO;
		else		Numb = (Numb-K300_1)*K_T1+ZERO;

		if(Numb<-273)	Numb =-273;
	}

	if(TermoRS)
	{
		if(kun>=0)
		{
			Caliber[kun] = Numb;

			K100 = 129.0/(K150_D - K100_D); 
			K_50 = 261.8/(K150_D - K100_D);
			K_T1 = 129.0/(K150_1 - K100_1); 
			K_T2 = 129.0/(K150_2 - K100_2); 

			return;
		}

		if(Desk==dsk_BKSD)
		{
modbus[chan+DATASTART+8] = Numb;
			if(chan<6)	Numb = (Numb-K100_D)*K100;
			else		Numb = (Numb-K100_D)*K_50;// - 6.56;
		}
		else
		{
			if(chan&1)	Numb = (Numb-K100_2)*K_T2;
			else		Numb = (Numb-K100_1)*K_T1;
		}

		if(Numb<-273)	Numb =-273;
		if(Numb>1000)	Numb =1000;

		Numb = filterbat(&out_filter[chan],Numb*100)/100;
	}

	if(bSecretBt|cSecretBt) Numb = 40.0;

	Modbus[chan+DATASTART].all = (int)(Numb*10);

	Temper = (int)Numb;

	error.all = 0;
	if(!(bSecretBt|cSecretBt))
	error = control_ADC(chan, adc_table_tpl[chan], 200);

	if(!error.all)
	{
		ignor = sens_error[chan].bit.Ignor;
	  	if(((Temper>sens_hi_edge[chan]-Cooling) && (sens_error[chan].bit.Hyper)) ||
			(Temper>sens_hi_edge[chan]) )
		{  	
			error.bit.Hyper	= 1;
			if(!ignor)
			{
				error.bit.Stop = 1;
				chk.bit.OverHeat= 1;
		}	}

		else

//		Предупреждение по температуре
 
  		if(Temper>sens_lo_edge[chan])
		{
			error.bit.Over	= 1;
			if(!ignor)
			chk.bit.SubHeat	= 1;
	}	}

	if(error.all) chk.bit.OutHeat = 1;

	reset_errs(chan,error);

	}

void Power_count(int chan)
{
  float Numb;
  int Power,ignor,bitt;
  ERROR error;

	if(sens_error[chan].bit.Bypas)
	{
		sens_error[chan].all = 0;		
		sens_error[chan].bit.Bypas = 1;
		Modbus[chan+DATASTART].all = 0;
		return;
	}

	error.all = 0;
	ignor = sens_error[chan].bit.Ignor;

	bitt = chan*2;
	if(chan!=7)
	error.bit.Discr1 = er_anal(((KeyPressed.all>>bitt)&1), &din_count[bitt], 1000, 0); bitt++;
	error.bit.Discr2 = er_anal(((KeyPressed.all>>bitt)&1), &din_count[bitt], 1000, 0); bitt++;

	if(chan==6)
	{
		error.bit.Discr3 = er_anal(((KeyPressed.all>>bitt)&1), &din_count[bitt], 1000, 0); bitt++;
		error.bit.Discr4 = er_anal(((KeyPressed.all>>bitt)&1), &din_count[bitt], 1000, 0); bitt++;
	}

	error.bit.Contr = error.bit.Discr1 | error.bit.Discr2 | error.bit.Discr3 | error.bit.Discr4;

	Numb = adc_table_lem[chan];
	Power = Numb * powK[sens_type[chan]];

	Modbus[chan+DATASTART].all = Power;

	if(Power <sens_lo_edge[chan])
	{		
		error.bit.Out	= 1;

		if(sens_error[sens_pair[chan]].bit.Out)
		{
			if(!ignor)
			error.bit.Stop = 1;
	}	}

/*	Повышенное напражение
	if(Power > sens_hi_edge[chan])
	{		
		error.bit.Hyper	= 1;
		if(!ignor)
		error.bit.Stop = 1;
	}
*/

	if(error.all) 
	if(!ignor)
	chk.bit.Alarm = 1;
	reset_errs(chan,error);
}


void measure_all()
{
  int i;

	for(i=0;i<15;i++)
	if(sens_type[i])
	{
		if(sens_type[i]==TERMO_AD)	Temper_count(i);
		else						Power_count(i);
	}

	sig.all = chk.all;
	chk.all = 0;
}