diff --git a/server/block.cc b/server/block.cc index 2d56cf9..5bbc80e 100644 --- a/server/block.cc +++ b/server/block.cc @@ -251,7 +251,7 @@ int Blocks::SetLockedby (string blname, string lockedby, int lock_onoff) { Block *bl = NULL; JSONParse jp; int res = -1; - int x, y; + int x, y, cnt; debug (0, "Blocks::SetLockedby block:'%s' locked for '%s' locked:%d", blname.c_str(), lockedby.c_str(), lock_onoff); @@ -264,7 +264,8 @@ int Blocks::SetLockedby (string blname, string lockedby, int lock_onoff) { if(network) network->ChangeListPushToAll(jp.ToString()); x = -1; y = -1; - while (server->railways.FindReference(&x, &y, blname)) { + cnt = 0; + while (server->railways.FindReference(&x, &y, blname, (cnt++))) { server->railways.SetLockedby(x, y, lockedby, lock_onoff); } diff --git a/server/locomotive.cc b/server/locomotive.cc index 53ad296..cf3d7e4 100644 --- a/server/locomotive.cc +++ b/server/locomotive.cc @@ -362,7 +362,8 @@ int Locomotives::SetAssign (string name, string block, int direction) { UnLock(); if (i < max && i >=0) { - while (server->railways.FindReference(&x, &y, block)) { + int cnt = 0; + while (server->railways.FindReference(&x, &y, block, (cnt++))) { string bl = server->blocks.GetLockedby(block); if(bl.length() == 0 || bl.compare(name) == 0) { server->blocks.SetLockedby(block, name, 1); @@ -700,20 +701,25 @@ int Locomotives::SchedulerNextStep(Locomotive *loc) { // // with each call only check one single step of the way, if we have to -// turn a turnout try to do it and return 0 -// if everything is set up till the destination block return 1 +// turn a turnout try to do it and return 0. +// if everything is set up till the destination (next) block return 1 +// to speed things up: we only prepare ways wich we have locked already // int Locomotives::AutoCheckWaySingleStep(string way, string locname, int *data) { int res = 0; size_t pos1; size_t curpos; size_t blockpos; + int cnt; int state; int newdata = 0; + string turnout; + int x, y; + Railway r; if (*data < 0) *data = 0; - debug (0, "Locomotives::AutoCheckWaySingleStep Way:%s", way.c_str()); + debug (0, "Locomotives::AutoCheckWaySingleStep Prepare for Loco: %s Way:%s", locname.c_str(), way.c_str()); curpos = 0; do { @@ -723,28 +729,45 @@ int Locomotives::AutoCheckWaySingleStep(string way, string locname, int *data) { pos1 = way.find(":", curpos+3); if (pos1 == string::npos) { debug (0, "%s:%d turnout without value? '%s'", __FILE__, __LINE__, way.c_str()); + server->PowerOnOff(0); + server->SetModeManual(); return 0; } + turnout = way.substr(curpos+3, pos1-curpos-3); - state = server->turnouts.Get(way.substr(curpos+3, pos1-curpos-3)); + state = server->turnouts.Get(turnout); if (state == -1) { - debug (0, "%s:%d turnout not found '%s'", __FILE__, __LINE__, way.substr(curpos+3, pos1-curpos-3).c_str()); + debug (0, "%s:%d turnout not found '%s'", __FILE__, __LINE__, turnout.c_str()); + server->PowerOnOff(0); + server->SetModeManual(); return 0; } - if (way[pos1+1] == '0' && state != 0) { - server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 0); - break; - } - else if (way[pos1+1] == '1' && state != 1) { - server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 1); - break; + cnt = 0; + while (server->railways.FindReference(&x, &y, turnout, cnt++)) { + cnt++; + r = server->railways.RailwayGet(x, y); + debug (0, "Locomotives::AutoCheckWaySingleStep Loco:%s Turnout: %s LockedBy:%s", locname.c_str(), turnout.c_str(), r.lockedby); + + if (locname.compare(r.lockedby) == 0) { + if (way[pos1+1] == '0' && state != 0) { + server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 0); + break; + } + else if (way[pos1+1] == '1' && state != 1) { + server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 1); + break; + } + else if (newdata > *data) { + if (way[pos1+1] == '0') server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 0); + if (way[pos1+1] == '1') server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 1); + (*data) = newdata; + break; + } + } } - else if (newdata > *data) { - if (way[pos1+1] == '0') server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 0); - if (way[pos1+1] == '1') server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 1); - (*data) = newdata; - break; + if (cnt == 0) { + debug (0, "Locomotives::AutoCheckWaySingleStep Loco:%s Turnout:%s Reference not found", locname.c_str(), turnout.c_str()); } } } while (curpos != string::npos); @@ -932,7 +955,8 @@ int Locomotives::Loco_OnRoute(Locomotive *loco) { if (Loco_SearchAndLock(loco) == 1) { loco->auto_onroute = LOCO_OR_ENTERBLOCKNEXT; debug (0, "Locomotives::Loco_OnRoute Found Way Prepare '%s'\n", loco->name); - SetSpeed(loco->name, loco->flags & LOCO_F_REVERSE ? -loco->vmin : loco->vmid); + SetSpeed(loco->name, loco->flags & LOCO_F_REVERSE ? -loco->vslow : loco->vslow); + timer_start(&loco->auto_timenext); } else { loco->auto_onroute = LOCO_OR_ENTERBLOCKSTOP; @@ -1012,13 +1036,18 @@ int Locomotives::Loco_BlockEnterNext(Locomotive *loco) { s_enter = server->blocks.GetSensorEnter(dir_reverse, block, loco->flags); if (server->sensors.GetActive(s_stop) == 1) { - - if (loco->auto_timenext.tv_sec > 0) { + if (!AutoCheckWaySingleStep(loco->auto_way, loco->name, &loco->auto_data)) { // // end of block and not finished preparing the way. // stop everything - debug (0, "Locomotives::Loco_BlockEnterNext ********** Loco:%s could not prepare way in time. STOP", loco->name); - server->PowerOnOff(0); + debug (0, "Locomotives::Loco_BlockEnterNext could not prepare way in time '%s'\n", loco->name); + SetSpeed(loco->name, 0); + loco->auto_onroute = LOCO_OR_STOPWAIT; + server->railways.UnLockWay(loco->auto_wayold, loco->name); + server->blocks.SetLockedby(loco->blockprev+2, loco->name, 0); + loco->auto_wayold[0] = 0; + timer_start(&loco->auto_timenext); + return 0; } else { @@ -1035,14 +1064,14 @@ int Locomotives::Loco_BlockEnterNext(Locomotive *loco) { } if (loco->auto_timenext.tv_sec > 0 && timer_get(&loco->auto_timenext) > LOCO_TO_TURNOUT) { - debug (0, "* Locomotives::Loco_BlockEnterNext prepare way for loco: %s", loco->name); - if (AutoCheckWaySingleStep(loco->auto_way, loco->name, &loco->auto_data) == 1) { - // - // finished preparing new way, need to stay in ENTERBLOCKNEXT to unlock old way - debug (0, "* Locomotives::Loco_BlockEnterNext finished preparing way for loco: %s", loco->name); - loco->auto_timenext.tv_sec = 0; - } - else timer_start(&loco->auto_timenext); + debug (0, "* Locomotives::Loco_BlockEnterNext prepare way for loco: %s", loco->name); + if (AutoCheckWaySingleStep(loco->auto_way, loco->name, &loco->auto_data) == 1) { + // + // finished preparing new way, need to stay in ENTERBLOCKNEXT to unlock old way + debug (0, "* Locomotives::Loco_BlockEnterNext finished preparing way for loco: %s", loco->name); + loco->auto_timenext.tv_sec = 0; + } + else timer_start(&loco->auto_timenext); } return 0; diff --git a/server/railway.cc b/server/railway.cc index c45812a..e31cf1c 100644 --- a/server/railway.cc +++ b/server/railway.cc @@ -11,6 +11,19 @@ Railways::Railways() { mtx = { 0 }; mtx = PTHREAD_MUTEX_INITIALIZER; changed = 0; + + // + // do some tests on start + string way = "b:-:B43,t:w90:0,b:-:B1,t:w28:1,t:w30:1,t:w31:0,t:w32:0,t:w33:0,t:w33:0,t:w34:1,t:w35:0,t:w36:1,t:w31:1,t:w30:1,t:w28:1,b:+:B1,t:w90:1,b:+:B42a,t:w89:0,b:+:B42b,t:w91:1,b:+:BTest,t:w95:1,b:-:B31"; + if (!FindWayCheckIfNoDoubleTurnouts(&way) == 0) { + printf ("%s:%d FindWayCheckIfNoDoubleTurnouts not ok\n", __FILE__, __LINE__); + raise (SIGINT); + } + way = "b:-:B43,t:w90:0,b:-:B1,t:w28:1,t:w32:0,t:w33:0,t:w33:0,t:w34:1,t:w35:0,t:w36:1,t:w31:1,t:w30:1,t:w28:1,b:+:B1,t:w90:1,b:+:B42a,t:w89:0,b:+:B42b,t:w91:1,b:+:BTest,t:w95:1,b:-:B31"; + if (FindWayCheckIfNoDoubleTurnouts(&way) == 0) { + printf ("%s:%d FindWayCheckIfNoDoubleTurnouts not ok\n", __FILE__, __LINE__); + raise (SIGINT); + } }; @@ -42,6 +55,7 @@ int Railways::UnLock() { struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype) { struct s_findway_data np = { x: -1, y: -1, enterfrom: -1, oldx: pos.x, oldy:pos.y, oldenterfrom: pos.enterfrom, + dist: pos.dist + 1, way: pos.way }; // newpos if (dirtype == 1) { @@ -49,10 +63,12 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype) np.x = pos.x; np.y = pos.y + 1 ; np.enterfrom = EF_NORTH; + return np; } else if (pos.enterfrom == EF_SOUTH) { np.x = pos.x; np.y = pos.y - 1; np.enterfrom = EF_SOUTH; + return np; } } else if (dirtype == 2) { @@ -60,10 +76,12 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype) np.x = pos.x + 1; np.y = pos.y; np.enterfrom = EF_WEST; + return np; } else if (pos.enterfrom == EF_EAST) { np.x = pos.x - 1; np.y = pos.y; np.enterfrom = EF_EAST; + return np; } } else if (dirtype == 3) { @@ -71,10 +89,12 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype) np.x = pos.x; np.y = pos.y - 1; np.enterfrom = EF_SOUTH; + return np; } else if (pos.enterfrom == EF_NORTH){ np.x = pos.x - 1; np.y = pos.y; np.enterfrom = EF_EAST; + return np; } } else if (dirtype == 4) { @@ -82,10 +102,12 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype) np.x = pos.x + 1; np.y = pos.y; np.enterfrom = EF_WEST; + return np; } else if (pos.enterfrom == EF_EAST){ np.x = pos.x; np.y = pos.y - 1; np.enterfrom = EF_SOUTH; + return np; } } else if (dirtype == 5) { @@ -93,10 +115,12 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype) np.x = pos.x + 1; np.y = pos.y; np.enterfrom = EF_WEST; + return np; } else if (pos.enterfrom == EF_EAST){ np.x = pos.x; np.y = pos.y + 1; np.enterfrom = EF_NORTH; + return np; } } else if (dirtype == 6) { @@ -104,12 +128,18 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype) np.x = pos.x; np.y = pos.y + 1; np.enterfrom = EF_NORTH; + return np; } else if (pos.enterfrom == EF_SOUTH){ np.x = pos.x - 1; np.y = pos.y; np.enterfrom = EF_EAST; + return np; } } + else { + debug (0, "Railways::NextPos dirtype unknown:%d", dirtype); + return np; + } return np; }; @@ -393,37 +423,32 @@ void Railways::ClearLockedby(string name) { } -int Railways::_FindReference(int *x, int *y, string name) { +int Railways::_FindReference(int *x, int *y, string name, int cnt) { int found = 0; + int c; // counter of occurance if (x == NULL || y == NULL) return 0; - if (*x < 0 || *y < 0) { - *x = 0; - *y = 0; - } - - for (; found == 0 && *y < height; (*y)++) { - if (*x >= width) *x = 0; - else (*x)++; - for (; found == 0 && *x < width; (*x)++) { - if (strncmp (railways[GetRIdx(*x, *y)].name, name.c_str(), REFERENCENAME_LEN) == 0) { - found = 1; - break; - } + for (c = 0, *y = 0; found == 0 && *y < height; (*y)++) { + for (*x = 0; found == 0 && *x < width; (*x)++) { + if (strncmp (railways[GetRIdx(*x, *y)].name, name.c_str(), REFERENCENAME_LEN) == 0) + if ((c++) == cnt) { + found = 1; + break; + } } if (found) break; } if (*x < width && *y < height) return 1; - else return 0; + return 0; }; -int Railways::FindReference(int *x, int *y, string name) { +int Railways::FindReference(int *x, int *y, string name, int cnt) { int res; Lock(); - res = _FindReference (x, y, name); + res = _FindReference (x, y, name, cnt); UnLock(); return res; }; @@ -519,6 +544,23 @@ string Railways::GetDestBlock(int locoflags, string blockend) { return block; } +/* + * insert the element in an ordered list + */ +void FDListPush (std::list *list, struct s_findway_data *element) { + std::list::iterator iter; + + for (iter = list->begin(); iter != list->end(); iter++) + if (element->dist < iter->dist) { + // printf ("%s:%d Push dist list:%d dist element:%d (%d,%d E:%d)\n", __FILE__, __LINE__, iter->dist, element->dist, element->x, element->y, element->enterfrom); + list->insert(iter, *element); + return; + } + + // printf ("%s:%d Push dist list:--- dist element:%d (%d,%d E:%d)\n", __FILE__, __LINE__, element->dist, element->x, element->y, element->enterfrom); + list->push_back(*element); +} + /**************************************************************************** * FindWay: will be in two phases to decide where to go and how @@ -572,8 +614,9 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf fd_end.x = fd_start.x = -1; fd_end.y = fd_start.y = -1; fd_end.enterfrom = fd_start.enterfrom = -1; + fd_end.dist = fd_start.dist = 0; - if (FindReference(&fd_start.x, &fd_start.y, blockstart.substr(2, string::npos))) { + if (FindReference(&fd_start.x, &fd_start.y, blockstart.substr(2, string::npos),0)) { debug (0, "Railway::FindWay found startblock (%s).", blockstart.c_str()); if (Get(fd_start.x, fd_start.y).dir == 1) { if (blockstart[0] == '+') fd_start.enterfrom = 0; @@ -593,7 +636,7 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf return 0; } - if (FindReference(&fd_end.x, &fd_end.y, blockend.substr(2, string::npos))) { + if (FindReference(&fd_end.x, &fd_end.y, blockend.substr(2, string::npos), 0)) { debug (0, "Railway::FindWay found endblock (%s).", blockend.c_str()); if (Get(fd_end.x, fd_end.y).dir == 1) { if (blockend[0] == '+') fd_end.enterfrom = 0; @@ -620,8 +663,13 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf // loop through the map to find all possible ways to the destination // this will be done in respect of lockedby blocks and ways. // + // DebugPrintFindWay(fd_data); while ((fd_list.size() > 0 || fd_pos.enterfrom >= 0) && found == 0) { - // DebugPrintFindWay(fd_data); + // std::list::iterator iter; + // for (iter = fd_list.begin(); iter != fd_list.end(); iter++) + // printf ("List: Dist:%-4d Pos (%d,%d)\n", iter->dist, iter->x, iter->y); + // DebugPrintFindWay(fd_data); + if (fd_pos.enterfrom < 0) { std::list::iterator iter; @@ -629,6 +677,7 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf if (iter != fd_list.end()) { fd_pos = *iter; fd_list.pop_front(); + // printf ("%s:%d pop_front -> fd_pos (%d,%d e:%d) dist:%d\n", __FILE__, __LINE__, fd_pos.x, fd_pos.y, fd_pos.enterfrom, fd_pos.dist); } } @@ -680,23 +729,24 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf else if (rpos->type == RAILWAY_TURNOUT) { fd_tmp = NextPos(fd_pos, rpos->altdir); fd_tmp.way += (string)",t:" + (string)rpos->name + (string)":1"; - fd_list.push_back(fd_tmp); - fd_pos = NextPos(fd_pos, rpos->dir); - if (!_NextPosIsValid(lockedfor, locoflags, &fd_pos)) { - fd_pos.enterfrom = -1; - fd_pos.x = -1; - fd_pos.y = -1; - fd_pos.way = ""; - } - else fd_pos.way += (string)",t:" + (string)rpos->name + (string)":0"; + if (fd_tmp.enterfrom != -1) FDListPush(&fd_list, &fd_tmp); + + fd_tmp = NextPos(fd_pos, rpos->dir); + fd_tmp.way += (string)",t:" + (string)rpos->name + (string)":0"; + if (fd_tmp.enterfrom != -1) FDListPush(&fd_list, &fd_tmp); + + fd_pos.enterfrom = -1; + fd_pos.x = -1; + fd_pos.y = -1; + fd_pos.way = ""; } else if (rpos->type == RAILWAY_CONNECTOR) { int conector_found = 0; - x = -1; y = -1; - while (conector_found == 0 && _FindReference(&x, &y, rpos->name) == 1) { + int cnt = 0; + while (conector_found == 0 && _FindReference(&x, &y, rpos->name, (cnt++)) == 1) { if (fd_pos.x != x || fd_pos.y != y) { fd_pos.oldx = fd_pos.x; fd_pos.oldy = fd_pos.y; @@ -783,7 +833,13 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf else // nothing found? *next = ""; - if (!FindWayCheckIfNoDoubleTurnouts (next)) *next = ""; + // + // check that a way is not set twice + // + if (!FindWayCheckIfNoDoubleTurnouts (next)) { + debug (0, "FindWayCheckIfNoDoubleTurnouts way [%s] is not valid.", next->c_str()); + *next = ""; + } } UnLock(); @@ -798,16 +854,21 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf int Railways::FindWayCheckIfNoDoubleTurnouts (string *way) { size_t pos1, pos2; // block positions size_t tp1, tp2; // turnouts + int wayset; + string s; // search string (should not been found) for (pos1 = 0, pos2 = way->find(",b:"); pos2 != string::npos; pos1 = pos2, pos2 = way->find(",b:", pos1+1)) { tp1 = pos1; for (tp1 = way->find(",t:", tp1+1); tp1 != string::npos; tp1 = way->find(",t:", tp1+1)) { - tp2 = way->find (way->substr(tp1, way->find(":", tp1+4)-tp1), tp1+1); + if ((*way)[way->find(":", tp1+4)+1] == '0') + s = way->substr(tp1, way->find(":", tp1+4)-tp1) + ":1"; + else + s = way->substr(tp1, way->find(":", tp1+4)-tp1) + ":0"; + tp2 = way->find (s, tp1+4); if (tp2 == string::npos) continue; // not found if (tp2 < pos2) return 0; // found ... return 0 } } - return 1; } @@ -880,8 +941,8 @@ int Railways::_SplitBlockIsFreeAndAllowed(string loconame, int locoflags, string if (server->blocks.GetLockedby(pblock).length() != 0) return 0; // check the way between the blocks - if (_FindReference(&x1, &y1, pblock) == 0) return 0; - if (_FindReference(&x2, &y2, nblock) == 0) return 0; + if (_FindReference(&x1, &y1, pblock, 0) == 0) return 0; + if (_FindReference(&x2, &y2, nblock, 0) == 0) return 0; debug (0, "Railway::_SplitBlockIsFreeAndAllowed p pos(%d,%d) n pos(%d,%d)", x1, y1, x2, y2); x = x1; y = y1; @@ -981,7 +1042,7 @@ int Railways::LockWay (string way, string lockedby, int lockonoff) { // // retrieve final start position, in case of split blocks - if (FindReference(&start.x, &start.y, startblockfinal)) { + if (FindReference(&start.x, &start.y, startblockfinal, 0)) { if (Get(start.x, start.y).dir == 1) { if (startblock[0] == '+') start.enterfrom = 0; else start.enterfrom = 2; @@ -1027,7 +1088,7 @@ int Railways::LockWay (string way, string lockedby, int lockonoff) { } // get the final end position - if (!FindReference(&end.x, &end.y, endblockfinal)) { + if (!FindReference(&end.x, &end.y, endblockfinal, 0)) { debug (0, "Railway::FindWay could not find endblock (%s).", endblockfinal.c_str()); return 0; } @@ -1163,11 +1224,11 @@ int Railways::LockWay (string way, string lockedby, int lockonoff) { else if (r->type == RAILWAY_CONNECTOR) { int found = 0; - int x, y; + int x, y, cnt; printf ("%s:%d connector found Reference:'%s'\n", __FILE__, __LINE__, r->name); x = -1; y = -1; - while (found == 0 && _FindReference(&x, &y, r->name) == 1) { + while (found == 0 && _FindReference(&x, &y, r->name, (cnt++)) == 1) { if (pos.x != x || pos.y != y) { printf ("%s:%d found %d,%d\n", __FILE__, __LINE__, x, y); pos.oldx = pos.x; diff --git a/server/railway.h b/server/railway.h index 09fd5ca..8df9b49 100644 --- a/server/railway.h +++ b/server/railway.h @@ -74,6 +74,7 @@ struct s_findway_data { int oldx; int oldy; int oldenterfrom; + int dist; string way; }; @@ -98,7 +99,7 @@ class Railways { JSONParse _GetJSONRailway(int x, int y); void _New (int w, int h); void DebugPrintFindWay(struct s_findway_map *fw); - int _FindReference(int *x, int *y, string name); + int _FindReference(int *x, int *y, string name, int cnt); int _SplitBlockIsFreeAndAllowed(string loconame, int locoflags, string block); int _NextPosIsValid(string loconame, int locoflags, struct s_findway_data *fwd); string GetDestBlock(int locoflags, string blockend); @@ -127,7 +128,7 @@ class Railways { void GetJSONAll(JSONParse *json); Railway GetRailwayFromJSON(JSONParse *j); - int FindReference(int *x, int *y, string name); + int FindReference(int *x, int *y, string name, int cnt); int FindWay(string blockstart, string blockend, string lockedfor, string *next); int FindWayCheckIfNoDoubleTurnouts (string *next); int FindRandomWay(string blockstart, string lockedfor, string *next); diff --git a/server/server.cc b/server/server.cc index 435ca36..1d91c14 100644 --- a/server/server.cc +++ b/server/server.cc @@ -62,12 +62,13 @@ Server::~Server() { int Server::TurnoutSet(string name, int active) { int x, y, locked = 0; string lockedby; + int cnt = 0; Railway r; // // check if locked - while (server->railways.FindReference(&x, &y, name)) { + while (server->railways.FindReference(&x, &y, name, (cnt++))) { r = server->railways.Get(x, y); - // printf ("%s:%d Reference %d,%d Name:%s Lockedby:%s\n", __FILE__, __LINE__, x, y, r.name, r.lockedby); + debug (0, "%s:%d Reference %d,%d Name:%s Lockedby:%s", __FILE__, __LINE__, x, y, r.name, r.lockedby); if (r.lockedby[0] != 0) { locked = 1; lockedby = r.lockedby;