From 36cf7c13cbc38f228657d588e98ca0c8b16050af Mon Sep 17 00:00:00 2001
From: Matteo Cypriani <matteo.cypriani@uqat.ca>
Date: Wed, 14 May 2014 15:55:05 -0400
Subject: [PATCH] [Positioner] Add Stock::distance_from_closest_cp()

---
 owlps-positioner/stock.cc | 27 +++++++++++++++++++++++++++
 owlps-positioner/stock.hh |  2 ++
 2 files changed, 29 insertions(+)

diff --git a/owlps-positioner/stock.cc b/owlps-positioner/stock.cc
index d6c649d..39029d0 100644
--- a/owlps-positioner/stock.cc
+++ b/owlps-positioner/stock.cc
@@ -21,6 +21,7 @@
 #include "autocalibrationline.hh"
 
 #include <iostream>
+#include <cmath>
 
 using namespace std ;
 
@@ -335,6 +336,32 @@ const CapturePoint* Stock::is_cp_coordinate(const Point3D &coord)
 }
 
 
+/**
+ * @returns The distance from `location` to the closest CP.
+ * @returns -1 if there are no CPs in stock.
+ */
+float Stock::distance_from_closest_cp(const Point3D &location)
+{
+  auto cp = cps.begin() ;
+  if (cp == cps.end())
+    return -1 ;
+
+  float min_dist = location.square_distance(cp->second.get_coordinates()) ;
+  ++cp ;
+
+  while (cp != cps.end())
+    {
+      float tmp_dist =
+        location.square_distance(cp->second.get_coordinates()) ;
+      if (tmp_dist < min_dist)
+        min_dist = tmp_dist ;
+      ++cp ;
+    }
+
+  return sqrtf(min_dist) ;
+}
+
+
 
 /* *** ReferencePoint operations *** */
 
diff --git a/owlps-positioner/stock.hh b/owlps-positioner/stock.hh
index 8c1a9bb..a7ceaa4 100644
--- a/owlps-positioner/stock.hh
+++ b/owlps-positioner/stock.hh
@@ -156,6 +156,8 @@ public:
                                  const std::string &mac_receiver) ;
   /// Checks if a Point3D is the coordinate of an existing CP
   static const CapturePoint* is_cp_coordinate(const Point3D &coord) ;
+  /// Returns the distance from `location` to the closest CP
+  static float distance_from_closest_cp(const Point3D &location) ;
   //@}
 
   /** @name ReferencePoint operations */