Arduino GPS/GSM receiver (Uno+A7 Elecrow) - string

Question

I'm trying to make a GPS/GSM receiver with an Arduino Uno. My program is trying to ana]yse the incoming strings with a few ifs. Then I want to convert them to save them on an SD card, or send them as SMS.
This is the correct information from the GPS:
+GPSRD:
$GPGGA,151420.000,5222.61362,N,01658.51086,E,1,03,8.9,0.0,M,,M,,0000*44
$GPRMC,151420.000,A,5222.61362,N,01658.51086,E,0.46,160.58,201017,,,A*67
$GPVTG,160.58,T,,M,0.46,N,0.86,K,A*3B
The line I need is the third one, because there are coordinates. I want to save them. So I made some functions. I'm trying also to navigate the whole device by SMS commands. Everything is in the code. Here is the code:
#include <SoftwareSerial.h>
#include <SPI.h>
#include <SD.h>
#include <string.h>
#define rxPin 7
#define txPin 8
String message="lalala", callingnumber="";
String response="";
String coords;
String data[13]={"","","","","","","","","","","","",""};
String thetime=(""), date=(""), latitude=(""), longitude=("");
SoftwareSerial mySerial(rxPin, txPin);
void setup() {
Serial.begin(115200);
mySerial.begin(19200);
while (!Serial) {
}
digitalWrite(3, HIGH);
delay(2000);
digitalWrite(3, LOW);
for(int i = 0; i < 8; i++) {
mySerial.println("AT");
Serial.println("AT");
delay(1000);
}
mySerial.println("");
delay(1000);
mySerial.println("Jestem gotowy!");
delay(1000);
mySerial.println("s - lokalizacja / g - wlacz gps / o - wylacz gps");
delay(1000);
Serial.println("AT+GPS=1");
delay(1000);
Serial.println("AT+CMGF=1");
delay(1000);
while (Serial.available()) {
mySerial.write(Serial.read());
}
}
void loop() {
RecData();
}
void RecData() {
int firstindex, lastindex;
Serial.setTimeout(1000);
response = Serial.readString();
firstindex = response.indexOf("$GPRMC");
lastindex = response.indexOf("$GPVTG");
if (response.indexOf("+CMT: ") > 0) {
int numberindex = response.indexOf("+48");
if (response.indexOf("Start") > 0) {
callingnumber = response.substring(numberindex, numberindex+12);
SendSMS("wlaczono");
OpenGPS();
}
}
if (response.indexOf("+CMT: ") > 0) {
int numberindex = response.indexOf("+48");
if (response.indexOf("Stop") > 0) {
callingnumber = response.substring(numberindex, numberindex+12);
SendSMS("zatrzymano");
CloseGPS();
}
}
if (response.indexOf("+CMT: ") > 0) {
int numberindex = response.indexOf("+48");
if (response.indexOf("Sms") > 0) {
callingnumber = response.substring(numberindex, numberindex+12);
SendSMS(message);
}
}
int a = 0;
for (int i=firstindex; i<lastindex; i++) {
mySerial.print(response[i]);
if(response[i] == ',') {
a++;
} else {
data[a] += response[i];
}
}
mySerial.print(firstindex);
mySerial.print("::");
mySerial.print(lastindex);
mySerial.println(" loop is working");
date = data[9];
thetime = data[1];
latitude = data[3] + data[4];
longitude = data[5] + data[6];
message = date.substring(0,2) + "." + date.substring(2,4) + "." + date.substring(4,6) + " " + thetime.substring(0,2) + ":" + thetime.substring(2,4) + ":" + thetime.substring(4,6) + " coords: " + latitude.substring(0,2) + "'" + latitude.substring(2,4) + "''" + latitude.substring(5,10) + "'''" + latitude.substring(10,11)+ " " + longitude.substring(1,3) + "'" + longitude.substring(3,5) + "''" + longitude.substring(6,11) + "'''" + longitude.substring(11,12);
for(int ii = 0; ii < 13; ii++) {
mySerial.print(data[ii]);
data[ii] = "";
//czyszczenie na szybko
}
if (firstindex < 0 && lastindex < 0) {
mySerial.write("test");
mySerial.print(response);
mySerial.print(response.length());
}
}
void OpenGPS() {
mySerial.println("wlaczam gps");
delay(500);
Serial.println("AT+GPSRD=1");
delay(500);
mySerial.println("Wlaczono");
}
void CloseGPS() {
mySerial.println("Wylaczam GPS");
delay(500);
Serial.println("AT+GPSRD=0");
}
void SignalQuality() {
mySerial.print("Jakosc sygnalu: ");
delay(1000);
Serial.println("AT+CSQ");
}
void SendSMS(String msg) {
mySerial.println("wysylam...");
delay(500);
Serial.println("AT+CMGS="+callingnumber);
delay(500);
Serial.print(msg);
delay(500);
Serial.println((char)26);
delay(500);
mySerial.println("wyslano");
}
I'm looking for some help, because it doesn't work well. Here is the screenshot of the executed program:
The response is not good. There was some text conversion to get 13 strings of message.
Please help, if I missed any information I can edit the post.
Thanks.