#include "WiFlyUDP.h"
#include <Arduino.h>

boolean WiFlyUDP::findInResponse(const char *toMatch,
                                    unsigned int timeOut = 1000) {
  int byteRead;

  unsigned long timeOutTarget; // in milliseconds

  for (unsigned int offset = 0; offset < strlen(toMatch); offset++) {
    timeOutTarget = millis() + timeOut; // Doesn't handle timer wrapping
    while (!uart->available()) {
      // Wait, with optional time out.
      if (timeOut > 0) {
        if (millis() > timeOutTarget) {
          return false;
        }
      }
      delay(1); // This seems to improve reliability slightly
    }
    byteRead = uart->read();

    delay(1); // Removing logging may affect timing slightly

    if (byteRead != toMatch[offset]) {
      offset = 0;
      // Ignore character read if it's not a match for the start of the string
      if (byteRead != toMatch[offset]) {
        offset = -1;
      }
      continue;
    }
  }

  return true;
}

void WiFlyUDP::skipRemainderOfResponse() {
    while (!(uart->available() && (uart->read() == '\n'))) {
      // Skip remainder of response
    }
}

boolean WiFlyUDP::sendCommand(const __FlashStringHelper *command,
                                 boolean isMultipartCommand = false,
                                 const char *expectedResponse = "AOK") {
  uart->print(command);
  delay(20);
  if (!isMultipartCommand) {
    uart->flush();
    uart->println();

    // TODO: Handle other responses
    //       (e.g. autoconnect message before it's turned off,
    //        DHCP messages, and/or ERR etc)
    if (!findInResponse(expectedResponse, 3000)) {
      return false;
    }
    //findInResponse(expectedResponse);
  }
  return true;
}

boolean WiFlyUDP::sendCommand(const char *command,
                                 boolean isMultipartCommand = false,
                                 const char *expectedResponse = "AOK") {
  uart->print(command);
  delay(20);
  if (!isMultipartCommand) {
    uart->flush();
    uart->println();

    // TODO: Handle other responses
    //       (e.g. autoconnect message before it's turned off,
    //        DHCP messages, and/or ERR etc)
    if (!findInResponse(expectedResponse, 3000)) {
      return false;
    }
    //findInResponse(expectedResponse);
  }
  return true;
}

#define COMMAND_MODE_ENTER_RETRY_ATTEMPTS 2

#define COMMAND_MODE_GUARD_TIME 250 // in milliseconds

boolean WiFlyUDP::enterCommandMode() {
  for (int retryCount = 0; retryCount < COMMAND_MODE_ENTER_RETRY_ATTEMPTS; retryCount++) 
   {
    delay(COMMAND_MODE_GUARD_TIME);
    uart->print(F("$$$"));
    delay(COMMAND_MODE_GUARD_TIME);
    uart->println();
    uart->println();
    uart->println(F("ver"));
    if (findInResponse("\r\nWiFly Ver", 1000)) return true;
  }
  return false;
}


#define SOFTWARE_REBOOT_RETRY_ATTEMPTS 2

boolean WiFlyUDP::reboot() {
  for (int retryCount = 0; retryCount < SOFTWARE_REBOOT_RETRY_ATTEMPTS; retryCount++) 
    {   
      enterCommandMode();
      uart->println(F("reboot"));
      if (findInResponse("*READY*", 2000)) {
        return true;
      }
    }
  return false;
}

#define SOFTWARE_RESET_RETRY_ATTEMPTS 2

boolean WiFlyUDP::reset() {
  for (int retryCount = 0; retryCount < SOFTWARE_RESET_RETRY_ATTEMPTS; retryCount++) 
    { 
      enterCommandMode();
      if (sendCommand(F("factory RESET"), false, "Set Factory Defaults")) {
        return true;
      }
    }
  return false;
}

void WiFlyUDP::setConfiguration(char* ssid, char* pass, char* ip_host, uint16_t remote_Port, 
                                uint16_t local_Port) {
  if (enterCommandMode())
  {
    // TODO: Handle configuration better
    // Turn off auto-connect
  
    sendCommand(F("set wlan phrase "), true);
    sendCommand(pass);
    sendCommand(F("set wlan ssid "), true);
    sendCommand(ssid);
      
    sendCommand(F("set ip proto 1"));
      
    sendCommand(F("set ip host "), true); //(ip del host)
    sendCommand(ip_host);
      
    sendCommand(F("set ip remote "), true); 
    uart->print(remote_Port);
    sendCommand("");
      
    sendCommand(F("set ip local "), true);
    uart->print(local_Port);
    sendCommand("");

    sendCommand(F("set ip dhcp 1"));
    sendCommand(F("set wlan auth 4"));
    //sendCommand(F("set wlan auth 8")); // WEP64
    //sendCommand(F("set wlan key 6C61757261")); // WEP64 
    
    sendCommand(F("set comm remote 0"));
    sendCommand(F("set t z 1"));  //Zona horaria
    sendCommand(F("set time address 129.6.15.28")); //Servidor de reloj
    sendCommand(F("set time port 123")); //Puerto del servidor
    sendCommand(F("set t e 15")); //Toma hora cada 15 min
  }
}

boolean WiFlyUDP::baudrate(long baudrate_temp)
{
  if (enterCommandMode())
  {
    sendCommand(F("set uart baudrate "), true);
    uart->print(baudrate_temp);
    sendCommand("");
    //delay(100);
    uart->println(F("save"));
    delay(100);
    reboot();
    //uart->println("exit");
    return true;
  }
 return false;
} 

WiFlyUDP::WiFlyUDP(Stream* newUart) {
  uart = newUart;
}

void WiFlyUDP::begin(char* ssid, char* pass, char* ip_host, uint16_t remote_Port, 
uint16_t local_Port) {
	reboot();
        setConfiguration(ssid, pass, ip_host, remote_Port, local_Port);
}

boolean WiFlyUDP::join() {
  if (sendCommand(F("join"), false, "Associated!")) {
    skipRemainderOfResponse();
    return true;
  }
  return false;
}



#define IP_ADDRESS_BUFFER_SIZE 16 // "255.255.255.255\0"

const char * WiFlyUDP::ip() {
  /*

    The return value is intended to be dropped directly
    into calls to 'print' or 'println' style methods.

   */
  static char ip[IP_ADDRESS_BUFFER_SIZE] = "";

  // TODO: Ensure we're not in a connection?

  if (enterCommandMode())
  {
      // Version 2.19 of the WiFly firmware has a "get ip a" command but
      // we can't use it because we want to work with 2.18 too.
      if (sendCommand(F("get ip"), false, "IP="))
      {
          char newChar;
          byte offset = 0;
    
          // Copy the IP address from the response into our buffer
          while (offset < IP_ADDRESS_BUFFER_SIZE) {
             newChar = uart->read();
      
            if (newChar == ':') {
              ip[offset] = '\x00';
              break;
            } 
            else if (newChar != -1) {
              ip[offset] = newChar;
              offset++;
            }
          }
          ip[IP_ADDRESS_BUFFER_SIZE-1] = '\x00';
        
          findInResponse("> ");
        
          uart->println("exit");
          //sendCommand("exit", false, "EXIT");
      }        
  }
  return ip;
}

byte WiFlyUDP::scan(network_results *network) {
  char num = '0';
  char i = 0;
  static char net[5] = { 0x0D, 0x0A, 0x20, 0x31, '\x00'};
  if (enterCommandMode())
    {
      if (sendCommand(F("scan"), false, "Found "))
      {
        num = uart->read();
        if ((num > '0')&&(num <= '9'))
        {
          while ((i+'0') < num) 
            {
              net[3]=i+1+'0';
              if (findInResponse(net)) 
              {
                char newChar;
                byte offset = 0;
                newChar = uart->read();
                while (((newChar == -1)||(newChar == ' '))) newChar = uart->read();
                while (offset < SCAN_SSID_BUFFER_SIZE) {
                    if (newChar == ' ') {
                      network -> ssid[i][offset] = '\x00';
                      break;
                    } 
                    else if ((newChar != -1)) {
                          network -> ssid[i][offset] = newChar;
                          offset++;
                    }
                    newChar = uart->read();
                 }
                network -> ssid[i][SCAN_SSID_BUFFER_SIZE-1] = '\x00';
              }
              i++;
           }
         }
       }
       uart->println("exit");
    }   
  if ((num > '0')&&(num <= '9')) num = num-'0';
  else num = 0;
  
  return (num);
}