#include "MemoryFunctions.h"
#include "Spartan2E_Adr.h"
#include "TuneUpPlane.h"
#include "x_parallel_bus.h"
#include "x_serial_bus.h"
#include "xp_cds_tk.h"
#include "xp_tools.h"

///////////////////////////////////////////////
///////////////////////////////////////////////
///////////////////////////////////////////////
// 23550
///////////////////////////////////////////////
///////////////////////////////////////////////

int cds_tk_write_sbus_23550(T_cds_tk_23550 *v)
{
	unsigned int old_err, err = 0, err_ready = 0;

    if (v->useit == 0) 
	  return 0;

	old_err = v->status_serial_bus.count_write_error;
	x_serial_bus_project.slave_addr = v->plane_address; // number plate


//3 mask_protect_tk
    x_serial_bus_project.reg_addr 	= 3;   				 			// adr memory in plate
	x_serial_bus_project.write_data = v->write.sbus.mask_protect_tk.all;   // write data

    if  (x_serial_bus_project.write(&x_serial_bus_project))              // make write
		v->status_serial_bus.count_write_error++;


//4 protect_error
//    if (v->read.type_cds_xilinx == TYPE_CDS_XILINX_SP6)
//        v->write.sbus.protect_error.bit.enable_err_switch = 0; // для SP6 отключаем ошибку по разъему, т.к. ее нет пока

    x_serial_bus_project.reg_addr 	= 4;   				 			// adr memory in plate
	x_serial_bus_project.write_data = v->write.sbus.protect_error.all;   // write data

    if  (x_serial_bus_project.write(&x_serial_bus_project))              // make write
		v->status_serial_bus.count_write_error++;

//0 mask_tk_out_40pin
    x_serial_bus_project.reg_addr 	= 0;   				 			// adr memory in plate
	x_serial_bus_project.write_data = v->write.sbus.mask_tk_out_40pin.all;   // write data

    if  (x_serial_bus_project.write(&x_serial_bus_project))              // make write
		v->status_serial_bus.count_write_error++;

//1 dead_min_time
    x_serial_bus_project.reg_addr 	= 1;   				 			// adr memory in plate
	x_serial_bus_project.write_data = v->write.sbus.dead_min_time.all;   // write data

    if  (x_serial_bus_project.write(&x_serial_bus_project))              // make write
		v->status_serial_bus.count_write_error++;

//2 ack_time
    x_serial_bus_project.reg_addr 	= 2;   				 			// adr memory in plate
	x_serial_bus_project.write_data = v->write.sbus.ack_time.all;   // write data

    if  (x_serial_bus_project.write(&x_serial_bus_project))              // make write
		v->status_serial_bus.count_write_error++;





//7 cmd_reset_error
/*
    x_serial_bus_project.reg_addr 	= 7;   				 			// adr memory in plate
	x_serial_bus_project.write_data = v->write.sbus.cmd_reset_error;   // write data

    if  (x_serial_bus_project.write(&x_serial_bus_project))              // make write
		v->status_serial_bus.count_error++;
*/


	if (old_err == v->status_serial_bus.count_write_error)// no errors
	{
	  v->status_serial_bus.count_write_ok++;
	  err = 0; // no errors
	}
	else
	  err = 1; // !errors!

    err_ready = check_cds_ready_sbus( err, ITS_WRITE_BUS, &v->status_serial_bus);
	set_status_cds(err_ready, &v->status);

	return err_ready;


}


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

int cds_tk_read_sbus_23550(T_cds_tk_23550 *v)
{
	unsigned int old_err, err = 0, err_ready = 0;

    if (v->useit == 0) 
	  return 0;

	old_err = v->status_serial_bus.count_read_error;

	x_serial_bus_project.slave_addr = v->plane_address; 			// number plate

//0 mask_tk_out_40pin
    x_serial_bus_project.reg_addr 	= 0;   				 			// adr memory in plate
	x_serial_bus_project.read(&x_serial_bus_project); 				// read

	if (x_serial_bus_project.flags.bit.read_error == 0) // check error
		v->read.sbus.mask_tk_out_40pin.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;
	  

//1 dead_min_time
    x_serial_bus_project.reg_addr 	= 1;   				 			// adr memory in plate
	x_serial_bus_project.read(&x_serial_bus_project); 				// read

	if (x_serial_bus_project.flags.bit.read_error == 0) // check error
		v->read.sbus.dead_min_time.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;
	  

//2 ack_time
    x_serial_bus_project.reg_addr 	= 2;   				 			// adr memory in plate
	x_serial_bus_project.read(&x_serial_bus_project); 				// read

	if (x_serial_bus_project.flags.bit.read_error == 0) // check error
		v->read.sbus.ack_time.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;
	  

//3 mask_protect_tk
    x_serial_bus_project.reg_addr 	= 3;   				 			// adr memory in plate
	x_serial_bus_project.read(&x_serial_bus_project); 				// read

	if (x_serial_bus_project.flags.bit.read_error == 0) // check error
		v->read.sbus.mask_protect_tk.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;
	  
//4 protect_error
    x_serial_bus_project.reg_addr 	= 4;   				 			// adr memory in plate
	x_serial_bus_project.read(&x_serial_bus_project); 				// read

	if (x_serial_bus_project.flags.bit.read_error == 0) // check error
		v->read.sbus.protect_error.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;

//5 status_tk_40pin
    x_serial_bus_project.reg_addr 	= 5;   				 			// adr memory in plate
	x_serial_bus_project.read(&x_serial_bus_project); 				// read

	if (x_serial_bus_project.flags.bit.read_error == 0) // check error
		v->read.sbus.status_tk_40pin.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;

//6 status_tk_96pin
    x_serial_bus_project.reg_addr 	= 6;   				 			// adr memory in plate
	x_serial_bus_project.read(&x_serial_bus_project); 				// read

	if (x_serial_bus_project.flags.bit.read_error == 0) // check error
		v->read.sbus.status_tk_96pin.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;

//7 lock_status_error
    x_serial_bus_project.reg_addr 	= 7;   				 			// adr memory in plate
	x_serial_bus_project.read(&x_serial_bus_project); 				// read

	if (x_serial_bus_project.flags.bit.read_error == 0) // check error
		v->read.sbus.lock_status_error.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;

//8 status_protect_current_ack
    x_serial_bus_project.reg_addr 	= 8;   				 			// adr memory in plate
	x_serial_bus_project.read(&x_serial_bus_project); 				// read

	if (x_serial_bus_project.flags.bit.read_error == 0) // check error
		v->read.sbus.status_protect_current_ack.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;

//11 time_err_tk_all
    x_serial_bus_project.reg_addr 	= 11;   				 			// adr memory in plate
	x_serial_bus_project.read(&x_serial_bus_project); 				// read

	if (x_serial_bus_project.flags.bit.read_error == 0) // check error
		v->read.sbus.time_err_tk_all.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;

//15 current_status_error
    x_serial_bus_project.reg_addr 	= 15;   				 			// adr memory in plate
	x_serial_bus_project.read(&x_serial_bus_project); 				// read

	if (x_serial_bus_project.flags.bit.read_error == 0) // check error
	{
		v->read.type_cds_xilinx	= x_serial_bus_project.read_data & 0x1;
		v->type_cds_xilinx = v->read.type_cds_xilinx;
		v->read.sbus.current_status_error.all = x_serial_bus_project.read_data & 0xfffe;
	}
	else
	 	v->status_serial_bus.count_read_error++;

///////////

	if (old_err == v->status_serial_bus.count_read_error)// no errors
	{
	  v->status_serial_bus.count_read_ok++;
	  err = 0; // no errors
	}
	else
	  err = 1; // !errors!

	err_ready = check_cds_ready_sbus( err, ITS_READ_BUS, &v->status_serial_bus);
	set_status_cds(err_ready, &v->status);

	return err_ready;

}


///////////////////////////////////////////////
///////////////////////////////////////////////
///////////////////////////////////////////////
#pragma CODE_SECTION(cds_tk_read_pbus_23550,".fast_run2");
int cds_tk_read_pbus_23550(T_cds_tk_23550 *v)
{
   unsigned long adr_pbus;

   if (v->useit == 0) 
	  return 0;
	   
   if (v->status & (component_Started | component_Ready | component_Error | component_ErrorSBus))
   {
//#if (Cds_Tk_Xilinx_SP6 == 1) && (C_PROJECT_TYPE == PROJECT_23550)
   if (v->type_cds_xilinx == TYPE_CDS_XILINX_SP6)
   {
       adr_pbus = v->adr_pbus.adr_table[0] + ADR_FIRST_FREE;
       read_pbus_value_full_v2(v->setup_pbus.use_reg_in_pbus.bit.reg0,adr_pbus,v->read.pbus.status1.all);
       read_pbus_value_full_v2(v->setup_pbus.use_reg_in_pbus.bit.reg1,adr_pbus,v->read.pbus.DataReg0.all);
       read_pbus_value_full_v2(v->setup_pbus.use_reg_in_pbus.bit.reg2,adr_pbus,v->read.pbus.DataReg1.all);
       read_pbus_value_full_v2(v->setup_pbus.use_reg_in_pbus.bit.reg3,adr_pbus,v->read.pbus.DataReg2.all);
       read_pbus_value_full_v2(v->setup_pbus.use_reg_in_pbus.bit.reg4,adr_pbus,v->read.pbus.DataReg3.all);
       read_pbus_value_full_v2(v->setup_pbus.use_reg_in_pbus.bit.reg5,adr_pbus,v->read.pbus.status2.all);
   }
   }
   else
   {
       v->read.pbus.status1.all = 0;
       v->read.pbus.DataReg0.all = 0;
       v->read.pbus.DataReg1.all = 0;
       v->read.pbus.DataReg2.all = 0;
       v->read.pbus.DataReg3.all = 0;
       v->read.pbus.status2.all = 0;
   }

//#endif

  return 0;
}

///////////////////////////////////////////////
///////////////////////////////////////////////
#pragma CODE_SECTION(cds_tk_optical_bus_write_data,".fast_run");
void cds_tk_optical_bus_write_data(T_cds_tk_23550 *v)
{
   if (v->useit == 0)
      return ;

   i_WriteMemory(SI_OPTICS_WORD_TO_SEND_1,v->optical_data_out.buf[0]);
   i_WriteMemory(SI_OPTICS_WORD_TO_SEND_2,v->optical_data_out.buf[1]);
   i_WriteMemory(SI_OPTICS_WORD_TO_SEND_3,v->optical_data_out.buf[2]);
   i_WriteMemory(SI_OPTICS_WORD_TO_SEND_4,v->optical_data_out.buf[3]);
   v->optical_data_out.count_send++;

}


///////////////////////////////////////////////
///////////////////////////////////////////////
#pragma CODE_SECTION(cds_tk_optical_bus_check_error_read,".fast_run");
void cds_tk_optical_bus_check_error_read(T_cds_tk_23550 *v)
{
   unsigned int  delta_id_sbus = 0;
//   static unsigned int prev_id_sbus = 0;

   if (v->useit == 0)
      return ;

 //  i_led2_on_off(1);

   v->optical_data_in.status_1.all = v->read.pbus.status1.all;
   v->optical_data_in.status_2.all = v->read.pbus.status2.all;

//   if ( (v->read.pbus.status1.all == v->read.pbus.status2.all)
//           && (v->read.pbus.status1.bit.receiver_busy==0)
//           && (v->read.pbus.status1.bit.receiver_error==0)
//           )

   if ( v->optical_data_in.status_1.bit.id_sbus == v->optical_data_in.status_2.bit.id_sbus
        && v->optical_data_in.status_1.bit.id == v->optical_data_in.status_2.bit.id
        && (v->read.pbus.status1.bit.receiver_error==0)
           && (v->read.pbus.status2.bit.receiver_error==0) )
   {
       // если выставился этот бит, то знвчит приемник в приеме данных, но т.к. эти данные мы получаем по PBUS с некоторым лагом
       // смысл этого бита теряет свой смысл, просто как информация, данные мы получим всегда последние удачные.
       v->optical_data_in.status_read.bit.receiver_busy = v->optical_data_in.status_1.bit.receiver_busy || v->optical_data_in.status_2.bit.receiver_busy;

       // may be data new and ok?
       // счетчик новых принимаемых посылок не изменился
       if (v->optical_data_in.status_1.bit.id_sbus == v->optical_data_in.prev_id_sbus) {
       // data old
           v->optical_data_in.status_read.bit.old_data = 1;
           v->optical_data_in.same_id_count += 1;
       }
       else
       {
           // очистка ошибок
           v->optical_data_in.local_count_error = 0;
           v->optical_data_in.raw_local_error = 0;
           v->optical_data_in.ready = 1;

          // есть ли потери в чтении?
          if (v->optical_data_in.status_1.bit.id_sbus >= v->optical_data_in.prev_id_sbus)
            delta_id_sbus = v->optical_data_in.status_1.bit.id_sbus - v->optical_data_in.prev_id_sbus;
          else
            delta_id_sbus = 0x10 + v->optical_data_in.status_1.bit.id_sbus - v->optical_data_in.prev_id_sbus;

          if (delta_id_sbus == 1 )
          {
              // тут все ок.
              v->optical_data_in.count_ok++;
          }
          else
          {
              // есть потери
              v->optical_data_in.count_lost += (delta_id_sbus - 1);
              v->optical_data_in.count_ok++;
              v->optical_data_in.status_read.bit.lost_data = 1;
          }

          // вдруг данные мы не забрали?
          if (v->optical_data_in.status_read.bit.new_data_ready)
              v->optical_data_in.status_read.bit.overfull_new_data = 1; // переполнение данных, потеря предыдущих

          // забрали данные
          v->optical_data_in.buf[0] = v->read.pbus.DataReg0.all;
          v->optical_data_in.buf[1] = v->read.pbus.DataReg1.all;
          v->optical_data_in.buf[2] = v->read.pbus.DataReg2.all;
          v->optical_data_in.buf[3] = v->read.pbus.DataReg3.all;


          v->optical_data_in.status_read.bit.new_data_ready = 1;

       }

       v->optical_data_in.prev_id_sbus = v->read.pbus.status1.bit.id_sbus;
   }
   else
   {
       if ((v->optical_data_in.status_1.bit.id_sbus != v->optical_data_in.status_2.bit.id_sbus)
           || (v->optical_data_in.status_1.bit.id != v->optical_data_in.status_2.bit.id) )
           v->optical_data_in.status_read.bit.bad_status12 = 1;

       if (v->read.pbus.status1.bit.receiver_error==1 || v->read.pbus.status2.bit.receiver_error==1)
           v->optical_data_in.status_read.bit.receiver_error = 1;

       v->optical_data_in.raw_local_error = 1;
       v->optical_data_in.full_count_error++;

       if (v->optical_data_in.local_count_error >= v->optical_data_in.setup_count_error)
          {
            v->optical_data_in.ready = 0;

//            v->optical_data_in.buf[0] = 0;
//            v->optical_data_in.buf[1] = 0;
//            v->optical_data_in.buf[2] = 0;
//            v->optical_data_in.buf[3] = 0;
          }
          else
          {
            v->optical_data_in.local_count_error++;
          }

   }


//
//
//   if ( (v->read.pbus.status1.all == v->read.pbus.status2.all)
//           && (v->read.pbus.status1.bit.id_sbus != v->optical_data_in.prev_id_sbus )
//           && (v->read.pbus.status1.bit.receiver_busy==0)
//           && (v->read.pbus.status1.bit.receiver_error==0)
//           )
//   {
//
//
//       if (v->read.pbus.status1.bit.id_sbus >= v->optical_data_in.prev_id_sbus)
//         delta_id_sbus = v->read.pbus.status1.bit.id_sbus - v->optical_data_in.prev_id_sbus;
//       else
//         delta_id_sbus = 0x10 + v->read.pbus.status1.bit.id_sbus - v->optical_data_in.prev_id_sbus;
//
//       v->optical_data_in.local_count_error = 0;
//       v->optical_data_in.raw_local_error = 0;
//       v->optical_data_in.ready = 1;
//
//
//
//       if (delta_id_sbus == 1 )
//           v->optical_data_in.count_ok++;
//       else
//       {
//           v->optical_data_in.count_lost += (delta_id_sbus - 1);
//           v->optical_data_in.count_ok++;
//       }
//
//       v->optical_data_in.buf[0] = v->read.pbus.DataReg0.all;
//       v->optical_data_in.buf[1] = v->read.pbus.DataReg1.all;
//       v->optical_data_in.buf[2] = v->read.pbus.DataReg2.all;
//       v->optical_data_in.buf[3] = v->read.pbus.DataReg3.all;
//
//       // вдруг данные мы не забрали?
//       if (v->optical_data_in.new_data_ready)
//           v->optical_data_in.overfull_new_data = 1; // переполнение данных, потеря предыдущих
//
//       v->optical_data_in.new_data_ready = 1;
//
//   }
//   else
//   {
////       Led1_Toggle();
//
////       i_led1_on_off(1);
//
//       v->optical_data_in.raw_local_error = 1;
//
//       if (v->read.pbus.status1.bit.id_sbus != v->optical_data_in.prev_id_sbus) {
//           v->optical_data_in.same_id_count += 1;
//       }
//       v->optical_data_in.full_count_error++;
//       if (v->optical_data_in.local_count_error >= v->optical_data_in.setup_count_error)
//       {
//         v->optical_data_in.ready = 0;
//
//         v->optical_data_in.buf[0] = 0;
//         v->optical_data_in.buf[1] = 0;
//         v->optical_data_in.buf[2] = 0;
//         v->optical_data_in.buf[3] = 0;
//
//       }
//       else
//       {
//         v->optical_data_in.local_count_error++;
////         i_led2_toggle();
//       }
//
////       i_led1_on_off(0);
//   }
//   v->optical_data_in.prev_id_sbus = v->read.pbus.status1.bit.id_sbus;
// //  i_led2_on_off(0);
//
//
////   v->write.sbus.protect_error.all = v->store_protect_error; // restore all setup error.
////   v->write_sbus(v);
//

}

///////////////////////////////////////////////
///////////////////////////////////////////////
///////////////////////////////////////////////
#pragma CODE_SECTION(cds_tk_optical_bus_check_error_write,".fast_run");
void cds_tk_optical_bus_check_error_write(T_cds_tk_23550 *v)
{
   if (v->useit == 0)
      return ;

   if ( (v->read.pbus.status1.all == v->read.pbus.status2.all)
           && (v->read.pbus.status1.bit.trans_busy==0)
           && (v->read.pbus.status1.bit.trans_error==0)
           )
   {
       v->optical_data_out.local_count_error = 0;
       v->optical_data_out.ready = 1;
       v->optical_data_out.raw_local_error = 0;
   }
   else
   {
       v->optical_data_out.raw_local_error = 1;
       v->optical_data_out.full_count_error++;
       if (v->optical_data_out.local_count_error >= v->optical_data_out.setup_count_error)
         v->optical_data_out.ready = 0;
       else
       {
         v->optical_data_out.local_count_error++;
       }

   }

//   v->write.sbus.protect_error.all = v->store_protect_error; // restore all setup error.
//   v->write_sbus(v);


}

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