/*
 * @File Created: 2022-11-26, 16:52:29
 * @Last Modified: 2022-11-26, 18:51:06
 * @Author: forty-twoo
 * @Copyright (c) 2022, Caiyue Li(li_caiyue@zju.edu.cn), All rights reserved.
 */

#include <igl/opengl/glfw/Viewer.h>
#include <tinynurbs/tinynurbs.h>
#include "bvh.hpp"
#include "show_libigl.hpp"
#include "intersection.hpp"
#include "newton.hpp"

igl::opengl::glfw::Viewer viewer;

int main(int argc, char* argv[])
{

	viewer.data().point_size = 5;

	tinynurbs::RationalCurve<double> crv1;      // Planar curve using float32
	crv1.control_points = { glm::vec3(-3, 0, 0), // std::vector of 3D points
		glm::vec3(-1, 4, 0),
		glm::vec3(1, -5, 0),
		glm::vec3(2, 1, 0),
		glm::vec3(4, -1, 0),
		glm::vec3(6, 2, 0),
		glm::vec3(8, 4, 0)

	};
	crv1.knots = { 0, 0, 0, 1, 2, 3, 4, 5, 5, 5 }; // std::vector of floats
	crv1.degree = 2;
	crv1.weights = { 1, 1, 1, 1, 1, 1, 1 };


	ShowCurve_Igl(crv1, 500, YELLOW);
	BVH_AABB bvh_curve1(12, 2, 100, crv1);

	tinynurbs::RationalCurve<double> crv2;      // Planar curve using float32
	crv2.control_points = { glm::vec3(-4, 4, 0), // std::vector of 3D points
		glm::vec3(-5, 2, 0),
		glm::vec3(-4, 6, 0),
		glm::vec3(-2, -5, 0),
		glm::vec3(1, 8, 0),
		glm::vec3(5, -7, 0)
	};
	crv2.knots = { 0, 0, 0, 1, 2, 3, 4, 4, 4 }; // std::vector of floats
	crv2.degree = 2;
	crv2.weights = { 1, 1, 1, 1, 1, 1 };

	ShowCurve_Igl(crv2, 500, YELLOW);
	BVH_AABB bvh_curve2(12, 2, 100, crv2);

	double tstep1 = *(crv1.knots.end() - 1);
	double tstep2 = *(crv2.knots.end() - 1);
	bvh_curve1.Build_NurbsCurve(crv1, bvh_curve1.bvh_aabb_node, 0, 0, tstep1);
	bvh_curve2.Build_NurbsCurve(crv2, bvh_curve2.bvh_aabb_node, 0, 0, tstep2);

	//ShowBVHNode_Igl(bvh_curve1.bvh_aabb_node, WHITE);
	//ShowBVHNode_Igl(bvh_curve2.bvh_aabb_node, GREEN);


	std::vector<std::pair<BVH_AABB_NodePtr, BVH_AABB_NodePtr>> IstNodePtr;
	if(CurveCurveBVHIntersect(bvh_curve1.bvh_aabb_node, bvh_curve2.bvh_aabb_node, IstNodePtr))
	{
		if(IstNodePtr.size() != 0)
			std::cout << "Curve1 and Curve2 intersect!\n";
		else
			std::cout << "Curve1 and Curve2 not intersect!\n";
		int idx = 0;
		std::vector<glm::vec3> ansPoints;
		for(auto it : IstNodePtr)
		{
			ShowAABB_Igl(it.first->bound, RED);
			ShowAABB_Igl(it.second->bound, GREEN);

			std::cout << "The " << idx++ << "th boxes pair lie on\n"
				<< "     min_point ("
				<< it.first->bound.Bmin.x << ", " << it.first->bound.Bmin.y << ", " << it.first->bound.Bmin.z << ") , max_point (" << it.first->bound.Bmax.x << ", " << it.first->bound.Bmax.y << ", " << it.first->bound.Bmax.z << ")\n";
			std::cout << "     min_point ("
				<< it.second->bound.Bmin.x << ", " << it.second->bound.Bmin.y << ", " << it.second->bound.Bmin.z << ") , max_point (" << it.second->bound.Bmax.x << ", " << it.second->bound.Bmax.y << ", " << it.second->bound.Bmax.z << ")\n";

			auto itsPoint = newton_curve_curve(it.first, it.second, 1e-12, true);
			std::cout << "\n\n\n";
			//去重
			if(ansPoints.size() == 0)
				ansPoints.push_back(itsPoint);
			else
			{
				//相距太近则判作同一个点
				glm::dvec3 delt = itsPoint - ansPoints[ansPoints.size() - 1];
				double dis = std::sqrt(delt.x * delt.x + delt.y * delt.y + delt.z * delt.z);
				if(dis > 1e-5)
				{
					ansPoints.push_back(itsPoint);
					std::cout << "distance between last and now: ";
					std::cout << std::setprecision(10) << dis << "\n";
				}

			}

		}
		for(auto it : ansPoints)
		{
			viewer.data().add_points(Eigen::RowVector3d(it.x, it.y, it.z), BLACK);
			std::cout << "(" << it.x << "," << it.y << "," << it.z << ")" << std::endl;
		}




	}

	/*
	ShowCurve_Igl(viewer, crv, 100);
	BVH_AABB bvh_curve(5, 2, 100);
	double tstep = *(crv.knots.end() - 1);
	bvh_curve.Build_NurbsCurve(crv, bvh_curve.bvh_aabb_node, 0, 0, tstep);
	ShowBVHNode(viewer, bvh_curve.bvh_aabb_node);
	*/

	/*
	 double u = 0.5;
	 std::vector<double> basis = tinynurbs::bsplineBasis(crv.degree, tinynurbs::findSpan(crv.degree, crv.knots, u), crv.knots, u);
	 for (auto it : basis)
	 {
		 std::cout << it << std::endl;
	 }
	 */

	viewer.launch();
}