#include <back/threadPoolManager.h>
#include <halconAlgo/Correction.h>
#include <iostream>
#include <simpleTools/eulerCoordinateAlgo.h>
#include <appCore.hpp>
struct CorrectionPoints
{
	double OriginZ, OriginX, OriginY;
	std::vector<double> Origin;//X Y Z
	std::vector<double> Result_X;//X Y Z
	std::vector<double> Result_Y;//X Y Z
	std::vector<double> Result_Z;//X Y Z

	double AngleZX = -1;
	double AngleYZ = -1;
	double AngleXY = -1;

	double offsetX1, offsetX2, offsetX3;
	double offsetY1, offsetY2, offsetY3;
	double offsetZ1, offsetZ2, offsetZ3;
};
using namespace cv;
void calculateRotationMatrix(const CorrectionPoints& points, double rotationMatrix[3][3]) {
	rotationMatrix[0][0] = points.offsetX1;
	rotationMatrix[0][1] = points.offsetY1;
	rotationMatrix[0][2] = points.offsetZ1;
	rotationMatrix[1][0] = points.offsetX2;
	rotationMatrix[1][1] = points.offsetY2;
	rotationMatrix[1][2] = points.offsetZ2;
	rotationMatrix[2][0] = points.offsetX3;
	rotationMatrix[2][1] = points.offsetY3;
	rotationMatrix[2][2] = points.offsetZ3;
}

void extractRPY(const double rotationMatrix[3][3], double& roll, double& pitch, double& yaw) {
	// 计算pitch
	pitch = atan2(-rotationMatrix[2][0], sqrt(rotationMatrix[2][1] * rotationMatrix[2][1] + rotationMatrix[2][2] * rotationMatrix[2][2])) * (180.0 / M_PI);

	// 计算yaw
	yaw = atan2(rotationMatrix[1][0], rotationMatrix[0][0]) * (180.0 / M_PI);

	// 计算roll
	roll = atan2(rotationMatrix[2][1], rotationMatrix[2][2]) * (180.0 / M_PI);


}
Point3d getRPY(CorrectionPoints& oldPoints)
{
	double rotationMatrix[3][3];
	calculateRotationMatrix(oldPoints, rotationMatrix);
	double roll, pitch, yaw;
	extractRPY(rotationMatrix, roll, pitch, yaw);

	return Point3d(roll, pitch, yaw);
}

Point3d getRPYOffset(CorrectionPoints& oldPoints, CorrectionPoints& newPoints)
{
	double rotationMatrix[3][3];
	calculateRotationMatrix(oldPoints, rotationMatrix);
	double roll, pitch, yaw;
	extractRPY(rotationMatrix, roll, pitch, yaw);

	double rotationMatrix2[3][3];
	calculateRotationMatrix(newPoints, rotationMatrix2);
	double roll2, pitch2, yaw2;
	extractRPY(rotationMatrix2, roll2, pitch2, yaw2);

	double resultRoll = roll - roll2;
	double resultPitch = pitch - pitch2;
	double resultYaw = yaw - yaw2;
	return Point3d(resultRoll, resultPitch, resultYaw);
}

Point3d getOriginPointOffset(CorrectionPoints& oldPoints, CorrectionPoints& newPoints)
{
	return Point3d(oldPoints.Origin[0] - newPoints.Origin[0],
		oldPoints.Origin[1] - newPoints.Origin[1],
		oldPoints.Origin[2] - newPoints.Origin[2]);
}

bool HalconCorrection(correctionStruct* correctionData, CorrectionPoints& result)
{
#pragma region XOY_DEFINE
	HTuple hv_in_X_X0Y;
	for (auto rangeData : correctionData->in_X_X0Y)
		hv_in_X_X0Y.Append(rangeData);

	HTuple hv_in_Y_X0Y;
	for (auto rangeData : correctionData->in_Y_X0Y)
		hv_in_Y_X0Y.Append(rangeData);

	HTuple hv_in_Z_X0Y;
	for (auto rangeData : correctionData->in_Z_X0Y)
		hv_in_Z_X0Y.Append(rangeData);

	HTuple hv_in_Z_X0Y_Value;
	for (auto rangeData : correctionData->in_Z_X0Y_Value)
		hv_in_Z_X0Y_Value.Append(rangeData);
#pragma endregion

#pragma region XOZ_DEFINE
	HTuple hv_in_X_X0Z;
	for (auto rangeData : correctionData->in_X_X0Z)
		hv_in_X_X0Z.Append(rangeData);

	HTuple hv_in_Y_X0Z;
	for (auto rangeData : correctionData->in_Y_X0Z)
		hv_in_Y_X0Z.Append(rangeData);

	HTuple hv_in_Z_X0Z;
	for (auto rangeData : correctionData->in_Z_X0Z)
		hv_in_Z_X0Z.Append(rangeData);

	HTuple hv_in_Y_X0Z_Value;
	for (auto rangeData : correctionData->in_Y_X0Z_Value)
		hv_in_Y_X0Z_Value.Append(rangeData);
#pragma endregion

#pragma region YOZ_DEFINE
	HTuple hv_in_X_Y0Z;
	for (auto rangeData : correctionData->in_X_Y0Z)
		hv_in_X_Y0Z.Append(rangeData);

	HTuple hv_in_Y_Y0Z;
	for (auto rangeData : correctionData->in_Y_Y0Z)
		hv_in_Y_Y0Z.Append(rangeData);

	HTuple hv_in_Z_Y0Z;
	for (auto rangeData : correctionData->in_Z_Y0Z)
		hv_in_Z_Y0Z.Append(rangeData);

	HTuple hv_in_X_Y0Z_Value;
	for (auto rangeData : correctionData->in_X_Y0Z_Value)
		hv_in_X_Y0Z_Value.Append(rangeData);
#pragma endregion
	HTuple hv_out_Origin;
	HTuple hv_out_Result_X;
	HTuple hv_out_Result_Y;
	HTuple hv_out_Result_Z;
	HTuple hv_out_AngleZX;
	HTuple hv_out_AngleYZ;
	HTuple hv_out_AngleXY;
	HTuple hv_out_Flag;
	halcon_Correction(
		hv_in_X_X0Y, hv_in_Y_X0Y, hv_in_Z_X0Y, hv_in_Z_X0Y_Value,
		hv_in_X_X0Z, hv_in_Y_X0Z, hv_in_Z_X0Z, hv_in_Y_X0Z_Value,
		hv_in_X_Y0Z, hv_in_Y_Y0Z, hv_in_Z_Y0Z, hv_in_X_Y0Z_Value,
		&hv_out_Origin, &hv_out_Result_X, &hv_out_Result_Y, &hv_out_Result_Z,
		&hv_out_AngleZX, &hv_out_AngleYZ, &hv_out_AngleXY, &hv_out_Flag);

	int ok_Flag = 0;

	if (hv_out_Flag.Length() == 1)
		if (hv_out_Flag[0].D() == 1)
		{
			if (hv_out_Origin.Length() == 3)
			{
				result.Origin = { hv_out_Origin[0].D(),hv_out_Origin[1].D(), hv_out_Origin[2].D() };
				ok_Flag++;
			}
			if (hv_out_Result_X.Length() == 3)
			{
				result.Result_X = { hv_out_Result_X[0].D(),hv_out_Result_X[1].D(), hv_out_Result_X[2].D() };
				ok_Flag++;
			}
			if (hv_out_Result_Y.Length() == 3)
			{
				result.Result_Y = { hv_out_Result_Y[0].D(),hv_out_Result_Y[1].D(), hv_out_Result_Y[2].D() };
				ok_Flag++;
			}
			if (hv_out_Result_Z.Length() == 3)
			{
				result.Result_Z = { hv_out_Result_Z[0].D(),hv_out_Result_Z[1].D(), hv_out_Result_Z[2].D() };
				ok_Flag++;
			}
			if (hv_out_AngleZX.Length() == 1)
			{
				result.AngleZX = hv_out_AngleZX[0].D();
				ok_Flag++;
			}
			if (hv_out_AngleYZ.Length() == 1)
			{
				result.AngleYZ = hv_out_AngleYZ[0].D();
				ok_Flag++;
			}
			if (hv_out_AngleXY.Length() == 1)
			{
				result.AngleXY = hv_out_AngleXY[0].D();
				ok_Flag++;
			}
		}
		else
		{
			std::cout << "error";
		}

	if (ok_Flag != 7)
	{
		//qDebug()
		return false;
	}
	result.offsetX1 = result.Result_X[0] - result.Origin[0];
	result.offsetX2 = result.Result_X[1] - result.Origin[1];
	result.offsetX3 = result.Result_X[2] - result.Origin[2];

	result.offsetY1 = result.Result_Y[0] - result.Origin[0];
	result.offsetY2 = result.Result_Y[1] - result.Origin[1];
	result.offsetY3 = result.Result_Y[2] - result.Origin[2];

	result.offsetZ1 = result.Result_Z[0] - result.Origin[0];
	result.offsetZ2 = result.Result_Z[1] - result.Origin[1];
	result.offsetZ3 = result.Result_Z[2] - result.Origin[2];


	return true;
}

void test()
{
	correctionStruct* correctionData1 = new correctionStruct();
	correctionData1->in_X_X0Y = { 344.7,503.1,684.51 };
	correctionData1->in_Y_X0Y = { -1213.82,-1313.89,-1213.83 };
	correctionData1->in_Z_X0Y = { -130.5,-130.57,-130.47 };
	correctionData1->in_Z_X0Y_Value = { -77.8479,-78.1521,-76.2918 };

	correctionData1->in_X_X0Z = { 615.22,197.49,197.35 };
	correctionData1->in_Y_X0Z = { -372.67,-372.84,-372.88 };
	correctionData1->in_Z_X0Z = { -922.86,-933.43,-1004.15 };
	correctionData1->in_Y_X0Z_Value = { -74.1,-77.8791,-78.1404 };

	correctionData1->in_X_Y0Z = { -213.84,-213.9,-213.85 };
	correctionData1->in_Y_Y0Z = { -1007.81,-1072.33,-999.51 };
	correctionData1->in_Z_Y0Z = { -601.31,-601.34,-693.32 };
	correctionData1->in_X_Y0Z_Value = { 81.1512,80.4492,81.0303 };

	correctionStruct* correctionData2 = new correctionStruct();
	correctionData2->in_X_X0Y = { 344.7,503.1,684.49 };
	correctionData2->in_Y_X0Y = { -1213.81,-1313.8,-1213.81 };
	correctionData2->in_Z_X0Y = { -130.5,-130.58,-130.46 };
	correctionData2->in_Z_X0Y_Value = { -77.8479,-78.1794,-76.3074 };

	correctionData2->in_X_X0Z = { 615.2,197.49,197.38 };
	correctionData2->in_Y_X0Z = { -372.67,-372.84,-372.87 };
	correctionData2->in_Z_X0Z = { -922.86,-933.44,-1004.16 };
	correctionData2->in_Y_X0Z_Value = { -74.139,-77.8869,-78.1404 };

	correctionData2->in_X_Y0Z = { -213.84,-213.9,-213.86 };
	correctionData2->in_Y_Y0Z = { -1007.81,-1072.32,-999.51 };
	correctionData2->in_Z_Y0Z = { -601.31,-601.35,-693.32 };
	correctionData2->in_X_Y0Z_Value = { 81.1083,80.4141,80.9913 };
	CorrectionPoints oldPoints;
	HalconCorrection(correctionData1, oldPoints);
	CorrectionPoints newPoints;
	HalconCorrection(correctionData2, newPoints);
	//{
		Point3d rotationAnglesx = getRPY(oldPoints);
		std::cout << "ox:" << oldPoints.Origin[0] << "oy:" << oldPoints.Origin[1] << "oz:" << oldPoints.Origin[2] << std::endl;
		std::cout << "Rx:" << rotationAnglesx.x << "Ry:" << rotationAnglesx.y << "Rz:" << rotationAnglesx.z << std::endl;
	//}
	//{
		Point3d rotationAnglesx2 = getRPY(newPoints);
		std::cout << "ox:" << newPoints.Origin[0] << "oy:" << newPoints.Origin[1] << "oz:" << newPoints.Origin[2] << std::endl;
		std::cout << "Rx:" << rotationAnglesx2.x << "Ry:" << rotationAnglesx2.y << "Rz:" << rotationAnglesx2.z << std::endl;
	//}
	 Point3d rotationAngles = getRPYOffset(oldPoints, newPoints);
	 double offsetX = oldPoints.Origin[0] - newPoints.Origin[0];
	 double offsetY = oldPoints.Origin[1] - newPoints.Origin[1];
	 double offsetZ = oldPoints.Origin[2] - newPoints.Origin[2];

	 double offsetRx = rotationAnglesx2.x - rotationAnglesx.x;
	 double offsetRy = rotationAnglesx2.y - rotationAnglesx.y;
	 double offsetRz = rotationAnglesx2.z - rotationAnglesx.z;

	 //cv::Vec3d radIn1({ degToRad(rotationAnglesx.x), degToRad(rotationAnglesx.y), degToRad(rotationAnglesx.z) });
	 //cv::Vec3d radIn2({ degToRad(rotationAnglesx2.x), degToRad(rotationAnglesx2.y), degToRad(rotationAnglesx2.z) });
	 cv::Vec3d radIn1({0,0,0});
	 cv::Vec3d radIn2({ degToRad(offsetRx),degToRad(offsetRy),degToRad(offsetRz) });
	 // 计算新的坐标
	 cv::Mat coords = (cv::Mat_<double>(3, 1) << oldPoints.Origin[0], oldPoints.Origin[1], oldPoints.Origin[2]);
	 cv::Mat newCoords = transform_coordinates(coords, radIn1, radIn2);
	 std::cout << "New coordinates: (" << newCoords.at<double>(0, 0) << ", " << newCoords.at<double>(1, 0) << ", " << newCoords.at<double>(2, 0) << ")" << std::endl;
	 cv::Mat coordsOffset = (cv::Mat_<double>(3, 1) << coords.at<double>(0, 0) - newCoords.at<double>(0, 0), coords.at<double>(1, 0) - newCoords.at<double>(1, 0), coords.at<double>(2, 0) - newCoords.at<double>(2, 0));
	 std::cout << "offset: (" << coordsOffset.at<double>(0, 0) << ", " << coordsOffset.at<double>(1, 0) << ", " << coordsOffset.at<double>(2, 0) << ")" << std::endl;
	//Points newPoint = applyAngleDifference(angleDifference, point3);
	//std::vector<cv::Point3d> result=getoffsetPos(oldPoints, oldPoints);
	QThread::msleep(100);
}

void createModbusStrs(const int startPos,double& Value, std::shared_ptr <detData> data)
{
	data->writeBack("short", startPos, QString::number(Value < 0 ? 2 : 1));
	data->writeBack("short", startPos + 1, QString::number(abs(static_cast<int>(Value))));
	double pvalue = Value - static_cast<int>(Value);
	data->writeBack("short", startPos + 2, QString::number(static_cast<int>(abs(pvalue * 100))));
}

correctionStruct* correctionData1x = new correctionStruct();
double doubleLmit(double input, int num)
{
	std::ostringstream out;
	out << std::fixed << std::setprecision(num) << input;
	return std::stod(out.str());
}
bool simpleCorrection(correctionStruct* correctionData, CorrectionPoints& result)
{
	//Z
	{
		double sum = 0;
		for (auto zData : correctionData->in_Z_X0Y)
			sum += zData;
		for (auto zData : correctionData->in_Z_X0Y_Value)
		{
			if(zData > -100)
				sum += zData;
		}
		result.OriginZ = sum / 3;
	}
	//X
	{
		double sum = 0;
		for (auto xData : correctionData->in_X_Y0Z)
			sum += xData;
		for (auto xData : correctionData->in_X_Y0Z_Value)
		{
			if (xData > -100)
				sum += xData;
		}
		result.OriginX = sum / 3;
	}
	//Y
	{
		double sum = 0;
		for (auto yData : correctionData->in_Y_X0Z)
			sum += yData;
		for (auto yData : correctionData->in_Y_X0Z_Value)
			if (yData > -100)
				sum += yData;
		result.OriginY = sum / 3;
	}
	if (result.OriginZ < -1000 || result.OriginX < -1000 || result.OriginY < -1000)
		return false;
	return true;
}
double offsetLimit(double valueIn,double offsetLimitIn)
{
	valueIn = (valueIn < -offsetLimitIn) ? -offsetLimitIn : valueIn;
	valueIn = (valueIn > offsetLimitIn) ? offsetLimitIn : valueIn;
	return valueIn;
}
void ALGO_Correction(std::shared_ptr <detData> data)
{
	//data->writeBack("short", 274, "1");
	//return;
	CorrectionPoints resultPoints;
	bool correctionResult = simpleCorrection(&data->laserData, resultPoints);
	std::cout << "correctionResult_X" << resultPoints.OriginX << std::endl;
	std::cout << "correctionResult_Y" << resultPoints.OriginY << std::endl;
	std::cout << "correctionResult_Z" << resultPoints.OriginZ << std::endl;

	if (correctionResult == false)
	{
		data->writeBack("short", 274, "1");
		return;
	}
	Point3d point = getRPY(resultPoints); 
	auto currentProductConfig = _core._appConfig._switchProductWidgetDataPtr.getCurrentProductData();
	//json中记录的基础偏移
	double baseOffset_X = currentProductConfig._correctionConfigStruct.baseOffset_X;
	double baseOffset_Y = currentProductConfig._correctionConfigStruct.baseOffset_Y;
	double baseOffset_Z = currentProductConfig._correctionConfigStruct.baseOffset_Z;

	double correction_X = currentProductConfig._correctionConfigStruct.correction_X;
	double correction_Y = currentProductConfig._correctionConfigStruct.correction_Y;
	double correction_Z = currentProductConfig._correctionConfigStruct.correction_Z;

	double offsetLimit_X = currentProductConfig._correctionConfigStruct.offsetLimit_X;
	double offsetLimit_Y = currentProductConfig._correctionConfigStruct.offsetLimit_Y;
	double offsetLimit_Z = currentProductConfig._correctionConfigStruct.offsetLimit_Z;

	double correctionFinalResultX = correction_X - resultPoints.OriginX - baseOffset_X;
	double correctionFinalResultY = correction_Y - resultPoints.OriginY - baseOffset_Y;
	double correctionFinalResultZ = correction_Z - resultPoints.OriginZ - baseOffset_Z;

	std::cout << "correctionFinalResultX" << correctionFinalResultX << std::endl;
	std::cout << "correctionFinalResultY" << correctionFinalResultY << std::endl;
	std::cout << "correctionFinalResultZ" << correctionFinalResultZ << std::endl;

	correctionFinalResultX = doubleLmit(correctionFinalResultX, 3);
	correctionFinalResultY = doubleLmit(correctionFinalResultY, 3);
	correctionFinalResultZ = doubleLmit(correctionFinalResultZ, 3);

	std::cout << "correctionFinalResultX" << correctionFinalResultX << std::endl;
	std::cout << "correctionFinalResultY" << correctionFinalResultY << std::endl;
	std::cout << "correctionFinalResultZ" << correctionFinalResultZ << std::endl;

	correctionFinalResultX = offsetLimit(correctionFinalResultX, offsetLimit_X);
	correctionFinalResultY = offsetLimit(correctionFinalResultY, offsetLimit_Y);
	correctionFinalResultZ = offsetLimit(correctionFinalResultZ, offsetLimit_Z);

	std::cout << "correctionFinalResultX" << correctionFinalResultX << std::endl;
	std::cout << "correctionFinalResultY" << correctionFinalResultY << std::endl;
	std::cout << "correctionFinalResultZ" << correctionFinalResultZ << std::endl;

	//double offsetRx = data->_productDataStruct.baseRad[0] - point.x;
	//double offsetRy = data->_productDataStruct.baseRad[1] - point.y;
	//double offsetRz = data->_productDataStruct.baseRad[2] - point.z;

	//cv::Vec3d radIn1({ 0,0,0 });
	//cv::Vec3d radIn2({ degToRad(offsetRx),degToRad(offsetRy),degToRad(offsetRz) });
	//cv::Mat coords = (cv::Mat_<double>(3, 1) << data->_productDataStruct.basePos[0], data->_productDataStruct.basePos[1], data->_productDataStruct.basePos[2]);
	//cv::Mat newCoords = transform_coordinates(coords, radIn1, radIn2);
	//cv::Mat coordsOffset = (cv::Mat_<double>(3, 1) << coords.at<double>(0, 0) - newCoords.at<double>(0, 0), coords.at<double>(1, 0) - newCoords.at<double>(1, 0), coords.at<double>(2, 0) - newCoords.at<double>(2, 0));
	
	//double offsetX = doubleLmit(coordsOffset.at<double>(0, 0) + baseOffsetX,3);
	//double offsetY = doubleLmit(coordsOffset.at<double>(1, 0) + baseOffsetY,3);
	//double offsetZ = doubleLmit(coordsOffset.at<double>(2, 0) + baseOffsetZ,3);

	//offsetRx = doubleLmit(offsetRx, 3);
	//offsetRy = doubleLmit(offsetRy, 3);
	//offsetRz = doubleLmit(offsetRz, 3);

	std::cout << "offsetX:" << data->pointsOffsetStruct_.baseOffsetX << "offsetY:" << data->pointsOffsetStruct_.baseOffsetY << "offsetZ:" << data->pointsOffsetStruct_.baseOffsetZ << std::endl;
	//std::cout << "offsetRx:" << offsetRx << "offsetRy:" << offsetRy << "offsetRz:" << offsetRz << std::endl;

	/*createModbusStrs(310, data->pointsOffsetStruct_.baseOffsetX,data);
	createModbusStrs(313, data->pointsOffsetStruct_.baseOffsetY, data);
	createModbusStrs(316, data->pointsOffsetStruct_.baseOffsetZ, data);*/

	//createModbusStrs(319, offsetRx, data);
	//createModbusStrs(322, offsetRy, data);
	//createModbusStrs(325, offsetRz, data);
	
	data->writeBack("short", 274, "1");
}