view/rangesCtrl/UrgCtrl.cpp

/*!
  \file
  \brief kzd@ťĚ URG §ä

  \author Satofumi KAMIMURA

  $Id$
*/

#include "UrgCtrl.h"
#include "LockGuard.h"
#include "SerialCtrl.h"
#include "ScipHandler.h"
#include "ThreadCreator.h"
#include "PointerRingBuffer.h"
#include "Delay.h"
#include "DetectOS.h"
#include "SearchFilePath.h"
#include "FileToArgs.h"
#include "GetTicks.h"


#if defined(LINUX_OS)
#define DefaultDevice "/dev/ttyACM0"
#elif defined(WINDOWS_OS)
#define DefaultDevice "COM1"
#elif defined(MAC_OS)
#define DefaultDevice "/dev/tty.usbmodem1d11"
#endif

using namespace beego;


/*!
  \brief kzd@ťĚ URG §ä
*/
struct UrgCtrl::pImpl {

  std::string error_message;
  SDL_mutex* mutex;
  ScipHandler scip;
  SerialCtrl sci;
  std::string device;
  long baudrate_setting;
  CaptureMode capture_mode;

  size_t skip_frames;
  size_t groups;
  int first_index;
  int last_index;
  bool urg_handstand;

  class ThreadArgs {
  public:
    /*!
      \brief óMf[^Ěi[p
    */
    typedef struct {
      int n;
      long* data;
      size_t timestamp;
    } data_t;
    ScipHandler& scip;
    SDL_mutex* mutex;
    CaptureMode& capture_mode;
    SDL_sem* recv_size;
    long max_dataLength;
    size_t buffer_size;
    PointerRingBuffer<data_t*> ring_buffer;
    size_t current_timestamp;
    bool active;
    int scan_msec;
    bool capture_stop;

    ThreadArgs(ScipHandler& scip_ref, SDL_mutex* mutex_obj,
               CaptureMode& capture_mode_ref)
      : scip(scip_ref), mutex(mutex_obj), capture_mode(capture_mode_ref),
        recv_size(SDL_CreateSemaphore(0)),
        max_dataLength(0), buffer_size(0), current_timestamp(0),
        active(false), capture_stop(false) {
    }

    ~ThreadArgs(void) {
      SDL_DestroySemaphore(recv_size);
    }
  };
  ThreadArgs thread_args;
  ThreadCreator thread;
  long urgTimestamp_diff;

  pImpl(void)
    : error_message("not connected."), mutex(SDL_CreateMutex()),
      device(DefaultDevice), baudrate_setting(DefaultBaudrate),
      capture_mode(ManualCapture),
      skip_frames(0), groups(0), first_index(0), last_index(0),
      urg_handstand(false),
      thread_args(scip, mutex, capture_mode),
      thread(capture_thread, &thread_args), urgTimestamp_diff(0) {
  }

  ~pImpl(void) {
    //thread.wait();
    //fprintf(stderr, "pImpl::stopCapture()\n");
    scip.stopCapture();
    SDL_DestroyMutex(mutex);
  }

  void setHandstand(bool on) {
    urg_handstand = on;
  }

  void parseArgs(int argc, char *argv[]) {
    for (int i = 1; i < argc; ++i) {
      if (! strncmp("--urg_port=", argv[i], 11)) {
        device = &argv[i][11];

      } else if (! strncmp("--urg_baudrate=", argv[i], 15)) {
        baudrate_setting = atoi(&argv[i][15]);

      } else if (! strcmp("--urg_handstand", argv[i])) {
        setHandstand(true);
      }
    }
  }

  bool connect(const char* device, long baudrate) {
    if (isConnected()) {
      disconnect();
    }

    sci.disconnect();
    if (! sci.connect(device, baudrate)) {
      error_message = sci.what();
      return false;
    }
    baudrate_setting = baudrate;
    return connect(&sci);
  }

  bool connect(ConnectionInterface* con_obj) {
    scip.setConnection(con_obj);

    // đp˘˝{[[gíš
    if (! scip.adjustBaudrate(baudrate_setting)) {
      error_message = scip.what();
      return false;
    }

    // ZTîńĚ˝f
    if (! loadSensorParameter(NULL)) {
      return false;
    }

    // óMf[^ĚĹĺ
    thread_args.max_dataLength = getMaxDataLength();
    thread_args.scan_msec = (scip.getScanRpm() / 6) +1;

    // ćžXbhĚNŽ
    thread.run();

    return true;
  }

  void disconnect(void) {
    thread.stop();
    UnlockMutex(mutex);

    stopCaptures();

    //fprintf(stderr, "pImpl::disconnect::stopCapture()\n");
    //thread.wait();
    scip.disconnect();
    error_message = "disconnected.";

    // f[^obt@Ěí
    deleteBuffers();
  }

  void deleteBuffers(void) {

#if 0
    // !!! ˘¸ęAŔˇéąĆ

    for (size_t i = 0; i < n; ++i) {
      data_t* p = ring_buffer.front();
      delete [] p->data;
      // !!! ČşÍAÜ¸˘ńśáČ˘ĚŠČH
      // !!! ÜžA\˘ĚÉfXgN^đč`ˇ×ŤŠĆ
      delete p;
      ring_buffer.rotate();
    }

    // !!! Ü˝Acapture_thread() ŕĚ static buffer đđúˇéąĆ
    // !!! ÄÚąĚ˝ß
#endif
  }

  bool isConnected(void) {
    return (! scip.isConnected()) ? false : true;
  }

  bool loadSensorParameter(SensorParameter* parameter) {
    if (! scip.isConnected()) {
      return false;
    }
    bool ret = scip.loadSensorParameter(parameter);
    if (! ret) {
      error_message = scip.what();
    }
    return ret;
  }

  void setLaserOutput(bool on) {
    if (! isConnected()) {
      return;
    }
    scip.setLaserOutput(on);
  }

  bool getVersionInfo(std::vector<std::string>& lines) {
    if (! scip.isConnected()) {
      return false;
    }
    bool ret = scip.getVersionInfo(lines);
    if (! ret) {
      error_message = scip.what();
    }
    return ret;
  }

  long getMaxDataLength(void) const {
    return scip.getMaxDataLength();
  }

  void setMaxBufferNum(size_t size) {
    for (size_t i = thread_args.ring_buffer.capacity(); i < size; ++i) {
      ThreadArgs::data_t* new_data = new ThreadArgs::data_t;
      new_data->data = new long[thread_args.max_dataLength];
      new_data->timestamp = 0;
      thread_args.ring_buffer.push_buffer(new_data);
    }
    thread_args.buffer_size = size;
  }

  void startCaptures(void) {

    // XbhĹĚf[^ćžđÂ
    thread_args.active = true;

    // obt@ŕĚf[^đNA
    thread_args.ring_buffer.clear();
    while (SDL_SemValue(thread_args.recv_size) > 0) {
      SDL_SemWait(thread_args.recv_size);
    }

    // obt@đQÂČămŰľAćžńđłŔńÉˇé
    if (capture_mode == AutoCapture) {
      if (thread_args.buffer_size < 1) {
        setMaxBufferNum(2);
      }
      scip.setCaptureTimes(0);
    }
    if (! ((capture_mode == ManualCapture) &&
           (thread_args.buffer_size <= 1))) {
      scip.sendCaptureMessage();
    }
  }

  void stopCaptures(void) {

    // capture â~R}hĚs
    scip.stopCapture();

    // XbhĹĚf[^ćžđâ~
    thread_args.active = false;

    // !!! stop ăĚf[^đÇÝĚÄédgÝđÇÁˇ×Ť
    // !!!
  }

  int capture(long* data, size_t max_size) {

    if (capture_mode == ManualCapture) {
      // ManualCapture
      if (thread_args.buffer_size <= 1) {
        setLaserOutput(true);

        // ćžbZ[WĚM, 1 ńŞ
        scip.sendCaptureMessage('G');

      } else {
        // ćžbZ[WĚM, ĄńŞ
        scip.sendCaptureMessage();

        // QńÚČ~Ěf[^đXbhĹćžˇé˝ß
        thread_args.active = true;
      }
      // óM
      int n = scip.recvCaptureData(data, max_size,
                                   thread_args.current_timestamp);
      return n;

    } else {
      // AutoCapture

      // startCaptures() ŞÄÎęÄ˘ČŻęÎAÄŃoˇ
      if (thread_args.active == false) {
        startCaptures();
      }

      if (thread_args.ring_buffer.size() <= 0) {
        return 0;
      }
      if (SDL_SemTryWait(thread_args.recv_size) == SDL_MUTEX_TIMEDOUT) {
        // źOĹ ring_buffer.size() ťčđľÄ˘éĚĹA¸sľČ˘Í¸žŞę
        return 0;
      }

      // obt@ĚŕeđRs[ľÄÔˇ
      pImpl::ThreadArgs::data_t* p = thread_args.ring_buffer.front();
      int n = p->n;
      memmove(data, p->data, sizeof(p->data[0]) * n);
      thread_args.current_timestamp = p->timestamp;
      thread_args.ring_buffer.rotate();

      return n;
    }
  }

  static int capture_thread(void* args) {
    static long* buffer = NULL;
    ThreadArgs* info = static_cast<ThreadArgs*>(args);

    // f[^Ěćž
    LockMutex(info->mutex);
    if (info->active) {
      UnlockMutex(info->mutex);

      // óM
      if (! buffer) {
        // !!! ąĚobt@ÍAěčźšéKvŞ é
        // !!! ÄÚąĚŰÉAŻś URG ŞÂČŞéĆÍŔçČ˘
        buffer = new long[info->max_dataLength];
      }
      size_t recv_timestamp;
      int n = info->scip.recvCaptureData(buffer, info->max_dataLength,
                                         recv_timestamp);

      // QT ĚăĹf[^ŞćžĹŤČ˘ĆŤÉAIšłšé
      if ((info->capture_stop && (n < 0)) || (n == ScipHandler::QT_Recv)) {
        // ćžđ~ˇé
        info->active = false;
        info->capture_stop = false;
      }

      LockMutex(info->mutex);
      // AutoCapture Ĺobt@Ş˝łęÄ˘éĆŤÉÍPÂŞĚf[^đĚÄé
      if (info->capture_mode == AutoCapture) {
        if ((info->buffer_size <= info->ring_buffer.size()) && (n > 0)) {
          // PÂŞĚf[^đĚÄAóMĚćđěé
          info->ring_buffer.rotate();
        }
      }
      if ((info->buffer_size > info->ring_buffer.size()) && (n > 0)) {
        ThreadArgs::data_t* p = info->ring_buffer.get_buffer();
        p->n = n;
        p->timestamp = recv_timestamp;
        memmove(p->data, buffer, sizeof(long) * n);

        // ćžŹ÷
        SDL_SemPost(info->recv_size);
      }
    }
    UnlockMutex(info->mutex);
    delay(info->scan_msec / 10 +1);
    return 0;
  }
};


UrgCtrl::UrgCtrl(void) : pimpl(new pImpl) {
}


UrgCtrl::~UrgCtrl(void) {
  //fprintf(stderr, "~UrgCtrl()\n");
  disconnect();
}


const char* UrgCtrl::what(void) {
  return pimpl->error_message.c_str();
}


bool UrgCtrl::connect(int argc, char* argv[]) {
  LockGuard guard(pimpl->mutex);

  // ftHgÝčĚÇÝoľ
  std::vector<std::string> search_path;
  search_path.push_back("./");
  search_path.push_back("~/.beego/");

  std::string config_file;
  if (searchFilePath(config_file, "beegoconf", search_path)) {
    // Ýčt@CĚŕeđřĆľÄľ¤
    FileToArgs fileArgs;
    fileArgs.load(config_file.c_str(), argv[0]);
    pimpl->parseArgs(fileArgs.argc, fileArgs.argv);
  }
  pimpl->parseArgs(argc, argv);
  return pimpl->connect(pimpl->device.c_str(), pimpl->baudrate_setting);
}


bool UrgCtrl::connect(const char* device, long baudrate) {
  LockGuard guard(pimpl->mutex);
  return pimpl->connect(device, baudrate);
}


bool UrgCtrl::connect(ConnectionInterface* con) {
  LockGuard guard(pimpl->mutex);
  return pimpl->connect(con);
}


void UrgCtrl::disconnect(void) {
  //fprintf(stderr, "UrgCtrl::disconnect()\n");
  LockMutex(pimpl->mutex);
  pimpl->disconnect();
}


bool UrgCtrl::isConnected(void) {
  LockGuard guard(pimpl->mutex);
  return pimpl->isConnected();
}


bool UrgCtrl::getVersionInfo(std::vector<std::string>& lines) {
  LockGuard guard(pimpl->mutex);
  return pimpl->getVersionInfo(lines);
}


bool UrgCtrl::loadSensorParameter(SensorParameter* parameter) {
  LockGuard guard(pimpl->mutex);
  return pimpl->loadSensorParameter(parameter);
}


void UrgCtrl::setSensorParameter(const SensorParameter* parameter) {
  LockGuard guard(pimpl->mutex);
  pimpl->scip.setSensorParameter(parameter);
}


void UrgCtrl::setHandstand(bool on) {
  pimpl->setHandstand(on);
}


void UrgCtrl::setCaptureMode(CaptureMode mode) {
  LockGuard guard(pimpl->mutex);
  if (pimpl->capture_mode != mode) {
    // ćžĚâ~
    //pimpl->scip.stopCapture();
    pimpl->scip.forceStopCapture(ScipHandler::WaitReply);
    pimpl->thread_args.capture_stop = true;

#if 0
    if (pimpl->capture_mode == AutoCapture) {
      pimpl->thread.wait();
    }
#endif
    //pimpl->scip.forceStopCapture(ScipHandler::NoWaitReply);
  }
  pimpl->capture_mode = mode;
}


int UrgCtrl::getCaptureMode(void) {
  return pimpl->capture_mode;
}


void UrgCtrl::startCaptures(void) {
  LockGuard guard(pimpl->mutex);
  pimpl->startCaptures();
}


void UrgCtrl::stopCaptures(void) {
  LockGuard guard(pimpl->mutex);
  pimpl->stopCaptures();
}


int UrgCtrl::capture(long data[], size_t max_size) {
  LockGuard guard(pimpl->mutex);
  int ret = pimpl->capture(data, max_size);

  if (! pimpl->urg_handstand) {
    // f[^Ě˝]
    size_t n = pimpl->thread_args.max_dataLength;
    int front_index = pimpl->scip.getFrontIndex();
    if (front_index < 0) {
      // ˘ÚąĚęČÇ
      return -1;
    }
    if (max_size < n) {
      n = max_size;
    }
    for (size_t i = 0; i < static_cast<size_t>(front_index); ++i) {
      size_t j = (front_index - i) + front_index;
      if (j < n) {
        std::swap(data[i], data[j]);
      }
    }
  }
  return ret;
}


void UrgCtrl::setMaxBufferNum(size_t size) {
  LockGuard guard(pimpl->mutex);
  pimpl->setMaxBufferNum(size);
  if (pimpl->capture_mode == ManualCapture) {
    // ManualCapture ÍAćžobt@žŻf[^đóMˇé
    pimpl->scip.setCaptureTimes(size);
  } else {
    // AutoCapture ÍAíÉf[^đóMˇé
    pimpl->scip.setCaptureTimes(0);
  }
}


unsigned long UrgCtrl::getRawTimestamp(void) {
  LockGuard guard(pimpl->mutex);
  return pimpl->thread_args.current_timestamp;
}


void UrgCtrl::setFrameSkipFrames(size_t skip_frames) {
  LockGuard guard(pimpl->mutex);
  pimpl->skip_frames = skip_frames;
}


void UrgCtrl::setDataGroups(size_t groups) {
  LockGuard guard(pimpl->mutex);
  pimpl->groups = groups;
}


void UrgCtrl::setCaptureRange(int first_index, int last_index) {
  LockGuard guard(pimpl->mutex);

  if (pimpl->capture_mode == AutoCapture) {
    // AutoCapture ĚŰÍAžŚIÉÄćžđsíšé˝ßÉAPxâ~ˇé
    pimpl->scip.forceStopCapture(ScipHandler::NoWaitReply);
    pimpl->thread_args.capture_stop = true;
  }

  if (pimpl->urg_handstand) {
    pimpl->first_index = first_index;
    pimpl->last_index = last_index;

  } else {
    size_t front_index = pimpl->scip.getFrontIndex();
    pimpl->last_index = (front_index - first_index) + front_index;
    pimpl->first_index = (front_index - last_index) + front_index;
  }

  pimpl->scip.setCaptureRange(pimpl->first_index, pimpl->last_index);
}


long UrgCtrl::getMinDistance(void) const {
  LockGuard guard(pimpl->mutex);
  return pimpl->scip.getMinDistance();
}


long UrgCtrl::getMaxDistance(void) const {
  LockGuard guard(pimpl->mutex);
  return pimpl->scip.getMaxDistance();
}


int UrgCtrl::getMaxDataLength(void) const {
  LockGuard guard(pimpl->mutex);
  return pimpl->getMaxDataLength();
}


void UrgCtrl::setLaserOutput(bool on) {
  LockGuard guard(pimpl->mutex);
  pimpl->setLaserOutput(on);
}


double UrgCtrl::index2rad(const int index) const {
  LockGuard guard(pimpl->mutex);
  return pimpl->scip.index2rad(index);
}


int UrgCtrl::rad2index(const double radian) const {
  LockGuard guard(pimpl->mutex);
  return pimpl->scip.rad2index(radian);
}


int UrgCtrl::getScanMsec(void) {
  LockGuard guard(pimpl->mutex);
  return 1000 * 60 / pimpl->scip.getScanRpm();
}


void UrgCtrl::adjustTimestamp(int set_value) {

  // ÄŃoľĚ PC ^CX^vđćž
  size_t first_ticks = GetTicks();

  // URG ŠçĚ^CX^vđćžˇé
  int estimated_delay = 0;
  size_t urg_timestamp = pimpl->scip.getImmediateTimestamp(&estimated_delay);

  // URG ^CX^vćžăĚ PC ^CX^vđćžľAˇŞÔđvZˇé
  size_t last_ticks = GetTicks();

  pimpl->urgTimestamp_diff =
    (urg_timestamp - set_value) - (last_ticks - first_ticks) + estimated_delay;
}


long UrgCtrl::getTimestamp(void) {
  return (getRawTimestamp() - pimpl->urgTimestamp_diff);
}


ConnectionInterface* UrgCtrl::getConnection(void) {
  return pimpl->scip.getConnection();
}