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

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

// Config
const char ssid[] = "";
const char wifi_pw[] = "";
const char mqtt_user[] = "";
const char mqtt_pw[] = "";
const char mqtt_addr[] = "";
const char client_name[] = "";


// End Config

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;
const int ADJUSTING = 5;

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

long fullyOpenPosition = -14000;
long fullyClosedPosition = 0;

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

  Serial.println("\nconnecting...");
  while (!client.connect(client_name, mqtt_user, mqtt_pw)) {
    Serial.print(".");
    delay(1000);
  }

  Serial.println("\nconnected!");

  client.subscribe("/bedroom/blinds/2");
  client.subscribe("/bedroom/blinds/2/adjust");
}

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

  if (topic.endsWith("adjust")) {
    int adjustment;
    sscanf(payload.c_str(), "%d", &adjustment);
    adjustClosedPosition(adjustment);
  } else {
    if (payload.indexOf("open") != -1) {
      openBlinds();
    } else if (payload.indexOf("close") != -1) {
      closeBlinds();
    } else if (payload.indexOf("reset") != -1) {
      Serial.println("Resetting current position and setting blinds state to closed");
      stepper.setCurrentPosition(0);
      blindsOpen = false;
    } else {
      Serial.println(payload);
    }
  }
}

void updateState(int newState) {
  previousState = state;
  state = newState;

  if(newState == IDLE) {
    Serial.println("Transitioning to idle");
  } else {
    Serial.print("Moving ");
    Serial.print(previousState);
    Serial.print(" -> ");
    Serial.println(newState);
  }
}

void openBlinds() {
  if (!blindsOpen && state == IDLE) {
    Serial.println("Opening Blinds");
    updateState(OPENING_BLINDS);
    stepper.enableOutputs();
    stepper.moveTo(fullyOpenPosition);
  }
}

void closeBlinds() {
  if (blindsOpen && state == IDLE) {
    Serial.println("Closing Blinds");
    updateState(CLOSING_BLINDS);
    stepper.enableOutputs();
    stepper.moveTo(fullyClosedPosition);
  }
}

void adjustClosedPosition(int offset) {
  if(offset == 0 || state != IDLE) return;

  Serial.print("Adjust by: ");
  Serial.println(offset);

  updateState(ADJUSTING);
  stepper.enableOutputs();
  stepper.move(offset);
}

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

  WiFi.begin(ssid, wifi_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) {
        updateState(IDLE);
        stepper.disableOutputs();
        return;
      }

      stepper.run();

      // Update previous to SLACK until we take up the slack to prevent the stepper from moving forever
      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);
        updateState(SLACK);
      }
      break;
    case ADJUSTING:
      if (millis() - lastMillis > 1000) {
        Serial.println("Adjusting...");
        lastMillis = millis();
      }

      stepper.run();

      if (stepper.distanceToGo() == 0) {
        Serial.print("Finished adjusting to: ");
        Serial.println(stepper.currentPosition());

        updateState(IDLE);
        stepper.disableOutputs();

        if(blindsOpen) {
          fullyOpenPosition = stepper.currentPosition();

          Serial.print("New open position: ");
          Serial.println(fullyOpenPosition);
        } else {
          fullyClosedPosition = stepper.currentPosition();

          Serial.print("New closed position: ");
          Serial.println(fullyClosedPosition);
        }
      }

      break;
  }
}

void setup_stepper() {
  updateState(IDLE);
  stepper.setCurrentPosition(-3000);
}

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