protocol.h

#pragma once
#include <sys/types.h>
#include <v5_api.h>
 
#include <cstdint>
#include <cstdio>
#include <cstring>
#include <string>
#include <vector>
 
#include "types.h"
 
namespace VDB {
uint32_t time_ms();
void delay_ms(uint32_t ms);
}  // namespace VDB
 
#define VDPWarnf(fmt, ...) printf("WARN: " fmt "\n", ##__VA_ARGS__)
#define VDPDebugf(fmt, ...) printf("DEBUG: " fmt "\n", ##__VA_ARGS__)
#define VDPTracef(fmt, ...) printf("TRACE: " fmt "\n", ##__VA_ARGS__)
 
namespace VDP {
constexpr size_t MAX_CHANNELS = 256;

using ChannelID = uint8_t;

enum class PacketType : uint8_t {
  Data = 0b00000000,
  Schema = 0b00000001,
};

enum class PacketFunction : uint8_t {
  Send = 0b00000000,
  Acknowledge = 0b00000010,
};

enum PacketValidity : uint8_t {
  Ok,
  BadChecksum,
  TooSmall,
};

struct PacketHeader {
  PacketType type;
  PacketFunction func;
};

VDP::PacketValidity validate_packet(const VDP::Packet& packet);

VDP::Packet checksum_pac(VDP::Packet in);

uint8_t make_header_byte(PacketHeader head);

PacketHeader decode_header_byte(uint8_t hb);

Packet checksum_pac(VDP::Packet in);

template <typename T>
  requires IsField<std::remove_cvref_t<T>>::value
class Channel {
 public:
  explicit Channel(T& data, ChannelID id) : id_(id), data_(data), last_received_(0) {}

  ChannelID get_id() const { return id_; };

  T& get_data() { return data_; }

  bool apply_update(VDP::PacketSpan data_packet) {
    std::memcpy(&last_received_, data_packet.subspan(0, 4).data(), sizeof(uint32_t));
    return data_.apply_update(data_packet.subspan(4));
  }

  VDP::Packet serialize(PacketType pac_type) {
    VDP::Packet out;
    // add the header and channel id to the packet
    out.push_back((uint8_t)PacketFunction::Send | (uint8_t)pac_type);
    out.push_back((uint8_t)id_);
 
    // either serialize the data held by the channel or the channel schema
    VDP::Packet packet_body(sizeof(uint32_t));
    if (pac_type == PacketType::Data) {
      // add the timestamp to the packet
      uint64_t timestamp = vexSystemHighResTimeGet();
 
      VDP::Packet timestamp_pac(sizeof(uint64_t));
 
      // Copy the timestamp into the vector's underlying storage
      std::memcpy(timestamp_pac.data(), &timestamp, sizeof(uint64_t));
 
      out.insert(out.end(), timestamp_pac.begin(), timestamp_pac.end());
 
      packet_body = data_.serialize_data();
    } else if (pac_type == PacketType::Schema) {
      packet_body = data_.serialize_schema();
    }
    out.insert(out.end(), packet_body.begin(), packet_body.end());
 
    // add the checksum to the packet
    VDP::Packet checksum = checksum_pac(out);
    out.insert(out.end(), checksum.begin(), checksum.end());
    return out;
  };

  std::string schema_to_string() const {
    std::string out = "";
    out += "{\n  id : " + std::to_string(id_) + ",\n";
    out += data_.schema_to_string(1) + "\n}";
    return out;
  }

  std::string data_to_string() const {
    std::string out = "";
    out += "{\n  id : " + std::to_string(id_) + ",\n";
    out += "  last received: " + std::to_string(id_) + ",\n";
    out += data_.data_to_string(1) + "\n}";
    return out;
  }
 
  bool acked = false;
 
 private:
  ChannelID id_;
  T data_;
  uint32_t last_received_;
};

template <typename T>
Channel(ChannelID, T) -> Channel<T>;
 
}  // namespace VDP