#include <AccelStepper.h>
#include <esp_sleep.h>
#include <PubSubClient.h>
#include <WiFi.h>
#define CONNECTION_RETRY_DELAY 500 // In milliseconds
#define COVER_UPDATE_DELAY 1000 // In milliseconds
#define LOOP_DELAY 0
#define MQTT_BROKER "192.168.50.4"
#define MQTT_CLIENT_ID "persiana-dormitorio"
#define MQTT_PORT 1883
#define MQTT_TOPIC_COMMAND "persiana-dormitorio/command"
#define MQTT_TOPIC_POSITION "persiana-dormitorio/position"
#define MQTT_TOPIC_STATE "persiana-dormitorio/state"
#define MQTT_KEEP_ALIVE 60 // In seconds
#define MQTT_PASSWORD "***REDACTED***"
#define MQTT_UPDATE_DELAY 100
#define MQTT_USERNAME "***REDACTED***"
#define STEPPER_1_PIN_A 7 // Blue
#define STEPPER_1_PIN_B 6 // Green
#define STEPPER_1_PIN_C 5 // Yellow
#define STEPPER_1_PIN_D 1 // Black
#define STEPPER_2_PIN_A 4 // Black
#define STEPPER_2_PIN_B 3 // Yellow
#define STEPPER_2_PIN_C 20 // Green
#define STEPPER_2_PIN_D 10 // Blue
#define STEPPER_MAX_POSITION 14336 // 7 revs * 2048 steps
#define STEPPER_SPEED 256 // In steps/s
#define WAKEUP_PIN 2
#define WIFI_GATEWAY "192.168.50.1"
#define WIFI_PASSWORD "***REDACTED***"
#define WIFI_SSID "***REDACTED***"
#define WIFI_STATIC_IP "192.168.50.191"
#define WIFI_SUBNET "255.255.255.0"
#define COVER_ACTION_CLOSING -1
#define COVER_ACTION_OPENING 1
#define COVER_COMMAND_CLOSE "close"
#define COVER_COMMAND_OPEN "open"
#define COVER_COMMAND_STOP "stop"
#define COVER_COMMAND_TOGGLE "toggle"
#define COVER_STATE_CLOSED "closed"
#define COVER_STATE_CLOSING "closing"
#define COVER_STATE_OPEN "open"
#define COVER_STATE_OPENING "opening"
RTC_DATA_ATTR int lastStepperPosition = 0;
RTC_DATA_ATTR int lastCoverAction = COVER_ACTION_CLOSING;
int currentTime = 0;
int lastCoverUpdateTime = 0;
int lastMqttUpdateTime = 0;
int lastDebug = 0; // TODO: Remove this
IPAddress mqttBroker(MQTT_BROKER);
IPAddress wifiStaticIp(WIFI_STATIC_IP);
IPAddress wifiGateway(WIFI_GATEWAY);
IPAddress wifiSubnet(WIFI_SUBNET);
const int numSteppers = 2;
bool isStepperEnabled[numSteppers] { false, false };
AccelStepper steppers[numSteppers] {
  {AccelStepper::FULL4WIRE, STEPPER_1_PIN_A, STEPPER_1_PIN_B, STEPPER_1_PIN_C, STEPPER_1_PIN_D, false},
  {AccelStepper::FULL4WIRE, STEPPER_2_PIN_A, STEPPER_2_PIN_B, STEPPER_2_PIN_C, STEPPER_2_PIN_D, false}
};
WiFiClient wifi;
PubSubClient mqtt(wifi);
void publishCoverStatus() {
  String coverState;
  int coverPosition = ceil((float)steppers[0].currentPosition() * (float)100 / (float)STEPPER_MAX_POSITION);
  mqtt.publish(MQTT_TOPIC_POSITION, String(coverPosition).c_str(), true);
  if (steppers[0].distanceToGo() > 0) {
    coverState = COVER_STATE_OPENING;
  } else if (steppers[0].distanceToGo() > 0) {
    coverState = COVER_STATE_CLOSING;
  } else if (coverPosition == 0) {
    coverState = COVER_STATE_CLOSED;
  } else {
    coverState = COVER_STATE_OPEN;
  }
  mqtt.publish(MQTT_TOPIC_STATE, String(coverState).c_str(), true);
  lastCoverUpdateTime = millis();
}
void mqttCallback(char* topic, byte* message, unsigned int length) {
  if (!length) {
    return;
  }
  String payload;
  for (int i = 0; i < length; i++) {
    payload += (char)message[i];
  }
  Serial.print("Received MQTT message '");
  Serial.print(payload);
  Serial.print("' on topic ");
  Serial.println(topic);
  if (String(topic) == MQTT_TOPIC_COMMAND) {
    mqtt.publish(MQTT_TOPIC_COMMAND, NULL, true);
    if (payload == COVER_COMMAND_OPEN) moveCoverTo(100);
    else if (payload == COVER_COMMAND_CLOSE) moveCoverTo(0);
    else if (payload == COVER_COMMAND_TOGGLE) toggleCover();
    else if (payload == COVER_COMMAND_STOP) stopCover();
    else if (isDigit(payload[0])) moveCoverTo(payload.toInt());
  }
}
void moveCoverTo(int targetPercent) {
  if (targetPercent > 100) {
    targetPercent = 100;
  } else if (targetPercent < 0) {
    targetPercent = 0;
  }
  Serial.print("Moving cover to position: ");
  Serial.println(targetPercent);
  int targetPosition = STEPPER_MAX_POSITION * targetPercent / 100;
  if (targetPosition > steppers[0].currentPosition()) {
    lastCoverAction = COVER_ACTION_OPENING;
  } else if (targetPosition < steppers[0].currentPosition()) {
    lastCoverAction = COVER_ACTION_CLOSING;
  }
  for (int i = 0; i < sizeof(steppers) / sizeof(steppers[0]); i++) {
    steppers[i].moveTo(targetPosition);
    if (targetPosition >= steppers[i].currentPosition()) {
      steppers[i].setSpeed(STEPPER_SPEED);
    } else {
      steppers[i].setSpeed(-STEPPER_SPEED);
    }
  }
}
void stopCover() {
  Serial.println("Stopping cover");
  for (int i = 0; i < sizeof(steppers) / sizeof(steppers[0]); i++) {
    steppers[i].moveTo(steppers[i].currentPosition());
  }
}
void toggleCover() {
  Serial.println("Toggling cover");
  if (steppers[0].distanceToGo() != 0) {
    stopCover();
  } else if (lastCoverAction == COVER_ACTION_CLOSING) {
    moveCoverTo(100);
  } else {
    moveCoverTo(0);
  }
}
void updateSteppers() {
  for (int i = 0; i < sizeof(steppers) / sizeof(steppers[0]); i++) {
    if (steppers[i].distanceToGo() != 0) {
      if (!isStepperEnabled[i]) {
        steppers[i].enableOutputs();
        isStepperEnabled[i] = true;
      }
      steppers[i].runSpeed();
    } else {
      steppers[i].disableOutputs();
      isStepperEnabled[i] = false;
      if (i == 0 && steppers[i].currentPosition() != lastStepperPosition) {
        lastStepperPosition = steppers[i].currentPosition();
        publishCoverStatus();
      }
    }
  }
}
void setupSteppers() {
  for (int i = 0; i < sizeof(steppers) / sizeof(steppers[0]); i++) {
    steppers[i].setCurrentPosition(lastStepperPosition);
    steppers[i].setMaxSpeed(STEPPER_SPEED);
  }
}
void setupWifi() {
  WiFi.config(wifiStaticIp, wifiGateway, wifiGateway, wifiSubnet);
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  Serial.print("Connecting to WiFi ");
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
    delay(CONNECTION_RETRY_DELAY);
  }
  Serial.println();
  Serial.println("WiFi connected successfully");
}
void setupMqtt() {
  Serial.print("Connecting to MQTT ");
  mqtt.setServer(mqttBroker, MQTT_PORT);
  mqtt.setKeepAlive(MQTT_KEEP_ALIVE);
  mqtt.setCallback(mqttCallback);
  while (!mqtt.connected()) {
    if (!mqtt.connect(MQTT_CLIENT_ID, MQTT_USERNAME, MQTT_PASSWORD)) {
      Serial.print(".");
      delay(CONNECTION_RETRY_DELAY);
    }
  }
  Serial.println();
  Serial.println("MQTT connected successfully");
  mqtt.subscribe(MQTT_TOPIC_COMMAND);
}
void setup() {
  Serial.begin(115200);
  analogReadResolution(12);
  pinMode(WAKEUP_PIN, INPUT_PULLUP);
  setupSteppers();
  setupWifi();
  setupMqtt();
  if (esp_sleep_get_wakeup_cause() == ESP_SLEEP_WAKEUP_EXT0) {
    toggleCover();
  }
}
void loop() {
  if (!mqtt.connected()) {
    setupMqtt();
  }
  currentTime = millis();
  if (lastMqttUpdateTime == 0 || currentTime > lastMqttUpdateTime + MQTT_UPDATE_DELAY) {
    mqtt.loop();
  }
  updateSteppers();
  // TODO: Remove this
  if (currentTime > lastDebug + 500) {
    Serial.print("Current position: ");
    Serial.print(steppers[0].currentPosition());
    Serial.print(" | Target position: ");
    Serial.print(steppers[0].targetPosition());
    Serial.print(" | Distance to go: ");
    Serial.print(steppers[0].distanceToGo());
    Serial.print(" | Speed: ");
    Serial.print(steppers[0].speed());
    Serial.print(" | Last action: ");
    Serial.print(lastCoverAction);
    Serial.print(" | Last position: ");
    Serial.println(lastStepperPosition);
    lastDebug = currentTime;
  }
  if (lastCoverUpdateTime == 0 || currentTime > lastCoverUpdateTime + COVER_UPDATE_DELAY) {
    publishCoverStatus();
  }
  if (LOOP_DELAY > 0) {
    delay(LOOP_DELAY);
  }
}