#include <ESP8266WiFi.h>
#include <PubSubClient.h>
#include <Adafruit_MCP23X17.h>

#include "config.h"
#define MCP_INPUTS 16 // MCP23017
#define TRIGGER_TIME 1500

WiFiClient espClient;
PubSubClient client(espClient);
Adafruit_MCP23X17 mcp_relais;
Adafruit_MCP23X17 mcp_feedback;

unsigned long lastMsg = 0;
#define MSG_BUFFER_SIZE	(50)
char msg[MSG_BUFFER_SIZE];

#define TOPIC_BUFFER_SIZE (50)
char topic_recv[TOPIC_BUFFER_SIZE];
char topic_send[TOPIC_BUFFER_SIZE];

void setup_wifi()
{
    delay(10);
    Serial.println("Connecting Wifi ...");

    WiFi.mode(WIFI_STA);
    WiFi.begin(WLAN_SSID, WLAN_PASS);

    while (WiFi.status() != WL_CONNECTED) 
    {
        delay(500);
    }

    randomSeed(micros());
}

void callback(char* topic, byte* payload, unsigned int length)
{
    Serial.println("Receive message");

    int topic_len = (int)strlen(topic);
    int trigger_len = (int)strlen("/trigger");
    int set_state_len = (int)strlen("/set_state");
    if(!strcmp(topic + topic_len - trigger_len, "/trigger"))
    {
        char str_id[4] = {0};
        strncpy(str_id, topic  + topic_len - trigger_len - 3, 3);
        int id = atoi(str_id);

        if ((char)payload[0] == '1')
        {
            mcp_relais.digitalWrite(id, HIGH);
            delay(TRIGGER_TIME);
            mcp_relais.digitalWrite(id, LOW);
        } 
        else 
        {
            mcp_relais.digitalWrite(id, LOW);
            delay(TRIGGER_TIME);
            mcp_relais.digitalWrite(id, HIGH);
        }
    }

    if(!strcmp(topic + topic_len - set_state_len, "/set_state"))
    {
        char str_id[4] = {0};
        strncpy(str_id, topic  + topic_len - trigger_len - 3, 3);
        int id = atoi(str_id);

        if ((char)payload[0] == '1')
        {
            mcp_relais.digitalWrite(id, HIGH);
        } 
        else 
        {
            mcp_relais.digitalWrite(id, LOW);
        }
    }
}

void reconnect()
{
    while (!client.connected()) 
    {
        Serial.println("Connecting MQTT ...");
        String clientId = "FABLOCK-";
        clientId += String(random(0xffff), HEX);
        if (client.connect(clientId.c_str())) 
        {           
            snprintf (topic_recv, TOPIC_BUFFER_SIZE, "fablock/%03d/+/trigger", FABLOCKID);
            client.subscribe(topic_recv);
            snprintf (topic_recv, TOPIC_BUFFER_SIZE, "fablock/%03d/+/set_state", FABLOCKID);
            client.subscribe(topic_recv);
        }
        else
        {
            Serial.print("failed, rc=");
            Serial.print(client.state());
            Serial.println(" try again in 5 seconds");
            delay(5000);
        }
    }
}

void setup()
{
    int i = 0;

    Serial.begin(115200);
    Serial.println();

    setup_wifi();

    client.setServer(MQTT_BROKER, 1883);
    client.setCallback(callback);

    if (!mcp_relais.begin_I2C(MPC_RELAIS_ADDR)) 
    {
        Serial.println("MCP Error");
        while (1);
    }
    for(i = 0; i<MCP_INPUTS; i++)
    {
        mcp_relais.pinMode(i, OUTPUT);
    }

    if(HAS_FEEDBACK)
    {
        if (!mcp_feedback.begin_I2C(MPC_FEEDBACK_ADDR)) 
        {
            Serial.println("MCP Error");
            while (1);
        }
        for(i = 0; i<MCP_INPUTS; i++)
        {
            mcp_feedback.pinMode(i, INPUT_PULLUP);
        }
    }
}

void loop()
{
    if (!client.connected())
    {
        reconnect();
    }
    client.loop();

    unsigned long now = millis();
    if (now - lastMsg > 30000) 
    {
        lastMsg = now;
        int value;

        Serial.println("Publish State and Feedback");
        for(int i=0; i<MCP_INPUTS; i++)
        {
            value = mcp_relais.digitalRead(i);
            snprintf (msg, MSG_BUFFER_SIZE, "%d", value);
            snprintf (topic_send, TOPIC_BUFFER_SIZE, "fablock/%03d/%03d/state", FABLOCKID, i);
            client.publish(topic_send, msg);
        }

        if(HAS_FEEDBACK)
        {
            for(int i=0; i<MCP_INPUTS; i++)
            {
                value = !mcp_feedback.digitalRead(i);
                snprintf (msg, MSG_BUFFER_SIZE, "%d", value);
                snprintf (topic_send, TOPIC_BUFFER_SIZE, "fablock/%03d/%03d/feedback", FABLOCKID, i);
                client.publish(topic_send, msg);
            }
        }
    }
}