#include "robomaster.h"

#include "client.h"
#include "message.h"
#include "connection.h"

#include <stdlib.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <netinet/in.h>
#include <string.h>
#include <stdio.h>

Client client_new() {
    struct Client* client = malloc(sizeof(struct Client));
    memset(client, 0, sizeof(struct Client));
    // TODO: Make this configurable
    client->hostbyte = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_CLIENT_INDEX);
    return client;
}

void client_connect(Client client) {
    client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1");
    client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1");

    set_sdk_connection(client, CONNECTION_WIFI_AP, 0, 10010);
}

void poll_message(Client client, union Message* resp) {

    memset(resp, 0, sizeof(union Message));

    // Poll for messages
    static struct timeval timeout = {0, 0};
    fd_set read_fds;
    FD_ZERO(&read_fds);
    FD_SET(client->sdk_conn->sockfd, &read_fds);
    FD_SET(client->dev_conn->sockfd, &read_fds);
    int result = select(max_fd + 1, &read_fds, NULL, NULL, NULL);

    // Check for socket polling errors
    if(result < 0) {
        perror("message polling failed");
	exit(EXIT_FAILURE);
    }

    // Skip if nothing was received yet
    // TODO: Make a static "empty" message or something
    if (result == 0)
        return;

    // Read a message from the sockets
    for(int i = 0; i < 2; i++)
    {

        if(!FD_ISSET(client->conns[i]->sockfd, &read_fds))
            continue;

        int recvb = recvfrom(client->sdk_conn->sockfd, resp, sizeof(union Message), 0, (struct sockaddr*)&client->sdk_conn->remote_addr, &client->sdk_conn->addrlen);

        // Check for socket read errors
        if(recvb < 0) {
            perror("reading socket failed");
            exit(EXIT_FAILURE);
        }

        // Check for message errors
        if(message_validate(resp) != MESSAGEERR_NONE) {
            perror("invalid message");
            exit(EXIT_FAILURE);
        }

	return;

    }

}