// This source code is property of the Computer Graphics and Visualization
// chair of the TU Dresden. Do not distribute!
// Copyright (C) CGV TU Dresden - All Rights Reserved

#include "LineSegment.h"
#include "GridUtils.h"

//default constructor
LineSegment::LineSegment()
{
}

//constructs a line segment by the two end points v0, v1 without using the edge handle
LineSegment::LineSegment(const Eigen::Vector3f &v0, const Eigen::Vector3f &v1) : v0(v0), v1(v1)
{
}

//construct a line segment from the edge e of the halfedge mesh m
LineSegment::LineSegment(const HEMesh &m, const OpenMesh::EdgeHandle &e) : h(e)
{
	auto h = m.halfedge_handle(e, 0);
	v0 = ToEigenVector(m.point(m.from_vertex_handle(h)));
	v1 = ToEigenVector(m.point(m.to_vertex_handle(h)));
}

//returns an axis aligned bounding box of the line segment
Box LineSegment::ComputeBounds() const
{
	Box b;
	b.Insert(v0);
	b.Insert(v1);
	return b;
}

//returns true if the line segment overlaps the given box b
bool LineSegment::Overlaps(const Box &b) const
{

	Box aabb = ComputeBounds();

	if (!b.Overlaps(aabb))
		return false;

	Eigen::Vector3f o = b.Center();
	Eigen::Vector3f u1 = v0 - o;
	Eigen::Vector3f u2 = v1 - o;

	Eigen::Vector3f d1 = v1 - v0;
	d1.normalize();

	float r = b.Radius(d1);
	float lb = u1.dot(d1);
	float ub = u2.dot(d1);
	if (lb > ub)
		std::swap(lb, ub);
	if (lb > r || ub < -r)
		return false;

	Eigen::Vector3f e1(1, 0, 0);
	Eigen::Vector3f d2 = d1.cross(e1);
	r = b.Radius(d2);
	lb = u1.dot(d2);
	ub = u2.dot(d2);
	if (lb > ub)
		std::swap(lb, ub);
	if (!OverlapIntervals(-r, r, lb, ub))
		return false;

	Eigen::Vector3f e2(0, 1, 0);
	Eigen::Vector3f d3 = d1.cross(e2);
	r = b.Radius(d3);
	lb = u1.dot(d3);
	ub = u2.dot(d3);
	if (lb > ub)
		std::swap(lb, ub);
	if (!OverlapIntervals(-r, r, lb, ub))
		return false;

	Eigen::Vector3f e3(0, 0, 1);
	Eigen::Vector3f d4 = d1.cross(e3);
	r = b.Radius(d4);
	lb = u1.dot(d4);
	ub = u2.dot(d4);

	if (!OverlapIntervals(-r, r, lb, ub))
		return false;

	return true;
}

//returns the point with smallest distance topoint p which lies on the line segment
Eigen::Vector3f LineSegment::ClosestPoint(const Eigen::Vector3f &p) const
{
	//the two endpoints of the line segment are v0,v1
	/* Task 3.2.1 */
	return Eigen::Vector3f(0, 0, 0);
}

//returns the squared distance between point p and the line segment
float LineSegment::SqrDistance(const Eigen::Vector3f &p) const
{
	Eigen::Vector3f d = p - ClosestPoint(p);
	return d.squaredNorm();
}

//returns the euclidean distance between point p and the line segment
float LineSegment::Distance(const Eigen::Vector3f &p) const
{
	return sqrt(SqrDistance(p));
}

//returns a reference point  which is on the line segment and is used to sort the primitive in the AABB tree construction
Eigen::Vector3f LineSegment::ReferencePoint() const
{
	return 0.5f * (v0 + v1);
}