#include <SharknirDrotService.h>

///////////////////////////
// THIS FILE IS NOT USED //
///////////////////////////

int sharknir::drotServiceWorker::DeviceAsyncImpl::asyncWait(const Nice::Time & _t)
{
      return m_worker->timedWaitForNewState(_t);
}

int sharknir::drotServiceWorker::DeviceAsyncImpl::asyncStatus(const std::string & _s, int _d)
{
   m_worker->cmdAction(_d, _s);
   return 0;
}

sharknir::drotServiceWorker::drotServiceWorker(const Nice::JointPtr & _config)
 : sharknir::drotServiceWorker::drotServiceWorkerSkeleton(_config)
{
  m_deviceAsync.m_worker = this;
}

sharknir::drotServiceWorker::~drotServiceWorker()
{
  W_LOG("Not implemented !!");
}

void sharknir::drotServiceWorker::init()
{
  W_LOG("Not implemented !!");
}

void sharknir::drotServiceWorker::deinit()
{
  W_LOG("Not implemented !!");
}

void sharknir::drotServiceWorker::activate()
{
  W_LOG("Not implemented !!");
}

void sharknir::drotServiceWorker::deactivate()
{
  W_LOG("Not implemented !!");
}

void sharknir::drotServiceWorker::terminate()
{
  W_LOG("Not implemented !!");
}


void sharknir::drotServiceWorker::stop()
{
  changeStateOnlyFromTo(Basda::WORKING, Basda::ONLINE);
}

void sharknir::drotServiceWorker::abort()
{
  W_LOG("Not implemented !!");
}

bool sharknir::drotServiceWorker::isReachable()
{
  W_LOG("Not implemented !!");
}

double sharknir::drotServiceWorker::getPosition(std::string _unit)
{
  W_LOG("Not implemented !!");
}

void sharknir::drotServiceWorker::setPosition(double _position, std::string _unit)
{
  W_LOG("Not implemented !!");
}

double sharknir::drotServiceWorker::getDeviceEncoderPosition(std::string _unit)
{
  W_LOG("Not implemented !!");
}

double sharknir::drotServiceWorker::getAbsoluteEncoderPosition()
{
  for (int i = 0; i<=100; i+=25)
    {
      m_deviceAsync.asyncStatus("getAbsoluteEncoderPosition", i);
      m_deviceAsync.asyncWait(Nice::Time::seconds(1.0));
      if(stateHasChanged( ))
        {
          break;
        }
    }
  W_LOG("Not implemented !!");
}

double sharknir::drotServiceWorker::getIncrementalEncoderPosition()
{
  W_LOG("Not implemented !!");
}

double sharknir::drotServiceWorker::convertPosition(double _position, bool _absolute, std::string _unit_from, std::string _unit_to)
{
  W_LOG("Not implemented !!");
}

double sharknir::drotServiceWorker::getVelocity()
{
  W_LOG("Not implemented !!");
}

bool sharknir::drotServiceWorker::isAtHome()
{
  W_LOG("Not implemented !!");
}

bool sharknir::drotServiceWorker::isAtLimit()
{
  W_LOG("Not implemented !!");
}

bool sharknir::drotServiceWorker::isMoving()
{
  W_LOG("Not implemented !!");
}

double sharknir::drotServiceWorker::getNamedPosition(int _named_position)
{
  W_LOG("Not implemented !!");
}

std::string sharknir::drotServiceWorker::getRA()
{
  W_LOG("Not implemented !!");
}

void sharknir::drotServiceWorker::changeDerotationWithAbsoluteOffset(const Nice::Date & _start_date, double _position, std::string _unit)
{
  W_LOG("Not implemented !!");
}

void sharknir::drotServiceWorker::changeDerotationWithRelativeOffset(const Nice::Date & _start_date, double _position, std::string _unit)
{
  W_LOG("Not implemented !!");
}


void sharknir::drotServiceWorker::moveRelative(double _position, std::string _unit)
{
  for (int i = 0; i<=100; i+=25)
  {
     m_deviceAsync.asyncStatus("moveRelative", i);
     m_deviceAsync.asyncWait(Nice::Time::seconds(1.0));
     if(stateHasChanged( ))
     {
       break;
     }
  }
}

void sharknir::drotServiceWorker::moveAbsolute(double _position, std::string _unit)
{
  U8_LOG("MoveAbsolute!!")
  for (int i = 0; i<=100; i+=25)
  {
     m_deviceAsync.asyncStatus("moveAbsolute", i);
     m_deviceAsync.asyncWait(Nice::Time::seconds(1.0));
     if(stateHasChanged( ))
     {
       break;
     }
  }
}

void sharknir::drotServiceWorker::moveToHome()
{
  for (int i = 0; i<=100; i+=25)
  {
     m_deviceAsync.asyncStatus("moveToHome", i);
     m_deviceAsync.asyncWait(Nice::Time::seconds(1.0));
     if(stateHasChanged( ))
     {
       break;
     }
  }
}

void sharknir::drotServiceWorker::moveToLimit(double _limit)
{
  for (int i = 0; i<=100; i+=25)
  {
     m_deviceAsync.asyncStatus("moveToLimit", i);
     m_deviceAsync.asyncWait(Nice::Time::seconds(1.0));
     if(stateHasChanged( ))
     {
       break;
     }
  }
}

void sharknir::drotServiceWorker::moveToNamedPosition(int _named_position)
{
  for (int i = 0; i<=100; i+=25)
  {
     m_deviceAsync.asyncStatus("moveToNamedPosition", i);
     m_deviceAsync.asyncWait(Nice::Time::seconds(1.0));
     if(stateHasChanged( ))
     {
       break;
     }
  }
}

void sharknir::drotServiceWorker::startDerotation(const Nice::Date & _start_date, double _position, std::string _unit)
{
  for (int i = 0; i<=100; i+=25)
  {
     m_deviceAsync.asyncStatus("startDerotation", i);
     m_deviceAsync.asyncWait(Nice::Time::seconds(1.0));
     if(stateHasChanged( ))
     {
       break;
     }
  }
}


