#include "Simulator.h"
#include "iface/Animated.h"
#include "iface/Visualized.h"
#include "sim/Sector.h"
#include "sim/SimulationObject.h"
#include "sim/Car.h"
#include "Logger.h"
#include "conf/ConfigurationParser.h"
#include "sim/Building.h"

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <ctime>
#include <cstdlib>
#include <sstream>

using std::string;
using std::cout;
using std::endl;

Simulator* Simulator::_instance = NULL;

Simulator* Simulator::getInstance() {
	if (_instance == NULL) _instance = new Simulator();
	return _instance;
}

Simulator::Simulator():
	_numOfCars(-1),
	_visualize(true)
{
	_fn.version = DS_VERSION;
	_fn.start = &gStart;
	_fn.step = &gLoop;
	_fn.command = &gCommand;
	_fn.stop = 0;
}

Simulator::~Simulator()
{
	for (SOContainer::iterator oit = _sobjects.begin(); oit != _sobjects.end(); oit++)
		delete (*oit).second;
	_sobjects.clear();
	
	for (SectorMap::iterator sit = _sectors.begin(); sit != _sectors.end(); sit++)
		delete (*sit).second;
	_sectors.clear();
	
	dCloseODE();
}

void Simulator::printHelp()
{
	_DEBUG("Printing help");
	cout << "Simulator" << endl << endl;
	cout << "usage: simulator [parameters]" << endl;
	cout << "possible parameters:" << endl;
	cout << "\t-h - shows this help" << endl;
	cout << "\t-n - runs the simulation in headless mode - no visualization is done" << endl;
	cout << "\t-v<verbosity> - sets verbosity of logging, possible are: DEBUG < INFO < WARN < ERROR < FATAL < NONE, default is INFO" << endl;
	cout << "\t-l<log file> - sets name of the log file, default is log.txt" << endl;
	cout << "\t-s<setup file> - overall simulation configuration file, default is ./ini/setup.ini" << endl;
	cout << "\t-c<cars file> - configuration file for cars, default is ./ini/cars.ini" << endl;
	cout << "\t-w<world file> - configuration file with topography, default is ./ini/world.ini" << endl;
	cout << "for help on configuration files see in them directly" << endl << endl;
	cout << "Useful keystrokes during simulation:" << endl;
	cout << "\tq,Q - quits the simulation" << endl;
	cout << "\tc - cycles through cars in car-follow camera mode" << endl;	
	cout << "\ts - cycles through static viewpoints in static camera mode" << endl;	
	cout << "\ti - print info about current state of simulation to the console" << endl;	
	cout << "\t1 - show/hide car direction vector" << endl;	
	cout << "\t2 - show/hide car Bezier tracking line" << endl;	
	cout << "\t3 - show/hide car right and left tracking lines" << endl;	
	cout << "\t4 - show/hide car intersection points for sector borders" << endl;	
}

void Simulator::init(const std::string simfile, const std::string carsfile, const std::string worldfile, bool visualize)
{
	_INFO("starting initialization");
	_visualize = visualize;
	ConfigurationParser cp;
	
	//parse the simulation configuration file and set basic parameters
	cp.parseSimSetup(simfile.c_str());
	_world = dWorldCreate();
	dWorldSetGravity(_world, 0, 0, _gravity);
	dWorldSetERP(_world, _ERP);
	dWorldSetCFM(_world, _CFM);
	_space = dSimpleSpaceCreate(0);
	_contactgroup = dJointGroupCreate(0);
	_plane = dCreatePlane(_space, 0, 0, 1, 0);
	SimulationObject::setWorldID(_world);
	SimulationObject::setSpaceID(_space);
	_DEBUG("world succesfully initiated");

	//parse the cars file and create cars
	cp.parseObjectsSetup(carsfile.c_str());
	_DEBUG("cars succesfully created");
	
	//parse the topography file and create sectors
	cp.parseTopographySetup(worldfile.c_str());
	for (SectorMap::iterator sit = _sectors.begin(); sit != _sectors.end(); sit++) {
		if (!(*sit).second->circularize()) {
			char buffer[100];
			sprintf(buffer, "cannot circularize sector %d", (*sit).first);
			throw SimulatorException(buffer);
		}
		_DEBUG("Sector [" << (*sit).second->getID() << "] links: " << (*sit).second->describeLinks());
	}
	_DEBUG("sectors succesfully created");
	
	_INFO("simulation initialized");
}

void Simulator::start()
{
	//setting the vizualization viewpoint
	_DEBUG("setting viewpoint");
	_lastCarViewpoint = 0;
	_lastStaticViewpoint = 0;
	_viewpointType = VP_STATIC;
	ViewPoint vp = _viewpoints.at(0);
	dsSetViewpoint(vp._xyz, vp._hpr);
	_DEBUG("viewpoint set");
}

void Simulator::loop(int pause)
{
	if (!pause) {
		if (_viewpointType == VP_CAR_FOLLOW) {// movable viewpoint
			Car* car = dynamic_cast<Car*>(_sobjects[_lastCarViewpoint]);
			if (car == 0) {
				_INFO("Last followed car is not available anymore, setting viewpoint to last selected static viewpoint[" << _lastStaticViewpoint << "]");
				_viewpointType = VP_STATIC;
				ViewPoint vp = _viewpoints.at(_lastStaticViewpoint);
				dsSetViewpoint(vp._xyz, vp._hpr);
			} else {
				float xyz[3];
				float hpr[3];
				dsGetViewpoint(xyz, hpr);
				
				Vector3r prevPos = car->getPrevPosition();
				Vector3r deltaPos1(xyz[0] - prevPos[0], xyz[1] - prevPos[1], xyz[2] - prevPos[2]);
				Vector3r movement = car->getPosition() - car->getPrevPosition();
				
				Vector3r prevDirection = car->getPrevDirection();
				Vector3r direction = car->getDirection();
				float deltaHeading = RAD2DEG(atan2(direction.y, direction.x)) - RAD2DEG(atan2(prevDirection.y, prevDirection.x));
				
				Vector3r deltaPos2 = deltaPos1;
				deltaPos2.rotate(0, 0, deltaHeading);
				xyz[0] += movement.x + deltaPos2.x - deltaPos1.x;
				xyz[1] += movement.y + deltaPos2.y - deltaPos1.y;
				xyz[2] += movement.z + deltaPos2.z - deltaPos1.z;
				
				hpr[0] += deltaHeading;
				dsSetViewpoint(xyz, hpr);
			}
		}
		
		//add all simulation objects scheduled for addition
		if (_scheduledForAddition.size() > 0) addScheduledObjects();
		//remove all simulation objects scheduled for removal
		if (_scheduledForRemoval.size() > 0) removeScheduledObjects();
	
		AOContainer::iterator it;
		for (it = _aobjects.begin(); it != _aobjects.end(); it++) {
			(*it).second->computeStep();
		}
		for (it = _aobjects.begin(); it != _aobjects.end(); it++) {
			(*it).second->performStep();
		}
		dSpaceCollide(_space, 0, &gNearCallback);
		dWorldQuickStep(_world, _simstep);
		dJointGroupEmpty(_contactgroup);
	}
	
	//now visualize all objects
	for (VOContainer::iterator it = _vobjects.begin(); it != _vobjects.end(); it++) (*it).second->visualize();

	double dt = dsElapsedTime();
	double rem = _simstep - dt;
//	_DEBUG("dt=" << dt << ", rem=" << rem);
	if (rem > 0) dsSleep(rem * 1000);
}

void Simulator::loopHeadLess()
{
	//add all simulation objects scheduled for addition
	if (_scheduledForAddition.size() > 0) addScheduledObjects();
	//remove all simulation objects scheduled for removal
	if (_scheduledForRemoval.size() > 0) removeScheduledObjects();

	AOContainer::iterator it;
	for (it = _aobjects.begin(); it != _aobjects.end(); it++) {
//				_DEBUG("Calling computeStep() for object[" << dynamic_cast<SimulationObject*>((*it))->getID() << "]");
		(*it).second->computeStep();
	}
	for (it = _aobjects.begin(); it != _aobjects.end(); it++) {
//				_DEBUG("Calling performStep() for object[" << dynamic_cast<SimulationObject*>((*it))->getID() << "]");
		(*it).second->performStep();
	}
	dSpaceCollide(_space, 0, &gNearCallback);
	dWorldQuickStep(_world, _simstep);
	dJointGroupEmpty(_contactgroup);	
}

void Simulator::command(int cmd)
{
	switch (cmd) {
		case 'q':
		case 'Q':
			_INFO("Received keystroke " << (char)cmd << ", ending simulation now");
			dsStop();
			break;
		case 'c':
			cycleViewpointObject(true);
			break;
		case 'C':
			cycleViewpointObject(false);
			break;
		case 's':
			cycleStaticViewPoints(true);
			break;
		case 'S':
			cycleStaticViewPoints(false);
			break;
		case 'i':
			cout << getInfo();
			break;
		case '1':
			for (SOContainer::iterator oit = _sobjects.begin(); oit != _sobjects.end(); oit++) {
				Car* car = dynamic_cast<Car*>((*oit).second);
				if (car != 0) car->toggleShowCarDirection();
			}
			break;
		case '2':
			for (SOContainer::iterator oit = _sobjects.begin(); oit != _sobjects.end(); oit++) {
				Car* car = dynamic_cast<Car*>((*oit).second);
				if (car != 0) car->toggleShowCarBezierPath();
			}
			break;
		case '3':
			for (SOContainer::iterator oit = _sobjects.begin(); oit != _sobjects.end(); oit++) {
				Car* car = dynamic_cast<Car*>((*oit).second);
				if (car != 0) car->toggleShowCarBorderPaths();
			}
			break;
		case '4':
			for (SOContainer::iterator oit = _sobjects.begin(); oit != _sobjects.end(); oit++) {
				Car* car = dynamic_cast<Car*>((*oit).second);
				if (car != 0) car->toggleShowCarIntersectionPoints();
			}
			break;
		case 'e':
			FILE* f = fopen("state.dif","wt");
			if (f) {
				dWorldExportDIF(_world, f, "simulator");
				fclose(f);
			}
			break;
	}
}

void Simulator::nearCallback(void *data, dGeomID o1, dGeomID o2)
{
	if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) {
		// colliding a space with something
		dSpaceCollide2(o1, o2, data, &gNearCallback);
		// collide all geoms internal to the space(s)
		if (dGeomIsSpace(o1)) dSpaceCollide((dxSpace*)o1, data, &gNearCallback);
		if (dGeomIsSpace(o2)) dSpaceCollide((dxSpace*)o2, data, &gNearCallback);
	} else {
		// colliding two non-space geoms
		// test if geom o1 and geom o2 can collide
		long cat1 = dGeomGetCategoryBits(o1);
		long cat2 = dGeomGetCategoryBits(o2);
		long col1 = dGeomGetCollideBits(o1);
		long col2 = dGeomGetCollideBits(o2);
//		_DEBUG("cat1=" << cat1 << ", col2=" << col2 << ", cat2=" << cat2 << ", col1=" << col1);
		if ((cat1 & col2) || (cat2 & col1)) {
//			_DEBUG("colliding " << dGeomGetClass(o1) << " with " << dGeomGetClass(o2));
			// generate contact points between o1 and o2
		    const int N = 10;
			dContact contact[N];
			int n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact));
			if (n > 0) {
				for (int i = 0; i < n; i++) {
					contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1;
					contact[i].surface.mu = dInfinity;
					contact[i].surface.slip1 = 0.1;
					contact[i].surface.slip2 = 0.1;
					contact[i].surface.soft_erp = 0.5;
					contact[i].surface.soft_cfm = 0.3;
					dJointID c = dJointCreateContact(_world, _contactgroup, &contact[i]);
					dJointAttach(c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
				}
			}
		}
	}
}

void Simulator::run()
{
	_DEBUG("initializing all animated simulation objects");
	AOContainer::iterator it;
	for (it = _aobjects.begin(); it != _aobjects.end(); it++) 
		(*it).second->initialize();
	
	if (_visualize) {
		_INFO("Starting the main simulation loop in visualization mode");
		dsSimulationLoop(0, NULL, 352, 288, &_fn);
	} else {
		_INFO("Starting the main simulation loop in headless mode");
		do loopHeadLess(); while (_numOfCars == -1 || _numOfCars > 0);
		_INFO(getInfo());
	}
}

string Simulator::getInfo()
{
	std::ostringstream oss;
	oss << "--- Printing current state info ---" << endl;
	oss << endl << "Sectors:" << endl;
	for (SectorMap::iterator sit = _sectors.begin(); sit != _sectors.end(); sit++)
		oss << (*sit).second->toString() << endl;
	oss << endl << "Simulation objects:" << endl;
	for (SOContainer::iterator cit = _sobjects.begin(); cit != _sobjects.end(); cit++)
		oss << (*cit).second->toString() << endl;
	oss << endl << "---  End of current state info  ---" << endl;
	return oss.str();
}

void Simulator::scheduleForAddition(SimulationObject* o)
{
	_scheduledForAddition.push_back(o);
	_DEBUG("Object[" << o->getID() << "] scheduled for addition");
}

void Simulator::scheduleForRemoval(SimulationObject* o)
{
	_DEBUG("Object[" << o->getID() << "] scheduled for removal");
	_scheduledForRemoval.push_back(o);
}

void Simulator::addScheduledObjects()
{
	for (SOVector::iterator it = _scheduledForAddition.begin(); it != _scheduledForAddition.end(); it++) {
		SOContainer::iterator sit = _sobjects.find((*it)->getID());
		if (sit != _sobjects.end()) {
			_ERROR("Simulation object[" << (*it)->getID() << "] scheduled for addition but already registered in simulation");
			continue;
		}
		addSimulationObject(*it);
		//initialize if object is animated
		Animated* a = dynamic_cast<Animated*>(*it);
		if (a != NULL) a->initialize();
		_INFO("Inserted new object[" << (*it)->getID() << "] to simulation");
	}
	_scheduledForAddition.clear();
}

void Simulator::removeScheduledObjects()
{
	for (SOVector::iterator it = _scheduledForRemoval.begin(); it != _scheduledForRemoval.end(); it++) {
		SOContainer::iterator sit = _sobjects.find((*it)->getID());
		if (sit == _sobjects.end()) {
			_ERROR("Simulation object[" << (*it)->getID() << "] scheduled for removal but nonexistent in simulation");
			continue;
		}
		removeSimulationObject(*it);
	}
	_scheduledForRemoval.clear();
}

void Simulator::addSimulationObject(SimulationObject* object)
{
	mASSERT(object != NULL);
	int id = object->getID();
	_DEBUG("registering new object[" << id << "] to simulation");
	_sobjects[id] = object;
	Animated* animated = dynamic_cast<Animated*>(object);
	if (animated != 0) {
		_DEBUG("object[" << id << "] is animated, adding to _aobjects");
		_aobjects[id] = animated;
	}
	Visualized* visualized = dynamic_cast<Visualized*>(object);
	if (visualized != 0) {
		_DEBUG("object[" << id << "] is visualized, adding to _vobjects");
		_vobjects[id] = visualized;
	}
	Car* c = dynamic_cast<Car*>(object);
	if (c != 0)
		if (_numOfCars == -1) _numOfCars = 1;
		else _numOfCars++;
}

void Simulator::removeSimulationObject(SimulationObject* object)
{
	mASSERT(object != NULL);
	int id = object->getID();
	SOContainer::iterator it = _sobjects.find(object->getID());
	if (it == _sobjects.end()) { _ERROR("Asked to remove already removed object[" << id << "]"); }
	else _sobjects.erase(id);
	if ((dynamic_cast<Animated*>(object)) != 0) {
		_DEBUG("object[" << id << "] is animated, removing from _aobjects");
		AOContainer::iterator ait = _aobjects.find(id);
		if (ait != _aobjects.end()) _aobjects.erase(id);
	}
	if ((dynamic_cast<Visualized*>(object)) != 0) {
		_DEBUG("object[" << id << "] is visualized, removing from _vobjects");
		VOContainer::iterator vit = _vobjects.find(id);
		if (vit != _vobjects.end()) _vobjects.erase(id);
	}
	//ugly hack to remove car's listeners from all sectors it is subscribed to
	//can't be done otherwise since the car does not keep list of sectors it has added its listener to
	Car* c = NULL;
	if ((c = dynamic_cast<Car*>(object)) != 0) {
		_numOfCars--;
		std::ostringstream history;
		SectorVector ps = c->getPassedSectors();
		for (SectorVector::iterator it = ps.begin(); it != ps.end(); it++) {
			history << (*it)->getID() << ";";
		}
		_INFO("car[" << id << "] history of passed sectors: " << history.str());
		_DEBUG("object[" << id << "] is a car, removing all it's listeners");
		for (SectorMap::iterator it = _sectors.begin(); it != _sectors.end(); it++)
			if ((*it).second->hasCarEventListener(c)) (*it).second->removeCarEventListener(c);
	}
	_INFO("Removed object[" << object->getID() << "] from simulation");
	delete object;
}

void Simulator::addSector(Sector* sector)
{
	_sectors[sector->getID()] = sector;
	_vobjects[1000*sector->getID()] = dynamic_cast<Visualized*>(sector);
}

void Simulator::addViewPoint(float* xyz, float* hpr)
{
	ViewPoint vp;
	vp._xyz[0] = xyz[0]; vp._xyz[1] = xyz[1]; vp._xyz[2] = xyz[2];
	vp._hpr[0] = hpr[0]; vp._hpr[1] = hpr[1]; vp._hpr[2] = hpr[2];
	_viewpoints.push_back(vp);
	_INFO("added viewpoint position [" << xyz[0] << "," << xyz[1] << "," << xyz[2] << "], heading [" << hpr[0] << "," << hpr[1] << "," << hpr[2] << "]");
}

void Simulator::cycleViewpointObject(bool forward)
{
	Car* car;
	_viewpointType = VP_CAR_FOLLOW;
	do {
		_lastCarViewpoint = (forward ? ++_lastCarViewpoint % _sobjects.size() : --_lastCarViewpoint % _sobjects.size());
	} while ((car = dynamic_cast<Car*>(_sobjects[_lastCarViewpoint])) == 0);
	mASSERT(car != 0);
	Vector3r position = car->getPosition();
	Vector3r direction = car->getDirection();
	position -= direction * 5;
	float xyz[3] = {position.x, position.y, position.z + 10};
	float heading = atan2(direction.y, direction.x); 
	float hpr[3] = {RAD2DEG(heading), -65, 0};
	dsSetViewpoint(xyz, hpr);
	_INFO("viewpoint changed to follow car[" << _lastCarViewpoint << "]");
}

void Simulator::cycleStaticViewPoints(bool forward)
{
	_viewpointType = VP_STATIC;
	_lastStaticViewpoint = (forward ? ++_lastStaticViewpoint % _viewpoints.size() : --_lastStaticViewpoint % _viewpoints.size());
	ViewPoint vp = _viewpoints.at(_lastStaticViewpoint);
	dsSetViewpoint(vp._xyz, vp._hpr);
	_INFO("viewpoint changed to static #" << _lastStaticViewpoint);
}
