/*!
  \file
  \brief イベントのスケジュール管理

  \author Satofumi KAMIMURA

  $Id: EventScheduler.cpp 1678 2010-02-08 23:46:31Z satofumi $
*/

#include "EventScheduler.h"
#include "Device.h"
#include "ManagedTicks.h"
#include "OdeHandler.h"
#include "ModelManager.h"
#include "Thread.h"
#include "Lock.h"
#include "LockGuard.h"
#include "system_ticks.h"
#include "system_delay.h"
#include <vector>
#include <deque>
#include <map>
#include <algorithm>
#include <cstdlib>

using namespace qrk;
using namespace std;


namespace
{
  enum {
    X = 0,
    Y = 1,
    Z = 2,
  };

  typedef multimap<long, Device*> Devices;
  typedef map<dGeomID, long*> RayIds;

  typedef struct
  {
    ConditionVariable* condition;
    long ticks;
  } WakeupTicks;

  bool operator < (const WakeupTicks& lhs, const WakeupTicks& rhs)
  {
    return (lhs.ticks < rhs.ticks);
  }

  Lock mutex_;
  ManagedTicks timer_;
  long current_ticks_ = 0;
  bool stop_ = false;

  Devices devices_;

  deque<WakeupTicks> wakeup_ticks_;

  dWorldID world_;
  dSpaceID space_;
  dGeomID ground_;
  dJointGroupID contact_group_;

  RayIds ray_ids_;
  ModelManager model_manager_;


  bool measureLaserDistance(int n, dContact contact[], dGeomID o1, dGeomID o2)
  {
    if (n <= 0) {
      return false;
    }

    RayIds::iterator ray_it = ray_ids_.find(o1);
    if (ray_it == ray_ids_.end()) {
      ray_it = ray_ids_.find(o2);
    }
    if (ray_it == ray_ids_.end()) {
      return false;
    }

    // レーザーの判定処理
    dVector3 start;
    dVector3 dir;
    dGeomID ray_id = ray_it->first;
    dGeomRayGet(ray_id, start, dir);

    // 計算した距離には、ランダムで 10 [mm] の誤差を載せる
    // !!! 誤差を付加することで、エラーを示す距離になってもよいことにする
    // !!! 誤差を付加する処理は、range_finder 内で行うように修正する
    double ray_distance =
      sqrt(pow(contact[0].geom.pos[X] - start[X], 2) +
           pow(contact[0].geom.pos[Y] - start[Y], 2) +
           pow(contact[0].geom.pos[Z] - start[Z], 2)) * 1000.0;
    ray_distance += (20.0 * rand() / (RAND_MAX + 1.0)) - 10.0;

    long current_distance = *(ray_it->second);
    if ((current_distance == 1) || (ray_distance < current_distance)) {
      *(ray_it->second) = static_cast<long>(ray_distance);
    }
    return true;
  }


  bool isObstacle(dGeomID id)
  {
    dBodyID body_id = dGeomGetBody(id);
    return model_manager_.obstacle(body_id);
  }


  void nearCallback(void* data, dGeomID o1, dGeomID o2)
  {
    // !!! data が何なのかを確認すべき
    static_cast<void>(data);

    enum { N = 10 };            // 接触点数の最大値
    dContact contact[N];

    int n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact));
    if (measureLaserDistance(n, contact, o1, o2)) {
      return;
    }

    bool is_obstacle = isObstacle(o1) || isObstacle(o2);
    bool is_ground = (ground_ == o1) || (ground_ == o2);
    if (! is_ground) {
      if ((n <= 0) || (! is_obstacle)) {
        // 衝突判定させたい物体のときは return させない
        return;
      }
    }

    for (int i = 0; i < n; ++i) {
      contact[i].surface.mode =
        dContactBounce | dContactSoftCFM | dContactSoftERP;
      contact[i].surface.bounce = 0.5;
      contact[i].surface.bounce_vel = 0.1;

      contact[i].surface.soft_cfm = 0.01;
      contact[i].surface.soft_erp = 0.1;

      // !!! 車輪かそうでないかによって、摩擦係数を変更すべき
      // !!! 車輪でないときは、摩擦係数を 1.0 ぐらいにする
      contact[i].surface.mu = 6.2;

      dJointID c = dJointCreateContact(world_, contact_group_, &contact[i]);
      dJointAttach(c,
                   dGeomGetBody(contact[i].geom.g1),
                   dGeomGetBody(contact[i].geom.g2));
    }
  }


  void clearLasers(void)
  {
    for (RayIds::iterator it = ray_ids_.begin(); it != ray_ids_.end(); ++it) {
      dGeomDestroy(it->first);
    }
    ray_ids_.clear();
  }
}


struct EventScheduler::pImpl
{
  Thread thread_;


  pImpl(void)
    : thread_(scheduler_loop, NULL)
  {
  }


  static pImpl* object(void)
  {
    static pImpl singleton_object;
    return &singleton_object;
  }


  static int scheduler_loop(void*)
  {
    timer_.setTicksFunction(system_ticks);
    timer_.play();

    enum {
      MaxLoopTimes = 25,
    };

    OdeHandler ode_;
    world_ = ode_.world();
    space_ = ode_.space();
    ground_ = ode_.ground();
    contact_group_ = ode_.contactGroup();

    while (! stop_) {
      mutex_.lock();

      // システム時刻を取得
      long target_ticks = timer_.ticks();
      int loop_times = min(target_ticks - current_ticks_,
                           static_cast<long>(MaxLoopTimes));
      //loop_times = 20;

      for (int i = 0; i < loop_times; ++i) {

        // その時刻にイベント・リストに登録されているタスクがあれば、処理
        if (! devices_.empty()) {
          //fprintf(stderr, ".");

          for (Devices::iterator it = devices_.begin();
               it != devices_.upper_bound(current_ticks_); ++it) {
            Device* device = it->second;

            // 実行し、次のタイミングを取得して再登録する
            device->execute();
            long next_ticks = current_ticks_ + device->nextExecuteInterval();
            devices_.insert(pair<long, Device*>(next_ticks, device));
          }
          devices_.erase(devices_.begin(),
                         devices_.upper_bound(current_ticks_));
        }

        // ODE の時間を進める
        if (! model_manager_.empty()) {
          dSpaceCollide(space_, 0, &nearCallback);
          // !!! 遅ければ dWorldQuickStep(), dWorldStepFirst1() の使用を検討する
          //dWorldQuickStep(world_, 0.001);
          dWorldStep(world_, 0.001);
          clearLasers();
          dJointGroupEmpty(contact_group_);
        }

        ++current_ticks_;

        // delay() と待機タスクの起床
        if (! wakeup_ticks_.empty()) {
          WakeupTicks& wakeup = wakeup_ticks_.front();
          if (current_ticks_ >= wakeup.ticks) {
            ConditionVariable* condition = wakeup.condition;
            wakeup_ticks_.pop_front();
            condition->wakeup();
            break;
          }
        }
      }
      mutex_.unlock();

      // !!! QThread::msleep(1); に変更する
      //system_delay(1);
    }

    return 0;
  }


  void setLaser(long* buffer, dGeomID laser_id)
  {
    ray_ids_[laser_id] = buffer;
  }
};


EventScheduler::EventScheduler(void) : pimpl(pImpl::object())
{
}


EventScheduler::~EventScheduler(void)
{
}


Lock* EventScheduler::lock(void)
{
  mutex_.lock();
  return &mutex_;
}


Lock* EventScheduler::tryLock(void)
{
  if (! mutex_.tryLock()) {
    return NULL;
  }
  return &mutex_;
}


void EventScheduler::unlock(void)
{
  mutex_.unlock();
}


void EventScheduler::start(void)
{
  pimpl->thread_.run();
}


void EventScheduler::terminate(void)
{
  LockGuard guard(mutex_);
  stop_ = true;

  for (deque<WakeupTicks>::iterator it = wakeup_ticks_.begin();
       it != wakeup_ticks_.end(); ++it) {
    (*it).condition->wakeup();
  }
  wakeup_ticks_.clear();
}


long EventScheduler::ticks(void) const
{
  return current_ticks_;
}


void EventScheduler::play(void)
{
  LockGuard guard(mutex_);
  timer_.play();
}


void EventScheduler::pause(void)
{
  LockGuard guard(mutex_);
  timer_.pause();
}


void EventScheduler::moreFaster(void)
{
  LockGuard guard(mutex_);
  timer_.moreFaster();
}


void EventScheduler::moreSlower(void)
{
  LockGuard guard(mutex_);
  timer_.moreSlower();
}


void EventScheduler::registerDevice(Device* device)
{
  LockGuard guard(mutex_);
  devices_.insert(pair<long, Device*>(current_ticks_, device));
}


void EventScheduler::removeDevice(Device* device)
{
  LockGuard guard(mutex_);
  for (Devices::iterator it = devices_.begin(); it != devices_.end();) {
    if (it->second == device) {
      devices_.erase(it++);
    } else {
      ++it;
    }
  }
}


bool EventScheduler::registerDelayEvent(ConditionVariable* condition,
                                        size_t msec, bool absolute_msec)
{
  if (stop_) {
    return false;
  }

  long delay_wakeup_ticks = (absolute_msec) ? msec : current_ticks_ + msec;

  // ticks で昇順にソートする
  WakeupTicks wakeup;
  wakeup.condition = condition;
  wakeup.ticks = delay_wakeup_ticks;
  wakeup_ticks_.push_back(wakeup);
  sort(wakeup_ticks_.begin(), wakeup_ticks_.end());

  return true;
}


void EventScheduler::setLaser(long* buffer, dGeomID laser_id)
{
  pimpl->setLaser(buffer, laser_id);
}
