#include <Arduino.h>
#include <EspMQTTClient.h>
#include <arduino-timer.h>
#include <ArduinoJson.h>
#include <RFPowerView.h>
#include "secrets.h"

#define SER_BAUDRATE                   (115200)

// Copied from Powerview Hub userdata API 
// eg: http://POWERVIEW_HUB_IP_ADDRESS/api/userdata/ and find the field labeled "rfID"
#define RF_ID                          (0x2EC8)

RFPowerView powerView(RF_CE_PIN, RF_CS_PIN, RF_IRQ_PIN, RF_ID);

uint8_t lastRollingCode1 = 0x3D;
uint8_t lastRollingCode2 = 0x96;

EspMQTTClient client(
  SECRET_WIFI_SSID,       // Wifi SSID
  SECRET_WIFI_PASSWORD, // Wifi Password
  SECRET_MQTT_SERVER_IP,   // MQTT Broker server IP
  SECRET_MQTT_USERNAME,          // MQTT Username
  SECRET_MQTT_PASSWORD,    // MQTT Password
  "hotdog",          // MQTT Client ID
  1883
);

#define MAX_FETCH_COUNT               (20)
auto timer = Timer<10, millis, uint16_t>();

void processPacket(const Packet*);
void processSetMessage(const String& topic, const String &payload);
void processSetPositionMessage(const String& topic, const String &payload);

bool sendOpenPacket(uint16_t destination);
bool sendClosePacket(uint16_t destination);
bool sendStopPacket(uint16_t destination);
bool sendSetPosition(uint16_t destination, float percentage);
bool sendFetchPosition(uint16_t destination);
bool sendPacket(Packet *packet);

bool checkPosition(uint16_t shadeID);
void startFetchingPosition(uint16_t shadeID, int8_t targetPosition);

void publishPosition(const String& shadeName, const uint8_t position);
void publishState(const String& shadeName, const String& state);

void publishDiscoveryTopics();

struct Shade {
  uint16_t ID;
  String name;
  String friendlyName;
  int8_t lastTargetPosition;
  int8_t lastPosition;
  uint8_t samePositionCount;
  uint8_t positionFetchCount;
  void* timer;
};

std::vector<Shade> shades;

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

  Serial.println("Starting up");

  shades.push_back(Shade{0x4EF1, "study_blind", "Study Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0xA51F, "studio_blockout_left_blind", "Studio Blockout Left Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0xDAEF, "studio_blockout_right_blind", "Studio Blockout Right Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0x7687, "studio_left_blind", "Studio Left Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0xB0DE, "studio_right_blind", "Studio Right Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0xB451, "bedroom_door_blockout_blind", "Bedroom Door Blockout Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0x48A6, "bedroom_window_blockout_blind", "Bedroom Window Blockout Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0x9E14, "bedroom_door_blind", "Bedroom Door Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0x061C, "bedroom_window_blind", "Bedroom Window Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0x2959, "bathroom_blind", "Bathroom Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0x6FAD, "kitchen_door_blind", "Kitchen Door Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0xFB21, "kitchen_window_blind", "Kitchen Window Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0x8B10, "living_room_big_window_blind", "Living Room Big Window Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0x3EB8, "living_room_door_blind", "Living Room Door Blind", -1, -1, 0, 0, nullptr});
  shades.push_back(Shade{0x5463, "living_room_window_blind", "Living Room Window Blind", -1, -1, 0, 0, nullptr});

  powerView.setPacketCallback(processPacket);
  if (!powerView.begin()) {
    Serial.println("Failed to start RFPowerView");
    return;
  }

  client.setKeepAlive(10);
  client.enableLastWillMessage("hotdog/availability", "offline", true);
  client.enableDebuggingMessages();
  client.setMaxPacketSize(2048);

  delay(100);
  Serial.println("Ready");
}

void loop() {
  powerView.loop();
  client.loop();
  timer.tick();
}

void processPacket(const Packet *packet) {
  // Update last rolling codes each time a packet from a real hub is detected
  if (packet->source == 0x0000) {
    lastRollingCode1 = packet->rollingCode1;
    lastRollingCode2 = packet->rollingCode2;
  }

  if (packet->type == PacketType::FIELDS) {
    FieldsParameters parameters = std::get<FieldsParameters>(packet->parameters);
    for (size_t i = 0; i < parameters.fields.size(); i++) {
      Field field = parameters.fields[i];
      if (field.identifier == 0x50) {
        for (size_t i = 0; i < shades.size(); i++) {
          if (packet->source == shades[i].ID) {
            uint16_t value = std::get<uint16_t>(field.value);
            uint8_t position = (uint8_t)std::round(((float)value / 0xFFFF) * 100);

            if (shades[i].lastPosition == position) {
              shades[i].samePositionCount++;
            } else {
              shades[i].samePositionCount = 1;
            }
            shades[i].lastPosition = position;

            publishPosition(shades[i].name, position);
          }
        }
      }
    }
  }
}

void onConnectionEstablished() {
  Serial.println("Connection established");

  for (size_t i = 0; i < shades.size(); i++) {
    client.subscribe("hotdog/" + shades[i].name + "/set", processSetMessage);
    client.subscribe("hotdog/" + shades[i].name + "/set_position", processSetPositionMessage);
  }

  client.publish("hotdog/availability", "online", true);

  publishDiscoveryTopics();

  client.subscribe("homeassistant/status", [] (const String& topic, const String& message) {
    if (message == "online") {
      publishDiscoveryTopics();
    }
  });
}

bool sendOpenPacket(uint16_t destination) {
  Packet packet;
  packet.destination = destination;
  packet.source = 0x0000;
  packet.type = PacketType::OPEN;
  
  return sendPacket(&packet);
}

bool sendClosePacket(uint16_t destination) {
  Packet packet;
  packet.destination = destination;
  packet.source = 0x0000;
  packet.type = PacketType::CLOSE;
  
  return sendPacket(&packet);
}

bool sendStopPacket(uint16_t destination) {
  Packet packet;
  packet.destination = destination;
  packet.source = 0x0000;
  packet.type = PacketType::STOP;
  
  return sendPacket(&packet);
}

bool sendSetPosition(uint16_t destination, float percentage) {
  Packet packet;
  packet.destination = destination;
  packet.source = 0x0000;
  packet.type = PacketType::FIELD_COMMAND;
  
  std::vector<Field> fields;
  
  uint8_t identifier = 0x50; // position
  FieldType type = FieldType::SET;
  uint16_t position = (uint16_t)(0xFFFF * percentage);
  fields.push_back(Field{identifier, type, true, position});
  
  packet.parameters = FieldsParameters {fields};
  
  return sendPacket(&packet);
}

bool sendFetchPosition(uint16_t destination) {
  Packet packet;
  packet.destination = destination;
  packet.source = 0x0000;
  packet.type = PacketType::FIELD_COMMAND;
  
  std::vector<Field> fields;
  
  uint8_t identifier = 0x50; // position
  FieldType type = FieldType::FETCH;
  fields.push_back(Field{identifier, type, false, std::monostate{}});
  
  packet.parameters = FieldsParameters {fields};
  
  return sendPacket(&packet);
}

bool sendPacket(Packet *packet) {
  packet->rollingCode1 = lastRollingCode1 + 1;
  packet->rollingCode2 = lastRollingCode2 + 1;

  bool didSend = powerView.sendPacket(packet);
  if (!didSend) {
    Serial.println("Failed to send");
    return false;
  } else {
    lastRollingCode1++;
    lastRollingCode2++;
    return true;
  }
}

void processSetMessage(const String &topic, const String &payload) {
  int startIndex = topic.indexOf("/") + 1;
  int endIndex = topic.indexOf("/", startIndex);
  String shadeName = topic.substring(startIndex, endIndex);

  for (size_t i = 0; i < shades.size(); i++) {
    if (shades[i].name == shadeName) {
      if (payload == "OPEN") {
        sendOpenPacket(shades[i].ID);

        startFetchingPosition(shades[i].ID, 100);

        publishState(shades[i].name, "opening");
      } else if (payload == "CLOSE") {
        sendClosePacket(shades[i].ID);

        startFetchingPosition(shades[i].ID, 0);

        publishState(shades[i].name, "closing");
      } else if (payload == "STOP") {
        sendStopPacket(shades[i].ID);

        startFetchingPosition(shades[i].ID, -1);
        timer.in(100, sendFetchPosition, shades[i].ID);
      }
    }
  }
}

void processSetPositionMessage(const String& topic, const String &payload)  {
  int startIndex = topic.indexOf("/") + 1;
  int endIndex = topic.indexOf("/", startIndex);
  String shadeName = topic.substring(startIndex, endIndex);

  for (size_t i = 0; i < shades.size(); i++) {
    if (shades[i].name == shadeName) {
      float percentage = payload.toInt() / 100.0f;
      sendSetPosition(shades[i].ID, percentage);

      if (payload.toInt() > shades[i].lastPosition) {
        publishState(shades[i].name, "opening");
      } else if (payload.toInt() < shades[i].lastPosition) {
        publishState(shades[i].name, "closing");
      }

      startFetchingPosition(shades[i].ID, payload.toInt());
    }
  }
}

bool checkPosition(uint16_t shadeID) {
  for (size_t i = 0; i < shades.size(); i++) {
    if (shades[i].ID == shadeID) {
      // Keep fetching position if:
      // - the last reported position doesn't match the target position
      // - a position hasn't been reported yet
      // - there is no target position
      if (shades[i].lastTargetPosition != shades[i].lastPosition || shades[i].lastPosition == -1 || shades[i].lastTargetPosition == -1) {
        // Keep fetching position if the count limits have not been reached
        if (shades[i].positionFetchCount < MAX_FETCH_COUNT && shades[i].samePositionCount < 2) {
          shades[i].positionFetchCount++;
          sendFetchPosition(shadeID);
          return true;
        }
      }

      publishState(shades[i].name, shades[i].lastPosition > 0 ? "open" : "closed");
      return false;
    }
  }
  return false;
}

void startFetchingPosition(uint16_t shadeID, int8_t targetPosition) {
  for (size_t i = 0; i < shades.size(); i++) {
    if (shades[i].ID == shadeID) {
      // Cancel any existing timer
      if (shades[i].timer != nullptr) {
        timer.cancel(shades[i].timer);
        shades[i].timer = nullptr;
      }

      shades[i].lastTargetPosition = targetPosition;
      shades[i].positionFetchCount = 0;
      shades[i].samePositionCount = 1;

      shades[i].timer = timer.every(2000, checkPosition, shades[i].ID);
    }
  }
}

void publishState(const String& shadeName, const String& state) {
  client.publish("hotdog/" + shadeName + "/state", state, true);
}

void publishPosition(const String& shadeName, const uint8_t position) {
  client.publish("hotdog/" + shadeName + "/position", String(position), true);
}

void publishDiscoveryTopics() {
  char buffer[1024];

  for (size_t i = 0; i < shades.size(); i++) {
    Shade shade = shades[i];
    String objectID = String(shade.ID, HEX);

    JsonDocument doc;

    doc["name"] = shade.friendlyName;
    doc["unique_id"] = objectID;
    doc["availability_topic"] = "hotdog/availability";
    doc["state_topic"] = "hotdog/" + shade.name + "/state";
    doc["command_topic"] = "hotdog/" + shade.name + "/set";
    doc["position_topic"] = "hotdog/" + shade.name + "/position";
    doc["set_position_topic"] = "hotdog/" + shade.name + "/set_position";
    doc["position_open"] = 100;
    doc["position_closed"] = 0;
    doc["optimistic"] = false;

    serializeJson(doc, buffer);
    
    client.publish("homeassistant/cover/" + objectID + "/config", buffer);
  }
}