 int motor_2_output;
 int motor_3_output;
 int motor_4_output;
+int array_length=1;//To store array_length
 bool blinkState = false;
 bool dmpReady = false;
 uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU




void parse_gps(char buffer[60]);

//----------------------------Setup Code-----------------------------------------------//
void setup() {
	pinMode(MOTOR_1_PIN, OUTPUT);

//----------------------------Main Loop-----------------------------------------------//
void loop() {
  // put your main code here, to run repeatedly:
  char* gpsbuffer;
// Initialize buffer for gps data
  Serial.println("Try to communicate");
  Serial.println(gpsSerial.available());
 	if (gpsSerial.available() > 0&&gpsSerial.peek()=='$'){
   gpsbuffer=dynamic_array_Serial(gpsSerial,100);
   Serial.println(array_length);
   //Serial.print(gpsbuffer[0]);
   /* while (!(gpsSerial.findUntil("$GPRMC,",'*'))){
      gpsSerial.flush();
    }
    gpsSerial.readBytesUntil("*",gpsbuffer,60);// Problem, if it is N, the function will try to find enough bytes to fill the memory it is given

    }//Cannot read Bytes, just skip this if-clause, need to be solved
    //used findUntil method to skip other info
  */
  //How to return length???
  for(int i;i<array_length-1;i++){
  	Serial.print(gpsbuffer[i]);
   
    //gpsbuffer[i]=" ";
  }
 	}
  else{
    Serial.println("Fail to communicate!");
  }
 
  //Serial.println();
//	For Testing only
  array_length=1;
  delay(1000);
  gpsSerial.flush();
  free(gpsbuffer);
  Serial.println("Flushing serial");
  

}
char* dynamic_array_Serial(SoftwareSerial src,int max_length){
  src.listen();
  extern int array_length;
  Serial.println("Successful");
  //Serial.println(array_length); Testing
  if(src.available()>0){
    Serial.println("Checking IO buffer");
  //Serial.println(src.peek());
    Serial.println("Initializing buffer");
  char*  max_array=(char*)malloc(sizeof(char)*max_length);
  for(int i=0;i<max_length;i++){
    max_array[i]=NULL;
  }
  Serial.println("Start reading");
  while(!(src.available()>0));//Waiting for bytes to fill the IO buffer
  while(src.available()>0){//Cant go into this loop
    char temp=src.read();
    delay(20);
    if(temp!='*'){
      max_array[array_length-1]=temp;
      array_length++;
      //Serial.println(max_array[array_length-1]);
    }
    else{
      Serial.print("Array length: ");
      Serial.println(array_length);
      char* result=(char*)malloc(sizeof(char)*array_length);
      for(int i=0;i<max_length;i++){
        if(max_array[i]==NULL){
          free(max_array);//free memory
          Serial.println("End listening");
          //Serial.println(result[0]);
          return result;
        }
        else{
          result[i]=max_array[i];
          //Serial.print(max_array[i]);
          //Serial.print(":");
          //Serial.println(result[i]);
        }
      }
    }
  }
  }
  

 else{
    return NULL;
  }
}

