bool mtsIntuitiveResearchKitConsole::Connect(void)
{
    mtsManagerLocal * componentManager = mtsManagerLocal::GetInstance();

    const ArmList::iterator armsEnd = mArms.end();
    for (ArmList::iterator armIter = mArms.begin();
         armIter != armsEnd;
         ++armIter) {
        Arm * arm = armIter->second;

        // IO
        if (arm->IOInterfaceRequired) {
            componentManager->Connect(this->GetName(), "IO-" + arm->Name(),
                                      arm->IOComponentName(), arm->Name());
        }
        // PID
        if (arm->mType != Arm::ARM_SUJ) {
            if (arm->PIDInterfaceRequired) {
                componentManager->Connect(this->GetName(), "PID-" + arm->Name(),
                                          arm->PIDComponentName(), "Controller");
            }
        }
        // arm interface
        if (arm->ArmInterfaceRequired) {
            componentManager->Connect(this->GetName(), arm->Name(),
                                      arm->Name(), "Robot");
        }
        // arm specific interfaces
        arm->Connect();
        // connect to SUJ if needed
        if (arm->SUJInterfaceRequiredFromIO && arm->SUJInterfaceRequiredToSUJ) {
            componentManager->Connect(this->GetName(), arm->SUJInterfaceRequiredToSUJ->GetName(),
                                      "SUJ", arm->Name());
            componentManager->Connect(this->GetName(), arm->SUJInterfaceRequiredFromIO->GetName(),
                                      arm->IOComponentName(), arm->Name() + "-SUJClutch");
        }
    }

    const TeleopList::iterator teleopsEnd = mTeleops.end();
    for (TeleopList::iterator teleopIter = mTeleops.begin();
         teleopIter != teleopsEnd;
         ++teleopIter) {
        TeleopPSM * teleop = teleopIter->second;
        teleop->Connect();
    }

    // connect the foot pedals if needed
    if (mHasFootpedals) {
        this->ConnectFootpedalInterfaces();
    }

    // connect interfaces to retrieve base frame from ECM SUJ and send event to SUJ
    if (mSUJECMInterfaceRequired
        && mECMBaseFrameInterfaceProvided) {
        componentManager->Connect(this->GetName(), "BaseFrame", "SUJ", "ECM");
        componentManager->Connect("SUJ", "BaseFrame", this->GetName(), "ECMBaseFrame");
    }

    return true;
}
bool mtsIntuitiveResearchKitConsole::AddArm(mtsComponent * genericArm, const mtsIntuitiveResearchKitConsole::Arm::ArmType CMN_UNUSED(armType))
{
    // create new required interfaces to communicate with the components we created
    Arm * newArm = new Arm(genericArm->GetName(), "");
    if (SetupAndConnectInterfaces(newArm)) {
        mArms.push_back(newArm);
        return true;
    }
    CMN_LOG_CLASS_INIT_ERROR << GetName() << ": AddArm, unable to add new arm.  Are you adding two arms with the same name? "
                             << newArm->Name() << std::endl;
    delete newArm;
    return false;
}
bool mtsIntuitiveResearchKitConsole::AddArm(mtsComponent * genericArm, const mtsIntuitiveResearchKitConsole::Arm::ArmType CMN_UNUSED(armType))
{
    // create new required interfaces to communicate with the components we created
    Arm * newArm = new Arm(genericArm->GetName(), "");
    if (AddArmInterfaces(newArm)) {
        ArmList::iterator armIterator = mArms.find(newArm->mName);
        if (armIterator != mArms.end()) {
            mArms[newArm->mName] = newArm;
            return true;
        }
    }
    CMN_LOG_CLASS_INIT_ERROR << GetName() << ": AddArm, unable to add new arm.  Are you adding two arms with the same name? "
                             << newArm->Name() << std::endl;
    delete newArm;
    return false;
}
void mtsIntuitiveResearchKitConsole::Configure(const std::string & filename)
{
    mConfigured = false;

    std::ifstream jsonStream;
    jsonStream.open(filename.c_str());

    Json::Value jsonConfig, jsonValue;
    Json::Reader jsonReader;
    if (!jsonReader.parse(jsonStream, jsonConfig)) {
        CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to parse configuration\n"
                                 << jsonReader.getFormattedErrorMessages();
        this->mConfigured = false;
        return;
    }

    // extract path of main json config file to search other files relative to it
    cmnPath configPath(cmnPath::GetWorkingDirectory());
    std::string fullname = configPath.Find(filename);
    std::string configDir = fullname.substr(0, fullname.find_last_of('/'));
    configPath.Add(configDir);

    // IO default settings
    double periodIO = 0.5 * cmn_ms;
    int firewirePort = 0;
    // get user preferences
    jsonValue = jsonConfig["io"];
    if (!jsonValue.empty()) {
        CMN_LOG_CLASS_INIT_VERBOSE << "Configure: looking for user provided io:period and io:port" << std::endl;
        jsonValue = jsonConfig["io"]["period"];
        if (!jsonValue.empty()) {
            periodIO = jsonValue.asDouble();
        }
        jsonValue = jsonConfig["io"]["port"];
        if (!jsonValue.empty()) {
            firewirePort = jsonValue.asInt();
        }
    } else {
        CMN_LOG_CLASS_INIT_VERBOSE << "Configure: using default io:period and io:port" << std::endl;
    }
    CMN_LOG_CLASS_INIT_VERBOSE << "Configure: period IO is " << periodIO << std::endl
                               << "Configure: FireWire port is " << firewirePort << std::endl;
    // create IO
    mtsRobotIO1394 * io = new mtsRobotIO1394(mIOComponentName, periodIO, firewirePort);

    const Json::Value arms = jsonConfig["arms"];
    for (unsigned int index = 0; index < arms.size(); ++index) {
        if (!ConfigureArmJSON(arms[index], io->GetName(), configPath)) {
            CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to configure arms[" << index << "]" << std::endl;
            return;
        }
    }

    // loop over all arms to configure IO only
    const ArmList::iterator end = mArms.end();
    ArmList::iterator iter;
    for (iter = mArms.begin(); iter != end; ++iter) {
        std::string ioConfig = iter->second->mIOConfigurationFile;
        if (ioConfig != "") {
            io->Configure(ioConfig);
        }
    }

    mtsComponentManager::GetInstance()->AddComponent(io);

    // now can configure PID and Arms
    for (iter = mArms.begin(); iter != end; ++iter) {
        const std::string pidConfig = iter->second->mPIDConfigurationFile;
        if (pidConfig != "") {
            iter->second->ConfigurePID(pidConfig);
        }
        const std::string armConfig = iter->second->mArmConfigurationFile;
        if (armConfig != "") {
            iter->second->ConfigureArm(iter->second->mType, armConfig);
        }
    }

    bool hasSUJ = false;
    bool hasECM = false;

    // now load all PSM teleops
    const Json::Value psmTeleops = jsonConfig["psm-teleops"];
    for (unsigned int index = 0; index < psmTeleops.size(); ++index) {
        if (!ConfigurePSMTeleopJSON(psmTeleops[index])) {
            CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to configure psm-teleops[" << index << "]" << std::endl;
            return;
        }
    }

    // see which event is used for operator present
    // find name of button event used to detect if operator is present
    mOperatorPresentComponent = jsonConfig["operator-present"]["component"].asString();
    mOperatorPresentInterface = jsonConfig["operator-present"]["interface"].asString();
    //set defaults
    if (mOperatorPresentComponent == "") {
        mOperatorPresentComponent = mIOComponentName;
    }
    if (mOperatorPresentInterface == "") {
        mOperatorPresentInterface = "COAG";
    }

    // look for footpedals in json config
    jsonValue = jsonConfig["io"]["has-footpedals"];
    if (jsonValue.empty()) {
        mHasFootpedals = false;
    } else {
        mHasFootpedals = jsonValue.asBool();
    }
    // if we have any teleoperation component, we need to add the interfaces for the foot pedals
    if (mTeleops.size() > 0) {
        mHasFootpedals = true;
    }
    if (mHasFootpedals) {
        this->AddFootpedalInterfaces();
    }

    // interface to ecm to get ECM frame and then push to PSM SUJs as base frame
    mtsInterfaceRequired * ecmArmInterface = 0;
    for (iter = mArms.begin(); iter != end; ++iter) {
        if (iter->second->mType == Arm::ARM_ECM) {
            hasECM = true;
            ecmArmInterface = iter->second->ArmInterfaceRequired;
        }
        else if (iter->second->mType == Arm::ARM_SUJ) {
            hasSUJ = true;
        }
    }

    // add required and provided interfaces to grab positions from ECM SUJ and ECM
    if (hasSUJ && hasECM) {
        mSUJECMInterfaceRequired = AddInterfaceRequired("BaseFrame");
        if (mSUJECMInterfaceRequired) {
            mSUJECMInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::SUJECMBaseFrameHandler, this, "BaseFrameDesired");
        }
        if (ecmArmInterface) {
            ecmArmInterface->AddFunction("GetPositionCartesianLocal", mGetPositionCartesianLocalFromECM);
        } else {
            CMN_LOG_CLASS_INIT_VERBOSE << "Configure: arm interface not yet added for ECM" << std::endl;
        }
        mECMBaseFrameInterfaceProvided = AddInterfaceProvided("ECMBaseFrame");
        if (mECMBaseFrameInterfaceProvided) {
            mECMBaseFrameInterfaceProvided->AddEventWrite(mECMBaseFrameEvent, "BaseFrameDesired", prmPositionCartesianGet());
        }
    }

    // connect arm SUJ clutch button to SUJ
    if (hasSUJ) {
        for (iter = mArms.begin(); iter != end; ++iter) {
            Arm * arm = iter->second;
            if ((arm->mType == Arm::ARM_ECM) || (arm->mType == Arm::ARM_PSM)) {
                arm->SUJInterfaceRequiredFromIO = this->AddInterfaceRequired("SUJ-" + arm->Name() + "-IO");
                arm->SUJInterfaceRequiredFromIO->AddEventHandlerWrite(&Arm::SUJClutchEventHandlerFromIO, arm, "Button");
                arm->SUJInterfaceRequiredToSUJ = this->AddInterfaceRequired("SUJ-" + arm->Name());
                arm->SUJInterfaceRequiredToSUJ->AddFunction("Clutch", arm->SUJClutch);
            } else {
                arm->SUJInterfaceRequiredFromIO = 0;
                arm->SUJInterfaceRequiredToSUJ = 0;
            }
        }
    }
    mConfigured = true;
}