#include "ConfigurationParser.h"

#include "../Logger.h"
#include "../Simulator.h"
#include "../sim/Sector.h"
#include "../sim/Car.h"
#include "../sim/CarSource.h"
#include "../sim/CarSink.h"
#include "../sim/Building.h"
#include "../vectorMath/mathex.h"

#include <cstdarg>
#include <stdio.h>
#include <string>
#include <iostream>
#include <ode/ode.h>

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

void ConfigurationParser::parseSimSetup(const char* filename)
{
	string token;
	dReal gravity, ERP, CFM, simstep;
	
	_INFO("opening file " << filename);
	_file.open(filename);
	if (!_file) throw ConfigurationException("Cannot open simulation setup file");
	getLine();
	parseLine("s", &token);
	if (token == "{simulation") {
		_DEBUG("read header " << token);
		while (getLine()) {
			parseLine("s", &token);
			if (token == "gravity") {
				parseLine("R", &gravity);
			} else if (token == "ERP") {
				parseLine("R", &ERP);
			} else if (token == "CFM") {
				parseLine("R", &CFM);
			} else if (token == "simstep") {
				parseLine("R", &simstep);
			} else if (token == "}") {
				break;
			} else {
				char buffer[100];
				sprintf(buffer, "unknown tag at line %d: %s", _line, token.c_str());
				throw ConfigurationException(buffer);
			}
		}
	}
	Simulator* simulator = Simulator::getInstance();
	simulator->_gravity = gravity;
	simulator->_ERP = ERP;
	simulator->_CFM = CFM;
	simulator->_simstep = simstep;
	_file.close();
	_file.clear();
	_INFO("done reading file " << filename);
}

void ConfigurationParser::parseObjectsSetup(const char* filename)
{
	string token;
	
	_INFO("opening file " << filename);
	_file.open(filename);
	if (!_file) throw ConfigurationException("Cannot open cars setup file");
	while (getLine()) {
		parseLine("s", &token);
		if (token == "{cartype") parseCar();
		else {
			char buffer[100];
			sprintf(buffer, "unknown tag at line %d: %s", _line, token.c_str());
			throw ConfigurationException(buffer);
		}
	}	
	_file.close();
	_file.clear();
	_INFO("done reading file " << filename);
}

void ConfigurationParser::parseTopographySetup(const char* filename)
{
	Simulator* simulator = Simulator::getInstance();
	string token;
	
	_INFO("opening file " << filename);
	_file.open(filename);
	if (!_file) throw ConfigurationException("Cannot open world setup file");
	while (getLine()) {
		parseLine("s", &token);
		if (token == "{viewpoint") parseViewpoint();
		else if (token == "{polygon") parsePolygon();
		else if (token == "{carsink") parseCarSink();
		else if (token == "{carsource") parseCarSource();
		else if (token == "{building") parseBuilding();
		else if (token == "{sign" || token == "{semaphore") parseSigning();
		else {
			char buffer[100];
			sprintf(buffer, "unknown tag at line %d: %s", _line, token.c_str());
			throw ConfigurationException(buffer);
		}
	}
	_file.close();
	_file.clear();
	_INFO("done reading file " << filename);
	if (simulator->_viewpoints.size() == 0) {
		_WARN("no viewpoint defined in file, creating default one at [5, -5, 5] heading {90, -10, 0}");
		float xyz[3] = {5, -5, 5};
		float hpr[3] = {90, -10, 0};
		simulator->addViewPoint(xyz, hpr);
	}
}

void ConfigurationParser::parsePolygon() {
	string token, rel;
	char buffer[100];
	int id1, id2;
	id1 = id2 = -1;
	Polygon* p = new Polygon();
	Sector* sector1 = NULL;
	Sector* sector2 = NULL;
	
	_DEBUG("polygon reading starts");
	while (getLine()) {
		parseLine("s", &token);
		if (token.at(0) == '}') {
			break;
		} else if (token == "sectors") {
			parseLine("isi", &id1, &rel, &id2);
		} else if (token == "vertex") {
			dReal x, y, z;
			parseLine("RRR", &x, &y, &z);
			Vector3r v(x, y, z);
			p->push_back(v);
			_DEBUG("adding vertex nr. " << p->size() << " [" << v.x << ","  << v.y << "," << v.z << "]");
		} else _WARN("Read unknown token: " << token << " on line " << _line);
	}
	//checking if valid data present
	if (id1 == -1) {
		sprintf(buffer, "sectors data missing at line %d", _line);
		throw ConfigurationException(buffer);
	} else if (p->size() < 2) {
		sprintf(buffer, "polygon cannot have only %d vertices (polygon %d)", p->size(), id1);
		throw ConfigurationException(buffer);
	} else if (rel != "BORDER" && rel != "TRANSIT" && rel != "EXIT" && rel != "PARALLEL") {
		sprintf(buffer, "wrong relation type found for sector id=%d: %s", id1, rel.c_str());
		throw ConfigurationException(buffer);
	} else if ((rel == "TRANSIT" || rel == "PARALLEL") && id2 == -1) {
		sprintf(buffer, "invalid id2 found for relation type %s for sector %d", rel.c_str(), id1);
		throw ConfigurationException(buffer);
	}
	_DEBUG("polygon reading finished");
	
	//find if sector already exists and create if not
	sector1 = lookupSector(id1);
	
	if (rel == "BORDER") {
		//simple, just add the polygon
		_DEBUG("border polygon, just adding to sector ID=" << id1);
		sector1->_polygons.push_back(p);
	} else {
		//find if second sector already exists and create if not
		sector2 = lookupSector(id2);
		//create a copy of the polygon for the second sector
		//and attach each one the sectors respectively
		Polygon* p2 = new Polygon(*p);
		sector1->_polygons.push_back(p);
		sector2->_polygons.push_back(p2);
		sector1->_s2pmapping[sector2] = p;
		sector2->_s2pmapping[sector1] = p2;
		if (rel == "TRANSIT") {
			_DEBUG("entry polygon, cross-adding to sector ID=" << id1 << " and ID=" << id2);
			sector1->_exitSectors.push_back(sector2);
			sector2->_entrySectors.push_back(sector1);
		} else if (rel == "PARALLEL") {
			_DEBUG("parallel polygon, cross-adding to sector ID=" << id1 << " and ID=" << id2);
			sector1->_parallelSectors.push_back(sector2);
			sector2->_parallelSectors.push_back(sector1);
		}
	}
}

void ConfigurationParser::parseViewpoint()
{
	Simulator* simulator = Simulator::getInstance();
	string token;
	float xyz[3] = {-1, -1, -1};
	float hpr[3] = {-1, -1, -1};
	
	_INFO("reading viewpoint started");
	while (getLine()) {
		parseLine("s", &token);
		if (token.at(0) == '}') break;
		else if (token == "pos") parseLine("fff", &(xyz[0]), &(xyz[1]), &(xyz[2]));
		else if (token == "head") parseLine("fff", &(hpr[0]), &(hpr[1]), &(hpr[2]));
		else _WARN("Read unknown token: " << token << " on line " << _line);
	}
	if (hpr[0] == -1 || hpr[1] == -1 || hpr[2] == -1 || xyz[0] == -1 || xyz[1] == -1 || xyz[2] == -1)
		throw ConfigurationException("syntax error: viewpoint parameters missing");
	_INFO("reading viewpoint ended");
	
	simulator->addViewPoint(xyz, hpr);
}

void ConfigurationParser::parseCar()
{
	CarType type;
	string token, name = "";
	
	_INFO("reading car started");
	while (getLine()) {
		parseLine("s", &token);
		if (token.at(0) == '}') break;
		else if (token == "body") parseLine("RRRR", &(type._bodySize.x), &(type._bodySize.y), &(type._bodySize.z), &(type._weight));
		else if (token == "name") parseLine("s", &name);
		else if (token == "wheel") parseLine("RRR", &(type._wheelRad), &(type._wheelThick), &(type._wheelWeight));
		else if (token == "maxspeed") parseLine("R", &(type._maxSpeed));
		else if (token == "acceleration") parseLine("R", &(type._acceleration));
		else if (token == "decceleration") parseLine("R", &(type._decceleration));
		else _WARN("Read unknown token: " << token << " on line " << _line);
	}
	if (name.length() == 0 || !type.check())
		throw ConfigurationException("syntax error: car parameters missing");
	_carTypes[name] = type;
	_INFO("reading car type=" << name << " ended");
}	

bool ConfigurationParser::getLine()
{
	string line;
	unsigned pos;
	do {
		getline(_file, line);
		_line++;
		//get rid of comments if exist
		if ((pos = line.find("#")) != string::npos) {
			line = line.substr(0, pos);
		}
	} while (line.size() == 0 && !_file.eof());
	if (_file.eof()) return false;
	_iss.clear();
	_iss.str(line);
	return true;
}

void ConfigurationParser::parseLine(const char* types, ...)
{
	void* token;
	va_list ap;
	const char* type = types;
	va_start(ap, types);
	while (*type) {
		switch (*type) {
			case 'i':
			token = va_arg(ap, int*);
			_iss >> *((int*)token);
			break;
			case 'd':
			token = va_arg(ap, double*);
			_iss >> *((double*)token);
			break;
			case 'f':
			token = va_arg(ap, float*);
			_iss >> *((float*)token);
			break;
			case 's':
			token = va_arg(ap, string*);
			_iss >> *((string*)token);
			break;
			case 'R':
			token = va_arg(ap, dReal*);
			_iss >> *((dReal*)token);
			break;
			default:
			throw ConfigurationException("unknown type %s to read from configuration file");
		}
		if (_iss.fail()) {
			char buffer[256];
			sprintf(buffer, "syntax error at line %d: couldn't read token of type %s", _line, type);
			throw ConfigurationException(buffer);
		}
		type++;
	}
	va_end(ap);
}

void ConfigurationParser::parseCarSink()
{
	unsigned sectorID = 0;
	string token;
	
	_INFO("reading carsink started");
	while (getLine()) {
		parseLine("s", &token);
		if (token.at(0) == '}') break;
		else if (token == "sector") parseLine("i", &sectorID);
		else _WARN("Read unknown token: " << token << " on line " << _line);
	}
	if (sectorID == 0) throw ConfigurationException("syntax error: carsink parameters missing");
	_INFO("reading carsink ended");
	
	Sector* s = lookupSector(sectorID);
	mASSERT(s != NULL);
	s->_hasCarSink = true;
	CarSink* sink = new CarSink(s);
	Simulator::getInstance()->addSimulationObject(sink);
}

void ConfigurationParser::parseCarSource()
{
	Simulator* simulator = Simulator::getInstance();
	Vector3r pos(-1, -1, -1);
	dReal dir = 0;
	unsigned sectorID, limit, wait;
	sectorID = limit = wait = 0;
	string token, carTypeName = "";
	
	_INFO("reading carsource started");
	while (getLine()) {
		parseLine("s", &token);
		if (token.at(0) == '}') break;
		else if (token == "sector") parseLine("i", &sectorID);
		else if (token == "pos") parseLine("RRR", &(pos.x), &(pos.y), &(pos.z));
		else if (token == "dir") parseLine("R", &dir);
		else if (token == "cartype") parseLine("s", &carTypeName);
		else if (token == "limit") parseLine("i", &limit);
		else if (token == "wait") parseLine("i", &wait);
		else _WARN("Read unknown token: " << token << " on line " << _line);
	}
	if (sectorID == 0 || (pos.x == -1 && pos.y == -1 && pos.z == -1)) throw ConfigurationException("syntax error: carsource parameters missing");
	_INFO("reading carsource ended");
	
	CarTypeMap::iterator it;
	if ((it = _carTypes.find(carTypeName)) == _carTypes.end()) {
		char buffer[100];
		sprintf(buffer, "car type %s not configured", carTypeName.c_str());
		throw ConfigurationException(buffer);
	}
	CarSource* source = new CarSource(lookupSector(sectorID), (*it).second, pos, DegToRad<dReal>(dir), limit, wait);
	simulator->addSimulationObject(source);
}

void ConfigurationParser::parseBuilding()
{
	Simulator* simulator = Simulator::getInstance();
	string token;
	Vector3r size, pos;
	size[0] = size[1] = size[2] = pos[0] = pos[1] = pos[2] = -1;
	dReal weight = dInfinity;
	int rotation = 0;
	
	_INFO("reading building started");
	while (getLine()) {
		parseLine("s", &token);
		if (token.at(0) == '}') break;
		else if (token == "size") parseLine("RRR", &(size[0]), &(size[1]), &(size[2]));
		else if (token == "pos") parseLine("RRR", &(pos[0]), &(pos[1]), &(pos[2]));
		else if (token == "dir") parseLine("i", &rotation);
		else if (token == "weight") parseLine("R", &weight);
		else _WARN("Read unknown token: " << token << " on line " << _line);
	}
	if (size[0] == -1 || size[1] == -1 || size[2] == -1 || pos[0] == -1 || pos[1] == -1 || pos[2] == -1)
		throw ConfigurationException("syntax error: building parameters missing");
	_INFO("reading building ended");
	
	Building* building = new Building(size, pos, DegToRad<dReal>(rotation), weight);
	simulator->addSimulationObject(building);
}

void ConfigurationParser::parseSigning() {}

Sector* ConfigurationParser::lookupSector(int id)
{
	Simulator* simulator = Simulator::getInstance();
	SectorMap::iterator it;
	Sector* sector = NULL;
	mASSERT(id > 0);
	if ((it = simulator->_sectors.find(id)) != simulator->_sectors.end()) {
		sector = (*it).second;
	} else {
		_DEBUG("sector id=" << id << " not found yet, creating it");
		sector = new Sector(id);
		simulator->addSector(sector);
	}
	mASSERT(sector != NULL);
	return sector;
}
