GSM MQTT+CMT(SMS Reading)

Question

I'm working a code to pubsub over GSM SIM800L and ARDUINO MEGA.

I'm able to workout MQTT fine, using ElementzOnline / SIM800_MQTT code.

I'm now having trouble to use MQTT alongside SMSReading (+CMT) and Call Receiving. I've made a RecSMS() to receive incoming messages, and it works fine as long as TCP Connection is not true, The problem is: Once TCP/MQTT is connected, my RecSMS Function displays no more sms to Serial and calls can not be received

This is Call Receiving, which is handled via RingPing....

void GSM_MQTT::receivecall(){
  HangUp=1;
  if(MQTT.TCP_Flag==true){disconnect();}
  delay(1000);
  SIMSerial.write("AT+CGATT=0\n");
  delay(1000);
  //SIMSerial.write("AT+CIPSHUT\n");
  modemStatus=0;
  SIMSerial.write("ATA\n");SIMSerial.write(26);Monitor("Incomig Call");
}
void GSM_MQTT::endcall(){if(HangUp==1){HangUp=0;SIMSerial.write("ATH\n");SIMSerial.write(26);Monitor("Call Ended");}}

This is RecSMS():

char inchar4;char inchar3;char inchar2;char inchar1;int times=0;
void RecSMS(char data){
  if(times==3){inchar4=data;times++;}
  if(times==2){inchar3=data;times++;}
  if(times==1){inchar2=data;times++;}
  if(times==0){inchar1=data;times++;}
  if(times>=4){
    inchar1=inchar2;inchar2=inchar3;inchar3=inchar4;inchar4=data;
    if((inchar1== '+') && (inchar2== 'C')&&(inchar3== 'M')&&(inchar4== 'T')){RcvdConf = 1;}
    if(RcvdConf == 1){
      if(data == '\n'){RcvdEnd++;}
      if(RcvdEnd == 3){RcvdEnd = 0;}
      RcvdMsg[count] = data;
      count++;
      if(RcvdEnd == 2){RcvdConf = 0;MsgLength = count-2;count= 0;}
      if(RcvdConf == 0){
        Serial.print("Mobile Number is: ");
        for(int x = 4;x < 18;x++){MsgMob[x-4] = RcvdMsg[x];Serial.print(MsgMob[x-4]);}
        Serial.println();
        Serial.print("Message Text: ");
        for(int x = 47; x < MsgLength; x++){MsgTxt[x-47] = RcvdMsg[x];Serial.print(MsgTxt[x-47]);}
        Serial.println();
        char RcvdMsg[200] = "";int RcvdConf = 0;int count = 0;int RcvdEnd = 0;char MsgMob[15];char MsgTxt[50];int MsgLength = 0;
      }
    }
  }
}

SerialEvent() is called every second...

void serialEvent(){
  while(Serial3.available()){
    char inChar=(char)Serial3.read();
    RecSMS(inChar);//<////////////////////////sms reading
    if(MQTT.TCP_Flag==false){
      if(MQTT.index<200){MQTT.inputString[MQTT.index++]=inChar;}
      if(inChar=='\n'){
        MQTT.inputString[MQTT.index]=0;stringComplete=true;Serial.print(MQTT.inputString);
        if(strstr(MQTT.inputString,MQTT.reply)!=NULL){
          MQTT.GSM_ReplyFlag=1;
          if(strstr(MQTT.inputString," INITIAL")!=0){MQTT.GSM_ReplyFlag=2;}
          else if(strstr(MQTT.inputString," START")!=0){MQTT.GSM_ReplyFlag=3;}
          else if(strstr(MQTT.inputString," IP CONFIG")!=0){_delay_us(10);MQTT.GSM_ReplyFlag=4;}
          else if(strstr(MQTT.inputString," GPRSACT")!=0){MQTT.GSM_ReplyFlag=4;}
          else if((strstr(MQTT.inputString," STATUS")!=0)||(strstr(MQTT.inputString,"TCP CLOSED")!=0)){MQTT.GSM_ReplyFlag=5;}
          else if(strstr(MQTT.inputString," TCP CONNECTING")!=0){MQTT.GSM_ReplyFlag=6;}
          else if((strstr(MQTT.inputString," CONNECT OK")!=0)||(strstr(MQTT.inputString,"CONNECT FAIL")!=NULL)||(strstr(MQTT.inputString,"PDP DEACT")!=0)){MQTT.GSM_ReplyFlag=7;}}
        else if(strstr(MQTT.inputString,"OK")!=NULL){GSM_Response=1;}
        else if(strstr(MQTT.inputString,"ERROR")!=NULL){GSM_Response=2;}
        else if(strstr(MQTT.inputString,".")!= NULL){GSM_Response=3;}
        else if(strstr(MQTT.inputString,"CONNECT FAIL")!=NULL){GSM_Response=5;}
        else if(strstr(MQTT.inputString,"CONNECT")!=NULL){GSM_Response=4;MQTT.TCP_Flag=true;MQTT.AutoConnect();MQTT.pingFlag=true;MQTT.tcpATerrorcount=0;Serial.println("MQTT.TCP_Flag=True");}
        else if(strstr(MQTT.inputString,"CLOSED")!=NULL){GSM_Response=4;MQTT.TCP_Flag=false;MQTT.MQTT_Flag=false;Serial.println("TCP_Flag=False");}
        MQTT.index=0;MQTT.inputString[0]=0;
      }
    }else{
      uint8_t ReceivedMessageType=(inChar/16) & 0x0F;uint8_t DUP=(inChar & DUP_Mask)/DUP_Mask;
      uint8_t QoS=(inChar & QoS_Mask)/QoS_Scale;uint8_t RETAIN=(inChar & RETAIN_Mask);
      if((ReceivedMessageType>=CONNECT)&&(ReceivedMessageType<=DISCONNECT)){
        bool NextLengthByte=true;MQTT.length=0;MQTT.lengthLocal=0;uint32_t multiplier=1;delay(2);char Cchar=inChar;
        while((NextLengthByte==true)&&(MQTT.TCP_Flag==true)){
          if(Serial3.available()){
            inChar=(char)Serial3.read();/*Serial.println(inChar, DEC);*/
            if(((((Cchar & 0xFF)=='C')&&((inChar & 0xFF)=='L'))||(((Cchar & 0xFF)=='+')&&((inChar & 0xFF)=='P')))&&(MQTT.length==0)){
              MQTT.index=0;MQTT.inputString[MQTT.index++]=Cchar;MQTT.inputString[MQTT.index++]=inChar;
              MQTT.TCP_Flag=false;MQTT.MQTT_Flag=false;MQTT.pingFlag=false;Serial.println("Disconnecting");
              Serial.println("MQTT.TCP_Flag=False (ln223)");
            }else{
              if((inChar&128)==128){MQTT.length+=(inChar&127)*multiplier;multiplier*=128;Serial.println("More");}
              else{NextLengthByte=false;MQTT.length+=(inChar&127)*multiplier;multiplier*=128;}
            }
          }
        }
        MQTT.lengthLocal=MQTT.length;/*Serial.println(MQTT.length);*/ 
        if(MQTT.TCP_Flag==true){
          MQTT.printMessageType(ReceivedMessageType);MQTT.index=0L;uint32_t a=0;
          while((MQTT.length-- > 0)&&(Serial3.available())){MQTT.inputString[uint32_t(MQTT.index++)]=(char)Serial3.read();delay(1);}
          /*Serial.println(" ");*/  
          if (ReceivedMessageType==CONNACK){
            MQTT.ConnectionAcknowledgement=MQTT.inputString[0]*256+MQTT.inputString[1];
            if(MQTT.ConnectionAcknowledgement==0){MQTT.MQTT_Flag=true;MQTT.OnConnect();}
            MQTT.printConnectAck(MQTT.ConnectionAcknowledgement); /*MQTT.OnConnect();*/
          }else if(ReceivedMessageType==PUBLISH){
            uint32_t TopicLength=(MQTT.inputString[0])*256+(MQTT.inputString[1]);Serial.print("Topic: '");MQTT.PublishIndex = 0;
            for(uint32_t iter=2;iter<TopicLength+2;iter++){Serial.print(MQTT.inputString[iter]);MQTT.Topic[MQTT.PublishIndex++]=MQTT.inputString[iter];}
            MQTT.Topic[MQTT.PublishIndex]=0;Serial.print("' Message: '");
            MQTT.TopicLength=MQTT.PublishIndex;MQTT.PublishIndex=0;uint32_t MessageSTART=TopicLength+2UL;int MessageID=0;
            if(QoS!=0){MessageSTART+=2;MessageID=MQTT.inputString[TopicLength+2UL]*256+MQTT.inputString[TopicLength+3UL];}
            for(uint32_t iter=(MessageSTART);iter<(MQTT.lengthLocal);iter++){Serial.print(MQTT.inputString[iter]);MQTT.Message[MQTT.PublishIndex++]=MQTT.inputString[iter];}
            MQTT.Message[MQTT.PublishIndex]=0;Serial.println("'");MQTT.MessageLength=MQTT.PublishIndex;
            if(QoS==1){MQTT.publishACK(MessageID);}else if(QoS==2){MQTT.publishREC(MessageID);}
            MQTT.OnMessage(MQTT.Topic,MQTT.TopicLength,MQTT.Message,MQTT.MessageLength);MQTT.MessageFlag=true;
          }else if(ReceivedMessageType==PUBREC){MQTT.publishREL(0,MQTT.inputString[0]*256+MQTT.inputString[1]);
            int MessageID=MQTT.inputString[0]*256+MQTT.inputString[1];
          }else if(ReceivedMessageType==PUBREL){MQTT.publishCOMP(MQTT.inputString[0]*256+MQTT.inputString[1]);
            int MessageID=MQTT.inputString[0]*256+MQTT.inputString[1];
          }else if((ReceivedMessageType==PUBACK)||(ReceivedMessageType==PUBCOMP)||(ReceivedMessageType==SUBACK)||(ReceivedMessageType==UNSUBACK)){
            int MessageID=MQTT.inputString[0]*256+MQTT.inputString[1];
          }else if(ReceivedMessageType==PINGREQ){
            MQTT.TCP_Flag=false;MQTT.pingFlag=false;MQTT.sendATreply("AT+CIPSHUT\r\n",".",4000);MQTT.modemStatus=0;
            Serial.println("Disconnecting");Serial.println("MQTT.TCP_Flag=False (ln261)");
          }
        }
      }else if((inChar==13)||(inChar==10)){Serial.print("inChar=13||10");}else{Serial.print("Received: Unknown Message Type: ");Serial.println(inChar);}
    }
  }
}

My Code is published at pedromancuso/GSM_MQTT.

Suggestions?