#ifndef _CONTROL_H
#define _CONTROL_H

#include <unistd.h>
#include <glib.h>

#define INLINE static inline
#define UNUSED(x) ((void)(x))

typedef enum { ACCEL, ROLL, BREAK } accel_t;
typedef enum { TURN_HARD_LEFT, TURN_LEFT, TURN_STRAIGHT, TURN_RIGHT, TURN_HARD_RIGHT } turn_t;

typedef enum { BOLDER, CRATER, MARTIAN } object_t;

struct object;
typedef struct object object;

struct vehicle;
typedef struct vehicle vehicle;

struct telemetry;
typedef struct telemetry telemetry;

struct map;
typedef struct map map;

struct trial;
typedef struct trial trial;

typedef unsigned int timestamp;



struct object {
	object_t type;
	double x, y, rad;
	double dir, speed;
};

struct vehicle {
	accel_t accel;
	turn_t turn;
	double x, y;
	double dir, speed;
};

struct telemetry {
	timestamp ts;
	vehicle vehicle;
	GArray *objects;
};

struct map {
	float dx, dy;
	timestamp limit;
	float min_sensor, max_sensor;
	float max_speed, max_turn, max_turn_hard;

	GArray *solid_objects;
};

struct trial {
	map map;
	timestamp last_ts;
	GQueue telemetry;
	vehicle vehicle; /* our view */
	
	/* internal */
	int socket;
};

/* trial */
trial *trial_new();
void trial_reset_run(trial *t);
void trial_wait_for_start(trial *t);
void trial_check_input(trial *t);
void trial_wait_for_input(trial *t);
void trial_free(trial *t);

INLINE void vehicle_accel(trial *t);
INLINE void vehicle_roll(trial *t);
INLINE void vehicle_break(trial *t);

INLINE void vehicle_hard_left(trial *t);
INLINE void vehicle_left(trial *t);
INLINE void vehicle_straight(trial *t);
INLINE void vehicle_right(trial *t);
INLINE void vehicle_hard_right(trial *t);

/* Inline functions */

#define SENDCMD(t, cmd) write(t->socket, cmd, sizeof(cmd)-1)

INLINE void vehicle_accel(trial *t) {
	switch (t->vehicle.accel) {
		case ACCEL: return;
		case ROLL:  SENDCMD(t, "a;"); break;
		case BREAK: SENDCMD(t, "a;a;"); break;
	}
	t->vehicle.accel = ACCEL;
}
INLINE void vehicle_roll(trial *t) {
	switch (t->vehicle.accel) {
		case ACCEL: SENDCMD(t, "b;"); break;
		case ROLL:  return;
		case BREAK: SENDCMD(t, "a;"); break;
	}
	t->vehicle.accel = ROLL;
}
INLINE void vehicle_break(trial *t) {
	switch (t->vehicle.accel) {
		case ACCEL: SENDCMD(t, "b;b;"); break;
		case ROLL:  SENDCMD(t, "b;"); break;
		case BREAK: return;
	}
	t->vehicle.accel = BREAK;
}

INLINE void vehicle_hard_left(trial *t) {
	switch (t->vehicle.turn) {
		case TURN_HARD_LEFT:  return;
		case TURN_LEFT:       SENDCMD(t, "l;"); break;
		case TURN_STRAIGHT:   SENDCMD(t, "l;l;"); break;
		case TURN_RIGHT:      SENDCMD(t, "l;l;l;"); break;
		case TURN_HARD_RIGHT: SENDCMD(t, "l;l;l;l;"); break;
	}
	t->vehicle.turn = TURN_HARD_LEFT;
}

INLINE void vehicle_left(trial *t) {
	switch (t->vehicle.turn) {
		case TURN_HARD_LEFT:  SENDCMD(t, "r;"); break;
		case TURN_LEFT:       return;
		case TURN_STRAIGHT:   SENDCMD(t, "l;"); break;
		case TURN_RIGHT:      SENDCMD(t, "l;l;"); break;
		case TURN_HARD_RIGHT: SENDCMD(t, "l;l;l;"); break;
	}
	t->vehicle.turn = TURN_LEFT;
}
INLINE void vehicle_straight(trial *t) {
	switch (t->vehicle.turn) {
		case TURN_HARD_LEFT:  SENDCMD(t, "r;r;"); break;
		case TURN_LEFT:       SENDCMD(t, "r;"); break;
		case TURN_STRAIGHT:   return;
		case TURN_RIGHT:      SENDCMD(t, "l;"); break;
		case TURN_HARD_RIGHT: SENDCMD(t, "l;l;"); break;
	}
	t->vehicle.turn = TURN_STRAIGHT;
}
INLINE void vehicle_right(trial *t) {
	switch (t->vehicle.turn) {
		case TURN_HARD_LEFT:  SENDCMD(t, "r;r;r;"); break;
		case TURN_LEFT:       SENDCMD(t, "r;r;"); break;
		case TURN_STRAIGHT:   SENDCMD(t, "r;"); break;
		case TURN_RIGHT:      return;
		case TURN_HARD_RIGHT: SENDCMD(t, "l;"); break;
	}
	t->vehicle.turn = TURN_RIGHT;
}
INLINE void vehicle_hard_right(trial *t) {
	switch (t->vehicle.turn) {
		case TURN_HARD_LEFT:  SENDCMD(t, "r;r;r;r;"); break;
		case TURN_LEFT:       SENDCMD(t, "r;r;r;"); break;
		case TURN_STRAIGHT:   SENDCMD(t, "r;r;"); break;
		case TURN_RIGHT:      SENDCMD(t, "r;"); break;
		case TURN_HARD_RIGHT: return;
	}
	t->vehicle.turn = TURN_HARD_RIGHT;
}

#endif