PentaTrack/commons.hpp

#pragma once

// TODO: create bettter structure for gpio
#define LED_PORT GPIOC
#define LED2_PIN GPIO_PIN_9

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

#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;
};

}  // namespace commons

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;
  }

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

    old_pos = 0;
    new_pos = 0;
  }

 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;
    }

    HAL_GPIO_TogglePin(LED_PORT, LED2_PIN);
    std::copy(view.begin(), view.end(), current_line.begin());
    current_size = std::distance(view.begin(), view.end());
    valid = current_size > 50;
    HAL_GPIO_TogglePin(LED_PORT, LED2_PIN);
  }

  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;
};