//WiFi control of the sketch. by CNC Paradise.
#include "ESP8266WiFi.h"
#include "WiFiUDP.h"

char SendData;
WiFiUDP UDP;
IPAddress HostIP (192, 168, 4, 1);
IPAddress myIP(192, 168, 4, 2);
const char *ssid = "TJ-5"; // IDはロボット側と同じ設定。
const char *password = "***********"; // passwordはロボット側と同じ設定。

int count;
void connectWiFi() {
  WiFi.begin(ssid, password); // setting SSID and Password
  WiFi.config(myIP, WiFi.gatewayIP(), WiFi.subnetMask());
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }
}

void setup() {
  UDP.begin(8000); // setting UDP port,  port番号はロボット側と同じ設定。
  connectWiFi();
  Serial.begin(115200);
  pinMode(4,     INPUT); // L-switch
  pinMode(5,     INPUT); // R-switch
  pinMode(12,    INPUT); // F-switch
  pinMode(13,    INPUT); // B-switch
  pinMode(14,    INPUT); // Mode-switch
  pinMode(2,    OUTPUT); // Mode_LED
  digitalWrite(4,  HIGH);
  digitalWrite(5,  HIGH);
  digitalWrite(12, HIGH);
  digitalWrite(13, HIGH);
  digitalWrite(14, HIGH);
  digitalWrite(2,  LOW);
  delay(500); // waitTime
}

void SendUdp() { // send by UDP
  if (UDP.beginPacket(HostIP, 8000)) {
    UDP.write(SendData);
    //Serial.print("SendData: ");  Serial.println(SendData);
    UDP.endPacket();
  }
}
void loop() {
  count++;
  if (count > 2000) {
    count = 0;
    // Read the state of the switch.
    int val_L = digitalRead(4);
    int val_R = digitalRead(5);
    int val_F = digitalRead(12);
    int val_B = digitalRead(13);
    int val_M = digitalRead(14);

    if ((val_L + val_R + val_F + val_B + val_M) == 5) { // If all of the switch is off.
      SendData = 'S'; // Stop
      SendUdp();
    }
    if (val_L == 0 ) { // Left-switch ON
      SendData = 'L';
      SendUdp();
    }
    if (val_R == 0) { // Right-switch ON
      SendData = 'R';
      SendUdp();
    }
    if (val_F == 0) { // Foreard-switch ON
      SendData = 'F';
      SendUdp();
    }
    if (val_B == 0) { // backwards-switch ON
      SendData = 'B';
      SendUdp();
    }
    if (val_M == 0) { // Mode-switch ON
      SendData = 'M';
      SendUdp();
      digitalWrite(2, HIGH); // Mode_LED ON
    }
    else {
      digitalWrite(2, LOW); // Mode_LED OFF
    }
  }

  if (WiFi.status() == WL_CONNECTED) {
  }
  else {
    WiFi.disconnect();
    connectWiFi();
  }
}