//Code Notes : Connect to 5V/G and D3

//Include libraries 
#include <ESP8266WiFi.h>
#include <MQTT.h>
#include <Servo.h>
//Connect to uni wifi
const char ssid[] = "StudentResidences";
const char pass[] = "";
WiFiClient net;
MQTTClient client;
//Start Milli, an alternate delay
unsigned long lastMillis = 0;
//Servo pin attactched to D3
const int SERVO_PIN = D3;
Servo servo;

//Start angle at 0
int angle = 0;

void connect() {
  //Connect to the wifi
  Serial.print("checking wifi...");
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
    delay(1000);
  }
  Serial.print("\nconnecting...");
  //Change "mySessionID" to your own identifier (name, project etc)
  while (!client.connect("SusannaSubscribe", "", "")) {
    Serial.print(".");
    delay(1000);
  }
  Serial.println("\nconnected!");

  //connect to Susannatest
  client.subscribe("Susannatest");
  // client.unsubscribe("/myTopic");
}

//Get information from MQTT
void messageReceived(String &topic, String &payload) {
  //Connects to the topic on the MQTT broker, and returns the most recent data in that topic.
  //Data is held in the variable "payload"
  //Serial.println("incoming: " + topic + " - " + payload);
  int myVal = payload.toInt();  //convert the payload to an integer (if it actually is an integer)
                                // Serial.println(payload.length());

//If value is 10, move motor a small amount
  Serial.println(myVal);
  if (myVal == 10) {
    Serial.println("Low");
   // For presentations (Quicker)
   // angle += 5;  
   // For idle time (real time/Slower)
    angle += 1;
    servo.write(angle);
  }

//If value is 20, move motor a medium amount
  if (myVal == 20) {
    Serial.println("Medium");
   // For presentations (Quicker)
   //angle += 10;
   // For idle time (real time/Slower)
    angle += 2;
    servo.write(angle);
  }

//if value is 30, move motor a larger amount
  if (myVal == 30) {
    Serial.println("High");
   // For presentations (Quicker)
   // angle += 20;
   // For idle time (real time/Slower)
    angle += 4;
    servo.write(angle);
  }

//Once angle reaches 180, reset (This would only be for presentations)
  if(angle == 180) {
    angle = 0;
    servo.write(angle);
  }
  /* if (payload.length() == 4) {
    if (angle == 0)
      angle = 360;
    else if (angle == 360)
      angle = 0;
  } 
  */

  // control servo motor arccoding to the angle
  // servo.write(angle);
  //digitalWrite(D4,myVal);
  //Now the payload is in numeric form, you can use it to drive an actuator or display of some kind.
}


void setup() {
  Serial.begin(115200);
  //connect to wifi
  WiFi.begin(ssid, pass);
  client.begin("134.36.5.199", net);
  client.onMessage(messageReceived);
  connect();
  //Make sure servo is attached and set angle to 0
  servo.attach(SERVO_PIN, 500, 2400);
  servo.write(angle);
}

void loop() {
  client.loop();
  delay(10);  // <- fixes some issues with WiFi stability
  
 //Connect to client
  if (!client.connected()) {
    connect();
  }
}