/* +---------------------------------------------------------------------------+
   |                     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 "maps-precomp.h" // Precomp header

#include <mrpt/opengl/CAngularObservationMesh.h>
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/utils/stl_serialization.h>
#include <mrpt/utils/CStream.h>

#if MRPT_HAS_OPENGL_GLUT
	#ifdef MRPT_OS_WINDOWS
		// Windows:
		#include <windows.h>
	#endif

	#ifdef MRPT_OS_APPLE
		#include <OpenGL/gl.h>
	#else
		#include <GL/gl.h>
	#endif
#endif

// Include libraries in linking:
#if MRPT_HAS_OPENGL_GLUT && defined(MRPT_OS_WINDOWS)
		// WINDOWS:
		#if defined(_MSC_VER) || defined(__BORLANDC__)
			#pragma comment (lib,"opengl32.lib")
			#pragma comment (lib,"GlU32.lib")
		#endif
#endif // MRPT_HAS_OPENGL_GLUT



using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::math;
using namespace mrpt::opengl;
using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace mrpt::math;

IMPLEMENTS_SERIALIZABLE(CAngularObservationMesh,CRenderizableDisplayList,mrpt::opengl)

void CAngularObservationMesh::addTriangle(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) const	{
	const TPoint3D *arr[3]={&p1,&p2,&p3};
	CSetOfTriangles::TTriangle t;
	for (size_t i=0;i<3;i++)	{
		t.x[i]=arr[i]->x;
		t.y[i]=arr[i]->y;
		t.z[i]=arr[i]->z;
		t.r[i]=m_color.R*(1.f/255);
		t.g[i]=m_color.G*(1.f/255);
		t.b[i]=m_color.B*(1.f/255);
		t.a[i]=m_color.A*(1.f/255);
	}
	triangles.push_back(t);
	CRenderizableDisplayList::notifyChange();
}

void CAngularObservationMesh::updateMesh() const	{
	CRenderizableDisplayList::notifyChange();

	size_t numRows=scanSet.size();
	triangles.clear();
	if (numRows<=1)	{
		actualMesh.setSize(0,0);
		validityMatrix.setSize(0,0);
		meshUpToDate=true;
		return;
	}
	if (pitchBounds.size()!=numRows&&pitchBounds.size()!=2) return;
	size_t numCols=scanSet[0].scan.size();
	actualMesh.setSize(numRows,numCols);
	validityMatrix.setSize(numRows,numCols);
	double *pitchs=new double[numRows];
	if (pitchBounds.size()==2)	{
		double p1=pitchBounds[0];
		double p2=pitchBounds[1];
		for (size_t i=0;i<numRows;i++) pitchs[i]=p1+(p2-p1)*static_cast<double>(i)/static_cast<double>(numRows-1);
	}	else for (size_t i=0;i<numRows;i++) pitchs[i]=pitchBounds[i];
	const bool rToL=scanSet[0].rightToLeft;
	for (size_t i=0;i<numRows;i++)	{
		const std::vector<float> &scan=scanSet[i].scan;
		const std::vector<char> valid=scanSet[i].validRange;
		const double pitchIncr=scanSet[i].deltaPitch;
		const double aperture=scanSet[i].aperture;
		const CPose3D origin=scanSet[i].sensorPose;
		//This is not an error...
		for (size_t j=0;j<numCols;j++) if ((validityMatrix(i,j)=(valid[j]!=0)))	{
			double pYaw=aperture*((static_cast<double>(j)/static_cast<double>(numCols-1))-0.5);
			// Without the pitch since it's already within each sensorPose:
			actualMesh(i,j)=(origin+CPose3D(0,0,0,rToL?pYaw:-pYaw,pitchIncr))+CPoint3D(scan[j],0,0);
		}
	}
	delete[] pitchs;
	triangles.reserve(2*(numRows-1)*(numCols-1));
	for (size_t k=0;k<numRows-1;k++)	{
		for (size_t j=0;j<numCols-1;j++)	{
			int b1=validityMatrix(k,j)?1:0;
			int b2=validityMatrix(k,j+1)?1:0;
			int b3=validityMatrix(k+1,j)?1:0;
			int b4=validityMatrix(k+1,j+1)?1:0;
			switch (b1+b2+b3+b4)	{
				case 0:
				case 1:
				case 2:
					break;
				case 3:
					if (!b1) addTriangle(actualMesh(k,j+1),actualMesh(k+1,j),actualMesh(k+1,j+1));
					else if (!b2) addTriangle(actualMesh(k,j),actualMesh(k+1,j),actualMesh(k+1,j+1));
					else if (!b3) addTriangle(actualMesh(k,j),actualMesh(k,j+1),actualMesh(k+1,j+1));
					else if (!b4) addTriangle(actualMesh(k,j),actualMesh(k,j+1),actualMesh(k+1,j));
					break;
				case 4:
					addTriangle(actualMesh(k,j),actualMesh(k,j+1),actualMesh(k+1,j));
					addTriangle(actualMesh(k+1,j+1),actualMesh(k,j+1),actualMesh(k+1,j));
			}
		}
	}
	meshUpToDate=true;
}

void CAngularObservationMesh::render_dl() const	{
#if MRPT_HAS_OPENGL_GLUT
	if (mEnableTransparency)	{
		glEnable(GL_BLEND);
		glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
	}	else	{
		glEnable(GL_DEPTH_TEST);
		glDisable(GL_BLEND);
	}
	glEnable(GL_COLOR_MATERIAL);
	glShadeModel(GL_SMOOTH);
	if (!meshUpToDate) updateMesh();
	if (!mWireframe) glBegin(GL_TRIANGLES);
	for (size_t i=0;i<triangles.size();i++)	{
		const CSetOfTriangles::TTriangle &t=triangles[i];
		float ax=t.x[1]-t.x[0];
		float bx=t.x[2]-t.x[0];
		float ay=t.y[1]-t.y[0];
		float by=t.y[2]-t.y[0];
		float az=t.z[1]-t.z[0];
		float bz=t.z[2]-t.z[0];
		glNormal3f(ay*bz-az*by,az*bx-ax*bz,ax*by-ay*bx);
		if (mWireframe) glBegin(GL_LINE_LOOP);
		for (int i=0;i<3;i++)	{
			glColor4f(t.r[i],t.g[i],t.b[i],t.a[i]);
			glVertex3f(t.x[i],t.y[i],t.z[i]);
		}
		if (mWireframe) glEnd();
	}
	if (!mWireframe) glEnd();
	glDisable(GL_BLEND);
	glDisable(GL_LIGHTING);
#endif
}

bool CAngularObservationMesh::traceRay(const mrpt::poses::CPose3D &o,double &dist) const	{
	MRPT_UNUSED_PARAM(o);
	MRPT_UNUSED_PARAM(dist);
	//TODO: redo
	return false;
}

bool CAngularObservationMesh::setScanSet(const std::vector<mrpt::obs::CObservation2DRangeScan> &scans)	{
	CRenderizableDisplayList::notifyChange();

	//Returns false if the scan is inconsistent
	if (scans.size()>0)	{
		size_t setSize=scans[0].scan.size();
		bool rToL=scans[0].rightToLeft;
		for (std::vector<CObservation2DRangeScan>::const_iterator it=scans.begin()+1;it!=scans.end();++it)	{
			if (it->scan.size()!=setSize) return false;
			if (it->rightToLeft!=rToL) return false;
		}
	}
	scanSet=scans;
	meshUpToDate=false;
	return true;
}

void CAngularObservationMesh::setPitchBounds(const double initial,const double final)	{
	CRenderizableDisplayList::notifyChange();

	pitchBounds.clear();
	pitchBounds.push_back(initial);
	pitchBounds.push_back(final);
	meshUpToDate=false;
}
void CAngularObservationMesh::setPitchBounds(const std::vector<double> &bounds)	{
	CRenderizableDisplayList::notifyChange();

	pitchBounds=bounds;
	meshUpToDate=false;
}
void CAngularObservationMesh::getPitchBounds(double &initial,double &final) const {
	initial=pitchBounds.front();
	final=pitchBounds.back();
}
void CAngularObservationMesh::getPitchBounds(std::vector<double> &bounds) const {
	bounds=pitchBounds;
}
void CAngularObservationMesh::getScanSet(std::vector<CObservation2DRangeScan> &scans) const	{
	scans=scanSet;
}

void CAngularObservationMesh::generateSetOfTriangles(CSetOfTrianglesPtr &res) const	{
	if (!meshUpToDate) updateMesh();
	res->insertTriangles(triangles.begin(),triangles.end());
	//for (vector<CSetOfTriangles::TTriangle>::iterator it=triangles.begin();it!=triangles.end();++it) res->insertTriangle(*it);
}

struct CAngularObservationMesh_fnctr	{
		CPointsMap *m;
		CAngularObservationMesh_fnctr(CPointsMap *p):m(p)	{}
		inline void operator()(const CObservation2DRangeScan &obj)	{
			m->insertObservation(&obj);
		}
	};

void CAngularObservationMesh::generatePointCloud(CPointsMap *out_map) const {
	ASSERT_(out_map);
	out_map->clear();
/*	size_t numRows=scanSet.size();
	if ((pitchBounds.size()!=numRows)&&(pitchBounds.size()!=2)) return;
	std::vector<double> pitchs(numRows);
	if (pitchBounds.size()==2)	{
		double p1=pitchBounds[0];
		double p2=pitchBounds[1];
		for (size_t i=0;i<numRows;i++) pitchs[i]=p1+(p2-p1)*static_cast<double>(i)/static_cast<double>(numRows-1);
	}	else for (size_t i=0;i<numRows;i++) pitchs[i]=pitchBounds[i];
	for (size_t i=0;i<numRows;i++) out_map->insertObservation(&scanSet[i]);
*/

	std::for_each(scanSet.begin(),scanSet.end(),CAngularObservationMesh_fnctr(out_map));
}

/*---------------------------------------------------------------
   Implements the writing to a CStream capability of
     CSerializable objects
  ---------------------------------------------------------------*/
void CAngularObservationMesh::writeToStream(mrpt::utils::CStream &out,int *version) const	{
	if (version) *version=0;
	else	{
		writeToStreamRender(out);
		//Version 0:
		out<<pitchBounds<<scanSet<<mWireframe<<mEnableTransparency;
	}
}

/*---------------------------------------------------------------
	Implements the reading from a CStream capability of
		CSerializable objects
  ---------------------------------------------------------------*/
void CAngularObservationMesh::readFromStream(mrpt::utils::CStream &in,int version)	{
	switch (version)	{
		case 0:
			readFromStreamRender(in);
			in>>pitchBounds>>scanSet>>mWireframe>>mEnableTransparency;
			break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
	};
	meshUpToDate=false;
}

void CAngularObservationMesh::TDoubleRange::values(std::vector<double> &vals) const	{
	double value=initialValue();
	double incr=increment();
	size_t am=amount();
	vals.resize(am);
	for (size_t i=0;i<am-1;i++,value+=incr) vals[i]=value;
	vals[am-1]=finalValue();
}

void CAngularObservationMesh::getTracedRays(CSetOfLinesPtr &res) const	{
	if (!meshUpToDate) updateMesh();
	size_t count=0;
	for (size_t i=0;i<validityMatrix.getRowCount();i++) for (size_t j=0;j<validityMatrix.getColCount();j++) if (validityMatrix(i,j)) count++;
	res->reserve(count);
	for (size_t i=0;i<actualMesh.getRowCount();i++) for (size_t j=0;j<actualMesh.getColCount();j++) if (validityMatrix(i,j)) res->appendLine(TPose3D(scanSet[i].sensorPose),actualMesh(i,j));
}

class FAddUntracedLines	{
public:
	CSetOfLinesPtr &lins;
	const CPoint3D &pDist;
	std::vector<double> pitchs;
	FAddUntracedLines(CSetOfLinesPtr &l,const CPoint3D &p,const vector<double> &pi):lins(l),pDist(p),pitchs()	{
		pitchs.reserve(pi.size());
		for (vector<double>::const_reverse_iterator it=pi.rbegin();it!=pi.rend();++it) pitchs.push_back(*it);
	}
	void operator()(const CObservation2DRangeScan& obs)	{
		size_t hm=obs.scan.size();
		for (vector<char>::const_iterator it=obs.validRange.begin();it!=obs.validRange.end();++it) if (*it) hm--;
		lins->reserve(hm);
		for (size_t i=0;i<obs.scan.size();i++) if (!obs.validRange[i])	{
			double yaw=obs.aperture*((static_cast<double>(i)/static_cast<double>(obs.scan.size()-1))-0.5);
			lins->appendLine(TPoint3D(obs.sensorPose),TPoint3D(obs.sensorPose+CPose3D(0,0,0,obs.rightToLeft?yaw:-yaw,obs.deltaPitch*i+pitchs.back(),0)+pDist));
		}
		pitchs.pop_back();
	}
};
void CAngularObservationMesh::getUntracedRays(CSetOfLinesPtr &res,double dist) const	{
	for_each(scanSet.begin(),scanSet.end(),FAddUntracedLines(res,dist,pitchBounds));
}

TPolygon3D createFromTriangle(const CSetOfTriangles::TTriangle &t)	{
	TPolygon3D res(3);
	for (size_t i=0;i<3;i++)	{
		res[i].x=t.x[i];
		res[i].y=t.y[i];
		res[i].z=t.z[i];
	}
	return res;
}

void CAngularObservationMesh::generateSetOfTriangles(std::vector<TPolygon3D> &res) const	{
	if (!meshUpToDate) updateMesh();
	res.resize(triangles.size());
	transform(triangles.begin(),triangles.end(),res.begin(),createFromTriangle);
}

void CAngularObservationMesh::getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
{
	if (!meshUpToDate) updateMesh();

	bb_min = mrpt::math::TPoint3D(std::numeric_limits<double>::max(),std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
	bb_max = mrpt::math::TPoint3D(-std::numeric_limits<double>::max(),-std::numeric_limits<double>::max(),-std::numeric_limits<double>::max());

	for (size_t i=0;i<triangles.size();i++)
	{
		const CSetOfTriangles::TTriangle &t=triangles[i];

		keep_min(bb_min.x, t.x[0]);  keep_max(bb_max.x, t.x[0]);
		keep_min(bb_min.y, t.y[0]);  keep_max(bb_max.y, t.y[0]);
		keep_min(bb_min.z, t.z[0]);  keep_max(bb_max.z, t.z[0]);

		keep_min(bb_min.x, t.x[1]);  keep_max(bb_max.x, t.x[1]);
		keep_min(bb_min.y, t.y[1]);  keep_max(bb_max.y, t.y[1]);
		keep_min(bb_min.z, t.z[1]);  keep_max(bb_max.z, t.z[1]);

		keep_min(bb_min.x, t.x[2]);  keep_max(bb_max.x, t.x[2]);
		keep_min(bb_min.y, t.y[2]);  keep_max(bb_max.y, t.y[2]);
		keep_min(bb_min.z, t.z[2]);  keep_max(bb_max.z, t.z[2]);
	}

	// Convert to coordinates of my parent:
	m_pose.composePoint(bb_min, bb_min);
	m_pose.composePoint(bb_max, bb_max);
}
