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 | } |
---|