/******************************************************************************
 *
 * 
 *
 *
 ******************************************************************************/
 
#include <stdio.h>
#include <vector.h>

#include <vector>
#include <algorithm>
#include <cmath>
#include <qt/qpainter.h>

#include <aigeom.h>
#include <geomhelp.h>
#include <jaba.h>

//=============================================================================

static double lvJabaSpeed = 0.01;

lcLabyrinth::lcLabyrinth()
	:
	_picture(),
	_isPictureUpdated(false),
	_maze(0)
{
	
	_maze.push_back(aic2Rect(0, 0, 1, 0.01));
	_maze.push_back(aic2Rect(0, 0, 0.01, 1));
	_maze.push_back(aic2Rect(0, 0.99, 1, 0.01));
	_maze.push_back(aic2Rect(0.99, 0, 0.01, 1));
}
	
lcLabyrinth::lcLabyrinth(const lcLabyrinth& rhs)
	:
	_picture(),
	_isPictureUpdated(false),
	_maze(0)
{
	_assign(rhs);
}

lcLabyrinth::~lcLabyrinth()
{
}
	
lcLabyrinth& lcLabyrinth::operator=(const lcLabyrinth& rhs)
{
	_assign(rhs);
}

void lcLabyrinth::draw(QPainter& p)
{
	_generatePicture();
	
	p.drawPicture(_picture);
}
	
void lcLabyrinth::addBound(const aic2Rect& rz, bool regenPicture = false)
{
	_maze.push_back(rz);

	_isPictureUpdated = false;
		
	if (regenPicture)
		_generatePicture();
}
	
bool lcLabyrinth::onmaze(const aic2Vect& pnt) const
{
	for (int i = 0;i < _maze.size();i++)
	{
		if (_maze[i].inside(pnt))
			return true;
	}
	
	return false;
}
	
bool lcLabyrinth::hit(const aic2Vect& from, const aic2Vect& vel, 
											double& time, double& angle) const
{
	time = 1; // For a hit 0 < time < 1
	
	double tt, ang;
	
	for (int i = 0;i < _maze.size();i++)
	{
		if (_maze[i].hit(from, vel, tt, ang))
		{
			if (tt < time)
			{
				time = tt;
				angle = ang;
			}
		}
	}
	
	return (time < 1);
}
	
void lcLabyrinth::_assign(const lcLabyrinth& rhs)
{
	_maze = rhs._maze;
	_isPictureUpdated = false;
}

void lcLabyrinth::_generatePicture()
{
	if (_isPictureUpdated)	// No need to regenerate the picture;
		return;
	
	QPainter p;
	double scale(pictureScale());
	
	p.begin(&_picture);
	
	p.setBrush(QBrush(QColor("Black")));
	p.drawRect(0, 0, pictureScale(), pictureScale());

	p.setPen(QPen(QColor("Green"), 1));
	p.setBrush(QBrush(QColor("Green")));
	
	for (int i = 0;i < _maze.size();i++)
	{
		const aic2Rect& rc = _maze[i];
		
		p.drawRect(int(rc.left() * scale), int(rc.top() * scale), 
								int(rc.width() * scale), int(rc.height() * scale));
	}
	
	p.end();
	_isPictureUpdated = true;
}


//=============================================================================
//=============================================================================
//=============================================================================


struct SetTimeFunc 
{
	unsigned long t;

	void operator()(lcHitRecord& hit) { hit.timestamp(t); };
};

void lcDiscoveredLabyrinth::addHits(const vector<lcHitRecord>& hits)
{
	SetTimeFunc stf;
	
	int oldSize = _newHits.size();
	
	_newHits.insert(_newHits.end(), hits.begin(), hits.end());
	
	if (hits.size() > 0)
	{
		stf.t = _curTime;
		for_each(_newHits.begin() + oldSize, _newHits.end(), stf); // Set timestamp
				
		_hits.insert(_hits.end(), _newHits.begin(), _newHits.end()); // Copy to _hits
	}
}
	
void lcDiscoveredLabyrinth::_draw(QPainter& p, 
						const vector<lcHitRecord>& hits) const
{
	double scale = double(pictureScale());
	
	// For each hits in the list, draw item
	p.save();
	
	QPen pb(QColor("Red"));
	p.setPen(pb);
	p.setBackgroundColor(QColor("Black"));
	QBrush br(QColor("Black"), SolidPattern);

	for(vector<lcHitRecord>::const_iterator iter = hits.begin();
			iter != hits.end(); iter++)
	{
		const lcHitRecord& hit = *iter;

		// Set the rotation of the painter.
		// The normal with the angle 0 corresponds to the vertical wall hit from 
		// right

//		printf ("Time %ld, Wall at (%f, %f) with normal at %f degrees\n", 
//						hit.timestamp(), hit.at().x(), hit.at().y(), 180 * hit.dir() / M_PI);
		p.setWorldMatrix(QWMatrix(cos(hit.dir()), sin(hit.dir()), 
													 -sin(hit.dir()), cos(hit.dir()), 
													 hit.at().x() * scale , hit.at().y() * scale ), 
										false);	// Replace the matrix
		
		// Now I can safely draw a patch

		p.fillRect(-int(0.01 * scale), -int(0.01 * scale),
								0, int(0.01 * scale), br);

		p.drawLine(0, -int(0.01 * scale), 
								0, int(0.01 * scale));
	}
	
	p.restore();
}

//=============================================================================
//=============================================================================
//=============================================================================

lcJabaNode::lcJabaNode(const lcLabyrinth * labyrinth, 
					const aic2Vect& startPos, double strength)
	:
	_labyrinth(labyrinth),
	_presumedPos(HUGE_VAL, HUGE_VAL), 	// Invalid position in the beginning
	_strength(strength),
	_pos(startPos), // Real position
	_dirU(-1.0, 0) // Pointing down
{
	_mqueue = list<lcJabaEtherMessage>();
}

lcJabaNode::~lcJabaNode()
{
}
	
bool lcJabaNode::turnAndMove(double alpha)
{
	// Compute the new direction of motion
#ifdef _NOISE_INSERT
#endif
	_dirU.rotate(alpha);
	_dirU /= _dirU.r(); // Renormalize
	
	// The velocity of the jaba
	aic2Vect vel = _dirU * lvJabaSpeed;
	
	// Compute the path of the jaba. 
	// If jaba does not hit anything, then report no hit.
	// if it hits an obstacle with the angle to normal < 45 degrees report a hit. 
	// If the angle was > 45 degrees, then ricochet, and repeat the loop 
	// until the timeslice is done
	double time = 0;
	while (time < 1.0)
	{
		double t;
		double angle;
		
		bool ht = _labyrinth->hit(_pos, vel, t, angle);
		if (ht) // The point should be a bit off the wall
		{
			t -= 2 * DISTANCE_UNIT / vel.r();
		}
		
		if (ht && (time + t) < 1)
		{
#ifdef _NOISE_INSERT
#endif
//			printf ("Hit at time %f, angle %f", time + t, 180.0 / M_PI * angle);
			time += t;
			// The next leg of the path hit before the timeslice ended
			if (fabs(angle) < (M_PI / 4)) 	// Hit
			{
				if (time > 0) // It did not try to ram the wall
				{
					_pos += vel * t;
//					printf (" HIT\n");
				}
				return true;
			}
			else
			{
//				printf (" RICOCHET\n");				
				_pos += vel * t;
				vel.rotate(M_PI - 2 * angle); // Ricochet
				_dirU.rotate(M_PI - 2 * angle); // Rotate the direction vector
			}
		}
		else	// The jaba has moved without hitting anything.
		{
			_pos += vel * (1.0 - time);
			time = 1.0;
		}
	}
	return false;
}

//=============================================================================

void lcJabaTower::processMessages(const list<lcJabaReceivedMessage>& msgs)
{
	// Create a message queue which contains a single message
	addMessageToQueue(lcJabaSentMessage(_presumedPos, strength()), _cell);
	
	// For each message if it is a hit, record it in the discovered labyrinth
	list<lcJabaReceivedMessage>::const_iterator msg;

	vector<lcHitRecord>	hits(0);
	
	for (msg = msgs.begin(); msg != msgs.end();msg++)
	{
		const lcJabaReceivedMessage& m = *msg;

		if (m.hit())
		{
			hits.push_back(lcHitRecord(m.hitPos(), m.hitNormal().angle()));
		}
	}
	_dlabyrinth->addHits(hits);
}

void lcJabaTower::draw(QPainter& p) const
{
	// Draw the tower
	double scale(pictureScale());
	
	p.setPen(QPen(QColor("Blue"), 1));
	p.setBrush(QBrush(QColor("Blue")));
	
	int cx = int(_presumedPos.x() * scale);
	int cy = int(_presumedPos.y() * scale);
	int d = int (0.06 * scale);
	
//	printf ("Drawing tower at %d %d\n", cx, cy);
	p.drawEllipse(cx - d / 2, cy - d / 2, d, d);
}

//=============================================================================
//=============================================================================

lcJabaMobile::lcJabaMobile(const lcLabyrinth *labyrinth, 
					const aic2Vect& startPos, double strength)
	:
	lcJabaNode(labyrinth, startPos, strength),
	_presumedPrevPos(HUGE_VAL, HUGE_VAL), 	// Invalid values
	_prevHit(false),
	_curHits(intRnd(HITS_REPORT))
{
}
	
void lcJabaMobile::draw(QPainter& p) const
{
	// Draw the tower
	double scale(pictureScale());

	RasterOp r = p.rasterOp();
	p.setRasterOp(XorROP);

	p.setPen(QPen(QColor("Cyan"), 1));
	p.setBrush(QBrush(QColor("Cyan")));

	// Draw the jabas at the real, not presumed position
	int cx = int(pos().x() * scale);
	int cy = int(pos().y() * scale);
	int d = int (0.01 * scale);
	
//	printf ("Drawing jaba at %d %d\n", cx, cy);
	p.drawEllipse(cx - d / 2, cy - d / 2, d, d);
	
	aic2Vect to = (pos() + dir() * 0.01);
	int tox = int(to.x() * scale);
	int toy = int(to.y() * scale);
	p.drawLine(cx, cy, tox, toy);
	
	p.setRasterOp(r);	
}
		
//=============================================================================
//*****************************************************************************
//=============================================================================
//*****************************************************************************
//=============================================================================

/*
 * THIS IS THE MOBILE NODE CODE. THE WHOLE REASON FOR EVERYTHING ELSE!!!!!
 */

double computeDistance(double ini, double rec, double unit = 0.0001)
{
	return sqrt(ini / rec) * unit;
}

// Get the minimum distance from p to p1, p2, p3
double minDistance(const aic2Vect& p, const aic2Vect& p1, 
								aic2Vect& p2, aic2Vect& p3)
{
	return aicMin (
							aicMin(
								(p1-p).r(), 
								(p2-p).r()), 
							(p3-p).r());
}

void collectDistances(const list<lcJabaReceivedMessage>& msgs, 
					vector<ais2TriagData>& dists)
{
	for (list<lcJabaReceivedMessage>::const_iterator msg = msgs.begin(); 
			msg != msgs.end();msg++)
	{
		dists.push_back(ais2TriagData(msg->messagePos(), 
						computeDistance(msg->strength(), msg->receiveStrength())));
	}
}

void lcJabaMobile::processMessages(const list<lcJabaReceivedMessage>& msgs)
{
	// The maximum distance at which the message would be heard
	double signalDist = computeDistance(strength(), 1.0);

	// Data used in the processing (temporary state)
	// Iterators
	list<lcJabaReceivedMessage>::const_iterator msg_iter;
	vector<ais2TriagData>::const_iterator cd_iter;
	
	// State
	double minDist; 	// Minimum distance to towers
	vector<ais2TriagData> coords; // Coordinate/distance pairs for all heard jabas
	aic2Vect vel; 		// The current velocity
	aic2Vect newVel;	// New velocity (actually only the direction is used)

	bool hit = false;	// Is there a collision to report
	aic2Vect hitPos;	// Collision position
	aic2Vect hitNormal; // Collision normal (assumed to be the direction of hit)
	
	aic2Vect closestPoint(0, 0);	// The closest jaba 
	double closestDist = HUGE_VAL;	// The distance to closest jaba
	
	//------------------------------------------------------------------------
	// JABA operation cycle
	//------------------------------------------------------------------------
	

	// Collect distance/coordinate pairs
	collectDistances(msgs, coords);
		
	// Compute the new coordinates of the jaba, and estimate the 	
	// current velocity
	{ 
		try
		{
			_presumedPos = aifTriangulate2D(coords);
			// Compute the minimum distance to the towers
			minDist = minDistance(_presumedPos, coords[0].p, 
						coords[1].p, coords[2].p);
		}
		catch(int&)
		{
			return; 	// For now
		}
		// Estimate the currect direction.
		vel = _presumedPos - _presumedPrevPos;
	}
	

	// Process all of the messages (the first 3, that are expected to 
	// be the towers)
	if (msgs.size() > 3)
	{	
		// Skip first three messages
		msg_iter = msgs.begin(); 
		cd_iter = coords.begin();
		
		for (; msg_iter != msgs.end();msg_iter++, cd_iter++)
		{
			const lcJabaReceivedMessage& msg = *msg_iter;
			const ais2TriagData& cd = *cd_iter;
			
			// Compute the direction from the jaba to the sender of the current 
			// message
			aic2Vect dir = cd.p - _presumedPos;
			
			// Add the opposite direction to the new velocity estimate, as there
			// should be a motivation to run away from them
			newVel -= dir / dir.r() / cd.dist;
				
			if (cd.dist < closestDist)
			{
				closestPoint = cd.p;
				closestDist = cd.dist;
			}

			// If a message was a collision report, 
			if (!hit && msg.hit())
			{
				// If it makes sense to resend a message (in A* sense, I am closer
				// to one of the towers than the jaba that sent the message)
				if (minDist < minDistance(cd.p, coords[0].p, coords[1].p, coords[2].p))
				{
					hit = true;
					hitPos = msg.hitPos();
					hitNormal = msg.hitNormal();
						
//						printf ("At ");_presumedPos.print();
//						printf ("\t resending hit from"); hitPos.print();
				}
			}
		}
	}
	
	// If the system has hit a wall since the previous message, then
	// my hit takes the presedence over everybody else
	if (_prevHit && vel.r() > 0)
	{
		hit = true;
		hitPos = _presumedPos;
		hitNormal = -vel;
	}

	// Send out the message
	addMessageToQueue(
						lcJabaSentMessage(_presumedPos, strength(), hit, 
						hitPos, hitNormal), 
						intRnd(totalCells - 3) + 3);

	
	// Now move.
	if (_pprevHit || intRnd(EXPLORE_TIMES) == 0) // Make a random move
	{
		_prevHit = turnAndMove(M_PI * 2 * realRnd());
	}
	else if (!_prevHit && 
			(closestDist < HUGE_VAL &&		// If there was some contact
				closestDist > signalDist * 0.75)) // Do not loose contact
	{
		_prevHit = turnAndMove((closestPoint - _presumedPos).angle() - vel.angle());
	}
	else	// Move away from everybody else
	{
		_prevHit = turnAndMove(_prevHit ? M_PI: 
							/*newVel.angle() - vel.angle()*/ 0);
		
	}
	
	if (_prevHit)	
		_curHits = (_curHits + 1) % HITS_REPORT;
	
	_presumedPrevPos = _presumedPos;
	_pprevHit = _prevHit;
}