/*!
  \file
  \brief Phidget モータドライバの制御

  \author Satofumi KAMIMURA

  $Id: PhidgetDevice.cpp 1949 2011-05-06 07:18:01Z satofumi $
*/

#include "PhidgetDevice.h"
#include <phidget21.h>
#include <string>
#include <cstdio>

using namespace qrk;
using namespace std;


struct PhidgetDevice::pImpl
{
  string error_message_;
  CPhidgetMotorControlHandle motor_control_;
  bool connected_;


  pImpl(void)
    : error_message_("no error."), motor_control_(0), connected_(false)
  {
    CPhidgetMotorControl_create(&motor_control_);
    CPhidget_set_OnAttach_Handler(reinterpret_cast<CPhidgetHandle>
                                  (motor_control_), attach_handler, NULL);
    CPhidget_set_OnDetach_Handler(reinterpret_cast<CPhidgetHandle>
                                  (motor_control_), detach_handler, NULL);
    CPhidget_set_OnError_Handler(reinterpret_cast<CPhidgetHandle>
                                 (motor_control_), error_handler, NULL);

    CPhidgetMotorControl_set_OnInputChange_Handler(motor_control_,
                                                   InputChangeHandler, NULL);
    CPhidgetMotorControl_set_OnVelocityChange_Handler(motor_control_,
                                                      VelocityChangeHandler,
                                                      NULL);
    CPhidgetMotorControl_set_OnCurrentChange_Handler(motor_control_,
                                                     CurrentChangeHandler,
                                                     NULL);
  }


  ~pImpl(void)
  {
    CPhidget_delete(reinterpret_cast<CPhidgetHandle>(motor_control_));
  }


  static int attach_handler(CPhidgetHandle MC, void *userptr)
  {
    static_cast<void>(MC);
    static_cast<void>(userptr);

#if 0
    const char *name;
    CPhidget_getDeviceName(MC, &name);

    int serialNo;
    CPhidget_getSerialNumber(MC, &serialNo);
    log_prinprintf("%s %10d attached!\n", name, serialNo);
#endif

    return 0;
  }


  static int detach_handler(CPhidgetHandle MC, void *userptr)
  {
    static_cast<void>(MC);
    static_cast<void>(userptr);

#if 0
    const char *name;
    CPhidget_getDeviceName(MC, &name);

    int serialNo;
    CPhidget_getSerialNumber(MC, &serialNo);
    printf("%s %10d detached!\n", name, serialNo);
#endif

    return 0;
  }


  static int error_handler(CPhidgetHandle MC, void *userptr, int ErrorCode, const char *Description)
  {
    static_cast<void>(MC);
    static_cast<void>(userptr);
    static_cast<void>(ErrorCode);
    static_cast<void>(Description);

#if 0
    printf("Error handled. %d - %s\n", ErrorCode, Description);
#endif

    return 0;
  }


  static int InputChangeHandler(CPhidgetMotorControlHandle MC, void *usrptr, int Index, int State)
  {
    static_cast<void>(MC);
    static_cast<void>(usrptr);
    static_cast<void>(Index);
    static_cast<void>(State);

#if 0
    printf("Input %d > State: %d\n", Index, State);
#endif

    return 0;
  }


  static int VelocityChangeHandler(CPhidgetMotorControlHandle MC, void *usrptr, int Index, double Value)
  {
    static_cast<void>(MC);
    static_cast<void>(usrptr);
    static_cast<void>(Index);
    static_cast<void>(Value);

#if 0
    printf("Motor %d > Current Speed: %f\n", Index, Value);
#endif

    return 0;
  }

  static int CurrentChangeHandler(CPhidgetMotorControlHandle MC, void *usrptr, int Index, double Value)
  {
    static_cast<void>(MC);
    static_cast<void>(usrptr);
    static_cast<void>(Index);
    static_cast<void>(Value);

#if 0
    printf("Motor: %d > Current Draw: %f\n", Index, Value);
#endif

    return 0;
  }
};


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


PhidgetDevice::~PhidgetDevice(void)
{
}


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


bool PhidgetDevice::connect(void)
{
  // !!! -1 の意味を確認する
  CPhidget_open((CPhidgetHandle)pimpl->motor_control_, -1);
  int result = CPhidget_waitForAttachment((CPhidgetHandle)pimpl->motor_control_, 10000);
  // !!! 戻り値を確認する
  if (result) {
    const char *err;
    CPhidget_getErrorDescription(result, &err);
    printf("Problem waiting for attachment: %s\n", err);
    return 0;
  }

  pimpl->connected_ = true;
  return false;
}


void PhidgetDevice::disconnect(void)
{
  CPhidget_close((CPhidgetHandle)pimpl->motor_control_);
  pimpl->connected_ = false;
}


bool PhidgetDevice::isConnected(void) const
{
  return pimpl->connected_;
}


void PhidgetDevice::setMotorDuty(int id, unsigned char duty)
{
  double velocity = duty / 255.0;

  CPhidgetMotorControl_setAcceleration(pimpl->motor_control_, id, 50.00);
  CPhidgetMotorControl_setVelocity(pimpl->motor_control_, id, velocity);
}


void PhidgetDevice::freeServo(int id)
{
  (void)id;

  // !!! 実装方法、不明
}
