#include <array>
#include <cstdint>

#include <gtest/gtest.h>

#include "origincar_base/command_frame.hpp"

using origincar_base_core::Command;
using origincar_base_core::encodeCommandFrame;

namespace
{
uint8_t checksum(const std::array<uint8_t, 11> & frame)
{
  uint8_t value = 0;
  for (std::size_t index = 0; index < 9; ++index) {
    value ^= frame[index];
  }
  return value;
}
}  // namespace

TEST(CommandFrameTest, EncodesTwistCommandUsingExistingProtocol)
{
  const auto frame = encodeCommandFrame(Command{0.25, -0.1, 0.4});

  EXPECT_EQ(frame[0], 0x7B);
  EXPECT_EQ(frame[3], 0x00);
  EXPECT_EQ(frame[4], 0xFA);
  EXPECT_EQ(frame[5], 0xFF);
  EXPECT_EQ(frame[6], 0x9C);
  EXPECT_EQ(frame[7], 0x01);
  EXPECT_EQ(frame[8], 0x90);
  EXPECT_EQ(frame[9], checksum(frame));
  EXPECT_EQ(frame[10], 0x7D);
}

TEST(CommandFrameTest, EncodesAckermannSteeringWithFirmwareScale)
{
  Command command;
  command.linear_x = 0.25;
  command.steering_angle = 0.4;
  command.ackermann = true;

  const auto frame = encodeCommandFrame(command);

  EXPECT_EQ(frame[3], 0x00);
  EXPECT_EQ(frame[4], 0xFA);
  EXPECT_EQ(frame[5], 0x00);
  EXPECT_EQ(frame[6], 0x00);
  EXPECT_EQ(frame[7], 0x00);
  EXPECT_EQ(frame[8], 0xC8);
  EXPECT_EQ(frame[9], checksum(frame));
}

TEST(CommandFrameTest, ZeroCommandEncodesZeroPayload)
{
  const auto frame = encodeCommandFrame(Command{});

  for (std::size_t index = 1; index <= 8; ++index) {
    EXPECT_EQ(frame[index], 0x00);
  }
  EXPECT_EQ(frame[9], checksum(frame));
}
