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

Sector::Sector(int id):
	_id(id),
	_maxSpeed(2),
	_type(LANE),
	_hasCarSink(false),
	_polygons(),
	_entrySectors(),
	_exitSectors(),
	_parallelSectors(),
	_s2pmapping(),
	_cars(),
	_carEventListeners()
{	
}

Sector::~Sector()
{
	//delete all the dynamic-allocated memory of polygons, as some of them are shared by
	//two sectors, only delete them the first time we encounter them
	for (PolygonVector::iterator it = _polygons.begin(); it != _polygons.end(); it++) {
		if ((*it) != NULL) {
			delete (*it);
			(*it) = NULL;
		}
	}
}

void Sector::visualize()
{
	for (PolygonVector::iterator bit = _polygons.begin(); bit != _polygons.end(); bit++) dsDrawPolygon(*(*bit), WHITE);
}

void Sector::registerCar(Car* car)
{
	mASSERT(car != NULL);
	for (CarVector::const_iterator it = _cars.begin(); it != _cars.end(); it++)
		if ((*it) == car) {
			_DEBUG("sector[" << _id << "]: car[" << car->getID() <<"] asked to register but already registered in this sector, registered cars" << _cars);
			return;
		}
	_cars.push_back(car);
	for (CarVector::iterator it = _carEventListeners.begin(); it != _carEventListeners.end(); it++)
		if (*it != car) (*it)->carAddedToSectorHandler(this, car);
	_DEBUG("sector[" << _id << "]: car[" << car->getID() <<"] newly registered, current cars" << _cars);
}

void Sector::deregisterCar(Car* car)
{
	mASSERT(car != NULL);
	for (CarVector::iterator it = _cars.begin(); it != _cars.end(); it++) {
		if ((*it) == car) {
			_cars.erase(it);
			for (CarVector::iterator it = _carEventListeners.begin(); it != _carEventListeners.end(); it++)
				if (*it != car) (*it)->carRemovedFromSectorHandler(this, car);
			_DEBUG("sector[" << _id << "]: car[" << car->getID() <<"] deregistered, remaining cars" << _cars);
			return;
		}
	}
	_DEBUG("sector[" << _id << "]: car[" << car->getID() <<"] asked to deregister but not registered in this sector, registered cars" << _cars);
}

void Sector::addCarEventListener(Car* car)
{
	mASSERT(car != NULL);
	if (hasCarEventListener(car)) {
		_DEBUG("sector[" << _id << "]: car[" << car->getID() << "] already in listeners, current listeners list: " << _carEventListeners);
		return;
	}
	_carEventListeners.push_back(car);
	_DEBUG("sector[" << _id << "]: added car[" << car->getID() << "] to listeners, current listeners list: " << _carEventListeners);
}

void Sector::removeCarEventListener(Car* car)
{
	mASSERT(car != NULL);
	for (CarVector::iterator it = _carEventListeners.begin(); it != _carEventListeners.end(); it++)
		if (*it == car) {
			_carEventListeners.erase(it);
			_DEBUG("sector[" << _id << "]: removed car[" << car->getID() << "] from listeners, current listeners list: " << _carEventListeners);
			return;
		}
	_DEBUG("sector[" << _id << "]: car[" << car->getID() << "] can't be removed as not in listeners, current listeners list: " << _carEventListeners);
}

bool Sector::hasCarEventListener(Car* car) const
{
	for (CarVector::const_iterator it = _carEventListeners.begin(); it != _carEventListeners.end(); it++)
		if (*it == car) return true;
	return false;
}

bool Sector::hasEntrySector(Sector* sector) const
{
	for (SectorVector::const_iterator it = _entrySectors.begin(); it != _entrySectors.end(); it++)
		if (*it == sector) return true;
	return false;
}

bool Sector::hasExitSector(Sector* sector) const
{
	for (SectorVector::const_iterator it = _exitSectors.begin(); it != _exitSectors.end(); it++)
		if (*it == sector) return true;
	return false;
}

bool Sector::hasParallelSector(Sector* sector) const
{
	for (SectorVector::const_iterator it = _parallelSectors.begin(); it != _parallelSectors.end(); it++)
		if (*it == sector) return true;
	return false;
}


Line Sector::getTransitLine(Sector* s)
{
	mASSERT(s != NULL);
	Line l;
	SectorPolygonMapping::iterator it;
	if ((it = _s2pmapping.find(s)) == _s2pmapping.end()) {
		char buffer[100];
		sprintf(buffer, "sector ID=%d not in _s2pmapping", s->_id);
		throw SimulatorException(buffer);
	}
	Polygon* p = (*it).second;
	l._a = p->front();
	l._b = p->back();
	Vector3r v = (l._b - l._a).getPerpendicular();
	v.normalize();
	if (containsPoint(midPoint(l) + v)) {
		l.invert();
		_DEBUG("Sector ID=" << _id << ": inverting transit line to sector ID=" << s->getID());
	}
	return l;
}

bool Sector::circularize()
{
	bool found;
	PolygonVector circle;
	PolygonVector::iterator it;
	Polygon* p = _polygons.front();
	_polygons.pop_front();
	circle.push_back(p);
	Vector3r startPoint = p->front();
	Vector3r endPoint = p->back();
	_DEBUG("initial startPoint=" << startPoint << ", endPoint=" << endPoint);
	while (_polygons.size() > 0) {
		found = false;
		for (it = _polygons.begin(); it != _polygons.end(); it++) {
			p = *it;
			if (p->front() == endPoint) {
				//the ideal situation - end of first polygon is the start of the next
				//just copy over and NULL the original
				circle.push_back(p);
				found = true;
				_DEBUG("pushed back " << p->front() << "," << p->back());
				break;
			} else if (p->front() == startPoint) {
				p = new Polygon(p->rbegin(), p->rend());
				circle.push_front(p);
				found = true;
				_DEBUG("reversed and pushed front " << p->front() << "," << p->back());
				break;
			} else if (p->back() == startPoint) {
				//the second best situation - this polygon precedes the first one
				//copy it to the front
				circle.push_front(p);
				found = true;
				_DEBUG("pushed front " << p->front() << "," << p->back());
				break;
			} else if (p->back() == endPoint) {
				p = new Polygon(p->rbegin(), p->rend());
				circle.push_back(p);
				found = true;
				_DEBUG("reversed and pushed back " << p->front() << "," << p->back());
				break;
			}
		}
		if (found) {
			_polygons.erase(it);
			startPoint = p->front();
			endPoint = p->back();
			_DEBUG("new startPoint=" << startPoint << ", endPoint=" << endPoint);
		} else break;
	}
	if (!found) {
		_ERROR("Circularization failed for sector id=" << _id << " - could not find connection for endpoints " << startPoint << " and " << endPoint);
		return false;		
	}
	Polygon* p1 = NULL;
	for (unsigned i = 0; i < circle.size(); i++) {
		p = circle.at(i);
		p1 = circle.at((i + 1) % circle.size());
		if (p->back() != p1->front()){ //next adjacent polygon not found
			_ERROR("Circularization failed for sector id=" << _id << " - not connected at " << p->back() << " != " << p1->front());
			return false;
		}
	}
	_polygons.insert(_polygons.begin(), circle.begin(), circle.end());
	
	if (_entrySectors.size() > 1 || _exitSectors.size() > 1) _type = JUNCTION;
	else _type = LANE;
	
	_DEBUG("Successfully circularized sector id=" << _id << " of type " << _type);
	return true;
}

bool Sector::containsPoint(const Vector3r &point) const
{
	Polygon p;
	//concatenate all polygons to one first
	for (PolygonVector::const_iterator it = _polygons.begin(); it != _polygons.end(); it++) {
		p.insert(p.end(), (*it)->begin(), (*it)->end());
	}
	return pointInPolygon(p, point);
}

std::string Sector::describeLinks() const
{
	SectorVector::const_iterator it;
	std::ostringstream output;
	output << "Entry sectors=[";
	for (it = _entrySectors.begin(); it != _entrySectors.end(); it++) {
		output << (*it)->getID() << ",";
	}
	output << "], exit sectors=[";
	for (it = _exitSectors.begin(); it != _exitSectors.end(); it++) {
		output << (*it)->getID() << ",";
	}
	output << "], parallel sectors=[";
	for (it = _parallelSectors.begin(); it != _parallelSectors.end(); it++) {
		output << (*it)->getID() << ",";
	}
	output << "]";
	return output.str();
}

std::string Sector::toString() const
{
	std::ostringstream oss;
	oss << "Sector[" << _id << "](";
//	for (PolygonVector::const_iterator it = _polygons.begin(); it != _polygons.end(); it++) oss << *it << "; ";
	oss << "), _cars" << _cars << ", _carEventListeners" << _carEventListeners;
	return oss.str();
}

std::ostream& operator<<(std::ostream& os, const Sector* s)
{
	os << s->toString();
	return os;
}
