// -*-C++-*-
/*!
 * @file  RefHardCtrlProvSVC_impl.cpp
 * @brief Service implementation code of RefHardCtrlProv.idl
 *
 */

#include "RefHardCtrlProvSVC_impl.h"

/*
 * Example implementational code for IDL interface RefHardCtrl
 */
RefHardCtrlSVC_impl::RefHardCtrlSVC_impl()
{
  // Please add extra constructor code here.
}


RefHardCtrlSVC_impl::~RefHardCtrlSVC_impl()
{
  // Please add extra destructor code here.
}


/*
 * Methods corresponding to IDL attributes and operations
 */
void RefHardCtrlSVC_impl::RHMoveTo(const Position2D& pos, CORBA::Short direction)
{
  // Please insert your code here and remove the following warning pragma
  std::cout << "---RHMoveTo---" << std::endl;
  std::cout << "Not Supported." << std::endl;
  if (CORBA::is_nil((terminal_)->getObject())==false) {
    std::cout << "SetState(0x99, \"NOT SUPPORTED\")" << std::endl;
    (*terminal_)->SetState(0x99, "NOT SUPPORTED");
  }
}

void RefHardCtrlSVC_impl::RHStop()
{
  // Please insert your code here and remove the following warning pragma
  std::cout << "---RHStop---" << std::endl;
  int  ret=0;
  char cmd[64];
  sprintf( cmd, "python ../script/control/RHStop.py");
  ret = system(cmd);
  if(ret >= 0){
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(" << ret << ", \"COMPLETE)\")" << std::endl;
      (*terminal_)->SetState(ret, "COMPLETE");
    }
  } else {
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x98, \"UNKNOWN ERROR(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x98, "UNKNOWN ERROR");
    }
  }
}

void RefHardCtrlSVC_impl::RHReturn(const Position2D& base)
{
  // Please insert your code here and remove the following warning pragma
  std::cout << "---RHReturn---" << std::endl;
  std::cout << "base  :" << base.x << "," << base.y << ","<< base.theta << std::endl;
  int  ret=0;
  if( iamhungry != 1 ){
    char cmd[64];
    sprintf( cmd, "python ../script/control/RHReturn.py");
//    sprintf( cmd, "python ../script/control/Dummy.py");
    ret = system(cmd);
  }

  if(iamhungry == 1){
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x03, \"BATTERY ALERT(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x03, "BATTERY ALERT");
    }
  } else if(ret >= 0){
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(" << ret << ", \"COMPLETE)\")" << std::endl;
      (*terminal_)->SetState(ret, "COMPLETE");
    }
  } else {
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x98, \"UNKNOWN ERROR(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x98, "UNKNOWN ERROR");
    }
  }
}

CORBA::Long RefHardCtrlSVC_impl::RHGetStatus()
{
  // Please insert your code here and remove the following warning pragma
  //std::cout << "---RHGetStatus---" << std::endl;
  //std::cout << "Not Supported." << std::endl;
  if (CORBA::is_nil((terminal_)->getObject())==false) {
    //std::cout << "SetState(0x99, \"NOT SUPPORTED\")" << std::endl;
    (*terminal_)->SetState(0x99, "NOT SUPPORTED");
  }
  return 0;
}

Position3D RefHardCtrlSVC_impl::RHGetArmPosition()
{
  // Please insert your code here and remove the following warning pragma
  Position3D result;
  std::cout << "---RHGetArmPosition---" << std::endl;
  std::cout << "Not Supported." << std::endl;
  if (CORBA::is_nil((terminal_)->getObject())==false) {
    std::cout << "SetState(0x99, \"NOT SUPPORTED\")" << std::endl;
    (*terminal_)->SetState(0x99, "NOT SUPPORTED");
  }
  return result;
}

Position2D RefHardCtrlSVC_impl::RHGetLocalizePosition()
{
  // Please insert your code here and remove the following warning pragma
  Position2D result;
  std::cout << "---RHGetLocalizePosition---" << std::endl;
  std::cout << "Not Supported." << std::endl;
  if (CORBA::is_nil((terminal_)->getObject())==false) {
    std::cout << "SetState(0x99, \"NOT SUPPORTED\")" << std::endl;
    (*terminal_)->SetState(0x99, "NOT SUPPORTED");
  }
  return result;
}

void RefHardCtrlSVC_impl::RHGuideReq(const Position2D& entrance, const Position2D& room, const char* name)
{
  // Please insert your code here and remove the following warning pragma
  std::cout << "---RHGuideReq---" << std::endl;
  std::cout << "Not Supported." << std::endl;
  if (CORBA::is_nil((terminal_)->getObject())==false) {
    std::cout << "SetState(0x99, \"NOT SUPPORTED\")" << std::endl;
    (*terminal_)->SetState(0x99, "NOT SUPPORTED");
  }
}

void RefHardCtrlSVC_impl::RHDrinkServeReq(const Position2D& visitor, const Position2D& warehouse, const char* name)
{
  // Please insert your code here and remove the following warning pragma

  std::cout << "---RHDrinkServeReq---" << std::endl;
  std::cout << "visitor  :" << visitor.x << "," << visitor.y << ","<< visitor.theta << std::endl;
  std::cout << "warehouse:" << warehouse.x << "," << warehouse.y << ","<< warehouse.theta << std::endl;
  std::cout << "name     :" << name << std::endl;
  int  ret=0;
  if( iamhungry != 1 ){
    char cmd[64];
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x04, \"DRINK SERVING(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x04, "DRINK SERVING");
    }
    sprintf( cmd, "python ../script/control/1_scenario_waiter.py");
//    sprintf( cmd, "python ../script/control/RHDrinkServe.py");
//    sprintf( cmd, "python ../script/control/Dummy.py");
    ret = system(cmd);
  }

  if(iamhungry == 1){
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x03, \"BATTERY ALERT(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x03, "BATTERY ALERT");
    }
  } else if(ret >= 0){
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(" << ret << ", \"COMPLETE)\")" << std::endl;
      (*terminal_)->SetState(ret, "COMPLETE");
    }
  } else {
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x98, \"UNKNOWN ERROR(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x98, "UNKNOWN ERROR");
    }
  }
}

void RefHardCtrlSVC_impl::RHDrinkTransportReq(const Position2D& dist, const char* name)
{
  // Please insert your code here and remove the following warning pragma
  std::cout << "---RHDrinkTransportReq---" << std::endl;
  std::cout << "dist :" << dist.x << "," << dist.y << ","<< dist.theta << std::endl;
  std::cout << "name :" << name << std::endl;
  int  ret=0;
  if( iamhungry != 1 ){
    char cmd[64];
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x04, \"DRINK SERVING(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x04, "DRINK SERVING");
    }
    sprintf( cmd, "python ../script/control/2_scenario_waiter.py");
//    sprintf( cmd, "python ../script/control/RHDrinkTransport.py");
//    sprintf( cmd, "python ../script/control/Dummy.py");
    ret = system(cmd);
  }

  if(iamhungry == 1){
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x03, \"BATTERY ALERT(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x03, "BATTERY ALERT");
    }
  } else if(ret >= 0){
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(" << ret << ", \"COMPLETE)\")" << std::endl;
      (*terminal_)->SetState(ret, "COMPLETE");
    }
  } else {
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x98, \"UNKNOWN ERROR(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x98, "UNKNOWN ERROR");
    }
  }
}

void RefHardCtrlSVC_impl::RHGarbageCollectReq(const Position2D& dist, const char* name)
{
  // Please insert your code here and remove the following warning pragma
  std::cout << "---RHGarbageCollectReq---" << std::endl;
  std::cout << "dist :" << dist.x << "," << dist.y << ","<< dist.theta << std::endl;
  std::cout << "name :" << name << std::endl;
  int  ret=0;
  if( iamhungry != 1 ){
    char cmd[64];
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x05, \"GARBAGE COLLECTING(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x05, "GARBAGE COLLECTING");
    }
    sprintf( cmd, "python ../script/control/3_scenario_garbage.py");
//    sprintf( cmd, "python ../script/control/RHGarbageColl.py");
//    sprintf( cmd, "python ../script/control/Dummy.py");
    ret = system(cmd);
  }

  if(iamhungry == 1){
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x03, \"BATTERY ALERT(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x03, "BATTERY ALERT");
    }
  } else if(ret >= 0){
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(" << ret << ", \"COMPLETE)\")" << std::endl;
      (*terminal_)->SetState(ret, "COMPLETE");
    }
  } else {
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x98, \"UNKNOWN ERROR(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x98, "UNKNOWN ERROR");
    }
  }
}

void RefHardCtrlSVC_impl::RHGarbageDumpReq(const Position2D& dumpsite, const char* name)
{
  // Please insert your code here and remove the following warning pragma
  std::cout << "---RHGarbageDumpReq---" << std::endl;
  std::cout << "dumpsite :" << dumpsite.x << "," << dumpsite.y << ","<< dumpsite.theta << std::endl;
  std::cout << "name     :" << name << std::endl;
  if (CORBA::is_nil((terminal_)->getObject())==false) {
    std::cout << "SetState(0x99, \"NOT SUPPORTED\")" << std::endl;
    (*terminal_)->SetState(0x99, "NOT SUPPORTED");
  }
#if 0
  int  ret=0;
  if( iamhungry != 1 ){
    char cmd[64];
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x05, \"GARBAGE COLLECTING(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x05, "GARBAGE COLLECTING");
    }
    sprintf( cmd, "python ../script/control/RHGarbageDump.py");
    //sprintf( cmd, "python ../script/control/Dummy.py");
    ret = system(cmd);
  }    

  if(iamhungry == 1){
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x03, \"BATTERY ALERT(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x03, "BATTERY ALERT");
    }
  } else if(ret >= 0){
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(" << ret << ", \"COMPLETE)\")" << std::endl;
      (*terminal_)->SetState(ret, "COMPLETE");
    }
  } else {
    if (CORBA::is_nil((terminal_)->getObject())==false) {
      std::cout << "SetState(0x98, \"UNKNOWN ERROR(" << ret << ")\")" << std::endl;
      (*terminal_)->SetState(0x98, "UNKNOWN ERROR");
    }
  }
#endif
}

/*********************************************************************/
//  setConsumers
/*********************************************************************/
void RefHardCtrlSVC_impl::setConsumers(
	RTC::CorbaConsumer<StateChangeFromRH>* terminal)
{
  terminal_ = terminal;
}

// End of example implementational code
