monitorTask.cpp

/*
  制御タスクの処理スレッド
  Satofumi KAMIMURA
  $Id$
*/

#include "monitorTask.h"
#include "vmonitor.h"
#include "parseArgs.h"
#include "screenTask.h"
#include <algorithm>


unsigned long MonitorTask::ticks = 0;
std::vector<TaskInterface*> MonitorTask::tasks;
VXV::Position3D MonitorTask::bodyPosition = VXV::Position3D();
RunCtrl* MonitorTask::runObj = NULL;
ScreenTask* MonitorTask::scr = NULL;


int MonitorTask::taskThread(void* args) {
  MonitorTask* obj = (MonitorTask*)args;

  bool active = true;
  unsigned long processed_ticks = 0;
  obj->lock();
  obj->pre_ticks = VXV::GetTicks();
  obj->unlock();
  do {
    obj->lock();

    if (obj->is_pause) {
      // screenTask の実行
      if (!tasks.empty() && tasks[0]) {
        tasks[0]->send();
      }
    } else {
      unsigned long ticks_diff = VXV::GetTicks() - obj->pre_ticks;
      obj->ticks = obj->pre_total +
        static_cast<unsigned long>(ticks_diff * obj->time_magnify);
      unsigned long loop_times = obj->ticks - processed_ticks;
      processed_ticks = obj->ticks;

      // ロボットの位置を更新
      for (std::vector<TaskInterface*>::iterator it = tasks.begin();
           it != tasks.end(); ++it) {
        if (!*it) {
          continue;
        }
        if ((*it)->updatePosition()) {
          bodyPosition = (*it)->getBodyPosition(bodyPosition);
        }
      }

      // 登録タスクの処理
      for (std::vector<TaskInterface*>::iterator it = tasks.begin();
           it != tasks.end(); ++it) {
        if (*it) {
          (*it)->recv();
        }
      }
      for (unsigned long times = 0; times < loop_times; ++times) {
        for (std::vector<TaskInterface*>::iterator it = tasks.begin();
             it != tasks.end(); ++it) {
          if (*it) {
            (*it)->exec1msec(obj->ticks + times);
          }
        }
      }
      for (std::vector<TaskInterface*>::iterator it = tasks.begin();
           it != tasks.end(); ++it) {
        if (*it) {
          (*it)->send();
        }
      }

      // 待機スレッドの起床評価
      for (std::vector<waits_t>::iterator it = obj->waits.begin();
           it != obj->waits.end();) {
        if (obj->ticks > it->ticks) {
          SDL_LockMutex(it->mutex);
          SDL_CondSignal(it->cond);
          SDL_UnlockMutex(it->mutex);
          it = obj->waits.erase(it);
        } else {
          ++it;
        }
      }
    }

    active = obj->active;
    obj->unlock();
    SDL_Delay(1);
  } while (active);

  return 0;
}


MonitorTask::MonitorTask(void)
  : mutex(NULL), thread(NULL), active(true), is_pause(false), pre_total(0),
    pre_ticks(VXV::GetTicks()), time_magnify(1.0) {
  waits.clear();
  mutex = SDL_CreateMutex();
  thread = SDL_CreateThread(taskThread, this);
}


MonitorTask::~MonitorTask(void) {
  lock();
  active = false;
  unlock();
  SDL_WaitThread(thread, NULL);
  SDL_DestroyMutex(mutex);
}


void MonitorTask::lock(void) {
  SDL_LockMutex(mutex);
}


void MonitorTask::unlock(void) {
  SDL_UnlockMutex(mutex);
}


SDL_cond* MonitorTask::setWakeupTicks(unsigned long wait_ticks,
                                      SDL_cond* wait_cond,
                                      SDL_mutex* wait_mutex) {
  if (wait_ticks < ticks) {
    return false;
  }

  waits_t add;
  add.ticks = wait_ticks;
  add.cond = wait_cond;
  add.mutex = wait_mutex;
  waits.push_back(add);

  return add.cond;
}


void MonitorTask::add(TaskInterface* task) {
  if (task) {
    lock();
    task->init();
    tasks.push_back(task);
    unlock();
  }
}


void MonitorTask::del(TaskInterface* task) {
  if (task) {
    lock();
    tasks.erase(remove(tasks.begin(), tasks.end(), task));
    unlock();
  }
}


unsigned long MonitorTask::getTicks(void) {
  lock();
  unsigned long ret_ticks = ticks;
  unlock();
  return ret_ticks;
}


void MonitorTask::pause(void) {
  lock();
  is_pause = true;
  pre_total +=
    static_cast<unsigned long>((VXV::GetTicks() - pre_ticks) * time_magnify);
  unlock();
}


void MonitorTask::start(void) {
  lock();
  pre_ticks = VXV::GetTicks();
  is_pause = false;
  unlock();
}


void MonitorTask::setTimeMagnify(double magnify) {
  pause();
  lock();
  time_magnify = magnify;
  unlock();
  start();
}


void MonitorTask::waitToTicks(unsigned long wait_ticks,
                              SDL_cond* cond, SDL_mutex* wait_mutex) {
  SDL_LockMutex(wait_mutex);

  lock();
  SDL_cond* mustWait = setWakeupTicks(wait_ticks, cond, wait_mutex);
  if (!mustWait) {
    SDL_UnlockMutex(wait_mutex);
  }
  unlock();

  if (mustWait) {
    SDL_CondWait(cond, wait_mutex);
    SDL_UnlockMutex(wait_mutex);
  }
}


void MonitorTask::waitOnlyCommand(vmonitor* mon, int monitorMode,
                                  SDL_cond* cond, SDL_mutex* wait_mutex,
                                  const char* module, const char* command) {

  if (monitorMode == Monitor::Playback) {
    // 待機
    mon->log->lock();
    unsigned long ticks = mon->log->readTag(module, command);
    mon->log->unlock();
    waitToTicks(ticks, cond, wait_mutex);

  } else {
    // 記録
    mon->log->lock();
    mon->log->writeTag(module, command, mon->getTicks());
    mon->log->writeTagEnd();
    mon->log->unlock();
  }
}


void MonitorTask::setRunObject(RunCtrl* run_obj) {
  runObj = run_obj;
}


VXV::Position MonitorTask::getRunPosition(void) {
  if (runObj) {
#if 0
    return runObj->getCrdPosition(runObj, runObj->local_offset + bodyPosition);
#else
    VXV::Position3D position;
    convertWithAngle(position, bodyPosition, runObj->local_offset);
    position += VXV::Position(0, 0, bodyPosition.zt);

    return runObj->getCrdPosition(runObj, position);
#endif
  } else {
    return bodyPosition;
  }
}


void MonitorTask::setScreenObject(ScreenTask* scr_obj) {
  if (tasks.empty()) {
    add(scr_obj);
    scr = scr_obj;

  } else {
    tasks[0] = scr_obj;
    scr = scr_obj;
  }
}