esp8266WiFi connect to wifi

Hi, I'm trying to program my esp8266 (NodeMCu v0.9) in the arduino IDE such that I can ask it what wifi networks it finds then ask it to connect to any of them with a password that I enter.

I have got it working to the point where it gives me the SSID's and then I can enter the password and it tries to connect, but it seems unable to connect to the network so I am wondering if I am handling the SSID and password variable incorrectly?

The following is all the relevant code, I begin by sending '1' in the serial monitor of the Arduino IDE, then I receive in return a list of the SSID's that are available.
I then send '2' followed by the index of the SSID I wish to connect to, followed by the password, example: 20PASSWORD to connect to the first SSID in the list.

The device then attempts to connect but times out (connectWifi() returns 0 after 5 milliseconds as I have programmed it), I have tried to give it more time but it still returns 0 even after 50 seconds. I am printing the ssid and the password variables inside the connectWifi() function before attempting to connect and I see my routers SSID and its correct password printed in the serial monitor.

Any advice on how to handle this would be highly appreciated!

Regards David

#include <ESP8266WiFi.h>

#define CONNECT_MAX_WAIT 5000

char ssid[SSID_MAX_LEN];
char password[PASS_MAX_LEN];
char message[32];
bool Sflag = false;


int scanWifi()
{
 WiFi.mode(WIFI_STA); //Station mode (Not AP)
 WiFi.disconnect();   //Disconnect from any network
 int n = WiFi.scanNetworks();
 return n;
}


bool connectWifi(int SSIDnr)
{
 
 strncpy(ssid, WiFi.SSID(SSIDnr).c_str(), 32);
 Serial.println(ssid);
 Serial.println(password);
 
 WiFi.begin(ssid, password);
 long start_time = millis();
 while (WiFi.status() != WL_CONNECTED)
 {
   if (millis() - start_time > CONNECT_MAX_WAIT)
     return 0;
   delay(100);
 }
 return 1;
}


void loop()
{
 if (Serial.available() ) {
   int Sindex = 0;
   while (Serial.available()) 
     message[Sindex++] = Serial.read();
   Sflag = true;
   message[Sindex] = 0;
 }

 if (Sflag)
 {
   Sflag = false;

   if (message[0] == '1') //Requests SSID's
   {
     int n = scanWifi();
     for (int i = 0; i < n; i ++)
       Serial.println(WiFi.SSID(i));
   }

   if (message[0] == '2') //Receive SSIDnr and password for connection
   {
     int SSIDnr = message[1] - '0';
     for (int i = 2; i < 2 + PASS_MAX_LEN; i++)
       password[i-2] = message[i];
     Serial.print(connectWifi(SSIDnr));
   }

 }
}

Your code has been mangled by the forum software because you did not use code tags round it. Please edit your post, select the code then click on the code tags icon ( </> ) to add the tags. That way the code will not turn into italics when it includes [­i] as it does now

Thank you I have added the code tags!

It seems I have this problem: WiFi.status() always not connected. · Issue #2593 · esp8266/Arduino · GitHub
My ESP actually connects (Can see in router menu) but it doesn't recognize it in the correct way...