#include "redFlyClient.hpp"

RedFlyClient client;
byte ip[4];

void debugout(const char* fmt, ...) {
  RedFly.disable();
  char buffer[100];
  va_list args;
  va_start(args,fmt);
  vsprintf(buffer, fmt, args);
  Serial.write(buffer);
  va_end(args);
  RedFly.enable();
}


int8_t extractIpAddress(char *sourceString, byte *ipAddress) {
  byte len=0;
  char buf[5];
  byte cnt=0,cnt1=0,i;

  len=strlen(sourceString);
  for(i=0;i<len;i++)
  {
    if(sourceString[i]!='.' && cnt < 4){
      buf[cnt++] =sourceString[i];
    }
    if((sourceString[i]=='.' || i==len-1) && cnt1 < 4){
      buf[cnt]='\0';
      cnt=0;
      ipAddress[cnt1++]=atoi(buf);
    }
  }
  if(cnt1 != 4) {
    return 1;
  }
  return 0;
}

int8_t connect() {
  //init the WiFi module on the shield
  // ret = RedFly.init(br, pwr) //br=9600|19200|38400|57600|115200|200000|230400, pwr=LOW_POWER|MED_POWER|HIGH_POWER

  wdt_reset();
  {
    size_t len = strnlen(config.ssid, sizeof(config.ssid));
    if(len == 0 || len == sizeof(config.ssid)) {
      debugout("CONF ERR\n");
      return 1;
    }
  }
  RedFly.scan();

  wdt_reset();
  matrix.bytes[20] ^= 0x0F;

  if(RedFly.join(config.ssid, config.pass, INFRASTRUCTURE)) {
    debugout("JOIN ERR\n");
    return 2;
  }

  wdt_reset();
  matrix.bytes[21] ^= 0x0F;

  if(RedFly.begin()) {
    debugout("BEGIN ERR\n");
    RedFly.disconnect();
    return 3;
  }

  wdt_reset();
  matrix.bytes[22] ^= 0x0F;

  if(extractIpAddress(config.host, ip) != 0) {
    if(RedFly.getip(config.host, ip) != 0) {
      debugout("DNS ERR\n");
      return 4;
    }
  }

  wdt_reset();
  matrix.bytes[23] ^= 0x0F;

  client.connectUDP(ip, config.port, 2021);
  client.write('C');
  return 0;
}

void ping() {
  if(client.connected()) {
    client.write('A');
  }
}

int8_t readUDP() {
  uint16_t len = client.available();
  uint8_t buf[4];
 	
  wdt_reset();

  if(!client.connected()) {
    debugout("DC\n");
    client.stop(); //stop and reset server
    matrix.bytes[22] ^= 0x0F;

    if(extractIpAddress(config.host, ip) != 0) {
      if(RedFly.getip(config.host, ip) != 0) {
        debugout("DNS ERR\n");
        return 4;
      }
    }
    client.connectUDP(ip, config.port, 2021);
    client.write('C');
    return 2;
  }
  if(len < N_ROWS*N_COLS+1) {
    return -1;
  }

  client.read(buf, 1);
  if(buf[0] != 'A') {
    client.write(buf[0]);
    return 1;
  }

  client.read(matrix.bytes, N_ROWS*N_COLS);
  //sprintf((char*)buf, "%d\n", len);
  //client.write(buf, 3);

  return 0;
}