icfp11/src/path.c

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "path.h"

command *command_new(timestamp ts, accel_t a, turn_t t){
	command* res;
	res = g_slice_new(command);
	res->ts = ts;
	res->accel = a;
	res->turn = t;
	return res;
}

void command_free(command* c){
	g_slice_free(command, c);
}

path *path_new(map* m,vehicle *v){
	command* tmp;
	path* res;
	double angle, stop;

	res = g_slice_new(path);
	res->commands = g_queue_new();
	/* Calculate a trivial path to the origin*/

	/* Turn towards origin, take shorter direction*/
	angle = atan(v->y/v->x);
	stop = abs(angle - v->dir)/m->max_turn;
	if(angle - v->dir > 0){
		/*clockwise/left turn*/
		tmp = command_new(0,BREAK,TURN_LEFT);
		g_queue_push_tail(res->commands,tmp);
	} else {
		/*counterclockwise/right turn*/
		tmp = command_new(0,BREAK,TURN_RIGHT);
		g_queue_push_tail(res->commands,tmp);
	}
	tmp = command_new(stop,BREAK,TURN_STRAIGHT);
	g_queue_push_tail(res->commands,tmp);

	/*start driving*/
	tmp = command_new(stop,ACCEL,TURN_STRAIGHT);
	g_queue_push_tail(res->commands,tmp);
	return res;
}

void path_execute(trial* t,path* p){
	command* tmp;
	tmp = (command*) g_queue_peek_head(p->commands);
	if(tmp == NULL){
		fprintf(stderr,"warning: cannot execute empty path\n");
	}
	if(t == NULL){
		fprintf(stderr,"trial is null\n");
		return;
	}
	/*magic number for latency, send messages that much earlier*/
	while(tmp != NULL && tmp->ts - t->last_ts < 20){
		tmp = (command*) g_queue_pop_head(p->commands);
		switch(tmp->turn){
			case TURN_HARD_LEFT: vehicle_hard_left(t); break;
			case TURN_LEFT: vehicle_left(t); break;
			case TURN_STRAIGHT: vehicle_straight(t); break;
			case TURN_RIGHT: vehicle_right(t); break;
			case TURN_HARD_RIGHT: vehicle_hard_right(t); break;
		}
		switch(tmp->accel){
			case ACCEL: vehicle_accel(t); break;
			case ROLL: vehicle_roll(t); break;
			case BREAK: vehicle_break(t); break;
		}
		command_free(tmp);
		tmp = (command*) g_queue_peek_head(p->commands);
	}
}


void path_free(path* p){
	int length;
	command* tmp;
	length = g_queue_get_length(p->commands);
	if(length > 0){
		fprintf(stderr,"Freeing non-empty path:\n");
		while(g_queue_get_length(p->commands) > 0){
			tmp = g_queue_pop_head(p->commands);
			fprintf(stderr,"\tTimestamp: %d	Turn: %d Accel: %d\n",tmp->ts,tmp->turn,tmp->accel);
			command_free(tmp);
		}
	}
	g_queue_free(p->commands);
	g_slice_free(path,p);
}