/*
* Fonction de réservation qui test si les voies peuvent être empruntées
*/
bool canStart(Train* train_train_quotidien){
bool inProgress = true;
Voie* nextVoie = getNextVoieTRAIN(train_train_quotidien);
while (inProgress){
if ((nextVoie->reserve && nextVoie->reserveId != train_train_quotidien->id) || nextVoie->nbTrainAct == nextVoie->nbMaxTrain || (nextVoie->currentSens != 0 && nextVoie->currentSens != train_train_quotidien->sens)) {
// On réinitialise le tableau de réservation du train
for (int j = 0; j < train_train_quotidien->nbReservation; j++) {
train_train_quotidien->reservationTab[j]->reserve = false;
train_train_quotidien->reservationTab[j]->reserveId = -1;
train_train_quotidien->reservationTab[j]->currentSens = train_train_quotidien->reservationTab[j]->sens;
}
train_train_quotidien->nbReservation = 0;
printf("Le Train n°%d est en ATTENTE (Voie %d non libre)\n", train_train_quotidien->id, nextVoie->id);
pthread_cond_wait(&nextVoie->voieLibre, &mutex);
nextVoie = train_train_quotidien->position;
printf("Le Train n°%d se REVEILLE et recommence à réserver de %d \n\n",train_train_quotidien->id, nextVoie->id);
}else{
makeReservation(train_train_quotidien, nextVoie);
if (nextVoie->canStop || nextVoie->id == train_train_quotidien->endPos->id) {
printf("Arrêt réservation pour train n°%d\n", train_train_quotidien->id);
inProgress = false;
}
}
//printf("Next voie %d\n", nextVoie->id);
nextVoie = getNextVoie(nextVoie, train_train_quotidien->sens, train_train_quotidien->priorite);
//printf("Next next voie %d\n", nextVoie->id);
}
return true;
}
static int groupSetSpeed(MultiTrainDriver* me, int speed) {
PrintDebug(me->ui, "MultiTrain %d set speed %d", me->trainNum, speed);
int shouldSetSpeed = 1;
if (!me->tailMode) {
if (speed == -1) {
// make every train stop and wait for there stop response
speed = 0;
me->isReversing = 1;
me->speedAfterReverse = me->info[0].trainSpeed;
me->stoppedCount = 0; // required hack, the count will be accumulated back to numTrain soon
} else if (speed == 0){
me->stoppedCount = 0; // required hack, the count will be accumulated back to numTrain soon
}
if (speed != 0) {
// everyone is moving again
me->stoppedCount = 0;
if (me->info[0].trainSpeed == 0) {
int stoppingDistance;
DriverMsg qMsg;
qMsg.type = QUERY_STOPPING_DISTANCE;
qMsg.data2 = speed;
Send(me->trainId[0], (char *)&qMsg, sizeof(DriverMsg), (char*)&stoppingDistance, sizeof(int));
shouldSetSpeed = makeReservation(me, stoppingDistance) == RESERVE_SUCESS;
}
}
}
DriverMsg msg;
msg.type = SET_SPEED;
msg.data2 = speed;
if (shouldSetSpeed) {
for (int i = 0; i < me->numTrainInGroup; i++) {
Send(me->trainId[i], (char *)&msg, sizeof(DriverMsg), (char*)1, 0);
}
}
if (speed == 0 && !me->reserveTrackMode) {
clearReservation(me->trackManager, me->trainNum);
}
return shouldSetSpeed;
}
void multitrain_driver() {
MultiTrainDriver me;
initDriver(&me);
unsigned int naggCount = 0;
unsigned int updateStoppingDistanceCount = 0;
for (;;) {
int tid = -1;
MultiTrainDriverMsg actualMsg;
DriverMsg* msg = (DriverMsg*)&actualMsg;
msg->data3 = 0;
Receive(&tid, (char *)msg, sizeof(MultiTrainDriverMsg));
if (msg->type != REPORT_INFO &&
msg->type != QUERY_STOP_COUNT &&
msg->type != MULTI_TRAIN_DRIVER_COURIER &&
msg->type != SENSOR_TRIGGER &&
msg->type != DELTA_DISTANCE) {
Reply(tid, (char*)1, 0);
}
switch (msg->type) {
case SET_SPEED: {
if (!me.tailMode) {
Reply(msg->replyTid, (char*)1, 0);
}
groupSetSpeed(&me, msg->data2);
break;
}
case SENSOR_TRIGGER: {
if (me.tailMode && tid == me.sensorWatcher) {
Reply(tid, (char *)NULL, 0);
break;
}
if (me.tailMode) {
int isHandled = 0;
Send(me.trainId[0],
(char*)msg, sizeof(DriverMsg), (char *)&isHandled, sizeof(int));
Reply(tid, (char *)&isHandled, sizeof(int));
} else {
int isSensorReserved = QueryIsSensorReserved(&me, msg->data2, msg->data3);
if (isSensorReserved) {
// sensor is reserved
for (int i = 0; i < me.numTrainInGroup; i ++) {
int isHandled = 0;
Send(me.trainId[i],
(char*)msg, sizeof(DriverMsg), (char *)&isHandled, sizeof(int));
if (isHandled) {
break;
}
}
}
Reply(tid, (char *)NULL, 0);
}
break;
} // case
case NAVIGATE_NAGGER: {
if (me.tailMode) break;
updateInfo(&me);
if (me.routeRemaining != -1) {
if (!me.stopCommited) {
if (shouldStopNow(&me)) {
if (me.route.nodes[me.stopNode].num == REVERSE) {
//TrainDebug(&me, "Navi reversing.");
groupSetSpeed(&me, -1); // reverse
}
else {
//TrainDebug(&me, "Navi Nagger stopping.");
groupSetSpeed(&me, 0); // stopping
me.route.length = 0; // Finished the route.
me.testMode = 0;
me.routeRemaining = -1;
}
me.stopCommited = 1;
me.useLastSensorNow = 0;
me.stopNow = 0;
me.stopSensorHit = 0;
} else {
if ((++updateStoppingDistanceCount & 15) == 0) updateStopNode(&me);
}
}
}
if (me.nextSetSwitchNode != -1 && (++me.setSwitchNaggerCount & 3) == 0) {
trySetSwitch_and_getNextSwitch(&me);
}
if (me.rerouteCountdown-- == 0) {
if (me.testMode) {
int reserveStatus = makeReservation(&me, 440);
if (reserveStatus == RESERVE_FAIL) {
reroute(&me);
} else {
me.nextSetSwitchNode = -1;
updateSetSwitch(&me);
groupSetSpeed(&me, 8);
}
} else {
// reroute
if (me.route.length != 0) {
setRoute(&me, &(me.info[0].pos), &(me.routeMsg));
}
}
}
break;
}
case UPDATE_PREDICTION: {
if (me.tailMode) {
MultiTrainDriverCourierMsg cMsg;
cMsg.destTid = me.headTid;
cMsg.msg = actualMsg;
cMsg.msg.data = MyTid();
Reply(me.courier, (char *)&cMsg, sizeof(MultiTrainDriverCourierMsg));
break;
}
for (int i = 0; i < MAX_TRAIN_IN_GROUP; i++) {
if (actualMsg.data == me.trainId[i]) {
for (int j = 0; j < actualMsg.numSensors; j++) {
me.sensorToReserve[i][j] = actualMsg.sensors[j];
}
me.numSensorToReserve[i] = actualMsg.numSensors;
}
}
makeReservation(&me, me.info[0].maxStoppingDistance);
break;
}
case STOP_COMPLETED: {
// notify actual train controller.
if (me.tailMode) {
MultiTrainDriverCourierMsg cMsg;
cMsg.destTid = me.headTid;
cMsg.msg = actualMsg;
cMsg.msg.data = MyTid();
Reply(me.courier, (char *)&cMsg, sizeof(MultiTrainDriverCourierMsg));
break;
}
me.stoppedCount++;
if (me.stoppedCount == me.numTrainInGroup) {
makeReservation(&me, 1);
if (me.isReversing) {
handleReverse(&me);
} else if (me.route.length != 0) {
// Reroute.
setRoute(&me, &(me.info[0].pos), &(me.routeMsg));
}
}
if (!me.reserveTrackMode) {
clearReservation(me.trackManager, me.trainNum);
}
break;
}
case SET_ROUTE: {
Reply(msg->replyTid, (char*)1, 0);
me.routeMsg = *msg;
setRoute(&me, &(me.info[0].pos), msg);
break;
}
case GET_POSITION: {
if (me.tailMode) {
PrintDebug(me.ui, "Get position from a tail train ??");
break;
}
// If don't have any valid info yet, reply empty message
if (me.infoUpdater == -1) {
Reply(msg->replyTid, (char*)1, 0);
} else {
Reply(msg->replyTid, (char*)&(me.info[0]), sizeof(DumbDriverInfo));
}
break;
}
case FIND_POSITION: {
if (me.tailMode) {
PrintDebug(me.ui, "find position while merge????");
break;
}
me.reserveTrackMode = (msg->data2 == RESERVE);
PrintDebug(me.ui, "Train locking %d", me.trainNum);
// Only 1 train can lock at the same time.
lock(me.timeserver);
// begin finding position in a slow speed
DumbTrainSetSpeed(me.trainId[0], 5);
Reply(msg->replyTid, (char*)1, 0);
for (;;) {
Receive(&tid, (char*)msg, sizeof(MultiTrainDriverMsg));
Reply(tid, (char*)1, 0);
if (msg->type == SENSOR_TRIGGER) {
Send(me.trainId[0], (char*)msg,
sizeof(DriverMsg), (char*)NULL, 0);
DumbTrainSetSpeed(me.trainId[0], 0);
break;
} else if (msg->type == GET_POSITION) {
Reply(msg->replyTid, (char*)1, 0);
} else {
PrintDebug(me.ui, "WARNN Drop %d", msg->type);
}
}
DriverMsg dMsg;
dMsg.type = REPORT_INFO;
for (int i = 0; i < me.numTrainInGroup; i++) {
Send(me.trainId[i],
(char *)&dMsg, sizeof(DriverMsg),
(char*)&me.info[i], sizeof(DumbDriverInfo));
}
me.infoUpdater = Create(3, trainNavigateNagger);
unlock();
break;
}
case MERGE_HEAD: {
if (me.tailMode) {
PrintDebug(me.ui, "Cannot be a head when in tail mode??");
break;
}
me.reserveTrackMode = 1; // Head always reserves track.
// Other train controller's id.
me.trainId[me.numTrainInGroup] = msg->data2;
me.numTrainInGroup++;
Reply(msg->replyTid, (char*)1, 0);
DriverMsg dMsg;
dMsg.type = QUERY_STOP_COUNT;
int tailStopCount = 0;
Send(msg->data2, (char *)&dMsg, sizeof(DriverMsg), (char *)&tailStopCount, sizeof(int));
me.stoppedCount += tailStopCount;
dMsg.type = UPDATE_PARENT_ABOUT_PREDICTION;
Send(msg->data2, (char *)&dMsg, sizeof(DriverMsg), (char *)NULL, 0);
PrintDebug(me.ui, "merged. head is %d", me.trainNum);
break;
}
case MERGE_TAIL: {
if (me.tailMode) {
PrintDebug(me.ui, "Double merge tail??");
break;
}
// Enters courier mode that passes dumb_train msg to 'real' controller
me.tailMode = 1;
me.headTid = msg->data2;
clearReservation(me.trackManager, me.trainNum);
Reply(msg->replyTid, (char*)1, 0);
PrintDebug(me.ui, "Train %d is tail", me.trainNum);
break;
}
case SEPARATE_TAIL: {
if (!me.tailMode) {
PrintDebug(me.ui, "Not in tail mode..??"); break;
}
me.tailMode = 0;
me.headTid = 0;
// TODO, behaviour is not clearly defined yet.
// reserve my own track and prediction??
break;
}
case REPORT_INFO: {
if (!me.tailMode) {
PrintDebug(me.ui, "Report info Not in tail mode..??"); break;
}
// ASSUME ONLY 1 train when in tail mode.
Send(me.trainId[0], (char*)msg, sizeof(DriverMsg),
(char*)&me.info[0], sizeof(DumbDriverInfo));
// Reply the head that made query.
Reply(me.headTid, (char*)&me.info[0], sizeof(DumbDriverInfo));
break;
}
case DELTA_DISTANCE: {
if (!me.tailMode) {
PrintDebug(me.ui, "Delta distance and not in tail mode..??"); break;
}
// ASSUME ONLY 1 train when in tail mode.
Send(me.trainId[0], (char*)msg,
sizeof(DriverMsg) - sizeof(Position), (char*)1, 0);
// Reply the head that made query.
Reply(me.headTid, (char*)1, 0);
break;
}
case UPDATE_PARENT_ABOUT_PREDICTION: {
if (!me.tailMode) {
PrintDebug(me.ui, "%d Update Parent Not in tail mode..??", me.trainNum);
break;
}
// ASSUME ONLY 1 train when in tail mode.
Send(me.trainId[0], (char*)msg, sizeof(DriverMsg),
(char*)NULL, 0);
break;
}
case QUERY_STOP_COUNT: {
// asuumption: no tree strucutre
Reply(tid, (char *)&me.stoppedCount, sizeof(int));
break;
}
case MULTI_TRAIN_DRIVER_COURIER: {
// nothing
break;
}
case REVERSE_SPEED: {
if (!me.tailMode) {
PrintDebug(me.ui, "Reverse Speed Not in tail mode..??"); break;
}
for (int i = 0; i < me.numTrainInGroup; i++) {
Send(me.trainId[i], (char *)msg, sizeof(DriverMsg), (char*)1, 0);
}
break;
}
case QUERY_STOPPING_DISTANCE: {
if (!me.tailMode) {
PrintDebug(me.ui, "Query stopping dist Not in tail mode..??"); break;
}
int stoppingDist = 0;
// ASSUME ONLY 1 train when in tail mode.
Send(me.trainId[0], (char*)msg, sizeof(DriverMsg),
(char*)&stoppingDist, sizeof(int));
// Reply the head that made query.
Reply(me.headTid, (char*)&stoppingDist, sizeof(int));
break;
}
case HIT_SECONDARY: {
if (me.route.length != 0) {
PrintDebug(me.ui, "Hit secondary rerouting..");
groupSetSpeed(&me, 0);
}
break;
}
case SET_FOLLOWING_DISTANCE: {
me.minFollowingDist = msg->data2;
me.maxFollowingDist = msg->data3;
Reply(msg->replyTid, (char*)1, 0);
break;
}
default: {
PrintDebug(me.ui, "Not Handled %d", msg->type);
}
} // switch
} // for
}
