#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 без этого будет крив!!!

unsigned int CanPowse,CanGO;

unsigned int Maska[2][8]; 

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

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

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

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];
FILTERBAT zer_filter[4];

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_blink=0, count_bright=0, count_mode, 
  blink_over, blink_alarm, work_lamp, heat_lamp, errr_lamp;
  static int preTest;

	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((!sig.bit.Error)|(cTestLamp))	toggle_READY();
	else								set_READY();

	if(Read_Log) return;

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

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

		dat_LMP1(work_lamp);
		dat_LMP2(heat_lamp);
	}

	if(count_bright == Brightness)
	if(!cTestLamp)
	{
		clear_LMP1();
		clear_LMP2();
	}

	if(cTestLamp & !preTest) 
	{
		count_blink = period_blink; 
		count_mode = 0;
	}
	preTest = cTestLamp;
	
	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(cTestLamp){toggle_READY();
toggle_LED1();toggle_LED2();
toggle_RES1();toggle_RES2();}
*/
	}

	if(cExtLamp)
	{
		work_lamp = cExtLite;
		heat_lamp = cExtLite;
	}
	else if(cTestLamp)
	{
		work_lamp = blink_over;
		heat_lamp = blink_over;
	}
	else
	{
		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;
							work_lamp  = 1;

}	}

void Init_sensors()
{
  int i;

	period_blink = READY_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;
	}

#ifndef MODUL

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

		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;
	}
#else	// MODUL
	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_RS;
		sens_type[5]=TERMO_RS;
		sens_type[6]=TERMO_RS;
		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;

		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)
	{
		sens_type[0]=TERMO_AD;
		sens_type[1]=TERMO_AD;
		sens_type[2]=TERMO_AD;
		sens_type[3]=TERMO_AD;

		sens_type[4]=TERMO_RS;
		sens_type[5]=TERMO_RS;
		sens_type[6]=TERMO_RS;
		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_INV2)
	{
		sens_type[8]=TERMO_RS;
		sens_type[9]=TERMO_RS;
		sens_type[10]=TERMO_RS;
		sens_type[11]=TERMO_RS;
	}

#endif	// MODUL

	for(i=0;i<4; i++) err_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<4; i++) zer_filter[i] = def_FILTERBAT;

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

	MAX_TPL_CANAL = 12;
}

void Init_packMask()
{
  int i,j;

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

	for(i=0;i<24;i++)
	if(sens_type[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];	//	Уставки

	Maska[m_FAST][1]|=0x0080;				//	Дискретные входы

	Maska[m_SLOW][6] = 0x0070;				//	Яркость ламп, период посылок

	if(Mode<adr_INV1)
	Maska[m_SLOW][7]|= 0x000F;				// Нули датчиков

	Maska[m_SLOW][7]|= 0xE000;				// Адрес, команды, и чтоб не вылазило

}

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 int presum = 800;
  static float FFreq = 500.0;

  int i,sum=0,bum=0;

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

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

	if(bum)
	{
		for(i=0;i<4;i++) sum += tics[i] + tacs[i];

		if(sum > presum+4) sum = presum+4;
		if(sum < presum-4) sum = presum-4;
		presum = sum;

		FFreq += ((80.0 * ADC_FREQ) / sum - FFreq)/16;
	}

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

	return (int)FFreq;
}

void Current_count(int sens)
{
  float Numb,Current,Deist;
  static float aCurrent,Amplitude;
  static int hay = 0;
  int chan, pair, ist, thrd, fazz, 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);
	fazz = (sens/8 + 1)*8 + thrd*3;
	thrd=  thrd + 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[sens];

	freq = input_freq(chan,Numb);

modbus[0x68+chan] = Numb;

	zer_count[chan] += (Numb-zer_count[chan])/(5.0 * ADC_FREQ);
	adc0[sens] =  filterbat(&zer_filter[chan],zer_count[chan]);

	if(!hay)
	{
		Zeroes[chan] = adc0[sens];
	}

	if(cTermoCal)
	{
		sens_data[sens] = adc0[sens];
		return;
	}

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

	lev_quadr[chan] += ((Current*Current)-lev_quadr[chan])/(1.0 * ADC_FREQ);
	lev_count[chan] = sqrt(lev_quadr[chan]);

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

		hay = (Deist> sens_lo_edge[sens]);

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

		sens_data[sens-1] = Deist;
		sens_data[sens] = freq;

//		Третьа фаза дла проверок
		lev_quadr[thrd] += ((Current+aCurrent)*(Current+aCurrent)-lev_quadr[thrd])/(1.0 * ADC_FREQ);
		lev_count[thrd] = sqrt(lev_quadr[thrd]);

		sens_data[fazz  ] = lev_count[pair];
		sens_data[fazz+1] = lev_count[chan];
		sens_data[fazz+2] = lev_count[thrd];
	}

//	Зашиты!
	if(Current/RADIX2 > 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) && hay,
				&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) && hay,
				&err_count[thrd],time_1_5sec,0))
	{
		error.bit.Wry = 1;
		if(!ignor)
		error.bit.Stop = 1;
	}

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

		if(Amplitude/RADIX2 < 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;
		sens_data[chan] = 0;
		return;
	}

	Numb = ADC_table[chan];

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

	Temper = (int)Numb;

	error.all = 0; 
	if(sens_type[chan]==TERMO_AD) zer0=500;
	if(sens_type[chan]==TERMO_RS) zer0=100;
	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);

}