// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/test/obs_ctrl/sequencerI.cpp 301 2017-11-21 10:33:13Z depa $

#include "sequencerI.h"
/* Parses setup string into IndiMessages to be sent to INDI server
 * This is a prototype, so the INDI driver and property names, as well as the
 * elements are hard coded. A better solution would be to ask for these details with a
   a get message.
 */


SequencerI::SequencerI(int argc, char* argv[], const std::string& _strAliasFileName,
                       boost::shared_ptr<osfsm::OSFsm>& _ptrSM)://, IceUtil::Mutex& _mtxSm):
  m_motion(argc, argv, _strAliasFileName, _ptrSM)
{

  //m_mtxCounter = new Mutex();
  // m_msgCount = 0;
  // Start state machine
  // m_ptrStateMachine = &_fsm;
  // m_ptrStateMachine = boost::make_shared<osfsm::OSFsm>(_fsm);
  m_ptrStateMachine = _ptrSM;
  std::cout << m_ptrStateMachine << std::endl;
  // m_ptrMtxStateMachine = boost::make_shared<IceUtil::Mutex>(_mtxSm);
  m_ptrStateMachine->start();
  // state change to STANDBY:
  // NOTE a check on subsystems states should be performed first
  // NOTE add methods to get state from subsystems
  m_ptrStateMachine->process_event(osfsm::init_standby(true));

  // state change to ONLINE
  // NOTE same as above
  m_ptrStateMachine->process_event(osfsm::standby_online(true, false));

}

SequencerI::~SequencerI()
{
  //m_msgCount = NULL;
  //
}

// converts setup string to map. The generated map will be sent
// to the TAN device

std::map<std::string, int> SequencerI::parse_setup_string(const std::string& _strSetup)
{
  int cmdPos = -1; // position of wheel is a positive integer
  int i = 1;
  const std::string sepSpace = " ";
  const std::string sepEqual = "=";
  std::string::size_type pos = 0;
  std::string::size_type spacePos = 0;
  std::string::size_type equalPos = 0;
  std::string::size_type strLen = 0;
  std::string strCmd   = "";
  std::string aliasName = "";
  std::string errorMsg = "";

  std::map<std::string, int> mapSetup;

  std::cout << __FUNCTION__ << " --- " << _strSetup << std::endl;
  while ((spacePos = _strSetup.find(sepSpace, pos)) != std::string::npos) 
    {
      // Parsing element name and value
      // strCmd will contain elemName=N
      strLen = spacePos - pos;
      strCmd = _strSetup.substr(pos, strLen);
      std::cout << __FUNCTION__ << std::endl
                << strCmd << std::endl;

      equalPos = strCmd.find(sepEqual);
      strLen = strCmd.size() - equalPos;
      std::cout << strCmd.substr(equalPos+1, strLen) << std::endl;
      cmdPos = atoi(strCmd.substr(equalPos+1, strLen).c_str());
      std::cout << cmdPos << std::endl;
      aliasName = strCmd.substr(0, equalPos);
#if ! defined BOOST_NO_UNIFIED_INITIALIZATION_SYNTAX
      mapSetup.insert({aliasName, cmdPos});
#else
      mapSetup.insert(std::pair<std::string, int>(aliasName, cmdPos));
#endif
      pos = spacePos + 1;
      i++;
    } // end while

  strCmd = _strSetup.substr(pos, _strSetup.size());
  std::cout << __FUNCTION__ << std::endl
            << strCmd << std::endl;
  equalPos = strCmd.find(sepEqual);
  strLen = strCmd.size() - equalPos;

  cmdPos = atoi(strCmd.substr(equalPos+1, strLen).c_str());
  std::cout << cmdPos << std::endl;
  aliasName = strCmd.substr(0, equalPos);
#if ! defined BOOST_NO_UNIFIED_INITIALIZATION_SYNTAX
  mapSetup.insert({aliasName, cmdPos});
#else
  mapSetup.insert(std::pair<std::string, int>(aliasName, cmdPos));
#endif
  std::cout << __FUNCTION__ << " ---- " << mapSetup.size() << std::endl;
  return mapSetup;
}

// this will probably become deprecated, in favor of setupMotionDict
int SequencerI::setupMotion(const dictNonSciAssembly& d, const std::string& strSetup, const Ice::Current&)
{
  /*
   * strSetup is a repetition of several strings functionN=valueN space separated
   */
  int status = 0;
  std::map<std::string, int> mapSetup;
  // NOTE: prototyping motion_ctrl: reporting connection status only to its IndiClient
  // std::cout << "====================================================" <<std::endl;

  if (strSetup.empty())
    // raise an error
    throw std::invalid_argument("Invalid input parameter sciStr");
  else
    {
      mapSetup = parse_setup_string(strSetup);
      // m_ptrStateMachine.process_event(osfsm::read_busy(mapSetup.size()));
      m_motion.moveToNamedPosition(mapSetup);
    }
  return status;
}

int SequencerI::setupMotionDict(const dictSetup& mapSetup, const Ice::Current&)
{
  // dSetup keys are devices aliases
  // "INBEAM_DEP"
  // "INBEAM_TT_X"
  // "INBEAM_TT_Y"
  // "SHUTTER"
  // "APO_W"
  // "ND_W"
  // "CORO_W"
  // "LYOT_W"
  // "SCI1_W"
  // "SCI2_W"
  // "LENS_DEP"
  // "DB_W"
  int status = 0;
  if (mapSetup.empty())
    // raise error, LOG a warning
    throw std::invalid_argument("Invalid input parameter for setup motion");
  else
    {
      std::map<std::string, int> m = mapSetup;
      for (BOOST_AUTO(it, m.begin()); it != m.end(); ++it) {
        std::cout << it->first << std::endl;
      }
      m_motion.moveToNamedPosition(m);
    }

  return status;
}

int SequencerI::setupTracking(const std::string& strSetup, const Ice::Current&)
{
  //
  return 0;
}


int SequencerI::setupCalibration(const dictNonSciAssembly& d,
                                 const std::string& strSetup, const Ice::Current&)
{
  //
  return 0;
}


// std::vector<int> SequencerI::get_indiClient_fd()
// {
//   std::vector<int> result;
//   std::vector<IndiClient>::iterator it;
//   for (it = m_vecClient.begin(); it < m_vecClient.end(); ++it)
//     result.push_back(it->get_socket_fd());
//   return result;
// }
