// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2020 FZI Forschungszentrum Informatik
// Created on behalf of Universal Robots A/S
//
// 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.
// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
/*!\file
 *
 * \author  Felix Exner mauch@fzi.de
 * \date    2020-07-09
 *
 */
//----------------------------------------------------------------------

#include <cassert>
#include <iostream>
#include <vector>
#include <gtest/gtest.h>
#include <string.h>

#include <ur_client_library/comm/bin_parser.h>
#include <ur_client_library/primary/package_header.h>
#include <ur_client_library/primary/primary_parser.h>
#include <ur_client_library/primary/robot_state/configuration_data.h>
#include <ur_client_library/primary/robot_state/masterboard_data.h>
#include "ur_client_library/primary/robot_message/key_message.h"

using namespace urcl;

namespace
{
// ConfigurationData payload before optional int16 reserved fields (older UR software).
constexpr size_t CONFIGURATION_DATA_PAYLOAD_BYTES =
    6 * 2 * sizeof(double) +  // joint position limits
    6 * 2 * sizeof(double) +  // joint motion limits
    5 * sizeof(double) +      // v/a joint/tool defaults + eq_radius
    4 * 6 * sizeof(double) +  // dh_a, dh_d, dh_alpha, dh_theta
    4 * sizeof(int32_t);      // masterboard, controller, robot type, sub type

static_assert(CONFIGURATION_DATA_PAYLOAD_BYTES == 440);

std::vector<uint8_t> makeRobotStatePacketWithConfigurationSubmessage(const std::vector<uint8_t>& configuration_payload)
{
  const uint32_t sub_size = static_cast<uint32_t>(sizeof(uint32_t) + sizeof(uint8_t) + configuration_payload.size());
  const uint32_t total_packet_size = static_cast<uint32_t>(sizeof(int32_t) + sizeof(uint8_t) + sub_size);

  std::vector<uint8_t> packet(total_packet_size);
  size_t offset = 0;
  packet[offset++] = static_cast<uint8_t>((total_packet_size >> 24) & 0xFF);
  packet[offset++] = static_cast<uint8_t>((total_packet_size >> 16) & 0xFF);
  packet[offset++] = static_cast<uint8_t>((total_packet_size >> 8) & 0xFF);
  packet[offset++] = static_cast<uint8_t>(total_packet_size & 0xFF);
  packet[offset++] = static_cast<uint8_t>(primary_interface::RobotPackageType::ROBOT_STATE);

  packet[offset++] = static_cast<uint8_t>((sub_size >> 24) & 0xFF);
  packet[offset++] = static_cast<uint8_t>((sub_size >> 16) & 0xFF);
  packet[offset++] = static_cast<uint8_t>((sub_size >> 8) & 0xFF);
  packet[offset++] = static_cast<uint8_t>(sub_size & 0xFF);
  packet[offset++] = static_cast<uint8_t>(primary_interface::RobotStateType::CONFIGURATION_DATA);

  memcpy(packet.data() + offset, configuration_payload.data(), configuration_payload.size());
  offset += configuration_payload.size();
  assert(offset == packet.size());
  return packet;
}

// Masterboard payload sizes (not including packageSize + packageType header).
// Base payload up to and including `immiInterfaceInstalled` flag.
constexpr size_t MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI = 2 * sizeof(uint32_t) +  // digital input/output bits
                                                         2 * sizeof(uint8_t) +   // analog input ranges
                                                         2 * sizeof(double) +    // analog inputs
                                                         2 * sizeof(char) +      // analog output domains
                                                         2 * sizeof(double) +    // analog outputs
                                                         4 * sizeof(float) +     // temperature + voltages + currents
                                                         sizeof(uint8_t) +       // safety mode
                                                         sizeof(uint8_t) +       // in reduced mode (bool)
                                                         sizeof(uint8_t);        // immi interface installed (bool)
constexpr size_t MASTERBOARD_OPTIONAL_IMMI_BYTES = 2 * sizeof(uint32_t) + 2 * sizeof(float);
constexpr size_t MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI = sizeof(uint32_t) +  // reserved
                                                        sizeof(uint8_t) +   // operational mode selector input
                                                        sizeof(uint8_t) +   // three position enabling device (bool)
                                                        sizeof(uint8_t);    // reserved

std::vector<uint8_t> makeRobotStatePacketWithMasterboardSubmessage(const std::vector<uint8_t>& masterboard_payload)
{
  const uint32_t sub_size = static_cast<uint32_t>(sizeof(uint32_t) + sizeof(uint8_t) + masterboard_payload.size());
  const uint32_t total_packet_size = static_cast<uint32_t>(sizeof(int32_t) + sizeof(uint8_t) + sub_size);

  std::vector<uint8_t> packet(total_packet_size);
  size_t offset = 0;
  packet[offset++] = static_cast<uint8_t>((total_packet_size >> 24) & 0xFF);
  packet[offset++] = static_cast<uint8_t>((total_packet_size >> 16) & 0xFF);
  packet[offset++] = static_cast<uint8_t>((total_packet_size >> 8) & 0xFF);
  packet[offset++] = static_cast<uint8_t>(total_packet_size & 0xFF);
  packet[offset++] = static_cast<uint8_t>(primary_interface::RobotPackageType::ROBOT_STATE);

  packet[offset++] = static_cast<uint8_t>((sub_size >> 24) & 0xFF);
  packet[offset++] = static_cast<uint8_t>((sub_size >> 16) & 0xFF);
  packet[offset++] = static_cast<uint8_t>((sub_size >> 8) & 0xFF);
  packet[offset++] = static_cast<uint8_t>(sub_size & 0xFF);
  packet[offset++] = static_cast<uint8_t>(primary_interface::RobotStateType::MASTERBOARD_DATA);

  memcpy(packet.data() + offset, masterboard_payload.data(), masterboard_payload.size());
  offset += masterboard_payload.size();
  assert(offset == packet.size());
  return packet;
}

// Helpers for big-endian serialization of primitive values used by masterboard payload tests.
template <typename T>
void appendBigEndian(std::vector<uint8_t>& buffer, T value)
{
  uint8_t bytes[sizeof(T)];
  std::memcpy(bytes, &value, sizeof(T));
  // Host may be little-endian; the UR wire format is big-endian.
  for (size_t i = 0; i < sizeof(T); ++i)
  {
    buffer.push_back(bytes[sizeof(T) - 1 - i]);
  }
}

void appendBigEndianDouble(std::vector<uint8_t>& buffer, double value)
{
  uint64_t inner;
  std::memcpy(&inner, &value, sizeof(double));
  appendBigEndian<uint64_t>(buffer, inner);
}

void appendBigEndianFloat(std::vector<uint8_t>& buffer, float value)
{
  uint32_t inner;
  std::memcpy(&inner, &value, sizeof(float));
  appendBigEndian<uint32_t>(buffer, inner);
}
}  // namespace

/* First RobotState of UR5e from URSim v5.8
 *
 * This package contains:
 *  - ROBOT_MODE_DATA
 *  - JOINT_DATA
 *  - CARTESIAN_INFO
 *  - KINEMATICS_INFO
 *  - NEEDED_FOR_CALIB_DATA
 *  - MASTERBOARD_DATA
 *  - TOOL_DATA
 *  - CONFIGURATION_DATA
 *  - FORCE_MODE_DATA
 *  - ADDITIONAL_INFO
 *  - SAFETY_DATA
 *  - TOOL_COMM_INFO
 *  - TOOL_MODE_INFO
 */
const unsigned char ROBOT_STATE[] = {
  0x00, 0x00, 0x05, 0x6a, 0x10, 0x00, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x05, 0xf8, 0x7d, 0x17, 0x40, 0x01,
  0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x3f, 0xef, 0x0a, 0x3d, 0x70, 0xa3, 0xd7, 0x0a, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb, 0x01,
  0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0xa3, 0xa8, 0x79, 0x38, 0x00, 0x00, 0x00, 0x00, 0x41, 0xcc, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0xfd, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x09, 0x06, 0x5a, 0x00, 0x00, 0x00, 0x00, 0x41, 0xc8, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xc0, 0x01, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x38, 0xc0, 0x01, 0x9f, 0xbe, 0x76,
  0xc8, 0xb4, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xbf, 0x0f, 0xf7, 0x00, 0x00, 0x00, 0x00,
  0x41, 0xc4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0xe9, 0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0xbf, 0xe9,
  0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x6e, 0xbb, 0xe2, 0x00,
  0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97,
  0xf6, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97, 0xf6, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3a, 0xe6,
  0x05, 0x69, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0x9f, 0xbe, 0x76,
  0xc8, 0xb4, 0x39, 0x00, 0xbf, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x39, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00,
  0x00, 0x00, 0x65, 0x04, 0xbf, 0xc2, 0x6d, 0x90, 0xa0, 0x1d, 0xe7, 0x77, 0xbf, 0xdb, 0xe1, 0x32, 0xf6, 0xa8, 0x66,
  0x44, 0x3f, 0xc9, 0xdc, 0x1e, 0xb0, 0x03, 0x31, 0xe6, 0xbf, 0x54, 0x02, 0xc0, 0xf8, 0xe6, 0xe6, 0x79, 0x40, 0x08,
  0xee, 0x22, 0x63, 0x78, 0xfa, 0xe0, 0x3f, 0xa3, 0xe9, 0xa4, 0x23, 0x7a, 0x7b, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe1, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33,
  0x33, 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc,
  0xcc, 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f,
  0xb9, 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45,
  0x50, 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x4b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00,
  0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x42, 0x40, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00,
  0x25, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, 0x00, 0x01, 0xbd,
  0x06, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19,
  0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28,
  0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63,
  0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57,
  0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c,
  0x2f, 0x63, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40,
  0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94,
  0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6,
  0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x3f, 0xf0, 0xc1, 0x52, 0x38, 0x2d, 0x73, 0x65, 0x3f, 0xf6, 0x57, 0x18, 0x4a, 0xe7, 0x44, 0x87,
  0x3f, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf3, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x3f, 0xd0, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, 0x33,
  0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, 0xcc,
  0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, 0xb9,
  0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50,
  0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01,
  0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x3d, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x3f, 0x6c, 0xf5, 0xac, 0x1d, 0xb9, 0xa1, 0x08, 0x00, 0x00, 0x00, 0x09, 0x08, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00,
  0x00, 0x2b, 0x0a, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0xbf, 0xb8, 0x4d, 0xc2, 0x84, 0x9f, 0xed, 0xcf, 0xbf, 0xb0,
  0x37, 0x9e, 0xd0, 0x87, 0xba, 0x97, 0x3f, 0xe2, 0xa2, 0x5b, 0x78, 0xc3, 0x9f, 0x6c, 0x3f, 0xc0, 0xa3, 0xd7, 0x0a,
  0x3d, 0x70, 0xa4, 0x00, 0x00, 0x00, 0x1a, 0x0b, 0x00, 0x00, 0x01, 0xc2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x01, 0x3f, 0xc0, 0x00, 0x00, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x0c, 0x00, 0x01, 0x01
};

// Create a VersionMessage
const unsigned char VERSION_MESSAGE[] = {
  // message size
  0x00, 0x00, 0x00, 0x37,
  // message type
  0x14,
  // timestamp
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  // source
  0xfe,
  // robot_message_type
  0x03,
  // payload
  0x09, 0x55, 0x52, 0x43, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x05, 0x18, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x30, 0x34, 0x2d, 0x31, 0x30, 0x2d, 0x32, 0x30, 0x32, 0x35, 0x2c, 0x20, 0x30, 0x30, 0x3a, 0x32, 0x35, 0x3a,
  0x33, 0x33
};

const unsigned char KEY_MESSAGE[] = {
  // message size
  0x00, 0x00, 0x00, 0x32,
  // message type
  0x14,
  // timestamp
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  // source
  0xfe,
  // robot_message_type
  0x07,
  // payload
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x13, 0x50, 0x52, 0x4f, 0x47, 0x52, 0x41, 0x4d, 0x5f, 0x58, 0x58, 0x58, 0x5f,
  0x53, 0x54, 0x41, 0x52, 0x54, 0x45, 0x44, 0x74, 0x65, 0x78, 0x74, 0x6d, 0x73, 0x67
};

const unsigned char RUNTIME_EXCEPTION_MESSAGE[] = {
  // message size
  0x00, 0x00, 0x00, 0x39,
  // message type robot message
  0x14,
  // timestamp
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  // source
  0xfd,
  // robot_message_type RUNTIME_EXCEPTION_MESSAGE
  0x0a,
  // line number
  0x00, 0x00, 0x00, 0x03,
  // column number
  0x00, 0x00, 0x00, 0x01,
  // message: "compile_error_name_not_found:txtmsg:"
  0x63, 0x6f, 0x6d, 0x70, 0x69, 0x6c, 0x65, 0x5f, 0x65, 0x72, 0x72, 0x6f, 0x72, 0x5f, 0x6e, 0x61, 0x6d, 0x65, 0x5f,
  0x6e, 0x6f, 0x74, 0x5f, 0x66, 0x6f, 0x75, 0x6e, 0x64, 0x3a, 0x74, 0x78, 0x74, 0x6d, 0x73, 0x67, 0x3a
};

const unsigned char SAFETY_MODE_MESSAGE[] = {
  // message size
  0x00, 0x00, 0x00, 0x20,
  // message type robot message
  0x14,
  // timestamp
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  // source
  0xfd,
  // robot_message_type SAFETY_MODE
  0x05,
  // Robot message code
  0x00, 0x00, 0x00, 0x00,
  // Robot message argument
  0x00, 0x00, 0x00, 0x00,
  // Safety mode type
  0x03,
  // Report data type
  0x00, 0x00, 0x00, 0x00,
  // Report data
  0x00, 0x00, 0x00, 0x01
};

const unsigned char SAFETY_MODE_MESSAGE_STRING[] = {
  // message size
  0x00, 0x00, 0x00, 0x20,
  // message type robot message
  0x14,
  // timestamp
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  // source
  0xfd,
  // robot_message_type SAFETY_MODE
  0x05,
  // Robot message code
  0x00, 0x00, 0x00, 0x00,
  // Robot message argument
  0x00, 0x00, 0x00, 0x00,
  // Safety mode type
  0x03,
  // Report data type
  0x00, 0x00, 0x00, 0x00,
  // Report data
  0x80, 0x07, 0x06, 0x05
};

class PrimaryParserTest : public ::testing::Test
{
protected:
  virtual void SetUp() override
  {
    // In these tests we use strict mode, which means that the parser will not ignore any extra
    // bytes in the payload. This allows us to verify that the parser correctly identifies the size
    // of each message and does not consume more bytes than it should.
    // If new software versions add new fields to the messages, these tests will fail, which is
    // desirable as it will prompt us to update the parser with the new fields.
    parser_.setStrictMode(true);
  }

  primary_interface::PrimaryParser parser_;
};

TEST_F(PrimaryParserTest, parse_calibration_data)
{
  unsigned char raw_data[sizeof(ROBOT_STATE)];
  memcpy(raw_data, ROBOT_STATE, sizeof(ROBOT_STATE));
  comm::BinParser bp(raw_data, sizeof(raw_data));

  std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
  parser_.parse(bp, products);

  EXPECT_EQ(products.size(), 13);

  if (primary_interface::KinematicsInfo* data = dynamic_cast<primary_interface::KinematicsInfo*>(products[3].get()))
  {
    EXPECT_EQ(data->checksum_,
              vector6uint32_t({ 4294967295, 4294967295, 4294967295, 4294967295, 4294967295, 4294967295 }));
    EXPECT_EQ(data->dh_theta_, vector6d_t({ 0, 0, 0, 0, 0, 0 }));
    EXPECT_EQ(data->dh_a_, vector6d_t({ 0, -0.425, -0.3922, 0, 0, 0 }));
    EXPECT_EQ(data->dh_d_, vector6d_t({ 0.1625, 0, 0, 0.1333, 0.0997, 0.0996 }));
    EXPECT_EQ(data->dh_alpha_, vector6d_t({ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 }));
  }
}

TEST_F(PrimaryParserTest, parse_robot_state_with_single_parser)
{
  unsigned char raw_data[sizeof(ROBOT_STATE)];
  memcpy(raw_data, ROBOT_STATE, sizeof(ROBOT_STATE));
  comm::BinParser bp(raw_data, sizeof(raw_data));

  std::unique_ptr<primary_interface::PrimaryPackage> product;
  ASSERT_FALSE(parser_.parse(bp, product));
};

TEST_F(PrimaryParserTest, parse_version_message)
{
  unsigned char raw_data[sizeof(VERSION_MESSAGE)];
  memcpy(raw_data, VERSION_MESSAGE, sizeof(VERSION_MESSAGE));
  comm::BinParser bp(raw_data, sizeof(raw_data));

  std::unique_ptr<primary_interface::PrimaryPackage> product;
  ASSERT_TRUE(parser_.parse(bp, product));

  EXPECT_NE(product, nullptr);
  if (primary_interface::VersionMessage* data = dynamic_cast<primary_interface::VersionMessage*>(product.get()))
  {
    EXPECT_EQ(data->major_version_, 5);
    EXPECT_EQ(data->minor_version_, 24);
    EXPECT_EQ(data->svn_version_, 0);
    EXPECT_EQ(data->build_date_, "04-10-2025, 00:25:33");
    EXPECT_EQ(data->project_name_, "URControl");
  }
  else
  {
    FAIL() << "Parsed package is not of type VersionMessage";
  }
}

TEST_F(PrimaryParserTest, parse_key_message)
{
  unsigned char raw_data[sizeof(KEY_MESSAGE)];
  memcpy(raw_data, KEY_MESSAGE, sizeof(KEY_MESSAGE));
  comm::BinParser bp(raw_data, sizeof(raw_data));

  std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
  ASSERT_TRUE(parser_.parse(bp, products));

  ASSERT_EQ(products.size(), 1);
  if (auto data = dynamic_cast<primary_interface::KeyMessage*>(products[0].get()))
  {
    EXPECT_EQ(data->message_code_, 0);
    EXPECT_EQ(data->message_argument_, 0);
    EXPECT_EQ(data->title_length_, 19);
    EXPECT_EQ(data->title_, "PROGRAM_XXX_STARTED");
    EXPECT_EQ(data->text_, "textmsg");
    //
    // verify that copying a message works correctly
    primary_interface::KeyMessage clone = *data;
    EXPECT_EQ(data->message_code_, clone.message_code_);
    EXPECT_EQ(data->message_argument_, clone.message_argument_);
    EXPECT_EQ(data->title_length_, clone.title_length_);
    EXPECT_EQ(data->title_, clone.title_);
    EXPECT_EQ(data->text_, clone.text_);
  }
  else
  {
    FAIL() << "Parsed package is not of type KeyMessage";
  }
}

TEST_F(PrimaryParserTest, parse_runtime_exception_message)
{
  unsigned char raw_data[sizeof(RUNTIME_EXCEPTION_MESSAGE)];
  memcpy(raw_data, RUNTIME_EXCEPTION_MESSAGE, sizeof(RUNTIME_EXCEPTION_MESSAGE));
  comm::BinParser bp(raw_data, sizeof(raw_data));

  std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
  ASSERT_TRUE(parser_.parse(bp, products));

  EXPECT_EQ(products.size(), 1);

  if (primary_interface::RuntimeExceptionMessage* data =
          dynamic_cast<primary_interface::RuntimeExceptionMessage*>(products[0].get()))
  {
    EXPECT_EQ(data->line_number_, 3);
    EXPECT_EQ(data->column_number_, 1);
    EXPECT_EQ(data->text_, "compile_error_name_not_found:txtmsg:");

    // verify that copying a message works correctly
    primary_interface::RuntimeExceptionMessage clone = *data;
    EXPECT_EQ(data->line_number_, clone.line_number_);
    EXPECT_EQ(data->column_number_, clone.column_number_);
    EXPECT_EQ(data->text_, clone.text_);
  }
  else
  {
    FAIL() << "Parsed package is not of type RuntimeExceptionMessage";
  }
}

TEST_F(PrimaryParserTest, parse_robot_state_with_oversized_submessage)
{
  // KinematicsInfo parses exactly 220 bytes of payload:
  //   6×uint32 (checksum) + 4×vector6d (DH params) + uint32 (calibration_status)
  const size_t ki_payload_size = 6 * sizeof(uint32_t) + 4 * 6 * sizeof(double) + sizeof(uint32_t);
  const size_t extra_bytes = 10;
  const size_t sub1_size = sizeof(uint32_t) + sizeof(uint8_t) + ki_payload_size + extra_bytes;
  const size_t sub2_size = sizeof(uint32_t) + sizeof(uint8_t) + 1;
  const size_t total_size = sizeof(int32_t) + sizeof(uint8_t) + sub1_size + sub2_size;

  std::vector<uint8_t> test_data(total_size, 0);
  size_t offset = 0;

  // Total packet size (big-endian int32)
  test_data[offset++] = (total_size >> 24) & 0xFF;
  test_data[offset++] = (total_size >> 16) & 0xFF;
  test_data[offset++] = (total_size >> 8) & 0xFF;
  test_data[offset++] = total_size & 0xFF;

  // Packet type: ROBOT_STATE (0x10)
  test_data[offset++] = 0x10;

  // Submessage 1: KinematicsInfo with extra trailing bytes
  test_data[offset++] = (sub1_size >> 24) & 0xFF;
  test_data[offset++] = (sub1_size >> 16) & 0xFF;
  test_data[offset++] = (sub1_size >> 8) & 0xFF;
  test_data[offset++] = sub1_size & 0xFF;
  test_data[offset++] = 0x05;  // KINEMATICS_INFO
  offset += ki_payload_size;   // payload is all zeros
  memset(test_data.data() + offset, 0xAA, extra_bytes);
  offset += extra_bytes;

  // Submessage 2: ADDITIONAL_INFO (parsed by base RobotState → rawData)
  test_data[offset++] = (sub2_size >> 24) & 0xFF;
  test_data[offset++] = (sub2_size >> 16) & 0xFF;
  test_data[offset++] = (sub2_size >> 8) & 0xFF;
  test_data[offset++] = sub2_size & 0xFF;
  test_data[offset++] = 0x08;  // ADDITIONAL_INFO
  test_data[offset++] = 0x42;  // 1 byte of payload

  ASSERT_EQ(offset, total_size);

  {
    // With strict mode enabled, an execption should be thrown due to the oversized KinematicsInfo
    // submessage
    std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
    unsigned char raw_data[total_size];
    memcpy(raw_data, test_data.data(), test_data.size());
    comm::BinParser bp(raw_data, total_size);
    EXPECT_THROW(parser_.parse(bp, products), UrException);
  }

  {
    // Using a non-strict parser that allows oversized submessages should succeed and parse all
    // submessages
    std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
    unsigned char raw_data[total_size];
    memcpy(raw_data, test_data.data(), test_data.size());
    comm::BinParser bp(raw_data, total_size);
    primary_interface::PrimaryParser non_strict_parser;
    EXPECT_TRUE(non_strict_parser.parse(bp, products));

    // Both submessages should be parsed despite the oversized first one
    ASSERT_EQ(products.size(), 2);

    auto* ki = dynamic_cast<primary_interface::KinematicsInfo*>(products[0].get());
    ASSERT_NE(ki, nullptr);
    EXPECT_EQ(ki->calibration_status_, 0u);
    EXPECT_EQ(ki->dh_theta_, vector6d_t({ 0, 0, 0, 0, 0, 0 }));
  }
}

TEST_F(PrimaryParserTest, parse_safetymode_msg)
{
  unsigned char raw_data[sizeof(SAFETY_MODE_MESSAGE)];
  memcpy(raw_data, SAFETY_MODE_MESSAGE, sizeof(SAFETY_MODE_MESSAGE));
  comm::BinParser bp(raw_data, sizeof(raw_data));

  std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
  ASSERT_TRUE(parser_.parse(bp, products));
  ASSERT_EQ(products.size(), 1);

  if (primary_interface::SafetyModeMessage* data =
          dynamic_cast<primary_interface::SafetyModeMessage*>(products[0].get()))
  {
    EXPECT_EQ(data->message_code_, 0);
    EXPECT_EQ(data->message_argument_, 0);
    EXPECT_EQ(data->safety_mode_type_, urcl::SafetyMode::PROTECTIVE_STOP);  // 3
    EXPECT_EQ(data->report_data_type_, 0);
    EXPECT_EQ(std::get<uint32_t>(data->report_data_), 1);

    primary_interface::SafetyModeMessage clone = *data;
    EXPECT_EQ(data->message_code_, clone.message_code_);
    EXPECT_EQ(data->message_argument_, clone.message_argument_);
    EXPECT_EQ(data->safety_mode_type_, clone.safety_mode_type_);
    EXPECT_EQ(data->report_data_type_, clone.report_data_type_);
    EXPECT_EQ(data->report_data_, clone.report_data_);
  }

  // Test all report data types
  for (unsigned char i = 1; i < 5; i++)
  {
    memcpy(raw_data, SAFETY_MODE_MESSAGE, sizeof(SAFETY_MODE_MESSAGE));
    raw_data[27] = i;
    // Make negative number
    if (i == 2)
    {
      raw_data[28] ^= 0xff;
      raw_data[29] ^= 0xff;
      raw_data[30] ^= 0xff;
      raw_data[31] ^= 0xff;
    }
    comm::BinParser bp_loop(raw_data, sizeof(raw_data));
    std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products_loop;
    ASSERT_TRUE(parser_.parse(bp_loop, products_loop));
    ASSERT_EQ(products_loop.size(), 1);

    if (primary_interface::SafetyModeMessage* data_loop =
            dynamic_cast<primary_interface::SafetyModeMessage*>(products_loop[0].get()))
    {
      EXPECT_EQ(data_loop->report_data_type_, static_cast<uint32_t>(i));
      if (data_loop->report_data_type_ == 2)
      {
        EXPECT_EQ(std::get<int32_t>(data_loop->report_data_), -2);
      }
      else if (data_loop->report_data_type_ == 3)
      {
        EXPECT_EQ(std::get<float>(data_loop->report_data_), float(2.3509887e-38));
      }
      else
      {
        EXPECT_EQ(std::get<uint32_t>(data_loop->report_data_), 1);
      }
    }
  }
}

std::string constructString(int data_type, std::string data)
{
  std::stringstream ss;
  ss << "SafetyModeMessage\n";
  ss << "Message code: 0\n";
  ss << "Message argument: 0\n";
  ss << "Safety mode type: 3\n";
  ss << "Report data type: " << data_type << "\n";
  ss << "Report data: " << data;
  return ss.str();
}

TEST_F(PrimaryParserTest, parsing_safetymode_results_in_correct_string)
{
  unsigned char raw_data[sizeof(SAFETY_MODE_MESSAGE_STRING)];
  // Test all report data types
  for (unsigned char i = 0; i < 5; i++)
  {
    memcpy(raw_data, SAFETY_MODE_MESSAGE_STRING, sizeof(SAFETY_MODE_MESSAGE_STRING));
    raw_data[27] = i;
    comm::BinParser bp(raw_data, sizeof(raw_data));
    std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
    ASSERT_TRUE(parser_.parse(bp, products));
    ASSERT_EQ(products.size(), 1);

    if (primary_interface::SafetyModeMessage* data =
            dynamic_cast<primary_interface::SafetyModeMessage*>(products[0].get()))
    {
      EXPECT_EQ(data->report_data_type_, static_cast<uint32_t>(i));
      if (data->report_data_type_ == 0 || data->report_data_type_ == 1)
      {
        EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "2147943941"));
      }
      else if (data->report_data_type_ == 2)
      {
        EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "-2147023355"));
      }
      else if (data->report_data_type_ == 3)
      {
        EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "6.30203e-36"));
      }
      else
      {
        EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "0x80070605"));
      }
    }
  }
}

TEST_F(PrimaryParserTest, parse_configuration_data_without_reserved_fields)
{
  std::vector<uint8_t> payload(CONFIGURATION_DATA_PAYLOAD_BYTES, 0);
  std::vector<uint8_t> packet = makeRobotStatePacketWithConfigurationSubmessage(payload);

  comm::BinParser bp(packet.data(), packet.size());
  std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
  ASSERT_TRUE(parser_.parse(bp, products));
  ASSERT_EQ(products.size(), 1u);

  auto* config = dynamic_cast<primary_interface::ConfigurationData*>(products[0].get());
  ASSERT_NE(config, nullptr);
  EXPECT_EQ(config->reserved_1_, 0);
  EXPECT_EQ(config->reserved_2_, 0);
}

TEST_F(PrimaryParserTest, parse_configuration_data_with_reserved_fields)
{
  std::vector<uint8_t> payload(CONFIGURATION_DATA_PAYLOAD_BYTES, 0);
  // Big-endian int16 values appended after the legacy payload.
  payload.push_back(0x12);
  payload.push_back(0x34);
  payload.push_back(0x56);
  payload.push_back(0x78);

  std::vector<uint8_t> packet = makeRobotStatePacketWithConfigurationSubmessage(payload);

  comm::BinParser bp(packet.data(), packet.size());
  std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
  ASSERT_TRUE(parser_.parse(bp, products));
  ASSERT_EQ(products.size(), 1u);

  auto* config = dynamic_cast<primary_interface::ConfigurationData*>(products[0].get());
  ASSERT_NE(config, nullptr);
  EXPECT_EQ(config->reserved_1_, static_cast<int16_t>(0x1234));
  EXPECT_EQ(config->reserved_2_, static_cast<int16_t>(0x5678));
}

TEST_F(PrimaryParserTest, parse_masterboard_data_without_immi)
{
  std::vector<uint8_t> payload;
  payload.reserve(MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI);

  appendBigEndian<uint32_t>(payload, 0xDEADBEEFu);  // digital input bits
  appendBigEndian<uint32_t>(payload, 0xCAFEBABEu);  // digital output bits
  payload.push_back(0x01);                          // analog input range 0
  payload.push_back(0x02);                          // analog input range 1
  appendBigEndianDouble(payload, 1.5);              // analog input 0
  appendBigEndianDouble(payload, -2.25);            // analog input 1
  payload.push_back(0x03);                          // analog output domain 0
  payload.push_back(0x04);                          // analog output domain 1
  appendBigEndianDouble(payload, 3.75);             // analog output 0
  appendBigEndianDouble(payload, -4.5);             // analog output 1
  appendBigEndianFloat(payload, 42.0f);             // master board temperature
  appendBigEndianFloat(payload, 48.1f);             // robot voltage 48V
  appendBigEndianFloat(payload, 1.23f);             // robot current
  appendBigEndianFloat(payload, 0.5f);              // master IO current
  payload.push_back(0x01);                          // safety mode (NORMAL)
  payload.push_back(0x00);                          // in reduced mode (false)
  payload.push_back(0x00);                          // immi interface installed (false)
  appendBigEndian<uint32_t>(payload, 0u);           // reserved
  payload.push_back(0x02);                          // operational mode selector input
  payload.push_back(0x01);                          // three position enabling device input (true)
  payload.push_back(0x00);                          // reserved

  ASSERT_EQ(payload.size(), MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI);

  std::vector<uint8_t> packet = makeRobotStatePacketWithMasterboardSubmessage(payload);
  comm::BinParser bp(packet.data(), packet.size());
  std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
  ASSERT_TRUE(parser_.parse(bp, products));
  ASSERT_EQ(products.size(), 1u);

  auto* data = dynamic_cast<primary_interface::MasterboardData*>(products[0].get());
  ASSERT_NE(data, nullptr);
  EXPECT_EQ(data->digital_input_bits_, std::bitset<32>(0xDEADBEEFu));
  EXPECT_EQ(data->digital_output_bits_, std::bitset<32>(0xCAFEBABEu));
  EXPECT_EQ(data->analog_input_range_0_, 0x01);
  EXPECT_EQ(data->analog_input_range_1_, 0x02);
  EXPECT_DOUBLE_EQ(data->analog_input_0_, 1.5);
  EXPECT_DOUBLE_EQ(data->analog_input_1_, -2.25);
  EXPECT_EQ(static_cast<int>(data->analog_output_domain_0_), 0x03);
  EXPECT_EQ(static_cast<int>(data->analog_output_domain_1_), 0x04);
  EXPECT_DOUBLE_EQ(data->analog_output_0_, 3.75);
  EXPECT_DOUBLE_EQ(data->analog_output_1_, -4.5);
  EXPECT_FLOAT_EQ(data->master_board_temperature_, 42.0f);
  EXPECT_FLOAT_EQ(data->robot_voltage_48v_, 48.1f);
  EXPECT_FLOAT_EQ(data->robot_current_, 1.23f);
  EXPECT_FLOAT_EQ(data->master_io_current_, 0.5f);
  EXPECT_EQ(data->safety_mode_, SafetyMode::NORMAL);
  EXPECT_FALSE(data->in_reduced_mode_);
  EXPECT_FALSE(data->immi_interface_installed_);
  EXPECT_EQ(data->operational_mode_selector_input_, 2);
  EXPECT_TRUE(data->three_position_enabling_device_input_);

  auto msg_str = data->toString();
  std::cout << msg_str << std::endl;
  EXPECT_NE(msg_str.find("IMMI interface installed: false"), std::string::npos);
  EXPECT_EQ(msg_str.find("IMMI input bits:"), std::string::npos);
  EXPECT_EQ(msg_str.find("IMMI output bits:"), std::string::npos);
  EXPECT_EQ(msg_str.find("IMMI voltage 24V:"), std::string::npos);
  EXPECT_EQ(msg_str.find("IMMI current:"), std::string::npos);
}

TEST_F(PrimaryParserTest, parse_masterboard_data_with_immi)
{
  std::vector<uint8_t> payload;
  payload.reserve(MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_OPTIONAL_IMMI_BYTES +
                  MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI);

  appendBigEndian<uint32_t>(payload, 0x00000001u);
  appendBigEndian<uint32_t>(payload, 0x00000002u);
  payload.push_back(0x00);
  payload.push_back(0x00);
  appendBigEndianDouble(payload, 0.0);
  appendBigEndianDouble(payload, 0.0);
  payload.push_back(0x00);
  payload.push_back(0x00);
  appendBigEndianDouble(payload, 0.0);
  appendBigEndianDouble(payload, 0.0);
  appendBigEndianFloat(payload, 20.0f);
  appendBigEndianFloat(payload, 48.0f);
  appendBigEndianFloat(payload, 0.0f);
  appendBigEndianFloat(payload, 0.0f);
  payload.push_back(0x02);                          // safety mode (REDUCED)
  payload.push_back(0x01);                          // in reduced mode (true)
  payload.push_back(0x01);                          // immi interface installed (true)
  appendBigEndian<uint32_t>(payload, 0x11112222u);  // immi input bits
  appendBigEndian<uint32_t>(payload, 0x33334444u);  // immi output bits
  appendBigEndianFloat(payload, 24.0f);             // immi voltage 24V
  appendBigEndianFloat(payload, 0.75f);             // immi current
  appendBigEndian<uint32_t>(payload, 0u);           // reserved
  payload.push_back(0x00);
  payload.push_back(0x00);
  payload.push_back(0x00);

  ASSERT_EQ(payload.size(), MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_OPTIONAL_IMMI_BYTES +
                                MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI);

  std::vector<uint8_t> packet = makeRobotStatePacketWithMasterboardSubmessage(payload);
  comm::BinParser bp(packet.data(), packet.size());
  std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
  ASSERT_TRUE(parser_.parse(bp, products));
  ASSERT_EQ(products.size(), 1u);

  auto* data = dynamic_cast<primary_interface::MasterboardData*>(products[0].get());
  ASSERT_NE(data, nullptr);
  EXPECT_EQ(data->safety_mode_, SafetyMode::REDUCED);
  EXPECT_TRUE(data->in_reduced_mode_);
  EXPECT_TRUE(data->immi_interface_installed_);
  EXPECT_EQ(data->immi_input_bits_, std::bitset<32>(0x11112222u));
  EXPECT_EQ(data->immi_output_bits_, std::bitset<32>(0x33334444u));
  EXPECT_FLOAT_EQ(data->immi_voltage_24v_, 24.0f);
  EXPECT_FLOAT_EQ(data->immi_current_, 0.75f);

  auto msg_str = data->toString();  // also verify that the presence of IMMI fields does not cause issues in the
  // toString() method
  std::cout << msg_str << std::endl;
  EXPECT_NE(msg_str.find("IMMI interface installed: true"), std::string::npos);
  EXPECT_NE(msg_str.find("IMMI input bits: 0b00010001000100010010001000100010"), std::string::npos);
  EXPECT_NE(msg_str.find("IMMI output bits: 0b00110011001100110100010001000100"), std::string::npos);
  EXPECT_NE(msg_str.find("IMMI voltage 24V: 24"), std::string::npos);
  EXPECT_NE(msg_str.find("IMMI current: 0.75"), std::string::npos);
}

int main(int argc, char* argv[])
{
  ::testing::InitGoogleTest(&argc, argv);

  return RUN_ALL_TESTS();
}
