From f350174fd8e7450793e4fd87b679b0c26ba0a0ab Mon Sep 17 00:00:00 2001
From: Antonio Ramos <antonio.ramosdet@gmail.com>
Date: Tue, 10 Apr 2018 15:37:07 +0200
Subject: [PATCH] Fix bugs

---
 src/algorithms/libs/gnss_circular_deque.h     | 53 ++++++++++++++-----
 .../gnuradio_blocks/hybrid_observables_cc.cc  | 17 ++----
 2 files changed, 44 insertions(+), 26 deletions(-)

diff --git a/src/algorithms/libs/gnss_circular_deque.h b/src/algorithms/libs/gnss_circular_deque.h
index f453be3d2..12a8e64c0 100644
--- a/src/algorithms/libs/gnss_circular_deque.h
+++ b/src/algorithms/libs/gnss_circular_deque.h
@@ -33,6 +33,7 @@
 
 #ifndef GNSS_SDR_CIRCULAR_DEQUE_H_
 #define GNSS_SDR_CIRCULAR_DEQUE_H_
+#include <exception>
 
 template <class T>
 class Gnss_circular_deque
@@ -89,6 +90,10 @@ Gnss_circular_deque<T>::Gnss_circular_deque(const unsigned int max_size, const u
                     d_index_pop[i] = 0;
                     d_index_push[i] = 0;
                     d_history[i] = new T[d_max_size];
+                    for (unsigned int ii = 0; ii < d_max_size; ii++)
+                        {
+                            d_history[i][ii] = d_return_void;
+                        }
                 }
         }
 }
@@ -127,12 +132,14 @@ T& Gnss_circular_deque<T>::back(const unsigned int ch)
                 }
             else
                 {
-                    index = d_max_size;
+                    index = d_max_size - 1;
                 }
             return d_history[ch][index];
         }
     else
         {
+            std::exception ex;
+            throw ex;
             return d_return_void;
         }
 }
@@ -146,6 +153,8 @@ T& Gnss_circular_deque<T>::front(const unsigned int ch)
         }
     else
         {
+            std::exception ex;
+            throw ex;
             return d_return_void;
         }
 }
@@ -155,11 +164,13 @@ T& Gnss_circular_deque<T>::at(const unsigned int ch, const unsigned int pos)
 {
     if (d_size[ch] > 0 and pos < d_size[ch])
         {
-            unsigned int index = (d_index_pop[ch] + pos) % d_max_size;
+            unsigned int index = ((d_index_pop[ch] + pos) % d_max_size);
             return d_history[ch][index];
         }
     else
         {
+            std::exception ex;
+            throw ex;
             return d_return_void;
         }
 }
@@ -175,31 +186,45 @@ void Gnss_circular_deque<T>::clear(const unsigned int ch)
 template <class T>
 T Gnss_circular_deque<T>::pop_front(const unsigned int ch)
 {
-    T result;
     if (d_size[ch] > 0)
         {
             d_size[ch]--;
-            result = d_history[ch][d_index_pop[ch]];
-            d_index_pop[ch]++;
-            d_index_pop[ch] %= d_max_size;
+            T result = d_history[ch][d_index_pop[ch]];
+            if (d_size[ch] > 0)
+                {
+                    d_index_pop[ch]++;
+                    d_index_pop[ch] %= d_max_size;
+                }
+            else
+                {
+                    clear(ch);
+                }
+            return result;
+        }
+    else
+        {
+            std::exception ex;
+            throw ex;
+            return d_return_void;
         }
-    return result;
 }
 
 template <class T>
 void Gnss_circular_deque<T>::push_back(const unsigned int ch, const T& new_data)
 {
+    bool increment_pop = true;
+    if (d_size[ch] < d_max_size)
+        {
+            increment_pop = false;
+            d_size[ch]++;
+        }
+
     d_history[ch][d_index_push[ch]] = new_data;
     d_index_push[ch]++;
     d_index_push[ch] %= d_max_size;
-    if (d_size[ch] < d_max_size)
+    if (increment_pop)
         {
-            d_size[ch]++;
-        }
-    else
-        {
-            d_index_pop[ch]++;
-            d_index_pop[ch] %= d_max_size;
+            d_index_pop[ch] = d_index_push[ch];
         }
 }
 
diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
index 6a87b156c..603b28c8e 100644
--- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
+++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
@@ -307,6 +307,7 @@ bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, const unsigned i
         }
     std::pair<unsigned int, unsigned int> ind = find_interp_elements(ch, ti);
 
+    //Linear interpolation parameters: y(t) = m * t + c
     double m = 0.0;
     double c = 0.0;
 
@@ -378,8 +379,8 @@ std::pair<unsigned int, unsigned int> hybrid_observables_cc::find_interp_element
     double dt = 0.0;
     for (unsigned int i = 0; i < d_gnss_synchro_history->size(ch); i++)
         {
-            dt = std::fabs(ti - d_gnss_synchro_history->at(ch, i).RX_time);
-            if (dt < dif)
+            dt = ti - d_gnss_synchro_history->at(ch, i).RX_time;
+            if (dt < dif and dt > 0.0)
                 {
                     dif = dt;
                     closest = i;
@@ -399,16 +400,8 @@ std::pair<unsigned int, unsigned int> hybrid_observables_cc::find_interp_element
         }
     else
         {
-            if (d_gnss_synchro_history->at(ch, closest).RX_time < ti)
-                {
-                    index1 = closest;
-                    index2 = closest + 1;
-                }
-            else
-                {
-                    index1 = closest - 1;
-                    index2 = closest;
-                }
+            index1 = closest;
+            index2 = closest + 1;
         }
     return std::pair<unsigned int, unsigned int>(index1, index2);
 }