// Original version: Ola Martin Lykkja
// Tweaks: Adam Wulkiewicz

// This application tests different ways of searching for nearest points in spherical_equatorial CS
// 1. std::sort()
// 2. rtree.insert()
// 3. rtree pack-create
// The method 1 is considered as producing the correct results

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/foreach.hpp>
namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

// Standard C++ includes
#include <vector>
#include <algorithm>
#include <iostream>

typedef struct pos_s {
        double lat;
        double lon;
} pos_t;

typedef struct poi_s {
        int         id;
        const char *name;
        double      lat;
        double      lon;
} poi_t;

// This includefile contains a lot of data points.
#include "test_boost_geo2_data.h"

// Calculate distance on earth surface
template <typename Pt>
double geo_calculate_distance(Pt const& from, Pt const& to)
{
    double res = bg::comparable_distance(from, to);
    /*return bg::distance(geo_point(from->lon, from->lat),
                        geo_point(to->lon, to->lat),
                        bg::strategy::distance::haversine<double>(6371.0 * 1000.0));*/

    return res * 100000000.0;
}

template <typename Pt>
class sort_geo_poi_on_distance
{
private:
    const Pt &zero;
public:
    sort_geo_poi_on_distance(const Pt &z) : zero(z) { }

    template <typename V>
    bool operator()(V const & a, V const & b) const
    {
        double ad = geo_calculate_distance(zero, a.first);
        double bd = geo_calculate_distance(zero, b.first);
        return ad < bd;
    }
};

// Print result list
template<typename T>
void print(const bg::model::point<double, 2, T> &target,
           const std::vector< std::pair< bg::model::point<double, 2, T>, const poi_t *> > & res,
           const std::vector< std::pair< bg::model::point<double, 2, T>, const poi_t *> > & expected)
{
   typedef bg::model::point<double, 2, T> geo_point;
   typedef std::pair<geo_point, const poi_t *> geo_value;

   std::cout << "t:" << bg::get<0>(target) << "," << bg::get<1>(target) << "\n";

   // Display results
   unsigned int idx = 0;
   BOOST_FOREACH(geo_value const& v, res)
   {
      pos_t poi_pos; poi_pos.lat=bg::get<1>(v.first); poi_pos.lon=bg::get<0>(v.first);
      double dist = geo_calculate_distance(target, v.first);
      bool correct = (expected.size() > idx && expected[idx].second->id == v.second->id);

      printf(" %9.6f,%9.6f   %6d %-40s %8.2f", poi_pos.lon, poi_pos.lat, v.second->id, v.second->name, dist);
      if (!expected.empty())
         printf(correct ? "   Ok" : "   Wrong");
      printf("\n");
      idx++;
   }
}



int main()
{
    typedef bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> > geo_point;
    typedef bg::model::box<geo_point> geo_box;
    typedef std::pair<geo_point, const poi_t *> geo_poi_value;
    typedef bgi::rtree< geo_poi_value, bgi::dynamic_rstar > poi_rtree;

    size_t knn = 20;
    size_t max = 30000;

    std::vector<geo_poi_value> values;
    poi_rtree rtree(bgi::dynamic_rstar(32));
    size_t i = 0;
    for (const poi_t *poi = get_poi_list(); poi->id && i < max; poi++, ++i)
    {
        geo_point pt(poi->lon, poi->lat);
        values.push_back(std::make_pair(pt, poi));
        rtree.insert(std::make_pair(pt, poi));
    }
    poi_rtree pack_rtree(values.begin(), values.end(), bgi::dynamic_rstar(32));

    const poi_t *poi = get_poi_list();
    geo_point target(poi->lon, poi->lat);

    std::sort(values.begin(), values.end(), sort_geo_poi_on_distance<geo_point>(target));
    values.resize(knn);

    std::vector<geo_poi_value> result;
    rtree.query(bgi::nearest(target, knn), std::back_inserter(result));
    std::sort(result.begin(), result.end(), sort_geo_poi_on_distance<geo_point>(target));

    std::vector<geo_poi_value> result2;
    pack_rtree.query(bgi::nearest(target, knn), std::back_inserter(result2));
    std::sort(result2.begin(), result2.end(), sort_geo_poi_on_distance<geo_point>(target));
    
    std::cout << "------------------------------------\n\n";
    print(target, values, values);
    std::cout << "------------------------------------\n\n";
    print(target, result, values);
    std::cout << "------------------------------------\n\n";
    print(target, result2, values);
    std::cout << "------------------------------------\n\n";

    return 0;
}