void test_read_lat_long() {
FILE* file = fopen("testdata/gps_example_1", "r");
assert(file);
double lat, lon;
/* Check the first line */
assert(read_lat_long(file, &lat, &lon));
assert(abs(lat - 39.315828) < 0.000001);
assert(abs(lon - -120.167838) < 0.000001);
/* Check there are 132 lines total */
for (int i = 0; i < 131; ++i) {
assert(read_lat_long(file, &lat, &lon));
}
assert(!read_lat_long(file, &lat, &lon));
fclose(file);
}
void straight_line(int32_t s_east, int32_t s_north, int32_t e_east,
int32_t e_north, double *s_dist) {
double delta_east = e_east - s_east;
double delta_north = e_north - s_north;
double delta = sqrt(delta_east * delta_east + delta_north + delta_north);
// haversine formula for great-circle distance
double a = pow(sin(delta_north * RAD_PER_ARC), 2) +
pow(sin(delta_east * RAD_PER_ARC), 2) *
cos(s_north * RAD_PER_ARC) * cos(e_north * RAD_PER_ARC);
double c = 2 * atan2(sqrt(a), sqrt(1-a));
double dist = EARTH_RADIUS * c;
uint32_t num_steps = delta / WAYPOINT_GRANULARITY;
for (uint32_t i = 0; i < num_steps; ++i) {
int32_t east = s_east + (delta_east * i / num_steps);
int32_t north = s_north + (delta_north * i / num_steps);
uint16_t result = read_lat_long(east, north);
double cur_dist = *s_dist + dist * i / num_steps;
printf("%f, %d\n", cur_dist, result);
}
*s_dist += dist;
}