 
 void setup()
 {
-  uView.begin();	// begin of MicroView  
+  uView.begin();	// begin of MicroView
   uView.setFontType(0);
   uView.clear(ALL);	// erase hardware memory inside the OLED controller
-  
+
   C64Serial.begin(C64_BAUD);
   WifiSerial.begin(WIFI_BAUD);
- 
+
+  C64Serial.setTimeout(1000);
+
   pinMode(RTS, INPUT);
-  
+
   display2("Wi-Fi \nInit...");
-  
+
   boolean ok = wifly.begin(&WifiSerial, &C64Serial);
-  
+
   if (ok)
   {
-     display2("READY.");
+    display2("READY.");
   }
   else
   {
     display2("Wi-Fi Failed!");
   }
 }
-  
+
 void loop()
-{  
-   C64Serial.println();
-   C64Serial.println("Commodore 64 Wi-Fi Modem");
-   C64Serial.println("Telnet Test");
-   ShowStats();   
-   C64Serial.print("\nhost: ");
-   C64Serial.setTimeout(1000);
-   
-   String readString = GetInput();
-   readString.trim();
-         
-   if (readString.length() > 0)
-   {
-     lastHost = readString;
-     Telnet(readString);
-   }
-   else
-   {
-      Telnet(lastHost);
-   }
+{
+  C64Serial.println();
+  C64Serial.println(F("Commodore 64 Wi-Fi Modem\n"));
+  ShowStats();
+
+  while (true)
+  {
+    C64Serial.println(F("\n1. Telnet to host or BBS"));
+    C64Serial.println(F("2. Wait for incoming connection"));
+    C64Serial.println(F("3. Terminal Mode (direct to WiFly)"));
+    int option = ReadByte(C64Serial);
+
+    switch (option)
+    {
+      case '1': doTelnet();
+        return;
+
+      case '2': Incoming();
+        return;
+
+      case '3': RawTerminalMode();
+        return;
+
+      default: C64Serial.println(F("Unknown option, try again"));
+        continue;
+    }
+  }
+}
+
+void doTelnet()
+{
+  int port = lastPort;
+
+  C64Serial.print("\nTelnet host: ");
+  String hostName = GetInput();
+  hostName.trim();
+
+  if (hostName.length() > 0)
+  {
+    C64Serial.print("\nPort (23): ");
+    String strport = GetInput();
+    strport.trim();
+
+    if (strport.length() > 0)
+    {
+      port = strport.toInt();
+    }
+    else
+    {
+      port = lastPort;
+    }
+
+    lastHost = hostName;
+    lastPort = port;
+
+    Telnet(hostName, port);
+  }
+  else
+  {
+    Telnet(lastHost, lastPort);
+  }
 }
 
 // ----------------------------------------------------------
 
 void display(String message)
 {
-   uView.clear(PAGE);	// erase the memory buffer, when next uView.display() is called, the OLED will be cleared.
-   uView.setCursor(0,0);  
-   uView.println(message);
-   uView.display();
+  uView.clear(PAGE);	// erase the memory buffer, when next uView.display() is called, the OLED will be cleared.
+  uView.setCursor(0, 0);
+  uView.println(message);
+  uView.display();
 }
 
 
 void display2(String message)
 {
-   display(message);
-   C64Serial.println(message);
-}
-
-// ----------------------------------------------------------
-
-String GetInput() 
+  display(message);
+  C64Serial.println(message);
+}
+
+// ----------------------------------------------------------
+
+String GetInput()
 {
   char in_char[100];
-  int max_length=sizeof(in_char);
-  
-  int i=0; // Input buffer pointer
+  int max_length = sizeof(in_char);
+
+  int i = 0; // Input buffer pointer
   char key;
-  
-  while(true) 
-  {
-      key=ReadByte(C64Serial); // Read in one character
-      in_char[i]=key;      
-      C64Serial.write(key); // Echo key press back to the user.
-      
-      if ((key=='\b')&&(i>0)) i-=2; // Handles back space.
-      
-      if (((int)key==13)||(i==max_length-1)) 
-      { // The \n represents enter key.
-        in_char[i]=0; // Terminate the string with 0.
-        return String(in_char);
-      }
-      i++;
-      if (i<0) i=0;
+
+  while (true)
+  {
+    key = ReadByte(C64Serial); // Read in one character
+    in_char[i] = key;
+    C64Serial.write(key); // Echo key press back to the user.
+
+    if ((key == '\b') && (i > 0)) i -= 2; // Handles back space.
+
+    if (((int)key == 13) || (i == max_length - 1))
+    { // The \n represents enter key.
+      in_char[i] = 0; // Terminate the string with 0.
+      return String(in_char);
+    }
+    i++;
+    if (i < 0) i = 0;
   }
 }
 

  wifly.write(IAC);                           // send character 255 (start negotiation)
  wifly.write(254);                           // send character 254 (dont)
  wifly.write(34);                            // linemode
  
  wifly.write(IAC);                           // send character 255 (start negotiation)
  wifly.write(254);                           // send character 253 (do)
  wifly.write(1);                             // echo

#define DCD  A2
#define CTS  A1
#define DSR  A0
#define TxD  6
#define RxD  5

SoftwareSerial C64Serial(RxD, TxD);
HardwareSerial& WifiSerial = Serial;
WiFly wifly;

// Telnet Stuff
#define IAC 255

String lastHost = "";
int lastPort = 23;

// ----------------------------------------------------------

