#include "PacketAccess.h"


namespace
{
  void setValue(std::vector<char>& packet, unsigned long value, size_t byte_size)
  {
    for (size_t i = 0; i < byte_size; ++i) {
      packet.push_back(value >> (8 * (byte_size - i - 1)));
    }
    ++packet[6];
  }


  unsigned long getValue(unsigned char*& p, size_t byte_size)
  {
    unsigned long value = 0;
    for (size_t i = 0; i < byte_size; ++i) {
      value <<= 8;
      value |= *p++;
    }
    return value;
  }
}


void robot_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(0);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_system_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(4);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_system_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_system_major_set(std::vector<char>& packet, unsigned char value)
{
  packet.push_back(5);
  size_t byte_size = sizeof(unsigned char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


unsigned char robot_system_major_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<unsigned char>(getValue(p, byte_size));
}


void robot_system_minor_set(std::vector<char>& packet, unsigned char value)
{
  packet.push_back(6);
  size_t byte_size = sizeof(unsigned char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


unsigned char robot_system_minor_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<unsigned char>(getValue(p, byte_size));
}


void robot_system_micro_set(std::vector<char>& packet, unsigned char value)
{
  packet.push_back(7);
  size_t byte_size = sizeof(unsigned char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


unsigned char robot_system_micro_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<unsigned char>(getValue(p, byte_size));
}


void robot_system_sec_set(std::vector<char>& packet, unsigned long value)
{
  packet.push_back(8);
  size_t byte_size = sizeof(unsigned long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


unsigned long robot_system_sec_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<unsigned long>(getValue(p, byte_size));
}


void robot_system_msec_set(std::vector<char>& packet, unsigned short value)
{
  packet.push_back(12);
  size_t byte_size = sizeof(unsigned short);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


unsigned short robot_system_msec_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<unsigned short>(getValue(p, byte_size));
}


void robot_system_mode_set(std::vector<char>& packet, ControlMode value)
{
  packet.push_back(16);
  size_t byte_size = sizeof(ControlMode);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


ControlMode robot_system_mode_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<ControlMode>(getValue(p, byte_size));
}


void robot_encoder_0_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(20);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_encoder_0_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_encoder_1_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(26);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_encoder_1_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_motor_0_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(32);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_motor_0_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_motor_0_gain_p_set(std::vector<char>& packet, long value)
{
  packet.push_back(36);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_motor_0_gain_p_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_motor_0_gain_i_set(std::vector<char>& packet, long value)
{
  packet.push_back(40);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_motor_0_gain_i_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_motor_1_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(44);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_motor_1_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_motor_1_gain_p_set(std::vector<char>& packet, long value)
{
  packet.push_back(48);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_motor_1_gain_p_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_motor_1_gain_i_set(std::vector<char>& packet, long value)
{
  packet.push_back(52);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_motor_1_gain_i_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_body_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(72);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_body_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_body_rotate_coeffecient_set(std::vector<char>& packet, long value)
{
  packet.push_back(76);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_body_rotate_coeffecient_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_position_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(80);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_position_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_position_mm_0_set(std::vector<char>& packet, long value)
{
  packet.push_back(84);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_position_mm_0_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_position_mm_1_set(std::vector<char>& packet, long value)
{
  packet.push_back(88);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_position_mm_1_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_position_dir16_set(std::vector<char>& packet, unsigned short value)
{
  packet.push_back(92);
  size_t byte_size = sizeof(unsigned short);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


unsigned short robot_position_dir16_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<unsigned short>(getValue(p, byte_size));
}


void robot_follow_position_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(116);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_follow_position_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_follow_position_mm_0_set(std::vector<char>& packet, long value)
{
  packet.push_back(120);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_follow_position_mm_0_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_follow_position_mm_1_set(std::vector<char>& packet, long value)
{
  packet.push_back(124);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_follow_position_mm_1_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_follow_position_dir16_set(std::vector<char>& packet, unsigned short value)
{
  packet.push_back(128);
  size_t byte_size = sizeof(unsigned short);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


unsigned short robot_follow_position_dir16_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<unsigned short>(getValue(p, byte_size));
}


void robot_path_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(152);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_path_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_path_reset_offset_set(std::vector<char>& packet, char value)
{
  packet.push_back(153);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_path_reset_offset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_path_mode_set(std::vector<char>& packet, PathMode value)
{
  packet.push_back(156);
  size_t byte_size = sizeof(PathMode);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


PathMode robot_path_mode_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<PathMode>(getValue(p, byte_size));
}


void robot_path_rotate_direction_set(std::vector<char>& packet, long value)
{
  packet.push_back(164);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_path_rotate_direction_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_path_circle_radius_set(std::vector<char>& packet, long value)
{
  packet.push_back(168);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_path_circle_radius_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_straight_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(172);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_straight_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_straight_is_stable_set(std::vector<char>& packet, char value)
{
  packet.push_back(173);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_straight_is_stable_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_straight_target_velocity_set(std::vector<char>& packet, long value)
{
  packet.push_back(176);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_straight_target_velocity_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_straight_control_velocity_set(std::vector<char>& packet, long value)
{
  packet.push_back(180);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_straight_control_velocity_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_straight_control_acceleration_set(std::vector<char>& packet, long value)
{
  packet.push_back(184);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_straight_control_acceleration_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_rotate_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(188);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_rotate_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_rotate_is_stable_set(std::vector<char>& packet, char value)
{
  packet.push_back(189);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_rotate_is_stable_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_rotate_target_velocity_set(std::vector<char>& packet, long value)
{
  packet.push_back(192);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_rotate_target_velocity_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_rotate_control_velocity_set(std::vector<char>& packet, long value)
{
  packet.push_back(196);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_rotate_control_velocity_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_rotate_control_acceleration_set(std::vector<char>& packet, long value)
{
  packet.push_back(200);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_rotate_control_acceleration_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_direct_reset_set(std::vector<char>& packet, char value)
{
  packet.push_back(204);
  size_t byte_size = sizeof(char);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


char robot_direct_reset_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<char>(getValue(p, byte_size));
}


void robot_direct_wheel_0_target_velocity_set(std::vector<char>& packet, long value)
{
  packet.push_back(212);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_direct_wheel_0_target_velocity_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}


void robot_direct_wheel_1_target_velocity_set(std::vector<char>& packet, long value)
{
  packet.push_back(228);
  size_t byte_size = sizeof(long);
  packet.push_back(byte_size);
  setValue(packet, value, byte_size);
}


long robot_direct_wheel_1_target_velocity_get(unsigned char*& p)
{
  ++p;
  size_t byte_size = *p++;
  return static_cast<long>(getValue(p, byte_size));
}
