Portable emergency button with localization (LinkIt One)

#include <LGPS.h>
#include <LGSM.h>
#include <LBattery.h>

gpsSentenceInfoStruct info;
char buff[256];
char url[120];
int fix=0;
int Ledpingreen = 12;
int Ledpinred = 10;
int Ledpinyellow = 11;
int valor=0;

const int noofDest = 2;
const char *numbers[] = {"+351XXXXXXXX"}; // numbers (+XXXXXXXXXX,+XXXXXXXXX)
char incoming_num[21] = "";
char message[256];

int panicButtonPin = 2;

static unsigned char getComma(unsigned char num,const char *str)
{
  unsigned char i,j = 0;
  int len=strlen(str);
  for(i = 0;i < len;i ++)
  {
     if(str[i] == ',')
      j++;
     if(j == num)
      return i + 1; 
  }
  return 0; 
}

static double getDoubleNumber(const char *s)
{
  char buf[10];
  unsigned char i;
  double rev;
  
  i=getComma(1, s);
  i = i - 1;
  strncpy(buf, s, i);
  buf[i] = 0;
  rev=atof(buf);
  return rev; 
}

static double getIntNumber(const char *s)
{
  char buf[10];
  unsigned char i;
  double rev;
  
  i=getComma(1, s);
  i = i - 1;
  strncpy(buf, s, i);
  buf[i] = 0;
  rev=atoi(buf);
  return rev; 
}

float toWGS84(float nmeaCoord) {
  float degWhole = float(int(nmeaCoord/100));
  float degDec = (nmeaCoord - degWhole*100)/60;
  return degWhole + degDec;  
}

void parseGPGGA(const char* GPGGAstr)
{
  
  double latitude;
  double longitude;
  int tmp, hour, minute, second, num;
  
  if(GPGGAstr[0] == '$')  {
    //latitude
    tmp = getComma(2, GPGGAstr);
    latitude = getDoubleNumber(&GPGGAstr[tmp]);
    tmp = getComma(3, GPGGAstr); // N or S
    float latWGS84 = toWGS84(latitude);
    if('S' == GPGGAstr[tmp]) {
      latWGS84 = -latWGS84; // negate if southern hemisphere coordinate
    }
    //longitude
    tmp = getComma(4, GPGGAstr);
    longitude = getDoubleNumber(&GPGGAstr[tmp]);
    tmp = getComma(5, GPGGAstr); // E or W
    float lonWGS84 = toWGS84(longitude);
    if('W' == GPGGAstr[tmp]) {
      lonWGS84 = -lonWGS84; // negate if western coordinate
    }
    
    sprintf(url,"http://maps.google.com/?q=%f,%f", latWGS84, lonWGS84);

    // location fix
    tmp = getComma(6, GPGGAstr);
    fix = getIntNumber(&GPGGAstr[tmp]);
  } else {
    Serial.println("GPS Data empty"); 
  }
}

void send_sms(char* number, char* message) {
  LSMS.beginSMS(number);
  for(int i = 0; i < strlen(message); i++) {
    LSMS.write(message[i]);
  }
  LSMS.endSMS();
}

void setup() {
  pinMode(panicButtonPin, INPUT_PULLUP);
  pinMode(Ledpingreen, OUTPUT);
  pinMode(Ledpinyellow, OUTPUT);
  pinMode(Ledpinred, OUTPUT);
  Serial.begin(115200);
  LGPS.powerOn();
  Serial.println("GPS turning on, wait ...");
  delay(3000);
}

void loop() {
    valor=100;
    valor= LBattery.level();
  if (valor==100){
    digitalWrite(Ledpingreen, HIGH);
     digitalWrite(Ledpinyellow, LOW);
     digitalWrite(Ledpinred, LOW);
     delay(500);
     digitalWrite(Ledpingreen, LOW);
     
  }
  else if (valor==66) {
    digitalWrite(Ledpingreen, LOW);
     digitalWrite(Ledpinyellow, HIGH);
     digitalWrite(Ledpinred, LOW);
     delay(500);
     digitalWrite(Ledpinyellow, LOW);
  }
  else{
    digitalWrite(Ledpingreen, LOW);
     digitalWrite(Ledpinyellow, LOW);
     digitalWrite(Ledpinred, HIGH);
     delay(500);
     digitalWrite(Ledpinred, LOW);
  }
  LGPS.getData(&info);
  Serial.print((char*)info.GPGGA);
  parseGPGGA((const char*)info.GPGGA);

  if(digitalRead(panicButtonPin) == LOW) {
    if(fix != 0) {
      sprintf(message, "I need help. I'm here: %s", url);
    } else {
      sprintf(message, "I need help. My last location was here: %s", url);
    }
    Serial.println(message);
    for(int i=0; i < noofDest; i++) {
      char num[20];
      sprintf(num, numbers[i]);
      send_sms(num, message);
    }
    delay(5000);
  }
  
  if(LSMS.available()) {
    LSMS.remoteNumber(incoming_num, 20);
    if(fix != 0) {
      sprintf(message, "My Location: %s", url);
    } else {
      sprintf(message, "Last Location: %s", url);
    }
    Serial.println(message);
    send_sms(incoming_num, message);
    LSMS.flush();
  }
}