/*
 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. 
*/

/* 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/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::RegisterPolicyEngine(int32_t socketFD)
{
    LOG("Cognitive Radio Shell:: Received registration from Policy Engine on socket %d.\n", \
            socketFD);

    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 %d closed.\n", socketFD);
}


void
CognitiveRadioShell::RegisterSML(int32_t socketFD)
{
    LOG("Cognitive Radio Shell:: Received registration from SML on socket %d.\n", \
            socketFD);

    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 %d closed.\n", socketFD);
}

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

    /* TODO always returning one seems useless? */
    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); 
    }

    /* 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 [] 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 true;
}


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, "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 = 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;
}