#include "libraries.h"
#include "planes.h"

//-------------------------------------------------------+
// Component functions of fn 'GetPlanePoints'

// will need these functions to test whether the outputs used from Eigen 
// are the same as the outputs from ckmath

Eigen::Vector3f get_polygon_normal(const Eigen::Vector3f* vertices, const size_t nvertices) {
    Eigen::Vector3f x = {0.0f, 0.0f, 0.0f};
    for (size_t i = 0; i < nvertices; ++i) {
		const size_t kIndexA = ((nvertices - 1) + i) % nvertices;
		const size_t kIndexB = i;

		const Eigen::Vector3f& krA = vertices[kIndexA];
		const Eigen::Vector3f& krB = vertices[kIndexB];

		const Eigen::Vector3f kCrossProduct = krA.cross(krB);
        x += kCrossProduct;
    }
    x.normalize();
    return x;
}

inline Eigen::Vector3f get_center(const Eigen::Vector3f* kppoints, 
                                      const size_t npoints) {
	Eigen::Vector3f result{0.0f, 0.0f, 0.0f};
	for(size_t i = 0; i < npoints; ++i)
	{
		result += kppoints[i];
	}
    result *= (1.0f / (float)npoints);
    return result;
}

inline float get_width(const Eigen::Vector3f* kppoints,
                       const size_t npoints,
                       const Eigen::Vector3f center) {
	float width = 1.0f;
	for(size_t i = 0; i < npoints; ++i) {
        // norm() = vector magnitude in Eigen.
		width = std::max(width, (kppoints[i] - center).norm());
	}
    return width;
}

inline Eigen::Vector3f get_tangent(const Eigen::Vector3f* x, 
                                   float width) {
    Eigen::Vector3f t = (x[1] - x[0]);
    t.normalize();
    return (t *  (width * 10.0f));
}

inline Eigen::Vector3f get_bitangent(const Eigen::Vector3f& tan, 
                                     const Eigen::Vector3f& norm,
                                     float width) {
    Eigen::Vector3f x = norm.cross(tan);
    x.normalize();
    return (x * (width * 10.0f));
}

//-------------------------------------------------------+

TPlanePoints GetPlanePoints(const Eigen::Vector3f* _kpPoints, const size_t _kNumPoints)
{
	TPlanePoints PlanePoints;
	const Eigen::Vector3f kNormal  = get_polygon_normal(_kpPoints, _kNumPoints);
	const Eigen::Vector3f kCenter  = get_center(_kpPoints, _kNumPoints);
	float fWidth                   = get_width(_kpPoints, _kNumPoints, kCenter);
	const Eigen::Vector3f kTangent   = get_tangent(_kpPoints, fWidth);
	const Eigen::Vector3f kBiTangent = get_bitangent(kTangent, kNormal, fWidth);

	PlanePoints.m_A = kCenter + kBiTangent;
	PlanePoints.m_B = kCenter;
	PlanePoints.m_C = kCenter + kTangent;

	return(PlanePoints);
}

std::vector<TPlanePoints> GetBrushPlanes(const TBrush& _krBrush)
{
	std::vector<TPlanePoints> Planes;
	for(const TFace& krFace : _krBrush.m_Faces)
	{
		std::vector<Eigen::Vector3f> Verts(krFace.m_Indices.size());
		for(size_t i = 0; i < krFace.m_Indices.size(); ++i)
		{
			Verts[i] = _krBrush.m_Vertices[krFace.m_Indices[i]];
		}
		TPlanePoints PP = GetPlanePoints(Verts.data(), Verts.size());
		PP.m_material = krFace.m_Material;
		Planes.push_back(PP);
	}

	return(Planes);
}

std::string GetBrushString(const TBrush& _krBrush)
{
	std::vector<TPlanePoints> Planes = GetBrushPlanes(_krBrush);
	std::stringstream ssOutput;

	if(Planes.size())
	{
		ssOutput << "{" << std::endl;
		for(const TPlanePoints& krPlane : Planes)
		{
			ssOutput << "( " << krPlane.m_A[X] << " " << krPlane.m_A[Z] << " " << krPlane.m_A[Y] << " ) ";
			ssOutput << "( " << krPlane.m_B[X] << " " << krPlane.m_B[Z] << " " << krPlane.m_B[Y] << " ) ";
			ssOutput << "( " << krPlane.m_C[X] << " " << krPlane.m_C[Z] << " " << krPlane.m_C[Y] << " ) ";
			if(krPlane.m_material.length())
			{
				ssOutput << krPlane.m_material << " 0 0 0 0.500000 0.500000" << std::endl;
			}
			else
			{
				ssOutput << "common/caulk 0 0 0 0.500000 0.500000" << std::endl;
			}
		}
		ssOutput << "}" << std::endl;
	}

	return(ssOutput.str());
}