#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 isolation1 = ISOLATION_DEFAULT;
ISOLATION isolation2 = ISOLATION_DEFAULT;

static void calibrateIsolation();
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,kff=1; 
  static float fRiso[2];
  static int ist[2] = {1,1};
  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

	ServiceDog();

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

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

	isolation1.pause_counter += 1;
	isolation2.pause_counter += 1;

	for(i=0;i<2;i++)
	{
		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);

					if (i == 0) {
						isolation1.adc_value = numb;
					} else {
						isolation2.adc_value = numb;
					}
					Riso=numb;
					Riso = Riso/256;	// предположим
					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();

}

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

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

#define COEFF_ACP_TO_MOMx10_NEW 1033584219L
#define COEFF_ACP_TO_MOMx10_OLD 1644429312L

//#define COEFF_ACP_TO_MOMx10 	1604321280L //Получаетси из разрйадности АЦП, коэфф. усилен., измерительного сопротивлению
#define COEFF_ACP_TO_MOMx10_1	COEFF_ACP_TO_MOMx10_NEW	//АЦП 24-битный, 15 значащий разрйадов
											//R = 5100 Om
#define COEFF_ACP_TO_MOMx10_2 	COEFF_ACP_TO_MOMx10_NEW	//COEFF_ACP_TO_MOMx10_OLD //; 32 - 1668494131L



int calibration1 = 0; 	//ПЧ 31 - 0; 32 - (-2); 21 - 1
int calibration2 = 0;	//ПЧ 31 - 3; 32 - (-4);	//21 - 1


void init_isolation_struct(void)
{
	int* pint = (int*)&isolation1;
	int i = 0;
	int size = sizeof(isolation1)/sizeof(int);
	for(i = 0; i < size; i++)
	{
		*(pint + i) = 0;
	}
	pint = (int*)&isolation2;
	size = sizeof(isolation2)/sizeof(int);
	for(i = 0; i < size; i++)
	{
		*(pint + i) = 0;
	}
	isolation1.f_not_ready = 1;
	isolation2.f_not_ready = 1;
	isolation1.pause_time = 12400;
	isolation2.pause_time = 12400;
}

#define iso_time_pause 6200

void isolation_calc(void)
{
	volatile unsigned long int acp_val1_l, acp_val2_l;
	long int tmp_delta;
	int acp_val1, acp_val2, k, l, m;
	int buff[IS_BUFF_SIZE];

	

//	calibrateIsolation();
		
	if(isolation1.pause_counter > isolation1.pause_time)	//Поскольку всё медленно, сохраняю значения раз 3 секунды
	{														//чтобы немьше отсчётов попадало, навыбросы
		isolation1.pause_counter = 0;
		acp_val1_l = isolation1.adc_value;
		acp_val1 = (int)(acp_val1_l >> 10);	//Уменьшаю размер, дабы не иметь дело с long
		if(acp_val1)
		{
			isolation1.buff[isolation1.buff_position++] = acp_val1;
		}
		else
		{
			if (isolation1.buff_position > 0) {
				isolation1.buff[isolation1.buff_position] = isolation1.buff[isolation1.buff_position - 1];
			} else {
				isolation1.buff[isolation1.buff_position] = isolation1.buff[IS_BUFF_SIZE - 1];
			}
			++isolation1.buff_position;
		}
		
		if(isolation1.buff_position >= IS_BUFF_SIZE) 
		{
			isolation1.buff_position -= IS_BUFF_SIZE;
			isolation1.f_not_ready = 0;
//			if (isolation1.max_val || isolation1.min_val) {
//			}
		}

		for (k = 0; k < IS_BUFF_SIZE; k++) {
		    buff[k] = 0x8000; //minimal int
		}
		k = 0;
		l = 0;
		while (k < IS_BUFF_SIZE) {
		    l = 0;
		    while ((l < IS_BUFF_SIZE) && (isolation1.buff[k] < buff[l])) {
		        l++;
		    }
		    if (l < IS_BUFF_SIZE) {
		        for (m = IS_BUFF_SIZE - 1; (m > l) && (m > 0); m--) {
		            buff[m] = buff[m - 1];
		        }
		    }
		    buff[l] = isolation1.buff[k];
		    k++;
		}
		isolation1.max_val = buff[5];
        isolation1.min_val = buff[IS_BUFF_SIZE - 5];
		k = 0;
//        while(k < IS_BUFF_SIZE)
//        {
//            if((isolation1.buff[(k + 1) % IS_BUFF_SIZE] - isolation1.buff[k]) > 1500)
//            {
//                isolation1.max_val = isolation1.buff[(k + 7) % IS_BUFF_SIZE];
//                isolation1.min_val = isolation1.buff[(k) % IS_BUFF_SIZE];
//                break;
//            }
//            k++;
//        }
		tmp_delta = isolation1.max_val - isolation1.min_val;
		if(!isolation1.f_not_ready)
		{
			isolation1.row_MOms = tmp_delta ? COEFF_ACP_TO_MOMx10_1 / (tmp_delta << 10) - 200 + calibration1
														: isolation1.row_MOms;
			if (isolation1.row_MOms > isolation1.MOms_x_10) {
			    isolation1.MOms_x_10 += 2;   //isolation1.row_MOms;
			}
			else if (isolation1.row_MOms < isolation1.MOms_x_10) {isolation1.MOms_x_10 -= 1;}

		} else {
			isolation1.MOms_x_10 = tmp_delta ? COEFF_ACP_TO_MOMx10_1 / (tmp_delta << 10) - 200 + calibration1
														: isolation1.MOms_x_10;
		}
		if (isolation1.MOms_x_10 > 50) { isolation1.MOms_x_10 = 50;}
		if (isolation1.MOms_x_10 < 0) { isolation1.MOms_x_10 = 0;}
		if (isolation1.f_not_ready == 0) {
			Modbus[26].all = isolation1.MOms_x_10;
		}
	}
	//Втой канал
	if(isolation2.pause_counter > isolation2.pause_time)
	{
		isolation2.pause_counter = 0;
		acp_val2_l = isolation2.adc_value;
		acp_val2 = (int)(acp_val2_l >> 10);
		if(acp_val2)
		{
			isolation2.buff[isolation2.buff_position++] = acp_val2;
		}
		else
		{
			if (isolation2.buff_position > 0) {
				isolation2.buff[isolation2.buff_position] = isolation2.buff[isolation2.buff_position - 1];
			} else {
				isolation2.buff[isolation2.buff_position] = isolation2.buff[IS_BUFF_SIZE - 1];
			}
			++isolation2.buff_position;
		}
		if(isolation2.buff_position >= IS_BUFF_SIZE) 
		{
			isolation2.buff_position -= IS_BUFF_SIZE;
			isolation2.f_not_ready = 0;
//			if (isolation2.max_val || isolation2.min_val) {
//			}
		}
		for (k = 0; k < IS_BUFF_SIZE; k++) {
            buff[k] = 0x8000;
        }
        k = 0;
        l = 0;
        while (k < IS_BUFF_SIZE) {
            l = 0;
            while ((l < IS_BUFF_SIZE) && (isolation2.buff[k] < buff[l])) {
                l++;
            }
            if (l < IS_BUFF_SIZE) {
                for (m = IS_BUFF_SIZE - 1; (m > l) && (m > 0); m--) {
                    buff[m] = buff[m - 1];
                }
            }
            buff[l] = isolation2.buff[k];
            k++;
        }
        isolation2.max_val = buff[5];
        isolation2.min_val = buff[IS_BUFF_SIZE - 5];
        //
//		k = 0;
//		while(k < IS_BUFF_SIZE)
//		{
//			if((isolation2.buff[(k + 1) % IS_BUFF_SIZE] - isolation2.buff[k]) > 1500)
//			{
//				isolation2.max_val = isolation2.buff[(k + 7) % IS_BUFF_SIZE];
//				break;
//			}
//			k++;
//		}
//		k = 0;
//		while(k < IS_BUFF_SIZE)
//		{
//			if((isolation2.buff[(k + 1) % IS_BUFF_SIZE] - isolation2.buff[k]) < -1500)
//			{
//				isolation2.min_val = isolation2.buff[(k + 7) % IS_BUFF_SIZE];
//				break;
//			}
//			k++;
//		}
		tmp_delta = isolation2.max_val - isolation2.min_val;
		if(!isolation2.f_not_ready) {
			isolation2.row_MOms = tmp_delta ? (COEFF_ACP_TO_MOMx10_2) / (tmp_delta << 10) - 200 + calibration2	//1668494131L
					: isolation2.row_MOms;
			if (isolation2.row_MOms > isolation2.MOms_x_10 ) {
				isolation2.MOms_x_10 += 2;	// isolation2.row_MOms;
			}
			else if (isolation2.row_MOms < isolation2.MOms_x_10 ) {isolation2.MOms_x_10 -= 1; }
		} else {
			isolation2.MOms_x_10 = tmp_delta ? (COEFF_ACP_TO_MOMx10_2) / (tmp_delta << 10) - 200 + calibration2
											: isolation2.MOms_x_10;
		}
		if (isolation2.MOms_x_10 > 50) { isolation2.MOms_x_10 = 50;}
		if (isolation2.MOms_x_10 < 0) { isolation2.MOms_x_10 = 0;}
		if (isolation2.f_not_ready == 0) {
			Modbus[27].all = isolation2.MOms_x_10;
		}
	}
}
/*
void calibrateIsolation() {

	switch (modbus_table_in[280].all) {
	case 12:
			calibration1 = 0;
			calibration2 = 0;
			break;
	case 21:
		calibration1 = -4;
		calibration2 = -1;
		break;
	case 22:
		calibration1 = -5;
		calibration2 = -5;
		break;
	case 31:
		calibration1 = -5;
		calibration2 = -1;
		break;
	case 32:
		calibration1 = 0;
		calibration2 = -2;
		break;
	}
}
*/