/************************************************************************************
 * Copyright (C) 2008 by Politehnica University of Bucharest and Rutgers University
 * All rights reserved.
 * Refer to LICENSE for terms and conditions of use.
 ***********************************************************************************/
package vnsim.vehicular.simulator;

import java.io.File;
import java.io.FileOutputStream;
import java.io.PrintWriter;
import java.util.ArrayList;

import vnsim.applications.adaptiveTL.WirelessTrafficLight;
import vnsim.applications.trafficview.SimulatedCarInfo;
import vnsim.core.CarInfo;
import vnsim.core.Engine;
import vnsim.map.object.Globals;
import vnsim.map.object.PeanoKey;
import vnsim.map.object.Point;
import vnsim.map.object.Road;
import vnsim.vehicular.emissions.EmissionsResults;
import vnsim.vehicular.emissions.EmissionsUtil;
import vnsim.vehicular.routePlan.RouteComputingUtils;
import vnsim.vehicular.routePlan.RoutingConstants;
import vnsim.vehicular.simulator.intersections.DirectedRoadSegment;
import vnsim.vehicular.simulator.intersections.Intersection;
import vnsim.vehicular.simulator.intersections.IntersectionWithTrafficLights;
import vnsim.vehicular.simulator.intersections.IntersectionWithoutTrafficLights;

public class CarInstance extends CarInfo implements Comparable<CarInstance> {

	protected SimulatedCarInfo simulatedCarInfo;

	// Routing
	public static PrintWriter logWriter;

	public static RouteComputingUtils routeComputer = new RouteComputingUtils();

	public long timeOnSegment = 0L;

	public double timeOnSegment1 = 0;
	public double distanceOnSegment = 0;

	private double avgSpeed = -1.0;
	private byte avgSpeedStatus = 0;
	private static byte SPEED_RESET = 0;
	private static byte SPEED_COMPUTED = 1;
	private static byte SPEED_DELAY_RESET = 2;
	// nb of cycles that this car has to wait at a traffic light
	private int nTLCycles = 0; // traffic lights need to broadcast
	// PROT_TL_FEEDBACK messages

	public Location source, destination;

	public int currentRoadState = 0;

	public long startTime, stopTime;

	public int toLateFailCount = 0, noRouteFailCount = 0, sentMsgCount = 0, receivedMsgCount = 0, emptyMsgCount = 0,
			inccorectReplyCount = 0, noChangeNeededCount = 0, routeComputingFailCount = 0, routeChangesCount = 0;

	public int routingType; // 0 _ShortestPath without dynamic search

	// 1 _ShortestPath with dynamic search
	// 2 _BestPath without dynamic search
	// 3 _BestPath with dynamic search
	public int queryTime;

	static {
		File log = new File(Globals.routePlanConstants.fisOutCar);
		try {
			log.delete();
			log.createNewFile();
			logWriter = new PrintWriter(new FileOutputStream(log));
		} catch (Exception ex) {
			ex.printStackTrace();
		}
	}

	public int ID; // unique id;

	public int index; // the index of "this" object in the "carsOnThisRoad"
	// vector from the corresponding "Road" object;

	// stare
	// public int roadIndex; //the road it is on
	// public int pointIndex;
	// public double offset; //offset to the point, along the road (km!)
	// //offset>0.0
	// public int direction; //1=along the road; 0=reverse
	// public int lane; //the lane
	//
	public boolean finished = false;
	//
	// public double speed; //km/h

	// personality
	public Personality p;

	// route
	public RouteSegment[] route = null; // the route segments MUST NOT include
	// intersections; if they do, they should first be divided until
	// they include no intersections
	public int routeIndex = -1;

	public void setRoute(java.util.ArrayList<RouteSegment> route) {
		
		if (route == null || route.size() == 0) {
			System.out.println("Error... route is null");
			return;
		}
		// to be called at the beginning
		this.route = new RouteSegment[route.size()];
		for (int i = 0; i < route.size(); i++) {
			this.route[i] = route.get(i);
		}
		routeIndex = 0;

		// also adjust the direction
		RouteSegment first = route.get(0);
		if (first.roadIndex != roadIdx) {
			// System.out.println("ERROR! First route segment is not on current
			// road!");
			// the vehicle was probably placed in an intersection and the first
			// route segment is not on its current road; adjust the current road
			this.roadIdx = first.roadIndex;
			this.pointIdx = first.pt1;
			if (first.pt2 < pointIdx)
				direction = 0;
			else
				direction = 1;
			return;
		}
		if (first.pt2 < pointIdx)
			direction = 0;
		else
			direction = 1;
	}

	// public void setRoute(RouteSegment[] route) {
	// this.route=route;
	// routeIndex=0;
	// }

	// values obtained from the "Personality" object
	private double accelFreeDriving;
	private double wantedSpeed;

	private double maxWantedSpeed;

	private boolean havePriority = false;
	public Intersection intersection; // the following intersection

	private double inflDist;
	private double inflDistLaneChange;

	private double maxSpeed;
	private int nextIntersectionPointIndex; // the point index (on "roadIndex"
	// road)
	// where the next intersection is

	private boolean wasApproaching = false;
	private int nIterationsApproaching = 0;

	private boolean influenced = false;
	public boolean influencedLaneChange = false;

	private ArrayList<Byte> desiredLanes; // which are the lanes on which
	// the vehicle must be before the end of its current route segment?
	public boolean wellPositioned = false; // am I well positioned for the
	// turn?

	// only valid if the intersection ahead has no traffic lights
	private ArrayList<DirectedRoadSegment> yieldFor;

	public boolean influencedByTrafficLight = false;
	private long timeStartInfluenced = 0;
	private long timeExpectedInfluenced = 0;
	private long timeRealInfluenced = 0;
	public int controlDelay;
	public int totalControlDelay = 0;
	public boolean startMonitor = false;
	public boolean endMonitor = false;
	public double startMonitorDistance;
	public Intersection lastIntersection;
	public int lastIntersectionPointIndex;
	double previousSpeed = 0.0;

	public EmissionsResults em = new EmissionsResults();
	public boolean turningLeft = false;

	public boolean demanded = false;
	public boolean queued = false;

	// get the average speed on the segment
	// return -1 if avgSpeed is not computed or if this function is
	// called more than kMaxAttempts after the avgSpeed is computed
	public double getAvgSpeed() {
		if (avgSpeedStatus != SPEED_DELAY_RESET) {
			return -1.0;
		}
		avgSpeedStatus = SPEED_RESET;
		return avgSpeed;
	}

	// get the nb of red Cycles this car has waited at a trafficLight
	public int getTLCycles() {
		int nTL = nTLCycles;
		nTLCycles = 0;
		return nTL;
	}

	// get the segmentIndex on which avgSpeed was computed
	public int getSegmentIndex() 
	{
		Intersection it;
		RouteSegment rs;
		if (avgSpeedStatus == SPEED_DELAY_RESET) {
			// car has already changed the segment
			it = lastIntersection;
			rs = route[routeIndex - 1];
		} else {
			it = intersection;
			rs = route[routeIndex];
		}
		int index = -1;
		DirectedRoadSegment drs;
		for (int i = 0; i < it.segments.size(); i++) {
			drs = it.segments.get(i);
			if (drs.roadIndex == rs.roadIndex) {
				if (drs.direction && drs.pointIndex == rs.pt1) {
					index = i;
					break;
				}
				if (!drs.direction && drs.pointIndex == rs.pt2) {
					index = i;
					break;
				}
			}
		}
		return index;
	}

	public void startOnRoad() {
		// method to be called once, just after the vehicle gets on a new road

		canSignal = false;
		turningLeft = false;
		demanded = false;
		queued = false;

		if (Engine.simulationType == RoutingConstants.DYNAMIC_CITY_ROUTE) {
			if (avgSpeedStatus == SPEED_RESET && timeOnSegment1 != 0) {
				avgSpeed = distanceOnSegment * 3600 / timeOnSegment1;
				avgSpeedStatus = SPEED_DELAY_RESET;
			}
		}
		distanceOnSegment = 0;
		timeOnSegment1 = 0;

		if (intersection != null) {
			lastIntersection = intersection;
			lastIntersectionPointIndex = pointIdx;
			if (Globals.monitorWTL != null && intersection.equals(Globals.monitorWTL)) {
				Globals.volume++;
			}

			if (Globals.engine.startedWithGUI && Globals.demo.mv.currentCar != null
					&& simulatedCarInfo.equals(Globals.demo.mv.currentCar)) {
				Globals.demo.st.setSemTime(-1, -1);
			}
		}
		if (influencedByTrafficLight) {
			influencedByTrafficLight = false;
			timeRealInfluenced = (Globals.engine.crtTime - timeStartInfluenced) / Globals.executionFPS;
			controlDelay = (int) (timeRealInfluenced - timeExpectedInfluenced);
			totalControlDelay += controlDelay;
			// System.out.println("ACTUAL TIME:"+timeRealInfluenced+" [secs];
			// EXPECTED="+timeExpectedInfluenced+" [secs];
			// DELAY="+(timeRealInfluenced-timeExpectedInfluenced));
		}

		accelFreeDriving = p.getAccelerationFreeDriving();
		maxWantedSpeed = wantedSpeed = p.getWantedSpeed(roadIdx);
		influenced = false;
		influencedLaneChange = false;
		wellPositioned = false;

		// obtain the intersection object corresponding to the next intersection
		// it has to pass through; also set "nextIntersectionPointIndex"
		// System.out.println("Setting intersection!Pt="+pointIdx);

		if (route != null && routeIndex == route.length - 1) {
			// I am entering the last route segment
			// Simulate a special type of intersection, where I must stop
			// (it's the final destination of this vehicle)

			intersection = new IntersectionWithoutTrafficLights();
			DirectedRoadSegment drs = null;
			if (route[routeIndex].pt1 > route[routeIndex].pt2) {
				drs = new DirectedRoadSegment(roadIdx, route[routeIndex].pt2, true);
			} else {
				drs = new DirectedRoadSegment(roadIdx, route[routeIndex].pt2, false);
			}
			intersection.addCrossingSegment(drs);
			intersection.addCrossingSegment(drs); // no matter
			((IntersectionWithoutTrafficLights) intersection).mustStop = new java.util.ArrayList<Boolean>();
			// ((IntersectionWithoutTrafficLights)intersection).mustStop.add(new
			// Boolean(true));
			// ((IntersectionWithoutTrafficLights)intersection).mustStop.add(new
			// Boolean(true));
			((IntersectionWithoutTrafficLights) intersection).mustStop.add(new Boolean(false));
			((IntersectionWithoutTrafficLights) intersection).mustStop.add(new Boolean(false));

			nextIntersectionPointIndex = route[routeIndex].pt2;
			maxSpeed = 50.0;
			inflDist = 0.100;
			inflDistLaneChange = 0.200;
			yieldFor = new ArrayList<DirectedRoadSegment>();

			desiredLanes = new ArrayList<Byte>();
			for (int i = 0; i < Globals.map.roads.get(route[routeIndex].roadIndex).laneNo; i++) {
				desiredLanes.add((byte) (i + 1));
			}
			if (lane > Globals.map.roads.get(roadIdx).laneNo) {
				lane = (byte) Globals.map.roads.get(roadIdx).laneNo;
			}
			return;
		}

		Road r = Globals.map.roads.get(roadIdx);

		if (direction == 1) {
			for (int i = 0; i < r.crosses.size(); i++) {
				if (r.crosses.get(i).getPointIndex() > pointIdx) {
					// that's the next cross
					intersection = Globals.map.allIntersections.get(r.crosses.get(i).intersectionIndex);
					nextIntersectionPointIndex = r.crosses.get(i).getPointIndex();
					break;
				}
			}
		} else {
			for (int i = r.crosses.size() - 1; i >= 0; i--) {
				if (r.crosses.get(i).getPointIndex() < pointIdx) {
					// that's the next cross
					intersection = Globals.map.allIntersections.get(r.crosses.get(i).intersectionIndex);
					nextIntersectionPointIndex = r.crosses.get(i).getPointIndex();
					// System.out.println("FOUND!");
					break;
				}
			}
		}
		inflDist = intersection.getInfluenceDistance(route[routeIndex], route[routeIndex + 1]);
		inflDistLaneChange = intersection.getInfluenceDistanceLaneChange(route[routeIndex], route[routeIndex + 1]);
		maxSpeed = intersection.getMaximumSpeed(route[routeIndex], route[routeIndex + 1]);
		havePriority = intersection.havePriority(route[routeIndex], route[routeIndex + 1]);

		desiredLanes = intersection.getDesiredLanes(route[routeIndex], route[routeIndex + 1]);

		if (!intersection.hasTrafficLights()) {
			yieldFor = intersection.yieldFor(route[routeIndex], route[routeIndex + 1]);
		}
		if (lane > Globals.map.roads.get(roadIdx).laneNo) {
			lane = (byte) Globals.map.roads.get(roadIdx).laneNo;
		}
		if (intersection.hasTrafficLights()) {
			IntersectionWithTrafficLights iwtl = (IntersectionWithTrafficLights) intersection;
			turningLeft = iwtl.isLeftTurn(route[routeIndex], route[routeIndex + 1]);
		}

	}

	public void setIntersection() {
		if (routeIndex == route.length - 1) {
			return;

		}
		Road r = Globals.map.roads.get(roadIdx);

		if (direction == 1) {
			for (int i = 0; i < r.crosses.size(); i++) {
				if (r.crosses.get(i).getPointIndex() > pointIdx) {
					// that's the next cross
					intersection = Globals.map.allIntersections.get(r.crosses.get(i).intersectionIndex);
					nextIntersectionPointIndex = r.crosses.get(i).getPointIndex();
					break;
				}
			}
		} else {
			for (int i = r.crosses.size() - 1; i >= 0; i--) {
				if (r.crosses.get(i).getPointIndex() < pointIdx) {
					// that's the next cross
					intersection = Globals.map.allIntersections.get(r.crosses.get(i).intersectionIndex);
					nextIntersectionPointIndex = r.crosses.get(i).getPointIndex();
					// System.out.println("FOUND!");
					break;
				}
			}
		}

		// System.out.println("carReceivingMsg "+this.ID+" got route ");
		// for(int i=0;i<route.length;i++){
		// System.out.print(" "+route[i]);
		// }

		inflDist = intersection.getInfluenceDistance(route[routeIndex], route[routeIndex + 1]);
		inflDistLaneChange = intersection.getInfluenceDistanceLaneChange(route[routeIndex], route[routeIndex + 1]);
		maxSpeed = intersection.getMaximumSpeed(route[routeIndex], route[routeIndex + 1]);
		havePriority = intersection.havePriority(route[routeIndex], route[routeIndex + 1]);

		desiredLanes = intersection.getDesiredLanes(route[routeIndex], route[routeIndex + 1]);

		if (!intersection.hasTrafficLights())
			yieldFor = intersection.yieldFor(route[routeIndex], route[routeIndex + 1]);
		if (lane > Globals.map.roads.get(roadIdx).laneNo) {
			lane = (byte) Globals.map.roads.get(roadIdx).laneNo;
		}

		// System.out.println("carReceivingMsg "+this.ID+" after staff ");
		// for(int i=0;i<route.length;i++){
		// System.out.print(" "+route[i]);
		// }

	}

	public double accel = 0;
	public boolean canSignal = false;
	// public double speed1s = 0;

	int lastInfoTime = -1;
	public int travelTime = 0;

	public void move() {
		if (finished) {
			// System.out.println("Car finished!");
			return;
		}

		if (!demanded && intersection != null && Globals.monitorWTL != null && intersection.equals(Globals.monitorWTL)
				&& (influenced || influencedByTrafficLight)) {
			Globals.demand++;
			demanded = true;
		}
		// compute emissions!
		vnsim.vehicular.emissions.EmissionsResults emAux = null;
		double time22 = 0;
		accel = 0;

		// if (Globals.engine.crtTime % Globals.SECOND == 0){
		// if (Globals.engine.crtTime < Globals.SECOND )
		// speed1s = speed;
		// accel1s = (speed - speed1s) * (3600.0/time22);
		// speed1s = speed;
		// }

		time22 = (1.0 / Globals.FPS);
		accel = (speed - previousSpeed) * (3600.0 / time22);
		emAux = EmissionsUtil.estimateFuelConsumption((speed + previousSpeed) / 2, accel, time22);

		if (startMonitor) {
			em.addEmissionsResults(emAux, 1);
		}
		travelTime += Globals.FPS;
		timeOnSegment1 += time22;

		if (Globals.engine.startedWithGUI && Globals.demo.mv.currentCar != null
				&& simulatedCarInfo.equals(Globals.demo.mv.currentCar) && Globals.engine.crtTime % Globals.SECOND == 0) {
			Globals.demo.st.setDemand("ne", (int) this.speed, 0, false);
			// System.out.println("move t:" + (Globals.engine.crtTime /
			// Globals.SECOND) +" speed: " + (speed+previousSpeed)/2 +" acc: " +
			// accel / (3.6 * 3600) +" fuel: " + (emAux.fc * (3600.0/time22) /
			// 1000 ) );
			// System.out.println((Globals.engine.crtTime / Globals.SECOND) +" "
			// + (speed+previousSpeed)/2 +" " + accel / (3.6 * 3600) +" " +
			// (emAux.fc * (3600.0/time22) / 1000 ) );
		}

		if (lastIntersection != null && Globals.monitorWTL != null && lastIntersection.equals(Globals.monitorWTL)) {
			double d1 = Globals.map.roads.get(roadIdx).points.get(lastIntersectionPointIndex).getDistance();
			double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
			double dif = d2 - d1;
			if (d1 > d2)
				dif = d1 - d2;
			if (dif > 0.5 && !endMonitor && startMonitor) {
				endMonitor = true;
			}
		}

		// am I influenced by the next intersection?
		if (intersection != null) {
			double d1 = Globals.map.roads.get(roadIdx).points.get(nextIntersectionPointIndex).getDistance();
			double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
			double dif;
			if (d1 > d2)
				dif = d1 - d2;
			else
				dif = d2 - d1;

			if (dif < 0.2) {
				canSignal = true;
			}
			if (!startMonitor && Globals.monitorWTL != null && intersection.equals(Globals.monitorWTL) && dif < 0.5) {
				startMonitor = true;
				startMonitorDistance = totalDistance;
			}

			if (!queued && intersection != null && Globals.monitorWTL != null
					&& intersection.equals(Globals.monitorWTL) && (influenced || influencedByTrafficLight)
					&& speed < WirelessTrafficLight.LOW_SPEED) {

				queued = true;
			}

			// int toEndNextGreen=-1;
			// if(simulatedCarInfo.currentPhase==Globals.GREEN) {
			// toEndNextGreen=simulatedCarInfo.phaseRemainingTime;
			// } else {
			// toEndNextGreen=simulatedCarInfo.phaseRemainingTime+
			//simulatedCarInfo.cycleLength-simulatedCarInfo.interGreenPhaseTime;
			// }

			if (simulatedCarInfo.infoTime != lastInfoTime && simulatedCarInfo.currentPhase != -1
					&& simulatedCarInfo.intersection != null && intersection.equals(simulatedCarInfo.intersection)
					&& intersection.hasTrafficLights()) {
				lastInfoTime = simulatedCarInfo.infoTime;
				int dischargeTime = (int) (simulatedCarInfo.queueSize * (Globals.CARSPERKM) / (Globals.QUEUEDISCHARGEFLOW));

				int greenTime = simulatedCarInfo.cycleLength - simulatedCarInfo.interGreenPhaseTime;

				if (Globals.engine.startedWithGUI && Globals.demo.mv.currentCar != null
						&& simulatedCarInfo.equals(Globals.demo.mv.currentCar)) {
					// System.out.println();
				}
				if (simulatedCarInfo.currentPhase == Globals.GREEN) {
					int nCars = (int) (simulatedCarInfo.phaseRemainingTime * Globals.QUEUEDISCHARGEFLOW);
					if ((double) nCars / Globals.CARSPERKM > simulatedCarInfo.queueSize) {
						// pot trece pe verdele curent

					} else {
						dischargeTime -= simulatedCarInfo.phaseRemainingTime;
						if (dischargeTime < 0)
							dischargeTime = 1;
					}
				}

				if (dischargeTime < 0)
					dischargeTime = 0;
				double nCycles = (double) dischargeTime / greenTime;

				if (Globals.engine.startedWithGUI && Globals.demo.mv.currentCar != null
						&& simulatedCarInfo.equals(Globals.demo.mv.currentCar)) {
					// System.out.println("NCYCLES=" + nCycles);
					// System.out.println("DT =" + dischargeTime);
				}

				int nIntCycles = (int) Math.ceil(nCycles);
				// set the trafficlight cycles this car has to wait
				if (avgSpeedStatus == SPEED_COMPUTED && nTLCycles < nIntCycles) {
					nTLCycles = nIntCycles;
				}
				int toEndNextGreen = -1;
				if (simulatedCarInfo.currentPhase == Globals.RED) {
					toEndNextGreen = nIntCycles * simulatedCarInfo.cycleLength + simulatedCarInfo.phaseRemainingTime
							+ greenTime;
				} else {
					toEndNextGreen = simulatedCarInfo.phaseRemainingTime + nIntCycles * simulatedCarInfo.cycleLength;
				}

				// int secsToIntersection=(int) (((double)dif /
				// (double)p.getWantedSpeed(roadIdx)) * 3600);

				double fracGreen = (nIntCycles - nCycles) * greenTime;

				int timeToArrive = (int) (toEndNextGreen - fracGreen);

				double suggestedSpeed = dif * 3600 / (double) timeToArrive;

				if (suggestedSpeed < 10) {
					suggestedSpeed = 10;
				}
				if (Globals.engine.startedWithGUI && Globals.demo.mv.currentCar != null
						&& simulatedCarInfo.equals(Globals.demo.mv.currentCar)) {
					// System.out.println("MAXSSUGGESTED = " + suggestedSpeed+"
					// ; MAX = "+p.getWantedSpeed(roadIdx));
					Globals.demo.st.setDemand("nw", (int) suggestedSpeed, 0, false);
				}

				// if(suggestedSpeed < p.getWantedSpeed(roadIdx)) {
				// wantedSpeed=suggestedSpeed;
				// System.out.println("SET TO:"+maxSpeed);
				// }

			}
			// if(secsToIntersection > toEndNextGreen) {
			// int
			// toEndAfterNextGreen=toEndNextGreen+simulatedCarInfo.cycleLength;
			// if(secsToIntersection > toEndAfterNextGreen) {
			// //compute speed
			//			
			// //foarte mica
			//			
			// } else {
			//			
			//			
			// }
			// } else {
			// maxSpeed=p.getWantedSpeed(roadIdx);
			// }

			if (dif < inflDist) {

				wantedSpeed = p.getWantedSpeed(roadIdx);

				// if I dont't have priority, check if I will be able to pass
				influenced = true;
				if (!intersection.hasTrafficLights()) {
					boolean canPass = true;
					if (routeIndex == route.length - 1) {
						// I am on the last segment
						canPass = true;
					} else {
						if (havePriority) {
							canPass = true;
						} else {
							if (yieldFor != null) {
								for (int i = 0; i < yieldFor.size(); i++) {
									// look on that segment if there are close
									// vehicles
									CarInstance c2 = getFirstApproachingVehicle(yieldFor.get(i));

									try {
										if (intersection.isScissors(route[routeIndex], route[routeIndex + 1],
												c2.route[c2.routeIndex], c2.route[c2.routeIndex + 1]))
											continue;
									} catch (Exception ex) {
										// ex.printStackTrace();
									}
									// how far is it?
									if (c2 == null) {
										continue;
									}
									double do1 = Globals.map.roads.get(c2.roadIdx).points.get(c2.pointIdx)
											.getDistance()
											+ offset;
									double do2 = Globals.map.roads.get(yieldFor.get(i).roadIndex).points.get(
											yieldFor.get(i).pointIndex).getDistance();
									double dist = 0.0;
									if (do1 - do2 > 0)
										dist = do1 - do2;
									else
										dist = do2 - do1;

									// dist=distance of other vehicle (c2) from
									// intersection
									// dif=my distance from intersection

									// dif, dist = [km]
									if (!p.estimateRoadChangePossible(speed, c2.speed, dif, dist))
										canPass = false;
								}
							}
						}
					}
					if (!canPass) {
						// try to get to a stop 10 meters before
						wantedSpeed = p.getWantedSpeedInfluenced(dif - 0.01, 0.0);
						// the intersection

						// if(ID==12)
						// System.out.println("I CANNOT PASS! MAX="+maxSpeed+" ;
						// DIF="+dif+" ; WS="+wantedSpeed);
					} else {
						if (maxSpeed != -1.0) {
							// -1.0 means there is no speed reducing necessary
							wantedSpeed = p.getWantedSpeedInfluenced(dif, maxSpeed);
							// if(ID==12)
							// System.out.println("I CAN PASS! MAX="+maxSpeed+"
							// ; DIF="+dif+" ; WS="+wantedSpeed);
						}
					}
					if (wantedSpeed > maxWantedSpeed)
						wantedSpeed = maxWantedSpeed;
					// System.out.println("NO LIGHTS : MODIFY WS="+wantedSpeed);
				} else {
					// there are traffic lights
					IntersectionWithTrafficLights iwtl = (IntersectionWithTrafficLights) intersection;
					if (iwtl.isGreen(route[routeIndex])) {

						boolean canPass = true;
						ArrayList<DirectedRoadSegment> yieldFor = iwtl.yieldFor(route[routeIndex],
								route[routeIndex + 1]);
						for (int i = 0; i < yieldFor.size(); i++) {
							// look on that segment if there are close vehicles
							CarInstance c2 = getFirstApproachingVehicle(yieldFor.get(i));

							try {
								if (intersection.isScissors(route[routeIndex], route[routeIndex + 1],
										c2.route[c2.routeIndex], c2.route[c2.routeIndex + 1]))
									continue;
							} catch (Exception ex) {
								// ex.printStackTrace();
							}

							// how far is it?
							if (c2 == null) {
								canPass = true;
								continue;
							}
							double do1 = Globals.map.roads.get(c2.roadIdx).points.get(c2.pointIdx).getDistance()
									+ offset;
							double do2 = Globals.map.roads.get(yieldFor.get(i).roadIndex).points.get(
									yieldFor.get(i).pointIndex).getDistance();
							double dist = 0.0;
							if (do1 - do2 > 0)
								dist = do1 - do2;
							else
								dist = do2 - do1;

							// dif, dist = [km]
							if (!p.estimateRoadChangePossible(speed, c2.speed, dif, dist))
								canPass = false;
						}

						if (!canPass) {
							wantedSpeed = p.getWantedSpeedInfluenced(dif - 0.003, 0.0); // try
							// to
							// get
							// to
							// a
							// stop
							// 3 meters before the
							// intersection
						} else {
							if (maxSpeed != -1.0) {
								// -1.0 means there is no speed reducing
								// necessary
								wantedSpeed = p.getWantedSpeedInfluenced(dif, maxSpeed);
							}
						}
					} else {
						if (dif < 0.010 && iwtl.isYellow(route[routeIndex])) { // closer
							// than
							// 10
							// meters;
							// go
							// ahead
							// if(ID==57)
							// System.out.println("ELSE - IF1");
							wantedSpeed = p.getWantedSpeedInfluenced(dif, maxSpeed);
						} else {

							if (dif < 0.006) {
								// go ahead! no matter the color
								wantedSpeed = p.getWantedSpeedInfluenced(dif, maxSpeed);
							} else {
								if (!influencedByTrafficLight) {
									influencedByTrafficLight = true;
									timeStartInfluenced = Globals.engine.crtTime;
									// also compute expected time!
									double time = (dif / wantedSpeed) * 3600; // secs
									timeExpectedInfluenced = (long) time;
								}
								wantedSpeed = p.getWantedSpeedInfluenced(dif - 0.008, 0.0); // try
								// to
								// get
								// to
								// a
								// stop 5 meters
								// before the
								// intersection
							}
						}
					}
					if (wantedSpeed > maxWantedSpeed)
						wantedSpeed = maxWantedSpeed;

					// if(ID==57)
					// System.out.println("DIF= "+dif+" THIS="+this+"WITH LIGHTS
					// : MODIFY WS="+wantedSpeed);
				}
			}

			if (dif < inflDistLaneChange || influencedByTrafficLight) {
				influencedLaneChange = true;
				// try to change lane;

				if (!wellPositioned && dif < 0.005) {
					desiredLanes.add((byte) lane);
					wellPositioned = true;
				}

				if (!wellPositioned) {
					if (desiredLanes.contains(lane)) {
						wellPositioned = true;
					} else {
						int first = desiredLanes.get(0);
						if (lane < first) {
							// should go to upper
							if (canSafelyChangeUpperLane()) {
								lane++;
							} else {
								// should brake (try to get to a stop 20 meters
								// before the intersection)
								wantedSpeed = p.getWantedSpeedInfluenced(dif - 0.020, maxSpeed);
							}
						} else {
							// should go to lower
							if (canSafelyChangeLowerLane()) {
								lane--;
							} else {
								// should brake (try to get to a stop 20 meters
								// before the intersection)
								wantedSpeed = p.getWantedSpeedInfluenced(dif - 0.020, maxSpeed);
							}
						}
						if (wantedSpeed > maxWantedSpeed)
							wantedSpeed = maxWantedSpeed;
					}
				}
			}
		}

		// System.out.println("Calling getDistance");
		CarInstance c = getPrecedingCarOnLane(this.lane);

		// System.out.println("Returned:"+c);
		double distance = 0.0;
		if (c != null) {
			distance = getDistance(c);
			// System.out.println("DISTANCE="+distance);
		}

		// if(ID==12) {
		// System.out.println("THIS="+this+" ; PREC="+c+" DISTANCE="+distance);
		// }

		if (c == null || distance >= p.getInfluenceDistance(speed)) {
			// free driving;
			// can change lane to a lower lane and still be in free driving?

			// System.out.println("I AM:"+this+" ; FREE DRIVING; CAN I CHANGE
			// LANE LOWER?");
			// System.out.println("I AM "+ID+" DISTANCE="+distance);
			// System.out.println("I AM "+ID+"; FREE DRIVING ;
			// PT="+pointIdx+"LANE="+lane);

			// if(!influencedLaneChange && checkFreeDrivingLowerLane()) {
			if (((desiredLanes.contains((byte) (lane - 1))) || (!influencedLaneChange)) && checkFreeDrivingLowerLane()) {
				lane--;
			}
			previousSpeed = speed;
			computeSpeedFreeDriving();
			changePosition();
			return;
		}
		if (distance < p.getInfluenceDistance(speed) && distance >= p.getSafetyDistance(speed)) {
			// depends on the speed
			if (c.speed > this.speed) {
				if (wasApproaching) {
					nIterationsApproaching++;
					if (nIterationsApproaching < p.getReactionTimeFreeDriving() * Globals.FPS) {
						previousSpeed = speed;
						changePosition(); // no speed change
						return;
					}
					wasApproaching = false;
					nIterationsApproaching = 0;
				}

				// System.out.println("I AM "+ID+"; FREE DRIVING2 ;
				// LANE="+lane);
				// System.out.println("I AM:"+this+" ; FREE DRIVING2; CAN I
				// CHANGE LANE LOWER?");

				// free driving
				if ((!influencedLaneChange || (desiredLanes.contains((byte) (lane - 1))))
						&& checkFreeDrivingLowerLane()) {
					lane--;
				}
				previousSpeed = speed;
				computeSpeedFreeDriving();
				changePosition();
				return;
			}
			if (c.speed == this.speed) {
				// System.out.println("I AM "+ID+"; FOLLOWING ;
				// PT="+pointIdx+"LANE="+lane);
				if (c.speed == 0.0) {
					// practically stopped behind another stopped vehicle;
					// it's safe to get a little closer
					previousSpeed = speed;
					computeSpeedFreeDriving();
					changePosition();
					return;
				}
				// following; no change of speed
				if ((!influencedLaneChange || (desiredLanes.contains((byte) (lane + 1))))
						&& checkFreeDrivingUpperLane()) {
					lane++;
					previousSpeed = speed;
					computeSpeedFreeDriving();
				} else {
					previousSpeed = speed;
				}
				changePosition();
				return;
			}
			if (c.speed < this.speed) {
				// System.out.println("I AM "+ID+"; INFLUENCED ;
				// PT="+pointIdx+"LANE="+lane);
				// should brake; first check if it can change lane

				// System.out.println("I AM:"+this+" ; INFLUENCED; SHOULD I GO
				// TO UPPER?");

				if ((!influencedLaneChange || (desiredLanes.contains((byte) (lane + 1))))
						&& checkFreeDrivingUpperLane()) {
					// System.out.println("YES!");
					lane++;
					previousSpeed = speed;
					computeSpeedFreeDriving();
					changePosition();
					return;
				}
				// System.out.println("I AM "+ID+"; APPROACHING ;
				// PT="+pointIdx+"; LANE="+lane);

				if (!wasApproaching) {
					wasApproaching = true;
					nIterationsApproaching = 0;
				}

				if (!influencedByTrafficLight) {
					if (c.influencedByTrafficLight) {
						becomeInfluencedByTrafficLight();
						timeStartInfluenced = Globals.engine.crtTime;

						// also compute expected time!
						if (intersection != null) {
							double d1 = Globals.map.roads.get(roadIdx).points.get(nextIntersectionPointIndex)
									.getDistance();
							double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
							double dif;
							if (d1 > d2)
								dif = d1 - d2;
							else
								dif = d2 - d1;
							double time = (dif / wantedSpeed) * 3600; // secs
							timeExpectedInfluenced = (long) time;
						}
					}
				}

				previousSpeed = speed;
				computeSpeedApproaching(c.speed, distance);
				changePosition();
				return;
			}
		}

		if (distance < p.getSafetyDistance(speed)) {
			if (c.speed > speed) {
				// no need to brake, but do not accelerate either
				previousSpeed = speed;
				changePosition();
				return;
			}

			// System.out.println("I AM "+ID+" MAXIMUM BRAKE !");
			// first check if it can change lane
			if ((!influencedLaneChange || (desiredLanes.contains((byte) (lane + 1)))) && checkFreeDrivingUpperLane()) {
				// System.out.println("YES!");
				lane++;
				previousSpeed = speed;
				computeSpeedFreeDriving();
				changePosition();
				return;
			}
			if ((!influencedLaneChange || (desiredLanes.contains((byte) (lane + 1)))) && checkApproachingUpperLane()) {
				lane++;
				CarInstance c2 = getPrecedingCarOnLane(this.lane);
				// System.out.println("Returned:"+c);
				double distance2 = 0.0;
				if (c2 != null) {
					// System.out.println("Calling getDistance");
					distance2 = getDistance(c2);
					// System.out.println("DISTANCE="+distance);

					if (!influencedByTrafficLight) {
						if (c2.influencedByTrafficLight) {
							becomeInfluencedByTrafficLight();
							timeStartInfluenced = Globals.engine.crtTime;

							// also compute expected time!
							if (intersection != null) {
								double d1 = Globals.map.roads.get(roadIdx).points.get(nextIntersectionPointIndex)
										.getDistance();
								double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
								double dif;
								if (d1 > d2)
									dif = d1 - d2;
								else
									dif = d2 - d1;
								double time = (dif / wantedSpeed) * 3600; // secs
								timeExpectedInfluenced = (long) time;
							}
						}
					}

					previousSpeed = speed;
					computeSpeedApproaching(c2.speed, distance2);
					changePosition();
					return;
				}
				previousSpeed = speed;
				computeSpeedFreeDriving();
				changePosition();
				return;
			}
			// maximum brake
			// System.out.println("I AM "+ID+"; MAXIMUM BRAKE ; LANE="+lane);
			previousSpeed = speed;
			computeSpeedMaximumBrake();
			changePosition();
		}
	}

	public boolean canSafelyChangeLowerLane() {
		if (lane == 1)
			return false;

		CarInstance c = getFollowingCarOnLane(lane - 1);
		CarInstance c2 = getPrecedingCarOnLane(lane - 1);

		double distance1, otherSpeed;
		if (c == null) {
			distance1 = Double.MAX_VALUE;
			otherSpeed = 0.0;
		} else {
			distance1 = getDistance(c);
			otherSpeed = c.speed;
		}

		double distance2, otherSpeed2;
		if (c2 == null) {
			distance2 = Double.MAX_VALUE;
			otherSpeed2 = 0.0;
		} else {
			distance2 = getDistance(c2);
			otherSpeed2 = c2.speed;
		}

		if (distance1 > p.getLaneChangeSafetyDistance(speed, otherSpeed)
				&& distance2 > p.getLaneChangeSafetyDistanceInFront(speed, otherSpeed2))
			return true;
		return false;
	}

	public boolean canSafelyChangeUpperLane() {
		if (lane >= Globals.map.roads.get(roadIdx).laneNo)
			return false;
		CarInstance c = getFollowingCarOnLane(lane + 1);
		CarInstance c2 = getPrecedingCarOnLane(lane + 1);

		double distance1, otherSpeed;
		if (c == null) {
			distance1 = Double.MAX_VALUE;
			otherSpeed = 0.0;
		} else {
			distance1 = getDistance(c);
			otherSpeed = c.speed;
		}

		double distance2, otherSpeed2;
		if (c2 == null) {
			distance2 = Double.MAX_VALUE;
			otherSpeed2 = 0.0;
		} else {
			distance2 = getDistance(c2);
			otherSpeed2 = c2.speed;
		}
		if (distance1 > p.getLaneChangeSafetyDistance(speed, otherSpeed)
				&& distance2 > p.getLaneChangeSafetyDistanceInFront(speed, otherSpeed2)) {
			return true;
		}
		return false;
	}

	public boolean checkFreeDrivingLowerLane() {
		if (!canSafelyChangeLowerLane())
			return false;
		CarInstance c = getPrecedingCarOnLane(this.lane - 1);
		double distance = 0.0;
		if (c != null) {
			distance = getDistance(c);
		}
		if (c == null || distance > p.getInfluenceDistance(speed)) {
			// can change lane and still be in free driving
			// System.out.println("Returning true, because
			// c="+c+"distance="+distance+">"+p.getInfluenceDistance(speed));

			return true;
		}
		if (distance < p.getInfluenceDistance(speed) && distance >= p.getSafetyDistance(speed)) {
			if (c.speed > this.speed) {
				// can change lane and still be in free driving
				return true;
			}
		}
		return false;
	}

	public boolean checkFreeDrivingUpperLane() {
		if (!canSafelyChangeUpperLane())
			return false;
		CarInstance c = getPrecedingCarOnLane(this.lane + 1);
		double distance = 0.0;
		if (c != null) {
			distance = getDistance(c);
		}
		if (c == null || distance > p.getInfluenceDistance(speed)) {
			// can change lane and still be in free driving
			return true;
		}
		if (distance < p.getInfluenceDistance(speed) && distance >= p.getSafetyDistance(speed)) {
			if (c.speed > this.speed) {
				// can change lane and still be in free driving
				return true;
			}
		}
		return false;
	}

	public boolean checkApproachingUpperLane() {
		if (!canSafelyChangeUpperLane())
			return false;
		CarInstance c = getPrecedingCarOnLane(this.lane + 1);
		double distance = 0.0;
		if (c != null) {
			distance = getDistance(c);
		}
		if (c == null || distance > p.getInfluenceDistance(speed)) {
			// can change lane and be in free driving
			return true;
		}
		if (distance < p.getInfluenceDistance(speed) && distance >= p.getSafetyDistance(speed)) {
			return true; // can change lane and be approaching or in free
			// driving
		}
		return false;
	}

	public void computeSpeedFreeDriving() {
		if (speed < wantedSpeed) {
			speed = speed + accelFreeDriving / (double) Globals.FPS;
		}
		if (speed > wantedSpeed)
			speed = wantedSpeed;
	}

	public void computeSpeedApproaching(double otherCarSpeed, double otherCarDistance) {
		// brake so that "desiredDistance" is obtained when the
		// speed reaches the speed of the preceding vehicle

		// System.out.println("DISTANCE="+otherCarDistance+";
		// DESIRED="+p.getDesiredDistance(otherCarSpeed));

		double accel = -(this.speed - otherCarSpeed) * (this.speed - otherCarSpeed)
				/ (2 * (otherCarDistance - p.getDesiredDistance(otherCarSpeed)));

		accel /= 2.0; // correction

		if (accel < 0) {
			// accel should be negative
			// System.out.println("ACCEL="+accel);
			speed = speed + accel / (double) Globals.FPS;
		} else {
			// maximum brake (I am already closer than "desiredDistance"
			// System.out.println("Maximum brake!");
			computeSpeedMaximumBrake();
		}
		if (speed > wantedSpeed)
			speed = wantedSpeed;
	}

	public void computeSpeedMaximumBrake() {
		double accel = p.getAccelerationMaximumBrake();
		speed = speed + accel / (double) Globals.FPS;
	}

	public double totalDistance = 0.0; // [km]

	public void changePosition() {
		// change position according to "speed"

		if (speed < 0) {
			speed = 0.0;
			return;
		}
		if (avgSpeedStatus == SPEED_DELAY_RESET) {
			avgSpeedStatus = SPEED_RESET;
			nTLCycles = 0;
		}

		double distance = speed / 3600.0 * (1.0 / Globals.FPS); // km!

		distanceOnSegment += distance;
		totalDistance += distance;
		// System.out.println("I AM "+ID+";PT="+pointIdx+"SPEED="+speed+";
		// INTERS
		// AT="+nextIntersectionPointIndex+"DIR="+direction+";OFFSET="+offset);

		if (direction == 1) {
			double newOffset = offset + distance;
			if (pointIdx >= Globals.map.roads.get(roadIdx).points.size() - 1) {
				// last point on road + offset = error
				System.out.println("ERROR 1 ! changePosition()");
				return;
			}
			double gapBetweenPoints = Globals.map.roads.get(roadIdx).points.get(pointIdx + 1).getDistance()
					- Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance();
			// [km]
			if (newOffset > gapBetweenPoints) {
				// must increase pointIdx
				pointIdx++;
				offset = newOffset - gapBetweenPoints;
				// after increasing pointIdx, have I reached the last point
				// on this road segment?
				// if yes, than change road segment!
				if (pointIdx == route[routeIndex].pt2) {
					if (routeIndex == route.length - 1)
						finished = true;
					else {
						roadIdx = route[routeIndex + 1].roadIndex;
						pointIdx = route[routeIndex + 1].pt1;
						if (route[routeIndex + 1].pt2 > pointIdx)
							direction = 1;
						else
							direction = 0;
						offset = 0;
						routeIndex++;
						startOnRoad();
					}
				}
			} else
				offset = newOffset;
		} else {
			if (pointIdx == 0) {
				double newOffset = offset - distance;
				if (newOffset < 0) {
					offset = 0;
				} else
					offset = newOffset;
			} else {
				double newOffset = offset - distance;
				double gapBetweenPoints = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance()
						- Globals.map.roads.get(roadIdx).points.get(pointIdx - 1).getDistance();
				// [km]
				if (newOffset < 0) {
					pointIdx--;
					offset = gapBetweenPoints + newOffset;
					// after decreasing pointIdx, have I passed the last point
					// on this road segment?
					// if yes, than change road segment!
					if ((pointIdx + 1) == route[routeIndex].pt2) {
						if (routeIndex == route.length - 1)
							finished = true;
						else {
							roadIdx = route[routeIndex + 1].roadIndex;
							pointIdx = route[routeIndex + 1].pt1;
							if (route[routeIndex + 1].pt2 > pointIdx)
								direction = 1;
							else
								direction = 0;
							offset = 0;
							routeIndex++;
							startOnRoad();
						}
					}
				} else
					offset = newOffset;
			}
		}

		// if it is "very close" to the intersection, change segment
		// then change segment

		double dif = 1.0;
		if (intersection != null && influenced) {
			double d1 = Globals.map.roads.get(roadIdx).points.get(nextIntersectionPointIndex).getDistance();
			double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
			if (d1 > d2)
				dif = d1 - d2;
			else
				dif = d2 - d1;
		}
		if (dif < 0.001) { // less than 1 meter
			if (routeIndex < route.length - 1) {
				roadIdx = route[routeIndex + 1].roadIndex;
				pointIdx = route[routeIndex + 1].pt1;
				if (route[routeIndex + 1].pt2 > pointIdx)
					direction = 1;
				else
					direction = 0;
				offset = 0;
				routeIndex++;
				startOnRoad();
			} else {
				finished = true;
			}
		}

		// if the cars waits at a traffic light compute the average speed
		if (Engine.simulationType == RoutingConstants.DYNAMIC_CITY_ROUTE && avgSpeedStatus == SPEED_RESET && !queued
				&& intersection != null && speed < 0.5) {
			// queued = true;
			avgSpeed = distanceOnSegment * 3600 / timeOnSegment1;
			avgSpeedStatus = SPEED_COMPUTED;
		}
	}

	public CarInstance(short roadIdx, short pointIdx, Personality p) {
		this.roadIdx = roadIdx;
		this.pointIdx = pointIdx;
		this.p = p;
	}

	public CarInstance(CarInfo car) {
		super(car);
	}

	// public CarInstance(int ID){
	// this.ID = ID;
	// }

	private double getDistance(CarInstance c) {
		// return the absolute distance between "this" car and "c" (meters)
		// c can be either on the same road with "this" or on the next road
		// (according to this.route)
		// or on the previous road (according to this.route)
		if (c.roadIdx == roadIdx) {
			double d1 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
			double d2 = Globals.map.roads.get(c.roadIdx).points.get(c.pointIdx).getDistance() + c.offset;
			if (d1 > d2)
				return (d1 - d2) * 1000;
			return (d2 - d1) * 1000;
		} else {
			if (routeIndex < route.length - 1) {
				// can check next road
				if (c.roadIdx == route[routeIndex + 1].roadIndex) {
					double d1 = Globals.map.roads.get(roadIdx).points.get(nextIntersectionPointIndex).getDistance();
					double d2 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
					double dist1;
					if (d1 > d2)
						dist1 = d1 - d2;
					else
						dist1 = d2 - d1;
					d1 = Globals.map.roads.get(route[routeIndex + 1].roadIndex).points.get(route[routeIndex + 1].pt1)
							.getDistance();
					d2 = Globals.map.roads.get(route[routeIndex + 1].roadIndex).points.get(c.pointIdx).getDistance()
							+ c.offset;
					double dist2;
					if (d1 > d2)
						dist2 = d1 - d2;
					else
						dist2 = d2 - d1;
					return (dist1 + dist2) * 1000;
				}
			}
			if (routeIndex > 0) {
				// can check previous road
				if (c.roadIdx == route[routeIndex - 1].roadIndex) {
					double d1 = Globals.map.roads.get(roadIdx).points.get(pointIdx).getDistance() + offset;
					double d2 = Globals.map.roads.get(roadIdx).points.get(route[routeIndex].pt1).getDistance();
					double dist1;
					if (d1 > d2)
						dist1 = d1 - d2;
					else
						dist1 = d2 - d1;

					d1 = Globals.map.roads.get(c.roadIdx).points.get(route[routeIndex - 1].pt2).getDistance();
					d2 = Globals.map.roads.get(c.roadIdx).points.get(c.pointIdx).getDistance() + c.offset;
					double dist2;
					if (d1 > d2)
						dist2 = d1 - d2;
					else
						dist2 = d2 - d1;
					return (dist1 + dist2) * 1000;
				}
			}
			// System.out.println("ERROR! getDistance called, but cars not on
			// the same road or on consecutive roads!");
			// System.out.println("THIS = "+this);
			// System.out.println("OTHER = "+c);
			return -1.0;
		}
	}

	public double getDistanceToCarInFront() {
		CarInstance c = getPrecedingCarOnLane(lane);
		if (c == null)
			return 1000.0;
		double d = getDistance(c);
		if (d < 0)
			return 1000.0;
		return d;
	}

	private CarInstance getPrecedingCarOnLane(int laneNo) {
		// returns the car in front of "this", on lane "laneNo";
		Road thisRoad = Globals.map.roads.get(roadIdx);
		if (direction == 1) {
			int i = index + 1;
			while (i < thisRoad.carsOnThisRoad.size()) {
				CarInstance c = thisRoad.carsOnThisRoad.get(i);
				if (c.lane == laneNo && c.direction == this.direction && c.pointIdx < route[routeIndex].pt2)
					return c;
				i++;
			}
			// there was no car on the current road! But is there one on the
			// "next" road?
			return getFirstCarOnNextSegment();
		} else {
			// System.out.println("else; index="+index);
			int i = index - 1;
			while (i >= 0) {
				// System.out.println("Trying a car...");
				CarInstance c = thisRoad.carsOnThisRoad.get(i);
				if (c.lane == laneNo && c.direction == this.direction && c.pointIdx >= route[routeIndex].pt2) {
					// System.out.println("IS GOOD!");
					return c;
				} else {
				}
				i--;
			}
			// there was no car in the current road! But is there one on the
			// "next" road?
			return getFirstCarOnNextSegment();

		}
	}

	private CarInstance getFirstCarOnNextSegment() {
		boolean considerLanes;

		if (routeIndex < route.length - 1) {
			if (Globals.map.roads.get(route[routeIndex].roadIndex).laneNo <= Globals.map.roads
					.get(route[routeIndex + 1].roadIndex).laneNo)
				considerLanes = true;
			else
				considerLanes = false;

			Road nextRoad = Globals.map.roads.get(route[routeIndex + 1].roadIndex);
			if (route[routeIndex + 1].pt1 > route[routeIndex + 1].pt2) {
				for (int i = nextRoad.carsOnThisRoad.size() - 1; i >= 0; i--) {
					CarInstance c = nextRoad.carsOnThisRoad.get(i);
					// if(ID==22) {
					// System.out.println("TESTING : "+c);
					// }
					if (c.pointIdx < route[routeIndex + 1].pt1 && c.pointIdx > route[routeIndex + 1].pt2
							&& c.direction == 0 && (!considerLanes || (considerLanes && c.lane == this.lane))) {
						// if(ID==22) {
						// System.out.println("WAS GOOD : "+c);
						// }
						return c;
					}
				}
				return null;
			} else {
				for (int i = 0; i < nextRoad.carsOnThisRoad.size(); i++) {
					CarInstance c = nextRoad.carsOnThisRoad.get(i);
					// if(ID==22) {
					// System.out.println("TESTING : "+c);
					// }
					if (c.pointIdx < route[routeIndex + 1].pt2 && c.pointIdx >= route[routeIndex + 1].pt1
							&& c.direction == 1 && (!considerLanes || (considerLanes && c.lane == this.lane))) {
						// if(ID==22) {
						// System.out.println("WAS GOOD : "+c);
						// }
						return c;
					}
				}
				return null;
			}
		} else
			return null;
	}

	private CarInstance getFirstApproachingVehicle(DirectedRoadSegment drs) {
		SortedCarVector sv = Globals.map.roads.get(drs.roadIndex).carsOnThisRoad;
		if (drs.direction) {
			for (int i = 0; i < sv.size(); i++) {
				CarInstance c = sv.get(i);
				if (c.pointIdx < drs.pointIndex || c.direction == 1)
					continue;
				// else this is the one
				return c;
			}
		} else {
			for (int i = sv.size() - 1; i >= 0; i--) {
				CarInstance c = sv.get(i);
				if (c.pointIdx >= drs.pointIndex || c.direction == 0)
					continue;
				// else this is the one
				return c;
			}
		}
		return null;
	}

	private CarInstance getFollowingCarOnLane(int laneNo) {
		// returns the car behind "this", on lane "laneNo";
		Road thisRoad = Globals.map.roads.get(roadIdx);
		if (direction == 1) {
			int i = index - 1;
			while (i >= 0) {
				CarInstance c = thisRoad.carsOnThisRoad.get(i);
				if (c.lane == laneNo && c.direction == this.direction)
					return c;
				i--;
			}
			return getFollowingCarOnPrecRoad(laneNo);
		} else {
			int i = index + 1;
			while (i < thisRoad.carsOnThisRoad.size()) {
				CarInstance c = thisRoad.carsOnThisRoad.get(i);
				if (c.lane == laneNo && c.direction == this.direction)
					return c;
				i++;
			}
			return getFollowingCarOnPrecRoad(laneNo);
		}
	}

	private CarInstance getFollowingCarOnPrecRoad(int laneNo) {
		// returns the car just "behind" the current car, but
		// situated on the previous road segment (according to
		// the route)
		if (routeIndex == 0)
			return null;
		Road prevRoad = Globals.map.roads.get(route[routeIndex - 1].roadIndex);
		if (route[routeIndex - 1].pt1 > route[routeIndex - 1].pt2) {
			int i = 0;
			while (i < prevRoad.carsOnThisRoad.size()) {
				CarInstance c = prevRoad.carsOnThisRoad.get(i);
				if (c.lane == laneNo && c.pointIdx >= route[routeIndex - 1].pt2 && c.direction == 0)
					return c;
				i++;
			}
			return null;
		} else {
			int i = prevRoad.carsOnThisRoad.size() - 1;
			while (i >= 0) {
				CarInstance c = prevRoad.carsOnThisRoad.get(i);
				if (c.lane == laneNo && c.pointIdx < route[routeIndex - 1].pt2 && c.direction == 1)
					return c;
				i--;
			}
			return null;
		}
	}

	private void becomeInfluencedByTrafficLight() {
		influencedByTrafficLight = true;
		influencedLaneChange = true;

		// which lane is the best?
		double bestDist = -1.0;
		int lane = 1;
		for (int i = 0; i < Globals.map.roads.get(roadIdx).laneNo; i++) {
			if (!desiredLanes.contains(new Byte((byte) (i + 1))))
				continue;

			CarInstance c = getPrecedingCarOnLane(i + 1);
			double dist = 0.0;
			if (c != null) {
				dist = getDistance(c);
			} else {
				dist = Double.MAX_VALUE;
			}
			if (dist > bestDist) {
				bestDist = dist;
				lane = i;
			}
		}
		if (bestDist == -1.0) {
			System.err.println("ERROR! becomeInfluencedByTrafficLight!");
		}
		desiredLanes = new ArrayList<Byte>();
		desiredLanes.add((byte) (lane + 1));
	}

	public int compareTo(CarInstance c) {
		// if this>c ==> return 1;
		// if this=c ==> return 0;
		// if this<c ==> return -1;

		// assert(c.roadIdx==this.roadIdx);
		if (c.roadIdx != this.roadIdx) {
			System.out.println("ERROR! Comparing cars on different roads!: " + this + " with " + c);
			return 0;
		}

		if (this.pointIdx > c.pointIdx)
			return 1;
		if (this.pointIdx < c.pointIdx)
			return -1;
		if (this.offset == c.offset)
			return this.ID - c.ID;
		if (this.offset > c.offset)
			return 1;
		return -1;
	}

	public void printRoute() {
		for (int i = 0; i < route.length; i++) {
			System.out.println(route[i] + " ");
		}
		System.out.println("**");
	}

	public String toString() {
		String ret = "";
		ret = ret + "Road:" + roadIdx + ";Point=" + pointIdx + ";offSet=" + offset + ";Speed=" + speed + ";Lane="
				+ lane;
		return ret;
	}

	public void deleteCarToPointMapping() {
		Road r = Globals.map.roads.get(this.getRoadIdx());
		Point p = r.points.get(this.getPointIdx());
		PeanoKey pk = Globals.map.peanoKeys.get(p.getPeanoKeyIdx());
		pk.removeCar(this.simulatedCarInfo);
	}

	public void updateCarToPointMapping() {
		Road r = Globals.map.roads.get(this.getRoadIdx());
		Point p = r.points.get(this.getPointIdx());
		PeanoKey pk = Globals.map.peanoKeys.get(p.getPeanoKeyIdx());
		pk.addCar(this.simulatedCarInfo);
	}

	public SimulatedCarInfo getSimulatedCarInfo() {
		return simulatedCarInfo;
	}

	public void setSimulatedCarInfo(SimulatedCarInfo record) {
		this.simulatedCarInfo = record;
	}

	public void finish() {
		// System.out.println(" printing...");
		double dist = 0.0;
		stopTime = (Globals.engine.crtTime * 1000) / Globals.engine.fps;
		dist = RouteComputingUtils.getDistanceForRoute(this.route);
		CarInstance.logWriter
				.println(this.ID + " " + route[0].roadIndex + " " + this.route[0].pt1 + " "
						+ this.route[route.length - 1].roadIndex + " " + this.route[route.length - 1].pt1 + " "
						+ (stopTime - startTime) + " " + this.routingType + " " + dist + " " + this.routeChangesCount
						+ " " + toLateFailCount + " " + noRouteFailCount + " " + sentMsgCount + " " + receivedMsgCount
						+ " " + emptyMsgCount + " " + inccorectReplyCount + " " + noChangeNeededCount + " "
						+ routeComputingFailCount);
		CarInstance.logWriter.flush();
	}

}