#include "Car.h"
#include "../Logger.h"
#include <sstream>
#include "../drawstuff/drawstuff.h"
#include "../drawstuff/helperFunctions.h"
#include "../vectorMath/VectorMath.h"
#include "../vectorMath/bezier.h"

#define DEVIATION_LIMIT 2
#define BEZIER_RATIO 0.8

Car::Car(
	Sector* sector,
	const Vector3r &size,
	const Vector3r &pos,
	const dReal direction,
	const dReal weight,
	const dReal whrad,
	const dReal whthick,
	const dReal whweight,
	const dReal maxSpeed
):
	SimulationObject(),
	_position(pos),
	_prevPosition(pos),
	_direction(Vector3r(1,0,0)),
	_prevDirection(Vector3r(1,0,0)),
	_sector(sector),
	_transitLine(),
	_size(size),
	_wheelRadius(whrad),
	_wheelThickness(whthick),
	_trajectoryDeviation(0),
	_carsInSight(),
	_showCarBorderPaths(false),
	_showCarBezierPath(true),
	_showCarDirection(true),
	_showCarIntersectionPoints(false),
	_speed(0),
	_maxSpeed(maxSpeed),
	_acceleration(0),
	_maxAcceleration(0),
	_maxDecceleration(0),
	_expectNewSector(false),
	_driveMode(MODE_FOLLOW)
{
	_DEBUG("car[" << getID() << "]: creating new Car");
	mASSERT(_sector != NULL);
	mASSERT(_sector->containsPoint(_position));
	
	int i;
	_DEBUG("car[" << getID() << "]: creating torso");
	_body = dBodyCreate(getWorldID());
	_space = dSimpleSpaceCreate(getSpaceID());
	dBodySetPosition(_body, pos.x, pos.y, pos.z + 0.5);
	dMass m;
	dMassSetBox(&m, 1, _size.x, _size.y, _size.z);
	dMassAdjust(&m, weight);
	dBodySetMass(_body, &m);
	_box = dCreateBox(_space, _size.x, _size.y, _size.z);
	dGeomSetBody(_box, _body);
			
	// wheel bodies
	_DEBUG("car[" << getID() << "]: creating wheel bodies & boxes");
	for (i = 0; i < 4; i++) {
		_wheels[i] = dBodyCreate(getWorldID());
		dQuaternion q;
		dQFromAxisAndAngle(q, 1, 0, 0, M_PI*0.5);
		dBodySetQuaternion(_wheels[i], q);
		dMassSetSphere(&m, 1, _wheelRadius);
		dMassAdjust(&m, whweight);
		dBodySetMass(_wheels[i], &m);
		_spheres[i] = dCreateSphere(_space, _wheelRadius);
		dGeomSetBody(_spheres[i], _wheels[i]);
	}
	
	_DEBUG("car[" << getID() << "]: adjusting wheel positions");
	dReal widthOffset = 0.5 * _size.y;
	dReal lengthOffset = 0.5 * _size.x - _wheelRadius;
	Vector3r wp;
	for (i = 0; i < 4; i++) {
		wp.x = (i < 2 ? 1 : -1) * lengthOffset;
		wp.y = (i % 2 == 0 ? 1 : -1) * widthOffset;
		wp.z = 0;
		wp.rotate(0, 0, RAD2DEG(direction));
		dBodySetPosition(_wheels[i], pos.x + wp.x, pos.y + wp.y, pos.z);
	}
	
	_DEBUG("car[" << getID() << "]: rotating car to start position");
	dMatrix3 mBoxRotation;
	dRFromAxisAndAngle(mBoxRotation, 0, 0, 1, direction);
	dBodySetRotation(_body, mBoxRotation);
	
	//start of rotation I: rotation of car when geoms are done
	dMatrix3 mWheelRotation;
	dRFromEulerAngles(mWheelRotation, M_PI*0.5, direction, 0);
	dBodySetRotation(_wheels[0], mWheelRotation);
	dBodySetRotation(_wheels[1], mWheelRotation);
	dBodySetRotation(_wheels[2], mWheelRotation);
	dBodySetRotation(_wheels[3], mWheelRotation);
	
	// front and back wheel hinges
	Vector3r axis2(0, 1, 0);
	axis2.rotate(0, 0, RAD2DEG(direction));
	for (i = 0; i < 4; i++) {
		_hinges[i] = dJointCreateHinge2(getWorldID(), 0);
		dJointAttach(_hinges[i], _body, _wheels[i]);
		const dReal* a = dBodyGetPosition(_wheels[i]);
		dJointSetHinge2Anchor(_hinges[i], a[0], a[1], a[2]);
		dJointSetHinge2Axis1(_hinges[i], 0, 0, 1);
		dJointSetHinge2Axis2(_hinges[i], axis2.x, axis2.y, axis2.z);
	}
	
	// set joint suspension
	_DEBUG("car[" << getID() << "]: setting joint suspension parameters");
	for (i = 0; i < 4; i++) {
		dJointSetHinge2Param(_hinges[i], dParamSuspensionERP, 0.4);
		dJointSetHinge2Param(_hinges[i], dParamSuspensionCFM, 0.8);
		dJointSetHinge2Param(_hinges[i], dParamVel2, 0);
		dJointSetHinge2Param(_hinges[i], dParamFMax2, 25);
	}
	
	// setting steering parameters for front wheel hinges
    dJointSetHinge2Param(_hinges[0], dParamFMax, 0.2);
    dJointSetHinge2Param(_hinges[0], dParamLoStop, -0.75);
    dJointSetHinge2Param(_hinges[0], dParamHiStop, 0.75);
    dJointSetHinge2Param(_hinges[0], dParamFudgeFactor, 0.1);
    dJointSetHinge2Param(_hinges[1], dParamFMax, 0.2);
    dJointSetHinge2Param(_hinges[1], dParamLoStop, -0.75);
    dJointSetHinge2Param(_hinges[1], dParamHiStop, 0.75);
    dJointSetHinge2Param(_hinges[1], dParamFudgeFactor, 0.1);
    
	// lock back wheels along the steering axis
	// set stops to make sure wheels always stay in alignment
	_DEBUG("car[" << getID() << "]: locking rear wheels along steering axis");
	dJointSetHinge2Param(_hinges[3], dParamLoStop, 0);
	dJointSetHinge2Param(_hinges[3], dParamHiStop, 0);
	dJointSetHinge2Param(_hinges[2], dParamLoStop, 0);
	dJointSetHinge2Param(_hinges[2], dParamHiStop, 0);

	//set a category to the wheels so we don't compute the collisions between
	//the body and the wheels of the same car
	long wheelcat = 1 << 2;
	long boxcat = 1 << 3;
	for (i = 0; i < 4; i++) {
		dGeomSetCategoryBits(_spheres[i], wheelcat);
		dGeomSetCollideBits(_spheres[i], !boxcat);
	}
	dGeomSetCategoryBits(_box, boxcat);
	dGeomSetCollideBits(_box, !wheelcat);

	//update path tracking point
	_pathTrackPoint = midPoint(dBodyGetPosition(_wheels[0]), dBodyGetPosition(_wheels[1]));
	_pathTrackPoint.z = 0;

	_DEBUG("car[" << getID() << "]: new Car succesfully created");
}

Car::~Car()
{
	_DEBUG("car[" << getID() << "]: destroying Car");
	_sector->deregisterCar(this);
	_carsInSight.clear();
	
	for (int i = 0; i < 4; i++) {
		dJointDestroy(_hinges[i]);
		dGeomDestroy(_spheres[i]);
		dBodyDestroy(_wheels[i]);
	}
	dGeomDestroy(_box);
	dBodyDestroy(_body);
	dSpaceDestroy(_space);
	_DEBUG("car[" << getID() << "]: Car succesfully destroyed");
}

void Car::initialize()
{
	//update car position based on the ODE-computed position
	const dReal* pos = dBodyGetPosition(_body);
	_position.x = pos[0];
	_position.y = pos[1];
	_position.z = pos[2];
	_prevPosition = _position;
	
	//update direction using alignment of two wheels on one side
	const dReal* pos1 = dBodyGetPosition(_wheels[0]);
	const dReal* pos2 = dBodyGetPosition(_wheels[2]);
	_direction.x = pos1[0] - pos2[0];
	_direction.y = pos1[1] - pos2[1];
	_direction.z = pos1[2] - pos2[2];
	_direction.normalize();
	_prevDirection = _direction;
	
	//update path tracking point
	const dReal* pos3 = dBodyGetPosition(_wheels[1]);
	_pathTrackPoint = midPoint(pos1, pos3);
	_pathTrackPoint.z = 0;
	
	//schedule initial two sectors to the path
	_sector->registerCar(this);
	addCarEventHandler(_sector);
	Sector* nextSector = _sector->getExitSectors().front();
	_plannedSectors.push_back(nextSector);
	addCarEventHandler(nextSector);
	_transitLine = _sector->getTransitLine(nextSector);
	_DEBUG("Initial _transitLine=" << _transitLine);
	
	if (!nextSector->hasCarSink()) {
		//randomly select one exit sector
		int selected = -1;
		do {
			selected = randomIntInRange(0, nextSector->getExitSectors().size());
//			selected++;
		} while (_sector->hasParallelSector(nextSector->getExitSectors().at(selected))); //this prevents cars from doing U-turns on junctions
		_plannedSectors.push_back(nextSector->getExitSectors().at(selected));
		addCarEventHandler(_plannedSectors.at(1));
		
		if (nextSector->getType() == Sector::JUNCTION) {
			//if we're going to drive through a junction, include all possible cars incoming from other sectors into sight
			SectorVector entrySectors = nextSector->getEntrySectors();
			for (SectorVector::iterator sit = entrySectors.begin(); sit != entrySectors.end(); sit++) {
				if (*sit == _sector) continue;
				addCarEventHandler(*sit);
			}
		}
		_DEBUG("car[" << getID() << "]: Chose the nextnext sector randomly to be sector ID=" << _plannedSectors.at(1)->getID());
	}
	
	_bezierPath.push_back(_pathTrackPoint);
	_bezierPath.push_back(midPoint(_transitLine));
	tracePath();
	selectNextBezierPoint();
	_expectNewSector = true;
}

void Car::computeStep()
{
	//update car position based on the ODE-computed position
	_prevPosition = _position;
	const dReal* pos = dBodyGetPosition(_body);
	_position.x = pos[0];
	_position.y = pos[1];
	_position.z = pos[2];
	
	//update direction using alignment of two wheels on one side
	_prevDirection = _direction;
	const dReal* pos1 = dBodyGetPosition(_wheels[0]);
	const dReal* pos2 = dBodyGetPosition(_wheels[2]);
	_direction.x = pos1[0] - pos2[0];
	_direction.y = pos1[1] - pos2[1];
	_direction.z = pos1[2] - pos2[2];
	_direction.normalize();
	
	//update path tracking point
	const dReal* pos3 = dBodyGetPosition(_wheels[1]);
	_pathTrackPoint = midPoint(pos1, pos3);
	_pathTrackPoint.z = 0;
	
	_trajectoryDeviation = orientedDistFromLine(Line(_lastPoint, _targetPoint), _pathTrackPoint);
	if (_trajectoryDeviation < -DEVIATION_LIMIT) _trajectoryDeviation = -DEVIATION_LIMIT;
	if (_trajectoryDeviation > DEVIATION_LIMIT) _trajectoryDeviation = DEVIATION_LIMIT;
	
	if (_expectNewSector && orientedDistFromLine(_transitLine, _pathTrackPoint) <= 0) switchSector();
	
	if (orientedDistFromLine(_switchLine, _pathTrackPoint) >= 0) {//if we have just passed the next Bezier point
		if (_bezierPath.size() <= 2) {//trace path through next sector if we're running out of Bezier points
			_expectNewSector = true;
			_DEBUG("car[" << getID() << "]: _expectNewSector set to true");
			tracePath();
		}
		selectNextBezierPoint();
	}
	dReal targetSpeed;
	dReal dist;
	
	Car* carToFollow = NULL;
	dReal collisionDistance = 10000;
	_driveMode = MODE_FREEWAY;
	for (SectorCarMapping::iterator it = _carsInSight.begin(); it != _carsInSight.end(); it++) {
		if ((*it).second.second == CAR_FOLLOW) {
			dist = (_pathTrackPoint - (*it).second.first->_pathTrackPoint).length();
			if (dist < collisionDistance) {
				carToFollow = (*it).second.first;
				collisionDistance = dist;
				_driveMode = MODE_FOLLOW;
			}
		} else if ((*it).second.second == CAR_LET_GO) {
			dist = (midPoint(_transitLine) - _pathTrackPoint).length();
			if (dist < collisionDistance) {
				carToFollow = (*it).second.first;
				collisionDistance = dist;
				_driveMode = MODE_LET_GO;
			}
		}
	}
	if (_driveMode == MODE_FOLLOW) {
		dReal a = (
			2*_speed
			+ (_speed - carToFollow->getSpeed() > 0
				? (_speed - carToFollow->getSpeed())*(_speed - carToFollow->getSpeed())/(2*_maxDecceleration)
				: 0)
			+ _size.x/2 + carToFollow->_size.x/2
			);
		if (collisionDistance < (_size.x/2 + carToFollow->_size.x/2)*1.5) {
			targetSpeed = 0;
			_DEBUG("Car[" << getID() << "]: in mode MODE_FOLLOW & too close to car[" << carToFollow->getID() << "] - braking to speed 0, _followMeCarDist=" << collisionDistance << ", a=" << a);			
		} else if (collisionDistance < a) {
			targetSpeed = carToFollow->getSpeed();
			_DEBUG("Car[" << getID() << "]: in mode MODE_FOLLOW & braking to car[" << carToFollow->getID() << "]'s speed of " << carToFollow->getSpeed() << ", _followMeCarDist=" << collisionDistance << ", a=" << a);
		} else {
			targetSpeed = _maxSpeed;
			_DEBUG("Car[" << getID() << "]: in mode MODE_FOLLOW not braking to car[" << carToFollow->getID() << "], _followMeCarDist=" << collisionDistance << ", a=" << a);
		}
	} else if (_driveMode == MODE_LET_GO) {
		dReal a = (2*_speed + _speed*_speed/(2*-_maxDecceleration));
		if (collisionDistance < _size.x/2) {
			targetSpeed = 0;
			_DEBUG("Car[" << getID() << "]: in mode MODE_LET_GO for car[" << carToFollow->getID() << "] and braking hard to zero, _followMeCarDist=" << collisionDistance << ", a=" << a);
		} else if (collisionDistance < a) {
			targetSpeed = 0;
			_DEBUG("Car[" << getID() << "]: in mode MODE_LET_GO for car[" << carToFollow->getID() << "] and braking to zero, _followMeCarDist=" << collisionDistance << ", a=" << a);
		} else {
			targetSpeed = _maxSpeed;
			_DEBUG("Car[" << getID() << "]: in mode MODE_LET_GO and not braking to car[" << carToFollow->getID() << "], _followMeCarDist=" << collisionDistance << ", a=" << a);
		}
	} else {
		targetSpeed = _maxSpeed;
		_DEBUG("Car[" << getID() << "]: in mode MODE_FREEWAY and speeding to _maxSpeed");
	}
	
	dReal speedDelta = targetSpeed - _speed;
	if (_speed == 0) _speed = 0.01;
	if (speedDelta > 0) {
//		_DEBUG("speeding");
		dReal sh = sinh(_speed / _maxAcceleration);
		dReal th = tanh(speedDelta / _maxAcceleration);
		_acceleration = _maxAcceleration * (sh < 1 ? sh : th);
	} else {
//		_DEBUG("braking");
		_acceleration = _maxDecceleration * tanh(speedDelta / _maxDecceleration);
	}
}

void Car::performStep()
{
	_speed += _acceleration;
    // set speed to the motor (all wheels)
    for (int i = 0; i < 4; i++) {
	    dJointSetHinge2Param(_hinges[i], dParamVel2, -_speed);
	    dJointSetHinge2Param(_hinges[i], dParamFMax2, 2);
    }

    // steering the front wheels
    dReal v = -_trajectoryDeviation / DEVIATION_LIMIT * M_PI / 2 - dJointGetHinge2Angle1(_hinges[0]);
    if (v > 0.1) v = 0.1;
    if (v < -0.1) v = -0.1;
    v *= 10.0;
    dJointSetHinge2Param(_hinges[0], dParamVel, v);
    dJointSetHinge2Param(_hinges[1], dParamVel, v);
}

void Car::selectNextBezierPoint()
{
	_lastPoint = _bezierPath.at(0);
	_targetPoint = _bezierPath.at(1);
	_nextPoint = _bezierPath.at(2);
	_bezierPath.pop_front();
	_switchLine._a = _targetPoint;
	Vector3r tmp = (_targetPoint - _lastPoint).getPerpendicular();
	tmp.normalize();
	_switchLine._b = _targetPoint + tmp;
}

void Car::visualize()
{	
	//draw the car's body
  	dsSetTexture(DS_NONE);
	const float lowersides[3] = {_size.x, _size.y, _size.z * 0.5};
	const float uppersides[3] = {_size.x * 0.6, _size.y * 0.9, _size.z * 0.5};
	const dReal* pos = dBodyGetPosition(_body);
	Vector3r up(pos[0], pos[1], pos[2]);
	up -= _direction * _size.x * 0.1;
	up.z = pos[2] + _size.z * 0.5;
	dReal upperpos[3] = {up.x, up.y, up.z};
	dsSetColor(0,0.6,0.6);
  	dsDrawBox(pos, dBodyGetRotation(_body), lowersides);
	dsSetColor(0.75,0.93,0.93);
  	dsDrawBox(upperpos, dBodyGetRotation(_body), uppersides);
  	
  	//draw the wheels
  	dsSetColor(1,1,1);
 	for (int i = 0; i < 4; i++)
 		dsDrawCylinder(dBodyGetPosition(_wheels[i]), dBodyGetRotation(_wheels[i]), _wheelThickness, _wheelRadius);
 	
 	//draw the current target direction
	if (_showCarDirection) {
	  	dsDrawVector(Vector3r(pos[0], pos[1], pos[2]), _direction * _size.length(), GREEN);
	}

	//draw the tracking paths
	if (_showCarBorderPaths) {
		dsDrawPolygon(_leftPathBorder, RED);
		dsDrawPolygon(_rightPathBorder, BLUE);
	}

	//draw the Bezier path and points
	if (_showCarBezierPath) {
		dsDrawPolygon(_bezierPath, GREEN);
		dsDrawPoint(_lastPoint, BLUE);
		dsDrawPoint(_targetPoint, RED);
		for (unsigned i = 0; i < _bezierPath.size(); i++)
			dsDrawPoint(_bezierPath[i], GREEN);
	}

	if (_showCarIntersectionPoints) {
		for (unsigned i = 0; i < _points.size(); i++) dsDrawPoint(_points[i], WHITE);
		for (unsigned j = 0; j < _vectors.size(); j++)
			dsDrawVector(_vectors[j]);
	}
}

void Car::tracePath()
{
	//right and left temporary tracking lines
	Line RTL, LTL;
	_rightPathBorder.clear();
	_leftPathBorder.clear();
	_path.clear();
	_points.clear();
	_vectors.clear();
	
	Sector* nextSector = _plannedSectors.front();
	_DEBUG("car[" << getID() << "]: Creating path through sector[" << nextSector->getID() << "]");
	
	Line startLine = _sector->getTransitLine(nextSector);
	Line endLine;
	if (nextSector->hasCarSink()) {//if next sector is watched by a CarSink
		//just create a path somewhere to the sector and that's it - the car's going to be terminated anyway
		_DEBUG("car[" << getID() << "]: sector[" << nextSector->getID() << "] has CarSink, creating only partial path into middle of the sector");
		Vector3r slv = (startLine._b - startLine._a).getPerpendicular();
		slv.normalize();
		endLine._a = startLine._a + slv;
		endLine._b = startLine._b + slv;
		_DEBUG("car[" << getID() << "]: startLine=" << startLine << "; endLine=" << endLine);
	} else {
		mASSERT(_plannedSectors.size() >= 2);
		endLine = nextSector->getTransitLine(_plannedSectors.at(1));
	}
	
	Vector3r startPoint = midPoint(startLine);
	Vector3r endPoint = midPoint(endLine);
	Vector3r slv = startLine._b - startLine._a;
	slv.normalize();
	Vector3r elv = endLine._b - endLine._a;
	elv.normalize();
	
	//get start and end points of the initial border lines
	RTL._a = startPoint + slv * (_size.y / 2 + 0.3);
	RTL._b = endPoint + elv * (_size.y / 2 + 0.3);
	LTL._a = startPoint - slv * (_size.y / 2 + 0.3);
	LTL._b = endPoint - elv * (_size.y / 2 + 0.3);
	
	//store the first point of the path polygon
	_DEBUG("car[" << getID() << "]: RTL = " << RTL);
	_DEBUG("car[" << getID() << "]: LTL = " << LTL);
	_path.push_back(startPoint);
	_rightPathBorder.push_back(RTL._a);
	_leftPathBorder.push_back(LTL._a);
	_DEBUG("car[" << getID() << "]: first tracking lines set");
	
	_points.push_back(RTL._a);
	_points.push_back(RTL._b);
	_points.push_back(LTL._a);
	_points.push_back(LTL._b);
	//RTLIsection - RightTrackingLine intersection
	//LTLIsection - LeftTrackingLine intersection
	//RTLIsectedPer - perpendicular vector of RightTrackingLine-intersected polygon
	//LTLIsectedPer - perpendicular vector of LeftTrackingLine-intersected polygon
	Vector3r RTLIsection, LTLIsection, RTLIsectedPer, LTLIsectedPer;
	dReal RTLMinDist, LTLMinDist;
	Vector3r intersections[32];
	unsigned intersected[32];
	unsigned count = 0;
	PolygonVector sectorBorders = nextSector->getPolygons();
	PolygonVector::const_iterator pvit;
	Vector3r oppositeISection;
	dReal oppositeISectionDist = dInfinity;
	Polygon* p = NULL;
	unsigned iteration = 0;

	while (iteration++ < 200) {//loop until we can't find any more intersections but limit to max iterations
		RTLMinDist = LTLMinDist = dInfinity;
		//find intersections of the tracking lines with all polygons and store them
		for (pvit = sectorBorders.begin(); pvit != sectorBorders.end(); pvit++) {
			p = *pvit;
			//skip the transit lines to and from the sector
			if (
				p->front() == startLine._a && p->back() == startLine._b ||
				p->front() == startLine._b && p->back() == startLine._a ||
				p->front() == endLine._a && p->back() == endLine._b ||
				p->front() == endLine._b && p->back() == endLine._a
			) continue;
			//if the RTL does intersect the current polygon in at least one point
			if ((count = IntersectPolygon(p, RTL, intersections, intersected)) > 0) {
				_DEBUG("car[" << getID() << "]: Found " << count << " intersection of RTL with polygon");
				mASSERT(count <= 32);
				//choose the closest intersection
				for (unsigned i = 0; i < count; i++) {
					if ((intersections[i] - RTL._a).length() < RTLMinDist) {
						RTLIsection = intersections[i];
						RTLMinDist = (intersections[i] - RTL._a).length();
						RTLIsectedPer = Vector3r(p->at(intersected[i]+1) - p->at(intersected[i])).getPerpendicular();
						RTLIsectedPer.normalize();
						if (!nextSector->containsPoint(RTLIsection + RTLIsectedPer)) RTLIsectedPer.invert();
						_points.push_back(RTLIsection);
						_vectors.push_back(PVector3r(RTLIsection, RTLIsectedPer));
					}
				}
			}
			//if the LTL does intersect the current polygon in at least one point
			if ((count = IntersectPolygon(p, LTL, intersections, intersected)) > 0) {
				_DEBUG("car[" << getID() << "]: Found " << count << " intersection of LTL with polygon");
				mASSERT(count <= 32);
				//choose the closest intersection
				for (unsigned i = 0; i < count; i++) {
					if ((intersections[i] - LTL._a).length() < LTLMinDist) {
						LTLIsection = intersections[i];
						LTLMinDist = (intersections[i] - LTL._a).length();
						LTLIsectedPer = Vector3r(p->at(intersected[i]+1) - p->at(intersected[i])).getPerpendicular();
						LTLIsectedPer.normalize();
						if (!nextSector->containsPoint(LTLIsection + LTLIsectedPer)) LTLIsectedPer.invert();
						_points.push_back(LTLIsection);
						_vectors.push_back(PVector3r(LTLIsection, LTLIsectedPer));
					}
				}
			}
		}
		_DEBUG("car[" << getID() << "]: finished search for closest intersections, found RTLIsection=" << RTLIsection << " at RTLMinDist=" << RTLMinDist << "; LTLIsection=" << LTLIsection << " at LTLMinDist=" << LTLMinDist);
		
		if (RTLMinDist > 10000 && LTLMinDist > 10000) {//no intersections found, we can drive straight through
			_DEBUG("car[" << getID() << "]: no other intersections found, ending search");
			break;
		} else if (RTLMinDist <= LTLMinDist) {//one or two intersection founds, find the closer
			_DEBUG("car[" << getID() << "]: closest intersection found for RTL - " << RTLMinDist);
			oppositeISectionDist = dInfinity;
			for (pvit = sectorBorders.begin(); pvit != sectorBorders.end(); pvit++) {
				p = *pvit;
				//if the RTL does intersect the current polygon in at least one point
				if ((count = IntersectPolygon(p, RTLIsection + RTLIsectedPer * 0.1, RTLIsection + RTLIsectedPer * 1000, intersections, intersected)) > 0) {
					mASSERT(count <= 32);
					//choose the closest intersection
					for (unsigned i = 0; i < count; i++) {
						if ((intersections[i] - RTLIsection).length() < oppositeISectionDist) {
							oppositeISection = intersections[i];
							oppositeISectionDist = (intersections[i] - RTLIsection).length();
						}
					}
				}
			}
			if (nextSector->getType() == Sector::LANE) {
				RTL._a = midPoint(RTLIsection, oppositeISection) - RTLIsectedPer * (_size.y / 2 + 0.3);
				LTL._a = midPoint(RTLIsection, oppositeISection) + RTLIsectedPer * (_size.y / 2 + 0.3);
				_path.push_back(midPoint(RTLIsection, oppositeISection));
			} else {
				RTL._a = RTLIsection + RTLIsectedPer * 0.4;
				LTL._a = RTLIsection + RTLIsectedPer * (_size.y + 0.4);
				_path.push_back(midPoint(RTL._a, LTL._a));
			}
			
			if (!nextSector->containsPoint(RTL._a)) throw SimulatorException("Sector not wide enough");
			if (!nextSector->containsPoint(LTL._a)) throw SimulatorException("Sector not wide enough");
		} else {
			_DEBUG("car[" << getID() << "]: closest intersection found for LTL - " << LTLMinDist);
			oppositeISectionDist = dInfinity;
			for (pvit = sectorBorders.begin(); pvit != sectorBorders.end(); pvit++) {
				p = *pvit;
				//if the LTL does intersect the current polygon in at least one point
				if ((count = IntersectPolygon(p, LTLIsection + LTLIsectedPer * 0.1, LTLIsection + LTLIsectedPer * 1000, intersections, intersected)) > 0) {
					mASSERT(count <= 32);
					//choose the closest intersection
					for (unsigned i = 0; i < count; i++) {
						if ((intersections[i] - LTLIsection).length() < oppositeISectionDist) {
							oppositeISection = intersections[i];
							oppositeISectionDist = (intersections[i] - LTLIsection).length();
						}
					}
				}
			}
			if (nextSector->getType() == Sector::LANE) {
				RTL._a = midPoint(LTLIsection, oppositeISection) + LTLIsectedPer * (_size.y / 2 + 0.3);
				LTL._a = midPoint(LTLIsection, oppositeISection) - LTLIsectedPer * (_size.y / 2 + 0.3);
				_path.push_back(midPoint(LTLIsection, oppositeISection));
			} else {
				RTL._a = LTLIsection + LTLIsectedPer * (_size.y + 0.4);
				LTL._a = LTLIsection + LTLIsectedPer * 0.4;
				_path.push_back(midPoint(RTL._a, LTL._a));
			}

			if (!nextSector->containsPoint(RTL._a)) throw SimulatorException("Sector not wide enough");
			if (!nextSector->containsPoint(LTL._a)) throw SimulatorException("Sector not wide enough");
		}
		_rightPathBorder.push_back(RTL._a);
		_leftPathBorder.push_back(LTL._a);
		_points.push_back(RTL._a);
		_points.push_back(LTL._a);
		_DEBUG("car[" << getID() << "]: finished adding new point to the path, current path is " << _path);
	}
	_rightPathBorder.push_back(RTL._b);
	_leftPathBorder.push_back(LTL._b);
	_path.push_back(endPoint);
	_DEBUG("car[" << getID() << "]: final path is " << _path);
	
	//now let's simplify the path by removing redundant points
	Polygon reducedPath;
	//don't remove the start point
	reducedPath.push_back(_path.front());
	Vector3r dir1, dir2;
	for (unsigned i = 1; i < _path.size()-1; i++) {
		dir1 = _path[i] - _path[i-1];
		dir1.normalize();
		float angle1 = atan2(dir1.y, dir1.x);
		dir2 = _path[i+1] - _path[i];
		dir2.normalize();
		float angle2 = atan2(dir2.y, dir2.x);
		if (std::abs(angle2 - angle1) > EPSILON) reducedPath.push_back(_path[i]);
	}
	//don't remove the end point
	reducedPath.push_back(_path.back());
	_path.swap(reducedPath);
	
	unsigned rps = _path.size();
	mASSERT(_bezierPath.size() == 2);
	computeBezier(_bezierPath,
		midPoint(_bezierPath.at(0), _bezierPath.at(1)),
		_bezierPath.at(1),
		midPoint(_path[0], _path[1]),
		_bezierPath.at(1),
		BEZIER_RATIO
	);
	_bezierPath.pop_front();
	_bezierPath.pop_front();
	_targetPoint = _bezierPath.at(0);
	_nextPoint = _bezierPath.at(1);
	for (unsigned int i = 0; i < rps - 2; i++) {
		computeBezier(_bezierPath,
			midPoint(_path[i+1], _path[i]),
			_path[i+1],
			midPoint(_path[i+2], _path[i+1]),
			_path[i+1],
			BEZIER_RATIO
		);
	}
	computeBezier(_bezierPath,
		midPoint(_path[rps-2], _path[rps-1]),
		midPoint(midPoint(_path[rps-2], _path[rps-1]), _path[rps-1]),
		_path[rps-1],
		midPoint(midPoint(_path[rps-2], _path[rps-1]), _path[rps-1])
	);
	_DEBUG("car[" << getID() << "]: _bezierPath="<<_bezierPath);

	//let's remove redundant points from the _bezierPath
	Polygon _tmpBezierPath;
	_tmpBezierPath.push_back(_bezierPath.at(0));
	for (unsigned i = 1; i < _bezierPath.size(); i++)
		if (_bezierPath.at(i) != _bezierPath.at(i-1)) _tmpBezierPath.push_back(_bezierPath.at(i));
	_bezierPath.swap(_tmpBezierPath);	
	_DEBUG("car[" << getID() << "]: _bezierPath="<<_bezierPath);
}

void Car::switchSector()
{
	_expectNewSector = false;
	_DEBUG("Switching sector");
	
	//re-register to the new sector
	_sector->deregisterCar(this);
	removeCarEventHandler(_sector);
	mASSERT(_plannedSectors.size() > 0);
	_passedSectors.push_back(_sector);
	_sector = _plannedSectors.front();
	_plannedSectors.pop_front();
	mASSERT(_sector != NULL);
	//if we're entering a junction, we no longer care the for cars coming from other entry sectors
	// - we assume once we're in the junction they let us go
	if (_sector->getType() == Sector::JUNCTION) {
		SectorVector entrySectors = _sector->getEntrySectors();
		for (SectorVector::iterator sit = entrySectors.begin(); sit != entrySectors.end(); sit++)
			removeCarEventHandler(*sit);
	}
	if (!_sector->hasCarSink()) {
		Sector* nextSector = _plannedSectors.front();
		if (!nextSector->hasCarSink()) {
			//randomly select one exit sector
			int selected = -1;
			do {
				selected = randomIntInRange(0, nextSector->getExitSectors().size());
//					selected++;
			} while (_sector->hasParallelSector(nextSector->getExitSectors().at(selected))); //this prevents cars from doing U-turns on junctions
			_plannedSectors.push_back(nextSector->getExitSectors().at(selected));
			addCarEventHandler(_plannedSectors.at(1));
			
			if (nextSector->getType() == Sector::JUNCTION) {
				//if we're going to drive through a junction, include all possible cars incoming from other sectors into sight
				SectorVector entrySectors = nextSector->getEntrySectors();
				for (SectorVector::iterator sit = entrySectors.begin(); sit != entrySectors.end(); sit++) {
					if (*sit == _sector) continue;
					addCarEventHandler(*sit);
				}
			}
			_DEBUG("car[" << getID() << "]: Chose the nextnext sector randomly to be sector[" << _plannedSectors.at(1)->getID() << "]");
		}
		
		mASSERT(_plannedSectors.size() > 0);
		_transitLine = _sector->getTransitLine(_plannedSectors.front());
	}
	
	_sector->registerCar(this);
}

void Car::addCarEventHandler(Sector* sector)
{
	mASSERT(sector != NULL);
	if (sector->hasCarEventListener(this)) {
		_DEBUG("car[" << getID() << "]: sector[" << sector->getID() << "] to be added as observed but already is observed");
		return;
	}
	
	sector->addCarEventListener(this);
	
	CarVector cars = sector->getCars();
	for (CarVector::iterator cit = cars.begin(); cit != cars.end(); cit++) {
		if ((*cit)->getID() == getID()) {
			_DEBUG("car[" << getID() << "]: skipping myself in sector[" << sector->getID() << "] when adding cars to _carsInSight");
			continue;
		}
		bool inCarsInSight = false;
		for (SectorCarMapping::iterator scmit = _carsInSight.begin(); scmit != _carsInSight.end(); scmit++)
			if (*cit == (*scmit).second.first) {
				(*scmit).second.second = decidePriority(*cit);
				inCarsInSight = true;
				break;
			}
		if (inCarsInSight) {
			_DEBUG("car[" << getID() << "]: in sector[" << sector->getID() << "] - car[" << (*cit)->getID() << "] is already in _carsInSight");
			continue;
		}
		_DEBUG("car[" << getID() << "]: in sector[" << sector->getID() << "] adding car[" << (*cit)->getID() << "] to _carsInSight");
		_carsInSight.insert(std::make_pair(sector, std::make_pair(*cit, decidePriority(*cit))));
	}
	_DEBUG("car[" << getID() << "]: sector[" << sector->getID() << "] added among observed, current _carsInSight" << _carsInSight);
}

void Car::removeCarEventHandler(Sector* sector)
{
	mASSERT(sector != NULL);
	if (!sector->hasCarEventListener(this)) {
		_DEBUG("car[" << getID() << "]: sector[" << sector->getID() << "] to be removed from observed but already not observed");
		return;
	}
	
	sector->removeCarEventListener(this);
	
	if (_carsInSight.count(sector) > 0) {
		//remove all cars we register in that sector
		_carsInSight.erase(sector);
		_DEBUG("car[" << getID() << "]: sector[" << sector->getID() << "] removed from observed, current _carsInSight" << _carsInSight);
	}
}

void Car::carAddedToSectorHandler(Sector* s, Car* c)
{
	mASSERT(s != NULL);
	mASSERT(c != NULL);
	CarPriority p = decidePriority(c);
	for (SectorCarMapping::iterator scmit = _carsInSight.begin(); scmit != _carsInSight.end(); scmit++)
		if (c == (*scmit).second.first) {
			(*scmit).second.second = p;
			return;
		}
	
	_carsInSight.insert(std::make_pair(s, std::make_pair(c, p)));
	_DEBUG("car[" << getID() << "]: new car[" << c->getID() << "] in sight in sector[" << s->getID() << "], current _carsInSight" << _carsInSight);
}

void Car::carRemovedFromSectorHandler(Sector* s, Car* c)
{
	mASSERT(s != NULL);
	mASSERT(c != NULL);
	for (SectorCarMapping::iterator it = _carsInSight.begin(); it != _carsInSight.end(); it++)
		if ((*it).second.first == c) {
			_carsInSight.erase(it);
			_DEBUG("car[" << getID() << "]: car[" << c->getID() << "] out of sight in sector[" << s->getID() << "], current _carsInSight" << _carsInSight);
			return;
		}
}

bool Car::isColliding(Car* car)
{
	Vector3r aa, bb, ab, ba;
	car->getBoundingBox(aa, bb);
	ab.x = bb.x; ab.y = aa.y; ab.z = 0;
	ba.x = aa.x; ba.y = bb.y; ba.z = 0;
	Vector3r intersections[32];
	unsigned intersected[32];
	if (IntersectPolygon(_bezierPath, aa, ab, intersections, intersected) > 0) return true;
	if (IntersectPolygon(_bezierPath, ab, bb, intersections, intersected) > 0) return true;
	if (IntersectPolygon(_bezierPath, bb, ba, intersections, intersected) > 0) return true;
	if (IntersectPolygon(_bezierPath, ba, aa, intersections, intersected) > 0) return true;
	return false;
}

CarPriority Car::decidePriority(Car* car)
{
	_DEBUG("car[" << getID() << "]: deciding priority for car[" << car->getID() << "]");
	mASSERT(car != NULL);
	Sector* ccs = car->getSector(); //current car sector
	Sector* nextSector = _plannedSectors.front();
	if (ccs == _sector) {//car is in our sector, decide if to follow it or ignore (e.g. it's behind us)
		_DEBUG("ccs == _sector");
		if (_sector->getType() == Sector::LANE) {
			if (isColliding(car)) return CAR_FOLLOW;
			else return CAR_NOT_INTERFERING;
		}
		//a fix for car letting go car which has the same way through a junction and has entered it later
		if (_passedSectors.size() > 0 && car->_passedSectors.size() > 0 && _passedSectors.back() == car->_passedSectors.back()) return CAR_NOT_INTERFERING;
		Line myWay(_position, midPoint(_transitLine));
		Line carWay(car->getPosition(), midPoint(car->_transitLine));
		_DEBUG("Car[" << getID() << "]: myWay=" << myWay << ", carWay=" << carWay);
		
		Vector3r intersection;
		IntersectResult r = intersect(myWay, carWay, intersection);
		if (r == NOT_INTERESECTING || r == PARALLEL) return CAR_NOT_INTERFERING;
		else if (r == INTERESECTING) {
			if (orientedDistFromLine(myWay, carWay._a) > 0) return CAR_LET_GO;
			else return CAR_LETS_GO;
		} else {
			if (isColliding(car)) return CAR_FOLLOW;
			else return CAR_NOT_INTERFERING;
		}
	} else if (ccs == nextSector) {//car is in the next sector
		_DEBUG("ccs == nextSector");
		//check if the next sector is a junction
		if (nextSector->getType() == Sector::LANE) return CAR_FOLLOW; //if not, simply follow the car
		if (car->_passedSectors.size() > 0 && car->_passedSectors.back() == _sector) { //car is coming from our sector
			return CAR_FOLLOW;
		}
		Line myWay(midPoint(_sector->getTransitLine(nextSector)), midPoint(nextSector->getTransitLine(_plannedSectors.at(1))));
		Line carWay(car->getPosition(), midPoint(car->_transitLine));
		_DEBUG("Car[" << getID() << "]: myWay=" << myWay << ", carWay=" << carWay);
		
		Vector3r intersection;
		IntersectResult r = intersect(myWay, carWay, intersection);
		if (r == NOT_INTERESECTING || r == PARALLEL) return CAR_NOT_INTERFERING;
		else if (r == COINCIDENT) return CAR_FOLLOW;
		else return CAR_LET_GO;
	} else if (ccs == _plannedSectors.at(1)) {//car is in the nextnext sector
		_DEBUG("ccs == _plannedSectors.at(1)");
		return CAR_FOLLOW; //we shall follow the cars that are already in the nextnext sector
	} else if (nextSector->hasEntrySector(ccs)) { //car is in one of the sectors that lead to our nextsector - need to decide right/left hand priority here
		_DEBUG("nextSector->hasEntrySector(ccs)");
		SectorDeque sectors = car->getPlannedSectors();
		_DEBUG("Car[" << getID() << "]: _plannedSectors=" << _plannedSectors << ", car[" << car->getID() << "]'s _plannedSectors" << sectors);
		if (sectors.at(0) != nextSector) return CAR_NOT_INTERFERING; // car doesn't drive to our junction sector
		Line myWay(midPoint(_sector->getTransitLine(nextSector)), midPoint(nextSector->getTransitLine(_plannedSectors.at(1))));
		Line carWay(midPoint(ccs->getTransitLine(nextSector)), midPoint(nextSector->getTransitLine(sectors.at(1))));
		_DEBUG("Car[" << getID() << "]: myWay=" << myWay << ", carWay=" << carWay);
		
		Vector3r intersection;
		IntersectResult r = intersect(myWay, carWay, intersection);
		if (r == NOT_INTERESECTING || r == PARALLEL) return CAR_NOT_INTERFERING;
		else if (r == INTERESECTING) {
			if (orientedDistFromLine(myWay, carWay._a) > 0) return CAR_LET_GO;
			else return CAR_LETS_GO;
		} else _FATAL("Car[" << getID() << "]: car[" << car->getID() << "]'s sector[" << ccs->getID() << "] is not leading to the next junction sector[" << nextSector->getID() << "]");
	} else {
		_FATAL("Car[" << getID() << "]: car[" << car->getID() << "] has the same way to go through sector[" << nextSector->getID() << "] - that shouldn't happen");
		throw SimulatorException("Fatal error in Car::decidePriority()");
	}
	return CAR_LET_GO;
}

void Car::getBoundingBox(Vector3r &aa, Vector3r &bb)
{
	dReal aabb[6];
	dGeomGetAABB(_box, aabb);
	aa.x = aabb[0];
	aa.y = aabb[2];
	aa.z = 0;
	bb.x = aabb[1];
	bb.y = aabb[3];
	bb.z = 0;
}

std::string Car::toString() const
{
	std::ostringstream oss;
	const Vector3r pos = getPosition();
	oss << "car[" << getID() << "](pos=" << pos[0] << "," << pos[1] << "," << pos[2] << "; speed=" << _speed << "), _sector[" << _sector->getID() << "], _driveMode[" << _driveMode << "], _carsInSight" << _carsInSight;
	return oss.str();
}

std::ostream& operator<<(std::ostream& os, const Car& car)
{
	os << car.toString();
	return os;
}
