#ifndef VIZBLOCKS_H
#define VIZBLOCKS_H
#include "Arduino.h"
#include "Behaviours.h"
#include "ServoBehaviours.h"
#include "LEDBehaviours.h"

#include <ESP8266WiFi.h>
#include "Adafruit_MQTT.h"
#include "Adafruit_MQTT_Client.h"

#define MQTT_topic  "new001"




class VizBlocks {
  char* _ssid;
  char* _wifi_pass;
  char* _id;
  char* _server;
  int _port;
  BehaviourTable _behaviours;
  Behaviour* _active = nullptr;
  int _loop_time = 5;
  Adafruit_MQTT_Client* _mqtt;
  Adafruit_MQTT_Subscribe* _device_subscription;
  Adafruit_MQTT_Publish* _announce;
  Adafruit_MQTT_Publish* _my_announce;
  WiFiClient* _client;
  String _my_announce_channel;
  boolean _wifi;

public:
  VizBlocks(char* id, char* ssid="VizBlocks", char* wifi_pass="dataisfun",
    char* server="192.168.4.1",int port=1883) : _id(id), _server(server), _port(port), _ssid(ssid), _wifi_pass(wifi_pass) {} ;

  void command_callback(char *data, uint16_t len) {
    Serial.println("Got command: ");
    Serial.println(data);
  }

  void set_wifi(boolean v ) { _wifi = v; }


  /*
   * Set up the VizBlocks node - WiFi, MQTT
   */
  void init() {
    Serial.setTimeout(100);
    Serial.println();
    Serial.println(F("VizBlocks Node starting up"));
    Serial.println("Initialising " + String(_id));

    if( _wifi ) {

      WiFi.mode(WIFI_STA);

      // Connect to WiFi access point.
      Serial.println();
      Serial.print("Connecting to ");
      Serial.println(_ssid);

      WiFi.begin(_ssid, _wifi_pass);
      while (WiFi.status() != WL_CONNECTED) {
       delay(500);
       Serial.print(".");
      }
      Serial.println();

      Serial.println("WiFi connected");
      Serial.println("IP address: "); Serial.println(WiFi.localIP());
      // Done Wifi

      // Setup MQTT
      _client = new WiFiClient();
      _mqtt = new Adafruit_MQTT_Client(_client, _server, _port, "" /* mqttt username */, "" /* mqtt pass*/);
      _device_subscription = new Adafruit_MQTT_Subscribe(_mqtt, _id);
      _announce = new Adafruit_MQTT_Publish(_mqtt, "announce");
      _my_announce_channel = String("announce/") + String(_id);
      _my_announce = new Adafruit_MQTT_Publish(_mqtt, _my_announce_channel.c_str());

      // Setup MQTT subscription for this device
      _mqtt->subscribe(_device_subscription);
      // This *would* setup a callback, but we're not doing this right now...
      //_device_subscription->setCallback(test_callback);

      MQTT_connect();
    }

    announce_capabilities();
    Serial.println("Init finished");

  }



  /*
   * Add a behaviour to the list of possible behaviours
   */
  void add(Behaviour *b) {
    _behaviours.add(b);
  }

  /*
   * This is the main loop. It should be called from within loop() - really
   * this function is the only thing you should need to call. It will manage
   * it's own delay, so you can call as often as possible.
   */
  void vis_loop() {
    int loop_start_time = millis();
    serial_command();
    if( _wifi ) { mqtt_command(); }
    if( _active ) {
      //Serial.println("Updating "+_active->name());
      _active -> update();
      if( ! _active->is_running() ) _active = nullptr;
    }
    int loop_time_taken = millis()-loop_start_time;
    if( loop_time_taken < _loop_time ) {
      delay( _loop_time - loop_time_taken );
    }
  }

  /*
   * Read a command from the serial input and process it
   */
  void serial_command() {
    if( Serial.available() ) {
      String cmd = Serial.readStringUntil('\n');
      Serial.println(process(cmd));
    }
  }

  /*
   * Read a command from the serial input and process it. It only waits for
   * 50ms to allow other behaviours to continue.
   */
  void mqtt_command() {
      MQTT_connect(); //ensure connection
      Adafruit_MQTT_Subscribe *subscription;
      while ((subscription = _mqtt->readSubscription(50))) {
        if (subscription == _device_subscription) {
          Serial.print(F("Got: "));
          Serial.println((char *)_device_subscription->lastread);
          Serial.println(process((char *)_device_subscription->lastread));
        }
      }
  }

  /*
   * Process a command. This means:
   * - split the command name from the arguments
   * - call process_comman with the separated command and argument string
   */
  String process(String input) {
    int index = input.indexOf(" ");
    String command = "";
    String args = "";

    if( index) {
      command = input.substring(0,index);
      args = input.substring(index+1);
    } else {
      command = input;
    }
    return process_command(command, args);
  }

  /*
   * Process a command and its arguments. This means:
   * - look for a Behaiour with the right name
   * - if found, then call that behaviour with the arguments (which are still a single string)
   */
  String process_command(String command, String args) {
    Serial.println("Processing <"+command+"> <"+args+">");
    Behaviour* b = _behaviours.get(command);
    if(b) {
      if( _active ) { _active->stop(); }
      Serial.println( "Found behaviour: <"+command+">" );
      _active = b;
      return( b->start(args) );
    } else {
      return "Couldn't process command: " + command;
    }
  }

  /*
  * Function to connect and reconnect as necessary to the MQTT server.
  */
  // Should be called in the loop function and it will take care if connecting.
  void MQTT_connect() {
    int8_t ret;

    // Stop if already connected.
    if (_mqtt->connected()) {
      return;
    }

    Serial.print("Connecting to MQTT... ");

    uint8_t retries = 3;
    while ((ret = _mqtt->connect()) != 0) { // connect will return 0 for connected
      Serial.println(_mqtt->connectErrorString(ret));
      Serial.println("Retrying MQTT connection in 5 seconds...");
      _mqtt->disconnect();
      delay(5000);  // wait 5 seconds
      retries--;
      if (retries == 0) {
        // basically die and wait for WDT to reset me
        while (1);
      }
    }
    Serial.println("MQTT Connected!");
  }

  void announce_capabilities() {
    Serial.print("Node ");
    Serial.println(_id);
    if( _wifi ) {
      if( ! _announce->publish(_id) ) { Serial.println("Couldn't make announcement"); }
    }
    for( int i = 0; i < _behaviours.get_num_behaviours(); i++ ) {
      Behaviour* b = _behaviours.get_by_num(i);
      String doc = b->name() + ": " + b->args();
      if( _wifi ) { _my_announce->publish(doc.c_str()); }
      Serial.print("-->");
      Serial.println(doc);
    }
  }

};



/* Left here temporarily - can't use a member function as a callback
   So this was the start of a way to create a static function that would work
void* current_vizblocks;

void test_callback(char *data, uint16_t len) {
  Serial.println("Callback: ");
  Serial.println(data);
  ((VizBlocks*)current_vizblocks)->process(String(data));
}
*/


#endif