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