#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->connection = connection_new();

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

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

    // Poll for messages
    static struct timeval timeout = {0, 0};
    fd_set read_fds;
    FD_ZERO(&read_fds);
    FD_SET(client->connection->sockfd, &read_fds);
    int result = select(client->connection->sockfd + 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 socket
    // TODO: Use union to make all messages same size
    int recvb = recvfrom(client->connection->sockfd, resp, sizeof(union Message), 0, (struct sockaddr*)&client->connection->remote_addr, &client->connection->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);
    }

}