//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 = "TJ-8"; // ID is the same setting as the host side.
const char *password = "**********"; // password is the same setting as the host side.

uint8_t nunbuff[2];    // array to store ESP output
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);  // transmit to device 0x52
  Wire.write (0x40);   // sends memory address
  Wire.write (0x00);   // sends sent a zero.
  Wire.endTransmission ();  // stop transmitting
}

void send_zero () {
  Wire.beginTransmission (0x52);  // transmit to device 0x52
  Wire.write (0x00);   // sends one byte
  Wire.endTransmission ();  // stop transmitting
}

void wifiStart() {
  WiFi.config(myIP, WiFi.gatewayIP(), WiFi.subnetMask());
  WiFi.begin(ssid, password);
  Serial.print("Connecting to ");
  Serial.println(ssid);
  UDP.begin(8000); // setting UDP port,  port番号はロボット側と同じ設定。
  while (WiFi.status() != WL_CONNECTED) {
    delay(10);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");
  printWifiStatus();
}

void printWifiStatus() {
  // print the SSID of the network you're attached to:
  Serial.print("SSID: ");
  Serial.println(WiFi.SSID());
  // print your WiFi shield's IP address:
  IPAddress ip = WiFi.localIP();
  Serial.print("IP Address: ");
  Serial.println(ip);

  // print the received signal strength:
  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); // request data from nunchuck
  while (Wire.available ()) {
    nunbuff[cnt] = nunchuk_decode_byte (Wire.read ());  // receive byte as an integer
    cnt++;
  }

  // If we recieved the 6 bytes, then go print them
  // accel_x_axis, accel_y_axis, accel_z_axis do not use.
  if (cnt >= 5) {
  int x = nunbuff[0]; // joy_x_axis,
  int y = nunbuff[1]; // joy_y_axis,
  int z = 0;
  int c = 0;
  if ((nunbuff[5] >> 0) & 1) z = 1; // The first bit of data of the 5byte (Nuncuck Z-button)
  if ((nunbuff[5] >> 1) & 1) c = 1; // The second bit of data of the 5byte (Nuncuck C-button)

      SendData_H = 'X'; // X-axis
      SendData_D = x ; // MAX 0~255 (Actual measurement 23~227)
      SendUDP();
      delay(10);
      SendData_H = 'Y'; // Y-axis 
      SendData_D = y ; // MAX 0~255 (Actual measurement 23~227)
      SendUDP();
       delay(10);
      SendData_H = 'Z'; // Nuncuck Z-button
      SendData_D = z ; // 0or1
      SendUDP();
      delay(10);
      SendData_H = 'C'; // Nuncuck C-button 
      SendData_D = c ; // 0or1
      SendUDP();
       delay(10);             
    }
  cnt = 0;
  send_zero (); // send the request for next bytes
  delay (10);
}

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