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

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

int READY = 1 * ADC_FREQ;
int WAKE = 3 * ADC_FREQ;

long low_count[2] = {0,0};
long err_count[6];
int lev_count[6];
float lev_quadr[6];
float zer_count[4];

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

unsigned int Caliber_time = 0;
int Curr_Edge;

float powK[] = 
{
	0.127,	// питание 380В
	0.127,	// питание 380В

	1.000,	// ток
	1.000,	// напражение

	0.0076,	// питание 31В
	0.0076,	// питание 24В
	0.0076,	// питание 24В
	0.0076,	// питание 15В
	1.0000,	// термодатчик мелкосхема
};

FILTERBAT def_FILTERBAT = DEF_FILTERBAT;
FILTERBAT adc_filter[20+4];	//	макс.TPL*2 + 4 калибр
FILTERBAT out_filter[20+4];	//	макс.TPL*2 + 4 напраж
FILTERBAT zer_filter[4];	// 4 напраж

long sens_count[24+4];

float K100,K_50;
float K_T1,K_T2;

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_sensor_koef()
{
  int i;
  float K;

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

	for(i=0;i<4;i++)
	{
		K = Caliber[i];
		if(sens_type[i]==CURRENT)	K/=1000.0;
		else						K/=10000.0;
		powK[i] = K;
}	}

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

  int TST;

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

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

	if(!cReset)	ServiceDog();

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

	TST = cTestLamp|bTestLamp;

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

	if(Desk==dsk_LOAD)
	{
		Load_runner();
		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(!TST)
	if(count_bright == Brightness)
	{
		clear_LED_OUT_1();
		clear_LED_OUT_2();
		clear_RES_OUT_1();
	}

	if(TST & !preTest) 
	{
		count_blink = BLINK_TIME; 
		count_mode = 0;
	}
	preTest = TST;

	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(TST)
		{
			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;
  
	DigErr.all=0;

	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;

		Curr_Edge = 300;
	}

	if(Desk==dsk_COMM)
	{
		Curr_Edge = 200;
		for(i=0;i<4;i++)
		{
			sens_type[i]=VOLTAGE;
			sens_pair[i] = (i&2)| !(i&1);
	}	}

	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;
//		sens_type[15] = TERMO_AD;
	}

	for(i=0;i<6; i++) 
	{
		err_count[i] = 0;
		lev_count[i] = 0;
		lev_quadr[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;
	}
	for(i=0;i<4; i++)	
		zer_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 = TPL_SHK;		cal_addr = tpl_cans-2;	break;
}	}

void Init_sensors_more()
{
  int i;

	for(i=0;i<24;i++) modbus[i] &= NOER;
	for(i=0;i<4; i++) zer_count[i] = Zero_lev[i];

	if(Desk==dsk_LOAD)
	for(i=0;i<4;i++) 
	{
		if(sens_type[i]==VOLTAGE)
		{ 
			if(i&1) sens_data[i]=560;
			else	sens_data[i]=390;
	}	}

 	if(Desk==dsk_COMM)
	{
		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(Desk==dsk_LOAD)
	{
		Modbus[0].bit.bitE = 1;	//	Ignore
		Modbus[1].bit.bitE = 1;	//	Ignore
	}

	if(Kalibro) calc_sensor_koef();
}

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

	can = tpl_cans;

	if(Kurrent)	can+= 4;

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

	switch(Desk)
	{
		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(Kurrent)			
			{					Maska[m_SLOW][7]|= 0x000F;	//	Ноль датчиков напражениа
								Maska[m_FAST][6]|= 0x3F00;	//	Фазные значениа
			}

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

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

int er_anal(int term, long * count, long 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 err)
{
  unsigned int set;

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

void Current_count(int chan)
{
  float Numb,Current,Level;
  static float aCurrent,Amplitude;
  int sens, pair, ist, thrd, fazz, feed, no_ignor;

  ERROR error;

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

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

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

	Numb = adc_table_lem[chan];

modbus[0x64+chan] = Numb;

//	Средний (он же нулевой) уровень показаний АЦП
	zer_count[chan] += (Numb-zer_count[chan])/(5.0 * ADC_FREQ);
	Zero_lev[chan] = (int)(filterbat(&zer_filter[chan],zer_count[chan]));

//	Мгновенное значение фазы
	Current = (Numb - Zero_lev[chan]) * powK[chan];

//	Действующее (среднеквадратичное) значение фазы
	lev_quadr[chan] += ((Current*Current)-lev_quadr[chan])/(1.0 * ADC_FREQ);
	lev_count[chan] = sqrt(lev_quadr[chan]);

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

		if(sens_type[chan]==VOLTAGE)
		if(Amplitude<20) Amplitude = 0;

		if(sens_type[chan]==CURRENT)
		if(Amplitude<75) Amplitude = 0;

		Level = Amplitude / RADIX2;

		sens_data[sens  ] = Amplitude;
		sens_data[sens-1] = Level;

//		Действующее (среднеквадратичное) значение третьей фазы
		Numb =-Current-aCurrent;
		lev_quadr[thrd] += (Numb*Numb-lev_quadr[thrd])/(1.0 * ADC_FREQ);
		lev_count[thrd] = sqrt(lev_quadr[thrd]);
/*
if(!no_write)
{
Test_mem_limit(8);

Log_to_mem(Current);
Log_to_mem(aCurrent);
Log_to_mem(Numb);

Log_to_mem(lev_count[chan]);
Log_to_mem(lev_count[pair]);
Log_to_mem(lev_count[thrd]);

Log_to_mem(Amplitude);
Log_to_mem(Level);
}
*/
modbus[0x68+fazz  ] = lev_count[pair];
modbus[0x68+fazz+1] = lev_count[chan];
modbus[0x68+fazz+2] = lev_count[thrd];
	}

//	Это максимальное
								Numb = lev_count[chan];
	if(Numb<lev_count[pair])	Numb = lev_count[pair];
	if(Numb<lev_count[thrd])	Numb = lev_count[thrd];

//	Зашиты!
	error.all = 0;
	no_ignor = !sens_error[chan].bit.Ignor;

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

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

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

		if(sens_type[chan]==VOLTAGE)

		if(er_anal( (Level < sens_lo_edge[sens]), 
					&low_count[feed],time_1_5sec,0))
		{
			error.bit.Out	= 1;
			error.bit.Stop = no_ignor;
		}

/*		if(Level < sens_lo_edge[sens])
		{
			error.bit.Out	= 1;
			error.bit.Stop = no_ignor;
		}
*/
		if(sens_type[chan]==CURRENT)
		if(Level > sens_lo_edge[sens])
		{
			error.bit.Over	= 1;
	}	}


if(chan==1)
if(error.all)
if(!no_write)
{
Test_mem_limit(10);

Log_to_mem(Current);
Log_to_mem(aCurrent);
Log_to_mem(Numb);

Log_to_mem(lev_count[chan]);
Log_to_mem(lev_count[pair]);
Log_to_mem(lev_count[thrd]);

Log_to_mem(Amplitude);
Log_to_mem(Level);

Log_to_mem(Curr_Edge);
Log_to_mem(error.all);


}




	if(!WAKE)
	reset_errs(sens,error);
}

void Temper_count(int chan)
{
  float Numb;
  static int Temper;
  int i,j, kun, no_ignor;
  long s;
  ERROR error;

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

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

	Numb = adc_table_tpl[chan];

	if(cRawMeat)
	{
		sens_data[chan] = Numb;
		return;
	}

	if(chan<tpl_cans)
	if(sens_error[chan].bit.Discr4)
	{
		j=0;s=0;
		for(i=0;i<(tpl_cans-2*(Mode==adr_ENG1));i++)
		{
			if(!sens_error[i].bit.Bypas)
			if(!sens_error[i].bit.Ignor)
			if(!sens_error[i].bit.Discr4)
			{
				s+=adc_table_tpl[i];
				j++;
		}	}

		if(j) s/=j;
		Numb = s;
	}

	kun = chan - cal_addr;

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

			if(kun>=0)
			{
				TCaliber[kun] = Numb;
				calc_sensor_koef();
				return;
		}	}

		if(chan>tpl_cans) return;

//		Kun именно чтоб скомпенсировать нечетность
		if(kun&1)	Numb = (Numb-K300_2)*K_T2+ZERO;
		else		Numb = (Numb-K300_1)*K_T1+ZERO;
	}

	if(TermoRS)
	{
		if(kun>=0)
		{
			TCaliber[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)
		{
sens_data[chan+8] = adc_table_tpl[chan];
			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 > -20)	if(Numb < 200) 
		Numb = filterbat(&out_filter[chan],Numb*100)/100;
	}

	if(Numb < -20) Numb = -20;
	if(Numb > 200) Numb = 200;

	if(bSecretBt|cSecretBt) Numb = 40.0;

	sens_data[chan] = (int)(Numb*10);

	Temper = (int)Numb;

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

//	Обрыв или неисправность
	if(Temper==-20 || Temper==200)
	{
		error.bit.Tear  = 1;
		chk.bit.OutHeat = no_ignor;
	}
	else

//	Перегрев
  	if(((Temper>sens_hi_edge[chan]-Cooling) && (sens_error[chan].bit.Hyper)) ||
		(Temper>sens_hi_edge[chan]) )
	{  	
		error.bit.Hyper	= 1;
		error.bit.Stop	= no_ignor;
		chk.bit.OverHeat= no_ignor;
	}
	else

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

	if(!WAKE)
	reset_errs(chan,error);
}

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

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

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

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

	bitt = chan*2;

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

	if(chan==6)
	{
		error.bit.Discr3 = er_anal(((DigErr.all>>bitt)&1), &din_count[bitt], 1000, 0); bitt++;
		error.bit.Discr4 = er_anal(((DigErr.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_tpl[chan];

	if(cRawMeat)
	{
		sens_data[chan] = Numb;
		return;
	}

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

	sens_data[chan] = 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;

	if(!WAKE)
	reset_errs(chan,error);
}