vdb_device.h

#pragma once
#include <cstdint>
#include <cstdio>
#include <deque>
#include <functional>
#include <tuple>
 
#include "core/device/cobs_device.h"
#include "core/device/vdb/protocol.h"

namespace VDB {
template <typename... Fields>
class Device : public COBSSerialDevice {
 public:
  static constexpr int32_t NO_ACTIVITY_DELAY = 2;  // ms
  static constexpr std::size_t MAX_OUT_QUEUE_SIZE = 50;
  static constexpr std::size_t MAX_IN_QUEUE_SIZE = 50;
 
  enum SEND_PACKET_STATE { SUCCESS, NONE_QUEUED, ERROR };

  explicit Device(int32_t port, int32_t baud_rate, VDP::Channel<Fields>&... channels)
      : COBSSerialDevice(port, baud_rate), channels_(channels...) {
    static_assert(sizeof...(channels) <= VDP::MAX_CHANNELS,
                  "There can be no more than 256 Channels sent to a VDB Device");
    serial_task = vex::task(Device::serial_thread, (void*)this, vex::thread::threadPriorityHigh);
  }

  bool add_to_queue(const VDP::Packet& packet) {
    if (packet.empty()) {
      printf("VDP WARNING: Empty Packets are not allowed\n");
      return false;
    }
    outbound_mutex.lock();
    if (outbound_packets.size() >= MAX_OUT_QUEUE_SIZE) {
      outbound_mutex.unlock();
      return false;
    }
    outbound_packets.push_back(packet);
    outbound_mutex.unlock();
    return true;
  }

  SEND_PACKET_STATE write_packet_from_queue() {
    // packet to write to the device
    VDP::Packet outbound_packet = {};
    // lock the serial port
    outbound_mutex.lock();
    // check if we have a packet to write
    if (outbound_packets.size() > 0) {
      // of we do take the latest packet out of the vector of packets we have
      outbound_packet = std::move(outbound_packets.front());
      outbound_packets.pop_front();
    }
    // unlock
    outbound_mutex.unlock();
    if (outbound_packet.size() == 0) {
      return NONE_QUEUED;
    }
 
    int sent = send_cobs_packet_blocking(outbound_packet.data(), outbound_packet.size());
 
    if (sent >= 0) {
      return SUCCESS;
    } else {
      printf("Failed to send packet (%d):\n", sent);
      hexdump(outbound_packet.data(), outbound_packet.size());
      return ERROR;
    }
  }

  static int serial_thread(void* vself) {
    // defines itself within the thread
    Device& self = *(Device*)vself;
 
    // serial thread loop
    while (true) {
      bool did_something = false;
 
      // Writing
      SEND_PACKET_STATE send_state = self.write_packet_from_queue();
      if (send_state != NONE_QUEUED) {
        did_something = true;
      }
      // Reading
      if (self.poll_incoming_data_once()) {
        Packet decoded = {};
        decoded = self.get_last_decoded_packet();
        self.apply_packet(decoded);
        did_something = true;
      }
      if (!did_something) {
        vexDelay(NO_ACTIVITY_DELAY);
      }
    }
    return 0;
  }

  void send_channel(VDP::ChannelID id) {
    std::apply(
        [&](auto&... channel) {
          (
              [&] {
                if (id == channel.get_id()) {
                  if (channel.acked) {
                    this->add_to_queue(channel.serialize(VDP::PacketType::Data));
                  } else {
                    this->add_to_queue(channel.serialize(VDP::PacketType::Schema));
                  }
                }
              }(),
              ...);
        },
        channels_);
  }

  void apply_packet(VDP::Packet in) {
    const VDP::PacketValidity status = VDP::validate_packet(in);
 
    // check if the packet is valid;
    if (status == VDP::PacketValidity::BadChecksum) {
      VDPWarnf("Controller: Bad packet checksum. Skipping");
      return;
    } else if (status == VDP::PacketValidity::TooSmall) {
      VDPWarnf("Controller: Packet too small to be valid (%d bytes). Skipping", (int)in.size());
      return;
    } else if (status != VDP::PacketValidity::Ok) {
      VDPWarnf("Controller: Unknown validity of packet (BAD). Skipping");
      return;
    }
 
    // decode the header byte and route it accordingly
    VDP::PacketHeader header = VDP::decode_header_byte(in[0]);
    if (header.func == VDP::PacketFunction::Send) {
      if (header.type == VDP::PacketType::Data) {
        // decode packet and apply to channel
        VDP::ChannelID id_to_update = in[1];
        std::apply(
            [&](auto&... channel) {
              (
                  [&] {
                    if (id_to_update == channel.get_id()) {
                      channel.apply_update(VDP::PacketSpan(in).subspan(2));
                    }
                  }(),
                  ...);
            },
            channels_);
        // acknowledge packet
        VDP::Packet data_ack;
        data_ack.push_back(
            VDP::make_header_byte({VDP::PacketType::Data, VDP::PacketFunction::Acknowledge}));
        data_ack.push_back(id_to_update);
        VDP::Packet checksum = VDP::checksum_pac(data_ack);
        data_ack.insert(data_ack.end(), checksum.begin(), checksum.end());
        add_to_queue(data_ack);
 
      } else if (header.type == VDP::PacketType::Schema) {
        VDP::ChannelID acked_id = in[1];
        std::apply(
            [&](auto&... channel) {
              (
                  [&] {
                    if (acked_id == channel.get_id()) {
                      channel.acked = false;
                    }
                  }(),
                  ...);
            },
            channels_);
      }
    } else if (header.func == VDP::PacketFunction::Acknowledge) {
      // mark channel as acknowledged, dont do anything if data packet
      if (header.type == VDP::PacketType::Schema) {
        VDP::ChannelID acked_id = in[1];
        std::apply(
            [&](auto&... channel) {
              (
                  [&] {
                    if (acked_id == channel.get_id()) {
                      channel.acked = true;
                    }
                  }(),
                  ...);
            },
            channels_);
      }
    }
  }
 
 private:
  std::deque<VDP::Packet> outbound_packets{};
  vex::mutex outbound_mutex;
  std::tuple<VDP::Channel<Fields>...> channels_;
 
  bool waiting_to_receive = false;
  std::deque<WirePacket> inbound_packets;

  WirePacket inbound_buffer;

  vex::task serial_task;
 
  std::function<void(const VDP::Packet& packet)> callback;
};
template <typename... Fields>
Device(int32_t, int32_t, VDP::Channel<Fields>...) -> Device<Fields...>;
 
}  // namespace VDB