telemetry/doc/tt/fm4ex.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <stdint.h>
#include <syslog.h>
#include <stdarg.h>

#include "../../status.h"
#include "../../feeder.h"


#define FM4_BUFFER_SIZE 32768

//static unsigned char * int_buffer;
//static unsigned int int_buffer_pos = 0, int_buffer_pos_wr = 0;



//static int imei_flag = 0;
//char imei_str[16]; //todo: tohle nejak predavat
unsigned char reply_buffer[32];
unsigned int reply_len;

static double values_out[64];
static uint64_t values_sensor[64];
static uint8_t values_type[64];
static time_t values_time;
//static uint8_t values_type;
static uint8_t values_len;



typedef struct unit_t
{
    int fd;
    char imei[16];
    struct unit_t * next;
    unsigned char * int_buffer;
    unsigned int int_buffer_pos;
    unsigned int int_buffer_pos_wr;

    time_t t_t;
} unit_t;


static struct unit_t * units;
static struct unit_t * aunit;

static void (* feederLog)(int priority, const char * fmt, ...);

int setLog(void * func)
{
    feederLog = func;
    return 0;
}


void fm4LogHex(char * buffer, unsigned char * data, unsigned int length);

typedef struct avl_header_t
{
    uint16_t dummy __attribute__ ((packed));
    char imei[15];
} avl_header_t;

uint16_t byteSwap16(uint16_t value)
{
    uint16_t swapped;
    swapped = (((0x00FF) & (value >> 8)) |
              ((0xFF00) & (value << 8)));
    return swapped;
}

uint32_t byteSwap32(uint32_t value)
{
    uint32_t swapped;
    swapped = (((0x000000FF) & (value >> 24)) |
              ((0x0000FF00) & (value >> 8)) |
              ((0x00FF0000) & (value << 8)) |
              ((0xFF000000) & (value << 24)));
    return swapped;
}

uint64_t byteSwap64(uint64_t value)
{
    uint64_t swapped;
    swapped = (((0x00000000000000FFULL) & (value >> 56)) |
              ((0x000000000000FF00ULL) & (value >> 40)) |
              ((0x0000000000FF0000ULL) & (value >> 24)) |
              ((0x00000000FF000000ULL) & (value >> 8)) |
              ((0x000000FF00000000ULL) & (value << 8)) |
              ((0x0000FF0000000000ULL) & (value << 24)) |
              ((0x00FF000000000000ULL) & (value << 40)) |
              ((0xFF00000000000000ULL) & (value << 56)));
    return swapped;
}

void arraySwap4(uint8_t * array)
{
    uint8_t temp;

    temp = array[0];
    array[0] = array[3];
    array[3] = temp;
    temp = array[1];
    array[1] = array[2];
    array[2] = temp;
}

enum states {STATE_IDLE = 0, STATE_DATA_LENGTH, STATE_CODEC_ID, STATE_NO_OF_REC, 
    STATE_TIMESTAMP, STATE_PRIORITY, 
    STATE_LON, STATE_LAT, STATE_ALT, STATE_ANGLE, STATE_SATTS, STATE_SPEED, 
    STATE_EVENT_IO_ID, STATE_TOTAL_IO,
    STATE_BYTE1_IO, STATE_BYTE1_ID, STATE_BYTE1_VAL,
    STATE_BYTE2_IO, STATE_BYTE2_ID, STATE_BYTE2_VAL,
    STATE_BYTE4_IO, STATE_BYTE4_ID, STATE_BYTE4_VAL,
    STATE_BYTE8_IO, STATE_BYTE8_ID, STATE_BYTE8_VAL,
    STATE_BYTEX_IO, STATE_BYTEX_ID, STATE_BYTEX_LEN, STATE_BYTEX_VAL,
    STATE_NO_OF_REC_END, STATE_CRC,
    STATE_TYPE, STATE_COMMAND_RESPONSE_SIZE, STATE_COMMAND_RESPONSE
    };
    
static unsigned char state = STATE_IDLE;		//state of status machine, must be global to be accesible by process function when determining if this packet could be imei packet (state is IDLE)
    

uint32_t counts_codec8[] =
{
    4, 4, 1, 1, 		//AVL data packet
    8, 1,			//AVL data 
    4, 4, 2, 2, 1, 2,		//GPS element
    1, 1,			//IO element
    1, 1, 1,			//N1 IO
    1, 1, 2,			//N2 IO
    1, 1, 4,			//N4 IO
    1, 1, 8,			//N8 IO
    0, 0, 0, 0,			//placeholder for codec8ex NX IO
    1, 4			//No of data, CRC
};

uint32_t counts_codec8ex[] =
{
    4, 4, 1, 1, 		//AVL data packet
    8, 1,			//AVL data 
    4, 4, 2, 2, 1, 2,		//GPS element
    2, 2,			//IO element
    2, 2, 1,			//N1 IO
    2, 2, 2,			//N2 IO
    2, 2, 4,			//N4 IO
    2, 2, 8,			//N8 IO
    2, 2, 2, 0,			//NX IO, length is upadted in state machine
    1, 4			//No of data, CRC
};

uint32_t counts_codec12[] =
{
    4, 4, 1, 1, 		//AVL data packet

    8, 1,			//AVL data 
    4, 4, 2, 2, 1, 2,		//GPS element
    1, 1,			//IO element
    1, 1, 1,			//N1 IO
    1, 1, 2,			//N2 IO
    1, 1, 4,			//N4 IO
    1, 1, 8,			//N8 IO
    0, 0, 0, 0,			//NX IO, length is upadted in state machine
    1, 4,			//No of data, CRC

    1,				//Type
    4,				//Command/Response size
    0				//Command/Response - length is updated in state machine
};


#define PRIO_LOW	0
#define PRIO_HIGHT	1
#define PRIO_PANIC	2
#define PRIO_SECURITY	3

#define VAL_DI1		1
#define VAL_DI2		2


void valuesCopyPos(double lat, double lon, double alt, time_t t, uint8_t type, double * values)
{
    values[0] = lat;
    values[1] = lon;
    values[2] = alt;
    values_time = t;
    values_type[0] = type;
    values_type[1] = type;
    values_type[2] = type;
}

//todo: nasledujici dve pole do pole struktur, tedy vcetne multiplu
/* {FM4 type, FEEDER sensor type id (AA), sensor id (BBB), alertable (if prio is 1(high)), sensor particular type (last for number - CCCC)} */
/*
uint16_t sensorTypes[][5] =
{
    {1, TYPE_DIGITAL_INPUT, 5, 0, 0},	//0
    {2, TYPE_DIGITAL_INPUT, 6, 1, 0},
    {3, TYPE_DIGITAL_INPUT, 7, 0, 0},
    {4, TYPE_DIGITAL_INPUT, 8, 0, 0},
    {9, TYPE_ANALOG_INPUT, 1, 0, 0},
    {10, TYPE_ANALOG_INPUT, 2, 0, 0},	//5
    {11, TYPE_ANALOG_INPUT, 3, 0, 0},
//    {19, TYPE_ANALOG_INPUT, 4, 0, 0},	//possibly collision
    {21, TYPE_SIGNAL_STRENGTH, 3, 0, 0},
    {24, TYPE_SPEED, 2, 0, 0},
    {66, TYPE_VOLTAGE, 11, 0, 0},	//10
    {67, TYPE_VOLTAGE, 10, 0, 0},
    {68, TYPE_CURRENT, 2, 0, 0},
    {69, TYPE_SIGNAL_STRENGTH, 4, 0, 0},
    {70, TYPE_THERMOMETER, 11, 0, 0},
    {72, TYPE_THERMOMETER, 12, 0, 0},	//15
    {73, TYPE_THERMOMETER, 13, 0, 0},
    {74, TYPE_THERMOMETER, 14, 0, 0},
    {79, TYPE_ACCELERATION, 2, 0, 0},
    {80, TYPE_SPEED, 3, 0, 0},
    {81, TYPE_CRUISE, 1, 0, 0},		//20
    {82, TYPE_CRUISE, 2, 0, 0},
    {83, TYPE_CRUISE, 3, 0, 0},
    {84, TYPE_ENGINE, 1, 0, 0},
    {85, TYPE_ENGINE, 2, 0, 0},
//    {86, TYPE_ENGINE, 3, 0, 0},//todo: possibly collision
//    {86, TYPE_HUMIDITY, 20, 0, 0},
    {87, TYPE_LEVEL, 4, 0, 0},		//25
    {88, TYPE_FREQUENCY, 1, 0, 0},
//    {104, TYPE_TIME, 1, 0, 0},//todo: possibly collision
    {127, TYPE_TEMP, 20, 0, 0},
    {128, TYPE_TEMP, 21, 0, 0},
    {135, TYPE_FLOW, 6, 0, 0},
    {136, TYPE_FLOW, 7, 0, 0},		//30
    {207, TYPE_ID, 1, 0, 0},
    {249, TYPE_SIGNAL_STRENGTH, 5, 0, 0},
    {25, TYPE_TEMP, 58, 0, 1},
    {86, TYPE_HUMIDITY, 20, 0, 1},
    {331, TYPE_DIGITAL_INPUT, 19, 1, 1}, //BTSMP1 magnetic	//35
//    {239, TYPE_DIGITAL_INPUT, 20, 1, 0}, //Fm4 ignition
    {239, TYPE_DIGITAL_INPUT, 4, 1, 0}, //Fm4 ignition mapped on General ignition (senslog hardcoded)
    {240, TYPE_DIGITAL_INPUT, 21, 1, 0}, //Fm4 movement
    {26, TYPE_TEMP, 58, 0, 2},
    {104, TYPE_HUMIDITY, 20, 0, 2},
    {332, TYPE_DIGITAL_INPUT, 19, 1, 2}, //BTSMP1 magnetic
    {113, 56, 4, 0, 0},		//battery level
    {205, 43, 10, 0, 0},	//cell id
    {17, 39, 3, 0, 0},		//x axis
    {18, 39, 4, 0, 0},		//y axis
    {19, 39, 5, 0, 0},		//z axis
    {0, 0, 0, 0, 0}
};
*/

uint16_t sensorTypes[][5] =
{
//    {1, TYPE_DIGITAL_INPUT, 5, 0, 0},	//0
    {2, TYPE_DIGITAL_INPUT, 6, 1, 0},
    {3, TYPE_DIGITAL_INPUT, 7, 0, 0},
    {4, TYPE_DIGITAL_INPUT, 8, 0, 0},
    {9, TYPE_ANALOG_INPUT, 1, 0, 0},
    {10, TYPE_ANALOG_INPUT, 2, 0, 0},	//5
    {11, TYPE_ANALOG_INPUT, 3, 0, 0},
//    {19, TYPE_ANALOG_INPUT, 4, 0, 0},	//possibly collision
//    {21, TYPE_SIGNAL_STRENGTH, 3, 0, 0},
    {24, TYPE_SPEED, 2, 0, 0},
    {66, TYPE_VOLTAGE, 11, 0, 0},	//10
    {67, TYPE_VOLTAGE, 10, 0, 0},
//    {68, TYPE_CURRENT, 2, 0, 0},
//    {69, TYPE_SIGNAL_STRENGTH, 4, 0, 0},
    {70, TYPE_THERMOMETER, 11, 0, 0},
    {72, TYPE_THERMOMETER, 12, 0, 0},	//15
    {73, TYPE_THERMOMETER, 13, 0, 0},
    {74, TYPE_THERMOMETER, 14, 0, 0},
    {79, TYPE_ACCELERATION, 2, 0, 0},
    {80, TYPE_SPEED, 3, 0, 0},
    {81, TYPE_CRUISE, 1, 0, 0},		//20
    {82, TYPE_CRUISE, 2, 0, 0},
    {83, TYPE_CRUISE, 3, 0, 0},
    {84, TYPE_ENGINE, 1, 0, 0},
    {85, TYPE_ENGINE, 2, 0, 0},
//    {86, TYPE_ENGINE, 3, 0, 0},//todo: possibly collision //25
//    {86, TYPE_HUMIDITY, 20, 0, 0},
//    {87, TYPE_LEVEL, 4, 0, 0},
    {88, TYPE_FREQUENCY, 1, 0, 0},
//    {104, TYPE_TIME, 1, 0, 0},//todo: possibly collision
    {127, TYPE_TEMP, 20, 0, 0},	//30
    {128, TYPE_TEMP, 21, 0, 0},
    {135, TYPE_FLOW, 6, 0, 0},
    {136, TYPE_FLOW, 7, 0, 0},
    {207, TYPE_ID, 1, 0, 0},
//    {249, TYPE_SIGNAL_STRENGTH, 5, 0, 0},	//35
    {25, TYPE_TEMP, 58, 0, 1},
    {86, TYPE_HUMIDITY, 20, 0, 1},
//    {331, TYPE_DIGITAL_INPUT, 19, 1, 1}, //BTSMP1 magnetic
//    {239, TYPE_DIGITAL_INPUT, 20, 1, 0}, //Fm4 ignition
    {239, TYPE_DIGITAL_INPUT, 4, 1, 0}, //Fm4 ignition mapped on General ignition (senslog hardcoded)	//40
    {240, TYPE_DIGITAL_INPUT, 21, 1, 0}, //Fm4 movement
    {26, TYPE_TEMP, 58, 0, 2},
    {104, TYPE_HUMIDITY, 20, 0, 2},
//    {332, TYPE_DIGITAL_INPUT, 19, 1, 2}, //BTSMP1 magnetic
    {113, 56, 4, 0, 0},		//battery level		//45
//    {205, 43, 10, 0, 0},	//cell id
//    {17, 39, 3, 0, 0},		//x axis
//    {18, 39, 4, 0, 0},		//y axis
//    {19, 39, 5, 0, 0},		//z axis
    {148, 90, 1, 0, 0},			//50
    {78, 68, 4, 0, 0},			// IButton 
    {149, 68, 5, 0, 0},			// IButton Teltonika Terminal
    {548, 68, 6, 0, 0},			// BLE EYE Beacon
    {549, 38, 11, 0, 0},		// BLE EYE Beacon rssi, possibly not coliding
    {0, 0, 0, 0, 0}		//55
};

double sensorMulti[] =
{
//    1.0,//0
    1.0,
    1.0,
    1.0,
    1.0,
    1.0,//5
    1.0,
//
//    1.0,
    1.0,
    0.001,	//10
    0.001,
//    0.001,
//    1.0,
    0.1,
    0.1,	//15
    0.1,
    0.1,
    1.0,
    1.0,
    1.0,	//20
    1.0,
    1.0,
    1.0,
    1.0,
//		//25    
//
//    1.0,
    1.0,
//
    1.0,	//30
    1.0,
    1.0,
    1.0,
    1.0,
//    1.0,	//35
    0.01,
    1.0,
//    1.0,
//
    1.0,	//40
    1.0,
    0.01,
    1.0,
//    1.0,
    1.0,	//45
//    1.0,
//    1.0,
//    1.0,
//    1.0
    1.0,	//50
    1.0,
    1.0,
    1.0,
    1.0
};

int getSensorType(uint16_t sensor)
{
    int i;

    i = 0;
    while (sensorTypes[i][0] != 0)
    {
	if (sensorTypes[i][0] == sensor)
	    return i;
	i++;
    }
    return -1;
}

void valuesCopyVal(double value, uint16_t sensor, time_t t, uint8_t type, double * values)
{
    int sensor_index;

    sensor_index = getSensorType(sensor);
    if (sensor_index < 0)
    {
	feederLog(LOG_DEBUG, "fm4: Sensor fm4 type %d unknowen\n", sensor);
	return;
    }
    values[values_len] = value * sensorMulti[sensor_index];
    values_sensor[values_len] = sensorTypes[sensor_index][2] * 10000 + sensorTypes[sensor_index][1] * 10000000;
    values_sensor[values_len] += sensorTypes[sensor_index][4];
    values_time = t;
    if ((type == VALUES_TYPE_ALERT) && (sensorTypes[sensor_index][3] == 1))
	values_type[values_len] = VALUES_TYPE_ALERT;
    else
	values_type[values_len] = VALUES_TYPE_OBS;
    feederLog(LOG_DEBUG, "fm4: Observation type %s, index %d\n", (values_type[values_len] == VALUES_TYPE_ALERT) ? "alert" : "obs", sensor_index);
	
    values_len++;
}

void valuesSpecCareRfid(uint64_t val, uint8_t sensor, time_t t, uint8_t type, double * values)
{
    char str[9], c;
    int i, j;
    int in;
    uint8_t rfid[8];
    uint32_t id;
    uint8_t rssi;
    
    if (val == 0)
    {
	feederLog(LOG_DEBUG, "fm4: RFID zero value\n");
	return;
    }
    in = 0;
    str[0] = 0;
    for (i = 0, j = 0; i < 8; i++)
    {
    	c = (uint8_t)(val >> (8 * i));
    	if (in)
	{
	    if (c == ']')
	    {
		in  = 0;
		str[j++] = 0;
	    }
	    else
	    {
		rfid[j++] = c;
	    }
	}
	if (c == '[')
	    in = 1;
    }
//    rfid = atol(str);
    (void)str;	//otherwise compilator complains
    
    if (j < 5)
    {
	feederLog(LOG_DEBUG, "fm4: RFID no tag detected\n");
	return;
    }
    
    id = (uint32_t)rfid[3] | ((uint32_t)rfid[2] << 8) | ((uint32_t)rfid[1] << 16);
    rssi = rfid[0];
    feederLog(LOG_DEBUG, "fm4: RFID %#06X, rssi %#02X\n", id, rssi);
    valuesCopyVal((double)id, sensor, t, type, values);
    valuesCopyVal((double)rssi, 249, t, type, values);
}

void valuesSpecCareiButton(uint64_t val, uint8_t sensor, time_t t, uint8_t type, double * values)
{
    uint32_t id;

    id = (val >> 8) & 0xFFFFFF;
    feederLog(LOG_DEBUG, "fm4: iButton %ld\n", id);
    valuesCopyVal((double)id, sensor, t, type, values);
}

void valuesSpecCareBleFlags(uint8_t val, uint16_t sensor, time_t t, uint8_t type, double * values)
{
    uint8_t magnetic;

    magnetic = (val >> 3) & 0x01;
    feederLog(LOG_DEBUG, "fm4: Ble %d magnetic %d\n", 1, magnetic);
    valuesCopyVal((double)magnetic, sensor, t, type, values);
}


void valuesSpecCareBLEAdvetismentData(uint8_t * val, uint16_t len, uint16_t sensor, time_t t, uint8_t type, double * values)
{
    char buffer[256];
    uint16_t i;
    
    (void)sensor;
    (void)t;
    (void)type;
    (void)values;
    
    feederLog(LOG_DEBUG, "fm4: Ble advertisment data len %d\n", len);

    for (i = 1; i < len; i += 18)
    {
        fm4LogHex(buffer, val + i + 1, 16);
        feederLog(LOG_DEBUG, "fm4: Ble advertisment data %s\n", buffer);
    }
}

void valuesSpecCareBLEAdvetismentDataAdvanced(uint8_t * val, uint16_t len, uint16_t sensor, time_t t, uint8_t type, double * values)
{
    char buffer[256];
    uint16_t i, j, p, par_len;
    int8_t rssi, rssi_max;
    uint64_t val_int;

    (void)len;

    feederLog(LOG_DEBUG, "fm4: Ble advertisment advanced beacon count %d\n", val[0]);
    p = 1;
    rssi_max = -127;
    val_int = 0;
    for (i = 0; i < val[0]; i++)
    {
	rssi = (int8_t)val[p];
	p += 1;
	par_len = val[p];
	p += 1;
        fm4LogHex(buffer, val + p, par_len);
        feederLog(LOG_DEBUG, "fm4: Ble advertisment advanced rssi %d id %s\n", rssi, buffer);
        if (rssi > rssi_max)
        {
    	    rssi_max = rssi;
    	    val_int = 0;
    	    for (j = 0; j < par_len; j++)
    	    {
    		if (j > 0)
    		    val_int <<= 8;
    		val_int |= (uint64_t)(val[p + j]);
    		feederLog(LOG_DEBUG, "fm4: %llX\n", val_int);
    	    }
        }
	p += par_len;
	par_len = val[p];
	p += 1;
	p += par_len;
    }
    if (val[0] > 0)
    {
        feederLog(LOG_DEBUG, "fm4: %lf\n", (double)val_int);
	valuesCopyVal((double)val_int, sensor, t, type, values);
	valuesCopyVal((double)rssi_max, 549, t, type, values);
    }
}


#define CODEC_NONE 0
#define CODEC_CODEC8 0x08
#define CODEC_CODEC8EX 0x8E
#define CODEC_CODEC16 0x10
#define CODEC_CODEC12 0x0C

int stateMachine(unsigned char c)
{
    static uint8_t temp[2048];//todo: prechacani pri codecu12 (umi poslat hodne najednou)
    static uint32_t count = 0;
    static uint16_t qua = 0, c_qua = 0;
    static uint8_t no_of_recs = 0, current_rec = 0;
    static uint16_t val_byte_id;
    static uint64_t val_byte_val;
    static uint8_t codec = CODEC_NONE;
    
    static float lat, lon, alt;
    static uint8_t prio;
    static time_t t_t;
    static struct tm t;
    
    static uint32_t * counts = counts_codec8;
    
    int ret;
    
    ret = 0;
/*
    if (state != STATE_IDLE)
    //    printf("0x%x %d|", c, state);
        feederLog(LOG_DEBUG, "fm4: |0x%x %d|\n", c, state);
*/
    temp[count] = c;
    count++;
    
    if (count < counts[state])
	return 0;
    count = 0;

    switch (state)
    {
	case STATE_IDLE:
	    if (*((uint32_t *)temp) == 0x00000000)
		state = STATE_DATA_LENGTH;
	break;
	case STATE_DATA_LENGTH:
	    feederLog(LOG_DEBUG, "fm4: Data size: %d\n", byteSwap32(*((uint32_t *)temp)));
	    state = STATE_CODEC_ID;
	break;
	case STATE_CODEC_ID:
	    codec = temp[0];
	    if (codec == CODEC_CODEC8)
	    {
	    	feederLog(LOG_DEBUG, "fm4: Codec 8 detected\n");
	    	counts = counts_codec8;
		state = STATE_NO_OF_REC;
	    }
	    else if (codec == CODEC_CODEC8EX)
	    {
	    	feederLog(LOG_DEBUG, "fm4: Codec 8 extended detected\n");
	    	counts = counts_codec8ex;
		state = STATE_NO_OF_REC;
	    }
	    else if (codec == CODEC_CODEC12)
	    {
	    	feederLog(LOG_DEBUG, "fm4: Codec 12 detected\n");
	    	counts = counts_codec12;
		state = STATE_NO_OF_REC;
	    }
	    else
	    {
		feederLog(LOG_DEBUG, "fm4: Bad codec, going to idle state\n");
		state = STATE_IDLE;
		ret = 1;
	    }
	break;
	case STATE_NO_OF_REC:
	    no_of_recs = temp[0];
	    current_rec = 0;
	    feederLog(LOG_DEBUG, "fm4: Number of records: %d\n", no_of_recs);
	    if (codec == CODEC_CODEC12)
		state = STATE_TYPE;
	    else
		state = STATE_TIMESTAMP;
	break;
	case STATE_TIMESTAMP:
/*	    if (current_rec > 0)
	    {
		if ((lat != 0.0) && (lon != 0.0))
		{
		}
	    }*/
	    
	    lat = lon = alt = 0.0;
	    prio = PRIO_LOW;
	    t_t = 0;
	    gmtime_r(&t_t, &t);
	
	    t_t = (byteSwap64(*((uint64_t *)temp))) / 1000;	//in ms
//	    gmtime_r(&t_t, &t);
	    localtime_r(&t_t, &t);//todo: save time in local zone of server, is this good?
	    t_t = mktime(&t) - timezone;

	    feederLog(LOG_DEBUG, "fm4: Time of rec: %04d-%02d-%02d %02d:%02d:%02d\n",
		t.tm_year + 1900, t.tm_mon + 1, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec);
/*
	    if (t_t == aunit->t_t)	//ignore packet with same timestamp as (omit ms) as packet before
	    {
		state = STATE_IDLE;
		feederLog(LOG_DEBUG, "fm4: Packet with duplicate time (ignoring ms), omitting");
		current_rec++;
		break;
	    }*/
	    aunit->t_t = t_t;
	    
	    feederLog(LOG_DEBUG, "fm4: Processing record: %d\n", current_rec);
    	    current_rec++;
	    state = STATE_PRIORITY;
	break;
	case STATE_PRIORITY:
	    prio = temp[0] & 0x03;
	    feederLog(LOG_DEBUG, "fm4: Priority: %d\n", prio); 
	    state = STATE_LON;
	break;
	case STATE_LON:
	    lon = (float)(int32_t)(byteSwap32(*(uint32_t *)temp)) / 10000000.0;
	    feederLog(LOG_DEBUG, "fm4: lon: %f\n", lon);
	    state = STATE_LAT;
	break;
	case STATE_LAT:
	    lat = (float)(int32_t)(byteSwap32(*(uint32_t *)temp)) / 10000000.0;
	    feederLog(LOG_DEBUG, "fm4: lat: %f\n", lat);
	    state = STATE_ALT;
	break;
	case STATE_ALT:
	    alt = (float)(byteSwap16(*(uint16_t *)temp));
	    feederLog(LOG_DEBUG, "fm4: alt: %f\n", alt);
	    state = STATE_ANGLE;
	break;
	case STATE_ANGLE:
	    feederLog(LOG_DEBUG, "fm4: angle: %f\n", (float)(byteSwap16(*(uint16_t *)temp)));
	    state = STATE_SATTS;
	break;
	case STATE_SATTS:
	    feederLog(LOG_DEBUG, "fm4: sattelites: %d\n", temp[0]);
	    state = STATE_SPEED;
	break;
	case STATE_SPEED:
	    feederLog(LOG_DEBUG, "fm4: speed: %f\n", (float)(byteSwap16(*(uint16_t *)temp)));
	    
	    valuesCopyPos(lat, lon, alt, t_t, VALUES_TYPE_POS, values_out);
	    ret = 2;
	    
	    state = STATE_EVENT_IO_ID;
	break;
	case STATE_EVENT_IO_ID:
	    //feederLog(LOG_DEBUG, "fm4: event io id: %d\n", temp[0]);
	    state = STATE_TOTAL_IO;
	break;
	case STATE_TOTAL_IO:
	    //feederLog(LOG_DEBUG, "fm4: total io: %d\n", temp[0]);
	    values_len = 0;
	    state = STATE_BYTE1_IO;
	break;
	case STATE_BYTE1_IO:
	    if ((codec == CODEC_CODEC8) || (codec == CODEC_CODEC12))
		qua = temp[0];
	    else
		qua = (uint16_t)(byteSwap16(*(uint16_t *)temp));
	    c_qua = 0;
	    feederLog(LOG_DEBUG, "fm4: byte1 io: %d\n", qua);
	    if (qua == 0)
		state = STATE_BYTE2_IO;
	    else
		state = STATE_BYTE1_ID;
	break;
	case STATE_BYTE1_ID:
	    if ((codec == CODEC_CODEC8) || (codec == CODEC_CODEC12))
		val_byte_id = temp[0];
	    else
		val_byte_id = (uint16_t)(byteSwap16(*(uint16_t *)temp));
	    feederLog(LOG_DEBUG, "fm4: byte1 id: %d\n", val_byte_id);
	    state = STATE_BYTE1_VAL;
	break;
	case STATE_BYTE1_VAL:
	    feederLog(LOG_DEBUG, "fm4: byte1 val: %d\n", temp[0]);
	    val_byte_val = temp[0];
	    if (prio == 0)
		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    else
		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_ALERT, values_out);
	    c_qua++;
	    if (qua == c_qua)
		state = STATE_BYTE2_IO;
	    else
		state = STATE_BYTE1_ID;
	break;
	case STATE_BYTE2_IO:
	    if ((codec == CODEC_CODEC8) || (codec == CODEC_CODEC12))
		qua = temp[0];
	    else
		qua = (uint16_t)(byteSwap16(*(uint16_t *)temp));
	    c_qua = 0;
	    feederLog(LOG_DEBUG, "fm4: byte2 io: %d\n", qua);
	    if (qua == 0)
		state = STATE_BYTE4_IO;
	    else
		state = STATE_BYTE2_ID;
	break;
	case STATE_BYTE2_ID:
	    if ((codec == CODEC_CODEC8) || (codec == CODEC_CODEC12))
		val_byte_id = temp[0];
	    else
		val_byte_id = (uint16_t)(byteSwap16(*(uint16_t *)temp));
	    feederLog(LOG_DEBUG, "fm4: byte2 id: %d\n", val_byte_id);
	    state = STATE_BYTE2_VAL;
	break;
	case STATE_BYTE2_VAL:
	    feederLog(LOG_DEBUG, "fm4: byte2 val: %lld\n", (byteSwap16(*(uint16_t *)temp)));
	    val_byte_val = (byteSwap16(*(uint16_t *)temp));
	    if (prio == 0)
    		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
    	    else
    		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_ALERT, values_out);
	    c_qua++;
	    if (qua == c_qua)
		state = STATE_BYTE4_IO;
	    else
		state = STATE_BYTE2_ID;
	break;
	case STATE_BYTE4_IO:
	    if ((codec == CODEC_CODEC8) || (codec == CODEC_CODEC12))
		qua = temp[0];
	    else
		qua = (uint16_t)(byteSwap16(*(uint16_t *)temp));
	    c_qua = 0;
	    feederLog(LOG_DEBUG, "fm4: byte4 io: %d\n", qua);
	    if (qua == 0)
			state = STATE_BYTE8_IO;
	    else
			state = STATE_BYTE4_ID;
	break;
	case STATE_BYTE4_ID:
	    if ((codec == CODEC_CODEC8) || (codec == CODEC_CODEC12))
		val_byte_id = temp[0];
	    else
		val_byte_id = (uint16_t)(byteSwap16(*(uint16_t *)temp));
	    feederLog(LOG_DEBUG, "fm4: byte4 id: %d\n", val_byte_id);
	    state = STATE_BYTE4_VAL;
	break;
	case STATE_BYTE4_VAL:
	    feederLog(LOG_DEBUG, "fm4: byte4 val: %d\n", (byteSwap32(*(uint32_t *)temp)));
	    val_byte_val = (byteSwap32(*(uint32_t *)temp));
	    if (prio == 0)
		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    else
		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_ALERT, values_out);
	    c_qua++;
	    if (qua == c_qua)
			state = STATE_BYTE8_IO;
	    else
			state = STATE_BYTE4_ID;
	break;
	case STATE_BYTE8_IO:
	    if ((codec == CODEC_CODEC8) || (codec == CODEC_CODEC12))
		qua = temp[0];
	    else
		qua = (uint16_t)(byteSwap16(*(uint16_t *)temp));
	    c_qua = 0;
	    feederLog(LOG_DEBUG, "fm4: byte8 io: %ld\n", qua);
	    if (qua == 0)
	    {
    		if (codec == CODEC_CODEC8)
		{
		    if (current_rec == no_of_recs)
			state = STATE_NO_OF_REC_END;
		    else
			state = STATE_TIMESTAMP;
		    ret = 2;
		}
		else if (codec == CODEC_CODEC12)
		{
		    state = STATE_NO_OF_REC_END;
		    ret = 2;
		}
		else
		    state = STATE_BYTEX_IO;
	    }
	    else
		state = STATE_BYTE8_ID;
	break;
	case STATE_BYTE8_ID:
	    if ((codec == CODEC_CODEC8) || (codec == CODEC_CODEC12))
		val_byte_id = temp[0];
	    else
		val_byte_id = (uint16_t)(byteSwap16(*(uint16_t *)temp));
	    feederLog(LOG_DEBUG, "fm4: byte8 id: %d\n", val_byte_id);
	    state = STATE_BYTE8_VAL;
	break;
	case STATE_BYTE8_VAL:
	    feederLog(LOG_DEBUG, "fm4: byte8 val: %lld\n", (byteSwap64(*(uint64_t *)temp)));
	    val_byte_val = (byteSwap64(*(uint64_t *)temp));
	    if (val_byte_id == 207)		//rfid value
		valuesSpecCareRfid(val_byte_val, val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    else if ((val_byte_id == 78) || (val_byte_id == 149))
		valuesSpecCareiButton(val_byte_val, val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    else 
		valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    
	    c_qua++;
	    if (qua == c_qua)
	    {
    		if (codec == CODEC_CODEC8)
		{
		    if (current_rec == no_of_recs)
			state = STATE_NO_OF_REC_END;
		    else
			state = STATE_TIMESTAMP;
		    ret = 2;
		}
		else if (codec == CODEC_CODEC12)
		{
		    state = STATE_NO_OF_REC_END;
		    ret = 2;
		}
		else
		    state = STATE_BYTEX_IO;
	    }
	    else
		state = STATE_BYTE8_ID;
	break;
	
	case STATE_BYTEX_IO:
	    qua = (uint16_t)(byteSwap16(*(uint16_t *)temp));
	    c_qua = 0;
	    feederLog(LOG_DEBUG, "fm4: bytex io: %ld\n", qua);
	    if (qua == 0)
	    {
		if (current_rec == no_of_recs)
		    state = STATE_NO_OF_REC_END;
		else
		    state = STATE_TIMESTAMP;
		ret = 2;
	    }
	    else
		state = STATE_BYTEX_ID;
	break;
	case STATE_BYTEX_ID:
	    val_byte_id = (uint16_t)(byteSwap16(*(uint16_t *)temp));
	    feederLog(LOG_DEBUG, "fm4: bytex id: %d\n", val_byte_id);
	    state = STATE_BYTEX_LEN;
	break;
	case STATE_BYTEX_LEN:
	    counts[STATE_BYTEX_VAL] = (uint16_t)(byteSwap16(*(uint16_t *)temp));
	    feederLog(LOG_DEBUG, "fm4: bytex len: %d\n", counts[STATE_BYTEX_VAL]);
	    state = STATE_BYTEX_VAL;
	    if (counts[STATE_BYTEX_VAL] > 0)
	    	break;
	case STATE_BYTEX_VAL:
	    //feederLog(LOG_DEBUG, "fm4: bytex val: %lld\n", (byteSwap64(*(uint64_t *)temp)));
	    //val_byte_val = (byteSwap64(*(uint64_t *)temp));
	    //valuesCopyVal((double)val_byte_val, val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    //todo: special care
	    
	    if ((val_byte_id == 331) || (val_byte_id == 332) || (val_byte_id == 333) || (val_byte_id == 334))	//BLE 1  Custom #1
	    {
		valuesSpecCareBleFlags(temp[0], val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    }
	    else if (val_byte_id == 10500)
	    {
		valuesSpecCareBLEAdvetismentData(temp, counts[STATE_BYTEX_VAL], val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    }
	    else if (val_byte_id == 548)
	    {
		valuesSpecCareBLEAdvetismentDataAdvanced(temp, counts[STATE_BYTEX_VAL], val_byte_id, t_t, VALUES_TYPE_OBS, values_out);
	    }
	    else
		feederLog(LOG_DEBUG, "fm4: unprocessed bytex id %d\n", val_byte_id);
	    
	    c_qua++;
	    if (qua == c_qua)
	    {
		if (current_rec == no_of_recs)
		    state = STATE_NO_OF_REC_END;
		else
		    state = STATE_TIMESTAMP;
		
		ret = 2;
	    }
	    else
		state = STATE_BYTEX_ID;
	break;
	
	
	
	
	case STATE_NO_OF_REC_END:
	    no_of_recs = temp[0];
	    feederLog(LOG_DEBUG, "fm4: Number of records: %d\n", temp[0]);
	    state = STATE_CRC;
	break;
	case STATE_CRC:
	    state = STATE_IDLE;
	    feederLog(LOG_DEBUG, "fm4: Crc\n");

	    reply_buffer[0] = 0x00;
	    reply_buffer[1] = 0x00;
	    reply_buffer[2] = 0x00;
	    reply_buffer[3] = no_of_recs;
	    reply_len = 4;
	    ret = 1;
	break;
	
	
	
	case STATE_TYPE:
	    feederLog(LOG_DEBUG, "fm4: Type %d\n", temp[0]);
	    state = STATE_COMMAND_RESPONSE_SIZE;
	break;
	case STATE_COMMAND_RESPONSE_SIZE:
	    counts[STATE_COMMAND_RESPONSE] = (uint32_t)(byteSwap32(*(uint32_t *)temp));
	    feederLog(LOG_DEBUG, "fm4: Command/Response length %d\n", counts[STATE_COMMAND_RESPONSE]);
	    //state = STATE_COMMAND_RESPONSE;
	    state = STATE_EVENT_IO_ID;
	break;
	case STATE_COMMAND_RESPONSE:
	    state = STATE_NO_OF_REC_END;
	break;
    }

    memset(temp, 0, 16);
    return ret;
}

int parse(struct unit_t * unit)
{
    int ret, res;
    
    ret = 0;

    while (res = stateMachine(unit->int_buffer[unit->int_buffer_pos]), res == 0)
    {
	if (unit->int_buffer_pos + 1 >= unit->int_buffer_pos_wr)
	    break;
	unit->int_buffer_pos++;

    }
    unit->int_buffer_pos++;
    
    if (res == 1)
    {
		feederLog(LOG_DEBUG, "fm4: End of data packet\n");

		unit->int_buffer_pos_wr = unit->int_buffer_pos = 0;
//		aunit->imei[0] = 0;		//todo: predpokladame ze dalsi paket bude opet uvozen 17byty s IMEI
    }
    if (res == 2)
    {
	feederLog(LOG_DEBUG, "fm4: Valid data decoded\n");
    
	ret = 1;
    }
    return ret;
}

//----------------------------------------------------------------------------------------------

struct unit_t * unitFind(int fd)
{
    struct unit_t * unit;
    
    unit = units;
    while (unit != NULL)
    {
	if (unit->fd == fd)
	{
	    return unit;
	}
	unit = unit->next;
    }
    return NULL;
}

struct unit_t * unitCreate(int fd)
{
    struct unit_t * unit, * unit_new;
    
    unit_new = malloc(sizeof(struct unit_t));

    if (unit_new == NULL)
	return NULL;
    unit_new->fd = fd;
    unit_new->imei[0] = 0;
    unit_new->next = NULL;
    unit_new->int_buffer = malloc(FM4_BUFFER_SIZE);
    if (unit_new->int_buffer == NULL)
    {
	free(unit_new);
	return NULL;
    }
    unit_new->int_buffer_pos = 0;
    unit_new->int_buffer_pos_wr = 0;

    if (units == NULL)
	units = unit_new;
    else
    {
	unit = units;
	while (unit->next != NULL)
	{
	    unit = unit->next;
	}
	unit->next = unit_new;
    }

    return unit_new;
}


int imei(struct unit_t * unit, unsigned char * data)
{
    struct avl_header_t * avl_header;
    char imei_str[16];

    avl_header = (struct avl_header_t *)data;
    memcpy(imei_str, avl_header->imei, 15);
    imei_str[15] = 0;
    feederLog(LOG_DEBUG, "fm4: Imei is %s\n", imei_str);

    strcpy(unit->imei, imei_str);

    return 1;
}

int init(void * param)
{
    param = param;

    bzero(reply_buffer, 32);
    reply_len = 0;
    units = aunit = NULL;
    return 0;
}


void fm4LogHex(char * buffer, unsigned char * data, unsigned int length)
{
    unsigned int i;

    buffer[0] = 0;
    for (i = 0; i < length; i++)
    {
	sprintf(buffer + strlen(buffer), "%02X ", data[i]);
    }
}

unsigned int
process(void *lib_data, int socket, unsigned char *data,
        unsigned int length, unsigned long long int *id, time_t * tm,
        double *result_array, uint64_t * sensors, unsigned int *type)
{
    unsigned int ret = 0;
    struct unit_t * unit;
    uint8_t i;
    char buffer[32768];
    unsigned int l;

    (void)lib_data;
    if (data != NULL)
    {
        feederLog(LOG_DEBUG, "fm4: Incoming data: len %d: \n", length);
        l = length;
        while (l > 0)
        {
	    fm4LogHex(buffer, data + length - l, (l > 16) ? 16:l);
	    feederLog(LOG_DEBUG, "%s\n", buffer);
	    l -= (l > 16) ? 16:l;
	}
    }
    unit = unitFind(socket);
    
    if (unit == NULL)
    {
	feederLog(LOG_DEBUG, "fm4: new unit for socket %d\n", socket);
    
        unit = unitCreate(socket);	//todo: check if scucsefull
        state = STATE_IDLE;	///todo: state do structury jednotky
    }
    else
    {
	feederLog(LOG_DEBUG, "fm4: data from known unit on socket %d\n", socket);
    }

    aunit = unit;

//    if ((length != 0) && (strlen(aunit->imei) == 0))
    if ((length != 0) && ((memcmp(data, "\0\0\0\0", 4) != 0) && (state == STATE_IDLE)) && (unit->int_buffer_pos_wr == 0))
    {
	imei(aunit, data);
	feederLog(LOG_DEBUG, "fm4: imei for socket %d is %s\n", socket, unit->imei);
	
	unit->int_buffer_pos = unit->int_buffer_pos_wr = 0;
	
	reply_buffer[0] = 0x01;
	reply_len = 1;

	ret = 0;
    }
    else
    {
	if (length != 0)
	{
	    feederLog(LOG_DEBUG, "fm4: Copying data for unit, imei %s\n", unit->imei);
	
	    if (unit->int_buffer_pos_wr + length >= FM4_BUFFER_SIZE)
	    {
		feederLog(LOG_WARNING, "fm4: Buffer full, reseting,  imei %s\n", unit->imei);
		unit->int_buffer_pos = unit->int_buffer_pos_wr = 0;
	    }
	    else
	    {
		if ((unit->int_buffer_pos_wr != 0) || (*((uint32_t *)data) == 0x00000000))
		{
		    memcpy(unit->int_buffer + unit->int_buffer_pos_wr, data, length);
		    unit->int_buffer_pos_wr += length;
		    feederLog(LOG_DEBUG, "fm4: Copy done, imei %s\n", unit->imei);
		}
	    }
	}
	
	if ((unit->int_buffer_pos_wr > 0) && (*((uint32_t *)unit->int_buffer) == 0x00000000))
	{
	    uint32_t reported_packet_size = byteSwap32(*((uint32_t *)(unit->int_buffer + 4)));
	    reported_packet_size += 4 + 4 + 4;		/* leading zeroes + packet size + crc */
//	    if (unit->int_buffer_pos_wr != reported_packet_size)
	    if (unit->int_buffer_pos_wr < reported_packet_size)
	    {
		feederLog(LOG_DEBUG, "fm4: Not enough packet data, waiting for more, imei %s\n", unit->imei);
	    }
	    else
	    {
	if ((parse(unit)) || (values_len != 0))
	{
	    *id = atol(aunit->imei);
	    *tm = values_time;
//	    printf("val type %d\n", values_type[0]);
	    if (values_type[0] == VALUES_TYPE_POS)
	    {
		memcpy(result_array, values_out, sizeof(double) * 3);
	    
	        *type = VALUES_TYPE_POS;
	        sensors[0] = sensors[1] = sensors[2] = 0x10;
	        ret = 3;
	        values_len -= 3;
	    }
	    else if (values_type[values_len - 1] == VALUES_TYPE_OBS)
	    {
		i = 0;
		while ((values_len > 0) && (values_type[values_len - 1] == VALUES_TYPE_OBS))
		{
		    result_array[i] = values_out[values_len - 1];
		    sensors[i] = values_sensor[values_len - 1];
		    i++;
		    values_len--;
		}
		*type = VALUES_TYPE_OBS;
		ret = i;
		
		/*
		memcpy(result_array, values_out, sizeof(double) * values_len);
		*type = VALUES_TYPE_OBS;
		memcpy(sensors, values_sensor, sizeof(uint64_t) * values_len);
		ret = values_len;
		*/
	    }
	    else if (values_type[values_len - 1] == VALUES_TYPE_ALERT)
	    {
		i = 0;
		while ((values_len > 0) && (values_type[values_len - 1] == VALUES_TYPE_ALERT))
		{
		    result_array[i] = values_out[values_len - 1];
		    sensors[i] = values_sensor[values_len - 1];
		    i++;
		    values_len--;
		}
		*type = VALUES_TYPE_ALERT;
		ret = i;
	    
/*		memcpy(result_array, values_out, sizeof(double) * values_len);
		*type = VALUES_TYPE_ALERT;
		memcpy(sensors, values_sensor, sizeof(uint64_t) * values_len);
		ret = values_len;*/
	    }
	    }
	    }
	}
    }

    return ret;
}

int reply(void *lib_data, int socket, unsigned char *data)
{
    unsigned int temp_reply_len;

    (void)lib_data;
    (void)socket;
    feederLog(LOG_DEBUG, "fm4: replying\n");

    memcpy(data, reply_buffer, reply_len);
    temp_reply_len = reply_len;
    reply_len = 0;

    return temp_reply_len;
}

int open(void *lib_data, int socket)
{
    (void)lib_data;
    (void)socket;
    feederLog(LOG_DEBUG, "fm4: socket %d opened\n", socket);

    return 0;
}

int close(void *lib_data, int socket)
{
    struct unit_t * unit;

    (void)lib_data;
    (void)socket;
    feederLog(LOG_DEBUG, "fm4: socket %d closed\n", socket);

    unit = unitFind(socket);
    if (unit != NULL)
    {
//	free(unit->int_buffer);
	unit->imei[0] = 0;		//todo: nebo mozna lepe, zrusit celou jednotku - coz bacha na next
	unit->fd = 0;
    }

    unit->int_buffer_pos_wr = unit->int_buffer_pos = 0;
    return 0;
}