#include "xp_project.h"
#include "xp_incremental_sensors.h"

#include "xp_project.h"

#pragma DATA_SECTION(incr_sensors,".slow_vars");
T_Incremental_sensors incr_sensors;// = T_Incremental_sensors_DEFAULT;



//static void read_in_sensor_line1(T_inc_sensor *inc_s);
//static void read_in_sensor_line2(T_inc_sensor *inc_s);

static void incremental_sensors_read_cmd_pm67(T_Incremental_sensors *inc_s);
static void incremental_sensors_write_cmd_pm67(T_Incremental_sensors *inc_s);

static void incremental_sensors_tune_sampling_time_pm67(T_Incremental_sensors *inc_s);
static void incremental_sensors_wait_for_registers_updated_pm67(T_Incremental_sensors *inc_s);

void incremental_sensors_read_data_pm67(T_Incremental_sensors *inc_s);
void incremental_sensors_read_pm67(T_Incremental_sensors *inc_s);


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

    
void incremental_sensors_write_cmd_pm67(T_Incremental_sensors *inc_s)
{
    WriteMemory(ADR_SENSOR_CMD, inc_s->write_comand_reg_pm67.all);    	
}

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


void incremental_sensors_wait_for_registers_updated_pm67(T_Incremental_sensors *inc_s)
{
//    int counter_in_while = 0;

    incremental_sensors_read_cmd_pm67(inc_s);

	if (inc_s->read_comand_reg_pm67.bit.update_registers)
	{
		inc_s->error_alarms_counters.for_pm67.error_update_registers = 0;
	}
	else
    while(inc_s->read_comand_reg_pm67.bit.update_registers)
    {
        incremental_sensors_read_cmd_pm67(inc_s);
	
		inc_s->error_alarms_counters.for_pm67.error_update_registers++;
		inc_s->error_alarms_stat.for_pm67.error_update_registers++;

		if(inc_s->error_alarms_counters.for_pm67.error_update_registers > inc_s->config.error_alarms_timeouts.for_pm67.error_update_registers)
		{
			inc_s->error_alarms_status.for_pm67.error_update_registers |= 1;
			break;
		}
    }
	
}

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

void incremental_sensors_read_cmd_pm67(T_Incremental_sensors *inc_s)
{
    inc_s->read_comand_reg_pm67.all = i_ReadMemory(ADR_SENSOR_CMD);
}

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

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


void incremental_sensors_tune_sampling_time_pm67(T_Incremental_sensors *inc_s)
{
// Ïðîâåðêà èäåò òîëüêî íà âðåìß äëèòåëüíîñòè èìïóëüñà - âðåìß åäèíè÷êè (one_time_line)
// åñëè ìåàíäð ñ äàò÷èêà èäåëüíûé òî one_time_line áóäåò ïðèìåðíî ðàâåí zero_time_line = ñêâàæíîñòü 50%

// ñíà÷àëà ïðîâåðßåì íà ìàêñèìóì, ò.ê. åñëè äàò÷èê îòâàëèëñß, òî îí ïîêàæåò = 0.
    if((inc_s->config.for_pm67.use_s1 && (inc_s->data_from_pm67.data_from_xilinx.one_time_line1 > LEVEL_SWITCH_TUNE_TIME_PM67_MICROSEC))
        ||  (inc_s->config.for_pm67.use_s2 && (inc_s->data_from_pm67.data_from_xilinx.one_time_line2 > LEVEL_SWITCH_TUNE_TIME_PM67_MICROSEC)))
    {
		inc_s->write_comand_reg_pm67.bit.set_sampling_time     = SAMPLING_TIME_PM67_MS;
		return;
    }

// ïðîâåðêà íà ìèíèìóì
    if((inc_s->config.for_pm67.use_s1 && (inc_s->data_from_pm67.data_from_xilinx.one_time_line1 < LEVEL_SWITCH_TUNE_TIME_PM67_NANOSEC))
        ||  (inc_s->config.for_pm67.use_s2 && (inc_s->data_from_pm67.data_from_xilinx.one_time_line2 < LEVEL_SWITCH_TUNE_TIME_PM67_NANOSEC)))
    {
		inc_s->write_comand_reg_pm67.bit.set_sampling_time     = SAMPLING_TIME_PM67_NS;
    }

}



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


void incremental_sensors_init(T_Incremental_sensors *inc_s)
{

/*
    if(!inc_s->cds_in->useit)
    {
        return;
    }

    inc_s->cds_in->write.sbus.enabled_channels.all = inc_s->write.sbus.enabled_channels.all;
    inc_s->cds_in->write.sbus.first_sensor.all = inc_s->write.sbus.first_sensor_inputs.all;
    inc_s->cds_in->write.sbus.second_sensor.all = inc_s->write.sbus.second_sensor_inputs.all;
    // inc_s->cds_in->write_sbus(inc_s->cds_in);
    write_command_reg(inc_s);
*/

	incremental_sensors_write_cmd_pm67(inc_s);

}


void incremental_sensors_read_pm67(T_Incremental_sensors *inc_s)
{
    if(inc_s->config.for_pm67.use_s1 || inc_s->config.for_pm67.use_s2)
    {
        incremental_sensors_wait_for_registers_updated_pm67(inc_s);
    }
    else
    {
        return;
    }

    incremental_sensors_read_data_pm67(inc_s);

	if (inc_s->config.for_pm67.ModeAutoDiscret != MODE_OFF_SPEED_TUNE_PM67)
       incremental_sensors_tune_sampling_time_pm67(inc_s);

}

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

void incremental_sensors_read_data_pm67(T_Incremental_sensors *inc_s)
{
	if (inc_s->read_comand_reg_pm67.bit.update_registers)
	{
	    inc_s->data_from_pm67.data_from_xilinx.time_line1       = i_ReadMemory(ADR_SENSOR_S1_T_PERIOD);
	    inc_s->data_from_pm67.data_from_xilinx.n_impulses_line1 = i_ReadMemory(ADR_SENSOR_S1_COUNT_IMPULS);

	    inc_s->data_from_pm67.data_from_xilinx.zero_time_line1  = i_ReadMemory(ADR_SENSOR_S1_T_PERIOD_LOW_ONE_IMPULS);
        inc_s->data_from_pm67.data_from_xilinx.one_time_line1   = i_ReadMemory(ADR_SENSOR_S1_T_PERIOD_HIGH_ONE_IMPULS);

        inc_s->data_from_pm67.data_from_xilinx.time_line2       = i_ReadMemory(ADR_SENSOR_S2_T_PERIOD);
        inc_s->data_from_pm67.data_from_xilinx.n_impulses_line2 = i_ReadMemory(ADR_SENSOR_S2_COUNT_IMPULS);

        inc_s->data_from_pm67.data_from_xilinx.zero_time_line2  = i_ReadMemory(ADR_SENSOR_S2_T_PERIOD_LOW_ONE_IMPULS);
        inc_s->data_from_pm67.data_from_xilinx.one_time_line2   = i_ReadMemory(ADR_SENSOR_S2_T_PERIOD_HIGH_ONE_IMPULS);
	}


	if (inc_s->config.for_pm67.use_s1)
	{
        inc_s->data_from_pm67.raw_solo.time_s1 = inc_s->data_from_pm67.data_from_xilinx.one_time_line1 + inc_s->data_from_pm67.data_from_xilinx.zero_time_line1;

        //Counter`s freq is 60ÌÃö => N/60 = time in mksec
        if (inc_s->data_from_pm67.data_from_xilinx.n_impulses_line1>2)
           inc_s->data_from_pm67.raw_pulses.TimeCalcS1 = inc_s->data_from_pm67.data_from_xilinx.time_line1*1000/inc_s->data_from_pm67.data_from_xilinx.n_impulses_line1/60;
        else
           inc_s->data_from_pm67.raw_pulses.TimeCalcS1 = 0;
	}
	else
	{
	    inc_s->data_from_pm67.raw_solo.time_s1 = 0;
        inc_s->data_from_pm67.raw_pulses.TimeCalcS1 = 0;
	}

    if (inc_s->config.for_pm67.use_s2)
    {
        inc_s->data_from_pm67.raw_solo.time_s2 = inc_s->data_from_pm67.data_from_xilinx.one_time_line2 + inc_s->data_from_pm67.data_from_xilinx.zero_time_line2;

        //Counter`s freq is 60ÌÃö => N/60 = time in mksec
        if (inc_s->data_from_pm67.data_from_xilinx.n_impulses_line2>2)
           inc_s->data_from_pm67.raw_pulses.TimeCalcS2 = inc_s->data_from_pm67.data_from_xilinx.time_line2*1000/inc_s->data_from_pm67.data_from_xilinx.n_impulses_line2/60;
        else
           inc_s->data_from_pm67.raw_pulses.TimeCalcS2 = 0;
    }
    else
    {
        inc_s->data_from_pm67.raw_solo.time_s2 = 0;
        inc_s->data_from_pm67.raw_pulses.TimeCalcS2 = 0;
    }

//	inc_s->data.counter_freq1 	= inc_s->pm67regs.read_comand_reg.bit.sampling_time1;
//	inc_s->data.direction1 = inc_s->read.pbus.direction.bit.sensor1;
}

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

void incremental_sensors_update_sensors_data_pm67(T_Incremental_sensors *inc_s)
{
    inc_s->write_comand_reg_pm67.bit.update_registers           = 1;
    incremental_sensors_write_cmd_pm67(inc_s);
//    inc_s->in_plane.write.regs.comand_reg.bit.update_registers = 0;
}

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


void read_direction_in_plane(T_Incremental_sensors *inc_s)
{
/*
	inc_s->read.pbus.direction.bit.sensor1 = inc_s->cds_in->read.pbus.direction_in.bit.dir0 == 2 ? 1 :
										inc_s->cds_in->read.pbus.direction_in.bit.dir0 == 1 ? -1 :
										0;
	inc_s->read.pbus.direction.bit.sensor2 = inc_s->cds_in->read.pbus.direction_in.bit.dir1 == 2 ? 1 :
										inc_s->cds_in->read.pbus.direction_in.bit.dir1 == 1 ? -1 :
										0;
	inc_s->read.pbus.direction.bit.sens_err1 = inc_s->cds_in->read.pbus.direction_in.bit.dir0 == 3;
	inc_s->read.pbus.direction.bit.sens_err2 = inc_s->cds_in->read.pbus.direction_in.bit.dir1 == 3;
	//Direction changes not often. May be, it`s enough to read it in main cycle.
*/
}

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