#include "xp_project.h"
#include "xp_inc_sensor.h"

#include "xp_project.h"


T_inc_sensor inc_sensor = T_INC_SENSOR_DEFAULT;

//Äèñêðåòèçàöèþ, ïðè êîòîðîé ðàñ÷èòûâàåòñþ äëèòåëüíîñòü èìïóëüñîâ
#define SAMPLING_TIME_NS  1  // 16,666667ns
#define SAMPLING_TIME_MS  0  // 1,666667us
// Êîëè÷åñòâî èìïóëüñîâ, ïðè êîòîðûõ ïåðåêëþ÷àåòñó ïåðèîä äèñêðåòèçàöèè.
// Âåëè÷èíû âûáðàíû ñ "íàõë¸ñòîì" ÷òî áû íå áûëî ïîñòîþííûõ ïåðåêëþ÷åíèé
// â ðàéîíå ãðàíè÷íûõ âåëè÷èí
#define LEVEL_SWITCH_NANOSEC 300
#define LEVEL_SWITCH_MICROSEC 40000


static void read_in_sensor_line1(T_inc_sensor *inc_s);
static void read_in_sensor_line2(T_inc_sensor *inc_s);
static void read_command_reg(T_inc_sensor *inc_s);
static void write_command_reg(T_inc_sensor *inc_s);
static void tune_sampling_time(T_inc_sensor *inc_s);
static void wait_for_registers_updated(T_inc_sensor *inc_s);
static void read_direction_in_plane(T_inc_sensor *inc_s);
static void detect_break_sensor_1_2(T_inc_sensor *inc_s);

void sensor_set(T_inc_sensor *inc_s)
{
/*
    if(inc_s->use_sensor1 || inc_s->use_sensor2)
    {
        inc_s->in_plane.set(&inc_s->in_plane);
    }
    if(inc_s->use_angle_plane)
    {
        inc_s->rotation_plane.set(&inc_s->rotation_plane);
    }
*/
}

void inc_sensor_set(T_inc_sensor *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);
*/

	write_command_reg(inc_s);

}


void inc_sensor_read(T_inc_sensor *inc_s)
{
    if (inc_s->use_sensor1 || inc_s->use_sensor2)
    {
        wait_for_registers_updated(inc_s);
        read_direction_in_plane(inc_s);
    }
    else
    {
        return;
    }

    if (inc_s->use_sensor1)
    {
        inc_s->read_sensor1(inc_s);
    }

    if (inc_s->use_sensor2)
    {
        inc_s->read_sensor2(inc_s);
    }

    detect_break_sensor_1_2(inc_s);

#ifdef AUTO_CHANGE_SAMPLING_TIME
    tune_sampling_time(inc_s);
#endif
}

////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
#define MAX_COUNT_OVERFULL_DISCRET_3    150
#pragma CODE_SECTION(inc_sensor_read1,".fast_run");
void inc_sensor_read1(T_inc_sensor *inc_s)
{
    read_in_sensor_line1(inc_s);

//#if C_PROJECT_TYPE != PROJECT_BALZAM
    inc_s->data.Impulses1 		 = inc_s->pm67regs.n_impulses_line1;
    inc_s->data.Time1			 = inc_s->pm67regs.time_line1/60;
    //Counter`s freq is 60ÌÃö => N/60 = time in mksec
	
	if (inc_s->pm67regs.n_impulses_line1>=2)
	{
  	  inc_s->data.TimeCalcFromImpulses1 = (unsigned long)inc_s->pm67regs.time_line1*1000/inc_s->pm67regs.n_impulses_line1;
	  inc_s->data.TimeCalcFromImpulses1 /= 60;
	}
	else
	  inc_s->data.TimeCalcFromImpulses1 = 0;


//#endif
	//inc_s->data.CountZero1      = inc_s->pm67regs.zero_time_line1;

    if (inc_s->pm67regs.zero_time_line1==0)
    {
        if (inc_s->data.countCountZero1==MAX_COUNT_OVERFULL_DISCRET_3)
        {
            inc_s->data.prev_CountZero1 =  inc_s->data.CountZero1 = 0;
        }
        else
        {
            inc_s->data.CountZero1 = inc_s->data.prev_CountZero1;
            inc_s->data.countCountZero1++;
        }
    }
    else
    {
       inc_s->data.countCountZero1 = 0;
       inc_s->data.CountZero1 = inc_s->pm67regs.zero_time_line1;
       inc_s->data.prev_CountZero1 = inc_s->pm67regs.zero_time_line1;
    }

//	inc_s->data.CountOne1 		= inc_s->pm67regs.one_time_line1;
    if (inc_s->pm67regs.one_time_line1==0)
    {
        if (inc_s->data.countCountOne1==MAX_COUNT_OVERFULL_DISCRET_3)
        {
            inc_s->data.prev_CountOne1 =  inc_s->data.CountOne1 = 0;
        }
        else
        {
            inc_s->data.CountOne1 = inc_s->data.prev_CountOne1;
            inc_s->data.countCountOne1++;
        }
    }
    else
    {
       inc_s->data.countCountOne1 = 0;
       inc_s->data.CountOne1 = inc_s->pm67regs.one_time_line1;
       inc_s->data.prev_CountOne1 = inc_s->pm67regs.one_time_line1;
    }


	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;
}

////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
#pragma CODE_SECTION(inc_sensor_read2,".fast_run");
void inc_sensor_read2(T_inc_sensor *inc_s)
{
    read_in_sensor_line2(inc_s);

//#if C_PROJECT_TYPE != PROJECT_BALZAM
    inc_s->data.Impulses2 		= inc_s->pm67regs.n_impulses_line2;
    inc_s->data.Time2 			= inc_s->pm67regs.time_line2 / 60;
    //Counter`s freq is 60ÌÃö => N/60 = time in mksec

	if (inc_s->pm67regs.n_impulses_line2>=2)
	{
	 inc_s->data.TimeCalcFromImpulses2 = (unsigned long)inc_s->pm67regs.time_line2*1000/inc_s->pm67regs.n_impulses_line2;
	 inc_s->data.TimeCalcFromImpulses2 /= 60;
	}
	else
     inc_s->data.TimeCalcFromImpulses2 = 0;

//#endif
    //inc_s->data.CountZero1      = inc_s->pm67regs.zero_time_line1;

    if (inc_s->pm67regs.zero_time_line2==0)
    {
        if (inc_s->data.countCountZero2==MAX_COUNT_OVERFULL_DISCRET_3)
        {
            inc_s->data.prev_CountZero2 =  inc_s->data.CountZero2 = 0;
        }
        else
        {
            inc_s->data.CountZero2 = inc_s->data.prev_CountZero2;
            inc_s->data.countCountZero2++;
        }
    }
    else
    {
       inc_s->data.countCountZero2 = 0;
       inc_s->data.CountZero2 = inc_s->pm67regs.zero_time_line2;
       inc_s->data.prev_CountZero2 = inc_s->pm67regs.zero_time_line2;
    }

//  inc_s->data.CountOne1       = inc_s->pm67regs.one_time_line1;
    if (inc_s->pm67regs.one_time_line2==0)
    {
        if (inc_s->data.countCountOne2==MAX_COUNT_OVERFULL_DISCRET_3)
        {
            inc_s->data.prev_CountOne2 =  inc_s->data.CountOne2 = 0;
        }
        else
        {
            inc_s->data.CountOne2 = inc_s->data.prev_CountOne2;
            inc_s->data.countCountOne2++;
        }
    }
    else
    {
       inc_s->data.countCountOne2 = 0;
       inc_s->data.CountOne2 = inc_s->pm67regs.one_time_line2;
       inc_s->data.prev_CountOne2 = inc_s->pm67regs.one_time_line2;
    }

	inc_s->data.counter_freq2 	= inc_s->pm67regs.read_comand_reg.bit.sampling_time2;

//	inc_s->data.direction2 = inc_s->read.pbus.direction.bit.sensor2;

}

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


void read_in_sensor_line1(T_inc_sensor *inc_s)
{

    if(!inc_s->pm67regs.read_comand_reg.bit.update_registers)
    {

        inc_s->pm67regs.time_line1 			= i_ReadMemory(ADR_SENSOR_S1_T_PERIOD);//TODO check time when turn off
        inc_s->pm67regs.n_impulses_line1 	= i_ReadMemory(ADR_SENSOR_S1_COUNT_IMPULS);

        inc_s->pm67regs.zero_time_line1		= i_ReadMemory(ADR_SENSOR_S1_T_PERIOD_LOW_ONE_IMPULS);
        inc_s->pm67regs.one_time_line1 		= i_ReadMemory(ADR_SENSOR_S1_T_PERIOD_HIGH_ONE_IMPULS);
    }

}

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


void read_in_sensor_line2(T_inc_sensor *inc_s)
{
    if(!inc_s->pm67regs.read_comand_reg.bit.update_registers)
    {

        inc_s->pm67regs.time_line2 			= i_ReadMemory(ADR_SENSOR_S2_T_PERIOD);//TODO check time when turn off
        inc_s->pm67regs.n_impulses_line2 	= i_ReadMemory(ADR_SENSOR_S2_COUNT_IMPULS);

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

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

    
void write_command_reg(T_inc_sensor *inc_s)
{
    WriteMemory(ADR_SENSOR_CMD, inc_s->pm67regs.write_comand_reg.all);	
}

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


void read_command_reg(T_inc_sensor *inc_s)
{
    inc_s->pm67regs.read_comand_reg.all = i_ReadMemory(ADR_SENSOR_CMD);
}

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


void update_sensors_data_s(T_inc_sensor *inc_s)
{
	inc_s->pm67regs.write_comand_reg.bit.update_registers  	 	= 1;
    write_command_reg(inc_s);
//    inc_s->in_plane.write.regs.comand_reg.bit.update_registers = 0;
}

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


void read_direction_in_plane(T_inc_sensor *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.
*/
}

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


void wait_for_registers_updated(T_inc_sensor *inc_s)
{
    int counter_in_while = 0;
    read_command_reg(inc_s);
    while(inc_s->pm67regs.read_comand_reg.bit.update_registers)
    {
        read_command_reg(inc_s);
        inc_s->count_wait_for_update_registers++;
		counter_in_while++;
		if(counter_in_while > 1000)
		{
			inc_s->error_update++;
			break;
		}
    }
}

////////////////////////////////////////////////////////
void detect_break_sensor_1_2(T_inc_sensor *inc_s)
{
    unsigned int f1 = (inc_s->data.CountOne1 || inc_s->data.CountZero1);
    unsigned int f2 = (inc_s->data.CountOne2 || inc_s->data.CountZero2);


    if (f1 && f2==0)
    {
        inc_s->break_sensor1 = 0;
        inc_s->break_sensor2 = 1;
    }

    if (f1==0 && f2)
    {
        inc_s->break_sensor1 = 1;
        inc_s->break_sensor2 = 0;
    }

    if ((f1==0 && f2==0) || (f1 && f2))
    {
        inc_s->break_sensor1 = 0;
        inc_s->break_sensor2 = 0;
    }


}
////////////////////////////////////////////////////////


void tune_sampling_time(T_inc_sensor *inc_s)
{

// ñíà÷àëà ïðîâåðßåì íà ìàêñèìóì, ò.ê. åñëè äàò÷èê îòâàëèëñß, òî îí ïîêàæåò = 0.

    if(
            (inc_s->use_sensor1 && inc_s->break_sensor1==0 && (inc_s->data.CountOne1 > LEVEL_SWITCH_MICROSEC) && (inc_s->data.CountZero1 > LEVEL_SWITCH_MICROSEC) )
        ||  (inc_s->use_sensor2 && inc_s->break_sensor2==0 && (inc_s->data.CountOne2 > LEVEL_SWITCH_MICROSEC) && (inc_s->data.CountZero2 > LEVEL_SWITCH_MICROSEC) )
        )
    {
        inc_s->pm67regs.write_comand_reg.bit.set_sampling_time = SAMPLING_TIME_MS;
		return;
    }

// ïðîâåðêà íà ìèíèìóì
	if(
	      (inc_s->use_sensor1 && inc_s->break_sensor1==0 && (inc_s->data.CountOne1 < LEVEL_SWITCH_NANOSEC) && (inc_s->data.CountZero1 < LEVEL_SWITCH_NANOSEC) )
       || (inc_s->use_sensor2 && inc_s->break_sensor2==0 && (inc_s->data.CountOne2 < LEVEL_SWITCH_NANOSEC) && (inc_s->data.CountZero2 < LEVEL_SWITCH_NANOSEC) )
       )
    {
        inc_s->pm67regs.write_comand_reg.bit.set_sampling_time = SAMPLING_TIME_NS;
    }

}



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