PentaTrack/commons.hpp

#pragma once

#include <array>
#include <span>
#include <optional>

#include "logging.hpp"

namespace commons {

template<typename LockType>
class lock
{
public:
  ~lock() { locker.unlock(); }

  static std::optional<lock> get(LockType& l)
  {
    if(!l.lock())
    {
      log::debug("Could not acquire lock.");
      return std::nullopt;
    }
    return lock<LockType>{l};
  }

private:
  explicit lock(LockType& l) : locker{l} {}
  LockType& locker;
};

class mutex
{
public:
  bool lock()
  {
    if(locked)
    {
      return false;
    }

    locked = true;
    return true;
  }

  void unlock()
  {
    locked = false;
  }

  bool is_locked() const {
    return locked;
  }

protected:
  bool locked = false;
};

}

template <typename T, size_t S>
struct buffer {
  constexpr buffer() {}
  constexpr auto size() { return S; }

  bool copy_from(const std::span<T>& input, uint16_t amount)
  {
    const auto lk = commons::lock<commons::mutex>::get(m);
    if(!lk)
    {
      return false;
    }

    if(amount > max_size)
    {
      return false;
    }

    old_pos = new_pos;

    if (old_pos + amount > max_size) {
      const auto size_to_copy = max_size - old_pos;
      std::copy(input.begin(),
                std::next(input.begin(), size_to_copy),
                std::next(buf.begin(), old_pos));
      old_pos = 0;

      //TODO: this only works if amount is not double the size of main_buf
      std::copy(std::next(input.begin(), size_to_copy),
                input.begin() + amount, buf.begin());
      new_pos = amount - size_to_copy;
    } else {
      std::copy(input.begin(),
                input.begin() + amount,
                std::next(buf.begin(), old_pos));
      new_pos = old_pos + amount;
    }

    return true;
  }

  template<typename Func_t>
  bool execute(const Func_t& executor)
  {
    const auto lk = commons::lock<commons::mutex>::get(m);
    if(!lk)
    {
      return false;
    }

    executor(buf);
  }

  bool print() const
  {
    const auto lk = commons::lock<commons::mutex>::get(m);
    if(!lk)
    {
      return false;
    }

    uart_interface::write(
        {reinterpret_cast<const char *>(buf.data()), new_pos});
    return true;
  }

private:
  static constexpr auto max_size = S;
  std::array<T, S> buf{};
  size_t old_pos = 0;
  size_t new_pos = 0;
  mutable commons::mutex m;
};

class gps_data
{
public:
  void extract_gps_data(auto& buf)
  {
    buf.execute([this](auto& value) { this->copy_to_own_buf(value); });
  }

  void copy_to_own_buf(std::span<uint8_t> other)
  {
    auto view = std::string_view{reinterpret_cast<const char*>(other.data()), other.size()};

    const auto it_begin = view.find("$GPGGA");

    if(it_begin == view.npos)
    {
      return;
    }

    view.remove_prefix(it_begin);
    const auto it_end = view.find("\r\n", it_begin);

    if(it_end == view.npos)
    {
      return;
    }

    view.remove_suffix(view.size() - it_end);

    if(view.size() > max_size)
    {
      return;
    }

    std::copy(view.begin(), view.end(), current_line.begin());
    current_size = std::distance(view.begin(), view.end());
    valid = true;
  }

  bool is_valid() const
  {
    return valid;
  }

  void print() const
  {
    log::info("Current GPS Data:");
    uart_interface::write(
        {reinterpret_cast<const char *>(current_line.data()), current_size});
    log::linebreak();
  }

private:
  static constexpr uint8_t max_size = 82; //defined by GPS NMEA 0183 protocol
  size_t current_size = 0;
  std::array<uint8_t, max_size> current_line;
  bool valid = false;
};