/*
The port of TinySec to the MICAz motes is
 (c) 2007-2008 University of Cambridge
 
  The original source code was adapted for the use of TinySec library
  on MICAz motes by Matt Lewis and Dan Cvrcek as part of
     WINES II - Smart Infrastructure EPSRC Project EP/D076870/1
     http://www.winesinfrastructure.org/
  The security-related part of this project was led by Frank Stajano.
  None of the above parties will provide support for this port.
 
  This port is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.
 
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
 
  You should have received a copy of the GNU General Public License
  along with this program.  If not, see http://www.gnu.org/licenses/.

  Parts of the code are subject of Moteworks 2.0 license. 
  See license.txt file in the MoteWorks distribution for details.
*/
/*                                                                      tab:4
 *  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.  By
 *  downloading, copying, installing or using the software you agree to
 *  this license.  If you do not agree to this license, do not download,
 *
 */
/*                                                                      tab:4
 * "Copyright (c) 2000-2003 The Regents of the University  of California.
 * All rights reserved.
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose, without fee, and without written agreement is
 * hereby granted, provided that the above copyright notice, the following
 * two paragraphs and the author appear in all copies of this software.
 *
 * IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY FOR
 * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
 * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF
 * CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
 * AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
 *
 */
/*                                                                      tab:4
 * Copyright (c) 2003 Intel Corporation
 * All rights reserved Contributions to the above software program by Intel
 * Corporation is program is licensed subject to the BSD License, available at
 * http://www.opensource.org/licenses/bsd-license.html
 *
 */
/*
 * Authors:	Mark Yarvis
 *
 */

#include <stdio.h>

#include <errno.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <pthread.h>
#include <sys/time.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include <netdb.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <termios.h>

#include <signal.h>

#include <stdint.h>

#ifndef VERSION
#define VERSION "unknown"
#endif

#define DEFAULT_COM "COM1"
#define DEFAULT_SERVER_PORT 9001

#define PERSISTENCE_CHECK_PERIOD 5000

#define MAX_IO_STREAMS 100

// The max number of characters that could be in either a raw or ASCII message
#define MAX_MESSAGE_LEN 5000

#define BUFFER_SIZE (MAX_MESSAGE_LEN*3)

char* comPorts[][2] = { { "COM1", "/dev/ttyS0"}, 
                        { "COM2", "/dev/ttyS1"},
                        { "COM3", "/dev/ttyS2"},
                      /*  { "COM4", "/dev/tts/3"},
                        { "COM5", "/dev/tts/4"},
                        { "COM6", "/dev/tts/5"},
                        { "COM7", "/dev/tts/6"}, */
                        { 0, 0}
                      };

#define MICA 1
#define MICA2 2
#define IMOTE 3
#define MAX_MOTE_TYPE 3

#define BAUD_RATE 0
#define USE_PREAMBLE 1
int mote_configs[][2] = { { 0, 0 },       // NOT USED
                          { B19200, 0 },  // MICA
                          { B57600, 1 },  // MICA2
                          { B115200, 0 }  // Imote
                   };

#define SF_VERSION_STRING "T "

char uart_frame_vals[3] = {0x97, 0x53, 0x71};
char uart_preamble_vals[5] = {0xFF, 0x00, 0xFF, 0x00, 0xFF};

/*WINES wines - new variables for wines needs*/
int wines=0; //switch enabling creation of special files
int sniffer=0; //switch enabling extra functions for network monitoring
int client=0;
unsigned char wines_buf[MAX_MESSAGE_LEN];
int wines_ptr=0;
int wines_ptr_sync=0;
int msgTypeFilter=0;
unsigned char wines_data[256][10];
int exclude_heartbeat = 0;  

unsigned char wines_network[256][257];
int wines_motes=10;
unsigned int file_number=0;
unsigned int msg_record=0;
unsigned int experiment=0;

#define DC_DEBUG 0 
// types
#define CLIENT_SOCKET   1
#define SERVER_SOCKET   2
#define SERIAL_PORT     3
#define FW_SOCKET_OPEN  4
#define FW_SOCKET_CLOSE 5

// formats
#define ASCII_FORMAT  1
#define RAW_FORMAT    2
#define FRAMED_FORMAT 3
   // for communication with serial forwarder
#define SF_FORMAT     4 
#define DEMO_FORMAT   5 //for Wines demo application

typedef struct {
   int fd;
   char name[100];
   char buf[BUFFER_SIZE];  // input buffer
   int bufLen; // number of bytes in buffer
   char type;
//   char useRawIO;
//   char useFramedIO;   // only valid for Raw IO
   char format;   // only valid for Raw IO
   char writePreamble;
   char bePersistent;
   struct termios origtio;  // original serial port settings to restore
} ioStream;

ioStream streams[MAX_IO_STREAMS];
ioStream *fw_socketStream;

int numStreams = 0;

char usePersistentSockets = 1;

int packetLen = 0;  // use for raw unframed communication only!

void handleClientSocket(ioStream *s);

void printUsage(char *progName) {
   printf("Usage: %s [-1 | -2 | -3] [ <com spec> ... ] [ -r<len> <com spec>] [ -sf <host>:<port>]\n", progName);
   printf("    Where <com spec> is one of\n");
   printf("        port        - to create a server socket\n");
   printf("        host:port   - to connect to a server\n");
   printf("        COMn        - to connect to a comm port\n");
   printf("        -SNIFFER    - options for attack monitoring\n");
   printf("    And additional options are\n");
   printf("        -r<len>        - opens any subsequent comm ports for unframed\n");
   printf("                         and non-ascii communication with packets of\n");
   printf("                         the specified length\n");
   printf("        -sf        - opens any subsequent sockets for communication \n");
   printf("                     using the serial forwarder protocol\n");
   printf("        -1              - opens any subsequent comm ports with a baud\n");
   printf("                      rate for RENE or MICA motes\n");
   printf("        -2              - opens any subsequent comm ports with a baud\n");
   printf("                      rate for MICA2 motes\n");
   printf("        -3              - opens any subsequent comm ports with a baud\n");
   printf("                      rate for XMesh motes\n");
   printf("        -NoHB        Exclude heartbeat packats from log file\n");
}

uint64_t currentTimeMillis() {
   struct timeval tv;

   gettimeofday(&tv, 0);
   return ( ((uint64_t) tv.tv_sec) *1000) + (tv.tv_usec / 1000);
}

void restoreSerialSettings(ioStream *s) {
   if (s->type == SERIAL_PORT) {
      printf("Restoring serial settings on %s\n", s->name);
      tcsetattr(s->fd,TCSANOW,&(s->origtio));
   } else {
      fprintf(stderr, "WARNING: Attempt to restore serial settings on ");
      fprintf(stderr, "non-serial file descriptor\n");
   }
}

void cleanExit(int code) {
   int i;

   for (i=0; i<numStreams; i++) {
      if (streams[i].type == SERIAL_PORT) {
         restoreSerialSettings(&(streams[i]));
      }
   }

   exit(code);
}

char *filenameForCommPort(char *commName) {
   int i;
   
   for (i=0; comPorts[i][0]!=0; i++) {
      if (strcmp(commName, comPorts[i][0])==0) {
         return comPorts[i][1];
      }
   }
   if (strncmp(commName,"COM=",4)==0)
      return commName+4;
   return NULL;
}

ioStream *createStreamStruct() {
   if (numStreams+1 >= MAX_IO_STREAMS) {
      printf("Exceeded maximum number of streams!\n");
      cleanExit(1);
   }

   streams[numStreams].fd = -1;
   streams[numStreams].bufLen = 0;
   streams[numStreams].type = 0;
   streams[numStreams].format = 0;
   streams[numStreams].writePreamble = 0;  // valid if format=FRAMED_FORMAT
   streams[numStreams].bePersistent = 0;
   return &(streams[numStreams++]);
}

void deleteStream(ioStream *s) {
   if (s->bePersistent) {
      s->fd = -1;
      return;
   }

   if (s != &(streams[numStreams-1])) { // if not last entry
      // copy the last entry into the hole
      memcpy(s, &(streams[numStreams-1]), sizeof(ioStream));
   }
   numStreams--;
}

int isNum(char *s) {
   if ((s == 0 ) || (*s == 0)) { // if NULL or a null string is passed, fail
      return 0;
   }

   while (*s != 0) {
      if ((*s > '9') || (*s < '0')) {
         return 0;
      }
      s++;
   }

   return 1;
}

void shiftBufferLeft(ioStream *s, int n) {
   int i;

   s->bufLen -= n;

   for (i=0; i < s->bufLen; i++) {
      s->buf[i] = s->buf[i+n];
   }
}

int hexToDecimal(char c) {
   if ((c>='0') && (c<='9')) {
      return c - '0';
   } else if ((c>='a') && (c<='f')) {
      return c - 'a' + 10;
   } else if ((c>='A') && (c<='F')) {
      return c - 'A' + 10;
   } else {
      fprintf(stderr, "ERROR: Bad character in hex value: %c\n", c);
      return 0;
   }
}

int switchToRawUnframedIO(char * arg) {
   if ((arg[0] == '-') && (arg[1] == 'r') && (isNum(arg+2))) {
      if (packetLen > 0) {  // only allowed once!
         printUsage("uartserver");
         cleanExit(0);
      }
      packetLen = atoi(arg+2);
      printf("Setting packet length for raw connections to %d\n", packetLen);
      return 1;
   } else {
      return 0;
   }
}

int writeMessage(int fd, char* buf, int buflen) {
   struct timeval zero_timeout;
   fd_set wfds, efds;
   int written=-1;
   
   FD_ZERO(&wfds);
   FD_SET(fd, &wfds);
   FD_ZERO(&efds);
   FD_SET(fd, &efds);

   zero_timeout.tv_sec = 0;
   zero_timeout.tv_usec = 0;

   if (select(fd+1, NULL, &wfds, &efds, &zero_timeout) > 0) {
//     printf("writee %d %d %s\n",fd, buflen,buf);
     written=write(fd, buf, buflen);
   }
   return written;
}

void write_bytes(int fd, const void *buf, size_t count) {
#ifdef WRITE_PACE
   int i;
   for (i=0; i<count; i++) {
      write(fd, buf[i], 1);
      usleep(WRITE_PACE);
   }
#else
   write(fd, buf, count);
#endif
}

// input buf is in the format "hh hh ... \n", where h is a hex digit
// should call normalizeMessage before calling this function
void writeRawBytes(int fd, char* buf, int buflen, char format, 
                                                int writePreamble) {
   int i;
   char out;

   if (format == FRAMED_FORMAT) {
      if (writePreamble) {
         write_bytes(fd, uart_preamble_vals, 5);
      }
      write_bytes(fd, uart_frame_vals, 3);
   }

   if ((format == FRAMED_FORMAT) || (format == SF_FORMAT)) {
      out = buflen/3;
      write_bytes(fd, &out, 1);
   }

   i = 0;
   do {
      i++;
      if ((format == RAW_FORMAT) && (i > packetLen)) {
         printf("WARNING: attempt to write a long packet on a raw unframed port\n");
         return;
      }
      out = (hexToDecimal(*(buf++))<<4);
      out |= hexToDecimal(*(buf++));
      write_bytes(fd, &out, 1);
   } while (*(buf++) != '\n');

   if ((format == RAW_FORMAT) && (i < packetLen)) {
      printf("WARNING: padding a short packet written to a raw unframed port\n");
      out = 0;
      for (; i < packetLen; i++) {
         write_bytes(fd, &out, 1);
      }
   }
}

int isHexDigit(char c) {
   return (((c>='0') && (c<='9')) ||
           ((c>='a') && (c<='f')) ||
           ((c>='A') && (c<='F')));
}

int isWhitespace(char c) {
   return ((c == ' ') || (c == '\t') || (c == '\r'));
}

// normalizes buf to the format "hh hh ... \n", where h is a hex digit
// len may be shortened
// the return value is 1 iff the conversion was successful
int normalizeMessage(char *buf, int *len) {
   char out[BUFFER_SIZE];
   int out_i=0;
   int i=0;

   while (1) {
      while ((i < *len) && isWhitespace(buf[i])) { // skip whitespace
         i++;
      }

      if (i >= *len) {
         return 0;     // string can't end here
      }

      if (! isHexDigit(buf[i])) {  // check first hex digit
         return 0;
      }

      out[out_i++] = buf[i++]; // copy byte

      if (i >= *len) {
         return 0;     // string can't end here
      }

      if (! isHexDigit(buf[i])) { // check second hex digit
         return 0;
      }

      out[out_i++] = buf[i++]; // copy byte

      while ((i < *len) && isWhitespace(buf[i])) { // skip whitespace
         i++;
      }

      if (i >= *len) {
         return 0;     // string can't end here
      }

      if (buf[i] == '\n') {  // this is end of message
         out[out_i++] = '\n';  // add a newline to the end

         // update the length and copy the string back
         *len = out_i;
         bcopy(out, buf, *len);

         return 1;   // success!
      }

      out[out_i++] = ' ';
   }
}

// should call normalizeMessage before calling this function
void forwardMessage(char *buf, int n, ioStream *sourceStream) {
   int i;

   for (i=0; i<numStreams; i++) {
//      if (streams[i].type == FW_SOCKET_OPEN)
//         handleClientSocket(&(streams[i]));
      if ((streams[i].type != SERVER_SOCKET) && // don't write to server sock
          (streams[i].fd != -1) &&              // don't write to closed sock
          (&(streams[i]) != sourceStream)) {    // don't repeat to self
         if (streams[i].type == FW_SOCKET_OPEN) {
           if (writeMessage(streams[i].fd, buf, n)<0)
             streams[i].type = FW_SOCKET_CLOSE;
         } else if ((streams[i].format == ASCII_FORMAT)) {
            writeMessage(streams[i].fd, buf, n);
         } else {
            writeRawBytes(streams[i].fd, buf, n, streams[i].format, 
                          streams[i].writePreamble);
         }
      }
   }
}

void handleClientSocket(ioStream *s) {
  char buff[10];
  int len;
  
  if ((s->fd<0)||(s->type==FW_SOCKET_CLOSE)){
    s->type = FW_SOCKET_CLOSE;
    s->fd=-1;
    return;
  }
  if (s->type!=FW_SOCKET_OPEN){
    return;
  }

  //len=read(s->fd, buff, 0);
  len=recv(s->fd, buff, sizeof(buff), MSG_PEEK | MSG_DONTWAIT);
  if (len==0){
    s->type = FW_SOCKET_CLOSE;
    s->fd=-1;
//printf ("Hurrayyyy\n");
  } else if (len>0){
    recv(s->fd, buff, sizeof(buff),  MSG_DONTWAIT);
    if (strtod(buff,NULL)!=0) {
       msgTypeFilter=(int)strtod(buff,NULL)  % 256;
       if (msgTypeFilter>0)
          printf("Message filter set to %d (%02X)\n",msgTypeFilter,msgTypeFilter);
       else
          printf("Message filter set to %d (-%02X)\n",msgTypeFilter,-msgTypeFilter);
    } else {
       msgTypeFilter=0;
       printf("Message filter reset!\n");
    }
    buff[0]=0;
  }
}

void handleServerSocket(ioStream *s) {
   struct sockaddr_in sa;
   unsigned int addrlen;
   struct hostent *he;
   char *hostname=NULL;
   ioStream *newStream;
   int fd;
   
   addrlen = sizeof(sa);
   fd = accept(s->fd, (struct sockaddr *) &sa, &addrlen);
   if (fd < 0) {
      if (errno!=EAGAIN) {    // it's nonblocking, so this is ok
         perror("accept()");
      }
      return;
   }

   he = gethostbyaddr((char *) &(sa.sin_addr), 4, AF_INET);
   if (he == NULL) {
      char *bytes = (char *) &(sa.sin_addr);
      sa.sin_addr.s_addr = ntohl(sa.sin_addr.s_addr);
      hostname = (char *) malloc(16);
      sprintf(hostname, "%d.%d.%d.%d", bytes[0], bytes[1], bytes[2], bytes[3]);
   } else {
      hostname = (char *) malloc(strlen(he->h_name)+1);
      strcpy(hostname, he->h_name);
   }

   printf("Got connection from %s:%d to %s\n", hostname, 
                                               ntohs(sa.sin_port), s->name);

   newStream = createStreamStruct();
   newStream->fd = fd;
   newStream->type = CLIENT_SOCKET;
   newStream->format = s->format;
   sprintf(newStream->name, "%s:%d", hostname, ntohs(sa.sin_port));

   if ((newStream->format == SF_FORMAT)&&(sniffer==0)) {
      char buf[2];

      write(newStream->fd, SF_VERSION_STRING, 2);
      read(newStream->fd, buf, 2);
   }
}

char intToHexChar(int n) {
   if (n < 10) {
      return n + '0';
   } else {
      return n - 10 + 'A';
   }
}

char *byteToHexString(char b, char *s) {
   s[0] = intToHexChar((b>>4)&0xf);
   s[1] = intToHexChar(b&0xf);
   s[2] = '\0';
   return s;
}

int fillBuffer(ioStream *s) {
   int n;

   n = read(s->fd, &(s->buf[s->bufLen]), BUFFER_SIZE - s->bufLen);
   if (n <= 0) {   // close or EOF
      if (n < 0) {
         printf("Input %s failed: %s\n", s->name, strerror(errno));
      } else {
         printf("Input %s closed\n", s->name);
      }
      if (s->type == SERIAL_PORT) {
         restoreSerialSettings(s);
      }
      close(s->fd);
      deleteStream(s);
      return 1;
   }

   s->bufLen += n;

   return 0;
}

int handleWinesOutput(ioStream *s, int bytes, int useBytes, int repeated, char *hexbuf){
  int i=0,j;
  FILE *wine_fd;
  int msg_offset,offset;
  int min,value;
  unsigned char cycle[32];
  char filename[32];
  int sync_old;
  struct stat check_file;
  char forwardBuf[BUFFER_SIZE];
  int inside;

  if ((!wines)&&(!sniffer)){
     return 0;
  } else {
    if (DC_DEBUG) printf("Wines or sniffer is here\n");
  }
  if ((repeated==0)&&(hexbuf==NULL)){
    if (useBytes){
      if (DC_DEBUG) printf("We have got some bytes %d\n",bytes);
      memcpy(wines_buf+wines_ptr, s->buf, bytes);
      wines_ptr= wines_ptr+bytes;
    } else {
      for (i=0; (i < s->bufLen); i++) {
         wines_buf[wines_ptr++]=s->buf[i];
      }
    }
  }

  // we copy the new bytes into our internal buffer first
  if ((repeated==0)&&(hexbuf!=NULL)){
//      printf("ok, we have got the message %d",wines_ptr);
      memcpy(wines_buf+wines_ptr, hexbuf, bytes);
      wines_ptr+=bytes;
//      printf(" %d\n",wines_ptr);
  }
  //zeroise the rest of buffer
  memset(wines_buf+wines_ptr,0,sizeof(wines_buf)-wines_ptr);

  if (DC_DEBUG){
     printf("1 still alive, wines_ptr = %d %d\n",wines_ptr,wines_ptr_sync);
     printf("DBG: ");
     for(i=0;i<wines_ptr+5;i++){
        printf("%02X ",wines_buf[i]);
     }
     printf("\n");
  }
  // now, we have data stored in our own buffer, let's do some commputation
  // but first, remove control codes
  sync_old=wines_ptr_sync;
  // the new frame should start at the beginning of the buffer
  // the exception may be the very first frame or if something bad happens
  //   - wines_ptr_sync=0
  // or when the buffer didnot contain the whole frame in the last run 
  //    - we are in the middle of the frame in this case (wines_ptr_sync>0)
  if (wines_ptr_sync==0)
     inside=0;
  else
     inside=1;
 
  if ((client==0)||(sniffer==0)){
    for (i=wines_ptr_sync; i<wines_ptr-1; i++){
        if ((inside==2)||(inside==3))
          inside=1;
        if (wines_buf[i]==0x7e) {
          if (wines_buf[i+1]==0x7e && inside==0){  // if there is a sequence of two 0x7E bytes
             i++;                        // the second begins a new frame 
          }
          if (inside==0){// remove the rubish before the frame
             for (j=i;j<wines_ptr;j++)
               wines_buf[j-i]=wines_buf[j];
             wines_ptr=wines_ptr-i;
             inside=2; // we are not yet inside but we will be there if there's at least one more byte
             i=1;  //the wines_buf should start with 0x7E - beginning of a new frame
          } else {
             i++;
             inside = -1; // flag for later on - that we got here
             break; // get out of the for-cycle, we've found what we wanted
          }
        } else if (wines_buf[i]==0x7d){ // escaped character
           wines_buf[i+1]=wines_buf[i+1]^0x20; // remove ESC
           for (j=i;j<wines_ptr-1;j++)
             wines_buf[j]=wines_buf[j+1];
           wines_ptr--;
           inside=3; //just mark that we've just escaped a character - it might be 0x7E
        }
    }
    if ((wines_buf[wines_ptr-1]==0x7E)&&(inside==1)){ // the end of frame is exactly the last byt in the buffer
      wines_ptr_sync=wines_ptr-1;
      inside=-1; //it is what we were looking for 
    } else
      //remember where we have finished the synchronisation
      wines_ptr_sync = i-1; //update pointer to the last "un-control-coded" byte
      if (inside==3)
        wines_ptr_sync = i;
  }
/*printf("XXXX");
       for(j=0;j<wines_ptr;j++){
          printf("%02X ",wines_buf[j]);
        }
       printf("\n");
*/
  if (inside!=-1){ //not synchronised
     if (DC_DEBUG) {printf("packet is not synchronised,\n");
       printf("not synchronised %d %d sync:%d %d\n",wines_ptr, i, sync_old, wines_ptr_sync);
       for(j=0;j<wines_ptr;j++){
          printf("%02X ",wines_buf[j]);
        }
       printf("\n");
     }
     printf("*");
     return 0;
  }

  if (DC_DEBUG) printf("3 still alive, synchronisation wines_ptr = %d %d\n",wines_ptr, wines_ptr_sync);
  //it may happen that the content is not really correct

  if (wines_buf[1]!=0x42 && wines_buf[1]!=0x41 && wines_buf[1]!=0x40){
    if (wines_buf[1]!=0x42){
      for(i=0;i<wines_ptr;i++){
        printf("%02X ",wines_buf[i]);
      }
      printf("\n");
    }
    memmove(wines_buf,wines_buf+wines_ptr_sync+1,(wines_ptr-wines_ptr_sync-1));
    wines_ptr-=(wines_ptr_sync+1);
    wines_ptr_sync=0;
    return 0;
  }


  if ((wines_ptr<7)||(wines_ptr<wines_buf[6]+7+3)){
    if (DC_DEBUG){
      printf("Not correct length of the  frame? %d %d\n",wines_ptr,wines_buf[6]+7+2);
      for(i=0;i<wines_ptr;i++){
        printf("%02X ",wines_buf[i]);
      }
      printf("\n");
    }
    printf("Not correct length of the frame? %d %d\n",wines_ptr,wines_buf[6]+7+2);
    for(i=0;i<wines_ptr;i++){
      printf("%02X ",wines_buf[i]);
    }
    printf("\n");
    //wines_buf[0]=0; //marked frame as processed
    memmove(wines_buf,wines_buf+wines_ptr_sync+1,(wines_ptr-wines_ptr_sync-1));
    wines_ptr-=(wines_ptr_sync+1);
    wines_ptr_sync=0;
    return 0;
  }
// the buffer starts with 0x7e [frame] ...
// control output - to be deleted
/*  if (DC_DEBUG){
     for(i=0;i<wines_ptr;i++){
        printf("%02X ",wines_buf[i]);
     }
     printf("\n");
  }
*/
  if (sniffer){
    // let's log the message first
    if (experiment==0){
      do{
        memset(&check_file,0,sizeof(check_file));
        sprintf(filename, "sniffer%02d001.log",(int)experiment);
        if ((wine_fd=fopen(filename, "r"))!=NULL){
           experiment++;
           fclose(wine_fd);
           i=0;
        } else{
           i=1;
        }
      } while (i==0);
      printf("Experiment no %d\n",experiment);
    }


    // Log packet to file

    if (   exclude_heartbeat
        && (wines_buf[4] == 0xfd && wines_buf[6] == 0x02))
    {    
      // omit heatbeat packet
    }
    else 
    {
      if (msg_record % 5000 ==0)
         file_number++;
      
      sprintf(filename, "sniffer%02d%03d.log",experiment,file_number);
      wine_fd=fopen(filename,"a");
      if (msg_record==0){
        /* let's print the version info first */
        fprintf(wine_fd,"Log Version: 1\n");
      } 

      uint64_t t = currentTimeMillis();
      fprintf(wine_fd,"%lli.%03lli ", t/1000, t%1000);

      for(i=2;i<wines_buf[6]+7+2;i++)
        fprintf(wine_fd,"%02X ",wines_buf[i]);

      fprintf(wine_fd,"\n");

      fclose(wine_fd);
      msg_record++;
    }

    // Forward packet
    for(i=2;i<wines_buf[6]+7+2;i++)
      sprintf(forwardBuf+3*(i-2),"%02X ",wines_buf[i]);

    sprintf(forwardBuf+3*(i-2),"\n");


    // forward the message - to another socket
    if (((msgTypeFilter==0)||(wines_buf[4]==msgTypeFilter)||(msgTypeFilter<0 && wines_buf[4]!=-msgTypeFilter))&&(client==0))
      forwardMessage(forwardBuf,strlen(forwardBuf), s);

    msg_offset=0;

    if (wines_buf[msg_offset+4]==0x03){  //debug msg        !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
      if (DC_DEBUG){
        printf("\n");
        for(i=0;i<wines_ptr;i++){
           printf("%02X ",wines_buf[i]);
           if (i%8==7) printf("|");
        }
        printf("\n");
      }

      if ((wines_buf[8]==1)&&(wines_buf[9]==8)&&(wines_buf[11]==0xff)&&(wines_buf[12]==0xff)){
         msg_offset=10; 
      }
      if (DC_DEBUG) printf("msg 03... \n");
      i=0;
      j=wines_buf[msg_offset+14]&0x0f; //number of entries here
//  printf("Entries: %d version: %d\n",j, wines_buf[msg_offset+16]);
      if(DC_DEBUG) printf("No of neighbours is %d\n",j);
//      memset(wines_network[wines_buf[9]], sizeof(wines_network[0]), 0); //wines_buf[9] --addr of sender
      //let's compute info offset
      if (wines_buf[msg_offset+16]==2)
        offset=msg_offset+17;
      else 
        offset=msg_offset+17+14;
//      for (i=0;i<256;i++)
//        wines_network[wines_buf[9]][i]=0;
      for (i=0;i<j;i++){
        /*         wines_buf[offset+i*5]  //id of neighbour
         *         wines_buf[offset+i*5+2]  //link quality
         *         wines_buf[offset+i*5+3]  //path cost
         *         wines_buf[offset+i*5+4]  LQI */
        value = wines_buf[offset+i*5+3];
        wines_network[wines_buf[msg_offset+9]][wines_buf[offset+i*5]]=value;
        //zero cost is bad for us let's increment it
        if (value==0)
           wines_network[wines_buf[msg_offset+9]][wines_buf[offset+i*5]]++;

        if (DC_DEBUG) printf("from %d to %d, quality %d\n",wines_buf[msg_offset+9],wines_buf[offset+i*5], wines_buf[offset+5*i+3]);
      }
      min=255;
      //store neighbour with the least path cast
      for (i=0;i<256;i++){
        value=wines_network[wines_buf[msg_offset+9]][i];
        if ((value>0)&&(value<min)){
          min=value;
          wines_network[wines_buf[msg_offset+9]][256]=i+1;
        }
      }
      wine_fd=fopen("connectiondata.csv","w");
      fprintf(wine_fd,"#ID|-> path towards gateway\n");
      printf("\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");
      printf("Table of path costs.\n");
      fprintf(wine_fd,"Table of path costs\n");
      for (i=1;i<10;i++){
        if (wines_network[i][256]==0){
          printf("%d| %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d   'parent': -\n",i,wines_network[i][0],wines_network[i][1],wines_network[i][2],wines_network[i][3],wines_network[i][4],wines_network[i][5],wines_network[i][6],wines_network[i][7],wines_network[i][8],wines_network[i][9]);
          fprintf(wine_fd,"%d| %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d   'parent': -\n",i,wines_network[i][0],wines_network[i][1],wines_network[i][2],wines_network[i][3],wines_network[i][4],wines_network[i][5],wines_network[i][6],wines_network[i][7],wines_network[i][8],wines_network[i][9]);        
        } else {
          printf("%d| %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d   'parent': %d\n",i,wines_network[i][0],wines_network[i][1],wines_network[i][2],wines_network[i][3],wines_network[i][4],wines_network[i][5],wines_network[i][6],wines_network[i][7],wines_network[i][8],wines_network[i][9],wines_network[i][256]-1);
         fprintf(wine_fd,"%d| %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d   'parent':%d\n",i,wines_network[i][0],wines_network[i][1],wines_network[i][2],wines_network[i][3],wines_network[i][4],wines_network[i][5],wines_network[i][6],wines_network[i][7],wines_network[i][8],wines_network[i][9],wines_network[i][256]-1); 
        }
      }
      printf("#ID|-> path towards gateway\n");
      for (i=0;i<256;i++){
        memset(cycle, 0, sizeof(cycle));
        if (wines_network[i][256]>0){
           fprintf(wine_fd,"%d| ",i);
           printf("%d| ",i);
           j=i;
           do {
//             printf("Cycle: %d\n",cycle[0]);
             cycle[j>>3]|=(1<<(j&0x7));
             value=wines_network[j][256];
             if (value<1){
               fprintf(wine_fd,"-> ?");
               printf("-> ?"); 
             } else {
               fprintf(wine_fd,"-> %d (%5.2f) ",value-1,(float)wines_network[j][value-1]/256);
               printf("-> %d (%5.2f) ",value-1,(float)wines_network[j][value-1]/256);
             }
             j=value-1;
           } while ((value>1)&&((cycle[j>>3]&(1<<(j&0x7)))==0) );
           fprintf(wine_fd,"\n");
           printf("\n");
        }
      }
      fclose(wine_fd);
    } else { //it is not debug/health message
      if (client==0) printf("."); // and we are not in the tapping mode
    }
    // we have processed a message and we can get rid of it
    //maybe????
    //wines_buf[0]=0;
    memmove(wines_buf,wines_buf+wines_ptr_sync+1,(wines_ptr-wines_ptr_sync-1));
    wines_ptr-=(wines_ptr_sync+1);
    wines_ptr_sync=0;
    // is there another message?
    if (DC_DEBUG)
      printf("DDDDD %d %d %d",7+wines_buf[6]+2,wines_buf[wines_buf[6]+7+2+7],wines_ptr);
//    if (7+wines_buf[6]+2+wines_buf[wines_buf[6]+7+2+7]+5>wines_ptr)
    if (wines_ptr_sync>=wines_ptr-1)
       return 0;
    else{
       return 1;
    }
  }
  return 0;
}

void handleRawUnframedInput(ioStream *s, int bytesToRead) {
   int i;
   char scratch[3];
   char output[BUFFER_SIZE];
   int j;

   i=0;
   while (handleWinesOutput(s, bytesToRead, 1,i,NULL)){i=1;};
   if (s->bufLen < bytesToRead) {
      return;   // not enough bytes in the buffer yet
   }

   if (!(wines || sniffer)){
     // create a message in ASCII format
     j=0;
     for (i=0; i < bytesToRead; i++) {
        if ((s->buf[i]==0x7d)&&(i+1<bytesToRead)){
          s->buf[i+1]=s->buf[i+1]^0x20;
          i=i+1;
        }
        byteToHexString(s->buf[i], scratch);
        output[j++]=scratch[0];
        output[j++]=scratch[1];
        if (i < bytesToRead-1) {
           output[j++]=' ';
        } else {
           output[j++]='\n';
        }
     }
     output[j]='\0';

     // print out the message
     printf("%s", output);
     fflush(stdout);
     // forward the message
     forwardMessage(output, j, s);

   }  
   // remove the packet bytes from the buffer and keep going
   shiftBufferLeft(s, bytesToRead);
}

void handleSFInput(ioStream *s) {
   int len;

   if (s->bufLen < 2) {
      return;   // not enough bytes in the buffer yet
   }

   len = (s->buf[0] & 0xFF);

   shiftBufferLeft(s, 1);

   handleRawUnframedInput(s, len);
}

void handleRawFramedInput(ioStream *s) {
   int i;

   if (fillBuffer(s)) {
      return;
   }

   i=0;
   while (handleWinesOutput(s,0,0,i,NULL)){i=1;};
   while (1) {
      char scratch[3];

      // throw away bytes until we see first byte of uart frame

      if (sniffer==0){
        for (i=0; (i < s->bufLen) && (s->buf[i] != uart_frame_vals[0]); i++) {
           if ((s->buf[i]==0x7d)&&(i+1 < s->bufLen)){
             s->buf[i+1] ^= 0x20;
             i++;
           }
           if ((sniffer==0)||(DC_DEBUG==1))
             printf("(%s) ", byteToHexString(s->buf[i], scratch));
        }
      } else {
        for (i=0; (i < s->bufLen); i++) {
           if ((s->buf[i]==0x7d)&&(i+1 < s->bufLen)){
             s->buf[i+1] ^= 0x20;
             i++;
           }
           if ((sniffer==0)||(DC_DEBUG==1))
             printf("(%s) ", byteToHexString(s->buf[i], scratch));
        }
      }
      fflush(stdout);

      shiftBufferLeft(s, i);

      // if there aren't enough bytes for a whole frame, wait for more
      if (s->bufLen < 4) {  //4 is an arbitrary? (is it?) value
         return;
      }

      // is this a frame?
      if (sniffer==0){ //only for non sniffer modes
        if ((s->buf[1] != uart_frame_vals[1]) || 
            (s->buf[2] != uart_frame_vals[2])) {
           // not a frame, discard the "frame start" byte and keep going
           if ((unsigned char)(s->buf[0])==0x7d){
             s->buf[1]=s->buf[1]^0x20;
           } else
           if ((sniffer==0)||(DC_DEBUG==1))
              printf("(%s) ", byteToHexString(s->buf[0], scratch));
           fflush(stdout);
           shiftBufferLeft(s, 1);
        } else {
           // this is the frame
           char output[BUFFER_SIZE];
           int len = s->buf[3];
           int j;

           // is the entire packet in the buffer?
           if (s->bufLen < len + 4) {
              return;  // no, wait for more data
           }

           // print out the frame
           for (i=0; i<4; i++) {
              printf("[%s] ", byteToHexString(s->buf[i], scratch));
           }

           // create a message in ASCII format
           j=0;
           for (i=4; i < len + 4; i++) {
              byteToHexString(s->buf[i], scratch);
              output[j++]=scratch[0];
              output[j++]=scratch[1];
              if (i < len+3) {
                 output[j++]=' ';
              } else {
                 output[j++]='\n';
              }
           }
           output[j]='\0';

           // print out the message
           printf("%s", output);
           fflush(stdout);

           // forward the message
           forwardMessage(output, j, s);

           // remove the packet bytes from the buffer and keep going
           shiftBufferLeft(s, len + 4);
         }
      }
   }
}

void handleAsciiInput(ioStream *s) {
   int bytesRead, i, offset;
   char hexBuf[MAX_MESSAGE_LEN];

   bytesRead = read(s->fd, &(s->buf[s->bufLen]), BUFFER_SIZE - s->bufLen);
   
   if ((sniffer==1)&&(bytesRead>0)){
     // change to hex array
     i=s->bufLen;
     offset=1;
     hexBuf[0]=0x7e;
//     hexBuf[1]=0x42;
//     printf("Do not know - we got %d bytes\n",bytesRead);;
     while (i < bytesRead+s->bufLen){
       if (s->buf[i]<0x30){
          break; //it is \n that we do not need
       }
       hexBuf[offset]=16 * ((s->buf[i]&0xf)+((s->buf[i]&0x40)>>6)*9);
       hexBuf[offset]+=((s->buf[i+1]&0xf)+((s->buf[i+1]&0x40)>>6)*9);
//       printf("%c%c %02X \n",s->buf[i],s->buf[i+1],(unsigned char)hexBuf[offset]);
       i=i+3;
       offset++;
     }
     hexBuf[offset++]=0x7e;
//     printf("called our handle\n");
     i=0;
     while (handleWinesOutput(NULL, offset,0,i,hexBuf)){i=1;};
   }

   if (bytesRead <= 0) {   // close or EOF
      if (bytesRead < 0) {
         printf("Input %s failed: %s\n", s->name, strerror(errno));
      } else {
         printf("Input %s closed\n", s->name);
      }
      close(s->fd);
      deleteStream(s);
   } else {
      int i;
      s->bufLen += bytesRead;

      i = 0;
      while (i < s->bufLen) {
         if (s->buf[i] == '\n') {
            char buf[BUFFER_SIZE];
            int msgLen=i+1;

            bcopy(s->buf, buf, msgLen);

            if (normalizeMessage(buf, &msgLen)) {
               buf[msgLen]=0;
               if (sniffer==0)
                  printf("Forwarding message from %s: %s\n", s->name, buf);
               else
                  printf("%s",buf);
               forwardMessage(buf, msgLen, s);
            } else {
               bcopy(s->buf, buf, i+1);
               buf[i+1]=0;
               fprintf(stderr, "Bad message format: %s\n", buf);
                  
            }

            shiftBufferLeft(s, i+1);
            i=0;
         } else {
            i++;
         }
      }
   }
}

void handleInput(ioStream *s) {
   if (s->type == SERVER_SOCKET) {
      handleServerSocket(s);
   } else if (s->type == FW_SOCKET_OPEN) {
      handleClientSocket(s);
   } else if (s->format == ASCII_FORMAT) {
      handleAsciiInput(s);
   } else if (s->format == FRAMED_FORMAT) {
      handleRawFramedInput(s);
   } else if ((s->format == RAW_FORMAT) || (s->format == SF_FORMAT)) {
      if (fillBuffer(s)) {
         return;
      }

      if (s->format == RAW_FORMAT) {
         handleRawUnframedInput(s, packetLen);
      } else if (s->format == SF_FORMAT) {
         handleSFInput(s);
      }
   } else {
      fprintf(stderr, "FATAL ERROR: Invalid stream format.");
      exit(1);
   }
}

// timeout is in msec
void waitForInputs(long timeout) {
   fd_set rfds;
   fd_set efds;
   int highest=0;
   int i;
   struct timeval tv;

   tv.tv_sec = timeout / 1000;
   tv.tv_usec = (timeout % 1000) * 1000;

   FD_ZERO(&rfds);
   FD_ZERO(&efds);
   for (i=0; i<numStreams; i++) {
      if (streams[i].fd > -1) {   // ignore persistent streams that are closed
         if (streams[i].fd > highest) {
            highest = streams[i].fd;
         }
         FD_SET(streams[i].fd, &rfds);
         FD_SET(streams[i].fd, &efds);
      }
   }
   if (select(highest+1, &rfds, NULL, &efds, &tv) > 0) {
      for (i=0; i<numStreams; i++) {
         if ((streams[i].fd > -1) && 
             (FD_ISSET(streams[i].fd, &rfds) || 
              FD_ISSET(streams[i].fd, &efds))
            ) {
            // Note: It's possible, as a result of handleInput(), for the
            //       array of streams to be changed.  Most notably, an entry
            //       can be deleted, shifting others up.  The worst this
            //       can cause is for us to miss reading from a descriptor
            //       this time around and have to catch it on the next 
            //       call to select().
            handleInput(&(streams[i]));
         }
      }
   }
}

void createComm(char *portName, ioStream *s, char format, int moteType) {
   char * filename = filenameForCommPort(portName);
   struct termios newtio;

   strcpy(s->name, portName);
   s->type = SERIAL_PORT;
   s->format=format;

   if (s->format == FRAMED_FORMAT) {
      s->writePreamble = mote_configs[moteType][USE_PREAMBLE];
   }

   /* open serial port for read/write */
   printf("Opening port: %s (%s)\n", portName, filename);
   s->fd = open(filename, O_RDONLY|O_NONBLOCK);//WR);//|O_NOCTTY);
   if (s->fd < 0) {
      perror("open()");
      cleanExit(1);
   }

   tcgetattr(s->fd, &(s->origtio));

   /* Serial port setting */
   memset(&newtio, 0, sizeof(newtio));
   newtio.c_cflag = mote_configs[moteType][BAUD_RATE] | CS8 | CLOCAL | CREAD;
   newtio.c_iflag = IGNPAR;

   cfsetospeed(&newtio, (speed_t)mote_configs[moteType][BAUD_RATE]);
   cfsetispeed(&newtio, (speed_t)mote_configs[moteType][BAUD_RATE]);

   /* Raw output_file */
   newtio.c_oflag = 0;
   tcflush(s->fd, TCIFLUSH);
   tcsetattr(s->fd, TCSANOW, &newtio);
}


void createServerSocket(int port, ioStream *s, char format) {
   struct sockaddr_in sa;

   printf("Listening to port %d\n", port);

   sa.sin_family=AF_INET;
   sa.sin_port=htons(port);
   sa.sin_addr.s_addr=INADDR_ANY;

   s->fd=socket(PF_INET, SOCK_STREAM, 0);
   if (s->fd < 0) {
      perror("socket()");
      cleanExit(1);
   }

   {
      // Allow immediate socket reuse.
      int reuse_opt = 1;
      if (setsockopt(s->fd, SOL_SOCKET, SO_REUSEADDR, 
                     &reuse_opt, sizeof(reuse_opt)) < 0) {
         perror("setsockopt(SO_REUSEADDR)");
         cleanExit(1);
      }
   }

   if (bind(s->fd, (struct sockaddr *) &sa, sizeof(sa)) < 0) {
      printf("Failure binding to port %d: %s\n", port, strerror(errno));
      cleanExit(1);
   }

   if (listen(s->fd, 5)<0) {
      perror("listen()");
      cleanExit(1);
   }

   // Make it nonblocking.  Otherwise, accept can block even if select 
   // indicated that a new connection was present (if the connection died
   // before the call to accept())
   if (fcntl(s->fd, F_SETFL, O_NONBLOCK)<0) {
      perror("fcntl()");
      cleanExit(1);
   }

   sprintf(s->name, "%d", port);
   s->type = SERVER_SOCKET;
   s->format = format;
}

void createClientSocket(char *hostSpec, ioStream *s, char format) {
   char hostname[strlen(hostSpec)];
   int port = DEFAULT_SERVER_PORT;
   char *colon;
   struct hostent *he;
   struct sockaddr_in sa;

   printf("Connecting to %s\n", hostSpec);

   strcpy(hostname, hostSpec);

   colon = index(hostname, ':');
   if (colon != NULL) {
      *colon=0;
      if (!isNum(colon+1)) {
         printf("Invalid port number in argument: %s\n", hostSpec);
         cleanExit(1);
      }
      port=atoi(colon+1);
   }

   s->type = CLIENT_SOCKET;
   s->bePersistent = usePersistentSockets;
   s->format = format;
   strcpy(s->name, hostSpec);

   he = gethostbyname(hostname);
   if (he == NULL) {
      printf("Unknown host name: %s\n", hostname);
      cleanExit(1);
   }

   sa.sin_family=AF_INET;
   sa.sin_port=htons(port);
   memcpy(&(sa.sin_addr), he->h_addr, he->h_length);

   s->fd=socket(PF_INET, SOCK_STREAM, 0);
   if (s->fd < 0) {
      perror("socket()");
      cleanExit(1);
   }
   if (connect(s->fd, (struct sockaddr *) &sa, sizeof(sa)) < 0) {
      printf("Failure connecting to %s: %s\n", hostSpec, strerror(errno));
      deleteStream(s);
   }

   if ((format == SF_FORMAT)&&(sniffer==0)) {
      char buf[2];

      write(s->fd, SF_VERSION_STRING, 2);
      read(s->fd, buf, 2);
   }
}

int isDigit(char c) {
   return ((c >= '0') && (c <= '9'));
}

void createStreams(int argc, char* argv[]) {
   int i,j;
   char serial_format = FRAMED_FORMAT;
   char socket_format = ASCII_FORMAT;
   int moteType=MICA;

   // no arguments, assume DEFAULT_COM and DEFAULT_SERVER_PORT
   if (argc == 1) {
      createComm(DEFAULT_COM, createStreamStruct(), FRAMED_FORMAT, MICA);
      createServerSocket(DEFAULT_SERVER_PORT, createStreamStruct(), ASCII_FORMAT);
      return;
   }

   // create a streams record for each item on the command line, which can be
   // COMn, hostname, hostname:port, port
   for (i=1; i<argc; i++) {
//      printf("Handling argument: %s\n", argv[i]);

      for (j=0; j < strlen(argv[i]); j++)
        argv[i][j]=toupper(argv[i][j]);
//      printf("%s \n",argv[i]);

      ioStream *newStream = createStreamStruct();

      if ((strcmp(argv[i], "-V")==0) || (strcmp(argv[i], "--VERSION")==0)) {
         printf("Version: %s\n", VERSION);
         exit(0);
      } else if ((strcmp(argv[i], "-H")==0) || 
                 (strcmp(argv[i], "--HELP")==0)) {
         printUsage(argv[0]);
         cleanExit(0);
      } else if ((argv[i][0] == '-') && isDigit(argv[i][1]) && 
                 (argv[i][2] == '\0')) {
         moteType = argv[i][1] - '0';
         if ((moteType == 0) || (moteType > MAX_MOTE_TYPE)) {
            printf("Bad mote type\n");
            printUsage(argv[0]);
            cleanExit(0);
         }
      } else if (strcmp(argv[i], "-SF")==0) {
         socket_format = SF_FORMAT;
      } else if (switchToRawUnframedIO(argv[i])) {
         serial_format = RAW_FORMAT;
         socket_format = RAW_FORMAT;
      } else if (strcmp(argv[i], "-SNIFFER")==0){
         sniffer=1;
         wines_ptr=0;
      } else if (strcmp(argv[i], "-NOHB")==0){
         exclude_heartbeat=1;
      } else if (filenameForCommPort(argv[i]) != NULL) {  // is it COMn?
         createComm(argv[i], newStream, serial_format, moteType);
      } else if (isNum(argv[i])) {  // is it a server port number?
         createServerSocket(atoi(argv[i]), newStream, socket_format);
      } else {  // assume it's a host or host:port
         client=1;
         createClientSocket(argv[i], newStream, socket_format);
      }
   }
}

void signalHandler(int signum) {
   switch (signum) {
      case SIGHUP:
         printf("Exiting on SIGHUP\n");
         break;
      case SIGINT:
         printf("Exiting on SIGINT\n");
         break;
      case SIGTERM:
         printf("Exiting on SIGTERM\n");
         break;
      default:
         printf("Exiting on signal %d\n", signum);
   }

   cleanExit(1);
}

void setupSignalHandler() {
   signal(SIGHUP, &signalHandler);
   signal(SIGINT, &signalHandler);
   signal(SIGTERM, &signalHandler);
}

void doPersistenceCheck() {
   int i;

   for (i=0; i<numStreams; i++) {
      if ((streams[i].fd == -1) && (streams[i].type == CLIENT_SOCKET)) {
         createClientSocket(streams[i].name, &(streams[i]), 
                            streams[i].format);
      }
   }
}

void *start_TCP_server(){
    int socket_in, new_connection, one;
    unsigned int sinlen;
    struct sockaddr_in sockin;
    int listening_port=3000;

    printf("Process uses default listening port: %d\n", listening_port);
    if ( (socket_in = socket(PF_INET, SOCK_STREAM, 0 ) ) < 0) { /* create socket*/
        printf("error on socket\n");  /* socket error */
        return NULL;
    }

    sockin.sin_family = AF_INET;              /*set address family to Internet */
    sockin.sin_port = htons(listening_port);  /* set port no. */
    sockin.sin_addr.s_addr = INADDR_ANY; /* set IP addr to any interface */

    one = 1;

    setsockopt(socket_in, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));

    if (bind(socket_in, (struct sockaddr *)&sockin, sizeof(struct sockaddr_in) ) < 0 ) {
        printf("error on bind %d\n", errno); return NULL;  /* bind error */
    }
    if (listen(socket_in, 5)) {
       printf ("error on listen\n"); /* listen error*/
       return NULL;
    }
    printf("Launching the main cycle for serving socket connections.\n");
    sinlen = sizeof(sockin);
    while (1) {
          /* accepting new connection request from client,
           *            socket id for the new connection is returned in t */
      while (fw_socketStream->type!=FW_SOCKET_CLOSE){sleep(1); }; //wait for the descriptor to be freed
      
      if ( (new_connection = accept(socket_in, (struct sockaddr *) &sockin, &sinlen) ) < 0 ) {
         printf("error on accept %d\n",new_connection);  /* accept error */
         return NULL;
      }
      printf("new connection was accepted and a thread spawned.\n");
      printf( "Spawning a new connection from %s: %d.\n",inet_ntoa(sockin.sin_addr), ntohs(sockin.sin_port) );
      fw_socketStream->fd=new_connection;
      fw_socketStream->type=FW_SOCKET_OPEN;

      while (fw_socketStream->type!=FW_SOCKET_CLOSE){ sleep(1); };
         /* close connection, clean up sockets */
      if (close(new_connection) < 0) {
         printf("error on close");
         return NULL;
      } else {
        printf("Connection closed\n");
      }
    } // not reach below
    if (close(listening_port) < 0) {
       printf("close");
    }
}

int main(int argc, char* argv[]) {
   uint64_t lastPersistenceCheck = currentTimeMillis();
   pthread_t mythread;

   
   fw_socketStream=createStreamStruct();
   printf("Stream created %d\n",numStreams);
   fw_socketStream->type=FW_SOCKET_CLOSE;  
   fw_socketStream->bePersistent=1;
   
 if ( pthread_create( &mythread,  NULL, start_TCP_server, NULL) ) {
    printf("error creating thread.");
    abort();
  }

  memset(wines_network,0, sizeof(wines_network));
  memset(wines_buf,0,sizeof(wines_buf));
 
  setupSignalHandler();

  createStreams(argc, argv);

  while (1) {
     uint64_t time = currentTimeMillis();
     if ((time - lastPersistenceCheck) > PERSISTENCE_CHECK_PERIOD) {
        doPersistenceCheck();
        time = lastPersistenceCheck = currentTimeMillis();
     }
     waitForInputs(PERSISTENCE_CHECK_PERIOD - (time - lastPersistenceCheck));
  }
  if ( pthread_join ( mythread, NULL ) ) {
     printf("error joining thread.");
     abort();
   }
   return 0;
}