GoldenCheetah/src/Route.cpp

/*
 * Copyright (c) 2011,2014 Damien Grauser (Damien.Grauser@pev-geneve.ch)
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
 * Software Foundation; either version 2 of the License, or (at your option)
 * any later version.
 *
 * 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 "Route.h"

#include "Athlete.h"
#include "MainWindow.h"
#include "RideCache.h"
#include "RideItem.h"
#include "IntervalItem.h"
#include "RouteParser.h"
#include "RideFile.h"
#include "GProgressDialog.h"

#include <QString>
#include <QFile>
#include <QXmlInputSource>
#include <QXmlSimpleReader>


#define tr(s) QObject::tr(s)

#define pi 3.14159265358979323846

/*
 * RouteSegment
 *
 */
RouteSegment::RouteSegment()
{
    _id = QUuid::createUuid(); // in case it isn't set yet
}

RouteSegment::RouteSegment(Routes *routes) : routes(routes)
{
    _id = QUuid::createUuid(); // in case it isn't set yet
}

QString RouteSegment::getName() {
    return name;
}

void
RouteSegment::setName(QString _name)
{
    name = _name;
}

QList<RoutePoint> RouteSegment::getPoints() {
    return points;
}

int
RouteSegment::addPoint(RoutePoint _point)
{
    points.append(_point);
    return points.count();
}

void 
RouteSegment::search(RideItem *item, RideFile*ride, QList<IntervalItem*>&here)
{
    double minimumprecision = 0.100; //100m
    double maximumprecision = 0.010; //10m
    double precision = -1;

    int found = 0;
    int diverge = 0;
    int lastpoint = -1; // Last point to match
    double start = -1, stop = -1; // Start and stop secs

    for (int n=0; n< this->getPoints().count();n++) {
        RoutePoint routepoint = this->getPoints().at(n);

        bool present = false;
        RideFilePoint* point;

        for (int i=lastpoint+1; i<ride->dataPoints().count();i++) {
            point = ride->dataPoints().at(i);

            double minimumdistance = -1;

            if (point->lat != 0 && point->lon !=0 &&
                ceil(point->lat) != 180 && ceil(point->lon) != 180 &&
                ceil(point->lat) != 540 && ceil(point->lon) != 540) {
                // Valid GPS value
                if (start == -1) {
                    diverge = 0;
                    // Calculate distance to route point
                    double _dist = distance(routepoint.lat, routepoint.lon, point->lat, point->lon) ;
                    minimumdistance = _dist;

                    if (precision == -1 || _dist<precision)
                        precision = _dist;

                    if (_dist>1) {
                        // fare away from reference point
                        i += 50;
                    } else {
                        if (_dist<minimumprecision) {
                            start = 0; //try to start
                        }
                    }
                }

                if (start != -1) {
                    int end = i+10;
                    for (int j=i; j<ride->dataPoints().count() && j<end;j++) {
                        RideFilePoint* nextpoint = ride->dataPoints().at(j);

                        if (nextpoint->lat != 0 && nextpoint->lon !=0 && ceil(nextpoint->lat) != 180 && ceil(nextpoint->lon) != 180) {
                            double _nextdist = distance(routepoint.lat, routepoint.lon, nextpoint->lat, nextpoint->lon) ;

                            if (minimumdistance ==-1 || _nextdist<minimumdistance){
                                //new minimumdistance
                                point = nextpoint;
                                i = j;
                                minimumdistance = _nextdist;
                            }
                            if (_nextdist <= minimumprecision) {
                                end = j+10;
                            }

                            if (_nextdist <= maximumprecision) {
                                // maximum precision reached
                                j = end;
                            }
                        }
                    }

                    if (minimumdistance <= minimumprecision) {
                        // Close enough from reference point

                        present = true;
                        lastpoint = i;
                        if (start == 0) {
                            start = point->secs;
                            precision = 0;
                        }

                        if (minimumdistance>precision)
                            precision = minimumdistance;

                        break;
                    }
                    else {
                        diverge++;
                        if (diverge>2) {
                            start = -1; //try to restart
                            n = 0;
                        }
                        //present = true;
                    }



                }
            }

        }
        stop = point->secs;


        if (!present) {
            break;
        } else {
            if (n == this->getPoints().count()-1){

                // Add the interval and continue search
                IntervalItem *intervalItem = new IntervalItem(item, name,
                                                              start, stop,
                                                              ride->timeToDistance(start),
                                                              ride->timeToDistance(stop),
                                                              ++found,
                                                              QColor(Qt::gray),
                                                              RideFileInterval::ROUTE);
                here << intervalItem;
                start = -1;
                n=0;
                return; // just find the first occurrence
            }
        }
    }
}


//  This function converts decimal degrees to radians
double deg2rad(double deg) {
  return (deg * pi / 180);
}

// This function converts radians to decimal degrees
double rad2deg(double rad) {
  return (rad * 180 / pi);
}

// lat1, lon1 = point 1, Latitude and Longitude of
// lat2, lon2 = Latitude and Longitude of point 2
double
RouteSegment::distance(double lat1, double lon1, double lat2, double lon2) {
  double _theta, _dist;
  _theta = lon1 - lon2;
  if (_theta == 0 && (lat1 - lat2) == 0)
      _dist = 0;
  else {
      _dist = sin(deg2rad(lat1)) * sin(deg2rad(lat2)) + cos(deg2rad(lat1)) * cos(deg2rad(lat2)) * cos(deg2rad(_theta));
      _dist = acos(_dist) * 6371;
  }
  return (_dist);
}



/*
 * Routes (list of RouteSegment)
 *
 *
 */

Routes::Routes(Context *context, const QDir &home)
{
    this->home = home;
    this->context = context;
    readRoutes();
}

Routes::~Routes()
{
    writeRoutes();
}

quint16
Routes::getFingerprint() const
{
    // all the QUuids will suffice
    QByteArray ba;
    foreach(RouteSegment segment, routes) ba += segment.id().toByteArray();

    // we spot other things separately
    return qChecksum(ba, ba.length());
}

void
Routes::readRoutes()
{
    QFile routeFile(home.canonicalPath() + "/routes.xml");

    QXmlInputSource source( &routeFile );
    QXmlSimpleReader xmlReader;
    RouteParser( handler );
    xmlReader.setContentHandler(&handler);
    xmlReader.setErrorHandler(&handler);
    xmlReader.parse( source );
    routes = handler.getRoutes();
}

int
Routes::newRoute(QString name)
{
    RouteSegment add;
    add.setName(name);
    routes.insert(0, add);

    return 0; // always add at the top
}

void
Routes::deleteRoute(int index)
{
    // now delete!
    routes.removeAt(index);
    writeRoutes();
}

void
Routes::writeRoutes()
{
    // update routes.xml

    QString file = QString(home.canonicalPath() + "/routes.xml");
    RouteParser::serialize(file, routes);
}

void
Routes::search(RideItem *item, RideFile*ride, QList<IntervalItem*>&here)
{
    if (ride) {

        // search all segments
        for (int routecount=0;routecount<routes.count();routecount++) {
            RouteSegment *segment = &routes[routecount];
            segment->search(item, ride, here);
        }
    }
}

void
Routes::createRouteFromInterval(IntervalItem *activeInterval) 
{

    // if refresh is running cancel it now, as we're adding
    // a new route to search, so will need to restart
    context->athlete->rideCache->cancel();

    // create a new route
    int index = context->athlete->routes->newRoute("route");
    RouteSegment *route = &context->athlete->routes->routes[index];

    QRegExp watts("\\([0-9]* *watts\\)");

    QString name = activeInterval->name; //activeInterval->text(0).trimmed();
    if (name.contains(watts))
        name = name.left(name.indexOf(watts)).trimmed();

    if (name.length()<4 || name.startsWith("Selection #") )
        name = QString(tr("Route #%1")).arg(context->athlete->routes->routes.length());

    route->setName(name);

    // Construct the route with interval gps data
    double dist = 0, lastLat = 0, lastLon = 0;

    foreach (RideFilePoint *point, activeInterval->rideItem()->ride()->dataPoints()) {
        if (point->secs >= activeInterval->start && point->secs < activeInterval->stop) {
            if (lastLat != 0 && lastLon != 0 &&
                point->lat != 0 && point->lon != 0 &&
                ceil(point->lat) != 180 && ceil(point->lon) != 180) {
                // distance ith last point
                double _dist = route->distance(lastLat, lastLon, point->lat, point->lon);

                if (_dist>=0.001)
                    dist += _dist;

                if (dist>0.02) {
                   RoutePoint _point = RoutePoint(point->lon, point->lat);
                   route->addPoint(_point);
                   dist = 0;
                }
            }
            lastLat = point->lat;
            lastLon = point->lon;
        }
    }

    // update on disk
    context->athlete->routes->writeRoutes();

    // now go and refresh !
    context->athlete->rideCache->refresh();
}