Accommodate n-Dimensional HR coordinates

refs: #3751

Change-Id: Ib705b671daba56f58e09876a48d8b31649bd7ab1
diff --git a/src/conf-file-processor.cpp b/src/conf-file-processor.cpp
index 211609a..789efc4 100644
--- a/src/conf-file-processor.cpp
+++ b/src/conf-file-processor.cpp
@@ -528,17 +528,32 @@
   }
 
   try {
-    /* Radius and angle is mandatory configuration parameter in hyperbolic section.
+    /* Radius and angle(s) are mandatory configuration parameters in hyperbolic section.
      * Even if router can have hyperbolic routing calculation off but other router
      * in the network may use hyperbolic routing calculation for FIB generation.
      * So each router need to advertise its hyperbolic coordinates in the network
      */
     double radius = section.get<double>("radius");
-    double angle = section.get<double>("angle");
+    std::string angleString = section.get<std::string>("angle");
+
+    std::stringstream ss(angleString);
+    std::vector<double> angles;
+
+    double angle;
+
+    while (ss >> angle)
+    {
+      angles.push_back(angle);
+      if (ss.peek() == ',' || ss.peek() == ' ')
+      {
+        ss.ignore();
+      }
+    }
+
     if (!m_nlsr.getConfParameter().setCorR(radius)) {
       return false;
     }
-    m_nlsr.getConfParameter().setCorTheta(angle);
+    m_nlsr.getConfParameter().setCorTheta(angles);
   }
   catch (const std::exception& ex) {
     std::cerr << ex.what() << std::endl;
diff --git a/src/conf-parameter.cpp b/src/conf-parameter.cpp
index 8498615..e19fe5e 100644
--- a/src/conf-parameter.cpp
+++ b/src/conf-parameter.cpp
@@ -47,7 +47,10 @@
   _LOG_INFO("Max Faces Per Prefix: " << m_maxFacesPerPrefix);
   _LOG_INFO("Hyperbolic Routing: " << m_hyperbolicState);
   _LOG_INFO("Hyp R: " << m_corR);
-  _LOG_INFO("Hyp theta: " << m_corTheta);
+  int i=0;
+  for (auto const& value: m_corTheta) {
+    _LOG_INFO("Hyp Angle " << i++ << ": "<< value);
+  }
   _LOG_INFO("Log Directory: " << m_logDir);
   _LOG_INFO("Seq Directory: " << m_seqFileDir);
 
diff --git a/src/conf-parameter.hpp b/src/conf-parameter.hpp
index c3573bb..9bcd95c 100644
--- a/src/conf-parameter.hpp
+++ b/src/conf-parameter.hpp
@@ -110,7 +110,6 @@
     , m_infoInterestInterval(HELLO_INTERVAL_DEFAULT)
     , m_hyperbolicState(HYPERBOLIC_STATE_OFF)
     , m_corR(0)
-    , m_corTheta(0)
     , m_maxFacesPerPrefix(MAX_FACES_PER_PREFIX_MIN)
     , m_isLog4cxxConfAvailable(false)
   {
@@ -338,12 +337,12 @@
   }
 
   void
-  setCorTheta(double ct)
+  setCorTheta(const std::vector<double>& ct)
   {
     m_corTheta = ct;
   }
 
-  double
+  std::vector<double>
   getCorTheta() const
   {
     return m_corTheta;
@@ -435,7 +434,7 @@
 
   int32_t m_hyperbolicState;
   double m_corR;
-  double m_corTheta;
+  std::vector<double> m_corTheta;
 
   uint32_t m_maxFacesPerPrefix;
 
diff --git a/src/lsa.cpp b/src/lsa.cpp
index 78e5837..ad35a81 100644
--- a/src/lsa.cpp
+++ b/src/lsa.cpp
@@ -34,7 +34,6 @@
 #include "adjacent.hpp"
 #include "logger.hpp"
 
-
 namespace nlsr {
 
 INIT_LOGGER("Lsa");
@@ -138,14 +137,14 @@
 
 CoordinateLsa::CoordinateLsa(const ndn::Name& origR, uint32_t lsn,
                              const ndn::time::system_clock::TimePoint& lt,
-                             double r, double theta)
+                             double r, std::vector<double> theta)
   : Lsa(CoordinateLsa::TYPE_STRING)
 {
   m_origRouter = origR;
   m_lsSeqNo = lsn;
   m_expirationTimePoint = lt;
   m_corRad = r;
-  m_corTheta = theta;
+  m_angles = theta;
 }
 
 const ndn::Name
@@ -159,9 +158,18 @@
 bool
 CoordinateLsa::isEqualContent(const CoordinateLsa& clsa)
 {
+  if (clsa.getCorTheta().size() != m_angles.size()) {
+    return false;
+  }
+
+  std::vector<double> m_angles2 = clsa.getCorTheta();
+  for (unsigned int i = 0; i < clsa.getCorTheta().size(); i++) {
+    if (std::abs(m_angles[i] - m_angles2[i]) > std::numeric_limits<double>::epsilon()) {
+      return false;
+    }
+  }
+
   return (std::abs(m_corRad - clsa.getCorRadius()) <
-          std::numeric_limits<double>::epsilon()) &&
-         (std::abs(m_corTheta - clsa.getCorTheta()) <
           std::numeric_limits<double>::epsilon());
 }
 
@@ -171,7 +179,12 @@
   std::ostringstream os;
   os << m_origRouter << "|" << CoordinateLsa::TYPE_STRING << "|" << m_lsSeqNo << "|"
      << ndn::time::toIsoString(m_expirationTimePoint) << "|" << m_corRad << "|"
-     << m_corTheta << "|";
+     << m_angles.size() << "|";
+
+  for (const auto& angle: m_angles) {
+    os << angle << "|";
+  }
+
   return os.str();
 }
 
@@ -186,6 +199,7 @@
   if (!(m_origRouter.size() > 0)) {
     return false;
   }
+
   try {
     if (*tok_iter++ != CoordinateLsa::TYPE_STRING) {
       return false;
@@ -194,7 +208,11 @@
     m_lsSeqNo  = boost::lexical_cast<uint32_t>(*tok_iter++);
     m_expirationTimePoint = ndn::time::fromIsoString(*tok_iter++);
     m_corRad   = boost::lexical_cast<double>(*tok_iter++);
-    m_corTheta = boost::lexical_cast<double>(*tok_iter++);
+    int numAngles = boost::lexical_cast<uint32_t>(*tok_iter++);
+
+   for (int i = 0; i < numAngles; i++) {
+     m_angles.push_back(boost::lexical_cast<double>(*tok_iter++));
+   }
   }
   catch (const std::exception& e) {
     _LOG_ERROR(e.what());
@@ -212,7 +230,10 @@
   _LOG_DEBUG("  Ls Seq No: " << m_lsSeqNo);
   _LOG_DEBUG("  Ls Lifetime: " << m_expirationTimePoint);
   _LOG_DEBUG("    Hyperbolic Radius: " << m_corRad);
-  _LOG_DEBUG("    Hyperbolic Theta: " << m_corTheta);
+  int i = 0;
+  for(auto const& value: m_angles) {
+    _LOG_DEBUG("    Hyperbolic Theta " << i++ << ": "<< value);
+  }
 }
 
 AdjLsa::AdjLsa(const ndn::Name& origR, uint32_t lsn,
diff --git a/src/lsa.hpp b/src/lsa.hpp
index 9d4f89f..4840c34 100644
--- a/src/lsa.hpp
+++ b/src/lsa.hpp
@@ -282,13 +282,12 @@
   CoordinateLsa()
     : Lsa(CoordinateLsa::TYPE_STRING)
     , m_corRad(0)
-    , m_corTheta(0)
   {
   }
 
   CoordinateLsa(const ndn::Name& origR, uint32_t lsn,
                 const ndn::time::system_clock::TimePoint& lt,
-                double r, double theta);
+                double r, std::vector<double> theta);
 
   const ndn::Name
   getKey() const;
@@ -324,16 +323,16 @@
       m_corRad = cr;
   }
 
-  double
+  const std::vector<double>
   getCorTheta() const
   {
-    return m_corTheta;
+    return m_angles;
   }
 
   void
-  setCorTheta(double ct)
+  setCorTheta(std::vector<double> ct)
   {
-    m_corTheta = ct;
+    m_angles = ct;
   }
 
   bool
@@ -344,7 +343,7 @@
 
 private:
   double m_corRad;
-  double m_corTheta;
+  std::vector<double> m_angles;
 
 public:
   static const std::string TYPE_STRING;
diff --git a/src/route/routing-table-calculator.cpp b/src/route/routing-table-calculator.cpp
index 54d486d..7aa6f48 100644
--- a/src/route/routing-table-calculator.cpp
+++ b/src/route/routing-table-calculator.cpp
@@ -483,37 +483,125 @@
     return UNKNOWN_DISTANCE;
   }
 
-  double srcTheta = srcLsa->getCorTheta();
-  double destTheta = destLsa->getCorTheta();
-
-  double diffTheta = fabs(srcTheta - destTheta);
-
-  if (diffTheta > MATH_PI) {
-    diffTheta = 2 * MATH_PI - diffTheta;
-  }
+  std::vector<double> srcTheta = srcLsa->getCorTheta();
+  std::vector<double> destTheta = destLsa->getCorTheta();
 
   double srcRadius = srcLsa->getCorRadius();
   double destRadius = destLsa->getCorRadius();
 
-  if (srcRadius == UNKNOWN_RADIUS && destRadius == UNKNOWN_RADIUS) {
+  double diffTheta = calculateAngularDistance(srcTheta, destTheta);
+
+  if (srcRadius == UNKNOWN_RADIUS || destRadius == UNKNOWN_RADIUS ||
+      diffTheta == UNKNOWN_DISTANCE) {
     return UNKNOWN_DISTANCE;
   }
 
-  if (diffTheta == 0) {
-    distance = fabs(srcRadius - destRadius);
-  }
-  else {
-    distance = acosh((cosh(srcRadius) * cosh(destRadius)) -
-                     (sinh(srcRadius) * sinh(destRadius) * cos(diffTheta)));
-  }
+  // double r_i, double r_j, double delta_theta, double zeta = 1 (default)
+  distance = calculateHyperbolicDistance(srcRadius, destRadius, diffTheta);
 
   _LOG_TRACE("Distance from " << src << " to " << dest << " is " << distance);
 
   return distance;
 }
 
-void HyperbolicRoutingCalculator::addNextHop(ndn::Name dest, std::string faceUri,
-                                             double cost, RoutingTable& rt)
+double
+HyperbolicRoutingCalculator::calculateAngularDistance(std::vector<double> angleVectorI,
+                                                      std::vector<double> angleVectorJ)
+{
+  // It is not possible for angle vector size to be zero as ensured by conf-file-processor
+
+  // https://en.wikipedia.org/wiki/N-sphere#Spherical_coordinates
+
+  // Check if two vector lengths are the same
+  if (angleVectorI.size() != angleVectorJ.size()) {
+    _LOG_ERROR("Angle vector sizes do not match");
+    return UNKNOWN_DISTANCE;
+  }
+
+  // Check if all angles are within the [0, PI] and [0, 2PI] ranges
+  if (angleVectorI.size() > 1) {
+    for (unsigned int k = 0; k < angleVectorI.size() - 1; k++) {
+      if ((angleVectorI[k] > M_PI && angleVectorI[k] < 0.0) ||
+          (angleVectorJ[k] > M_PI && angleVectorJ[k] < 0.0)) {
+        _LOG_ERROR("Angle outside [0, PI]");
+        return UNKNOWN_DISTANCE;
+      }
+    }
+  }
+  if (angleVectorI[angleVectorI.size()-1] > 2.*M_PI ||
+      angleVectorI[angleVectorI.size()-1] < 0.0) {
+    _LOG_ERROR("Angle not within [0, 2PI]");
+    return UNKNOWN_DISTANCE;
+  }
+
+  if (angleVectorI[angleVectorI.size()-1] > 2.*M_PI ||
+      angleVectorI[angleVectorI.size()-1] < 0.0) {
+    _LOG_ERROR("Angle not within [0, 2PI]");
+    return UNKNOWN_DISTANCE;
+  }
+
+  // deltaTheta = arccos(vectorI . vectorJ) -> do the inner product
+  double innerProduct = 0.0;
+
+  // Calculate x0 of the vectors
+  double x0i = std::cos(angleVectorI[0]);
+  double x0j = std::cos(angleVectorJ[0]);
+
+  // Calculate xn of the vectors
+  double xni = std::sin(angleVectorI[angleVectorI.size() - 1]);
+  double xnj = std::sin(angleVectorJ[angleVectorJ.size() - 1]);
+
+  // Do the aggregation of the (n-1) coordinates (if there is more than one angle)
+  // i.e contraction of all (n-1)-dimensional angular coordinates to one variable
+  for (unsigned int k = 0; k < angleVectorI.size() - 1; k++) {
+    xni *= std::sin(angleVectorI[k]);
+    xnj *= std::sin(angleVectorJ[k]);
+  }
+  innerProduct += (x0i * x0j) + (xni * xnj);
+
+  // If d > 1
+  if (angleVectorI.size() > 1) {
+    for (unsigned int m = 1; m < angleVectorI.size(); m++) {
+      // calculate euclidean coordinates given the angles and assuming R_sphere = 1
+      double xmi = std::cos(angleVectorI[m]);
+      double xmj = std::cos(angleVectorJ[m]);
+      for (unsigned int l = 0; l < m; l++) {
+        xmi *= std::sin(angleVectorI[l]);
+        xmj *= std::sin(angleVectorJ[l]);
+      }
+      innerProduct += xmi * xmj;
+    }
+  }
+
+  // ArcCos of the inner product gives the angular distance
+  // between two points on a d-dimensional sphere
+  return std::acos(innerProduct);
+}
+
+double
+HyperbolicRoutingCalculator::calculateHyperbolicDistance(double rI, double rJ,
+                                                         double deltaTheta)
+{
+  if (deltaTheta == UNKNOWN_DISTANCE) {
+    return UNKNOWN_DISTANCE;
+  }
+
+  // Usually, we set zeta = 1 in all experiments
+  double zeta = 1;
+
+  if (deltaTheta <= 0.0 || rI <= 0.0 || rJ <= 0.0) {
+    _LOG_ERROR("Delta theta or rI or rJ is <= 0");
+    return UNKNOWN_DISTANCE;
+  }
+
+  double xij = (1. / zeta) * std::acosh(std::cosh(zeta*rI) * std::cosh(zeta*rJ) -
+               std::sinh(zeta*rI)*std::sinh(zeta*rJ)*std::cos(deltaTheta));
+  return xij;
+}
+
+void
+HyperbolicRoutingCalculator::addNextHop(ndn::Name dest, std::string faceUri,
+                                        double cost, RoutingTable& rt)
 {
   NextHop hop(faceUri, cost);
   hop.setHyperbolic(true);
diff --git a/src/route/routing-table-calculator.hpp b/src/route/routing-table-calculator.hpp
index 680ef52..6c6ab18 100644
--- a/src/route/routing-table-calculator.hpp
+++ b/src/route/routing-table-calculator.hpp
@@ -223,6 +223,13 @@
   void
   addNextHop(ndn::Name destinationRouter, std::string faceUri, double cost, RoutingTable& rt);
 
+  double
+  calculateHyperbolicDistance(double rI, double rJ, double deltaTheta);
+
+  double
+  calculateAngularDistance(std::vector<double> angleVectorI,
+                           std::vector<double> angleVectorJ);
+
 private:
   const size_t m_nRouters;
   const bool m_isDryRun;
@@ -236,4 +243,4 @@
 
 } // namespace nlsr
 
-#endif //NLSR_ROUTING_TABLE_CALCULATOR_HPP
+#endif // NLSR_ROUTING_TABLE_CALCULATOR_HPP
diff --git a/src/tlv/coordinate-lsa.cpp b/src/tlv/coordinate-lsa.cpp
index cb32d04..92809fe 100644
--- a/src/tlv/coordinate-lsa.cpp
+++ b/src/tlv/coordinate-lsa.cpp
@@ -24,9 +24,12 @@
 
 #include <ndn-cxx/util/concepts.hpp>
 #include <ndn-cxx/encoding/block-helpers.hpp>
+#include "logger.hpp"
 
 namespace nlsr {
-namespace tlv  {
+namespace tlv {
+
+INIT_LOGGER("CoordinateLsa");
 
 BOOST_CONCEPT_ASSERT((ndn::WireEncodable<CoordinateLsa>));
 BOOST_CONCEPT_ASSERT((ndn::WireDecodable<CoordinateLsa>));
@@ -35,7 +38,6 @@
 
 CoordinateLsa::CoordinateLsa()
   : m_hyperbolicRadius(0.0)
-  , m_hyperbolicAngle(0.0)
 {
 }
 
@@ -51,10 +53,14 @@
   size_t totalLength = 0;
   size_t doubleLength = 10;
 
-  const uint8_t* doubleBytes1 = reinterpret_cast<const uint8_t*>(&m_hyperbolicAngle);
-  totalLength += block.prependByteArrayBlock(ndn::tlv::nlsr::Double, doubleBytes1, 8);
-  totalLength += block.prependVarNumber(doubleLength);
-  totalLength += block.prependVarNumber(ndn::tlv::nlsr::HyperbolicAngle);
+  const uint8_t* doubleBytes1;
+  for (auto it = m_hyperbolicAngle.rbegin(); it != m_hyperbolicAngle.rend(); ++it) {
+    doubleBytes1 = reinterpret_cast<const uint8_t*>(&*it);
+
+    totalLength += block.prependByteArrayBlock(ndn::tlv::nlsr::Double, doubleBytes1, 8);
+    totalLength += block.prependVarNumber(doubleLength);
+    totalLength += block.prependVarNumber(ndn::tlv::nlsr::HyperbolicAngle);
+  }
 
   const uint8_t* doubleBytes2 = reinterpret_cast<const uint8_t*>(&m_hyperbolicRadius);
   totalLength += block.prependByteArrayBlock(ndn::tlv::nlsr::Double, doubleBytes2, 8);
@@ -97,7 +103,7 @@
 CoordinateLsa::wireDecode(const ndn::Block& wire)
 {
   m_hyperbolicRadius = 0.0;
-  m_hyperbolicAngle = 0.0;
+  m_hyperbolicAngle.clear();
 
   m_wire = wire;
 
@@ -105,7 +111,7 @@
     std::stringstream error;
     error << "Expected CoordinateLsa Block, but Block is of a different type: #"
           << m_wire.type();
-    throw Error(error.str());
+    BOOST_THROW_EXCEPTION(Error(error.str()));
   }
 
   m_wire.parse();
@@ -117,7 +123,8 @@
     ++val;
   }
   else {
-    throw Error("Missing required LsaInfo field");
+    std::cout << "Missing required LsaInfo field" << std::endl;
+    BOOST_THROW_EXCEPTION(Error("Missing required LsaInfo field"));
   }
 
   if (val != m_wire.elements_end() && val->type() == ndn::tlv::nlsr::HyperbolicRadius) {
@@ -125,31 +132,33 @@
     ndn::Block::element_const_iterator it = val->elements_begin();
     if (it != val->elements_end() && it->type() == ndn::tlv::nlsr::Double) {
       m_hyperbolicRadius = *reinterpret_cast<const double*>(it->value());
+
+      ++val;
     }
     else {
-      throw Error("HyperbolicRadius: Missing required Double field");
+      std::cout << "HyperbolicRadius: Missing required Double field" << std::endl;
+      BOOST_THROW_EXCEPTION(Error("HyperbolicRadius: Missing required Double field"));
     }
-
-    ++val;
   }
   else {
-    throw Error("Missing required HyperbolicRadius field");
+    std::cout << "Missing required HyperbolicRadius field" << std::endl;
+    BOOST_THROW_EXCEPTION(Error("Missing required HyperbolicRadius field"));
   }
 
-  if (val != m_wire.elements_end() && val->type() == ndn::tlv::nlsr::HyperbolicAngle) {
-    val->parse();
-    ndn::Block::element_const_iterator it = val->elements_begin();
-    if (it != val->elements_end() && it->type() == ndn::tlv::nlsr::Double) {
-      m_hyperbolicAngle = *reinterpret_cast<const double*>(it->value());
-    }
-    else {
-      throw Error("HyperbolicAngle: Missing required Double field");
-    }
+  for (; val != m_wire.elements_end(); ++val) {
+    if (val->type() == ndn::tlv::nlsr::HyperbolicAngle) {
+      val->parse();
 
-    ++val;
-  }
-  else {
-    throw Error("Missing required HyperbolicAngle field");
+      for (auto it = val->elements_begin(); it != val->elements_end(); ++it) {
+        if (it->type() == ndn::tlv::nlsr::Double) {
+          m_hyperbolicAngle.push_back(*reinterpret_cast<const double*>(it->value()));
+        }
+        else {
+          std::cout << "HyperbolicAngle: Missing required Double field" << std::endl;
+          BOOST_THROW_EXCEPTION(Error("HyperbolicAngle: Missing required Double field"));
+        }
+      }
+    }
   }
 }
 
@@ -158,8 +167,20 @@
 {
   os << "CoordinateLsa("
      << coordinateLsa.getLsaInfo() << ", "
-     << "HyperbolicRadius: " << coordinateLsa.getHyperbolicRadius() << ", "
-     << "HyperbolicAngle: " << coordinateLsa.getHyperbolicAngle() << ")";
+     << "HyperbolicRadius: " << coordinateLsa.getHyperbolicRadius() << ", ";
+
+  os << "HyperbolicAngles: ";
+  int i = 0;
+  for (const auto& value: coordinateLsa.getHyperbolicAngle()) {
+    if (i == 0) {
+      os << value;
+    }
+    else {
+      os << ", " << value;
+    }
+    ++i;
+  }
+  os << ")";
 
   return os;
 }
diff --git a/src/tlv/coordinate-lsa.hpp b/src/tlv/coordinate-lsa.hpp
index 2e5d813..0568cc1 100644
--- a/src/tlv/coordinate-lsa.hpp
+++ b/src/tlv/coordinate-lsa.hpp
@@ -39,9 +39,9 @@
    CoordinateLsa := COORDINATE-LSA-TYPE TLV-LENGTH
                       LsaInfo
                       HyperbolicRadius
-                      HyperbolicAngle
+                      HyperbolicAngle+
 
-   \sa http://redmine.named-data.net/projects/nlsr/wiki/LSDB_DataSet
+   \sa https://redmine.named-data.net/projects/nlsr/wiki/LSDB_DataSet
  */
 class CoordinateLsa
 {
@@ -89,14 +89,14 @@
     return *this;
   }
 
-  double
+  const std::vector<double>
   getHyperbolicAngle() const
   {
     return m_hyperbolicAngle;
   }
 
   CoordinateLsa&
-  setHyperbolicAngle(double hyperbolicAngle)
+  setHyperbolicAngle(const std::vector<double>& hyperbolicAngle)
   {
     m_hyperbolicAngle = hyperbolicAngle;
     m_wire.reset();
@@ -116,7 +116,7 @@
 private:
   LsaInfo m_lsaInfo;
   double m_hyperbolicRadius;
-  double m_hyperbolicAngle;
+  std::vector<double> m_hyperbolicAngle;
 
   mutable ndn::Block m_wire;
 };