Resolve "navigation_tests"

Closes #7 (closed)

include/gnss_utils/navigation.h

@@ -5,6 +5,8 @@
 #include <iostream>
 #include <memory>
 
+#include "gnss_utils/utils/utils.h"
+
 extern "C" {
 #include "rtklib.h"
 }
@@ -67,12 +69,6 @@ public:
   void print();
 
   //////////////////////////////// nav UTILS //////////////////////////////////////
-  template <typename T>
-  static bool addToArray(const T& new_element, T*& array, int& n, int& nmax);
-  template <typename T>
-  static bool copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out);
-  template <typename T>
-  static void freeArray(T*& array, int& n, int& nmax);
   static void freeEph(nav_t& nav);
   static void freeGeph(nav_t& nav);
   static void freeSeph(nav_t& nav);
@@ -247,67 +243,6 @@ inline void Navigation::freeAlmanac()
 }
 
 //////////////////////////////// nav UTILS //////////////////////////////////////
-template <typename T>
-inline bool Navigation::addToArray(const T& new_element, T*& array, int& n, int& nmax)
-{
-  // std::cout << "addToArray: n = " << n << " nmax = " << nmax << "\n";
-  // "inspired" from RTKLIB rinex.c
-  T* array_ref;
-  if (nmax <= n)
-  {
-    // std::cout << "addToArray: nmax <= n\n";
-    nmax += 1024;
-    if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax)))
-    {
-      printf("addToArray malloc error: n=%d\n", nmax);
-      free(array);
-      array = NULL;
-      n = nmax = 0;
-      return false;
-    }
-    // std::cout << "addToArray: assigning reallocated array\n";
-    array = array_ref;
-  }
-  // std::cout << "addToArray: adding element " << n << "\n";
-  array[n++] = new_element;
-  // std::cout << "addToArray: added!\n";
-  return true;
-}
-
-template <typename T>
-inline bool Navigation::copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out)
-{
-  // std::cout << "copyArray: " << n_in << " elements\n";
-  if (array_in == NULL)
-    return false;
-
-  // std::cout << "copyArray: array in not null\n";
-
-  for (int i = 0; i < n_in; i++)
-  {
-    // std::cout << "copyArray: adding element " << i << "\n";
-    if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
-    {
-      // std::cout << "copyArray: failed to add..\n";
-      return false;
-    }
-    // std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
-  }
-
-  // std::cout << "copyArray: all copied\n";
-
-  return true;
-}
-
-template <typename T>
-void Navigation::freeArray(T*& array, int& n, int& nmax)
-{
-  if (array != NULL)
-    free(array);
-  array = NULL;
-  n = nmax = 0;
-}
-
 inline void Navigation::freeEph(nav_t& nav)
 {
   freeArray<eph_t>(nav.eph, nav.n, nav.nmax);