#include "MemoryFunctions.h"
#include "xp_cds_in.h"

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


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

void cds_in_init(T_cds_in *v)
{
	int old_started = 0;
	unsigned int i;

    if (v->useit == 0) 
	{
	  clear_adr_sync_table(v->plane_address);
	  return ;
	}
	set_adr_sync_table(v->plane_address);

	if (x_parallel_bus_project.flags.bit.init==0)
      x_parallel_bus_project.init(&x_parallel_bus_project);

	
	old_started = x_parallel_bus_project.flags.bit.started;

	if (x_parallel_bus_project.flags.bit.started)
	  x_parallel_bus_project.stop(&x_parallel_bus_project);
	  

	x_parallel_bus_project.slave_addr = v->plane_address;

//	for (i=0;i<v->setup_pbus.count_elements_pbus;i++)
	for (i=0;i<16;i++)
	{
		if (v->setup_pbus.use_reg_in_pbus.all & (1<<i))
		{
		    if (x_parallel_bus_project.check_free_table(&x_parallel_bus_project))
		    {
                x_parallel_bus_project.reg_addr = i;
                v->adr_pbus.adr_table[i] = x_parallel_bus_project.setup.size_table;
                x_parallel_bus_project.add_table(&x_parallel_bus_project);
                x_parallel_bus_project.reg_addr++;
                x_parallel_bus_project.setup.size_table++;
		    }
		    else
		    {
		        // места в таблице нет!!!
		        xerror(xparall_bus_er_ID(1),(void *)0);
		        v->setup_pbus.use_reg_in_pbus.all &= (~(1<<i));
		    }
		}
	}

//
	if (old_started)
	  x_parallel_bus_project.start(&x_parallel_bus_project);

	  

}

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


int cds_in_read_all(T_cds_in *v)
{
 int err = 0;

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

  err = cds_in_read_sbus(v); 
  err |= cds_in_read_pbus(v);

  return err;
}

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


int cds_in_write_all(T_cds_in *v)
{
 int err = 0;

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

  err = cds_in_write_sbus(v); 
  err |= cds_in_write_pbus(v);

  return err;
}

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

int cds_in_write_sbus(T_cds_in *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

//0 enabled channels
    x_serial_bus_project.reg_addr 	= 0;   				 			// adr memory in plate
	x_serial_bus_project.write_data = v->write.sbus.enabled_channels.all;//use_invers_sensor_speed.all;   // write data

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

//1 first sensor connection
    x_serial_bus_project.reg_addr 	= 1;   				 			// adr memory in plate
	x_serial_bus_project.write_data = v->write.sbus.first_sensor.all;//sensor_combo;   // write data

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

//2 second sensor connection
    x_serial_bus_project.reg_addr 	= 2;   				 			// adr memory in plate
	x_serial_bus_project.write_data = v->write.sbus.second_sensor.all;//sensor_combo;   // write data

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

    if (v->type_cds_xilinx == TYPE_CDS_XILINX_SP6)
	{
//3 zero sensor connection
     x_serial_bus_project.reg_addr 	= 3;   				 			// adr memory in plate
	 x_serial_bus_project.write_data = v->write.sbus.zero_sensors.all;//sensor_combo;   // write data

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


//6 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 	= 6;   				 			// 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++;


//7 cmd_reset_error
/*
    x_serial_bus_project.reg_addr 	= 7;   				 			// adr memory in plate
	x_serial_bus_project.write_data = v->write.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_in_write_pbus(T_cds_in *v)
{
  if (v->useit == 0) 
	  return 0;

  return 0;
}

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

int cds_in_read_sbus(T_cds_in *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 enabled_channels
    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.enabled_channels.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;
	  

//1 first_sensor
    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.first_sensor.all = x_serial_bus_project.read_data;
	else
	 	v->status_serial_bus.count_read_error++;

//2 second_sensor
    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.second_sensor.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++;



//3 zero sensors
    if (v->type_cds_xilinx == TYPE_CDS_XILINX_SP6)
	{
     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.zero_sensors.all = x_serial_bus_project.read_data;
	 else
	 	v->status_serial_bus.count_read_error++;

	}
//6 protect_error
    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.protect_error.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++;

//14 id_plate
    x_serial_bus_project.reg_addr 	= 14;   				 			// 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.id_plate.all = x_serial_bus_project.read_data;
	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;
}

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

int cds_in_read_pbus(T_cds_in *v)
{    
	unsigned long adr_pbus;
	unsigned int t;

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

    if (v->status & (component_Started | component_Ready | component_Error | component_ErrorSBus ))
    {
	adr_pbus = v->adr_pbus.adr_table[0] + ADR_FIRST_FREE;

	if ((v->type_cds_xilinx == TYPE_CDS_XILINX_SP6) && (v->type_plate == cds_in_type_in_1))
	{
	    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg0,adr_pbus, t);
	    v->read.pbus.data_in.all = (t & 0xff00) | ((~t) & 0x00ff);
	}
	else
	    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg0,adr_pbus,v->read.pbus.data_in.all);

    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg1,adr_pbus,v->read.pbus.ready_in.all);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg2,adr_pbus,v->read.pbus.direction_in.all);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg3,adr_pbus,v->read.pbus.SpeedS1_cnt);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg4,adr_pbus,v->read.pbus.SpeedS1_cnt90);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg5,adr_pbus,v->read.pbus.SpeedS2_cnt);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg6,adr_pbus,v->read.pbus.SpeedS2_cnt90);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg7,adr_pbus,v->read.pbus.Time_since_zero_point_S1);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg8,adr_pbus,v->read.pbus.Impulses_since_zero_point_Rising_S1);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg9,adr_pbus,v->read.pbus.Impulses_since_zero_point_Falling_S1);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg10,adr_pbus,v->read.pbus.Time_since_zero_point_S2);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg11,adr_pbus,v->read.pbus.Impulses_since_zero_point_Rising_S2);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg12,adr_pbus,v->read.pbus.Impulses_since_zero_point_Falling_S2);
    read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg13,adr_pbus,v->read.pbus.channel_alive.all);
/*
//0
    if (v->setup_pbus.use_reg_in_pbus.bit.reg0)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[0];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.data_in.all = x_parallel_bus_project.data_table_read;
	}
//1
    if (v->setup_pbus.use_reg_in_pbus.bit.reg1)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[1];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.ready_in.all = x_parallel_bus_project.data_table_read;
	}
//2
    if (v->setup_pbus.use_reg_in_pbus.bit.reg2)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[2];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.direction_in.all = x_parallel_bus_project.data_table_read;
	}
//3
    if (v->setup_pbus.use_reg_in_pbus.bit.reg3)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[3];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.SpeedS1_cnt = x_parallel_bus_project.data_table_read;
	}
//4
    if (v->setup_pbus.use_reg_in_pbus.bit.reg4)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[4];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.SpeedS1_cnt90 = x_parallel_bus_project.data_table_read;
	}
//5
    if (v->setup_pbus.use_reg_in_pbus.bit.reg5)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[5];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.SpeedS2_cnt = x_parallel_bus_project.data_table_read;
	}
//6
    if (v->setup_pbus.use_reg_in_pbus.bit.reg6)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[6];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.SpeedS2_cnt90 = x_parallel_bus_project.data_table_read;
	}
//7
    if (v->setup_pbus.use_reg_in_pbus.bit.reg7)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[7];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.Time_since_zero_point_S1 = x_parallel_bus_project.data_table_read;
	}
//8
    if (v->setup_pbus.use_reg_in_pbus.bit.reg8)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[8];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.Time_since_zero_point_Rising_S1 = x_parallel_bus_project.data_table_read;
	}
//9
    if (v->setup_pbus.use_reg_in_pbus.bit.reg9)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[9];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.Time_since_zero_point_Falling_S1 = x_parallel_bus_project.data_table_read;
	}
//10
    if (v->setup_pbus.use_reg_in_pbus.bit.reg10)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[10];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.Time_since_zero_point_S2 = x_parallel_bus_project.data_table_read;
	}
//11
    if (v->setup_pbus.use_reg_in_pbus.bit.reg11)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[11];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.Time_since_zero_point_Rising_S2 = x_parallel_bus_project.data_table_read;
	}
//12
    if (v->setup_pbus.use_reg_in_pbus.bit.reg12)
	{
	x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[12];
	x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
	v->read.pbus.Time_since_zero_point_Falling_S2 = x_parallel_bus_project.data_table_read;
	}

*/
    }
    else
    {
        v->read.pbus.data_in.all = 0;
        v->read.pbus.ready_in.all = 0;
        v->read.pbus.direction_in.all = 0;
        v->read.pbus.SpeedS1_cnt = 0;
        v->read.pbus.SpeedS1_cnt90 = 0;
        v->read.pbus.SpeedS2_cnt = 0;
        v->read.pbus.SpeedS2_cnt90 = 0;
        v->read.pbus.Time_since_zero_point_S1 = 0;
        v->read.pbus.Impulses_since_zero_point_Rising_S1 = 0;
        v->read.pbus.Impulses_since_zero_point_Falling_S1 = 0;
        v->read.pbus.Time_since_zero_point_S2 = 0;
        v->read.pbus.Impulses_since_zero_point_Rising_S2 = 0;
        v->read.pbus.Impulses_since_zero_point_Falling_S2 = 0;
        v->read.pbus.channel_alive.all = 0;
    }

  return 0;
}

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

void cds_in_reset_error(T_cds_in *v)
{
    if (v->useit == 0) 
	  return ;

	if (v->status == component_Error || v->status == component_ErrorSBus)
	   v->status = component_Started;
	clear_cur_stat_sbus(&v->status_serial_bus);
	clear_cur_stat_pbus(&v->status_parallel_bus);


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

//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_write_error++;

}

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

void cds_in_store_disable_error(T_cds_in *v)
{
   if (v->useit == 0) 
	  return ;

   v->store_protect_error = v->write.sbus.protect_error.all;
   v->write.sbus.protect_error.all = 0; // disable all error.
   
   cds_in_write_sbus(v);

}

///////////////////////////////////////////////
///////////////////////////////////////////////
void cds_in_restore_enable_error(T_cds_in *v)
{
   if (v->useit == 0) 
	  return ;

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

}

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