#include "robomaster.h"

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

#include <stdlib.h>
#include <string.h>

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

void client_connect(Client client) {
    client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1");
    // TODO: This should probably use the source IP in the response body
    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) {

    struct Connection* conn = connection_poll_ready(client);
    connection_read(conn, resp);

}

void drive(Client client, float x, float y, float r) {
    float w1 = y + x + r, w2 = y - x -r, w3 = y + x -r, w4 = y - x + r;
    set_wheel_speed(client, w1 * 1000, -w2 * 1000, -w3 * 1000, w4 * 1000);
}