#include "stdafx.h"
#include "config.h"
#include "SnappingAnalyzer.h"

CSnappingAnalyzer::CSnappingAnalyzer(const string& filename) : m_maxSnappingDist(0.0f), m_worldRadius(0.0f)
{
	SConfig config = GetConfig();

	{
		TiXmlHandle hdl(&config.schedulerXml);
		TiXmlElement* groups = hdl.FirstChild("Scheduler").ToElement();
		if (!groups)
			throw std::runtime_error("no 'Scheduler' node in Scheduler.xml");
		for (TiXmlNode* group = groups->FirstChildElement("Group"); group; group = group->NextSibling("Group"))
		{
			const char* name = group->ToElement()->Attribute("name");
			if (!name)
				throw std::runtime_error("unnamed group in Scheduler.xml");
			uint32 groupKey = StringToKey(name);
			SScalers scalers;
			const char* priority = group->ToElement()->Attribute("priority");
			scalers.priority = priority ? atof(priority) : 0.0f;

			const char* normalDistance = group->ToElement()->Attribute("normalDistance");
			float fNormalDistance = normalDistance ? atof(normalDistance) : 0.0f;
			const char* minBump = group->ToElement()->Attribute("close");
			float fMinBump = minBump ? atof(minBump) : 0.0f;
			const char* maxBump = group->ToElement()->Attribute("far");
			float fMaxBump = maxBump ? atof(maxBump) : 0.0f;
			scalers.dis.Init(fNormalDistance, fMinBump, fMaxBump);

			const char* sfoi = group->ToElement()->Attribute("foi");
			float foi = sfoi ? atof(sfoi) : 0.0f;
			const char* sfront = group->ToElement()->Attribute("front");
			float front = sfront ? atof(sfront) : 0.0f;
			const char* sback = group->ToElement()->Attribute("back");
			float back = sback ? atof(sback) : 0.0f;
			scalers.dir.Init(foi, front, back);

			m_schedulerGroupScalerMap[groupKey] = scalers; // default copy constructor should be ok
		}
	}

	{
		TiXmlHandle hdl(&config.entitySchedulerXml);
		TiXmlElement* entityScheduler = hdl.FirstChild("EntityScheduler").ToElement();
		if (!entityScheduler)
			throw std::runtime_error("no 'EntityScheduler' node in EntityScheduler.xml");
		for (TiXmlNode* cls = entityScheduler->FirstChildElement("Class"); cls; cls = cls->NextSibling("Class"))
		{
			const char* name = cls->ToElement()->Attribute("name"); assert(name);
			const char* group = cls->ToElement()->Attribute("policy"); assert(group);
			m_entityClassSchedulerGroupMap[name] = StringToKey(group);
		}
	}

	TiXmlDocument doc(filename.c_str());
	if (!doc.LoadFile())
		throw std::runtime_error("error loading " + filename);
	TiXmlHandle hdl(&doc);
	TiXmlElement* table = hdl.FirstChild("table").ToElement();
	if (!table)
		throw std::runtime_error("no 'table' node in " + filename);
	for (TiXmlNode* record = table->FirstChildElement("record"); record; record = record->NextSibling("record"))
	{
		SPlotParams params;

		Vec3 witnessPos;
		TiXmlElement* tiWitnessPos = record->FirstChildElement("witnessPos");
		assert(tiWitnessPos);
		const char* sx = tiWitnessPos->Attribute("x");
		const char* sy = tiWitnessPos->Attribute("y");
		const char* sz = tiWitnessPos->Attribute("z");
		witnessPos.x = sx ? atof(sx) : 0.0f;
		witnessPos.y = sy ? atof(sy) : 0.0f;
		witnessPos.z = sz ? atof(sz) : 0.0f;

		Vec3 witnessDir;
		TiXmlElement* tiWitnessDir = record->FirstChildElement("witnessDir");
		assert(tiWitnessDir);
		sx = tiWitnessDir->Attribute("x");
		sy = tiWitnessDir->Attribute("y");
		sz = tiWitnessDir->Attribute("z");
		witnessDir.x = sx ? atof(sx) : 0.0f;
		witnessDir.y = sy ? atof(sy) : 0.0f;
		witnessDir.z = sz ? atof(sz) : 0.0f;

		Vec3 entityPos0;
		TiXmlElement* tiEntityPos0 = record->FirstChildElement("entityPos0");
		assert(tiEntityPos0);
		sx = tiEntityPos0->Attribute("x");
		sy = tiEntityPos0->Attribute("y");
		sz = tiEntityPos0->Attribute("z");
		entityPos0.x = sx ? atof(sx) : 0.0f;
		entityPos0.y = sy ? atof(sy) : 0.0f;
		entityPos0.z = sz ? atof(sz) : 0.0f;

		Vec3 entityPos1;
		TiXmlElement* tiEntityPos1 = record->FirstChildElement("entityPos1");
		assert(tiEntityPos1);
		sx = tiEntityPos1->Attribute("x");
		sy = tiEntityPos1->Attribute("y");
		sz = tiEntityPos1->Attribute("z");
		entityPos1.x = sx ? atof(sx) : 0.0f;
		entityPos1.y = sy ? atof(sy) : 0.0f;
		entityPos1.z = sz ? atof(sz) : 0.0f;

		string entityCls;
		TiXmlElement* tiEntityClass = record->FirstChildElement("entityClass");
		assert(tiEntityClass);
		entityCls = tiEntityClass->Attribute("name");

		Vec3 entityPos = (entityPos0 + entityPos1) * 0.5f; // take the middle point as entity pos
		float distance = witnessPos.GetDistance(entityPos);
		float cosTheta = witnessDir.normalize().Dot( (entityPos - witnessPos).GetNormalized() );

		std::map<string, uint32>::const_iterator itor = m_entityClassSchedulerGroupMap.find(entityCls);
		if (itor != m_entityClassSchedulerGroupMap.end())
		{
			std::map<uint32, SScalers>::const_iterator itor2 = m_schedulerGroupScalerMap.find(itor->second);
			if (itor2 != m_schedulerGroupScalerMap.end())
			{
				params.group = itor2->first;

				const SScalers& scalers = itor2->second;
				params.theoreticalBump = scalers.priority;
				params.theoreticalBump += scalers.dis.GetBump(distance);
				params.theoreticalBump += scalers.dir.GetBump(cosTheta);
				params.snappingDistance = entityPos0.GetDistance(entityPos1);

				Matrix33 m33; m33.SetIdentity();
				m33.SetRotationV0V1(witnessDir.normalize(), Vec3(0.0f,0.0f,1.0f));
				Matrix34 m34; m34.SetIdentity();
				m34.SetTranslation(-witnessPos);
				m34.SetRotation33(m33);
				params.relativePos = m34 * entityPos;

				m_plottingParameters.push_back(params);
			}
		}
	}

	for ( std::vector<SPlotParams>::const_iterator itor = m_plottingParameters.begin(); itor != m_plottingParameters.end(); ++itor )
	{
		if (itor->snappingDistance > m_maxSnappingDist)
			m_maxSnappingDist = itor->snappingDistance;

		if (itor->relativePos.GetLength() > m_worldRadius)
			m_worldRadius = itor->relativePos.GetLength();
	}
}

void CSnappingAnalyzer::Plot(CImage<iRGB>& image, uint32 group, bool opaque)
{
	//const float dimention = 512.0f;
	const int W = image.GetWidth();
	const int H = image.GetHeight();

	// clear the image to blue, we use [r,g] component to represent snapping or bump at a given pixel
	// larger color value (thus whiter) means snapping more (or in the theoretical case, the max allowed
	// snapping is bigger)
	image.Clear(iRGB(0, 0, 255));

	// plot theoretical as background
	{
		std::map<uint32, SScalers>::const_iterator itor = m_schedulerGroupScalerMap.find(group);
		if (itor != m_schedulerGroupScalerMap.end())
		{
			for (int y = 0; y < H; ++y)
				for (int x = 0; x < W; ++x)
				{
					float dist = sqrt( (W/2.0f-x)*(W/2.0f-x) + (H/2.0f-y)*(H/2.0f-y) );
					float cost = (H/2.0f-y) / dist;

					dist = dist / (H/2.0f) * m_worldRadius;

					const SScalers& scalers = itor->second;
					float bump = scalers.priority;
					bump += scalers.dis.GetBump(dist);
					bump += scalers.dir.GetBump(cost);
					uint8 c = (1.0f - bump / 16.0f) * 255; // higher bump, higher priority, less snapping, deeper color
					image(x, y) = iRGB(c, c, 255);
				}
		}
	}

	// overlay snapping data over the theoretical background
	{
		for ( std::vector<SPlotParams>::const_iterator itor = m_plottingParameters.begin(); itor != m_plottingParameters.end(); ++itor )
		{
			if (group != 0 && group != itor->group)
				continue;

			int x = itor->relativePos.x / m_worldRadius * W / 2.0f - W / 2.0f;
			int y = itor->relativePos.y / m_worldRadius * H / 2.0f + H / 2.0f;
			uint8 c = itor->snappingDistance / m_maxSnappingDist * 255; // higher snapping, lighter color
			//uint8 c = itor->snappingDistance / ( (1.0f - itor->theoreticalBump / 16.0f) * m_worldRadius ) * 255;
			if (opaque || c > image(x, y).r ) // bigger value = lighter color; we take the greatest snapping at a given pixel if multiple snapping happens at that spot (which happens often)
				image(x, y) = iRGB(c, c, 255);
		}
	}
}

