#include "eyemLib.h"

void eyemClp2dDistanceTwoPoints(EyemOcsDXY tpPoint1, EyemOcsDXY tpPoint2, double &tpDist)
{
	tpDist = std::sqrt(std::pow(tpPoint2.dX - tpPoint1.dX, 2) + std::pow(tpPoint2.dY - tpPoint1.dY, 2));
}

void eyemClp2dCenterTwoPoints(EyemOcsDXY tpPoint1, EyemOcsDXY tpPoint2, EyemOcsDXY &tpCenter)
{
	tpCenter.dX = (tpPoint1.dX + tpPoint2.dX) / 2.0;
	tpCenter.dY = (tpPoint1.dY + tpPoint2.dY) / 2.0;
}

int	eyemClp2dLineTwoPoints(EyemOcsDXY tpPoint1, EyemOcsDXY tpPoint2, EyemOcsDABC &tpLine)
{
	if (std::abs(tpPoint1.dX - tpPoint2.dX) < EPS && std::abs(tpPoint1.dY - tpPoint2.dY) < EPS) {
		return FUNC_CANNOT_CALC;
	}
	tpLine.dA = -(tpPoint1.dY - tpPoint2.dY);
	tpLine.dB = (tpPoint1.dX - tpPoint2.dX);
	tpLine.dC = tpPoint2.dX*tpPoint1.dY - tpPoint1.dX*tpPoint2.dY;
	return FUNC_OK;
}

int	eyemClp2dMidperpendicularTwoPoints(EyemOcsDXY tpPoint1, EyemOcsDXY tpPoint2, EyemOcsDABC &tpLine)
{
	if (std::abs(tpPoint1.dX - tpPoint2.dX) < EPS && std::abs(tpPoint1.dY - tpPoint2.dY) < EPS) {
		return FUNC_CANNOT_CALC;
	}
	tpLine.dA = 2.0 * (tpPoint2.dX - tpPoint1.dX);
	tpLine.dB = 2.0 * (tpPoint2.dY - tpPoint1.dY);
	tpLine.dC = pow(tpPoint1.dX, 2) - pow(tpPoint2.dX, 2) + pow(tpPoint1.dY, 2) - pow(tpPoint2.dY, 2);
	return FUNC_OK;
}

int	eyemClp2dVerticalLinePointAndLine(EyemOcsDXY tpPoint, EyemOcsDABC tpLine, EyemOcsDABC &tpVertical)
{
	double v = std::sqrt(std::pow(tpLine.dB, 2) + std::pow(-tpLine.dA, 2));
	if (v < EPS) {
		return FUNC_CANNOT_CALC;
	}
	tpVertical.dA = tpLine.dB / v;
	tpVertical.dB = -tpLine.dA / v;
	tpVertical.dC = -tpVertical.dA*tpPoint.dX - tpVertical.dB*tpPoint.dY;
	return FUNC_OK;
}

void eyemClp2dLinePointAndSlope(EyemOcsDXY tpPoint, double dSlope, EyemOcsDABC &tpLine)
{
	tpLine.dA = -sin(dSlope*PI / 180.0);
	tpLine.dB = cos(dSlope*PI / 180.0);
	tpLine.dC = -tpLine.dA*tpPoint.dX - tpLine.dB*tpPoint.dY;
}

int	eyemClp2dIntersectionTwoLines(EyemOcsDABC tpLine1, EyemOcsDABC tpLine2, EyemOcsDXY &tpPoint)
{
	double det = tpLine1.dA*tpLine2.dB - tpLine2.dA*tpLine1.dB;
	if (abs(det) < EPS) {
		return FUNC_CANNOT_CALC;
	}
	tpPoint.dX = (tpLine1.dB*tpLine2.dC - tpLine2.dB*tpLine1.dC) / det;
	tpPoint.dY = (tpLine2.dA*tpLine1.dC - tpLine1.dA*tpLine2.dC) / det;
	return FUNC_OK;
}

int eyemClp2dIntersectionLineSegment(EyemOcsDABC tpLine, EyemOcsDXY tpPtSt, EyemOcsDXY tpPtEd, int *ipStatus, EyemOcsDXY &tpPoint)
{
	if ((tpPtSt.dX - tpPtEd.dX) + (tpPtSt.dY - tpPtEd.dY) < EPS) {
		return FUNC_CANNOT_CALC;
	}
	double d1, d2;
	d1 = abs(tpLine.dA*tpPtSt.dX + tpLine.dB*tpPtSt.dY + tpLine.dC) / sqrt(tpLine.dA*tpLine.dA + tpLine.dB*tpLine.dB);
	if (abs(tpLine.dB) < FLT_EPSILON) {
		d1 = tpPtSt.dX < -tpLine.dC / tpLine.dA ? -d1 : d1;
	}
	else {
		d1 = tpPtSt.dY >= ((-tpLine.dA / tpLine.dB)*tpPtSt.dX - tpLine.dC / tpLine.dB) ? d1 : -d1;
	}

	d2 = abs(tpLine.dA*tpPtEd.dX + tpLine.dB*tpPtEd.dY + tpLine.dC) / sqrt(tpLine.dA*tpLine.dA + tpLine.dB*tpLine.dB);
	if (abs(tpLine.dB) < FLT_EPSILON) {
		d2 = tpPtEd.dX < -tpLine.dC / tpLine.dA ? -d2 : d2;
	}
	else {
		d2 = tpPtEd.dY >= ((-tpLine.dA / tpLine.dB)*tpPtEd.dX - tpLine.dC / tpLine.dC) ? d2 : -d2;
	}
	//异号或者为0则交点存在
	if (d1*d2 <= 0) {
		d1 = abs(d1); d2 = abs(d2);
		double k = d1 / (d1 + d2);
		if ((d1 + d2 != 0) && ((tpPtSt.dX + k*(tpPtEd.dX - tpPtSt.dX) != tpPtSt.dX) || ((tpPtSt.dY + k*(tpPtEd.dY - tpPtSt.dY)) != tpPtSt.dY))) {
			tpPoint.dX = tpPtSt.dX + k*(tpPtEd.dX - tpPtSt.dX); tpPoint.dY = tpPtSt.dY + k*(tpPtEd.dY - tpPtSt.dY);
			*ipStatus = EYEM_CG_INTERSECTION;
		}
		else if (d1 + d2 == 0) {
			*ipStatus = EYEM_CG_SEG1_OVERLAP_SGM2;
		}
	}
	else {
		*ipStatus = EYEM_CG_NOT_INTERSECTION;
	}
	return FUNC_OK;
}

int	eyemClp2dIntersectionOfTwoSegments(EyemOcsDXY tpPt1St, EyemOcsDXY tpPt1Ed, EyemOcsDXY tpPt2St, EyemOcsDXY tpPt2Ed, int *ipStatus, EyemOcsDXY &tpPoint)
{

	return FUNC_OK;
}

int	eyemClp2dAngleTwoLines(EyemOcsDABC tpLine1, EyemOcsDABC tpLine2, double &dpAngle)
{
	if (abs(tpLine1.dA*tpLine2.dB - tpLine2.dA*tpLine1.dB) < EPS) {
		return FUNC_CANNOT_CALC;
	}
	double u, v, uv;
	u = sqrt(pow(tpLine1.dA, 2) + pow(tpLine1.dB, 2));
	v = sqrt(pow(tpLine2.dA, 2) + pow(tpLine2.dB, 2));
	uv = sqrt(pow(tpLine1.dA*tpLine2.dB - tpLine2.dA*tpLine1.dB, 2));
	dpAngle = std::asin(uv / u*v)*180.0 / PI;
	return FUNC_OK;
}

int	eyemClp2dCenterLineOfTwoLines(EyemOcsDABC tpLine1, EyemOcsDABC tpLine2, EyemOcsDABC &tpLineC)
{
	double la, lb, lc;
	la = tpLine1.dA / (sqrt(pow(tpLine1.dA, 2) + pow(tpLine1.dB, 2))) + tpLine2.dA / (sqrt(pow(tpLine2.dA, 2) + pow(tpLine2.dB, 2)));

	lb = tpLine1.dB / (sqrt(pow(tpLine1.dA, 2) + pow(tpLine1.dB, 2))) + tpLine2.dB / (sqrt(pow(tpLine2.dA, 2) + pow(tpLine2.dB, 2)));

	lc = tpLine1.dC / (sqrt(pow(tpLine1.dA, 2) + pow(tpLine1.dB, 2))) + tpLine2.dC / (sqrt(pow(tpLine2.dA, 2) + pow(tpLine2.dB, 2)));

	tpLineC.dA = la / (sqrt(pow(la, 2) + pow(lb, 2)));
	tpLineC.dB = lb / (sqrt(pow(la, 2) + pow(lb, 2)));
	tpLineC.dC = lc / (sqrt(pow(la, 2) + pow(lb, 2)));
	return FUNC_OK;
}

int	eyemClp2dDistancePointToLine(EyemOcsDXY tpPoint, EyemOcsDABC tpLine, double &dpDist)
{
	dpDist = std::abs(tpLine.dA*tpPoint.dX + tpLine.dB*tpPoint.dY + tpLine.dC) / sqrt(tpLine.dA*tpLine.dA + tpLine.dB*tpLine.dB);
	return FUNC_OK;
}

int	eyemClp2dTranslationOfLine(EyemOcsDABC tpSrcL, EyemOcsDXY tpTrans, EyemOcsDABC &tpDstL)
{
	tpDstL.dA = tpSrcL.dA, tpDstL.dB = tpSrcL.dB;
	tpDstL.dC = tpSrcL.dC - tpSrcL.dA*tpTrans.dX - tpSrcL.dB*tpTrans.dY;
	return FUNC_OK;
}

void eyemClp2dAreaTriangle(EyemOcsDXY tpPoint1, EyemOcsDXY tpPoint2, EyemOcsDXY tpPoint3, double &dpArea)
{
	dpArea = ((tpPoint2.dX - tpPoint1.dX)*(tpPoint3.dY - tpPoint1.dY) - ((tpPoint2.dY - tpPoint1.dY)*(tpPoint3.dX - tpPoint1.dX))) / 2.0;
}

int	eyemClp2dCircleThreePoints(EyemOcsDXY tpPoint1, EyemOcsDXY tpPoint2, EyemOcsDXY tpPoint3, EyemOcsDXYR &tpCircle)
{
	//计算系数
	double sa[9], sb[3], sx[3];
	cv::Mat a = cv::Mat(3, 3, CV_64F, sa), b = cv::Mat(3, 1, CV_64F, sb);
	//系数矩阵A*X=B;
	cv::Mat x = cv::Mat(3, 1, CV_64F, sx);

	std::memset(sa, 0, sizeof(sa));
	std::memset(sb, 0, sizeof(sb));
	std::memset(sx, 0, sizeof(sx));

	sa[0] = -2.0*tpPoint1.dX, sa[1] = -2.0*tpPoint1.dY, sa[2] = 1.;
	sa[3] = -2.0*tpPoint2.dX, sa[4] = -2.0*tpPoint2.dY, sa[5] = 1.;
	sa[6] = -2.0*tpPoint3.dX, sa[7] = -2.0*tpPoint3.dY, sa[8] = 1.;

	sb[0] = -(pow(tpPoint1.dX, 2) + pow(tpPoint1.dY, 2)),
		sb[1] = -(pow(tpPoint2.dX, 2) + pow(tpPoint2.dY, 2)),
		sb[2] = -(pow(tpPoint3.dX, 2) + pow(tpPoint3.dY, 2));
	//solve
	cv::solve(a, b, x, cv::DECOMP_SVD);

	tpCircle.dX = sx[0];
	tpCircle.dY = sx[1];
	tpCircle.dR = sqrt(pow(sx[0], 2) + pow(sx[1], 2) - (sx[2]));
	return FUNC_OK;
}

int eyemClp2dIntersectionLineAndCircle(EyemOcsDABC tpLine, EyemOcsDXYR tpCircle, EyemOcsDXY &tpPoint1, EyemOcsDXY &tpPoint2)
{
	double dist = abs(tpLine.dA*tpCircle.dX + tpLine.dB*tpCircle.dY + tpLine.dC) / sqrt(tpLine.dA*tpLine.dA + tpLine.dB*tpLine.dB);
	if (dist > tpCircle.dR) {
		return FUNC_CANNOT_CALC;
	}
	double dt = tpCircle.dR*tpCircle.dR*(1 + (-tpLine.dA / tpLine.dB)*(-tpLine.dA / tpLine.dB)) - std::pow((tpCircle.dY -
		(-tpLine.dA / tpLine.dB)*tpCircle.dX - (-tpLine.dC / tpLine.dB)), 2);

	tpPoint1.dX = ((tpCircle.dX + tpCircle.dY*(-tpLine.dA / tpLine.dB)) - (-tpLine.dC / tpLine.dB)*(-tpLine.dA / tpLine.dB) + sqrt(dt)) / (1 + std::pow(-tpLine.dA / tpLine.dB, 2));
	tpPoint1.dY = ((-tpLine.dC / tpLine.dB) + tpCircle.dX*(-tpLine.dA / tpLine.dB) + tpCircle.dY*std::pow(-tpLine.dA / tpLine.dB, 2) + (-tpLine.dA / tpLine.dB)*sqrt(dt)) / (1 + std::pow(-tpLine.dA / tpLine.dB, 2));
	tpPoint2.dX = ((tpCircle.dX + tpCircle.dY*(-tpLine.dA / tpLine.dB)) - (-tpLine.dC / tpLine.dB)*(-tpLine.dA / tpLine.dB) - sqrt(dt)) / (1 + std::pow(-tpLine.dA / tpLine.dB, 2));
	tpPoint2.dY = ((-tpLine.dC / tpLine.dB) + tpCircle.dX*(-tpLine.dA / tpLine.dB) + tpCircle.dY*std::pow(-tpLine.dA / tpLine.dB, 2) - (-tpLine.dA / tpLine.dB)*sqrt(dt)) / (1 + std::pow(-tpLine.dA / tpLine.dB, 2));
	return FUNC_OK;
}

int eyemClp2dTangentPointToCircle(EyemOcsDXY tpPoint, EyemOcsDXYR tpCircle, EyemOcsDABC &tpTangent1, EyemOcsDXY &tpContact1, EyemOcsDABC &tpTangent2, EyemOcsDXY &tpContact2)
{
	double dist = std::sqrt(std::pow(tpCircle.dX - tpPoint.dX, 2) + std::pow(tpCircle.dY - tpPoint.dY, 2));
	if (dist < tpCircle.dR) {
		return FUNC_CANNOT_CALC;
	}
	else if (abs(dist - tpCircle.dR) < EPS) {
		//点在圆上
		tpContact1.dX = tpContact2.dX = tpPoint.dX;
		tpContact1.dY = tpContact2.dY = tpPoint.dY;

		tpTangent1.dA = tpTangent2.dA = tpPoint.dX - tpCircle.dX;
		tpTangent1.dB = tpTangent2.dB = tpPoint.dY - tpCircle.dY;
		tpTangent1.dC = tpTangent2.dC = -(tpCircle.dR*tpCircle.dR + tpCircle.dX*(tpPoint.dX - tpCircle.dX) + tpCircle.dY*(tpPoint.dY - tpCircle.dY));
		return FUNC_OK;
	}
	tpContact1.dX = (tpCircle.dR*tpCircle.dR*(tpPoint.dX - tpCircle.dX) + tpCircle.dR*(tpPoint.dY - tpCircle.dY)*sqrt(std::pow(tpPoint.dX - tpCircle.dX, 2)
		+ std::pow(tpPoint.dY - tpCircle.dY, 2) - tpCircle.dR*tpCircle.dR)) / (std::pow(tpPoint.dX - tpCircle.dX, 2) + std::pow(tpPoint.dY - tpCircle.dY, 2)) + tpCircle.dX;
	tpContact1.dY = (tpCircle.dR*tpCircle.dR*(tpPoint.dY - tpCircle.dY) - tpCircle.dR*(tpPoint.dX - tpCircle.dX)*sqrt(std::pow(tpPoint.dX - tpCircle.dX, 2)
		+ std::pow(tpPoint.dY - tpCircle.dY, 2) - tpCircle.dR*tpCircle.dR)) / (std::pow(tpPoint.dX - tpCircle.dX, 2) + std::pow(tpPoint.dY - tpCircle.dY, 2)) + tpCircle.dY;
	tpTangent1.dA = -(tpPoint.dY - tpContact1.dY); tpTangent1.dB = (tpPoint.dX - tpContact1.dX); tpTangent1.dC = tpContact1.dX*tpPoint.dY -
		tpPoint.dX*tpContact1.dY;

	tpContact2.dX = (tpCircle.dR*tpCircle.dR*(tpPoint.dX - tpCircle.dX) - tpCircle.dR*(tpPoint.dY - tpCircle.dY)*sqrt(std::pow(tpPoint.dX - tpCircle.dX, 2)
		+ std::pow(tpPoint.dY - tpCircle.dY, 2) - tpCircle.dR*tpCircle.dR)) / (std::pow(tpPoint.dX - tpCircle.dX, 2) + std::pow(tpPoint.dY - tpCircle.dY, 2)) + tpCircle.dX;
	tpContact2.dY = (tpCircle.dR*tpCircle.dR*(tpPoint.dY - tpCircle.dY) + tpCircle.dR*(tpPoint.dX - tpCircle.dX)*sqrt(std::pow(tpPoint.dX - tpCircle.dX, 2)
		+ std::pow(tpPoint.dY - tpCircle.dY, 2) - tpCircle.dR*tpCircle.dR)) / (std::pow(tpPoint.dX - tpCircle.dX, 2) + std::pow(tpPoint.dY - tpCircle.dY, 2)) + tpCircle.dY;
	tpTangent2.dA = -(tpPoint.dY - tpContact2.dY); tpTangent2.dB = (tpPoint.dX - tpContact2.dX); tpTangent2.dC = tpContact2.dX*tpPoint.dY -
		tpPoint.dX*tpContact2.dY;
	return FUNC_OK;
}