MARobotState.cpp

/*
 *  This file is part of the AiBO+ project
 *
 *  Copyright (C) 2005-2016 Csaba Kertész (csaba.kertesz@gmail.com)
 *
 *  AiBO+ is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  AiBO+ is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Street #330, Boston, MA 02111-1307, USA.
 *
 */

#include "MARobotState.hpp"

#include "core/MANum.hpp"
#include "ml/MAClassifierModels.hpp"
#include "MAAudio.hpp"
#include "MABodyMotion.hpp"
#include "MAComplexIndicators.hpp"
#include "MAEmotions.hpp"
#include "MAGoals.hpp"
#include "MAHead.hpp"
#include "MAHri.hpp"
#include "MATorso.hpp"
#include "MATypeRanges.hpp"

#include <MCBinaryData.hpp>
#include <MCContainers.hpp>
#include <MCLog.hpp>

#include <boost/algorithm/string.hpp>

#define BIND_VARIABLE(_group, _variable, _display_name) \
  BindVariable(#_group "::" #_variable, _group->_variable, _display_name);

#define BIND_VARIABLE_WITH_NAME(_group, _variable, _variable_name, _display_name) \
  BindVariable(#_group "::" #_variable_name, _group->_variable, _display_name);

namespace MA
{
MCThreadLocalData<MARobotState> RobotState(true);
}

namespace
{
// Maximum number elements for the string cache
static const int MaxSize = 1000;

// Wrapper class to avoid using thread local data in the string query
struct CacheWrapper
{
  ~CacheWrapper()
  {
    for (auto str : Strs)
      delete str;
  }

  MC::StringPtrList Strs;
};
// Variable names
MCThreadLocalData<CacheWrapper> StringCache(true);

void CheckStaticStringCache()
{
  if (unlikely(!StringCache.get()))
  {
    StringCache.reset(new CacheWrapper);
    StringCache->Strs.reserve(MaxSize);
    StringCache->Strs.push_back(new std::string(""));
  }
}

int16_t GetStrIndex(const std::string& new_string)
{
  auto& Strs = StringCache->Strs;

  foreach_i (i, str, Strs)
  {
    if (**str == new_string)
      return (int16_t)i;
  }
  Strs.push_back(new std::string(new_string));
  if (unlikely(MaxSize < Strs.size()))
  {
    MC_WARNING("The robot state string cache is over its limit\n");
  }
  return (int16_t)(Strs.size()-1);
}
}

MARobotState::MARobotState(bool static_instance, bool updates) : SignalConnections()
{
  CheckStaticStringCache();
  StrBasePtr = &StringCache->Strs[0];
  Init(static_instance, updates);
}


MARobotState::MARobotState(const MARobotState& other) : SignalConnections()
{
  Init(false, false);
  operator=(other);
}


MARobotState::~MARobotState()
{
  if (MA::RobotState.get() == this)
  {
    MA::RobotState.release();
  }
  RemoveCustomVariables();
}


void MARobotState::Init(bool static_instance, bool updates)
{
  if (static_instance)
  {
    if (MA::RobotState.get())
    {
      MC_WARNING("The global robot state is overridden.");
    }
    MA::RobotState.reset(this);
  }
  Torso.reset(new MATorso);
  LegLF.reset(new MALeg);
  LegLF->Type = MA::Leg::LF;
  LegLH.reset(new MALeg);
  LegLH->Type = MA::Leg::LH;
  LegRF.reset(new MALeg);
  LegRF->Type = MA::Leg::RF;
  LegRH.reset(new MALeg);
  LegRH->Type = MA::Leg::RH;
  Head.reset(new MAHead);
  BodyMotion.reset(new MABodyMotion);
  Audio.reset(new MAAudio);
  Emotions.reset(new MAEmotions);
  ComplexIndicators.reset(new MAComplexIndicators);
  Goals.reset(new MAGoals);
  Hri.reset(new MAHri);
  if (updates)
  {
    MARobotStateUpdater::RegisterUpdater(*Torso);
    MARobotStateUpdater::RegisterUpdater(*BodyMotion);
    MARobotStateUpdater::RegisterUpdater(*LegLF);
    MARobotStateUpdater::RegisterUpdater(*LegLH);
    MARobotStateUpdater::RegisterUpdater(*LegRF);
    MARobotStateUpdater::RegisterUpdater(*LegRH);
    MARobotStateUpdater::RegisterUpdater(*Head);
    MARobotStateUpdater::RegisterUpdater(*Audio);
    MARobotStateUpdater::RegisterUpdater(*Emotions);
    MARobotStateUpdater::RegisterUpdater(*ComplexIndicators);
    MARobotStateUpdater::RegisterUpdater(*Goals);
  }
  // Bind the variables
  std::string SubOpen = "<sub>";
  std::string SubClose = "</sub>";

  // Torso
  BIND_VARIABLE(Torso, BuildID, "BuildID"+SubOpen+"ID"+SubClose);
  BIND_VARIABLE(Torso, SessionID, "SessionID"+SubOpen+"ID"+SubClose);
  BIND_VARIABLE(Torso, CycleID, "Cycle"+SubOpen+"ID"+SubClose);
  BIND_VARIABLE(Torso, CycleTime, "Cycle"+SubOpen+"time"+SubClose);
  BIND_VARIABLE(Torso, FreeMemory, "Memory"+SubOpen+"free"+SubClose);
  BIND_VARIABLE(Torso, Timestamp.Seconds, "Timestamp"+SubOpen+"sec"+SubClose);
  BIND_VARIABLE(Torso, Timestamp.Milliseconds, "msec"+SubClose);
  BIND_VARIABLE(Torso, Timestamp.Microseconds, "Timestamp"+SubOpen+"µsec"+SubClose);
  BIND_VARIABLE(Torso, BatteryCapacity, "Battery"+SubOpen+"capacity"+SubClose+" (mAh)");
  BIND_VARIABLE(Torso, BatteryLevel, "Battery"+SubOpen+"level"+SubClose+" (%)");
  BIND_VARIABLE(Torso, PowerConsumption, "Power usage (mW)");
  BIND_VARIABLE(Torso, EstimatedSessionPowerConsumption, "PC"+SubOpen+"estimated"+SubClose+" (mWh)");
  BIND_VARIABLE(Torso, RealSessionPowerConsumption, "PC"+SubOpen+"real"+SubClose+" (mWh)");
  BIND_VARIABLE(Torso, CpuLoad.All, "CPU"+SubOpen+"all"+SubClose+" (%)");
  BIND_VARIABLE(Torso, CpuLoad.Behaviors, "CPU"+SubOpen+"behaviors"+SubClose+" (%)");
  BIND_VARIABLE(Torso, CpuLoad.Camera, "CPU"+SubOpen+"camera"+SubClose+" (%)");
  BIND_VARIABLE(Torso, CpuLoad.Network, "CPU"+SubOpen+"network"+SubClose+" (%)");
  BIND_VARIABLE(Torso, CpuLoad.Audio, "CPU"+SubOpen+"audio"+SubClose+" (%)");
  BIND_VARIABLE(Torso, WlanStats.Link, "WLAN"+SubOpen+"link"+SubClose+" (%)");
  BIND_VARIABLE(Torso, WlanStats.Signal, "WLAN"+SubOpen+"signal"+SubClose+" (%)");
  BIND_VARIABLE(Torso, WlanStats.Noise, "WLAN"+SubOpen+"noise"+SubClose+" (%)");
  BIND_VARIABLE(Torso, Temperature, "Temperature (°C)");
  BIND_VARIABLE(Torso, Vibration, "Vibration");
  BIND_VARIABLE(Torso, DataUpload, "DataUpload (bytes)");
  BIND_VARIABLE(Torso, TailPan, "Tail"+SubOpen+"pan"+SubClose);
  BIND_VARIABLE(Torso, TailPanForce, "Tail"+SubOpen+"pan,force"+SubClose);
  BIND_VARIABLE(Torso, TailTilt, "Tail"+SubOpen+"tilt"+SubClose);
  BIND_VARIABLE(Torso, TailTiltForce, "Tail"+SubOpen+"tile,force"+SubClose);
  BIND_VARIABLE(Torso, BackButtonF, "Button"+SubOpen+"back,fore"+SubClose);
  BIND_VARIABLE(Torso, BackButtonM, "Button"+SubOpen+"back,middle"+SubClose);
  BIND_VARIABLE(Torso, BackButtonR, "Button"+SubOpen+"back,rear"+SubClose);
  BIND_VARIABLE(Torso, ChestIR, "IR"+SubOpen+"chest"+SubClose);
  BIND_VARIABLE(Torso, NearIR, "IR"+SubOpen+"near"+SubClose);
  BIND_VARIABLE(Torso, FarIR, "IR"+SubOpen+"far"+SubClose);
  BIND_VARIABLE(Torso, BackStroked, "Back"+SubOpen+"stroked"+SubClose);
  BIND_VARIABLE(Torso, BackGrabbed, "Back"+SubOpen+"grabbed"+SubClose);
  BIND_VARIABLE(Torso, BackFButtonPressed, "Button"+SubOpen+"bf,pressed"+SubClose);
  BIND_VARIABLE(Torso, BackMButtonPressed, "Button"+SubOpen+"bm,pressed"+SubClose);
  BIND_VARIABLE(Torso, BackRButtonPressed, "Button"+SubOpen+"br,pressed"+SubClose);
  BIND_VARIABLE(Torso, NightTime, "NightTime");
  BIND_VARIABLE(Torso, NetworkConnection, "NetworkConnection");
  BIND_VARIABLE(Torso, ConnectedClients, "Clients");
  BIND_VARIABLE(Torso, Debug, "Debug");
  // Body motion
  BIND_VARIABLE(BodyMotion, OnStation, "OnStation");
  BIND_VARIABLE(BodyMotion, AccelX.Raw, "Accel"+SubOpen+"x,raw"+SubClose);
  BIND_VARIABLE(BodyMotion, AccelX.Degree, "Accel"+SubOpen+"x,degree"+SubClose);
  BIND_VARIABLE(BodyMotion, AccelY.Raw, "Accel"+SubOpen+"y,raw"+SubClose);
  BIND_VARIABLE(BodyMotion, AccelY.Degree, "Accel"+SubOpen+"y,degree"+SubClose);
  BIND_VARIABLE(BodyMotion, AccelZ.Raw, "Accel"+SubOpen+"z,raw"+SubClose);
  BIND_VARIABLE(BodyMotion, AccelZ.Degree, "Accel"+SubOpen+"z,degree"+SubClose);
  BIND_VARIABLE(BodyMotion, BodyPitch, "Body"+SubOpen+"pitch"+SubClose);
  BIND_VARIABLE(BodyMotion, BodyRoll, "Body"+SubOpen+"roll"+SubClose);
  BIND_VARIABLE(BodyMotion, UpsideDown, "UpsideDown");
  BIND_VARIABLE(BodyMotion, LateralPosition, "LateralPosition");
  BIND_VARIABLE(BodyMotion, HeadStand, "HeadStand");
  BIND_VARIABLE(BodyMotion, LyingOnBack, "Lying"+SubOpen+"onback"+SubClose);
  BIND_VARIABLE(BodyMotion, LyingOnBackLeftSide, "Lying"+SubOpen+"onback,left"+SubClose);
  BIND_VARIABLE(BodyMotion, LyingOnBackRightSide, "Lying"+SubOpen+"onback,right"+SubClose);
  BIND_VARIABLE(BodyMotion, BodyOnLeftSide, "BodyOnLeftSide");
  BIND_VARIABLE(BodyMotion, BodyOnRightSide, "BodyOnRightSide");
  BIND_VARIABLE(BodyMotion, Lying, "Lying");
  BIND_VARIABLE(BodyMotion, LyingOnLeftSide, "Lying"+SubOpen+"onleftside"+SubClose);
  BIND_VARIABLE(BodyMotion, LyingOnRightSide, "Lying"+SubOpen+"onrightside"+SubClose);
  BIND_VARIABLE(BodyMotion, Sitting, "Sitting");
  BIND_VARIABLE(BodyMotion, Flying, "Flying");
  BIND_VARIABLE(BodyMotion, Poked, "Poked");
  BIND_VARIABLE(BodyMotion, SkitterPlayback, "SkitterPlayback");
  BIND_VARIABLE(BodyMotion, Floor.Surface, "Flooring"+SubOpen+"surface"+SubClose);
  BIND_VARIABLE(BodyMotion, Floor.CurrentSurface, "Flooring"+SubOpen+"currentsurface"+SubClose);
  BIND_VARIABLE(BodyMotion, Floor.OwnerFeedback, "Flooring"+SubOpen+"ownerfeedback"+SubClose);
  BIND_VARIABLE(BodyMotion, BodyState, "Body"+SubOpen+"state"+SubClose);
  BIND_VARIABLE(BodyMotion, InMotion, "Body"+SubOpen+"inmotion"+SubClose);
  BIND_VARIABLE(BodyMotion, SkitPlayback, "SkitPlayback");
  BIND_VARIABLE(BodyMotion, MotionControlAge, "MotionControl"+SubOpen+"age"+SubClose);
  // LegLF
  BIND_VARIABLE(LegLF, Paw, "Paw"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, Joint1, "Joint1"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, Force1, "Force1"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, Joint2, "Joint2"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, Force2, "Force2"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, Joint3, "Joint3"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, Force3, "Force3"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, PoweredJoints, "Powered joints"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, ForeBack, "ForeBack"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, UpDown, "UpDown"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, Sidelong, "Sidelong"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, BentStretched, "BentStretched"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, Stopped, "Stopped"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, Overloaded, "Overloaded"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, Moving, "Moving"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, InMotion, "InMotion"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, IsLyingPosition, "IsLyingPosition"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, IsStandingPosition, "IsStandingPosition"+SubOpen+"LF"+SubClose);
  BIND_VARIABLE(LegLF, IsSittingPosition, "IsSittingPosition"+SubOpen+"LF"+SubClose);
  // LegLH
  BIND_VARIABLE(LegLH, Paw, "Paw"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, Joint1, "Joint1"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, Force1, "Force1"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, Joint2, "Joint2"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, Force2, "Force2"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, Joint3, "Joint3"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, Force3, "Force3"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, PoweredJoints, "Powered joints"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, ForeBack, "ForeBack"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, UpDown, "UpDown"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, Sidelong, "Sidelong"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, BentStretched, "BentStretched"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, Stopped, "Stopped"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, Overloaded, "Overloaded"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, Moving, "Moving"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, InMotion, "InMotion"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, IsLyingPosition, "IsLyingPosition"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, IsStandingPosition, "IsStandingPosition"+SubOpen+"LH"+SubClose);
  BIND_VARIABLE(LegLH, IsSittingPosition, "IsSittingPosition"+SubOpen+"LH"+SubClose);
  // LegRF
  BIND_VARIABLE(LegRF, Paw, "Paw"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, Joint1, "Joint1"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, Force1, "Force1"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, Joint2, "Joint2"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, Force2, "Force2"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, Joint3, "Joint3"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, Force3, "Force3"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, PoweredJoints, "Powered joints"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, ForeBack, "ForeBack"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, UpDown, "UpDown"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, Sidelong, "Sidelong"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, BentStretched, "BentStretched"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, Stopped, "Stopped"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, Overloaded, "Overloaded"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, Moving, "Moving"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, InMotion, "InMotion"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, IsLyingPosition, "IsLyingPosition"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, IsStandingPosition, "IsStandingPosition"+SubOpen+"RF"+SubClose);
  BIND_VARIABLE(LegRF, IsSittingPosition, "IsSittingPosition"+SubOpen+"RF"+SubClose);
  // LegRH
  BIND_VARIABLE(LegRH, Paw, "Paw"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, Joint1, "Joint1"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, Force1, "Force1"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, Joint2, "Joint2"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, Force2, "Force2"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, Joint3, "Joint3"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, Force3, "Force3"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, PoweredJoints, "Powered joints"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, ForeBack, "ForeBack"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, UpDown, "UpDown"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, Sidelong, "Sidelong"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, BentStretched, "BentStretched"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, Stopped, "Stopped"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, Overloaded, "Overloaded"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, Moving, "Moving"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, InMotion, "InMotion"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, IsLyingPosition, "IsLyingPosition"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, IsStandingPosition, "IsStandingPosition"+SubOpen+"RH"+SubClose);
  BIND_VARIABLE(LegRH, IsSittingPosition, "IsSittingPosition"+SubOpen+"RH"+SubClose);
  // Head
  BIND_VARIABLE(Head, Pan, "Neck"+SubOpen+"pan"+SubClose);
  BIND_VARIABLE(Head, PanForce, "Neck"+SubOpen+"pan,force"+SubClose);
  BIND_VARIABLE(Head, Tilt1, "Neck"+SubOpen+"tilt1"+SubClose);
  BIND_VARIABLE(Head, Tilt1Force, "Nec"+SubOpen+"tilt1,force"+SubClose);
  BIND_VARIABLE(Head, Tilt2, "Neck"+SubOpen+"tilt2"+SubClose);
  BIND_VARIABLE(Head, Tilt2Force, "Neck"+SubOpen+"tilt2,force"+SubClose);
  BIND_VARIABLE(Head, Moving, "Head"+SubOpen+"moving"+SubClose);
  BIND_VARIABLE(Head, InMotion, "Head"+SubOpen+"inmotion"+SubClose);
  BIND_VARIABLE(Head, Mouth, "Mouth");
  BIND_VARIABLE(Head, MouthForce, "Mouth"+SubOpen+"force"+SubClose);
  BIND_VARIABLE(Head, MouthMoving, "Mouth"+SubOpen+"moving"+SubClose);
  BIND_VARIABLE(Head, MouthClosed, "Mouth"+SubOpen+"closed"+SubClose);
  BIND_VARIABLE(Head, MouthStopped, "Mouth"+SubOpen+"stopped"+SubClose);
  BIND_VARIABLE(Head, CameraBrightness, "Camera"+SubOpen+"brightness"+SubClose);
  BIND_VARIABLE(Head, LeftRight, "Head"+SubOpen+"left,right"+SubClose);
  BIND_VARIABLE(Head, ChinButton, "Chin"+SubOpen+"button"+SubClose);
  BIND_VARIABLE(Head, CrownButton, "Crown"+SubOpen+"button"+SubClose);
  BIND_VARIABLE(Head, ForeBack, "Head"+SubOpen+"fore,back"+SubClose);
  BIND_VARIABLE(Head, UpDown, "Head"+SubOpen+"up,down"+SubClose);
  BIND_VARIABLE(Head, CrownPressed, "Crown"+SubOpen+"pressed"+SubClose);
  BIND_VARIABLE(Head, ChinStroked, "Chin"+SubOpen+"stroked"+SubClose);
  BIND_VARIABLE(Head, CrownStroked, "Crown"+SubOpen+"stroked"+SubClose);
  BIND_VARIABLE(Head, PushedLeft, "Head"+SubOpen+"pushed,left"+SubClose);
  BIND_VARIABLE(Head, PushedRight, "Head"+SubOpen+"pushed,right"+SubClose);
  BIND_VARIABLE(Head, DaylightLevel, "DaylightLevel");
  BIND_VARIABLE(Head, SleptAwaken, "SleptAwaken");
  BIND_VARIABLE(Head, EyePupilSize, "EyePupilSize");
  // Audio
  BIND_VARIABLE(Audio, FullVolume, "FullVolume");
  BIND_VARIABLE(Audio, SpeakerVolume, "Speaker volume");
  BIND_VARIABLE(Audio, RemainingPlaybackTime, "Remaining playback time (msec)");
  BIND_VARIABLE(Audio, LeftMicrophonePower, "Microphone"+SubOpen+"left,power"+SubClose);
  BIND_VARIABLE(Audio, RightMicrophonePower, "Microphone"+SubOpen+"right,power"+SubClose);
  BIND_VARIABLE(Audio, BackgroundSoundPower, "Sound"+SubOpen+"background,power"+SubClose);
  BIND_VARIABLE(Audio, CurrentSoundEvent, "Sound"+SubOpen+"event,current"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[1], SoundEventVotes1, "Sound"+SubOpen+"votes,ambient_noise"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[2], SoundEventVotes2, "Sound"+SubOpen+"votes,baby_cry"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[3], SoundEventVotes3, "Sound"+SubOpen+"votes,bell"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[4], SoundEventVotes4, "Sound"+SubOpen+"votes,cat_meow"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[5], SoundEventVotes5, "Sound"+SubOpen+"votes,cicada"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[6], SoundEventVotes6, "Sound"+SubOpen+"votes,dog_bark"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[7], SoundEventVotes7, "Sound"+SubOpen+"votes,fart"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[8], SoundEventVotes8, "Sound"+SubOpen+"votes,guitar"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[9], SoundEventVotes9, "Sound"+SubOpen+"votes,laugh"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[10], SoundEventVotes10, "Sound"+SubOpen+"votes,parrot"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[11], SoundEventVotes11, "Sound"+SubOpen+"votes,piano"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[12], SoundEventVotes12, "Sound"+SubOpen+"votes,speech"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[13], SoundEventVotes13, "Sound"+SubOpen+"votes,traffic"+SubClose);
  BIND_VARIABLE_WITH_NAME(Audio, SoundEventVotes[14], SoundEventVotes14, "Sound"+SubOpen+"votes,vacuum_cleaner"+SubClose);
  BIND_VARIABLE(Audio, Deafness, "Deafness");
  BIND_VARIABLE(Audio, EffectsPlayed, "EffectsPlayed");
  // Emotions
  BIND_VARIABLE(Emotions, Happy, "Emotion"+SubOpen+"happy"+SubClose);
  BIND_VARIABLE(Emotions, Angry, "Emotion"+SubOpen+"angry"+SubClose);
  BIND_VARIABLE(Emotions, Sad, "Emotion"+SubOpen+"sad"+SubClose);
  BIND_VARIABLE(Emotions, Bored, "Emotion"+SubOpen+"bored"+SubClose);
  // Complex indicators
  BIND_VARIABLE(ComplexIndicators, IsLyingPosture, "IsLyingPosture");
  BIND_VARIABLE(ComplexIndicators, IsStandingPosture, "IsStandingPosture");
  BIND_VARIABLE(ComplexIndicators, IsSittingPosture, "IsSittingPosture");
  BIND_VARIABLE(ComplexIndicators, WalkPeriod, "WalkPeriod");
  BIND_VARIABLE(ComplexIndicators, ElapsedForwardWalkTime, "ElapsedForwardWalkTime");
  BIND_VARIABLE(ComplexIndicators, ElapsedBackwardWalkTime, "ElapsedBackwardWalkTime");
  BIND_VARIABLE(ComplexIndicators, ElapsedLeftTurnTime, "ElapsedLeftTurnTime");
  BIND_VARIABLE(ComplexIndicators, ElapsedRightTurnTime, "ElapsedRightTurnTime");
  BIND_VARIABLE(ComplexIndicators, NearObjectForward, "Object"+SubOpen+"near"+SubClose);
  BIND_VARIABLE(ComplexIndicators, AbyssWhileWalking, "Abyss"+SubOpen+"walking"+SubClose);
  BIND_VARIABLE(ComplexIndicators, AbyssWhileLying, "Abyss"+SubOpen+"lying"+SubClose);
  BIND_VARIABLE(ComplexIndicators, BodyOutOfWalkingRange, "Body"+SubOpen+"notwalking"+SubClose);
  BIND_VARIABLE(ComplexIndicators, OverloadDangerWhileSitting, "Sitting"+SubOpen+"legoverload"+SubClose);
  BIND_VARIABLE(ComplexIndicators, PoseTransition, "PoseTransition");
  BIND_VARIABLE(ComplexIndicators, ListenWithEars, "ListenWithEars");
  BIND_VARIABLE(ComplexIndicators, LeftMicrophoneDominance, "Microphone"+SubOpen+"left,dom"+SubClose);
  BIND_VARIABLE(ComplexIndicators, RightMicrophoneDominance, "Microphone"+SubOpen+"right,dom"+SubClose);
  BIND_VARIABLE(ComplexIndicators, MicrophoneChannelsBalance, "Microphone"+SubOpen+"bal,dom"+SubClose);
  // Goals
  BIND_VARIABLE(Goals, SitPostureDesire, "Desire"+SubOpen+"sit"+SubClose);
  BIND_VARIABLE(Goals, StandPostureDesire, "Desire"+SubOpen+"stand"+SubClose);
  BIND_VARIABLE(Goals, LiePostureDesire, "Desire"+SubOpen+"lie"+SubClose);
  BIND_VARIABLE(Goals, DesiredForwardWalkDuration, "Desire"+SubOpen+"walk,forward"+SubClose);
  BIND_VARIABLE(Goals, DesiredBackwardWalkDuration, "Desire"+SubOpen+"walk,backward"+SubClose);
  BIND_VARIABLE(Goals, DesiredLeftTurnDuration, "Desire"+SubOpen+"walk,left"+SubClose);
  BIND_VARIABLE(Goals, DesiredRightTurnDuration, "Desire"+SubOpen+"walk,right"+SubClose);
  BIND_VARIABLE(Goals, DesireLyingOnTheFloor, "Desire"+SubOpen+"turnover"+SubClose);
  BIND_VARIABLE(Goals, ArousalFeedbackNeeded, "ArousalFeedback"+SubOpen+"needed"+SubClose);
  BIND_VARIABLE(Goals, ListenToAudioDesire, "Desire"+SubOpen+"listen audio"+SubClose);
  BIND_VARIABLE(Goals, DiscoverSoundSource, "Discover"+SubOpen+"sound,source"+SubClose);
  BIND_VARIABLE(Goals, HappyOnCarpetFeedback, "HappyFeedback"+SubOpen+"carpet"+SubClose);
  BIND_VARIABLE(Goals, TurnHeadToLeft, "Head"+SubOpen+"turnleft"+SubClose);
  BIND_VARIABLE(Goals, TurnHeadToRight, "Head"+SubOpen+"turnright"+SubClose);
  BIND_VARIABLE(Goals, MotionControl, "MotionControl");
  // Human-robot interaction data
  BIND_VARIABLE(Hri, GambledNumber, "Number"+SubOpen+"gambled"+SubClose);
  BIND_VARIABLE(Hri, GuessedNumber, "Number"+SubOpen+"guessed"+SubClose);

  // Value notes for floor surface
  MA::ValueNoteList FloorSurfaceNotes;

  FloorSurfaceNotes.push_back(MA::ValueNoteType((int)MA::UnknownFloor, *(StrBasePtr+(int)GetStrIndex("Unknown"))));
  FloorSurfaceNotes.push_back(MA::ValueNoteType((int)MA::WoodFlooring, *(StrBasePtr+(int)GetStrIndex("Wood"))));
  FloorSurfaceNotes.push_back(MA::ValueNoteType((int)MA::Field, *(StrBasePtr+(int)GetStrIndex("Field"))));
  FloorSurfaceNotes.push_back(MA::ValueNoteType((int)MA::Carpet, *(StrBasePtr+(int)GetStrIndex("Carpet"))));
  FloorSurfaceNotes.push_back(MA::ValueNoteType((int)MA::Vinyl, *(StrBasePtr+(int)GetStrIndex("Vinyl"))));
  FloorSurfaceNotes.push_back(MA::ValueNoteType((int)MA::Tiles, *(StrBasePtr+(int)GetStrIndex("Tiles"))));
  FloorSurfaceNotes.push_back(MA::ValueNoteType((int)MA::CarpetedFloor, *(StrBasePtr+(int)GetStrIndex("Carpeted floor"))));
  FloorSurfaceNotes.push_back(MA::ValueNoteType((int)MA::UserDefinedFloor, *(StrBasePtr+(int)GetStrIndex("User defined"))));
  VariableValueNotes.insert(std::make_pair(&BodyMotion->Floor.Surface, FloorSurfaceNotes));
  VariableValueNotes.insert(std::make_pair(&BodyMotion->Floor.CurrentSurface, FloorSurfaceNotes));
  // Safety limit for LF1/RF1 joints while sitting (?)
  MA::ValueNoteList SafetyLimitForce1;

  SafetyLimitForce1.push_back(MA::ValueNoteType(MA::ForeLegForce1SittingLimit, *(StrBasePtr+(int)GetStrIndex("Limit"))));
  SafetyLimitForce1.push_back(MA::ValueNoteType(MA::ForeLegForce1SittingEmergencyLimit,
                              *(StrBasePtr+(int)GetStrIndex("Emergency"))));
  VariableValueNotes.insert(std::make_pair(&LegLF->Force1, SafetyLimitForce1));
  VariableValueNotes.insert(std::make_pair(&LegRF->Force1, SafetyLimitForce1));
  // Safety limit for LF3/RF3 joints while sitting (?)
  MA::ValueNoteList SafetyLimitForce3;

  SafetyLimitForce3.push_back(MA::ValueNoteType(MA::ForeLegForce3SittingLimit, *(StrBasePtr+(int)GetStrIndex("Limit"))));
  VariableValueNotes.insert(std::make_pair(&LegLF->Force3, SafetyLimitForce3));
  VariableValueNotes.insert(std::make_pair(&LegRF->Force3, SafetyLimitForce3));
  // Value notes for body state
  MA::ValueNoteList BodyStateNotes;

  BodyStateNotes.push_back(MA::ValueNoteType((int)MA::UnknownBodyState, *(StrBasePtr+(int)GetStrIndex("Unknown"))));
  BodyStateNotes.push_back(MA::ValueNoteType((int)MA::NormalBodyState, *(StrBasePtr+(int)GetStrIndex("Normal"))));
  BodyStateNotes.push_back(MA::ValueNoteType((int)MA::PickupMode, *(StrBasePtr+(int)GetStrIndex("Pick-up"))));
  BodyStateNotes.push_back(MA::ValueNoteType((int)MA::FallingOver, *(StrBasePtr+(int)GetStrIndex("Falling over"))));
  BodyStateNotes.push_back(MA::ValueNoteType((int)MA::Poked, *(StrBasePtr+(int)GetStrIndex("Poked"))));
  VariableValueNotes.insert(std::make_pair(&BodyMotion->BodyState, BodyStateNotes));
  // Value notes for sound events
  MA::ValueNoteList SoundEventNotes;

  for (unsigned int i = 0; i < MA::SoundEventTypeStrs.size(); ++i)
  {
    SoundEventNotes.push_back(MA::ValueNoteType(i+1, *(StrBasePtr+(int)GetStrIndex(MA::SoundEventTypeStrs[i]))));
  }
  VariableValueNotes.insert(std::make_pair(&Audio->CurrentSoundEvent, SoundEventNotes));
}


MALeg& MARobotState::GetLeg(MA::Leg::LegTypes leg) const
{
  if (leg == MA::Leg::LF)
    return *LegLF;
  if (leg == MA::Leg::LH)
    return *LegLH;
  if (leg == MA::Leg::RF)
    return *LegRF;
  return *LegRH;
}


std::string MARobotState::GetDecodedVariableMap() const
{
  return DecodedVariableMap;
}


const MANum<int>* MARobotState::GetVariable(const std::string& variable_name) const
{
  for (unsigned int i = 0; i < VariableNames.size(); ++i)
  {
    if (**(StrBasePtr+(int)VariableNames[i]) == variable_name)
      return Variables[i];
  }
  return nullptr;
}


std::string MARobotState::GetDisplayName(const std::string& variable_name) const
{
  for (unsigned int i = 0; i < VariableNames.size(); ++i)
  {
    if (**(StrBasePtr+(int)VariableNames[i]) == variable_name)
    {
      if ((*(StrBasePtr+(int)VariableDisplayNames[i]))->empty())
        return variable_name;
      else
        return **(StrBasePtr+(int)VariableDisplayNames[i]);
    }
  }
  return "";
}


MC::StringList MARobotState::GetVariableNames() const
{
  MC::StringList Strs;

  for (unsigned int i = 0; i < VariableNames.size(); ++i)
  {
    Strs.push_back(**(StrBasePtr+(int)VariableNames[i]));
  }
  return Strs;
}


MA::ValueNoteList MARobotState::GetValueNotes(const std::string& variable_name) const
{
  const MANum<int>* VariablePtr = GetVariable(variable_name);

  if (VariablePtr)
  {
    const auto Iter = VariableValueNotes.find(VariablePtr);

    if (Iter != VariableValueNotes.end())
      return Iter->second;
  }
  return MA::ValueNoteList();
}


std::string MARobotState::GetAsRichText(const MC::StringList& highlighted_variables) const
{
  std::string ContentStr = "<b><u>Robot State</u></b><br>";
  std::string GroupName;

  for (unsigned int i = 0; i < VariableNames.size(); ++i)
  {
    MC::StringList NameElements;

    boost::algorithm::split(NameElements, **(StrBasePtr+(int)VariableNames[i]),
                            boost::algorithm::is_any_of("::"));
    if (GroupName.empty() || GroupName != NameElements[0])
    {
      GroupName = NameElements[0];
      ContentStr += "<b>"+GroupName+"</b><br>";
    }
    MANum<int>* VariablePtr = Variables[i];
    const MC::IntList& HighResValues = VariablePtr->GetHighResolutionValues();
    std::string HighResValuesStr;

    if (HighResValues.size() > 0)
    {
      HighResValuesStr = " (";
      for (unsigned int i1 = 0; i1 < HighResValues.size(); ++i1)
      {
        HighResValuesStr += MCToStr(HighResValues[i1]);
        if (i1 < HighResValues.size()-1)
        {
          HighResValuesStr += ',';
        }
      }
      HighResValuesStr += ')';
    }
    const auto Iter = std::find(highlighted_variables.begin(), highlighted_variables.end(),
                                **(StrBasePtr+(int)VariableNames[i]));

    if (Iter != highlighted_variables.end())
    {
      ContentStr += "<span style=\"color:#7777ee\">"+NameElements[2]+": "+MCToStr(*VariablePtr)+
                    HighResValuesStr+"</span><br>";
    } else {
      ContentStr += NameElements[2]+": "+MCToStr((int)*VariablePtr, NameElements[2] == "BuildID")+HighResValuesStr+"<br>";
    }
  }
  return ContentStr;
}


std::string MARobotState::GetAsNormalText() const
{
  std::string RobotStateStr = GetAsRichText();

  boost::replace_all(RobotStateStr, "<br>", "\n");
  boost::replace_all(RobotStateStr, "<b><u>", " ++++ ");
  boost::replace_all(RobotStateStr, "</u></b>", " ++++ ");
  boost::replace_all(RobotStateStr, "<b>", " ++ ");
  boost::replace_all(RobotStateStr, "</b>", " ++ ");
  return RobotStateStr;
}


MARobotState& MARobotState::operator=(const MARobotState& other)
{
  if (&other == this)
    return *this;

  SignalConnections = other.SignalConnections;
  CheckStaticStringCache();
  StrBasePtr = &StringCache->Strs[0];
  *LegLF = *other.LegLF;
  *LegLH = *other.LegLH;
  *LegRF = *other.LegRF;
  *LegRH = *other.LegRH;
  *Torso = *other.Torso;
  *Head = *other.Head;
  *Audio = *other.Audio;
  *BodyMotion = *other.BodyMotion;
  *Emotions = *other.Emotions;
  *ComplexIndicators = *other.ComplexIndicators;
  *Goals = *other.Goals;
  *Hri = *other.Hri;
  // Remove the old custom variables
  RemoveCustomVariables();
  for (auto& variable : other.DecodedCustomVariables)
  {
    MANum<int>* NewVariable = new MANum<int>(*variable);

    DecodedCustomVariables.push_back(NewVariable);
    Variables.push_back(NewVariable);
    // Lookup the variable from the other robot state instance
    auto NameIter = other.VariableNames.begin();
    auto DisplayNameIter = other.VariableDisplayNames.begin();
    auto VariableIter = other.Variables.begin();

    for (unsigned int i = 0; i < other.Variables.size(); ++i)
    {
      if (variable == *VariableIter)
      {
        VariableNames.push_back(GetStrIndex(**(other.StrBasePtr+(int)*NameIter)));
        VariableDisplayNames.push_back(GetStrIndex(**(other.StrBasePtr+(int)*DisplayNameIter)));
        break;
      }
      ++NameIter;
      ++DisplayNameIter;
      ++VariableIter;
    }
  }
  return *this;
}


MCBinaryData* MARobotState::Encode(bool encode_variable_map) const
{
  std::string VariableMap;

  if (encode_variable_map)
  {
    for (unsigned int i = 0; i < VariableNames.size(); ++i)
    {
      VariableMap += **(StrBasePtr+(int)VariableNames[i])+'|';
      if (Variables[i]->IsUchar())
      {
        VariableMap += "u8,";
      } else
      if (Variables[i]->IsInt16())
      {
        VariableMap += "i16,";
      } else
      if (Variables[i]->IsInt32())
      {
        VariableMap += "i32,";
      } else {
        MC_ERROR("Variable %s cannot be mapped to uchar, int16 or int32 ranges.",
                 (*(StrBasePtr+(int)VariableNames[i]))->c_str());
      }
    }
  }
  MCBinaryData* BinaryData = nullptr;

  if (encode_variable_map)
  {
    BinaryData = new MCBinaryData(GetBinarySize()+2+VariableMap.size()+1);
    BinaryData->AddInt16(VariableMap.size()+1);
    memcpy(&BinaryData->GetData()[2], VariableMap.c_str(), VariableMap.size()+1);
    BinaryData->IncrementPosition(VariableMap.size()+1);
  } else {
    BinaryData = new MCBinaryData(GetBinarySize()+2);
    BinaryData->AddInt16(0);
  }
  for (auto& variable : Variables)
  {
    const MC::IntList& HighResValues = variable->GetHighResolutionValues();

    if (variable->IsUchar())
    {
      BinaryData->AddUChar(*variable);
      BinaryData->AddUChar(HighResValues.size());
      for (auto& value : HighResValues)
      {
        BinaryData->AddUChar(value);
      }
    } else
    if (variable->IsInt16())
    {
      BinaryData->AddInt16(*variable);
      BinaryData->AddUChar(HighResValues.size());
      for (auto& value : HighResValues)
      {
        BinaryData->AddInt16(value);
      }
    } else {
      BinaryData->AddInt32(*variable);
      BinaryData->AddUChar(HighResValues.size());
      for (auto& value : HighResValues)
      {
        BinaryData->AddInt32(value);
      }
    }
  }
  return BinaryData;
}


MARobotState* MARobotState::Decode(const MCBinaryData& data, const std::string& variable_map)
{
  MCBinaryData& BinaryData = const_cast<MCBinaryData&>(data);
  int OldPosition = BinaryData.GetPosition();
  MARobotState* NewRobotState = new MARobotState;
  int VariableMapSize;

  BinaryData.SetPosition(0);
  VariableMapSize = BinaryData.GetInt16();
  if (VariableMapSize < 1 && variable_map.empty())
  {
    MC_WARNING("No variable map in the packet or cached during robot state decoding.");
    delete NewRobotState;
    BinaryData.SetPosition(OldPosition);
    return nullptr;
  }
  std::string VariableMap;
  MA::Int16List& DisplayNames = NewRobotState->VariableDisplayNames;
  MA::Int16List& Names = NewRobotState->VariableNames;
  MA::MANumIntPtrList& Variables = NewRobotState->Variables;
  MA::MANumIntPtrList& DecodedVariables = NewRobotState->DecodedCustomVariables;

  if (VariableMapSize > 0)
  {
    if (BinaryData.GetSize() < 2+VariableMapSize+1 || BinaryData.GetData()[2+VariableMapSize-1] != 0)
    {
      MC_WARNING("No space for variable map or it is invalid.");
      delete NewRobotState;
      BinaryData.SetPosition(OldPosition);
      return nullptr;
    }
    VariableMap = BinaryData.GetString(VariableMapSize-1, "|,:.");
    if ((int)VariableMap.size() != VariableMapSize-1)
    {
      MC_WARNING("The variable map is invalid.");
      delete NewRobotState;
      BinaryData.SetPosition(OldPosition);
      return nullptr;
    }
    // Jump other the string closing '\0'
    BinaryData.IncrementPosition(1);
    NewRobotState->DecodedVariableMap = VariableMap;
  } else {
    VariableMap = variable_map;
  }
  int VariableIndex = -1;
  auto NameIter = Names.end();
  auto VariableIter = Variables.end();

  while (VariableMap.size() > 2)
  {
    const int VariableSectionSize = VariableMap.find(',');
    std::string VariableSection = VariableMap.substr(0, VariableSectionSize);
    const int VariableNameSize = VariableSection.find('|');
    const std::string VariableName = VariableSection.substr(0, VariableNameSize);

    if (VariableIndex != -1)
    {
      ++NameIter;
      ++VariableIter;

      if (NameIter != Names.end() && **(NewRobotState->StrBasePtr+(int)*NameIter) == VariableName)
      {
        VariableIndex++;
        goto continue_decoding;
      }
      VariableIndex = -1;
    }
    NameIter = Names.begin();
    VariableIter = Variables.begin();
    for (unsigned int i = 0; i < Names.size(); ++i)
    {
      if (**(NewRobotState->StrBasePtr+(int)*NameIter) == VariableName)
      {
        VariableIndex = i;
        break;
      }
      ++NameIter;
      ++VariableIter;
    }
continue_decoding:
    VariableMap.erase(0, VariableSectionSize+1);
    VariableSection.erase(0, VariableNameSize+1);
    // Non-existing variable -> add as a new custom variable
    if (VariableIndex == -1)
    {
      MANum<int>* NewVariable = new MANum<int>;

      // Set the minimum and maximum integer values since it real limits are not known
      // for an unknown variable.
      NewVariable->SetMinMax(std::numeric_limits<int>::min(), std::numeric_limits<int>::max());
      DecodedVariables.push_back(NewVariable);
      Variables.push_back(NewVariable);
      VariableIter = --Variables.end();
      Names.push_back(GetStrIndex(VariableName));
      NameIter = --Names.end();
      DisplayNames.push_back(GetStrIndex(""));
    }
    int WordSize;
    bool IsUChar = VariableSection == "u8" || VariableSection == "uchar";
    bool IsInt16 = VariableSection == "i16" || VariableSection == "int16";

    if (IsUChar)
    {
      WordSize = 1;
    } else
    if (IsInt16)
    {
      WordSize = 2;
    } else {
      WordSize = 4;
    }
    if ((int)BinaryData.GetRemainingCapacity(WordSize) < WordSize)
    {
      MC_WARNING("No remaining binary data size. Invalid binary BinaryData supplied!?");
      delete NewRobotState;
      return nullptr;
    }
    if (IsUChar)
    {
      int TempInt = (int)BinaryData.GetUChar();

      // Assign the value twice to reset the changed variable to false
      **VariableIter = TempInt;
      **VariableIter = TempInt;
    } else
    if (IsInt16)
    {
      int TempInt = (int)BinaryData.GetInt16();

      // Assign the value twice to reset the changed variable to false
      **VariableIter = TempInt;
      **VariableIter = TempInt;
    } else {
      int TempInt = (int)BinaryData.GetInt32();

      // Assign the value twice to reset the changed variable to false
      **VariableIter = TempInt;
      **VariableIter = TempInt;
    }
    if (BinaryData.GetRemainingCapacity(1) == 0)
    {
      MC_WARNING("No remaining binary data size. Invalid binary BinaryData supplied!?");
      delete NewRobotState;
      return nullptr;
    }
    int HighResValueNumber = BinaryData.GetUChar();

    if (HighResValueNumber > 0)
    {
      MC::IntList HighResValues;

      for (int i = 0; i < HighResValueNumber; ++i)
      {
        if ((int)BinaryData.GetRemainingCapacity(WordSize) < WordSize)
        {
          MC_WARNING("No remaining binary data size. Invalid binary BinaryData supplied!?");
          delete NewRobotState;
          return nullptr;
        }
        if (IsUChar)
        {
          HighResValues.push_back((int)BinaryData.GetUChar());
        } else
        if (IsInt16)
        {
          HighResValues.push_back((int)BinaryData.GetInt16());
        } else {
          HighResValues.push_back((int)BinaryData.GetInt32());
        }
      }
      (*VariableIter)->SetHighResolutionValues(HighResValues);
    }
  }
  BinaryData.SetPosition(OldPosition);
  return NewRobotState;
}


void MARobotState::BindVariable(const std::string& name, MANum<int>& variable,
                                const std::string& display_name)
{
  Variables.push_back(&variable);
  VariableNames.push_back(GetStrIndex(name));
  VariableDisplayNames.push_back(GetStrIndex(display_name));
}


void MARobotState::RemoveCustomVariables()
{
  for (auto& variable : DecodedCustomVariables)
  {
    // Remove custom variable references (name, display name, variable pointer)
    auto NameIter = VariableNames.begin();
    auto DisplayNameIter = VariableDisplayNames.begin();
    auto VariableIter = Variables.begin();

    for (unsigned int i = 0; i < VariableNames.size(); ++i)
    {
      if (variable == *VariableIter)
      {
        VariableNames.erase(NameIter);
        VariableDisplayNames.erase(DisplayNameIter);
        Variables.erase(VariableIter);
        break;
      }
      ++NameIter;
      ++DisplayNameIter;
      ++VariableIter;
    }
    delete variable;
  }
  DecodedCustomVariables.clear();
}


int MARobotState::GetBinarySize() const
{
  int BinaryStorageSize = 0;

  for (auto& variable : Variables)
  {
    const MC::IntList& HighResValues = variable->GetHighResolutionValues();

    // Add the high resolution number byte
    BinaryStorageSize++;
    // Add the actual storage requirements for the new item
    if (variable->IsUchar())
    {
      BinaryStorageSize++;
      BinaryStorageSize += HighResValues.size()*1;
    } else
    if (variable->IsInt16())
    {
      BinaryStorageSize += 2;
      BinaryStorageSize += HighResValues.size()*2;
    } else {
      BinaryStorageSize += 4;
      BinaryStorageSize += HighResValues.size()*4;
    }
  }
  return BinaryStorageSize;
}