s32 tcp_socket (void) {
s32 s, res;
s = net_socket (PF_INET, SOCK_STREAM, 0);
if (s < 0) {
printDebugMsg(NORMAL_DEBUG_MESSAGE,"net_socket failed: %d\n", s);
return s;
}
res = net_fcntl (s, F_GETFL, 0);
if (res < 0) {
printDebugMsg(NORMAL_DEBUG_MESSAGE,"F_GETFL failed: %d\n", res);
net_close (s);
return res;
}
res = net_fcntl (s, F_SETFL, res | 4);
if (res < 0) {
printDebugMsg(NORMAL_DEBUG_MESSAGE,"F_SETFL failed: %d\n", res);
net_close (s);
return res;
}
return s;
}
s32 tcp_connect (char *host, const u16 port) {
struct hostent *hp;
struct sockaddr_in sa;
s32 s, res;
s64 t;
hp = net_gethostbyname (host);
if (!hp || !(hp->h_addrtype == PF_INET)) {
printDebugMsg(NORMAL_DEBUG_MESSAGE,"net_gethostbyname failed: %d\n", errno);
return errno;
}
s = tcp_socket ();
if (s < 0)
return s;
memset (&sa, 0, sizeof (struct sockaddr_in));
sa.sin_family= PF_INET;
sa.sin_len = sizeof (struct sockaddr_in);
sa.sin_port= htons (port);
memcpy ((char *) &sa.sin_addr, hp->h_addr_list[0], hp->h_length);
t = gettime ();
while (true) {
if (ticks_to_millisecs (diff_ticks (t, gettime ())) >
TCP_CONNECT_TIMEOUT) {
printDebugMsg(NORMAL_DEBUG_MESSAGE,"tcp_connect timeout\n");
net_close (s);
return -ETIMEDOUT;
}
res = net_connect (s, (struct sockaddr *) &sa,
sizeof (struct sockaddr_in));
if (res < 0) {
if (res == -EISCONN)
break;
if (res == -EINPROGRESS || res == -EALREADY) {
usleep (20 * 1000);
continue;
}
printDebugMsg(NORMAL_DEBUG_MESSAGE,"net_connect failed: %d\n", res);
net_close (s);
return res;
}
break;
}
return s;
}
bool tcp_read (const s32 s, u8 **buffer, const u32 length) {
u8 *p;
u32 step, left, block, received;
s64 t;
s32 res;
step = 0;
p = *buffer;
left = length;
received = 0;
t = gettime ();
while (left) {
if (ticks_to_millisecs (diff_ticks (t, gettime ())) >
TCP_BLOCK_RECV_TIMEOUT) {
printDebugMsg(NORMAL_DEBUG_MESSAGE,"tcp_read timeout\n");
break;
}
block = left;
if (block > 2048)
block = 2048;
res = net_read (s, p, block);
if ((res == 0) || (res == -EAGAIN)) {
usleep (20 * 1000);
continue;
}
if (res < 0) {
printDebugMsg(NORMAL_DEBUG_MESSAGE,"net_read failed: %d\n", res);
break;
}
received += res;
left -= res;
p += res;
if ((received / TCP_BLOCK_SIZE) > step) {
t = gettime ();
step++;
}
}
return left == 0;
}
bool tcp_write (const s32 s, const u8 *buffer, const u32 length) {
const u8 *p;
u32 step, left, block, sent;
s64 t;
s32 res;
step = 0;
p = buffer;
left = length;
sent = 0;
t = gettime ();
while (left) {
if (ticks_to_millisecs (diff_ticks (t, gettime ())) >
TCP_BLOCK_SEND_TIMEOUT) {
printDebugMsg(NORMAL_DEBUG_MESSAGE,"tcp_write timeout\n");
break;
}
block = left;
if (block > 2048)
block = 2048;
res = net_write (s, p, block);
if ((res == 0) || (res == -56)) {
usleep (20 * 1000);
continue;
}
if (res < 0) {
printDebugMsg(NORMAL_DEBUG_MESSAGE,"net_write failed: %d\n", res);
break;
}
sent += res;
left -= res;
p += res;
if ((sent / TCP_BLOCK_SIZE) > step) {
t = gettime ();
step++;
}
}
return left == 0;
}
char * tcp_readln (const s32 s, const u16 max_length, const u64 start_time, const u32 timeout) {
char *buf;
u16 c;
s32 res;
char *ret;
buf = (char *) malloc (max_length);
c = 0;
ret = NULL;
while (true) {
if (ticks_to_millisecs (diff_ticks (start_time, gettime ())) > timeout)
break;
res = net_read (s, &buf[c], 1);
if ((res == 0) || (res == -EAGAIN)) {
usleep (20 * 1000);
continue;
}
if (res < 0) {
printDebugMsg(NORMAL_DEBUG_MESSAGE,"tcp_readln failed: %d\n", res);
break;
}
if ((c > 0) && (buf[c - 1] == '\r') && (buf[c] == '\n')) {
if (c == 1) {
ret = strdup ("");
break;
}
ret = strndup (buf, c - 1);
break;
}
c++;
if (c == max_length)
break;
}
free (buf);
return ret;
}
void esp::Worklist::solveWorklist(){
while(works.size() != 0){
Work *currentWork = selectAWork();
removeFromWorklist(currentWork);
//do real work here
if(currentWork != NULL)
doEachWork(currentWork);
printDebugMsg("Instructions in worklist:");
for(set<Work*>::iterator it = works.begin(); it!=works.end(); it++){
Work *workInList = (*it);
workInList->printContent();
}
}
}
bool
MSLane::isEmissionSuccess(MSVehicle* aVehicle,
SUMOReal speed, SUMOReal pos,
bool patchSpeed, size_t startStripId) throw(ProcessError) {
// and the speed is not too high (vehicle should decelerate)
// try to get a leader on consecutive lanes
// we have to do this even if we have found a leader on our lane because it may
// be driving into another direction
//std::cerr<<"EMISSION speed:"<<speed<<std::endl;
std::cerr<<"EMISSION vehicle:"<<aVehicle->getID()<<std::endl;
size_t endStripId = startStripId + aVehicle->getWidth() - 1;
assert(startStripId >=0 && endStripId < myStrips.size());
aVehicle->getBestLanes(true, this);
const MSCFModel &cfModel = aVehicle->getCarFollowModel();
const std::vector<MSLane*> &bestLaneConts = aVehicle->getBestLanesContinuation(this);
std::vector<MSLane*>::const_iterator ri = bestLaneConts.begin();
SUMOReal seen = getLength() - pos;
SUMOReal dist = cfModel.brakeGap(speed);
const MSRoute &r = aVehicle->getRoute();
MSRouteIterator ce = r.begin();
MSLane *currentLane = this;
MSLane *nextLane = this;
while (seen<dist&&ri!=bestLaneConts.end()&&nextLane!=0/*&&ce!=r.end()*/) {
// get the next link used...
MSLinkCont::const_iterator link = currentLane->succLinkSec(*aVehicle, 1, *currentLane, bestLaneConts);
// ...and the next used lane (including internal)
if (!currentLane->isLinkEnd(link) && (*link)->havePriority() && (*link)->getState()!=MSLink::LINKSTATE_TL_RED) { // red may have priority?
#ifdef HAVE_INTERNAL_LANES
bool nextInternal = false;
nextLane = (*link)->getViaLane();
if (nextLane==0) {
nextLane = (*link)->getLane();
} else {
nextInternal = true;
}
#else
nextLane = (*link)->getLane();
#endif
} else {
nextLane = 0;
}
// check how next lane effects the journey
if (nextLane!=0) {
SUMOReal gap = 0;
//TODO: fix get & set partial occupator to strip level
MSVehicle * leader = 0;//currentLane->getPartialOccupator();
if (leader!=0) {
gap = getPartialOccupatorEnd();
} else {
// check leader on next lane
leader = nextLane->getLastVehicle(aVehicle->getStrips());
if (leader!=0) {
gap = seen+leader->getPositionOnLane()-leader->getVehicleType().getLength();
}
}
if (leader!=0) {
SUMOReal nspeed = gap>=0 ? cfModel.ffeV(aVehicle, speed, gap, leader->getSpeed()) : 0;
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
return false;
}
}
}
// check next lane's maximum velocity
SUMOReal nspeed = nextLane->getMaxSpeed();
if (nspeed<speed) {
// patch speed if needed
if (patchSpeed) {
speed = MIN2(cfModel.ffeV(aVehicle, speed, seen, nspeed), speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we would be too fast on the next lane
return false;
}
}
// check traffic on next junctions
const SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / speed);
#ifdef HAVE_INTERNAL_LANES
const SUMOTime leaveTime = (*link)->getViaLane()==0 ? arrivalTime + TIME2STEPS((*link)->getLength() * speed) : arrivalTime + TIME2STEPS((*link)->getViaLane()->getLength() * speed);
#else
const SUMOTime leaveTime = arrivalTime + TIME2STEPS((*link)->getLength() * speed);
#endif
if ((*link)->hasApproachingFoe(arrivalTime, leaveTime)) {
SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
return false;
}
}
} else {
// we can only drive to the end of the current lane...
SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
return false;
}
}
}
seen += nextLane->getLength();
++ce;
++ri;
currentLane = nextLane;
}
}
if (seen<dist) {
SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
MsgHandler::getErrorInstance()->inform("Vehicle '" + aVehicle->getID() + "' will not be able to emit using given velocity!");
// !!! we probably should do something else...
return false;
}
}
}
// get the pointer to the vehicle next in front of the given position
MSVehicle *pred;
std::vector<MSVehicle *> predCont;
std::vector<MSVehicle *>::iterator predIt, it;
for (unsigned int i=startStripId; i<=endStripId; ++i) {
predCont.push_back(myStrips.at(i)->getPredAtPos(pos));
}
predIt = predCont.begin();
SUMOReal currMin = -1;
if (*predIt != 0) {
currMin = (*predIt)->getPositionOnLane();
} else {
// signals no leader in front
predIt = predCont.end();
}
for (it = predCont.begin(); it != predCont.end(); ++it) {
if (*it == 0) continue;
if ((*it)->getPositionOnLane() < currMin) {
predIt = it;
currMin = (*it)->getPositionOnLane();
}
}
if (predIt != predCont.end()) {
// ok, there is one (a leader)
MSVehicle* leader = *predIt;
SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed()));
SUMOReal gap = MSVehicle::gap(leader->getPositionOnLane(), leader->getVehicleType().getLength(), pos);
if (gap<frontGapNeeded) {
// too close to the leader on this lane
return false;
}
}
// FIXME: implement look back
// check back vehicle
if (false/*predIt!=myVehicles.begin()*/) {
// there is direct follower on this lane
MSVehicle *follower = *(predIt-1);
SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed));
SUMOReal gap = MSVehicle::gap(pos, aVehicle->getVehicleType().getLength(), follower->getPositionOnLane());
if (gap<backGapNeeded) {
// too close to the follower on this lane
return false;
}
} else if (false) {
// check approaching vehicle (consecutive follower)
SUMOReal lspeed = getMaxSpeed();
// in order to look back, we'd need the minimum braking ability of vehicles in the net...
// we'll assume it to be 4m/s^2
// !!!revisit
SUMOReal dist = lspeed * lspeed * SUMOReal(1./2.*4.) + SPEED2DIST(lspeed);
std::pair<const MSVehicle * const, SUMOReal> approaching = getFollowerOnConsecutive(dist, 0, speed, pos - aVehicle->getVehicleType().getLength());
if (approaching.first!=0) {
const MSVehicle *const follower = approaching.first;
SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed));
SUMOReal gap = approaching.second - pos - aVehicle->getVehicleType().getLength();
if (gap<backGapNeeded) {
// too close to the consecutive follower
return false;
}
}
// check for in-lapping vehicle
MSVehicle* leader = getPartialOccupator();
if (leader!=0) {
SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed()));
SUMOReal gap = getPartialOccupatorEnd() - pos;
if (gap<=frontGapNeeded) {
// too close to the leader on this lane
return false;
}
}
}
// may got negative while adaptation
if (speed<0) {
return false;
}
// enter
//XXX: later change to enterStripAtEmit()?
//if (speed < 0.0001) speed += 10.0;
StripCont strips;
strips.resize(aVehicle->getWidth());
StripCont::iterator start = myStrips.begin() + startStripId;
std::copy(start, start + aVehicle->getWidth(), strips.begin());
aVehicle->enterLaneAtEmit(this, pos, speed, strips);
bool wasInactive = getVehicleNumber()==0;
if (true/*predIt==myVehicles.end()*/) {
// vehicle will be the first on the lane
//std::cerr<<"startStripId:"<<startStripId<<", NumStrips:"<<strips.size()<<", VehWidth:"<<aVehicle->getWidth()<<std::endl;
for (size_t i=startStripId; i<startStripId+strips.size(); ++i) {
this->getStrip(i)->pushIntoStrip(aVehicle);
this->getStrip(i)->setVehLenSum(this->getStrip(i)->getVehLenSum() +
aVehicle->getVehicleType().getLength());
}
aVehicle->printDebugMsg("Emitting");
printDebugMsg();
} else {
//this->getStrip(0).insert(0, aVehicle);
}
//myVehicleLengthSum += aVehicle->getVehicleType().getLength();
if (wasInactive) {
MSNet::getInstance()->getEdgeControl().gotActive(this);
}
return true;
}
bool Calc::optimizeZ3()
{
double maxGuess;
double upperGuess, lowerGuess;
double upperError, lowerError;
// Our residual error threshold
const double maxErr = 0.01*radPerDeg;
int ntries;
// Calculate the true angular separation of the two stars,
// based on their RA/Dec. This need be calculated only once.
const double trueSep = calcSeparation(alignRA[1], alignDec[1], alignRA[2], alignDec[2]);
if (!MathLib::finite(trueSep)) {
printDebugMsg("Can't calculate star separation!");
return false;
}
if (trueSep < 5.0*radPerDeg) {
printDebugMsg("Stars too close together!");
return false;
}
maxGuess = 10.0*radPerDeg;
if (maxGuess > trueSep/2)
maxGuess = trueSep/2;
Z3 = upperGuess = maxGuess;
upperError = calcApparentSeparation() - trueSep;
if (MathLib::fabs(upperError) <= maxErr) {
printDebugMsg("Lucky pos. first guess");
return true; // Lucky guess!
}
Z3 = lowerGuess = -maxGuess;
lowerError = calcApparentSeparation() - trueSep;
if (MathLib::fabs(lowerError) <= maxErr) {
printDebugMsg("Lucky neg. first guess");
return true; // Lucky guess!
}
// For the interpolation to succeed, the upper and
// lower errors must be of opposite sign.
if (upperError * lowerError > 0.0) {
printDebugMsg("Correct Z3 out of range");
return false;
}
ntries = 0;
for (;;) {
double newGuess, newError;
ntries++;
// Interpolate between the upper and lower guesses to
// get a new guess.
Z3 = newGuess = lowerGuess +
((lowerError/(lowerError-upperError))*(upperGuess-lowerGuess));
newError = calcApparentSeparation() - trueSep;
if (MathLib::fabs(newError) <= maxErr) {
std::cout << "Converged after " << ntries << " tries: Z3= " << Z3*degPerRad << std::endl;
return true; // We have converged! Z3 is set.
}
if (ntries > 25) {
printDebugMsg("No convergence after 25 tries");
return false; // Failure to converge
}
// NewGuess replaces either lowerGuess or upperGuess, depending
// on the sign of the newError.
if (lowerError * newError > 0) {
// newError has the same sign as lowerError
lowerGuess = newGuess;
lowerError = newError;
} else {
upperGuess = newGuess;
upperError = newError;
}
}
// We should never get here!
printDebugMsg("Broke out of loop!?!");
return false;
}
