// Send an image encoded with CRAN encoding scheme to a remote XBee
// C. Pham

/*
 * XBeeSendCRANImage -mac 0013A20040922070 img.dat
 * XBeeSendCRANImage -mac -net 1234 img.dat
 * XBeeSendCRANImage -b 0013A20040922070 img.dat
 */

// IMPORTANT: need an XBee module in API mode 2, ATAP2

#include <iostream>

#include <stdio.h>
#include <string.h> 
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <stdlib.h>
#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>

using namespace std;

#define MAX_API_FRAME_LENGTH 130
#define MAX_AT_CMD_LENGTH 10

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

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

bool send_mac=false;
bool send_net=false;
bool send_broadcast=false;
bool send_DM=false;
bool send_libelium=false;
bool sendToSensor=false;
bool send_wflowid=false;
int flowid=0;
bool src_addr_mode=false;

char* src_addr;

int nbPktSent=0;
int nbPktSuccess=0;
int nbPktTimeout=0;
int nbPktError=0;
int nbDroppedPkt=0;

int qualityFactor=50;

// 50ms by default
unsigned int timeBetweenPkt=50000;
// 25ms by default, wait after having written to radio
unsigned int timeAfterRadioWrite=25000;
// 20us time between each write to radio
unsigned int timeBetweenSerialByte=20;
// indicate whether we wait for a transmit status response or not
bool waitForRadioAnswer=true;
// 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;
// fake sending, just write into a file, same format than input file, useful with drop mode
bool fakeSend=false;
// indicate drop mode, usefull with fake send mode to write a partially truncated file
bool dropPkt=false;
// the pkt drop percentage
unsigned int dropPercentage=0;
// use framing bytes
bool framing_mode=false;

struct termios opciones;

int speed = B38400;
int customSpeed=0;

timespec startSendGeneric, stopSendGeneric;
timespec startSend, stopSend, startSendRadio, stopSendRadio, startTxStatusResponse, stopTxStatusResponse, startSendImg, stopSendImg;

// see http://www.guyrutenberg.com/2007/09/22/profiling-code-using-clock_gettime/
// also http://aufather.wordpress.com/2010/09/08/high-performance-time-measuremen-in-linux/
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, 125000 or 250000, then the custon divisor is 480, 240, 192 and 96 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);
       }
   }
}

int send_pkt(char* dest_address, uint8_t* pkt, uint8_t chunkSize, uint8_t SN=0)
{
   clock_gettime(CLOCK_REALTIME, &startSend);

   char aux[2];
   uint8_t destination[8];
   uint8_t source[2];

   uint8_t checksum = 0;
   uint8_t it, pos;
   uint8_t command[MAX_API_FRAME_LENGTH];
   uint8_t response[MAX_API_FRAME_LENGTH];
   int err=0;

   // flush both input and output to be sure
   tcflush(sd, TCIOFLUSH);

   fprintf(stderr, "\nBuilding API frame... \n");

   uint8_t i=0;
   uint8_t j=0;

   if (send_net) {
       // transform the 4-char string format net address in 16-bit array
       while(j<2)
       {
          aux[i-j*2]=dest_address[i];
          aux[(i-j*2)+1]=dest_address[i+1];
          destination[j]=str2hex(aux);
          i+=2;
          j++;
       }
   }
   else {
        // transform the 16-char string format address in 64-bit array
        while(j<8)
        {
           aux[i-j*2]=dest_address[i];
           aux[(i-j*2)+1]=dest_address[i+1];
           destination[j]=str2hex(aux);
           i+=2;
           j++;
        }
   }

   command[0] = START_BYTE;
   command[1] = 0x00;

   if (send_net) {
       command[2] = chunkSize+5;
       command[3] = 0x01; // 16-bit address transmit request
   }
   else {
       if (send_DM) {
           command[2] = chunkSize+14;
           command[3] = 0x10; // 64-bit address transmit request for DigiMesh protocol
       }
       else {
            command[2] = chunkSize+11;
            command[3] = 0x00; // 64-bit address transmit request
       }
   }

   // frame id
   command[4] = 0x01;

   if (command[3] == 0x00 || command[3] == 0x10) {
       // will maybe ignored, but still, set the 64-bit address to the broadcast address for frame content clarity
       command[5] = 0x00;
       command[6] = 0x00;
       command[7] = 0x00;
       command[8] = 0x00;
       command[9] = 0x00;
       command[10] = 0x00;
       command[11] = 0xFF;
       command[12] = 0xFF;

       if (send_mac)
           // overwrite to set the 64-bit address
           for (it=0; it<8; it++) command[it+5]=destination[it];

       if (send_DM) {
            // set the 16-bit address field to this special value
            command[13]=0xFF;
            command[14]=0xFE;
            pos=15;
       }
       else
            pos=13;
   }
   else
       pos=5;

   if (send_DM) {
       // set broadcast radius to 0
       command[pos++]=0x00;
   }

   if (send_net && !send_DM) {
       // set 16-bit network address
       command[pos++]=destination[0];
       command[pos++]=destination[1];
   }

   // option byte. valid for both transmit request and remote AT request
   command[pos++]=0x00;

   // Libelium app API header
   if (send_libelium) {
       // add the Libelium app API header length
       command[2] = command[2]+9;
       command[pos++] = 0x52; // default application ID
       command[pos++] = SN; // fragment number
       command[pos++] = 0x23; // the #
       command[pos++] = 2; // use node id identification NI_TYPE
       char ni[] = "WASP#"; // the source id string
       for (it=0; it<5; it++)
           command[pos++]=ni[it];
   }

   if (framing_mode) {
        // add the 5 framing bytes: 0xFF0x50 + SN + Q + chunksize
	command[pos++]=0xFF;
	// 0x50 for image mode
	command[pos++]=0x50+flowid;

        if (src_addr_mode) {
            command[2] = command[2]+7;

            i=0;
            j=0;

            // transform the 4-char string format net address in 16-bit array
            while(j<2) {
               aux[i-j*2]=src_addr[i];
               aux[(i-j*2)+1]=src_addr[i+1];
               source[j]=str2hex(aux);
               i+=2;
               j++;
            }
            // high part
            command[pos++]=source[0];
            // low part
            command[pos++]=source[1];
        }
        else {
            command[2] = command[2]+5;
        }
	// the sequence number 
        command[pos++]=SN;
        // the qualityFactor
	command[pos++]=qualityFactor;
	// the chunksize 
	command[pos++]=chunkSize;
   }

   // set the payload
   for (it=0; it<chunkSize; it++)
       command[pos++]=pkt[it];

   // pos is now the total length of the frame
   for(it=3; it<pos; it++)
        checksum=checksum+command[it];

   while( (checksum>255))
        checksum=checksum-256;

   checksum=255-checksum;
   command[pos]=checksum;

   fprintf(stderr, "Built API frame... \n");

   if (pktDisplay)
       for (it=0; it<pos+1; it++) {
            fprintf(stderr, "%.2X ", command[it]);
       }

   fprintf(stderr, "\n");
   fprintf(stderr, "Will write to radio API frame... \n");

   uint8_t tmp;

   if (pktDisplay) {
       fprintf(stderr, "%.2X ", command[0]);

       for (it=1; it<pos; it++) {
            tmp=command[it];

            // handle characters that need to be escaped
            if (tmp == START_BYTE || tmp == ESCAPE || tmp == XON || tmp == XOFF) {
                    fprintf(stderr, "%.2x ", ESCAPE);
                    tmp = tmp ^ 0x20;
            }
            fprintf(stderr, "%.2X ", tmp);
       }
       // the checksum
       tmp=command[pos];
       fprintf(stderr, "%.2X ", tmp);
   }

   fprintf(stderr, "\n");

   // now, we really send it to the radio
   // write the start delimiter

   clock_gettime(CLOCK_REALTIME, &startSendRadio);

   tmp = command[0];
   write(sd, &tmp, 1);

   for (it=1; it<pos; it++) {
        tmp = command[it];

        // handle characters that need to be escaped
        if (tmp == START_BYTE || tmp == ESCAPE || tmp == XON || tmp == XOFF) {
                uint8_t escapeChar=ESCAPE;
                write(sd, &escapeChar, 1);
                usleep(timeBetweenSerialByte);
                tmp = tmp ^ 0x20;
        }
        write(sd, &tmp, 1);
        // we may need to sleep a bit to slow down on a computer
        usleep(timeBetweenSerialByte);
   }

   // write the checksum
   tmp = command[pos];
   write(sd, &tmp, 1);

   clock_gettime(CLOCK_REALTIME, &stopSendRadio);

   // sleeping here is important, the radio could have some strange behavior otherwise
   //sleep(3);
   // you should get no timeout with unicast traffic
   usleep(timeAfterRadioWrite);

   if (waitForRadioAnswer) {

       fprintf(stderr, "Awaiting response. ");
       // read response
       // get message that comes from remote XBee module

       char c;
       i=0;
       int readChar=1;
       bool got_frame=false;
       bool stop_reading=false;
       bool skip_frame=false;

       clock_gettime(CLOCK_REALTIME, &startTxStatusResponse);

       fprintf(stderr, "Polling for answer 30ms max.");

       timespec now1, now2;

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

       //30ms=30000000ns
       // we can increase timeAfterRadioWrite if necessary, but it is a hard waiting while here it is an uppoer bound
       while( !stop_reading && diff(now1, now2).tv_nsec < 30000000.0 ) {

            // non blocking read
            readChar=read(sd,&c,1);

            if (readChar>0) {

                //fprintf(stderr, "%.2x ", (uint8_t)c);

                if ((c==(char)START_BYTE && !got_frame) || skip_frame ) {

                    // if we have skipped a frame, we already stored the START_BYTE
                    if (!skip_frame)
                        response[i++]=(uint8_t)c;

                    // length MSB
                    read(sd,&c,1);
                    response[i++]=(uint8_t)c;
                    //fprintf(stderr, "%.2x ", (uint8_t)c);

                    // length LSB
                    read(sd,&c,1);
                    response[i++]=(uint8_t)c;
                    //fprintf(stderr, "%.2x ", (uint8_t)c);

                    // the XBee API code at position 3
                    read(sd,&c,1);
                    //fprintf(stderr, "%.2x ", (uint8_t)c);

                    // if neither an AT command response or a transmis status (0x89 or 0x8B for DM), skip all following data until next frame
                    if (c!=(char)0x89 && c!=(char)0x8B) {
                        fprintf(stderr, "-S");
                        i=0;
                        while( read(sd,&c,1)>0 && c!=(char)START_BYTE )
                            fprintf(stderr, "%.2x ", (uint8_t)c);

                        // start of a new frame
                        fprintf(stderr, "\n%.2x ", (uint8_t)c);
                        skip_frame=true;
                    }
                    else {
                        got_frame=true;
                        //fprintf(stderr, "GOT ");
                    }
                }

                // second frame, after an expected frame
                if (c==(char)START_BYTE && got_frame)
                    stop_reading=true;

                if (!stop_reading) {
                    if (c==(char)ESCAPE) {
                        read(sd,&c,1);
                        //fprintf(stderr, "%.2x ", (uint8_t)c);
                        c = 0x20 ^ c;
                    }

                    response[i]=(uint8_t)c;
                    // we reach the end of the supposed frame
                    if (i==response[2]+3)
                        stop_reading=true;
                    else
                        i++;
                }
                else
                    i--;
            }

            clock_gettime(CLOCK_REALTIME, &now2);
       }

       clock_gettime(CLOCK_REALTIME, &stopTxStatusResponse);

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

           fprintf(stderr, "\nGot answer. ");

           for (it=0; it<i; it++) {
               tmp = response[it];
               fprintf(stderr, "%.2x ", tmp);
           }

           fprintf(stderr, "\n");

           //the transmit command response API code
           if (response[3]==0x89) {

                fprintf(stderr, "Response to transmit request is ");

                if (response[5]==0) {
                    fprintf(stderr, "Success\n");
                    nbPktSuccess++;
                    err=0;
                }

                if (response[5]==1) {
                    fprintf(stderr, "No ACK received\n");
                    err=1;
                }

                if (response[5]==2) {
                    fprintf(stderr, "CCA failure\n");
                    err=1;
                }

                if (response[5]==3) {
                    fprintf(stderr, "Purged\n");
                    err=1;
                }
           }
           else
           //the transmit command response API code for DigiMesh protocol
           if (response[3]==0x8B) {

                fprintf(stderr, "Transmit retry count is %d\n", response[7]);

                fprintf(stderr, "Response to transmit request is ");

                if (response[8]==0) {
                    fprintf(stderr, "Success\n");
                    nbPktSuccess++;
                    err=0;
                }

                if (response[8]==1) {
                    fprintf(stderr, "MAC ACK failure\n");
                    err=1;
                }

                if (response[5]==0x15) {
                    fprintf(stderr, "Invalid destination endpoint\n");
                    err=1;
                }

                if (response[5]==0x21) {
                    fprintf(stderr, "Network ACK failure\n");
                    err=1;
                }

                if (response[5]==0x25) {
                    fprintf(stderr, "Route not found\n");
                    err=1;
                }
           }
           else
                fprintf(stderr, "Error when getting the response\n");
       }
       if (err)
            nbPktError++;
   }

   clock_gettime(CLOCK_REALTIME, &stopSend);
   return err;
}

void send_img(char* dest_address, char* file_to_send) {

clock_gettime(CLOCK_REALTIME, &startSendImg);

FILE* pFile;
FILE* dFile;
int res;
timespec startSendImgPkt, stopSendImgPkt;

uint8_t ImageData[100];

pFile = fopen(file_to_send, "r");
// file to write the sent packet for verification purposes
if (pktFile) {
    if (fakeSend)
        dFile = fopen("fakeSend.dat", "w");
    else
        dFile = fopen("imageSent.pktlist", "w");
}

double timeBetween2send=0.0;
double meanTimeBetween2Send=0.0;

// if we send to a sensor sniffer, we have to send a special preamble to indicate begin and end of binary stream
// we use "BB" for Binary Begin and "BE" for Binary End
if (sendToSensor) {
    for (int i=0; i<3; i++) {
            res=sprintf((char*)ImageData, "BB%d#*****", i);

            fprintf(stderr, "\nSending image preamble: %s\n", (char*)ImageData);

            if (fakeSend) {
                fprintf(stderr, "Fake send\n");
            }
            else {
                // send the preamble packet for Image Start
                send_pkt(dest_address, ImageData, res);
                // wait 60ms between 2 send
                usleep(60000);
            }
    }
}

if (pFile) {
        int chunkSize;
        uint8_t pktSN=0;
        unsigned int storedNbPackets, imageSizeX, imageSizeY, qualityFactor;

        fscanf(pFile, "%04X", &storedNbPackets);
        fscanf(pFile, "%04X", &imageSizeX);
        fscanf(pFile, "%04X", &imageSizeY);
        fscanf(pFile, "%04X", &qualityFactor);

        if (fakeSend) {
             fprintf(dFile, "%04X ", storedNbPackets);
             fprintf(dFile, "%04X ", imageSizeX);
             fprintf(dFile, "%04X ", imageSizeY);
             fprintf(dFile, "%04X ", qualityFactor);
        }

        int j;

        do {

                bool dropThisPkt=false;

                if (dropPkt && (rand() % 100 < dropPercentage)) {
                        dropThisPkt=true;
                        nbDroppedPkt++;
                }

                clock_gettime(CLOCK_REALTIME, &startSendImgPkt);

                // get the size of the packet
                res=fscanf(pFile, "%04X", &chunkSize);

                if (res==EOF)
                        continue;

                fprintf(stderr, "\npkt %d: %04X ", pktSN, chunkSize);

                if (pktFile && !dropThisPkt)
                    if (fakeSend)
                        fprintf(dFile, "%04X ", chunkSize);
                    else
                        fprintf(dFile, "%04X (%d)", chunkSize, chunkSize);

                for (j=0; j<chunkSize; j++) {

                        res = fscanf(pFile, "%2X", &ImageData[j]);

                        if (pktDisplay)
                            fprintf(stderr, "%02X", ImageData[j]);

                        if (pktFile && !dropThisPkt)
                            if (fakeSend)
                                fprintf(dFile, "%02X ", ImageData[j]);
                            else
                                fprintf(dFile, "%02X", ImageData[j]);

                        if ( ((j==0 || j==1) && pktFile && !dropThisPkt && !fakeSend ) )
                            fprintf(dFile, " ");
                }

                if (pktFile && !dropThisPkt && !fakeSend)
                    fprintf(dFile, "\n");

                if (pktSN!=1) {
                    clock_gettime(CLOCK_REALTIME, &stopSendGeneric);

                    timeBetween2send=diff(startSendGeneric, stopSendGeneric).tv_sec*1000.0+diff(startSendGeneric, stopSendGeneric).tv_nsec/1000000.0;
                    meanTimeBetween2Send+=timeBetween2send;
                }

                if (dropThisPkt) {
                    fprintf(stderr, "\nDROPPED\n");
                }
                else {

                    if (fakeSend) {
                        fprintf(stderr, "\nFake send\n");
                    }
                    else {
                        clock_gettime(CLOCK_REALTIME, &startSendGeneric);

                        // send the packet
                        send_pkt(dest_address, ImageData, chunkSize, pktSN);

                        fprintf(stderr, "BetweenSend (%.2f mean %.2f) Send(%ld %ld %.2f) BRadio(%ld %ld %.2f) WRadio(%ld %ld %.2f) TXStatus(%ld %ld %.2f)\n",
                                timeBetween2send,
                                meanTimeBetween2Send/(pktSN-1),
                                diff(startSend,stopSend).tv_sec, diff(startSend,stopSend).tv_nsec, diff(startSend,stopSend).tv_nsec/1000000.0,
                                diff(startSend,startSendRadio).tv_sec, diff(startSend,startSendRadio).tv_nsec, diff(startSend,startSendRadio).tv_nsec/1000000.0,
                                diff(startSendRadio,stopSendRadio).tv_sec, diff(startSendRadio,stopSendRadio).tv_nsec, diff(startSendRadio,stopSendRadio).tv_nsec/1000000.0,
                                diff(startTxStatusResponse,stopTxStatusResponse).tv_sec, diff(startTxStatusResponse,stopTxStatusResponse).tv_nsec, diff(startTxStatusResponse,stopTxStatusResponse).tv_nsec/1000000.0);

                        fprintf(stderr, "-----------\n");

                        clock_gettime(CLOCK_REALTIME, &stopSendImgPkt);

                        long timeRemainToSleep;

                        if (diff(startSendImgPkt,stopSendImgPkt).tv_nsec/1000.0 < timeBetweenPkt)
                            timeRemainToSleep = timeBetweenPkt - diff(startSendImgPkt,stopSendImgPkt).tv_nsec/1000.0;
                        else
                            timeRemainToSleep = 0;

                        usleep(timeRemainToSleep);

                    }
                }

                pktSN++;
                nbPktSent++;

        } while (res!=EOF);
    }

    clock_gettime(CLOCK_REALTIME, &stopSendImg);

    if (pktFile)
        fclose(dFile);

    if (sendToSensor) {
        for (int i=0; i<3; i++) {
                res=sprintf((char*)ImageData, "BE%d#*****", i);

                fprintf(stderr, "\nSending image trailer: %s\n", (char*)ImageData);

                if (fakeSend) {
                    fprintf(stderr, "Fake send\n");
                }
                else {
                    // send the preamble packet for Image Start
                    send_pkt(dest_address, ImageData, res);
                    // wait 60ms between 2 send
                    usleep(60000);
                }
        }
    }

    fprintf(stderr, "send_img (%ld %ld %.2fms)\n", diff(startSendImg,stopSendImg).tv_sec, diff(startSendImg,stopSendImg).tv_nsec, diff(startSendImg,stopSendImg).tv_nsec/1000000.0);
    fprintf(stderr, "send_img pkt: %d success: %d timeout: %d error/dropped: %d error/dropped ratio: %.2f\n", nbPktSent, nbPktSuccess, 
			nbPktTimeout, nbPktError+nbDroppedPkt, (float)(nbPktError+nbDroppedPkt)/(float)nbPktSent);
}

void * printERROR(char *argv[])
{
   fprintf(stderr, "USAGE:\t%s -baud baudrate -p dev -sensor -framing -srcaddr a -camid id -Q q -L -timing pkt_us serialbyte_us afterradio_us -nw -fake -drop d -pktd -pktf -mac|-net|-addr|-b img_file\n", argv[0]);
   fprintf(stderr, "USAGE:\t-baud 125000, 38400 by default\n");
   fprintf(stderr, "USAGE:\t-sensor, will send image pkt to a sensor for storage, need specific sensor behavior\n");
   fprintf(stderr, "USAGE:\t-framing, will use framing bytes 0xFF0x50+SN for image packets\n");
   fprintf(stderr, "USAGE:\t-srcaddr 1234, will use src addr 0x1234. Insert the src addr after the framing bytes 0xFF0x50\n");
   fprintf(stderr, "USAGE:\t-camid 1, set flow id to 1 to indicate cam id 1 (0xFF0x51 are inserted)\n");
   fprintf(stderr, "USAGE:\t-Q 40, use 40 as Quality Factor, default is 50\n");
   fprintf(stderr, "USAGE:\t-L, insert Libelium API header\n");
   fprintf(stderr, "USAGE:\t-timing 50000 20 25000 by default\n");
   fprintf(stderr, "USAGE:\t-nw, do not wait for TX status response\n");
   fprintf(stderr, "USAGE:\t-fake, emulate sending. Will write in fakeSend.dat, same format than original file (display with display_image -onlydisplay fakeSend.dat original_bmp_file\n");
   fprintf(stderr, "USAGE:\t-drop 50, will introduce 50 of packet drop. Useful with -fake\n");
   fprintf(stderr, "USAGE:\t-pktd, display generated XBee frames\n");
   fprintf(stderr, "USAGE:\t-pktf, generate a pkt list file\n");
   fprintf(stderr, "USAGE:\t-mac 0013a2004069165d\n");
   fprintf(stderr, "USAGE:\t-net 5678 \n");
   fprintf(stderr, "USAGE:\t-addr 64_or_16_bit_addr, set either 64-bit or 16-bit dest. address\n");
   fprintf(stderr, "USAGE:\t-b, for broadcast\n");
   fprintf(stderr, "USAGE:\t img_file, image .dat file with CRAN encoding\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);
   fprintf(stderr, "should see for the last number something close to 15ms\n");
   return 0;
}


int main(int argc, char *argv[])
{
   if (argc < 3)
   {
      printERROR(argv);
      exit (-1);
   }

   char* dest_address;
   char* file_to_send;

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

       if (!strcmp(argv[i], "-baud"))
       {
          customSpeed=atoi(argv[i+1]);
          printf("custom baud rate parameter is: %d\n", customSpeed);
       }

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

       if (!strcmp(argv[i], "-sensor"))
       {
          sendToSensor=true;
          printf("will send image pkt to sensor (sniffer)\n");
       }

       if (!strcmp(argv[i], "-timing"))
       {
          timeBetweenPkt=atoi(argv[i+1]);
          timeBetweenSerialByte=atoi(argv[i+2]);
          timeAfterRadioWrite=atoi(argv[i+3]);
       }

       if (!strcmp(argv[i], "-nw"))
       {
          waitForRadioAnswer=false;
       }

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

       if (!strcmp(argv[i], "-fake"))
       {
          fakeSend=true;
       }

       if (!strcmp(argv[i], "-drop"))
       {
          dropPercentage=atoi(argv[i+1]);
          dropPkt=true;
       }

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


      if (!strcmp(argv[i], "-mac"))
       {
          dest_address=argv[i+1];

          if (strlen(dest_address)==16)
              send_mac=true;
          else {
              printf("need a 64-bit MAC address\n");
              exit(-1);
          }
       }

       if (!strcmp(argv[i], "-net"))
       {
          dest_address=argv[i+1];

          if (strlen(dest_address)==4)
              send_net=true;
          else {
              printf("need a 16-bit short address\n");
              exit(-1);
          }
       }

       if (!strcmp(argv[i], "-addr"))
       {
          dest_address=argv[i+1];

          if (strlen(dest_address)==16)
              send_mac=true;
          else
          if (strlen(dest_address)==4)
              send_net=true;
          else {
              printf("need a 64-bit or 16-bit address\n");
              exit(-1);
          }
       }

       if (!strcmp(argv[i], "-b"))
       {
          send_broadcast=true;
       }

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

       if (!strcmp(argv[i], "-srcaddr"))
        {
           src_addr_mode=true;
           src_addr=argv[i+1];
           if (strlen(src_addr)!=4) {
               printf("need a 16-bit address\n");
               exit(-1);
           }
           printf("use src addr mode: 0x%04X\n", src_addr);
        }

       if (!strcmp(argv[i], "-camid"))
       {
          send_wflowid=true;
          flowid=atoi(argv[i+1]);

	  if (flowid>9)
            flowid=0;
       }

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

   }

   srand(time(NULL));

   if (!fakeSend) {
       init_serial();
       tcflush(sd, TCIOFLUSH);
   }

   file_to_send=argv[argc-1];

   fprintf(stderr, "Preparing to send file %s\n", file_to_send);

   send_img(dest_address, file_to_send);

   close(sd);
   exit(0);
}