root/vtcross/branches/nikhil/crossmodel1/src/shell/CognitiveRadioShell.cpp @ 554

/*
 Copyright 2009 Virginia Polytechnic Institute and State University  

 Licensed under the Apache License, Version 2.0 (the "License"); 
 you may not use this file except in compliance with the License. 
 You may obtain a copy of the License at 
 
 http://www.apache.org/licenses/LICENSE-2.0 

 Unless required by applicable law or agreed to in writing, software 
 distributed under the License is distributed on an "AS IS" BASIS, 
 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 
 See the License for the specific language governing permissions and 
 limitations under the License. 
*/

/*! This file provides the default implementation of the Cognitive Radio Shell.
 *
 * The CRS acts as the central control component in a CROSS radio system.  It
 * handles component registration and message passing.
 */


#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/containers.h"
#include "vtcross/cross_shell.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;

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


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


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

    SML_present = false;
    PE_present = false;
    CE_present = false;

    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()
{
    LOG("Shutting down Cognitive Radio Shell.\n");
}


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


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 false;
    }

    return true;
}

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");

    char buffer[256];
    memset(buffer, 0, 256); 
    ReadMessage(socketFD, buffer);
    if(strcmp(buffer, "receive_exp_ack") != 0) {
        WARNING("Cognitive Radio Shell:: Unexpected response: %s\n", buffer);
        return false;
    }

    return true;
}


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

    numberOfCognitiveEngines++;
    
    /* More efficient to always set to true than add a conditional branch to the
     * code. */
    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 %d closed.\n", socketFD);
}


void 
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;
                    }
                }
            }

  
//////////////////WE are not looking for relationship between them..            
//            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);
}


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);
    }
////////////// NEW    INCLUSION ///////////////////

/* Receive Set of Utilities */
    memset(buffer, 0, 256);
    ReadMessage(commandSocketFD,buffer);
    uint32_t numUtilities = atoi(buffer);

    LOG("Cognitive Radio Shell:: Attempting to get %i utilities.\n",numUtilities);
    Utility *u = new Utility[numUtilities];

    for (size_t i = 0;i < numUtilities; i++){
        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD,buffer);
        u[i].name = std::string(buffer);

        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD,buffer);
        u[i].value = atof(buffer);
    }

///////////////////////////////////////

    /* Send to Cognitive Engine 
     * TODO: With multiple CEs we need to make a decision about where
     * to send this information
     */
    SendMessage(ceSocketFD,"request_optimization");

    LOG("Cognitive Radio Shell:: Passing on observables.\n");
    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); 
    }

/////////////  NEW  ////////////////////////////////////////////////////////
    LOG("Cognitive Radio Shell:: Passing on utilities.\n");
    sprintf(counter,"%i",numUtilities);
    SendMessage(ceSocketFD,counter);
    for(size_t i = 0; i < numUtilities; i++) {
        SendMessage(ceSocketFD,u[i].name.c_str());
        sprintf(var,"%f",u[i].value);
        SendMessage(ceSocketFD,var);
    }

////////////// END of NEW /////////////////////////////////////

    /* Receive Set of Parameters */
    LOG("Cognitive Radio Shell:: Receiving optimized parameters.\n");
    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 [] u;
    delete [] p;
}


void
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);
    }

    /* Receive Set of Utilities */
    memset(buffer, 0, 256);
    ReadMessage(commandSocketFD, buffer);
    uint32_t numUtilities = atoi(buffer);

    Utility *u = new Utility[numUtilities];

    for(size_t i = 0; i < numUtilities; i++) {
        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD, buffer);
        u[i].name = std::string(buffer);

        memset(buffer, 0, 256);
        ReadMessage(commandSocketFD, buffer);
        u[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 */
    memset(counter, 0, 55);
    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); 
    }   

     /* Send Utilities */
     memset(counter, 0, 55);
     sprintf(counter, "%i", numUtilities);
     SendMessage(ceSocketFD,counter);
     for(size_t i = 0; i < numUtilities; i++) {
        SendMessage(ceSocketFD, u[i].name.c_str());
        sprintf(var, "%f", u[i].value);
        SendMessage(ceSocketFD, var);
     }


    delete [] p;
    delete [] o;
    delete [] u;
}


int32_t
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 block 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, "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);  
    } 

    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 = 50;
    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);
                                break;
                            }

                            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;
}