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

#include "RS485.h"
#include "filter_bat2.h"
#include "measure.h"
#include "message.h"
#include "package.h"
#include "peripher.h"
#include "crc16.h"
#include "isolatio.h"
#include <math.h>	//	Это чтобы мерить амплитуду! sqrt без этого будет крив!!!

OPTOCANAL opt[2];

ISOLATION iso[2];

float iso_KOEF[2] = {-0.5, -0.5};

void init_isolation_struct(void);

void DCLK(int i, int x)
{
	x=!x;

	if(i)
	{
		if(x)	GpioDataRegs.GPASET.bit.GPIO26=1;
		else	GpioDataRegs.GPACLEAR.bit.GPIO26=1;
	}
	else
	{
		if(x)	GpioDataRegs.GPBSET.bit.GPIO32=1;
		else	GpioDataRegs.GPBCLEAR.bit.GPIO32=1;
}	}

int DIN(int i)
{
	if(i)	return 	!GpioDataRegs.GPBDAT.bit.GPIO52;
	else	return 	!GpioDataRegs.GPADAT.bit.GPIO23;
}       

void BLIN(int i)
{
	if(i)	GpioDataRegs.GPBTOGGLE.bit.GPIO53=1;
	else	GpioDataRegs.GPATOGGLE.bit.GPIO24=1;
}       

interrupt void cpu_timer1_isr_ISOL(void)
{
  ERROR error;
  float Riso=0; 
  long numb=0;
  int i;
  static unsigned int  count_ready=0;

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

	if(!cReset)	ServiceDog();

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

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

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

	for(i=0;i<2;i++)
	{
		iso[i].pause_counter++;

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

		if(opt[i].Wait)
		{
			opt[i].Wait--; 
			opt[i].bit = 0;
			opt[i].clk = 0; 
			DCLK(i,0);
			continue;
		}

		opt[i].clk=!opt[i].clk; 
		DCLK(i,opt[i].clk);
		if(!opt[i].clk) 
		{
	    	opt[i].Numb = (opt[i].Numb<<1) | DIN(i);
			if(++opt[i].bit>=32) 
			{
				error.all = 0;

				opt[i].Wait = (TELE_FREQ/1000)*optopowse;
				opt[!i].Wait =(TELE_FREQ/2000)*optopowse;
				if(get_crc32(&(opt[i].Numb)))
				{
					numb = opt[i].Numb;
					numb = numb / 256;	// удалаем контрольную сумму

					Riso=numb;
/*
					if(ist[i]) { kff=1; ist[i]=0; }
					else kff = optofiltr;
					fRiso[i] += (Riso-fRiso[i])/kff;
					numb = (long)fRiso[i];
*/
					Modbus[i*2+0x10].all = (int)(numb & 0xFFFF);
					Modbus[i*2+0x11].all = (int)(numb>>16);

					Riso=numb;
					Riso = Riso/1024;//256;	// предположим

					iso[i].adc_value = Riso;	//	на Лешин алгоритм

					Modbus[i+DATASTART].all = Riso;	// какбе
					opt[i].ers = 0; BLIN(i);
				}
				else
				{
					if(++opt[i].ers > 20)
					{
						opt[i].ers = 20;
						error.bit.Tear = 1;
				}	}
			
				reset_errs(i,error);

	}	}	}

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

}
void timer_Init()
{
	EALLOW;  // This is needed to write to EALLOW protected registers
	PieVectTable.XINT13 = &cpu_timer1_isr_ISOL;
	EDIS;    // This is needed to disable write to EALLOW protected registers

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

	period_ready = TELE_FREQ / (READY_FREQ * 2);

	init_isolation_struct();
	Calcul_Iso_Koef();
}

////////////////////////////////////////////////////////////////////////////////////////////

void Calcul_Iso_Koef()
{
	iso_KOEF[0] = (isoMOM[1]-isoMOM[0]);
	iso_KOEF[0]/= (isoADC[1]-isoADC[0]);

	iso_KOEF[1] = (isoMOM[3]-isoMOM[2]);
	iso_KOEF[1]/= (isoADC[3]-isoADC[2]);
}

void init_isolation_struct(void)
{
  int i;
  int* pint = (int*)iso;
  int size = sizeof(ISOLATION)/sizeof(int);

	for(i=0; i < size*2; i++)	pint[i] = 0;

	iso[1].pause_counter = ISO_PAUSE / 2;
}

#define iso_time_pause 6200

void isolation_calc(void)
{
  float our_delta;
  static int i;
  int k, l, m;
  int buff[BUFF_LEN];

	for(i=0; i<2; i++)
		
	if(iso[i].pause_counter > ISO_PAUSE)	//Поскольку всё медленно, сохранаю значениа раз 3 секунды
	{														//чтобы немьше отсчётов попадало, навыбросы
		iso[i].pause_counter = 0;

		iso[i].buff[iso[i].buff_position] = iso[i].adc_value;

//		Это идет сортировка всего массива данных	---------------------------

		for (k = 0; k < BUFF_LEN; k++)	buff[k] = 0x8000; //minimal int
		for (k = 0; k <=iso[i].prim_position; k++)
		{
			for (l = 0; (l < BUFF_LEN) && (iso[i].buff[k] < buff[l]); l++);
		    if (l < BUFF_LEN)
			{
				for (m = BUFF_LEN - 1; (m > l) && (m > 0); m--)	buff[m] = buff[m-1];
			    buff[l] = iso[i].buff[k];
		}	}

//		-----------------------------------------------------------------------

		m = (buff[0]+buff[iso[i].prim_position])/2;

		for(k=0; k < iso[i].prim_position; k++) if( buff[k] <= m) break;

		iso[i].max_val = buff[k - 1 - iso[i].cycles];

		iso[i].min_val = buff[k + iso[i].cycles];

/*		Так было
		iso[i].max_val = buff[iso[i].cycles];
        iso[i].min_val = buff[iso[i].prim_position - iso[i].cycles];
*/
		our_delta = iso[i].max_val - iso[i].min_val;


//		Новый алгоритм счета МОмов	-------------------------------------------

	if(!iso[i].cycles)
		iso[i].MOms_x_10 = isoMOM[i*2+1];
	else
		iso[i].MOms_x_10 = (our_delta-isoADC[i*2]) * iso_KOEF[i] + isoMOM[i*2];

//	---------------------------------------------------------------------------

Modbus[26+i].all = iso[i].MOms_x_10;
Modbus[32+i].all = iso[i].adc_value;
Modbus[40+i].all = iso[i].max_val;
Modbus[48+i].all = iso[i].min_val;
Modbus[56+i].all = our_delta;
Modbus[64+i].all = iso[i].prim_position;
Modbus[72+i].all = iso[i].buff_position;
Modbus[80+i].all = iso[i].cycles;

//for(j=0;j<35;j++) Modbus[0x20+j].all = iso[i].buff[j];

//	---------------------------------------------------------------------------

		iso[i].buff_position++;

		if(iso[i].cycles < BUFF_CYCLES)
		{
			iso[i].prim_position = iso[i].buff_position;
			iso[i].cycles = iso[i].prim_position / CYCLE_LEN;
		}
		else
			iso[i].prim_position = BUFF_LEN-1;

		if (iso[i].buff_position >= BUFF_LEN) iso[i].buff_position = 0;
}	}