// Determines if 3D segment and triangle have a unique intersection.  
// If true and rpoint is not NULL, returns intersection point.
bool Intersects(const Segment3D& seg, const Triangle3D& tri, Point3D *rpoint)
{
  Vector3D n = ( tri[1] - tri[0] ) ^ ( tri[2] - tri[1] );

  float a = n[0], b = n[1], c = n[2];
  float d = -( a * tri[0][0] + b * tri[0][1] + c * tri[0][2] );

  if ( !Intersects( seg, Plane3D( a, b, c, d ), rpoint ) )
    return false;

  return tri.contains( *rpoint );
}

void CUmbralMap::GetMeshes(Palleon::MeshArray& meshes, const Palleon::CCamera* camera)
{
	auto cameraFrustum = camera->GetFrustum();
	for(const auto& instance : m_instances)
	{
		auto boundingSphere = instance->GetWorldBoundingSphere();
		if(cameraFrustum.Intersects(boundingSphere))
		{
			meshes.push_back(instance.get());
		}
	}
}

void Button::OnMouseHover(SDL_MouseMotionEvent e)
{
	if (!Active) return;
	if (!Enabled) return;

	if (Intersects({ e.x, e.y, 1, 1 }))
	{
		isHovering = true;
		GuiElement::OnMouseHover(e);
	}
	else isHovering = false;
}

  bool AxisAlignedBoundingBox3D::Intersection(const AxisAlignedBoundingBox3D& rhs, AxisAlignedBoundingBox3D& intersection, f32 epsilon) const
  {
    if(!Intersects(rhs, epsilon))
      return false;

    intersection.max.x = (max.x <= rhs.max.x ? max.x : rhs.max.x);
    intersection.max.y = (max.y <= rhs.max.y ? max.y : rhs.max.y);
    intersection.max.z = (max.z <= rhs.max.z ? max.z : rhs.max.z);
    intersection.min.x = (min.x <= rhs.min.x ? rhs.min.x : min.x);
    intersection.min.y = (min.y <= rhs.min.y ? rhs.min.y : min.y);
    intersection.min.z = (min.z <= rhs.min.z ? rhs.min.z : min.z);
    return true;
  }

bool Line::Intersects(const Rectangle& rectangle) const {

    if(this->IsVertical()) {
        double Xr = rectangle.GetX();
        double Xrw = rectangle.GetWidth();
        double Xl = _extent_one.GetX();
        if(Xr > Xl) return false;
        double Xrb = Xr + Xrw;
        if(Xrb < Xl) return false;
        return true;
    } else if(this->IsHorizontal()) {
        double Yr = rectangle.GetY();
        double Yrh = rectangle.GetHeight();
        double Yl = _extent_one.GetY();
        if(Yr > Yl) return false;
        double Yrb = Yr + Yrh;
        if(Yrb < Yl) return false;
        return true;
    }

    double eOne_x = _extent_one.GetX();
    double eOne_y = _extent_one.GetY();
    double eTwo_x = _extent_two.GetX();
    double eTwo_y = _extent_two.GetY();
    double rTop = rectangle.GetY();
    double rLeft = rectangle.GetX();
    double rRight = rLeft + rectangle.GetWidth();
    double rBottom = rTop + rectangle.GetHeight();

    bool point_one_result = eOne_x > rLeft && eOne_x < rRight && eOne_y > rTop && eOne_y < rBottom;
    bool point_two_result = eTwo_x > rLeft && eTwo_x < rRight && eTwo_y > rTop && eTwo_y < rBottom;
    if(point_one_result || point_two_result) return true;

    bool result = Intersects(rectangle.GetTop()) ||
                  Intersects(rectangle.GetLeft()) ||
                  Intersects(rectangle.GetRight()) ||
                  Intersects(rectangle.GetBottom());
    return result;
}

void Systems::CollisionSystem::UpdateEntity(double dt, EntityID entity, EntityID parent)
{
	/*if (parent != 0)
	{
		auto transform = m_World->GetComponent<Components::Transform>(entity, "Transform");
		auto parentTransform = m_World->GetComponent<Components::Transform>(parent, "Transform");

		transform->Position[0] += parentTransform->Position[0];
		transform->Position[1] += parentTransform->Position[1];
		transform->Position[2] += parentTransform->Position[2];
	}*/

	auto collisionComponent = m_World->GetComponent<Components::Collision>(entity, "Collision");
	if (!collisionComponent)
		return;

	// Clear old collisions
	collisionComponent->CollidingEntities.clear();

	// Quit out if we're not interested in collision events
	if (!collisionComponent->Interested)
		return;

	auto entities = m_World->GetEntities();

	for (auto pair : *entities) {
		EntityID entity2 = pair.first;
		EntityID parent2 = pair.second;

		// Check if entity2 is a child of entity
		/*auto currentParent = parent;
		bool childOfEntity2 = false;
		while (currentParent != 0) {
			if (currentParent == entity2) {
				childOfEntity2 = true;
				break;
			}
			currentParent = entities[currentParent];
		}
		if (childOfEntity2)
			continue;*/

		// Check if we're the parent to entity2
	
		//pair.first, pair.second;
		if(entity == entity2)
			continue;

		Intersects(entity, entity2);
	}
}

Triangle* OctreeInternal::Query(const Ray& ray, float& t) const
{
	float tBox = std::numeric_limits<float>::min();
	if (!Intersects(ray, bb, tBox) || tBox > t)
		return nullptr;

	Triangle* triangle = nullptr;
	for (int i = 0; i < 8; ++i)
	{
		Triangle* tri = children[i]->Query(ray, t);
		triangle = tri == nullptr ? triangle : tri;
	}
	return triangle;
}

/** The algorithm for Polyhedron-Polyhedron intersection is from Christer Ericson's Real-Time Collision Detection, p. 384.
	As noted by the author, the algorithm is very naive (and here unoptimized), and better methods exist. [groupSyntax] */
bool Polyhedron::Intersects(const Polyhedron &polyhedron) const
{
	if (polyhedron.Contains(this->Centroid()))
		return true;
	if (this->Contains(polyhedron.Centroid()))
		return true;

	// This test assumes that both this and the other polyhedron are closed.
	// This means that for each edge running through vertices i and j, there's a face
	// that contains the line segment (i,j) and another neighboring face that contains
	// the line segment (j,i). These represent the same line segment (but in opposite direction)
	// so we only have to test one of them for intersection. Take i < j as the canonical choice
	// and skip the other winding order.

	// Test for each edge of this polyhedron whether the other polyhedron intersects it.
	for(size_t i = 0; i < f.size(); ++i)
	{
		assert(!f[i].v.empty()); // Cannot have degenerate faces here, and for performance reasons, don't start checking for this condition in release mode!
		int v0 = f[i].v.back();
		float3 l0 = v[v0];
		for(size_t j = 0; j < f[i].v.size(); ++j)
		{
			int v1 = f[i].v[j];
			float3 l1 = v[v1];
			if (v0 < v1 && polyhedron.Intersects(LineSegment(l0, l1))) // If v0 < v1, then this line segment is the canonical one.
				return true;
			l0 = l1;
			v0 = v1;
		}
	}

	// Test for each edge of the other polyhedron whether this polyhedron intersects it.
	for(size_t i = 0; i < polyhedron.f.size(); ++i)
	{
		assert(!polyhedron.f[i].v.empty()); // Cannot have degenerate faces here, and for performance reasons, don't start checking for this condition in release mode!
		int v0 = polyhedron.f[i].v.back();
		float3 l0 = polyhedron.v[v0];
		for(size_t j = 0; j < polyhedron.f[i].v.size(); ++j)
		{
			int v1 = polyhedron.f[i].v[j];
			float3 l1 = polyhedron.v[v1];
			if (v0 < v1 && Intersects(LineSegment(l0, l1))) // If v0 < v1, then this line segment is the canonical one.
				return true;
			l0 = l1;
			v0 = v1;
		}
	}

	return false;
}

void CGEPropDynamic::Materialize(void)
{
	//A more lightweight and optional func_rebreakable materalize function 
	CreateVPhysics();

	if (m_bRobustSpawn)
	{
		// iterate on all entities in the vicinity.
		CBaseEntity *pEntity;
		Vector max = CollisionProp()->OBBMaxs();
		for (CEntitySphereQuery sphere(GetAbsOrigin(), max.NormalizeInPlace()); (pEntity = sphere.GetCurrentEntity()) != NULL; sphere.NextEntity())
		{
			if (pEntity == this || pEntity->GetSolid() == SOLID_NONE || pEntity->GetSolid() == SOLID_BSP || pEntity->GetSolidFlags() & FSOLID_NOT_SOLID || pEntity->GetMoveType() & MOVETYPE_NONE)
				continue;

			// Ignore props that can't move
			if (pEntity->VPhysicsGetObject() && !pEntity->VPhysicsGetObject()->IsMoveable())
				continue;

			// Prevent respawn if we are blocked by anything and try again in 1 second
			if (Intersects(pEntity))
			{
				SetSolid(SOLID_NONE);
				AddSolidFlags(FSOLID_NOT_SOLID);
				VPhysicsDestroyObject();
				SetNextThink(gpGlobals->curtime + 1.0f);
				return;
			}
		}
	}

	m_iHealth = m_iHealthOverride;

	if (m_iHealth > 0)
		m_takedamage = DAMAGE_YES;

	RemoveEffects(EF_NODRAW);
	RemoveSolidFlags(FSOLID_NOT_SOLID);

	m_bUsingBrokenSkin = false;

	if (m_bUseRandomSkins)
		PickNewSkin();
	else
		m_nSkin = m_iStartingSkin;

	SetRenderColor(m_col32basecolor.r, m_col32basecolor.g, m_col32basecolor.b);

	m_Respawn.FireOutput(this, this);
}

bool PoolCue::SetContactPos(float x, float y)
{
  float oldx = m_x;
  float oldy = m_y;
  m_x = x;
  m_y = y;
  if (Intersects())
  {
    m_x = oldx;
    m_y = oldy;
    return false;
  }
  return true;
}

std::vector<float3> Circle::IntersectsFaces(const OBB &obb) const
{
	std::vector<float3> intersectionPoints;
	for(int i = 0; i < 6; ++i)
	{		
		Plane p = obb.FacePlane(i);
		float3 pt1, pt2;
		int numIntersections = Intersects(p, &pt1, &pt2);
		if (numIntersections >= 1 && obb.Contains(pt1))
			intersectionPoints.push_back(pt1);
		if (numIntersections >= 2 && obb.Contains(pt2))
			intersectionPoints.push_back(pt2);
	}
	return intersectionPoints;
}

bool Polygon::Intersects(const Circle& other) const
{
    const Polygon& B = *this;

    if (Intersects(other.Position()))
        return true; // If the center of the circle is inside the polygon

    for (int i = 0; i < B.NumPoints(); ++i)
    {
        if (other.Intersects(Segment(B[i], B[i + 1])))
            return true;
    }

    return false;
}

AirspaceInterceptSolution
AbstractAirspace::Intercept(const AircraftState &state,
                            const GeoPoint &end,
                            const FlatProjection &projection,
                            const AirspaceAircraftPerformance &perf) const
{
  AirspaceInterceptSolution solution = AirspaceInterceptSolution::Invalid();
  for (const auto &i : Intersects(state.location, end, projection)) {
    auto new_solution = Intercept(state, perf, i.first, i.second);
    if (new_solution.IsEarlierThan(solution))
      solution = new_solution;
  }

  return solution;
}

bool sreFrustum::ObjectIntersectsNearClipVolume(const sreObject& so) const {
    if (light_position_type & SRE_LIGHT_POSITION_IN_NEAR_PLANE) {
        // First check the only plane defined.
        if (Dot(near_clip_volume.plane[0], so.sphere.center) <= - so.sphere.radius)
            return false;
        // The object is outside the near clip volume if the object does not intersect the near plane.
        // Calculate distance from center of the bounding sphere to the near plane in world space.
        if (fabsf(Dot(frustum_world.plane[0], so.sphere.center)) < so.sphere.radius)
            return true; // Intersect.
        return false;
    }
    // When the light position is in front of or behind the near plane, check the five planes of the near-clip
    // volume, six plane for point lights.
    return Intersects(so, near_clip_volume);
}

bool
NormalizedConstraintSet::StringRange::Merge(const StringRange& aOther)
{
  if (!Intersects(aOther)) {
    return false;
  }
  Intersect(aOther);

  ValueType unioned;
  set_union(mIdeal.begin(), mIdeal.end(),
            aOther.mIdeal.begin(), aOther.mIdeal.end(),
            std::inserter(unioned, unioned.begin()));
  mIdeal = unioned;
  return true;
}

void MainScreen::UpdatePhysics(float dt)
{
    (void) dt;

    auto& scene = mScene;
    SceneNode* teapot = mScene->FindNodeByUuid("teapot_1");
    for(auto& p : scene->GetNodes())
    {
        SceneNode* cur = p.second.get();
        if(cur->GetUUID() == teapot->GetUUID())
            continue;

        if(Intersects(teapot->GetAABB(), cur->GetAABB()))
            scene->Move(teapot, CalcCollisionResponce(teapot->GetAABB(), cur->GetAABB()));
    }
}

float3 Triangle::ClosestPoint(const Line &line, float3 *otherPt) const
{
	///\todo Optimize this function.
	float3 intersectionPoint;
	if (Intersects(line, 0, &intersectionPoint))
	{
		if (otherPt)
			*otherPt = intersectionPoint;
		return intersectionPoint;
	}

	float u1,v1,d1;
	float3 pt1 = ClosestPointToTriangleEdge(line, &u1, &v1, &d1);
	if (otherPt)
		*otherPt = line.GetPoint(d1);
	return pt1;
}

Point Polygon::ClosestPoint(const Point& point) const
{
    if (Intersects(point))
        return point;

    Point closestSoFar = GetPoint(0);

    for (int i = 0; i < NumPoints(); ++i)
    {
        Segment seg(GetPoint(i), GetPoint(i + 1));
        Point closestNow = seg.ClosestPoint(point);
        if (point.IsCloserToFirstThanSecond(closestNow, closestSoFar))
            closestSoFar = closestNow;
    }

    return closestSoFar;
}

void TControl::CheckTargetSelection(QPoint position) {
    int ax1 = position.x();
    int ax2 = position.x();
    int ay1 = position.y();
    int ay2 = position.y();
    for (size_t i = 0; i < World->planets_size(); ++i) {
        float r = World->planets(i).radius() * World->Scale;
        int bx1 = World->planets(i).x() * World->Scale + World->OffsetX - r;
        int bx2 = World->planets(i).x() * World->Scale + World->OffsetX + r;
        int by1 = World->planets(i).y() * World->Scale + World->OffsetY - r;
        int by2 = World->planets(i).y() * World->Scale + World->OffsetY + r;
        if (Intersects(ax1, ax2, ay1, ay2, bx1, bx2, by1, by2)) {
            World->SelectedTarget = i;
            return;
        }
    }
    World->SelectedTarget.reset();
}

ZRect ZRect::Intersection(const ZRect &rect) const
{
    float tempX=0,tempY=0,tempW=0,tempH=0;

    //can only grab the intersection if they intersect
    if(Intersects(rect))
    {
        tempX = rX > rect.X() ? rX : rect.X();
        tempY = rY > rect.Y() ? rY : rect.Y();
        tempW = rX+rWidth < rect.Right() ? rX+rWidth : rect.Right();
        tempH = rY+rHeight < rect.Bottom() ? rY+rHeight : rect.Bottom();

        tempW -= tempX;        //adjust width and height
        tempH -= tempY;
    }

    return ZRect(tempX,tempY,tempW,tempH);
}