#include "rayCaster.h"
#include "mesh.h"
#include "node.h"

using namespace std;

namespace vb01{
	vector<RayCaster::CollisionResult> RayCaster::cast(Vector3 rayPos, Vector3 rayDir, Node *node, const float rayLength, const float distToRay){
		return cast(rayPos, rayDir, vector<Node*>{node}, rayLength, distToRay);
	}

	vector<RayCaster::CollisionResult> RayCaster::cast(Vector3 rayPos, Vector3 rayDir, vector<Node*> nodes, const float rayLength, const float distToRay){
		vector<CollisionResult> results;

		for(Node *node : nodes){
			vector<Node*> descendants = vector<Node*>{node};
			node->getDescendants(descendants);

			for(Node *desc : descendants){
				vector<CollisionResult> res = retrieveCollisions(rayPos, rayDir, desc, rayLength, distToRay);
				results.insert(results.end(), res.begin(), res.end());
			}
		}

		if(!results.empty()) sortResults(results);

		return results;
	}

	vector<RayCaster::CollisionResult> RayCaster::retrieveCollisions(Vector3 rayPos, Vector3 rayDir, Node *node, float rayLength, const float distToRay){
		Vector3 pos = node->localToGlobalPosition(Vector3::VEC_ZERO);
		Quaternion rot = node->localToGlobalOrientation(Quaternion::QUAT_W);
		vector<CollisionResult> results;

		for(Mesh *m : node->getMeshes()){
			const int numVerts = m->getMeshBase().numTris * 3;
			MeshData::Vertex *vertices = m->getMeshBase().vertices;
			u32 *indices = m->getMeshBase().indices;

			for(int i = 0; i < numVerts / 3; i++){
				if(distToRay > 0.0){
					bool skip = false;

					for(int j = 0; j < 3; j++){
						Vector3 rayPosToVert = (*(vertices[indices[i * 3 + j]].pos) - rayPos);
						float angle = rayDir.getAngleBetween(rayPosToVert.norm());

						if(angle > PI / 2) angle = PI - angle;

						float dist = sin(angle) * rayPosToVert.getLength();

						if(dist > distToRay){
							skip = true;
							break;
						}
					}

					if(skip) continue;
				}

				Vector3 pointA = pos + rot * *(vertices[indices[i * 3]].pos);
				Vector3 pointB = pos + rot * *(vertices[indices[i * 3 + 1]].pos);
				Vector3 pointC = pos + rot * *(vertices[indices[i * 3 + 2]].pos);
				Vector3 hypVec = pointA - rayPos;
				Vector3 perpVec = (pointB - pointA).cross(pointC - pointA).norm();
				float a1 = hypVec.norm().getAngleBetween(perpVec);

				if(a1 > PI / 2){
					a1 = PI - a1;
					perpVec = -perpVec;
				}	 

				float perpLine = hypVec.getLength() * cos(a1);
		   		float a2 = perpVec.getAngleBetween(rayDir.norm());

				if(a2 <= PI / 2){
					float distance = perpLine / cos(a2);

					if((distance <= rayLength && rayLength != .0) || rayLength == .0){
						Vector3 contactPoint = rayPos + rayDir.norm() * distance;
						Vector3 vAB = (pointB - pointA);
						Vector3 vBC = (pointC - pointB);
						Vector3 vAC = (pointC - pointA);

						float angleA = vAB.norm().getAngleBetween(vAC.norm());
						float angleB = (-vAB.norm()).getAngleBetween(vBC.norm());
						float angleC = (-vBC.norm()).getAngleBetween(-vAC.norm());
						Vector3 bisecAVec = (vAB * vAC.getLength() + vAC * vAB.getLength()).norm();
						Vector3 bisecBVec = (vBC * vAB.getLength() - vAB * vBC.getLength()).norm();
						Vector3 bisecCVec = (-vAC * vBC.getLength() - vBC * vAC.getLength()).norm();
						float eps = .001;
						bool withinBisecA = ((contactPoint - pointA).norm().getAngleBetween(bisecAVec.norm()) - angleA / 2) <= eps;
						bool withinBisecB = ((contactPoint - pointB).norm().getAngleBetween(bisecBVec.norm()) - angleB / 2) <= eps;
						bool withinBisecC = ((contactPoint - pointC).norm().getAngleBetween(bisecCVec.norm()) - angleC / 2) <= eps;

						if((withinBisecA && withinBisecB) || (withinBisecB && withinBisecC) || (withinBisecA && withinBisecC)){
							CollisionResult result;
							result.pos = contactPoint;
							result.norm = -perpVec;
							result.distance = distance;
							result.mesh = m;
							results.push_back(result);
						}
					}
				}
			}
		}

		return results;
	}
        
	void RayCaster::sortResults(std::vector<CollisionResult> &results){
		for(int i = 0; i < results.size(); i++){
			for(int j = i; j < results.size(); j++)
				if(results[i].distance > results[j].distance)
					swap(results[i], results[j]);
		}
	}
}