// Use to send ASCII command to a remote XBee
// C. Pham

/*
 * Send remote AT command or message to an XBee
 * XBeeSendCmd -at -mac 0013A20040922070 "ATCH"
 * XBeeSendCmd -at -net 1234 "ATCH"
 * XBeeSendCmd -mac 0013A20040922070 "HELLO"
 * XBeeSendCmd -mac 0013A20040922070 "Z35#F500#"
 */

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

#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>

#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_at_cmd=false;
bool send_mac=false;
bool send_net=false;
bool send_broadcast=false;
bool send_libelium=false;
bool send_DM=false;
bool send_tinyos=false;
uint8_t tinyos_AM_ID=0x05;
bool send_wsize=false;
bool send_wflowid=false;
int flowid=0;
int customSize=0;
int pkt_count=1;
int inter_pkt=1000;

struct termios opciones;

int speed = B38400;
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 (customSpeed)  {
       struct serial_struct sstruct;

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

       if (ioctl(sd, TIOCGSERIAL, &sstruct) < 0) {
           printf("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) {
           printf("Error: could not set custom comm baud divisor\n");
           exit(-1);
       }

       printf("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) {
            printf("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_msg(char* dest_address, char* msg_to_send)
{
   char aux[2];
   uint8_t destination[8];
   uint8_t checksum = 0;
   uint8_t it, pos;
   uint8_t command[MAX_API_FRAME_LENGTH];
   uint8_t response[MAX_API_FRAME_LENGTH];
   int k;

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

   fprintf(stderr, "Building 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;

   // API command for remote AT command
   if (send_at_cmd) {
       // don't count the "AT" characters, ATMM02 gives "MM" 0x02 so 3 bytes instead of 4
       command[2] = 2 + (strlen(msg_to_send)-4)/2  +13;
       command[3] = 0x17; // send remote AT request, work with 802.15.4 and DigiMesh
   }
   else
       if (send_net) {
           command[2] = strlen(msg_to_send)+5;
           command[3] = 0x01; // 16-bit address transmit request
       }
       else {
           if (send_DM) {
               command[2] = strlen(msg_to_send)+14;
               command[3] = 0x10; // 64-bit address transmit request for DigiMesh protocol
           }
           else {
                command[2] = strlen(msg_to_send)+11;
                command[3] = 0x00; // 64-bit address transmit request
           }
       }

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

   if (command[3] == 0x17 || 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_at_cmd || 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 && !send_at_cmd) {
       // set broadcast radius to 0
       command[pos++]=0x00;
   }

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

   // option byte. valid for both transmit request and remote AT request in which case an ATAC cmd should be sent
   command[pos++]=0x00;

   if (send_at_cmd) {
       // the AT command to be performed remotely, skip the "AT" characters
       command[pos++]=msg_to_send[2];
       command[pos++]=msg_to_send[3];

       int k=4;

       while (msg_to_send[k]!='\0') {
            // always need 2 hex digit in ASCII format: e.g. 03 for 0x03
            command[pos++]=str2hex((char*)&msg_to_send[k]);
            k+=2;
       }

   }
   else {

           // 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++] = 1; // 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];
           }

           // Tinyos API header
           if (!send_libelium && send_tinyos) {
               // add 2 extra bytes
               command[2] = command[2]+2;
               command[pos++] = 0x3F; // TinyOS 6LowPan network ID
               command[pos++] = tinyos_AM_ID; // the tinyos application AM ID
           }

           // with flow id
           if (send_wflowid) {
               // add 2 extra bytes
               command[2] = command[2]+2;
               command[pos++] = 0xFF; // special flag
               command[pos++] = 0x50+flowid; // the flow id + 0x50
           }

           // set the payload
           for (it=0; it<strlen(msg_to_send); it++)
               command[pos++]=msg_to_send[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;

   uint8_t tmp;

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

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

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

   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");


   for (k=0; k<pkt_count; k++)  {

           fprintf(stderr, "S%d.", k);
	   // now, we really send it to the radio
	   // write the start delimiter
	   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);
		        tmp = tmp ^ 0x20;
		}
		write(sd, &tmp, 1);
		usleep(100);
	   }

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

           if (pkt_count>1)
		usleep(inter_pkt*1000);
   }

   fprintf(stderr, "\n");

   // sleeping here is important, the radio could have some strange behavior otherwise
   usleep(20000);
   //sleep(1);
   //tcflush(sd, TCOFLUSH);
   //usleep(5000);

   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;
   bool time_is_up=false;

   if (send_at_cmd) {
   	fprintf(stderr, "Polling for answer 3s max.\n");
   }
   else {
        fprintf(stderr, "Polling for answer 1ms max.\n");
   }

   timespec now1, now2;

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

   if (send_at_cmd)
	time_is_up = (diff(now1, now2).tv_sec < 3.0)? false:true;
   else
   	time_is_up = (diff(now1, now2).tv_nsec < 1000000)? false:true;

   //15ms=15000000ns
   while( !stop_reading && !time_is_up ) {

        // 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);
                fprintf(stderr, "%.2x ", (uint8_t)c);

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

                response[i++]=(uint8_t)c;

                // length LSB
                read(sd,&c,1);
                fprintf(stderr, "%.2x ", (uint8_t)c);

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

                response[i++]=(uint8_t)c;

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

                // if neither an remote AT command response or a transmis status (0x89 or 0x8B for DM), skip all following data until next frame
                // 0x97 is the remote AT command response for both 802.15.4 and DigiMesh
                if (c!=(char)0x97 && 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;
                i++;
            }
            else
                i--;
        }
        clock_gettime(CLOCK_REALTIME, &now2);

   	if (send_at_cmd)
		time_is_up = (diff(now1, now2).tv_sec < 3.0)? false:true;
   	else
   		time_is_up = (diff(now1, now2).tv_nsec < 1000000)? false:true;
   }


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

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

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

       fprintf(stderr, "\n");

       // the remote AT command response API code
       if (response[3]==0x97) {

            fprintf(stderr, "Response to remote AT cmd %c%c is: ", response[15], response[16]);

            if (response[17]==0) {

                fprintf(stderr, "OK ");

                for (it=0; it < (response[2]-15); it++)
                    fprintf(stderr,"Ox%.2x ", response[18+it]);

                fprintf(stderr, "\n");
                return 0;
            }

            if (response[17]==1) {
                fprintf(stderr, "Error\n");
                return 1;
            }

            if (response[17]==2) {
                fprintf(stderr, "Invalid command\n");
                return 1;
            }

            if (response[17]==3) {
                fprintf(stderr, "Invalid parameter\n");
                return 1;
            }

            if (response[17]==4) {
                fprintf(stderr, "No response\n");
                return 1;
            }
       }

       //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");
                return 0;
            }

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

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

            if (response[5]==3) {
                fprintf(stderr, "Purged\n");
                return 1;
            }
       }

       //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");
                return 0;
            }

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

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

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

            if (response[5]==0x25) {
                fprintf(stderr, "Route not found\n");
                return 1;
            }
       }

       fprintf(stderr, "Error when getting the response\n");
   }
   return 1;
}


void * printERROR(char *argv[])
{
   fprintf(stderr, "USAGE:\t%s -baud baudrate -p dev [-L][-DM][-at] -tinyos -tinyos_amid id_hex -flowid id -mac|-net|-addr|-b -size s -n n -t t\n", argv[0]);
   fprintf(stderr, "USAGE:\t-baud, set baud rate, default is 38400\n");
   fprintf(stderr, "USAGE:\t-p /dev/ttyUSB1, set setial port, default is /dev/ttyUSB0\n");
   fprintf(stderr, "USAGE:\t-L, insert Libelium API header for WaspMote under full Libelium API\n");
   fprintf(stderr, "USAGE:\t-DM, specify DigiMesh firmware\n");
   fprintf(stderr, "USAGE:\t-at, send remote AT command: -at -mac ""0013a2004069165d"" ""ATMM""\n");
   fprintf(stderr, "USAGE:\t-tinyos, forge a TinyOS ActiveMessage compatible packet (0x3F0x05 are inserted)\n");
   fprintf(stderr, "USAGE:\t-tinyos_amid 6F, set the ActiveMessage identifier to 0x6F (0x05 is the default)\n");
   fprintf(stderr, "USAGE:\t-flowid 1, set flow id to 1 (0xFF0x51 are inserted)\n");
   fprintf(stderr, "USAGE:\t-mac 0013a2004069165d, set 64-bit dest. MAC address\n");
   fprintf(stderr, "USAGE:\t-net 5678, set 16-bit dest. address\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, use broadcast\n");
   fprintf(stderr, "USAGE:\t-size 50, set packet size to 50 bytes\n");
   fprintf(stderr, "USAGE:\t-n 10, send 10 packets\n");
   fprintf(stderr, "USAGE:\t-t 500, set 500ms between each packet\n");
   return 0;
}


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

   char* dest_address;
   char* msg_to_send;
   int k;

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

   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], "-at"))
       {
          send_at_cmd=true;
       }

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

       if (!strcmp(argv[i], "-DM"))
       {
          send_DM=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], "-tinyos"))
       {
          send_tinyos=true;
          send_libelium=false;
       }

       if (!strcmp(argv[i], "-tinyos_amid"))
       {
          send_tinyos=true;
          send_libelium=false;
          tinyos_AM_ID = str2hex(argv[i+1]);
       }

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

	  if (flowid>9)
            flowid=0;
       }

       if (!strcmp(argv[i], "-size"))
       {
          send_wsize=true;
          customSize=atoi(argv[i+1]);

          if (customSize>100)
                customSize=100;
       }

       if (!strcmp(argv[i], "-n"))
       {
          pkt_count=atoi(argv[i+1]);

          if (pkt_count>100)
                pkt_count=100;
       }

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

          if (inter_pkt<10)
                inter_pkt=10;
       }
   }

   init_serial();
   tcflush(sd, TCIOFLUSH);

   if (send_wsize) {
        msg_to_send = (char*)calloc(customSize+1, sizeof(char));

        for (int i=0; i<customSize; i++)
            msg_to_send[i]='*';

        msg_to_send[customSize]='\0';
   }
   else
        msg_to_send=argv[argc-1];

   send_msg(dest_address, msg_to_send);


   if (send_wsize)
       delete msg_to_send;

   close(sd);
   exit(0);
}