Adding comments to route.c / route.h
[profile/ivi/navit.git] / navit / navit / route.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 /** @file route.c
21  * @brief Contains code related to finding a route from a position to a destination
22  *
23  * Routing uses segments, points and items. Items are items from the map: Streets, highways, etc.
24  * Segments represent such items, or parts of it. Generally, a segment is a driveable path. An item
25  * can be represented by more than one segment - in that case it is "segmented". Each segment has an
26  * "offset" associated, that indicates at which position in a segmented item this segment is - a 
27  * segment representing a not-segmented item always has the offset 1.
28  * A point is located at the end of segments, often connecting several segments.
29  * 
30  * The code in this file will make navit find a route between a position and a destination.
31  * It accomplishes this by first building a "route graph". This graph contains segments and
32  * points.
33  *
34  * After building this graph in route_graph_build(), the function route_graph_flood() assigns every 
35  * point and segment a "value" which represents the "costs" of traveling from this point to the
36  * destination. This is done by Dijkstra's algorithm.
37  *
38  * When the graph is built a "route path" is created, which is a path in this graph from a given
39  * position to the destination determined at time of building the graph.
40  */
41
42 #include <stdio.h>
43 #include <stdlib.h>
44 #include <string.h>
45 #if 0
46 #include <math.h>
47 #include <assert.h>
48 #include <unistd.h>
49 #include <sys/time.h>
50 #endif
51 #include <glib.h>
52 #include "config.h"
53 #include "debug.h"
54 #include "profile.h"
55 #include "coord.h"
56 #include "projection.h"
57 #include "map.h"
58 #include "mapset.h"
59 #include "item.h"
60 #include "route.h"
61 #include "track.h"
62 #include "point.h"
63 #include "graphics.h"
64 #include "transform.h"
65 #include "plugin.h"
66 #include "fib.h"
67
68
69 struct map_priv {
70         struct route *route;
71 };
72
73 int debug_route=0;
74
75 /**
76  * @brief A point in the route graph
77  *
78  * This represents a point in the route graph. A point usually connects two or more segments,
79  * but there are also points which don't do that (e.g. at the end of a dead-end).
80  */
81 struct route_graph_point {
82         struct route_graph_point *next;          /** Linked-list pointer to a list of all route_graph_points */
83         struct route_graph_point *hash_next; /** Pointer to a chained hashlist of all route_graph_points with this hash */
84         struct route_graph_segment *start;       /** Pointer to a list of segments of which this point is the start. The links 
85                                                                                   *  of this linked-list are in route_graph_segment->start_next.*/
86         struct route_graph_segment *end;         /** Pointer to a list of segments of which this pointer is the end. The links
87                                                                                   *  of this linked-list are in route_graph_segment->end_next. */
88         struct route_graph_segment *seg;         /** Pointer to the segment one should use to reach the destination at
89                                                                                   *  least costs */
90         struct fibheap_el *el;                           /** When this point is put on a Fibonacci heap, this is a pointer
91                                                                                   *  to this point's heap-element */
92         int value;                                                       /** The cost at which one can reach the destination from this point on */
93         struct coord c;                                          /** Coordinates of this point */
94 };
95
96 /**
97  * @brief A segment in the route graph
98  *
99  * This is a segment in the route graph. A segment represents a driveable way.
100  */
101 struct route_graph_segment {
102         struct route_graph_segment *next;                       /** Linked-list pointer to a list of all route_graph_segments */
103         struct route_graph_segment *start_next;         /** Pointer to the next element in the list of segments that start at the 
104                                                                                                  *  same point. Start of this list is in route_graph_point->start. */
105         struct route_graph_segment *end_next;           /** Pointer to the next element in the list of segments that end at the
106                                                                                                  *  same point. Start of this list is in route_graph_point->end. */
107         struct route_graph_point *start;                        /** Pointer to the point this segment starts at. */
108         struct route_graph_point *end;                          /** Pointer to the point this segment ends at. */
109         struct item item;                                                       /** The item (e.g. street) that this segment represents. */
110         int flags;
111         int len;                                                                        /** Length of this segment */
112         int offset;                                                                     /** If the item represented by this segment is "segmented" (i.e. 
113                                                                                                  *  is represented by several segments instead of just one), this 
114                                                                                                  *  indicates the position of this segment in the item - for items
115                                                                                                  *  that are not segmented this should always be 1 */
116 };
117
118 /**
119  * @brief A segment in the route path
120  *
121  * This is a segment in the route path.
122  */
123 struct route_path_segment {
124         struct route_path_segment *next;        /** Pointer to the next segment in the path */
125         struct item item;                                       /** The item (e.g. street) this segment represents */
126         int length;                                                     /** Length of the segment */
127         int offset;                                                     /** Same as in route_graph_segment->offset */
128         int direction;                                          /** Order in which the coordinates are ordered. >0 means "First
129                                                                                  *  coordinate of the segment is the first coordinate of the item", <=0 
130                                                                                  *  means reverse. */
131         unsigned ncoords;                                       /** How many coordinates does this segment have? */
132         struct coord c[0];                                      /** Pointer to the ncoords coordinates of this segment */
133 };
134
135 /**
136  * @brief Usually represents a destination or position
137  *
138  * This struct usually represents a destination or position
139  */
140 struct route_info {
141         struct coord c;                  /** The actual destination / position */
142         struct coord lp;                 /** The nearest point on a street to c */
143         int pos;                                 /** The position of lp within the coords of the street */
144         int lenpos;                      /** Distance between lp and the end of the street */
145         int lenneg;                      /** Distance between lp and the start of the street */
146         int lenextra;                    /** Distance between lp and c */
147
148         struct street_data *street; /** The street lp is on */
149 };
150
151 /**
152  * @brief A complete route path
153  *
154  * This structure describes a whole routing path
155  */
156 struct route_path {
157         struct route_path_segment *path;                        /** The first segment in the path, i.e. the segment one should 
158                                                                                                  *  drive in next */
159         struct route_path_segment *path_last;           /** The last segment in the path */
160         /* XXX: path_hash is not necessery now */
161         struct item_hash *path_hash;                            /** A hashtable of all the items represented by this route's segements */
162 };
163
164 #define RF_FASTEST      (1<<0)
165 #define RF_SHORTEST     (1<<1)
166 #define RF_AVOIDHW      (1<<2)
167 #define RF_AVOIDPAID    (1<<3)
168 #define RF_LOCKONROAD   (1<<4)
169 #define RF_SHOWGRAPH    (1<<5)
170
171 struct route {
172         int version;
173         struct mapset *ms;
174         unsigned flags;
175         struct route_info *pos;
176         struct route_info *dst;
177
178         struct route_graph *graph;
179         struct route_path *path2;
180         struct map *map;
181         struct map *graph_map;
182         int speedlist[route_item_last-route_item_first+1];
183 };
184
185 /**
186  * @brief A complete route graph
187  *
188  * This structure describes a whole routing graph
189  */
190 struct route_graph {
191         struct route_graph_point *route_points;         /** Pointer to the first route_graph_point in the linked list of 
192                                                                                                  *  all points */
193         struct route_graph_segment *route_segments; /** Pointer to the first route_graph_segment in the linked list of all 
194                                                                                                  *  segments */
195 #define HASH_SIZE 8192
196         struct route_graph_point *hash[HASH_SIZE];      /** A hashtable containing all route_graph_points in this graph */
197 };
198
199 #define HASHCOORD(c) ((((c)->x +(c)->y) * 2654435761UL) & (HASH_SIZE-1))
200
201 static struct route_info * route_find_nearest_street(struct mapset *ms, struct pcoord *c);
202 static struct route_graph_point *route_graph_get_point(struct route_graph *this, struct coord *c);
203 static void route_graph_update(struct route *this);
204 static struct route_path *route_path_new(struct route_graph *this, struct route_path *oldpath, struct route_info *pos, struct route_info *dst, int *speedlist);
205 static void route_process_street_graph(struct route_graph *this, struct item *item);
206 static void route_graph_destroy(struct route_graph *this);
207 static void route_path_update(struct route *this);
208
209 /**
210  * @brief Returns the projection used for this route
211  *
212  * @param route The route to return the projection for
213  * @return The projection used for this route
214  */
215 static enum projection route_projection(struct route *route)
216 {
217         struct street_data *street;
218         street = route->pos ? route->pos->street : route->dst->street;
219         return map_projection(street->item.map);
220 }
221
222 /**
223  * @brief Destroys a route_path
224  *
225  * @param this The route_path to be destroyed
226  */
227 static void
228 route_path_destroy(struct route_path *this)
229 {
230         struct route_path_segment *c,*n;
231         if (! this)
232                 return;
233         if (this->path_hash) {
234                 item_hash_destroy(this->path_hash);
235                 this->path_hash=NULL;
236         }
237         c=this->path;
238         while (c) {
239                 n=c->next;
240                 g_free(c);
241                 c=n;
242         }
243         g_free(this);
244 }
245
246 /**
247  * @brief Creates a completely new route structure
248  *
249  * @param attrs Not used
250  * @return The newly created route
251  */
252 struct route *
253 route_new(struct attr **attrs)
254 {
255         struct route *this=g_new0(struct route, 1);
256         if (!this) {
257                 printf("%s:Out of memory\n", __FUNCTION__);
258                 return NULL;
259         }
260         return this;
261 }
262
263 /**
264  * @brief Sets the mapset of the route passwd
265  *
266  * @param this The route to set the mapset for
267  * @param ms The mapset to set for this route
268  */
269 void
270 route_set_mapset(struct route *this, struct mapset *ms)
271 {
272         this->ms=ms;
273 }
274
275 /**
276  * @brief Returns the mapset of the route passed
277  *
278  * @param this The route to get the mapset of
279  * @return The mapset of the route passed
280  */
281 struct mapset *
282 route_get_mapset(struct route *this)
283 {
284         return this->ms;
285 }
286
287 /**
288  * @brief Returns the current position within the route passed
289  *
290  * @param this The route to get the position for
291  * @return The position within the route passed
292  */
293 struct route_info *
294 route_get_pos(struct route *this)
295 {
296         return this->pos;
297 }
298
299 /**
300  * @brief Returns the destination of the route passed
301  *
302  * @param this The route to get the destination for
303  * @return The destination of the route passed
304  */
305 struct route_info *
306 route_get_dst(struct route *this)
307 {
308         return this->dst;
309 }
310
311 /**
312  * @brief Returns the speedlist of the route passed
313  *
314  * @param this The route to get the speedlist for
315  * @return The speedlist of the route passed
316  */
317 int *
318 route_get_speedlist(struct route *this)
319 {
320         return this->speedlist;
321 }
322
323 /**
324  * @brief Checks if the path is calculated for the route passed
325  *
326  * @param this The route to check
327  * @return True if the path is calculated, false if not
328  */
329 int
330 route_get_path_set(struct route *this)
331 {
332         return this->path2 != NULL;
333 }
334
335 /**
336  * @brief Sets the driving speed for a certain itemtype
337  *
338  * @param this The route to set the speed for
339  * @param type The itemtype to set the speed for
340  * @param value The speed that should be set
341  * @return True on success, false if the itemtype does not exist
342  */
343 int
344 route_set_speed(struct route *this, enum item_type type, int value)
345 {
346         if (type < route_item_first || type > route_item_last) {
347                 dbg(0,"street type %d out of range [%d,%d]", type, route_item_first, route_item_last);
348                 return 0;
349         }
350         this->speedlist[type-route_item_first]=value;
351         return 1;
352 }
353
354 /**
355  * @brief Checks if the route passed contains a certain item within the route path
356  *
357  * This function checks if a certain items exists in the path that navit will guide
358  * the user to his destination. It does *not* check if this item exists in the route 
359  * graph!
360  *
361  * @param this The route to check for this item
362  * @param item The item to search for
363  * @return True if the item was found, false if the item was not found or the route was not calculated
364  */
365 int
366 route_contains(struct route *this, struct item *item)
367 {
368         if (! this->path2 || !this->path2->path_hash)
369                 return 0;
370         return  (int)item_hash_lookup(this->path2->path_hash, item);
371 }
372
373 /**
374  * @brief Checks if the current position in a route is a certain item
375  *
376  * @param this The route to check for this item
377  * @param item The item to search for
378  * @return True if the current position is this item, false otherwise
379  */
380 int
381 route_pos_contains(struct route *this, struct item *item)
382 {
383         if (! this->pos)
384                 return 0;
385         return item_is_equal(this->pos->street->item, *item);
386 }
387
388 /**
389  * @brief Updates the route graph and the route path if something changed with the route
390  *
391  * This will update the route graph and the route path of the route if some of the
392  * route's settings (destination, position) have changed. 
393  * 
394  * @attention For this to work the route graph has to be destroyed if the route's 
395  * @attention destination is changed somewhere!
396  *
397  * @param this The route to update
398  */
399 static void
400 route_path_update(struct route *this)
401 {
402         struct route_path *oldpath = NULL;
403         if (! this->pos || ! this->dst) {
404                 route_path_destroy(this->path2);
405                 this->path2 = NULL;
406                 return;
407         }
408         /* the graph is destroyed when setting the destination */
409         if (this->graph && this->pos && this->dst && this->path2) {
410                 // we can try to update
411                 oldpath = this->path2;
412                 this->path2 = NULL;
413         }
414         if (! this->graph || !(this->path2=route_path_new(this->graph, oldpath, this->pos, this->dst, this->speedlist))) {
415                 profile(0,NULL);
416                 route_graph_update(this);
417                 this->path2=route_path_new(this->graph, oldpath, this->pos, this->dst, this->speedlist);
418                 profile(1,"route_path_new");
419                 profile(0,"end");
420         }
421         if (oldpath) {
422                 /* Destroy what's left */
423                 route_path_destroy(oldpath);
424         }
425 }
426
427 /** 
428  * @brief This will calculate all the distances stored in a route_info
429  *
430  * @param ri The route_info to calculate the distances for
431  * @param pro The projection used for this route
432  */
433 static void
434 route_info_distances(struct route_info *ri, enum projection pro)
435 {
436         int npos=ri->pos+1;
437         struct street_data *sd=ri->street;
438         /* 0 1 2 X 3 4 5 6 pos=2 npos=3 count=7 0,1,2 3,4,5,6*/
439         ri->lenextra=transform_distance(pro, &ri->lp, &ri->c);
440         ri->lenneg=transform_polyline_length(pro, sd->c, npos)+transform_distance(pro, &sd->c[ri->pos], &ri->lp);
441         ri->lenpos=transform_polyline_length(pro, sd->c+npos, sd->count-npos)+transform_distance(pro, &sd->c[npos], &ri->lp);
442 }
443
444 /**
445  * @brief This sets the current position of the route passed
446  *
447  * This will set the current position of the route passed to the street that is nearest to the
448  * passed coordinates. It also automatically updates the route.
449  *
450  * @param this The route to set the position of
451  * @param pos Coordinates to set as position
452  */
453 void
454 route_set_position(struct route *this, struct pcoord *pos)
455 {
456         if (this->pos)
457                 route_info_free(this->pos);
458         this->pos=NULL;
459         this->pos=route_find_nearest_street(this->ms, pos);
460         dbg(1,"this->pos=%p\n", this->pos);
461         if (! this->pos)
462                 return;
463         route_info_distances(this->pos, pos->pro);
464         if (this->dst) 
465                 route_path_update(this);
466 }
467
468 /**
469  * @brief Sets a route's current position based on coordinates from tracking
470  *
471  * @param this The route to set the current position of
472  * @param tracking The tracking to get the coordinates from
473  */
474 void
475 route_set_position_from_tracking(struct route *this, struct tracking *tracking)
476 {
477         struct coord *c;
478         struct route_info *ret;
479
480         dbg(2,"enter\n");
481         c=tracking_get_pos(tracking);
482         ret=g_new0(struct route_info, 1);
483         if (!ret) {
484                 printf("%s:Out of memory\n", __FUNCTION__);
485                 return;
486         }
487         if (this->pos)
488                 route_info_free(this->pos);
489         this->pos=NULL;
490         ret->c=*c;
491         ret->lp=*c;
492         ret->pos=tracking_get_segment_pos(tracking);
493         ret->street=street_data_dup(tracking_get_street_data(tracking));
494         route_info_distances(ret, projection_mg);
495         dbg(3,"c->x=0x%x, c->y=0x%x pos=%d item=(0x%x,0x%x)\n", c->x, c->y, ret->pos, ret->street->item.id_hi, ret->street->item.id_lo);
496         dbg(3,"street 0=(0x%x,0x%x) %d=(0x%x,0x%x)\n", ret->street->c[0].x, ret->street->c[0].y, ret->street->count-1, ret->street->c[ret->street->count-1].x, ret->street->c[ret->street->count-1].y);
497         this->pos=ret;
498         if (this->dst) 
499                 route_path_update(this);
500         dbg(2,"ret\n");
501 }
502
503 /* Used for debuging of route_rect, what routing sees */
504 struct map_selection *route_selection;
505
506 /**
507  * @brief Returns a single map selection
508  */
509 struct map_selection *
510 route_rect(int order, struct coord *c1, struct coord *c2, int rel, int abs)
511 {
512         int dx,dy,sx=1,sy=1,d,m;
513         struct map_selection *sel=g_new(struct map_selection, 1);
514         if (!sel) {
515                 printf("%s:Out of memory\n", __FUNCTION__);
516                 return sel;
517         }
518         sel->order[layer_town]=0;
519         sel->order[layer_poly]=0;
520         sel->order[layer_street]=order;
521         dbg(1,"%p %p\n", c1, c2);
522         dx=c1->x-c2->x;
523         dy=c1->y-c2->y;
524         if (dx < 0) {
525                 sx=-1;
526                 sel->u.c_rect.lu.x=c1->x;
527                 sel->u.c_rect.rl.x=c2->x;
528         } else {
529                 sel->u.c_rect.lu.x=c2->x;
530                 sel->u.c_rect.rl.x=c1->x;
531         }
532         if (dy < 0) {
533                 sy=-1;
534                 sel->u.c_rect.lu.y=c2->y;
535                 sel->u.c_rect.rl.y=c1->y;
536         } else {
537                 sel->u.c_rect.lu.y=c1->y;
538                 sel->u.c_rect.rl.y=c2->y;
539         }
540         if (dx*sx > dy*sy) 
541                 d=dx*sx;
542         else
543                 d=dy*sy;
544         m=d*rel/100+abs;        
545         sel->u.c_rect.lu.x-=m;
546         sel->u.c_rect.rl.x+=m;
547         sel->u.c_rect.lu.y+=m;
548         sel->u.c_rect.rl.y-=m;
549         sel->next=NULL;
550         return sel;
551 }
552
553 /**
554  * @brief Returns a list of map selections useable to create a route graph
555  *
556  * Returns a list of  map selections useable to get a  map rect from which items can be
557  * retrieved to build a route graph. The selections are a rectangle with
558  * c1 and c2 as two corners.
559  *
560  * @param c1 Corner 1 of the rectangle
561  * @param c2 Corder 2 of the rectangle
562  */
563 static struct map_selection *
564 route_calc_selection(struct coord *c1, struct coord *c2)
565 {
566         struct map_selection *ret,*sel;
567         sel=route_rect(4, c1, c2, 25, 0);
568         ret=sel;
569         sel->next=route_rect(8, c1, c1, 0, 40000);
570         sel=sel->next;
571         sel->next=route_rect(18, c1, c1, 0, 10000);
572         sel=sel->next;
573         sel->next=route_rect(8, c2, c2, 0, 40000);
574         sel=sel->next;
575         sel->next=route_rect(18, c2, c2, 0, 10000);
576         /* route_selection=ret; */
577         return ret;
578 }
579
580 /**
581  * @brief Destroys a list of map selections
582  *
583  * @param sel Start of the list to be destroyed
584  */
585 static void
586 route_free_selection(struct map_selection *sel)
587 {
588         struct map_selection *next;
589         while (sel) {
590                 next=sel->next;
591                 g_free(sel);
592                 sel=next;
593         }
594 }
595
596
597 /**
598  * @brief Sets the destination of a route
599  *
600  * This sets the destination of a route to the street nearest to the coordinates passed
601  * and updates the route.
602  *
603  * @param this The route to set the destination for
604  * @param dst Coordinates to set as destination
605  */
606 void
607 route_set_destination(struct route *this, struct pcoord *dst)
608 {
609         profile(0,NULL);
610         if (this->dst)
611                 route_info_free(this->dst);
612         this->dst=NULL;
613         if (dst)
614                 this->dst=route_find_nearest_street(this->ms, dst);
615         route_info_distances(this->dst, dst->pro);
616         profile(1,"find_nearest_street");
617
618         /* The graph has to be destroyed and set to NULL, otherwise route_path_update() doesn't work */
619         route_graph_destroy(this->graph);
620         this->graph=NULL;
621         route_path_update(this);
622         profile(0,"end");
623 }
624
625 /**
626  * @brief Gets the route_graph_point with the specified coordinates
627  *
628  * @param this The route in which to search
629  * @param c Coordinates to search for
630  * @return The point at the specified coordinates or NULL if not found
631  */
632 static struct route_graph_point *
633 route_graph_get_point(struct route_graph *this, struct coord *c)
634 {
635         struct route_graph_point *p;
636         int hashval=HASHCOORD(c);
637         p=this->hash[hashval];
638         while (p) {
639                 if (p->c.x == c->x && p->c.y == c->y) 
640                         return p;
641                 p=p->hash_next;
642         }
643         return NULL;
644 }
645
646 /**
647  * @brief Inserts a point into the route graph at the specified coordinates
648  *
649  * This will insert a point into the route graph at the coordinates passed in f.
650  * Note that the point is not yet linked to any segments.
651  *
652  * @param this The route to insert the point into
653  * @param f The coordinates at which the point should be inserted
654  * @return The point inserted or NULL on failure
655  */
656 static struct route_graph_point *
657 route_graph_add_point(struct route_graph *this, struct coord *f)
658 {
659         int hashval;
660         struct route_graph_point *p;
661
662         p=route_graph_get_point(this,f);
663         if (!p) {
664                 hashval=HASHCOORD(f);
665                 if (debug_route)
666                         printf("p (0x%x,0x%x)\n", f->x, f->y);
667                 p=g_new(struct route_graph_point,1);
668                 if (!p) {
669                         printf("%s:Out of memory\n", __FUNCTION__);
670                         return p;
671                 }
672                 p->hash_next=this->hash[hashval];
673                 this->hash[hashval]=p;
674                 p->next=this->route_points;
675                 p->el=NULL;
676                 p->start=NULL;
677                 p->end=NULL;
678                 p->seg=NULL;
679                 p->value=INT_MAX;
680                 p->c=*f;
681                 this->route_points=p;
682         }
683         return p;
684 }
685
686 /**
687  * @brief Frees all the memory used for points in the route graph passed
688  *
689  * @param this The route graph to delete all points from
690  */
691 static void
692 route_graph_free_points(struct route_graph *this)
693 {
694         struct route_graph_point *curr,*next;
695         curr=this->route_points;
696         while (curr) {
697                 next=curr->next;
698                 g_free(curr);
699                 curr=next;
700         }
701         this->route_points=NULL;
702         memset(this->hash, 0, sizeof(this->hash));
703 }
704
705 /**
706  * @brief Inserts a new segment into the route graph
707  *
708  * This function performs a check if a segment for the item specified already exists, and inserts
709  * a new segment representing this item if it does not.
710  *
711  * @param this The route graph to insert the segment into
712  * @param start The graph point which should be connected to the start of this segment
713  * @param end The graph point which should be connected to the end of this segment
714  * @param len The length of this segment
715  * @param item The item that is represented by this segment
716  * @param flags Flags for this segment
717  * @param offset If the item passed in "item" is segmented (i.e. divided into several segments), this indicates the position of this segment within the item
718  */
719 static void
720 route_graph_add_segment(struct route_graph *this, struct route_graph_point *start,
721                         struct route_graph_point *end, int len, struct item *item,
722                         int flags, int offset)
723 {
724         struct route_graph_segment *s;
725         s=start->start;
726         while (s) {
727                 if (item_is_equal(*item, s->item)) 
728                         return;
729                 s=s->start_next;
730         }
731         s = g_new0(struct route_graph_segment, 1);
732         if (!s) {
733                 printf("%s:Out of memory\n", __FUNCTION__);
734                 return;
735         }
736         s->start=start;
737         s->start_next=start->start;
738         start->start=s;
739         s->end=end;
740         s->end_next=end->end;
741         end->end=s;
742         g_assert(len >= 0);
743         s->len=len;
744         s->item=*item;
745         s->flags=flags;
746         s->offset = offset;
747         s->next=this->route_segments;
748         this->route_segments=s;
749         if (debug_route)
750                 printf("l (0x%x,0x%x)-(0x%x,0x%x)\n", start->c.x, start->c.y, end->c.x, end->c.y);
751 }
752
753 /**
754  * @brief Gets all the coordinates of an item
755  *
756  * This will get all the coordinates of the item i and return them in c,
757  * up to max coordinates. Additionally it is possible to limit the coordinates
758  * returned to all the coordinates of the item between the two coordinates
759  * start end end.
760  *
761  * @important Make shure that whatever c points to has enough memory allocated
762  * @important to hold max coordinates!
763  *
764  * @param i The item to get the coordinates of
765  * @param c Pointer to memory allocated for holding the coordinates
766  * @param max Maximum number of coordinates to return
767  * @param start First coordinate to get
768  * @param end Last coordinate to get
769  */
770 static int get_item_seg_coords(struct item *i, struct coord *c, int max,
771                 struct coord *start, struct coord *end)
772 {
773         struct map_rect *mr;
774         struct item *item;
775         int rc = 0, p = 0;
776         struct coord c1;
777         mr=map_rect_new(i->map, NULL);
778         if (!mr)
779                 return 0;
780         item = map_rect_get_item_byid(mr, i->id_hi, i->id_lo);
781         if (item) {
782                 rc = item_coord_get(item, &c1, 1);
783                 while (rc && (c1.x != start->x || c1.y != start->y)) {
784                         rc = item_coord_get(item, &c1, 1);
785                 }
786                 while (rc && p < max) {
787                         c[p++] = c1;
788                         if (c1.x == end->x && c1.y == end->y)
789                                 break;
790                         rc = item_coord_get(item, &c1, 1);
791                 }
792         }
793         map_rect_destroy(mr);
794         return p;
795 }
796
797 /**
798  * @brief Returns and removes one segment from a path
799  *
800  * @param path The path to take the segment from
801  * @param item The item whose segment to remove
802  * @param offset Offset of the segment within the item to remove. If the item is not segmented this should be 1.
803  * @return The segment removed
804  */
805 static struct route_path_segment *
806 route_extract_segment_from_path(struct route_path *path, struct item *item,
807                                                  int offset)
808 {
809         struct route_path_segment *sp = NULL, *s;
810         s = path->path;
811         while (s) {
812                 if (s->offset == offset && item_is_equal(s->item,*item)) {
813                         if (sp) {
814                                 sp->next = s->next;
815                                 break;
816                         } else {
817                                 path->path = s->next;
818                                 break;
819                         }
820                 }
821                 sp = s;
822                 s = s->next;
823         }
824         if (s)
825                 item_hash_remove(path->path_hash, item);
826         return s;
827 }
828
829 /**
830  * @brief Adds a segment and the end of a path
831  *
832  * @param this The path to add the segment to
833  * @param segment The segment to add
834  */
835 static void
836 route_path_add_segment(struct route_path *this, struct route_path_segment *segment)
837 {
838         if (!this->path)
839                 this->path=segment;
840         if (this->path_last)
841                 this->path_last->next=segment;
842         this->path_last=segment;
843 }
844
845 /**
846  * @brief Adds a new item to a path
847  *
848  * This adds a new item to a path, creating a new segment for it. Please note that this function does not check
849  * if the item passed is segmented - it will create exactly one segment.
850  *
851  * @param this The path to add the item to
852  * @param item The item to add
853  * @param len The length of the item
854  * @param first (Optional) coordinate to add to the start of the item. If none should be added, make this NULL.
855  * @param c Pointer to count coordinates of the item.
856  * @param cound Number of coordinates in c
857  * @param last (Optional) coordinate to add to the end of the item. If none should be added, make this NULL.
858  * @param dir Direction to add the coordinates in. Greater than zero means "start with the first coordinate in c", all other values mean "start with the last coordinate in c"
859  */
860 static void
861 route_path_add_item(struct route_path *this, struct item *item, int len, struct coord *first, struct coord *c, int count, struct coord *last, int dir)
862 {
863         int i,idx=0,ccount=count + (first ? 1:0) + (last ? 1:0);
864         struct route_path_segment *segment;
865         
866         segment=g_malloc0(sizeof(*segment) + sizeof(struct coord) * ccount);
867         segment->ncoords=ccount;
868         segment->direction=dir;
869         if (first)
870                 segment->c[idx++]=*first;
871         if (dir > 0) {
872                 for (i = 0 ; i < count ; i++)
873                         segment->c[idx++]=c[i];
874         } else {
875                 for (i = 0 ; i < count ; i++)
876                         segment->c[idx++]=c[count-i-1];
877         }
878         if (last)
879                 segment->c[idx++]=*last;
880         segment->length=len;
881         if (item)
882                 segment->item=*item;
883         route_path_add_segment(this, segment);
884 }
885
886 /**
887  * @brief Inserts a new item into the path
888  * 
889  * This function does almost the same as "route_apth_add_item()", but identifies
890  * the item to add by a segment from the route graph. Another difference is that it "copies" the
891  * segment from the route graph, i.e. if the item is segmented, only the segment passed in rgs will
892  * be added to the route path, not all segments of the item. 
893  *
894  * The function can be sped up by passing an old path already containing this segment in oldpath - 
895  * the segment will then be extracted from this old path. Please note that in this case the direction
896  * parameter has no effect.
897  *
898  * @param this The path to add the item to
899  * @param oldpath Old path containing the segment to be added. Speeds up the function, but can be NULL.
900  * @param rgs Segment of the route graph that should be "copied" to the route path
901  * @param len Length of the item to be added
902  * @param offset Offset of rgs within the item it represents
903  * @param dir Order in which to add the coordinates. See route_path_add_item()
904  */
905 static void
906 route_path_add_item_from_graph(struct route_path *this, struct route_path *oldpath,
907                 struct route_graph_segment *rgs, int len, int offset, int dir)
908 {
909         struct route_path_segment *segment;
910         int i,ccnt = 0;
911         struct coord ca[2048];
912
913         if (oldpath) {
914                 ccnt = (int)item_hash_lookup(oldpath->path_hash, &rgs->item);
915                 if (ccnt) {
916                         segment = route_extract_segment_from_path(oldpath,
917                                                          &rgs->item, offset);
918                         if (segment)
919                                 goto linkold;
920                 }
921         }
922
923         ccnt = get_item_seg_coords(&rgs->item, ca, 2047, &rgs->start->c, &rgs->end->c);
924         segment= g_malloc0(sizeof(*segment) + sizeof(struct coord) * ccnt);
925         if (!segment) {
926                 printf("%s:Out of memory\n", __FUNCTION__);
927                 return;
928         }
929         segment->direction=dir;
930         if (dir > 0) {
931                 for (i = 0 ; i < ccnt ; i++)
932                         segment->c[i]=ca[i];
933         } else {
934                 for (i = 0 ; i < ccnt ; i++)
935                         segment->c[i]=ca[ccnt-i-1];
936         }
937         segment->ncoords = ccnt;
938         segment->item=rgs->item;
939         segment->offset = offset;
940 linkold:
941         segment->length=len;
942         segment->next=NULL;
943         item_hash_insert(this->path_hash,  &rgs->item, (void *)ccnt);
944         route_path_add_segment(this, segment);
945 }
946
947 /**
948  * @brief Destroys all segments of a route graph
949  *
950  * @param this The graph to destroy all segments from
951  */
952 static void
953 route_graph_free_segments(struct route_graph *this)
954 {
955         struct route_graph_segment *curr,*next;
956         curr=this->route_segments;
957         while (curr) {
958                 next=curr->next;
959                 g_free(curr);
960                 curr=next;
961         }
962         this->route_segments=NULL;
963 }
964
965 /**
966  * @brief Destroys a route graph
967  * 
968  * @param this The route graph to be destroyed
969  */
970 static void
971 route_graph_destroy(struct route_graph *this)
972 {
973         if (this) {
974                 route_graph_free_points(this);
975                 route_graph_free_segments(this);
976                 g_free(this);
977         }
978 }
979
980 /**
981  * @brief Returns the time needed to drive len on item
982  *
983  * @param speedlist The speedlist that should be used
984  * @param item The item to be driven on
985  * @param len The length to drive
986  * @return The time needed to drive len on item
987  */
988 int
989 route_time(int *speedlist, struct item *item, int len)
990 {
991         if (item->type < route_item_first || item->type > route_item_last) {
992                 dbg(0,"street type %d out of range [%d,%d]", item->type, route_item_first, route_item_last);
993                 return len*36;
994         }
995         return len*36/speedlist[item->type-route_item_first];
996 }
997
998 /**
999  * @brief Returns the "costs" of driving len on item
1000  *
1001  * @param speedlist The speedlist that should be used
1002  * @param item The item to be driven on
1003  * @param len The length to drive
1004  * @return The "costs" needed to drive len on item
1005  */  
1006 static int
1007 route_value(int *speedlist, struct item *item, int len)
1008 {
1009         int ret;
1010         if (len < 0) {
1011                 printf("len=%d\n", len);
1012         }
1013         g_assert(len >= 0);
1014         ret=route_time(speedlist, item, len);
1015         dbg(1, "route_value(0x%x, %d)=%d\n", item->type, len, ret);
1016         return ret;
1017 }
1018
1019 /**
1020  * @brief Adds an item to the route graph
1021  *
1022  * This adds an item (e.g. a street) to the route graph, creating as many segments as needed for a
1023  * segmented item.
1024  *
1025  * @param this The route graph to add to
1026  * @param item The item to add
1027  */
1028 static void
1029 route_process_street_graph(struct route_graph *this, struct item *item)
1030 {
1031 #ifdef AVOID_FLOAT
1032         int len=0;
1033 #else
1034         double len=0;
1035 #endif
1036         struct route_graph_point *s_pnt,*e_pnt;
1037         struct coord c,l;
1038         struct attr attr;
1039         int flags = 0;
1040         int segmented = 0;
1041         int offset = 1;
1042
1043         if (item_coord_get(item, &l, 1)) {
1044                 if (item_attr_get(item, attr_flags, &attr)) {
1045                         flags = attr.u.num;
1046                         if (flags & AF_SEGMENTED)
1047                                 segmented = 1;
1048                 }
1049                 s_pnt=route_graph_add_point(this,&l);
1050                 if (!segmented) {
1051                         while (item_coord_get(item, &c, 1)) {
1052                                 len+=transform_distance(map_projection(item->map), &l, &c);
1053                                 l=c;
1054                         }
1055                         e_pnt=route_graph_add_point(this,&l);
1056                         g_assert(len >= 0);
1057                         route_graph_add_segment(this, s_pnt, e_pnt, len, item, flags, offset);
1058                 } else {
1059                         int isseg,rc;
1060                         int sc = 0;
1061                         do {
1062                                 isseg = item_coord_is_segment(item);
1063                                 rc = item_coord_get(item, &c, 1);
1064                                 if (rc) {
1065                                         len+=transform_distance(map_projection(item->map), &l, &c);
1066                                         l=c;
1067                                         if (isseg) {
1068                                                 e_pnt=route_graph_add_point(this,&l);
1069                                                 route_graph_add_segment(this, s_pnt, e_pnt, len, item, flags, offset);
1070                                                 offset++;
1071                                                 s_pnt=route_graph_add_point(this,&l);
1072                                                 len = 0;
1073                                         }
1074                                 }
1075                         } while(rc);
1076                         e_pnt=route_graph_add_point(this,&l);
1077                         g_assert(len >= 0);
1078                         sc++;
1079                         route_graph_add_segment(this, s_pnt, e_pnt, len, item, flags, offset);
1080                 }
1081         }
1082 }
1083
1084 /**
1085  * @brief Compares the costs of reaching the destination from two points on
1086  * 
1087  * @important Do not pass anything other than route_graph_points in v1 and v2!
1088  *
1089  * @param v1 Point1 
1090  * @param v2 Point2
1091  * @return The additional costs of v1 compared to v2 (may be negative)
1092  */
1093 static int
1094 compare(void *v1, void *v2)
1095 {
1096         struct route_graph_point *p1=v1;
1097         struct route_graph_point *p2=v2;
1098 #if 0
1099         if (debug_route)
1100                 printf("compare %d (%p) vs %d (%p)\n", p1->value,p1,p2->value,p2);
1101 #endif
1102         return p1->value-p2->value;
1103 }
1104
1105 /**
1106  * @brief Calculates the routing costs for each point
1107  *
1108  * This function is the heart of routing. It assigns each point in the route graph a
1109  * cost at which one can reach the destination from this point on. Additionally it assigns
1110  * each point a segment one should follow from this point on to reach the destination at the
1111  * stated costs.
1112  * 
1113  * This function uses Dijkstra's algorithm to do the routing. To understand it you should have a look
1114  * at this algorithm.
1115  */
1116 static void
1117 route_graph_flood(struct route_graph *this, struct route_info *dst, int *speedlist)
1118 {
1119         struct route_graph_point *p_min,*end=NULL;
1120         struct route_graph_segment *s;
1121         int min,new,old,val;
1122         struct fibheap *heap; /* This heap will hold all points with "temporarily" calculated costs */
1123         struct street_data *sd=dst->street;
1124
1125         heap = fh_makeheap();   
1126         fh_setcmp(heap, compare);
1127
1128         if (! (sd->flags & AF_ONEWAYREV)) { /* If we may drive in the direction of the coordinates of the item, the first coordinate is one starting point */
1129                 end=route_graph_get_point(this, &sd->c[0]);
1130                 g_assert(end != 0);
1131                 end->value=route_value(speedlist, &sd->item, dst->lenneg);
1132                 end->el=fh_insert(heap, end);
1133         }
1134
1135         if (! (sd->flags & AF_ONEWAY)) { /* If we may drive against the direction of the coordinates, the last coordinate is another starting point */
1136                 end=route_graph_get_point(this, &sd->c[sd->count-1]);
1137                 g_assert(end != 0);
1138                 end->value=route_value(speedlist, &sd->item, dst->lenpos);
1139                 end->el=fh_insert(heap, end);
1140         }
1141
1142         dbg(1,"0x%x,0x%x\n", end->c.x, end->c.y);
1143         for (;;) {
1144                 p_min=fh_extractmin(heap); /* Starting Dijkstra by selecting the point with the minimum costs on the heap */
1145                 if (! p_min) /* There are no more points with temporarily calculated costs, Dijkstra has finished */
1146                         break;
1147                 min=p_min->value;
1148                 if (debug_route)
1149                         printf("extract p=%p free el=%p min=%d, 0x%x, 0x%x\n", p_min, p_min->el, min, p_min->c.x, p_min->c.y);
1150                 p_min->el=NULL; /* This point is permanently calculated now, we've taken it out of the heap */
1151                 s=p_min->start;
1152                 while (s) { /* Iterating all the segments leading away from our point to update the points at their ends */
1153                         val=route_value(speedlist, &s->item, s->len);
1154 #if 0
1155                         val+=val*2*street_route_contained(s->str->segid);
1156 #endif
1157                         new=min+val;
1158                         if (debug_route)
1159                                 printf("begin %d len %d vs %d (0x%x,0x%x)\n",new,val,s->end->value, s->end->c.x, s->end->c.y);
1160                         if (new < s->end->value && !(s->flags & AF_ONEWAY)) { /* We've found a less costly way to reach the end of s, update it */
1161                                 s->end->value=new;
1162                                 s->end->seg=s;
1163                                 if (! s->end->el) {
1164                                         if (debug_route)
1165                                                 printf("insert_end p=%p el=%p val=%d ", s->end, s->end->el, s->end->value);
1166                                         s->end->el=fh_insert(heap, s->end);
1167                                         if (debug_route)
1168                                                 printf("el new=%p\n", s->end->el);
1169                                 }
1170                                 else {
1171                                         if (debug_route)
1172                                                 printf("replace_end p=%p el=%p val=%d\n", s->end, s->end->el, s->end->value);
1173                                         fh_replacedata(heap, s->end->el, s->end);
1174                                 }
1175                         }
1176                         if (debug_route)
1177                                 printf("\n");
1178                         s=s->start_next;
1179                 }
1180                 s=p_min->end;
1181                 while (s) { /* Doing the same as above with the segments leading towards our point */
1182                         val=route_value(speedlist, &s->item, s->len);
1183                         new=min+val;
1184                         if (debug_route)
1185                                 printf("end %d len %d vs %d (0x%x,0x%x)\n",new,val,s->start->value,s->start->c.x, s->start->c.y);
1186                         if (new < s->start->value && !(s->flags & AF_ONEWAYREV)) {
1187                                 old=s->start->value;
1188                                 s->start->value=new;
1189                                 s->start->seg=s;
1190                                 if (! s->start->el) {
1191                                         if (debug_route)
1192                                                 printf("insert_start p=%p el=%p val=%d ", s->start, s->start->el, s->start->value);
1193                                         s->start->el=fh_insert(heap, s->start);
1194                                         if (debug_route)
1195                                                 printf("el new=%p\n", s->start->el);
1196                                 }
1197                                 else {
1198                                         if (debug_route)
1199                                                 printf("replace_start p=%p el=%p val=%d\n", s->start, s->start->el, s->start->value);
1200                                         fh_replacedata(heap, s->start->el, s->start);
1201                                 }
1202                         }
1203                         if (debug_route)
1204                                 printf("\n");
1205                         s=s->end_next;
1206                 }
1207         }
1208         fh_deleteheap(heap);
1209 }
1210
1211 /**
1212  * @brief Starts an "offroad" path
1213  *
1214  * This starts a path that is not located on a street. It creates a new route path
1215  * adding only one segment, that leads from pos to dest, and which is not associated with an item.
1216  *
1217  * @param this Not used
1218  * @param pos The starting position for the new path
1219  * @param dst The destination of the new path
1220  * @param dir Not used
1221  * @return The new path
1222  */
1223 static struct route_path *
1224 route_path_new_offroad(struct route_graph *this, struct route_info *pos, struct route_info *dst, int dir)
1225 {
1226         struct route_path *ret;
1227
1228         ret=g_new0(struct route_path, 1);
1229         ret->path_hash=item_hash_new();
1230         route_path_add_item(ret, NULL, pos->lenextra+dst->lenextra, &pos->c, NULL, 0, &dst->c, 1);
1231
1232         return ret;
1233 }
1234
1235 /**
1236  * @brief Creates a new "trivial" route
1237  * 
1238  * This function creates a new "trivial" route. A trivial route is a route that starts and ends on the same street,
1239  * so there is no routing needed. Depending on pos and dst it can optionally add some "offroad" part to the route.
1240  *
1241  * @param this The route graph to place the route on
1242  * @param pos The starting position for the new path
1243  * @param dst The destination of the new path
1244  * @param dir Direction of the coordinates to be added
1245  * @return The new path
1246  */
1247 static struct route_path *
1248 route_path_new_trivial(struct route_graph *this, struct route_info *pos, struct route_info *dst, int dir)
1249 {
1250         struct street_data *sd=pos->street;
1251         struct route_path *ret;
1252
1253         if (dir > 0) {
1254                 if (pos->lenextra + dst->lenextra + pos->lenneg-dst->lenneg > transform_distance(map_projection(sd->item.map), &pos->c, &dst->c))
1255                         return route_path_new_offroad(this, pos, dst, dir);
1256         } else {
1257                 if (pos->lenextra + dst->lenextra + pos->lenpos-dst->lenpos > transform_distance(map_projection(sd->item.map), &pos->c, &dst->c))
1258                         return route_path_new_offroad(this, pos, dst, dir);
1259         }
1260         ret=g_new0(struct route_path, 1);
1261         ret->path_hash=item_hash_new();
1262         if (pos->lenextra) 
1263                 route_path_add_item(ret, NULL, pos->lenextra, &pos->c, NULL, 0, &pos->lp, 1);
1264         if (dir > 0)
1265                 route_path_add_item(ret, &sd->item, pos->lenneg-dst->lenneg, &pos->lp, sd->c+pos->pos+1, dst->pos+pos->pos, &dst->lp, 1);
1266         else
1267                 route_path_add_item(ret, &sd->item, pos->lenpos-dst->lenpos, &pos->lp, sd->c+dst->pos+1, pos->pos-dst->pos, &dst->lp, -1);
1268         if (dst->lenextra) 
1269                 route_path_add_item(ret, NULL, dst->lenextra, &dst->lp, NULL, 0, &dst->c, 1);
1270         return ret;
1271 }
1272
1273 /**
1274  * @brief Creates a new route path
1275  * 
1276  * This creates a new non-trivial route. It therefore needs the routing information created by route_graph_flood, so
1277  * make shure to run route_graph_flood() after changing the destination before using this function.
1278  *
1279  * @param this The route graph to create the route from
1280  * @param oldpath (Optional) old path which may contain parts of the new part - this speeds things up a bit. May be NULL.
1281  * @param pos The starting position of the route
1282  * @param dst The destination of the route
1283  * @param speedlist The speedlist to use
1284  * @return The new route path
1285  */
1286 static struct route_path *
1287 route_path_new(struct route_graph *this, struct route_path *oldpath, struct route_info *pos, struct route_info *dst, int *speedlist)
1288 {
1289         struct route_graph_point *start1=NULL,*start2=NULL,*start;
1290         struct route_graph_segment *s=NULL;
1291         int len=0,segs=0;
1292         int seg_len;
1293 #if 0
1294         int time=0,hr,min,sec
1295 #endif
1296         unsigned int val1=0xffffffff,val2=0xffffffff;
1297         struct street_data *sd=pos->street;
1298         struct route_path *ret;
1299
1300         if (item_is_equal(pos->street->item, dst->street->item)) { /* We probably don't have to leave this street and can use a trivial route */
1301                 if (!(sd->flags & AF_ONEWAY) && pos->lenneg >= dst->lenneg) {
1302                         return route_path_new_trivial(this, pos, dst, -1);
1303                 }
1304                 if (!(sd->flags & AF_ONEWAYREV) && pos->lenpos >= dst->lenpos) {
1305                         return route_path_new_trivial(this, pos, dst, 1);
1306                 }
1307         } 
1308         if (! (sd->flags & AF_ONEWAY)) { /* Using the start of the current segment as one starting point */
1309                 start1=route_graph_get_point(this, &sd->c[0]);
1310                 if (! start1)
1311                         return NULL;
1312                 val1=start1->value+route_value(speedlist, &sd->item, pos->lenneg);
1313                 dbg(1,"start1: %d(route)+%d=%d\n", start1->value, val1-start1->value, val1);
1314         }
1315         if (! (sd->flags & AF_ONEWAYREV)) { /* Using the start of the current segment as an alternative starting point */
1316                 start2=route_graph_get_point(this, &sd->c[sd->count-1]);
1317                 if (! start2)
1318                         return NULL;
1319                 val2=start2->value+route_value(speedlist, &sd->item, pos->lenpos);
1320                 dbg(1,"start2: %d(route)+%d=%d\n", start2->value, val2-start2->value, val2);
1321         }
1322         dbg(1,"val1=%d val2=%d\n", val1, val2);
1323         if (val1 == val2) {
1324                 val1=start1->start->start->value;
1325                 val2=start2->end->end->value;
1326         }
1327         ret=g_new0(struct route_path, 1);
1328         if (pos->lenextra) 
1329                 route_path_add_item(ret, NULL, pos->lenextra, &pos->c, NULL, 0, &pos->lp, 1);
1330         if (start1 && (val1 < val2)) {
1331                 start=start1;
1332                 route_path_add_item(ret, &sd->item, pos->lenneg, &pos->lp, sd->c, pos->pos+1, NULL, -1);
1333         } else {
1334                 if (start2) {
1335                         start=start2;
1336                         route_path_add_item(ret, &sd->item, pos->lenpos, &pos->lp, sd->c+pos->pos+1, sd->count-pos->pos-1, NULL, 1);
1337                 } else {
1338                         printf("no route found, pos blocked\n");
1339                         return NULL;
1340                 }
1341         }
1342         ret->path_hash=item_hash_new();
1343         while ((s=start->seg)) { /* following start->seg, which indicates the least costly way to reach our destination */
1344                 segs++;
1345 #if 0
1346                 printf("start->value=%d 0x%x,0x%x\n", start->value, start->c.x, start->c.y);
1347 #endif
1348                 seg_len=s->len;
1349                 len+=seg_len;
1350                 if (s->start == start) {
1351                         route_path_add_item_from_graph(ret, oldpath, s, seg_len, s->offset, 1);
1352                         start=s->end;
1353                 } else {
1354                         route_path_add_item_from_graph(ret, oldpath, s, seg_len, s->offset, -1);
1355                         start=s->start;
1356                 }
1357         }
1358         sd=dst->street;
1359         dbg(1,"start->value=%d 0x%x,0x%x\n", start->value, start->c.x, start->c.y);
1360         dbg(1,"dst sd->flags=%d sd->c[0]=0x%x,0x%x sd->c[sd->count-1]=0x%x,0x%x\n", sd->flags, sd->c[0].x,sd->c[0].y, sd->c[sd->count-1].x, sd->c[sd->count-1].y);
1361         if (start->c.x == sd->c[0].x && start->c.y == sd->c[0].y) { /* Adding a final segment to reach the destination within the destination street */
1362                 route_path_add_item(ret, &sd->item, dst->lenneg, &dst->lp, sd->c, dst->pos+1, NULL, -1);
1363         } else if (start->c.x == sd->c[sd->count-1].x && start->c.y == sd->c[sd->count-1].y) {
1364                 route_path_add_item(ret, &sd->item, dst->lenpos, &dst->lp, sd->c+dst->pos+1, sd->count-dst->pos-1, NULL, 1);
1365         } else {
1366                 printf("no route found\n");
1367                 route_path_destroy(ret);
1368                 return NULL;
1369         }
1370         if (dst->lenextra) 
1371                 route_path_add_item(ret, NULL, dst->lenextra, &dst->lp, NULL, 0, &dst->c, 1);
1372         dbg(1, "%d segments\n", segs);
1373         return ret;
1374 }
1375
1376 /**
1377  * @brief Builds a new route graph from a mapset
1378  *
1379  * This function builds a new route graph from a map. Please note that this function does not
1380  * add any routing information to the route graph - this has to be done via the route_graph_flood()
1381  * function.
1382  *
1383  * The function does not create a graph covering the whole map, but only covering the rectangle
1384  * between c1 and c2.
1385  *
1386  * @param ms The mapset to build the route graph from
1387  * @param c1 Corner 1 of the rectangle to use from the map
1388  * @param c2 Corner 2 of the rectangle to use from the map
1389  * @return The new route graph.
1390  */
1391 static struct route_graph *
1392 route_graph_build(struct mapset *ms, struct coord *c1, struct coord *c2)
1393 {
1394         struct route_graph *ret=g_new0(struct route_graph, 1);
1395         struct map_selection *sel;
1396         struct mapset_handle *h;
1397         struct map_rect *mr;
1398         struct map *m;
1399         struct item *item;
1400
1401         if (!ret) {
1402                 printf("%s:Out of memory\n", __FUNCTION__);
1403                 return ret;
1404         }
1405         sel=route_calc_selection(c1, c2);
1406         h=mapset_open(ms);
1407         while ((m=mapset_next(h,1))) {
1408                 mr=map_rect_new(m, sel);
1409                 if (! mr)
1410                         continue;
1411                 while ((item=map_rect_get_item(mr))) {
1412                         if (item->type >= type_street_0 && item->type <= type_ferry) {
1413                                 route_process_street_graph(ret, item);
1414                         }
1415                 }
1416                 map_rect_destroy(mr);
1417         }
1418         mapset_close(h);
1419         route_free_selection(sel);
1420
1421         return ret;
1422 }
1423
1424 /**
1425  * @brief Updates the route graph
1426  *
1427  * This updates the route graph after settings in the route have changed. It also
1428  * adds routing information afterwards by calling route_graph_flood().
1429  * 
1430  * @param this The route to update the graph for
1431  */
1432 static void
1433 route_graph_update(struct route *this)
1434 {
1435         route_graph_destroy(this->graph);
1436         profile(1,"graph_free");
1437         this->graph=route_graph_build(this->ms, &this->pos->c, &this->dst->c);
1438         profile(1,"route_graph_build");
1439         route_graph_flood(this->graph, this->dst, this->speedlist);
1440         profile(1,"route_graph_flood");
1441         this->version++;
1442 }
1443
1444 /**
1445  * @brief Gets street data for an item
1446  *
1447  * @param item The item to get the data for
1448  * @return Street data for the item
1449  */
1450 struct street_data *
1451 street_get_data (struct item *item)
1452 {
1453         int maxcount=10000;
1454         struct coord c[maxcount];
1455         int count=0;
1456         struct street_data *ret;
1457         struct attr attr;
1458
1459         while (count < maxcount) {
1460                 if (!item_coord_get(item, &c[count], 1))
1461                         break;
1462                 count++;
1463         }
1464         if (count >= maxcount) {
1465                 dbg(0, "count=%d maxcount=%d id_hi=0x%x id_lo=0x%x\n", count, maxcount, item->id_hi, item->id_lo);
1466                 if (item_attr_get(item, attr_debug, &attr)) 
1467                         dbg(0,"debug='%s'\n", attr.u.str);
1468         }
1469         g_assert(count < maxcount);
1470         ret=g_malloc(sizeof(struct street_data)+count*sizeof(struct coord));
1471         ret->item=*item;
1472         ret->count=count;
1473         if (item_attr_get(item, attr_flags, &attr)) 
1474                 ret->flags=attr.u.num;
1475         else
1476                 ret->flags=0;
1477         memcpy(ret->c, c, count*sizeof(struct coord));
1478
1479         return ret;
1480 }
1481
1482 /**
1483  * @brief Copies street data
1484  * 
1485  * @param orig The street data to copy
1486  * @return The copied street data
1487  */ 
1488 struct street_data *
1489 street_data_dup(struct street_data *orig)
1490 {
1491         struct street_data *ret;
1492         int size=sizeof(struct street_data)+orig->count*sizeof(struct coord);
1493
1494         ret=g_malloc(size);
1495         memcpy(ret, orig, size);
1496
1497         return ret;
1498 }
1499
1500 /**
1501  * @brief Frees street data
1502  *
1503  * @param sd Street data to be freed
1504  */
1505 void
1506 street_data_free(struct street_data *sd)
1507 {
1508         g_free(sd);
1509 }
1510
1511 /**
1512  * @brief Finds the nearest street to a given coordinate
1513  *
1514  * @param ms The mapset to search in for the street
1515  * @param pc The coordinate to find a street nearby
1516  * @return The nearest street
1517  */
1518 static struct route_info *
1519 route_find_nearest_street(struct mapset *ms, struct pcoord *pc)
1520 {
1521         struct route_info *ret=NULL;
1522         int max_dist=1000;
1523         struct map_selection *sel;
1524         int dist,mindist=0,pos;
1525         struct mapset_handle *h;
1526         struct map *m;
1527         struct map_rect *mr;
1528         struct item *item;
1529         struct coord lp;
1530         struct street_data *sd;
1531         struct coord c;
1532         struct coord_geo g;
1533
1534         c.x = pc->x;
1535         c.y = pc->y;
1536         /*
1537          * This is not correct for two reasons:
1538          * - You may need to go back first
1539          * - Currently we allow mixing of mapsets
1540          */
1541         sel = route_rect(18, &c, &c, 0, max_dist);
1542         h=mapset_open(ms);
1543         while ((m=mapset_next(h,1))) {
1544                 c.x = pc->x;
1545                 c.y = pc->y;
1546                 if (map_projection(m) != pc->pro) {
1547                         transform_to_geo(pc->pro, &c, &g);
1548                         transform_from_geo(map_projection(m), &g, &c);
1549                 }
1550                 mr=map_rect_new(m, sel);
1551                 if (! mr)
1552                         continue;
1553                 while ((item=map_rect_get_item(mr))) {
1554                         if (item->type >= type_street_0 && item->type <= type_ferry) {
1555                                 sd=street_get_data(item);
1556                                 dist=transform_distance_polyline_sq(sd->c, sd->count, &c, &lp, &pos);
1557                                 if (!ret || dist < mindist) {
1558                                         if (ret) {
1559                                                 street_data_free(ret->street);
1560                                                 g_free(ret);
1561                                         }
1562                                         ret=g_new(struct route_info, 1);
1563                                         if (!ret) {
1564                                                 printf("%s:Out of memory\n", __FUNCTION__);
1565                                                 return ret;
1566                                         }
1567                                         ret->c=c;
1568                                         ret->lp=lp;
1569                                         ret->pos=pos;
1570                                         mindist=dist;
1571                                         ret->street=sd;
1572                                         dbg(1,"dist=%d id 0x%x 0x%x pos=%d\n", dist, item->id_hi, item->id_lo, pos);
1573                                 } else 
1574                                         street_data_free(sd);
1575                         }
1576                 }  
1577                 map_rect_destroy(mr);
1578         }
1579         mapset_close(h);
1580         map_selection_destroy(sel);
1581
1582         return ret;
1583 }
1584
1585 /**
1586  * @brief Destroys a route_info
1587  *
1588  * @param info The route info to be destroyed
1589  */
1590 void
1591 route_info_free(struct route_info *inf)
1592 {
1593         if (inf->street)
1594                 street_data_free(inf->street);
1595         g_free(inf);
1596 }
1597
1598
1599 #include "point.h"
1600
1601 /**
1602  * @brief Returns street data for a route info 
1603  *
1604  * @param rinf The route info to return the street data for
1605  * @return Street data for the route info
1606  */
1607 struct street_data *
1608 route_info_street(struct route_info *rinf)
1609 {
1610         return rinf->street;
1611 }
1612
1613 #if 0
1614 struct route_crossings *
1615 route_crossings_get(struct route *this, struct coord *c)
1616 {
1617       struct route_point *pnt;
1618       struct route_segment *seg;
1619       int crossings=0;
1620       struct route_crossings *ret;
1621
1622        pnt=route_graph_get_point(this, c);
1623        seg=pnt->start;
1624        while (seg) {
1625                 printf("start: 0x%x 0x%x\n", seg->item.id_hi, seg->item.id_lo);
1626                crossings++;
1627                seg=seg->start_next;
1628        }
1629        seg=pnt->end;
1630        while (seg) {
1631                 printf("end: 0x%x 0x%x\n", seg->item.id_hi, seg->item.id_lo);
1632                crossings++;
1633                seg=seg->end_next;
1634        }
1635        ret=g_malloc(sizeof(struct route_crossings)+crossings*sizeof(struct route_crossing));
1636        ret->count=crossings;
1637        return ret;
1638 }
1639 #endif
1640
1641
1642 struct map_rect_priv {
1643         struct route_info_handle *ri;
1644         enum attr_type attr_next;
1645         int pos;
1646         struct map_priv *mpriv;
1647         struct item item;
1648         int length;
1649         unsigned int last_coord;
1650         struct route_path_segment *seg,*seg_next;
1651         struct route_graph_point *point;
1652         struct route_graph_segment *rseg;
1653         char *str;
1654 };
1655
1656 static void
1657 rm_coord_rewind(void *priv_data)
1658 {
1659         struct map_rect_priv *mr = priv_data;
1660         mr->last_coord = 0;
1661 }
1662
1663 static void
1664 rm_attr_rewind(void *priv_data)
1665 {
1666         struct map_rect_priv *mr = priv_data;
1667         mr->attr_next = attr_street_item;
1668 }
1669
1670 static int
1671 rm_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr)
1672 {
1673         struct map_rect_priv *mr = priv_data;
1674         struct route_path_segment *seg=mr->seg;
1675         struct route *route=mr->mpriv->route;
1676         attr->type=attr_type;
1677         switch (attr_type) {
1678                 case attr_any:
1679                         while (mr->attr_next != attr_none) {
1680                                 if (rm_attr_get(priv_data, mr->attr_next, attr))
1681                                         return 1;
1682                         }
1683                         return 0;
1684                 case attr_street_item:
1685                         mr->attr_next=attr_direction;
1686                         if (seg && seg->item.map)
1687                                 attr->u.item=&seg->item;
1688                         else
1689                                 return 0;
1690                         return 1;
1691                 case attr_direction:
1692                         mr->attr_next=attr_length;
1693                         if (seg) 
1694                                 attr->u.num=seg->direction;
1695                         else
1696                                 return 0;
1697                         return 1;
1698                 case attr_length:
1699                         if (seg)
1700                                 attr->u.num=seg->length;
1701                         else
1702                                 attr->u.num=mr->length;
1703                         mr->attr_next=attr_time;
1704                         return 1;
1705                 case attr_time:
1706                         mr->attr_next=attr_none;
1707                         if (seg)
1708                                 attr->u.num=route_time(route->speedlist, &seg->item, seg->length);
1709                         else
1710                                 return 0;
1711                         return 1;
1712                 default:
1713                         attr->type=attr_none;
1714                         return 0;
1715         }
1716         return 0;
1717 }
1718
1719 static int
1720 rm_coord_get(void *priv_data, struct coord *c, int count)
1721 {
1722         struct map_rect_priv *mr = priv_data;
1723         struct route_path_segment *seg = mr->seg;
1724         int i,rc=0;
1725         if (! seg)
1726                 return 0;
1727         for (i=0; i < count; i++) {
1728                 if (mr->last_coord >= seg->ncoords)
1729                         break;
1730                 if (i >= seg->ncoords)
1731                         break;
1732                 c[i] = seg->c[mr->last_coord++];
1733                 rc++;
1734         }
1735         dbg(1,"return %d\n",rc);
1736         return rc;
1737 }
1738
1739 static struct item_methods methods_route_item = {
1740         rm_coord_rewind,
1741         rm_coord_get,
1742         rm_attr_rewind,
1743         rm_attr_get,
1744 };
1745
1746 static void
1747 rp_attr_rewind(void *priv_data)
1748 {
1749         struct map_rect_priv *mr = priv_data;
1750         mr->attr_next = attr_label;
1751 }
1752
1753 static int
1754 rp_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr)
1755 {
1756         struct map_rect_priv *mr = priv_data;
1757         struct route_graph_point *p = mr->point;
1758         if (mr->item.type != type_rg_point) 
1759                 return 0;
1760         switch (attr_type) {
1761         case attr_any:
1762                 while (mr->attr_next != attr_none) {
1763                         if (rm_attr_get(priv_data, mr->attr_next, attr))
1764                                 return 1;
1765                 }
1766         case attr_label:
1767                 attr->type = attr_label;
1768                 if (mr->str)
1769                         g_free(mr->str);
1770                 if (p->value != INT_MAX)
1771                         mr->str=g_strdup_printf("%d", p->value);
1772                 else
1773                         mr->str=g_strdup("-");
1774                 attr->u.str = mr->str;
1775                 mr->attr_next=attr_none;
1776                 return 1;
1777         case attr_debug:
1778                 attr->type = attr_debug;
1779                 if (mr->str)
1780                         g_free(mr->str);
1781                 mr->str=g_strdup_printf("x=%d y=%d", p->c.x, p->c.y);
1782                 attr->u.str = mr->str;
1783                 mr->attr_next=attr_none;
1784                 return 1;
1785         default:
1786                 return 0;
1787         }
1788 }
1789
1790 static int
1791 rp_coord_get(void *priv_data, struct coord *c, int count)
1792 {
1793         struct map_rect_priv *mr = priv_data;
1794         struct route_graph_point *p = mr->point;
1795         struct route_graph_segment *seg = mr->rseg;
1796         int rc = 0,i,dir;
1797         for (i=0; i < count; i++) {
1798                 if (mr->item.type == type_rg_point) {
1799                         if (mr->last_coord >= 1)
1800                                 break;
1801                         c[i] = p->c;
1802                 } else {
1803                         if (mr->last_coord >= 2)
1804                                 break;
1805                         dir=0;
1806                         if (seg->end->seg == seg)
1807                                 dir=1;
1808                         if (mr->last_coord)
1809                                 dir=1-dir;
1810                         if (dir)
1811                                 c[i] = seg->end->c;
1812                         else
1813                                 c[i] = seg->start->c;
1814                 }
1815                 mr->last_coord++;
1816                 rc++;
1817         }
1818         return rc;
1819 }
1820
1821 static struct item_methods methods_point_item = {
1822         rm_coord_rewind,
1823         rp_coord_get,
1824         rp_attr_rewind,
1825         rp_attr_get,
1826 };
1827
1828 static void
1829 rm_destroy(struct map_priv *priv)
1830 {
1831         g_free(priv);
1832 }
1833
1834 static struct map_rect_priv * 
1835 rm_rect_new(struct map_priv *priv, struct map_selection *sel)
1836 {
1837         struct map_rect_priv * mr;
1838         dbg(1,"enter\n");
1839         if (! route_get_pos(priv->route))
1840                 return NULL;
1841         if (! route_get_dst(priv->route))
1842                 return NULL;
1843         if (! priv->route->path2)
1844                 return NULL;
1845         mr=g_new0(struct map_rect_priv, 1);
1846         mr->mpriv = priv;
1847         mr->item.priv_data = mr;
1848         mr->item.type = type_street_route;
1849         mr->item.meth = &methods_route_item;
1850         mr->seg_next=priv->route->path2->path;
1851         return mr;
1852 }
1853
1854 static struct map_rect_priv * 
1855 rp_rect_new(struct map_priv *priv, struct map_selection *sel)
1856 {
1857         struct map_rect_priv * mr;
1858         dbg(1,"enter\n");
1859         if (! priv->route->graph || ! priv->route->graph->route_points)
1860                 return NULL;
1861         mr=g_new0(struct map_rect_priv, 1);
1862         mr->mpriv = priv;
1863         mr->item.priv_data = mr;
1864         mr->item.type = type_rg_point;
1865         mr->item.meth = &methods_point_item;
1866         return mr;
1867 }
1868
1869 static void
1870 rm_rect_destroy(struct map_rect_priv *mr)
1871 {
1872         if (mr->str)
1873                 g_free(mr->str);
1874         g_free(mr);
1875 }
1876
1877 static struct item *
1878 rp_get_item(struct map_rect_priv *mr)
1879 {
1880         struct route *r = mr->mpriv->route;
1881         struct route_graph_point *p = mr->point;
1882         struct route_graph_segment *seg = mr->rseg;
1883
1884         if (mr->item.type == type_rg_point) {
1885                 if (!p)
1886                         p = r->graph->route_points;
1887                 else
1888                         p = p->next;
1889                 if (p) {
1890                         mr->point = p;
1891                         mr->item.id_lo++;
1892                         rm_coord_rewind(mr);
1893                         rp_attr_rewind(mr);
1894                         return &mr->item;
1895                 } else
1896                         mr->item.type = type_rg_segment;
1897         }
1898         if (!seg)
1899                 seg=r->graph->route_segments;
1900         else
1901                 seg=seg->next;
1902         if (seg) {
1903                 mr->rseg = seg;
1904                 mr->item.id_lo++;
1905                 rm_coord_rewind(mr);
1906                 rp_attr_rewind(mr);
1907                 return &mr->item;
1908         }
1909         return NULL;
1910         
1911 }
1912
1913 static struct item *
1914 rp_get_item_byid(struct map_rect_priv *mr, int id_hi, int id_lo)
1915 {
1916         struct item *ret=NULL;
1917         while (id_lo-- > 0) 
1918                 ret=rp_get_item(mr);
1919         return ret;
1920 }
1921
1922
1923 static struct item *
1924 rm_get_item(struct map_rect_priv *mr)
1925 {
1926         struct route *r = mr->mpriv->route;
1927         struct route_path_segment *seg = mr->seg;
1928         dbg(1,"enter\n", mr->pos);
1929
1930         mr->seg=mr->seg_next;
1931         if (! mr->seg)
1932                 return NULL;
1933         mr->seg_next=mr->seg->next;
1934         mr->last_coord = 0;
1935         mr->item.id_lo++;
1936         rm_attr_rewind(mr);
1937         return &mr->item;
1938 }
1939
1940 static struct item *
1941 rm_get_item_byid(struct map_rect_priv *mr, int id_hi, int id_lo)
1942 {
1943         struct item *ret=NULL;
1944         while (id_lo-- > 0) 
1945                 ret=rm_get_item(mr);
1946         return ret;
1947 }
1948
1949 static struct map_methods route_meth = {
1950         projection_mg,
1951         NULL,
1952         rm_destroy,
1953         rm_rect_new,
1954         rm_rect_destroy,
1955         rm_get_item,
1956         rm_get_item_byid,
1957         NULL,
1958         NULL,
1959         NULL,
1960 };
1961
1962 static struct map_methods route_graph_meth = {
1963         projection_mg,
1964         NULL,
1965         rm_destroy,
1966         rp_rect_new,
1967         rm_rect_destroy,
1968         rp_get_item,
1969         rp_get_item_byid,
1970         NULL,
1971         NULL,
1972         NULL,
1973 };
1974
1975 void 
1976 route_toggle_routegraph_display(struct route *route)
1977 {
1978         if (route->flags & RF_SHOWGRAPH) {
1979                 route->flags &= ~RF_SHOWGRAPH;
1980         } else {
1981                 route->flags |= RF_SHOWGRAPH;
1982         }
1983 }
1984
1985 static struct map_priv *
1986 route_map_new_helper(struct map_methods *meth, struct attr **attrs, int graph)
1987 {
1988         struct map_priv *ret;
1989         struct attr *route_attr;
1990
1991         route_attr=attr_search(attrs, NULL, attr_route);
1992         if (! route_attr)
1993                 return NULL;
1994         ret=g_new0(struct map_priv, 1);
1995         if (graph)
1996                 *meth=route_graph_meth;
1997         else
1998                 *meth=route_meth;
1999         ret->route=route_attr->u.route;
2000
2001         return ret;
2002 }
2003
2004 static struct map_priv *
2005 route_map_new(struct map_methods *meth, struct attr **attrs)
2006 {
2007         return route_map_new_helper(meth, attrs, 0);
2008 }
2009
2010 static struct map_priv *
2011 route_graph_map_new(struct map_methods *meth, struct attr **attrs)
2012 {
2013         return route_map_new_helper(meth, attrs, 1);
2014 }
2015
2016 static struct map *
2017 route_get_map_helper(struct route *this_, struct map **map, char *type, char *description)
2018 {
2019         if (! *map) 
2020                 *map=map_new((struct attr*[]){
2021                                 &(struct attr){attr_type,{type}},
2022                                 &(struct attr){attr_route,.u.route=this_},
2023                                 &(struct attr){attr_data,{""}},
2024                                 &(struct attr){attr_description,{description}},
2025                                 NULL});
2026         return *map;
2027 }
2028
2029 struct map *
2030 route_get_map(struct route *this_)
2031 {
2032         return route_get_map_helper(this_, &this_->map, "route","Route");
2033 }
2034
2035
2036 struct map *
2037 route_get_graph_map(struct route *this_)
2038 {
2039         return route_get_map_helper(this_, &this_->graph_map, "route_graph","Route Graph");
2040 }
2041
2042 void
2043 route_set_projection(struct route *this_, enum projection pro)
2044 {
2045 }
2046
2047 void
2048 route_init(void)
2049 {
2050         plugin_register_map_type("route", route_map_new);
2051         plugin_register_map_type("route_graph", route_graph_map_new);
2052 }