#include "RadarLobe.h"

#include <osg/LineWidth>
#include <osg/Depth>
#include <osg/BlendFunc>

//#include <DPLineSegmentIntersector>
#include <osgSim/LineOfSight>
#include <osgUtil/IntersectionVisitor>
#include <osgUtil/LineSegmentIntersector>

#include "RadarLobeOperate.h"
//#include "IntersectionOperate.h"

//#include "UserTool.h"
#include <QDebug>

//#include "UDPComm.h"

bool g_bIntersectTerrain = false;
double g_dDisHill = 0.0;
double g_dAngle = 0.0;

class RadarLobeCallback : public osg::NodeCallback
{
public:
	RadarLobeCallback(RadarLobe *pRadarLobe, float loopTime, osg::Vec4d surfaceColor)
	{
		_pRadarLobe = pRadarLobe;

		_loopTime = loopTime;

		_surfaceColor = surfaceColor;

		_previous = 0.0;
		_currTime = 0.0;

		m_angleYaw = 0.0;
		m_anglePitch = 0.0;

		m_bFollow = false;
		m_bStart = false;
		m_bInit = false;

		m_angleYawInit = 0.0;
		m_anglePitchInit = 0.0;

		m_currRadio = 1.0;
	}

	void SetLoopTime(double loopTime)
	{
		_loopTime = loopTime;
	}

	void SetAngle(double angleYaw, double anglePitch)
	{
		OpenThreads::ScopedLock<OpenThreads::Mutex> lock(_mutex);

		m_angleYaw = angleYaw;

		if (m_anglePLast != anglePitch)
		{
			//_pRadarLobe->UpdateArea(-anglePitch);

			m_anglePLast = anglePitch;
		}

		m_anglePitch = anglePitch;
	}

	void SetStart(bool bStart)
	{
		m_bStart = bStart;
	}

	void SetAngleInit(double angleYaw, double anglePitch)
	{
		m_angleYawInit = angleYaw;
		m_anglePitchInit = anglePitch;

		_pRadarLobe->UpdateArea(-m_anglePitchInit);

		m_bInit = true;
	}

	void SetFollowState(bool bFlag)
	{
		m_bFollow = bFlag;
	}

	void SetCurrRadio(double currRadio)
	{
		m_currRadio = currRadio;
	}

	void SetCurrTimer(double t)
	{
		OpenThreads::ScopedLock<OpenThreads::Mutex> lock(_mutex);
		_currTime = t;
	}

	virtual void operator() (osg::Node * node, osg::NodeVisitor * nv)
	{
		const osg::FrameStamp* f = nv->getFrameStamp();
		float dt = f->getSimulationTime() - _previous;

		if (m_bStart)
		{
			osg::Geode* geode = dynamic_cast<osg::Geode *>(node);

			if (!geode)
				return;

			geode->removeDrawables(0, geode->getNumDrawables());

			UpdateRay(geode);
		}
		else
		{
			if (m_bInit)
			{
				m_bInit = false;

				osg::Geode* geode = dynamic_cast<osg::Geode *>(node);

				if (!geode)
					return;

				geode->removeDrawables(0, geode->getNumDrawables());

				UpdateRay(geode);
			}
		}
		
		_previous = f->getSimulationTime();

		//traverse(node, nv);
	}

	void UpdateRay(osg::Geode* geode)
	{
		double dAngleYaw = 0.0;//_currTime * 360.0 / _loopTime;
		double dAnglePitch = 0.0;

		if (m_bFollow)
		{
			OpenThreads::ScopedLock<OpenThreads::Mutex> lock(_mutex);
			dAngleYaw = m_angleYaw;
			dAnglePitch = m_anglePitch;

			{
				double dotVar = dAngleYaw - int(dAngleYaw);

				int var = int(dAngleYaw) % 360;

				dAngleYaw = var + dotVar;
			}

			if (dAngleYaw < 0.0)
			{
				dAngleYaw = 360.0 + dAngleYaw;
			}
		}
		else
		{
			OpenThreads::ScopedLock<OpenThreads::Mutex> lock(_mutex);

			dAngleYaw = m_angleYawInit + _currTime * /*360.0 /*/ _loopTime;
			dAnglePitch = m_anglePitchInit;

			dAngleYaw -= 90.0;

			{
				double dotVar = dAngleYaw - int(dAngleYaw);

				int var = int(dAngleYaw) % 360;

				dAngleYaw = var + dotVar;
			}

			if (dAngleYaw < 0.0)
			{
				dAngleYaw = 360.0 + dAngleYaw;
			}

			dAngleYaw = 360.0 - dAngleYaw;
		}

		//QList<osg::Vec3d> pts= _pRadarLobe->GetLobePt();

		QVariant var;
		//_pRadarLobe->Intersection(dAngleYaw, dAnglePitch, var);
		QVariantList varList = var.toList();

		int nNum = varList.size();
		int nCount = nNum / 100.0;

		osg::Vec3Array *coords = NULL;
		osg::Vec4Array* colors = NULL;

		for (int nI = 0; nI < varList.size() - 4; nI++)
		{
			QString str1 = varList.at(nI).toString();
			QStringList strList1 = str1.split(',');
			QString str2 = varList.at(nI + 4).toString();
			QStringList strList2 = str2.split(',');

			if (nI % 4 == 0)
			{
				if (coords)
				{
					osg::Geometry *pGeometry = _pRadarLobe->CreateGeometry(colors, coords, osg::PrimitiveSet::QUAD_STRIP);
					geode->addDrawable(pGeometry);
				}
				
				coords = new osg::Vec3Array;
				colors = new osg::Vec4Array;
			}

			osg::Vec3d pt1 = osg::Vec3d(strList1.at(0).toDouble(), strList1.at(1).toDouble(), strList1.at(2).toDouble());
			osg::Vec3d pt2 = osg::Vec3d(strList2.at(0).toDouble(), strList2.at(1).toDouble(), strList2.at(2).toDouble());
			coords->push_back(pt1);
			coords->push_back(pt2);

			if (strList1.size() == 4)
			{
				bool bHit = strList1.at(3).toInt();

				if (bHit)
				{
					colors->push_back(osg::Vec4(1.0, 0.0, 0.0, 1.0));
				}
				else
				{
					colors->push_back(_surfaceColor);
				}
			}
			else
			{
				colors->push_back(_surfaceColor);
			}

			if (strList2.size() == 4)
			{
				bool bHit = strList2.at(3).toInt();

				if (bHit)
				{
					colors->push_back(osg::Vec4(1.0, 0.0, 0.0, 1.0));
				}
				else
				{
					colors->push_back(_surfaceColor);
				}
			}
			else
			{
				colors->push_back(_surfaceColor);
			}

			//colors->push_back(_surfaceColor);
			//colors->push_back(_surfaceColor);
			//for (int nJ = 0; nJ < ptList1.size(); nJ++)
			//{
			//	QString str1 = ptList1.at(nJ).toString();
			//	QStringList strList1 = str1.split(',');

			//	if (strList1.size() == 4)
			//	{
			//		bool bHit = strList1.at(3).toInt();

			//		if (bHit)
			//		{
			//			colors->push_back(osg::Vec4(1.0, 0.0, 0.0, 1.0));
			//		}
			//		else
			//		{
			//			colors->push_back(_surfaceColor);
			//		}
			//	}
			//	else
			//	{
			//		colors->push_back(_surfaceColor);
			//	}

			//	osg::Vec3d pt1 = osg::Vec3d(strList1.at(0).toDouble(), strList1.at(1).toDouble(), strList1.at(2).toDouble());

			//	QString str2 = ptList2.at(nJ).toString();
			//	QStringList strList2 = str2.split(',');

			//	if (strList2.size() == 4)
			//	{
			//		bool bHit = strList2.at(3).toInt();

			//		if (bHit)
			//		{
			//			colors->push_back(osg::Vec4(1.0, 0.0, 0.0, 1.0));
			//		}
			//		else
			//		{
			//			colors->push_back(_surfaceColor);
			//		}
			//	}
			//	else
			//	{
			//		colors->push_back(_surfaceColor);
			//	}

			//	osg::Vec3d pt2 = osg::Vec3d(strList2.at(0).toDouble(), strList2.at(1).toDouble(), strList2.at(2).toDouble());

			//	coords->push_back(pt1);
			//	coords->push_back(pt2);
			//}
		}

		osgSim::DOFTransform *pDof = _pRadarLobe->GetRadarDof();
		if (pDof)
		{
			osg::Vec3 hpr = pDof->getCurrentHPR();
			double yaw = osg::RadiansToDegrees(hpr.x());

			hpr.x() = osg::DegreesToRadians(dAngleYaw - 90.0);
			pDof->setCurrentHPR(hpr);
		}
	}

private:
	float _previous;
	float _currTime;
	float _loopTime;

	osg::Vec4d _surfaceColor;

	RadarLobe *_pRadarLobe;

	double m_angleYawInit;
	double m_anglePitchInit;
	bool m_bInit;

	double m_angleYaw;
	double m_anglePitch;

	double m_anglePLast;

	bool m_bFollow;

	bool m_bStart;

	OpenThreads::Mutex _mutex;

	double m_currRadio;
};

RadarLobe::RadarLobe()
{
	m_pTerrain = NULL;
	m_pIntersectTerrain = NULL;
	m_pGeodeRay = NULL;

	//osg::Depth *depth = new osg::Depth;
	//depth->setWriteMask(false);
	//getOrCreateStateSet()->setAttributeAndModes(depth, osg::StateAttribute::ON);
}

RadarLobe::~RadarLobe()
{
	for (int nI = 0; nI < m_listOperation.size(); nI++)
	{
		RadarLobeOperate *pOperation = m_listOperation[nI];

		delete pOperation;
		pOperation = NULL;
	}
}

void RadarLobe::setRadius(double radius)
{
	m_radius = radius;
}

void RadarLobe::setCenter(osg::Vec3d center)
{
	m_center = center;
}

void RadarLobe::setTerrain(osg::Node *terrain)
{
	m_pTerrain = terrain;
}

void RadarLobe::setLobeAngleHorizontal(double angle)
{
	m_angleH = angle;
}

void RadarLobe::setLobeAngleVertical(double angle)
{
	m_angleV = angle;
}

bool RadarLobe::setRay(double yaw, double pitch, osg::Vec4d surfaceColor, double loopTime)
{
	m_surfaceColor = surfaceColor;
	m_yaw = yaw;
	m_loopTime = loopTime;

	if (m_radius > 0.0)
	{
		if (m_pGeodeRay)
		{
			setCurrRadarAngle(yaw, pitch, true);
			return true;
		} 
	} 
	else
	{
		return false;
	}

	m_pGeodeRay = new osg::Geode;

	QList<osg::Vec3d> pts1 = GetLobePt(m_radius, m_angleH);
	QList<osg::Vec3d> pts2 = GetLobePt(m_radius, m_angleV);
	for (int nI = 0; nI < pts1.size() - 1; nI++)
	{
		osg::Vec3d pt11 = pts1.at(nI);
		osg::Vec3d pt12 = pts2.at(nI);

		osg::Vec3d pt21 = pts1.at(nI + 1);
		osg::Vec3d pt22 = pts2.at(nI + 1);

		osg::Vec3Array *array = new osg::Vec3Array;

		osg::Vec3d org1 = osg::Vec3d(pt11.x(), 0.0, 0.0);
		double a1 = pt11.z();
		double b1 = pt12.z();

		osg::Vec3d org2 = osg::Vec3d(pt21.x(), 0.0, 0.0);
		double a2 = pt21.z();
		double b2 = pt22.z();
			
		int nSample = 100;
		double theta = osg::DegreesToRadians(0.0);
		for (int nJ = 0; nJ <= nSample; nJ++)
		{
			{
				double x = a1 * cos(theta);
				double y = b1 * sin(theta);

				osg::Vec3d pt;
				pt.x() = org1.x();
				pt.y() = org1.y() + x;
				pt.z() = org1.z() + y;

				array->push_back(pt);
			}
			
			{
				double x = a2 * cos(theta);
				double y = b2 * sin(theta);

				osg::Vec3d pt;
				pt.x() = org2.x();
				pt.y() = org2.y() + x;
				pt.z() = org2.z() + y;

				array->push_back(pt);
			}

			theta += osg::DegreesToRadians(360.0 / nSample);
		}

		osg::Geometry *pGeometry = CreateGeometry(surfaceColor, array, osg::PrimitiveSet::QUAD_STRIP);
		m_pGeodeRay->addDrawable(pGeometry);
	}

	m_pGeodeRay->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
	m_pGeodeRay->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);

	osg::ref_ptr<osg::BlendFunc>blendFunc = new osg::BlendFunc();
	blendFunc->setSource(osg::BlendFunc::SRC_ALPHA);
	blendFunc->setDestination(osg::BlendFunc::ONE_MINUS_CONSTANT_ALPHA);

	CreateRadarLobeOperate();

	m_pGeodeRay->setUpdateCallback(new RadarLobeCallback(this, loopTime, surfaceColor));

	addChild(m_pGeodeRay);

	setCurrRadarAngle(yaw, pitch, true);
	//pGeode->getOrCreateStateSet()->setRenderBinDetails(105, "OSGEARTH_SCREEN_SPACE_LAYOUT_BIN");

	return true;
}

osg::Vec3d RadarLobe::GetBezier(osg::Vec3d p1, osg::Vec3d p2, osg::Vec3d p3, osg::Vec3d p4, double t)
{
	osg::Vec3d p;

	double a1 = pow((1 - t), 3);
	double a2 = pow((1 - t), 2) * 3 * t;
	double a3 = 3 * t*t*(1 - t);
	double a4 = t*t*t;

	p.x() = a1*p1.x() + a2*p2.x() + a3*p3.x() + a4*p4.x();
	p.z() = a1*p1.z() + a2*p2.z() + a3*p3.z() + a4*p4.z();
	p.y() = 0.0;

	return p;
}

osg::Geometry * RadarLobe::CreateGeometry(osg::Vec4d sideColor, osg::Vec3Array * coords, osg::PrimitiveSet::Mode mode)
{
	osg::Geometry *pGeometry = new osg::Geometry;
	osg::Vec4Array* colors = new osg::Vec4Array;

	pGeometry->setVertexArray(coords);
	pGeometry->addPrimitiveSet(new osg::DrawArrays(mode, 0, coords->size()));

	colors->push_back(sideColor);
	pGeometry->setColorArray(colors);
	pGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);

	pGeometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF | osg::StateAttribute::PROTECTED);
	pGeometry->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);

	return pGeometry;
}

osg::Geometry * RadarLobe::CreateGeometry(osg::Vec4Array* colors, osg::Vec3Array * coords, osg::PrimitiveSet::Mode mode)
{
	osg::Geometry *pGeometry = new osg::Geometry;

	pGeometry->setVertexArray(coords);
	pGeometry->addPrimitiveSet(new osg::DrawArrays(mode, 0, coords->size()));

	pGeometry->setColorArray(colors);
	pGeometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);

	pGeometry->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF | osg::StateAttribute::PROTECTED);
	pGeometry->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);

	return pGeometry;
}

osg::Vec3 RadarLobe::GetPointMapping(osg::Vec3 org, double ang, osg::Vec3d axis, osg::Vec3d pt)
{
	osg::Matrixd matrix =
		osg::Matrixd::translate(pt - org) *
		osg::Matrixd::rotate(osg::DegreesToRadians(ang), axis) *
		osg::Matrixd::translate(org);

	osg::Vec3 result = matrix.getTrans();

	return result;
}

QList<osg::Vec3d> RadarLobe::GetLobePt(double radius, double angle)
{
	double dMax = 0.707;

	osg::Vec3d v3d1;
	v3d1.x() = dMax * radius;
	v3d1.z() = tan(osg::DegreesToRadians(angle / 2.0)) * v3d1.x();
	v3d1.y() = 0.0;

	double radio1 = 0.4;
	osg::Vec3d v3d2;
	v3d2.x() = v3d1.x() - v3d1.x() * radio1;
	v3d2.z() = v3d1.z();
	v3d2.y() = 0.0;

	osg::Vec3d v3d0 = osg::Vec3d(0, 0, 0);

	double radio2 = 0.5;
	double radio4 = 0.2;
	osg::Vec3d v3d4;
	v3d4.x() = v3d1.x() * radio4;
	v3d4.z() = v3d1.z() * radio2;
	v3d4.y() = 0.0;

	QList<osg::Vec3d> pts;

	osg::Vec3Array *coords = new osg::Vec3Array;
	for (double t = 0.0; t <= 1.0; t += 0.2)
	{
		osg::Vec3d pt = GetBezier(v3d0, v3d4, v3d2, v3d1, t);

		pts.push_back(pt);
	}

	double radio3 = 0.8;
	osg::Vec3d v3d3;
	v3d3.x() = v3d1.x() + (radius - v3d1.x()) * radio3;
	v3d3.z() = v3d1.z();
	v3d3.y() = 0.0;

	osg::Vec3d v3d5;
	v3d5.x() = radius;
	v3d5.z() = 0.0;
	v3d5.y() = 0.0;

	double radio6 = 0.3;
	osg::Vec3d v3d6;
	v3d6.x() = v3d5.x();
	v3d6.z() = v3d1.z() * radio6;;
	v3d6.y() = 0.0;

	for (double t = 0.0; t <= 1.0; t += 0.2)
	{
		osg::Vec3d pt = GetBezier(v3d1, v3d3, v3d6, v3d5, t);

		pts.push_back(pt);
	}

	pts.push_back(v3d5);

	return pts;
}

bool RadarLobe::setArea(double pitch, double lineWidth, osg::Vec4d gridColor, osg::Vec4d surfaceColor)
{
	m_lineWidth = lineWidth;
	m_gridColorArea = gridColor;
	m_surfaceColorArea = surfaceColor;
	m_pitch = pitch;

	if (m_radius > 0.0)
	{
		//if (!m_pIntersectTerrain)
		{
			//CreateIntersectTerrain();
		}

		UpdateArea(pitch);
	}
	
	return true;
}
//
//QVariant RadarLobe::ComputerIntersect(QList<osg::Vec3d> pts)
//{
//	osg::Matrixd localToWorld, worldToLocal;
//	osg::NodePathList nodePaths = getParentalNodePaths();
//	if (!nodePaths.empty())
//	{
//		osg::NodePath nodePath = nodePaths[0];
//		localToWorld = osg::computeLocalToWorld(nodePath);
//		worldToLocal = osg::computeWorldToLocal(nodePath);
//	}
//
//	osg::ref_ptr<osgUtil::IntersectorGroup> ivGroup = new osgUtil::IntersectorGroup();
//
//	int nCount = 0;
//	const double deltaAng = 5.0;
//
//	for (int nI = 0; nI < pts.size(); nI++)
//	{
//		osg::Vec3d pt = pts.at(nI);
//
//		for (int nJ = 0; deltaAng * nJ <= 360.0; nJ++)
//		{
//			osg::Vec3d org = osg::Vec3d(0.0, 0.0, 0.0);
//			osg::Vec3d axis = osg::Z_AXIS;
//
//			osg::Vec3 result = GetPointMapping(org, deltaAng * nJ, axis, pt);
//
//			osg::Vec3d end = result * localToWorld;
//
//			osg::ref_ptr<osgEarth::DPLineSegmentIntersector> dplsi = new osgEarth::DPLineSegmentIntersector(m_center, end);
//			ivGroup->addIntersector(dplsi.get());
//
//			nCount++;
//		}
//	}
//
//	if (m_pTerrain)
//	{
//		osgUtil::IntersectionVisitor iv;
//		iv.setIntersector(ivGroup.get());
//
//		m_pTerrain->accept(iv);
//
//		int nCurrRow = -1;
//
//		QList<osg::Vec3d> ptList1;
//		QList<osg::Vec3d> ptList2;
//
//		QVariantList varList;
//		QVariantList ptList;
//
//		for (int nI = 0; nI < nCount; nI++)
//		{
//			osgEarth::DPLineSegmentIntersector* los = dynamic_cast<osgEarth::DPLineSegmentIntersector*>(ivGroup->getIntersectors()[nI].get());
//			if (!los)
//				continue;
//
//			int nRow = nI / (360.0 / deltaAng + 1);
//
//			if (nCurrRow != nRow)
//			{
//				nCurrRow = nRow;
//
//				if (nCurrRow != 0)
//				{
//					varList.push_back(QVariant(ptList));
//
//					ptList.clear();
//				}
//			}
//
//			osgEarth::DPLineSegmentIntersector::Intersections& hits = los->getIntersections();
//
//			osg::Vec3d start = los->getStart();
//			osg::Vec3d end = los->getEnd();
//
//			osg::Vec3d hit;
//			bool hasLOS = hits.empty();
//			if (!hasLOS)
//			{
//				hit = hits.begin()->getWorldIntersectPoint();
//				hit = hit * worldToLocal;
//
//				QString str =
//					QString::number(hit.x()) + "," +
//					QString::number(hit.y()) + "," +
//					QString::number(hit.z() + 0.5);
//
//				ptList.push_back(QVariant(str));
//			}
//			else
//			{
//				end = end * worldToLocal;
//
//				QString str =
//					QString::number(end.x()) + "," +
//					QString::number(end.y()) + "," +
//					QString::number(end.z());
//
//				ptList.push_back(QVariant(str));
//			}
//		}
//
//		varList.push_back(QVariant(ptList));
//
//		return QVariant(varList);
//	}
//
//	return QVariant();
//}
//
//QVariant RadarLobe::ComputerIntersect(QList<osg::Vec3d> pts, double angle)
//{
//	osg::Matrixd localToWorld, worldToLocal;
//	osg::NodePathList nodePaths = getParentalNodePaths();
//	if (!nodePaths.empty())
//	{
//		osg::NodePath nodePath = nodePaths[0];
//		localToWorld = osg::computeLocalToWorld(nodePath);
//		worldToLocal = osg::computeWorldToLocal(nodePath);
//	}
//
//	osg::ref_ptr<osgUtil::IntersectorGroup> ivGroup = new osgUtil::IntersectorGroup();
//
//	int nCount = 0;
//	const double deltaAng = 5.0;
//	for (int nI = 0; nI < pts.size(); nI++)
//	{
//		osg::Vec3d pt = pts.at(nI);
//		pt = GetPointMapping(osg::Vec3d(0.0, 0.0, 0.0), angle, osg::Z_AXIS, pt);
//
//		for (int nJ = 0; deltaAng * nJ <= 360.0; nJ++)
//		{
//			osg::Vec3d org = osg::Vec3d(pt.x(), 0.0, 0.0);
//
//			org = GetPointMapping(osg::Vec3d(0.0, 0.0, 0.0), angle, osg::Z_AXIS, org);
//
//			osg::Vec3d axis = org;
//			axis.normalize();
//
//			osg::Vec3 result = GetPointMapping(org, deltaAng * nJ, axis, pt);
//
//			osg::Vec3d end = result * localToWorld;
//
//			osg::ref_ptr<osgEarth::DPLineSegmentIntersector> dplsi = new osgEarth::DPLineSegmentIntersector(m_center, end);
//			ivGroup->addIntersector(dplsi.get());
//
//			nCount++;
//		}
//	}
//
//	if (m_pTerrain)
//	{
//		osgUtil::IntersectionVisitor iv;
//		iv.setIntersector(ivGroup.get());
//
//		m_pTerrain->accept(iv);
//
//		int nCurrRow = -1;
//
//		QList<osg::Vec3d> ptList1;
//		QList<osg::Vec3d> ptList2;
//
//		QVariantList varList;
//		QVariantList ptList;
//
//		for (int nI = 0; nI < nCount; nI++)
//		{
//			osgEarth::DPLineSegmentIntersector* los = dynamic_cast<osgEarth::DPLineSegmentIntersector*>(ivGroup->getIntersectors()[nI].get());
//			if (!los)
//				continue;
//
//			int nRow = nI / (360.0 / deltaAng + 1);
//
//			if (nCurrRow != nRow)
//			{
//				nCurrRow = nRow;
//
//				if (nCurrRow != 0)
//				{
//					varList.push_back(QVariant(ptList));
//
//					ptList.clear();
//				}
//			}
//
//			osgEarth::DPLineSegmentIntersector::Intersections& hits = los->getIntersections();
//
//			osg::Vec3d start = los->getStart();
//			osg::Vec3d end = los->getEnd();
//
//			osg::Vec3d hit;
//			bool hasLOS = hits.empty();
//			if (!hasLOS)
//			{
//				hit = hits.begin()->getWorldIntersectPoint();
//				hit = hit * worldToLocal;
//
//				QString str =
//					QString::number(hit.x()) + "," +
//					QString::number(hit.y()) + "," +
//					QString::number(hit.z());
//
//				ptList.push_back(QVariant(str));
//			}
//			else
//			{
//				end = end * worldToLocal;
//
//				QString str =
//					QString::number(end.x()) + "," +
//					QString::number(end.y()) + "," +
//					QString::number(end.z());
//
//				ptList.push_back(QVariant(str));
//			}
//		}
//
//		varList.push_back(QVariant(ptList));
//
//		return QVariant(varList);
//	}
//
//	return QVariant();
//}

osg::Geometry *RadarLobe::GetGridCoords(QString strName, osg::Geode *pGeode, osg::Vec3Array *(&coords))
{
	int nNum = pGeode->getNumDrawables();
	for (int nI=0; nI<nNum; nI++)
	{
		osg::Drawable *pDrawable = pGeode->getDrawable(nI);
		std::string name = pDrawable->getName();

		if (strName.compare(QString::fromLocal8Bit(name.c_str())) == 0)
		{
			osg::Geometry *pGeometry = dynamic_cast<osg::Geometry *>(pDrawable);

			coords = dynamic_cast<osg::Vec3Array *>(pGeometry->getVertexArray());

			return pGeometry;
		}
	}

	return NULL;
}

void RadarLobe::CreateRadarLobeOperate()
{
	//CreateIntersectTerrain();

	int numThreads = 4;

	osg::Matrixd localToWorld, worldToLocal;
	osg::NodePathList nodePaths = getParentalNodePaths();
	if (!nodePaths.empty())
	{
		osg::NodePath nodePath = nodePaths[0];
		localToWorld = osg::computeLocalToWorld(nodePath);
		worldToLocal = osg::computeWorldToLocal(nodePath);
	}

	for (int nI = 0; nI < numThreads; ++nI)
	{
		osg::ref_ptr<RadarLobeOperate> pOperate = new RadarLobeOperate(m_pTerrain, m_center, localToWorld, worldToLocal, m_mutex);

		m_listOperation.push_back(pOperate.get());

		pOperate->ref();

		//osg::OperationThread* operationThread = new osg::OperationThread;

		//m_operationThreads.push_back(operationThread);
	}
}
//
//bool RadarLobe::Intersection(double angleYaw, double anglePitch, QVariant &var)
//{
//	QList<osg::Vec3d> pts;
//
//	QList<osg::Vec3d> pts1 = GetLobePt(m_radius, m_angleH);
//	QList<osg::Vec3d> pts2 = GetLobePt(m_radius, m_angleV);
//	for (int nI = 0; nI < pts1.size(); nI++)
//	{
//		osg::Vec3d pt1 = pts1.at(nI);
//		osg::Vec3d pt2 = pts2.at(nI);
//
//		osg::Vec3d org = osg::Vec3d(pt1.x(), 0.0, 0.0);
//		double a = pt1.z();
//		double b = pt2.z();
//
//		int nSample = 4;
//		double theta = osg::DegreesToRadians(0.0);
//		for (int nJ = 0; nJ < nSample; nJ++)
//		{
//			double x = a * cos(theta);
//			double y = b * sin(theta);
//
//			osg::Vec3d pt;
//			pt.x() = org.x();
//			pt.y() = org.y() + x;
//			pt.z() = org.z() + y;
//
//			pt = GetPointMapping(osg::Vec3d(0.0, 0.0, 0.0), -anglePitch, osg::Y_AXIS, pt);
//			pt = GetPointMapping(osg::Vec3d(0.0, 0.0, 0.0), angleYaw, osg::Z_AXIS, pt);
//
//			pts.push_back(pt);
//
//			theta += osg::DegreesToRadians(360.0 / nSample);
//		}
//	}
//
//	if (pts.size() == 0)
//		return false;
//
//	bool bSuc = false;
//
//	int nNum = m_listOperation.size();
//
//	OpenThreads::BlockCount *pBlockCount = new OpenThreads::BlockCount(nNum);
//	pBlockCount->reset();
//
//	typedef std::list< osg::ref_ptr<osg::OperationThread> > Threads;
//	Threads operationThreads;
//
//	int nNumCount = ceil((double)pts.size() / (double)nNum);
//
//	for (int nI = 0; nI < nNum; ++nI)
//	{
//		osg::OperationThread* operationThread = new osg::OperationThread;
//
//		operationThreads.push_back(operationThread);
//
//		RadarLobeOperate *pOperate = m_listOperation[nI];
//		pOperate->SetBlockCount(pBlockCount);
//
//		QList<osg::Vec3d> listPt;
//		if (nI == nNum-1)
//		{
//			listPt = pts.mid(nI * nNumCount, pts.size() - nI * nNumCount);
//		} 
//		else
//		{
//			listPt = pts.mid(nI * nNumCount, nNumCount);
//		}
//		
//		pOperate->SetComputerPara(listPt, angleYaw, anglePitch);
//
//		operationThread->add(pOperate);
//		operationThread->startThread();
//	}
//
//	pBlockCount->block();
//	
//	if (pBlockCount)
//	{
//		delete pBlockCount;
//		pBlockCount = NULL;
//	}
//
//	bool bIntersectTerrain = true;
//
//	QVariantList varList;
//	for (int nI = 0; nI < m_listOperation.size(); nI++)
//	{
//		RadarLobeOperate *pOperation = m_listOperation[nI];
//
//		QVariantList vars = pOperation->GetIntersections();
//
//		if (vars.size() > 0)
//		{
//			bSuc = true;
//		}
//
//		varList.append(vars);
//
//		bool bIntersect = pOperation->IsIntersectTerrain();
//		bIntersectTerrain &= !bIntersect;
//
//		if (bIntersect)
//		{
//			g_dDisHill = pOperation->GetIntersectDistance();
//			g_dAngle = pOperation->GetAngle();
//		}
//	}
//
//	if (!bIntersectTerrain)
//	{
//		g_bIntersectTerrain = true;
//
//		UDPComm::Instance()->SendWavePara(g_bIntersectTerrain, g_dDisHill, g_dAngle);
//	} 
//	else
//	{
//		g_bIntersectTerrain = false;
//
//		g_dDisHill = 0.0;
//		g_dAngle = 0.0;
//	}
//
//	var = QVariant(varList);
//
//	if (!operationThreads.empty())
//	{
//		for (Threads::iterator itr = operationThreads.begin();
//			itr != operationThreads.end();
//			++itr)
//		{
//			(*itr)->cancel();
//		}
//	}
//
//	return bSuc;
//}
//
//void RadarLobe::CreateIntersectTerrain()
//{
//	m_pIntersectTerrain = new osg::Geode;
//
//	double minX = m_center.x() - m_radius;
//	double maxX = m_center.x() + m_radius;
//	double minY = m_center.y() - m_radius;
//	double maxY = m_center.y() + m_radius;
//
//	//m_pTerrain->asGroup()->addChild(m_pIntersectTerrain);
//
//	int numThreads = 4;
//
//	double deltaY = (maxY - minY) / 4;
//
//	OpenThreads::BlockCount *pBlockCount = new OpenThreads::BlockCount(numThreads);
//	pBlockCount->reset();
//
//	std::vector<IntersectionOperate *> listOperation;
//
//	typedef std::list< osg::ref_ptr<osg::OperationThread> > Threads;
//	Threads operationThreads;
//
//	for (int i = 0; i < numThreads; ++i)
//	{
//		double minYCurr = minY + deltaY * i;
//		double maxYCurr = minY + deltaY * (i + 1);
//
//		osg::OperationThread* operationThread = new osg::OperationThread;
//
//		operationThreads.push_back(operationThread);
//
//		osg::ref_ptr<IntersectionOperate> pOperate = new IntersectionOperate(minX, maxX, minYCurr, maxYCurr, pBlockCount, m_mutex);
//		pOperate->setTerrain(m_pTerrain);
//
//		pOperate->ref();
//
//		listOperation.push_back(pOperate.get());
//
//		operationThread->add(pOperate);
//		operationThread->startThread();
//	}
//
//	pBlockCount->block();
//
//	if (pBlockCount)
//	{
//		delete pBlockCount;
//		pBlockCount = NULL;
//	}
//
//	QVariantList intersections;
//	for (int nI = 0; nI < listOperation.size(); nI++)
//	{
//		IntersectionOperate *pOperation = listOperation[nI];
//
//		QVariantList varList = pOperation->GetIntersections();
//
//		intersections.append(varList);
//
//		pOperation->unref();
//	}
//
//	for (int nI = 0; nI < intersections.size() - 1; nI++)
//	{
//		QVariantList ptList1 = intersections.at(nI).toList();
//		QVariantList ptList2 = intersections.at(nI + 1).toList();
//
//		osg::Vec3Array *coords = new osg::Vec3Array;
//
//		for (int nJ = 0; nJ < ptList1.size(); nJ++)
//		{
//			QString str1 = ptList1.at(nJ).toString();
//			QStringList strList1 = str1.split(',');
//
//			osg::Vec3d pt1 = osg::Vec3d(strList1.at(0).toDouble(), strList1.at(1).toDouble(), strList1.at(2).toDouble());
//			coords->push_back(pt1);
//
//			if (ptList2.size() > nJ)
//			{
//				QString str2 = ptList2.at(nJ).toString();
//				QStringList strList2 = str2.split(',');
//
//				osg::Vec3d pt2 = osg::Vec3d(strList2.at(0).toDouble(), strList2.at(1).toDouble(), strList2.at(2).toDouble());
//
//				coords->push_back(pt2);
//			}
//		}
//
//		osg::Geometry *pGeometry = CreateGeometry(osg::Vec4(1.0, 0.0, 0.0, 1.0), coords, osg::PrimitiveSet::QUAD_STRIP);
//		m_pIntersectTerrain->addDrawable(pGeometry);
//	}
//
//	if (!operationThreads.empty())
//	{
//		for (Threads::iterator itr = operationThreads.begin();
//			itr != operationThreads.end();
//			++itr)
//		{
//			(*itr)->cancel();
//		}
//	}
//}

void RadarLobe::setCurrRadarAngle(double angleYaw, double anglePitch, bool bInit)
{
	if (!m_pGeodeRay)
		return;

	RadarLobeCallback *pRadarLobeCallback = dynamic_cast<RadarLobeCallback *>(m_pGeodeRay->getUpdateCallback());
	if (pRadarLobeCallback)
	{
		if (bInit)
		{
			pRadarLobeCallback->SetAngleInit(angleYaw, anglePitch);
		} 
		else
		{
			//pRadarLobeCallback->SetAngle(angleYaw, anglePitch);
		}
	}
}

void RadarLobe::UpdateArea(double pitch)
{
	QList<osg::Vec3d> pts = GetLobePt(m_radius, m_angleV);

	QList<osg::Vec3d> pts1;
	for (int nI = pts.size() - 1; nI >= 0; nI--)
	{
		osg::Vec3d pt = pts.at(nI);
		osg::Vec3d org = osg::Vec3d(pt.x(), 0.0, 0.0);
		osg::Vec3d axis = osg::X_AXIS;
		osg::Vec3 result = GetPointMapping(org, 180.0, axis, pt);

		pts1.push_back(result);
	}
	pts.append(pts1);

	pts1.clear();
	for (int nI = 0; nI < pts.size(); nI++)
	{
		osg::Vec3d pt = pts.at(nI);
		osg::Vec3d org = osg::Vec3d(0.0, 0.0, 0.0);
		osg::Vec3d axis = osg::Y_AXIS;
		osg::Vec3 result = GetPointMapping(org, pitch, axis, pt);

		pts1.push_back(result);
	}

	if (geode_.valid()) {
		geode_->removeDrawables(0, geode_->getNumDrawables());
	} else {
		geode_ = new osg::Geode;
		geode_->setName("Area");

        geode_->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF | osg::StateAttribute::PROTECTED);
        geode_->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
        geode_->getOrCreateStateSet()->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);

        osg::ref_ptr<osg::BlendFunc>blendFunc = new osg::BlendFunc();
        blendFunc->setSource(osg::BlendFunc::SRC_ALPHA);
        blendFunc->setDestination(osg::BlendFunc::ONE_MINUS_CONSTANT_ALPHA);

		geode_->getOrCreateStateSet()->setAttributeAndModes(blendFunc);

        osg::ref_ptr<osg::LineWidth> pLW = new osg::LineWidth;
        pLW->setWidth(m_lineWidth);
		geode_->getOrCreateStateSet()->setAttribute(pLW);

        //pGeode->getOrCreateStateSet()->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF);//È¡ÏûÉî¶È²âÊÔ

        osg::Depth* depth = new osg::Depth;
        depth->setWriteMask(false);
		geode_->getOrCreateStateSet()->setAttributeAndModes(depth, osg::StateAttribute::ON);

        addChild(geode_);
	}

    /*QVariant var = ComputerIntersect(pts1);
    QVariantList varList = var.toList();

    for (int nI = 0; nI < varList.size() - 1; nI++) {
        QVariantList ptList1 = varList.at(nI).toList();
        QVariantList ptList2 = varList.at(nI + 1).toList();

        osg::Vec3Array* coords = new osg::Vec3Array;

        for (int nJ = 0; nJ < ptList1.size(); nJ++) {
            QString str1 = ptList1.at(nJ).toString();
            QStringList strList1 = str1.split(',');

            osg::Vec3d pt1 = osg::Vec3d(strList1.at(0).toDouble(), strList1.at(1).toDouble(), strList1.at(2).toDouble());

            QString str2 = ptList2.at(nJ).toString();
            QStringList strList2 = str2.split(',');

            osg::Vec3d pt2 = osg::Vec3d(strList2.at(0).toDouble(), strList2.at(1).toDouble(), strList2.at(2).toDouble());

            coords->push_back(pt1);
            coords->push_back(pt2);
        }

        osg::Geometry* pGeometry = CreateGeometry(m_surfaceColorArea, coords, osg::PrimitiveSet::QUAD_STRIP);
        pGeode->addDrawable(pGeometry);
    }

	for (int nI = 0; nI < varList.size(); nI++)
	{
		QVariantList ptList = varList.at(nI).toList();

		osg::Vec3Array *coords = new osg::Vec3Array;

		for (int nJ = 0; nJ < ptList.size(); nJ++)
		{
			QString str = ptList.at(nJ).toString();
			QStringList strList = str.split(',');

			osg::Vec3d pt = osg::Vec3d(strList.at(0).toDouble(), strList.at(1).toDouble(), strList.at(2).toDouble());

			coords->push_back(pt);
		}

		osg::Geometry *pGeometry = CreateGeometry(m_gridColorArea, coords, osg::PrimitiveSet::LINE_LOOP);
		pGeode->addDrawable(pGeometry);
	}

	for (int nI = 0; nI < varList.size(); nI++)
	{
		QVariantList ptList = varList.at(nI).toList();

		for (int nJ = 0; nJ < ptList.size(); nJ++)
		{
			QString str = ptList.at(nJ).toString();
			QStringList strList = str.split(',');

			osg::Vec3d pt = osg::Vec3d(strList.at(0).toDouble(), strList.at(1).toDouble(), strList.at(2).toDouble());

			QString strName = "V-Grid|" + QString::number(nJ);

			if (nI == 0)
			{
				osg::Vec3Array *coords = new osg::Vec3Array;
				coords->push_back(pt);

				osg::Geometry *pGeometry = CreateGeometry(m_gridColorArea, coords, osg::PrimitiveSet::LINE_LOOP);
				pGeometry->setName(std::string(strName.toLocal8Bit()));
				pGeode->addDrawable(pGeometry);
			}
			else
			{
				osg::Vec3Array *coords = NULL;
				osg::Geometry *pGeometry = GetGridCoords(strName, pGeode, coords);
				if (pGeometry)
				{
					coords->push_back(pt);

					pGeometry->removePrimitiveSet(0);

					pGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINE_LOOP, 0, coords->size()));
				}
			}
		}
	}*/
}

void RadarLobe::setCurrTarget(osg::Vec3d pos)
{
	if (!m_pGeodeRay)
		return;

	RadarLobeCallback *pRadarLobeCallback = dynamic_cast<RadarLobeCallback *>(m_pGeodeRay->getUpdateCallback());
	if (pRadarLobeCallback)
	{
		double dis = (m_center - pos).length();
		if (dis < m_radius)
		{
			double heading = 0.0;
			double pitch = 0.0;
			//getHeadingPitch(m_center, pos, heading, pitch);

			pRadarLobeCallback->SetAngle(heading + 90.0, pitch);

			pRadarLobeCallback->SetFollowState(true);
		}
		else
		{
			pRadarLobeCallback->SetFollowState(false);
		}
	}
}

void RadarLobe::AddIntersectObject(osg::Node *pObject)
{
	int nNum = m_listOperation.size();

	for (int nI = 0; nI < nNum; ++nI)
	{
		RadarLobeOperate *pOperate = m_listOperation[nI];
		
		pOperate->AddIntersectObject(pObject);
	}
}

osg::MatrixTransform * RadarLobe::CreateIntersectObject()
{
	osg::MatrixTransform *mt = new osg::MatrixTransform;

	osg::Matrixd localToWorld, worldToLocal;
	osg::NodePathList nodePaths = getParentalNodePaths();
	if (!nodePaths.empty())
	{
		osg::NodePath nodePath = nodePaths[0];
		localToWorld = osg::computeLocalToWorld(nodePath);
		worldToLocal = osg::computeWorldToLocal(nodePath);
	}

	mt->setMatrix(localToWorld);
	mt->addChild(m_pGeodeRay);

	return mt;
}

void RadarLobe::setStartCallback(bool bStart)
{
	if (!m_pGeodeRay)
		return;

	RadarLobeCallback *pRadarLobeCallback = dynamic_cast<RadarLobeCallback *>(m_pGeodeRay->getUpdateCallback());
	if (pRadarLobeCallback)
	{
		pRadarLobeCallback->SetStart(bStart);
	}
}

void RadarLobe::SetCallbackTimer(double t)
{
	if (!m_pGeodeRay)
		return;

	RadarLobeCallback *pRadarLobeCallback = dynamic_cast<RadarLobeCallback *>(m_pGeodeRay->getUpdateCallback());
	if (pRadarLobeCallback)
	{
		pRadarLobeCallback->SetCurrTimer(t);
	}
}

void RadarLobe::setCurrRadarLoopTime(double loopTime)
{
	if (!m_pGeodeRay)
	{
		setRay(m_yaw, -m_pitch, m_surfaceColor, loopTime);
	}

	RadarLobeCallback *pRadarLobeCallback = dynamic_cast<RadarLobeCallback *>(m_pGeodeRay->getUpdateCallback());
	if (pRadarLobeCallback)
	{
		pRadarLobeCallback->SetLoopTime(loopTime);
	}
}