/*
 * @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> crv;      // Planar curve using float32
	crv.control_points = { glm::vec3(-4, 0, 0), // std::vector of 3D points
		glm::vec3(-1, -1.2, -0.5),
		glm::vec3(2, 2.3, 1),
		glm::vec3(-3, -1.8, -2),

	};
	crv.degree = 2;
	crv.knots = { 0, 0, 0, 1, 2, 2, 2 }; // std::vector of floats
	crv.weights = { 1, 1, 1, 1 };


	tinynurbs::RationalSurface<double> srf;
	srf.degree_u = 3;
	srf.degree_v = 3;
	srf.knots_u = { 0, 0, 0, 0, 1, 1, 1, 1 };
	srf.knots_v = { 0, 0, 0, 0, 1, 1, 1, 1 };

	srf.control_points = { 4, 4, {
		glm::vec3(2, -2, -2), glm::vec3(-2.5, -2.2, -1.5), glm::vec3(-2, -2, -0.5), glm::vec3(-2, -2, 1.5),
		glm::vec3(2, -1, -2), glm::vec3(-2.5, -1.2, -1.5), glm::vec3(-2, -1, -0.5), glm::vec3(-2, -1, 1.5),
		glm::vec3(3, 1.2, -2), glm::vec3(-2.5, 1.2, -1.5), glm::vec3(-2, 1.5, -0.5), glm::vec3(-2, 1.5, 1.5),
		glm::vec3(2, 2, -2), glm::vec3(-2.5, 2, -1.5), glm::vec3(-2, 2.5, -0.5), glm::vec3(-2, 2, 1.5)
	}
	};

	srf.weights = { 4, 4,
		{
			1, 1, 1, 1,
			1, 1, 1, 1,
			1, 1, 1, 1,
			1, 1, 1, 1,
	}
	};


	BVH_AABB bvh_curve(10, 2, 100, crv);
	BVH_AABB bvh_surface(6, 4, 100, srf);
	ShowCurve_Igl(crv, 500, YELLOW);
	ShowSurface_Igl(srf, 10, 10, BLUE);



	double tstep = *(crv.knots.end() - 1);
	bvh_curve.Build_NurbsCurve(crv, bvh_curve.bvh_aabb_node, 0, 0, tstep);

	double ustep = *(srf.knots_u.end() - 1);
	double vstep = *(srf.knots_v.end() - 1);
	bvh_surface.Build_NurbsSurface(srf, bvh_surface.bvh_aabb_node, 0, 0, 0, ustep, vstep);

	//ShowBVHNode_Igl(bvh_curve.bvh_aabb_node, RED);
	//ShowBVHNode_Igl(bvh_surface.bvh_aabb_node, GREEN);


	std::vector<std::pair<BVH_AABB_NodePtr, BVH_AABB_NodePtr>> IstNodePtr;
	if(BVHIntersect(bvh_curve.bvh_aabb_node, bvh_surface.bvh_aabb_node, IstNodePtr))
	{
		if(IstNodePtr.size() != 0)
			std::cout << "Curve and Surface intersect!\n";
		else
			std::cout << "Curve and Surface 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_surface(it.first, it.second, 1e-12, true);
			ansPoints.push_back(itsPoint);
			std::cout << "\n\n\n";
			//std::cout << "(" << itsPoint.x << "," << itsPoint.y << "," << itsPoint.z << ")" << std::endl;
			//去重
			//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);
			//	ansPoints.push_back(itsPoint);

			//	//if(dis > 1e-5)
			//	//{
			//	//	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;
		}
	}


	viewer.launch();
	return  0;


}