#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 READY=0;	//	Все загрузилос

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

int period_ready, period_blink;

FLAG chk,sig;
unsigned int time_1_5sec, time_5msec, time_3sec, time_5sec;

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

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

int ext_temp[8];
int ext_diag[8];

unsigned int Caliber_time = 0;
int Curr_Edge;
ERROR okay;

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;

	if(TermoRS)
	{
		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); 
	}

	if(TermoAD)
	{
		K_T1 = 100.0/(K400_1 - K300_1);
		K_T2 = 100.0/(K400_2 - K300_2);
	}

//	if(Kurrent)
	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;
  
	TPL_CANS = tpl_cans = cal_addr = 0;

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

	pow_addr = tpl_cans + Owen;

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

		adc_filter[i] = def_FILTERBAT;
		out_filter[i] = def_FILTERBAT;
	}

	if(TermoSW)
	for(i=0;i<tpl_cans;i++)
	{	
		if(TermoRS) sens_type[i] = TERMO_RS;
		if(TermoAD) sens_type[i] = TERMO_AD;
	}

	for(i=0;i<Owen;i++)
	{	
		sens_type[tpl_cans+i] = TRM_OBEH;
	}

	if(Kurrent)
	{
		Curr_Edge = 200;

		for(i=0;i<4;i++)
		{
			sens_type[pow_addr+i]=VOLTAGE;
			sens_pair[pow_addr+i]=pow_addr+(i^1);

			zer_count[i] = Zero_lev[i];
			zer_filter[i] = def_FILTERBAT;
	}	}

	if(Desk==dsk_LOAD)
	{
		Curr_Edge = 300;

		sens_type[2]=
		sens_type[3]=CURRENT;
	}

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

	DigErr.all=0;

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

	okay.all=0;
	okay.bit.Tear=1;
	okay.bit.Bypas=1;
	okay.bit.Ignor=1;
	okay.bit.Imit=1;

	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<8; i++) ext_temp[i] = ext_diag[i] = 0;

}	

void Init_sensors_more()
{
  int i;

	for(i=0;i<24;i++) 
	{
		modbus[i] &= NOER;
		sens_data[i]=0;
	}

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

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

 	if(Desk==dsk_COMM)
	{
		Modbus[pow_addr+0].bit.bitE = 1;	//	Ignore
		Modbus[pow_addr+1].bit.bitE = 1;	//	Ignore
		Modbus[pow_addr+2].bit.bitE = 1;	//	Ignore
		Modbus[pow_addr+3].bit.bitE = 1;	//	Ignore
	}

	if(Desk==dsk_LOAD)
	{
		Modbus[0].bit.bitE = 1;	//	Ignore
		Modbus[1].bit.bitE = 1;	//	Ignore
	}

	calc_sensor_koef();
}

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

	can = pow_addr;

	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_SLOW][7]|= 0x00F0;	//	Калибровка датчиков напражениа
								Maska[m_FAST][6]|= 0x3F00;	//	Фазные значениа
			}
			if(Owen)			Maska[m_FAST][6]|= 0x0008;	//	Период опроса OWEN
			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;								//	Адрес, команды, и чтоб не вылазило
}

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

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

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

  ERROR error;

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

	sens = pow_addr + 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);
	Zer = (int)(filterbat(&zer_filter[chan],zer_count[chan]));
	if(WAKE < (WAKE_TIME /2)) Zero_lev[chan] = Zer;
/*
Test_mem_limit(12);
Log_to_mem((int)adc_table_lem[chan]);
Log_to_mem((int)zer_count[chan]);
Log_to_mem((int)Zero_lev[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[sens]==VOLTAGE)
		if(Amplitude<20) Amplitude = 0;

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

		Level = Amplitude / RADIX2;
	
		if(!WAKE)
		{
			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;
	if(WAKE || sens_error[chan].bit.Ignor) goto fin;

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

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

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

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

		if(sens_type[sens]==CURRENT)
		if(Level > sens_lo_edge[sens])
		{
			error.bit.Over	= 1;
	}	}
/*
if((chan==1)||(chan==3))
if(error.all)
if(!no_write)
{
Test_mem_limit(12);
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(Zero_lev[chan]);
Log_to_mem(Curr_Edge);
Log_to_mem(error.all);
Log_to_mem(WAKE);
}
*/
	fin:
	reset_errs(sens,error);
}

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

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

	Numb = ADC_table[chan];

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

	if(sens_error[chan].bit.Imit)
	if(chan<tpl_cans)
	{
		j=0;s=0;

		for(i=0;i<start_data;i++)
		{
			if( sens_type[i]==sens_type[chan])
			if(!(sens_error[i].all & okay.all))
			{
				s+=ADC_table[i];
				j++;
		}	}

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

	kun = chan - cal_addr;

	if(own)
	if(sens_type[chan]==TRM_OBEH)
	{
		ovn = chan-tpl_cans;
		Numb= 0.1 * ext_temp[ovn];
		if(ext_diag[ovn])
		Numb = -100;
	}

	if(!own)
	if(TermoAD)
	{
		if(cTermoCal|bTermoCal)
		if(!WAKE)
		{
			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)
		{
			if(WAKE) out_filter[chan] = adc_filter[chan];
			else
			{
//				Калибровку отфильтруем вторым слоем
				Numb = filterbat(&out_filter[chan],Numb);

				TCaliber[kun] = Numb;
				calc_sensor_koef();
			}
			return;
		}

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

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

	if(bSecretBt|cSecretBt) Numb = 40.0;

	sens_data[chan] = Numb*10;

	Temper = Numb;

	error.all = 0;
	if(WAKE || sens_error[chan].bit.Ignor) goto fin;

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

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

	fin:
	reset_errs(chan,error);
}

void Power_count(int chan)
{
  float Numb;
  int Power,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;
	}

	Numb = ADC_table[chan];

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

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

	sens_data[chan] = Power;

	error.all = 0;
	
	if(WAKE || sens_error[chan].bit.Ignor) goto fin;

//	Дискретные каналы
	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;

//	Пониженное напражение
	if(Power <sens_lo_edge[chan])
	{		
		error.bit.Out	= 1;

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

//	Повышенное напражение
/*	if(Power > sens_hi_edge[chan])
	{		
		error.bit.Hyper	= 1;
		error.bit.Stop = 1;
	}
*/
	if(error.all) 
	chk.bit.Alarm = 1;
	
	fin:
	reset_errs(chan,error);
}