/********************************************************************/
/* Copyright (c) 2019 System fugen G.K. and Yuzi Mizuno          */
/* All rights reserved.                                             */
/********************************************************************/
/********************************************************************/
/**
 * @file curve.cpp
 * @brief 曲線作成系関数の実装ファイル。
 */
#include "stdafx.h"

// MGCL
#include "mg/CCisect.h"
#include "mg/CCisects.h"
#include "mg/Ellipse.h"
#include "mg/LBRep.h"
#include "mg/LBRepEndC.h"
#include "mg/Plane.h"
#include "mg/RLBRep.h"
#include "mg/Straight.h"
#include "mg/Transf.h"
#include "topo/Loop.h"
#include "Calc/curve.h"
#include "Calc/evalcs.h"
#include "Calc/line.h"

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

namespace mgcalc{

	// Interpolates between curve1 and curve2 as an object of class MGLBRep.
	std::unique_ptr<MGCurve> blend(
		const MGCurve& curve1, // start point side
		const MGCurve& curve2, // end point side
		double t1,   // parameter of curve1 that is near to the blend point
		double t2,   // parameter of curve2 that is near the end point
		double r1,   // tangent magnitude at the curve1 point
		double r2    // tangent magnitude at the curve2 point
	){
		double s = curve1.param_se(t1);
		double e = curve2.param_se(t2);

		MGBPointSeq bp(2, 3);
		bp.store_at(0, curve1.eval(s));
		bp.store_at(1, curve2.eval(e));

		MGVector T1, N1, T2, N2, nouse;
		double curva1, curva2, tor1, tor2;

		curve1.Frenet_frame(s, T1, N1, nouse, curva1, tor1);
		curve2.Frenet_frame(e, T2, N2, nouse, curva2, tor2);

		MGVector v1 = T1 * r1;
		MGVector v2 = T2 * r2;
		//ASSERT(v1 != mgZERO_VEC);
		//ASSERT(v2 != mgZERO_VEC);
		if(s == curve1.param_s()) v1 = -v1;
		if(e == curve2.param_e()) v2 = -v2;
		
		MGVector L = bp(1) - bp(0);
		double sintheta1 = L.sangle(v1);
		double sintheta2 = L.sangle(v2);
		double costheta1 = L.cangle(v1);
		double costheta2 = L.cangle(v2);

		double len = L.len() / (costheta1*sintheta2 + costheta2*sintheta1);
		if(len < 0.) len = -len;
		double L1 = len * sintheta2;
		double L2 = len * sintheta1;
		ASSERT(L1 > 0.);
		ASSERT(L2 > 0.);

		double tau = L1/v2.len() + L2/v1.len();

		MGLBRepEndC sc, ec;
		sc.set_1st(v1);
		ec.set_1st(v2);

		v1 = N1; v2 = N2;

		v1 *= (v1%v1*curva1/sintheta1);
		v2 *= (v2%v2*curva2/sintheta2);
		sc.set_2nd(v1);
		ec.set_2nd(v2);

		UniqueLBRep lb=std::make_unique<MGLBRep>();
		lb->buildByInterpolationEC(sc, ec, MGNDDArray(2, 0., tau), bp, 6);
		return lb;
	}

	// 2曲線の3次元合成
	MGSSisects compose_3D(const MGCurve& curve1, const MGCurve& curve2){
		MGSSisects ls;
		
		MGPlane plane1;
		if(!curve1.is_planar(plane1)){
			// failed
			return ls;
		}
		MGPlane plane2;
		if(!curve2.is_planar(plane2)){
			// failed
			return ls;
		}

		const MGVector& v1 = plane1.normal();
		const MGVector& v2 = plane2.normal();
		if(v1.parallel(v2)){
			// failed
			return ls;
		}

		MGBox box1 = curve1.box(), box2 = curve2.box();
		double maxlen = (box1 | box2).len() * 2;

		std::unique_ptr<MGSurface> surf1(curve1.sweep(v1, -maxlen, maxlen));
		if(!surf1.get()){
			// failed
			return ls;
		}
		std::unique_ptr<MGSurface> surf2(curve2.sweep(v2, -maxlen, maxlen));
		if(!surf2.get()){
			// failed
			return ls;
		}
		return surf1->isect(*surf2);
	}

	// 曲線を端点で接続する
	std::unique_ptr<MGCurve> connect(const MGCurve& curve1, const MGCurve& curve2){
		if(!is_finite(curve1) || !is_finite(curve2)){
			return std::unique_ptr<MGCurve>(nullptr);
		}

		// 仮のコード
		// 本格的な実装はいずれ行う

		std::unique_ptr<MGLBRep> tmp1(new MGLBRep(curve1));
		std::unique_ptr<MGLBRep> tmp2(new MGLBRep(curve2));

		// continuity & connect メソッド
		double ratio = 1; // 不使用
		int which = 0;
		int gcon = tmp1->continuity(*tmp2, which, ratio);
		if(gcon == -1){
			return std::unique_ptr<MGCurve>(nullptr);
		}
		else{
			tmp1->connect(gcon, which, *tmp2);
			return std::unique_ptr<MGCurve>(tmp1.release());
		}
	}

	// 曲線をポリラインに変換する
	std::unique_ptr<MGCurve> convert_to_polyline(
		const MGCurve& curve,
		double         azero,
		double         lzero
		){
		assert(azero > 0);
		assert(lzero > 0);
		if(dynamic_cast<const MGStraight*>(&curve)){
			return std::unique_ptr<MGCurve>(curve.clone());
		}
		
		// 直線以外の場合
		
		std::unique_ptr<MGLBRep> tmp(new MGLBRep);
		curve.polygonize(lzero,*tmp);		
		return std::unique_ptr<MGCurve>(tmp.release());
	}

	// ループのワールドカーブをオブジェクト化する
	std::unique_ptr<MGCurve> create_curve(const MGLoop& loop){
		std::vector<UniqueCurve> c = loop.curves_world();
		assert(!c.empty());
		
		std::vector<UniqueCurve>::iterator first = c.begin(), last = c.end();
		UniqueCurve curve(c.front()->clone());
		assert(curve.get());
		
		++first;
		for(; first != last; ++first){
			curve = connect(*curve, **first);
			if(!curve.get()){
				return UniqueCurve(nullptr);
			}
		}
		assert(curve.get());
		return curve;
	}

	// Extends a curve.
	MGCurve* extend(
		const MGCurve& curve, // original curve
		double         param, // parameter on curve
		double         length,// length of chord
		double         dk     // curvature variation
	){
		assert(length > MGTolerance::wc_zero());

		double t = curve.param_se(param);
		bool bst = (t == curve.param_s());

		std::unique_ptr<MGCurve> copied(curve.clone());
		if(MGLBRep* lbrep = dynamic_cast<MGLBRep*>(copied.get())){
			lbrep->extend(bst, length, dk);
		}
		else if(MGRLBRep* rlbrep = dynamic_cast<MGRLBRep*>(copied.get())){
			rlbrep->extend(bst, length, dk);
		}
		else{
			copied->extend(length, bst);
		}
		return copied.release();
	}

	// 曲線の一方の端点方向から直線を引く
	MGStraight* extension(
		const MGCurve& curve,
		double         hint_param
		){
		double rounded = curve.param_se(hint_param);
		assert(rounded == curve.param_s() || rounded == curve.param_e());
		
		MGVector direction = curve.eval(rounded, 1);
		assert(!direction.is_null());
		
		std::unique_ptr<MGStraight> ext;
		if(rounded == curve.param_s()){
			ext.reset(
				new MGStraight(
					MGSTRAIGHT_HALF_LIMIT,
					-direction,  // 向きがオリジナルと反対になるのが気にいらない
					curve.start_point()
					)
				);
			// よって反転する
			ext->negate();
		}
		else{
			ext.reset(
				new MGStraight(
					MGSTRAIGHT_HALF_LIMIT,
					direction,
					curve.end_point()
					)
				);
		}
		assert(ext.get());
		return ext.release();
	}

	// 曲線が有限かどうかを判定する
	bool is_finite(const MGCurve& curve){
		return curve.param_range().finite();
	}

	// Returns whether curve is Jordan-curve or not.
	// Jordan curve is defined as follows:
	// 1 - curve is closed
	// 2 - is C2 continuous
	// 3 - is planar
	// 4 - and does not intersect itself.
	bool is_jordan(const MGCurve& curve){
		if(!curve.is_closed()) return false;
		MGPlane plane;
		if(!curve.is_planar(plane)) return false;
		if(is_self_isect(curve)) return false;

		return true;
	}

	// Returns whether curve intersects itself or not.
	bool is_self_isect(const MGCurve& curve){
		if(dynamic_cast<const MGStraight*>(&curve))
			return false;

		if(dynamic_cast<const MGEllipse*>(&curve))
			return false;

		// ????
		return false;
	}

	// Create a polyline through A, B, C, D, A.
	std::unique_ptr<MGLBRep> rectangular_line(
		const MGPosition& A,
		const MGPosition& B,
		const MGPosition& C,
		const MGPosition& D
	){
		MGBPointSeq bp(5, 3);
		bp.store_at(0, A);
		bp.store_at(1, B);
		bp.store_at(2, C);
		bp.store_at(3, D);
		bp.store_at(4, A);
		std::unique_ptr<MGLBRep> lb(new MGLBRep);
		lb->buildByInterpolation(bp, 2);
		return lb;
	}

} // namespace mgcalc
