#include "MQTT_Com.h"
#include "MQTTEthernet.h"

int connack_rc = 0; // MQTT connack return code
const char* ip_addr = "";
bool netConnecting = false;
int connectTimeout = 1000;
bool mqttConnecting = false;
bool netConnected = false;
bool connected = false;
int retryAttempt = 0;
char subscription_url[MQTT_MAX_PAYLOAD_SIZE];


int mconnect(MQTT::Client<MQTTEthernet, Countdown, MQTT_MAX_PACKET_SIZE>* client, MQTTEthernet* ipstack) {
    char hostname[strlen(HOSTNAME) + 1];  
    sprintf(hostname, "%s", HOSTNAME);

    // Network debug statements
    LOG("=====================================\n");
    LOG("Connecting Ethernet.\n");
    LOG("Nucleo IP ADDRESS: %s\n", ipstack->getEth().getIPAddress());
    LOG("Nucleo MAC ADDRESS: %s\n", ipstack->getEth().getMACAddress());
    LOG("Server Hostname: %s port: %d\n", hostname, MQTT_PORT);
    LOG("=====================================\n");

    
    ///////// CONNECT TO TCP SOCKET /////////////////
    netConnecting = true;

    int rc = ipstack->connect(hostname, MQTT_PORT);
    if (rc != 0) {
        WARN("IP Stack connect returned: %d\n", rc);
        return rc;
    }
    LOG("--->TCP Connected\n");
    netConnected = true;
    netConnecting = false;

    ///////// CONNECT TO MQTT BROKER /////////////////
    mqttConnecting = true;
    MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
    data.MQTTVersion=3;
    data.struct_version=0;
    data.clientID.cstring = "ETcontroller";

    if ((rc = client->connect(&data)) == 0){
        connected = true;
        LOG("--->MQTT Connected\n");
        mqttConnecting = false;         
    } else {
        WARN("MQTT connect returned %d\n", rc);
    }

    if (rc >= 0)
        connack_rc = rc;
    return rc;
}

int getConnTimeout(int attemptNumber) {  // First 10 attempts try within 3 seconds, next 10 attempts retry after every 1 minute
   // after 20 attempts, retry every 10 minutes
    return (attemptNumber < 10) ? 3 : (attemptNumber < 20) ? 60 : 600;
}

void attemptConnect(MQTT::Client<MQTTEthernet, Countdown, MQTT_MAX_PACKET_SIZE>* client, MQTTEthernet* ipstack) {
    connected = false;
           
    while (mconnect(client, ipstack) != MQTT_CONNECTION_ACCEPTED) {
        if (connack_rc == MQTT_NOT_AUTHORIZED || connack_rc == MQTT_BAD_USERNAME_OR_PASSWORD) {
            WARN ("File: %s, Line: %d Error: %d\n",__FILE__,__LINE__, connack_rc);
            return; // don't reattempt to connect if credentials are wrong
        } 
        int timeout = getConnTimeout(++retryAttempt);
        WARN("Retry attempt number %d waiting %d\n", retryAttempt, timeout);
        
        // if ipstack and client were on the heap we could deconstruct and goto a label where they are constructed
        //  or maybe just add the proper members to do this disconnect and call attemptConnect(...)        
        // this works - reset the system when the retry count gets to a threshold
        if (retryAttempt == 5)
            NVIC_SystemReset();
        else
            wait(timeout);
    }
}

int publish(MQTT::Client<MQTTEthernet, Countdown, MQTT_MAX_PACKET_SIZE>* client, MQTTEthernet* ipstack,char* topic, char* msg) {
    MQTT::Message message;            
    char buf[MQTT_MAX_PAYLOAD_SIZE];
    
    sprintf(buf,"%s" ,msg);
    message.qos = MQTT::QOS0;
    message.retained = false;
    message.dup = false;
    message.payload = (void*)buf;
    message.payloadlen = strlen(buf);
    return client->publish(topic, &message);
}