void check_isReservableRequests(Queue* Q, Queue *train_serverQ, track_node* track, sw_info* sw_infos, track_reservation* track_reservations, train_reservation_info* tr_reservation_infos, int train_server_id) {
train_server_request tsr;
isReservableQueueItem irqi;
int size = Q->size, ret;
while(size > 0) {
Queue_pop(Q, &irqi);
//char reply_buff;
if(is_reservation_possible(track_reservations,
track,
tr_reservation_infos,
sw_infos,
irqi.trnum,
irqi.mm)) {
tsr.type = TRAIN_TYPE_RESERVATION_POSSIBLE;
tsr.data.tmp = irqi.trnum;
// Send(train_server_id, &tsr, sizeof(train_server_request), &reply_buff, sizeof(char));
ret = Queue_push(train_serverQ, &tsr);
Warning(ret == 0, "TRACKSERVER: Queue_push failed");
}
else {
Queue_push(Q, &irqi);
}
size--;
}
}
static bool push_range(
Queue * queue,
ssize_t initial_size,
uintptr_t start,
uintptr_t stop)
{
uintptr_t data;
bool push_successful;
for (data = start; data < stop; ++data)
{
push_successful = Queue_push(queue, (void *)data);
TEST_BOOL(push_successful, true);
TEST(ssize_t, "%zd", Queue_size(queue), ==,
initial_size + (ssize_t) (data - start + 1));
}
return true;
}
void pull_out_switch_orders(routing_command* commands, switch_command* sw_commands, Queue* Q) {
int i = 0, j = 0;
Queue_reset(Q);
while (commands[i].type != COMMAND_UNKNOWN) {
if (commands[i].type == COMMAND_THROW_SWITCH) {
sw_commands[j].swnum = commands[i].sw_num;
sw_commands[j].swdir = commands[i].sw_dir;
Queue_push(Q, &sw_commands[j]);
j++;
}
i++;
}
sw_commands[j].swnum = 0;
sw_commands[j].swdir = 0;
for(i=0; i < Q->size; i++) {
HardDebug("Q->buff[%d]: swnum=%d ~ swdir=%c", i, (int)((switch_command*)Q->buff)[i].swnum, ((switch_command*)Q->buff)[i].swdir);
}
}
void track_server_code (){
RegisterAs("TRACKSERVER");
// Server data
sw_info sw_infos [NUM_SWITCHES];
int sensor_info [NUM_SENSORS]; // time when the sensor is hit
int broken_sensors[NUM_SENSORS] = {0};
// Server's track data
track_node track[TRACK_MAX];
init_track(track);
init_broken_sensors(broken_sensors);
track_node *src, *dst;
// Received stuff
track_receive rcv_buff;
int sender_tid;
int request_type;
int sensor_index = 0;
//Reply buffs
int reply_dist = 0;
int reply_index = 0;
struct track_loc reply_loc;
char reply_buff;
int reply_buff_loc_dist [2];
struct train_server_request tsr;
int train_server_id;
while((train_server_id = WhoIs("TRAINSERVER")) == -1);
int track_to_train_courier_tid = Create(PR_COURIER, track_to_train_courier_code);
int track_to_train_courier_ready = 0;
Queue train_serverQ;
struct train_server_request allocated_buff[TRACK_TO_TRAIN_QUEUE_SIZE];
Queue_init (&train_serverQ, allocated_buff, TRACK_TO_TRAIN_QUEUE_SIZE, sizeof(struct train_server_request));
//things for shortest path request
track_node* path[TRACK_MAX] = {0};
int dist[TRACK_MAX] = {0};
int shortest_dist = 0;
routing_command commands[TRACK_MAX];
init_routing_commands(commands);
// switch_command sw_commands[NUM_MAX_SWITCH_ROUTE_COMMANDS];
// memset(sw_commands, 0, NUM_MAX_SWITCH_ROUTE_COMMANDS * sizeof(switch_command));
//things for changing switches when train goes
//within a specified route
Queue switch_queues[NUM_TRAINS];
Queue *Q;
switch_command switch_queue_buffers[NUM_TRAINS][NUM_MAX_SWITCH_ROUTE_COMMANDS];
memset(switch_queue_buffers, 0, NUM_TRAINS * NUM_MAX_SWITCH_ROUTE_COMMANDS * sizeof(switch_command));
int trnum;
for (trnum = 0; trnum < NUM_TRAINS; trnum++) {
Queue_init(&switch_queues[trnum], switch_queue_buffers[trnum], NUM_MAX_SWITCH_ROUTE_COMMANDS, sizeof(switch_command));
}
//
//Reservation data structures
track_reservation track_reservations[TRACK_MAX];
train_reservation_info tr_reservation_infos[NUM_TRAINS];
memset(track_reservations, 0, TRACK_MAX * sizeof(track_reservation));
memset(tr_reservation_infos, -1, NUM_TRAINS * sizeof(train_reservation_info));
//
Queue isReservableQueue;
isReservableQueueItem isReservableQueueItems[NUM_TRAINS];
Queue_init(&isReservableQueue, isReservableQueueItems, NUM_TRAINS, sizeof(isReservableQueueItem));
isReservableQueueItem irqi;
int reserve_ret = -1;
int ret;
while(1){
Receive(&sender_tid, &rcv_buff, sizeof(track_receive));
request_type = rcv_buff.type;
switch (request_type){
case TRACK_TYPE_COURIER_RTS:
track_to_train_courier_ready = 1;
break;
case TRACK_TYPE_SENSOR_INFO:
sensor_index = (rcv_buff.data.track_data.data2 - 'A')*16+rcv_buff.data.track_data.data1-1;
//if (sensor_index == 56) {HardDebug("D9 is hit. Hit time:%d. Rcv Time:%d", rcv_buff.data.track_data.time, Time());}
sensor_info[sensor_index] = rcv_buff.data.track_data.time;
Reply(sender_tid, &reply_buff, sizeof(char));
tsr.type = TRAIN_TYPE_SENSOR_UPDATE;
memcpy(&tsr.data, &rcv_buff.data.track_data, sizeof(track_info));
if (!broken_sensors[sensor_index]) {
//Send(train_server_id, &tsr, sizeof(struct train_server_request), &reply_buff, sizeof(char));
ret = Queue_push(&train_serverQ, &tsr);
Warning(ret == 0, "TRACKSERVER: Queue_push failed");
}
break;
case TRACK_TYPE_SWITCH_INFO:
(sw_infos[(int)rcv_buff.data.track_data.data1]).swdir = rcv_buff.data.track_data.data2;
(sw_infos[(int)rcv_buff.data.track_data.data1]).time = rcv_buff.data.track_data.time;
Reply(sender_tid, &reply_buff, sizeof(char));
tsr.type = TRAIN_TYPE_SWITCH_UPDATE;
memcpy(&tsr.data, &rcv_buff.data.track_data, sizeof(track_info));
//Send(train_server_id, &tsr, sizeof(struct train_server_request), &reply_buff, sizeof(char));
ret = Queue_push(&train_serverQ, &tsr);
Warning(ret == 0, "TRACKSERVER: Queue_push failed");
break;
case TRACK_TYPE_DIST:
if((int)rcv_buff.data.track_data.data1 >= TRACK_MAX || (int)rcv_buff.data.track_data.data2 >= TRACK_MAX) {
reply_dist = 0;
}
else {
src = &track[(int)rcv_buff.data.track_data.data1];
dst = &track[(int)rcv_buff.data.track_data.data2];
reply_dist = get_dist_between_nodes(src, dst, sw_infos);
}
Reply(sender_tid, &reply_dist, sizeof(int));
break;
case TRACK_TYPE_NEXT_LOC:
//get current loc_index and find next node
__get_next_node((int)rcv_buff.data.track_data.data1, track, sw_infos, &reply_loc.index, reply_loc.name);
Reply(sender_tid, &reply_loc, sizeof(struct track_loc));
break;
case TRACK_TYPE_NEXT_SENSOR:
reply_index = get_next_sensor_index((int)rcv_buff.data.track_request_data.data1, track, sw_infos, broken_sensors, &tsr.data.spd.spd_val);
Assert(reply_index == tsr.data.spd.spd_val.next_expected_sensor_index, "reply_index != result->next_expected_sensor_index");
//Debug("TRACKSERVER: Got next expected sensor %d, after that %d", tsr.data.spd.spd_val.next_expected_sensor_index, tsr.data.spd.spd_val.sensor_after_expected_sensor);
Reply(sender_tid, &reply_index, sizeof(int));
tsr.type = TRAIN_TYPE_SENSOR_PREDICTION;
tsr.data.spd.trnum = (int)rcv_buff.data.track_request_data.data2;
if (rcv_buff.data.track_request_data.data3) {
//Send(train_server_id, &tsr, sizeof(train_server_request), &reply_buff, sizeof(char));
ret = Queue_push(&train_serverQ, &tsr);
Warning(ret == 0, "TRACKSERVER: Queue_push failed");
}
break;
case TRACK_TYPE_LOC_NAME:
Reply(sender_tid, (char*)track[(int)rcv_buff.data.track_data.data1].name, 6 * sizeof(char));
break;
case TRACK_TYPE_SENSOR_REVERSE:
reply_index = get_reverse_sensor((int)rcv_buff.data.track_request_data.data1, track, sw_infos, broken_sensors, &tsr.data.spd.spd_val);
Reply(sender_tid, &reply_index, sizeof(int));
tsr.data.spd.trnum = (int)rcv_buff.data.track_request_data.data2;
if (rcv_buff.data.track_request_data.data3) {
//Send(train_server_id, &tsr, sizeof(train_server_request), &reply_buff, sizeof(char));
ret = Queue_push(&train_serverQ, &tsr);
Warning(ret == 0, "TRACKSERVER: Queue_push failed");
}
break;
case TRACK_TYPE_UPDATE_LOC_AND_DISTANCE:
translate_sensor_loc(rcv_buff.data.track_request_data.data1, rcv_buff.data.track_request_data.data2, track, sw_infos, reply_buff_loc_dist);
Reply(sender_tid, reply_buff_loc_dist, 2*sizeof(int));
break;
case TRACK_TYPE_FIND_ROUTE:
Warning(((0 <= rcv_buff.data.track_request_data.data2 && rcv_buff.data.track_request_data.data2 < TRACK_MAX) &&
(0 <= rcv_buff.data.track_request_data.data3 && rcv_buff.data.track_request_data.data3 < TRACK_MAX)),
"TRACKSERVER: TRACK_TYPE_FIND_ROUTE - Source(%d) or destination(%d) is invalid", rcv_buff.data.track_request_data.data2, rcv_buff.data.track_request_data.data3);
//Debug("FINDING ROUTE:trnum:%d ~ src:%s ~ dest:%s ~ offset:%d",rcv_buff.data.track_request_data.data1, track[rcv_buff.data.track_request_data.data2].name, track[rcv_buff.data.track_request_data.data3].name, rcv_buff.data.track_request_data.data4);
shortest_dist = shortest_path((int)rcv_buff.data.track_request_data.data2, (int)rcv_buff.data.track_request_data.data3, track, path, dist, track_reservations);
if (shortest_dist == __INFINITY)
{
shortest_path((int)rcv_buff.data.track_request_data.data2, (int)rcv_buff.data.track_request_data.data3, track, path, dist, 0);
}
generate_commands(path, track, commands);
print_commands(commands);
pull_out_switch_orders(commands, tsr.data.path_commands.sw_commands, &switch_queues[rcv_buff.data.track_request_data.data1]);
pull_out_reverse_orders(commands, tsr.data.path_commands.rv_commands);
tsr.type = TRAIN_TYPE_ROUTE_SWITCH_ORDERS;
Reply(sender_tid, &tsr, sizeof(train_server_request));
break;
case TRACK_TYPE_RESERVATION_INIT:
reserve_ret = reserve_inital_piece (track_reservations,
track,
tr_reservation_infos,
sw_infos,
&switch_queues[rcv_buff.data.track_request_data.data1],
rcv_buff.data.track_request_data.data1,
rcv_buff.data.track_request_data.data2,
rcv_buff.data.track_request_data.data3,
rcv_buff.data.track_request_data.data4);
Debug("TRACKSERVER: Received reservation initialization request for train %d", rcv_buff.data.track_request_data.data1);
tsr.type = TRAIN_TYPE_RESERVATION_SUCCESS;
Reply(sender_tid, &tsr, sizeof(train_server_request));
break;
case TRACK_TYPE_RESERVATION:
Q = &switch_queues[rcv_buff.data.track_request_data.data1];
reserve_ret = reserve_track(track_reservations, track, tr_reservation_infos, sw_infos, Q, rcv_buff.data.track_request_data.data1, rcv_buff.data.track_request_data.data2);
ret = release_track(track_reservations, track, tr_reservation_infos, sw_infos, rcv_buff.data.track_request_data.data1, rcv_buff.data.track_request_data.data3);
Assert (ret != -1, "TRACKSERVER: release failed: ret:%d", ret);
tsr.type = (reserve_ret == -1) ? TRAIN_TYPE_RESERVATION_FAILURE : TRAIN_TYPE_RESERVATION_SUCCESS;
Reply(sender_tid, &tsr, sizeof(train_server_request));
if (/*rcv_buff.data.track_request_data.data2 < 0 ||*/ rcv_buff.data.track_request_data.data3 > 0) {
check_isReservableRequests(&isReservableQueue, &train_serverQ, track, sw_infos, track_reservations, tr_reservation_infos, train_server_id);
}
print_total_reserved_dist(track_reservations, tr_reservation_infos, rcv_buff.data.track_request_data.data1);
break;
case TRACK_TYPE_RESERVATION_REVERSE:
reserve_ret = reverse_reservation (track,
tr_reservation_infos,
sw_infos,
rcv_buff.data.track_request_data.data1);
tsr.type = TRAIN_TYPE_RESERVATION_SUCCESS;
Reply(sender_tid, &tsr, sizeof(train_server_request));
break;
case TRACK_TYPE_RESERVATION_SENSOR_UPDATE:
reserve_ret = reservation_sensor_update(track_reservations,
track,
tr_reservation_infos,
sw_infos,
&switch_queues[rcv_buff.data.track_request_data.data1],
rcv_buff.data.track_request_data.data1,
rcv_buff.data.track_request_data.data2,
rcv_buff.data.track_request_data.data3,
rcv_buff.data.track_request_data.data4,
rcv_buff.data.track_request_data.data5);
tsr.type = (reserve_ret == -1) ? TRAIN_TYPE_RESERVATION_FAILURE : TRAIN_TYPE_RESERVATION_SUCCESS;
Reply(sender_tid, &tsr, sizeof(train_server_request));
//check_isReservableRequests(&isReservableQueue, &train_serverQ, track, sw_infos, track_reservations, tr_reservation_infos, train_server_id);
break;
case TRACK_TYPE_RESERVATION_STOP_UPDATE:
reserve_ret = reservation_stop_update ( track_reservations,
track,
tr_reservation_infos,
sw_infos,
&switch_queues[rcv_buff.data.track_request_data.data1],
rcv_buff.data.track_request_data.data1,
rcv_buff.data.track_request_data.data2,
rcv_buff.data.track_request_data.data3,
rcv_buff.data.track_request_data.data4,
rcv_buff.data.track_request_data.data5);
tsr.type = /*(reserve_ret == -1) ? TRAIN_TYPE_RESERVATION_FAILURE :*/ TRAIN_TYPE_RESERVATION_SUCCESS;
Reply(sender_tid, &tsr, sizeof(train_server_request));
break;
case TRACK_TYPE_IS_RESERVABLE:
tsr.type = TRAIN_TYPE_REQ_IN_PROGRESS;
Reply(sender_tid, &tsr, sizeof(train_server_request));
irqi.trnum = rcv_buff.data.track_request_data.data1;
irqi.mm = rcv_buff.data.track_request_data.data2;
ret = Queue_push(&isReservableQueue, &irqi);
Warning(ret == 0, "TRACKSERVER: Queue_push failed");
break;
case TRACK_TYPE_PRINT_RESERVATIONS:
Reply(sender_tid, &reply_buff, sizeof(char));
print_reservations(track_reservations, track, tr_reservation_infos);
break;
default:
Reply(sender_tid, &reply_buff, sizeof(char));
Warning(0,"Unknown receive in track_server_code: type=%d from %d", request_type, sender_tid);
break;
}
if (track_to_train_courier_ready && !Queue_pop(&train_serverQ, &tsr)){
Reply(track_to_train_courier_tid, &tsr, sizeof(train_server_request));
track_to_train_courier_ready = 0;
}
}
Exit();
Warning(0,"Unexpected return from Exit() at track_server_code\n\r");
}
void Queue_test()
{
queue_t testQueue;
Queue_init(&testQueue, 5, sizeof(int));
assert( Queue_isEmpty(&testQueue) == true );
assert( Queue_isFull(&testQueue) == false );
int a = 34;
int b = 35;
int c = 36;
int d = 37;
int e = 38;
assert( Queue_push( &testQueue, (uint8_t*)&a ) == true);
assert( Queue_push( &testQueue, (uint8_t*)&b ) == true);
assert( Queue_push( &testQueue, (uint8_t*)&c ) == true);
assert( Queue_isEmpty(&testQueue) == false );
assert( Queue_isFull(&testQueue) == false );
assert( Queue_push( &testQueue, (uint8_t*)&d ) == true);
assert( Queue_push( &testQueue, (uint8_t*)&e ) == true);
assert( Queue_push( &testQueue, (uint8_t*)&e ) == false);
assert( Queue_isEmpty(&testQueue) == false );
assert( Queue_isFull(&testQueue) == true );
int read;
assert( Queue_pop( &testQueue, (uint8_t*)&read ) == true);
assert(read == 34);
assert( Queue_pop( &testQueue, (uint8_t*)&read ) == true);
assert(read == 35);
assert( Queue_pop( &testQueue, (uint8_t*)&read ) == true);
assert(read == 36);
assert( Queue_pop( &testQueue, (uint8_t*)&read ) == true);
assert(read == 37);
assert( Queue_pop( &testQueue, (uint8_t*)&read ) == true);
assert(read == 38);
assert( Queue_pop( &testQueue, (uint8_t*)&read ) == false);
assert(read == 38);
assert( Queue_isEmpty(&testQueue) == true );
assert( Queue_isFull(&testQueue) == false );
assert( Queue_push( &testQueue, (uint8_t*)&a ) == true);
assert( Queue_push( &testQueue, (uint8_t*)&b ) == true);
assert( Queue_push( &testQueue, (uint8_t*)&c ) == true);
Queue_clear(&testQueue);
assert( Queue_isEmpty(&testQueue) == true );
assert( Queue_isFull(&testQueue) == false );
}