#include "wakaama-client.h"
#include <liblwm2m.h>
#include <USB/USBAPI.h>
#include <Dns.h>
#include <Ethernet.h>


#ifndef LWM2M_WITH_LOGS
// Same usage as C89 printf()
extern void lwm2m_printf(const char * format, ...);
#endif

/**
 * "coap://localhost:5683"
 * @param uri
 * @todo init and use uri in security object
 */
void ArduinoClient::init() {
    SerialUSB.println(F("init():start"));
    memset(&data, 0, sizeof(client_data_t));

    const __FlashStringHelper * objFail = F("Failed to create object");

    // create udp listener
    data.udp = new EthernetUDP();
    data.udp->begin(localPort);

    // init objects
    SerialUSB.println(F("*object:security"));
    objArray[0] = get_security_object(uri);
    if (nullptr == objArray[0])
    {
        SerialUSB.println(objFail);
        exit(0);
    }
    data.securityObjP = objArray[0];

    SerialUSB.println(F("*object:server"));
    objArray[1] = get_server_object();
    if (nullptr == objArray[1])
    {
        SerialUSB.println(objFail);
        exit(0);
    }

    SerialUSB.println(F("*object:device"));
    objArray[2] = get_object_device();
    if (nullptr == objArray[2])
    {
        SerialUSB.println(objFail);
        exit(0);
    }

    SerialUSB.println(F("*object:test"));
    objArray[3] = get_test_object();
    if (nullptr == objArray[3])
    {
        SerialUSB.println(objFail);
        exit(0);
    }

    /*
     * The liblwm2m library is now initialized with the functions that will be in
     * charge of communication
     */
    SerialUSB.println(F("*lwm2m_init()"));
    lwm2mH = lwm2m_init(&data);
    if (NULL == lwm2mH)
    {
        SerialUSB.println(F("lwm2m_init() failed"));
        exit(0);
    }

    /*
     * We configure the liblwm2m library with the name of the client - which shall be unique for each client -
     * the number of objects we will be passing through and the objects array
     */
    SerialUSB.println(F("*lwm2m_configure()"));
    result = lwm2m_configure(lwm2mH, name, NULL, NULL, OBJ_COUNT, objArray);
    if (result != 0)
    {
        SerialUSB.println(F("lwm2m_configure() failed"));
        exit(0);
    }
    SerialUSB.println(F("init():done"));
}


void ArduinoClient::doWorkStep() {
    SerialUSB.println(F("doWorkStep():start"));

    /*
     * This function does two things:
     *  - first it does the work needed by liblwm2m (eg. (re)sending some packets).
     *  - Secondly it adjusts the timeout value (default 60s) depending on the state of the transaction
     *    (eg. retransmission) and the time before the next operation
     */
    SerialUSB.println(F("lwm2m_step()"));
    result = lwm2m_step(lwm2mH, &step_delay);
    if (result != 0)
    {
        SerialUSB.print(F("lwm2m_step() failed"));
        exit(0);
    }

    // wait for socket event
    SerialUSB.println(F("parsePacket()"));
    int packetSize = data.udp->parsePacket();
    if (packetSize) {
        int numBytes = data.udp->read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);

        connection_t * connP;
        connP = connection_find(data.connList, data.udp);
        if (connP != nullptr)
        {
            /*
             * Let liblwm2m respond to the query depending on the context
             */
            lwm2m_handle_packet(lwm2mH, (uint8_t*)packetBuffer, numBytes, connP);
        }
    }
    SerialUSB.println(F("doWorkStep():done"));
}

/**
 * Destructor, actually this won't ever run on arduino...
 */
ArduinoClient::~ArduinoClient() {
    lwm2m_close(lwm2mH);
    data.udp->stop();
    connection_free(data.connList);

    free_security_object(objArray[0]);
    free_server_object(objArray[1]);
    free_object_device(objArray[2]);
    free_test_object(objArray[3]);
}

/**
 * called from registration.c prv_register()
 *
 * @param secObjInstID
 * @param userData
 * @return
 */
void * lwm2m_connect_server(uint16_t secObjInstID, void *userData) {
    client_data_t * dataP;
    char * uri;
    char * host;
    char * port;
    connection_t * newConnP = nullptr;

    dataP = (client_data_t *)userData;

    uri = get_server_uri(dataP->securityObjP, secObjInstID);

    if (uri == nullptr) return nullptr;

    SerialUSB.print(F("Connecting to "));
    SerialUSB.println(uri);

    // parse uri in the form "coaps://[host]:[port]"
    if (0 == strncmp(uri, "coaps://", strlen("coaps://")))
    {
        host = uri+strlen("coaps://");
    }
    else if (0 == strncmp(uri, "coap://", strlen("coap://")))
    {
        host = uri+strlen("coap://");
    }
    else
    {
        lwm2m_free(uri);
        return (void *)newConnP;
    }
    port = strrchr(host, ':');
    if (port == nullptr) {
        lwm2m_free(uri);
        return (void *)newConnP;
    }
    // remove brackets
    if (host[0] == '[')
    {
        host++;
        if (*(port - 1) == ']')
        {
            *(port - 1) = 0;
        }
        else {
            lwm2m_free(uri);
            return (void *)newConnP;
        }
    }
    // split strings
    *port = 0;
    port++;

    IPAddress * remoteIp = new IPAddress();
    if(!remoteIp->fromString(host)) {
        DNSClient dns;
        dns.begin(Ethernet.dnsServerIP());
        dns.getHostByName(host, *remoteIp);
    }

    String portStr = port;

    newConnP = connection_create(dataP->connList, dataP->udp, remoteIp, portStr.toInt());
    if (newConnP == nullptr) {
        SerialUSB.println(F("Connection creation failed"));
    }
    else {
        dataP->connList = newConnP;
    }
    SerialUSB.println(F("Connection created"));

    lwm2m_free(uri);
    return (void *)newConnP;
};

/**
 * called from
 *
 * @param sessionH
 * @param userData
 */
void lwm2m_close_connection(void *sessionH, void *userData) {
    client_data_t * app_data;
    connection_t * targetP;

    app_data = (client_data_t *)userData;
    targetP = (connection_t *)sessionH;

    if (targetP == app_data->connList)
    {
        app_data->connList = targetP->next;
        lwm2m_free(targetP);
    }
    else
    {
        connection_t * parentP;

        parentP = app_data->connList;
        while (parentP != nullptr && parentP->next != targetP)
        {
            parentP = parentP->next;
        }
        if (parentP != nullptr)
        {
            parentP->next = targetP->next;
            lwm2m_free(targetP);
        }
    }
};

void lwm2m_printf(const char * format, ...)
{
    SerialUSB.println(format);
}