#ifndef _GEOMETRY_HPP_
#define _GEOMETRY_HPP_

#include <algorithm>
#include <cmath>
#include <iostream>
#include <utility>

namespace geo
{
	#define MEMBER_TYPE double
	const MEMBER_TYPE EPS = 1e-10;

	// 度数法から弧度法へ変換する。
	template<typename T>
	inline T degToRad(T d)
	{
		return d * M_PI / 180.0;
	}

	// 弧度法から度数法へ変換する。
	template<typename T>
	inline T radToDeg(T r)
	{
		return r * 180.0 / M_PI;
	}

	template<typename T>
	class _Vector2D
	{
	public:
		T x, y;

		inline _Vector2D() {}
		inline _Vector2D(T x, T y) : x(x), y(y) {}
	
		// ノルムの二乗を返す。
		inline T sqnorm()const {
			return x*x + y*y;
		}
		// ノルムを返す。
		inline T norm()const {
			return std::sqrt(sqnorm());
		}
		// 偏角を返す。
		inline T arg()const {
			return std::atan2(y, x);
		}
		// 同じ方向の単位ベクトルを返す。
		inline _Vector2D getNormalized()const {
			return *this / norm();
		}
		// 回転したベクトルを返す。
		inline _Vector2D getRotated(T angle)const {
			T c = std::cos(angle), s = std::sin(angle);
			return _Vector2D(c*x - s*y, s*x + c*y);
		}
		// 法線ベクトルを返す。
		inline _Vector2D getNormal()const {
			return _Vector2D(-y, x);
		}
		// 0ベクトルかどうかを返す。
		inline bool isZero()const {
			return std::abs(x) < EPS && std::abs(y) < EPS;
		}

		// unary operators
		inline _Vector2D operator+ ()const {
			return *this;
		}
		inline _Vector2D operator- ()const {
			return _Vector2D(-x, -y);
		}

		// binary operators
		inline _Vector2D operator+ (const _Vector2D& v)const {
			return _Vector2D(x+v.x, y+v.y);
		}
		inline _Vector2D operator- (const _Vector2D& v)const {
			return _Vector2D(x-v.x, y-v.y);
		}
		inline _Vector2D operator* (T k)const {
			return _Vector2D(x*k, y*k);
		}
		inline _Vector2D operator/ (T k)const {
			return _Vector2D(x/k, y/k);
		}
		inline _Vector2D& operator+=(const _Vector2D& v) {
			x += v.x;
			y += v.y;
			return *this;
		}
		inline _Vector2D& operator-=(const _Vector2D& v) {
			x -= v.x;
			y -= v.y;
			return *this;
		}
		inline _Vector2D& operator*=(T k) {
			x *= k;
			y *= k;
			return *this;
		}
		inline _Vector2D& operator/=(T k) {
			x /= k;
			y /= k;
			return *this;
		}
		inline bool operator==(const _Vector2D& v)const {
			return (*this-v).sqnorm() < EPS;
		}

		// 直交座標からベクトルを定義する。
		inline static _Vector2D fromCoordinates(T x, T y)
		{
			return _Vector2D(x, y);
		}

		// 長さr, 偏角θのベクトルを定義する。
		inline static _Vector2D fromPolarForm(T r, T theta)
		{
			return _Vector2D(r*std::cos(theta), r*std::sin(theta));
		}
	};

	// 内積を返す。
	template<typename T>
	inline T dot(const _Vector2D<T>& a, const _Vector2D<T>& b)
	{
		return a.x*b.x + a.y*b.y;
	}

	// 外積を返す。2次元ベクトルの場合はスカラ。
	template<typename T>
	inline T cross(const _Vector2D<T>& a, const _Vector2D<T>& b)
	{
		return a.x*b.y - a.y*b.x;
	}

	// 二つのベクトルが成す角(a->b)のsinを返す。
	template<typename T>
	inline T sin(const _Vector2D<T>& a, const _Vector2D<T>& b)
	{
		return cross(a, b) / sqrt(a.sqnorm() * b.sqnorm());
	}

	// 二つのベクトルが成す角(a->b)のcosを返す。
	template<typename T>
	inline T cos(const _Vector2D<T>& a, const _Vector2D<T>& b)
	{
		return dot(a, b) / sqrt(a.sqnorm() * b.sqnorm());
	}

	// 二つのベクトルが成す角(a->b)のtanを返す。
	template<typename T>
	inline T tan(const _Vector2D<T>& a, const _Vector2D<T>& b)
	{
		return cross(a, b) / dot(a, b);
	}

	// 二つのベクトルが成す角(a->b)を(-π, π]の範囲で返す。
	template<typename T>
	inline T arg(const _Vector2D<T>& a, const _Vector2D<T>& b)
	{
		return std::acos(cos(a, b)) * (cross(a, b) < 0.0 ? -1.0 : 1.0);
	}

	// scalar * vector
	template<typename T>
	inline _Vector2D<T> operator* (MEMBER_TYPE k, const _Vector2D<T>& v)
	{
		return _Vector2D<T>(k*v.x, k*v.y);
	}

	// output
	template<typename T>
	inline std::ostream& operator<<(std::ostream& out, const _Vector2D<T>& v)
	{
		return out << '(' << v.x << ", " << v.y << ')';
	}

	template<typename T>
	class _Line2D
	{
	public:
		_Vector2D<T> p; // p : 直線上の一点の位置ベクトル
		_Vector2D<T> d; // d : 直線の方向ベクトル
		
		inline _Line2D() {}
		inline _Line2D(const _Vector2D<T>& p, const _Vector2D<T>& d) : p(p), d(d) {}

		// 指定されたy座標に対応する直線上のx座標を返す。
		inline T getX(T y)const {
			return p.x + (y - p.y)*d.x / d.y;
		}
		
		// 指定されたx座標に対応する直線上のy座標を返す。
		inline T getY(T x)const {
			return p.y + (x - p.x)*d.y / d.x;
		}

		// 直線がx軸と成す角を(-π/2, π/2]の範囲で返す。
		inline T getAngle()const {
			T angle = d.arg();
			return angle + (angle > M_PI_2 ? -M_PI : (angle > -M_PI_2 ? 0.0 : M_PI));
		}

		// 直線の傾きを返す。
		inline T getSlope()const {
			return d.y / d.x;
		}

		// 直線の方程式 ax+by+c=0 の係数a, b, cをそれぞれ引数に代入する。
		inline void getLineEquation(T& a, T& b, T& c)const {
			a = d.y;
			b = -d.x;
			c = cross(d, p);
		}

		inline bool operator==(const _Line2D L)const {
			return std::abs(cross(d, L.d)) < EPS && std::abs(cross(p-L.p, d)) < EPS;
		}

		// 点pを通り、dに平行な直線を定義する。
		inline static _Line2D fromPointAndDirection(const _Vector2D<T>& p, const _Vector2D<T>& d)
		{
			return _Line2D(p, d);
		}

		// 2点pとqを通る直線を定義する。
		inline static _Line2D fromTwoPoints(const _Vector2D<T>& p, const _Vector2D<T>& q)
		{
			return _Line2D(p, q-p);
		}

		// pを通り、x軸と成す角度がθの直線を定義する。
		inline static _Line2D fromPointAndAngle(const _Vector2D<T>& p, T theta)
		{
			return _Line2D(p, _Vector2D<T>::fromPolarForm(1.0, theta));
		}

		// pを通り、傾きがaの直線を定義する。
		inline static _Line2D fromPointAndSlope(const _Vector2D<T>& p, T a)
		{
			return _Line2D(p, _Vector2D<T>(1.0, a));
		}

		// 直線 y=ax+b を定義する。
		inline static _Line2D fromSlopeAndIntercept(T a, T b)
		{
			return fromPointAndSlope(_Vector2D<T>(0.0, b), a);
		}

		// 直線 ax+by+c=0 を定義する。
		inline static _Line2D fromLineEquation(T a, T b, T c)
		{
			if(std::abs(a) > std::abs(b))
				return _Line2D(_Vector2D<T>(-c/a, 0.0), _Vector2D<T>(-b, a));
			else
				return _Line2D(_Vector2D<T>(0.0, -c/b), _Vector2D<T>(b, -a));
		}
	};

	template<typename T>
	class _Ellipse2D
	{
	public:
		_Vector2D<T> center; // 中心座標
		T A, B;              // A : 軸A方向の半長, B : 軸B方向の半長
		T angle;             // 軸Aとx軸の成す角

		inline _Ellipse2D() {}
		inline _Ellipse2D(const _Vector2D<T>& center, T A, T B, T angle) : center(center), A(A), B(B), angle(angle) {}

		// 長軸の半長を返す。
		inline T getHalfMajorLength()const {
			return std::max(A, B);
		}

		// 長軸の全長を返す。
		inline T getMajorLength()const {
			return getHalfMajorLength() * 2.0;
		}

		// 短軸の半長を返す。
		inline T getHalfMinorLength()const {
			return std::min(A, B);
		}

		// 短軸の全長を返す。
		inline T getMinorLength()const {
			return getHalfMinorLength() * 2.0;
		}

		// 長軸の両端の座標を返す。
		inline std::pair<_Vector2D<T>, _Vector2D<T> > getMajorEnds()const {
			_Vector2D<T> tmp;
			if(A < B)
				tmp = B * _Vector2D<T>(-std::sin(angle), std::cos(angle));
			else
				tmp = A * _Vector2D<T>(std::cos(angle), std::sin(angle));
			return std::make_pair(center + tmp, center - tmp);
		}

		// 短軸の両端の座標を返す。
		inline std::pair<_Vector2D<T>, _Vector2D<T> > getMinorEnds()const {
			_Vector2D<T> tmp;
			if(A < B)
				tmp = A * _Vector2D<T>(std::cos(angle), std::sin(angle));
			else
				tmp = B * _Vector2D<T>(-std::sin(angle), std::cos(angle));
			return std::make_pair(center + tmp, center - tmp);
		}

		// 長軸とx軸が成す角を返す。
		inline T getMajorArgument()const {
			if(A < B)
				return angle + (angle < 0.0 ? M_PI_2 : -M_PI_2);
			else
				return angle;
		}

		// 短軸とx軸が成す角を返す。
		inline T getMinorArgument()const {
			if(A < B)
				return angle;
			else
				return angle + (angle < 0.0 ? M_PI_2 : -M_PI_2);
		}

		// 長軸の直線を返す。
		inline _Line2D<T> getMajorLine()const {
			T arg = getMajorArgument();
			return _Line2D<T>(center, _Vector2D<T>(std::cos(arg), std::sin(arg)));
		}
		
		// 短軸の直線を返す。
		inline _Line2D<T> getMinorLine()const {
			T arg = getMinorArgument();
			return _Line2D<T>(center, _Vector2D<T>(std::cos(arg), std::sin(arg)));
		}

		// 楕円が円かどうかを返す。
		inline bool isCircle()const {
			return abs(A-B) < EPS;
		}

		// 面積を返す。
		inline T getArea()const {
			return M_PI * A * B;
		}

		// 楕円弧上で最もx座標が小さい点を返す。
		inline _Vector2D<T> getLeft()const {
			T cos = std::cos(angle);
			T sin = std::sin(angle);
			T x = -_Vector2D<T>(A*cos, B*sin).norm();
			return _Vector2D<T>(x+center.x, (A*A-B*B)*sin*cos/x + center.y);
		}

		// 楕円弧上で最もx座標が大きい点を返す。
		inline _Vector2D<T> getRight()const {
			T cos = std::cos(angle);
			T sin = std::sin(angle);
			T x = _Vector2D<T>(A*cos, B*sin).norm();
			return _Vector2D<T>(x+center.x, (A*A-B*B)*sin*cos/x + center.y);
		}

		// 楕円弧上で最もy座標が小さい点を返す。
		inline _Vector2D<T> getTop()const {
			T cos = std::cos(angle);
			T sin = std::sin(angle);
			T y = -_Vector2D<T>(A*sin, B*cos).norm();
			return _Vector2D<T>((A*A-B*B)*sin*cos/y + center.x, y+center.y);
		}

		// 楕円弧上で最もy座標が大きい点を返す。
		inline _Vector2D<T> getBottom()const {
			T cos = std::cos(angle);
			T sin = std::sin(angle);
			T y = _Vector2D<T>(A*sin, B*cos).norm();
			return _Vector2D<T>((A*A-B*B)*sin*cos/y + center.x, y+center.y);
		}
		
		// 指定されたy座標に対応する直線上のx座標を引数にそれぞれ代入する(x1≦x2)。
		// 戻り値は対応点の個数(0～2)。
		inline int getX(T y, T& x1, T& x2)const {
			T a, b, c, d, e, f;
			getEquation(a, b, c, d, e, f);
			b = b*y + d;
			c = c*y*y + e*y + f;

			T D = b*b - 4.0*a*c;
			if(std::abs(D) < EPS){
				x1 = x2 = -b / (2.0*a);
				return 1;
			}
			else if(D > 0.0){
				D = std::sqrt(D);
				x1 = (-b-D)/(2.0*a);
				x2 = (-b+D)/(2.0*a);
				return 2;
			}
			else
				return 0;
		}

		// 楕円の方程式 ax^2+bxy+cy^2+dx+ey+f=0 の各係数をそれぞれ引数に代入する。
		inline void getEquation(T& a, T& b, T& c, T& d, T& e, T& f)const {
			T cos = std::cos(angle);
			T sin = std::sin(angle);
			T a2 = 1.0 / (A * A);
			T b2 = 1.0 / (B * B);

			a = cos*cos*a2 + sin*sin*b2;
			b = 2.0*(a2-b2)*sin*cos;
			c = sin*sin*a2 + cos*cos*b2;
			d = -2.0*center.x*a - center.y*b;
			e = -2.0*center.y*c - center.x*b;
			T tmp1 =  center.x*cos + center.y*sin;
			T tmp2 = -center.x*sin + center.y*cos;
			f = tmp1*tmp1*a2 + tmp2*tmp2*b2 - 1.0;
		}

		inline bool operator==(const _Ellipse2D& ell)const {
			return center == ell.center
				&& std::abs(getMajorLength() - ell.getMajorLength()) < EPS
				&& std::abs(getMinorLength() - ell.getMinorLength()) < EPS
				&& (std::abs(A-B) < EPS || std::abs(getMajorArgument() - ell.getMajorArgument()));
		}

		// 楕円の中心と軸の半長と角度から楕円を定義する。
		inline static _Ellipse2D fromCenterAndLengthesAndAngle(const _Vector2D<T>& center, T A, T B, T angle)
		{
			return _Ellipse2D(center, A, B, angle);
		}

		// (v.x, v.y), (v.x+w, v.y), (v.x, v.y+h), (v.x+w, v.y+h)を頂点とする長方形に内接する楕円を定義する。
		inline static _Ellipse2D fromRectangle(const _Vector2D<T>& v, T w, T h)
		{
			w *= 0.5;
			h *= 0.5;
			return _Ellipse2D(_Vector2D<T>(v.x + w, v.y + h), w, h, 0.0);
		}

		// 二次曲線 ax^2+bxy+cy^2+dx+ey+f=0 が楕円の方程式かどうかを判別する。
		inline static bool isEllipseEquation(T a, T b, T c, T d, T e, T f)
		{
			T D = 4.0*a*c - b*b;
			return D > 0.0 && a*((d*(b*e-2.0*c*d)+e*(b*d-2.0*a*e))/D + 2.0*f) < 0.0;
		}

		// 楕円の方程式 ax^2+bxy+cy^2+dx+ey+f=0 から楕円を定義する。
		inline static _Ellipse2D fromEquation(T a, T b, T c, T d, T e, T f)
		{
			_Ellipse2D ell;
			ell.center = _Vector2D<T>(b*e-2.0*c*d, b*d-2.0*a*e) / (4.0*a*c - b*b);
			T sum = a + c;
			T diff = a - c;
			T sq = std::sqrt(diff*diff + b*b);
			T beta = -d*ell.center.x - e*ell.center.y - 2.0*f;
			ell.A = std::sqrt(beta/(sum+sq));
			ell.B = std::sqrt(beta/(sum-sq));
			ell.angle = a>c ? std::atan2(b, diff+sq) : std::atan2(-diff+sq, b);
			return ell;
		}
	};

	template<typename T>
	class _Circle2D
	{
	public:
		_Vector2D<T> center; // 中心座標
		T radius;            // 半径

		inline _Circle2D() {}
		inline _Circle2D(const _Vector2D<T>& center, T radius) : center(center), radius(radius) {}

		// 円の方程式 x^2+ax+y^2+by+c=0 の各係数a, b, cをそれぞれ引数に代入する。
		inline void getCircleEquation(T& a, T& b, T& c)const {
			a = -2.0 * center.x;
			b = -2.0 * center.y;
			c = center.sqnorm() - radius*radius;
		}

		// 面積を返す。
		inline T getArea()const {
			return M_PI * radius * radius;
		}

		inline bool operator==(const _Circle2D& C)const {
			return center == C.center && std::abs(radius - C.radius) < EPS;
		}

		// 楕円への変換
		inline operator _Ellipse2D<T>() const{
			return _Ellipse2D<T>::fromCenterAndLengthesAndAngle(center, radius, radius, 0.0);
		}

		// 中心座標と半径から円を定義する。
		inline static _Circle2D fromCenterAndRadius(const _Vector2D<T>& c, T r)
		{
			return _Circle2D(c, r);
		}

		// 中心座標と円周上の一点から円を定義する。
		inline static _Circle2D fromCenterAndPoint(const _Vector2D<T>& c, const _Vector2D<T>& p)
		{
			return _Circle2D(c, (c-p).norm());
		}

		// 指定された二点を直径の両端とする円を定義する。
		inline static _Circle2D fromDiaEnds(const _Vector2D<T>& a, const _Vector2D<T>& b)
		{
			return _Circle2D((a+b)*0.5, (a-b).norm()*0.5);
		}

		// 円周上の三点から円を定義する。
		inline static _Circle2D fromThreePoints(const _Vector2D<T>& a, const _Vector2D<T>& b, const _Vector2D<T>& c)
		{
			_Vector2D<T> A(2.0*(b.x-a.x), 2.0*(c.x-a.x));
			_Vector2D<T> B(2.0*(b.y-a.y), 2.0*(c.y-a.y));
			T asq = a.sqnorm();
			_Vector2D<T> C(asq-b.sqnorm(), asq-c.sqnorm());
			_Vector2D<T> center = Vector2D(cross(B, C), cross(C, A)) / cross(A, B);
			return _Circle2D(center, (center-a).norm());
		}

		// (v.x, v.y), (v.x+a, v.y), (v.x, v.y+a), (v.x+a, v.y+a)を頂点とする正方形に内接する円を定義する。
		inline static _Circle2D fromSquare(const _Vector2D<T>& v, T a)
		{
			a *= 0.5;
			return _Circle2D(_Vector2D<T>(v.x+a, v.y+a), std::abs(a));
		}

		// 円 x^2+ax+y^2+by+c=0 を定義する。
		inline static _Circle2D fromCircleEquation(T a, T b, T c)
		{
			_Vector2D<T> center(-a*0.5, -b*0.5);
			return _Circle2D(center, sqrt(center.sqnorm() - c));
		}
	};

	// 点Pと直線Lの距離を返す。
	template<typename T>
	inline T getDistance(const _Vector2D<T>& P, const _Line2D<T>& L)
	{
		T a, b, c;
		L.getLinearEquation(a, b, c);
		return std::abs(a*P.x + b*P.y + c)/std::sqrt(a*a + b*b);
	}

	// 直線Lと直線Mの交点をinterに代入する。
	// 戻り値は交点の個数(0～1)。aとbが重なっている時は-1。
	template<typename T>
	int getIntersection(const _Line2D<T>& L, const _Line2D<T>& M, _Vector2D<T>& inter)
	{
		T denominator = cross(L.d, M.d);
		T numerator = cross(M.p-L.p, M.d);
		if(std::abs(denominator) < EPS){
			return std::abs(numerator) < EPS ? -1 : 0;
		}
		else{
			inter = L.p + numerator/denominator * L.d;
			return 1;
		}
	}

	// 直線Lと円Cの交点をinter1, inter2に代入する。
	// 戻り値は交点の個数(0～2)。
	template<typename T>
	int getIntersection(const _Line2D<T>& L, const _Circle2D<T>& C, _Vector2D<T>& inter1, _Vector2D<T>& inter2)
	{
		_Vector2D<T> diff = L.p - C.center;
		T a = L.d.sqnorm();
		T b = dot(L.d, diff);
		T c = diff.sqnorm() - C.radius * C.radius;
		T D = b*b - a*c;
		if(std::abs(D) < EPS){
			inter1 = inter2 = L.p - b/a * L.d;
			return 1;
		}
		else if(D > 0.0){
			D = std::sqrt(D);
			inter1 = L.p + (-b+D)/a * L.d;
			inter2 = L.p - (b+D)/a * L.d;
			return 2;
		}
		else{
			return 0;
		}
	}
	
	typedef _Vector2D<MEMBER_TYPE> Vector2D;
	typedef _Line2D<MEMBER_TYPE> Line2D;
	typedef _Circle2D<MEMBER_TYPE> Circle2D;
	typedef _Ellipse2D<MEMBER_TYPE> Ellipse2D;
	
#ifdef __OPENCV_CORE_C_H__
	namespace opencv
	{
		template<typename T>
		inline CvPoint vecToCP(T x, T y) {
			return cvPoint(static_cast<int>(x + 0.5), static_cast<int>(y + 0.5));
		}

		template<typename T>
		inline CvPoint vecToCP(const _Vector2D<T>& v) {
			return vecToCP(v.x, v.y);
		}

		// 点を描く。
		template<typename T>
		inline void drawPoint(CvArr* img, const _Vector2D<T>& point, CvScalar color, int radius=3, int thickness=-1, int lineType=CV_AA, int shift=0)
		{
			cvCircle(img, vecToCP(point), radius, color, thickness, lineType, shift);
		}

		// 直線を描く。
		template<typename T>
		void drawLine(IplImage* img, const _Line2D<T>& line, CvScalar color, int thickness=1, int line_type=8, int shift=0)
		{
			double h = img->height + 1.0, w = img->width + 1.0;
			
			CvPoint p1, p2;
			if(std::abs(line.d.x) < std::abs(line.d.y)){
				p1 = vecToCP(line.getX(-1.0), -1.0);
				p2 = vecToCP(line.getX(h), h);
			}
			else{
				p1 = vecToCP(-1.0, line.getY(-1.0));
				p2 = vecToCP(w, line.getY(w));
			}
			cvLine(img, p1, p2, color, thickness, line_type, shift);
		}

		// 円を描く。
		template<typename T>
		inline void drawCircle(CvArr* img, const _Circle2D<T>& circle, CvScalar color, int thickness=1, int lineType=8, int shift=0)
		{
			cvCircle(img, vecToCP(circle.center), static_cast<int>(circle.radius + 0.5), color, thickness, lineType, shift);
		}

		// 楕円を描く。
		template<typename T>
		inline void drawEllipse(CvArr* img, const _Ellipse2D<T>& ellipse, CvScalar color, int thickness=1, int lineType=8, int shift=0)
		{
			cvEllipse(
				img,
				vecToCP(ellipse.center),
				cvSize(static_cast<int>(ellipse.A + 0.5), static_cast<int>(ellipse.B + 0.5)),
				radToDeg(ellipse.angle), 0.0, 360.0,
				color, thickness, lineType, shift
				);
		}
	}
#endif
}

#endif