navit/navit/track.c

/**
 * ZANavi, Zoff Android Navigation system.
 * Copyright (C) 2011-2012 Zoff <zoff@zoff.cc>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * version 2 as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the
 * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
 * Boston, MA  02110-1301, USA.
 */

/**
 * Navit, a modular navigation system.
 * Copyright (C) 2005-2008 Navit Team
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * version 2 as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the
 * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
 * Boston, MA  02110-1301, USA.
 */

#include <glib.h>
#include <string.h>
#include <time.h>
#include <math.h>
#include "item.h"
#include "attr.h"
#include "track.h"
#include "debug.h"
#include "transform.h"
#include "coord.h"
#include "route.h"
#include "projection.h"
#include "map.h"
#include "mapset.h"
#include "plugin.h"
#include "vehicleprofile.h"
#include "vehicle.h"
#include "roadprofile.h"
#include "util.h"
#include "config.h"

struct tracking_line
{
        struct street_data *street;
        struct tracking_line *next;
        int angle[0];
};

/**
 * @brief Conatins a list of previous speeds
 *
 * This structure is used to hold a list of previously reported
 * speeds. This data is used by the CDF.
 */
struct cdf_speed
{
        struct cdf_speed *next;
        int speed;
        time_t time;
};

/**
 * @brief Contains data for the CDF
 *
 * This structure holds all data needed by the
 * cumulative displacement filter.
 */
struct cdf_data
{
        int extrapolating;
        int available;
        int first_pos;
        int poscount;
        int hist_size;
        struct cdf_speed *speed_hist;
        struct pcoord *pos_hist;
        int *dir_hist;
        double last_dist;
        struct pcoord last_out;
        int last_dir;
};

struct tracking
{
        struct mapset *ms;
        struct route *rt;
        struct map *map;
        struct vehicle *vehicle;
        struct vehicleprofile *vehicleprofile;
        struct coord last_updated;
        struct tracking_line *lines;
        struct tracking_line *curr_line;
        int pos;
        struct coord curr[2], curr_in, curr_out;
        int curr_angle;
        struct coord last[2], last_in, last_out;
        struct cdf_data cdf;
        struct attr *attr;
        int valid;
        int time;
        double direction;
        double speed;
        int coord_geo_valid;
        struct coord_geo coord_geo;
        enum projection pro;
        int street_direction;
        int no_gps;
        int tunnel;
        int angle_pref;
        int connected_pref;
        int nostop_pref;
        int offroad_limit_pref;
        int route_pref;
        int overspeed_pref;
        int overspeed_percent_pref;
        int tunnel_extrapolation;
};

static void tracking_init_cdf(struct cdf_data *cdf, int hist_size)
{
        cdf->extrapolating = 0;
        cdf->available = 0;
        cdf->poscount = 0;
        cdf->last_dist = 0;
        cdf->hist_size = hist_size;

cdf->pos_hist = g_new0(struct pcoord, hist_size);
cdf->dir_hist = g_new0(int, hist_size);
}

// Variables for finetuning the CDF

// Minimum average speed
#define CDF_MINAVG 1.f
// Maximum average speed
#define CDF_MAXAVG 6.f // only ~ 20 km/h 
// We need a low value here because otherwise we would extrapolate whenever we are not accelerating

// Mininum distance (square of it..), below which we ignore gps updates
#define CDF_MINDIST 49 // 7 meters, I guess this value has to be changed for pedestrians.
#if 0
static void
tracking_process_cdf(struct cdf_data *cdf, struct pcoord *pin, struct pcoord *pout, int dirin, int *dirout, int cur_speed, time_t fixtime)
{
struct cdf_speed *speed,*sc,*sl;
double speed_avg;
int speed_num,i;

if (cdf->hist_size == 0)
{
        dbg(1,"No CDF.\n");
        *pout = *pin;
        *dirout = dirin;
        return;
}

speed = g_new0(struct cdf_speed, 1);
speed->speed = cur_speed;
speed->time = fixtime;

speed->next = cdf->speed_hist;
cdf->speed_hist = speed;

sc = speed;
sl = NULL;
speed_num = 0;
speed_avg = 0;
while (sc && ((fixtime - speed->time) < 4))
{ // FIXME static maxtime
        speed_num++;
        speed_avg += sc->speed;
        sl = sc;
        sc = sc->next;
}

speed_avg /= (double)speed_num;

if (sl)
{
        sl->next = NULL;
}

while (sc)
{
        sl = sc->next;
        g_free(sc);
        sc = sl;
}

if (speed_avg < CDF_MINAVG)
{
        speed_avg = CDF_MINAVG;
}
else if (speed_avg > CDF_MAXAVG)
{
        speed_avg = CDF_MAXAVG;
}

if (cur_speed >= speed_avg)
{
        if (cdf->extrapolating)
        {
                cdf->poscount = 0;
                cdf->extrapolating = 0;
        }

        cdf->first_pos--;
        if (cdf->first_pos < 0)
        {
                cdf->first_pos = cdf->hist_size - 1;
        }

        if (cdf->poscount < cdf->hist_size)
        {
                cdf->poscount++;
        }

        cdf->pos_hist[cdf->first_pos] = *pin;
        cdf->dir_hist[cdf->first_pos] = dirin;

        *pout = *pin;
        *dirout = dirin;
}
else if (cdf->poscount > 0)
{

        double mx,my; // Average position's x and y values
        double sx,sy; // Support vector
        double dx,dy; // Difference between average and current position
        double len; // Length of support vector
        double dist;

        mx = my = 0;
        sx = sy = 0;

        for (i = 0; i < cdf->poscount; i++)
        {
                mx += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x / cdf->poscount;
                my += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y / cdf->poscount;

                if (i != 0)
                {
                        sx += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].x;
                        sy += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].y;
                }

        }

        if (cdf->poscount > 1)
        {
                // Normalize the support vector
                len = sqrt(sx * sx + sy * sy);
                sx /= len;
                sy /= len;

                // Calculate the new direction
                *dirout = (int)rint(atan(sx / sy) / M_PI * 180 + 180);
        }
        else
        {
                // If we only have one position, we can't use differences of positions, but we have to use the reported
                // direction of that position
                sx = sin((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
                sy = cos((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
                *dirout = cdf->dir_hist[cdf->first_pos];
        }

        dx = pin->x - mx;
        dy = pin->y - my;
        dist = dx * sx + dy * sy;

        if (cdf->extrapolating && (dist < cdf->last_dist))
        {
                dist = cdf->last_dist;
        }

        cdf->last_dist = dist;
        cdf->extrapolating = 1;

        pout->x = (int)rint(mx + sx * dist);
        pout->y = (int)rint(my + sy * dist);
        pout->pro = pin->pro;

}
else
{
        // We should extrapolate, but don't have an old position available
        *pout = *pin;
        *dirout = dirin;
}

if (cdf->available)
{
        int dx,dy;

        dx = pout->x - cdf->last_out.x;
        dy = pout->y - cdf->last_out.y;

        if ((dx*dx + dy*dy) < CDF_MINDIST)
        {
                *pout = cdf->last_out;
                *dirout = cdf->last_dir;
        }
}

cdf->last_out = *pout;
cdf->last_dir = *dirout;

cdf->available = 1;
}
#endif

int tracking_get_angle(struct tracking *tr)
{
        return tr->curr_angle;
}

struct coord *
tracking_get_pos(struct tracking *tr)
{
        return &tr->curr_out;
}

int tracking_get_street_direction(struct tracking *tr)
{
        return tr->street_direction;
}

int tracking_get_segment_pos(struct tracking *tr)
{
        return tr->pos;
}

struct street_data *
tracking_get_street_data(struct tracking *tr)
{
        if (tr->curr_line)
                return tr->curr_line->street;
        return NULL;
}

int tracking_get_attr(struct tracking *_this, enum attr_type type, struct attr *attr, struct attr_iter *attr_iter)
{
        struct item *item;
        struct map_rect *mr;
        int result = 0;
        // dbg(1,"enter %s\n",attr_to_name(type));

        if (_this->attr)
        {
                attr_free(_this->attr);
                _this->attr = NULL;
        }

        switch (type)
        {
                case attr_position_valid:
                        attr->u.num = _this->valid;
                        return 1;
                case attr_position_direction:
                        attr->u.numd = &_this->direction;
                        return 1;
                case attr_position_speed:
                        attr->u.numd = &_this->speed;
                        return 1;
                case attr_directed:
                        attr->u.num = _this->street_direction;
                        return 1;
                case attr_position_coord_geo:
                        if (!_this->coord_geo_valid)
                        {
                                struct coord c;
                                c.x = _this->curr_out.x;
                                c.y = _this->curr_out.y;
                                transform_to_geo(_this->pro, &c, &_this->coord_geo);
                                _this->coord_geo_valid = 1;
                        }
                        attr->u.coord_geo = &_this->coord_geo;
                        return 1;
                case attr_current_item:
                        if (!_this->curr_line || !_this->curr_line->street)
                                return 0;
                        attr->u.item = &_this->curr_line->street->item;
                        return 1;
                default:
                        if (!_this->curr_line || !_this->curr_line->street)
                                return 0;
                        item = &_this->curr_line->street->item;
                        mr = map_rect_new(item->map, NULL);
                        item = map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
                        if (item_attr_get(item, type, attr))
                        {
                                _this->attr = attr_dup(attr);
                                *attr = *_this->attr;
                                result = 1;
                        }
                        map_rect_destroy(mr);
                        return result;
        }
}

struct item *
tracking_get_current_item(struct tracking *_this)
{
        if (!_this->curr_line || !_this->curr_line->street)
                return NULL;
        return &_this->curr_line->street->item;
}

int *
tracking_get_current_flags(struct tracking *_this)
{
        if (!_this->curr_line || !_this->curr_line->street)
                return NULL;
        return &_this->curr_line->street->flags;
}

static void tracking_get_angles(struct tracking_line *tl)
{
        int i;
        struct street_data *sd = tl->street;
        for (i = 0; i < sd->count - 1; i++)
                tl->angle[i] = transform_get_angle_delta(&sd->c[i], &sd->c[i + 1], 0);
}

static int street_data_within_selection(struct street_data *sd, struct map_selection *sel)
{
        struct coord_rect r;
        struct map_selection *curr;
        int i;

        if (!sel)
                return 1;
        r.lu = sd->c[0];
        r.rl = sd->c[0];
        for (i = 1; i < sd->count; i++)
        {
                if (r.lu.x > sd->c[i].x)
                        r.lu.x = sd->c[i].x;
                if (r.rl.x < sd->c[i].x)
                        r.rl.x = sd->c[i].x;
                if (r.rl.y > sd->c[i].y)
                        r.rl.y = sd->c[i].y;
                if (r.lu.y < sd->c[i].y)
                        r.lu.y = sd->c[i].y;
        }
        curr = sel;
        while (curr)
        {
                struct coord_rect *sr = &curr->u.c_rect;
                if (r.lu.x <= sr->rl.x && r.rl.x >= sr->lu.x && r.lu.y >= sr->rl.y && r.rl.y <= sr->lu.y)
                        return 1;
                curr = curr->next;
        }
        return 0;
}

static void tracking_doupdate_lines(struct tracking *tr, struct coord *pc, enum projection pro)
{
        int max_dist = 1000;
        struct map_selection *sel;
        struct mapset_handle *h;
        struct map *m;
        struct map_rect *mr;
        struct item *item;
        struct street_data *street;
        struct tracking_line *tl;
        struct coord_geo g;
        struct coord cc;

        //dbg(1,"enter\n");
        h = mapset_open(tr->ms);
        while ((m = mapset_next(h, 2)))
        {
                cc.x = pc->x;
                cc.y = pc->y;
                if (map_projection(m) != pro)
                {
                        transform_to_geo(pro, &cc, &g);
                        transform_from_geo(map_projection(m), &g, &cc);
                }
                sel = route_rect(18, &cc, &cc, 0, max_dist);
                mr = map_rect_new(m, sel);
                if (!mr)
                        continue;
                while ((item = map_rect_get_item(mr)))
                {
                        if (item_get_default_flags(item->type))
                        {
                                street = street_get_data(item);
                                if (street_data_within_selection(street, sel))
                                {
                                        tl = g_malloc(sizeof(struct tracking_line) + (street->count - 1) * sizeof(int));
                                        tl->street = street;
                                        tracking_get_angles(tl);
                                        tl->next = tr->lines;
                                        tr->lines = tl;
                                }
                                else
                                        street_data_free(street);
                        }
                }
                map_selection_destroy(sel);
                map_rect_destroy(mr);
        }
        mapset_close(h);
        //dbg(1, "exit\n");
}

void tracking_flush(struct tracking *tr)
{
        struct tracking_line *tl = tr->lines, *next;
        dbg(1, "enter(tr=%p)\n", tr);

        while (tl)
        {
                next = tl->next;
                street_data_free(tl->street);
                g_free(tl);
                tl = next;
        }
        tr->lines = NULL;
        tr->curr_line = NULL;
}

static int tracking_angle_diff(int a1, int a2, int full)
{
        int ret = (a1 - a2) % full;
        if (ret > full / 2)
                ret -= full;
        if (ret < -full / 2)
                ret += full;
        return ret;
}

static int tracking_angle_abs_diff(int a1, int a2, int full)
{
        int ret = tracking_angle_diff(a1, a2, full);
        if (ret < 0)
                ret = -ret;
        return ret;
}

static int tracking_angle_delta(struct tracking *tr, int vehicle_angle, int street_angle, int flags)
{
        int full = 180, ret = 360, fwd = 0, rev = 0;
        struct vehicleprofile *profile = tr->vehicleprofile;

        if (profile)
        {
                fwd = ((flags & profile->flags_forward_mask) == profile->flags);
                rev = ((flags & profile->flags_reverse_mask) == profile->flags);
        }
        if (fwd || rev)
        {
                if (!fwd || !rev)
                {
                        full = 360;
                        if (rev)
                                street_angle = (street_angle + 180) % 360;
                }
                ret = tracking_angle_abs_diff(vehicle_angle, street_angle, full);
        }
        return ret * ret;
}

static int tracking_is_connected(struct tracking *tr, struct coord *c1, struct coord *c2)
{
        if (c1[0].x == c2[0].x && c1[0].y == c2[0].y)
                return 0;
        if (c1[0].x == c2[1].x && c1[0].y == c2[1].y)
                return 0;
        if (c1[1].x == c2[0].x && c1[1].y == c2[0].y)
                return 0;
        if (c1[1].x == c2[1].x && c1[1].y == c2[1].y)
                return 0;
        return tr->connected_pref;
}

static int tracking_is_no_stop(struct tracking *tr, struct coord *c1, struct coord *c2)
{
        if (c1->x == c2->x && c1->y == c2->y)
                return tr->nostop_pref;
        return 0;
}

static int tracking_is_on_route(struct tracking *tr, struct route *rt, struct item *item)
{
#ifdef USE_ROUTING
        if (! rt)
        return 0;
        if (route_contains(rt, item))
        return 0;
        return tr->route_pref;
#else
        return 0;
#endif  
}

static int tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags)
{
        int value = 0;
        struct street_data *sd = t->street;
        //dbg(2, "%d: (0x%x,0x%x)-(0x%x,0x%x)\n", offset, sd->c[offset].x, sd->c[offset].y, sd->c[offset+1].x, sd->c[offset+1].y);
        if (flags & 1)
        {
                struct coord c1, c2, cp;
                c1.x = sd->c[offset].x;
                c1.y = sd->c[offset].y;
                c2.x = sd->c[offset + 1].x;
                c2.y = sd->c[offset + 1].y;
                cp.x = tr->curr_in.x;
                cp.y = tr->curr_in.y;
                value += transform_distance_line_sq(&c1, &c2, &cp, lpnt);
        }
        if (value >= min)
                return value;
        if (flags & 2)
                value += tracking_angle_delta(tr, tr->curr_angle, t->angle[offset], sd->flags) * tr->angle_pref >> 4;
        if (value >= min)
                return value;
        if ((flags & 4) && tr->connected_pref)
                value += tracking_is_connected(tr, tr->last, &sd->c[offset]);
        if ((flags & 8) && tr->nostop_pref)
                value += tracking_is_no_stop(tr, lpnt, &tr->last_out);
        if (value >= min)
                return value;
        if ((flags & 16) && tr->route_pref)
                value += tracking_is_on_route(tr, tr->rt, &sd->item);
        if ((flags & 32) && tr->overspeed_percent_pref && tr->overspeed_pref)
        {
                struct roadprofile *roadprofile = g_hash_table_lookup(tr->vehicleprofile->roadprofile_hash, (void *) t->street->item.type);
                if (roadprofile && tr->speed > roadprofile->speed * tr->overspeed_percent_pref / 100)
                        value += tr->overspeed_pref;
        }
        return value;
}

void tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile, enum projection pro)
{
        struct tracking_line *t;
        int i, value, min, time;
        struct coord lpnt;
        struct coord cin;
        struct attr valid, speed_attr, direction_attr, coord_geo, lag, time_attr, static_speed, static_distance;
        double speed, direction;
        if (v)
                tr->vehicle = v;
        if (vehicleprofile)
                tr->vehicleprofile = vehicleprofile;

        if (!tr->vehicle)
                return;
        if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL))
                valid.u.num = attr_position_valid_valid;
        if (valid.u.num == attr_position_valid_invalid)
        {
                tr->valid = valid.u.num;
                return;
        }
        if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL) || !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL) || !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL) || !vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL))
        {
                //dbg(0,"failed to get position data %d %d %d %d\n",
                //vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL),
                //vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL),
                //vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL),
                //vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL));
                return;
        }
        if (!vehicleprofile_get_attr(vehicleprofile, attr_static_speed, &static_speed, NULL) || !vehicleprofile_get_attr(vehicleprofile, attr_static_distance, &static_distance, NULL))
        {
                static_speed.u.num = 3;
                static_distance.u.num = 10;
                //dbg(1,"Using defaults for static position detection\n");
        }
        //dbg(2,"Static speed: %u, static distance: %u\n",static_speed.u.num, static_distance.u.num);
        time = iso8601_to_secs(time_attr.u.str);
        speed = *speed_attr.u.numd;
        direction = *direction_attr.u.numd;
        tr->valid = attr_position_valid_valid;
        transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in);
        if ((speed < static_speed.u.num && transform_distance(pro, &tr->last_in, &tr->curr_in) < static_distance.u.num))
        {
                //dbg(1,"static speed %f coord 0x%x,0x%x vs 0x%x,0x%x\n",speed,tr->last_in.x,tr->last_in.y, tr->curr_in.x, tr->curr_in.y);
                tr->valid = attr_position_valid_static;
                tr->speed = 0;
                return;
        }

        if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0)
        {
                double espeed;
                int edirection;
                if (time - tr->time == 1)
                {
                        /* dbg(1,"extrapolating speed from %f and %f (%f)\n",tr->speed, speed, speed-tr->speed); */
                        espeed = speed + (speed - tr->speed) * lag.u.num / 10;
                        /* dbg(1,"extrapolating angle from %f and %f (%d)\n",tr->direction, direction, tracking_angle_diff(direction,tr->direction,360)); */
                        edirection = direction + tracking_angle_diff(direction, tr->direction, 360) * lag.u.num / 10;
                }
                else
                {
                        //dbg(1,"no speed and direction extrapolation\n");
                        espeed = speed;
                        edirection = direction;
                }
                //dbg(1,"lag %d speed %f direction %d\n",lag.u.num,espeed,edirection);
                //dbg(1,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
                transform_project(pro, &tr->curr_in, espeed * lag.u.num / 36, edirection, &tr->curr_in);
                //dbg(1,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
        }
        tr->time = time;
        tr->pro = pro;
#if 0

        tracking_process_cdf(&tr->cdf, pc, &pcf, angle, &anglef, speed, fixtime);
#endif
        tr->curr_angle = tr->direction = direction;
        tr->speed = speed;
        tr->last_in = tr->curr_in;
        tr->last_out = tr->curr_out;
        tr->last[0] = tr->curr[0];
        tr->last[1] = tr->curr[1];
        if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > 500)
        {
                //dbg(1, "update\n");
                tracking_flush(tr);
                tracking_doupdate_lines(tr, &tr->curr_in, pro);
                tr->last_updated = tr->curr_in;
                //dbg(1,"update end\n");
        }

        tr->street_direction = 0;
        t = tr->lines;
        tr->curr_line = NULL;
        min = INT_MAX / 2;
        while (t)
        {
                struct street_data *sd = t->street;
                for (i = 0; i < sd->count - 1; i++)
                {
                        value = tracking_value(tr, t, i, &lpnt, min, -1);
                        if (value < min)
                        {
                                struct coord lpnt_tmp;
                                int angle_delta = tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360);
                                tr->curr_line = t;
                                tr->pos = i;
                                tr->curr[0] = sd->c[i];
                                tr->curr[1] = sd->c[i + 1];
                                /*
                                 dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i,
                                 transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt_tmp),
                                 tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*tr->angle_pref,
                                 tracking_is_connected(tr, tr->last, &sd->c[i]) ? tr->connected_pref : 0,
                                 lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? tr->nostop_pref : 0,
                                 value
                                 );
                                 */
                                tr->curr_out.x = lpnt.x;
                                tr->curr_out.y = lpnt.y;
                                tr->coord_geo_valid = 0;
                                if (angle_delta < 70)
                                        tr->street_direction = 1;
                                else if (angle_delta > 110)
                                        tr->street_direction = -1;
                                else
                                        tr->street_direction = 0;
                                min = value;
                        }
                }
                t = t->next;
        }

        //dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min);

        if (!tr->curr_line || min > tr->offroad_limit_pref)
        {
                tr->curr_out = tr->curr_in;
                tr->coord_geo_valid = 0;
                tr->street_direction = 0;
        }

        if (tr->curr_line && (tr->curr_line->street->flags & AF_UNDERGROUND))
        {
                //dbg(0,"AF_UNDERGROUND 1");
                if (tr->no_gps)
                {
                        tr->tunnel = 1;
                }
        }
        else if (tr->tunnel)
        {
                //dbg(0,"AF_UNDERGROUND 2");
                tr->speed = 0;
        }
        //dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
}

static int tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial)
{
        switch (attr->type)
        {
                case attr_angle_pref:
                        tr->angle_pref = attr->u.num;
                        return 1;
                case attr_connected_pref:
                        tr->connected_pref = attr->u.num;
                        return 1;
                case attr_nostop_pref:
                        tr->nostop_pref = attr->u.num;
                        return 1;
                case attr_offroad_limit_pref:
                        tr->offroad_limit_pref = attr->u.num;
                        return 1;
                case attr_route_pref:
                        tr->route_pref = attr->u.num;
                        return 1;
                case attr_overspeed_pref:
                        tr->overspeed_pref = attr->u.num;
                        return 1;
                case attr_overspeed_percent_pref:
                        tr->overspeed_percent_pref = attr->u.num;
                        return 1;
                case attr_tunnel_extrapolation:
                        tr->tunnel_extrapolation = attr->u.num;
                        return 1;
                default:
                        return 0;
        }
}

int tracking_set_attr(struct tracking *tr, struct attr *attr)
{
        return tracking_set_attr_do(tr, attr, 0);
}

struct tracking *
tracking_new(struct attr *parent, struct attr **attrs)
{
        struct tracking *this=g_new0(struct tracking, 1);
        struct attr hist_size;
        this->angle_pref = 10;
        this->connected_pref = 10;
        this->nostop_pref = 10;
        this->offroad_limit_pref = 15000; // was 5000 originally!
        this->route_pref = 300;

        if (!attr_generic_get_attr(attrs, NULL, attr_cdf_histsize, &hist_size, NULL))
        {
                hist_size.u.num = 0;
        }
        if (attrs)
        {
                for (; *attrs; attrs++)
                        tracking_set_attr_do(this, *attrs, 1);
        }

        tracking_init_cdf(&this->cdf, hist_size.u.num);

        return this;
}

void tracking_set_mapset(struct tracking *this, struct mapset *ms)
{
        this->ms = ms;
}

void tracking_set_route(struct tracking *this, struct route *rt)
{
        this->rt = rt;
}

void tracking_destroy(struct tracking *tr)
{
        if (tr->attr)
                attr_free(tr->attr);
        tracking_flush(tr);
        g_free(tr);
}

struct map *
tracking_get_map(struct tracking *this_)
{
        struct attr *attrs[5];
        struct attr type, navigation, data, description;
        type.type = attr_type;
        type.u.str = "tracking";
        navigation.type = attr_trackingo;
        navigation.u.tracking = this_;
        data.type = attr_data;
        data.u.str = "";
        description.type = attr_description;
        description.u.str = "Tracking";

        attrs[0] = &type;
        attrs[1] = &navigation;
        attrs[2] = &data;
        attrs[3] = &description;
        attrs[4] = NULL;
        if (!this_->map)
                this_->map = map_new(NULL, attrs);
        return this_->map;
}

struct map_priv
{
        struct tracking *tracking;
};

struct map_rect_priv
{
        struct tracking *tracking;
        struct item item;
        struct tracking_line *curr, *next;
        int coord;
        enum attr_type attr_next;
        int ccount;
        int debug_idx;
        char *str;
};

static int tracking_map_item_coord_get(void *priv_data, struct coord *c, int count)
{
        struct map_rect_priv *this = priv_data;
        enum projection pro;
        int ret = 0;
        dbg(1, "enter\n");
        while (this->ccount < 2 && count > 0)
        {
                pro = map_projection(this->curr->street->item.map);
                if (projection_mg != pro)
                {
                        transform_from_to(&this->curr->street->c[this->ccount + this->coord], pro, c, projection_mg);
                }
                else
                        *c = this->curr->street->c[this->ccount + this->coord];
                dbg(1, "coord %d 0x%x,0x%x\n", this->ccount, c->x, c->y);
                this->ccount++;
                ret++;
                c++;
                count--;
        }
        return ret;
}

static int tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr)
{
        struct map_rect_priv *this_ = priv_data;
        struct coord lpnt, *c;
        struct tracking *tr = this_->tracking;
        int value;
        attr->type = attr_type;

        if (this_->str)
        {
                g_free(this_->str);
                this_->str = NULL;
        }

        switch (attr_type)
        {
                case attr_debug:
                        switch (this_->debug_idx)
                        {
                                case 0:
                                        this_->debug_idx++;
                                        this_->str = attr->u.str = g_strdup_printf("overall: %d (limit %d)", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, -1), tr->offroad_limit_pref);
                                        return 1;
                                case 1:
                                        this_->debug_idx++;
                                        c = &this_->curr->street->c[this_->coord];
                                        value = tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 1);
                                        this_->str = attr->u.str = g_strdup_printf("distance: (0x%x,0x%x) from (0x%x,0x%x)-(0x%x,0x%x) at (0x%x,0x%x) %d", tr->curr_in.x, tr->curr_in.y, c[0].x, c[0].y, c[1].x, c[1].y, lpnt.x, lpnt.y, value);
                                        return 1;
                                case 2:
                                        this_->debug_idx++;
                                        this_->str = attr->u.str = g_strdup_printf("angle: %d to %d (flags %d) %d", tr->curr_angle, this_->curr->angle[this_->coord], this_->curr->street->flags & 3, tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 2));
                                        return 1;
                                case 3:
                                        this_->debug_idx++;
                                        this_->str = attr->u.str = g_strdup_printf("connected: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 4));
                                        return 1;
                                case 4:
                                        this_->debug_idx++;
                                        this_->str = attr->u.str = g_strdup_printf("no_stop: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 8));
                                        return 1;
                                case 5:
                                        this_->debug_idx++;
                                        this_->str = attr->u.str = g_strdup_printf("route: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 16));
                                        return 1;
                                case 6:
                                        this_->debug_idx++;
                                        this_->str = attr->u.str = g_strdup_printf("overspeed: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 32));
                                        return 1;
                                case 7:
                                        this_->debug_idx++;
                                        this_->str = attr->u.str = g_strdup_printf("line %p", this_->curr);
                                        return 1;
                                default:
                                        this_->attr_next = attr_none;
                                        return 0;
                        }
                case attr_any:
                        while (this_->attr_next != attr_none)
                        {
                                if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr))
                                        return 1;
                        }
                        return 0;
                default:
                        attr->type = attr_none;
                        return 0;
        }
}

static struct item_methods tracking_map_item_methods = { NULL, tracking_map_item_coord_get, NULL, tracking_map_item_attr_get, };

static void tracking_map_destroy(struct map_priv *priv)
{
        g_free(priv);
}

static void tracking_map_rect_init(struct map_rect_priv *priv)
{
        priv->next = priv->tracking->lines;
        priv->curr = NULL;
        priv->coord = 0;
        priv->item.id_lo = 0;
        priv->item.id_hi = 0;
}

static struct map_rect_priv *
tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel)
{
        struct tracking *tracking = priv->tracking;
        struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1);
        ret->tracking = tracking;
        tracking_map_rect_init(ret);
        ret->item.meth = &tracking_map_item_methods;
        ret->item.priv_data = ret;
        ret->item.type = type_tracking_100;
        return ret;
}

static void tracking_map_rect_destroy(struct map_rect_priv *priv)
{
        g_free(priv);
}

static struct item *
tracking_map_get_item(struct map_rect_priv *priv)
{
        struct item *ret = &priv->item;
        int value;
        struct coord lpnt;

        if (!priv->next)
                return NULL;
        if (!priv->curr || priv->coord + 2 >= priv->curr->street->count)
        {
                priv->curr = priv->next;
                priv->next = priv->curr->next;
                priv->coord = 0;
                priv->item.id_lo = 0;
                priv->item.id_hi++;
        }
        else
        {
                priv->coord++;
                priv->item.id_lo++;
        }
        value = tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, INT_MAX / 2, -1);
        if (value < 64)
                priv->item.type = type_tracking_100;
        else if (value < 128)
                priv->item.type = type_tracking_90;
        else if (value < 256)
                priv->item.type = type_tracking_80;
        else if (value < 512)
                priv->item.type = type_tracking_70;
        else if (value < 1024)
                priv->item.type = type_tracking_60;
        else if (value < 2048)
                priv->item.type = type_tracking_50;
        else if (value < 4096)
                priv->item.type = type_tracking_40;
        else if (value < 8192)
                priv->item.type = type_tracking_30;
        else if (value < 16384)
                priv->item.type = type_tracking_20;
        else if (value < 32768)
                priv->item.type = type_tracking_10;
        else
                priv->item.type = type_tracking_0;
        dbg(1, "item %d %d points\n", priv->coord, priv->curr->street->count);
        priv->ccount = 0;
        priv->attr_next = attr_debug;
        priv->debug_idx = 0;
        return ret;
}

static struct item *
tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo)
{
        struct item *ret;
        tracking_map_rect_init(priv);
        while ((ret = tracking_map_get_item(priv)))
        {
                if (ret->id_hi == id_hi && ret->id_lo == id_lo)
                        return ret;
        }
        return NULL;
}

static struct map_methods tracking_map_meth = { projection_mg, "utf-8", tracking_map_destroy, tracking_map_rect_new, tracking_map_rect_destroy, tracking_map_get_item, tracking_map_get_item_byid, NULL, NULL, NULL, };

static struct map_priv *
tracking_map_new(struct map_methods *meth, struct attr **attrs, struct callback_list *cbl)
{
        struct map_priv *ret;
        struct attr *tracking_attr;

        tracking_attr = attr_search(attrs, NULL, attr_trackingo);
        if (!tracking_attr)
                return NULL; ret=g_new0(struct map_priv, 1);
        *meth = tracking_map_meth;
        ret->tracking = tracking_attr->u.tracking;

        return ret;
}

void tracking_init(void)
{
        plugin_register_map_type("tracking", tracking_map_new);
}