gpxtools/gpxsim.cpp

#include <cmath>
#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>

#include "Algorithm.h"
#include "Arguments.h"
#include "XMLRawParser.h"

// ----------------------------------------------------------------------------

namespace gpxtools
{
  class GPXSim : public XMLParserHandler
  {
  public:
    // -- Constructor -----------------------------------------------------------
    GPXSim() :
      _arguments ("gpxsim [OPTION].. [FILE]\nSimplify a GPX-file.\n", "gpxsim v0.1",
                  "Simplify a route or track using the distance threshold and/or the Douglas-Peucker algorithm and display the resulting GPX-file on standard output."),
      _verbose   (_arguments, true,  'v', "verbose",                         "display the results of the simplification"),
      _distance  (_arguments, true,  'd', "distance",   "METRES",            "remove route or track points within distance of the previous point in METRES", ""),
      _number    (_arguments, true,  'n', "number",     "NUMBER",            "remove route or track points until the route or track contains NUMBER points", ""),
      _crossTrack(_arguments, true,  'x', "crossTrack", "METRES",            "remove route or track points with a cross track distance less than METRES", ""),
      _xmlParser(this),
      _simplifyDistance(0.0),
      _simplifyCrossTrack(0.0),
      _simplifyToNumber(0),
      _inPoints(false)
    {
    }

    // -- Deconstructor ---------------------------------------------------------
    virtual ~GPXSim()
    {
    }

    // -- Parse arguments -------------------------------------------------------
    bool processArguments(int argc, char *argv[])
    {
      std::vector<std::string> filenames;

      if (!_arguments.parse(argc,argv, filenames))
      {
        return false;
      }
      else if (!checkArguments(filenames))
      {
        return false;
      }
      else if (filenames.empty())
      {
        return _xmlParser.parse(std::cin);
      }
      else
      {
        return parseFile(filenames.front());
      }
    }

    // -- Check arguments ---------------------------------------------------------
    bool checkArguments(const std::vector<std::string> &filenames)
    {
      if (filenames.size() > 1)
      {
        std::cerr << "Too many input files."  << std::endl;
        return false;
      }

      if (!_distance.value().empty())
      {
        try
        {
          _simplifyDistance = std::stoi(_distance.value());

          if (_simplifyDistance < 0)
          {
            std::cerr << "Invalid value for --" << _distance.longOption() << " : " << _distance.value() << std::endl;
            return false;
          }
        }
        catch(...)
        {
          std::cerr << "Invalid value for --" << _distance.longOption() << " : " << _distance.value() << std::endl;
          return false;
        }
      }

      if (!_number.value().empty())
      {
        try
        {
          _simplifyToNumber = std::stoi(_number.value());

          if (_simplifyToNumber < 0)
          {
            std::cerr << "Invalid value for --" << _number.longOption() << " : " << _number.value() << std::endl;
            return false;
          }
        }
        catch(...)
        {
          std::cerr << "Invalid value for --" << _number.longOption() << " : " << _number.value() << std::endl;
          return false;
        }
      }

      if (!_crossTrack.value().empty())
      {
        try
        {
          _simplifyCrossTrack = std::stoi(_crossTrack.value());

          if (_simplifyCrossTrack < 0)
          {
            std::cerr << "Invalid value for --" << _crossTrack.longOption() << " : " << _crossTrack.value() << std::endl;
            return false;
          }
        }
        catch(...)
        {
          std::cerr << "Invalid value for --" << _crossTrack.longOption() << " : " << _crossTrack.value() << std::endl;
          return false;
        }
      }

      return true;
    }

    // -- Parse a file ----------------------------------------------------------
    bool parseFile(const std::string &filename)
    {
      bool ok =false;

      std::ifstream file(filename);

      if (file.is_open())
      {
        ok = _xmlParser.parse(file);

        file.close();
      }
      else
      {
        std::cerr << "Unable to open: " << filename << std::endl;
      }

      return ok;
    }

  private:
    // Structs
    enum ChunkType { TEXT, POINT };

    struct Chunk
    {
      void clear()
      {
        _type       = TEXT;
        _text.clear();
        _lat        = 0.0;
        _lon        = 0.0;
        _crossTrack = std::numeric_limits<double>::max();
      }

      void point(double lat, double lon)
      {
        _type       = POINT;
        _lat        = lat;
        _lon        = lon;
        _crossTrack = std::numeric_limits<double>::max();
      }

      ChunkType     _type;
      std::string   _text;
      double        _lat;
      double        _lon;
      double        _crossTrack;
    };

    void store(const std::string &text)
    {
      if (_inPoints)
      {
        _current._text.append(text);
      }
      else if (!_verbose.active())
      {
        std::cout << text;
      }
    }

    bool isEmpty(const std::string &text)
    {
      for (auto ch = text.begin(); ch != text.end(); ++ch)
      {
        if (!::isspace(*ch)) return false;
      }

      return true;
    }

    void outputChunks()
    {
      ChunkType last = ChunkType::POINT;

      while (!_chunks.empty())
      {
        if ((_chunks.front()._type == ChunkType::POINT) || (last == ChunkType::POINT) || (!isEmpty(_chunks.front()._text)))
        {
          if (!_verbose.active())
          {
            std::cout << _chunks.front()._text;
          }
        }

        last = _chunks.front()._type;

        _chunks.pop_front();
      }
    }

    void verboseChunks(const std::string &title)
    {
      int points      = 0;
      double distance = 0.0;

      auto prev = _chunks.end();

      for (auto iter = _chunks.begin(); iter != _chunks.end(); ++iter)
      {
        if (iter->_type == ChunkType::POINT)
        {
          points++;

          if (prev != _chunks.end())
          {
            distance += gpx::calcDistance(prev->_lat, prev->_lon, iter->_lat, iter->_lon);
          }

          prev = iter;
        }
      }

      std::cout << title << " Points: " << std::setw(4) << points << " Distance: " << std::setw(10) << std::setprecision(2) << std::fixed << distance << " m" << std::endl;
    }

    void simplifyDistance()
    {
      auto iter = _chunks.begin();
      auto prev = _chunks.end();

      while (iter != _chunks.end())
      {
        if (iter->_type == ChunkType::POINT)
        {
          if (prev != _chunks.end() && gpx::calcDistance(prev->_lat, prev->_lon, iter->_lat, iter->_lon) < _simplifyDistance)
          {
            iter = _chunks.erase(iter);
          }
          else
          {
            prev = iter++;
          }
        }
        else
        {
          ++iter;
        }
      }
    }

    int setCrossTracks()
    {
      int points = 0;

      auto p1 = _chunks.end();
      auto p2 = _chunks.end();

      for (auto p3 = _chunks.begin(); p3 != _chunks.end(); ++p3)
      {
        if (p3->_type != ChunkType::POINT) continue;

        points++;

        p3->_crossTrack = std::numeric_limits<double>::max();

        if (p1 != _chunks.end() && p2 != _chunks.end())
        {
          p2->_crossTrack = fabs(gpx::calcCrosstrack(p1->_lat, p1->_lon, p3->_lat, p3->_lon, p2->_lat, p2->_lon));
        }

        p1 = p2;
        p2 = p3;
      }

      return points;
    }

    std::list<Chunk>::iterator forwards(std::list<Chunk>::iterator p)
    {
      do
      {
        ++p;
      }
      while ((p != _chunks.end()) && (p->_type != ChunkType::POINT));

      return p;
    }

    std::list<Chunk>::iterator backwards(std::list<Chunk>::iterator p)
    {
      while (p != _chunks.begin())
      {
        if ((--p)->_type == ChunkType::POINT)
        {
          return p;
        }
      }

      return _chunks.end();
    }

    void UpdateCrossTrack(std::list<Chunk>::iterator p1, std::list<Chunk>::iterator p2, std::list<Chunk>::iterator p3)
    {
      if (p2 != _chunks.end())
      {
        if (p1 != _chunks.end() && p3 != _chunks.end())
        {
          p2->_crossTrack = fabs(gpx::calcCrosstrack(p1->_lat, p1->_lon, p3->_lat, p3->_lon, p2->_lat, p2->_lon));
        }
        else
        {
          p2->_crossTrack = std::numeric_limits<double>::max();
        }
      }
    }

    void simplifyByCrossTrack(int points)
    {
      while (true)
      {
        auto lowest = _chunks.end();

        for (auto p = _chunks.begin(); p != _chunks.end(); ++p)
        {
          if ((p->_type == ChunkType::POINT) && ((lowest == _chunks.end()) || (lowest->_crossTrack > p->_crossTrack)))
          {
            lowest = p;
          }
        }

        if (lowest == _chunks.end()) break;

        if ((_simplifyCrossTrack > 0.0) && (lowest->_crossTrack > _simplifyCrossTrack)) break;

        auto p2 = backwards(lowest);
        auto p1 = (p2 != _chunks.end() ? backwards(p2) : _chunks.end());
        // p3 = lowest
        auto p4 = forwards(lowest);
        auto p5 = (p4 != _chunks.end() ? forwards(p4) : _chunks.end());

        _chunks.erase(lowest);
        points--;

        if ((_simplifyToNumber > 0) && (points <= _simplifyToNumber)) break;

        // Update the crosstracks
        UpdateCrossTrack(p1, p2, p4);

        UpdateCrossTrack(p2, p4, p5);
      }
    }

    void simplifyCrossTrack()
    {
      int points = setCrossTracks();

      if ((_simplifyToNumber > 0) && (points <= _simplifyToNumber)) return;

      simplifyByCrossTrack(points);
    }

    static bool getDoubleAttribute(const Attributes &atts, const std::string &key, double &value)
    {
      auto iter = atts.find(key);

      if (iter == atts.end()) return false;

      if (iter->second.empty()) return false;
      try
      {
        value = std::stod(iter->second);

        return true;
      }
      catch(...)
      {
        return false;
      }
    }

  public:
    // -- Callbacks -------------------------------------------------------------
    virtual void startElement(const std::string &path, const std::string &text, const std::string &, const Attributes &attributes)
    {
      if (path == "/gpx/trk/trkseg" || path == "/gpx/rte")
      {
        _current.clear();

        _inPoints = true;
      }
      else if (path == "/gpx/trk/trkseg/trkpt" || path == "/gpx/rte/rtept")
      {
        if (!_current._text.empty()) _chunks.push_back(_current);

        _current.clear();

        double lat,lon;

        if (getDoubleAttribute(attributes, "lat", lat) && getDoubleAttribute(attributes, "lon", lon))
        {
          _current.point(lat, lon);
        }
      }

      store(text);
    }

    virtual void text(const std::string &, const std::string &text)
    {
      store(text);
    }

    virtual void unhandled(const std::string &, const std::string &text)
    {
      store(text);
    }

    virtual void endElement(const std::string &path, const std::string &text, const std::string &)
    {
      store(text);

      if (path == "/gpx/trk/trkseg" || path == "/gpx/rte")
      {
        if (!_current._text.empty())
        {
          _chunks.push_back(_current);
        }

        if (_verbose.active())
        {
          verboseChunks("Original  segment:");
        }

        if (_simplifyDistance > 0.0)   simplifyDistance();
        if ((_simplifyCrossTrack > 0.0) || (_simplifyToNumber > 0)) simplifyCrossTrack();

        if (_verbose.active())
        {
          verboseChunks("Optimized segment:");
        }

        outputChunks();

        _inPoints = false;
      }
      else if (path == "/gpx/trk/trkseg/trkpt" || path == "/gpx/rte/rtept")
      {
        _chunks.push_back(_current);

        _current.clear();
      }
    }

  private:
    // Members
    arg::Arguments    _arguments;

    arg::Argument     _verbose;
    arg::Argument     _distance;
    arg::Argument     _number;
    arg::Argument     _crossTrack;

    XMLRawParser      _xmlParser;

    double            _simplifyDistance;
    double            _simplifyCrossTrack;
    int               _simplifyToNumber;

    bool              _inPoints;
    Chunk             _current;
    std::list<Chunk>  _chunks;
  };
}

// -- Main program ------------------------------------------------------------

int main(int argc, char *argv[])
{
  gpxtools::GPXSim gpxSim;

  return gpxSim.processArguments(argc, argv) ? 0 : 1;
}