 
 int ReadByte(Stream& in)
 {
-    while (in.available() == 0) {}   
-    return in.read();
+  while (in.available() == 0) {}
+  return in.read();
 }
 
 int PeekByte(Stream& in)
 {
-    while (in.available() == 0) {}   
-    return in.peek();
-}
-
+  while (in.available() == 0) {}
+  return in.peek();
+}
+

  char mac[20];
  char ip[20];
  char ssid[20];
  
  wifly.getMAC(mac,20);
  wifly.getIP(ip,20);  
  wifly.getSSID(ssid,20);  
  
  C64Serial.print("\nMAC Address: ");   C64Serial.println(mac); 
  C64Serial.print("IP Address:  ");   C64Serial.println(ip); 
  C64Serial.print("Wi-Fi SSID:  ");   C64Serial.println(ssid); 
}
  


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

void Telnet(String host)
{
   sprintf(temp, "Connecting to %s", host.c_str() );   
   display2(temp);
   
   wifly.setIpProtocol(WIFLY_PROTOCOL_TCP);
   
   boolean ok = wifly.open(host.c_str(), 23);
   
   if (ok)
   {
      sprintf(temp, "Connected to %s", host.c_str() );   
      display2(temp); 
   }
   else
   {
     display2("Connect \nFailed!");
     return;
   }
   
   TerminalMode(); 
}

void TerminalMode()
{ 
   C64Serial.println("Determining Connection Type\n"); 
   CheckTelnet();
   C64Serial.println("*** Entering Terminal Mode ***\n");
  
   while (wifly.available() != -1) // -1 means closed
   {
     while (wifly.available() > 0)
     {
         C64Serial.write( wifly.read() );
     }
     
     while (C64Serial.available() > 0)
     {
        wifly.write( C64Serial.read() );
     }
     
     // Alternate check
     if (!wifly.isConnected())
     {
        break;
     }
   }
   
    wifly.close();
    display2("Connection closed");
}

void CheckTelnet()     //  inquiry host for telnet parameters / negotiate telnet parameters with host
{
  int inpint, verbint, optint;                         //    telnet parameters as integers
  
  // Wait for first character
  inpint = PeekByte(wifly);  
  if (inpint != IAC)  
  {
    C64Serial.println("Raw TCP Connection Detected");
    return;   // Not a telnet session
  }
  
  C64Serial.println("Telnet Connection Detected");
  
  // IAC handling
  SendTelnetParameters();    // Start off with negotiating
  
  while (true)
  {
    inpint = PeekByte(wifly);       //      peek at next character
     
    if (inpint != IAC)  
    {
      return;   // Let Terminal mode handle character
    }
    
    inpint = ReadByte(wifly);    // Eat IAC character    
    verbint = ReadByte(wifly);   // receive negotiation verb character        
    if (verbint == - 1) continue;                //          if negotiation verb character is nothing break the routine  (should never happen)
        
    switch (verbint) {                             //          evaluate negotiation verb character
        case IAC:                                    //          if negotiation verb character is 255 (restart negotiation)
          continue;                                     //            break the routine
          
        case 251:                                    //          if negotiation verb character is 251 (will) or
        case 252:                                    //          if negotiation verb character is 252 (wont) or
        case 253:                                    //          if negotiation verb character is 253 (do) or
        case 254:                                    //          if negotiation verb character is 254 (dont)
          optint = ReadByte(wifly);                  //            receive negotiation option character         
          if (optint == -1) continue;                   //            if negotiation option character is nothing break the routine  (should never happen) 
                  
          switch (optint) {            
            
            case 3:                                   // if negotiation option character is 3 (suppress-go-ahead)
              SendTelnetDoWill(verbint, optint);
              break;
              
          default:                                     //        if negotiation option character is none of the above (all others)
              SendTelnetDontWont(verbint, optint);
            break;                                     //          break the routine
      }
    }
  } 
}

void SendTelnetDoWill(int verbint, int optint)

#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;

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

