static void min_heapify(int *row_len, int *reorder_map, int rows, int pos)
{
int min, tmp;
int left, right;
while (pos < rows)
{
// find minimum value from current position and its children
min = pos;
if ((left = LEFT(pos)) < rows && row_len[left] < row_len[pos])
{
min = left;
}
if ((right = RIGHT(pos)) < rows && row_len[right] < row_len[min])
{
min = right;
}
// if current position is the minimum
if (pos == min)
{
break;
}
// swap row length
swap_pos(row_len, pos, min);
// swap reorder map
swap_pos(reorder_map, pos, min);
pos = min;
}
}
void swap(int i,int j)
{
heap temp = *pq[i] ;
*pq[i] = *pq[j] ;
*pq[j] = temp ;
swap_pos(pq[i]->v->idx,pq[j]->v->idx) ;
}
static void row_sort(int *row_len, int *reorder_map, int rows)
{
int i;
// build the initial heap first
for (i = (rows - 2) >> 1; i >= 0; i--)
{
min_heapify(row_len, reorder_map, rows, i);
}
// move heap top(minimum) to the end and reheapify
for (i = rows - 1; i > 0; i--)
{
swap_pos(row_len, 0, i);
swap_pos(reorder_map, 0, i);
min_heapify(row_len, reorder_map, i, 0);
}
}
NV_INT32 pos_read_record (FILE *fp, POS_OUTPUT_T *pos)
{
if (!fread (pos, sizeof (POS_OUTPUT_T), 1, fp)) return (-1);
if (swap) swap_pos (pos);
/* Dealing with end of week midnight *&^@$^#%*! */
if (midnight && pos->gps_time < start_gps_time) pos->gps_time += WEEK_OFFSET;
return (0);
}
NV_INT64 pos_find_record (FILE *fp, POS_OUTPUT_T *pos, NV_INT64 timestamp)
{
NV_INT64 x[3], time_found;
NV_INT32 y[3], j;
POS_OUTPUT_T prev_pos, new_pos;
NV_FLOAT64 t1;
if (timestamp < start_timestamp || timestamp > end_timestamp) return (0);
t1 = (NV_FLOAT64) timestamp / 1000000.0 - start_week;
/* Load the x and y values into the local arrays. */
y[0] = start_record;
y[2] = end_record;
x[0] = start_timestamp;
x[1] = timestamp;
x[2] = end_timestamp;
/* Give it three shots at finding the time. */
for (j = 0; j < 3; j++)
{
y[1] = y[0] + (NV_INT32) ((NV_FLOAT64) (y[2] - y[0]) * ((NV_FLOAT64) (x[1] - x[0]) / (NV_FLOAT64) (x[2] - x[0])));
/* Get the time of the interpolated record. */
fseek (fp, (start_record + y[1] * sizeof (POS_OUTPUT_T)), SEEK_SET);
fread (pos, sizeof (POS_OUTPUT_T), 1, fp);
if (swap) swap_pos (pos);
/* Dealing with end of week midnight *&^@$^#%*! */
if (midnight && pos->gps_time < start_gps_time) pos->gps_time += WEEK_OFFSET;
time_found = ((NV_FLOAT64) start_week + pos->gps_time) * 1000000.0;
/* If time found is less than the time searched for... */
if (time_found < x[1])
{
x[0] = time_found;
y[0] = y[1];
}
/* If time found is greater than or equal to the time searched for... */
else
{
x[2] = time_found;
y[2] = y[1];
}
}
prev_pos = *pos;
/* If time found is less than the time searched for, walk forward. */
if (time_found <= x[1])
{
while (1)
{
y[1]++;
fseek (fp, (start_record + y[1] * sizeof (POS_OUTPUT_T)), SEEK_SET);
fread (pos, sizeof (POS_OUTPUT_T), 1, fp);
if (swap) swap_pos (pos);
/* Dealing with end of week midnight *&^@$^#%*! */
if (midnight && pos->gps_time < start_gps_time) pos->gps_time += WEEK_OFFSET;
time_found = ((NV_FLOAT64) start_week + pos->gps_time) * 1000000.0;
if (time_found >= x[1])
{
/*
pos_dump_record (prev_pos);
pos_dump_record (*pos);
*/
if (pos->gps_time - prev_pos.gps_time < 1000000.0)
{
new_pos = pos_interp (prev_pos, *pos, t1);
*pos = new_pos;
/*
pos_dump_record (*pos);
*/
return (timestamp);
}
else
{
return (time_found);
}
}
}
}
/* If time found is greater than the time searched for, walk backward. */
else
{
while (1)
{
y[1]--;
fseek (fp, (start_record + y[1] * sizeof (POS_OUTPUT_T)), SEEK_SET);
fread (pos, sizeof (POS_OUTPUT_T), 1, fp);
if (swap) swap_pos (pos);
/* Dealing with end of week midnight *&^@$^#%*! */
if (midnight && pos->gps_time < start_gps_time) pos->gps_time += WEEK_OFFSET;
time_found = ((NV_FLOAT64) start_week + pos->gps_time) * 1000000.0;
if (time_found <= x[1])
{
/*
pos_dump_record (*pos);
pos_dump_record (prev_pos);
*/
if (pos->gps_time - prev_pos.gps_time < 1000000.0)
{
new_pos = pos_interp (*pos, prev_pos, t1);
*pos = new_pos;
/*
pos_dump_record (*pos);
*/
return (timestamp);
}
else
{
return (time_found);
}
}
}
}
/* If you get to here you haven't found a match. */
return (0);
}
FILE *open_pos_file (NV_CHAR *path)
{
FILE *fp;
NV_INT32 i;
POS_OUTPUT_T pos;
time_t tv_sec;
NV_INT32 tv_nsec;
struct tm tm;
static NV_INT32 tz_set = 0;
NV_INT32 big_endian ();
/* Check the file name for following the naming convention as best we can. */
if (path[strlen (path) - 16] != '_' || path[strlen (path) - 9] != '_' || path[strlen (path) - 4] != '.')
{
fprintf (stderr, "\n\n\nFilename %s does not conform to standard\n", path);
fprintf (stderr, "Please correct to match _YYMMDD_NNNN.pos or _YYMMDD_NNNN.out standard.\n\n\n");
fflush (stderr);
return (NULL);
}
sscanf (&path[strlen (path) - 15], "%02d%02d%02d", &year, &month, &day);
/* tm struct wants years since 1900!!! */
tm.tm_year = year + 100;
tm.tm_mon = month - 1;
tm.tm_mday = day;
tm.tm_hour = 0.0;
tm.tm_min = 0.0;
tm.tm_sec = 0.0;
tm.tm_isdst = -1;
if (!tz_set)
{
#ifdef NVWIN3X
#ifdef __MINGW64__
putenv("TZ=GMT");
tzset();
#else
_putenv("TZ=GMT");
_tzset();
#endif
#else
putenv("TZ=GMT");
tzset();
#endif
tz_set = 1;
}
/* Get seconds from the epoch (01-01-1970) for the date in the filename.
This will also give us the day of the week for the GPS seconds of
week calculation. */
tv_sec = mktime (&tm);
tv_nsec = 0.0;
/* Subtract the number of days since Saturday midnight (Sunday morning) in seconds. */
tv_sec = tv_sec - (tm.tm_wday * 86400);
start_week = tv_sec;
/* We have to assume that the file is little endian since there is no
header and no field that we can use to deduce what it is. */
swap = (NV_BOOL) big_endian ();
if ((fp = fopen (path, "rb")) == NULL)
{
return ((FILE *) NULL);
}
else
{
fread (&pos, sizeof (POS_OUTPUT_T), 1, fp);
if (swap) swap_pos (&pos);
start_timestamp = (NV_INT64) (((NV_FLOAT64) start_week + pos.gps_time) * 1000000.0);
start_record = 0;
start_gps_time = pos.gps_time;
i = fseek (fp, -sizeof (POS_OUTPUT_T), SEEK_END);
fread (&pos, sizeof (POS_OUTPUT_T), 1, fp);
if (swap) swap_pos (&pos);
end_timestamp = (NV_INT64) (((NV_FLOAT64) start_week + pos.gps_time) * 1000000.0);
/* Check for crossing midnight at end of GPS week (stupid f***ing Applanix bozos). */
if (end_timestamp < start_timestamp)
{
midnight = NVTrue;
end_timestamp += ((NV_INT64) WEEK_OFFSET * 1000000);
}
end_record = ftell (fp) / sizeof (POS_OUTPUT_T);
fseek (fp, 0, SEEK_SET);
}
return (fp);
}
