#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 "RS485.h"
#include "message.h"
#include "log_to_mem.h"

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

int MAX_TPL_CANAL=0;	// Количество температурных каналов 
int period_ready, period_blink, period_dac, time_dac;

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

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

int sens_type[24];
int sens_pair[24];
long din_count[32];

int adc0[24];
int tmp0[24];
float tmpK[24];

FILTERBAT def_FILTERBAT = DEF_FILTERBAT;
FILTERBAT filter[40];

long sens_count[28];

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);
}


interrupt void cpu_timer1_isr_SENS(void)
{
  static unsigned int
  count_ready=0, count_blink=0, count_bright=0, count_mode, 
  blink_over, blink_alarm, work_lamp, heat_lamp, errr_lamp;

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

	if(++count_ready >= period_ready)
	{
		count_ready=0;

		if((!sig.bit.Error)|(cTestLamp))	toggle_READY();
		else								set_READY();
		ServiceDog();
	}

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

		if(Desk==dsk_COMM)
		{
			if(work_lamp)	GpioDataRegs.GPBCLEAR.bit.GPIO60=1;
			else			GpioDataRegs.GPBSET.bit.GPIO60=1;
			if(heat_lamp)	GpioDataRegs.GPBCLEAR.bit.GPIO58=1;
			else			GpioDataRegs.GPBSET.bit.GPIO58=1;
			if(errr_lamp)	GpioDataRegs.GPBCLEAR.bit.GPIO62=1;
			else			GpioDataRegs.GPBSET.bit.GPIO62=1;
		}

		if(Mode==adr_SHKF)
		{
			if(work_lamp)	GpioDataRegs.GPBCLEAR.bit.GPIO62=1;
			else			GpioDataRegs.GPBSET.bit.GPIO62=1;
	}	}

	if(count_bright == Brightness)
	{
		if(Desk==dsk_COMM)
		{
			GpioDataRegs.GPBSET.bit.GPIO60=1;
			GpioDataRegs.GPBSET.bit.GPIO58=1;
			GpioDataRegs.GPBSET.bit.GPIO62=1;
		}
	
		if(Desk==dsk_SHKF)
		{
			GpioDataRegs.GPBSET.bit.GPIO62=1;
	}	}

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

		if(cExtLamp)
		{
			work_lamp = cExtLite;
			heat_lamp = cExtLite;
			errr_lamp = cExtLite;
		}
		else
		{
			if(cTestLamp)
			{
				work_lamp = blink_over;
				heat_lamp = blink_over;
				errr_lamp = blink_over;
			}
			else
			{
				if(Mode==adr_SHKF)
				{
					if(sig.bit.Error)	work_lamp = blink_over;
					else				work_lamp = 1;
				}
				else
				{
						 if(sig.bit.Error)		work_lamp  = 0;//blink_over;
//					else if(sig.bit.Alarm)		work_lamp  = blink_alarm;
					else						work_lamp  = 1;

						 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_optic()
{
  int i;

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

	sens_type[0] = OPTIC;
	sens_type[1] = OPTIC;
}

void Init_sensors()
{
  int i;
  
	EALLOW;  // This is needed to write to EALLOW protected registers
	PieVectTable.XINT13 = &cpu_timer1_isr_SENS;
	EDIS;    // This is needed to disable write to EALLOW protected registers

	ConfigCpuTimer(&CpuTimer1, (SYSCLKOUT/1000000), 1000000/SIG_FREQ);
	CpuTimer1Regs.TCR.all = 0x4020; // Use write-only instruction to set TSS bit = 0
	IER |= M_INT13;

	period_ready = SIG_FREQ / (READY_FREQ * 2);
	period_blink = SIG_FREQ / (BLINK_FREQ * 2);

	period_dac	= SIG_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<24;i++) 
	{
		sens_type[i]=0;
		sens_pair[i]=i;
	}

	if((Mode==adr_REC1)||(Mode==adr_REC2))
	{
		sens_type[0]=TERMO_AD;
		sens_type[1]=TERMO_AD;
		sens_type[2]=TERMO_AD;
		sens_type[3]=TERMO_AD;

		sens_type[4]=TERMO_AD;
		sens_type[5]=TERMO_AD;

//sens_type[6]=TERMO_AD;
//sens_type[7]=TERMO_AD;


		sens_type[8]=TERMO_RS;
		sens_type[9]=TERMO_RS;
		sens_type[10]=TERMO_RS;
		sens_type[11]=TERMO_RS;

		sens_type[12]=VOLTAGE; sens_pair[12]=13;
		sens_type[13]=VOLTAGE; sens_pair[13]=12;
		sens_type[14]=VOLTAGE; sens_pair[14]=15;
		sens_type[15]=VOLTAGE; sens_pair[15]=14;

		Modbus[12].bit.bitE = 1;	//	Ignore
		Modbus[13].bit.bitE = 1;	//	Ignore
		Modbus[14].bit.bitE = 1;	//	Ignore
		Modbus[15].bit.bitE = 1;	//	Ignore
	}

	if((Mode==adr_INV1)||(Mode==adr_INV2))
	{
		sens_type[0]=TERMO_AD;
		sens_type[1]=TERMO_AD;
		sens_type[2]=TERMO_AD;
		sens_type[3]=TERMO_AD;

		sens_type[4]=TERMO_AD;
		sens_type[5]=TERMO_AD;
//sens_type[6]=TERMO_AD;

		sens_type[7]=TERMO_RS;
		sens_type[8]=TERMO_RS;
		sens_type[9]=TERMO_RS;
		sens_type[10]=TERMO_RS;
		sens_type[11]=TERMO_RS;
	}

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

	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<40;i++) filter[i] = def_FILTERBAT;

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

	MAX_TPL_CANAL = 12;
}

void Init_packMask()
{
  int i,j;

	for(i=0;i<3; i++)
	for(j=0;j<9;j++) { Maska[i][j]=0; }

	for(i=0;i<24;i++)
	if(sens_type[i])
	{
		Maska[0][ i    /16]|=(1<<( i    %16));
		Maska[0][(i+24)/16]|=(1<<((i+24)%16));
	}

	for(i=0;i<3; i++)
		Maska[1][i+3] = Maska[0][i];

	if(Desk==dsk_COMM) Maska[0][1]|=0x80;	//	OilSensor, ButtsPressed

	if(Desk==dsk_ISOL) Maska[0][1]|=0x0F;	//	raw data
	
	Maska[1][6] = 0x00F3;
	Maska[1][7] = 0xC000;
}

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;
}   

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

	err=er;

	if(!sens_error[sens].bit.Latch)
	{
		set = sens_error[sens].all & NOER;
		sens_error[sens].all = err.all | set;
	}
	else
	{
		sens_error[sens].all |= err.all;
	}
	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/100)))),
				&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;
}






int input_freq(int chan, int Volt)
{
  static int prevolt[4],tics[4],tacs[4],tic[4],tac[4];
  static long Freq = 0;
  int i,sum=0,bum=0;

	if(Volt			>=	Zero_lev[chan])
	if(prevolt[chan]<	Zero_lev[chan])
	{
		tics[chan] = tic[chan]; tic[chan] = 0; bum = 1;
	}

	if(Volt			<	Zero_lev[chan])
	if(prevolt[chan]>=	Zero_lev[chan])
	{
		tacs[chan] = tac[chan]; tac[chan] = 0; bum = 1;
	}

	if(bum)
	{
		for(i=0;i<4;i++) sum += tics[i] + tacs[i];
		Freq = (80L * ADC_FREQ) / sum;
	}

	prevolt[chan] = Volt;
	tic[chan]++;
	tac[chan]++;

	return Freq;
}




void Current_count(int sens)
{
  float Numb,Current,fAmpl;
  static float aCurrent,Amplitude;
  static int prezer0=0;
  int chan, pair, ist, thrd, i, ignor;
  int freq=0;

  ERROR error;

	error.all = 0;

	chan = sens - MAX_TPL_CANAL;
	pair = sens_pair[sens] - MAX_TPL_CANAL;
	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[sens];

	if(cTermoCal||cSetZero)
	{
		if(!prezer0)
		for(i=0;i<4;i++) lev_count[i] = Numb;
		lev_count[chan] += (Numb-lev_count[chan])/1000.0;
		adc0[sens] = (int)(filterbat(&filter[sens],lev_count[chan]));
		Zero_lev[chan] = adc0[sens];
		Modbus[sens+DATASTART].all = adc0[sens];
	}
	prezer0 = (cTermoCal||cSetZero);

	Current = (Numb - adc0[sens]) * tmpK[sens];

	if(!(cTermoCal||cSetZero))
	{
		freq = input_freq(chan,Numb);


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

//		Запомним
		if(ist)	
		{
			aCurrent = -Current;	// Запомнили мгновенное значение - дла амплитуды
		}
		else	
		{
//			Вычисление амплитуды
			Amplitude = im_calc(Current,aCurrent);
			fAmpl = filterbat(&filter[sens],Amplitude);

			if(fAmpl<100)
			{
				fAmpl=0;	freq=0;
			}


//			Modbus[sens+DATASTART-1].all   = (int)fAmpl;//(int)Amplitude;
			Modbus[sens+DATASTART-1].all = (int)(fAmpl/RADIX2);

//			Третья фаза для проверок
			lev_count[thrd] += (fabs(-Current-aCurrent)-lev_count[thrd])/1000.0;

i=(8-((sens+DATASTART-1)%8));
Modbus[sens+DATASTART+i-1+(thrd-4)*3].all = lev_count[chan];
Modbus[sens+DATASTART+i  +(thrd-4)*3].all = lev_count[pair];
Modbus[sens+DATASTART+i+1+(thrd-4)*3].all = lev_count[thrd];

		}

		Modbus[sens+DATASTART].all = freq;

//		Зашиты!
		if(Current > 1.1 * sens_hi_edge[sens])
		{
				error.bit.Hyper	= 1;
				error.bit.Stop = 1;
		}

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

		ignor = sens_error[sens].bit.Ignor;


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

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

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

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

	reset_errs(sens,error);

}

void Temper_count(int chan)
{
  float Numb;
  int Temper;
  int ignor;
  ERROR error;
  int zer0;

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

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

	Numb = ADC_table[chan];

	if(cTermoCal)
	{
		Modbus[chan+DATASTART].all = (int)(Numb);
		return;	//	штобы структура ошибок не влезала в данные
	}
	
	Numb = (Numb-adc0[chan])*tmpK[chan]+tmp0[chan]-273;
	Modbus[chan+DATASTART].all = (int)(Numb*10);

	Temper = (int)Numb;

	error.all = 0; 
	if(sens_type[chan]==TERMO_AD) zer0=500;
	if(sens_type[chan]==TERMO_RS) zer0=10;
	error = control_ADC(chan, ADC_table[chan], zer0);

	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;
	}

	Numb = ADC_table[chan];

	if(cTermoCal)
	{
		Modbus[chan+DATASTART].all = (int)(Numb);
		return;	//	штобы структура ошибок не влезала в данные
	}

	Power = (Numb * tmpK[chan]+5)/10.0; //  powK[sens_type[chan]];

	Modbus[chan+DATASTART].all = Power;
	
	error.all = 0;
	ignor = sens_error[chan].bit.Ignor;

	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(chan>3)
	{
		bitt = (chan-4)*2;
		error.bit.Contr1 = er_anal(((Inputs.all>>bitt)&1), &din_count[bitt], 1000, 0); bitt++;
		error.bit.Contr2 = er_anal(((Inputs.all>>bitt)&1), &din_count[bitt], 1000, 0);
	}

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