/***************************************************************************
 *            routing.c
 *
 *  Copyright  2011 - Steffen Pohle
 *  steffen@gulpe.de
 ****************************************************************************/

#include <math.h>

#include "osmroute.h"
#include "gps.h"
#include "routing.h"
#include "system.h"
#include "memoryleak.h"
#include "map.h"
#include "vector.h"

struct rtdata *route = NULL;

void route_free (struct rtdata *rt);
struct rtdata *route_alloc (struct rtdata *rt, int max, int stmax);

void route_calc (struct rtdata *r);
void route_onway (struct rtdata *r);
int route_checkway (struct rtdata *r, struct rtway *rtw);
int route_calc_way (struct rtdata *r, struct rtway *rtw, struct map_way *mw, struct rtway *rtwnew, struct map_way *mwnew);
int route_optway (struct rtdata *r, struct rtway *rtw);
int route_optrefsconnectways (struct rtdata *r, struct rtrefway from, struct rtrefway to, struct rtref *ref);
void route_optrefs (struct rtdata *r, struct rtref *ref);
struct rtwaylist *route_getstack (struct rtdata *r, int waytype);

int rtl_find (struct rtwaylist *rtl, struct rtwayid *id);
int rtl_realloc (struct rtwaylist *rtl, int max);
int rtl_fifo_push (struct rtwaylist *rtl, struct rtway *rtw);
int rtl_lifo_push (struct rtwaylist *rtl, struct rtway *rtw);
int rtl_pop (struct rtwaylist *rtl, struct rtway *rtw);
int rtl_addsorted (struct rtwaylist *rtl, struct rtway *rtw);
void rtl_debug (struct rtwaylist *rtl);
void rtl_delete (struct rtwaylist *rtl, int nr);


void rtrlrefs_alloc (struct rtreflist *rtrl, int cnt);
void rtrl_addway (struct rtdata *r, struct rtreflist *rtrl, struct map_way *mw);
void rtrl_addwayname (struct rtdata *r, struct rtreflist *rtrl, struct map_way *mw, char *name);
void rtrefways_realloc (struct rtref *rtref, int cnt);
void rtrl_recalculate (struct rtdata *r, struct map_way *mw, struct rtwayid from);


/****************************************************************************
 * references and names ways
 */
void rtrlrefs_alloc (struct rtreflist *rtrl, int cnt) {
	int i;

	if (rtrl == NULL) return;
	
	if (cnt == 0) {
		/* free all data */
		for (i = 0; i < rtrl->cnt && rtrl->refs != NULL; i++) {
			if (rtrl->refs[i].ways) ml_free (rtrl->refs[i].ways);
			rtrl->refs[i].ways = NULL;
		}
		if (rtrl->refs)	ml_free (rtrl->refs);
		rtrl->refs = NULL;
		rtrl->max = 0;
		rtrl->cnt = 0;
	}
	else if (cnt <= rtrl->max) return;
	else {
		/* allocate more memory */
		rtrl->refs = (struct rtref *) ml_realloc (rtrl->refs, cnt * sizeof (struct rtref));
		for (i = rtrl->cnt; i < cnt; i++) {
			/* clear reset data */
			rtrl->refs[i].refname[0] = 0;
			rtrl->refs[i].cnt = 0;
			rtrl->refs[i].max = 0;
			rtrl->refs[i].ways = NULL;
			rtrl->refs[i].lastnr = 0;
		}
		rtrl->max = cnt;
	}
};


/* add the giving way to the stack list, before check for double entry and
 * set the priority to 0 */
void rtrl_recalculate (struct rtdata *r, struct map_way *mw, struct rtwayid from) {
	int nr, i;
	struct rtwaylist *stack;
	struct rtwayid wid;
	struct rtway rtw;
	
	if (mw == NULL) {
		d_printf ("%s:%d %s: mw == NULL abort", __FILE__, __LINE__, __FUNCTION__);
		return;
	}
	wid.id = mw->id;
	wid.sid = mw->subid;
	wid.type = mw->type;
	
	/* check if this way is already calculated.. */
	stack = &r->ways;
	if ((nr = rtl_find (stack, &wid)) >= 0) {
		d_printf ("%s:%d mw[%lld:%d] is already calculated.", __FILE__, __LINE__, wid.id, wid.sid);
		return;
	}
	
	/* find this way inside the stacks to set the priority to 0.0 */
	stack = route_getstack (r, wid.type);
	if ((nr = rtl_find (stack, &wid)) >= 0) {
		rtw = stack->array[nr];
		rtl_delete (stack, nr);
		rtw.priority = 0.0;
		rtw.cycle2 = r->cycle;
	}
	
	/* rtw has to be created from the begining */
	else {
		/* search from inside n */
		for (i = mw->n_cnt-1; (i >= 0 && (from.id != mw->n[i].d_id || from.sid != mw->n[i].d_subid)); i--);
		rtw.way = wid;
		rtw.pos = mw->p[0];
		if (i >= 0)	rtw.pnr = mw->n[i].pnr;
		else rtw.pnr = -1;
		rtw.dist2dest = -1.0;
		rtw.dist2pos = -1.0;
		rtw.step = -1;			/* indicate we know nothing.. just need this on top of the list..
		  						 * so it will be ignored as long as there is nothing known .. */
		rtw.priority = 0.0;		
		rtw.cycle1 = r->cycle;
		rtw.cycle2 = r->cycle;
		rtw.deltadist = 0.0;
	}
	rtl_addsorted (stack, &rtw);
};


/* add reference to the list of references, also recheck if we have two not
 * connected references, abort at 1.5times distance */
#define RTRL_MAXWAYS 512
void rtrl_addwayname (struct rtdata *r, struct rtreflist *rtrl, struct map_way *mw, char *name) {
	int ir;			// index refnames
	int nr;			// segment number
	int i, n;
	struct rtref *ref;	// pointer to reference data
//	static struct rtwayid _rtrl_wlist [RTRL_MAXWAYS];	// only create this once, saves time..
	int _rtrl_wlist_cnt = 0;	// re-init with zero elements

	d_printf ("   rtrl_addwayname '%s' id:%lld:%d  mw->ref:%s", name, mw->id, mw->subid, mw->ref);

	for (ir = rtrl->cnt; (ir >= 0 && strcmp (name, rtrl->refs[ir].refname) != 0); ir--);
	
	if (ir < 0) {
		/* nothing found add new reference */
		if (rtrl->cnt >= rtrl->max || rtrl->refs)
			rtrlrefs_alloc (rtrl, rtrl->max + RT_ALLOCREFS);
		ref = &rtrl->refs[rtrl->cnt];
		strncpy (ref->refname, name, MAP_W_NAMELEN);
		rtrl->cnt++;
	}
	else {
		ref = &rtrl->refs[ir];
	}

	/* add to ref the right way.. 
	 * first check if we have one big segment or if we enter the road again */
	for (nr = -1, i = 0; i < ref->cnt; i++) {
		/* check way, if found return to caller */
		if (mw->id == ref->ways[i].way.id
		    && mw->subid == ref->ways[i].way.sid) return;							
			
		/* check nodes */
		for (n = 0; n < mw->n_cnt; n++) {
			if (mw->n[n].d_id == ref->ways[i].way.id 
			    && mw->n[n].d_subid == ref->ways[i].way.sid)
				nr = ref->ways[i].nr;
		}
	}

	/* nothing found? */
	if (nr < 0) {
		float dist = -1.0;
		float f;
		
		/* find closest way to current position */
		d_printf ("         searching way to position. cnt:%d, ref:'%s'", ref->cnt, ref->refname);
		for (nr = -1, i = 0; i < ref->cnt; i++) {
			/* search shortest distance.. */
			f = map_way_getdistance (mw, &ref->ways[i].pos, NULL);
			if (dist < 0.0 || f < dist) {
				dist = f;
				nr = i;
			}
		}

		/* add ways to the stacks... 
		 * as long as recalc waydist < 1.5times dist
		 * and (mwleft || right) != &ref->ways[nr] */
		if (nr >= 0) {
			/* go to the left and to the right until 1.5time dist */
			struct map_hash *mhtmp = NULL;
			struct map_way *mwtmp = NULL;
			struct rtwayid fromdir[2];				/* 0.. left, 1.. right */
			struct map_way *mwdir[2] = {NULL, NULL};
//			int pnrdir[2] = {-1, -1};	/* pnr: where we entered the way */
			int dir;

			d_printf ("         found closest way nr:%d way:%lld:%d dist:%f", 
			          nr, ref->ways[nr].way.id, ref->ways[nr].way.sid, dist);

			/* 
			 * start checking: prepare the variables mh/mw left and right 
			 */
			fromdir[0].id = fromdir[1].id = ref->ways[nr].way.id;
			fromdir[0].sid = fromdir[1].sid = ref->ways[nr].way.sid;
			mhtmp = map_hash_get (ref->ways[nr].pos.lon, ref->ways[nr].pos.lat, MHLOAD_RELOAD);
			mwtmp = map_way_find (mhtmp, ref->ways[nr].way.id, ref->ways[nr].way.sid);
			if (mwtmp == NULL) {
				d_printf ("%s:%d %s: could not find way: %lld:%d",
				          __FILE__, __LINE__, __FUNCTION__, ref->ways[nr].way.id, 
				          ref->ways[nr].way.sid);
				return;
			}
			for (i = 0; i < mwtmp->n_cnt; i++) {
				struct map_hash *mhtmp1 = NULL;
				struct map_way *mwtmp1 = NULL;
				
				mhtmp1 = map_hash_get (mwtmp->n[i].d.lon, mwtmp->n[i].d.lat, MHLOAD_RELOAD);
				mwtmp1 = map_way_find (mhtmp1, mwtmp->n[i].d_id, mwtmp->n[i].d_subid);
				if (!mwtmp1) {
					d_printf ("%s:%d %s: could not find way: %lld:%d pos:%f,%f",
				          __FILE__, __LINE__, __FUNCTION__, mwtmp->n[i].d_id, mwtmp->n[i].d_subid);
					return;
				}
				d_printf ("check name[%s] == [%s]", name, mwtmp1->ref); 
				if (map_way_checkref (mwtmp1->ref, name)) {
					/* found way */
					if (mwdir[0] == NULL) {
						mwdir[0]= mwtmp1;
//						pnrdir[0] = mwtmp->n[i].d_pnr;
					}
					else if (mwdir[1] == NULL) {
						mwdir[1]= mwtmp1;
//						pnrdir[1] = mwtmp->n[i].d_pnr;
					}
					else {
						d_printf ("%s:%d %s: why there are three connecting referneces? ignoring way:%lld:%d",
						          __FILE__, __LINE__, __FUNCTION__,  mw->n[i].d_id, mw->n[i].d_subid);
					}
				}
			}
			while ((mwdir[0] != NULL || mwdir[1] != NULL) && _rtrl_wlist_cnt++ < RTRL_MAXWAYS) {
				struct map_hash *mhtmp1 = NULL;
				struct map_way *mwtmp1 = NULL;
				for (dir = 0; dir < 2; dir++) if (mwdir[dir]) {
					d_printf ("         mwdir[%d]: %lld:%d  fromdir:%lld:%d", dir, mwdir[dir]->id, mwdir[dir]->subid, fromdir[dir].id, fromdir[dir].sid);
					for (i = 0; i < mwdir[dir]->n_cnt; i++) {
					mhtmp1 = map_hash_get (mwdir[dir]->n[i].d.lon, mwdir[dir]->n[i].d.lat, MHLOAD_RELOAD);
					mwtmp1 = map_way_find (mhtmp1, mwdir[dir]->n[i].d_id, mwdir[dir]->n[i].d_subid);
					if (!mwtmp1) {
						d_printf ("%s:%d %s: could not find way: %lld:%d pos:%f,%f",
						          __FILE__, __LINE__, __FUNCTION__, mwdir[dir]->n[i].d_id, mwdir[dir]->n[i].d_subid);
						return;
					}
					if (map_way_checkref (mwtmp1->ref, name) 
					    && (mwtmp1->id != fromdir[dir].id || mwtmp1->subid != fromdir[dir].sid))
						break;
					}
					
					if (i < mwdir[dir]->n_cnt) {
						/* found next way.. */
						if (map_way_getdistance (mwtmp1, &ref->ways[nr].pos, NULL) * 1.5 < dist) {
							fromdir[dir].id = mwdir[dir]->id;
							fromdir[dir].sid = mwdir[dir]->subid;
//							pnrdir[dir] = mwdir[dir]->n[i].d_pnr;
							mwdir[dir] = mwtmp1;
						}
						else mwdir[dir] = NULL;
					}
					else mwdir[dir] = NULL;
				}
			}

			/* add to stack list */
			for (dir = 0; dir < 2; dir++) 
				if (mwdir[dir])  rtrl_recalculate (r, mwdir[dir], fromdir[dir]);
		}
		else 
			d_printf ("         found nothing..");

		/* no connection way found... need to add new segment.. */
		ref->lastnr++;
		nr = ref->lastnr;
	}

	/* save data */
	if (ref->cnt+1 >= ref->max) rtrefways_realloc (ref, ref->max+RT_ALLOCREFS);
	ref->ways[ref->cnt].way.id = mw->id;
	ref->ways[ref->cnt].way.sid = mw->subid;
	ref->ways[ref->cnt].way.type = mw->type;
	ref->ways[ref->cnt].pos = mw->p[0];
	ref->ways[ref->cnt].nr = nr;
	ref->cnt++;
	d_printf ("         cnt:%d  lastnr:%d  nr:%d", ref->cnt, ref->lastnr, nr);
};


void rtrl_addway (struct rtdata *r, struct rtreflist *rtrl, struct map_way *mw) {
	int i, j;
	char n[MAP_W_NAMELEN];

	/* add name to refrence list 
	 * adding streetnames is not a good idea, exist at different locations */
	// if (strlen (mw->name) > 0) rtrl_addwayname (rtrl, mw, mw->name);

	/* add references to reference list */
	for (n[0] = 0, j = 0, i = 0; i < strlen (mw->ref); i++) {
		if (j >= MAP_W_NAMELEN-1) {
			d_printf ("%s:%d (%s) something went very wrong", __FILE__, __LINE__, __FUNCTION__);
			break;
		}
		if (mw->ref[i] == ';') {
			rtrl_addwayname (r, rtrl, mw, n);
			j = 0;
			n[0] = 0;
		}
		else if (mw->ref[i] != ' ' ) { 
			n[j++] = mw->ref[i];
			n[j] = 0;
		}
	}
	if (strlen (n) > 0) rtrl_addwayname (r, rtrl, mw, n);
};


void rtrefways_realloc (struct rtref *rtref, int cnt) {
	int i;

	if (cnt == 0) {
		/* free data */
		if (rtref->ways != NULL) ml_free (rtref->ways);
		rtref->ways = NULL;
		rtref->cnt = 0;
		rtref->max = 0;
	}
	else if (cnt < rtref->max) return;
	else {
		rtref->ways = (struct rtrefway *) ml_realloc (rtref->ways, cnt * sizeof (struct rtrefway));
		for (i = rtref->max-1; i >= cnt; i--) {
			rtref->ways[i].way.id = 0;
			rtref->ways[i].way.sid = 0;
			rtref->ways[i].nr = -1;
			rtref->ways[i].pos.lon = 0.0;
			rtref->ways[i].pos.lat = 0.0;
		}
		rtref->max = cnt;
	}
};



/****************************************************************************
 * routing memory management
 */
void rtl_debug (struct rtwaylist *rtl) {
	int i;
	
	d_printf ("debug rtwaylist:");
	d_printf ("================");

	for (i = 0; i < rtl->cnt; i++) {
		d_printf ("  %-4d: %lld:%d", i, rtl->array[i].way.id, rtl->array[i].way.sid);
	}
};


/*
 * alloc or realloc data from memory
 */
struct rtdata *route_alloc (struct rtdata *rt, int max, int stmax) {
	struct rtdata *r = rt;
	d_printf ("route alloc: %p max:%d ig:%d sel:%d st:%d", rt, max, stmax);

	/* new route */
	if (r == NULL) {
		d_printf ("route alloc: new alloc");
		r = (struct rtdata *) ml_malloc (sizeof (struct rtdata));
		r->pos.lon = 0.0;
		r->pos.lat = 0.0;
		
		r->cycle = 0;
		r->ways.array = NULL;
		r->ways.cnt = 0;
		r->ways.max = 0;
		r->stack1.array = NULL;
		r->stack1.cnt = 0;
		r->stack1.max = 0;
		r->stack2.array = NULL;
		r->stack2.cnt = 0;
		r->stack2.max = 0;
		r->stack3.array = NULL;
		r->stack3.cnt = 0;
		r->stack3.max = 0;
		r->route.array = NULL;
		r->route.cnt = 0;
		r->route.max = 0;
		r->state = ROUTING_NONE;
		r->reflist.cnt = 0;
		r->reflist.refs = NULL;
		r->reflist.max = 0;
	}

	rtrlrefs_alloc (&r->reflist, RT_ALLOCREFS);
	rtl_realloc (&r->ways, max);
	rtl_realloc (&r->stack1, stmax);
	rtl_realloc (&r->stack2, stmax);
	rtl_realloc (&r->stack3, stmax);
	r->stack1.array[0].priority = 0.0;
	r->stack2.array[0].priority = 0.0;
	r->stack3.array[0].priority = 0.0;
	r->ways.array[0].priority = 0.0;
	
	return r;
};



/****************************************************************************
 * routing data functions
 */

/*
 * find the given element inside the givven array */
int rtl_find (struct rtwaylist *rtl, struct rtwayid *id) {
	int i;

	for (i = rtl->cnt-1; i >= 0; i--) {
		if (rtl->array[i].way.id == id->id && rtl->array[i].way.sid == id->sid)
			break;
	}
	
	return i;
};


/*
 * alloc or free memory 
 */
int rtl_realloc (struct rtwaylist *rtl, int max) {
	void *ptr;
	d_printf ("realloc: %p, size:%d neu:%d", rtl->array, rtl->max, max);
	ptr = ml_realloc (rtl->array, sizeof (struct rtway) * max);
	if (ptr == NULL && max > 0) {
		d_printf ("%s:%d (%s) ptr == %p", __FILE__, __LINE__, __FUNCTION__, ptr);
		errorexit (-1);
	}
	rtl->array = ptr;
	rtl->max = max;
	if (rtl->max < rtl->cnt) rtl->cnt = max;
	return 1;
};


/*
 * first in and first out push.. element will be put on the first place
 */
int rtl_fifo_push (struct rtwaylist *stack, struct rtway *rtw) {
	if ((stack->cnt+1) >= stack->max) rtl_realloc (stack, stack->max+RT_ALLOCSTACK);
	memmove (&stack->array[1], &stack->array[0], sizeof (struct rtway) * stack->cnt);

	stack->array[0] = *rtw;
	stack->cnt++;	

	return 1;
};


/*
 * first in and first out push.. element will be put on the first place
 */
int rtl_lifo_push (struct rtwaylist *stack, struct rtway *rtw) {
	if ((stack->cnt+1) >= stack->max) rtl_realloc (stack, stack->max+RT_ALLOCSTACK);

	stack->array[stack->cnt] = *rtw;
	stack->cnt++;	

	return 1;
};


/*
 * last element will be removed from list.
 */
int rtl_pop (struct rtwaylist *stack, struct rtway *rtw) {
	if (stack->cnt > 0) {
		stack->cnt--;
		*rtw = stack->array[stack->cnt];
		return 1;
	}
	d_printf ("ERROR: rtl_pop() cnt==0");
	return 0;
};


/*
 * add element to the list. In sorted order.
 */
int rtl_addsorted (struct rtwaylist *rtl, struct rtway *rtw) {
	int i = 0;

	if (!rtl) return 0;

	// d_printf ("     rtl_addsorted: %lld:%d", rtw->way.id, rtw->way.sid);
	if ((rtl->cnt+1) >= rtl->max) rtl_realloc (rtl, rtl->max+RT_ALLOCSTACK);

	i = 0;
	for (i = 0; i < rtl->cnt; i++) {
		if (rtl->array[i].priority < rtw->priority) break;
	}

	if (i < rtl->cnt)
		memmove (&rtl->array[i+1], &rtl->array[i], sizeof (struct rtway) * (rtl->cnt-i));
	rtl->array[i] = *rtw;
	rtl->cnt++;
	
	return 1;
};


/*
 * delete entry from the list
 */
void rtl_delete (struct rtwaylist *rtl, int nr) {
	if (nr < rtl->cnt-1 && nr >= 0 && rtl->cnt > 0) {
		rtl->cnt--;
		if (nr < rtl->cnt) memmove (&rtl->array[nr], &rtl->array[nr+1], sizeof (struct rtway) * (rtl->cnt-nr));
	}
};


/****************************************************************************
 * routing function itself
 */
/*
 * free route data from memory
 */
void route_free (struct rtdata *rt) {
	if (rt == NULL) return;

	d_printf ("route free: %p", rt);
	rtrlrefs_alloc (&rt->reflist, 0);
	rtl_realloc (&rt->ways, 0);
	rtl_realloc (&rt->stack1, 0);
	rtl_realloc (&rt->stack2, 0);
	rtl_realloc (&rt->stack3, 0);
	rtl_realloc (&rt->route, 0);
	
	ml_free (rt);
};


int route_calc_way (struct rtdata *r, struct rtway *rtw, struct map_way *mw, struct rtway *rtwnew, struct map_way *mwnew) {
	float deltadist = 0.0;
	fPoint v1, v2, v3, v4;
	float f, ideal = 0.0;
	int i, step, deltastep;

	r->cycle++;
	rtwnew->cycle1 = r->cycle;
	rtwnew->cycle2 = -1;

	/*
	 * check the distance to the ideal line, also look for the closest distacne
	 */
	v1.x = r->pos_dest.lon;
	v1.y = r->pos_dest.lat;
	for (i = 0; i < mwnew->n_cnt; i++) {
		v2.x = map_lon2km (r->dest.lon - mwnew->p[mwnew->n[i].pnr].lon, mwnew->p[mwnew->n[i].pnr].lat);
		v2.y = map_lat2km (r->dest.lat - mwnew->p[mwnew->n[i].pnr].lat);
		v3 = vec_normale (v1, v2);

		/* check if the normale vector is on the v1 vector, 
		 * if not we just get the distance */
		if (((v3.x <= 0.0 && v1.x <= 0.0) || (v3.x >= 0.0 && v1.x >= 0.0) 
		    || (v3.y <= 0.0 && v1.y <= 0.0) || (v3.y >= 0.0 && v1.y >= 0.0))
		    && fabs(v3.x) <= fabs(v1.x) && fabs(v3.y) <= fabs(v1.y)) {

			v4 = vec_sub (v3, v2);
			f = sqrt (v4.x * v4.x + v4.y * v4.y);
		}
		else f = sqrt (v2.x * v2.x + v2.y * v2.y);

		/* check ideal */
		if (f < ideal || i == 0) ideal = f;
		ideal = fabsf (ideal);

		f = map_getdistance (mwnew->p[mwnew->n[i].pnr], r->pos);
		/* check distance */
		if (f < r->closest_distance || r->closest_distance < 0.0) {		// for debugging and status information
			r->closest_distance = f;
			r->closest_lon = mwnew->p[0].lon;
			r->closest_lat = mwnew->p[0].lat;
		}
		if (f < rtwnew->dist2pos || i == 0) {
			rtwnew->dist2pos = f;
		}
	}
	
	/*
	 * calculate the delta between distance on this way since start (deltaway)
	 * calculate delta step and calculate delta distance to dest. (deltadist)
	 */
	deltadist = rtwnew->dist2pos - rtw->dist2pos;
	rtwnew->deltadist = deltadist;
	deltastep = rtwnew->step - rtw->step;
	if (deltastep) {
		step = rtw->step;
	}
	else {
		step = rtw->step-1;
	}
	
	/*
	 * calclating priority
	 */
	// until now the best way to calculate..
	// rtwnew->priority = (powf (1.0 + 4.0 * ideal, 2.0) + powf (1.0 + rtwnew->dist2pos, 2.0) + rtwnew->dist2dest) * (1.0 + step/5.0);
	rtwnew->priority = (powf (1.0 + 4.0 * ideal, 2.0) + powf (1.0 + rtwnew->dist2pos, 2.0) + rtwnew->dist2dest) * (1.0 + step/5.0);
	if (isinf(rtwnew->priority) || isnan(rtwnew->priority)) {
			rtwnew->priority = fabsf (rtw->priority)*0.9;
	}

	/*
	 * only a way no connection nodes.. check this route right away
	 */
	if ((mwnew->n_cnt <= 2 && mwnew->p_cnt < 2) || deltadist == 0.0 || (mwnew->flags & MWAY_F_junction)) {
		rtwnew->priority = 0.0;
	}
	return 1;
};


int route_checkway (struct rtdata *r, struct rtway *rtw) {
	struct map_hash *mh = map_hash_get (rtw->pos.lon, rtw->pos.lat, MHLOAD_RELOAD);
	struct map_way *mw = map_way_find (mh, rtw->way.id, rtw->way.sid);
	struct map_hash *mhto;
	struct map_way *mwto;
	struct rtway rtwnew, *rtwtmp;
	struct rtwaylist *rtltmp;
	int i, j, nr, w;

	// d_printf ("route_checkway: rtw:%lld:%d step:%d dist2pos:%g priority:%g", rtw->way.id, rtw->way.sid, rtw->step, rtw->dist2pos, rtw->priority);
	r->cycle++;
	rtw->cycle2 = r->cycle;
	if (mw == NULL) {
		d_printf ("%s: could not find the way with id: %lld:%d at %f,%f", __FUNCTION__, rtw->way.id, rtw->way.sid, rtw->pos.lon, rtw->pos.lat);
		return 0;
	}

	/* ignore the following ways */
	if (mw->type == MWAY_railway) return 0;
	if (mw->type == MWAY_unknown_railway) return 0;
	if (mw->type == MWAY_cycleway) return 0;
	if (mw->type == MWAY_footway) return 0;
	if (mw->type == MWAY_service) return 0;

	// d_printf ("  way %lld:%d pnr:%d name:%s ref:%s flags:%d", mw->id, mw->subid, rtw->pnr, mw->name, mw->ref, mw->flags);
	
	/* check all dnodes and add them to the list if they aren't already inside */
	if ((mw->flags & (MWAY_F_oneway + MWAY_F_roundabout)) == MWAY_F_oneway) {
		nr = rtw->pnr;
	}
	else {
		nr = 0;
	}

	/* add to reference list */
	rtrl_addway (r, &r->reflist, mw);

	for (i = 0; i < mw->n_cnt; i++) if (mw->id != mw->n[i].d_id || mw->subid != mw->n[i].d_subid) {
		rtwnew.way.id = mw->n[i].d_id;
		rtwnew.way.sid = mw->n[i].d_subid;
		rtwnew.pos = mw->n[i].d;		
		rtwnew.pnr = mw->n[i].d_pnr;

		mhto = map_hash_get (rtwnew.pos.lon, rtwnew.pos.lat, MHLOAD_RELOAD);
		mwto = map_way_find (mhto, rtwnew.way.id, rtwnew.way.sid);

		if (mwto == NULL) {
			continue;
		}

		if (((mw->flags & (MWAY_F_oneway+MWAY_F_roundabout)) == MWAY_F_oneway) && 
		     ((mw->id != mwto->id && mw->n[i].pnr > nr) || (mw->id == mwto->id && mw->subid < mwto->subid))) { 
			continue;	// prevent us from going backwards on oneways
		}

		if (mwto->id == mw->id && mwto->subid == mw->subid) {
			continue;
		}

		/* increase step */
		j =  map_way_checkref (mwto->ref, mw->ref);
		if (rtwnew.way.id == rtw->way.id || j > 0)
			rtwnew.step = rtw->step;
		else
			rtwnew.step = rtw->step + 1;

		/* calc distance */
		rtwnew.dist2dest = rtw->dist2dest;
		rtwnew.dist2dest += map_way_getlenght (mw, rtw->pnr, mw->n[i].pnr);
		route_calc_way (r, rtw, mw, &rtwnew, mwto);
		rtwtmp = NULL;
		rtltmp = NULL;
		if ((w = rtl_find(&r->ways, &rtwnew.way)) >= 0) { 
			rtwtmp = &r->ways.array[w];
			rtltmp = &r->ways;
		}

		else {
			rtltmp = route_getstack(r, mwto->type);
			if ((w = rtl_find(rtltmp, &rtwnew.way)) >= 0)
				rtwtmp = &rtltmp->array[w];
		}

		/* make sure we are not rechecking ways with only one p_cnt 
		 * (will happen if the last point is inside a different hash) */
		if (rtwtmp && rtltmp == &r->ways && mw->p_cnt == 1) continue;

		if (rtwtmp) {
				/* we found the element */
				// d_printf ("   add_sorted overwrite: %lld:%d", rtwtmp->way.id, rtwtmp->way.sid);
				// d_printf ("   dist2dest:rtwtmp:%f > rtwnew:%f (%d) prio:rtwtmp:%f > rtwnew:%f (%d)", 
				//          rtwtmp->dist2dest, rtwnew.dist2dest, rtwtmp->priority, rtwnew.priority, 
				//          rtwtmp->dist2dest > rtwnew.dist2dest , rtwtmp->priority > rtwnew.priority);
				// d_printf ("   step:rtwtmp:%d , rtwnew:%d rtw->pnr:%d mw->n_cnt:%d p_cnt:%d", rtwtmp->step,
				//          rtwnew.step, rtw->pnr, mw->n_cnt, mw->p_cnt);
				if (rtwtmp->step < rtwnew.step) rtwnew.step = rtwtmp->step;
				if (rtwtmp->dist2dest > rtwnew.dist2dest || rtwtmp->priority > rtwnew.priority) {
					rtl_delete (rtltmp, w);				// delete old data
					rtl_addsorted (route_getstack(r, mwto->type), &rtwnew);
				}
		}
		else 
			rtl_addsorted (route_getstack(r, mwto->type), &rtwnew);
	}

	if (mw->id == r->pos_way_id && mw->subid == r->pos_way_sid) {
		d_printf ("** routing: way found go to the next step");
		rtl_lifo_push (&r->route, rtw);
		rtl_debug (&r->route);
		r->state = ROUTING_OPTIMIZE;
	}

	return 1;
};


/*
 * read data from stack
 */
#define FACTOR_HI 2.0
#define FACTOR_IA 3.0
#define FACTOR_HA FACTOR_HI*FACTOR_IA
int route_popstack (struct rtdata *r, struct rtway *rtw) {
	int selstack = 0;
	int i;
	int res = 0;

	/* check if we have some need to calculate routes.. i.e. just ways without connections */
	if (r->stack1.cnt > 0 && r->stack1.array[route->stack1.cnt-1].priority == 0.0)
		res = rtl_pop (&r->stack1, rtw);
	else if (r->stack2.cnt > 0 && r->stack2.array[route->stack2.cnt-1].priority == 0.0) 
		res = rtl_pop (&r->stack2, rtw);
	else if (r->stack3.cnt > 0 && r->stack3.array[route->stack3.cnt-1].priority == 0.0) 
		res = rtl_pop (&r->stack3, rtw);

	/* check if we are on the way inside the stack */
	if (res <= 0)
		for (i = 0; i < r->stack1.cnt; i++) if (r->stack1.array[i].way.id == r->pos_way_id && r->stack1.array[i].way.sid == r->pos_way_sid) {
			*rtw = r->stack1.array[i];
			res = 1;
			rtl_delete (&r->stack1, i);
			break;
		}
	if (res <= 0)
		for (i = 0; i < r->stack2.cnt; i++) if (r->stack2.array[i].way.id == r->pos_way_id && r->stack2.array[i].way.sid == r->pos_way_sid) {
			*rtw = r->stack2.array[i];
			res = 1;
			rtl_delete (&r->stack2, i);
			break;
		}
	if (res <= 0)
		for (i = 0; i < r->stack3.cnt; i++) if (r->stack3.array[i].way.id == r->pos_way_id && r->stack3.array[i].way.sid == r->pos_way_sid) {
			*rtw = r->stack3.array[i];
			res = 1;
			rtl_delete (&r->stack3, i);
			break;
		}

	if (res <= 0) {
		if (r->stack1.cnt > 0) {
			selstack = 1;
			if (r->stack2.cnt > 0 && r->stack1.array[route->stack1.cnt-1].priority/FACTOR_HI > r->stack2.array[route->stack2.cnt-1].priority) selstack = 2;
			if (r->stack3.cnt > 0 && r->stack2.cnt == 0 && r->stack1.array[route->stack1.cnt-1].priority/FACTOR_HA > r->stack3.array[route->stack3.cnt-1].priority) selstack = 3;
			if (r->stack3.cnt > 0 && r->stack2.cnt > 0 && r->stack2.array[route->stack2.cnt-1].priority/FACTOR_IA > r->stack3.array[route->stack3.cnt-1].priority) selstack = 3;
		}

		if (selstack == 0 && r->stack2.cnt > 0) {
			selstack = 2;
			if (r->stack3.cnt > 0 && r->stack2.array[route->stack2.cnt-1].priority/2.0 > r->stack3.array[route->stack3.cnt-1].priority) selstack = 3;
		}

		if (selstack == 0 && r->stack3.cnt > 0) {
			selstack = 3;
		}

		if (selstack == 1)
			res = rtl_pop (&r->stack1, rtw);
		else if (selstack == 2) 
			res = rtl_pop (&r->stack2, rtw);
		else if (selstack == 3) 
			res = rtl_pop (&r->stack3, rtw);
	}
//	if (res >= 0) {
//		d_printf ("route_popstack: %lld:%d", rtw->way.id, rtw->way.sid);
//	}
	return res;
};


/*
 * calculating the route.. 
 * 1. reading unchecked ways from stack1 (motor/highway), stack2 (tertiary)
 *    or stack3 (residential/unknown ways).
 * 2. check these ways and add connected unknown ways to the stack
 */
void route_calc (struct rtdata *r) {
	struct rtway rtw;
	int res;

	if ((res = route_popstack (r, &rtw)) == 1) {
		d_printf ("route_calc: rtw: %lld:%d", rtw.way.id, rtw.way.sid);
		if (route_checkway (r, &rtw)) 
		    rtl_fifo_push (&r->ways, &rtw);
	}
	else if (r->state == ROUTING_INIT) {
		d_printf ("route_calc: processing from destination way");
		rtw.way.id = r->dest_way_id;
		rtw.way.sid = r->dest_way_sid;
		rtw.way.type = MWAY_unknown;
		r->start_way_id = drawgps_on_way_id;
		r->start_way_sid = drawgps_on_way_sid;
		rtw.pos = r->dest;
		rtw.step = 0;
		rtw.pnr = 0;
		d_printf ("route_calc %s:%d FIX ME: nr must be set to the closest node.. ", __FILE__, __LINE__);
		if (drawgps_on_way_id != 0) {
			rtl_lifo_push (&r->stack3, &rtw);
			r->state = ROUTING_CALC;
		}
	}
	else {
		d_printf ("something went very wrong. cannot find ways which are connected to this area. (state:%d res:%d)", r->state, res);
		r->state = ROUTING_ABORT;
	}
};


/* return pointer to current way stack, depending on the type of way */
struct rtwaylist *route_getstack (struct rtdata *r, int waytype) {
	if (!r) return NULL;
	if (waytype == MWAY_highway) return &r->stack1;
	if (waytype == MWAY_tertiary) return &r->stack2;
	else return &r->stack3;
};


void route_onway (struct rtdata *r) {
	static int _once = 1;
	if (_once) d_printf ("route_onway: we are on the way... ");
	_once = 0;
};


/* optimize the route and create the route from current position */
void route_optimize (struct rtdata *r) {
	if (r->state == ROUTING_OPTIMIZE) r->state = ROUTING_ONWAY;
};


int route_optway (struct rtdata *r, struct rtway *rtw) {
	return 0;
};


/***************************************************************************
 * start stop and loop functions for routing
 */
void route_start (struct map_pos dest) {
	static int _called = 0;
	struct map_pos pos;
	struct gps_data *gpsdata;
	int i, calcto;

	if (_called) return;
	_called = 1;


	if (route != NULL) {
		route_free (route);
		route = NULL;
	}
	route = route_alloc (NULL, RT_ALLOC, RT_ALLOCSTACK);

	d_printf ("*********************************************");
	d_printf ("route_start to: %f,%f", dest.lon, dest.lat);
	route->state = ROUTING_NONE;
	route->dest = dest;

	/* find closest way */
	i = map_way_findclosest (&route->dest, &route->dest_way_pos, 0, route->dest_way_name, MAP_W_NAMELEN, &route->dest_way_id, &route->dest_way_sid);
	d_printf ("  wayid: %lld:%d", route->dest_way_id, route->dest_way_sid);
	d_printf ("   name: %s", route->dest_way_name);
	route->closest_distance = -1.0;
	route->closest_lon = dest.lon;
	route->closest_lat = dest.lat;
	route->state = ROUTING_INIT;

	do {
		if (gps_isrunning ()) {
			gpsdata = gps_getposition ();
			if (gpsdata != NULL && gpsdata->valid) {
				pos.lon = gpsdata->lon;
				pos.lat = gpsdata->lat;
				route->pos_dest.lon = map_lon2km (route->dest.lon - pos.lon, pos.lat);
				route->pos_dest.lat = map_lat2km (route->dest.lat - pos.lat);
				route->pos_way_id = drawgps_on_way_id;
				route->pos_way_sid = drawgps_on_way_sid;
				
				route->pos = pos;
				route->distance = sqrtf (route->pos_dest.lon * route->pos_dest.lon + route->pos_dest.lat * route->pos_dest.lat);
				route->angle = (180.0 / M_PI) * atan2f(route->pos_dest.lon, route->pos_dest.lat);
				if (route->angle < 0.0) route->angle += 360.0;
				if (route->angle > 360.0) route->angle -= 360.0;
				if (route->state == ROUTING_INIT) {
					route->start = pos;
					route_calc (route);
					route->state = ROUTING_CALC;
				}
				else if (route->state == ROUTING_INIT)
					for (i = 0; i < route->ways.cnt; i++) if (route->ways.array[i].way.id == route->pos_way_id && route->ways.array[i].way.sid == route->pos_way_sid) {				
						d_printf ("** routing: pos found inside ways go to the next step");
						route->state = ROUTING_OPTIMIZE;
					}
			}

			if (route->state == ROUTING_CALC) {
				route_calc (route);
			}
			
			else if (route->state == ROUTING_OPTIMIZE) {
				route_optimize (route);
			}
			
			else if (route->state == ROUTING_ONWAY) {
				route_onway (route);
			}
		}

		if (app.status == APPSTATUS_quit) route_stop ();
		
		if (route->state == ROUTING_CALC) {
			if ((++calcto) % 200 == 0) main_wnd_update ();
		}
		else
			 main_wnd_update ();
	} while (route && route->state != ROUTING_NONE && route->state != ROUTING_ABORT);

	d_printf ("*********************************************");
	d_printf ("route_start: finished or aborted");
	_called = 0;

	return;
};


void route_stop () {
	d_printf ("*********************************************");
	d_printf ("route_stop");
	if (route) route->state = ROUTING_ABORT;
};