#include <Servo.h>
#include <ESP8266WiFi.h>

/* variables to enable communication by WiFi */
const char* ssid;
const char* password;
WiFiServer server(80);
int counter;

/* variables to manage the servos from the robotic arm */
Servo sb3, sf2b, sf2a, sh1, sf6b;
int sb3_p, sf2b_p, sf2a_p, sh1_p, sf6b_p;
int min_sb3, min_sf2b, min_sf2a, min_sh1, min_sf6b;
int max_sb3, max_sf2b, max_sf2a, max_sh1, max_sf6b;
int curr_sb3, curr_sf2b, curr_sf2a, curr_sh1, curr_sf6b;
int speed_move = 3;
int enable_servo;

int delay_t = 500;
const int LEFT = 0;
const int RIGHT = 1;
void move_arm(Servo , int , int , int );

void setup_servos(){
  Serial.println("Preparing servos...");
  /* Prepare servo's settings*/
  /* Setting for servo SB3 - Base(BL BR) */
  min_sb3 = 40;
  max_sb3 = 140;
  curr_sb3 = 90;
  sb3_p = D1;
  sb3.attach(sb3_p);
  /* Setting for servo SF2b - UpDown (UDL UDR)*/
  min_sf2b = 60;
  max_sf2b = 130;
  curr_sf2b = 90;
  sf2b_p = D2;
  sf2b.attach(sf2b_p);
  /* Setting for servo SF2a - FrontBack (FBL FBR)*/
  min_sf2a = 50;
  max_sf2a = 130;
  curr_sf2a = 90;
  sf2a_p = D3;
  sf2a.attach(sf2a_p);
  /* Setting for servo SF6b - Wrist (WL WR)*/
  min_sf6b = 60;
  max_sf6b = 120;
  curr_sf6b = 90;
  sf6b_p = D4;
  sf6b.attach(sf6b_p);
  /* Setting for servo SH1 - Catch (HOLD UNHOLD)*/
  min_sh1 = 60;
  max_sh1 = 95;
  curr_sh1 = 90;
  sh1_p = D5;
  sh1.attach(sh1_p);
  
  /* Set the initial position */
  Serial.println("Initial position...");
  sb3.write(curr_sb3);
  sf2b.write(curr_sf2b);
  sf2a.write(curr_sf2a);
  sf6b.write(curr_sf6b);
  sh1.write(curr_sh1);
  
  Serial.println("Servos ready.");
}

void setup_wifi(){
  Serial.println("Initializing WiFi...");
  
  /* Connect to WiFi */
  ssid = "ingegno";
  password = "ingegnofablab";
  counter = 0;
  
  ssid = "*******";
  password = "******";

  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");

  /* Initialize the server */
  /* Start the server */
  server.begin();
  Serial.println("Server started...");
 
  // Print the IP address
  Serial.print("Use this URL to connect: ");
  Serial.print("http://");
  Serial.print(WiFi.localIP());
  Serial.println("/");
  
  Serial.println("WiFi ready.");
}

void setup() {
  Serial.begin(57600);
  Serial.println("\nStarting arduino...");
  setup_servos();
  setup_wifi();
  Serial.println("Arduino ready!");
}

int move_arm(Servo s, int curr, int v_min, int v_max, int dir ){
  int new_pos = curr;
  if(dir == LEFT){
    new_pos = curr - speed_move;
    if(new_pos >= v_min) s.write(new_pos);
    else Serial.println("Min movement...");
  }
  else{
    new_pos = curr + speed_move;
    if(new_pos <= v_max) s.write(new_pos);
    else Serial.println("Max movement...");
  }
  return new_pos;
}

void loop() {

  // Check if a client has connected
  WiFiClient client = server.available();
  if (!client) {
    return;
  }

  // Wait until the client sends some data
  Serial.println("new client...");
  while(!client.available()){
    delay(1000);
  }
  String request = client.readStringUntil('\r');

  Serial.print("Value read from wifi: ");
  Serial.println(request);
  client.flush();

// Match the request
  String ret;
  if (request.indexOf("/move/BL") != -1)  {
    ret = "Move base arm left...";
    curr_sb3 = move_arm(sb3, curr_sb3, min_sb3, max_sb3, RIGHT);
  }
  else if (request.indexOf("/move/BR") != -1)  {
    ret = "Move base arm right...";
    curr_sb3 = move_arm(sb3, curr_sb3, min_sb3, max_sb3, LEFT);
  }
  else if (request.indexOf("/move/UDL") != -1)  {
    ret = "Move UpDown arm left...";
    curr_sf2b = move_arm(sf2b, curr_sf2b, min_sf2b, max_sf2b, LEFT);
  }
  else if (request.indexOf("/move/UDR") != -1)  {
    ret = "Move UpDown arm right...";
    curr_sf2b = move_arm(sf2b, curr_sf2b, min_sf2b, max_sf2b, RIGHT);
  }
  else if (request.indexOf("/move/FBL") != -1)  {
    ret = "Move FrontBack arm left...";
    curr_sf2a = move_arm(sf2a, curr_sf2a, min_sf2a, max_sf2a, LEFT);
  }
  else if (request.indexOf("/move/FBR") != -1)  {
    ret = "Move FrontBack arm right...";
    curr_sf2a = move_arm(sf2a, curr_sf2a, min_sf2a, max_sf2a, RIGHT);
  }
  else if (request.indexOf("/move/WL") != -1)  {
    ret = "Move Wrist arm left...";
    curr_sf6b = move_arm(sf6b, curr_sf6b, min_sf6b, max_sf6b, LEFT);
  }
  else if (request.indexOf("/move/WR") != -1)  {
    ret = "Move Wrist arm right...";
    curr_sf6b = move_arm(sf6b, curr_sf6b, min_sf6b, max_sf6b, RIGHT);
  }
  else if (request.indexOf("/HOLD") != -1)  {
    ret = "Hold object...";
    sh1.write(min_sh1);
    //curr_sh1 = move_arm(sh1, curr_sh1, min_sh1, max_sh1, RIGHT);
  }
  else if (request.indexOf("/UNHOLD") != -1)  {
    ret = "Unhold object...";
    sh1.write(max_sh1);
    //curr_sh1 = move_arm(sh1, curr_sh1, min_sh1, max_sh1, LEFT);
  }
  Serial.print("Last move: ");
  Serial.println(ret);
  counter++;
 
  // Return the response
  client.println("HTTP/1.1 200 OK");
  client.println("Content-Type: text/html");
  client.println(""); //  do not forget this one
  client.println("<!DOCTYPE HTML>");
  client.println("<html>");

  client.print("Counter: ");
  client.println(counter);
  
  client.print("Last move: ");
  client.println(ret);

  client.println("</html>");
 
  delay(1);
  Serial.println("Client disonnected");
  Serial.println("");
  
}