// This file is a part of Framsticks SDK.  http://www.framsticks.com/
// Copyright (C) 2019-2020  Maciej Komosinski and Szymon Ulatowski.
// See LICENSE.txt for details.

#ifndef _PART_DISTANCE_ESTIMATOR_H_
#define _PART_DISTANCE_ESTIMATOR_H_

#include "frams/model/geometry/meshbuilder.h"
#include <cmath>

class PartDistanceEstimator
{
public:

	static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotation)
	{
		Part *tmpPart1 = new Part(shape);
		tmpPart1->scale = scale;
		tmpPart1->setRot(rotation);
		return tmpPart1;
	}

	/// Get some of the points from the surface of the part
	static vector <Pt3D> findSurfacePoints(Part *part, double  relativeDensity)
	{
		// Divide by maximal radius to avoid long computations
		MeshBuilder::PartSurface surface(relativeDensity / part->scale.maxComponentValue());
		surface.initialize(part);

		vector <Pt3D> points;
		Pt3D point;
		while (surface.tryGetNext(point))
		{
			points.push_back(point);
		}
		return points;
	}

	/// Check if there is a collision between the parts
	static bool isCollision(Part *part, vector <Pt3D> &points, Pt3D &vectorBetweenParts)
	{
		static double CBRT_3 = std::cbrt(3);
		double maxPartReachSq = pow(CBRT_3 * part->scale.maxComponentValue(), 2);
		for (int i = 0; i < int(points.size()); i++)
		{
			Pt3D shifted = points[i] + vectorBetweenParts;
			double distanceToPointSq = shifted.x * shifted.x + shifted.y * shifted.y + shifted.z * shifted.z;
			if (distanceToPointSq <= maxPartReachSq && GeometryUtils::isPointInsidePart(shifted, part))
				return true;
		}
		return false;
	}


	static double calculateDistance(Part tmpPart1, Part tmpPart2, double distanceTolerance, double relativeDensity)
	{
		/// tmpPart1 and tmpPart2 are copied for purpose and should not be passed as reference.
		/// This function can change some of the properties of those parts.
		/// tmpPart1 will be approximated by surface points.
		/// The collision between the parts is detected when any of these points is inside tmpPart2.
		/// If tmpPart1 and tmpPart2 are swapped, the calculated distance may slightly differ.
		Pt3D directionVersor = tmpPart1.p - tmpPart2.p;
		directionVersor.normalize();

		tmpPart1.p = Pt3D_0;
		tmpPart2.p = Pt3D_0;

		static double CBRT_3 = std::cbrt(3);
		vector <Pt3D> points = PartDistanceEstimator::findSurfacePoints(&tmpPart1, relativeDensity);

		double minDistance = tmpPart2.scale.minComponentValue() + tmpPart1.scale.minComponentValue();
		double maxDistance = CBRT_3 * (tmpPart2.scale.maxComponentValue() + tmpPart1.scale.maxComponentValue());
		double currentDistance = 0.5 * (maxDistance + minDistance);
		int collisionDetected = false;
		while (maxDistance - minDistance > distanceTolerance)
		{
			Pt3D vectorBetweenParts = directionVersor * currentDistance;
			collisionDetected = PartDistanceEstimator::isCollision(&tmpPart2, points, vectorBetweenParts);

			if (collisionDetected)
			{
				minDistance = currentDistance;
				currentDistance = 0.5 * (maxDistance + currentDistance);
			} else
			{
				maxDistance = currentDistance;
				currentDistance = 0.5 * (currentDistance + minDistance);
			}
		}
		return currentDistance;
	}
};


#endif //_PART_DISTANCE_ESTIMATOR_H_