root/vtcross/trunk/src/shell/CognitiveRadioShell.cpp @ 389

/* Virginia Tech Cognitive Radio Open Source Systems
 * Virginia Tech, 2009
 *
 * LICENSE INFORMATION GOES HERE
 */

/* DESCRIPTION OF FILE.
 */


#include <cstdlib>
#include <cstring>
#include <stdint.h>
#include <string>

#include <arpa/inet.h>
#include <iostream>
#include <netinet/in.h>
#include <netdb.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <sys/wait.h>

#include "tinyxml/tinyxml.h"
#include "tinyxml/tinystr.h"

#include "vtcross/common.h"
#include "vtcross/components.h"
#include "vtcross/containers.h"
#include "vtcross/debug.h"
#include "vtcross/error.h"
#include "vtcross/socketcomm.h"


CognitiveRadioShell::CognitiveRadioShell()
{
    LOG("Creating Cognitive Radio Shell.\n");
    SML_present = false;
    PE_present = false;
    CE_present = false;

    // TODO =BUG= The params, observables, and utils arrays are not being
    // allocated here. If an shell object is constructed, and then immediately
    // destructed, there will be no allocated memory, and the delete operater
    // will receive a null pointer, which will segfault.
}


CognitiveRadioShell::~CognitiveRadioShell()
{
    delete [] params;
    delete [] observables;
    delete [] utils;
}


CognitiveRadioShell::CognitiveRadioShell(const char* radioConfig, int16_t p1, \
        int16_t p2, int16_t p3)
{
    LOG("Creating Cognitive Radio Shell.\n");

    params = new Parameter[10];
    observables = new Observable[10];
    utils = new Utility[10];
    radio_info = new Radio_Info;

    LoadRadioConfiguration(radioConfig, params, utils, observables, radio_info);

    primaryPort = p1;
    policyPort = p2;
    commandPort = p3;
}


void
CognitiveRadioShell::SendComponentType(int32_t socketFD)
{
    SendMessage(socketFD, "response_shell");
    LOG("Cognitive Radio Shell responded to GetRemoteComponentType query.\n");
}


std::string 
CognitiveRadioShell::GetRemoteComponentType(int32_t socketFD)
{
    SendMessage(socketFD, "request_component_type");

    char buffer[256];
    memset(buffer, 0, 256);
    ReadMessage(socketFD, buffer);

    return std::string(buffer);
}


void 
CognitiveRadioShell::Shutdown()
{
    // TODO should something else be happening here?
}


void
CognitiveRadioShell::Reset()
{
    LOG("Resetting Cognitive Radio Shell.\n");
}


// TODO what is the point of always returning a 1?
bool
CognitiveRadioShell::SendRadioConfiguration(int32_t socketFD)
{
    LOG("Cognitive Radio Shell:: Sending radio configuration to Cognitive Engine.\n");

    char counter[55];
    char var[50];

    /* Send utilities */
    sprintf(counter, "%d", radio_info->numUtilities);
    SendMessage(socketFD, counter);
    for(size_t i = 0; i < radio_info->numUtilities; i++) {
        SendMessage(socketFD, utils[i].name.c_str());
        SendMessage(socketFD, utils[i].units.c_str());
        SendMessage(socketFD, utils[i].goal.c_str());
        sprintf(var,"%f", utils[i].target);
        SendMessage(socketFD, var);
    }

    /* Send parameters */
    sprintf(counter,"%i",radio_info->numParameters);
    SendMessage(socketFD,counter);
    for(size_t i = 0; i < radio_info->numParameters; i++) {
        SendMessage(socketFD, params[i].name.c_str());
        SendMessage(socketFD, params[i].units.c_str());
        sprintf(var, "%f", params[i].min);
        SendMessage(socketFD,var);
        sprintf(var, "%f", params[i].max);
        SendMessage(socketFD, var);
        sprintf(var, "%f", params[i].step);
        SendMessage(socketFD, var);

        sprintf(counter, "%i", params[i].numAffects); 
        SendMessage(socketFD, counter);
        for(size_t j = 0; j < params[i].numAffects; j++) {
            SendMessage(socketFD, params[i].affection_list[j].u->name.c_str());
            SendMessage(socketFD, params[i].affection_list[j].relation.c_str());
        }
    }

    /* Send observables */
    sprintf(counter,"%i",radio_info->numObservables);
    SendMessage(socketFD, counter);
    for(size_t i = 0; i < radio_info->numObservables; i++) {
        SendMessage(socketFD, observables[i].name.c_str());
        
        sprintf(counter, "%i", observables[i].numAffects); 
        SendMessage(socketFD, counter);
        for(size_t j = 0; j < observables[i].numAffects; j++) {
            SendMessage(socketFD, observables[i].affection_list[j].u->name.c_str());
            SendMessage(socketFD, observables[i].affection_list[j].relation.c_str());
        }
    }
    
    /* Receive ACK for radio configuration */
    char buffer[256];
    memset(buffer, 0, 256); 
    ReadMessage(socketFD, buffer);

    if(strcmp(buffer, "receive_config_ack") != 0) {
        LOG("Cognitive Radio Shell:: Unexpected response: %s\n", buffer);
        return 0;
    }

    return 1;
}

bool
CognitiveRadioShell::SendRadioExperience(int32_t socketFD)
{

    LOG("Cognitive Radio Shell:: Sending radio experience to Cognitive Engine.\n");
    int32_t numberExp = 4;
    char numberExpString[50];

    sprintf(numberExpString, "%d", numberExp);
    SendMessage(socketFD, "test");  //TODO modified

    char buffer[256];
    memset(buffer, 0, 256); 
    ReadMessage(socketFD, buffer);
    if(strcmp(buffer, "receive_exp_ack") != 0) {
        // TODO perhaps this should be a WARNING instead of a LOG?
        LOG("Cognitive Radio Shell:: Unexpected response: %s\n", buffer);
        return 0;
    }
    //printf("done sending exp \n");
    return 1;
}


void
CognitiveRadioShell::RegisterCognitiveEngine(int32_t socketFD)
{
    LOG("Cognitive Radio Shell:: Received registration message from Cognitive Engine.\n");
   
    SendMessage(socketFD, "register_ack");
    SendRadioConfiguration(socketFD);
    SendRadioExperience(socketFD);

    numberOfCognitiveEngines++;
    CE_present = true;
}


void 
CognitiveRadioShell::DeregisterCognitiveEngine(int32_t socketFD)
{
    LOG("Cognitive Radio Shell:: Received deregistration message from Cognitive Engine.\n");

    numberOfCognitiveEngines--;
    if(numberOfCognitiveEngines == 0)
        CE_present = false;

    SendMessage(socketFD, "deregister_ack");
    shutdown(socketFD, 2);
    close(socketFD);
    LOG("Cognitive Radio Shell:: Socket closed.\n");
}


void
CognitiveRadioShell::RegisterPolicyEngine(int32_t socketFD)
{
    LOG("Cognitive Radio Shell:: Received registration message from Policy Engine.\n");
    PE_present = true;
}


void 
CognitiveRadioShell::DeregisterPolicyEngine(int32_t socketFD)
{
    LOG("Cognitive Radio Shell:: Received deregistration message from Policy Engine.\n");

    PE_present = false;
    
    SendMessage(socketFD, "deregister_ack");
    shutdown(socketFD, 2);
    close(socketFD);
    LOG("Cognitive Radio Shell:: Socket closed.\n");
}


void
CognitiveRadioShell::RegisterSML(int32_t socketFD)
{
    LOG("Cognitive Radio Shell:: Received registration message from SML.\n");

    SML_present = true;
}


void 
CognitiveRadioShell::DeregisterSML(int32_t socketFD)
{
    LOG("Cognitive Radio Shell:: Received deregistration message from SML.\n");

    SML_present = false;

    SendMessage(socketFD, "deregister_ack");
    shutdown(socketFD, 2);
    close(socketFD);
    LOG("Cognitive Radio Shell:: Socket closed.\n");
}

void 
CognitiveRadioShell::SetActiveMission(int32_t socketFD)
{
    char buffer[256];

    LOG("Cognitive Radio Shell:: Received Set Active Mission command from host.\n");
   
    // Read the name of the active mission to be set from the host. 
    memset(buffer, 0, 256);
    ReadMessage(commandSocketFD, buffer);

    // Send command to SML
    SendMessage(ceSocketFD, "set_active_mission");
    SendMessage(ceSocketFD, buffer);

    // Get ack from SML saying the mission was set properly
    memset(buffer, 0, 256);
    ReadMessage(ceSocketFD, buffer);

    // Forward ack to host
    SendMessage(commandSocketFD, buffer);
}

int32_t 
CognitiveRadioShell::LoadRadioConfiguration(const char* radioConfig, \
        Parameter* &pList, Utility* &uList, Observable* &oList, \
        Radio_Info* radioInfo)
{
    TiXmlElement *pElem;
    TiXmlElement *pChild;
    TiXmlElement *pChild1;
    TiXmlElement *pSecondChild;
    TiXmlHandle hRoot(0);

    int32_t count = 0;
    size_t item_count = 0;
    size_t affect_count = 0;
    uint32_t attribute_count = 0;
    bool match_found = false;

    LOG("Cognitive Radio Shell:: Loading radio configuration.\n");

    TiXmlDocument doc( radioConfig );
    bool loadOkay = doc.LoadFile();
    if(!loadOkay)
        ERROR(1,"Loading radio configuration failed: %s\n", radioConfig);

    TiXmlHandle hDoc(&doc);
    
    pElem = hDoc.FirstChildElement().Element();

    if(!pElem) 
        ERROR(1, "No valid root!");

    hRoot = TiXmlHandle(pElem);

    pElem = hRoot.FirstChild("utilities").Element();
    pChild1 = hRoot.Child("utilities", count).Element();

    for(pChild = pChild1->FirstChildElement("utility"); pChild; \
        pChild = pChild->NextSiblingElement()) {

        const char *uName = pChild->Attribute("name");
        if(uName)
            uList[item_count].name = uName;    

        const char *uUnits = pChild->Attribute("units");
        if(uUnits) 
            uList[item_count].units = uUnits;

        const char *uGoal = pChild->Attribute("goal");
        if(uGoal) 
            uList[item_count].goal = uGoal;

        if(pChild->QueryFloatAttribute("target", &uList[item_count].target) != TIXML_SUCCESS) 
            uList[item_count].target = -1;

        item_count++;
    }

    radio_info->numUtilities = item_count;    
    LOG("Cognitive Radio Shell:: Parsed %d utilities.\n", radioInfo->numUtilities);

    item_count = 0;
    pElem = hRoot.FirstChild("observables").Element();
    pChild1 = hRoot.Child("observables", count).Element();

    for(pChild = pChild1->FirstChildElement("observable"); pChild; \
        pChild = pChild->NextSiblingElement()) {

        const char *oName = pChild->Attribute("name");
        if(oName) 
            oList[item_count].name = oName;
        
        affect_count = 0;
        for(pSecondChild = pChild->FirstChildElement("affect"); pSecondChild; \
            pSecondChild = pSecondChild->NextSiblingElement()) {

            const char *oUtilName = pSecondChild->Attribute("utility");
            if(oUtilName) {
                for(attribute_count = 0; attribute_count < radio_info->numUtilities; attribute_count++ ) {
                    if(uList[attribute_count].name == oUtilName) {

                        oList[item_count].affection_list[affect_count].u = &uList[attribute_count];
                        const char *oRelate = pSecondChild->Attribute("relationship");
                        if(oRelate) 
                            oList[item_count].affection_list[affect_count].relation = oRelate;

                        affect_count++;
                        match_found = true;
                        break;
                    }
                }
            }

            if(!match_found) { 
                ERROR(1, "Error: %s: %s is not a valid utility.\n", \
                    oList[item_count].name.c_str(), oUtilName);
            }
            else
                match_found = false;    
        }
        oList[item_count].numAffects = affect_count; 
        item_count++;
    }

    radioInfo->numObservables = item_count;    
    LOG("Cognitive Radio Shell:: Parsed %d observables.\n", radioInfo->numObservables);

    pElem = hRoot.FirstChild("parameters").Element();
    pChild1 = hRoot.Child("parameters", count).Element();
    
    item_count = 0;
    for(pChild = pChild1->FirstChildElement("parameter"); pChild; \
        pChild = pChild->NextSiblingElement()) {

        const char *pName = pChild->Attribute("name");
        if(pName) 
            pList[item_count].name = pName;    

        const char *pUnits = pChild->Attribute("units");
        if(pUnits) 
            pList[item_count].units = pUnits;

        if(pChild->QueryFloatAttribute("min", &pList[item_count].min) != TIXML_SUCCESS) 
            pList[item_count].min = -1;

        if(pChild->QueryFloatAttribute("max", &pList[item_count].max) != TIXML_SUCCESS) 
            pList[item_count].max = -1;

        if(pChild->QueryFloatAttribute("step", &pList[item_count].step) != TIXML_SUCCESS) 
            pList[item_count].step = -1;
        
        affect_count = 0;
        for(pSecondChild = pChild->FirstChildElement("affect"); pSecondChild; \

            pSecondChild = pSecondChild->NextSiblingElement()) {
            const char *pUtilName = pSecondChild->Attribute("utility");
            if(pUtilName) {
                for(attribute_count = 0; attribute_count < radio_info->numUtilities; attribute_count++) {
                    if(uList[attribute_count].name == pUtilName) {
                        pList[item_count].affection_list[affect_count].u = &uList[attribute_count];    

                        const char *pRelate = pSecondChild->Attribute("relationship");
                        if(pRelate)
                            pList[item_count].affection_list[affect_count].relation = pRelate;
                        else
                            LOG("Error: No relation found.\n");

                        match_found = true;
                        affect_count++;
                        break;
                    }
                }
            }

            if(!match_found) {
                ERROR(1, "Error: %s: %s is not a valid utility.\n", \
                    pList[item_count].name.c_str(), pUtilName);
            }

            match_found = false;    
        }

        pList[item_count].numAffects = affect_count;
        item_count++;
    }

    radioInfo->numParameters = item_count;
    LOG("Cognitive Radio Shell:: Parsed %d parameters.\n", radioInfo->numParameters);

    return 1;
}


void
CognitiveRadioShell::GetOptimalParameters(int32_t socketFD)
{
    char buffer[256];
    char counter[55];
    char var[50];

    /* Receive Set of Observables */
    LOG("Cognitive Radio Shell:: Got request for optimization.\n");
    memset(buffer, 0, 256);
    ReadMessage(commandSocketFD, buffer);
    uint32_t numObservables = atoi(buffer);
  
    LOG("Cognitive Radio Shell:: Attempting to get %i observables.\n", numObservables);
    Observable *o = new Observable[numObservables];
 
    for(size_t i = 0; i < numObservables; i++) {
        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD, buffer);
        o[i].name = std::string(buffer);
   
        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD, buffer);
        o[i].value = atof(buffer);
    }

    /* Receive Set of Current Parameters */
    memset(buffer, 0, 256);
    ReadMessage(commandSocketFD,buffer);
    uint32_t numCurrentParameters = atoi(buffer);
  
    LOG("Cognitive Radio Shell:: Attempting to get %i parameters.\n",numCurrentParameters);
    Parameter * cp = new Parameter[numCurrentParameters];

    for (size_t i = 0; i < numCurrentParameters; i++){
        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD,buffer);
        cp[i].name = std::string(buffer);
        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD,buffer);
        cp[i].value = atof(buffer);
    }

    /* Send to Cognitive Engine 
     * TODO: With multiple CEs we need to make a decision about where
     * to send this information
     */
    LOG("Cognitive Radio Shell:: Passing on observables.\n");
    SendMessage(ceSocketFD,"request_optimization");
    sprintf(counter,"%i",numObservables);
    SendMessage(ceSocketFD,counter);
    for(size_t i = 0; i < numObservables; i++) {
        SendMessage(ceSocketFD,o[i].name.c_str());
        sprintf(var,"%f",o[i].value);
        SendMessage(ceSocketFD,var);
    }
        
    LOG("Cognitive Radio Shell:: Passing on current parameters.\n");
    sprintf(counter,"%i",numCurrentParameters);
    SendMessage(ceSocketFD,counter);
    for(size_t i = 0; i < numCurrentParameters; i++) {
        SendMessage(ceSocketFD,cp[i].name.c_str());
        sprintf(var,"%f",cp[i].value);
        SendMessage(ceSocketFD,var); 
    }

    LOG("Cognitive Radio Shell:: Receiving optimized parameters.\n");
    /* Receive Set of Parameters */
    memset(buffer, 0, 256);
    ReadMessage(ceSocketFD, buffer);
    uint32_t numParameters = atoi(buffer);
   
    Parameter *p = new Parameter[numParameters];
 
    for(size_t i = 0; i < numParameters; i++) {
        memset(buffer, 0, 256);
        ReadMessage(ceSocketFD, buffer);
        p[i].name = std::string(buffer);
    
        memset(buffer, 0, 256);
        ReadMessage(ceSocketFD, buffer);
        p[i].value = atof(buffer);
    }

    /* Send to Application 
     */
    LOG("Cognitive Radio Shell:: Sending optimized parameters to Application.\n");
    memset(counter, 0, 55);
    sprintf(counter, "%i", numParameters);
    SendMessage(commandSocketFD, counter);
    for(size_t i = 0; i < numParameters; i++) {
        SendMessage(commandSocketFD, p[i].name.c_str());
        sprintf(var, "%f", p[i].value);
        SendMessage(commandSocketFD, var);
    }

    delete [] o;
    delete [] p;
}

// TODO point of always returning 1?
bool
CognitiveRadioShell::UpdateParameterPerformance(int32_t socketFD)
{
    char counter[55];
    char var[50];
    char buffer[256];

    /* Receive Set of Parameters */
    memset(buffer, 0, 256);
    ReadMessage(commandSocketFD,buffer);
    uint32_t numParameters = atoi(buffer);
  
    Parameter *p = new Parameter[numParameters];

    for (size_t i = 0; i < numParameters; i++){
        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD,buffer);
        p[i].name = std::string(buffer);
        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD,buffer);
        p[i].value = atof(buffer);
    }

    /* Receive Set of Observables */
    memset(buffer, 0, 256);
    ReadMessage(commandSocketFD, buffer);
    uint32_t numObservables = atoi(buffer);
  
    Observable *o = new Observable[numObservables];
 
    for(size_t i = 0; i < numObservables; i++) {
        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD, buffer);
        o[i].name = std::string(buffer);
   
        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD, buffer);
        o[i].value = atof(buffer);
    }

    SendMessage(ceSocketFD, "update_performance");
   
    /* Send Parameters */
    memset(counter, 0, 55);
    sprintf(counter, "%i", numParameters);
    SendMessage(ceSocketFD, counter);
    
    for(size_t i = 0; i < numParameters; i++) {
        SendMessage(ceSocketFD,p[i].name.c_str());
        sprintf(var,"%f",p[i].value);
        SendMessage(ceSocketFD,var);  
    }   
    
    /* Send Observables */
    sprintf(counter, "%i", numObservables);
    SendMessage(ceSocketFD, counter);
    for(size_t i = 0; i < numObservables; i++) {
        SendMessage(ceSocketFD, o[i].name.c_str());
        sprintf(var, "%f", o[i].value);
        SendMessage(ceSocketFD, var); 
    }   

    delete [] p;
    delete [] o;
    
    return 1;
}


int
CognitiveRadioShell::HandleMessage(int32_t socketFD)
{
    char buffer[256];
    int ret = 0;
    
    ret = ReadMessage(socketFD, buffer);
    if(ret == -1) {
        return ret;
    }

    // TODO trying to read this code lock makes my eyes bleed
    if(strcmp(buffer, "register_engine_cognitive") == 0) { 
        RegisterCognitiveEngine(socketFD);
    } else if(strcmp(buffer, "deregister_engine_cognitive") == 0) {
        DeregisterCognitiveEngine(socketFD);
    } else if(strcmp(buffer, "register_engine_policy") == 0) {
        RegisterPolicyEngine(socketFD);
    } else if(strcmp(buffer, "deregister_engine_policy") == 0) {
        DeregisterPolicyEngine(socketFD);
    } else if(strcmp(buffer, "register_sml") == 0) {
        RegisterSML(socketFD);
    } else if(strcmp(buffer, "deregister_sml") == 0) {
        DeregisterSML(socketFD);
    } else if(strcmp(buffer, "update_performance") == 0) {
        UpdateParameterPerformance(socketFD);
    } else if(strcmp(buffer, "get_number_utilities") == 0) {
        char numUtilities[20];
        sprintf(numUtilities, "%i", radio_info->numUtilities);
        SendMessage(commandSocketFD, numUtilities);
    } else if(strcmp(buffer, "get_number_observables") == 0) {
        char numObservables[20];
        sprintf(numObservables, "%i", radio_info->numObservables);
        SendMessage(commandSocketFD, numObservables);
    } else if(strcmp(buffer, "get_number_parameters") == 0) {
        char numParameters[20];
        sprintf(numParameters, "%i", radio_info->numParameters);
        SendMessage(commandSocketFD, numParameters);
    } else if(strcmp(buffer, "request_optimization") == 0) {
        /* Receive optimization request and current environment */
        GetOptimalParameters(socketFD);  
    } else if(strcmp(buffer, "set_active_mission") == 0) {
        SetActiveMission(socketFD);  
    } else if(strcmp(buffer, "request_optimization_service") == 0) {
        /* Receive optimization request and current environment */
        //GetOptimalParametersService(socketFD);  
    }

    return ret;
}


void
CognitiveRadioShell::StartShellServer()
{
    struct timeval selTimeout;
    int32_t primary = 0;
    int32_t policy = 1;
    int32_t command = 2;
    int32_t running = 1;
    int32_t port, rc, new_sd = 1;
    int32_t desc_ready = 1;
    int32_t timeout = 10;
    int32_t ret = 0;;
    fd_set sockSet;
   


    int32_t *servSock = new int32_t[3];

    servSock[primary] = CreateTCPServerSocket(primaryPort);
    servSock[policy] = CreateTCPServerSocket(policyPort);
    servSock[command] = CreateTCPServerSocket(commandPort);

    int32_t maxDescriptor; 

    if(servSock[primary] > servSock[policy])
        maxDescriptor = servSock[primary];
    else 
        maxDescriptor = servSock[policy];

    if(servSock[command] > maxDescriptor)
        maxDescriptor = servSock[command];

    if(InitializeTCPServerPort(servSock[primary]) == -1)
        ERROR(1,"Error initializing primary port\n");
 
    if(InitializeTCPServerPort(servSock[policy]) == -1)
        ERROR(1,"Error initializing policy port\n");

    if(InitializeTCPServerPort(servSock[command]) == -1)
        ERROR(1,"Error initializing command port\n");

    FD_ZERO(&sockSet);
    
    while (running) {
        /* Zero socket descriptor vector and set for server sockets */
        /* This must be reset every time select() is called */
        FD_SET(servSock[primary], &sockSet);
        FD_SET(servSock[policy], &sockSet);
        FD_SET(servSock[command], &sockSet);

        /* Timeout specification */
        /* This must be reset every time select() is called */
        selTimeout.tv_sec = timeout;       /* timeout (secs.) */
        selTimeout.tv_usec = 0;            /* 0 microseconds */

        /* Suspend program until descriptor is ready or timeout */
        rc = select(maxDescriptor + 1, &sockSet, NULL, NULL, &selTimeout); 
        if(rc == 0)
            LOG("No echo requests for %i secs...Server still alive\n", timeout);
        else {
            desc_ready = rc;

            for(port = 0; port <= maxDescriptor && desc_ready > 0; port++) {
                if(FD_ISSET(port, &sockSet)) {
                    desc_ready -= 1;
                    /* Check if request is new or on an existing open descriptor */
                    if((port == servSock[primary]) || (port == servSock[policy]) || (port == servSock[command])) { 
                        do {
                            new_sd = AcceptTCPConnection(port);
                            if(new_sd < 0) {
                                break;
                            }
                           
                            if(port == servSock[primary])
                                ceSocketFD = new_sd;
                            if(port == servSock[command])
                                commandSocketFD = new_sd;
                            if(port == servSock[policy])
                                policySocketFD = new_sd;

                            ret = HandleMessage(new_sd);

                            if(ret == -1) {
                                FD_CLR(new_sd,&sockSet);
                                close(new_sd);
                            }

                            FD_SET(new_sd,&sockSet);
                            if(new_sd > maxDescriptor) 
                                maxDescriptor = new_sd;
                        } while(new_sd != -1);
                    } 
                    else {
                        ret = HandleMessage(port);
                        if(ret == -1) {
                            FD_CLR(port,&sockSet);
                            close(port);
                        }
                    }
                }
            }
        }
    }

    LOG("Closing it all.\n\n");
    /* Close sockets */
    close(servSock[primary]);
    close(servSock[policy]);
    close(servSock[command]);

    /* Free list of sockets */
    delete servSock;

    return;
}