#include <WiFiEsp.h>
#include <WiFiEspClient.h>
#include <WiFiEspServer.h>
#include <WiFiEspUdp.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"

/*
 WiFiEsp example: WebServerLed

 A simple web server that lets you turn on and of an LED via a web page.
 This sketch will print the IP address of your ESP8266 module (once connected)
 to the Serial monitor. From there, you can open that address in a web browser
 to turn on and off the LED on pin 13.

 For more details see: http://yaab-arduino.blogspot.com/p/wifiesp.html
*/


// Emulate Serial1 on pins 6/7 if not present
#ifndef HAVE_HWSERIAL1
#include <SoftwareSerial.h>
SoftwareSerial Serial2(17, 16); // RX, TX

#endif

char ssid[] = "SSID";            // your network SSID (name)
char pass[] = "password";        // your network password
int status = WL_IDLE_STATUS;

int ledStatusBLUE = LOW;
int ledStatusRED = LOW;
int ledStatusYELLOW = LOW;
int ledStatusGREEN = LOW;
int BLUE_LED = 6;
int RED_LED = 5;
int YELLOW_LED = 4;
int GREEN_LED = 3;


WiFiEspServer server(80);

Servo servo1;
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myMotor1 = AFMS.getMotor(1);
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(3);
int pos = 0;

// use a ring buffer to increase speed and reduce memory allocation
RingBuffer buf(8);

void setup()
{
  pinMode(BLUE_LED, OUTPUT);  // initialize digital pin LED_BUILTIN as an output.
  pinMode(RED_LED, OUTPUT);
  pinMode(YELLOW_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
  Serial.begin(115200);   // initialize serial for debugging
  Serial2.begin(115200);    // initialize serial for ESP module
  WiFi.init(&Serial2);    // initialize ESP module
  AFMS.begin();

  // check for the presence of the shield
  if (WiFi.status() == WL_NO_SHIELD) {
    Serial.println("WiFi shield not present");
    // don't continue
    while (true);
  }

  // attempt to connect to WiFi network
  while (status != WL_CONNECTED) {
    Serial.print("Attempting to connect to WPA SSID: ");
    Serial.println(ssid);
    // Connect to WPA/WPA2 network
    status = WiFi.begin(ssid, pass);
  }

  Serial.println("You're connected to the network");
  printWifiStatus();

  // start the web server on port 80
  server.begin();

  servo1.attach(9);
  servo1.write(90);

  myMotor1->setSpeed(150);
  //myMotor1->run(FORWARD);
  // turn on motor
  //myMotor1->run(RELEASE);
}


void loop()
{
  WiFiEspClient client = server.available();  // listen for incoming clients

  if (client) {                               // if you get a client,
    Serial.println("New client");             // print a message out the serial port
    buf.init();                               // initialize the circular buffer
    while (client.connected()) {              // loop while the client's connected
      if (client.available()) {               // if there's bytes to read from the client,
        char c = client.read();               // read a byte, then
        buf.push(c);                          // push it to the ring buffer

        // printing the stream to the serial monitor will slow down
        // the receiving of data from the ESP filling the serial buffer
        //Serial.write(c);

        // you got two newline characters in a row
        // that's the end of the HTTP request, so send a response
        if (buf.endsWith("\r\n\r\n")) {
          sendHttpResponse(client);
          break;
        }

        // Check to see if the client request was "GET /H" or "GET /L":
        if (buf.endsWith("GET /HB")) {
          Serial.println("Turn led ON");
          ledStatusBLUE = HIGH;
          digitalWrite(BLUE_LED, HIGH);   // turn the LED on (HIGH is the voltage level)
        }
        else if (buf.endsWith("GET /LB")) {
          Serial.println("Turn led OFF");
          ledStatusBLUE = LOW;
          digitalWrite(BLUE_LED, LOW);    // turn the LED off by making the voltage LOW
        }
        else if (buf.endsWith("GET /HR")) {
          Serial.println("Turn led ON");
          ledStatusRED = HIGH;
          digitalWrite(RED_LED, HIGH);   // turn the LED on (HIGH is the voltage level)
        }
        else if (buf.endsWith("GET /LR")) {
          Serial.println("Turn led OFF");
          ledStatusRED = LOW;
          digitalWrite(RED_LED, LOW);    // turn the LED off by making the voltage LOW
        }
        else if (buf.endsWith("GET /HY")) {
          Serial.println("Turn led ON");
          ledStatusYELLOW = HIGH;
          digitalWrite(YELLOW_LED, HIGH);   // turn the LED on (HIGH is the voltage level)
        }
        else if (buf.endsWith("GET /LY")) {
          Serial.println("Turn led OFF");
          ledStatusYELLOW = LOW;
          digitalWrite(YELLOW_LED, LOW);    // turn the LED off by making the voltage LOW
        }
        else if (buf.endsWith("GET /HG")) {
          Serial.println("Turn led ON");
          ledStatusGREEN = HIGH;
          digitalWrite(GREEN_LED, HIGH);   // turn the LED on (HIGH is the voltage level)
        }
        else if (buf.endsWith("GET /LG")) {
          Serial.println("Turn led OFF");
          ledStatusGREEN = LOW;
          digitalWrite(GREEN_LED, LOW);    // turn the LED off by making the voltage LOW
        }/* else if (buf.endsWith("GET /SR")) { //Turning the servo to the right
            while (pos < 180) {
              servo1.write(pos++);
              delay(5);
            }
        } else if (buf.endsWith("GET /SL")) {   //Just testing turning the servo to the left
            while(pos > 3) {
            servo1.write(pos--);
            delay(5);
            }
        }*/ /*else if (buf.endsWith("GET /M")) {  //this command moves the DC motor forward
            myMotor1->run(FORWARD);
          } else if (buf.endsWith("GET /MS")) { //this command moves the DC motor backwards
            myMotor1->run(BACKWARD);
          }*/
        }
      }

      // close the connection
      client.stop();
      Serial.println("Client disconnected");
    }
}


void sendHttpResponse(WiFiEspClient client)
{
  // HTTP headers always start with a response code (e.g. HTTP/1.1 200 OK)
  // and a content-type so the client knows what's coming, then a blank line:
  client.println("HTTP/1.1 200 OK");
  client.println("Content-type:text/html");
  client.println();


  // the content of the HTTP response follows the header:

  //client.println("<script src=\"keypress.js\"></script>");


  client.println("<br><br>Click <a href=\"/HB\">here</a> turn the LED on<br>Click <a href=\"/LB\">here</a> turn the LED off<br>Click <a href=\"/HR\">here</a> turn the LED on<br>Click <a href=\"/LR\">here</a> turn the LED off<br>");
  client.println("Click <a href=\"/HY\">here</a> turn the LED on<br>Click <a href=\"/LY\">here</a> turn the LED off<br>Click <a href=\"/HG\">here</a> turn the LED on<br>Click <a href=\"/LG\">here</a> turn the LED off<br>");
  //client.println("Click <a href=\"/HR\">here</a> turn the LED on<br>Click <a href=\"/LR\">here</a> turn the LED off<br>");
  //client.println("Click <a href=\"/HY\">here</a> turn the LED on<br>Click <a href=\"/LY\">here</a> turn the LED off<br>");
  //client.println("Click <a href=\"/HG\">here</a> turn the LED on<br>Click <a href=\"/LG\">here</a> turn the LED off<br>");
  //client.println();
  //client.println("Click <a href=\"/SR\">here</a> to turn servo right<br>");
  //client.println("Click <a href=\"/SL\">here</a> to turn servo left<br>");
  //client.println("Click <a href=\"/M\">here</a> turn motor<br>");
  //client.println("Click <a href=\"/MS\">here</a> stop motor<br>");

  // The HTTP response ends with another blank line:
  client.println();
}

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 where to go in the browser
  Serial.println();
  Serial.print("To see this page in action, open a browser to http://");
  Serial.println(ip);
  Serial.println();
}