void nwxString::Join(const set<wxString> &ss, wxString *ps, const char *psSep)
{
wxString sSep(wxEmptyString);
size_t nReserve = 256;
size_t nLen = 0;
size_t nSepLen = 0;
bool bFirst = true;
ps->Empty();
ps->Alloc(nReserve);
for(set<wxString>::const_iterator itr = ss.begin();
itr != ss.end();
++itr)
{
nLen += (*itr).Len();
nLen += nSepLen;
if(nLen >= nReserve)
{
while(nLen >= nReserve)
{
nReserve <<= 1;
}
ps->Alloc(nReserve);
}
(*ps) += sSep;
(*ps) += (*itr);
if(bFirst)
{
bFirst = false;
if(psSep != NULL)
{
sSep = psSep;
nSepLen = sSep.Len();
}
}
}
}
String RobotCmd::processMessage(String incomingMessage, long currTime)
{
char* msg = const_cast<char*>( incomingMessage.c_str());
if(detectNewDanger(msg,currTime)){
return String(DANGER);
}
if(accel.collision){
motor.setState(STATE_COLLISION);
accel.collision = false;
return "";
}
if( processDanger(currTime))
{
return "";
}
if(motor.motorState == STATE_COLLISION)
{
//when encountering a collision get us out of a jam before resuming all activities
return "";
}
if(currState != RBT_DANCE && msg[0]==MARCO) //received marco
{
//Serial.println("process MARCO");
currState = RBT_WAIT_DANCE;
return String(POLO); //send back polo;
}
if(currState == RBT_WAIT_POLO && msg[0]==POLO) //recived polo
{
//Serial.println("process POLO");
currState = RBT_DANCE;
String newSong = singer.createSong(safeFeeling,safeFeeling+10);
String newDance = "";//dancer.createDance(); //disbaled dance cause we can't hear RF signals with it
return String(SONG+newSong+DANCE+newDance);
}
if(currState == RBT_WAIT_DANCE && msg[0]==SONG)
{
String sSep(SONG);
String dSep(DANCE);
//split message into song and dance
char * song = new char[255* sizeof(char)];
char * dance = new char[255* sizeof(char)];
dance = strwrd ( msg,song,255* sizeof(char),dSep.c_str());
//strip first char off of theDance and song and everything is correct; or handle it within the routine
singer.playSong(const_cast<char*>(song+1));
//dancer.performDance(const_cast<char*>(dance+1));
currState = RBT_DANCE;
return "";
}
//we just finished dancing
if(currState == RBT_DANCE)
{
if( !dancer.dancing && !singer.singing)
{
currState = RBT_IDLE;
freeTime = rand() *1000;//set the time before we connect to other robot
freeTime = freeTime<0?-1*freeTime:freeTime;
}
return "";
}
if(currState == RBT_IDLE)
{
if((currTime - freeTime )> commandTimeout)
{ //see if robot is responding
currState = RBT_WAIT_POLO;
lastCommandSent = currTime;
//determines the song we sing
safeFeeling = safeFeeling< (singer.maxSafeFeeling-10)?safeFeeling+10:safeFeeling;
return String(MARCO);
}
}
if((currTime - lastCommandSent) >commandTimeout)
{
currState = RBT_IDLE;
lastCommandSent = currTime;
freeTime = abs(rand() *500+currTime);
freeTime = freeTime<0?-1*freeTime:freeTime;
}
return "";
}
