#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <stdlib.h>
#include <cstdlib>
#include <termios.h>
#include <netinet/in.h>
#include <pthread.h>
#include <signal.h>
#include <sys/ipc.h>
#include <sys/sem.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <time.h>
#include <ctype.h>
#include <inttypes.h>

#include <linux/serial.h>
#include <sys/ioctl.h>

#ifdef WITH_IMAGE_DISPLAY
#include <SDL/SDL.h>
#include <SDL/SDL_image.h>
#endif

#define XBEE_START_DELIMITER_BYTE        0
#define XBEE_LENGTH_MSB_BYTE             1
#define XBEE_LENGTH_LSB_BYTE             2
#define XBEE_API_IDENTIFIER              3
#define XBEE_SRC_ADDR_MSB                4
#define XBEE_SRC_ADDR_LSB                8
#define XBEE_RSSI                        12
#define XBEE_OPTION                      13
#define XBEE_DATA                        14

#define XBEE_DM_RESERVED                 12
#define XBEE_DM_RCV_OPTION               14
#define XBEE_DM_DATA                     15

#define XBEE_CHECKSUM_SIZE               1
#define LIBELIUM_APP_HEADER_SIZE         9
#define IMOTE2_HEADER_SIZE               4
#define TOS_HEADER_SIZE			 2


#define MAX_API_FRAME_LENGTH 130
#define MAX_APP_FRAME_LENGTH 100

#define START_BYTE 0x7E
#define ESCAPE 0x7D
#define XON 0x11
#define XOFF 0x13

#define MAX_BINARY_FILE_SIZE             64000

uint8_t* myRawData=NULL;
uint8_t myRawData_len;
uint8_t* myData=NULL;
uint8_t myData_len;

uint8_t* myFileBuff=NULL;
uint16_t myFileBuff_len=0;

uint8_t* frame;

int nbPktRcv=0;
int nbPktAccepted=0;
int nbPktSuccess=0;
int nbPktTimeout=0;
int nbPktError=0;
int nbPktDiscarded=0;

// disable display if set to true with -nd
bool pktDisplay=false;
// do not write a pkt list file if set to true with -npktf
bool pktFile=false;
// indicate the image mode
bool image_mode=true;
// indicate the binary mode
bool binary_mode=false;
// indicate the pipe mode
bool pipe_mode=false;
// indicate the stream mode, no offset in received pkt
bool stream_mode=false;
// indicate the fill mode to fill miss/drop packets
bool fill_mode=false;
// value that will be used to fill miss packets
uint8_t binaryFilledValue=0x01;
// indicate an XBEE running in AP0 mode
bool xbeeAP0=false;
// use framing bytes
bool framing_mode=false;

int qualityFactor=50;
int fileSN=1;

int sd = 3;
char *serialPort = "/dev/ttyUSB0";

struct termios opciones;

int speed = B38400;

//int speed = B115200;
int customSpeed=0;

timespec diff(timespec start, timespec end)
{
        timespec temp;
        if ((end.tv_nsec-start.tv_nsec)<0) {
                temp.tv_sec = end.tv_sec-start.tv_sec-1;
                temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec;
        } else {
                temp.tv_sec = end.tv_sec-start.tv_sec;
                temp.tv_nsec = end.tv_nsec-start.tv_nsec;
        }
        return temp;
}

uint8_t str2hex(char* str)
{
        int aux=0, aux2=0;

        if( (*str>='0') && (*str<='9') )
        {
                aux=*str++-'0';
        }
        else if( (*str>='A') && (*str<='F') )
        {
                aux=*str++-'A'+10;
        }
        if( (*str>='0') && (*str<='9') )
        {
                aux2=*str-'0';
        }
        else if( (*str>='A') && (*str<='F') )
        {
                aux2=*str-'A'+10;
        }
        return aux*16+aux2;
}

int init_serial()
{
   if ((sd = open(serialPort, O_RDWR | O_NOCTTY | O_NONBLOCK | O_NDELAY )) == -1)
   {
      fprintf(stderr, "Unable to open the serial port %s - \n", serialPort);
      exit(-1);
   }
   else
   {
      if (!sd)
      {
         /* Sometimes the first time you call open it does not return the
          * right value (3) of the free file descriptor to use, for this
          * reason you can set manually the sd value to 3 or call again
          * the open function (normally returning 4 to sd), advised!
          */
         sd = open(serialPort, O_RDWR | O_NOCTTY | O_NONBLOCK | O_NDELAY);
      }

      fprintf(stderr,"Serial Port open at: %i\n", sd);
      //fcntl(sd, F_SETFL, 0);
      // non blocking read
      fcntl(sd, F_SETFL, FNDELAY);
   }

   tcgetattr(sd, &opciones);
   cfsetispeed(&opciones, speed);
   cfsetospeed(&opciones, speed);

   /*
   * apply
   */
   // use raw format to avoid conversion from the terminal
   /* Equivalent to
        c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP
                | INLCR | IGNCR | ICRNL | IXON);
        c_oflag &= ~OPOST;
        c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
        c_cflag &= ~(CSIZE | PARENB);
        c_cflag |= CS8;
   */
   cfmakeraw(&opciones) ;
   tcsetattr(sd, TCSANOW, &opciones);

   // if we have a custom speed, 50000, 100000 or 125000, then the custon divisor is 48°, 240 and 192 respectively
   // you need to have the XBee module working at the same speed
   if (customSpeed)  {
       struct serial_struct sstruct;

       fprintf(stderr, "init_serial: custom baud rate parameter is: %d\n", customSpeed);

       if (ioctl(sd, TIOCGSERIAL, &sstruct) < 0) {
           fprintf(stderr, "Error: could not get comm ioctl\n");
           exit(-1);
       }

       //int custDiv=24000000/customSpeed;

       //sstruct.custom_divisor = custDiv;

       sstruct.custom_divisor = (sstruct.baud_base + (customSpeed / 2)) / customSpeed;
       sstruct.flags |= ASYNC_SPD_CUST;

       if (ioctl(sd, TIOCSSERIAL, &sstruct) < 0) {
           fprintf(stderr, "Error: could not set custom comm baud divisor\n");
           exit(-1);
       }

       fprintf(stderr, "init_serial: actual baudrate is %d / %d = %f\n",
                                sstruct.baud_base, sstruct.custom_divisor,
                                (float)sstruct.baud_base / sstruct.custom_divisor);

       if (sstruct.custom_divisor * customSpeed != sstruct.baud_base) {
            fprintf(stderr, "Error: actual baudrate is %d / %d = %f\n",
                                     sstruct.baud_base, sstruct.custom_divisor,
                                     (float)sstruct.baud_base / sstruct.custom_divisor);
            exit(-1);
       }
   }
}

// begin from CRAN

/************************************************************************
 *																		*
 *	Fast DCT for image compression scheme								*
 *																		*
 *	Author: Vincent LECUIRE, CRAN UMR 7039, Nancy-Université, CNRS		*
 *	Date: march, 16 2012												*
 *																		*
 ************************************************************************/

#include "bmp.h"
#include "mqc.h"

#define CRAN_ENCODING_IMAGE_TYPE -1

#define a4   1.38703984532215
#define a7  -0.275899379282943
#define a47  0.831469612302545
#define a5   1.17587560241936
#define a6  -0.785694958387102
#define a56  0.98078528040323
#define a2   1.84775906502257
#define a3   0.765366864730179
#define a23  0.541196100146197
#define a32  1.306562964876376
#define rc2  1.414213562373095

double LuminanceJPEGTable[8][8] = {
  16.0,  11.0,  10.0,  16.0,  24.0,  40.0,  51.0,  61.0,
  12.0,  12.0,  14.0,  19.0,  26.0,  58.0,  60.0,  55.0,
  14.0,  13.0,  16.0,  24.0,  40.0,  57.0,  69.0,  56.0,
  14.0,  17.0,  22.0,  29.0,  51.0,  87.0,  80.0,  62.0,
  18.0,  22.0,  37.0,  56.0,  68.0, 109.0, 103.0,  77.0,
  24.0,  35.0,  55.0,  64.0,  81.0, 104.0, 113.0,  92.0,
  49.0,  64.0,  78.0,  87.0, 103.0, 121.0, 120.0, 101.0,
  72.0,  92.0,  95.0,  98.0, 112.0, 100.0, 103.0,  99.0
};

struct position { int row;	int col;
	} ZigzagCoordinates[8*8]=	// Matrice Zig-Zag
	{0, 0, 0, 1, 1, 0, 2, 0, 1, 1, 0, 2, 0, 3, 1, 2, 2, 1, 3, 0,
	 4, 0, 3, 1, 2, 2, 1, 3, 0, 4, 0, 5, 1, 4, 2, 3, 3, 2, 4, 1,
	 5, 0, 6, 0, 5, 1, 4, 2, 3, 3, 2, 4, 1, 5, 0, 6, 0, 7, 1, 6,
	 2, 5, 3, 4, 4, 3, 5, 2, 6, 1, 7, 0, 7, 1, 6, 2, 5, 3, 4, 4,
	 3, 5, 2, 6, 1, 7, 2, 7, 3, 6, 4, 5, 5, 4, 6, 3, 7, 2, 7, 3,
	 6, 4, 5, 5, 4, 6, 3, 7, 4, 7, 5, 6, 6, 5, 7, 4, 7, 5, 6, 6,
	 5, 7, 6, 7, 7, 6, 7, 7};

/*------------------------------- Compression JPEG ----------------------------------------*/
void QTinitialization(int Quality)
{
 double Qs;

 if (Quality <= 0)  Quality = 1;
 if (Quality > 100) Quality = 100;
 if (Quality < 50)   Qs = 50.0 / (double) Quality;
	else	     Qs = 2.0 - (double) Quality/50.0;


 // Calcul des coefficients de la table de quantification
 for (int u=0; u<8; u++)
   for (int v=0; v<8; v++)
	{
	 LuminanceJPEGTable[u][v] = LuminanceJPEGTable[u][v] * Qs;
	 if (LuminanceJPEGTable[u][v] < 1.0) LuminanceJPEGTable[u][v]=1.0;
	 if (LuminanceJPEGTable[u][v] > 255.0) LuminanceJPEGTable[u][v]=255.0;
	}

 return;
}


void JPEGdecoding(BMPImageStruct *InputImage , BMPImageStruct *OutputImage)
{
   double Block[8][8];
   double tmp0, tmp1, tmp2, tmp3, tmp4, tmp5, tmp6, tmp7;
   double tmp10, tmp11, tmp12, tmp13, tmp20, tmp23;
   double tmp;

   // Decodage bloc par bloc
   for (int i=0; i<InputImage->imageVsize; i=i+8)
	 for (int j=0; j<InputImage->imageHsize; j=j+8)
	{
	 for (int u=0; u<8; u++)
	   for (int v=0; v<8; v++) Block[u][v]=InputImage->data[i+u][j+v];

	// Quantification inverse
	for (int u=0; u<8; u++)
	   for (int v=0; v<8; v++)
	   Block[u][v] = Block[u][v] * LuminanceJPEGTable[u][v];

	// DCT inverse
	Block[0][0]+=1024.0;

	for (int u=0; u<8; u++)
	  {
	   // 1ère étape: Partie paire
	   tmp0=Block[u][0]+Block[u][4];
	   tmp1=Block[u][0]-Block[u][4];
	   tmp=(Block[u][2]+Block[u][6])*a23;
	   tmp2=tmp-(a2*Block[u][6]);
	   tmp3=tmp+(a3*Block[u][2]);

	   tmp10=tmp0+tmp3;
	   tmp13=tmp0-tmp3;
	   tmp11=tmp1+tmp2;
	   tmp12=tmp1-tmp2;

	   // 1ème étape: Partie impaire
	   tmp0=Block[u][1]-Block[u][7];
	   tmp3=Block[u][1]+Block[u][7];
	   tmp1=Block[u][3]*rc2;
	   tmp2=Block[u][5]*rc2;
	   tmp4=tmp0+tmp2;
	   tmp6=tmp0-tmp2;
	   tmp7=tmp3+tmp1;
	   tmp5=tmp3-tmp1;

	   // 2ème étape
	   tmp=(tmp4+tmp7)*a47;
	   tmp20=tmp-(a4*tmp7);
	   tmp23=tmp+(a7*tmp4);
	   Block[u][0]=tmp10+tmp23;
	   Block[u][7]=tmp10-tmp23;
	   Block[u][3]=tmp13+tmp20;
	   Block[u][4]=tmp13-tmp20;

	   tmp=(tmp5+tmp6)*a56;
	   tmp20=tmp-(a5*tmp6);
	   tmp23=tmp+(a6*tmp5);
	   Block[u][2]=tmp12+tmp20;
	   Block[u][5]=tmp12-tmp20;
	   Block[u][1]=tmp11+tmp23;
	   Block[u][6]=tmp11-tmp23;
	  }

	for (int v=0; v<8; v++)
	  {
	   // 1ère étape: Partie paire
	   tmp0=Block[0][v]+Block[4][v];
	   tmp1=Block[0][v]-Block[4][v];
	   tmp=(Block[2][v]+Block[6][v])*a23;
	   tmp2=tmp-(a2*Block[6][v]);
	   tmp3=tmp+(a3*Block[2][v]);

	   tmp10=tmp0+tmp3;
	   tmp13=tmp0-tmp3;
	   tmp11=tmp1+tmp2;
	   tmp12=tmp1-tmp2;

	   // 1ème étape: Partie impaire
	   tmp0=Block[1][v]-Block[7][v];
	   tmp3=Block[1][v]+Block[7][v];
	   tmp1=Block[3][v]*rc2;
	   tmp2=Block[5][v]*rc2;
	   tmp4=tmp0+tmp2;
	   tmp6=tmp0-tmp2;
	   tmp7=tmp3+tmp1;
	   tmp5=tmp3-tmp1;

	   // 2ème étape
	   tmp=(tmp4+tmp7)*a47;
	   tmp20=tmp-(a4*tmp7);
	   tmp23=tmp+(a7*tmp4);
	   Block[0][v]=tmp10+tmp23;
	   Block[7][v]=tmp10-tmp23;
	   Block[3][v]=tmp13+tmp20;
	   Block[4][v]=tmp13-tmp20;

	   tmp=(tmp5+tmp6)*a56;
	   tmp20=tmp-(a5*tmp6);
	   tmp23=tmp+(a6*tmp5);
	   Block[2][v]=tmp12+tmp20;
	   Block[5][v]=tmp12-tmp20;
	   Block[1][v]=tmp11+tmp23;
	   Block[6][v]=tmp11-tmp23;
	  }

	// On vérifie que cela ne déborde pas
	for (int u=0; u<8; u++)
	   for (int v=0; v<8; v++)
	{
	   Block[u][v]/=8.0;
	   if (Block[u][v] < 0.0) Block[u][v]=0.0;
	   if (Block[u][v] > 255.0) Block[u][v]=255.0;
	}

	// On range le résultat dans l'image de sortie
	 for (int u=0; u<8; u++)
	   for (int v=0; v<8; v++)
		OutputImage->data[i+u][j+v]=Block[u][v];
   }

   return;
}


bool JPEGdepacketization(BMPImageStruct *OutputImage, FILE* TRACEFILE)
{
   int Block[8][8];
   unsigned int BlockOffset, row, col, row_mix, col_mix, packetsize, index, q, r, K;
   opj_mqc_t mqobjet;
   opj_mqc_t *objet=NULL;
   unsigned char buffer[MQC_NUMCTXS];

	// On lit la taille du prochain packet
	if (fscanf(TRACEFILE, "%4X", &packetsize) ==EOF) return false;

	fscanf(TRACEFILE, "%2X %2X", &row, &col);
	BlockOffset = row * 256 + col;
	packetsize -= 2;

	for (int x=0; x<packetsize; x++) fscanf(TRACEFILE, "%2X", &buffer[x]);
	//packetcount++;
	objet=&mqobjet;
	mqc_init_dec(objet, buffer, packetsize);
	mqc_resetstates(objet);

   while (mqc_numbytes(objet) < packetsize) {
	// On décode
	q=0;
	while(mqc_decode(objet)==1) q++;
	r=mqc_decode(objet);
	K=q*2+r;
	for (int x=0; x<K; x++)
	{
	q=0;
	while(mqc_decode(objet)==1) q++;
	r=mqc_decode(objet);
	index=q*2+r;
	if ((index % 2) == 0)
		{  Block[ZigzagCoordinates[x].row][ZigzagCoordinates[x].col]=index / 2;	}
	   else {  Block[ZigzagCoordinates[x].row][ZigzagCoordinates[x].col]=0-((index+1) / 2);	}
	}
	for (int x=K; x<64; x++) Block[ZigzagCoordinates[x].row][ZigzagCoordinates[x].col]=0;
	// On copie le bloc décodé à sa place dans l'image
	row = (BlockOffset * 8) / OutputImage->imageHsize * 8;
	col = (BlockOffset * 8) % OutputImage->imageHsize;
	row_mix = ((row * 5) + (col *  8)) % (OutputImage->imageHsize);
	col_mix = ((row * 8) + (col * 13)) % (OutputImage->imageVsize);
	for (int u=0; u<8; u++)
	   for (int v=0; v<8; v++) OutputImage->data[row_mix+u][col_mix+v]=(double) Block[u][v];
	BlockOffset++;
   }

   if (BlockOffset != (OutputImage->imageHsize * OutputImage->imageVsize / 64)) {
	q=0;
	while((mqc_decode(objet)==1) && (q < 32)) q++;
	r=mqc_decode(objet);
	K=q*2+r;
	if (K > 64) return true;
	for (int x=0; x<K; x++)
	{
	q=0;
	while((mqc_decode(objet)==1) && (q < 32)) q++;
	r=mqc_decode(objet);
	index=q*2+r;
	if ((index % 2) == 0)
		{  Block[ZigzagCoordinates[x].row][ZigzagCoordinates[x].col]=index / 2;	}
	   else {  Block[ZigzagCoordinates[x].row][ZigzagCoordinates[x].col]=0-((index+1) / 2);	}
	}
	for (int x=K; x<64; x++) Block[ZigzagCoordinates[x].row][ZigzagCoordinates[x].col]=0;
	// On copie le bloc décodé à sa place dans l'image
	row = (BlockOffset * 8) / OutputImage->imageHsize * 8;
	col = (BlockOffset * 8) % OutputImage->imageHsize;
	row_mix = ((row * 5) + (col *  8)) % (OutputImage->imageHsize);
	col_mix = ((row * 8) + (col * 13)) % (OutputImage->imageVsize);
	for (int u=0; u<8; u++)
	   for (int v=0; v<8; v++) OutputImage->data[row_mix+u][col_mix+v]=(double) Block[u][v];
	BlockOffset++;
   }

   return true;
}

// end from CRAN

#ifdef WITH_IMAGE_DISPLAY

void displayImage(char* image, char* captionString, bool waitKeyPress=false)
{
	SDL_Surface *hello = NULL;
	SDL_Surface *screen = NULL;
	SDL_Event event; /* Cette variable servira plus tard à gérer les évènements */

	bool nokeypress=true;

	// Start SDL
	SDL_Init( SDL_INIT_VIDEO );

	// Load image
        hello = IMG_Load (image);

	if (hello) { 
		// Set up screen
		// screen = SDL_SetVideoMode( hello->w, hello->h, 32, SDL_HWSURFACE );
		screen = SDL_SetVideoMode( 320, 320, 32, SDL_HWSURFACE );
		SDL_WM_SetCaption(captionString, NULL);
	 
		// Apply image to screen
		SDL_BlitSurface( hello, NULL, screen, NULL);

		// Update Screen
		SDL_Flip( screen );
	
		if (waitKeyPress) {
			while (nokeypress) { /* TANT QUE la variable ne vaut pas 0 */
				SDL_WaitEvent(&event); /* On attend un évènement qu'on récupère dans event */
				switch(event.type) /* On teste le type d'évènement */
				{
					case SDL_QUIT:
						nokeypress = false;
						break;
					case SDL_KEYDOWN:
						nokeypress= false;
						break;
				}
			}
		}
		else {
			// Pause 1.5s
			SDL_Delay(1500);
		}
		  
		// Free the loaded image
		SDL_FreeSurface( hello );
	}
	else
                printf("Cannot load image, probably because the file is corrupted.\n");

	// Quit SDL
	SDL_Quit();
}

#endif

void startDisplayImage(FILE* pFile, char* originalFile) {

    BMPImageStruct OriginalImage;
    char tmpBMPFile[80];
    int err;

    // Read the original BMP file to get useful informations
    err = ReadBitmapFile(originalFile, &OriginalImage);

    if (err) {
            fprintf(stderr, "CANNOT read original BMP file %s\n", originalFile);

    }
    else {
            // reset content
            for (int i=0; i<OriginalImage.imageVsize; i++)
                    for (int j=0; j<OriginalImage.imageHsize; j++) OriginalImage.data[i][j]=0.0;

            // JPEG decoding
            while (JPEGdepacketization(&OriginalImage, pFile) == true);

            JPEGdecoding(&OriginalImage, &OriginalImage);

            sprintf(tmpBMPFile,"tmp_%d-%s.Q%d.bmp", fileSN, originalFile, qualityFactor);

            err = WriteBitmapFile(tmpBMPFile, &OriginalImage);

            if (err) {
                    fprintf(stderr, "CANNOT write tmp BMP file from received encoded image.\n");
            }
            else {
#ifdef WITH_IMAGE_DISPLAY
                    displayImage(tmpBMPFile, "test", true);
                    //remove(tmpBMPFile);
#else
		    fprintf(stderr, "No support of image display (need SDL library). Compile with -DWITH_IMAGE_DISPLAY\n");
#endif
            }

    }
}

// basic sniffer actually, maybe a problem with fast arriving pkt or when buffer is overwritten
int getRawData() {

    uint8_t c;
    int i=0, it;
    int readChar=0;
    bool got_frame=true;
    bool stop_reading=false;
    bool skip_frame=false;

    myData=NULL;
    myRawData=NULL;

    fprintf(stderr, "\nPolling port\n");

    timespec now1, now2;

    clock_gettime(CLOCK_REALTIME, &now1);
    clock_gettime(CLOCK_REALTIME, &now2);

    // waiting for a character, non blocking read, but only for 500ms maximum
    while ((readChar<=0 || c!=(uint8_t)START_BYTE) && diff(now1, now2).tv_nsec < 500000000.0 ) {
        readChar=read(sd,&c,1);
        clock_gettime(CLOCK_REALTIME, &now2);
    }

    // no frame and we have timeout
    if (readChar==0)
        return 0;

    fprintf(stderr, "c=%d, Polling new pkt\n", readChar);

    clock_gettime(CLOCK_REALTIME, &now1);
    clock_gettime(CLOCK_REALTIME, &now2);

    // read while we have something or time remains (30ms in total)
    // depends on the baud rate: 100-byte packet gives 100*10 bits= 1000bits, at 115200, needs at least 8m to read
    while( (readChar>0 || diff(now1, now2).tv_nsec < 09000000.0) && i<MAX_API_FRAME_LENGTH) {

        if (readChar>0) {

            if (c==(uint8_t)ESCAPE) {
                read(sd,&c,1);
                c = 0x20 ^ c;
            }
/*
	    // truncated frame? we will fill missed bytes with 0 and pass the data
            if (c==(uint8_t)START_BYTE) {
                read(sd,&c,1);
                c = 0x20 ^ c;
            }
*/
            frame[i]=c;
            i++;
        }
        // works with 38400 and 100000 bps
	//usleep(5);
	// for 125000 bps or 115200
        usleep(1);
        readChar=read(sd,&c,1);
        clock_gettime(CLOCK_REALTIME, &now2);
    }

    if (!got_frame) {
        nbPktTimeout++;
        fprintf(stderr, "Timeout!\n");
    }
    else {

        fprintf(stderr, "Got. ");
        nbPktRcv++;

        if (!stream_mode) {
            uint16_t checksum=0;

            for (it=0; it<i; it++) {
                if (pktDisplay) fprintf(stderr, "%02X ", frame[it]);

                // we include the received checksum
                if (it>2)
                    checksum+=frame[it];
            }

            if (checksum>255)
                checksum%=256;

            fprintf(stderr, "checksum is %02X\n", checksum);

            if (checksum!=0xFF) {
                nbPktError++;
                // could we accept the frame?
                if (frame[2]!=0 && frame[2]<MAX_APP_FRAME_LENGTH && i==frame[2]+3) {
                    fprintf(stderr, "ACC\n");
                }
                else {
                    nbPktDiscarded++;
                    fprintf(stderr, "DIS\n");
                    return 0;
                }
            }
            else {
                nbPktSuccess++;
                fprintf(stderr, "OK\n");
            }
        }

        nbPktAccepted++;

        myRawData=frame;
        myRawData_len=i;

        int appDataStart;
        int dataStart=XBEE_DATA;

        if (frame[XBEE_API_IDENTIFIER]==0x80)
            // by default: a pure XBee (Arduino) without any additional header
            dataStart=XBEE_DATA;

        if (frame[XBEE_API_IDENTIFIER]==0x81)
            // 16-bit receive
            dataStart=XBEE_DATA-6;

        if (frame[XBEE_API_IDENTIFIER]==0x90)
            // by default: a pure XBee (Arduino) without any additional header
            dataStart=XBEE_DM_DATA;

        appDataStart=dataStart;

        // test whether the packet comes from a WaspMote with the Libelium API
        if (frame[dataStart+2]=='#' && frame[dataStart+4]=='W' && frame[dataStart+5]=='A' && frame[dataStart+6]=='S' && frame[dataStart+7]=='P') {
                // first fragment
                appDataStart+=LIBELIUM_APP_HEADER_SIZE;
        }

        // test whether the packet comes from a WaspMote with the Libelium API, long packet fragmented in many small packets
        if (frame[dataStart+2]!='#' && frame[dataStart+3]=='W' && frame[dataStart+4]=='A' && frame[dataStart+5]=='S' && frame[dataStart+6]=='P') {
                // not a first fragment
                appDataStart+=LIBELIUM_APP_HEADER_SIZE-1;
        }

        // probably from an iMote2 no TOS version, we assume that PANID=0x1234 which is put as payload by the iMote2 communication library
        if (frame[dataStart]==0x34 && frame[dataStart+1]==0x12) {
                appDataStart+=IMOTE2_HEADER_SIZE;
        }

/*
        // probably from an TOS AM mote
        if (frame[dataStart]==0x3F && frame[dataStart+1]==0x01) {
                appDataStart+=TOS_HEADER_SIZE;
        }
*/

	if (framing_mode) {
		// first framing byte is always 0xFF
		if (frame[appDataStart]!=0xFF)
			return 0;
		else
			if (image_mode) {
				// second byte is 0x50-0x54 for images
				if (frame[appDataStart+1]<0x50 || frame[appDataStart+1]>0x54)
					return 0;
			} else {
				// second byte is 0x55-0x59 for binary (audio files are considered binary)
				if (frame[appDataStart+1]<0x55 || frame[appDataStart+1]>0x59)
					return 0;
			}
	}

        // set the length of the real data
        myData_len=i-1-appDataStart;

        // allocate a new buffer to copy the real data, plus one char for the null-terminated character
        myData = (uint8_t*) calloc(myData_len+1,sizeof(uint8_t));

        if (myData==NULL) {
                fprintf(stderr, "ERROR in getRawData:calloc:myData");
                myRawData=NULL;
                return 0;
        }

	// do not give framing bytes to app
	if (framing_mode) {
		// the framing bytes consists in 3 bytes: 0xFF0x55 + SN or 0xFF0x50 + SN
		appDataStart += 3;
		myData_len -= 3;	
	}

        for (int j=0; j<myData_len; j++)
                myData[j] = frame[appDataStart+j];

        // set the null-terminated character so one could directly print the real data if it is a string
        myData[myData_len]='\0';
    }

    return 1;
}

void receiveFilePkt(FILE* pFile, FILE* dFile) {

    int res;
    int serialByte;

    timespec now1, now2, now3, now4, now5, firstPkt, lastPkt, currentPkt;

    clock_gettime(CLOCK_REALTIME, &now1);
    clock_gettime(CLOCK_REALTIME, &now2);
    clock_gettime(CLOCK_REALTIME, &now3);

    // for binary mode, we store the file size and offset in the first 4 bytes of the pkt payload
    uint16_t newOffset=0, rcvOffset=0;

    // allow for 100s reading maximum total and 8s max between each packet
    while (diff(now1, now3).tv_sec < 100.0 && diff(now2, now3).tv_sec < 8.0) {

        // get the number of data waiting in serial buffer
        ioctl(sd, FIONREAD ,&serialByte);

        if (serialByte) {

            clock_gettime(CLOCK_REALTIME, &now4);

            res=getRawData();

            if (res) {

                clock_gettime(CLOCK_REALTIME, &currentPkt);
                // first accepted pkt?
                if (nbPktAccepted==1) {
                        firstPkt=currentPkt;
                        lastPkt=currentPkt;
                }

                if (image_mode) {

                    // TODO add a header "IP" for instance to identify image pkt in order to not get undesirable pkt

                    //printf("%s\n", (char*)myData);
                    fprintf(stderr, "pkt %d(%ld %.2fms -> %lds%.2fms), write %d bytes\n",
                                nbPktRcv,
                                currentPkt.tv_nsec,
                                diff(lastPkt, currentPkt).tv_nsec/1000000.0,
                                diff(firstPkt, currentPkt).tv_sec,
                                diff(firstPkt, currentPkt).tv_nsec/1000000.0,
                                myData_len);

                    lastPkt=currentPkt;

                    // write the image chunk size
                    fprintf(pFile, "%04X ", myData_len);
                    if (pktDisplay)
                        fprintf(stderr, "%.4X ", myData_len);

                    if (pktFile)
                        fprintf(dFile, "%04X (%d)", myData_len, myData_len);

                    for (int i=0; i<myData_len; i++) {

                        fprintf(pFile, "%02X ", myData[i]);

                        if (pktDisplay)
                            fprintf(stderr, "%.2X ", myData[i]);

                        if (pktFile)
                            fprintf(dFile, "%02X", myData[i]);

                        if ( (i==0 || i==1) && pktFile)
                            fprintf(dFile, " ");
                    }

                    fprintf(stderr, "\n");

                    if (pktFile)
                        fprintf(dFile, "\n");
                }
                else {

                    // TODO add a header "BP" for instance to identify binary pkt in order to not get undesirable pkt

                    if (stream_mode) {

                        for (int i=0; i<myData_len; i++) {

                            write(1, &myData[i], 1);

                        }

                        fflush(stdout);
                    }
                    else {
                        // get the total size of the file
                        // TODO maybe not suitable for real streaming since we may not know at the sender side the size of the data to be sent
                        if (myFileBuff_len==0)
                            myFileBuff_len = myData[0] * 256 + myData[1];

                        // first offset is 0
                        rcvOffset = myData[2] * 256 + myData[3];

                        if (pipe_mode && fill_mode) {
                            // did we miss some pkt? rcvOffset is usually strictly greater than newOffset otherwise we do nothing
                            if (rcvOffset > newOffset) {
                                // we have to fill with some value in pipe mode
                                // TODO what value is better for audio data, depends on the format?
                                uint8_t tmpv=binaryFilledValue;

                                for (int i=newOffset; i<rcvOffset; i++) {
                                    write(1, &tmpv, 1);
                                    fflush(stdout);
                                }
                            }
                        }

                        fprintf(stderr, "pkt %d, will write %d bytes in file buffer at offset%d\n", nbPktRcv,  myData_len-4, rcvOffset);

                        if (pktDisplay)
                            fprintf(stderr, "%.4X ", myData_len);

                        if (pktFile)
                            fprintf(dFile, "%04X (%d)", myData_len, myData_len);

                        for (int i=0; i<myData_len; i++) {

                            // store the packet content, excluding the first 4 bytes
                            if (i>3) {
                                // write the data to stdout for pipe mode
                                if (pipe_mode && rcvOffset >= newOffset)  {
                                    write(1, &myData[i], 1);
                                    fflush(stdout);
                                }

                                myFileBuff[rcvOffset+i-4]=myData[i];
                            }
                            if (pktDisplay)
                                fprintf(stderr, "%.2X ", myData[i]);

                            if (pktFile)
                                fprintf(dFile, "%02X", myData[i]);

                            if ( (i==0 || i==1) && pktFile)
                                fprintf(dFile, " ");
                        }

                        if (pipe_mode)
                            fflush(stdout);

                        // will expect this offset for next packet
                        if (rcvOffset >= newOffset)
                            newOffset=rcvOffset+myData_len-4;

                        fprintf(stderr, "\n");

                        if (pktFile)
                            fprintf(dFile, "\n");
                    }
                }
            }

            myRawData=NULL;
            if (myData)
                free(myData);
            myData=NULL;

            clock_gettime(CLOCK_REALTIME, &now2);
            clock_gettime(CLOCK_REALTIME, &now3);

            fprintf(stderr, "Processing took (%ld %ld %.2f ms)\n",
                    diff(now4,now3).tv_sec, diff(now4,now3).tv_nsec, diff(now4,now3).tv_nsec/1000000.0);
        }
        else {
            // if we accepted 0 pkt then reset for another 100s
            if (nbPktAccepted==0)
                clock_gettime(CLOCK_REALTIME, &now2);
            clock_gettime(CLOCK_REALTIME, &now3);
        }
    }
}

void * printERROR(char *argv[])
{
   fprintf(stderr, "USAGE:\t%s -baud baudrate -p dev -onlydisplay img_file.dat -pktd -pktf -B/-I -framing -ap0 -v val -fill -stdout -stream -Q 40 file_name\n", argv[0]);
   fprintf(stderr, "USAGE:\t-baud, set baud rate, default is 38400\n");
   fprintf(stderr, "USAGE:\t-p /dev/ttyUSB1\n");
   fprintf(stderr, "USAGE:\t-onlydisplay img_file.dat, display the .dat file only\n");
   fprintf(stderr, "USAGE:\t-pktd, display received XBee frames\n");
   fprintf(stderr, "USAGE:\t-pktf, generate pkt list file\n");
   fprintf(stderr, "USAGE:\t-B/-I, -B for binary mode, -I for image mode, default is image mode\n");
   fprintf(stderr, "USAGE:\t-framing, expects 0xFF0x55 for binary mode, 0xFF0x50 for image mode, default is no framing\n");
   fprintf(stderr, "USAGE:\t-ap0, indicates that the XBee is in AP mode 0 (transparent mode) so do not decode frame structure\n");
   fprintf(stderr, "USAGE:\t\tcan actually be used to read raw data from serial port, possibly from another 802.15.4 gateway\n");
   fprintf(stderr, "USAGE:\t-v 77, use 0x77 to fill in missing bytes in lost packet, work in binary mode\n");
   fprintf(stderr, "USAGE:\t-fill, will fill missing bytes\n");
   fprintf(stderr, "USAGE:\t-stdout, write to stdout for pipe mode, don't work with image mode\n");
   fprintf(stderr, "USAGE:\t-stream, will write all received pkt content to stdout without any offset information\n");
   fprintf(stderr, "USAGE:\t\tcan be similar to -ap0 without an XBee in AP0 mode\n");
   fprintf(stderr, "USAGE:\t-Q 40, use 40 as Quality Factor, default is 50\n");
   fprintf(stderr, "USAGE:\t file_name, for images: give the original bmp file. for binary: give a any file name\n");

   timespec start, stop, res;

   clock_getres(CLOCK_REALTIME, &res);

   int res1=clock_gettime(CLOCK_REALTIME, &start);
   usleep(15000);
   int res2=clock_gettime(CLOCK_REALTIME, &stop);

   fprintf(stderr, "debug info: %d %d %ld %ld %ld %ld %ld %ld %ld %.2f\n",
          res1, res2, res.tv_sec, res.tv_nsec, start.tv_sec, stop.tv_sec, start.tv_nsec, stop.tv_nsec,
          diff(start, stop).tv_sec, diff(start, stop).tv_nsec/1000000.0);

   return 0;
}

int main(int argc, char *argv[]) {

    FILE* pFile; // the output file
    FILE* dFile; // use to keep the received pkt in ASCII hex format for debug purpose mainly

    char rcvFile[80];
    char* originalFile;
    char* displayFile;
    bool onlyDisplay=false;

    int i=0;

    if (argc < 2)
    {
       printERROR(argv);
       exit (-1);
    }

    for (int arg = 1; arg < argc-1; arg++) {

        if (!strcmp(argv[arg], "-baud"))
        {
           customSpeed=atoi(argv[arg+1]);
           fprintf(stderr, "custom baud rate parameter is: %d\n", customSpeed);
	   if (customSpeed==115200) {
		speed=B115200;
		customSpeed=0;
	   }
        }

        if (!strcmp(argv[arg], "-p"))
        {
           serialPort=argv[arg+1];
           fprintf(stderr, "will use serial port: %s\n", serialPort);
        }

        if (!strcmp(argv[arg], "-onlydisplay"))
        {
            onlyDisplay=true;
            displayFile=argv[arg+1];
        }

        if (!strcmp(argv[arg], "-pktf"))
        {
           pktFile=true;
        }

        if (!strcmp(argv[arg], "-ap0"))
        {
           image_mode=false;
           binary_mode=true;
           xbeeAP0=true;
        }

        if (!strcmp(argv[arg], "-Q")) {
            qualityFactor=atoi(argv[arg+1]);
        }

        if (!strcmp(argv[arg], "-pktd"))
        {
           pktDisplay=true;
        }

        if (!strcmp(argv[arg], "-I"))
        {
           image_mode=true;
           binary_mode=false;
           fprintf(stderr, "set to image mode\n");
        }

        if (!strcmp(argv[arg], "-B"))
        {
           image_mode=false;
           binary_mode=true;
           fprintf(stderr, "set to binary mode\n");
        }

        if (!strcmp(argv[arg], "-fill"))
        {
           fill_mode=true;
           fprintf(stderr, "will fill miss packet with %02X\n", binaryFilledValue);
        }

        if (!strcmp(argv[arg], "-v"))
        {
           binaryFilledValue = str2hex(argv[arg+1]);
           fprintf(stderr, "will fill with value %02X\n", binaryFilledValue);
        }

        if (!strcmp(argv[arg], "-stdout"))
        {
           pipe_mode=true;
           fprintf(stderr, "set output to stdout\n");
        }

        if (!strcmp(argv[arg], "-stream"))
        {
           stream_mode=true;
           pipe_mode=true;
           fprintf(stderr, "set to stream mode\n");
        }

        if (!strcmp(argv[arg], "-framing"))
        {
           framing_mode=true;
           fprintf(stderr, "set to framing mode\n");
        }

    }

    if (pipe_mode && image_mode) {
           fprintf(stderr, "cannot output to stdout in image mode\n");
           exit(-1);
    }

    if (onlyDisplay && image_mode) {
        // the BMP file should always be the last argument
        originalFile=argv[argc-1];

        pFile = fopen(displayFile, "r");

        unsigned int tmp;

        // skip the field that are not usefull for display, just keep the quality Factor
        fscanf(pFile, "%04X", &tmp);
        fscanf(pFile, "%04X", &tmp);
        fscanf(pFile, "%04X", &tmp);
        fscanf(pFile, "%04X", &tmp);

        qualityFactor=tmp;

        // Initialize the quantization matrix
        QTinitialization(qualityFactor);

        printf("Display image %s, original BMP file is %s, original QualityFactor is %d\n", displayFile, originalFile, qualityFactor);

        startDisplayImage(pFile, originalFile);

        fclose(pFile);
    }
    else {

        init_serial();
        tcflush(sd, TCIOFLUSH);

        if (pipe_mode && xbeeAP0) {

            unsigned char c ;
            uint8_t sample ;
            int8_t sample2;
            int readChar=0;

            while (1) {
               readChar=read(sd, &c, 1) ;
               if (readChar>0) {
                   //sample = c ; //msb << 8 | lsb ;
                   //sample2 = sample ; //- 0x54 ;
                   write ( 1, &c, 1) ;
                   //write (1, &zero, 1) ;
                   fflush(stdout);
               }
            }
        }
        else {

	    frame = (uint8_t*)calloc(MAX_API_FRAME_LENGTH, sizeof(uint8_t));

    	    if (frame==NULL) {
            	fprintf(stderr,"ERROR in calloc:frame");
            	return 0;
    	    }

            // the file should always be the last argument
            originalFile=argv[argc-1];

            if (image_mode) {
                // Initialize the quantization matrix
                QTinitialization(qualityFactor);

                fprintf(stderr, "Wait for image, original BMP file is %s, QualityFactor is %d\n", originalFile, qualityFactor);
            }
            else
                fprintf(stderr, "Wait for binary file, use suffix file name %s\n", originalFile);

            while (1) {

                if (image_mode) {

                    if (pktFile) {
                        fprintf(stderr, "Creating file imageRcv.pktlist for storing the list of received packets\n");
                        dFile = fopen("imageRcv.pktlist", "w");
                    }
                    else dFile=NULL;

                    // will write received image pkt in this file
                    sprintf(rcvFile,"tmp_%d-%s.Q%d.dat", fileSN, originalFile, qualityFactor);
                    fprintf(stderr, "Creating file %s for storing the received image data file\n", rcvFile);

                    pFile = fopen(rcvFile, "w");

                    fprintf(stderr, "Wait for image for 100s maximum\n");

                    receiveFilePkt(pFile, dFile);

                    fclose(pFile);

                    if (pktFile)
                        fclose(dFile);

                    if (nbPktRcv) {
                        fprintf(stderr, "rcv pkt: %d accepted:%d success: %d timeout: %d error: %d discarded: %d\n",
                               nbPktRcv, nbPktAccepted, nbPktSuccess, nbPktTimeout, nbPktError, nbPktDiscarded);
                        fprintf(stderr, "Opening file %s for display\n", rcvFile);

                        // will read received image from this file
                        pFile = fopen(rcvFile, "r");

                        startDisplayImage(pFile, originalFile);

                        fclose(pFile);

                        //printf("Deleting file %s\n", rcvFile);
                        //remove(rcvFile);

                        // we collect stats per image
                        nbPktRcv=0;
                        nbPktAccepted=0;
                        nbPktSuccess=0;
                        nbPktTimeout=0;
                        nbPktError=0;
                        nbPktDiscarded=0;

                        // for naming temp file
                        fileSN++;
                    }
                    else
                        fprintf(stderr, "No pkt received, retry\n\n");
                }
                else {

                    if (pktFile) {
                        fprintf(stderr, "Creating file binRcv.pktlist for storing the list of received packets\n");
                        dFile = fopen("binRcv.pktlist", "w");
                    }
                    else dFile=NULL;

                    // will write received binary pkt in this file
                    sprintf(rcvFile,"tmp_%d-%s", fileSN, originalFile);
                    fprintf(stderr, "Creating file %s for storing the received binary data\n", rcvFile);

                    pFile = fopen(rcvFile, "wb");

                    fprintf(stderr, "Wait for binary file for 100s maximum\n");

                    myFileBuff = (uint8_t*)calloc(MAX_BINARY_FILE_SIZE, sizeof(uint8_t));

                    if (myFileBuff!=NULL) {

                        // set to 0x01 for codec2 at 3200bps or 0x55 for 1400bps and 0x77 for 2400bps
                        memset((void*)myFileBuff, binaryFilledValue, MAX_BINARY_FILE_SIZE);

                        receiveFilePkt(pFile, dFile);

                        if (pktFile)
                            fclose(dFile);

                        if (nbPktRcv) {
                            fprintf(stderr, "rcv pkt: %d accepted:%d success: %d timeout: %d error: %d discarded: %d\n",
                                   nbPktRcv, nbPktAccepted, nbPktSuccess, nbPktTimeout, nbPktError, nbPktDiscarded);

                            // we collect stats per file
                            nbPktRcv=0;
                            nbPktAccepted=0;
                            nbPktSuccess=0;
                            nbPktTimeout=0;
                            nbPktError=0;
                            nbPktDiscarded=0;

                            // write in the file
                            fwrite(myFileBuff, 1, myFileBuff_len, pFile);

                            // for naming temp file
                            fileSN++;
                        }
                        else
                            fprintf(stderr, "No pkt received, retry\n\n");

                        fclose(pFile);
                        // reset the variables
                        myFileBuff_len=0;
                        free(myFileBuff);
                    }
                    else
                        fprintf(stderr,"calloc error\n\n");

                }
            }
        }
    }
}