/*
 Pi_Serial_test.cpp - SerialProtocol library - demo
 Copyright (c) 2014 NicoHood.  All right reserved.
 Program to test serial communication

 Compile with:
 sudo gcc -o Pi_Serial_Test.o Pi_Serial_Test.cpp -lwiringPi -DRaspberryPi -pedantic -Wall
 sudo ./Pi_Serial_Test.o
 */

// just that the Arduino IDE doesnt compile these files.
#ifdef RaspberryPi

//include system librarys
#include <stdio.h> //for printf
#include <stdint.h> //uint8_t definitions
#include <stdlib.h> //for exit(int);
#include <string.h> //for errno
#include <errno.h> //error output
#include <stdarg.h>       // va_*


//wiring Pi
#include <wiringPi.h>
#include <wiringSerial.h>

// Find Serial device on Raspberry with ~ls /dev/tty*
// ARDUINO_UNO "/dev/ttyACM0"
// FTDI_PROGRAMMER "/dev/ttyUSB0"
// HARDWARE_UART "/dev/ttyAMA0"
char device[]= "/dev/ttyAMA0";
// filedescriptor
int fd;
unsigned long baud = 115200;
unsigned long time=0;

//prototypes
int main(void);
void loop(void);
void setup(void);

void setup(){

  printf("%s \n", "Raspberry Startup!");
  fflush(stdout);

  //get filedescriptor
  if ((fd = serialOpen (device, baud)) < 0){
    fprintf (stderr, "Unable to open serial device: %s\n", strerror (errno)) ;
    exit(1); //error
  }

  //setup GPIO in wiringPi mode
  if (wiringPiSetup () == -1){
    fprintf (stdout, "Unable to start wiringPi: %s\n", strerror (errno)) ;
    exit(1); //error
  }

}

char* concat(int count, ...)
{
    va_list ap;
    int i;

    // Find required length to store merged string
    int len = 1; // room for NULL
    va_start(ap, count);
    for(i=0 ; i<count ; i++)
        len += strlen(va_arg(ap, char*));
    va_end(ap);

    // Allocate memory to concat strings
    char *merged = calloc(sizeof(char),len);
    int null_pos = 0;

    // Actually concatenate strings
    va_start(ap, count);
    for(i=0 ; i<count ; i++)
    {
        char *s = va_arg(ap, char*);
        strcpy(merged+null_pos, s);
        null_pos += strlen(s);
    }
    va_end(ap);

    return merged;
}

char array[20];
int count = 1;
int tagRSSI = 0;
int tagNumber =0;


void loop(){
  // Pong every 3 seconds
  if(millis()-time>=3000){
    serialPuts (fd, "Pong!\n");
    // you can also write data from 0-255
    // 65 is in ASCII 'A'
    serialPutchar (fd, 65);
    time=millis();
  }

  // read signal
  if(serialDataAvail (fd)){
	char newChar = serialGetchar (fd);

	if(count == 20 && (unsigned int)newChar == 68)
	{
		tagRSSI = (unsigned int)array[5];
		char* wow = concat(3,array[8],array[9],array[10]);
		int number = (int)strtol(wow, NULL, 16);
		printf("tagRSSI = %d, tagNumber = %d", tagRSSI, number);
		count = 1;
	}

	array[count] = newChar;
	//printf("%d\n", array[count]);
	count++;
    fflush(stdout);
  }

}

// main function for normal c++ programs on Raspberry
int main(){
  setup();
  while(1) loop();
  return 0;
}

#endif //#ifdef RaspberryPi