enum ReceiveMessageState {
  WaitHeader = 0x1,
  WaitBody = 0x2,
  WaitCRC = 0x3
};

template<
  typename ReceiveBufferType, size_t receive_buffer_size,
  typename SendBufferType, size_t send_buffer_size,
  size_t receive_header_size, const uint8_t receive_header_bytes[receive_header_size],
  size_t send_header_size, const uint8_t send_header_bytes[send_header_size]
>
class ComPortReadWriteController: public Printable {
public:
  typedef uint8_t (*CrcCalculateFunction)(uint8_t*, size_t);
private:
  //stream
  Stream& m_stream{};
  // receive
  ReceiveBufferType m_receive_buffer[receive_buffer_size]{};
  ReceiveMessageState m_receive_state = ReceiveMessageState::WaitHeader;
  CrcCalculateFunction m_receive_crc_calculator{};
  // send
  SendBufferType m_send_buffer[send_buffer_size]{};
  CrcCalculateFunction m_send_crc_calculator{};
public:
  ComPortReadWriteController(Stream& stream, CrcCalculateFunction receive_crc_calculator, CrcCalculateFunction send_crc_calculator)
    :m_stream{ stream }, m_receive_crc_calculator{ receive_crc_calculator }, m_send_crc_calculator{ send_crc_calculator } {}
public:
  // receive
  const ReceiveBufferType* receive_data() const { return m_receive_buffer; }
  ReceiveBufferType* receive_data() { return m_receive_buffer; }
  size_t receive_size() const { return receive_buffer_size; }
  // send
  const SendBufferType* send_data() const { return m_send_buffer; }
  SendBufferType* send_data() { return m_send_buffer; }
  size_t send_size() const { return send_buffer_size; }
private:
  // receive
  constexpr size_t receive_bytes_count() const { return receive_buffer_size * sizeof(ReceiveBufferType); }
  const uint8_t* receive_raw_data() const { return reinterpret_cast<const uint8_t*>(m_receive_buffer); }
  uint8_t* receive_raw_data() { return reinterpret_cast<const uint8_t*>(m_receive_buffer); }
  // send
  constexpr size_t send_bytes_count() const { return send_buffer_size * sizeof(SendBufferType); }
  const uint8_t* send_raw_data() const { return reinterpret_cast<const uint8_t*>(m_send_buffer); }
  uint8_t* send_raw_data() { return reinterpret_cast<const uint8_t*>(m_send_buffer); }
public:
  size_t printToInternalStream() const { return printTo(m_stream); }
  virtual size_t printTo(Print& p) const override final {
    return (p.write(send_header_bytes, send_header_size) + p.write(send_raw_data(), send_bytes_count()) +
      p.write(m_send_crc_calculator(send_raw_data(), send_bytes_count())));
  }
  bool handle_read_process() {
    switch (m_receive_state) {
      case ReceiveMessageState::WaitHeader: {
        return handle_wait_header();
      }
      case ReceiveMessageState::WaitBody: {
        return handle_wait_body();
      }
      case ReceiveMessageState::WaitCRC: {
        return handle_wait_crc();
      }
      default: {
        m_receive_state = ReceiveMessageState::WaitHeader;
        return false;
      }
    }
  }
  ReceiveMessageState state() const {
    return m_receive_state;
  }
private:
  bool handle_wait_header() {
    if(m_stream.available() < receive_header_size)
      return false;

    for(size_t index = 0; index < receive_header_size; ++index)
    {
      const auto header_byte = static_cast<uint8_t>(m_stream.read());
      // Serial.print("Receive header byte: ");
      // Serial.print(header_byte);Serial.print(":");
      // Serial.print(receive_header_bytes[index]);Serial.print(":");
      // Serial.print(index);Serial.print(":");
      // Serial.println(receive_header_size);
      if(header_byte != receive_header_bytes[index])
        return false;
    }

    m_receive_state = ReceiveMessageState::WaitBody;
    return handle_wait_body();
  }
  bool handle_wait_body() {
    if(m_stream.available() < receive_bytes_count())
      return false;

    // Serial.println("Receive body: ");
    m_stream.readBytes(receive_raw_data(), receive_bytes_count());
    m_receive_state = ReceiveMessageState::WaitCRC;
    return handle_wait_crc();
  }
  bool handle_wait_crc() {
    if(m_stream.available() < 1)
      return false;

    const auto crc_byte = static_cast<uint8_t>(m_stream.read());
    // Serial.print("Receive CRC: "); Serial.println(crc_byte);
    m_receive_state = ReceiveMessageState::WaitHeader;
    return m_receive_crc_calculator(receive_raw_data(), receive_bytes_count()) == crc_byte;
  }
};

template<
  typename BufferType, size_t receive_buffer_size, size_t send_buffer_size,
  size_t header_size, const uint8_t header_bytes[header_size]
>
class ComPortSymmetricReadWriteController: public ComPortReadWriteController<
  BufferType, receive_buffer_size,
  BufferType, send_buffer_size,
  header_size, header_bytes,
  header_size, header_bytes
> {
public:
    ComPortSymmetricReadWriteController(Stream& stream, typename ComPortSymmetricReadWriteController::CrcCalculateFunction crc_calculator)
      :ComPortReadWriteController<
        BufferType, receive_buffer_size, BufferType, send_buffer_size,
        header_size, header_bytes, header_size, header_bytes
      >{ stream, crc_calculator, crc_calculator } {}
};



constexpr const uint8_t header_controller[2]{ 0x2B, 0x20 };
uint8_t calculate_crc_function(uint8_t* bytes, size_t length) {
  uint16_t data_sum{ 0 };
  for (size_t index = 0; index < length; ++index)
    data_sum += bytes[index];

  return static_cast<uint8_t>(256 - static_cast<uint8_t>(data_sum));
}
ComPortSymmetricReadWriteController<uint16_t, 3, 5, 2, header_controller> com_port_buffer {
  Serial, calculate_crc_function
};


void setup() {
  Serial.begin(9600);
}
void loop() {
  // Controller handing
  com_port_buffer.send_data()[1] = static_cast<uint16_t>(com_port_buffer.state());

  static uint64_t last_send_receive_state_time{ 0 };
  if(const auto current_time = millis(); last_send_receive_state_time + 1000 < current_time)
  {
    last_send_receive_state_time = current_time;
    // Serial.print("Current receive state: "); Serial.println(static_cast<uint16_t>(com_port_buffer.state()));
  }




  static uint64_t last_controller_receive_time{ 0 };
  if(const auto current_time = millis(); last_controller_receive_time + 500 < current_time)
  {
    last_controller_receive_time = current_time;
    com_port_buffer.send_data()[0] += 1;
  }
  // ==================
  // Send data to arrow
  static uint64_t last_send_message_to_serial_millis{ 0 };
  if(const auto current_time = millis(); last_send_message_to_serial_millis + 500 < current_time) {
    last_send_message_to_serial_millis = current_time;
    Serial.print(com_port_buffer);
  }
  // ==================

  // Receive data from arrow
  if(com_port_buffer.handle_read_process()) {
    com_port_buffer.send_data()[0] = 0;
    for(size_t index = 0; index < com_port_buffer.receive_size(); ++index) {
      com_port_buffer.send_data()[2 + index] = 2 * com_port_buffer.receive_data()[index];
    }
  }
  // =======================
}