/********************************************************************/
/* Copyright (c) 2019 System fugen G.K. and Yuzi Mizuno          */
/* All rights reserved.                                             */
/********************************************************************/
// distance.cpp
// implementation for functions about distance measure
#include "stdafx.h"
#include "Calc/distance.h"

#include "mg/Point.h"
#include "topo/Shell.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

namespace mgcalc{
	namespace detail{
		// Measures the distance between two points.
		// lp : point on lhs
		// rp : point on rhs
		// that d(lhs, rhs) == d(lp, rp).
		bool distance_aux(const MGPoint& lhs, const MGPoint& rhs, MGPosition& lp, MGPosition& rp){
			lp = lhs.position();
			rp = rhs.position();
			return true;
		}

		// Measures the distance between a point and a curve.
		// pos1 : equal to p
		// pos2 : point on c
		// that d(p, c) == d(pos1, pos2).
		bool distance_aux(const MGPoint& p, const MGCurve& c, MGPosition& pos1, MGPosition& pos2){
			// calculate the closest point
			pos1 = p.position();
			pos2 = c.eval(c.closest(pos1));
			return true;
		}

		// Measures the distance between two curves.
		// lp : point on lhs
		// rp : point on rhs
		// that d(lhs, rhs) == d(lp, rp).
		bool distance_aux(const MGCurve& lhs, const MGCurve& rhs, MGPosition& lp, MGPosition& rp){
			MGPosition pair = lhs.closest(rhs);
			lp = lhs.eval(pair[0]);
			rp = rhs.eval(pair[1]);
            return true;
		}

		// Measures the distance between a curve and a surface.
		bool distance_aux(const MGCurve& c, const MGSurface& surf, MGPosition& pos1, MGPosition& pos2){
			// 面直投影した曲線をみるのがよいだろう
			double dmin = -1.; // magic number
			std::vector<UniqueCurve> prj;
			MGPosition pos1tmp, pos2tmp;
			surf.project(c, prj);
			std::vector<UniqueCurve>::iterator first = prj.begin(), last = prj.end();
			for(; first != last; ++first){
				distance_aux(c, **first, pos1tmp, pos2tmp);
				double d = pos1tmp.distance(pos2tmp);
				if(dmin == -1. || d < dmin){
					dmin = d;
					pos1 = pos1tmp;
					pos2 = pos2tmp;
				}
			}
			if(dmin>=0.)
				return true; // found

			// 面の境界線を見るのがよいだろう
			int perim = surf.perimeter_num();
			for(int i = 0; i < perim; i++){
				std::unique_ptr<MGCurve> bnd(surf.perimeter_curve(i));
				assert(bnd.get());
				distance_aux(c, *bnd, pos1tmp, pos2tmp);
				double d = pos1tmp.distance(pos2tmp);
				if(dmin == -1. || d < dmin){
					dmin = d;
					pos1 = pos1tmp;
					pos2 = pos2tmp;
				}
			}
			return dmin>=0.;
		}

		// Measures the distance between a curve and a surface.
		bool distance_aux(const MGCurve& c, const MGFace& f, MGPosition& pos1, MGPosition& pos2){
			// 面直投影した曲線をみるのがよいだろう
			double dmin = -1.; // magic number
			std::vector<UniqueCurve> prj;
			MGPosition pos1tmp, pos2tmp;
			f.project(c, prj);
			std::vector<UniqueCurve>::iterator first = prj.begin(), last = prj.end();
			for(; first != last; ++first){
				distance_aux(c, **first, pos1tmp, pos2tmp);
				double d = pos1tmp.distance(pos2tmp);
				if(dmin == -1. || d < dmin){
					dmin = d;
					pos1 = pos1tmp;
					pos2 = pos2tmp;
				}
			}
			if(dmin>=0.)
				return true; // found

			// 面の境界線を見るのがよいだろう
			prj.clear();
			prj = f.outer_boundary();
			first = prj.begin();
			last = prj.end();
			for(; first != last; ++first){
				distance_aux(c, **first, pos1tmp, pos2tmp);
				double d = pos1tmp.distance(pos2tmp);
				if(dmin == -1. || d < dmin){
					dmin = d;
					pos1 = pos1tmp;
					pos2 = pos2tmp;
				}
			}
			return dmin>=0.;
		}

		// Measures the distance between a curve and a shell.
		bool distance_aux(const MGCurve& c, const MGShell& she, MGPosition& pos1, MGPosition& pos2){
			MGPosition pos1tmp, pos2tmp;
			double d = -1.; // magic number
			MGShell::const_iterator first = she.pcell_begin(), last = she.pcell_end();
			for(; first != last; ++first){
				if(distance_aux(c, *she.face(first), pos1tmp, pos2tmp)){
					double dtmp = pos1tmp.distance(pos2tmp);
					if(d == -1. || dtmp < d){ // min
						d = dtmp;
						pos1 = pos1tmp;
						pos2 = pos2tmp;
					}
				}
			}
			return d>=0.;
		}

		// Measures the distance between a point and a shell.
		// pos1 : equal to pnt
		// pos2 : point on she
		// that d(pnt, she) == d(pos1, pos2).
		bool distance_aux(const MGPoint& pnt, const MGShell& she, MGPosition& pos1, MGPosition& pos2){
			// calculate the closest point
			pos1 = pnt.position();
			pos2 = she.closest(pos1).eval();
			return true;
		}

		// 片方は点オブジェクトとわかっているときの
		// 距離計測関数呼び出しの振り分け
		//
		// 実行時にオブジェクトの型を判別して
		// オーバーロードdistance_auxを呼び出す
		bool distance_dispatch(const MGPoint& p, const MGObject& o, MGPosition& pos1, MGPosition& pos2){
			const MGShell* shl = dynamic_cast<const MGShell*>(&o);
			if(shl)
				return distance_aux(p, *shl, pos1, pos2);

			const MGFace* f = dynamic_cast<const MGFace*>(&o);
			if(f)
				return distance_aux(p, *f, pos1, pos2);

			const MGSurface* srf = dynamic_cast<const MGSurface*>(&o);
			if(srf)
				return distance_aux(p, *srf, pos1, pos2);

			const MGCurve* c = dynamic_cast<const MGCurve*>(&o);
			if(c)
				return distance_aux(p, *c, pos1, pos2);

			const MGPoint* p2 = dynamic_cast<const MGPoint*>(&o);
			if(p2)
				return distance_aux(p, *p2, pos1, pos2);

			return false;
		}

		// 片方は曲線オブジェクトとわかっているときの
		// 距離計測関数呼び出しの振り分け
		bool distance_dispatch(const MGCurve& c, const MGObject& o, MGPosition& pos1, MGPosition& pos2){
			const MGShell* shl = dynamic_cast<const MGShell*>(&o);
			if(shl)
				return distance_aux(c, *shl, pos1, pos2);

			const MGFace* f = dynamic_cast<const MGFace*>(&o);
			if(f)
				return distance_aux(c, *f, pos1, pos2);

			const MGSurface* srf = dynamic_cast<const MGSurface*>(&o);
			if(srf)
				return distance_aux(c, *srf, pos1, pos2);

			const MGCurve* c2 = dynamic_cast<const MGCurve*>(&o);
			if(c2)
				return distance_aux(c, *c2, pos1, pos2);

			const MGPoint* p2 = dynamic_cast<const MGPoint*>(&o);
			if(p2)
				return distance_aux(*p2, c, pos1, pos2);

			return false;
		}

		// 片方は曲面オブジェクトとわかっているときの
		// 距離計測関数呼び出しの振り分け
		bool distance_dispatch(const MGSurface& surf, const MGObject& o, MGPosition& pos1, MGPosition& pos2){
			const MGCurve* c = dynamic_cast<const MGCurve*>(&o);
			if(c)
				return distance_aux(*c, surf, pos1, pos2);

			const MGPoint* p2 = dynamic_cast<const MGPoint*>(&o);
			if(p2)
				return distance_aux(*p2, surf, pos1, pos2);
			return false;	
		}

		// 片方はfaceオブジェクトとわかっているときの
		// 距離計測関数呼び出しの振り分け
		bool distance_dispatch(const MGFace& f, const MGObject& o, MGPosition& pos1, MGPosition& pos2){
			const MGCurve* c = dynamic_cast<const MGCurve*>(&o);
			if(c)
				return distance_aux(*c, f, pos1, pos2);

			const MGPoint* p2 = dynamic_cast<const MGPoint*>(&o);
			if(p2)
				return distance_aux(*p2, f, pos1, pos2);
			return false;
		}

		// 片方はshellオブジェクトとわかっているときの
		// 距離計測関数呼び出しの振り分け
		bool distance_dispatch(const MGShell& she, const MGObject& o, MGPosition& pos1, MGPosition& pos2){
			const MGCurve* c = dynamic_cast<const MGCurve*>(&o);
			if(c)
				return distance_aux(*c, she, pos1, pos2);

			const MGPoint* p2 = dynamic_cast<const MGPoint*>(&o);
			if(p2)
				return distance_aux(*p2, she, pos1, pos2);
			return false;
		}
	}

	// Measures the distance between obj1 and obj2.
	bool distance(
		const MGObject& obj1,
		const MGObject& obj2,
		double& d,             // the distance is output
		MGPosition& pos1,      // the position on obj1
		MGPosition& pos2       // the position on obj2 that d(obj1, obj2) == d(pos1, pos2).
	){
		using namespace detail;
		bool ret = false;

		const MGShell* shl = dynamic_cast<const MGShell*>(&obj1);
		if(shl)
			ret = distance_dispatch(*shl, obj2, pos1, pos2);
		else{
			const MGFace* f = dynamic_cast<const MGFace*>(&obj1);
			if(f)
				ret = distance_dispatch(*f, obj2, pos1, pos2);
			else{
				const MGSurface* srf = dynamic_cast<const MGSurface*>(&obj1);
				if(srf)
					ret = distance_dispatch(*srf, obj2, pos1, pos2);
				else{
					const MGCurve* c2 = dynamic_cast<const MGCurve*>(&obj1);
					if(c2)
						ret = distance_dispatch(*c2, obj2, pos1, pos2);
					else{
						const MGPoint* p2 = dynamic_cast<const MGPoint*>(&obj1);
						if(p2)
							ret = distance_dispatch(*p2, obj2, pos1, pos2);
					}
				}
			}
		}
		// 現時点では曲面と曲面の距離計測は対応していない
		if(ret){
			d = pos1.distance(pos2);
			return true;
		}
		return false;
	}
}
