import java.io.*;
import java.util.*;

import org.jdom2.*;
import org.jdom2.filter.Filters;
import org.jdom2.input.SAXBuilder;
import org.jdom2.output.XMLOutputter;
import org.jdom2.xpath.*;



/**
 * Takes a featuremodel.xml file generated using FeatureModelConfigurator and a given 
 * ROS/ Gazebo .gazebo.xacro file.
 * The .gazebo.xacro file is modified to support the features specified in the
 * featuremodel.xml file.
 */
public class Adapter {
	
	/**
	 * Reads an .xml file from file path and returns a JDOM document.
	 * 
	 * @param src			The .xml file path
	 * @return JDOM document
	 * @throws JDOMException
	 * @throws IOException
	 */
	public static Document readXml(String src) throws JDOMException, IOException {
		File file = new File(src);
		SAXBuilder saxBuilder = new SAXBuilder();
		Document doc = saxBuilder.build(file);

		return doc;
	}
	
	/**
	 * Saves the given Document as XML to the given path.
	 * 
	 * @param oDoc			The JDOM document
	 * @param sPath			The output path
	 * @throws IOException
	 */
	public static void saveAsXml(Document oDoc, String sPath) throws IOException {
		FileOutputStream dest = new FileOutputStream(sPath);
		XMLOutputter xmlOutput = new XMLOutputter();		
		xmlOutput.output(oDoc, dest);
	}
	
	/**
	 * Executes given XPath expression for elements with JDOM Document as source.
	 * 
	 * @param doc			The JDOM Document
	 * @param xPath			XPath expression
	 * @return List with all elements found or empty list if no match found
	 */
	public static List<Element> xQueryElements(Document doc, String xPath) {
		XPathFactory xFactory = XPathFactory.instance();
		XPathExpression<Element> expr = xFactory.compile(xPath, Filters.element());
		List<Element> result = expr.evaluate(doc);
		
		return result;
	}
	
	/**
	 * Executes given XPath expression for elements with Element as source.
	 * 
	 * @param elem			The Element
	 * @param xPath			XPath expression
	 * @return List with all elements found or empty list if no match found
	 */
	public static List<Element> xQueryElements(Element elem, String xPath) {
		XPathFactory xFactory = XPathFactory.instance();
		XPathExpression<Element> expr = xFactory.compile(xPath, Filters.element());
		List<Element> result = expr.evaluate(elem);
		
		return result;
	}
	
	/**
	 * Executes given XPath expression for attributes.
	 * 
	 * @param doc			The JDOM Document
	 * @param xPath			XPath expression
	 * @return List with all attributes found or empty list if no match found
	 */
	public static List<Attribute> xQueryAttributes(Document doc, String xPath) {
		XPathFactory xFactory = XPathFactory.instance();
		XPathExpression<Attribute> expr = xFactory.compile(xPath, Filters.attribute());
		List<Attribute> result = expr.evaluate(doc);
		
		return result;
	}
	
	/**
	 * Searches a document for the first occurrence of an xml tag.
	 * 
	 * @param doc			The JDOM Document
	 * @param elementName	Name of the xml tag
	 * @return First element found or NULL if no match found
	 */
	public static Element searchFirstElement(Document doc, String elementName) {
		//XPath index starts with 1; [] has a higher precedence than //
		List<Element> query= xQueryElements(doc, "(//"+elementName+")[1]");
		
		if(query.isEmpty()) return null;
		return query.get(0);
	}
	
	/**
	 * Searches a document for all occurrences of an xml tag.
	 * 
	 * @param doc			The JDOM Document
	 * @param elementName	Name of the xml tag
	 * @return List with all elements found or empty list if no match found
	 */
	public static List<Element> searchAllElements(Document doc, String elementName) {
		return xQueryElements(doc, "//"+elementName);
	}
	
	/**
	 * Checks whether two elements have an equal xml tag.
	 * (Name of the xml tag and all attributes including their order must be equal)
	 * 
	 * @param element1		Element1
	 * @param element2		Element2
	 * @return true if of equal tag or false if not
	 */
	public static boolean equalTag(Element element1, Element element2) {
		List<Attribute> attributeList1 = element1.getAttributes();
		List<Attribute> attributeList2 = element2.getAttributes();
		
		if(!element1.getName().equals(element2.getName())) return false;
		if(attributeList1.size()!=attributeList2.size()) return false;
		for(int i=0; i<attributeList1.size(); i++) {
			if(!attributeList1.get(i).getName().equals(attributeList2.get(i).getName())) return false;
			if(!attributeList1.get(i).getValue().equals(attributeList2.get(i).getValue())) return false;
		}
		
		return true;
	}
	
	/**
	 * Recursive replacement of all text values in destElements with their corresponding 
	 * new ones from srcElements.
	 * (srcElements and destElements must be matching tags!)
	 * 
	 * TODO maybe add tags that are in srcElements but not yet in destElements
	 * 
	 * @param srcElements		Element list with new values
	 * @param destElements		Element list with values to be replaced
	 * @return true if reached deepest Element or false if not
	 */
	public static boolean replace(List<Element> srcElements, List<Element> destElements) {
		if(srcElements.isEmpty()&&destElements.isEmpty()) return true;
		if(srcElements.isEmpty()^destElements.isEmpty()) {
			System.err.println("Error during value replacement: Sensor xml trees have mismatching deph (maybe tags with no value)");
			System.exit(-1);
		}
		for(Element src : srcElements) {
			for(Element dest : destElements) {		
				if(equalTag(src, dest)) {
					boolean deadEnd = replace(src.getChildren(), dest.getChildren());
					if(deadEnd) dest.setText(src.getText());
					break;
				}
			}
		}
		
		return false;
	}
	
	/**
	 * Obtains the path attribute for the input/ output files as specified in adapterconfig.xml.
	 * 
	 * @param doc		The JDOM Document (adapterconfig.xml)
	 * @param str		{input or output}/{entity}
	 * @return path as specified in adapterconfig.xml
	 */
	private static String getConfigPath(Document doc, String str) {
		List<Attribute> pathList = xQueryAttributes(doc, "//"+str+"/@path");
		if(pathList.size()!=1) {
			System.err.println("Error while reading input/ output path from adapterconfig.xml");
			System.exit(-1);
		}

		return pathList.get(0).getValue();
	}
	
	/**
	 * Obtains the name attribute for the input/ output files as specified in adapterconfig.xml.
	 * 
	 * @param doc		The JDOM Document (adapterconfig.xml)
	 * @param str		{input or output}/{entity}
	 * @return name as specified in adapterconfig.xml
	 */
	private static String getConfigName(Document doc, String str) {
		List<Attribute> nameList = xQueryAttributes(doc, "//"+str+"/@name");
		if(nameList.size()!=1) {
			System.err.println("Error while reading input/ output name from adapterconfig.xml");
			System.exit(-1);
		}

		return nameList.get(0).getValue();
	}
	
	/**
	 * @param args
	 */
	public static void main(String[] args) {
		try {
			//TODO##### Input ###############################			
			Document adapterConfig = readXml("./adapterconfig.xml");
			
			String in_fmPath = getConfigPath(adapterConfig, "input/featuremodel");
			String in_scPath = getConfigPath(adapterConfig, "input/sensorconfig");
			String in_gmPath = getConfigPath(adapterConfig, "input/gazebomodel");
			String out_gmPath = getConfigPath(adapterConfig, "output/gazebomodel");
			//String in_gmName = 
			String out_gmName = getConfigName(adapterConfig, "output/gazebomodel");
			
			Document featureModel = readXml(in_fmPath);
			Document sensorConfig = readXml(in_scPath);
			Document gazeboModel = null;
			
			//TODO CustomBot not supported for now
			List<Attribute> botList = xQueryAttributes(featureModel, "//module[@types='TurtleBot']/@name");
			String botName = "";
			String fileName = "";
			
			switch(botList.size()) {
				case 0:
					System.err.println("Invalid input: No bot selected");
					System.exit(-1);
					break;
				case 1:
					botName = botList.get(0).getValue();
					break;
				default:
					System.err.println("Invalid input: Multiple bots selected");
					System.exit(-1);
			}
			//TODO get all .gazebo.xacro file names from /turtlebot3_description/urdf/ directory; match botName; select file
			switch(botName) {
				case "Burger":
					fileName = "turtlebot3_burger.gazebo.xacro";
					break;
				case "Waffle":
					fileName = "turtlebot3_waffle.gazebo.xacro";
					break;
				case "WafflePi":
					fileName = "turtlebot3_waffle_pi.gazebo.xacro";
					break;
				default:
					System.err.println("Cannot handle \'"+botName+"\': No input file known.");
					System.exit(-1);
			}
			gazeboModel = readXml(in_gmPath+fileName);
			//###############################################
			
			
			//TODO##### Operate on .gazebo.xacro ############
			List<Element> cSensorList = xQueryElements(sensorConfig, "//sensor");	
			List<Element> gSensorList = xQueryElements(gazeboModel, "//sensor");
			boolean match = false;
			
			if(cSensorList.isEmpty()) {
				System.out.println("No new sensor values in sensorconfig.xml");
				System.exit(-1);
			}
			if(gSensorList.isEmpty()) {
				System.out.println("No sensors found in "+fileName);
				System.exit(-1);
			}
						
			for(Element cSensor : cSensorList) {
				for(Element gSensor : gSensorList) {	
					if(equalTag(cSensor, gSensor)) {
						replace(cSensor.getChildren(),gSensor.getChildren());
						match = true;
					}
				}
			}
			
			if(!match) System.out.println("No matching sensors could be found in sensorconfig.xml and "+fileName);
			//###############################################
			
			
			//######### Output result #######################
			saveAsXml(gazeboModel, out_gmPath+out_gmName);
			System.out.println("Adapter finished.");
			//###############################################
			
		} catch (JDOMException | IOException e) {
			e.printStackTrace();
		}
	}
}


//Example dump

//hard-coded and only for demo purposes
//List<Element> cSensorList = xQueryElements(sensorConfig, "//sensor[@type='ray' and @name='lds_lfcd_sensor']");
//Element cSensor = cSensorList.get(0);
//
//List<Element> gSensorList = xQueryElements(gazeboModel, "//sensor[@type='ray' and @name='lds_lfcd_sensor']");
//Element gSensor = gSensorList.get(0);
//
//String new_min = cSensor.getChild("range").getChild("min").getText();
//String new_max = cSensor.getChild("range").getChild("max").getText();
//
//gSensor.getChild("ray").getChild("range").getChild("min").setText(new_min);
//gSensor.getChild("ray").getChild("range").getChild("max").setText(new_max);

//XPathExamples
//XPathFactory xFactory = XPathFactory.instance();
//XPathExpression<Element> xExpr = xFactory.compile("robot/gazebo/sensor/ray/range/min", Filters.element());
//Element test = xExpr.evaluateFirst(gazeboModel);
//List<Element> test2 = searchAllElements(gazeboModel, "min");
//List<Element> test3 = searchAllElements(gazeboModel, "max");
//Element test4 = searchFirstElement(gazeboModel, "plugin");
//List<Attribute> test5 = xQueryAttributes(gazeboModel, "(//plugin)[1]/@*");
//
//if(test != null) xmlOutput.output(test, System.out);
//System.out.println("\n");
//xmlOutput.output(test2, System.out);
//System.out.println("\n");
//xmlOutput.output(test3, System.out);
//System.out.println("\n");
//if(test4 != null) xmlOutput.output(test4, System.out);
//System.out.println("\n");
//for(Attribute a : test5) System.out.println(a.getName()+"="+a.getValue());