//WiFi control of the sketch. by CNC Paradise. (jun-2016)
#include "ESP8266WiFi.h"
#include "WiFiUDP.h"
#include "Servo.h"

WiFiUDP UDP;

char WiFibuff[2];
const int distancelimit = 25; //ソナーの前方25センチに設定
const int sidedistancelimit = 30 ; //ソナーの側方30センチに設定
const int turntime = 400; //障害を感知した時、方向転換時間の設定
char turndirection;
char choice;
int leftscanval, centerscanval, rightscanval;
int distance;
int numcycles = 0;
int ModeSW = 0;
int thereis;
int count;
Servo head;

IPAddress myIP(192, 168, 4, 1);
IPAddress HOSTIP (192, 168, 4, 2);
const char *ssid = "TJ-5"; // IDはコントローラー側と同じ設定。
const char *password = "*********"; // passwordはコントローラーと側と同じ設定。

void setup() {
  WiFi.softAP(ssid, password); // setting SSID and Password
  WiFi.config(myIP, WiFi.gatewayIP(), WiFi.subnetMask()); // setting IP Address
  UDP.begin(8000); // setting UDP port,  port番号はコントローラーと側と同じ設定。
  Serial.begin(115200);
  
  pinMode(4,    OUTPUT); // MotorR1
  pinMode(5,    OUTPUT); // MotorL1
  pinMode(12,   OUTPUT); // MotorR2
  pinMode(13,   OUTPUT); // MotorL2
  pinMode(0,    OUTPUT); // trigger
  pinMode(2,    OUTPUT); // ModeLED
  pinMode(16,     INPUT); // echo

  digitalWrite(4,    LOW); // MotorR1
  digitalWrite(5,    LOW); // MotorL1
  digitalWrite(12,   LOW); // MotorR2
  digitalWrite(13,   LOW); // MotorL2
  digitalWrite(2,    LOW); // ModeLED
  digitalWrite(0,    LOW); // trigger
  head.attach(14); // SERVO_head
  head.write(90); // SERVO_head_angle
}

int watch() {
  long howfar;
  digitalWrite(0,   LOW); // trigger
  delayMicroseconds(5);
  digitalWrite(0,  HIGH); // trigger
  delayMicroseconds(15);
  digitalWrite(0,   LOW); // trigger
  howfar = pulseIn(16,  HIGH); // echo
  howfar = howfar * 0.01657;
  return round(howfar);
}

void Forwards() {
  digitalWrite(4,   HIGH); // MotorR1
  digitalWrite(5,   HIGH); // MotorL1
  digitalWrite(12,   LOW); // MotorR2
  digitalWrite(13,   LOW); // MotorL2
}

void backwards() {
  digitalWrite(4,    LOW); // MotorR1
  digitalWrite(5,    LOW); // MotorL1
  digitalWrite(12,  HIGH); // MotorR2
  digitalWrite(13,  HIGH); // MotorL2
}

void turnleft(int t) {
  digitalWrite(4,   HIGH); // MotorR1
  digitalWrite(5,    LOW); // MotorL1
  digitalWrite(12,   LOW); // MotorR2
  digitalWrite(13,  HIGH); // MotorL2
  delay(t);
}

void turnright(int t) {
  digitalWrite(4,    LOW); // MotorR1
  digitalWrite(5,   HIGH); // MotorL1
  digitalWrite(12,  HIGH); // MotorR2
  digitalWrite(13,   LOW); // MotorL2
  delay(t);
}

void stopmove() {
  digitalWrite(4,    LOW); // MotorR1
  digitalWrite(5,    LOW); // MotorL1
  digitalWrite(12,   LOW); // MotorR2
  digitalWrite(13,   LOW); // MotorL2
}

void watchsurrounding() {
  head.write(90);
  delay(100);
  centerscanval = watch();
  if (centerscanval < distancelimit) {
    stopmove();
    delay(50);
    backwards();
    delay(50);
  }

  head.write(180);
  delay(150);
  leftscanval = watch();
  if (leftscanval < sidedistancelimit) {
    stopmove();
    delay(50);
    backwards();
    delay(50);
  }

  head.write(90);
  delay(100);
  centerscanval = watch();
  if (centerscanval < distancelimit) {
    stopmove();
  }

  head.write(0);
  delay(150);
  rightscanval = watch();
  if (rightscanval < sidedistancelimit) {
    stopmove();
    delay(50);
    backwards();
    delay(50);
  }
  head.write(90);
  delay(50);
}

char decide() {
  watchsurrounding();
  if (leftscanval > rightscanval && leftscanval > centerscanval) {
    choice = 'l';
  }
  else if (rightscanval > leftscanval && rightscanval > centerscanval) {
    choice = 'r';
  }
  else {
    choice = 'f';
  }
  return choice;
}

void translateWiFi() {
  if (!strcmp(WiFibuff, "F")) {
    Forwards();
  }
  if (!strcmp(WiFibuff, "B")) {
    backwards();
  }
  if (!strcmp(WiFibuff, "L")) {
      digitalWrite(4,   HIGH); // MotorR1
      digitalWrite(5,    LOW); // MotorL1
      digitalWrite(12,   LOW); // MotorR2
      digitalWrite(13,  HIGH); // MotorL2
  }
  if (!strcmp(WiFibuff, "R")) {
      digitalWrite(4,    LOW); // MotorR1
      digitalWrite(5,   HIGH); // MotorL1
      digitalWrite(12,  HIGH); // MotorR2
      digitalWrite(13,   LOW); // MotorL2
  }
  if (!strcmp(WiFibuff, "S")) {
    stopmove();
  }
}

void loop() {
  if (UDP.parsePacket() > 0) {
    UDP.read(WiFibuff, 2);
    UDP.flush();
  }

  if (!strcmp(WiFibuff, "M")) { // Automatic_Mode
    stopmove();
    Forwards();
    ++numcycles;
    if (numcycles > 50) {
      watchsurrounding();
      if (leftscanval < sidedistancelimit) {
        turnright(turntime);
      }
      if (rightscanval < sidedistancelimit) {
        turnleft(turntime);
      }
      numcycles = 0;
    }
    distance = watch();
    if (distance < distancelimit) {
      ++thereis;
    }
    if (distance > distancelimit) {
      thereis = 0;
    }
    if (thereis > 10
    ) {
      stopmove();
      turndirection = decide();
      switch (turndirection) {
        case 'l':
          turnleft(turntime);
          break;
        case 'r':
          turnright(turntime);
          break;
        case 'f':
          ;
          break;
      }
      thereis = 0;
    }
    digitalWrite(2,  HIGH); // ModeLED ON
  }
  else { //Manual_Mode
    stopmove();
    translateWiFi();
    digitalWrite(2,  LOW); // ModeLED OFF
  }
}