#include "RadarLobeOperate.h"

#include <osgViewer/View>

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

#include <QDebug>

RadarLobeOperate::RadarLobeOperate(osg::Node *pTerrain, osg::Vec3d center, osg::Matrixd localToWorld, osg::Matrixd worldToLocal, OpenThreads::Mutex &mutex)
	: osg::Operation("RadarLobe Operation", false), _mutex(mutex)
{
	m_pTerrain = pTerrain;
	m_center = center;

	m_localToWorld = localToWorld;
	m_worldToLocal = worldToLocal;

	m_bIntersectTerrain = false;
	m_dDisHill = 0.0;
	m_dAngle = 0.0;
}


RadarLobeOperate::~RadarLobeOperate()
{
}

void RadarLobeOperate::SetBlockCount(OpenThreads::BlockCount *pBlockCount)
{
	_pBlockCount = pBlockCount;
}

void RadarLobeOperate::operator()(osg::Object* callingObject)
{
	osgViewer::View* viewer = dynamic_cast<osgViewer::View *>(callingObject);

	{
		//_intersections = ComputerIntersect(m_pts, m_angleYaw, m_anglePitch);
	}

	if (_pBlockCount)
	{
		_pBlockCount->completed();
	}
}
//
//QVariantList RadarLobeOperate::ComputerIntersect(QList<osg::Vec3d> pts, double angleYaw, double anglePitch)
//{
//	QVariantList varList = IntersectTerrain(pts, angleYaw, anglePitch);
//
//	QVariantList retList = IntersectObject(varList);
//
//	if (retList.size() > 2)
//	{
//		return retList;
//	}
//
//	return varList;
//}

osg::Vec3d RadarLobeOperate::GetPointMapping(osg::Vec3d 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::Vec3d result = matrix.getTrans();

	return result;
}

void RadarLobeOperate::SetComputerPara(QList<osg::Vec3d> pts, double angleYaw, double anglePitch)
{
	m_pts = pts;
	m_angleYaw = angleYaw;
	m_anglePitch = anglePitch;

	m_bIntersectTerrain = false;
	m_dDisHill = 0.0;
	m_dAngle = 0.0;
}

void RadarLobeOperate::AddIntersectObject(osg::Node *pObject)
{
	if (!m_Objects.contains(pObject))
	{
		m_Objects.push_back(pObject);
	}
}
//
//QVariantList RadarLobeOperate::IntersectTerrain(QList<osg::Vec3d> pts, double angleYaw, double anglePitch)
//{
//	QVariantList varList;
//
//	osg::ref_ptr<osgUtil::IntersectorGroup> ivGroup = new osgUtil::IntersectorGroup();
//
//	//int nCount = 0;
//	//const double deltaAng = 90.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), angleYaw, 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), angleYaw, osg::Z_AXIS, org);
//
//		//	osg::Vec3d axis = org;
//		//	axis.normalize();
//
//		//	osg::Vec3 result = GetPointMapping(org, deltaAng * nJ, axis, pt);
//
//		//	osg::Vec3d axisY = osg::Z_AXIS ^ axis;
//		//	if (angleYaw < 90.0 ||
//		//		angleYaw > 270.0)
//		//	{
//		//		result = GetPointMapping(osg::Vec3d(0.0, 0.0, 0.0), -anglePitch, axisY, result);
//		//	}
//		//	else
//		//	{
//		//		result = GetPointMapping(osg::Vec3d(0.0, 0.0, 0.0), anglePitch, axisY, result);
//		//	}
//
//		//	osg::Vec3d end = result * m_localToWorld;
//
//		//	osg::ref_ptr<osgEarth::DPLineSegmentIntersector> dplsi = new osgEarth::DPLineSegmentIntersector(m_center, end);
//		//	ivGroup->addIntersector(dplsi.get());
//
//		//	nCount++;
//		//}
//
//		osg::Vec3d end = pt * m_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());
//	
//		{
//			OpenThreads::ScopedLock<OpenThreads::Mutex> lock(_mutex);
//			m_pTerrain->accept(iv);
//		}
//
//		//int nCurrRow = -1;
//
//		//QVariantList ptList;
//
//		for (int nI = 0; nI < pts.size(); 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();
//
//				if (hit.z() > 100.0)
//				{
//					m_bIntersectTerrain = true;
//					ComputerAngle(hit);
//				}
//
//				hit = hit * m_worldToLocal;
//
//				if (m_bIntersectTerrain)
//				{
//					m_dDisHill = hit.length();
//				}
//
//				QString str =
//					QString::number(hit.x()) + "," +
//					QString::number(hit.y()) + "," +
//					QString::number(hit.z());
//
//				varList.push_back(QVariant(str));
//			}
//			else
//			{
//				end = end * m_worldToLocal;
//
//				QString str =
//					QString::number(end.x()) + "," +
//					QString::number(end.y()) + "," +
//					QString::number(end.z());
//
//				varList.push_back(QVariant(str));
//			}
//		}
//
//		//varList.push_back(QVariant(ptList));
//	}
//
//	return varList;
//}
//
//QVariantList RadarLobeOperate::IntersectObject(QVariantList pts)
//{
//	QVariantList varList;
//
//	osg::ref_ptr<osgUtil::IntersectorGroup> ivGroup = new osgUtil::IntersectorGroup();
//
//	//int nCount = 0;
//	//int nRow = pts.size();
//	//int nCol = 0;
//	for (int nI = 0; nI < pts.size(); nI++)
//	{
//		//QVariantList ptList = pts.at(nI).toList();
//
//		QString str = pts.at(nI).toString();
//		QStringList strList = str.split(',');
//
//		osg::Vec3d pt = osg::Vec3d(strList.at(0).toDouble(), strList.at(1).toDouble(), strList.at(2).toDouble());
//
//		osg::Vec3d end = pt * m_localToWorld;
//
//		osg::ref_ptr<osgEarth::DPLineSegmentIntersector> dplsi = new osgEarth::DPLineSegmentIntersector(m_center, end);
//		ivGroup->addIntersector(dplsi.get());
//
//		/*	nCol = ptList.size();
//			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());
//
//				osg::Vec3d end = pt * m_localToWorld;
//
//				osg::ref_ptr<osgEarth::DPLineSegmentIntersector> dplsi = new osgEarth::DPLineSegmentIntersector(m_center, end);
//				ivGroup->addIntersector(dplsi.get());
//
//				nCount++;
//			}*/
//	}
//
//	QMap<int, bool> mapIntersect;
//	bool bIsHit = false;
//
//	for (int nI=0; nI<m_Objects.size(); nI++)
//	{
//		osg::Node *object = m_Objects.at(nI);
//
//		osgUtil::IntersectionVisitor iv;
//		iv.setIntersector(ivGroup.get());
//
//		object->accept(iv);
//
//		for (int nJ = 0; nJ < pts.size(); nJ++)
//		{
//			osgEarth::DPLineSegmentIntersector* los = dynamic_cast<osgEarth::DPLineSegmentIntersector*>(ivGroup->getIntersectors()[nJ].get());
//			if (!los)
//				continue;
//
//			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)
//			{
//				mapIntersect.insert(nJ, true);
//
//				bIsHit = true;
//			}
//			else
//			{
//				QMap<int, bool>::iterator ite = mapIntersect.find(nJ);
//				if (ite != mapIntersect.end())
//				{
//					bool bHit = ite.value();
//
//					if (!bHit)
//					{
//					}
//				}
//				else
//				{
//					mapIntersect.insert(nJ, false);
//				}
//			}
//		}
//	}
//
//	//int nCurrRow = -1;
//	//QVariantList ptList;
//
//	QMap<int, bool>::iterator ite = mapIntersect.begin();
//	while (ite != mapIntersect.end())
//	{
//		int nKey = ite.key();
//		bool bHit = ite.value();
//
//		osgEarth::DPLineSegmentIntersector* los = dynamic_cast<osgEarth::DPLineSegmentIntersector*>(ivGroup->getIntersectors()[nKey].get());
//		if (!los)
//			continue;
//
//		//int nRowCurr = nKey / nCol;
//
//		//if (nCurrRow != nRowCurr)
//		//{
//		//	nCurrRow = nRowCurr;
//
//		//	if (nCurrRow != 0)
//		//	{
//		//		varList.push_back(QVariant(ptList));
//
//		//		ptList.clear();
//		//	}
//		//}
//
//		osg::Vec3d start = los->getStart();
//		osg::Vec3d end = los->getEnd();
//
//		end = end * m_worldToLocal;
//
//		QString str =
//			QString::number(end.x()) + "," +
//			QString::number(end.y()) + "," +
//			QString::number(end.z()) + "," + 
//			QString::number(bIsHit);
//
//		varList.push_back(QVariant(str));
//
//		++ite;
//	}
//
//	//varList.push_back(QVariant(ptList));
//	
//	return varList;
//}

void RadarLobeOperate::RemoveIntersectObject(osg::Node *pObject)
{

}

void RadarLobeOperate::ComputerAngle(osg::Vec3d hit)
{
	osg::Vec3d dir1 = osg::Vec3d(19000.0, 14000.0, 0.0) - m_center;
	dir1.normalize();

	osg::Vec3d dir2 = hit - m_center;
	dir2.normalize();

	double angle = dir1 * dir2;
	angle = acos(angle);
	angle = osg::RadiansToDegrees(angle);

	m_dAngle = angle;
}