#include "x_parallel_bus.h"
#include "xp_cds_rs.h"

#include "x_parallel_bus.h"
#include "x_serial_bus.h"
#include "xp_tools.h"
#include "xp_tools.h"
#include "xerror.h"

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

void cds_rs_init(T_cds_rs *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_rs_read_all(T_cds_rs *v)
{
  if (v->useit == 0) 
	  return 0;

  cds_rs_read_sbus(v);
  cds_rs_read_pbus(v);
  return 0;

}

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


int cds_rs_write_all(T_cds_rs *v)
{
  if (v->useit == 0) 
	  return 0;

  cds_rs_write_sbus(v);
  cds_rs_write_pbus(v);
  return 0;

}

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

int cds_rs_write_sbus(T_cds_rs *v)
{
	unsigned int old_err;

    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

//8 config rs speed, master/slave and period of survey
    x_serial_bus_project.reg_addr 	= 8;   				 			// adr memory in plate
	x_serial_bus_project.write_data = v->write.sbus.config.all;   // write data

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

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

	return 1; // !error!

}

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

int cds_rs_write_pbus(T_cds_rs *v)
{
  if (v->useit == 0) 
	  return 0;

  return 0;
}

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

int cds_rs_read_sbus(T_cds_rs *v)
{
	unsigned int old_err;

    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
	if(v->write.sbus.config.bit.channel1_enable)
	{
		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.sensor[0].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
			v->read.sbus.sensor[0].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
		}	
		else
			v->status_serial_bus.count_read_error++;
	}	

//1
	if(v->write.sbus.config.bit.channel2_enable)
	{
		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.sensor[1].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
			v->read.sbus.sensor[1].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
		}
		else
			v->status_serial_bus.count_read_error++;
	}
//2
	if(v->write.sbus.config.bit.channel3_enable)
	{
		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.sensor[2].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
			v->read.sbus.sensor[2].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
		}
		else
			v->status_serial_bus.count_read_error++;
	}
//3
	if(v->write.sbus.config.bit.channel4_enable)
	{
		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.sensor[3].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
			v->read.sbus.sensor[3].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
		}
		else
			v->status_serial_bus.count_read_error++;
	}
//4
	if(v->write.sbus.config.bit.channel1_enable)
	{
		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.sensor[0].angle = x_parallel_bus_project.data_table_read << 2;
		}
		else
			v->status_serial_bus.count_read_error++;
	}
//5
	if(v->write.sbus.config.bit.channel2_enable)
	{
		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.sensor[1].angle = x_parallel_bus_project.data_table_read << 2;
		}
		else
			v->status_serial_bus.count_read_error++;
	}
//6
	if(v->write.sbus.config.bit.channel3_enable)
	{
		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.sensor[2].angle = x_parallel_bus_project.data_table_read << 2;
		}
		else
			v->status_serial_bus.count_read_error++;
	}	
//7
	if(v->write.sbus.config.bit.channel4_enable)
	{
		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.sensor[3].angle = x_parallel_bus_project.data_table_read << 2;
		}
		else
			v->status_serial_bus.count_read_error++;
	}

//8 
    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.config.all = x_parallel_bus_project.data_table_read;
	}
	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->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++;
	  return 0; // no errors
	}

	return 1; // !errors!
}

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

int cds_rs_read_pbus(T_cds_rs *v)
{
	if (v->useit == 0) 
	  return 0;

    if (v->status & (component_Started | component_Ready | component_Error | component_ErrorSBus))
    {
//0
	if(v->write.sbus.config.bit.channel1_enable && 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.sensor[0].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
		v->read.pbus.sensor[0].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
	}
//1
	if(v->write.sbus.config.bit.channel2_enable && 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.sensor[1].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
		v->read.pbus.sensor[1].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
	}
//2
	if(v->write.sbus.config.bit.channel3_enable && 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.sensor[2].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
		v->read.pbus.sensor[2].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
	}
//3
	if(v->write.sbus.config.bit.channel4_enable && 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.sensor[3].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
		v->read.pbus.sensor[3].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
	}
//4
	if(v->write.sbus.config.bit.channel1_enable && 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.sensor[0].angle = x_parallel_bus_project.data_table_read << 2;
	}
//5
	if(v->write.sbus.config.bit.channel2_enable && 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.sensor[1].angle = x_parallel_bus_project.data_table_read << 2;
	}
//6
	if(v->write.sbus.config.bit.channel3_enable && 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.sensor[2].angle = x_parallel_bus_project.data_table_read << 2;
	}
//7
	if(v->write.sbus.config.bit.channel4_enable && 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.sensor[3].angle = x_parallel_bus_project.data_table_read << 2;
	}
    }
    else
    {
        v->read.pbus.sensor[0].direction = 0;
        v->read.pbus.sensor[0].turned_angle = 0;
        v->read.pbus.sensor[0].angle = 0;

        v->read.pbus.sensor[1].direction = 0;
        v->read.pbus.sensor[1].turned_angle = 0;
        v->read.pbus.sensor[1].angle = 0;

        v->read.pbus.sensor[2].direction = 0;
        v->read.pbus.sensor[2].turned_angle = 0;
        v->read.pbus.sensor[2].angle = 0;

        v->read.pbus.sensor[3].direction = 0;
        v->read.pbus.sensor[3].turned_angle = 0;
        v->read.pbus.sensor[3].angle = 0;
    }
  return 0;
}

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