void initEM (INFO *info) {
unsigned int num_clusters = info -> num_clusters;
unsigned int i; /* Index into w1 */
unsigned int j; /* Index into w2 */
unsigned int k; /* Index into clusters */
PROBNODE sum;
time_t start;
time_t end;
time (&start);
PROGRESS_MSG ("Begin initialization...");
/* Assign probabilities to probz */
sum = 0.0;
for (k = 0; k < num_clusters; k++) {
GET_PROBZ_CURR (k) = RANDOM_FLOAT;
sum += GET_PROBZ_CURR (k);
}
for (k = 0; k < num_clusters; k++) {
GET_PROBZ_CURR (k) = DOLOG (GET_PROBZ_CURR (k) / sum);
}
/* Assign probabilities to probw1_z */
for (i = 0; i < (info -> num_clusters * info -> m); i++) {
info -> probw1_z_curr[i] = RANDOM_FLOAT;
}
for (k = 0; k < num_clusters; k++) {
sum = 0.0;
for (i = 0; i < info -> m; i++) {
sum += GET_PROBW1_Z_CURR (k, i);
}
for (i = 0; i < info -> m; i++) {
GET_PROBW1_Z_CURR (k, i) = DOLOG (GET_PROBW1_Z_CURR (k, i) / sum);
}
}
/* Assign probabilities to probw2_z */
for (i = 0; i < (info -> num_clusters * info -> n); i++) {
info -> probw2_z_curr[i] = RANDOM_FLOAT;
}
for (k = 0; k < num_clusters; k++) {
sum= 0.0;
for (j = 0; j < info -> n; j++) {
sum += GET_PROBW2_Z_CURR (k, j);
}
for (j = 0; j < info -> n; j++) {
GET_PROBW2_Z_CURR (k, j) = DOLOG (GET_PROBW2_Z_CURR (k, j) / sum);
}
}
PROGRESS_MSG ("Initialization complete...");
time (&end);
info -> initEM_time += difftime (end, start);
return;
}
static void
offload_full_buffers(struct thread_comp_input *par)
{
PROGRESS_DECL();
struct thread_queue *tq = par->tq;
while (tq->out_head && tq->out_head->status == FULL) {
tq->pool_status[tq->out_head->status]--;
tq->out_head->status = UNLOADING;
tq->pool_status[tq->out_head->status]++;
/* once tq->out_head->status is set to UNLOADING, program
logic ensures it is safe to use tq->out_head->buf */
pthread_mutex_unlock(&tq->out_mtx);
PROGRESS_START("OFFLOAD");
tq->offload(tq->offload_par, tq->out_head->buf);
PROGRESS_MSG("OFFLOAD");
pthread_mutex_lock(&tq->out_mtx);
tq->pool_status[tq->out_head->status]--;
tq->out_head->status = EMPTY;
tq->pool_status[tq->out_head->status]++;
unsigned b;
for (b = 0; b != tq->n_outputs; ++b) {
struct managed_buf *mb = &tq->out_head->buf[b];
ALLOC_CLEAR(mb->buf, mb->size, mb->alloc);
}
tq->out_head = tq->out_head->next;
pthread_cond_signal(&tq->out_buf_avail);
}
}
void Navigator::handleSubsumptionEvent( EventNotification* pEvent, SubsumptionParams* pSubsumptionParams )
{
float headingToWaypoint;
float headingError;
int distanceToWaypoint;
if ( _bEnabled ) {
// now, if this event has not already been subsumed, we need to plot a course
// from our current position to our current waypoint, if any.
if ( ! pSubsumptionParams->ControlFreak() ) {
// adjust the motors' speeds as necessary to correct our heading
headingToWaypoint = _pPosition->_theta; // current heading in radians, in case we don't have a waypoint
if ( _pCurrentWaypoint ) {
// compute distance to target
float dx = _pCurrentWaypoint->_x - _pPosition->_xInches;
float dy = _pCurrentWaypoint->_y - _pPosition->_yInches;
distanceToWaypoint = sqrt( dx * dx + dy * dy ); // thank you, Mr. Pythagoras
// If we're close enough to this waypoint, move to the next
if ( distanceToWaypoint < _pCurrentWaypoint->_radius ) {
_pCurrentWaypoint = _pWaypointManager->GetWaypoint( ++_waypointNumber );
_bCorrecting = false;
PROGRESS_MSG( "\nNext Waypoint\n" );
if ( !_pCurrentWaypoint ) {
// no further waypoints, so shut down and take control
PROGRESS_MSG( "\nWe have arrived!\n" );
pSubsumptionParams->SetThrottles( 0, 0, this);
_waypointNumber = 0;
}
}
else {
// compute heading to current waypoint
// note that atan2() calls for dy/dx, but that yields angles referenced to the
// x-axis, or 0 = East. For navigation, we want 0 = North, so we swap the
// arguments to get the correct alignment.
headingToWaypoint = atan2( dx, dy );
IF_MASK( MM_CALC ) {
PRINT_VAR( dx );
PRINT_VAR( dy );
PRINT_VAR( headingToWaypoint );
}
headingError = _pPosition->_theta - headingToWaypoint;
IF_MASK( MM_CALC ) {
PRINT_VAR( headingError );
}
// normalize the error value
float piOffset = headingError < 0.0 ? -PI : PI;
headingError = fmod( headingError + piOffset, 2.0 * PI ) - PI;
// headingError = atan( tan( headingError ) );
IF_MASK( MM_CALC ) {
Serial.print( F("Adjusted ") );
PRINT_VAR( headingError );
}
// if heading is outside our tolerance band, perform correction
if ( fabs( headingError ) > _headingTolerance ) {
// _bCorrecting means we already have a current snapshot
if ( ! _bCorrecting ) {
_bCorrecting = true;
// snapshot current throttle positions as a baseline
_leftThrottleSnapshot = pSubsumptionParams->GetLeftThrottle();
_rightThrottleSnapshot = pSubsumptionParams->GetRightThrottle();
IF_MASK( MM_CALC ) {
PRINT_VAR( _leftThrottleSnapshot );
PRINT_VAR( _rightThrottleSnapshot );
}
}
// negative error means too far left, so slow the right motor
if ( headingError < 0 ) {
// map error (0..3) to throttle ( rightsnapshot .. -leftsnapshot )
int rightThrottle = fmap( -headingError, 0.0, 3.14, _rightThrottleSnapshot, -_leftThrottleSnapshot );
pSubsumptionParams->SetThrottles( _leftThrottleSnapshot, rightThrottle , this);
IF_MASK( MM_CALC ) {
PRINT_VAR( rightThrottle );
}
}
else {
int leftThrottle = fmap( headingError, 0.0, 3.14, _leftThrottleSnapshot, -_rightThrottleSnapshot );
pSubsumptionParams->SetThrottles( leftThrottle, _rightThrottleSnapshot, this);
IF_MASK( MM_CALC ) {
PRINT_VAR( leftThrottle );
}
}
}
else {
/*!
** Read the co-occurrence data from file. The format of the file is:
**
** [rows][columns][row id+][column id+][w1 cos_count (w21 c21) ... (w2n c2n)]+**
**
** row and column ids are integer values that map to the original
** vocabulary. The number of values should be (info -> m) and
** (info -> n), respectively.
**
** Every value is an unsigned integer in binary format, unless
** textmode is TRUE -- if so, values are in text, separated
** by white space (tab).
**
** Note: i indexes for rows (w1); j indexes for columns (w2)
*/
bool readCO (INFO *info) {
FILE *fp = NULL;
unsigned int w1 = 0;
unsigned int w2 = 0;
unsigned int freq = 0;
unsigned int rows = 0;
unsigned int cols = 0;
unsigned int cos_count = 0;
unsigned int found_pairs = 0;
unsigned int found_w1 = 0;
unsigned int sum_freq = 0;
unsigned int nonzero_count = 0;
time_t start;
time_t end;
time (&start);
PROGRESS_MSG ("Reading from co-occurrence file...");
/* Open the file; read the number of rows and columns and check them */
if (info -> textio) {
FOPEN (info -> co_fn, fp, "r");
fscanf (fp, "%u", &rows);
fscanf (fp, "%u", &cols);
}
else {
FOPEN (info -> co_fn, fp, "rb");
fread (&rows, sizeof (unsigned int), 1, fp);
fread (&cols, sizeof (unsigned int), 1, fp);
}
info -> m = rows;
info -> n = cols;
initializePostInput (info);
info -> row_ids = wmalloc (info -> m * sizeof (unsigned int));
info -> column_ids = wmalloc (info -> n * sizeof (unsigned int));
if (info -> textio) {
for (unsigned int i = 0; i < info -> m; i++) {
fscanf (fp, "%u", &(info -> row_ids[i]));
}
for (unsigned int j = 0; j < info -> n; j++) {
fscanf (fp, "%u", &(info -> column_ids[j]));
}
}
else {
fread (info -> row_ids, sizeof (unsigned int), info -> m, fp);
fread (info -> column_ids, sizeof (unsigned int), info -> n, fp);
}
found_pairs = 0;
found_w1 = 0;
for (unsigned int i = 0; i < info -> m; i++) {
if (info -> textio) {
fscanf (fp, "%u", &w1);
}
else {
fread (&w1, sizeof (unsigned int), 1, fp);
}
if (feof (fp)) {
break;
}
found_w1++;
if (info -> textio) {
fscanf (fp, "%u", &cos_count);
}
else {
fread (&cos_count, sizeof (unsigned int), 1, fp);
}
/* Allocate space for the row */
info -> cos[i] = wmalloc (sizeof (COOCCUR) * (cos_count + 1));
/* Position 0 of each row is cos_count */
info -> cos[i][0].x = 0.0;
info -> cos[i][0].column = cos_count;
/* Term found is a query term */
for (unsigned int j = 1; j <= cos_count; j++) {
if (info -> textio) {
fscanf (fp, "%u", &w2);
fscanf (fp, "%u", &freq);
}
else {
fread (&w2, sizeof (unsigned int), 1, fp);
fread (&freq, sizeof (unsigned int), 1, fp);
}
if (freq != 0) {
nonzero_count++;
}
if (w2 > info -> n) {
fprintf (stderr, "Word 2 (%u) is out of range (%u).\n", w2, info -> n);
exit (EXIT_FAILURE);
}
if (info -> debug) {
fprintf (stderr, "==\t\tRead (%u, %u) --> %u\n", i, w2, freq);
}
SET_COS (i, j, w2, DOLOG (freq));
sum_freq += freq;
found_pairs++;
}
}
FCLOSE (fp);
/* Check if the header of the file matches reality */
if (found_w1 != info -> m) {
fprintf (stderr, "Not all query terms found! (%u, %u)\n", found_w1, info -> m);
exit (EXIT_FAILURE);
}
if (info -> verbose) {
unsigned int zero_count = (info -> m * info -> n) - nonzero_count;
fprintf (stderr, "==\tMaximum number of pairs: %u\n", info -> m * info -> n);
fprintf (stderr, "==\tActual number of pairs in data file: %u\n", found_pairs);
fprintf (stderr, "==\tPercentage of zeroes: %.2f %% (%u)\n", (double) zero_count / (double) ((info -> m * info -> n)) * 100, zero_count);
fprintf (stderr, "==\tSum of co-occurrence counts: %u\n", sum_freq);
}
#if DEBUG
debugCheckCo (info);
#endif
time (&end);
info -> readCO_time += difftime (end, start);
return (true);
}
/* this function is run by the thread */
static void *
worker_func(void *args)
{
struct thread_comp_input *par = args;
struct thread_queue *tq = par->tq;
PROGRESS_DECL();
tq->on_create();
unsigned more_input = 1;
while (more_input) {
PROGRESS_START("SCANWAIT");
pthread_mutex_lock(&tq->scan_mtx);
PROGRESS_MSG("SCANWAIT");
/* reserve next input range */
PROGRESS_START("SCAN");
tq->reader.scan(par->scan_info, g_max_input_chunk_bytes);
PROGRESS_MSG("SCAN");
/* reserve the first empty out buffer. we must do this within
the scan_mtx to ensure output order. */
if (tq->n_outputs) {
PROGRESS_START("RESERVEOUT");
pthread_mutex_lock(&tq->out_mtx);
while (! tq->pool_status[EMPTY])
pthread_cond_wait(&tq->out_buf_avail, &tq->out_mtx);
reserve_out_buffer(par);
pthread_mutex_unlock(&tq->out_mtx);
PROGRESS_MSG("RESERVEOUT");
}
pthread_mutex_unlock(&tq->scan_mtx);
/* wait and reserve reader slot */
PROGRESS_START("READWAIT");
pthread_mutex_lock(&tq->read_mtx);
while (tq->n_reading == tq->n_max_reading)
pthread_cond_wait(&tq->read_slot_avail, &tq->read_mtx);
++tq->n_reading;
pthread_mutex_unlock(&tq->read_mtx);
PROGRESS_MSG("READWAIT");
PROGRESS_START("READ");
more_input = tq->reader.read(par->scan_info, par->buf);
PROGRESS_MSG("READ");
pthread_mutex_lock(&tq->read_mtx);
--tq->n_reading;
pthread_mutex_unlock(&tq->read_mtx);
pthread_cond_signal(&tq->read_slot_avail);
/* load the output buffer. */
PROGRESS_START("WORK");
struct managed_buf *reserved_out_buf = tq->n_outputs ? par->out->buf : NULL;
tq->worker(par->buf, more_input, par->scan_info, reserved_out_buf);
PROGRESS_MSG("WORK");
if (tq->n_outputs) {
PROGRESS_START("RESERVEOUT_UNLOAD");
pthread_mutex_lock(&tq->out_mtx);
PROGRESS_MSG("RESERVEOUT_UNLOAD");
set_outnode_status(tq, par->out, FULL);
par->out = NULL;
offload_full_buffers(par);
pthread_mutex_unlock(&tq->out_mtx);
}
}
tq->on_exit();
pthread_exit(NULL);
}
