#include "MultiBodyTreeDebugGraph.hpp"

#include <cstdio>

namespace btInverseDynamics
{
int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
						 const char* filename)
{
	if (0x0 == tree)
	{
		bt_id_error_message("tree pointer is null\n");
		return -1;
	}
	if (0x0 == filename)
	{
		bt_id_error_message("filename is null\n");
		return -1;
	}

	FILE* fp = fopen(filename, "w");
	if (NULL == fp)
	{
		bt_id_error_message("cannot open file %s for writing\n", filename);
		return -1;
	}
	fprintf(fp,
			"// to generate postscript file, run dot -Tps %s -o %s.ps\n"
			"// details see graphviz documentation at http://graphviz.org\n"
			"digraph tree {\n",
			filename, filename);

	for (int body = 0; body < tree->numBodies(); body++)
	{
		std::string name;
		if (0x0 != map)
		{
			if (-1 == map->getBodyName(body, &name))
			{
				bt_id_error_message("can't get name of body %d\n", body);
				return -1;
			}
			fprintf(fp, "              %d [label=\"%d/%s\"];\n", body, body, name.c_str());
		}
	}
	for (int body = 0; body < tree->numBodies(); body++)
	{
		int parent;
		const char* joint_type;
		int qi;
		if (-1 == tree->getParentIndex(body, &parent))
		{
			bt_id_error_message("indexing error\n");
			return -1;
		}
		if (-1 == tree->getJointTypeStr(body, &joint_type))
		{
			bt_id_error_message("indexing error\n");
			return -1;
		}
		if (-1 == tree->getDoFOffset(body, &qi))
		{
			bt_id_error_message("indexing error\n");
			return -1;
		}
		if (-1 != parent)
		{
			fprintf(fp, "              %d -> %d [label= \"type:%s, q=%d\"];\n", parent, body,
					joint_type, qi);
		}
	}

	fprintf(fp, "}\n");
	fclose(fp);
	return 0;
}
}  // namespace btInverseDynamics