#include "VRender.h"
#include "Primitive.h"
#include "SortMethod.h"
#include "math.h" // fabs

using namespace vrender;
using namespace std;

double EGALITY_EPS 		= 0.0001;
double LINE_EGALITY_EPS = 0.0001;

typedef enum { BSP_CROSS_PLANE, BSP_UPPER, BSP_LOWER } BSPPosition;

class BSPNode;

class BSPTree
{
	public:
		BSPTree();
		~BSPTree();

		void insert(Polygone *);
		void insert(Segment *);
		void insert(Point *);

		void recursFillPrimitiveArray(vector<PtrPrimitive>&) const;
	private:
		BSPNode *_root;
        vector<Segment *> _segments;	// these are for storing segments and points when _root is null
		vector<Point *> _points;
};

void BSPSortMethod::sortPrimitives(std::vector<PtrPrimitive>& primitive_tab,VRenderParams& vparams)
{
	// 1 - build BSP using polygons only

	BSPTree tree;
	Polygone *P;

        unsigned int N = primitive_tab.size()/200 +1;
	int nbinserted = 0;

	vector<PtrPrimitive> segments_and_points;	// Store segments and points for pass 2, because polygons are deleted
																// by the insertion and can not be dynamic_casted anymore.
	for(unsigned int i=0;i<primitive_tab.size();++i,++nbinserted)
	{
        if((P = dynamic_cast<Polygone *>(primitive_tab[i])) != nullptr)
			tree.insert(P);
		else
			segments_and_points.push_back(primitive_tab[i]);

		if(nbinserted%N==0)
			vparams.progress(nbinserted/(float)primitive_tab.size(), QGLViewer::tr("BSP Construction"));
	}

	// 2 - insert points and segments into the BSP

	Segment *S;
	Point *p;

	for(unsigned int j=0;j<segments_and_points.size();++j,++nbinserted)
	{
        if((S = dynamic_cast<Segment *>(segments_and_points[j])) != nullptr)
			tree.insert(S);
        else if((p = dynamic_cast<Point *>(segments_and_points[j])) != nullptr)
			tree.insert(p);

		if(nbinserted%N==0)
			vparams.progress(nbinserted/(float)primitive_tab.size(), QGLViewer::tr("BSP Construction"));
	}

	// 3 - refill the array with the content of the BSP

	primitive_tab.resize(0);
	tree.recursFillPrimitiveArray(primitive_tab);
}

/////////////////////////////////////////////////////////////////////////////////////////////////

class BSPNode
{
	public:
		BSPNode(Polygone *);
		~BSPNode();

		void recursFillPrimitiveArray(vector<PtrPrimitive>&) const;

		void insert(Polygone *);
		void insert(Segment *);
		void insert(Point *);

	private:
		double a,b,c,d;

		BSPNode *fils_moins;
		BSPNode *fils_plus;

		vector<Segment *> seg_plus;
		vector<Segment *> seg_moins;

		vector<Point *> pts_plus;
		vector<Point *> pts_moins;

		Polygone *polygone;

		void Classify(Polygone *, Polygone * &, Polygone * &);
		void Classify(Segment *, Segment * &, Segment * &);
		int  Classify(Point *);

		void initEquation(const Polygone *P,double & a, double & b, double & c, double & d);
};

BSPTree::BSPTree()
{
    _root = nullptr;
}

BSPTree::~BSPTree()
{
	delete _root;
}

void BSPTree::insert(Point *P) 	{ if(_root == nullptr) _points.push_back(P) 	; else _root->insert(P); }
void BSPTree::insert(Segment *S) { if(_root == nullptr) _segments.push_back(S); else _root->insert(S); }
void BSPTree::insert(Polygone *P){ if(_root == nullptr) _root = new BSPNode(P); else _root->insert(P); }

void BSPTree::recursFillPrimitiveArray(vector<PtrPrimitive>& tab) const
{
    if(_root != nullptr) _root->recursFillPrimitiveArray(tab);

	for(unsigned int i=0;i<_points.size();++i) tab.push_back(_points[i]);
	for(unsigned int j=0;j<_segments.size();++j) tab.push_back(_segments[j]);
}

//----------------------------------------------------------------------------//

BSPNode::~BSPNode()
{
	delete fils_moins;
	delete fils_plus;
}

int BSPNode::Classify(Point *P)
{
  double Z = P->sommet3DColor(0).x() * a + P->sommet3DColor(0).y() * b + P->sommet3DColor(0).z() * c - d;

  if(Z > EGALITY_EPS)
    return 1;
  else
    return -1;
}

void BSPNode::Classify(Segment *S, Segment * & moins_, Segment * & plus_)
{
	double Z1 = S->sommet3DColor(0).x() * a + S->sommet3DColor(0).y() * b + S->sommet3DColor(0).z() * c - d;
	double Z2 = S->sommet3DColor(1).x() * a + S->sommet3DColor(1).y() * b + S->sommet3DColor(1).z() * c - d;

	int s1, s2;

	if(Z1 < -LINE_EGALITY_EPS)
		s1 = -1;
	else if(Z1 > EGALITY_EPS)
		s1 = 1;
	else
		s1 = 0;

	if(Z2 < -LINE_EGALITY_EPS)
		s2 = -1;
	else if(Z2 > EGALITY_EPS)
		s2 = 1;
	else
		s2 = 0;

	if(s1 == -s2)
	{
		if(s1 == 0)
		{
			moins_ = S;
            plus_  = nullptr;
			return;
		}
		else
		{
			double t = fabs(Z1/(Z2 - Z1));

			if((t < 0.0)||(t > 1.0))
			{
				if(t > 1.0) t = 0.999;
				if(t < 0.0) t = 0.001;
			}

			Feedback3DColor newVertex((1-t)*S->sommet3DColor(0) + t*S->sommet3DColor(1));

			if(s1 > 0)
			{
				plus_  = new Segment(S->sommet3DColor(0), newVertex);
				moins_ = new Segment(newVertex, S->sommet3DColor(1));
			}
			else
			{
				plus_  = new Segment(newVertex, S->sommet3DColor(1));
				moins_ = new Segment(S->sommet3DColor(0), newVertex);
			}

			delete S;
			return;
		}
	}
	else if(s1 == s2)
	{
		if(s1 == -1)
		{
			moins_ = S;
            plus_ = nullptr;
			return;
		}
		else
		{
            moins_ = nullptr;
			plus_  = S;
			return;
		}
	}
	else if(s1 == 0)
	{
		if(s2 > 0)
		{
            moins_ = nullptr;
			plus_  = S;
			return;
		}
		else
		{
			moins_ = S;
            plus_  = nullptr;
			return;
		}
	}
	else if(s2 == 0)
	{
		if(s1 > 0)
		{
            moins_ = nullptr;
			plus_  = S;
			return;
		}
		else
		{
			moins_ = S;
            plus_  = nullptr;
			return;
		}
	}
	//else
		//printf("BSPNode::Classify: unexpected classification case !!\n");
}

void BSPNode::Classify(Polygone *P, Polygone * & moins_, Polygone * & plus_)
{
	static int Signs[100];
	static double Zvals[100];

    moins_ = nullptr;
    plus_ = nullptr;

    if(P == nullptr)
	{
        //printf("BSPNode::Classify: Error. Null polygon.\n");
		return;
	}

	int n = P->nbVertices();

	int Smin = 1;
	int Smax = -1;

	// On classe les sommets en fonction de leur signe

	for(int i=0;i<n;i++)
	{
		double Z = P->vertex(i).x() * a + P->vertex(i).y() * b + P->vertex(i).z() * c - d;

		if(Z < -EGALITY_EPS)
			Signs[i] = -1;
		else if(Z > EGALITY_EPS)
			Signs[i] = 1;
		else
			Signs[i] = 0;

		Zvals[i] = Z;

		if(Smin > Signs[i]) Smin = Signs[i];
		if(Smax < Signs[i]) Smax = Signs[i];
	}

	// Polygone inclus dans le plan

	if((Smin == 0)&&(Smax == 0))
	{
		moins_ = P;
        plus_  = nullptr;
		return;
	}

	// Polygone tout positif

	if(Smin == 1)
	{
		plus_  = P;
        moins_ = nullptr;
		return;
	}

	// Polygone tout negatif

	if(Smax == -1)
	{
        plus_ = nullptr;
		moins_ = P;
		return;
	}

	if((Smin == -1)&&(Smax == 0))
	{
        plus_ = nullptr;
		moins_ = P;
		return;
	}

	if((Smin == 0)&&(Smax == 1))
	{
		plus_ = P;
        moins_ = nullptr;
		return;
	}

	// Reste le cas Smin = -1 et Smax = 1. Il faut couper

	vector<Feedback3DColor> Ps;
	vector<Feedback3DColor> Ms;

	// On teste la coherence des signes.

	int nZero = 0;
	int nconsZero = 0;

	for(int j=0;j<n;j++)
	{
		if(Signs[j] == 0)
		{
			nZero++;

			if(Signs[(j+1)%n] == 0)
				nconsZero++;
		}
	}

	if((nZero > 2)||(nconsZero > 0))
	{
		// Ils y a des imprecisions numeriques dues au fait que le poly estpres du plan.

		moins_ = P;
        plus_  = nullptr;
		return;
	}

	int dep=0;

	while(Signs[dep] == 0) dep++;

	int prev_sign = Signs[dep];

	for(int k=1;k<=n;k++)
	{
		int sign = Signs[(k+dep)%n];

		if(sign == prev_sign)
		{
			if(sign == 1)
				Ps.push_back(P->sommet3DColor(k+dep));

			if(sign == -1)
				Ms.push_back(P->sommet3DColor(k+dep));
		}
		else if(sign == -prev_sign)
		{
			//  Il faut effectuer le calcul en utilisant les memes valeurs que pour le calcul des signes,
			// sinon on risque des incoherences dues aux imprecisions numeriques.

			double Z1 = Zvals[(k+dep-1)%n];
			double Z2 = Zvals[(k+dep)%n];

			double t = fabs(Z1/(Z2 - Z1));

			if((t < 0.0)||(t > 1.0))
			{
				if(t > 1.0) t = 0.999;
				if(t < 0.0) t = 0.001;
			}

			Feedback3DColor newVertex((1-t)*P->sommet3DColor(k+dep-1) + t*P->sommet3DColor(k+dep));

			Ps.push_back(newVertex);
			Ms.push_back(newVertex);

			if(sign == 1)
				Ps.push_back(P->sommet3DColor(k+dep));

			if(sign == -1)
				Ms.push_back(P->sommet3DColor(k+dep));

			prev_sign = sign;
		} // prev_sign != 0 donc necessairement sign = 0. Le sommet tombe dans le plan
		else
		{
			Feedback3DColor newVertex = P->sommet3DColor(k+dep);

			Ps.push_back(newVertex);
			Ms.push_back(newVertex);

			prev_sign = -prev_sign;
		}
	}

	//if((Ps.size() > 100)||(Ms.size() > 100))
		//printf("BSPNode::Classify: Error. nPs = %d, nMs = %d.\n",int(Ps.size()),int(Ms.size()));

	//if(Ps.size() < 3)
		//printf("BSPNode::Classify: Error. nPs = %d.\n",int(Ps.size()));

	//if(Ms.size() < 3)
		//printf("BSPNode::Classify: Error. nMs = %d.\n",int(Ms.size()));

	// Les polygones sont convexes, car OpenGL les clip lui-meme.

	//  Si les parents ne sont pas degeneres, plus et moins ne le
	// sont pas non plus.

	plus_  = new Polygone(Ps);
	moins_ = new Polygone(Ms);

	delete  P;
}

void BSPNode::insert(Polygone *P)
{
    Polygone *side_plus = nullptr, *side_moins = nullptr;

	// 1 - Check on which size the polygon is, possibly split.

	Classify(P,side_moins,side_plus);

	// 2 - insert polygons

    if(side_plus != nullptr) {
        if(fils_plus == nullptr)
			fils_plus = new BSPNode(side_plus);
		else
			fils_plus->insert(side_plus);
	}
	
    if(side_moins != nullptr) {
        if(fils_moins == nullptr)
			fils_moins = new BSPNode(side_moins);
		else
			fils_moins->insert(side_moins);
	}
}

void BSPNode::recursFillPrimitiveArray(vector<PtrPrimitive>& primitive_tab) const
{
  if(fils_plus != nullptr)
    fils_plus->recursFillPrimitiveArray(primitive_tab);

  for(unsigned int i=0;i<seg_plus.size();++i)
    primitive_tab.push_back(seg_plus[i]);
  for(unsigned int j=0;j<pts_plus.size();++j)
    primitive_tab.push_back(pts_plus[j]);

  if(polygone != nullptr)
    primitive_tab.push_back(polygone);

  if(fils_moins != nullptr)
    fils_moins->recursFillPrimitiveArray(primitive_tab);

  for(unsigned int i2=0;i2<seg_moins.size();++i2)
    primitive_tab.push_back(seg_moins[i2]);
  for(unsigned int j2=0;j2<pts_moins.size();++j2)
    primitive_tab.push_back(pts_moins[j2]);
}

void BSPNode::insert(Point *P)
{
	int res = Classify(P);

	if(res == -1) {
        if(fils_moins == nullptr)
			pts_moins.push_back(P);
		else
			fils_moins->insert(P);
	}

	if(res == 1) {
        if(fils_plus == nullptr)
			pts_plus.push_back(P);
		else
			fils_plus->insert(P);
	}
}

void BSPNode::insert(Segment *S)
{
    Segment *side_plus = nullptr, *side_moins = nullptr;

	Classify(S,side_moins,side_plus);

    if(side_plus != nullptr) {
        if(fils_plus == nullptr)
			seg_plus.push_back(side_plus);
		else
			fils_plus->insert(side_plus);
	}

    if(side_moins != nullptr) {
        if(fils_moins == nullptr)
			seg_moins.push_back(side_moins);
		else
			fils_moins->insert(side_moins);
	}
}

BSPNode::BSPNode(Polygone *P)
{
  polygone = P;

  initEquation(P,a,b,c,d);

  fils_moins = nullptr;
  fils_plus  = nullptr;
}

void BSPNode::initEquation(const Polygone *P,double & a, double & b, double & c, double & d)
{
	Vector3 n(0.,0.,0.);
        unsigned int j = 0;

	while((j < P->nbVertices())&& n.infNorm() <= 0.00001)
	{
		n = (P->vertex(j+2) - P->vertex(j+1))^(P->vertex(j) - P->vertex(j+1));
		j++;
	}

	if(n.infNorm() <= 0.00001)
	{
                unsigned int ind = P->nbVertices();

                for(unsigned int i=0;i<P->nbVertices();i++)
			if((P->vertex(i+1)-P->vertex(i)).infNorm() > 0.00001)
			{
				ind = i;
				i = P->nbVertices();
			}

		if(ind < P->nbVertices())	// the polygon is a true segment
		{
			if((P->vertex(ind+1).x() != P->vertex(ind).x())||(P->vertex(ind+1).y() != P->vertex(ind).y()))
			{
				n[0] = - P->vertex(ind+1).y() + P->vertex(ind).y();
				n[1] =   P->vertex(ind+1).x() - P->vertex(ind).x();
				n[2] =   0;
			}
			else
			{
				n[0] = - P->vertex(ind+1).z() + P->vertex(ind).z();
				n[1] =   0;
				n[2] =   P->vertex(ind+1).x() - P->vertex(ind).x();
			}
		}
		else				// the polygon is a point
		{
			n[0] = 1.0;
			n[1] = 0.0;
			n[2] = 0.0;
		}
	}

	double D = n.norm();

	if(n[2] < 0.0)
		n /= -D;
	else
		n /= D;

	d = n*P->vertex(0);

	a = n[0];
	b = n[1];
	c = n[2];
}

