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

#include "core/MADevice.hpp"
#include "core/MAValueGenerators.hpp"
#include "types/MARobotState.hpp"
#include "types/skits/MASkit.hpp"
#include "types/skits/MASkitDatabase.hpp"
#include "behaviors/MABehaviorManager.hpp"

#include <MCLog.hpp>
#include <MCThreadLocalData.hpp>

#include <boost/algorithm/string/predicate.hpp>
#include <boost/algorithm/string.hpp>

#include <algorithm>
#include <stdio.h>

namespace
{
// Device pointer-int map
typedef boost::unordered_map<MADevice*, int> DevicePtrIntMap;

// Verbose mode
MCThreadLocalData<bool> Verbose(true);
// Reference counter for controllers
MCThreadLocalData<int> ControllerRef(true);

void CheckStaticControllerVariables()
{
  if (unlikely(!Verbose.get()))
  {
    Verbose.reset(new bool);
    *Verbose = false;
    ControllerRef.reset(new int);
    *ControllerRef = 0;
  }
}
}

MAController::MAController(MA::ControllerType controller_type) : MARobotStateUpdater(MA::UpdateSensorData),
  MA_CALL_CONTAINER_LOCKER_CTOR, Type(controller_type)
{
  MA_GET_CONTAINER_LOCK_ID(Transitions);

  RegisterUpdater(*this);
  CheckStaticControllerVariables();
  (*ControllerRef)++;
  if (*ControllerRef == 1)
  {
    new MARobotState(true, true);
  }
}


MAController::~MAController()
{
  (*ControllerRef)--;
  if (*ControllerRef == 0)
    delete MA::RobotState.release();
}


MC::StringList MAController::GetTransitionNames() const
{
  MC::StringList TransitionList;

  for (auto& transition : Transitions)
  {
    TransitionList.push_back(transition.first);
  }
  return TransitionList;
}


MC::StringList MAController::GetActiveTransitionsByDevice(const MADevice& device)
{
  MC::StringList TransitionList;

  for (auto& transition : Transitions)
  {
    const MA::DeviceGeneratorsMap& DeviceGenerators = transition.second.get<2>();

    for (auto& generator : DeviceGenerators)
    {
      if (generator.first == &device)
      {
        TransitionList.push_back(transition.first);
      }
    }
  }
  return TransitionList;
}


int MAController::StartTransition(const std::string& name)
{
  std::string Name(name);
  std::string RealSkitName;
  float Scale = 1.0;
  unsigned int InitialMovementDuration = 0;

  if (boost::starts_with(Name, MA::SkitNamePrefix))
  {
    std::vector<std::string> NameSlices;

    boost::split(NameSlices, Name, boost::is_any_of(":"));
    if (NameSlices.size() <= 1)
    {
      MC_WARNING("Invalid transition name was requested to start: %s", name.c_str());
      return -1;
    }
    Name = NameSlices[0]+':'+NameSlices[1];
    RealSkitName = NameSlices[1];
    // Get the scale factor
    if (NameSlices.size() >= 3 && !NameSlices[2].empty())
      Scale = MCStrConvert<float>(NameSlices[2]);
    // Get the initial movement duration
    if (NameSlices.size() >= 4 && !NameSlices[3].empty())
      InitialMovementDuration = MCStrConvert<unsigned int>(NameSlices[3]);
  }
  // Identify the transition
  auto Iter = Transitions.find(Name);

  if (unlikely(Iter == Transitions.end()))
  {
    Iter = RegisterSkitTransition(Name);
    if (unlikely(Iter == Transitions.end()))
    {
      MC_WARNING("Unregistered transition name was requested to start: %s", Name.c_str());
      return -1;
    }
  }
  if (*Verbose)
  {
    MC_LOG("Start transition: %s", Name.c_str());
  }
  MA::DeviceGeneratorsMap& DeviceGenerators = Iter->second.get<2>();
  int InitialMapSize = DeviceGenerators.size();
  MA::DeviceGeneratorsMap NewDeviceGenerators;

  if (boost::starts_with(Name, MA::SkitNamePrefix))
  {
    // Start a skit transition
    if (!MA::SkitDatabase.get())
    {
      MC_WARNING("Skit database is missing to start a skit transition (%s)", Name.c_str());
      return -1;
    }
    MA::SkitSPtr Skit = MA::SkitDatabase->GetSkit(RealSkitName);

    if (Skit.get())
    {
      if (Type == MA::MotorController)
      {
        MA::MotionSkitSPtr MotionSkit = Skit->GetMotionSkit();

        if (!MotionSkit.get())
        {
          MC_WARNING("Skit (%s) does not contain any motion transition", Name.c_str());
          return -1;
        }
        NewDeviceGenerators = GetSkitTransitionGenerators(MotionSkit);
      } else
      if (Type == MA::LedController)
      {
        MA::LedSkitSPtr LedSkit = Skit->GetLedSkit();

        if (!LedSkit.get())
        {
          MC_WARNING("Skit (%s) does not contain any LED transition", Name.c_str());
          return -1;
        }
        NewDeviceGenerators = GetSkitTransitionGenerators(LedSkit);
      }
    } else {
      MC_WARNING("Skit (%s) is not in the database to start a transition", Name.c_str());
      return -1;
    }
    // Scale the generators if needs be
    if (Scale > 0 && Scale != 1.0)
    {
      for (auto& generator : NewDeviceGenerators)
        generator.second->ScaleDuration(Scale);
    }
    // Add initial movements
    if (InitialMovementDuration > 0)
    {
      for (auto& generator : NewDeviceGenerators)
      {
        MAGeneratorBase* Generator = nullptr;

        Generator = new MASimpleGenerator(generator.second->GetDesiredStartingValue(), InitialMovementDuration);
        generator.second->AddGenerator(*Generator, true);
      }
    }
  } else {
    // Start a normal transition
    NewDeviceGenerators = Iter->second.get<0>()();
  }
  MA_CONTAINER_LOCK(Transitions)
  {
    // Set a controller and a transition Name for the new generators...
    for (auto& generator : NewDeviceGenerators)
    {
      generator.second->SetTransitionName(Name);
      MA_SIGNAL_CONNECT(generator.second->ExpiredSignal, MAController::GeneratorContainerExpired);
    }
    // ...and merge them.
    MA_MERGE_DEVICEGENERATORS(DeviceGenerators, NewDeviceGenerators);
  }
  if (InitialMapSize == (int)Iter->second.get<2>().size())
  {
    MC_WARNING("The transition did not register any generators: %s", Name.c_str());
    return -1;
  }
  return GetDeviceGeneratorsDuration(DeviceGenerators);
}


void MAController::StopTransition(const std::string& name)
{
  auto Iter = Transitions.find(name);

  if (unlikely(Iter == Transitions.end()))
  {
    Iter = RegisterSkitTransition(name);
    if (unlikely(Iter == Transitions.end()))
    {
      MC_WARNING("Unregistered transition name was requested to stop: %s", name.c_str());
      return;
    }
  }
  if (*Verbose)
  {
    MC_LOG("Stop transition: %s", name.c_str());
  }
  MA::DeviceGeneratorsMap& DeviceGenerators = Iter->second.get<2>();

  if (DeviceGenerators.size() == 0)
    return;

  MA_CONTAINER_LOCK(Transitions)
  {
    // Remove the generator containers
    for (auto& generator : DeviceGenerators)
    {
      MADevice* Device = generator.first;

      if (Device->HasGeneratorContainer(*generator.second))
      {
        generator.second->BlockExpiredSignal();
        Device->DeleteGeneratorContainer(*generator.second);
      }
    }
    // Clear the original container map
    DeviceGenerators.clear();
  }
}


bool MAController::IsTransitionActive(const std::string& name)
{
  auto Iter = Transitions.find(name);

  if (unlikely(Iter == Transitions.end()))
  {
    Iter = RegisterSkitTransition(name);
    if (unlikely(Iter == Transitions.end()))
    {
      MC_WARNING("Unregistered transition name was queried: %s", name.c_str());
      return false;
    }
  }
  return !Iter->second.get<2>().empty();
}


bool MAController::IsAnyActiveTransition()
{
  for (auto& transition : Transitions)
  {
    if (!transition.second.get<2>().empty())
      return true;
  }
  return false;
}


bool MAController::IsAnyActiveSkitTransition()
{
  for (auto& transition : Transitions)
  {
    if (boost::starts_with(transition.first, MA::SkitNamePrefix) && !transition.second.get<2>().empty())
      return true;
  }
  return false;
}


bool MAController::IsTransitionSuccessful(const std::string& name)
{
  auto Iter = Transitions.find(name);

  if (unlikely(Iter == Transitions.end()))
  {
    Iter = RegisterSkitTransition(name);
    if (unlikely(Iter == Transitions.end()))
    {
      MC_WARNING("Unregistered transition name was queried: %s", name.c_str());
      return false;
    }
  }
  MA::DeviceGeneratorsMap& DeviceGenerators = Iter->second.get<2>();

  if (!DeviceGenerators.empty())
  {
    bool Expired = true;
    MA::GeneratorContainerList ExpiredContainers;

    // If all the registered generators have only infinite delays then
    // the transition is finished.
    MA_CONTAINER_LOCK(Transitions)
    for (auto& generator : DeviceGenerators)
    {
      // Block the just-expired signal otherwise the iterator is invalidated in
      // GeneratorContainerExpired() and crash is granted on AIBO.
      generator.second->BlockExpiredSignal();
      if (generator.second->IsExpired())
      {
        ExpiredContainers.push_back(generator.second);
      } else {
        Expired = false;
      }
      generator.second->UnblockExpiredSignal();
    }
    // Remove the expired containers here
    for (unsigned int i = 0; i < ExpiredContainers.size(); ++i)
    {
      ExpiredContainers[i]->IsExpired();
    }
    if (!Expired)
    {
      return false;
    }
  }
  // Query the IsSuccessful()-like function if the generators are expired
  MA::BoolFuncPtr Func = Iter->second.get<1>();

  if (!Func.empty())
  {
    return Func();
  }
  return true;
}


bool MAController::TransitionHasCheck(const std::string& name)
{
  auto Iter = Transitions.find(name);

  if (unlikely(Iter == Transitions.end()))
  {
    Iter = RegisterSkitTransition(name);
    if (unlikely(Iter == Transitions.end()))
    {
      MC_WARNING("Unregistered transition name was queried: %s", name.c_str());
      return false;
    }
  }
  MA::BoolFuncPtr Func = Iter->second.get<1>();

  return !Func.empty();
}


void MAController::GeneratorContainerExpired(MAGeneratorContainer& container)
{
  std::string TransitionName = container.GetTransitionName();
  auto Iter = Transitions.find(TransitionName);

  if (unlikely(Iter == Transitions.end()))
  {
    Iter = RegisterSkitTransition(TransitionName);
    if (unlikely(Iter == Transitions.end()))
    {
      MC_WARNING("Generator container with unregistered transition name: %s", TransitionName.c_str());
      return;
    }
  }
  MA_CONTAINER_LOCK(Transitions)
  {
    MA::DeviceGeneratorsMap& DeviceGenerators = Iter->second.get<2>();
    bool Erasing = false;

    for (auto gen_iter = DeviceGenerators.begin(); gen_iter != DeviceGenerators.end();)
    {
      // Remove the non-existing or expired generator containers
      // Note: Erasing from multimap while iterating: see http://stackoverflow.com/questions/446205
      if (gen_iter->second == &container)
      {
        Erasing = true;
        DeviceGenerators.erase(gen_iter++);
        break;
      }
      ++gen_iter;
    }
    if (*Verbose && Erasing && DeviceGenerators.empty())
    {
      MC_LOG("Finished transition: %s", TransitionName.c_str());
    }
  }
}


MA::TransitionMap::iterator MAController::RegisterSkitTransition(const std::string& skit_name)
{
  if (!boost::starts_with(skit_name, MA::SkitNamePrefix))
  {
    return Transitions.end();
  }
  return Transitions.insert(std::make_pair(skit_name, MA::TransitionTuple())).first;
}


int MAController::GetDeviceGeneratorsDuration(MA::DeviceGeneratorsMap& device_generators)
{
  if (device_generators.empty())
    return 0;

  DevicePtrIntMap Devices;

  // Calculate the duration per device
  for (auto& generator : device_generators)
  {
    // This should not be needed, but be on the safe side
    if (Devices.find(generator.first) == Devices.end())
    {
      Devices[generator.first] = 0;
    }
    // Note: Somehow this check is needed after experiencing crash with GetDuration()
    if (generator.first->HasGeneratorContainer(*generator.second))
    {
      int Duration = generator.second->GetDuration();

      // An infinite generator has been found
      if (Duration < 0)
        return -1;
      Devices[generator.first] += Duration;
    }
  }
  // Select the maximal duration
  int MaxDuration = 0;

  for (auto& device : Devices)
  {
    MaxDuration = MCMax(MaxDuration, device.second);
  }
  return MaxDuration;
}


void MAController::SetVerbose(bool new_state)
{
  CheckStaticControllerVariables();
  *Verbose = new_state;
}


bool MAController::IsVerbose()
{
  CheckStaticControllerVariables();
  return *Verbose;
}