icfp11/src/control.c

245 regels
5.9 KiB
C

#include "control.h"
#include <sys/socket.h>
#include <netdb.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <poll.h>
static int opensocket(const char *hostname, const char *port) {
struct addrinfo *res = NULL, pref;
int ret, s, val;
pref.ai_flags = 0;
pref.ai_family = PF_UNSPEC;
pref.ai_socktype = SOCK_STREAM;
pref.ai_protocol = 0;
pref.ai_addrlen = 0;
pref.ai_addr = NULL;
pref.ai_canonname = NULL;
pref.ai_next = NULL;
if (0 != (ret = getaddrinfo(hostname, port, &pref, &res)) || !res) {
fprintf(stderr, "Couldn't find host/port '%s:%s': %s\n", hostname, port, strerror(errno));
return -1;
}
if (-1 == (s = socket(res->ai_family, res->ai_socktype, res->ai_protocol))) {
fprintf(stderr, "Couldn't create socket: %s\n", strerror(errno));
freeaddrinfo(res);
return -1;
}
if (0 != connect(s, res->ai_addr, res->ai_addrlen)) {
fprintf(stderr, "Couldn't connect socket to '%s:%s': %s\n", hostname, port, strerror(errno));
freeaddrinfo(res);
close(s);
return -1;
}
val = 1;
if (0 != setsockopt(s, IPPROTO_TCP, TCP_NODELAY, &val, sizeof(val))) {
fprintf(stderr, "Couldn't set socket option TCP_NODELAY: %s\n", strerror(errno));
freeaddrinfo(res);
close(s);
return -1;
}
freeaddrinfo(res);
return s;
}
static void telemetry_free(gpointer _te, gpointer x) {
telemetry *te = (telemetry*) _te;
UNUSED(x);
g_array_free(te->objects, TRUE);
g_slice_free(telemetry, te);
}
telemetry* telemetry_new() {
telemetry *te = g_slice_new0(telemetry);
te->objects = g_array_new(FALSE, FALSE, sizeof(object));
return te;
}
void telemetry_print(telemetry *t){
fprintf(stderr,"Telemetry object:\n");
fprintf(stderr,"\tTimestamp: %d Number of Objects: %d Vehicle:\n",t->ts,t->objects->len) ;
vehicle_print(&t->vehicle);
}
guint object_hash(gconstpointer _o) {
object *o = (object*) _o;
const gchar *p = (const gchar*) (&o->x);
gsize n = 2 * sizeof(double);
guint h = 0;
while (n--) { h = (h << 5) - h + *p; p++; }
return h;
}
gboolean object_equal(gconstpointer _a, gconstpointer _b) {
object *a = (object*) _a, *b = (object*) _b;
return (a->type == b->type && a->x == b->x && a->y == b->y && a->rad == b->rad);
}
object* object_new(object_t type, double x, double y, double rad, double dir, double speed) {
object *o = g_slice_new(object);
o->type = type; o->x = x; o->y = y; o->rad = rad; o->dir = dir; o->speed = speed;
return o;
}
void _object_free(gpointer p) {
g_slice_free(object, (object*) p);
}
trial *trial_new(const char *hostname, const char *port) {
trial *t;
int sock;
if (-1 == (sock = opensocket(hostname, port))) return NULL;
t = g_slice_new0(trial);
t->socket = sock;
g_queue_init(&t->telemetry);
t->map.solid_objects = g_hash_table_new_full(object_hash, object_equal, NULL, _object_free);
t->sim.tm.objects = g_array_new(FALSE, FALSE, sizeof(object));
control_parser_new(t);
t->runcnt = 0;
return t;
}
void trial_reset_run(trial *t) {
g_queue_foreach(&t->telemetry, telemetry_free, NULL);
g_queue_clear(&t->telemetry);
t->last_ts = 0;
t->runcnt++;
control_parser_reset(t);
}
int trial_wait_for_start(trial *t) {
while (!t->finished && !t->alive) {
if (-1 == trial_wait_for_input(t)) return -1;
}
return 0;
}
static char buffer[16*1024];
int trial_check_input(trial *t) {
ssize_t len;
struct pollfd p;
p.fd = t->socket;
p.events = POLLIN;
switch (poll(&p, 1, 0)) {
case -1:
switch (errno) {
case EAGAIN:
case EINTR:
return 0;
}
fprintf(stderr, "poll error: %s\n", strerror(errno));
return -1;
case 0:
return 0;
}
len = read(t->socket, buffer, sizeof(buffer));
if (len < 0) {
fprintf(stderr, "read error: %s\n", strerror(errno));
return -1;
} else if (len == 0) {
t->finished = 1;
if (t->alive) {
fprintf(stderr, "unexpected connection close\n");
return -1;
}
return 0;
}
g_string_append_len(t->parse_ctx->buffer, buffer, len);
return (RUN_ERROR != control_parse(t)) ? 0 : -1;
}
int trial_wait_for_input(trial *t) {
if (t->parse_ctx->buffer->len) return trial_check_input(t);
ssize_t len = read(t->socket, buffer, sizeof(buffer));
if (len < 0) {
if (errno == ECONNRESET) {
len = 0; /* treat as simply connection close */
} else {
fprintf(stderr, "read error: %s\n", strerror(errno));
return -1;
}
}
if (len == 0) {
if (t->alive) {
fprintf(stderr, "unexpected connection close\n");
return -1;
}
t->finished = 1;
return 0;
}
g_string_append_len(t->parse_ctx->buffer, buffer, len);
return (RUN_ERROR != control_parse(t)) ? 0 : -1;
}
void trial_free(trial *t) {
shutdown(t->socket, SHUT_RDWR);
close(t->socket);
g_queue_foreach(&t->telemetry, telemetry_free, NULL);
g_queue_clear(&t->telemetry);
g_hash_table_destroy(t->map.solid_objects);
g_array_free(t->sim.tm.objects, TRUE);
control_parser_free(t);
g_slice_free(trial, t);
}
void trial_print(trial *t){
fprintf(stderr,"Trial Alive: %d Last Timestep: %d Socket: %d\n",t->alive,t->last_ts,t->socket);
/*perhaps add map_print and vehicle_print*/
}
void telemetry_matlab(gpointer tele, gpointer file){
FILE* outfile;
telemetry* t;
vehicle * v;
outfile = (FILE*) file;
t = (telemetry*) tele;
v = &t->vehicle;
fprintf(outfile,"v(end+1,:) = [%d %f, %f, %f, %f];\n",t->ts,v->x,v->y,v->dir,v->speed);
}
/*prints coordinate history to matlab compatible file*/
void trial_matlab(trial *t){
FILE* outfile;
char name[80];
sprintf(name,"velocity%d.m",t->runcnt);
outfile = fopen(name,"w");
fprintf(outfile,"v = [];\n");
g_queue_foreach(&t->telemetry,telemetry_matlab,outfile);
fclose(outfile);
}
/* debugging stuff */
void object_print(object *o){
fprintf(stderr,"Object: Type: %d x: %f, y: %f, rad: %f dir: %f speed: %f\n",o->type,o->x,o->y,o->rad,o->dir,o->speed);
}
void vehicle_print(vehicle *v){
fprintf(stderr,"Vehicle: Accel: %d Turn: %d x: %f y: %f dir: %f speed: %f\n",v->accel,v->turn,v->x,v->y,v->dir,v->speed);
}
// void map_print(map *m){}