/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "opengl-precomp.h"  // Precompiled header


#include <mrpt/opengl/CEllipsoid.h>
#include <mrpt/math/CMatrix.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/matrix_serialization.h>
#include <mrpt/utils/CStream.h>

#include "opengl_internals.h"

using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::utils;
using namespace mrpt::math;
using namespace std;

IMPLEMENTS_SERIALIZABLE( CEllipsoid, CRenderizableDisplayList, mrpt::opengl )

/*---------------------------------------------------------------
							render
  ---------------------------------------------------------------*/
void   CEllipsoid::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
	MRPT_START

    const size_t dim = m_cov.getColCount();

	if(m_eigVal(0,0) != 0.0 && m_eigVal(1,1) != 0.0 && (dim==2 || m_eigVal(2,2) != 0.0) && m_quantiles!=0.0)
	{
		glEnable(GL_BLEND);
		checkOpenGLError();
		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
		checkOpenGLError();
	    	glLineWidth(m_lineWidth);
		checkOpenGLError();

		if (dim==2)
		{
			glDisable(GL_LIGHTING);  // Disable lights when drawing lines

			// ---------------------
			//     2D ellipse
			// ---------------------

			/* Equivalent MATLAB code:
			 *
			 * q=1;
			 * [vec val]=eig(C);
			 * M=(q*val*vec)';
			 * R=M*[x;y];
			 * xx=R(1,:);yy=R(2,:);
			 * plot(xx,yy), axis equal;
			 */

			double			ang;
			unsigned int	i;

			// Compute the new vectors for the ellipsoid:
			CMatrixDouble 	M;
			M.noalias() = double(m_quantiles) * m_eigVal * m_eigVec.adjoint();

			glBegin( GL_LINE_LOOP );

			// Compute the points of the 2D ellipse:
			for (i=0,ang=0;i<m_2D_segments;i++,ang+= (M_2PI/m_2D_segments))
			{
				double ccos = cos(ang);
				double ssin = sin(ang);

				const float x = ccos * M.get_unsafe(0,0) + ssin * M.get_unsafe(1,0);
				const float y = ccos * M.get_unsafe(0,1) + ssin * M.get_unsafe(1,1);

				glVertex2f( x,y );
			} // end for points on ellipse

			glEnd();

			// 2D: Save bounding box:
			const double max_radius = m_quantiles * std::max( m_eigVal(0,0), m_eigVal(1,1) );
			m_bb_min = mrpt::math::TPoint3D(-max_radius,-max_radius, 0);
			m_bb_max = mrpt::math::TPoint3D(max_radius,max_radius, 0);
			// Convert to coordinates of my parent:
			m_pose.composePoint(m_bb_min, m_bb_min);
			m_pose.composePoint(m_bb_max, m_bb_max);

			glEnable(GL_LIGHTING);
		}
		else
		{
			// ---------------------
			//    3D ellipsoid
			// ---------------------
			GLfloat		mat[16];

			//  A homogeneous transformation matrix, in this order:
			//
			//     0  4  8  12
			//     1  5  9  13
			//     2  6  10 14
			//     3  7  11 15
			//
			mat[3] = mat[7] = mat[11] = 0;
			mat[15] = 1;
			mat[12] = mat[13] = mat[14] = 0;

			mat[0] = m_eigVec(0,0); mat[1] = m_eigVec(1,0); mat[2] = m_eigVec(2,0);	// New X-axis
			mat[4] = m_eigVec(0,1); mat[5] = m_eigVec(1,1); mat[6] = m_eigVec(2,1);	// New X-axis
			mat[8] = m_eigVec(0,2); mat[9] = m_eigVec(1,2); mat[10] = m_eigVec(2,2);	// New X-axis

			GLUquadricObj	*obj = gluNewQuadric();
			checkOpenGLError();

			if (!m_drawSolid3D) glDisable(GL_LIGHTING);  // Disable lights when drawing lines

			gluQuadricDrawStyle( obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);

			glPushMatrix();
			glMultMatrixf( mat );
        		glScalef(m_eigVal(0,0)*m_quantiles,m_eigVal(1,1)*m_quantiles,m_eigVal(2,2)*m_quantiles);

			gluSphere( obj, 1,m_3D_segments,m_3D_segments);
			checkOpenGLError();

			glPopMatrix();

			gluDeleteQuadric(obj);
			checkOpenGLError();

			// 3D: Save bounding box:
			const double max_radius = m_quantiles * std::max( m_eigVal(0,0), std::max(m_eigVal(1,1), m_eigVal(2,2) ) );
			m_bb_min = mrpt::math::TPoint3D(-max_radius,-max_radius, 0);
			m_bb_max = mrpt::math::TPoint3D(max_radius,max_radius, 0);
			// Convert to coordinates of my parent:
			m_pose.composePoint(m_bb_min, m_bb_min);
			m_pose.composePoint(m_bb_max, m_bb_max);
		}


		glDisable(GL_BLEND);

		glEnable(GL_LIGHTING);
	}
	MRPT_END_WITH_CLEAN_UP( \
		cout << "Covariance matrix leading to error is:" << endl << m_cov << endl; \
	);
#endif
}

/*---------------------------------------------------------------
   Implements the writing to a CStream capability of
     CSerializable objects
  ---------------------------------------------------------------*/
void  CEllipsoid::writeToStream(mrpt::utils::CStream &out,int *version) const
{
	if (version)
		*version = 1;
	else
	{
		writeToStreamRender(out);
		out << m_cov << m_drawSolid3D << m_quantiles << (uint32_t)m_2D_segments << (uint32_t)m_3D_segments << m_lineWidth;
	}
}

/*---------------------------------------------------------------
	Implements the reading from a CStream capability of
		CSerializable objects
  ---------------------------------------------------------------*/
void  CEllipsoid::readFromStream(mrpt::utils::CStream &in,int version)
{
	switch(version)
	{
	case 0:
	case 1:
		{
			uint32_t	i;
			readFromStreamRender(in);
			if (version==0)
			{
				CMatrix c;
				in >> c; m_cov = c.cast<double>();
			}
			else
			{
				in >> m_cov;
			}

			in >> m_drawSolid3D >> m_quantiles;
			in >> i; m_2D_segments = i;
			in >> i; m_3D_segments = i;
			in >> m_lineWidth;

			// Update cov. matrix cache:
			m_prevComputedCov = m_cov;
			m_cov.eigenVectors(m_eigVec,m_eigVal);
			m_eigVal = m_eigVal.array().sqrt().matrix();

		} break;
	default:
		MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)

	};
	CRenderizableDisplayList::notifyChange();
}

bool quickSolveEqn(double a,double b_2,double c,double &t)	{
	double delta=square(b_2)-a*c;
	if (delta==0) return (t=-b_2/a)>=0;
	else if (delta>0)	{
		delta=sqrt(delta);
		if ((t=(-b_2-delta)/a)>=0) return true;
		else return (t=(-b_2+delta)/a)>=0;
	}	else return false;
}

bool CEllipsoid::traceRay(const mrpt::poses::CPose3D &o,double &dist) const	{
	if (m_cov.getRowCount()!=3) return false;
	TLine3D lin,lin2;
	createFromPoseX(o-this->m_pose,lin);
	lin.unitarize();	//By adding this line, distance from any point of the line to its base is exactly equal to the "t".
	for (size_t i=0;i<3;i++)	{
		lin2.pBase[i]=0;
		lin2.director[i]=0;
		for (size_t j=0;j<3;j++)	{
			double vji=m_eigVec(j,i);
			lin2.pBase[i]+=vji*lin.pBase[j];
			lin2.director[i]+=vji*lin.director[j];
		}
	}
	double a=0,b_2=0,c=-square(m_quantiles);
	for (size_t i=0;i<3;i++)	{
		double ev=m_eigVal(i,i);
		a+=square(lin2.director[i]/ev);
		b_2+=lin2.director[i]*lin2.pBase[i]/square(ev);
		c+=square(lin2.pBase[i]/ev);
	}
	return quickSolveEqn(a,b_2,c,dist);
}

void CEllipsoid::setCovMatrix( const mrpt::math::CMatrixDouble &m, int resizeToSize)
{
	MRPT_START

	ASSERT_( m.getColCount() == m.getRowCount() );
	ASSERT_( size(m,1)==2 || size(m,1)==3 || (resizeToSize>0 && (resizeToSize==2 || resizeToSize==3)));

	m_cov = m;
	if (resizeToSize>0 && resizeToSize<(int)size(m,1))
		m_cov.setSize(resizeToSize,resizeToSize);

	if (m_cov==m_prevComputedCov)
		return; // Done.

	CRenderizableDisplayList::notifyChange();

	// Handle the special case of an ellipsoid of volume = 0
	const double d=m_cov.det();
	if (d==0 || d!=d) // Note: "d!=d" is a great test for invalid numbers, don't remove!
	{
		// All zeros:
		m_prevComputedCov = m_cov;
		m_eigVec.zeros(3,3);
		m_eigVal.zeros(3,3);
	}
	else
	{
		// Not null matrix: compute the eigen-vectors & values:
		m_prevComputedCov = m_cov;
		m_cov.eigenVectors(m_eigVec,m_eigVal);
		m_eigVal = m_eigVal.array().sqrt().matrix();
		// Do the scale at render to avoid recomputing the m_eigVal for different m_quantiles
	}


	MRPT_END
}

void CEllipsoid::setCovMatrix( const mrpt::math::CMatrixFloat &m, int resizeToSize)
{
	CRenderizableDisplayList::notifyChange();
	setCovMatrix( CMatrixDouble(m), resizeToSize);
}

/** Evaluates the bounding box of this object (including possible children) in the coordinate frame of the object parent. */
void CEllipsoid::getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
{
	bb_min = m_bb_min;
	bb_max = m_bb_max;
}
