#ifndef CAR_H_
#define CAR_H_

#include <iostream>
#include <string>

#include "../config.h"
#include <ode/ode.h>
#include "../vectorMath/vmath.h"
#include "../iface/Visualized.h"
#include "../iface/Animated.h"
#include "SimulationObject.h"
#include "Sector.h"

class CarSource;

class Car: public SimulationObject, public Animated, public Visualized {
public:
	Car(
		Sector* sector,
		const Vector3r& size,
		const Vector3r& pos,
		const dReal direction,
		const dReal weight,
		const dReal whdia,
		const dReal whthick,
		const dReal whweight,
		const dReal maxSpeed
	);
	virtual ~Car();
	
	//Getters and setters
	//-----------------------------
	/** Gives the current sector the object is in */
	Sector* getSector() { return _sector; };
	/** Gives car's actual position */
	Vector3r getPosition() const { return _position; };
	/** Gives car's previous position */
	Vector3r getPrevPosition() const { return _prevPosition; };
	/** Gives car's actual direction */
	Vector3r getDirection() const { return _direction; };
	/** Gives car's previous direction */
	Vector3r getPrevDirection() const { return _prevDirection; };
	/** Gives car's actual speed */
	dReal getSpeed() { return _speed; };
	/** Gives the left bottom back and right top front corner points of the car's bounding box */
	void getBoundingBox(Vector3r &aa, Vector3r &bb);
	/** Gives the planned sectors the car intends to go through */
	SectorDeque &getPlannedSectors() { return _plannedSectors; };
	/** Gives the car's passed sectors */
	SectorVector &getPassedSectors() { return _passedSectors; };
	
	/** Initializes the car once the simulation starts */
	virtual void initialize();
	/** Computes the actions to be done during the Perform phase of the simulation step */
	virtual void computeStep();
	/** Performs the actions as computed in the Compute phase of the simulation step */
	virtual void performStep();
	/** Handler for new car in sector event */
	void carAddedToSectorHandler(Sector* s, Car* c);
	/** Handler for car removed from sector event */
	void carRemovedFromSectorHandler(Sector* s, Car* c);
	
	//Visualization-related methods
	//-----------------------------
	/** Display the car */
	void visualize();
	/** get status of showing car's right and left tracking lines */
	bool getShowCarBorderPaths() { return _showCarBorderPaths; };
	/** Toggle showing car's right and left tracking lines */
	void toggleShowCarBorderPaths() { _showCarBorderPaths = !_showCarBorderPaths; };
	/** get status of showing car's Bezier tracking line */
	bool getShowCarBezierPath() { return _showCarBezierPath; };
	/** toggle showing car's Bezier tracking line */
	void toggleShowCarBezierPath() { _showCarBezierPath = !_showCarBezierPath; };
	/** get status of showing car's direction vector */
	bool getShowCarDirection() { return _showCarDirection; };
	/** toggle showing car's direction vector */
	void toggleShowCarDirection() { _showCarDirection = !_showCarDirection; };
	/** get status of showing car's intersection points */
	bool getShowCarIntersectionPoints() { return _showCarIntersectionPoints; };
	/** toggle showing car's intersection points */
	void toggleShowCarIntersectionPoints() { _showCarIntersectionPoints = !_showCarIntersectionPoints; };
	
	/** Gives important car metrics in a string */
	std::string toString() const;
	
private:
	enum DriveMode { MODE_FREEWAY, MODE_FOLLOW, MODE_LET_GO };
	/** Traces a path through a sector based on _startLine and _transitLine */
	void tracePath();
	/** Selects the next point on the computed Bezier path */
	void selectNextBezierPoint();
	/** Switches to the next planned sector */
	void switchSector();
	/** Check if car is colliding with projected _bezierPath */
	bool isColliding(Car* car);
	/** Decides if car should let go or will let go */
	CarPriority decidePriority(Car* car);
	/** Registers with a sector to listen to car events */
	void addCarEventHandler(Sector* sector);
	/** Registers with a sector to listen to car events */
	void removeCarEventHandler(Sector* sector);
	
	/** The current sector */
	Sector* _sector;
	/** Vector of sectors planned to drive through */
	SectorDeque _plannedSectors;
	/** Sectors the car has passed */
	SectorVector _passedSectors;
	/** End line for path through the current sector */
	Line _transitLine;
	/** The current position */
	Vector3r _position;
	/** The position in previous step */
	Vector3r _prevPosition;
	/** The current direction */
	Vector3r _direction;
	/** The sideways deviation from the computed trajectory */
	dReal _trajectoryDeviation;
	/** The direction in previous step */
	Vector3r _prevDirection;
	/** The last point on the Bezier path the car has already passed */
	Vector3r _lastPoint;
	/** The current point on the Bezier path the car is aiming at */
	Vector3r _targetPoint;
	/** The next point on the Bezier path the car is going to aim at once it passes the _targetPoint */
	Vector3r _nextPoint;
	/** Line on which the car will swich to follow next segment of the Bezier path */
	Line _switchLine;
	/** Flag for state when the car has already computed the path through the next sector and will switch to it soon */
	bool _expectNewSector;
	/** Vector of cars in sight */
	SectorCarMapping _carsInSight;
	
	/** ODE body for main car hull */
	dBodyID _body;
	/** ODE bodies for wheels */
	dBodyID _wheels[4];
	/** ODE hinges to connect hull and wheels */
	dJointID _hinges[4];
	/** ODE geom for the hull */
	dGeomID _box;
	/** ODE geoms for the wheels */
	dGeomID _spheres[4];
	/** Car's own ODE collision space */
	dSpaceID _space;
	/** Car's size */
	Vector3r _size;
	/** Path tracking point this point is supposed to be in between the front wheels and act as a reference point for tracking deviation from the computed trajectory path */
	Vector3r _pathTrackPoint;
	/** Radius of car's wheels */
	dReal _wheelRadius;
	/** Thickness of car's wheels */
	dReal _wheelThickness;
	/** Path to follow in current sector */
	Polygon _path;
	/** Projected trajectory left border */
	Polygon _leftPathBorder;
	/** Projected trajectory right border */
	Polygon _rightPathBorder;
	/** Projected Bezier path in current sector */
	Polygon _bezierPath;
	/** Current car speed */
	dReal _speed;
	/** Target speed to reach */
	dReal _targetSpeed;
	/** Maximum speed the car can do */
	dReal _maxSpeed;
	/** Current car acceleration (can be negative if braking */
	dReal _acceleration;
	/** Maximum acceleration the car can do */
	dReal _maxAcceleration;
	/** Maximum deccleration the car can do */
	dReal _maxDecceleration;
	/** Current driving mode */
	DriveMode _driveMode;
	
	Polygon _points;
	std::vector<PVector3r> _vectors;
	
	/** Status attribute for showing car's right and left tracking lines */
	bool _showCarBorderPaths;
	/** Status attribute for showing car's Bezier tracking line */
	bool _showCarBezierPath;
	/** Status attribute for showing car's direction vector */
	bool _showCarDirection;
	/** Status attribute for showing car's intersection points with sector */
	bool _showCarIntersectionPoints;
	
	friend std::ostream& operator<<(std::ostream& os, const Car& car);
	friend class CarSource;
};

#endif /*CAR_H_*/
