/********************************************************************/
/* Copyright (c) 2019 System fugen G.K. and Yuzi Mizuno          */
/* All rights reserved.                                             */
/********************************************************************/

/** @file QSEval.cpp
 *  Implemenation for eval_smoothness() and eval_gap().
 */
#include "stdafx.h"
#include "mg/SBRep.h"
#include "mg/SBRepTP.h"
#include "mg/SBRepVecTP.h"
#include "mg/TPmaker.h"
#include "topo/Face.h"
#include "QS/QSEval.h"
#define _DBG_EV 0

using namespace std;

namespace QS{
// We don't place a tab space each lines, since all of the functions implemented
// in this file are within namespace QS.

double eval_gap(
	const MGSBRep& qsurf,   // (I/ ) quadangular surface that will ce evaluated.
	const MGCurve& pericrv, // (I/ ) perimeter curve that is used when qsurf is created.
	int            iperi,   // (I/ ) 0: vmin, 1: umax, 2: vmax, and 3: umin. 
	                        //       based on class MGSBRepTP.
	MGPosition&    uv       // ( /O) a candidate parameter of qsurf.
){
	assert(0 <= iperi && iperi <= 3);
	if(uv.sdim() != 2){ uv.resize(2);}

	MGCurve* peri2 = qsurf.perimeter_curve(iperi);

	int id = -1;
	switch(iperi){
	case 0:
		uv(1) = qsurf.param_s_v(); id = 0;
		break;
	case 1:
		uv(0) = qsurf.param_e_u(); id = 1;
		break;
	case 2:
		uv(1) = qsurf.param_e_v(); id = 0;
		break;
	case 3:
		uv(0) = qsurf.param_s_u(); id = 1;
		break;
	};

	MGNDDArray tau; 
	tau.buildByKnotVector(pericrv.knot_vector());

	double t0 = peri2->param_s();
	double t1 = peri2->param_e();
	const double ratio = (t1-t0)/(pericrv.param_e()-tau[0]);

	MGPosition P = pericrv.start_point();
	double t = peri2->closest(P);
	double lenmax = (peri2->eval(t) - P).len();

	int ntaum1=tau.length()-1;
	for(int i = 0; i < ntaum1; i++){
		double tmp = (tau[i] + tau[i+1]) * .5;
		P = pericrv.eval(tmp);
		double tg = ratio * (tmp - tau[0]) + t0;
		if(!peri2->perp_guess(t0, t1, P, tg, t)){
#if _DBG_EV
			cerr << "** perp_guess (gap) error\n";
#endif
			t = peri2->closest(P);
		}
		double len = (P - peri2->eval(t)).len();
		if(len > lenmax){
			lenmax = len;
			uv(id) = t;
		}
		
		P = pericrv.eval(tau[i+1]);
		tg = ratio * (tau[i+1] - tau[0]) + t0;
		if(!peri2->perp_guess(t0, t1, P, tg, t)){
#if _DBG_EV
			cerr << "** perp_guess (gap) error\n";
#endif
			t = peri2->closest(P);
		}
		len = (P - peri2->eval(t)).len();
		if(len > lenmax){
			lenmax = len;
			uv(id) = t;
		}
	}
	P = pericrv.end_point();
	if(!peri2->perp_guess(t0, t1, P, t1, t)){
#if _DBG_EV
		cerr << "** perp_guess  (gap) error\n";
#endif
		t = peri2->closest(P);
	}
	double len = (P - peri2->eval(t)).len();
	if(len > lenmax){
		lenmax = len;
		uv(id) = t;
	}
	delete peri2;
	return lenmax;
}

double eval_smoothness(
	const MGSBRep& qsurf,   // (I/ ) quadangular surface that will ce evaluated.
	const MGSBRepTP& tp,   // (I/ ) TP curve that used when qsurf is created.
	int            iperi,   // (I/ ) 0: vmin, 1: umax, 2: vmax, and 3: umin. 
	                        //       based on class MGSBRepTP.
	MGPosition&    uv       // ( /O) a candidate parameter of qsurf
){
	assert(0 <= iperi && iperi <= 3);
	if(!tp.specified(iperi)){
		return 0.;
	}
	const MGLBRep& tpcrv = tp.TP(iperi);
	if(uv.sdim() != 2){ uv.resize(2);}
	const double vmin = qsurf.param_s_v();
	const double umax = qsurf.param_e_u();
	const double vmax = qsurf.param_e_v();
	const double umin = qsurf.param_s_u();

	MGPosition tmpuv(2); // used to compute normal vector at a point of qsurf.
	int id = -1; // When (iperi == 0 || iperi == 2), then set id to 0 that means u will changes.
	        // When (iperi == 1 || iperi == 3), then set id to 1 that means v will changes.
	MGKnotVector ttmp(tpcrv.knot_vector());
	double peris = 0.;

	switch(iperi){
	case 0: // vmin
		id = 0; peris = vmin; tmpuv(1) = uv(1) = vmin; break;
	case 1: // umax
		id = 1; peris = umin; tmpuv(0) = uv(0) = umax; break;
	case 2: // vmax
		id = 0; peris = umin; tmpuv(1) = uv(1) = vmax; break;
	case 3: // umin
		id = 1; peris = vmin; tmpuv(0) = uv(0) = umin; break;
	}
	const double m = (id == 1 ? vmax-vmin : umax-umin) / (tpcrv.param_e() - tpcrv.param_s());
	
	MGNDDArray tau;
	tau.buildByKnotVector(ttmp);

	// We sample the original data points and the midpoints of them.
	int ntaum1 = tau.length()-1;
	double sa = 0., retval = 0.;
	for(int i=0; i < ntaum1; i++){
		// Evaluate the difference of angle of two normal vectors at 
		// a data point parameter.
		tmpuv(id) = peris + m * (tau[i] - tau[0]);
		assert(qsurf.in_range(tmpuv));
		sa = qsurf.normal(tmpuv).sangle(tpcrv.eval(tau[i]));

		if(retval < sa){
			retval = sa; uv(id) = tmpuv(id);
		}
		// Evaluate the difference of angle of two normal vectors at
		// a midpoint parameter of data points.
		tmpuv(id) = peris + m * ((tau[i]+tau[i+1]) * .5 - tau[0]);
		assert(qsurf.in_range(tmpuv));
		sa = qsurf.normal(tmpuv).sangle(tpcrv.eval((tau[i]+tau[i+1]) * .5));

		if(retval < sa){
			retval = sa; uv(id) = tmpuv(id);
		}
	}
	// Evaluate the difference of angle of two normal vectors at
	// the end parameter of data point sequence.
	tmpuv(id) = peris + m * (tau[ntaum1] - tau[0]);
	assert(qsurf.in_range(tmpuv));
	sa = qsurf.normal(tmpuv).sangle(tpcrv.eval(tau[ntaum1]));

	if(retval < sa){
		retval = sa; uv(id) = tmpuv(id);
	}
#if _DBG_EV
	cerr << "******* uv angle " << uv << ' ' << retval << endl;
#endif
	return retval;
}

double eval_smoothness(
	const MGSBRep& qsurf,
	const MGSBRepTP& tp,
	MGPosition& uv
){
	if(uv.sdim() != 2) uv.resize(2);
	MGPosition tmpuv(2);
	double maxdev = 0;
	for(int i = 0; i < 4; i++){
		if(tp.specified(i)){
			double ret = eval_smoothness(qsurf, tp, i, tmpuv);
			if(ret > maxdev){
				maxdev = ret;
				uv = tmpuv;
			}
		}
	}
#if _DBG_EV
	cerr << "DEBUG eval_smoothness max uv " << maxdev << ' ' << uv << endl;
#endif
	return maxdev;
}

double eval_gap(const MGSBRep & qsurf, const MGLBRep * perim[4], MGPosition & uv){
	const MGCurve* perims[4];
	extractConstPointerVec(perim, perim+4, perims);
	return eval_gap(qsurf, perims, uv);
}
double eval_gap(
	const MGSBRep& qsurf,
	const MGCurve* perim[4],
	MGPosition& uv
){
	if(uv.sdim() != 2) uv.resize(2);
	MGPosition tmpuv(2);
	double maxdev = 0;
	for(int i = 0; i < 4; i++){
		double ret = eval_gap(qsurf, *perim[i], i, tmpuv);
		if(ret > maxdev){
			maxdev = ret;
			uv = tmpuv;
		}
	}
#if _DBG_EV
	cerr << "DEBUG eval_gap max uv " << maxdev << ' ' << uv << endl;
#endif
	return maxdev;
}

} // QS


#define _STRICT_TRIM 1

// あとで実行時に指定できるようにする
const int ndiv = 20;

// こちらは指定させてはいけない気がする
const double tolerance = 0.1;

//////// face vs face /////////
namespace{
	/**
	 *  @brief eval_deviationの下請け関数
	 *  @param face1 一方の面データ
	 *  @param face2 もう一方の面データ
	 *  @param pos1  face1側の評価位置(戻り値)
	 *  @param pos2  face2側の評価位置(戻り値)
	 *  @param gap   離れの値(戻り値)
	 *  @param fold  折れの値(戻り値)
	 *  @param wcrv1 face1側エッジのワールドカーブ
	 *  @param wcrv2 face2側エッジのワールドカーブ
	 *  @param pcrv1 face1側エッジの面上パラメータカーブ
	 *  @param pcrv2 face2側エッジの面上パラメータカーブ
	 *
	 *  wcrv1とwcrv2はほぼ共線となっている。
	 */
	void gap_and_angle(
		const MGFSurface& face1,
		const MGFSurface& face2,
		const vector<MGPosition>& uvuvs,
		vector<MGPosition>& pos1,
		vector<MGPosition>& pos2,
		vector<double>&     gap,
		vector<double>&     fold
	){
		MGPosition P1(3), P2(3), uv1(2), uv2(2);
		for(size_t i = 0; i<uvuvs.size(); i++){
			const MGPosition& uvuvi = uvuvs[i];
			P1 = face1.eval(uvuvi[0], uvuvi[1]);
			P2 = face2.eval(uvuvi[2], uvuvi[3]);

			pos1.push_back(P1);
			pos2.push_back(P2);
			// 離れの値決定
			gap.push_back((P1 - P2).len());
			double angle = MGCL::radian_to_degree(
				face1.unit_normal(uvuvi[0], uvuvi[1]).angle(face2.unit_normal(uvuvi[2], uvuvi[3])));
			fold.push_back(angle);
		}
	}

} // namespace

namespace QS{
	/**
	 *  @brief eval_deviationの下請け関数
	 *  @param crv1  一方の面データ
	 *  @param crv2  もう一方の面データ
	 *  @param pos1  face1側の評価位置
	 *  @param pos2  face2側の評価位置
	 *  @param gap   離れの値
	 */
	void eval_deviation_core(
		const MGCurve&      crv1,
		const MGCurve&      crv2,
		vector<MGPosition>& pos1,
		vector<MGPosition>& pos2,
		vector<double>&     gap
	){
		const MGPosition crv1s = crv1.start_point();
		const MGPosition crv2s = crv2.start_point();
		const MGPosition crv1e = crv1.end_point();
		const MGPosition crv2e = crv2.end_point();

		// pos1, pos2それぞれの最初の要素を決定する

		pos1.push_back(crv1s);

		double lenss = (crv1s - crv2s).len();
		double lense = (crv1s - crv2e).len();
		double ps2, pe2;
		if(lenss < lense){
			pos2.push_back(crv2s);
			gap.push_back(lenss);
			ps2 = crv2.param_s(); pe2 = crv2.param_e();
		} else{
			pos2.push_back(crv2e);
			gap.push_back(lense);
			ps2 = crv2.param_e(); pe2 = crv2.param_s();
		}

		// 今のところ分割値は勝手に決めている
		double t = crv1.param_s();
		double dt = crv1.param_span() / ndiv;
		double dt2 = (pe2 - ps2) / ndiv;

		// pos1, pos2の、それぞれの最初と最後の要素以外のすべての要素を計算する

		MGPosition P1(3), P2(3); double t2;
		for(int i = 1; i < ndiv; i++){
			t += dt;
			P1 = crv1.eval(t);
			ps2 += dt2;
			if(!crv2.perp_guess(crv2.param_s(), crv2.param_e(), P1, ps2, t2)){
				t2 = crv2.closest(P1);
			}
			P2 = crv2.eval(t2);

			pos1.push_back(P1);
			pos2.push_back(P2);
			// 離れの値決定
			gap.push_back((P1 - P2).len());
		}

		// pos1, pos2それぞれの最後の点を決定する

		pos1.push_back(crv1e);
		if(lenss < lense){
			pos2.push_back(crv2e);
			gap.push_back((crv1e - crv2e).len());
		} else{
			pos2.push_back(crv2s);
			gap.push_back((crv1e - crv2s).len());
		}
	}

	/**
	 *  @brief 曲線の折れと離れを評価する。
	 *  @param crv1  一方の面データ
	 *  @param crv2  もう一方の面データ
	 *  @param pos1  face1側の評価位置
	 *  @param pos2  face2側の評価位置
	 *  @param gap   離れの値
	 */
	void eval_deviation(
		const MGCurve&      crv1,
		const MGCurve&      crv2,
		vector<MGPosition>& pos1,
		vector<MGPosition>& pos2,
		vector<double>&     gap
	){
		int cmn_count = 0;

		// 一時的にトレランスを緩くしてからMGCurve::common関数を呼ぶ。
		mgTolSetLineZero lineZeroSet(tolerance);//???????
		vector<double> spans;
		int cmnret = crv1.common(crv2, spans);
		lineZeroSet.restore();

		// 共線でなければ次のエッジの組み合わせへジャンプ。
		if(cmnret <= 0) return;

		// パラメータ範囲を広げられるか
		double sp0 = spans[0], sp1 = spans[1], sp2 = spans[2], sp3 = spans[3];
		cmnret *= 4;
		for(int k = 4; k < cmnret; k += 4){
			if(sp0 > spans[k]){
				sp0 = spans[k];
				sp2 = spans[k+2];
			}
			if(sp1 < spans[k+1]){
				sp1 = spans[k+1];
				sp3 = spans[k+3];
			}
		}

		MGUnit_vector v1, v2;

		// 端点の方向がほぼ一致しているかどうかを調べる。
		// トレランスがゆるいときに、
		// MGCurve::commonの計算結果がおかしいことがあるため。
		v1 = crv1.direction(sp0);
		v2 = crv2.direction(sp2);
		double inp = fabs(v1%v2);
		if(!MGAZero(inp - 1.)){
			// 変な結果を拾っていたので捨てる
			return;
		}

		// 曲線のパラメータ範囲を調べる

#if _STRICT_TRIM
		const MGPosition crv1s = crv1.start_point();
		const MGPosition crv1e = crv1.end_point();
		const MGPosition crv2s = crv2.start_point();
		const MGPosition crv2e = crv2.end_point();

		// 向こう側の曲線始点に一番近い点のパラメータ
		double prm1s = crv1.closest(crv2s);
		// 向こう側の曲線終点に一番近い点のパラメータ
		double prm1e = crv1.closest(crv2e);
		if(prm1s > prm1e) swap(prm1s, prm1e);

		// 向こう側の曲線始点に一番近い点のパラメータ
		double prm2s = crv2.closest(crv1s);
		// 向こう側の曲線終点に一番近い点のパラメータ
		double prm2e = crv2.closest(crv1e);
		if(prm2s > prm2e) swap(prm2s, prm2e);
#endif

		double s = sp0;
		double e = sp1;
#if _STRICT_TRIM
		if(sp0 < prm1s && prm1s < sp1){ s = prm1s; }
		if(sp0 < prm1e && prm1e < sp1){ e = prm1e; }
#endif

		// 曲線1側の共線部分曲線を取り出す
		//auto_ptr<MGCurve> crv1trmd(crv1.copy_limitted(MGInterval(s, e)));
		MGTrimmedCurve crv1trmd(crv1, s, e);
		if(sp2 > sp3) swap(sp2, sp3);

		s = sp2;
		e = sp3;
#if _STRICT_TRIM
		if(sp2 < prm2s && prm2s < sp3){ s = prm2s; }
		if(sp2 < prm2e && prm2e < sp3){ e = prm2e; }
#endif

		// 曲線2側の共線部分曲線を取り出す
		//auto_ptr<MGCurve> crv2trmd(crv2.copy_limitted(MGInterval(s, e)));
		MGTrimmedCurve crv2trmd(crv2, s, e);

		// 補助関数に丸投げ
		eval_deviation_core(crv1trmd, crv2trmd, pos1, pos2, gap);
	}

	/**
	 *  @brief 曲線とMGFaceの距離的な離れを計算する
	 *  @param curve 評価する曲線
	 *  @param face  評価するフェイス
	 *  @param pos1  curve側の評価した点
	 *  @param pos2  face側の評価した点
	 *  @param gap   離れの値
	 *
	 *  pos1[i]とpos2[i]とgap[i]が一対一対応している。
	 */
	void eval_deviation(
		const MGCurve&      curve,
		const MGFace&       face,
		vector<MGPosition>& pos1,
		vector<MGPosition>& pos2,
		vector<double>&     gap
	){
		std::vector<UniqueCurve> outer = face.outer_boundary();
		for(const auto& i: outer)
			eval_deviation(curve, *i, pos1, pos2, gap);
	}

/**
 *  @brief 面(MGFace)の折れと離れを評価する。
 *  @param f1    一方の面データ
 *  @param f2    もう一方の面データ
 *  @param pos1  face1側の評価位置
 *  @param pos2  face2側の評価位置
 *  @param gap   離れの値
 *  @param fold  折れの値
 */
	void eval_deviation(
		const MGFSurface& face1,
		const MGFSurface& face2,
		vector<MGPosition>& pos1,
		vector<MGPosition>& pos2,
		vector<double>&     gap,
		vector<double>&     fold
	){
		vector<MGPosition> uvuvs;
		face1.eval_discrete_deviation(face2, uvuvs);
		gap_and_angle(face1, face2, uvuvs, pos1, pos2, gap, fold);
	}

} // QS
