#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <pthread.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include "vsoa_server.h"
#include "vsoa_platform.h"

#define MY_SERVER_PASSWD "123456"

extern char ** environ;

static vsoa_server_t *server;

/*
 * Publish axis thread
 */
static void *publish_axis_thread (void *arg)
{
    vsoa_url_t url;
    vsoa_payload_t payload;
    char param[100];
    int roll = 1, pitch = 1, yaw = 1;

    url.url     = "/axis";
    url.url_len = strlen(url.url);

    payload.data = NULL;
    payload.data_len  = 0;
    payload.param = param;

    while (1) {
        sleep(1);

        if (!vsoa_server_is_subscribed(server, &url)) {
            continue;
        }

        payload.param_len = sprintf(param,
                                    "{\"roll\": %d, \"pitch\": %d, \"yaw\": %d}",
                                    roll++, pitch++, yaw++);

        vsoa_server_publish(server, &url, &payload);
    }

    return (NULL);
}

int main (int argc, char **argv)
{
    int cnt, max_fd, exits, count = 0;
    fd_set fds;
    pthread_t id;
    struct sockaddr_in addr;
    struct timespec timeout = { 1, 0 };
    char *auto_key = "VSOA_AUTO_PORT=";
    char *vsoa_auto_port="\0";
    char **env = environ;
    unsigned short  server_port = 0;

    while (*env) {
        exits = memcmp(*env, auto_key, (size_t)strlen(auto_key));
        if (exits == 0) {
            vsoa_auto_port = *env;
        }
        env++;
    }

    if (strlen(vsoa_auto_port) > 1) {
        const char s[2] = "=";
        char *port_str="\0";
        char *token;

        token = strtok(vsoa_auto_port, s);
        while (token != NULL) {
            port_str = token;
            token = strtok(NULL, s);
        }

        server_port = lib_atoi(port_str);
        printf("env vsoa_auto_port: %d\n", server_port);
    }

    bzero(&addr, sizeof(struct sockaddr_in));
    addr.sin_family      = AF_INET;
    addr.sin_port        = htons(server_port);
    addr.sin_addr.s_addr = INADDR_ANY;

#ifdef VSOA_HAS_SIN_LEN
    addr.sin_len = sizeof(struct sockaddr_in);
#endif

    /*
    * Initialize server
    */
    server = vsoa_server_create("{\"name\":\"axis_server\"}");
    if (!server) {
        fprintf(stderr, "Can not create VSOA server!\n");
        return  (-1);
    }

    /*
    * If need password
    */
    vsoa_server_passwd(server, MY_SERVER_PASSWD);

    /*
    * Start server
    */
    if (!vsoa_server_start(server, (struct sockaddr *)&addr, sizeof(struct sockaddr_in))) {
        vsoa_server_close(server);
        fprintf(stderr, "Can not start VSOA server!\n");
        return  (-1);
    }

    /*
    * Create publish thread
    */
    pthread_create(&id, NULL, publish_axis_thread, NULL);

    while (1) {
        FD_ZERO(&fds);
        max_fd = vsoa_server_fds(server, &fds);

        cnt = pselect(max_fd + 1, &fds, NULL, NULL, &timeout, NULL);
        if (cnt > 0) {
            vsoa_server_input_fds(server, &fds);
        }
    }

    return (0);
}