#include <ESP8266WiFi.h>
#include <MQTT.h>
#include <AccelStepper.h>

WiFiClient net;
MQTTClient client;
AccelStepper stepper(AccelStepper::HALF4WIRE, D0, D2, D1, D3);

const char ssid[] = "";
const char pw[] = "";
const char mqtt_addr[] = "192.168.10.30";

unsigned long lastMillis = 0;

// States
const int SETUP = 0;
const int SLACK = 1;
const int OPENING_BLINDS = 2;
const int CLOSING_BLINDS = 3;
const int IDLE = 4;

int state = SETUP;
int previousState = SETUP;
bool blindsOpen = false;

void connect() {
  Serial.print("checking wifi...");
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
    delay(1000);
  }

  Serial.print("\nconnecting...");
  while (!client.connect("bedroom-blinds-1", "cameron", pw)) {
    Serial.print(".");
    delay(1000);
  }

  Serial.println("\nconnected!");

  client.subscribe("/blinds/bedroom/1");
}

void messageReceived(String &topic, String &payload) {
  Serial.println("incoming: " + topic + " - " + payload);

  // Note: Do not use the client in the callback to publish, subscribe or
  // unsubscribe as it may cause deadlocks when other things arrive while
  // sending and receiving acknowledgments. Instead, change a global variable,
  // or push to a queue and handle it in the loop after calling `client.loop()`.
  if (payload.indexOf("open") != -1) {
    openBlinds();
  } else if (payload.indexOf("close") != -1) {
    closeBlinds();
  } else {
    Serial.println(payload);
  }
}

void openBlinds() {
  if (!blindsOpen) {
    Serial.println("Opening Blinds");
    previousState = state;
    state = OPENING_BLINDS;
    stepper.enableOutputs();
    stepper.moveTo(-14000);
  }
}

void closeBlinds() {
  if (blindsOpen) {
    Serial.println("Closing Blinds");
    previousState = state;
    state = CLOSING_BLINDS;
    stepper.enableOutputs();
    stepper.moveTo(0);
  }
}

void setup() {
  Serial.begin(115200);

  WiFi.begin(ssid, pw);

  client.begin(mqtt_addr, 1883, net);
  client.onMessage(messageReceived);

  connect();

  stepper.setMaxSpeed(600);
  stepper.setSpeed(100);
  stepper.setAcceleration(250);
}

void loop() {
  client.loop();

  if (!client.connected()) {
    connect();
  }

  switch (state) {
    case SETUP:
      setup_stepper();
      break;
    case IDLE:
      delay(10);  // <- fixes some issues with WiFi stability
      break;
    case SLACK:
      if (previousState == OPENING_BLINDS) {
        stepper.move(3000);
      } else if (previousState == CLOSING_BLINDS) {
        stepper.move(-3000);
      } else if (stepper.distanceToGo() == 0) {
        state = IDLE;
        stepper.disableOutputs();
        Serial.println("Transitioning to IDLE");
        return;
      } else {
        stepper.run();
      }

      // Update previous to SLACK until we take up the slack
      previousState = state;
      break;
    case OPENING_BLINDS:
    case CLOSING_BLINDS:
      if (stepper.distanceToGo() != 0) {
        if (millis() - lastMillis > 2000) {
          Serial.print("Speed: ");
          Serial.print(stepper.speed());
          Serial.print(" Dist: ");
          Serial.println(stepper.distanceToGo());
          lastMillis = millis();
        }
        stepper.run();
      } else {
        stepper.disableOutputs();
        bool open = state == OPENING_BLINDS;
        blindsOpen = open;
        sendBlindsUpdate(open);
        previousState = state;
        state = SLACK;
      }
      break;
  }
}

void setup_stepper() {
  previousState = state;
  state = IDLE;
  stepper.setCurrentPosition(-3000);
}

void sendBlindsUpdate(bool isOpen) {
  client.publish("/blinds/bedroom/1/status", isOpen ? "true" : "false", true);
}