[31] | 1 | /************************************************************************************ |
---|
| 2 | * Copyright (C) 2008 by Politehnica University of Bucharest and Rutgers University |
---|
| 3 | * All rights reserved. |
---|
| 4 | * Refer to LICENSE for terms and conditions of use. |
---|
| 5 | ***********************************************************************************/ |
---|
| 6 | package vnsim.vehicular.routePlan.selfRouted; |
---|
| 7 | |
---|
| 8 | |
---|
| 9 | import java.io.PrintWriter; |
---|
| 10 | import java.util.ArrayList; |
---|
| 11 | import java.util.Vector; |
---|
| 12 | |
---|
| 13 | import vnsim.map.object.Cross; |
---|
| 14 | import vnsim.map.object.Globals; |
---|
| 15 | import vnsim.map.object.Point; |
---|
| 16 | import vnsim.map.object.Road; |
---|
| 17 | import vnsim.vehicular.simulator.CarInstance; |
---|
| 18 | import vnsim.vehicular.simulator.Location; |
---|
| 19 | import vnsim.vehicular.simulator.RouteSegment; |
---|
| 20 | import vnsim.vehicular.simulator.RoutingUtils; |
---|
| 21 | import vnsim.vehicular.simulator.intersections.Intersection; |
---|
| 22 | import vnsim.vehicular.simulator.intersections.IntersectionWithTrafficLights; |
---|
| 23 | |
---|
| 24 | |
---|
| 25 | public class QueryReply { |
---|
| 26 | |
---|
| 27 | String message; |
---|
| 28 | |
---|
| 29 | CarInstance carReceivingMsg; |
---|
| 30 | |
---|
| 31 | int sourceRoadIdx, sourcePointIdx; |
---|
| 32 | |
---|
| 33 | public static PrintWriter fileWriter; |
---|
| 34 | |
---|
| 35 | myPrinter combinedWriter; |
---|
| 36 | |
---|
| 37 | class myPrinter { |
---|
| 38 | public myPrinter() { |
---|
| 39 | |
---|
| 40 | } |
---|
| 41 | |
---|
| 42 | public void println(String str) { |
---|
| 43 | // fileWriter.println(str); |
---|
| 44 | // System.out.println("Car " + carReceivingMsg.ID + ": " + str); |
---|
| 45 | // System.out.flush(); |
---|
| 46 | } |
---|
| 47 | |
---|
| 48 | } |
---|
| 49 | |
---|
| 50 | public QueryReply(String msg, CarInstance car, int rd, int pt) { |
---|
| 51 | this.message = msg; |
---|
| 52 | this.carReceivingMsg = car; |
---|
| 53 | this.sourcePointIdx = pt; |
---|
| 54 | this.sourceRoadIdx = rd; |
---|
| 55 | this.combinedWriter = new myPrinter(); |
---|
| 56 | |
---|
| 57 | } |
---|
| 58 | |
---|
| 59 | public boolean analizeMessage() { |
---|
| 60 | String reply = new String(this.message); |
---|
| 61 | int rdIdx = -1, ptIdx = -1, rdCond; |
---|
| 62 | int totalSpeed = 0; |
---|
| 63 | Vector<Integer> roadProblems = new Vector<Integer>(); |
---|
| 64 | Vector<Integer> problemPoints = new Vector<Integer>(); |
---|
| 65 | double speedCount = 0; |
---|
| 66 | String tip, tpVal; |
---|
| 67 | int i, j, k; |
---|
| 68 | |
---|
| 69 | this.carReceivingMsg.receivedMsgCount++; |
---|
| 70 | i = -1; |
---|
| 71 | i = reply.indexOf(" "); |
---|
| 72 | if (i > 0) { |
---|
| 73 | tip = reply.substring(0, i); |
---|
| 74 | if (tip.equals("RI")) { |
---|
| 75 | i++; |
---|
| 76 | while (i < reply.length()) { |
---|
| 77 | if (reply.substring(i, i + 6).equals("speed=")) { |
---|
| 78 | i = i + 6; |
---|
| 79 | tpVal = new String(); |
---|
| 80 | while (i < reply.length() && reply.charAt(i) != 's' |
---|
| 81 | && reply.charAt(i) != 'R') { |
---|
| 82 | tpVal = tpVal + reply.charAt(i); |
---|
| 83 | i++; |
---|
| 84 | } |
---|
| 85 | try { |
---|
| 86 | totalSpeed = totalSpeed |
---|
| 87 | + (new Double(tpVal)).intValue(); |
---|
| 88 | speedCount++; |
---|
| 89 | } catch (Exception e) { |
---|
| 90 | combinedWriter |
---|
| 91 | .println("Error in reply format! Incorect speed value!"); |
---|
| 92 | |
---|
| 93 | return false; |
---|
| 94 | } |
---|
| 95 | |
---|
| 96 | } else { |
---|
| 97 | if (reply.charAt(i) == 'R') { |
---|
| 98 | tpVal = new String(); |
---|
| 99 | i++; |
---|
| 100 | while (i < reply.length() && reply.charAt(i) != 'P') { |
---|
| 101 | |
---|
| 102 | tpVal = tpVal + reply.charAt(i); |
---|
| 103 | i++; |
---|
| 104 | } |
---|
| 105 | if (i == reply.length()) { |
---|
| 106 | combinedWriter.println("incompleteRoadCondition"); |
---|
| 107 | break; |
---|
| 108 | } |
---|
| 109 | try { |
---|
| 110 | rdIdx = (new Integer(tpVal)).intValue(); |
---|
| 111 | } catch (Exception e) { |
---|
| 112 | combinedWriter |
---|
| 113 | .println("Error in reply format! Incorect road value!" |
---|
| 114 | + tpVal); |
---|
| 115 | return false; |
---|
| 116 | } |
---|
| 117 | |
---|
| 118 | tpVal = new String(); |
---|
| 119 | i++; |
---|
| 120 | while (i < reply.length() && reply.charAt(i) != 'C') { |
---|
| 121 | tpVal = tpVal + reply.charAt(i); |
---|
| 122 | i++; |
---|
| 123 | } |
---|
| 124 | |
---|
| 125 | if (i == reply.length()) { |
---|
| 126 | combinedWriter.println("incompleteRoadCondition"); |
---|
| 127 | break; |
---|
| 128 | } |
---|
| 129 | try { |
---|
| 130 | ptIdx = (new Integer(tpVal)).intValue(); |
---|
| 131 | problemPoints.add(new Integer(ptIdx)); |
---|
| 132 | } catch (Exception e) { |
---|
| 133 | combinedWriter |
---|
| 134 | .println("Error in reply format! Incorect point value!"); |
---|
| 135 | return false; |
---|
| 136 | } |
---|
| 137 | |
---|
| 138 | tpVal = new String(); |
---|
| 139 | i++; |
---|
| 140 | while (i < reply.length() && reply.charAt(i) != 's' |
---|
| 141 | && reply.charAt(i) != 'R') { |
---|
| 142 | tpVal = tpVal + reply.charAt(i); |
---|
| 143 | i++; |
---|
| 144 | } |
---|
| 145 | |
---|
| 146 | try { |
---|
| 147 | rdCond = (new Integer(tpVal)).intValue(); |
---|
| 148 | roadProblems.add(new Integer(rdCond)); |
---|
| 149 | } catch (Exception e) { |
---|
| 150 | combinedWriter |
---|
| 151 | .println("Error in reply format! Incorect road condition value!"); |
---|
| 152 | return false; |
---|
| 153 | } |
---|
| 154 | |
---|
| 155 | } else { |
---|
| 156 | combinedWriter.println("Not a good format for the reply!"); |
---|
| 157 | return false; |
---|
| 158 | } |
---|
| 159 | } |
---|
| 160 | } |
---|
| 161 | // combinedWriter.println("CAR " + this.carReceivingMsg.ID+" primesc raspuns cu spCount="+speedCount); |
---|
| 162 | |
---|
| 163 | if (speedCount != 0) { |
---|
| 164 | // combinedWriter.println("average travel time is: " + totalSpeed |
---|
| 165 | // / speedCount); |
---|
| 166 | |
---|
| 167 | int h = getIndexInRouteForSegment(this.sourceRoadIdx, this.sourcePointIdx); |
---|
| 168 | // combinedWriter.println("CAR " + this.car.ID + "Aflu ceva rau de " |
---|
| 169 | // + this.rd + " la pct " + this.pt + " eu sunt la " |
---|
| 170 | // + this.car.getRoadIdx() + " sourcePointIdx=" |
---|
| 171 | // + this.car.getPointIdx()); |
---|
| 172 | if (h >= 0 && h < carReceivingMsg.route.length) { |
---|
| 173 | // combinedWriter.println("e in ruta mea " ); |
---|
| 174 | if ((((double) (totalSpeed / speedCount)) > timeAproximationForSegment(h))) { |
---|
| 175 | changeRoute(h); |
---|
| 176 | } else { |
---|
| 177 | this.carReceivingMsg.noChangeNeededCount++; |
---|
| 178 | // combinedWriter.println(" traff ok primesc: " |
---|
| 179 | // + (totalSpeed / speedCount) + " aprox " |
---|
| 180 | // + timeAproximationForSegment(h)); |
---|
| 181 | } |
---|
| 182 | } |
---|
| 183 | |
---|
| 184 | } else { |
---|
| 185 | this.carReceivingMsg.emptyMsgCount++; |
---|
| 186 | // combinedWriter.println("for road : " + sourceRoadIdx + " point" + sourcePointIdx |
---|
| 187 | // + " no speed was set: the reply was:" + this.msg); |
---|
| 188 | } |
---|
| 189 | |
---|
| 190 | // for (i = 0; i < roadProblems.size(); i++) { |
---|
| 191 | // combinedWriter.println("for road " + sourceRoadIdx + " condition:" |
---|
| 192 | // + roadProblems.elementAt(i).intValue()); |
---|
| 193 | // } |
---|
| 194 | return true; |
---|
| 195 | |
---|
| 196 | } |
---|
| 197 | } |
---|
| 198 | combinedWriter.println("a aparut o eroare la parsarea mesajui"); |
---|
| 199 | return false; |
---|
| 200 | |
---|
| 201 | } |
---|
| 202 | |
---|
| 203 | public double timeAproximationForSegment(int h) { |
---|
| 204 | double rez = Double.MAX_VALUE, speed, dist; |
---|
| 205 | Road r; |
---|
| 206 | Point p1, p2; |
---|
| 207 | r = Globals.map.roads.get(carReceivingMsg.route[h].roadIndex); |
---|
| 208 | p1 = r.points.get(carReceivingMsg.route[h].pt1); |
---|
| 209 | p2 = r.points.get(carReceivingMsg.route[h].pt2); |
---|
| 210 | dist = Math.abs((double) (p1.getDistance() - p2.getDistance())); |
---|
| 211 | // combinedWriter.println("Dist sourcePointIdx care calculez e:" + dist); |
---|
| 212 | // if (r.getRoadinfo()[1] > 50) { |
---|
| 213 | speed = 50.0; |
---|
| 214 | // } else { |
---|
| 215 | // speed = 130.0f; |
---|
| 216 | // } |
---|
| 217 | dist = (double) ((dist * 1800.0) / speed) * 3000.0; |
---|
| 218 | // combinedWriter.println("First aprox= sourcePointIdx care calculez e:" + dist); |
---|
| 219 | int i; |
---|
| 220 | Cross c; |
---|
| 221 | // if (r.getRoadinfo()[1] > 49) { |
---|
| 222 | |
---|
| 223 | Intersection it = null; |
---|
| 224 | for (i = 0; i < r.crosses.size(); i++) { |
---|
| 225 | c = r.crosses.get(i); |
---|
| 226 | if (c.getPointIndex() == carReceivingMsg.route[h].pt2) { |
---|
| 227 | it = Globals.map.allIntersections.get(c.intersectionIndex); |
---|
| 228 | break; |
---|
| 229 | } |
---|
| 230 | |
---|
| 231 | } |
---|
| 232 | double total = 0.0; |
---|
| 233 | if (it != null) { |
---|
| 234 | |
---|
| 235 | if(it instanceof IntersectionWithTrafficLights){ |
---|
| 236 | total=((IntersectionWithTrafficLights)it).cycleLength; |
---|
| 237 | int green=0; |
---|
| 238 | for(i=0;i<it.segments.size();i++){ |
---|
| 239 | if(it.segments.get(i).roadIndex==this.carReceivingMsg.route[h].roadIndex&&it.segments.get(i).pointIndex==this.carReceivingMsg.route[h].pt2){ |
---|
| 240 | |
---|
| 241 | green=((IntersectionWithTrafficLights)it).segments.get(i).getLightInfo().greenEnd-((IntersectionWithTrafficLights)it).segments.get(i).getLightInfo().greenStart; |
---|
| 242 | break; |
---|
| 243 | } |
---|
| 244 | |
---|
| 245 | } |
---|
| 246 | total=total-green; |
---|
| 247 | |
---|
| 248 | }else{ |
---|
| 249 | total = 60.0 + 10.0; |
---|
| 250 | total = total - 35.0; |
---|
| 251 | } |
---|
| 252 | } |
---|
| 253 | |
---|
| 254 | rez = dist + total * 1000.0; |
---|
| 255 | // } else { |
---|
| 256 | // rez = dist; |
---|
| 257 | // } |
---|
| 258 | // combinedWriter.println("Aproximez cu " + rez); |
---|
| 259 | return rez; |
---|
| 260 | } public int getIndexInRouteForSegment(int rd, int pt) { |
---|
| 261 | // combinedWriter.println("Caut pe ce segment se afla road:" + sourceRoadIdx + " cu punctul " |
---|
| 262 | // + sourcePointIdx); |
---|
| 263 | int i, p1, p2; |
---|
| 264 | int h = -1; |
---|
| 265 | for (i = 0; i < carReceivingMsg.route.length; i++) { |
---|
| 266 | |
---|
| 267 | if (carReceivingMsg.route[i].roadIndex == rd) { |
---|
| 268 | p1 = carReceivingMsg.route[i].pt1; |
---|
| 269 | p2 = carReceivingMsg.route[i].pt2; |
---|
| 270 | |
---|
| 271 | if (p2 < p1) { |
---|
| 272 | if ((p1 >= pt) && (p2 <= pt)) { |
---|
| 273 | h = i; |
---|
| 274 | break; |
---|
| 275 | } |
---|
| 276 | } else { |
---|
| 277 | if ((p1 <= pt) && (p2 >= pt)) { |
---|
| 278 | h = i; |
---|
| 279 | break; |
---|
| 280 | } |
---|
| 281 | } |
---|
| 282 | |
---|
| 283 | } |
---|
| 284 | } |
---|
| 285 | return h; |
---|
| 286 | } |
---|
| 287 | |
---|
| 288 | public void changeRoute(int h) { |
---|
| 289 | ArrayList<RouteSegment> tpRoute = null; |
---|
| 290 | int i, j; |
---|
| 291 | |
---|
| 292 | // combinedWriter |
---|
| 293 | // .println("Car " + this.car.ID |
---|
| 294 | // + " imi schimb cursul! recalculez....."); |
---|
| 295 | |
---|
| 296 | Location src, dest; |
---|
| 297 | |
---|
| 298 | // combinedWriter.println(" gasesc segmentul in ruta mea la poz : " + h); |
---|
| 299 | if (h <= carReceivingMsg.routeIndex) { |
---|
| 300 | carReceivingMsg.toLateFailCount++; |
---|
| 301 | // combinedWriter |
---|
| 302 | // .println("Mesaj primit prea tarziu! Sunt deja pe segmentul |
---|
| 303 | // respectiv"); |
---|
| 304 | } else { |
---|
| 305 | src = new Location(0, 0); |
---|
| 306 | dest = new Location(0, 0); |
---|
| 307 | int destIdx; |
---|
| 308 | if (h + Globals.routePlanConstants.HOW_MORE_TO_REMOVE >= carReceivingMsg.route.length) { |
---|
| 309 | // combinedWriter |
---|
| 310 | // .println("E chiar printre ultimele segmente catre |
---|
| 311 | // destinatie"); |
---|
| 312 | src.roadIdx = carReceivingMsg.route[carReceivingMsg.routeIndex].roadIndex; |
---|
| 313 | src.ptIdx = carReceivingMsg.route[carReceivingMsg.routeIndex].pt2; |
---|
| 314 | dest.roadIdx = carReceivingMsg.route[carReceivingMsg.route.length - 1].roadIndex; |
---|
| 315 | dest.ptIdx = carReceivingMsg.route[carReceivingMsg.route.length - 1].pt2; |
---|
| 316 | |
---|
| 317 | } else { |
---|
| 318 | src.roadIdx = carReceivingMsg.route[carReceivingMsg.routeIndex].roadIndex; |
---|
| 319 | src.ptIdx = carReceivingMsg.route[carReceivingMsg.routeIndex].pt2; |
---|
| 320 | dest.roadIdx = carReceivingMsg.route[h + Globals.routePlanConstants.HOW_MORE_TO_REMOVE].roadIndex; |
---|
| 321 | dest.ptIdx = carReceivingMsg.route[h + Globals.routePlanConstants.HOW_MORE_TO_REMOVE].pt2; |
---|
| 322 | |
---|
| 323 | } |
---|
| 324 | // combinedWriter.println("Incep calcul ruta de evitare cu |
---|
| 325 | // avoid="+source+" pana la "+destination); |
---|
| 326 | |
---|
| 327 | Location parentAvoid = null, nodeAvoid = null; |
---|
| 328 | parentAvoid = new Location(carReceivingMsg.route[h].roadIndex, carReceivingMsg.route[h].pt1); |
---|
| 329 | nodeAvoid = new Location(carReceivingMsg.route[h].roadIndex, carReceivingMsg.route[h].pt2); |
---|
| 330 | // combinedWriter.println("Caut ruta intre " + source + " si " + destination |
---|
| 331 | // + " cu parentAvoid:" + parentAvoid + " si nodeAvoid:" |
---|
| 332 | // + nodeAvoid); |
---|
| 333 | ArrayList<Location> points = carReceivingMsg.routeComputer.shortestDetour(src, |
---|
| 334 | dest, parentAvoid, nodeAvoid); |
---|
| 335 | if (points == null) { |
---|
| 336 | carReceivingMsg.noRouteFailCount++; |
---|
| 337 | // combinedWriter.println("Nu am mai gasit drum :("); |
---|
| 338 | return; |
---|
| 339 | } |
---|
| 340 | this.carReceivingMsg.routeChangesCount++; |
---|
| 341 | points.add(new Location(dest.roadIdx, dest.ptIdx)); |
---|
| 342 | |
---|
| 343 | ArrayList<RouteSegment> ret = RoutingUtils.mergeRoute(points); |
---|
| 344 | |
---|
| 345 | if (ret != null) { |
---|
| 346 | ret = RoutingUtils.splitRoute(ret); |
---|
| 347 | // combinedWriter.println("Found avoid route..inlocuiesc vechia ruta: " |
---|
| 348 | // + carReceivingMsg.route); |
---|
| 349 | int ii = 0; |
---|
| 350 | while (ii < ret.size()) { |
---|
| 351 | if (ret.get(ii).pt1 == ret.get(ii).pt2) { |
---|
| 352 | ret.remove(ii); |
---|
| 353 | } else { |
---|
| 354 | ii++; |
---|
| 355 | } |
---|
| 356 | |
---|
| 357 | } |
---|
| 358 | tpRoute = new ArrayList<RouteSegment>(); |
---|
| 359 | for (i = 0; i <= carReceivingMsg.routeIndex; i++) { |
---|
| 360 | tpRoute.add(carReceivingMsg.route[i]); |
---|
| 361 | } |
---|
| 362 | for (j = 0; j < ret.size(); j++) { |
---|
| 363 | tpRoute.add(ret.get(j)); |
---|
| 364 | } |
---|
| 365 | destIdx = i + j; |
---|
| 366 | |
---|
| 367 | for (i = h + Globals.routePlanConstants.HOW_MORE_TO_REMOVE; i < carReceivingMsg.route.length; i++) { |
---|
| 368 | tpRoute.add(carReceivingMsg.route[i]); |
---|
| 369 | } |
---|
| 370 | |
---|
| 371 | // combinedWriter.println("pctLipitura devine " |
---|
| 372 | // + destIdx); |
---|
| 373 | while (true) { |
---|
| 374 | if (destIdx >= (tpRoute.size() - 1)) { |
---|
| 375 | break; |
---|
| 376 | } |
---|
| 377 | if (tpRoute.get(destIdx).roadIndex == tpRoute |
---|
| 378 | .get(destIdx - 1).roadIndex) { |
---|
| 379 | |
---|
| 380 | if (tpRoute.get(destIdx).pt1 == tpRoute |
---|
| 381 | .get(destIdx - 1).pt2) { |
---|
| 382 | if (tpRoute.get(destIdx).pt2 == tpRoute |
---|
| 383 | .get(destIdx - 1).pt1) { |
---|
| 384 | tpRoute.remove(destIdx - 1); |
---|
| 385 | tpRoute.remove(destIdx - 1); |
---|
| 386 | destIdx--; |
---|
| 387 | } else { |
---|
| 388 | break; |
---|
| 389 | } |
---|
| 390 | } else { |
---|
| 391 | |
---|
| 392 | if ((tpRoute.get(destIdx).pt1 == tpRoute |
---|
| 393 | .get(destIdx - 1).pt1) |
---|
| 394 | && ((tpRoute.get(destIdx).pt2 == tpRoute |
---|
| 395 | .get(destIdx - 1).pt2))) { |
---|
| 396 | tpRoute.remove(destIdx - 1); |
---|
| 397 | destIdx--; |
---|
| 398 | } else { |
---|
| 399 | break; |
---|
| 400 | } |
---|
| 401 | |
---|
| 402 | } |
---|
| 403 | } else { |
---|
| 404 | break; |
---|
| 405 | } |
---|
| 406 | } |
---|
| 407 | |
---|
| 408 | carReceivingMsg.route = new RouteSegment[tpRoute.size()]; |
---|
| 409 | for (i = 0; i < tpRoute.size(); i++) { |
---|
| 410 | carReceivingMsg.route[i] = tpRoute.get(i); |
---|
| 411 | } |
---|
| 412 | |
---|
| 413 | // carReceivingMsg.setRouteFromNowOn(myRoute.route); |
---|
| 414 | combinedWriter.println("ROUTE succesfully changed to:" + tpRoute+" nRchanges are "+this.carReceivingMsg.routeChangesCount); |
---|
| 415 | this.carReceivingMsg.startOnRoad(); |
---|
| 416 | |
---|
| 417 | } else { |
---|
| 418 | this.carReceivingMsg.routeComputingFailCount++; |
---|
| 419 | combinedWriter.println("Could not find a detour"); |
---|
| 420 | } |
---|
| 421 | |
---|
| 422 | } |
---|
| 423 | |
---|
| 424 | } |
---|
| 425 | } |
---|