void get_history(int fd, struct vehicle_list *vehicles)
{
int32_t id, count, x, y;
struct single_vehicle *v;
struct position *p;
if(socket_read(fd, &id, sizeof(uint32_t)) != sizeof(uint32_t)) return;
id = ntohl(id);
count = begin_getting_history(vehicles, id, &v);
count = htonl(count);
if(socket_write(fd, &count, sizeof(uint32_t)) == sizeof(uint32_t) && v != NULL)
{
p = get_positions(v);
while(p != NULL)
{
x = htonl(p->x);
y = htonl(p->y);
if(socket_write(fd, &x, sizeof(int32_t)) != sizeof(int32_t)) break;
if(socket_write(fd, &y, sizeof(int32_t)) != sizeof(int32_t)) break;
p = get_next_position(p);
}
}
end_getting_history(vehicles, v);
}
static struct custom_data *decode_custom_data(unsigned char *buf, int len, struct custom_data *d)
{
gint ix = 0;
gboolean err = FALSE;
LOG("%s: len=%d", __FUNCTION__, len);
while (ix < len && !err) {
LOG("buf[%d]=0x%02x=%d", ix, buf[ix], buf[ix]);
switch (buf[ix++]) {
case CD_VERSION: {
gint ver;
ix += get_byte(&(buf[ix]), &ver);
if (ver != 0) ERROR("Wrong version %d", ver);
}
break;
case CD_NAME_LONG:
LOG("CD_NAME_LONG");
ix += get_string(&(buf[ix]), d->name_long);
break;
case CD_NAME_SHORT:
LOG("CD_NAME_SHORT");
ix += get_string(&(buf[ix]), d->name_short);
break;
case CD_COMPETITORS_MIN:
GET_SHORT(d->competitors_min);
LOG("CD_COMPETITORS_MIN = %d", d->competitors_min);
break;
case CD_COMPETITORS_MAX:
GET_SHORT(d->competitors_max);
LOG("CD_COMPETITORS_MAX = %d", d->competitors_max);
break;
case CD_NUM_RR_POOLS:
LOG("CD_NUM_RR_POOLS");
ix += get_rr_pools(&(buf[ix]), d, &err);
break;
case CD_NUM_MATCHES:
LOG("CD_NUM_MATCHES");
ix += get_matches(&(buf[ix]), d, &err);
break;
case CD_NUM_POSITIONS:
LOG("CD_NUM_POSITIONS");
ix += get_positions(&(buf[ix]), d, &err);
break;
case CD_NUM_B3_POOLS:
ix += get_best_of_three_pairs(&(buf[ix]), d, &err);
break;
case CD_NUM_GROUPS:
ix += get_groups(&(buf[ix]), d, &err);
break;
default:
g_print("Unknown CD type %d\n", buf[ix-1]);
return NULL;
}
}
if (err) return NULL;
return d;
}
placement_problem::placement_problem(rect bounding_box, std::vector<cell> icells, std::vector<std::vector<pin> > inets, std::vector<rect> fixed)
:
cells(icells),
nets(inets)
{
for(cell const c : cells){
position_constraints.emplace_back(bounding_box.xmin, bounding_box.ymin, bounding_box.xmax - c.width, bounding_box.ymax - c.height);
}
for(rect const R : fixed){
if(rect::intersection(R, bounding_box).get_area() > 0)
fixed_elts.emplace_back(rect::intersection(R, bounding_box));
}
// The simplest edges: the constraints that a net's upper bound is bigger than a net's lower bound
std::vector<MCF_graph::edge> basic_x_edges, basic_y_edges;
for(int i=0; i<net_count(); ++i){
int UB_ind = cell_count() + 1 + 2*i;
int LB_ind = UB_ind + 1;
basic_x_edges.emplace_back(UB_ind, LB_ind, 0, 1);
basic_y_edges.emplace_back(UB_ind, LB_ind, 0, 1);
}
// Edges for the placement constraints
for(int i=0; i<cell_count(); ++i){
basic_x_edges.emplace_back(i+1, 0, -bounding_box.xmin); // Edge to the fixed node: left limit of the region
basic_x_edges.emplace_back(0, i+1, bounding_box.xmax - cells[i].width); // Edge from the fixed node: right limit of the region
basic_y_edges.emplace_back(i+1, 0, -bounding_box.ymin); // Edge to the fixed node: lower limit of the region
basic_y_edges.emplace_back(0, i+1, bounding_box.ymax - cells[i].height); // Edge from the fixed node: upper limit of the region
}
x_flow = MCF_graph(cell_count() + 2*net_count() + 1, basic_x_edges);
y_flow = MCF_graph(cell_count() + 2*net_count() + 1, basic_y_edges);
//x_flow.print();
//y_flow.print();
//std::cout << "Net edges" << std::endl;
// Edges for the nets
for(int i=0; i<net_count(); ++i){
assert(not nets[i].empty());
int UB_ind = cell_count() + 1 + 2*i;
int LB_ind = UB_ind + 1;
for(pin const cur_pin : nets[i]){
assert(cur_pin.ind >= -1 and cur_pin.ind < cell_count());
// cur_pin.ind == -1 ==> Fixed pin case
x_flow.add_edge(UB_ind, cur_pin.ind+1, -cur_pin.xmax);
y_flow.add_edge(UB_ind, cur_pin.ind+1, -cur_pin.ymax);
x_flow.add_edge(cur_pin.ind+1, LB_ind, cur_pin.xmin);
y_flow.add_edge(cur_pin.ind+1, LB_ind, cur_pin.ymin);
}
}
for(point p : get_positions()){
assert(p.x != std::numeric_limits<int>::max());
assert(p.y != std::numeric_limits<int>::max());
}
//x_flow.print();
//y_flow.print();
}
std::vector<placement_problem> placement_problem::branch(branching_rule rule) const{
// Chose a good branch based simply on the positions of the cells
std::vector<point> pos = get_positions();
int best_fc, best_sc;
int best_cell_measure=-1;
bool found_cell_overlap=false;
// Branch to avoid overlaps between cells
for(int i=0; i+1<cells.size(); ++i){
for(int j=i+1; j<cells.size(); ++j){
int measure = evaluate_branch(i, j, pos, rule);
if(measure >= 0 and measure > best_cell_measure){
found_cell_overlap=true;
best_cell_measure = measure;
best_fc = i; best_sc = j;
}
}
}
rect best_fixed;
int best_fixed_c;
int best_fixed_measure=-1;
bool found_fixed_overlap=false;
for(rect const R : fixed_elts){
for(int i=0; i<cells.size(); ++i){
int measure = evaluate_branch(i, R, pos, rule);
if(measure >= 0 and measure > best_fixed_measure){
found_fixed_overlap=true;
best_fixed_measure = measure;
best_fixed = R; best_fixed_c = i;
}
}
}
if(found_cell_overlap and (not found_fixed_overlap or best_cell_measure >= best_fixed_measure) ){
return branch_overlap_removal(best_fc, best_sc);
}
else if(found_fixed_overlap){
return branch_overlap_removal(best_fixed_c, best_fixed);
}
else{
assert(is_correct());
return std::vector<placement_problem>();
}
}
void init(int use_http, char *address, int port){
// Setup periferal and library
init_gui();
init_motors();
init_sensor();
init_compass();
calibrate_compass(0); // 0 = do not execute a new calibration of the compass
// Load ball predefined positions
get_positions(POINTS_FILE_NAME, &positions, &n_points);
// Start bluetooth
if(use_http)
bluetooth_test_init(address, port);
else
bluetooth_init();
//bluetooth_register_and_start(fAction_t, fAck_t, fLead_t, fStart_t, fStop_t, fWait_t, fKick_t, fCancel_t);
bluetooth_register_and_start(&follower_action_cb, NULL, &lead_cb, &start_cb, &stop_cb, NULL, NULL, &cancel_cb);
// Reset global variables
send_action_flag = 1;
act = 0;
end_game = 0;
start_game = 0;
wait = 0;
cancel = 0;
// Waiting for the game start
set_light(LIT_LEFT, LIT_GREEN);
set_light(LIT_RIGHT, LIT_GREEN);
while(!start_game); //start game, robot rank and snake size initialized by start_cb
// Initial position
home.x = BORDER_X_MAX/2;
home.y = (snake_size - robot_rank -1)*40 + 20;
center.x = BORDER_X_MAX/4;
center.y = BORDER_Y_MAX/2;
robot = home;
printf("[DEBUG] Starting position X: %d, Y: %d, T: %d\n", robot.x , robot.y, robot.t);
update_sensor(SENSOR_GYRO);
gyro_init_val = get_sensor_value(SENSOR_GYRO);
set_light(LIT_LEFT, LIT_OFF);
set_light(LIT_RIGHT, LIT_OFF);
}
static void dsound_thread(void *data)
{
DWORD write_ptr;
dsound_t *ds = (dsound_t*)data;
SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL);
get_positions(ds, NULL, &write_ptr);
write_ptr = (write_ptr + ds->buffer_size / 2) % ds->buffer_size;
while (ds->thread_alive)
{
struct audio_lock region;
DWORD read_ptr, avail, fifo_avail;
get_positions(ds, &read_ptr, NULL);
avail = write_avail(read_ptr, write_ptr, ds->buffer_size);
EnterCriticalSection(&ds->crit);
fifo_avail = fifo_read_avail(ds->buffer);
LeaveCriticalSection(&ds->crit);
if (avail < CHUNK_SIZE || ((fifo_avail < CHUNK_SIZE) && (avail < ds->buffer_size / 2)))
{
/* No space to write, or we don't have data in our fifo,
* but we can wait some time before it underruns ... */
/* We could opt for using the notification interface,
* but it is not guaranteed to work, so use high
* priority sleeping patterns.
*/
retro_sleep(1);
continue;
}
if (!grab_region(ds, write_ptr, ®ion))
{
ds->thread_alive = false;
SetEvent(ds->event);
break;
}
if (fifo_avail < CHUNK_SIZE)
{
/* Got space to write, but nothing in FIFO (underrun),
* fill block with silence. */
memset(region.chunk1, 0, region.size1);
memset(region.chunk2, 0, region.size2);
release_region(ds, ®ion);
write_ptr = (write_ptr + region.size1 + region.size2) % ds->buffer_size;
}
else
{
/* All is good. Pull from it and notify FIFO. */
EnterCriticalSection(&ds->crit);
if (region.chunk1)
fifo_read(ds->buffer, region.chunk1, region.size1);
if (region.chunk2)
fifo_read(ds->buffer, region.chunk2, region.size2);
LeaveCriticalSection(&ds->crit);
release_region(ds, ®ion);
write_ptr = (write_ptr + region.size1 + region.size2) % ds->buffer_size;
SetEvent(ds->event);
}
}
ExitThread(0);
}
mat mars(Agraph_t* g, struct marsopts opts)
{
int i, j, n = agnnodes(g), k = MIN(n, MAX(opts.k, 2)), iter = 0;
mat dij, u, u_trans, q, r, q_t, tmp, tmp2, z;
double* s = (double*) malloc(sizeof(double)*k);
double* ones = (double*) malloc(sizeof(double)*n);
double* d;
int* anchors = (int*) malloc(sizeof(int)*k);
int* clusters = NULL;
double change = 1, old_stress = -1;
dij = mat_new(k, n);
u = mat_new(n,k);
tmp = mat_new(n,k);
darrset(ones,n,-1);
select_anchors(g, dij, anchors, k);
if(opts.color) {
for(i = 0; i < k; i++) {
Agnode_t* anchor = get_node(anchors[i]);
agset(anchor, "color", "red");
}
}
if(opts.power != 1) {
clusters = graph_cluster(g,dij,anchors);
}
singular_vectors(g, dij, opts.power, u, s);
vec_scalar_mult(s, k, -1);
u_trans = mat_trans(u);
d = mat_mult_for_d(u, s, u_trans, ones);
for(i = 0; i < u->c; i++) {
double* col = mat_col(u,i);
double* b = inv_mul_ax(d,col,u->r);
for(j = 0; j < u->r; j++) {
tmp->m[mindex(j,i,tmp)] = b[j];
}
free(b);
free(col);
}
tmp2 = mat_mult(u_trans,tmp);
for(i = 0; i < k; i++) {
tmp2->m[mindex(i,i,tmp2)] += (1.0/s[i]);
}
q = mat_new(tmp2->r, tmp2->c);
r = mat_new(tmp2->c, tmp2->c);
qr_factorize(tmp2,q,r);
q_t = mat_trans(q);
if(opts.given) {
z = get_positions(g, opts.dim);
} else {
z = mat_rand(n, opts.dim);
}
translate_by_centroid(z);
if(opts.viewer) {
init_viewer(g, opts.max_iter);
append_layout(z);
}
old_stress = stress(z, dij, anchors, opts.power);
while(change > EPSILON && iter < opts.max_iter) {
mat right_side;
double new_stress;
if(opts.power == 1) {
right_side = barnes_hut(z);
} else {
right_side = barnes_hut_cluster(z, dij, clusters, opts.power);
}
for(i = 0; i < opts.dim; i++) {
double sum = 0;
double* x;
double* b = mat_col(right_side,i);
for(j = 0; j < right_side->r; j++) {
sum += b[j];
}
x = inv_mul_full(d, b, right_side->r, u, u_trans, q_t, r);
for(j = 0; j < z->r; j++) {
z->m[mindex(j,i,z)] = x[j] - sum/right_side->r;
}
free(x);
free(b);
}
adjust_anchors(g, anchors, k, z);
update_anchors(z, dij, anchors, opts.power);
translate_by_centroid(z);
if(opts.viewer) {
append_layout(z);
}
new_stress = stress(z, dij, anchors, opts.power);
change = fabs(new_stress-old_stress)/old_stress;
old_stress = new_stress;
mat_free(right_side);
iter++;
}
mat_free(dij);
mat_free(u);
mat_free(u_trans);
mat_free(q);
mat_free(r);
mat_free(q_t);
mat_free(tmp);
mat_free(tmp2);
free(s);
free(ones);
free(d);
free(anchors);
free(clusters);
return z;
}
int placement_problem::get_cost() const{
int ret = x_flow.get_cost() + y_flow.get_cost();
if(is_feasible())
assert(get_solution_cost(get_positions()) == ret);
return ret;
}
// Verify that the pitches for the cells are respected and that the cells do not overlap
bool placement_problem::is_correct() const{
return is_solution_correct(get_positions());
}
/**
* Computing the Fisher's Exact Test (FET) and the corresponding standard deviation for a given thread.
* @param threadarg: the arguments to the thread: a thread_data struct
*/
void mycompute(void *threadarg) {
struct thread_data *my_data;
int tid;
int start, stop, regend, num_windows, wsize, wstep, alen, blen, asize, bsize, totalpos, npos, nsamples, my_task_id = 0;
double perc;
int *apos;
int *bpos;
double *avals;
double *bvals;
double *scores;
double *stddev;
/* fetch data from threadarg */
my_data = (struct thread_data *) threadarg;
tid = my_data->thread_id;
regend = my_data->regend;
num_windows = my_data->num_windows;
wsize = my_data->wsize;
wstep = my_data->wstep;
apos = my_data->apos;
bpos = my_data->bpos;
avals = my_data->avals;
bvals = my_data->bvals;
alen = my_data->alen;
blen = my_data->blen;
perc = my_data->perc;
scores = my_data->scores;
stddev = my_data->stddev;
int wcount = 0;
int wstart;
int wstop;
int *aidx;
int *bidx;
int *f;
int *tmp;
double *results;
double *fetscores = NULL;
double *samples = NULL;
double *stdsamples;
/* get size of population a and b*/
asize = get_population_size(apos);
bsize = get_population_size(bpos);
wcount = 0;
start = 0;
stop = wsize;
nsamples = 100;
/* setup idx arrays */
aidx = (int*)malloc(2*sizeof(int));
bidx = (int*)malloc(2*sizeof(int));
/* setup arrays for calc. of FET */
f = (int*)malloc(4*sizeof(int));
tmp = (int*)malloc(4*sizeof(int));
results = (double*)malloc(2*sizeof(double));
stdsamples = (double*)malloc(nsamples*sizeof(double));
int idx = 0;
/* generate seed for the PRNG */
unsigned short state[3] = {0,0,0};
unsigned short seed = time(NULL) + (unsigned short) pthread_self();
memcpy(state, &seed, sizeof(seed));
// fetch new tasks and calculate FET
while (my_task_id < num_tasks) {
// try to fetch new task
pthread_mutex_lock (&mutexTASK_ID);
my_task_id= task_id;
task_id++;
pthread_mutex_unlock(&mutexTASK_ID);
// if no more tasks, break
if (my_task_id > num_tasks)
break;
// get chromosome position of the current task
get_positions(wsize, wstep, my_task_id, &start, &stop, regend);
// to include the last (possibly smaller) window
if (stop >= regend) {
stop = regend + wstep;
}
wstart = start;
wstop = start+wsize;
aidx[0] = 0; aidx[1] = 0;
bidx[0] = 0; bidx[1] = 0;
// calculate FET for each window
while (wstart + wsize <= stop) {
slide_right(aidx, apos, wstart, wstop, alen);
slide_right(bidx, bpos, wstart, wstop, blen);
npos = (aidx[1] - aidx[0])/asize;
if (npos > 0) {
fetscores = (double*)realloc(fetscores, npos*sizeof(double));
samples = (double*)realloc(samples, npos*sizeof(double));
idx = wstart/wstep;
fisher_exact_test(results, &(avals[aidx[0]]), &(bvals[bidx[0]]), asize, bsize, npos, f, tmp, samples, stdsamples, nsamples, fetscores, state, perc);
scores[idx] = results[0];
stddev[idx] = results[1];
}
wstart += wstep;
wstop += wstep;
wcount++;
}
}
/* cleanup */
free(aidx);
free(bidx);
free(f);
free(tmp);
free(results);
free(samples);
free(stdsamples);
free(fetscores);
}
