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

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

#include <MCLog.hpp>

namespace MA
{
MCThreadLocalData<MAMouthController> Mouth(false);
}

MAMouthController::MAMouthController() : MAController(MA::MotorController)
{
  if (MA::Mouth.get())
  {
    MC_WARNING("The global mouth controller is overridden.");
    MA::Mouth.release();
  }
  MA::Mouth.reset(this);
  MA_BIND_TRANSITION(MAMouthController, CloseMouth, IsMouthClosed);
  MA_BIND_TRANSITION_ACTION(MAMouthController, Speaking);
  MA_BIND_TRANSITION_ACTION(MAMouthController, Burp);
}


MAMouthController::~MAMouthController()
{
  if (MA::Mouth.get() == this)
    MA::Mouth.release();
}


void MAMouthController::UpdateRobotState(MARobotState& state)
{
  state.Head->Mouth = (int)Mouth->GetSensorValue();
  state.Head->MouthForce = (int)Mouth->GetForce();
  state.Head->MouthMoving = Mouth->HasGenerator();
  state.Head->MouthClosed = IsMouthClosed();
}


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

  Motors.push_back(Mouth.get());
  return Motors;
}


bool MAMouthController::IsMovingMouth() const
{
  return Mouth->HasGenerator();
}


MA::DeviceGeneratorsMap MAMouthController::CloseMouth()
{
  MAGeneratorBase::FunctionType Function = MAGeneratorBase::GetRandomFunction();
  MAGeneratorContainer* Container = nullptr;

  Container = new MAGeneratorContainer(*CreateGenerator(MAMouthOpened(0), 500, Function));
  return ApplyGenerators(*Container, true);
}


bool MAMouthController::IsMouthClosed() const
{
  return (int)MouthOpened.Value < 10;
}


MA::DeviceGeneratorsMap MAMouthController::Speaking()
{
  MAGeneratorContainer* Container = nullptr;

  if ((int)MouthOpened.Value < 25 || (int)MouthOpened.Value > 35)
  {
    MA_RANDOM_POINT_1(MouthOpenedValue, 25, 35);

    Container = new MAGeneratorContainer(*CreateGenerator(MAMouthOpened(MouthOpenedValue), 400,
                                         MAGeneratorBase::GetRandomFunction()));
  }
  MA_RANDOM_POINT_N(MouthOpenedValues, 10, 0, 20);
  MA_RANDOM_POINT_N(MouthOpenedDurations, 10, 0, 100);

  for (int i = 0; i < 5; ++i)
  {
    MAGeneratorBase::FunctionType Function = MAGeneratorBase::GetRandomFunction();
    MAGeneratorBase* Generator = nullptr;

    Generator = CreateGenerator(MAMouthOpened(15+MouthOpenedValues[i]), 60+MouthOpenedDurations[i],
                                Function);
    if (!Container)
    {
      Container = new MAGeneratorContainer(*Generator);
    } else {
      Container->AddGenerator(*Generator);
    }
    Generator = CreateGenerator(MAMouthOpened(15+20-MouthOpenedValues[i]), 60+100-MouthOpenedDurations[i],
                                Function);
    Container->AddGenerator(*Generator);
  }
  return ApplyGenerators(*Container, true);
}


MA::DeviceGeneratorsMap MAMouthController::Burp()
{
  MAGeneratorContainer* Container = nullptr;
  MAGeneratorBase* Generator = nullptr;

  Generator = CreateGenerator(MAMouthOpened(70), 400, MAGeneratorBase::GetRandomFunction());
  Container = new MAGeneratorContainer(*Generator);
  Generator = CreateGenerator(MAMouthOpened(60), 100, MAGeneratorBase::GetRandomFunction());
  Container->AddGenerator(*Generator);
  Generator = CreateGenerator(MAMouthOpened(70), 100, MAGeneratorBase::GetRandomFunction());
  Container->AddGenerator(*Generator);
  Generator = CreateGenerator(MAMouthOpened(0), 400, MAGeneratorBase::GetRandomFunction());
  Container->AddGenerator(*Generator);
  return ApplyGenerators(*Container, true);
}


void MAMouthController::Stop()
{
  Mouth->ClearGenerators();
}


bool MAMouthController::IsPowerOn() const
{
  return Mouth->IsPowerOn();
}


void MAMouthController::SetPower(bool new_state)
{
  // Set the appropriate power state
  if (Mouth->IsPowerOn() != new_state)
  {
    Mouth->SetPower(new_state);
    // Clear the generators when switching off the joints
    if (!new_state)
      Stop();
  }
}


void MAMouthController::RegisterDeviceUpdates()
{
  MA_SIGNAL_CONNECT(Mouth->DeviceValueChangeSignal, MAMouthController::NotifyDeviceUpdate);
}


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

  // Recalculate the mouth state
  MouthOpened.FromJointDegree((int)Mouth->GetSensorValue());
}


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

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

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

  SetPower(true);
  Container = MotionSkit->GetGeneratorContainer(MA::MouthJoint);
  Mouth->ClearGenerators();
  Mouth->AddGeneratorContainer(*Container);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, Mouth.get(), Container);
  return DeviceGenerators;
}


MAGeneratorBase* MAMouthController::CreateGenerator(const MAMouthOpened& feature, int msec,
                                                    MAGeneratorBase::FunctionType function)
{
  MAGeneratorBase* Generator = nullptr;

  Generator = new MASimpleGenerator((float)feature.ToJointDegree(), msec);
  Generator->SetTransferFunction(function);
  return Generator;
}


MA::DeviceGeneratorsMap MAMouthController::ApplyGenerators(MAGeneratorContainer& container, bool clear)
{
  MA::DeviceGeneratorsMap DeviceGenerators;

  SetPower(true);
  if (clear)
  {
    Mouth->ClearGenerators();
  }
  Mouth->AddGeneratorContainer(container);
  MA_INSERT_DEVICEGENERATOR(DeviceGenerators, Mouth.get(), &container);
  return DeviceGenerators;
}