#include <geometry/distance.hpp>
#include <boost/static_assert.hpp>
#include <boost/timer.hpp>
#include <cstdio>

using namespace geometry;

template<typename T, size_t D>
class my_point 
{
public:
  // Member type/const 
  typedef T coordinate_type;
  static const size_t coordinate_count = D;

  /// Compile time access to coordinate values
  template <size_t K>
  inline coordinate_type& get()
  {
    BOOST_STATIC_ASSERT(K < D);
    return m_values[K];
  }

  template <size_t K>
  inline const coordinate_type& get() const
  {
    BOOST_STATIC_ASSERT(K < D);
    return m_values[K];
  }

  /// Runtime access to coordinate values
  inline coordinate_type& operator[] (size_t k)
  {
    return m_values[k];
  }

  inline const coordinate_type& operator[] (size_t k) const
  {
    return m_values[k];
  }

private:
  coordinate_type m_values[D];
};

template< typename P1, typename P2 >
inline distance_result compiled_distance(const P1& p1, const P2& p2)
{
  BOOST_STATIC_ASSERT(point_traits<P1>::coordinate_count == point_traits<P2>::coordinate_count);

  typedef typename select_type_traits<
                         typename point_traits<P1>::coordinate_type,
                         typename point_traits<P2>::coordinate_type
                           >::type T;
  return distance_result(strategy::distance::impl::compute_pythagoras<P1, P2, point_traits<P1>::coordinate_count, T>::result(p1, p2), true);
}

template< typename P1, typename P2 >
inline distance_result runtime_distance(const P1& p1, const P2& p2)
{
  BOOST_STATIC_ASSERT(point_traits<P1>::coordinate_count == point_traits<P2>::coordinate_count);
  typedef typename select_type_traits<
                         typename point_traits<P1>::coordinate_type,
                         typename point_traits<P2>::coordinate_type
                           >::type T;

  T sum = 0;
  for (size_t i = 0; i < point_traits<P1>::coordinate_count; ++i) {
    T d = p1[i] - p2[i];
    sum += d*d;
  }

  return distance_result(sum, true);
}

template< typename P1, typename P2 >
inline distance_result runtime_distance_unrolled(const P1& p1, const P2& p2)
{
  BOOST_STATIC_ASSERT(point_traits<P1>::coordinate_count == point_traits<P2>::coordinate_count);
  typedef typename select_type_traits<
                         typename point_traits<P1>::coordinate_type,
                         typename point_traits<P2>::coordinate_type
                           >::type T;

  T sum = 0;
  for (size_t i = 0; i < point_traits<P1>::coordinate_count; i += 4) {
    T d1 = p1[i] - p2[i];
    T d2 = p1[i+1] - p2[i+1];
    T d3 = p1[i+2] - p2[i+2];
    T d4 = p1[i+3] - p2[i+3];
    sum += d1*d1 + d2*d2 + d3*d3 + d4*d4;
  }

  return distance_result(sum, true);
}

#ifndef ITERATIONS
#define ITERATIONS 100000000
#endif
#ifndef DIMENSIONS
#define DIMENSIONS 3
#endif

int main(int argc, char** argv)
{
  my_point<volatile float, DIMENSIONS> p1, p2;
  for (size_t i = 0; i < DIMENSIONS; ++i) {
    p1[i] = 1;
    p2[i] = 0;
  }

  double d;
  boost::timer t;
  for (long i = 0; i < ITERATIONS; ++i) {
    d = compiled_distance(p1, p2).first;
  }
  double tc = t.elapsed();

  printf("Distance = %f\n", d);
  printf("Compile-time access: %fs\n", tc);

  t.restart();
  for (long i = 0; i < ITERATIONS; ++i) {
    d = runtime_distance(p1, p2).first;
  }
  double tr = t.elapsed();

  printf("Runtime access: %fs\n", tr);
  
#if DIMENSIONS % 4 == 0
  t.restart();
  for (long i = 0; i < ITERATIONS; ++i) {
    d = runtime_distance_unrolled(p1, p2).first;
  }
  double tu = t.elapsed();

  printf("Runtime access (unrolled): %fs\n", tu);
#endif

  return 0;
}