MAHeadController.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 "MAHeadController.hpp"

#include "core/MACameraDevice.hpp"
#include "core/MADevice.hpp"
#include "core/MAMicrophoneDevice.hpp"
#include "core/MARandomness.hpp"
#include "types/skits/MAMotionSkit.hpp"
#include "types/MAAudio.hpp"
#include "types/MAHead.hpp"
#include "types/MARobotState.hpp"

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

namespace MA
{
MCThreadLocalData<MAHeadController> Head(false);
}

MAHeadTransitionGenerators::MAHeadTransitionGenerators() : NeckTilt1(nullptr), NeckTilt2(nullptr),
  NeckPan(nullptr)
{
}


void MAHeadTransitionGenerators::AddGeneratorToNeckTilt1(MAGeneratorBase& generator)
{
  if (!NeckTilt1)
  {
    NeckTilt1 = new MAGeneratorContainer(generator);
  } else {
    NeckTilt1->AddGenerator(generator);
  }
}


void MAHeadTransitionGenerators::AddGeneratorToNeckTilt2(MAGeneratorBase& generator)
{
  if (!NeckTilt2)
  {
    NeckTilt2 = new MAGeneratorContainer(generator);
  } else {
    NeckTilt2->AddGenerator(generator);
  }
}


void MAHeadTransitionGenerators::AddGeneratorToNeckPan(MAGeneratorBase& generator)
{
  if (!NeckPan)
  {
    NeckPan = new MAGeneratorContainer(generator);
  } else {
    NeckPan->AddGenerator(generator);
  }
}


void MAHeadTransitionGenerators::AddDelayToNeckJoints(int duration)
{
  AddGeneratorToNeckPan(*new MADelayGenerator(duration));
  AddGeneratorToNeckTilt1(*new MADelayGenerator(duration));
  AddGeneratorToNeckTilt2(*new MADelayGenerator(duration));
}


void MAHeadTransitionGenerators::Normalize()
{
  if (!NeckTilt1 && !NeckTilt2 && !NeckPan)
  {
    return;
  }
  if ((NeckTilt1 && !NeckTilt1->IsFinite()) ||
      (NeckTilt2 && !NeckTilt2->IsFinite()) ||
      (NeckPan && !NeckPan->IsFinite()))
  {
    return;
  }

  int Duration = 0;

  if (NeckTilt1 && NeckTilt1->IsFinite())
    Duration = Duration > NeckTilt1->GetDuration() ? Duration : NeckTilt1->GetDuration();
  if (NeckTilt2 && NeckTilt2->IsFinite())
    Duration = Duration > NeckTilt2->GetDuration() ? Duration : NeckTilt2->GetDuration();
  if (NeckPan && NeckPan->IsFinite())
    Duration = Duration > NeckPan->GetDuration() ? Duration : NeckPan->GetDuration();

  bool Ret = true;

  Ret = Ret && (!NeckTilt1 || NeckTilt1->GetDuration() == Duration);
  Ret = Ret && (!NeckTilt2 || NeckTilt2->GetDuration() == Duration);
  Ret = Ret && (!NeckPan || NeckPan->GetDuration() == Duration);

  if (Ret)
  {
    return;
  }

  if (NeckTilt1 && NeckTilt1->IsFinite())
  {
    AddGeneratorToNeckTilt1(*new MADelayGenerator(Duration-NeckTilt1->GetDuration()));
  }
  if (NeckTilt2 && NeckTilt2->IsFinite())
  {
    AddGeneratorToNeckTilt2(*new MADelayGenerator(Duration-NeckTilt2->GetDuration()));
  }
  if (NeckPan && NeckPan->IsFinite())
  {
    AddGeneratorToNeckPan(*new MADelayGenerator(Duration-NeckPan->GetDuration()));
  }
}


MAHeadController::MAHeadController() : MAController(MA::MotorController),
  RubbingCenterForeBack(MCFloatInfinity()), RubbingCenterLeftRight(MCFloatInfinity())
{
  if (MA::Head.get())
  {
    MC_WARNING("The global head controller is overridden.");
    MA::Head.release();
  }
  MA::Head.reset(this);

  MA_BIND_TRANSITION(MAHeadController, TurnDownAndAhead, IsTurnedDownAndAhead);
  MA_BIND_TRANSITION(MAHeadController, TurnUp, IsTurnedUp);
  MA_BIND_TRANSITION(MAHeadController, ToNormalAhead, IsNormalAhead);
  MA_BIND_TRANSITION_ACTION(MAHeadController, GoRubbing);
  MA_BIND_TRANSITION_ACTION(MAHeadController, RubToLeft);
  MA_BIND_TRANSITION_ACTION(MAHeadController, RubToRight);
  MA_BIND_TRANSITION_ACTION(MAHeadController, FinishRubbing);
  MA_BIND_TRANSITION_ACTION(MAHeadController, DoCircle);
  MA_BIND_TRANSITION_ACTION(MAHeadController, HappyMoveToUp);
  MA_BIND_TRANSITION_ACTION(MAHeadController, LookAroundLying);
  MA_BIND_TRANSITION_ACTION(MAHeadController, LookAroundSitting);
  MA_BIND_TRANSITION_ACTION(MAHeadController, TurnLeftLying);
  MA_BIND_TRANSITION_ACTION(MAHeadController, TurnRightLying);
  MA_BIND_TRANSITION_ACTION(MAHeadController, TurnLeftSitting);
  MA_BIND_TRANSITION_ACTION(MAHeadController, TurnRightSitting);
}


MAHeadController::~MAHeadController()
{
  if (MA::Head.get() == this)
    MA::Head.release();
}


void MAHeadController::UpdateRobotState(MARobotState& state)
{
  state.Head->Pan = (int)NeckPan->GetSensorValue();
  state.Head->PanForce = (int)NeckPan->GetForce();
  state.Head->Tilt1 = (int)NeckTilt1->GetSensorValue();
  state.Head->Tilt1Force = (int)NeckTilt1->GetForce();
  state.Head->Tilt2 = (int)NeckTilt2->GetSensorValue();
  state.Head->Tilt2Force = (int)NeckTilt2->GetForce();
  state.Head->CameraBrightness = Camera->GetBrightness();
  state.Head->EyePupilSize = Camera->GetPupilSize();
  state.Head->LeftRight = (int)(float)LeftRightTurned.Value;
  state.Head->UpDown = (int)HeadPosition.UpDownValue;
  state.Head->ForeBack = (int)HeadPosition.ForeBackValue;
  state.Head->ChinButton = (int)ChinButton->GetSensorValue();
  state.Head->CrownButton = (int)CrownButton->GetSensorValue();
  state.Head->Moving = NeckPan->HasGenerator() || NeckTilt1->HasGenerator() || NeckTilt2->HasGenerator();
  state.Head->InMotion = NeckPan->IsInMotion() || NeckTilt1->IsInMotion() || NeckTilt2->IsInMotion();
  state.Audio->LeftMicrophonePower = (int)Microphone->GetLeftChannelPower();
  state.Audio->RightMicrophonePower = (int)Microphone->GetRightChannelPower();
  state.Audio->BackgroundSoundPower = (int)Microphone->GetBackgroundSoundPower();
  const MC::IntList& Votes = Microphone->GetSoundEventVotes();

  for (int i = 1; i < (int)Votes.size() && i < MA::MaxSoundEvents+1; ++i)
  {
    state.Audio->SoundEventVotes[i] = Votes[i];
  }
  state.Audio->CurrentSoundEvent = Microphone->GetDetectedSoundEvent();
}


MA::DeviceList MAHeadController::GetMotors() const
{
  MA::DeviceList Motors;

  Motors.push_back(NeckTilt1.get());
  Motors.push_back(NeckTilt2.get());
  Motors.push_back(NeckPan.get());
  return Motors;
}


MADevice* MAHeadController::GetMotor(const MA::HeadJointsType joint) const
{
  if (joint == MA::NeckPan)
  {
    return NeckPan.get();
  }
  if (joint == MA::NeckTilt1)
  {
    return NeckTilt1.get();
  }
  return NeckTilt2.get();
}


MA::HeadJointsType MAHeadController::GetMotorType(const MADevice& motor) const
{
  if (&motor == NeckPan.get())
  {
    return MA::NeckPan;
  }
  if (&motor == NeckTilt1.get())
  {
    return MA::NeckTilt1;
  }
  return MA::NeckTilt2;
}


void MAHeadController::SetPower(bool new_state)
{
  // Clear the generators when switching off the joints
  if (!new_state)
    Stop();
  SetPower(MA::NeckPan, new_state);
  SetPower(MA::NeckTilt1, new_state);
  SetPower(MA::NeckTilt2, new_state);
}


bool MAHeadController::IsPowerOn() const
{
  return IsPowerOn(MA::NeckPan) && IsPowerOn(MA::NeckTilt1) && IsPowerOn(MA::NeckTilt2);
}


bool MAHeadController::IsMovingNeck() const
{
  return NeckPan->HasGenerator() || NeckTilt1->HasGenerator() || NeckTilt2->HasGenerator();
}


bool MAHeadController::IsTurnedDown() const
{
  bool Ret = true;

  Ret = Ret && (int)HeadPosition.ForeBackValue > 60;
  Ret = Ret && (int)HeadPosition.UpDownValue < -20;
  return Ret;
}


bool MAHeadController::IsTurnedAhead() const
{
  bool Ret = true;

  Ret = Ret && MCAbs((int)LeftRightTurned.Value) < 30;
  return Ret;
}


bool MAHeadController::IsUp() const
{
  bool Ret = true;

  Ret = Ret && (int)HeadPosition.UpDownValue >= -20;
  return Ret;
}


MA::DeviceGeneratorsMap MAHeadController::TurnDownAndAhead()
{
  MALeftRightTurned LeftRight;
  MAHeadPosition Position;
  MAHeadTransitionGenerators Generators;

  LeftRight.Value = 0.0;
  Position.ForeBackValue = 100.0;
  Position.UpDownValue = -100.0;
  CreateGenerators(Generators, LeftRight, 1500);
  CreateGenerators(Generators, Position, 1500);
  return ApplyGenerators(Generators, true);
}


bool MAHeadController::IsTurnedDownAndAhead() const
{
  return IsTurnedDown() && IsTurnedAhead();
}


MA::DeviceGeneratorsMap MAHeadController::TurnUp()
{
  MALeftRightTurned LeftRight;
  MAHeadPosition Position;
  MAHeadTransitionGenerators Generators;

  LeftRight.Value = 0.0;
  Position.ForeBackValue = -100.0;
  Position.UpDownValue = 10.0;
  CreateGenerators(Generators, LeftRight, 1500);
  CreateGenerators(Generators, Position, 1500);
  return ApplyGenerators(Generators, true);
}


bool MAHeadController::IsTurnedUp() const
{
  bool Ret = true;

  Ret = Ret && MCAbs((int)LeftRightTurned.Value) < 30;
  Ret = Ret && (int)HeadPosition.ForeBackValue < 85;
  Ret = Ret && MCAbs((int)HeadPosition.UpDownValue) < 25;
  return Ret;
}


MA::DeviceGeneratorsMap MAHeadController::ToNormalAhead()
{
  MALeftRightTurned LeftRight;
  MAHeadPosition Position;
  MAHeadTransitionGenerators Generators;

  LeftRight.Value = 0.0;
  Position.ForeBackValue = 31.0;
  Position.UpDownValue = 0.0;
  CreateGenerators(Generators, LeftRight, 1500);
  CreateGenerators(Generators, Position, 1500);
  // The motors are not enough fast to finish the movements.
  Generators.AddDelayToNeckJoints();
  return ApplyGenerators(Generators, true);
}


bool MAHeadController::IsNormalAhead() const
{
  bool Ret = true;

  Ret = Ret && MCAbs((int)LeftRightTurned.Value) < 10;
  Ret = Ret && MCRangeCheck((int)HeadPosition.ForeBackValue, 25, 50);
  Ret = Ret && MCRangeCheck((int)HeadPosition.UpDownValue, -20, 5);
  return Ret;
}


MA::DeviceGeneratorsMap MAHeadController::GoRubbing()
{
  MAHeadPosition NewPosition = HeadPosition;
  MAHeadTransitionGenerators Generators;

  // We must force all joints now
  SetPower(true);
  // Set the center data of the rubbing
  RubbingCenterForeBack = (float)HeadPosition.ForeBackValue+40.0;
  RubbingCenterLeftRight = (float)LeftRightTurned.Value;

  NewPosition.ForeBackValue += 40;
  CreateGenerators(Generators, NewPosition, 700);
  return ApplyGenerators(Generators, false);
}


MA::DeviceGeneratorsMap MAHeadController::RubToLeft()
{
  MAHeadPosition NewPosition;
  MALeftRightTurned NewLeftRight;
  MAHeadTransitionGenerators Generators;
  MA_RANDOM_POINT_1(Speed1, 1, 10);
  MA_RANDOM_POINT_1(Speed2, 1, 10);
  MA_RANDOM_POINT_1(Speed3, 1, 10);
  MA_RANDOM_POINT_1(Speed4, 1, 10);

  NewPosition.ForeBackValue = RubbingCenterForeBack;
  NewPosition.ForeBackValue -= 25;
  CreateGenerators(Generators, NewPosition, 400);
  Generators.Normalize();
  NewPosition.ForeBackValue += 25;
  CreateGenerators(Generators, NewPosition, 200+Speed1*10);
  NewPosition.ForeBackValue -= 25;
  CreateGenerators(Generators, NewPosition, 300+Speed3*10);
  NewPosition.ForeBackValue += 25;
  CreateGenerators(Generators, NewPosition, 200+Speed2*10);
  NewPosition.ForeBackValue -= 25;
  CreateGenerators(Generators, NewPosition, 300+Speed4*10);
  NewPosition.ForeBackValue += 25;
  CreateGenerators(Generators, NewPosition, 400);

  NewLeftRight.Value = RubbingCenterLeftRight;
  NewLeftRight.Value -= 15;
  CreateGenerators(Generators, NewLeftRight, 200+Speed1*10);
  NewLeftRight.Value += 15;
  CreateGenerators(Generators, NewLeftRight, 300+Speed3*10);
  NewLeftRight.Value -= 15;
  CreateGenerators(Generators, NewLeftRight, 200+Speed1*10);
  NewLeftRight.Value += 15;
  CreateGenerators(Generators, NewLeftRight, 300+Speed4*10);
  return ApplyGenerators(Generators, false);
}


MA::DeviceGeneratorsMap MAHeadController::RubToRight()
{
  MAHeadPosition NewPosition;
  MALeftRightTurned NewLeftRight;
  MAHeadTransitionGenerators Generators;
  MA_RANDOM_POINT_1(Speed1, 1, 10);
  MA_RANDOM_POINT_1(Speed2, 1, 10);
  MA_RANDOM_POINT_1(Speed3, 1, 10);
  MA_RANDOM_POINT_1(Speed4, 1, 10);

  NewPosition.ForeBackValue = RubbingCenterForeBack;
  NewPosition.ForeBackValue -= 25;
  CreateGenerators(Generators, NewPosition, 200+Speed1*10);
  NewPosition.ForeBackValue += 25;
  CreateGenerators(Generators, NewPosition, 300+Speed3*10);
  NewPosition.ForeBackValue -= 25;
  CreateGenerators(Generators, NewPosition, 200+Speed2*10);
  NewPosition.ForeBackValue += 25;
  CreateGenerators(Generators, NewPosition, 300+Speed4*10);

  NewLeftRight.Value = RubbingCenterLeftRight;
  NewLeftRight.Value += 15;
  CreateGenerators(Generators, NewLeftRight, 200+Speed1*10);
  NewLeftRight.Value -= 15;
  CreateGenerators(Generators, NewLeftRight, 300+Speed3*10);
  NewLeftRight.Value += 15;
  CreateGenerators(Generators, NewLeftRight, 200+Speed1*10);
  NewLeftRight.Value -= 15;
  CreateGenerators(Generators, NewLeftRight, 300+Speed4*10);
  return ApplyGenerators(Generators, false);
}


MA::DeviceGeneratorsMap MAHeadController::FinishRubbing()
{
  MAHeadPosition NewPosition;
  MALeftRightTurned NewLeftRight;
  MAHeadTransitionGenerators Generators;

  NewPosition.ForeBackValue = RubbingCenterForeBack-40.0;
  CreateGenerators(Generators, NewPosition, 700);

  NewLeftRight.Value = RubbingCenterLeftRight;
  CreateGenerators(Generators, NewLeftRight, 700);

  // The motors are not enough fast to finish the movements.
  Generators.AddDelayToNeckJoints();
  // Clear the rubbing center data
  RubbingCenterForeBack = MCFloatInfinity();
  RubbingCenterLeftRight = MCFloatInfinity(); // -V656 (PVS Studio suppression)
  return ApplyGenerators(Generators, false);
}


MA::DeviceGeneratorsMap MAHeadController::DoCircle()
{
  MAHeadPosition NewPosition = HeadPosition;
  MALeftRightTurned NewLeftRight = LeftRightTurned;
  MAHeadTransitionGenerators Generators;
  MA_RANDOM_POINT_1(RadiusXDirection, 0, 1);
  MA_RANDOM_POINT_1(RadiusX, 5, 15);
  MA_RANDOM_POINT_1(CircleRadiusY, 10, 20);
  int CircleRadiusX = RadiusXDirection == 1 ? RadiusX : -RadiusX;

  NewPosition.UpDownValue += CircleRadiusY;
  CreateGenerators(Generators, NewPosition, 200, MAGeneratorBase::AcceleratingSines);
  NewPosition.UpDownValue += CircleRadiusY;
  CreateGenerators(Generators, NewPosition, 200, MAGeneratorBase::DeceleratingSines);
  NewPosition.UpDownValue -= CircleRadiusY;
  CreateGenerators(Generators, NewPosition, 200, MAGeneratorBase::AcceleratingSines);
  NewPosition.UpDownValue -= CircleRadiusY;
  CreateGenerators(Generators, NewPosition, 200, MAGeneratorBase::DeceleratingSines);

  NewLeftRight.Value -= CircleRadiusX;
  CreateGenerators(Generators, NewLeftRight, 200, MAGeneratorBase::DeceleratingSines);
  NewLeftRight.Value += CircleRadiusX;
  CreateGenerators(Generators, NewLeftRight, 200, MAGeneratorBase::AcceleratingSines);
  NewLeftRight.Value += CircleRadiusX;
  CreateGenerators(Generators, NewLeftRight, 200, MAGeneratorBase::DeceleratingSines);
  NewLeftRight.Value -= CircleRadiusX;
  CreateGenerators(Generators, NewLeftRight, 200, MAGeneratorBase::AcceleratingSines);
  return ApplyGenerators(Generators, true);
}


MA::DeviceGeneratorsMap MAHeadController::HappyMoveToUp()
{
  MAHeadPosition NewPosition = HeadPosition;
  MALeftRightTurned NewLeftRight = LeftRightTurned;
  MAHeadTransitionGenerators Generators;
  MA_RANDOM_POINT_1(RadiusXDirection, 0, 1);
  MA_RANDOM_POINT_1(RadiusX, 4, 25);
  MA_RANDOM_POINT_1(CircleRadiusY, 10, 30);
  int CircleRadiusX = RadiusXDirection == 1 ? RadiusX : -RadiusX;

  NewPosition.UpDownValue += CircleRadiusY;
  CreateGenerators(Generators, NewPosition, 400, MAGeneratorBase::AcceleratingSines);
  NewPosition.UpDownValue -= CircleRadiusY;
  CreateGenerators(Generators, NewPosition, 400, MAGeneratorBase::AcceleratingSines);

  NewLeftRight.Value -= CircleRadiusX;
  CreateGenerators(Generators, NewLeftRight, 400, MAGeneratorBase::DeceleratingSines);
  NewLeftRight.Value += CircleRadiusX;
  CreateGenerators(Generators, NewLeftRight, 400, MAGeneratorBase::DeceleratingSines);
  return ApplyGenerators(Generators, true);
}


std::string MAHeadController::DoHappyMovement()
{
  std::string TransitionName;
  MA_RANDOM_POINT_1(Movements, 1, 2);

  if (Movements == 1)
  {
    TransitionName = DoCircleStr;
  }
  if (Movements == 2)
  {
    TransitionName = HappyMoveToUpStr;
  }
  StartTransition(TransitionName);
  return TransitionName;
}


MA::DeviceGeneratorsMap MAHeadController::LookAroundLying()
{
  MAHeadTransitionGenerators Generators;
  MA_RANDOM_POINT_1(Motions, 1, 5);
  MA_RANDOM_POINT_N(Durations, Motions, 1500, 2500);
  MA_RANDOM_POINT_N(DelayDurations, Motions, 10, 20000);
  MA_RANDOM_POINT_N(LeftRightValues, Motions, -70, 70);
  MA_RANDOM_POINT_N(ForeBackValues, Motions, -50, 50);
  MA_RANDOM_POINT_N(UpDownValues, Motions, -20, 80);

  for (int i = 0; i < Motions; ++i)
  {
    MAHeadPosition Position;

    Generators.AddDelayToNeckJoints(DelayDurations[i]);
    CreateGenerators(Generators, MALeftRightTurned(LeftRightValues[i]), Durations[i],
                     MAGeneratorBase::GetRandomFunction());
    Position.ForeBackValue = (float)ForeBackValues[i];
    Position.UpDownValue = (float)UpDownValues[i];
    CreateGenerators(Generators, Position, Durations[i], MAGeneratorBase::GetRandomFunction());
  }
  return ApplyGenerators(Generators, true);
}


MA::DeviceGeneratorsMap MAHeadController::LookAroundSitting()
{
  MAHeadTransitionGenerators Generators;
  MA_RANDOM_POINT_1(Motions, 1, 5);
  MA_RANDOM_POINT_N(Durations, Motions, 1500, 2500);
  MA_RANDOM_POINT_N(DelayDurations, Motions, 10, 20000);
  MA_RANDOM_POINT_N(LeftRightValues, Motions, -70, 70);
  MA_RANDOM_POINT_N(ForeBackValues, Motions, -10, 30);
  MA_RANDOM_POINT_N(UpDownValues, Motions, -40, 45);

  for (int i = 0; i < Motions; ++i)
  {
    MAHeadPosition Position;

    Generators.AddDelayToNeckJoints(DelayDurations[i]);
    CreateGenerators(Generators, MALeftRightTurned(LeftRightValues[i]), Durations[i],
                     MAGeneratorBase::GetRandomFunction());
    Position.ForeBackValue = (float)ForeBackValues[i];
    Position.UpDownValue = (float)UpDownValues[i];
    CreateGenerators(Generators, Position, Durations[i], MAGeneratorBase::GetRandomFunction());
  }
  return ApplyGenerators(Generators, true);
}


MA::DeviceGeneratorsMap MAHeadController::TurnLeftLying()
{
  MAHeadTransitionGenerators Generators;
  MALeftRightTurned LeftRight = LeftRightTurned;
  MAHeadPosition Position = HeadPosition;

  LeftRight.Value = -95.0;
  Position.UpDownValue = 40;
  Position.ForeBackValue = 0;
  CreateGenerators(Generators, LeftRight, 1800);
  CreateGenerators(Generators, Position, 1800);
  return ApplyGenerators(Generators, true);
}


MA::DeviceGeneratorsMap MAHeadController::TurnRightLying()
{
  MAHeadTransitionGenerators Generators;
  MALeftRightTurned LeftRight = LeftRightTurned;
  MAHeadPosition Position = HeadPosition;

  LeftRight.Value = 95.0;
  Position.UpDownValue = 40;
  Position.ForeBackValue = 0;
  CreateGenerators(Generators, LeftRight, 1800);
  CreateGenerators(Generators, Position, 1800);
  return ApplyGenerators(Generators, true);
}


MA::DeviceGeneratorsMap MAHeadController::TurnLeftSitting()
{
  MAHeadTransitionGenerators Generators;
  MALeftRightTurned LeftRight = LeftRightTurned;
  MAHeadPosition Position = HeadPosition;

  LeftRight.Value = -95.0;
  Position.UpDownValue = 20;
  Position.ForeBackValue = 0;
  CreateGenerators(Generators, LeftRight, 1800);
  CreateGenerators(Generators, Position, 1800);
  return ApplyGenerators(Generators, true);
}


MA::DeviceGeneratorsMap MAHeadController::TurnRightSitting()
{
  MAHeadTransitionGenerators Generators;
  MALeftRightTurned LeftRight = LeftRightTurned;
  MAHeadPosition Position = HeadPosition;

  LeftRight.Value = 95.0;
  Position.UpDownValue = 20;
  Position.ForeBackValue = 0;
  CreateGenerators(Generators, LeftRight, 1800);
  CreateGenerators(Generators, Position, 1800);
  return ApplyGenerators(Generators, true);
}


void MAHeadController::Stop()
{
  Stop(MA::NeckPan);
  Stop(MA::NeckTilt1);
  Stop(MA::NeckTilt2);
}


void MAHeadController::PrintFeatures()
{
  MC_LOG("Head: LeftRight: %1.2f ForeBack: %1.2f UpDown: %1.2f",
         (float)LeftRightTurned.Value, (float)HeadPosition.ForeBackValue,
         (float)HeadPosition.UpDownValue);
}


bool MAHeadController::IsPowerOn(MA::HeadJointsType joint) const
{
  MADevice* Device = nullptr;

  if (joint == MA::NeckPan)
    Device = NeckPan.get();
  if (joint == MA::NeckTilt1)
    Device = NeckTilt1.get();
  if (joint == MA::NeckTilt2)
    Device = NeckTilt2.get();

  if (unlikely(!Device))
  {
    MC_ERROR("The motor device has not been created!");
    return false;
  }
  return Device->IsPowerOn();
}


void MAHeadController::SetPower(MA::HeadJointsType joint, bool new_state)
{
  if (joint == MA::EarLeft || joint == MA::EarRight)
  {
    MC_WARNING("The ear motors do not have power on/off states.");
    return;
  }
  MADevice* Device = nullptr;

  if (joint == MA::NeckPan)
    Device = NeckPan.get();
  if (joint == MA::NeckTilt1)
    Device = NeckTilt1.get();
  if (joint == MA::NeckTilt2)
    Device = NeckTilt2.get();

  if (unlikely(!Device))
  {
    MC_ERROR("The motor device has not been created!");
    return;
  }
  // Set the appropriate power state
  if (Device->IsPowerOn() != new_state)
  {
    Device->SetPower(new_state);
  }
}


void MAHeadController::RegisterDeviceUpdates()
{
  MA_SIGNAL_CONNECT(NeckPan->DeviceValueChangeSignal, MAHeadController::NotifyDeviceUpdate);
  MA_SIGNAL_CONNECT(NeckTilt1->DeviceValueChangeSignal, MAHeadController::NotifyDeviceUpdate);
  MA_SIGNAL_CONNECT(NeckTilt2->DeviceValueChangeSignal, MAHeadController::NotifyDeviceUpdate);
}


void MAHeadController::NotifyDeviceUpdate(MADevice& device)
{
  if (!device.HasSensorValue())
    return;

  if (&device == NeckPan.get())
  {
    // Recalculate the left-right state
    LeftRightTurned.FromJointDegree((int)device.GetSensorValue());
  }
  if (&device == NeckTilt1.get() || &device == NeckTilt2.get())
  {
    // Recalculate the for-back and up-down state
    HeadPosition.FromJointDegrees((int)NeckTilt1->GetSensorValue(), (int)NeckTilt2->GetSensorValue());
  }
}


void MAHeadController::Stop(MA::HeadJointsType joint)
{
  MADevice* Device = nullptr;

  if (joint == MA::NeckPan)
    Device = NeckPan.get();
  if (joint == MA::NeckTilt1)
    Device = NeckTilt1.get();
  if (joint == MA::NeckTilt2)
    Device = NeckTilt2.get();

  if (!Device)
  {
    MC_ERROR("The device has not been created!");
    return;
  }
  Device->ClearGenerators();
}


void MAHeadController::CreateGenerators(MAHeadTransitionGenerators& generators, const MABodyPartFeature& feature,
                                        int msec, MAGeneratorBase::FunctionType function)
{
  MAGeneratorBase* Generator = nullptr;

  if (dynamic_cast<const MALeftRightTurned*>(&feature))
  {
    Generator = new MASimpleGenerator((float)feature.ToJointDegree(), msec);
    Generator->SetTransferFunction(function);
    generators.AddGeneratorToNeckPan(*Generator);
  }
  const MAHeadPosition* Position = dynamic_cast<const MAHeadPosition*>(&feature);

  if (Position)
  {
    Generator = new MASimpleGenerator((float)Position->ToNeckTilt1Degree(), msec);
    Generator->SetTransferFunction(function);
    generators.AddGeneratorToNeckTilt1(*Generator);
    Generator = new MASimpleGenerator((float)Position->ToNeckTilt2Degree(), msec);
    Generator->SetTransferFunction(function);
    generators.AddGeneratorToNeckTilt2(*Generator);
  }
}


MA::DeviceGeneratorsMap MAHeadController::GetSkitTransitionGenerators(MA::SkitBaseSPtr skit)
{
  MAMotionSkit* MotionSkit = dynamic_cast<MAMotionSkit*>(skit.get());

  if (!MotionSkit)
    return MA::DeviceGeneratorsMap();

  MA::DeviceGeneratorsMap DeviceGenerators;
  MAGeneratorContainer* Container = nullptr;

  SetPower(true);
  // Neck tilt1
  NeckTilt1->ClearGenerators();
  Container = MotionSkit->GetGeneratorContainer(MA::NeckTilt1);
  NeckTilt1->AddGeneratorContainer(*Container);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, NeckTilt1.get(), Container);
  // Neck tilt2
  NeckTilt2->ClearGenerators();
  Container = MotionSkit->GetGeneratorContainer(MA::NeckTilt2);
  NeckTilt2->AddGeneratorContainer(*Container);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, NeckTilt2.get(), Container);
  // Neck pan
  NeckPan->ClearGenerators();
  Container = MotionSkit->GetGeneratorContainer(MA::NeckPan);
  NeckPan->AddGeneratorContainer(*Container);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, NeckPan.get(), Container);
  return DeviceGenerators;
}


MA::DeviceGeneratorsMap MAHeadController::ApplyGenerators(MAHeadTransitionGenerators& generators, bool clear)
{
  MA::DeviceGeneratorsMap DeviceGenerators;

  generators.Normalize();
  if (generators.NeckPan)
  {
    SetPower(MA::NeckPan, true);

    if (clear)
    {
      NeckPan->ClearGenerators();
    }
    NeckPan->AddGeneratorContainer(*generators.NeckPan);
    DeviceGenerators.insert(MA::DeviceGeneratorPair(NeckPan.get(), generators.NeckPan));
  }

  if (generators.NeckTilt1)
  {
    SetPower(MA::NeckTilt1, true);

    if (clear)
    {
      NeckTilt1->ClearGenerators();
    }
    NeckTilt1->AddGeneratorContainer(*generators.NeckTilt1);
    DeviceGenerators.insert(MA::DeviceGeneratorPair(NeckTilt1.get(), generators.NeckTilt1));
  }

  if (generators.NeckTilt2)
  {
    SetPower(MA::NeckTilt2, true);

    if (clear)
    {
      NeckTilt2->ClearGenerators();
    }
    NeckTilt2->AddGeneratorContainer(*generators.NeckTilt2);
    DeviceGenerators.insert(MA::DeviceGeneratorPair(NeckTilt2.get(), generators.NeckTilt2));
  }
  return DeviceGenerators;
}