navit/navit/track.c

   1 /**
   2  * Navit, a modular navigation system.
   3  * Copyright (C) 2005-2008 Navit Team
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License
   7  * version 2 as published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the
  16  * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
  17  * Boston, MA  02110-1301, USA.
  18  */
  19
  20 #include <glib.h>
  21 #include <string.h>
  22 #include <time.h>
  23 #include <math.h>
  24 #include "item.h"
  25 #include "attr.h"
  26 #include "track.h"
  27 #include "debug.h"
  28 #include "transform.h"
  29 #include "coord.h"
  30 #include "route.h"
  31 #include "projection.h"
  32 #include "map.h"
  33 #include "mapset.h"
  34 #include "plugin.h"
  35 #include "vehicleprofile.h"
  36 #include "vehicle.h"
  37 #include "roadprofile.h"
  38 #include "util.h"
  39 #include "config.h"
  40 #include "xmlconfig.h"
  41 #include "callback.h"
  42
  43 struct object_func tracking_func;
  44
  45 struct tracking_line
  46 {
  47         struct street_data *street;
  48         struct tracking_line *next;
  49         int angle[0];
  50 };
  51
  52
  53 /**
  54  * @brief Conatins a list of previous speeds
  55  *
  56  * This structure is used to hold a list of previously reported
  57  * speeds. This data is used by the CDF.
  58  */
  59 struct cdf_speed {
  60         struct cdf_speed *next;
  61         int speed;
  62         time_t time;
  63 };
  64
  65 /**
  66  * @brief Contains data for the CDF
  67  *
  68  * This structure holds all data needed by the
  69  * cumulative displacement filter.
  70  */
  71 struct cdf_data {
  72         int extrapolating;
  73         int available;
  74         int first_pos;
  75         int poscount;
  76         int hist_size;
  77         struct cdf_speed *speed_hist;
  78         struct pcoord *pos_hist;
  79         int *dir_hist;
  80         double last_dist;
  81         struct pcoord last_out;
  82         int last_dir;
  83 };
  84
  85 struct tracking {
  86         struct object_func *func;
  87         int refcount;
  88         struct attr *attrs;
  89         struct callback_list *callback_list;
  90         struct mapset *ms;
  91         struct route *rt;
  92         struct map *map;
  93         struct vehicle *vehicle;
  94         struct vehicleprofile *vehicleprofile;
  95         struct coord last_updated;
  96         struct tracking_line *lines;
  97         struct tracking_line *curr_line;
  98         int pos;
  99         struct coord curr[2], curr_in, curr_out;
 100         int curr_angle;
 101         struct coord last[2], last_in, last_out;
 102         struct cdf_data cdf;
 103         struct attr *attr;
 104         int valid;
 105         int time;
 106         double direction, direction_matched;
 107         double speed;
 108         int coord_geo_valid;
 109         struct coord_geo coord_geo;
 110         enum projection pro;
 111         int street_direction;
 112         int no_gps;
 113         int tunnel;
 114         int angle_pref;
 115         int connected_pref;
 116         int nostop_pref;
 117         int offroad_limit_pref;
 118         int route_pref;
 119         int overspeed_pref;
 120         int overspeed_percent_pref;
 121         int tunnel_extrapolation;
 122 };
 123
 124
 125
 126
 127 static void
 128 tracking_init_cdf(struct cdf_data *cdf, int hist_size)
 129 {
 130         cdf->extrapolating = 0;
 131         cdf->available = 0;
 132         cdf->poscount = 0;
 133         cdf->last_dist = 0;
 134         cdf->hist_size = hist_size;
 135
 136         cdf->pos_hist = g_new0(struct pcoord, hist_size);
 137         cdf->dir_hist = g_new0(int, hist_size);
 138 }
 139
 140 // Variables for finetuning the CDF
 141
 142 // Minimum average speed
 143 #define CDF_MINAVG 1.f
 144 // Maximum average speed
 145 #define CDF_MAXAVG 6.f // only ~ 20 km/h
 146                 // We need a low value here because otherwise we would extrapolate whenever we are not accelerating
 147
 148 // Mininum distance (square of it..), below which we ignore gps updates
 149 #define CDF_MINDIST 49 // 7 meters, I guess this value has to be changed for pedestrians.
 150
 151 #if 0
 152 static void
 153 tracking_process_cdf(struct cdf_data *cdf, struct pcoord *pin, struct pcoord *pout, int dirin, int *dirout, int cur_speed, time_t fixtime)
 154 {
 155         struct cdf_speed *speed,*sc,*sl;
 156         double speed_avg;
 157         int speed_num,i;
 158
 159         if (cdf->hist_size == 0) {
 160                 dbg(1,"No CDF.\n");
 161                 *pout = *pin;
 162                 *dirout = dirin;
 163                 return;
 164         }
 165
 166         speed = g_new0(struct cdf_speed, 1);
 167         speed->speed = cur_speed;
 168         speed->time = fixtime;
 169
 170         speed->next = cdf->speed_hist;
 171         cdf->speed_hist = speed;
 172
 173         sc = speed;
 174         sl = NULL;
 175         speed_num = 0;
 176         speed_avg = 0;
 177         while (sc && ((fixtime - speed->time) < 4)) { // FIXME static maxtime
 178                 speed_num++;
 179                 speed_avg += sc->speed;
 180                 sl = sc;
 181                 sc = sc->next;
 182         }
 183
 184         speed_avg /= (double)speed_num;
 185
 186         if (sl) {
 187                 sl->next = NULL;
 188         }
 189
 190         while (sc) {
 191                 sl = sc->next;
 192                 g_free(sc);
 193                 sc = sl;
 194         }
 195
 196         if (speed_avg < CDF_MINAVG) {
 197                 speed_avg = CDF_MINAVG;
 198         } else if (speed_avg > CDF_MAXAVG) {
 199                 speed_avg = CDF_MAXAVG;
 200         }
 201
 202
 203         if (cur_speed >= speed_avg) {
 204                 if (cdf->extrapolating) {
 205                         cdf->poscount = 0;
 206                         cdf->extrapolating = 0;
 207                 }
 208
 209                 cdf->first_pos--;
 210                 if (cdf->first_pos < 0) {
 211                         cdf->first_pos = cdf->hist_size - 1;
 212                 }
 213
 214                 if (cdf->poscount < cdf->hist_size) {
 215                         cdf->poscount++;
 216                 }
 217
 218                 cdf->pos_hist[cdf->first_pos] = *pin;
 219                 cdf->dir_hist[cdf->first_pos] = dirin;
 220
 221                 *pout = *pin;
 222                 *dirout = dirin;
 223         } else if (cdf->poscount > 0) {
 224
 225                 double mx,my; // Average position's x and y values
 226                 double sx,sy; // Support vector
 227                 double dx,dy; // Difference between average and current position
 228                 double len;   // Length of support vector
 229                 double dist;
 230
 231                 mx = my = 0;
 232                 sx = sy = 0;
 233
 234                 for (i = 0; i < cdf->poscount; i++) {
 235                         mx += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x / cdf->poscount;
 236                         my += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y / cdf->poscount;
 237
 238
 239                         if (i != 0) {
 240                                 sx += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].x;
 241                                 sy += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].y;
 242                         }
 243
 244                 }
 245
 246                 if (cdf->poscount > 1) {
 247                         // Normalize the support vector
 248                         len = sqrt(sx * sx + sy * sy);
 249                         sx /= len;
 250                         sy /= len;
 251
 252                         // Calculate the new direction
 253                         *dirout = (int)rint(atan(sx / sy) / M_PI * 180 + 180);
 254                 } else {
 255                         // If we only have one position, we can't use differences of positions, but we have to use the reported
 256                         // direction of that position
 257                         sx = sin((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
 258                         sy = cos((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
 259                         *dirout = cdf->dir_hist[cdf->first_pos];
 260                 }
 261
 262
 263                 dx = pin->x - mx;
 264                 dy = pin->y - my;
 265                 dist = dx * sx + dy * sy;
 266
 267                 if (cdf->extrapolating && (dist < cdf->last_dist)) {
 268                         dist = cdf->last_dist;
 269                 }
 270
 271                 cdf->last_dist = dist;
 272                 cdf->extrapolating = 1;
 273
 274           pout->x = (int)rint(mx + sx * dist);
 275           pout->y = (int)rint(my + sy * dist);
 276                 pout->pro = pin->pro;
 277
 278         } else {
 279                 // We should extrapolate, but don't have an old position available
 280                 *pout = *pin;
 281                 *dirout = dirin;
 282         }
 283
 284         if (cdf->available) {
 285                 int dx,dy;
 286
 287                 dx = pout->x - cdf->last_out.x;
 288                 dy = pout->y - cdf->last_out.y;
 289
 290                 if ((dx*dx + dy*dy) < CDF_MINDIST) {
 291                         *pout = cdf->last_out;
 292                         *dirout = cdf->last_dir;
 293                 }
 294         }
 295
 296         cdf->last_out = *pout;
 297         cdf->last_dir = *dirout;
 298
 299         cdf->available = 1;
 300 }
 301 #endif
 302
 303 int
 304 tracking_get_angle(struct tracking *tr)
 305 {
 306         return tr->curr_angle;
 307 }
 308
 309 struct coord *
 310 tracking_get_pos(struct tracking *tr)
 311 {
 312         return &tr->curr_out;
 313 }
 314
 315 int
 316 tracking_get_street_direction(struct tracking *tr)
 317 {
 318         return tr->street_direction;
 319 }
 320
 321 int
 322 tracking_get_segment_pos(struct tracking *tr)
 323 {
 324         return tr->pos;
 325 }
 326
 327 struct street_data *
 328 tracking_get_street_data(struct tracking *tr)
 329 {
 330         if (tr->curr_line)
 331                 return tr->curr_line->street;
 332         return NULL;
 333 }
 334
 335 int
 336 tracking_get_attr(struct tracking *_this, enum attr_type type, struct attr *attr, struct attr_iter *attr_iter)
 337 {
 338         struct item *item;
 339         struct map_rect *mr;
 340         struct tracking_line *tl;
 341
 342         int result=0;
 343         dbg(1,"enter %s\n",attr_to_name(type));
 344         if (_this->attr) {
 345                 attr_free(_this->attr);
 346                 _this->attr=NULL;
 347         }
 348         attr->type=type;
 349         switch (type) {
 350         case attr_position_valid:
 351                 attr->u.num=_this->valid;
 352                 return 1;
 353         case attr_position_direction:
 354                 attr->u.numd=&_this->direction;
 355                 return 1;
 356         case attr_position_direction_matched:
 357                 attr->u.numd=&_this->direction_matched;
 358                 return 1;
 359         case attr_position_speed:
 360                 attr->u.numd=&_this->speed;
 361                 return 1;
 362         case attr_directed:
 363                 attr->u.num=_this->street_direction;
 364                 return 1;
 365         case attr_position_coord_geo:
 366                 if (!_this->coord_geo_valid) {
 367                         struct coord c;
 368                         c.x=_this->curr_out.x;
 369                         c.y=_this->curr_out.y;
 370                         transform_to_geo(_this->pro, &c, &_this->coord_geo);
 371                         _this->coord_geo_valid=1;
 372                 }
 373                 attr->u.coord_geo=&_this->coord_geo;
 374                 return 1;
 375         case attr_current_item:
 376                 if (! _this->curr_line || ! _this->curr_line->street)
 377                         return 0;
 378                 attr->u.item=&_this->curr_line->street->item;
 379                 return 1;
 380         case attr_street_count:
 381                 attr->u.num=0;
 382                 tl=_this->lines;
 383                 while (tl) {
 384                         attr->u.num++;
 385                         tl=tl->next;
 386                 }
 387                 return 1;
 388         default:
 389                 if (! _this->curr_line || ! _this->curr_line->street)
 390                         return 0;
 391                 item=&_this->curr_line->street->item;
 392                 mr=map_rect_new(item->map,NULL);
 393                 item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
 394                 if (item_attr_get(item, type, attr)) {
 395                         _this->attr=attr_dup(attr);
 396                         *attr=*_this->attr;
 397                         result=1;
 398                 }
 399                 map_rect_destroy(mr);
 400                 return result;
 401         }
 402 }
 403
 404 struct item *
 405 tracking_get_current_item(struct tracking *_this)
 406 {
 407         if (! _this->curr_line || ! _this->curr_line->street)
 408                 return NULL;
 409         return &_this->curr_line->street->item;
 410 }
 411
 412 int *
 413 tracking_get_current_flags(struct tracking *_this)
 414 {
 415         if (! _this->curr_line || ! _this->curr_line->street)
 416                 return NULL;
 417         return &_this->curr_line->street->flags;
 418 }
 419
 420 static void
 421 tracking_get_angles(struct tracking_line *tl)
 422 {
 423         int i;
 424         struct street_data *sd=tl->street;
 425         for (i = 0 ; i < sd->count-1 ; i++)
 426                 tl->angle[i]=transform_get_angle_delta(&sd->c[i], &sd->c[i+1], 0);
 427 }
 428
 429 static int
 430 street_data_within_selection(struct street_data *sd, struct map_selection *sel)
 431 {
 432         struct coord_rect r;
 433         struct map_selection *curr;
 434         int i;
 435
 436         if (!sel)
 437                 return 1;
 438         r.lu=sd->c[0];
 439         r.rl=sd->c[0];
 440         for (i = 1 ; i < sd->count ; i++) {
 441                 if (r.lu.x > sd->c[i].x)
 442                         r.lu.x=sd->c[i].x;
 443                 if (r.rl.x < sd->c[i].x)
 444                         r.rl.x=sd->c[i].x;
 445                 if (r.rl.y > sd->c[i].y)
 446                         r.rl.y=sd->c[i].y;
 447                 if (r.lu.y < sd->c[i].y)
 448                         r.lu.y=sd->c[i].y;
 449         }
 450         curr=sel;
 451         while (curr) {
 452                 struct coord_rect *sr=&curr->u.c_rect;
 453                 if (r.lu.x <= sr->rl.x && r.rl.x >= sr->lu.x &&
 454                     r.lu.y >= sr->rl.y && r.rl.y <= sr->lu.y)
 455                         return 1;
 456                 curr=curr->next;
 457         }
 458         return 0;
 459 }
 460
 461
 462 static void
 463 tracking_doupdate_lines(struct tracking *tr, struct coord *pc, enum projection pro)
 464 {
 465         int max_dist=1000;
 466         struct map_selection *sel;
 467         struct mapset_handle *h;
 468         struct map *m;
 469         struct map_rect *mr;
 470         struct item *item;
 471         struct street_data *street;
 472         struct tracking_line *tl;
 473         struct coord_geo g;
 474         struct coord cc;
 475
 476         dbg(1,"enter\n");
 477         h=mapset_open(tr->ms);
 478         while ((m=mapset_next(h,2))) {
 479                 cc.x = pc->x;
 480                 cc.y = pc->y;
 481                 if (map_projection(m) != pro) {
 482                         transform_to_geo(pro, &cc, &g);
 483                         transform_from_geo(map_projection(m), &g, &cc);
 484                 }
 485                 sel = route_rect(18, &cc, &cc, 0, max_dist);
 486                 mr=map_rect_new(m, sel);
 487                 if (!mr)
 488                         continue;
 489                 while ((item=map_rect_get_item(mr))) {
 490                         if (item_get_default_flags(item->type)) {
 491                                 street=street_get_data(item);
 492                                 if (street_data_within_selection(street, sel)) {
 493                                         tl=g_malloc(sizeof(struct tracking_line)+(street->count-1)*sizeof(int));
 494                                         tl->street=street;
 495                                         tracking_get_angles(tl);
 496                                         tl->next=tr->lines;
 497                                         tr->lines=tl;
 498                                 } else
 499                                         street_data_free(street);
 500                         }
 501                 }
 502                 map_selection_destroy(sel);
 503                 map_rect_destroy(mr);
 504         }
 505         mapset_close(h);
 506         dbg(1, "exit\n");
 507 }
 508
 509
 510 void
 511 tracking_flush(struct tracking *tr)
 512 {
 513         struct tracking_line *tl=tr->lines,*next;
 514         dbg(1,"enter(tr=%p)\n", tr);
 515
 516         while (tl) {
 517                 next=tl->next;
 518                 street_data_free(tl->street);
 519                 g_free(tl);
 520                 tl=next;
 521         }
 522         tr->lines=NULL;
 523         tr->curr_line = NULL;
 524 }
 525
 526 static int
 527 tracking_angle_diff(int a1, int a2, int full)
 528 {
 529         int ret=(a1-a2)%full;
 530         if (ret > full/2)
 531                 ret-=full;
 532         if (ret < -full/2)
 533                 ret+=full;
 534         return ret;
 535 }
 536
 537 static int
 538 tracking_angle_abs_diff(int a1, int a2, int full)
 539 {
 540         int ret=tracking_angle_diff(a1, a2, full);
 541         if (ret < 0)
 542                 ret=-ret;
 543         return ret;
 544 }
 545
 546 static int
 547 tracking_angle_delta(struct tracking *tr, int vehicle_angle, int street_angle, int flags)
 548 {
 549         int full=180,ret=360,fwd=0,rev=0;
 550         struct vehicleprofile *profile=tr->vehicleprofile;
 551
 552         if (profile) {
 553             fwd=((flags & profile->flags_forward_mask) == profile->flags);
 554             rev=((flags & profile->flags_reverse_mask) == profile->flags);
 555         }
 556         if (fwd || rev) {
 557                 if (!fwd || !rev) {
 558                         full=360;
 559                         if (rev)
 560                                 street_angle=(street_angle+180)%360;
 561                 }
 562                 ret=tracking_angle_abs_diff(vehicle_angle, street_angle, full);
 563         }
 564         return ret*ret;
 565 }
 566
 567 static int
 568 tracking_is_connected(struct tracking *tr, struct coord *c1, struct coord *c2)
 569 {
 570         if (c1[0].x == c2[0].x && c1[0].y == c2[0].y)
 571                 return 0;
 572         if (c1[0].x == c2[1].x && c1[0].y == c2[1].y)
 573                 return 0;
 574         if (c1[1].x == c2[0].x && c1[1].y == c2[0].y)
 575                 return 0;
 576         if (c1[1].x == c2[1].x && c1[1].y == c2[1].y)
 577                 return 0;
 578         return tr->connected_pref;
 579 }
 580
 581 static int
 582 tracking_is_no_stop(struct tracking *tr, struct coord *c1, struct coord *c2)
 583 {
 584         if (c1->x == c2->x && c1->y == c2->y)
 585                 return tr->nostop_pref;
 586         return 0;
 587 }
 588
 589 static int
 590 tracking_is_on_route(struct tracking *tr, struct route *rt, struct item *item)
 591 {
 592 #ifdef USE_ROUTING
 593         if (! rt)
 594                 return 0;
 595         if (route_contains(rt, item))
 596                 return 0;
 597         return tr->route_pref;
 598 #else
 599         return 0;
 600 #endif
 601 }
 602
 603 static int
 604 tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags)
 605 {
 606         int value=0;
 607         struct street_data *sd=t->street;
 608         dbg(2, "%d: (0x%x,0x%x)-(0x%x,0x%x)\n", offset, sd->c[offset].x, sd->c[offset].y, sd->c[offset+1].x, sd->c[offset+1].y);
 609         if (flags & 1) {
 610                 struct coord c1, c2, cp;
 611                 c1.x = sd->c[offset].x;
 612                 c1.y = sd->c[offset].y;
 613                 c2.x = sd->c[offset+1].x;
 614                 c2.y = sd->c[offset+1].y;
 615                 cp.x = tr->curr_in.x;
 616                 cp.y = tr->curr_in.y;
 617                 value+=transform_distance_line_sq(&c1, &c2, &cp, lpnt);
 618         }
 619         if (value >= min)
 620                 return value;
 621         if (flags & 2)
 622                 value += tracking_angle_delta(tr, tr->curr_angle, t->angle[offset], sd->flags)*tr->angle_pref>>4;
 623         if (value >= min)
 624                 return value;
 625         if ((flags & 4)  && tr->connected_pref)
 626                 value += tracking_is_connected(tr, tr->last, &sd->c[offset]);
 627         if ((flags & 8)  && tr->nostop_pref)
 628                 value += tracking_is_no_stop(tr, lpnt, &tr->last_out);
 629         if (value >= min)
 630                 return value;
 631         if ((flags & 16) && tr->route_pref)
 632                 value += tracking_is_on_route(tr, tr->rt, &sd->item);
 633         if ((flags & 32) && tr->overspeed_percent_pref && tr->overspeed_pref ) {
 634                 struct roadprofile *roadprofile=g_hash_table_lookup(tr->vehicleprofile->roadprofile_hash, (void *)t->street->item.type);
 635                 if (roadprofile && tr->speed > roadprofile->speed * tr->overspeed_percent_pref/ 100)
 636                         value += tr->overspeed_pref;
 637         }
 638         if ((flags & 64) && !!(sd->flags & AF_UNDERGROUND) != tr->no_gps)
 639                 value+=200;
 640         return value;
 641 }
 642
 643
 644 void
 645 tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile, enum projection pro)
 646 {
 647         struct tracking_line *t;
 648         int i,value,min,time;
 649         struct coord lpnt;
 650         struct coord cin;
 651         struct attr valid,speed_attr,direction_attr,coord_geo,lag,time_attr,static_speed,static_distance;
 652         double speed, direction;
 653         if (v)
 654                 tr->vehicle=v;
 655         if (vehicleprofile)
 656                 tr->vehicleprofile=vehicleprofile;
 657
 658         if (! tr->vehicle)
 659                 return;
 660         if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL))
 661                 valid.u.num=attr_position_valid_valid;
 662         if (valid.u.num == attr_position_valid_invalid) {
 663                 tr->valid=valid.u.num;
 664                 return;
 665         }
 666         if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL) ||
 667             !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL) ||
 668             !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL) ||
 669             !vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL)) {
 670                 dbg(0,"failed to get position data %d %d %d %d\n",
 671                 vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL),
 672             vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL),
 673             vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL),
 674             vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL));
 675                 return;
 676         }
 677         if (tr->tunnel_extrapolation) {
 678                 struct attr fix_type;
 679                 if (!vehicle_get_attr(tr->vehicle, attr_position_fix_type, &fix_type, NULL))
 680                         fix_type.u.num=2;
 681                 if (fix_type.u.num) {
 682                         tr->no_gps=0;
 683                         tr->tunnel=0;
 684                 } else
 685                         tr->no_gps=1;
 686         }
 687         if (!vehicleprofile_get_attr(vehicleprofile,attr_static_speed,&static_speed,NULL) || !vehicleprofile_get_attr(vehicleprofile,attr_static_distance,&static_distance,NULL)) {
 688                 static_speed.u.num=3;
 689                 static_distance.u.num=10;
 690                 dbg(1,"Using defaults for static position detection\n");
 691         }
 692         dbg(2,"Static speed: %u, static distance: %u\n",static_speed.u.num, static_distance.u.num);
 693         time=iso8601_to_secs(time_attr.u.str);
 694         speed=*speed_attr.u.numd;
 695         direction=*direction_attr.u.numd;
 696         tr->valid=attr_position_valid_valid;
 697         transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in);
 698         if ((speed < static_speed.u.num && transform_distance(pro, &tr->last_in, &tr->curr_in) < static_distance.u.num )) {
 699                 dbg(1,"static speed %f coord 0x%x,0x%x vs 0x%x,0x%x\n",speed,tr->last_in.x,tr->last_in.y, tr->curr_in.x, tr->curr_in.y);
 700                 tr->valid=attr_position_valid_static;
 701                 tr->speed=0;
 702                 return;
 703         }
 704         if (tr->tunnel) {
 705                 tr->curr_in=tr->curr_out;
 706                 dbg(0,"tunnel extrapolation speed %f dir %f\n",tr->speed,tr->direction);
 707                 dbg(0,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
 708                 speed=tr->speed;
 709                 direction=tr->curr_line->angle[tr->pos];
 710                 transform_project(pro, &tr->curr_in, tr->speed*tr->tunnel_extrapolation/36, tr->direction, &tr->curr_in);
 711                 dbg(0,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
 712         } else if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0) {
 713                 double espeed;
 714                 int edirection;
 715                 if (time-tr->time == 1) {
 716                         dbg(1,"extrapolating speed from %f and %f (%f)\n",tr->speed, speed, speed-tr->speed);
 717                         espeed=speed+(speed-tr->speed)*lag.u.num/10;
 718                         dbg(1,"extrapolating angle from %f and %f (%d)\n",tr->direction, direction, tracking_angle_diff(direction,tr->direction,360));
 719                         edirection=direction+tracking_angle_diff(direction,tr->direction,360)*lag.u.num/10;
 720                 } else {
 721                         dbg(1,"no speed and direction extrapolation\n");
 722                         espeed=speed;
 723                         edirection=direction;
 724                 }
 725                 dbg(1,"lag %d speed %f direction %d\n",lag.u.num,espeed,edirection);
 726                 dbg(1,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
 727                 transform_project(pro, &tr->curr_in, espeed*lag.u.num/36, edirection, &tr->curr_in);
 728                 dbg(1,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
 729         }
 730         tr->time=time;
 731         tr->pro=pro;
 732 #if 0
 733
 734         tracking_process_cdf(&tr->cdf, pc, &pcf, angle, &anglef, speed, fixtime);
 735 #endif
 736         tr->curr_angle=tr->direction=direction;
 737         tr->speed=speed;
 738         tr->last_in=tr->curr_in;
 739         tr->last_out=tr->curr_out;
 740         tr->last[0]=tr->curr[0];
 741         tr->last[1]=tr->curr[1];
 742         if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > 500) {
 743                 dbg(1, "update\n");
 744                 tracking_flush(tr);
 745                 tracking_doupdate_lines(tr, &tr->curr_in, pro);
 746                 tr->last_updated=tr->curr_in;
 747                 dbg(1,"update end\n");
 748         }
 749
 750         tr->street_direction=0;
 751         t=tr->lines;
 752         tr->curr_line=NULL;
 753         min=INT_MAX/2;
 754         while (t) {
 755                 struct street_data *sd=t->street;
 756                 for (i = 0; i < sd->count-1 ; i++) {
 757                         value=tracking_value(tr,t,i,&lpnt,min,-1);
 758                         if (value < min) {
 759                                 struct coord lpnt_tmp;
 760                                 int angle_delta=tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360);
 761                                 tr->curr_line=t;
 762                                 tr->pos=i;
 763                                 tr->curr[0]=sd->c[i];
 764                                 tr->curr[1]=sd->c[i+1];
 765                                 tr->direction_matched=t->angle[i];
 766                                 dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i,
 767                                         transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt_tmp),
 768                                         tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*tr->angle_pref,
 769                                         tracking_is_connected(tr, tr->last, &sd->c[i]) ? tr->connected_pref : 0,
 770                                         lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? tr->nostop_pref : 0,
 771                                         value
 772                                 );
 773                                 tr->curr_out.x=lpnt.x;
 774                                 tr->curr_out.y=lpnt.y;
 775                                 tr->coord_geo_valid=0;
 776                                 if (angle_delta < 70)
 777                                         tr->street_direction=1;
 778                                 else if (angle_delta > 110)
 779                                         tr->street_direction=-1;
 780                                 else
 781                                         tr->street_direction=0;
 782                                 min=value;
 783                         }
 784                 }
 785                 t=t->next;
 786         }
 787         dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min);
 788         if (!tr->curr_line || min > tr->offroad_limit_pref) {
 789                 tr->curr_out=tr->curr_in;
 790                 tr->coord_geo_valid=0;
 791                 tr->street_direction=0;
 792         }
 793         if (tr->curr_line && (tr->curr_line->street->flags & AF_UNDERGROUND)) {
 794                 if (tr->no_gps)
 795                         tr->tunnel=1;
 796         } else if (tr->tunnel) {
 797                 tr->speed=0;
 798         }
 799         dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
 800         callback_list_call_attr_0(tr->callback_list, attr_position_coord_geo);
 801 }
 802
 803 static int
 804 tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial)
 805 {
 806         switch (attr->type) {
 807         case attr_angle_pref:
 808                 tr->angle_pref=attr->u.num;
 809                 return 1;
 810         case attr_connected_pref:
 811                 tr->connected_pref=attr->u.num;
 812                 return 1;
 813         case attr_nostop_pref:
 814                 tr->nostop_pref=attr->u.num;
 815                 return 1;
 816         case attr_offroad_limit_pref:
 817                 tr->offroad_limit_pref=attr->u.num;
 818                 return 1;
 819         case attr_route_pref:
 820                 tr->route_pref=attr->u.num;
 821                 return 1;
 822         case attr_overspeed_pref:
 823                 tr->overspeed_pref=attr->u.num;
 824                 return 1;
 825         case attr_overspeed_percent_pref:
 826                 tr->overspeed_percent_pref=attr->u.num;
 827                 return 1;
 828         case attr_tunnel_extrapolation:
 829                 tr->tunnel_extrapolation=attr->u.num;
 830                 return 1;
 831         default:
 832                 return 0;
 833         }
 834 }
 835
 836 int
 837 tracking_set_attr(struct tracking *tr, struct attr *attr)
 838 {
 839         return tracking_set_attr_do(tr, attr, 0);
 840 }
 841
 842 int
 843 tracking_add_attr(struct tracking *this_, struct attr *attr)
 844 {
 845         switch (attr->type) {
 846         case attr_callback:
 847                 callback_list_add(this_->callback_list, attr->u.callback);
 848                 return 1;
 849         default:
 850                 return 0;
 851         }
 852 }
 853
 854 int
 855 tracking_remove_attr(struct tracking *this_, struct attr *attr)
 856 {
 857         switch (attr->type) {
 858         case attr_callback:
 859                 callback_list_remove(this_->callback_list, attr->u.callback);
 860                 return 1;
 861         default:
 862                 return 0;
 863         }
 864 }
 865
 866 struct tracking *
 867 tracking_ref(struct tracking *this_)
 868 {
 869         this_->refcount++;
 870         dbg(0,"refcount %d\n",this_->refcount);
 871         return this_;
 872 }
 873
 874 void
 875 tracking_unref(struct tracking *this_)
 876 {
 877         this_->refcount--;
 878         dbg(0,"refcount %d\n",this_->refcount);
 879         if (this_->refcount <= 0)
 880                 tracking_destroy(this_);
 881 }
 882
 883 struct object_func tracking_func = {
 884         attr_trackingo,
 885         (object_func_new)tracking_new,
 886         (object_func_get_attr)tracking_get_attr,
 887         (object_func_iter_new)NULL,
 888         (object_func_iter_destroy)NULL,
 889         (object_func_set_attr)tracking_set_attr,
 890         (object_func_add_attr)tracking_add_attr,
 891         (object_func_remove_attr)tracking_remove_attr,
 892         (object_func_init)tracking_init,
 893         (object_func_destroy)tracking_destroy,
 894         (object_func_dup)NULL,
 895         (object_func_ref)tracking_ref,
 896         (object_func_unref)tracking_unref,
 897 };
 898
 899
 900 struct tracking *
 901 tracking_new(struct attr *parent, struct attr **attrs)
 902 {
 903         struct tracking *this=g_new0(struct tracking, 1);
 904         struct attr hist_size;
 905         this->func=&tracking_func;
 906         this->refcount=1;
 907         this->angle_pref=10;
 908         this->connected_pref=10;
 909         this->nostop_pref=10;
 910         this->offroad_limit_pref=5000;
 911         this->route_pref=300;
 912         this->callback_list=callback_list_new();
 913
 914
 915         if (! attr_generic_get_attr(attrs, NULL, attr_cdf_histsize, &hist_size, NULL)) {
 916                 hist_size.u.num = 0;
 917         }
 918         if (attrs) {
 919                 for (;*attrs; attrs++)
 920                         tracking_set_attr_do(this, *attrs, 1);
 921         }
 922
 923         tracking_init_cdf(&this->cdf, hist_size.u.num);
 924
 925         return this;
 926 }
 927
 928 void
 929 tracking_set_mapset(struct tracking *this, struct mapset *ms)
 930 {
 931         this->ms=ms;
 932 }
 933
 934 void
 935 tracking_set_route(struct tracking *this, struct route *rt)
 936 {
 937         this->rt=rt;
 938 }
 939
 940 void
 941 tracking_destroy(struct tracking *tr)
 942 {
 943         if (tr->attr)
 944                 attr_free(tr->attr);
 945         tracking_flush(tr);
 946         callback_list_destroy(tr->callback_list);
 947         g_free(tr);
 948 }
 949
 950 struct map *
 951 tracking_get_map(struct tracking *this_)
 952 {
 953         struct attr *attrs[5];
 954         struct attr type,navigation,data,description;
 955         type.type=attr_type;
 956         type.u.str="tracking";
 957         navigation.type=attr_trackingo;
 958         navigation.u.tracking=this_;
 959         data.type=attr_data;
 960         data.u.str="";
 961         description.type=attr_description;
 962         description.u.str="Tracking";
 963
 964         attrs[0]=&type;
 965         attrs[1]=&navigation;
 966         attrs[2]=&data;
 967         attrs[3]=&description;
 968         attrs[4]=NULL;
 969         if (! this_->map)
 970                 this_->map=map_new(NULL, attrs);
 971         return this_->map;
 972 }
 973
 974
 975 struct map_priv {
 976         struct tracking *tracking;
 977 };
 978
 979 struct map_rect_priv {
 980         struct tracking *tracking;
 981         struct item item;
 982         struct tracking_line *curr,*next;
 983         int coord;
 984         enum attr_type attr_next;
 985         int ccount;
 986         int debug_idx;
 987         char *str;
 988 };
 989
 990 static int
 991 tracking_map_item_coord_get(void *priv_data, struct coord *c, int count)
 992 {
 993         struct map_rect_priv *this=priv_data;
 994         enum projection pro;
 995         int ret=0;
 996         dbg(1,"enter\n");
 997         while (this->ccount < 2 && count > 0) {
 998                 pro = map_projection(this->curr->street->item.map);
 999                 if (projection_mg != pro) {
1000                         transform_from_to(&this->curr->street->c[this->ccount+this->coord],
1001                                 pro,
1002                                 c ,projection_mg);
1003                 } else
1004                 *c=this->curr->street->c[this->ccount+this->coord];
1005                 dbg(1,"coord %d 0x%x,0x%x\n",this->ccount,c->x,c->y);
1006                 this->ccount++;
1007                 ret++;
1008                 c++;
1009                 count--;
1010         }
1011         return ret;
1012 }
1013
1014 static int
1015 tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr)
1016 {
1017         struct map_rect_priv *this_=priv_data;
1018         struct coord lpnt,*c;
1019         struct tracking *tr=this_->tracking;
1020         int value;
1021         attr->type=attr_type;
1022
1023         if (this_->str) {
1024                 g_free(this_->str);
1025                 this_->str=NULL;
1026         }
1027
1028         switch(attr_type) {
1029         case attr_debug:
1030                 switch(this_->debug_idx) {
1031                 case 0:
1032                         this_->debug_idx++;
1033                         this_->str=attr->u.str=g_strdup_printf("overall: %d (limit %d)",tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, -1), tr->offroad_limit_pref);
1034                         return 1;
1035                 case 1:
1036                         this_->debug_idx++;
1037                         c=&this_->curr->street->c[this_->coord];
1038                         value=tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 1);
1039                         this_->str=attr->u.str=g_strdup_printf("distance: (0x%x,0x%x) from (0x%x,0x%x)-(0x%x,0x%x) at (0x%x,0x%x) %d",
1040                                 tr->curr_in.x, tr->curr_in.y,
1041                                 c[0].x, c[0].y, c[1].x, c[1].y,
1042                                 lpnt.x, lpnt.y, value);
1043                         return 1;
1044                 case 2:
1045                         this_->debug_idx++;
1046                         this_->str=attr->u.str=g_strdup_printf("angle: %d to %d (flags %d) %d",
1047                                 tr->curr_angle, this_->curr->angle[this_->coord], this_->curr->street->flags & 3,
1048                                 tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 2));
1049                         return 1;
1050                 case 3:
1051                         this_->debug_idx++;
1052                         this_->str=attr->u.str=g_strdup_printf("connected: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 4));
1053                         return 1;
1054                 case 4:
1055                         this_->debug_idx++;
1056                         this_->str=attr->u.str=g_strdup_printf("no_stop: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 8));
1057                         return 1;
1058                 case 5:
1059                         this_->debug_idx++;
1060                         this_->str=attr->u.str=g_strdup_printf("route: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 16));
1061                         return 1;
1062                 case 6:
1063                         this_->debug_idx++;
1064                         this_->str=attr->u.str=g_strdup_printf("overspeed: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 32));
1065                         return 1;
1066                 case 7:
1067                         this_->debug_idx++;
1068                         this_->str=attr->u.str=g_strdup_printf("tunnel: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 64));
1069                         return 1;
1070                 case 8:
1071                         this_->debug_idx++;
1072                         this_->str=attr->u.str=g_strdup_printf("line %p", this_->curr);
1073                         return 1;
1074                 default:
1075                         this_->attr_next=attr_none;
1076                         return 0;
1077                 }
1078         case attr_any:
1079                 while (this_->attr_next != attr_none) {
1080                         if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr))
1081                                 return 1;
1082                 }
1083                 return 0;
1084         default:
1085                 attr->type=attr_none;
1086                 return 0;
1087         }
1088 }
1089
1090 static struct item_methods tracking_map_item_methods = {
1091         NULL,
1092         tracking_map_item_coord_get,
1093         NULL,
1094         tracking_map_item_attr_get,
1095 };
1096
1097
1098 static void
1099 tracking_map_destroy(struct map_priv *priv)
1100 {
1101         g_free(priv);
1102 }
1103
1104 static void
1105 tracking_map_rect_init(struct map_rect_priv *priv)
1106 {
1107         priv->next=priv->tracking->lines;
1108         priv->curr=NULL;
1109         priv->coord=0;
1110         priv->item.id_lo=0;
1111         priv->item.id_hi=0;
1112 }
1113
1114 static struct map_rect_priv *
1115 tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel)
1116 {
1117         struct tracking *tracking=priv->tracking;
1118         struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1);
1119         ret->tracking=tracking;
1120         tracking_map_rect_init(ret);
1121         ret->item.meth=&tracking_map_item_methods;
1122         ret->item.priv_data=ret;
1123         ret->item.type=type_tracking_100;
1124         return ret;
1125 }
1126
1127 static void
1128 tracking_map_rect_destroy(struct map_rect_priv *priv)
1129 {
1130         g_free(priv);
1131 }
1132
1133 static struct item *
1134 tracking_map_get_item(struct map_rect_priv *priv)
1135 {
1136         struct item *ret=&priv->item;
1137         int value;
1138         struct coord lpnt;
1139
1140         if (!priv->next)
1141                 return NULL;
1142         if (! priv->curr || priv->coord + 2 >= priv->curr->street->count) {
1143                 priv->curr=priv->next;
1144                 priv->next=priv->curr->next;
1145                 priv->coord=0;
1146                 priv->item.id_lo=0;
1147                 priv->item.id_hi++;
1148         } else {
1149                 priv->coord++;
1150                 priv->item.id_lo++;
1151         }
1152         value=tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, INT_MAX/2, -1);
1153         if (value < 64)
1154                 priv->item.type=type_tracking_100;
1155         else if (value < 128)
1156                 priv->item.type=type_tracking_90;
1157         else if (value < 256)
1158                 priv->item.type=type_tracking_80;
1159         else if (value < 512)
1160                 priv->item.type=type_tracking_70;
1161         else if (value < 1024)
1162                 priv->item.type=type_tracking_60;
1163         else if (value < 2048)
1164                 priv->item.type=type_tracking_50;
1165         else if (value < 4096)
1166                 priv->item.type=type_tracking_40;
1167         else if (value < 8192)
1168                 priv->item.type=type_tracking_30;
1169         else if (value < 16384)
1170                 priv->item.type=type_tracking_20;
1171         else if (value < 32768)
1172                 priv->item.type=type_tracking_10;
1173         else
1174                 priv->item.type=type_tracking_0;
1175         dbg(1,"item %d %d points\n", priv->coord, priv->curr->street->count);
1176         priv->ccount=0;
1177         priv->attr_next=attr_debug;
1178         priv->debug_idx=0;
1179         return ret;
1180 }
1181
1182 static struct item *
1183 tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo)
1184 {
1185         struct item *ret;
1186         tracking_map_rect_init(priv);
1187         while ((ret=tracking_map_get_item(priv))) {
1188                 if (ret->id_hi == id_hi && ret->id_lo == id_lo)
1189                         return ret;
1190         }
1191         return NULL;
1192 }
1193
1194 static struct map_methods tracking_map_meth = {
1195         projection_mg,
1196         "utf-8",
1197         tracking_map_destroy,
1198         tracking_map_rect_new,
1199         tracking_map_rect_destroy,
1200         tracking_map_get_item,
1201         tracking_map_get_item_byid,
1202         NULL,
1203         NULL,
1204         NULL,
1205 };
1206
1207 static struct map_priv *
1208 tracking_map_new(struct map_methods *meth, struct attr **attrs, struct callback_list *cbl)
1209 {
1210         struct map_priv *ret;
1211         struct attr *tracking_attr;
1212
1213         tracking_attr=attr_search(attrs, NULL, attr_trackingo);
1214         if (! tracking_attr)
1215                 return NULL;
1216         ret=g_new0(struct map_priv, 1);
1217         *meth=tracking_map_meth;
1218         ret->tracking=tracking_attr->u.tracking;
1219
1220         return ret;
1221 }
1222
1223
1224 void
1225 tracking_init(void)
1226 {
1227         plugin_register_map_type("tracking", tracking_map_new);
1228 }