#include "ObjectIO.h"
#include "../VirtualRobotException.h"
#include "rapidxml.hpp"
#include <boost/pointer_cast.hpp>
#include <boost/filesystem.hpp>
#include "RobotIO.h"
#include "../Grasp.h"
#include "../GraspSet.h"
#include "../ManipulationObject.h"

namespace VirtualRobot {



ObjectIO::ObjectIO()
{
}

ObjectIO::~ObjectIO()
{
}

VirtualRobot::ManipulationObjectPtr ObjectIO::loadManipulationObject( const std::string &xmlFile )
{
	// load file
	std::ifstream in(xmlFile.c_str());

	THROW_VR_EXCEPTION_IF (!in.is_open(),"Could not open XML file:" << xmlFile);

	std::stringstream buffer;
	buffer << in.rdbuf();
	std::string objectXML(buffer.str());
	boost::filesystem::path filenameBaseComplete(xmlFile);
	boost::filesystem::path filenameBasePath = filenameBaseComplete.branch_path();
	std::string basePath = filenameBasePath.string();

	in.close();

	VirtualRobot::ManipulationObjectPtr res = createManipulationObjectFromString(objectXML, basePath);
	THROW_VR_EXCEPTION_IF (!res,"Error while parsing file " << xmlFile);
	res->setFilename(xmlFile);
	return res;
}

GraspPtr ObjectIO::processGrasp(rapidxml::xml_node<char>* graspXMLNode, const std::string& robotType, const std::string& eef, const std::string& objName)
{
	THROW_VR_EXCEPTION_IF(!graspXMLNode, "No <Grasp> tag ?!");
	// get name
	std::string name = processNameAttribute(graspXMLNode,true);
	std::string method = processStringAttribute("creation",graspXMLNode,true);
	float quality = processFloatAttribute(std::string("quality"),graspXMLNode,true);

	Eigen::Matrix4f pose;
	pose.setIdentity();

	rapidxml::xml_node<>* node = graspXMLNode->first_node();
	while (node)
	{
		std::string nodeName = getLowerCase(node->name());
		if (nodeName == "transform")
		{
			processTransformNode(graspXMLNode,name,pose);
			
		}  else
		{
			THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Grasp <" << name << ">." << endl);
		}

		node = node->next_sibling();
	}

	GraspPtr grasp(new Grasp(name,robotType,eef,pose,method,quality));
	return grasp;
}


GraspSetPtr ObjectIO::processGraspSet(rapidxml::xml_node<char>* graspSetXMLNode, const std::string& objName)
{
	THROW_VR_EXCEPTION_IF(!graspSetXMLNode, "No <GraspSet> tag ?!");

	// get name
	std::string gsName = processNameAttribute(graspSetXMLNode,true);
	std::string gsRobotType = processStringAttribute(std::string("robottype"),graspSetXMLNode,true);
	std::string gsEEF = processStringAttribute(std::string("endeffector"),graspSetXMLNode,true);

	if (gsName.empty() || gsRobotType.empty() || gsEEF.empty())
	{
		THROW_VR_EXCEPTION("GraspSet tags must have valid attributes 'Name', 'RobotType' and 'EndEffector'");
	}

	GraspSetPtr result(new GraspSet(gsName,gsRobotType,gsEEF));

	rapidxml::xml_node<>* node = graspSetXMLNode->first_node();
	while (node)
	{
		std::string nodeName = getLowerCase(node->name());
		if (nodeName == "grasp")
		{
			GraspPtr grasp = processGrasp(node, gsRobotType, gsEEF, objName);
			THROW_VR_EXCEPTION_IF(!grasp, "Invalid 'Grasp' tag in '" << objName << "'." << endl);
			result->addGrasp(grasp);
		} else
		{
			THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in GraspSet <" << gsName << ">." << endl);
		}

		node = node->next_sibling();
	}
	return result;
}

ManipulationObjectPtr ObjectIO::processManipulationObject(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath)
{
	THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <ManipulationObject> tag in XML definition");

	bool visuProcessed = false;
	bool colProcessed = false;
	VisualizationNodePtr visualizationNode;
	CollisionModelPtr collisionModel;
	bool useAsColModel = false;
	SceneObject::Physics physics;
	bool physicsDefined = false;
	std::vector<GraspSetPtr> graspSets;
	Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();

	// get name
	std::string objName = processNameAttribute(objectXMLNode);

	// first check if there is an xml file to load
	rapidxml::xml_node<>* xmlFileNode = objectXMLNode->first_node("file",0,false);
	if (xmlFileNode)
	{
		std::string xmlFile = processFileNode(xmlFileNode,basePath);
		ManipulationObjectPtr result = loadManipulationObject(xmlFile);
		if (!result)
			return result;
		if (!objName.empty())
			result->setName(objName);
		// update global pose
		rapidxml::xml_node<>* poseNode = objectXMLNode->first_node("globalpose",0,false);
		if (poseNode)
		{
			processTransformNode(poseNode, objName, globalPose);
			result->setGlobalPose(globalPose);
		}

		return result;
	}

	

	THROW_VR_EXCEPTION_IF (objName.empty(),"ManipulationObject definition expects attribute 'name'");

	rapidxml::xml_node<>* node = objectXMLNode->first_node();
	while (node)
	{
		std::string nodeName = getLowerCase(node->name());
		if (nodeName == "visualization")
		{
			THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in ManipulationObject '" << objName << "'." << endl);
			visualizationNode = processVisualizationTag(node, objName, basePath, useAsColModel);
			visuProcessed = true;
			if (useAsColModel)
			{
				THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in ManipulationObject '" << objName << "'." << endl);
				std::string colModelName = objName;
				colModelName += "_VISU_ColModel";
				// todo: ID?
				collisionModel.reset(new CollisionModel(visualizationNode,colModelName,CollisionCheckerPtr()));
				colProcessed = true;
			}
		} else if (nodeName == "collisionmodel")
		{
			THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in ManipulationObject '" << objName << "'." << endl);
			collisionModel = processCollisionTag(node, objName, basePath);
			colProcessed = true;
		} else if (nodeName == "physics")
		{
			THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in ManipulationObject '" << objName << "'." << endl);
			processPhysicsTag(node,objName,physics);
			physicsDefined = true;
		} else if (nodeName=="graspset")
		{
			GraspSetPtr gs = processGraspSet(node,objName);
			THROW_VR_EXCEPTION_IF(!gs, "Invalid grasp set in '" << objName << "'." << endl);
			graspSets.push_back(gs);
		
		} else if (nodeName == "globalpose")
		{
			processTransformNode(node, objName, globalPose);
		}  else
		{
			THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in ManipulationObject <" << objName << ">." << endl);
		}

		node = node->next_sibling();
	}



	// build object
	ManipulationObjectPtr object(new ManipulationObject(objName,visualizationNode,collisionModel,physics));
	for (size_t i=0; i<graspSets.size(); i++)
		object->addGraspSet(graspSets[i]);
	object->setGlobalPose(globalPose);

	return object;
}

ObstaclePtr ObjectIO::processObstacle(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath)
{
	THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <Obstacle> tag in XML definition");

	bool visuProcessed = false;
	bool colProcessed = false;
	VisualizationNodePtr visualizationNode;
	CollisionModelPtr collisionModel;
	bool useAsColModel = false;
	SceneObject::Physics physics;
	bool physicsDefined = false;
	Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();

	// get name
	std::string objName = processNameAttribute(objectXMLNode);
	THROW_VR_EXCEPTION_IF (objName.empty(),"Obstacle definition expects attribute 'name'");

	rapidxml::xml_node<>* node = objectXMLNode->first_node();
	while (node)
	{
		std::string nodeName = getLowerCase(node->name());
		if (nodeName == "visualization")
		{
			THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in Obstacle '" << objName << "'." << endl);
			visualizationNode = processVisualizationTag(node, objName, basePath, useAsColModel);
			visuProcessed = true;
			if (useAsColModel)
			{
				THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
				std::string colModelName = objName;
				colModelName += "_VISU_ColModel";
				// todo: ID?
				collisionModel.reset(new CollisionModel(visualizationNode,colModelName,CollisionCheckerPtr()));
				colProcessed = true;
			}
		} else if (nodeName == "collisionmodel")
		{
			THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
			collisionModel = processCollisionTag(node, objName, basePath);
			colProcessed = true;
		} else if (nodeName == "physics")
		{
			THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in Obstacle '" << objName << "'." << endl);
			processPhysicsTag(node,objName,physics);
			physicsDefined = true;
		} else if (nodeName == "globalpose")
		{
			processTransformNode(node, objName, globalPose);
		} else
		{
			THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Obstacle <" << objName << ">." << endl);
		}

		node = node->next_sibling();
	}

	// build object
	ObstaclePtr object(new Obstacle(objName,visualizationNode,collisionModel,physics));
	object->setGlobalPose(globalPose);
	return object;
}



VirtualRobot::ManipulationObjectPtr ObjectIO::createManipulationObjectFromString( const std::string &xmlString, const std::string &basePath /*= ""*/ )
{
	// copy string content to char array
	char* y = new char[xmlString.size() + 1];
	strncpy(y, xmlString.c_str(), xmlString.size() + 1);

	VirtualRobot::ManipulationObjectPtr obj;

	try
	{
		rapidxml::xml_document<char> doc;    // character type defaults to char
		doc.parse<0>(y);    // 0 means default parse flags
		rapidxml::xml_node<char>* objectXMLNode = doc.first_node("ManipulationObject");

		obj = processManipulationObject(objectXMLNode,basePath);
		
		

	}
	catch (rapidxml::parse_error& e)
	{
		delete[] y;
		THROW_VR_EXCEPTION("Could not parse data in xml definition" << endl
			<< "Error message:" << e.what() << endl
			<< "Position: " << endl << e.where<char>() << endl);
		return ManipulationObjectPtr();
	}
	catch (VirtualRobot::VirtualRobotException&)
	{
		// rethrow the current exception
		delete[] y;
		throw;
	}
	catch (std::exception& e)
	{
		delete[] y;
		THROW_VR_EXCEPTION("Error while parsing xml definition" << endl
			<< "Error code:" << e.what() << endl);
		return ManipulationObjectPtr();
	}
	catch (...)
	{
		delete[] y;
		THROW_VR_EXCEPTION("Error while parsing xml definition" << endl);
		return ManipulationObjectPtr();
	}

	delete[] y;
	return obj;
}

bool ObjectIO::saveManipulationObject( ManipulationObjectPtr object, const std::string &xmlFile )
{
	THROW_VR_EXCEPTION_IF(!object, "NULL object");

	boost::filesystem::path filenameBaseComplete(xmlFile);
	boost::filesystem::path filenameBasePath = filenameBaseComplete.branch_path();
	std::string basePath = filenameBasePath.string();

	std::string xmlString = object->getXMLString(basePath);

	// save file
	return BaseIO::writeXMLFile(xmlFile,xmlString,true);
}


} // namespace VirtualRobot