#include <WiFi.h>
#include <PubSubClient.h>

#undef DEBUG

const char* ssid     = "IOT-pecan";
const char* password = "aaaaaaaa";
const char* mqttServer = "192.168.1.110";
const int mqttPort = 1883;
const char* mqttTopic = "/sheller/angle";

WiFiClient wifiClient;
PubSubClient client(wifiClient);

String inputString = "";

const float homeAngle = 2.5;
const unsigned long fullRetractTime = 14000;
const unsigned long homeTime = 4030;
const float maxAngle = 5.5;
const float minAngle = 2.5;
const unsigned long timeTo5Degrees = 8200;

void setup() {

#ifdef DEBUG
  Serial.begin(9600);
#endif

  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);

#ifdef DEBUG 
    Serial.println("Connecting to WiFi...");
#endif

  }

#ifdef DEBUG
  Serial.println("Connected to WiFi");
#endif

  client.setServer(mqttServer, mqttPort);
  client.setCallback(callback);

  digitalWrite(LED_BUILTIN, HIGH);

  pinMode(RELAY3, OUTPUT);
  pinMode(RELAY4, OUTPUT);
}

void loop() {
  if (!client.connected()) {
    reconnect();
  }
  client.loop();

#ifdef DEBUG
  while (Serial.available() > 0) {
    char inChar = Serial.read();
    if (inChar == '\n') {
      handleInput(inputString);
      inputString = "";
    } else {
      inputString += inChar;
    }
  }
#endif
}

void callback(char* topic, byte* payload, unsigned int length) {
  String input;
  for (int i = 0; i < length; i++) {
    input += (char)payload[i];
  }
  handleInput(input);
}

void handleInput(String input) {
  float desiredAngle = input.toFloat();
  
  if(desiredAngle == 0) {
    // Fully retract without homing
    digitalWrite(RELAY3, HIGH);
    delay(fullRetractTime);
    digitalWrite(RELAY3, LOW);
  }
  else if (desiredAngle >= minAngle && desiredAngle <= maxAngle) {
    // Fully retract
    digitalWrite(RELAY3, HIGH);
    delay(fullRetractTime);
    digitalWrite(RELAY3, LOW);

    // Home
    digitalWrite(RELAY3, HIGH);
    digitalWrite(RELAY4, HIGH);
    delay(homeTime);
    digitalWrite(RELAY3, LOW);
    digitalWrite(RELAY4, LOW);

    unsigned long timeToDesiredAngle = ((desiredAngle - homeAngle) / (maxAngle - homeAngle)) * timeTo5Degrees;
    
    // Move to desired angle
    digitalWrite(RELAY3, HIGH);
    digitalWrite(RELAY4, HIGH);
    delay(timeToDesiredAngle);
    digitalWrite(RELAY3, LOW);
    digitalWrite(RELAY4, LOW);
  }
}

void reconnect() {
  while (!client.connected()) {
    if (client.connect("ArduinoClient")) {
      client.subscribe(mqttTopic);
    } else {
      delay(5000);
    }
  }
}