// Copyright (C) 2021 RoboMaster-OSS
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Additional modifications and features by Chengfu Zou, 2023.
//
// Copyright (C) FYT Vision Group. All rights reserved.

#ifndef SERIAL_DRIVER_FIXED_PACKET_HPP_
#define SERIAL_DRIVER_FIXED_PACKET_HPP_

// std
#include <cstdint>
#include <cstring>
#include <memory>

namespace fyt::serial_driver {

// Fixed-length data packet:
// [head_byte(0xff), ...(data_bytes)..., crc8_byte]
template <int capacity = 16>
class FixedPacket {
 public:
  using SharedPtr = std::shared_ptr<FixedPacket>;
  FixedPacket() {
    memset(buffer_, 0, capacity);
    buffer_[0] = 0xff;  // frame head
  }

 public:
  // Clear payload and crc.
  void clear() { memset(buffer_ + 1, 0, capacity - 1); }
  // Set crc8 byte (last byte).
  void setCrc8(uint8_t crc) { buffer_[capacity - 1] = crc; }
  // Backward-compatible alias.
  void setCheckByte(uint8_t check_byte) { setCrc8(check_byte); }
  // Copy data to internal buffer.
  void copyFrom(const void* src) { memcpy(buffer_, src, capacity); }
  // Get internal buffer (read-only).
  const uint8_t* buffer() const { return buffer_; }

  // Load custom data to buffer.
  template <typename T, int data_len = sizeof(T)>
  bool loadData(T const &data, int index) {
    // Bounds check
    if (index > 0 && ((index + data_len) < (capacity))) {
      memcpy(buffer_ + index, &data, data_len);
      return true;
    }
    return false;
  }

  // Unload data from buffer.
  template <typename T, int data_len = sizeof(T)>
  bool unloadData(T &data, int index) {
    // Bounds check
    if (index > 0 && ((index + data_len) < (capacity))) {
      memcpy(&data, buffer_ + index, data_len);
      return true;
    }
    return false;
  }

 private:
  // Packet buffer
  uint8_t buffer_[capacity];  // NOLINT
};

using FixedPacket16 = FixedPacket<16>;
using FixedPacket32 = FixedPacket<32>;
using FixedPacket64 = FixedPacket<64>;

}  // namespace fyt::serial_driver

#endif  // SERIAL_DRIVER_FIXED_PACKET_HPP_
