From 20fc2d091c8f0f99dfcd4a5dafeba31cda25b08f Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 4 Jun 2021 13:31:22 +1000
Subject: [PATCH] AP_CSVReader: add simple CSV reader

---
 libraries/AP_CSVReader/AP_CSVReader.cpp       | 102 +++++++++++++
 libraries/AP_CSVReader/AP_CSVReader.h         |  58 +++++++
 .../AP_CSVReader/tests/test_csvreader.cpp     | 142 ++++++++++++++++++
 libraries/AP_CSVReader/tests/wscript          |   7 +
 4 files changed, 309 insertions(+)
 create mode 100644 libraries/AP_CSVReader/AP_CSVReader.cpp
 create mode 100644 libraries/AP_CSVReader/AP_CSVReader.h
 create mode 100644 libraries/AP_CSVReader/tests/test_csvreader.cpp
 create mode 100644 libraries/AP_CSVReader/tests/wscript

diff --git a/libraries/AP_CSVReader/AP_CSVReader.cpp b/libraries/AP_CSVReader/AP_CSVReader.cpp
new file mode 100644
index 0000000000..8149ce5de3
--- /dev/null
+++ b/libraries/AP_CSVReader/AP_CSVReader.cpp
@@ -0,0 +1,102 @@
+#include "AP_CSVReader.h"
+
+#include <AP_Common/AP_Common.h>
+
+#include <stdio.h>
+
+AP_CSVReader::RetCode AP_CSVReader::handle_unquoted_term(uint8_t c)
+{
+    if (c == separator) {
+        set_state(State::START_OF_START_OF_TERM);
+        return RetCode::TERM_DONE;
+    }
+    switch (c) {
+    case '\r':
+        set_state(State::END_OF_VECTOR_CR);
+        return RetCode::VECTOR_DONE;
+    case '\n':
+        set_state(State::START_OF_START_OF_TERM);
+        return RetCode::VECTOR_DONE;
+    default:
+        if (term_ofs >= term_len-1) { // -1 for null termination
+            return RetCode::ERROR;
+        }
+        term[term_ofs++] = c;
+        term[term_ofs] = '\0';
+        return RetCode::OK;
+    }
+}
+
+AP_CSVReader::RetCode AP_CSVReader::handle_quoted_term(uint8_t c)
+{
+    if (c == '"') {
+        set_state(State::END_OF_QUOTED_TERM);
+        return RetCode::OK;
+    }
+    if (state == State::END_OF_QUOTED_TERM) {
+        if (c == separator) {
+            set_state(State::START_OF_START_OF_TERM);
+            return RetCode::TERM_DONE;
+        }
+
+        switch (c) {
+        case '\r':
+            set_state(State::END_OF_VECTOR_CR);
+            return RetCode::VECTOR_DONE;
+        case '\n':
+            set_state(State::START_OF_START_OF_TERM);
+            return RetCode::VECTOR_DONE;
+        }
+        return RetCode::ERROR;
+    }
+
+    // still within the quoted term, append to current value
+    if (term_ofs >= term_len-1) { // -1 for null termination
+        return RetCode::ERROR;
+    }
+    term[term_ofs++] = c;
+    term[term_ofs] = '\0';
+    return RetCode::OK;
+}
+
+AP_CSVReader::RetCode AP_CSVReader::feed(uint8_t c)
+{
+    if (term_len == 0) {
+        return RetCode::ERROR;
+    }
+
+again:
+    switch (state) {
+    case State::START_OF_START_OF_TERM:
+        term_ofs = 0;
+        term[term_ofs] = '\0';
+        state = State::START_OF_TERM;
+        FALLTHROUGH;
+    case State::START_OF_TERM:
+        // if (c == '"') {
+        //     set_state(State::START_OF_QUOTED_TERM);
+        //     return RetCode::OK;
+        // }
+        if (c == '"') {
+            set_state(State::IN_QUOTED_TERM);
+            return RetCode::OK;
+        } else {
+            set_state(State::IN_UNQUOTED_TERM);
+            return handle_unquoted_term(c);
+        }
+    case State::END_OF_VECTOR_CR:
+        if (c == '\n') {
+            set_state(State::START_OF_START_OF_TERM);
+            return RetCode::OK;
+        }
+        set_state(State::START_OF_START_OF_TERM);
+        goto again;
+    case State::IN_UNQUOTED_TERM:
+        return handle_unquoted_term(c);
+    case State::IN_QUOTED_TERM:
+    case State::END_OF_QUOTED_TERM:
+        return handle_quoted_term(c);
+    }
+
+    return RetCode::ERROR;
+}
diff --git a/libraries/AP_CSVReader/AP_CSVReader.h b/libraries/AP_CSVReader/AP_CSVReader.h
new file mode 100644
index 0000000000..e36ca3d541
--- /dev/null
+++ b/libraries/AP_CSVReader/AP_CSVReader.h
@@ -0,0 +1,58 @@
+#pragma once
+
+// Note: term is always null-terminated so a final line with no cr/lf
+// on it can still be fetched by the caller
+
+#include <stdint.h>
+
+class AP_CSVReader
+{
+
+public:
+
+    AP_CSVReader(uint8_t *_term, uint8_t _term_len, uint8_t _separator=',') :
+        separator{_separator},
+        term{_term},
+        term_len{_term_len}
+        {}
+
+    enum class RetCode : uint8_t {
+        OK,
+        ERROR,
+        TERM_DONE,
+        VECTOR_DONE,
+    };
+
+    RetCode feed(uint8_t c);
+//    RetCode feed(const uint8_t *buffer, uint8_t len);
+
+private:
+
+    enum class State : uint8_t {
+        START_OF_START_OF_TERM = 46,
+        START_OF_TERM = 47,
+        END_OF_VECTOR_CR = 48,
+        IN_UNQUOTED_TERM = 49,
+        IN_QUOTED_TERM = 50,
+        END_OF_QUOTED_TERM = 51,
+    } state = State::START_OF_START_OF_TERM;
+
+    // term separator
+    const uint8_t separator;
+
+    // pointer to memory where term will be assembled
+    uint8_t *term;
+
+    // amount of memory term points to
+    const uint8_t term_len;
+
+    // offset into term for next character
+    uint8_t term_ofs;
+
+    void set_state(State newstate) {
+        state = newstate;
+    }
+
+    AP_CSVReader::RetCode handle_unquoted_term(uint8_t c);
+    AP_CSVReader::RetCode handle_quoted_term(uint8_t c);
+};
diff --git a/libraries/AP_CSVReader/tests/test_csvreader.cpp b/libraries/AP_CSVReader/tests/test_csvreader.cpp
new file mode 100644
index 0000000000..c7469ec0d2
--- /dev/null
+++ b/libraries/AP_CSVReader/tests/test_csvreader.cpp
@@ -0,0 +1,142 @@
+#include <AP_gtest.h>
+#include <AP_Common/AP_Common.h>
+
+#include <AP_Math/AP_Math.h>
+
+#include <AP_CSVReader/AP_CSVReader.h>
+
+TEST(AP_CSVReader, basic)
+{
+    static const char *basic_csv =
+        "A 1\n"
+        "B 2\n"
+        "C 3\n"
+        "Fred 31\n"
+        ;
+    static const char *basic_csv_crlf =
+        "A 1\r\n"
+        "B 2\r\n"
+        "C 3\r\n"
+        "Fred 31\r\n"
+        ;
+    static const char *basic_csv_results[][2] = {
+        {"A", "1"},
+        {"B", "2"},
+        {"C", "3"},
+        {"Fred", "31"}
+    };
+
+    uint8_t term[16];
+    AP_CSVReader csvreader{term, ARRAY_SIZE(term), ' '};
+
+    const char *csvs[] {
+        basic_csv,
+        basic_csv_crlf
+    };
+
+    for (const char *csv : csvs) {
+        uint8_t termcount = 0;
+        uint8_t linecount = 0;
+        for (uint8_t i=0; i<strlen(csv); i++) {
+            switch (csvreader.feed(csv[i])) {
+            case AP_CSVReader::RetCode::ERROR:
+                abort();
+            case AP_CSVReader::RetCode::OK:
+                continue;
+            case AP_CSVReader::RetCode::TERM_DONE:
+                EXPECT_STREQ(basic_csv_results[linecount][termcount], (char*)term);
+                termcount++;
+                continue;
+            case AP_CSVReader::RetCode::VECTOR_DONE:
+                EXPECT_STREQ(basic_csv_results[linecount][termcount], (char*)term);
+                termcount++;
+                EXPECT_EQ(termcount, 2);
+                termcount = 0;
+                linecount++;
+                continue;
+            }
+        }
+
+        EXPECT_EQ(linecount, 4);
+        EXPECT_EQ(termcount, 0);
+    }
+}
+
+TEST(AP_CSVReader, commabasic)
+{
+    static const char *basic_csv =
+        "A,1\n"
+        "B,2\n"
+        "C,3\n"
+        "Fred,31\n"
+        ;
+    static const char *basic_csv_results[][2] = {
+        {"A", "1"},
+        {"B", "2"},
+        {"C", "3"},
+        {"Fred", "31"}
+    };
+
+    uint8_t term[16];
+    AP_CSVReader csvreader{term, ARRAY_SIZE(term), ','};
+
+    uint8_t termcount = 0;
+    uint8_t linecount = 0;
+    for (uint8_t i=0; i<strlen(basic_csv); i++) {
+        switch (csvreader.feed(basic_csv[i])) {
+        case AP_CSVReader::RetCode::ERROR:
+            abort();
+        case AP_CSVReader::RetCode::OK:
+            continue;
+        case AP_CSVReader::RetCode::TERM_DONE:
+            EXPECT_STREQ(basic_csv_results[linecount][termcount], (char*)term);
+            termcount++;
+            continue;
+        case AP_CSVReader::RetCode::VECTOR_DONE:
+            EXPECT_STREQ(basic_csv_results[linecount][termcount], (char*)term);
+            termcount++;
+            EXPECT_EQ(termcount, 2);
+            termcount = 0;
+            linecount++;
+            continue;
+        }
+    }
+
+    EXPECT_EQ(linecount, 4);
+    EXPECT_EQ(termcount, 0);
+}
+
+TEST(AP_CSVReader, missinglastcr)
+{
+    static const char *basic_csv =
+        "A,1"
+        ;
+    uint8_t term[16];
+    AP_CSVReader csvreader{term, ARRAY_SIZE(term), ','};
+
+    uint8_t termcount = 0;
+    uint8_t linecount = 0;
+    for (uint8_t i=0; i<strlen(basic_csv); i++) {
+        switch (csvreader.feed(basic_csv[i])) {
+        case AP_CSVReader::RetCode::ERROR:
+            abort();
+        case AP_CSVReader::RetCode::OK:
+            continue;
+        case AP_CSVReader::RetCode::TERM_DONE:
+            if (linecount == 0 && termcount == 0) {
+                EXPECT_STREQ((char*)term, "A");
+            }
+            termcount++;
+            continue;
+        case AP_CSVReader::RetCode::VECTOR_DONE:
+            abort();
+        }
+    }
+
+    EXPECT_STREQ((char*)term, "1");
+}
+
+AP_GTEST_MAIN()
+
+
+int hal = 0; // bizarrely, this fixes an undefined-symbol error but doesn't raise a type exception.  Yay.
diff --git a/libraries/AP_CSVReader/tests/wscript b/libraries/AP_CSVReader/tests/wscript
new file mode 100644
index 0000000000..cd3e5e3ce7
--- /dev/null
+++ b/libraries/AP_CSVReader/tests/wscript
@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    bld.ap_find_tests(
+        use='ap',
+    )