From 54097c0058bb5ab9975a2fa25fde931861ef4fe4 Mon Sep 17 00:00:00 2001
From: bert hubert <bert.hubert@netherlabs.nl>
Date: Tue, 24 Sep 2019 21:20:48 +0200
Subject: [PATCH] new sp3

---
 sp3.cc | 98 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 sp3.hh | 25 +++++++++++++++
 2 files changed, 123 insertions(+)
 create mode 100644 sp3.cc
 create mode 100644 sp3.hh

diff --git a/sp3.cc b/sp3.cc
new file mode 100644
index 0000000..954b52f
--- /dev/null
+++ b/sp3.cc
@@ -0,0 +1,98 @@
+#include "sp3.hh"
+#include <string>
+#include <string.h>
+#include <stdexcept>
+#include <sstream>
+#include <iostream>
+
+using namespace std;
+
+SP3Reader::SP3Reader(std::string_view fname)
+{
+  d_fp = fopen(&fname[0], "r");
+  if(!d_fp)
+    throw runtime_error("Unable to open "+(string)fname+": "+strerror(errno));
+                               
+}
+
+SP3Reader::~SP3Reader()
+{
+  if(d_fp)
+    fclose(d_fp);
+}
+
+bool SP3Reader::get(SP3Entry& entry)
+{
+  char line[80];
+  struct tm tm;
+
+  while(fgets(line, sizeof(line), d_fp)) {
+    if(line[0]=='*') {
+      //0   1    2  3  4  5  6
+      //*  2019  9 17  1  0  0.00000000
+
+      std::stringstream ss(line);
+      std::string token;
+      int num = 0;
+      memset(&tm, 0, sizeof(tm));
+      while (std::getline(ss, token, ' ')) {
+        if(token.empty())
+          continue;
+        int val = atoi(token.c_str());
+        if(num == 1)
+          tm.tm_year = val - 1900;
+        else if(num==2)
+          tm.tm_mon = val - 1;
+        else if(num == 3)
+          tm.tm_mday = val;
+        else if(num == 4)
+          tm.tm_hour = val;
+        else if(num == 5)
+          tm.tm_min = val;
+        else if(num==6)
+          tm.tm_sec = val;
+
+        num++;
+
+        //        cout<<"Token: "<<token<<endl;
+      }
+      d_time = timegm(&tm) - 18; // XXX leap second
+    }
+    else if(line[0]=='P') {
+      // 0       1                2             3             4
+      // PG01 -18824.158694  -8701.019206  16573.078969   -131.247183
+      std::stringstream ss(line);
+      std::string token;
+      int num = 0;
+
+      while (std::getline(ss, token, ' ')) {
+        if(token.empty())
+          continue;
+        if(!num) {
+          if(token[1]=='G')
+            entry.gnss = 0;
+          if(token[1]=='E')
+            entry.gnss = 2;
+          if(token[1]=='C')
+            entry.gnss = 3;
+          entry.sv = atoi(token.c_str()+2);
+        }
+        double val = atof(token.c_str());
+        if(num == 1)
+          entry.x = 1000.0*val;
+        if(num == 2)
+          entry.y = 1000.0*val;
+        if(num == 3)
+          entry.z = 1000.0*val;
+        if(num == 4)
+          entry.clockBias = 1000.0*val; // want nanoseconds
+        num++;
+      }
+      entry.t = d_time;
+      return true;
+    }
+            
+  }
+  return false;
+  
+}
diff --git a/sp3.hh b/sp3.hh
new file mode 100644
index 0000000..c26f076
--- /dev/null
+++ b/sp3.hh
@@ -0,0 +1,25 @@
+#pragma once
+#include <string>
+#include <time.h>
+#include <stdio.h>
+
+struct SP3Entry
+{
+  int gnss;
+  int sv;
+  time_t t;
+  double x, y, z;
+  double clockBias;
+};
+
+class SP3Reader
+{
+public:
+  SP3Reader(std::string_view fname);
+  bool get(SP3Entry& sp3);
+  ~SP3Reader();
+private:
+  std::string fname;
+  FILE* d_fp{0};
+  time_t d_time{0};
+};