#include <catch2/catch_test_macros.hpp>

#include "mission/acs/rwHelpers.h"

TEST_CASE("HDLC Encoding RW", "[acs]") {
  std::array<uint8_t, 256> encodedBuffer{};
  size_t encodedLen = 0;
  SECTION("Basic No Escaped Bytes") {
    std::array<uint8_t, 4> dummyNoEscaped = {1, 2, 3, 4};

    rws::encodeHdlc(dummyNoEscaped.data(), dummyNoEscaped.size(), encodedBuffer.data(), encodedLen);
    REQUIRE(encodedLen == 6);
    REQUIRE(encodedBuffer[0] == rws::FRAME_DELIMITER);
    REQUIRE(encodedBuffer[1] == 1);
    REQUIRE(encodedBuffer[2] == 2);
    REQUIRE(encodedBuffer[3] == 3);
    REQUIRE(encodedBuffer[4] == 4);
    REQUIRE(encodedBuffer[5] == rws::FRAME_DELIMITER);
  }

  SECTION("Basic Some Escaped Bytes") {
    std::array<uint8_t, 4> dummySomeEscaped = {1, 0x7D, 0x7E, 4};

    rws::encodeHdlc(dummySomeEscaped.data(), dummySomeEscaped.size(), encodedBuffer.data(),
                    encodedLen);
    REQUIRE(encodedLen == 8);
    REQUIRE(encodedBuffer[0] == rws::FRAME_DELIMITER);
    REQUIRE(encodedBuffer[1] == 1);
    REQUIRE(encodedBuffer[2] == 0x7D);
    REQUIRE(encodedBuffer[3] == 0x5D);
    REQUIRE(encodedBuffer[4] == 0x7D);
    REQUIRE(encodedBuffer[5] == 0x5E);
    REQUIRE(encodedBuffer[6] == 4);
    REQUIRE(encodedBuffer[7] == rws::FRAME_DELIMITER);
  }
}