#ifndef ORIGINCAR_BASE__COMMAND_FRAME_HPP_
#define ORIGINCAR_BASE__COMMAND_FRAME_HPP_

#include <array>
#include <cstddef>
#include <cstdint>

#include "origincar_base/command_gate.hpp"

namespace origincar_base_core
{

inline void writeInt16(std::array<uint8_t, 11> * frame, std::size_t high_index, int16_t value)
{
  const auto bits = static_cast<uint16_t>(value);
  (*frame)[high_index] = static_cast<uint8_t>(bits >> 8);
  (*frame)[high_index + 1] = static_cast<uint8_t>(bits & 0xFF);
}

inline std::array<uint8_t, 11> encodeCommandFrame(const Command & command)
{
  std::array<uint8_t, 11> frame{};
  frame[0] = 0x7B;

  writeInt16(&frame, 3, static_cast<int16_t>(command.linear_x * 1000.0));
  if (command.ackermann) {
    writeInt16(&frame, 5, 0);
    writeInt16(&frame, 7, static_cast<int16_t>(command.steering_angle * 500.0));
  } else {
    writeInt16(&frame, 5, static_cast<int16_t>(command.linear_y * 1000.0));
    writeInt16(&frame, 7, static_cast<int16_t>(command.angular_z * 1000.0));
  }

  for (std::size_t index = 0; index < 9; ++index) {
    frame[9] ^= frame[index];
  }
  frame[10] = 0x7D;
  return frame;
}

}  // namespace origincar_base_core

#endif  // ORIGINCAR_BASE__COMMAND_FRAME_HPP_
