// Copyright (C) Mocchi (mocchi_2003@yahoo.co.jp)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "opennurbs.h"
#include "ONGEO.h"
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <limits>
#include <ctime>
#include "TTW.h"

#ifdef max
#undef max
#endif
#ifdef min
#undef min
#endif

struct MeshClosestPoint{
	struct SNT_tuple{
		MeshClosestPoint *this_;
		ON_RTreeSphere rs;
		ON_3dPoint pt_q;
		ON_3dPoint pt_target;
		int face_id;
	};
	static bool SearchNearestTriangle(void *ctx, ON__INT_PTR id){
		SNT_tuple &snt = *static_cast<SNT_tuple *>(ctx);
		ON_RTree &rtree = snt.this_->rtree;
		ON_Mesh &m = snt.this_->mesh;
		ON_3dPoint pt_q = snt.pt_q;

		ON_MeshFace &f = m.m_F[id];
		ON_3dPoint ptT[] = {m.m_V[f.vi[0]], m.m_V[f.vi[1]], m.m_V[f.vi[2]]};
		ON_3dPoint &ptA = ptT[0], &ptB = ptT[1], &ptC = ptT[2];
		ON_Plane pln(ptA, ptB, ptC);

		ON_3dPoint ptPL = pln.ClosestPointTo(pt_q);

		// 三角形と点との内外判定 http://www.sousakuba.com/Programming/gs_hittest_point_triangle.html
		ON_3dVector cpA = ON_CrossProduct(ptA-ptC, ptPL-ptA);
		ON_3dVector cpB = ON_CrossProduct(ptB-ptA, ptPL-ptB);
		ON_3dVector cpC = ON_CrossProduct(ptC-ptB, ptPL-ptC);

		double dist_min = snt.rs.m_radius;
		ON_3dPoint pt_cand;

		if (ON_DotProduct(cpA, cpB) < 0 || ON_DotProduct(cpA, cpC) < 0){
			// 三角形が乗る面の投影点が三角形の外の場合は、三角形の各辺との距離を求める。
			for (int i = 0; i < 3; ++i){
				ON_Line lin(ptT[i], ptT[(i+1)%3]);
				double t; lin.ClosestPointTo(pt_q, &t);
				if (t < 0) t = 0; else if (t > 1) t = 1;
				ON_3dPoint pt_l = lin.PointAt(t);
				double dist = pt_l.DistanceTo(pt_q);
				if (dist_min > dist) pt_cand = pt_l, dist_min = dist;
			}
		}else{
			pt_cand = ptPL;
			dist_min = ptPL.DistanceTo(pt_q);
		}
		if (dist_min < snt.rs.m_radius){
			snt.pt_target = pt_cand;
			snt.rs.m_radius = dist_min;
			snt.face_id = id;
		}

		return true;
	}


	ON_Mesh &mesh;
	ON_RTree rtree;
	MeshClosestPoint(ON_Mesh &mesh) : mesh(mesh){
		rtree.CreateMeshFaceTree(&mesh);
	}
	double ClosestPoint(const ON_3dPoint &pt_q, ON_3dPoint &pt_out, int *face_id){
		if (mesh.m_F.Count() == 0) return -1;
		SNT_tuple snt; snt.this_ = this; snt.pt_q = pt_q;
		for (int i = 0; i < 3; ++i) snt.rs.m_point[i] = pt_q[i];

		// 初期値
		snt.pt_target = mesh.m_V[0];
		snt.rs.m_radius = snt.pt_target.DistanceTo(pt_q);

		rtree.Search(&snt.rs, SearchNearestTriangle, &snt);
		pt_out = snt.pt_target;
		if (face_id) *face_id = snt.face_id;
		return snt.rs.m_radius;
	}
};


int mesh_examine(char *stlfile){
	TTW_LapTimeWatch ltw;
	ONX_Model model;
	ON_Mesh mesh;
	ON_String header;
	ONGEO_ReadBinarySTL(ON_BinaryFile(ON::read, ON_FileStream::Open(stlfile, "rb")), mesh, header);

	ONX_Model_Object &mobj = model.m_object_table.AppendNew();
	mobj.m_object = &mesh;
	mobj.m_bDeleteObject = false;

	ON_BoundingBox bb = mesh.BoundingBox();

	MeshClosestPoint mcp(mesh);

	double dist_criteria = bb.Diagonal().Length()/2;

	ON_Color cols[] = {ON_Color(0, 0, 128), ON_Color(0, 0, 255), ON_Color(0, 128, 192), ON_Color(0, 255, 0), ON_Color(255, 255, 0), ON_Color(255, 0, 0)};
	// プレーン上に点列を生成
	int divides = 100;
	double ddivides = divides;
	for (int k = 0; k < 6; ++k){
		ON_Interval wint(bb.m_min[2], bb.m_max[2]);
		for (int j = 0; j <= divides; ++j){
			ON_Interval vint(bb.m_min[1], bb.m_max[1]);
			for (int i = 0; i <= divides; ++i){
				ON_Interval hint(bb.m_min[0], bb.m_max[0]);
				ON_3dPoint pt;
				if (k == 0 || k == 1) pt.Set(hint.ParameterAt(static_cast<double>(i)/ddivides), vint.ParameterAt(static_cast<double>(j)/ddivides), wint.ParameterAt(k));
				else if (k == 2 || k == 3) pt.Set(hint.ParameterAt(static_cast<double>(i)/ddivides), vint.ParameterAt(k - 2), wint.ParameterAt(static_cast<double>(j)/ddivides));
				else if (k == 4 || k == 5) pt.Set(hint.ParameterAt(k - 4), vint.ParameterAt(static_cast<double>(i)/ddivides), wint.ParameterAt(static_cast<double>(j)/ddivides));
				ON_3dPoint pt_out;
				ltw.Lap("ClosestPoint", ltw.LoopStart);
				double dist = mcp.ClosestPoint(pt, pt_out, 0);
				ltw.Lap("ClosestPoint", ltw.LoopEnd);
				int color_idx = static_cast<int>(std::floor(dist * 5.0 / dist_criteria));
				if (color_idx > 5) color_idx = 5;

				ON_PointCloud pc; pc.AppendPoint(pt), pc.AppendPoint(pt_out);

				ONX_Model_Object &ptobj = model.m_object_table.AppendNew();
				ptobj.m_object = pc.Duplicate();
				ptobj.m_bDeleteObject = true;
				ptobj.m_attributes.m_name.Format("%f", dist);
				ptobj.m_attributes.SetColorSource(ON::color_from_object);
				ptobj.m_attributes.m_color = cols[color_idx];
			}
		}
	}
	std::printf("%s\n", TTW_GetString(ltw.AnalyzeLoopRange()));
	model.Write("mesh_closest_point.3dm", 4);
	return 0;
}