/******************************************************************************
 *
 *
 *
 *
 ******************************************************************************/
 
#ifndef __JABA_H__
#define __JABA_H__

#include <vector>
#include <list>
#include <qt/qpainter.h>
#include <qt/qpicture.h>

#include <aiiutils.h>
#include <aigeom.h>

#define DISTANCE_UNIT 	0.0001
#define LABYRINTH_SCALE 10000
#define HITS_REPORT 		10
#define EXPLORE_TIMES 	5

extern int totalCells;

//-----------------------------------------------------------------------------\
//| This class defines a labyrinth (maze) where the autonomous vehicles - JABAs|
//| operate. The labyrinth may be fixed or dynamic. If a Jaba wants to know    |
//| what it has encountered it just asks the labyrinth (providing all important|
//| paramenters). The world is defined in the floating point (double)          |
//| coordinates in the rande [0..1][0..1].                                     |
//| NOTE: The maze automatically generates the boundaries at the sides of the  |
//| square                                                                     |
//-----------------------------------------------------------------------------/

class lcLabyrinth
{
public:
	lcLabyrinth();
	lcLabyrinth(const lcLabyrinth& rhs);
	~lcLabyrinth();

	lcLabyrinth& operator=(const lcLabyrinth& rhs);
		
	// Drawing routines. The actual size of the image is determined by the 
	// QPainter. Since all of the real coordinates are specified in floating
	// point 0..1 range, in order to draw the maze on the screen all coordinates 
	// are scaled by pictureScale
	void draw(QPainter& );
	int pictureScale() const { return LABYRINTH_SCALE; };
	
	// Accessors
	void addBound(const aic2Rect& rz, bool regenPicture = false);
	
	bool onmaze(const aic2Vect& pnt) const;
	
	bool hit(const aic2Vect& from, const aic2Vect& vel, 
											double& time, double& angle) const;
	
private:
	
	void _generatePicture();		
	void _assign(const lcLabyrinth& rhs);

	QPicture _picture;
	bool _isPictureUpdated;
	vector<aic2Rect> _maze;
};

//-----------------------------------------------------------------------------
// This collection of classes describes the currect discovered part of the 
// labyrinth.  In this simplest implementation it is just a list of recorded
// hits. Each of which has timestamp, coordinates of the hit, and the direction 
// of the normal.
//-----------------------------------------------------------------------------

class lcHitRecord
{
public:
	lcHitRecord() : _at(0, 0), _dir(0), _timestamp(0) {};
	lcHitRecord(const aic2Vect& at, double dir, unsigned long t = 0)
		: _at(at), _dir(dir), _timestamp(t) {};
	
	// Default copyctor, dtor and assignment ok
	
	// Accessors
	const aic2Vect& at() const { return _at; };
	double dir() const { return _dir; };
	
	unsigned long timestamp() const { return _timestamp; };
	void timestamp (unsigned long t) { _timestamp = t; };
	
	// Output NOTE ->>> lcDiscoveredLabyrinth will handle the drawing itself
	// void draw(QPainter& p);
	
	// Debug
	void print()
	{
		printf ("Hit (%f %f) at angle %f , time : %ld\n", _at.x(), _at.y(), 
						_dir, _timestamp);
	};
	
private:
	aic2Vect _at;
	double _dir;

	unsigned long _timestamp;
};

class lcDiscoveredLabyrinth
{
public:
	lcDiscoveredLabyrinth() : _hits(0), _newHits(0), _curTime(0) {};

	// Default copyctor, dtor and assignment ok
	
	void reset() { _newHits = _hits = vector<lcHitRecord>(); _curTime = 0; };
	
	// Accessors
	void addHits(const vector<lcHitRecord>& hits);
	void stepTime() { _curTime++; };
	
	// Output
	
	void draw(QPainter& p) const { _draw(p, _hits); };
	void drawNew(QPainter& p) 
			{ _draw(p, _newHits); _newHits = vector<lcHitRecord>();};
	// The last function paints the new hits, and cleans them

	// NOTE ->>> Define later
	void expireOldEntries() {};
	
	int pictureScale() const { return LABYRINTH_SCALE; };

private:
	void _draw(QPainter& p, const vector<lcHitRecord>& hits) const;

	vector<lcHitRecord> _hits;
	vector<lcHitRecord> _newHits;
	unsigned long _curTime;
};
//-----------------------------------------------------------------------------
// This is a base class for all the message classes in the simulation
//-----------------------------------------------------------------------------

// This function computes the signal strength between points to and from.
// In order not to get the infinite values for the very close points, if the 
// distance between the points is less then unit, it is set to maximum. 
// Otherwise the distanses are normalized by unit.

inline double lfComputeStrength(double ini, const aic2Vect& from, 
		const aic2Vect& to, double unit = DISTANCE_UNIT)
{
	double dist = (to - from).r() / unit;
	
	if (dist < 1)
		dist = 1;
	
	return ini / (Sqr(dist));
};

class lcJabaMessage
{
public:
	lcJabaMessage()
		: _from(0.0, 0.0), _strength(0), 
		  _hit(false), _hitPos(0.0, 0.0), _hitNormal(0, 0) {};
	
	virtual ~lcJabaMessage() {};
	// Default copy ctor, assignment ok
	
	const aic2Vect& messagePos() const { return _from; };
	// The position of the sender (as presumed by the sender).
	
	double strength() const { return _strength; };
	// The strength of the output signal

	bool hit() const { return _hit; };
	// Whether the hit has occured.
	const aic2Vect& hitPos() const { return _hitPos; };
	// The postion where the hit has occured.
	const aic2Vect& hitNormal() const { return _hitNormal; };
	// The presumed direction of teh wall normal

	// Debugging
	virtual void print() const
	{
		printf ("MESSAGE from (%f, %f) send strength %f. ", 
						_from.x(), _from.y(), _strength);

		if (_hit)
		{
			printf ("HIT at (%f %f) with normal (%f %f)\n", 
					_hitPos.x(), _hitPos.y(), _hitNormal.x(), _hitNormal.y());
		}
		else
			printf ("\n");
	};	
	
protected:
		
	aic2Vect _from;
	double _strength;

	bool _hit;
	aic2Vect _hitPos;
	aic2Vect _hitNormal;
};

//-----------------------------------------------------------------------------
// A SendMessage description
//-----------------------------------------------------------------------------

class lcJabaSentMessage : public lcJabaMessage
{
public:
	lcJabaSentMessage(const aic2Vect& from, double strength, 
								bool hit = false, const aic2Vect& hPos = aic2Vect(0.0, 0.0), 
								const aic2Vect& hNormal = aic2Vect(0.0, 0.0))
		: lcJabaMessage()
		{
			_from = from;
			_strength = strength;
			_hit = hit;
			_hitPos = hPos;
			_hitNormal = hNormal;
		};
};

//-----------------------------------------------------------------------------
// This is a simulation specific description for a jaba message in the "ether",
// it has the real position of a jaba, the id of the cell, and other information
// necessary to compute correct ReceivedMEssage at a given position
//-----------------------------------------------------------------------------
class lcJabaEtherMessage : public lcJabaMessage
{
public:
	lcJabaEtherMessage()
		:
		lcJabaMessage(), 
		_cell(-1),
		_realPos(0, 0)
	{};

	lcJabaEtherMessage(const lcJabaSentMessage& msg, int cell, 
										 const aic2Vect& realPos)
		: 
		lcJabaMessage(msg),
		_cell(cell),
		_realPos(realPos)
		{};

	
	// Accessors
	int cell() const { return _cell; };
	const aic2Vect& realPos() const { return _realPos; };
	
	// Debugging
	virtual void print() const
	{
		printf ("Message in the cell %i from real position (%f, %f) -->> ", 
						_cell, _realPos.x(), _realPos.y());
		lcJabaMessage::print(); // Call base print
	};
private:
	int _cell;
	aic2Vect _realPos;
};

// This operator is used for sorting the messages based on their TDMA cell id
inline bool operator < (const lcJabaEtherMessage& lhs, 
													const lcJabaEtherMessage& rhs)
{
	return lhs.cell() < rhs.cell();
}

//-----------------------------------------------------------------------------
// A ReceiveMessage description
//-----------------------------------------------------------------------------

class lcJabaReceivedMessage : public lcJabaMessage
{
public:
	lcJabaReceivedMessage()
		:
		lcJabaMessage()
	{}; // Need this for list operations

	lcJabaReceivedMessage(const lcJabaEtherMessage& msg, 
												double strength)
		: lcJabaMessage(msg),
			_receiveStrength(strength)
		{};
	
	double receiveStrength() const { return _receiveStrength; };

	virtual void print() const
	{
		printf ("Received Message with strength %f -->> ", _receiveStrength);
		lcJabaMessage::print();
	};
private:
	friend class lcJabaEtherMessage;

	double _receiveStrength;
};

//-----------------------------------------------------------------------------
// This is the base class for jaba nodes (Jabas and towers) it provides the 
// framework for moving, rending and receiving messages. The node cannot live
// outside of lcLabyrinth, and it always has 
//-----------------------------------------------------------------------------

class lcJabaNode
{
public:
	lcJabaNode(const lcLabyrinth * labyrinth, const aic2Vect& startPos, 
							double strength);
	virtual ~lcJabaNode();
	
	// Default copy ctor & assignment are OK.

	// Accessors:
	const aic2Vect& pos() const { return _pos; };
	// Effect 		Returns the real position of the node. Used for drawing 
	//						and other output. Should not be accessed by the algorithms
	//						of the mobile nodes.

	const aic2Vect& dir() const { return _dirU; };
	// Effect 		Returns the real direction of the node. Used for drawing 
	//						and other output. Should not be accessed by the algorithms
	//						of the mobile nodes.

//	void pos(const aic2Vect& p) { _pos = p; };
	// Effect 		Get/Set the real position of the node. 
	//						The position is used for signal strength calculation, etc.

	
	const list<lcJabaEtherMessage>& getPendingMessageQueue() const 
					{ return _mqueue; };
	void cleanMessageQueue() { _mqueue.resize(0); };
	// Effect 		These functions deal with the messages which the node 
	//						wants to send out.
	
	// Message Processing
	virtual void processMessages(const list<lcJabaReceivedMessage>& messages) {};
	
	// Output
	virtual void draw(QPainter& ) const { printf(" Drawing...\n"); };
	
protected:
	// The following functions are interfaces to actuators and sensors
	// for a node
	void addMessageToQueue(const lcJabaSentMessage& msg, int cell)
	{
		_mqueue.push_back(lcJabaEtherMessage(msg, cell, pos()));
	};
	// Effect 		Adds a new message to the output queue
	// Note 			Can be called only by the derived classes (as if anybody else
	//						can get to the preotected: member :)

	bool turnAndMove(double angle);
	// Effect 		Move in the labyrinth. Stop if hit the wall with angle to 
	//						normal less than 45 degrees. If the wall is hit with a larger
	//						angle, then the jaba ricochets off the wall and continues the 
	//						motion.
	// Returns		True if it has hit an obstacle

	double strength() const { return _strength; };
private:
	const lcLabyrinth* _labyrinth;
protected:
	int pictureScale() const { return _labyrinth->pictureScale();};

	aic2Vect _presumedPos;
private:
	double _strength;
	aic2Vect _pos;
	aic2Vect _dirU;	// Current direction, a unit vector
	list <lcJabaEtherMessage> _mqueue;
};


//-----------------------------------------------------------------------------
// This class describes a Tower in the labyrinth
// It sends out a single message with its coordinates, and accepts all messages
// and forwards then to the map estimation routines
//-----------------------------------------------------------------------------

class lcJabaTower : public lcJabaNode
{
public:
	lcJabaTower(const lcLabyrinth *labyrinth, 
							lcDiscoveredLabyrinth *dlabyrinth,
							const aic2Vect& p, 
							double strength, int cell)
		: lcJabaNode(labyrinth, p, strength),
			_cell(cell),
			_dlabyrinth(dlabyrinth)
	{
		_presumedPos = p;
		
		// Create a message queue which contains a single message
		addMessageToQueue(lcJabaSentMessage(_presumedPos, 
												strength), _cell);
	};
	
	virtual void processMessages(const list<lcJabaReceivedMessage>& messages);

	virtual void draw(QPainter& ) const;
private:
	int _cell;
	lcDiscoveredLabyrinth* _dlabyrinth;
};

//-----------------------------------------------------------------------------
//   This class defines a mobile node. The node can send and receive messages. 
// It can also request to move in a certain direction (relative to its current 
// direction. The messages it sends out are either (1) it has hit a wall while
// moving; or (2) It had received the message that some other node has hit 
// a wall, and it is closer to one of the towers than the node it has received 
// the message from.
//   If the node has hit the wall during the move, it first sends out a
// message, and only after that preforms a new move. In between the change 
// direction reqiests, the node attempts to move in a straight line, so it 
// can estimate the direction by the difference between the last and the current 
// locations
//-----------------------------------------------------------------------------

class lcJabaMobile : public lcJabaNode
{
public:
	lcJabaMobile(const lcLabyrinth *labyrinth, const aic2Vect& startPos, 
							double strength);
	
	// Default copy ctor & assignment are OK.

	// Message Processing
	virtual void processMessages(const list<lcJabaReceivedMessage>& messages);

	virtual void draw(QPainter& ) const ;
		
private:
	aic2Vect _presumedPrevPos;
	bool _prevHit;
	bool _pprevHit;
	int _curHits;
};
#endif