#include "DSP281x_Examples.h"   // DSP281x Examples Include File
#include "DSP281x_SWPrioritizedIsrLevels.h"   // DSP281x Examples Include File
#include "DSP281x_Device.h"     // DSP281x Headerfile Include File

#include <params.h>
#include <sync_tools.h>
#include <vector.h>

#include "big_dsp_module.h"
#include "MemoryFunctions.h"
#include "Spartan2E_Adr.h"
#include "Spartan2E_Functions.h"
#include "TuneUpPlane.h"
#include "pwm_test_lines.h"
#include "xp_write_xpwm_time.h"
#include "profile_interrupt.h"

#include "edrk_main.h"
#define SIZE_SYNC_BUF 20

//#pragma DATA_SECTION(logbuf_sync1,".fa");
unsigned int logbuf_sync1[SIZE_SYNC_BUF];
unsigned int c_logbuf_sync1=0;

//unsigned int capnum0;
//unsigned int capnum1;
//unsigned int capnum2;
//unsigned int capnum3;

int delta_capnum = 0;
int delta_error = 0;
//int level_find_sync_zero = LEVEL_FIND_SYNC_ZERO;
unsigned int temp;

unsigned int count_error_sync = 0;

unsigned int count_timeout_sync = 0;
//unsigned int  enable_profile_led1_sync = 1;
//unsigned int  enable_profile_led2_sync = 0;

SYNC_TOOLS_DATA sync_data=SYNC_TOOLS_DATA_DEFAULT;


#pragma CODE_SECTION(calculate_sync_detected,".fast_run2");
void calculate_sync_detected(void)
{



//  if (capnum0 > 1000) {
//          return;
//      }
//  sync_data.level_find_sync_zero = LEVEL_FIND_SYNC_ZERO;

    delta_capnum = sync_data.capnum0 - sync_data.capnum1;

    if (delta_capnum > 0) //падает
    {
        sync_data.pwm_freq_plus_minus_zero = -1;//1;

        if (count_error_sync < MAX_COUNT_ERROR_SYNC) {
            count_error_sync++;
            count_error_sync++;
            count_error_sync++;
        } else
            sync_data.local_flag_sync_1_2 = 0;
    }
    else
    if (delta_capnum < 0) //растёт
    {

        if (sync_data.capnum0 > sync_data.level_find_sync_zero)
        {
            delta_error = sync_data.capnum0 - sync_data.level_find_sync_zero;

            if (delta_error > 50) {
                if (count_error_sync < MAX_COUNT_ERROR_SYNC) {
                    count_error_sync++;
                    count_error_sync++;
                    count_error_sync++;
                } else
                    sync_data.local_flag_sync_1_2 = 0;
            } else {
                if (count_error_sync > 0) {
                    count_error_sync--;
                }
                if (count_error_sync == 0)
                    sync_data.local_flag_sync_1_2 = 1;
            }
            sync_data.pwm_freq_plus_minus_zero = 1;
        }
        else
        if (sync_data.capnum0 < sync_data.level_find_sync_zero)
        {

            delta_error = sync_data.level_find_sync_zero - sync_data.capnum0;

            if (delta_error > 50) {
                if (count_error_sync < MAX_COUNT_ERROR_SYNC) {
                    count_error_sync++;
                    count_error_sync++;
                    count_error_sync++;
                } else
                    sync_data.local_flag_sync_1_2 = 0;
            } else {
                if (count_error_sync > 0) {
                    count_error_sync--;
                }
                if (count_error_sync == 0)
                    sync_data.local_flag_sync_1_2 = 1;
            }

            sync_data.pwm_freq_plus_minus_zero = -1;

        }
        else
        {
            sync_data.pwm_freq_plus_minus_zero = 0;
            sync_data.local_flag_sync_1_2 = 1;
            count_error_sync = 0;
        }
    } else
        sync_data.pwm_freq_plus_minus_zero = 0;

    sync_data.delta_error_sync = delta_error;
    sync_data.delta_capnum = sync_data.capnum0 - sync_data.level_find_sync_zero; //delta_capnum;
    sync_data.count_error_sync = count_error_sync;


}


#pragma CODE_SECTION(sync_detected,".fast_run2");
void sync_detected(void)
{

#if(_ENABLE_PWM_LINES_FOR_TESTS_SYNC)
    PWM_LINES_TK_18_ON;
#endif

    sync_data.latch_interrupt = 1;
    //    stop_sync_interrupt();


    //    i_led2_on_off(1);

        //  //Enable more interrupts from this capture
    //      EvbRegs.EVBIMRC.bit.CAP6INT = 0;

  //      if (edrk.disable_interrupt_sync==0)


//        WriteMemory(ADR_SAW_REQUEST, 0x8000);
//        sync_data.capnum0 = ReadMemory(ADR_SAW_VALUE);

//        WriteMemory(ADR_SAW_REQUEST, 0x8000);
//        sync_data.capnum0  = ReadMemory(ADR_SAW_VALUE);

   //     pause_1000(1);

        WriteMemory(ADR_SAW_REQUEST, 0x8000);
        sync_data.capnum1 = ReadMemory(ADR_SAW_VALUE);

        WriteMemory(ADR_SAW_REQUEST, 0x8000);
        sync_data.capnum1 = ReadMemory(ADR_SAW_VALUE);

        sync_data.count_timeout_sync = 0;
        sync_data.timeout_sync_signal = 0;

        logbuf_sync1[c_logbuf_sync1++] = sync_data.capnum0;
//        logbuf_sync1[c_logbuf_sync1++] = sync_data.capnum1;

        if (c_logbuf_sync1==SIZE_SYNC_BUF)
            c_logbuf_sync1=0;


        if (sync_data.count_pause_ready < MAX_COUNT_PAUSE_READY) {
            sync_data.count_pause_ready++;
            sync_data.count_pause_ready++;
        } else
            sync_data.sync_ready = 1;

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

  //      calculate_sync_detected();



//        sync_data.capnum0 = capnum0;



    //
    //    stop_sync_interrupt();
    //    EvbRegs.EVBIFRC.all = BIT2;
    //


    //    // Acknowledge interrupt to receive more interrupts from PIE group 5
  //      PieCtrlRegs.PIEACK.all = PIEACK_GROUP5;

    //    if (edrk.disable_interrupt_sync==0)
    //    {
    ////    //Enable more interrupts from this capture
    ////    EvbRegs.EVBIMRC.bit.CAP6INT = 1;
    ////
    ////    //use mask to clear EVAIFRA register
    ////    EvbRegs.EVBIFRC.bit.CAP6INT = 1;
    //    }

            //Enable more interrupts from this capture
//            EvbRegs.EVBIMRC.bit.CAP6INT = 1;

            //use mask to clear EVAIFRA register
//            EvbRegs.EVBIFRC.bit.CAP6INT = 1;




    //    DINT;
    //    PieCtrlRegs.PIEIER5.all = TempPIEIER;

    //    return;



    //    IER &= ~(M_INT5);

        //Enable more interrupts from this capture
    //    EvbRegs.EVBIMRC.bit.CAP6INT = 1;

        // Note: To be safe, use a mask value to write to the entire
        // EVAIFRA register.  Writing to one bit will cause a read-modify-write
        // operation that may have the result of writing 1's to clear
        // bits other then those intended.
        //use mask to clear EVAIFRA register
    //  EvbRegs.EVBIFRC.bit.CAP6INT = 1;
    //    EvbRegs.EVBIFRC.all = BIT2;

     //   IER &= ~(M_INT5);


    //    asm(" NOP;");

    //    i_led2_on_off(0);


    //  start_sync_interrupt();
    //    EvbRegs.EVBIMRC.bit.CAP6INT = 1;
        // Clear CAPINT6 interrupt flag

            // Acknowledge interrupt to receive more interrupts from PIE group 5
   //         PieCtrlRegs.PIEACK.all = PIEACK_GROUP5;

        sync_data.count_interrupts++;

#if(_ENABLE_PWM_LINES_FOR_TESTS_SYNC)
    PWM_LINES_TK_18_OFF;
#endif

}


//static long k_3=50;

#pragma CODE_SECTION(Sync_handler,".fast_run2");
interrupt void Sync_handler(void) {

    // Set interrupt priority:
    volatile Uint16 TempPIEIER = PieCtrlRegs.PIEIER5.all;
    IER |= M_INT5;
    IER &= MINT5;                       // Set "global" priority
    PieCtrlRegs.PIEIER5.all &= MG57;   // Set "group"  priority
    PieCtrlRegs.PIEACK.all = 0xFFFF;   // Enable PIE interrupts

    WriteMemory(ADR_SAW_REQUEST, 0x8000);
    sync_data.capnum0 = ReadMemory(ADR_SAW_VALUE);

    stop_sync_interrupt_local(); // запретили прерывания, разрешим их по таймеру, чтоб избежать дребезга при плохой оптике.


#if (_ENABLE_INTERRUPT_PROFILE_LED1)
    if (profile_interrupt.for_led1.bits.sync)
    i_led1_on_off_special(1);
#endif
#if (_ENABLE_INTERRUPT_PROFILE_LED2)
    if (profile_interrupt.for_led2.bits.sync)
    i_led2_on_off_special(1);
#endif

    EINT;

//    i_led2_on_off(1);

    // Insert ISR Code here.......
    sync_detected();
//    pause_1000(k_3);
    // Next line for debug only (remove after inserting ISR Code):
    //ESTOP0;

//    i_led2_on_off(0);

    // Enable more interrupts from this timer
//     EvbRegs.EVBIMRC.bit.CAP6INT = 1;
     // Note: To be safe, use a mask value to write to the entire
      // EVBIFRA register. Writing to one bit will cause a read-modify-write
      // operation that may have the result of writing 1's to clear
      // bits other then those intended.
      EvbRegs.EVBIFRC.all = BIT2;
     // Acknowledge interrupt to recieve more interrupts from PIE group 5
//      PieCtrlRegs.PIEACK.all |= PIEACK_GROUP5;

    // Restore registers saved:
    DINT;
    PieCtrlRegs.PIEIER5.all = TempPIEIER;

#if (_ENABLE_INTERRUPT_PROFILE_LED1)
    if (profile_interrupt.for_led1.bits.sync)
    i_led1_on_off_special(0);
#endif
#if (_ENABLE_INTERRUPT_PROFILE_LED2)
    if (profile_interrupt.for_led2.bits.sync)
    i_led2_on_off_special(0);
#endif


}

void setup_sync_int(void) {

//       return;

//    EALLOW;

    if (edrk.flag_second_PCH==1)
        sync_data.level_find_sync_zero = xpwm_time.pwm_tics+5;
    else
        sync_data.level_find_sync_zero = LEVEL_FIND_SYNC_ZERO;

    sync_data.timeout_sync_signal = 0;
    sync_data.count_timeout_sync = 0;

//    sync_data.what_main_pch = 1; // Первый ПЧ
//    sync_data.what_main_pch = 2; // Второй ПЧ
    sync_data.what_main_pch = 0; // Любой ПЧ



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

//    EvbRegs.EVBIFRC.bit.CAP6INT = 1; //Resets flag  EVB Interrupt Flag Register
    EvbRegs.EVBIFRC.all = BIT2; //Resets flag  EVB Interrupt Flag Register
    EvbRegs.EVBIMRC.bit.CAP6INT = 0; //1 //SET flag EVB Interrupt Mask Register C
    // CAP6INT ENABLE
    //0 Disable
    //1 Enable


/////////////////////////////////////////////
	EvbRegs.T4PR = 0xFFFF;          //Set timer period
	EvbRegs.T4CNT = 0;              // Clear timer counter

	EvbRegs.T4CON.all = 0;          // Disable timer
	EvbRegs.T4CON.bit.FREE = 1;      //  FREE/SOFT, 00 = stop immediately on emulator suspend
	EvbRegs.T4CON.bit.SOFT = 0;
	EvbRegs.T4CON.bit.TMODE = 2;    //TMODEx, 10 = continuous-up count mode
	EvbRegs.T4CON.bit.TPS = 0;      //TPSx, 111 = x/1 prescaler
	EvbRegs.T4CON.bit.TENABLE = 0;      // TENABLE, 1 = enable timer
	EvbRegs.T4CON.bit.TCLKS10 = 0;     //TCLKS, 00 = HSPCLK is clock source
    EvbRegs.T4CON.bit.TCLD10 = 0;     //Timer compare register reload condition, 00 When counter is 0
	EvbRegs.T4CON.bit.TECMPR = 1;   // TECMPR, 1 = Enable timer compare operation
	EvbRegs.T4CON.bit.SET3PR = 0;  // SELT3PR: 0 - Use own period register


////////////////////////////////////////////////////
	EvbRegs.CAPCONB.all = 0;         // Clear

	EvbRegs.CAPCONB.bit.CAP6EDGE = 2;         //3:2   Edge Detection for Unit 6
	//Edge detection control for Capture Unit 6.
//	00 No detection
//	01 Detects rising edge
//	10 Detects falling edge
//	11 Detects both edges

	EvbRegs.CAPCONB.bit.CAP6TSEL = 0;  // GP Timer selection for Unit 6
//	GP timer selection for Capture Units 4 and 5
//	0 Selects GP timer 4
//	1 Selects GP timer 3
	EvbRegs.CAPFIFOB.bit.CAP6FIFO = 0; //CAP6 FIFO status
	EvbRegs.CAPCONB.bit.CAP6EN = 1;  //Enables Capture Unit 6
/////////////////////////////////////////


	EALLOW;
	PieVectTable.CAPINT6 = &Sync_handler;  //?CAP?????????????????
	EDIS;


	PieCtrlRegs.PIEIER5.bit.INTx7 = 1;



}

void start_sync_interrupt(void)
{
    EvbRegs.EVBIFRC.all = BIT2; //Resets flag  EVB Interrupt Flag Register

    IER |= M_INT5;      // @suppress("Symbol is not resolved")
//    PieCtrlRegs.PIEIER5.bit.INTx7 = 1;

    EvbRegs.EVBIMRC.bit.CAP6INT = 1; //SET flag EVB Interrupt Mask Register C
    //    //use mask to clear EVAIFRA register
//    PieCtrlRegs.PIEACK.all |= PIEACK_GROUP5;
    sync_data.latch_interrupt = 0;
    sync_data.enabled_interrupt = 1;

}

void stop_sync_interrupt(void)
{
    sync_data.latch_interrupt = 0;
    sync_data.enabled_interrupt = 0;

    IER &= ~(M_INT5);      // @suppress("Symbol is not resolved")
//    PieCtrlRegs.PIEIER5.bit.INTx7 = 0;
    EvbRegs.EVBIMRC.bit.CAP6INT = 0; //SET flag EVB Interrupt Mask Register C
    EvbRegs.EVBIFRC.all = BIT2; //Resets flag  EVB Interrupt Flag Register
//    PieCtrlRegs.PIEACK.all |= PIEACK_GROUP5;
}

void stop_sync_interrupt_local(void)
{
    sync_data.latch_interrupt = 0;

    IER &= ~(M_INT5);      // @suppress("Symbol is not resolved")
    EvbRegs.EVBIMRC.bit.CAP6INT = 0; //SET flag EVB Interrupt Mask Register C
    EvbRegs.EVBIFRC.all = BIT2; //Resets flag  EVB Interrupt Flag Register
}


void setup_sync_line(void) {

    // output
	EALLOW;
	GpioMuxRegs.GPBMUX.bit.TCLKINB_GPIOB12 = 0;
	GpioMuxRegs.GPBDIR.bit.GPIOB12 = 1;
	EDIS;

	//input
	EALLOW;
	GpioMuxRegs.GPBMUX.bit.CAP6QI2_GPIOB10 = 1;// Configure as CAP6
//	GpioMuxRegs.GPBDIR.bit.GPIOB10 = 1;
	EDIS;

}

#pragma CODE_SECTION(sync_inc_error,".fast_run");
void sync_inc_error(void)
{


    if (sync_data.count_pause_ready > 0) {
        sync_data.count_pause_ready--;
    } else
        sync_data.sync_ready = 0;


    if (sync_data.count_timeout_sync < MAX_COUNT_TIMEOUT_SYNC)
    {
        sync_data.count_timeout_sync++;
    }
    else
    {
        sync_data.timeout_sync_signal = 1;
        sync_data.count_pause_ready   = 0;
        sync_data.local_flag_sync_1_2 = 0;
    }


	if (count_error_sync < MAX_COUNT_ERROR_SYNC) {
		count_error_sync++;
	} else
		sync_data.local_flag_sync_1_2 = 0;
}

void clear_sync_error(void)
{
    sync_data.count_timeout_sync = 0;
    sync_data.timeout_sync_signal = 0;
}


int get_status_sync_line(void)
{
  return !GpioDataRegs.GPBDAT.bit.GPIOB10;
}

//int index_sync_ar = 0;
//
//
//void write_sync_logs(void)
//{
// static int c=0;
//	 return;
//
////	 logbuf1[index_filter_ar]=active_rect1.Id;//EvaRegs.CMPR1;//(active_rect1.pll_Ud);//svgenDQ.Wt;
////	 logbuf2[index_filter_ar]=active_rect1.Iq;//EvaRegs.CMPR2;//filter.iqU_1_long;// (active_rect1.pll_Uq);//Iq;
////	 logbuf3[index_filter_ar]=EvaRegs.CMPR1;//active_rect1.SetUzpt;////(active_rect1.Tetta);//abc_to_dq.Ud;
//
//     index_sync_ar++;
//     if (index_sync_ar>=SIZE_SYNC_BUF)
//	 {
//	        index_sync_ar=0;
//			c++;
//			if (c>=10)
//			  c=0;
//     }
//
//}
