//WiFi control of the sketch. by CNC Paradise. (july-2016)
#include "Wire.h"
#include "ESP8266WiFi.h"
#include "WiFiUDP.h"
#define I2C_SDA  4 // Nuncuck SDA
#define I2C_SCL  5 // Nuncuck SCL
WiFiUDP UDP;

char SendData_H; //　Data tag（header）
int SendData_D; // 1byte Data
IPAddress HostIP (192, 168, 4, 1);
IPAddress myIP(192, 168, 4, 2);
const char *ssid = "WiFi_BR"; 
const char *password = "**********"; 

uint8_t nunbuff[32]; 
int cnt = 0;

void setup () {
  Serial.begin (115200);
  Serial.print ("Finished setup\n");
  Wire.begin ();    // join i2c bus with address 0x52
  nunchuck_init (); // send the initilization handshake
}

void nunchuck_init () {
  Wire.beginTransmission (0x52);
  Wire.write (0x40);
  Wire.write (0x00);
  Wire.endTransmission ();
}

void send_zero () {
  Wire.beginTransmission (0x52);
  Wire.write (0x00); 
  Wire.endTransmission ();
}

void wifiStart() {
  WiFi.config(myIP, WiFi.gatewayIP(), WiFi.subnetMask());
  WiFi.begin(ssid, password);
  Serial.print("Connecting to ");
  Serial.println(ssid);
  UDP.begin(8000);
  while (WiFi.status() != WL_CONNECTED) {
    delay(10);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");
  printWifiStatus();
}

void printWifiStatus() {
  Serial.print("SSID: ");
  Serial.println(WiFi.SSID());
  IPAddress ip = WiFi.localIP();
  Serial.print("IP Address: ");
  Serial.println(ip);

  long rssi = WiFi.RSSI();
  Serial.print("signal strength (RSSI):");
  Serial.print(rssi);
  Serial.println(" dBm");
}

void SendUDP() {
  if (UDP.beginPacket(HostIP, 8000)) {
    UDP.write(SendData_H);
    UDP.write(SendData_D);
    //Serial.print("  SendData_H: ");  Serial.print(SendData_H);
    //Serial.print("  SendData_D: ");  Serial.println(SendData_D);
    UDP.endPacket();
  }
}

void loop () {
  if (WiFi.status() == WL_NO_SSID_AVAIL ||
      WiFi.status() == WL_CONNECTION_LOST ||
      WiFi.status() == WL_DISCONNECTED) {
    UDP.stop();
    wifiStart();
  }
  
  if (UDP.parsePacket() > 0) {
    UDP.read(nunbuff, 2);
    UDP.flush();
  }
  Wire.requestFrom (0x52, 6); 
  while (Wire.available ()) {
    nunbuff[cnt] = nunchuk_decode_byte (Wire.read ());  // receive byte as an integer
    cnt++;
  }

  if (cnt >= 5) {
    int x = nunbuff[0];
    int y = nunbuff[1];

    SendData_H = 'X'; // X-axis
    SendData_D = x ;
    SendUDP();
    delay(10);
    SendData_H = 'Y'; // Y-axis
    SendData_D = y ;
    SendUDP();
    delay(10);
  }
  cnt = 0;
  send_zero ();
  delay (10);
}

char nunchuk_decode_byte (char x) {
  x = (x ^ 0x17) + 0x17;
  return x;
}