boolean Adafruit_FONA::enableGPS(boolean onoff) {
  uint16_t state;

  // first check if its already on or off

  if (_type == FONA808_V2) {
    if (! sendParseReply(F("AT+CGNSPWR?"), F("+CGNSPWR: "), &state) )
      return false;
  } else {
    if (! sendParseReply(F("AT+CGPSPWR?"), F("+CGPSPWR: "), &state))
      return false;
  }

  if (onoff && !state) {
    if (_type == FONA808_V2) {
      if (! sendCheckReply(F("AT+CGNSPWR=1"), F("OK")))  // try GNS command
	return false;
    } else {
      if (! sendCheckReply(F("AT+CGPSPWR=1"), F("OK")))
	return false;
    }
  } else if (!onoff && state) {
    if (_type == FONA808_V2) {
      if (! sendCheckReply(F("AT+CGNSPWR=0"), F("OK"))) // try GNS command
	return false;
    } else {
      if (! sendCheckReply(F("AT+CGPSPWR=0"), F("OK")))
	return false;
    }
  }
  return true;
}

bool Adafruit_FONA::HTTP_initialize(char *url) {
  // Handle any pending
  HTTP_terminate();

  // Initialize and set parameters
  if (! sendCheckReply(F("AT+HTTPINIT"), F("OK")))
    return false;
  if (! sendCheckReply(F("AT+HTTPPARA=\"CID\",1"), F("OK")))
    return false;
  if (! sendCheckReplyQuoted(F("AT+HTTPPARA=\"UA\","), useragent, F("OK"), 10000))
    return false;

  flushInput();
  mySerial->print(F("AT+HTTPPARA=\"URL\",\""));
  mySerial->print(url);
  mySerial->println("\"");
  readline(FONA_DEFAULT_TIMEOUT_MS);
  if (strcmp(replybuffer, "OK") != 0)
    return false;

  // HTTPS redirect
  if (httpsredirect) {
    if (! sendCheckReply(F("AT+HTTPPARA=\"REDIR\",1"), F("OK")))
      return false;

    if (! sendCheckReply(F("AT+HTTPSSL=1"), F("OK")))
      return false;
  }

  return true;
}

bool Adafruit_FONA::enableNTPTimeSync(bool onoff, const char *ntpserver) {
    if (onoff) {
        if (! sendCheckReply(F("AT+CNTPCID=1"), F("OK")))
            return false;

    mySerial->print(F("AT+CNTP=\""));
    
    if (ntpserver != 0) {
        mySerial->print(ntpserver);
    } else {
        mySerial->print(F("pool.ntp.org"));
    }
    
    mySerial->println(F("\",0"));
    readline(FONA_DEFAULT_TIMEOUT_MS);
    
    if (strcmp(replybuffer, "OK") != 0)
        return false;

    if (! sendCheckReply(F("AT+CNTP"), F("OK"), 10000))
        return false;

    uint16_t status;
    readline(10000);
    
    if (! parseReply(F("+CNTP:"), &status))
        return false;
    } else {
        if (! sendCheckReply(F("AT+CNTPCID=0"), F("OK")))
            return false;
    }

    return true;
}

boolean Adafruit_FONA::begin(uint16_t baudrate) {
  pinMode(_rstpin, OUTPUT);
  digitalWrite(_rstpin, HIGH);
  delay(10);
  digitalWrite(_rstpin, LOW);
  delay(100);
  digitalWrite(_rstpin, HIGH);

  // give 3 seconds to reboot
  delay(3000);

  mySerial->begin(baudrate);
  delay(500);
  while (mySerial->available()) mySerial->read();

  sendCheckReply(F("AT"), F("OK"));
  delay(100);
  sendCheckReply(F("AT"), F("OK"));
  delay(100);
  sendCheckReply(F("AT"), F("OK"));
  delay(100);

  // turn off Echo!
  sendCheckReply(F("ATE0"), F("OK"));
  delay(100);
  
  if (! sendCheckReply(F("ATE0"), F("OK"))) {
    return false;
  }

  return true;
}

// Reading SMS's is a bit involved so we don't use helpers that may cause delays or debug
// printouts!
bool Adafruit_FONA::readSMS(uint8_t i, char *smsbuff, uint16_t maxlen, uint16_t *readlen) {
    // text mode
    if (! sendCheckReply(F("AT+CMGF=1"), F("OK"))) return false;
    
    // show all text mode parameters
    if (! sendCheckReply(F("AT+CSDH=1"), F("OK"))) return false;
    
    // parse out the SMS len
    uint16_t thesmslen = 0;
    
    //getReply(F("AT+CMGR="), i, 1000);  //  do not print debug!
    mySerial->print(F("AT+CMGR="));
    mySerial->println(i);
    readline(1000); // timeout
    
    // parse it out...
    parseReply(F("+CMGR:"), &thesmslen, ',', 11);
    
    readRaw(thesmslen);
    
    flushInput();
    
    uint16_t thelen = min(maxlen, strlen(replybuffer));
    strncpy(smsbuff, replybuffer, thelen);
    smsbuff[thelen] = 0; // end the string
    
    #ifdef ADAFRUIT_FONA_DEBUG
    Serial.println(replybuffer);
    #endif
    *readlen = thelen;
    return true;
}

boolean Adafruit_FONA::TCPconnect(char *server, uint16_t port) {
  flushInput();

  // close all old connections
  if (! sendCheckReply(F("AT+CIPSHUT"), F("SHUT OK"), 5000) ) return false;

  // single connection at a time
  if (! sendCheckReply(F("AT+CIPMUX=0"), F("OK")) ) return false;

  // manually read data
  if (! sendCheckReply(F("AT+CIPRXGET=1"), F("OK")) ) return false;

#ifdef ADAFRUIT_FONA_DEBUG
  Serial.print(F("AT+CIPSTART=\"TCP\",\""));
  Serial.print(server);
  Serial.print(F("\",\""));
  Serial.print(port);
  Serial.println(F("\""));
#endif

  mySerial->print(F("AT+CIPSTART=\"TCP\",\""));
  mySerial->print(server);
  mySerial->print(F("\",\""));
  mySerial->print(port);
  mySerial->println(F("\""));

  if (! expectReply(F("OK"))) return false;
  if (! expectReply(F("CONNECT OK"))) return false;
}

bool Adafruit_FONA::sendSMS(char *smsaddr, char *smsmsg) {
    if (! sendCheckReply("AT+CMGF=1", "OK")) return -1;
    
    char sendcmd[30] = "AT+CMGS=\"";
    strncpy(sendcmd+9, smsaddr, 30-9-2);  // 9 bytes beginning, 2 bytes for close quote + null
    sendcmd[strlen(sendcmd)] = '\"';
    
    if (! sendCheckReply(sendcmd, "> ")) return false;
    
    #ifdef ADAFRUIT_FONA_DEBUG
    Serial.print("> "); Serial.println(smsmsg);
    #endif
    mySerial->println(smsmsg);
    mySerial->println();
    mySerial->write(0x1A);
    #ifdef ADAFRUIT_FONA_DEBUG
    Serial.println("^Z");
    #endif
    
    readline(10000); // read the +CMGS reply, wait up to 10 seconds!!!
    //Serial.print("* "); Serial.println(replybuffer);
    if (strstr(replybuffer, "+CMGS") == 0) {
        return false;
    }
    
    readline(1000); // read OK
    //Serial.print("* "); Serial.println(replybuffer);
    
    if (strcmp(replybuffer, "OK") != 0) {
        return false;
    }
    
    return true;
}

boolean Adafruit_FONA_3G::enableGPRS(boolean onoff) {

  if (onoff) {
    // disconnect all sockets
    //sendCheckReply(F("AT+CIPSHUT"), F("SHUT OK"), 5000);

    if (! sendCheckReply(F("AT+CGATT=1"), F("OK"), 10000))
      return false;


    // set bearer profile access point name
    if (apn) {
      // Send command AT+CGSOCKCONT=1,"IP","<apn value>" where <apn value> is the configured APN name.
      if (! sendCheckReplyQuoted(F("AT+CGSOCKCONT=1,\"IP\","), apn, F("OK"), 10000))
        return false;

      // set username/password
      if (apnusername) {
	char authstring[100] = "AT+CGAUTH=1,1,\"";
	char *strp = authstring + strlen(authstring);
	strcpy_P(strp, (prog_char *)apnusername);
	strp+=strlen_P((prog_char *)apnusername);
	strp[0] = '\"';
	strp++;
	strp[0] = 0;

	if (apnpassword) {
	  strp[0] = ','; strp++;
	  strp[0] = '\"'; strp++;
	  strcpy_P(strp, (prog_char *)apnpassword);
	  strp+=strlen_P((prog_char *)apnpassword);
	  strp[0] = '\"';
	  strp++;
	  strp[0] = 0;
	}

	if (! sendCheckReply(authstring, "OK", 10000))
	  return false;
      }
    }

    // connect in transparent
    if (! sendCheckReply(F("AT+CIPMODE=1"), F("OK"), 10000))
      return false;
    // open network (?)
    if (! sendCheckReply(F("AT+NETOPEN=,,1"), F("Network opened"), 10000))
      return false;

    readline(); // eat 'OK'
  } else {
    // close GPRS context
    if (! sendCheckReply(F("AT+NETCLOSE"), F("Network closed"), 10000))
      return false;

    readline(); // eat 'OK'
  }

  return true;
}

boolean Adafruit_FONA::callerIdNotification(boolean enable, uint8_t interrupt) {
  if(enable){
    attachInterrupt(interrupt, onIncomingCall, FALLING);
    return sendCheckReply(F("AT+CLIP=1"), F("OK"));
  }

  detachInterrupt(interrupt);
  return sendCheckReply(F("AT+CLIP=0"), F("OK"));
}

bool Adafruit_FONA::FMradio(bool onoff, uint8_t a) {
    if (! onoff) {
        return sendCheckReply(F("AT+FMCLOSE"), F("OK"));
    }
    
    // 0 is headset, 1 is external audio
    if (a > 1) return false;
    
    return sendCheckReply(F("AT+FMOPEN="), a, F("OK"));
}

int FONA808::connect(const char* apn, const char* user, const char* password, mbed::util::FunctionPointer0<void> connectionCallback){
bool in = init();
if(!in)
  {
    return -1;
  }
 if( !m_ipInit )
  {
    m_ipInit = true;
    m_ppp.init();
  }
  m_ppp.setup(user, password, DEFAULT_MSISDN_GSM);
  
  DEBUG_PRINT("Birra\n");
  
/*
  if (m_onePort)
  {
     m_smsInit = false; //SMS status reset
     m_ussdInit = false; //USSD status reset
     m_linkMonitorInit = false; //Link monitor status reset
  }*/

  if(apn != NULL)
  {
    char cmd[48];
    int tries = 30;
    sendCheckReply("ATH","OK",500);
    sprintf(cmd, "AT+CGDCONT=1,\"IP\",\"%s\"", apn);
   /* do //Try 30 times because for some reasons it can fail *a lot* with the K3772-Z dongle
    {
      ret = m_at.executeSimple(cmd, &result);
      DBG("Result of command: Err code=%d", ret);
      if(ret)
      {
        Thread::wait(500);
      }
    } while(ret && --tries);*/
    if(!sendCheckReply(cmd,"OK",500)){
	 //printf("ATResult: AT return=%d (code %d)", result.result, result.code);
         DEBUG_PRINT("APN set to %s", apn);
    }
   
  }

  //Connect
  DEBUG_PRINT("Connecting PPP");

  int ret = m_ppp.connect(connectionCallback); //TODO impostare callback per connessione
  //printf("Result of connect: Err code=%d", ret);
  return ret;
}

bool Adafruit_FONA::enableNetworkTimeSync(bool onoff) {
    if (onoff) {
        if (! sendCheckReply(F("AT+CLTS=1"), F("OK")))
            return false;
    } else {
        if (! sendCheckReply(F("AT+CLTS=0"), F("OK")))
        return false;
    }
    
    flushInput(); // eat any 'Unsolicted Result Code'
    
    return true;
}

// Retrieve the sender of the specified SMS message and copy it as a string to
// the sender buffer.  Up to senderlen characters of the sender will be copied
// and a null terminator will be added if less than senderlen charactesr are
// copied to the result.  Returns true if a result was successfully retrieved,
// otherwise false.
bool Adafruit_FONA::getSMSSender(uint8_t i, char *sender, int senderlen) {
    // Ensure text mode and all text mode parameters are sent.
    if (! sendCheckReply(F("AT+CMGF=1"), F("OK"))) return false;
    if (! sendCheckReply(F("AT+CSDH=1"), F("OK"))) return false;
    // Send command to retrieve SMS message and parse a line of response.
    mySerial->print(F("AT+CMGR="));
    mySerial->println(i);
    readline(1000);
    // Parse the second field in the response.
    bool result = parseReplyQuoted(F("+CMGR:"), sender, senderlen, ',', 1);
    // Drop any remaining data from the response.
    flushInput();
    return result;
}

void FONA808::readGPSInfoSecond(){
	DEBUG_PRINT("ReadGPS Second\n");
	mSerial.cleanBuffer();
	float lat=0, latitude;
	float lon=0, longitude;
	float elevation=0;
	if(checkGPSFix()){
		mSerial.cleanBuffer();
		sendParseReplyGPS(&lat, &lon, &elevation, this->timestamp, 500);
		float degrees = floor(lat / 100);
	    	double minutes = lat - (100 * degrees);
	    	minutes /= 60;
	    	degrees += minutes;
	    	latitude = degrees;
	    	
	    	degrees = floor(lon / 100);
	    	minutes = lon - (100 * degrees);
	    	minutes /= 60;
	    	degrees += minutes;
		longitude = degrees;
		DEBUG_PRINT("\nRAW Latitude: %d, Longitude=%d\n",(int)lat, (int)lon);
		minar::Scheduler::postCallback(gpsReadCallback.bind(latitude, longitude, elevation));
	}else{
		minar::Scheduler::postCallback(gpsNoFixCallback.bind());
	}
	
	if(m_ppp.isPPPLinkOpen()){
		sendCheckReply("ATO","CONNECT",500);
		mSerial.cleanBuffer();
		mSerial.setPppPause(false);
 	}

}

bool Adafruit_FONA::HTTP_POST_start(char *url,
              const char *contenttype,
              const uint8_t *postdata, uint16_t postdatalen,
              uint16_t *status, uint16_t *datalen){
  if (! HTTP_initialize(url))
    return false;

  if (! sendCheckReplyQuoted(F("AT+HTTPPARA=\"CONTENT\","), contenttype, F("OK"), 10000))
    return false;

  // HTTP POST data
  flushInput();
  mySerial->print(F("AT+HTTPDATA="));
  mySerial->print(postdatalen);
  mySerial->println(",10000");
  readline(FONA_DEFAULT_TIMEOUT_MS);
  if (strcmp(replybuffer, "DOWNLOAD") != 0)
    return false;

  mySerial->write(postdata, postdatalen);
  readline(10000);
  if (strcmp(replybuffer, "OK") != 0)
    return false;

  // HTTP POST
  if (! sendCheckReply(F("AT+HTTPACTION=1"), F("OK")))
    return false;

  if (! HTTP_response(status, datalen))
    return false;

  return true;
}

bool Adafruit_FONA::setAutoAnswer(int ringCount){
    char szCmd[12];
    
    sprintf(szCmd, "ATS0=%d", ringCount);
    
    return sendCheckReply(F(szCmd), F("OK"));
}

boolean Adafruit_FONA::TCPconnected(void) {
  if (! sendCheckReply(F("AT+CIPSTATUS"), F("OK"), 100) ) return false;
  readline(100);
#ifdef ADAFRUIT_FONA_DEBUG
  Serial.print (F("\t<--- ")); Serial.println(replybuffer);
#endif
  return (strcmp(replybuffer, "STATE: CONNECT OK") == 0);
}

boolean Adafruit_FONA::playDTMF(char dtmf) {
  char str[4];
  str[0] = '\"';
  str[1] = dtmf;
  str[2] = '\"';
  str[3] = 0;
  return sendCheckReply(F("AT+CLDTMF=3,"), str, F("OK"));
}

boolean Adafruit_FONA::enableGPRS(boolean onoff) {

  if (onoff) {
    if (! sendCheckReply(F("AT+CGATT=1"), F("OK"), 10000))
      return false;

    // set bearer profile! connection type GPRS
    if (! sendCheckReply(F("AT+SAPBR=3,1,\"CONTYPE\",\"GPRS\""),
			   F("OK"), 10000))
      return false;

    // set bearer profile access point name
    if (apn) {
      // Send command AT+SAPBR=3,1,"APN","<apn value>" where <apn value> is the configured APN value.
      if (! sendCheckReplyQuoted(F("AT+SAPBR=3,1,\"APN\","), apn, F("OK"), 10000))
        return false;

      // set username/password
      if (apnusername) {
        // Send command AT+SAPBR=3,1,"USER","<user>" where <user> is the configured APN username.
        if (! sendCheckReplyQuoted(F("AT+SAPBR=3,1,\"USER\","), apnusername, F("OK"), 10000))
          return false;
      }
      if (apnpassword) {
        // Send command AT+SAPBR=3,1,"PWD","<password>" where <password> is the configured APN password.
        if (! sendCheckReplyQuoted(F("AT+SAPBR=3,1,\"PWD\","), apnpassword, F("OK"), 10000))
          return false;
      }
    }

    // open GPRS context
    if (! sendCheckReply(F("AT+SAPBR=1,1"), F("OK"), 10000))
      return false;
  } else {
    // close GPRS context
    if (! sendCheckReply(F("AT+SAPBR=0,1"), F("OK"), 10000))
      return false;

    if (! sendCheckReply(F("AT+CGATT=0"), F("OK"), 10000))
      return false;

  }
  return true;
}

int8_t Adafruit_FONA::getNumSMS(void) {
    uint16_t numsms;

    if (! sendCheckReply(F("AT+CMGF=1"), F("OK"))) return -1;
    // ask how many sms are stored

    if (! sendParseReply(F("AT+CPMS?"), F("+CPMS: \"SM_P\","), &numsms) ) return -1;

    return numsms;
}