#include "Arith_Utils.h"
#include <cmath>

#include "Arith_SysStruct.h"





void AddMat(double* A, double* B, int mn, double* AaddB)
{
	for (size_t i = 0; i < mn; i++)
	{
		AaddB[i] = A[i] + B[i];
	}

}

void MultiMatScalar(double scalar, double* A, double* sA, int mn)
{
	for (size_t i = 0; i < mn; i++)
	{
		sA[i] = scalar * A[i];
	}
}

void TransposeMat(double* A, int w, int h, double* AT)
{
	for (int i = 0; i < h; i++)
	{
		for (int j = 0; j < w; j++)
		{
			AT[j * w + i] = A[i * h + j];
		}
	}
}


/* 1    0      0
0    cos    sin
0   -sin    cos
*/
void Rx(double omega, double* Opp)
{
	double coso = cos(RADIAN(omega));
	double sino = sin(RADIAN(omega));

	Opp[0] = 1;
	Opp[1] = 0;
	Opp[2] = 0;
	Opp[3] = 0;
	Opp[4] = coso;
	Opp[5] = sino;
	Opp[6] = 0;
	Opp[7] = -sino;
	Opp[8] = coso;
}

/* cos    0    sin
0     1     0
-sin  0    cos
*/
void Ry(double omega, double* Opp)
{
	double coso = cos(RADIAN(omega));
	double sino = sin(RADIAN(omega));

	Opp[0] = coso;
	Opp[1] = 0;
	Opp[2] = sino;
	Opp[3] = 0;
	Opp[4] = 1;
	Opp[5] = 0;
	Opp[6] = -sino;
	Opp[7] = 0;
	Opp[8] = coso;
}



/* cos    sin    0
-sin cos  0
0   0     1
*/
void Rz(double omega, double* Opp)
{
	double coso = cos(RADIAN(omega));
	double sino = sin(RADIAN(omega));

	Opp[0] = coso;
	Opp[1] = sino;
	Opp[2] = 0;
	Opp[3] = -sino;
	Opp[4] = coso;
	Opp[5] = 0;
	Opp[6] = 0;
	Opp[7] = 0;
	Opp[8] = 1;
}

// 镜面反射矩阵
void RMirror(PointXYZ normalVec, double* Opp)
{
	//	M = eye(3)-(2*N*N');
	Opp[0] = 1 - 2 * normalVec.X * normalVec.X;
	Opp[1] = -2 * normalVec.X * normalVec.Y;
	Opp[2] = -2 * normalVec.X * normalVec.Z;
	Opp[3] = -2 * normalVec.X * normalVec.Y;
	Opp[4] = 1 - 2 * normalVec.Y * normalVec.Y;
	Opp[5] = -2 * normalVec.Y * normalVec.Z;
	Opp[6] = -2 * normalVec.X * normalVec.Z;
	Opp[7] = -2 * normalVec.Y * normalVec.Z;
	Opp[8] = 1 - 2 * normalVec.Z * normalVec.Z;

}
//*********************************************************************************************
//  矩阵乘法
//*********************************************************************************************
void  MultiMat(double* A, double* B, double* AB, int m, int n, int p)
{
	// A：m*n ; B：n*p  ;
	int	i, j, k;
	double Multisum;

	for (i = 0; i < m; i++) 
	{
		for (j = 0; j < p; j++) 
		{
			Multisum = 0;
			for (k = 0; k < n; k++)
				Multisum += A[i * n + k] * B[k * p + j];

			AB[i * p + j] = Multisum;
		}
	}
}



//*********************************************************************************************
//  
//*********************************************************************************************
void MultiMat33(double* A, double* B, double* AB)
{
	MultiMat(A, B, AB, 3, 3, 3);
}

//*********************************************************************************************
//  
//*********************************************************************************************
void MultiMat333(double* A, double* B, double* C, double* ABC)
{
	double BC[9];
	MultiMat(B, C, BC, 3, 3, 3);
	MultiMat(A, BC, ABC, 3, 3, 3);
}

// 对点/矢量旋转
PointXYZ RtPoint(double* M, PointXYZ Point)
{
	// M * [X,Y,Z]'
	double A[3];
	double G[3];
	A[0] = Point.X;
	A[1] = Point.Y;
	A[2] = Point.Z;
	MultiMat(M, A, G, 3, 3, 1);

	PointXYZ result;
	result.X = G[0];
	result.Y = G[1];
	result.Z = G[2];
	return result;

}

// 标准化矢量
PointXYZ NormPoint(PointXYZ Point)
{
	double Len = sqrt(Point.X * Point.X + Point.Y * Point.Y + Point.Z * Point.Z);
	Point.X /= Len;
	Point.Y /= Len;
	Point.Z /= Len;
	return Point;
}


void eye3(double* M)
{
	memset(M, 0, sizeof(double) * 9);
	M[0] = 1;
	M[4] = 1;
	M[8] = 1;
}

void invMat3(double* A, double* invA)
{
	double a1 = A[0];
	double a2 = A[1];
	double a3 = A[2];
	double b1 = A[3];
	double b2 = A[4];
	double b3 = A[5];
	double c1 = A[6];
	double c2 = A[7];
	double c3 = A[8];

	//A*
	double Astar[9] = { 0 };
	Astar[0] = b2 * c3 - c2 * b3;
	Astar[1] = c1 * b3 - b1 * c3;
	Astar[2] = b1 * c2 - c1 * b2;

	Astar[3] = c2 * a3 - a2 * c3;
	Astar[4] = a1 * c3 - c1 * a3;
	Astar[5] = a2 * c1 - a1 * c2;

	Astar[6] = a2 * b3 - b2 * a3;
	Astar[7] = b1 * a3 - a1 * b3;
	Astar[8] = a1 * b2 - a2 * b1;

	double detA = a1 * (b2 * c3 - c2 * b3) - a2 * (b1 * c3 - c1 * b3) + a3 * (b1 * c2 - c1 * b2);

	for (size_t i = 0; i < 9; i++)
	{
		invA[i] = Astar[i] / detA;
	}

}


// 旋转向量转矩阵
void rotationVectorToMatrix(PointXYZ rotaVec_One, float rotaAgl, double* Mat)
{
	// 
	PointXYZ vec_norm = NormPoint(rotaVec_One);

	// K
	double K[9] = { 0,-vec_norm.Z,vec_norm.Y,
					vec_norm.Z,0,-vec_norm.X,
					-vec_norm.Y,vec_norm.X,0 };

	// eye(3)
	double eye[9] = { 0 };
	eye3(eye);

	// eye + sind(r) * K + (1-cos(r)) * K * K
	double sK[9] = { 0 };
	MultiMatScalar(sin(RADIAN(rotaAgl)), K, sK, 9);

	double KK[9] = { 0 };
	MultiMat33(K, K, KK);

	double scK[9] = { 0 };
	MultiMatScalar(1 - cos(RADIAN(rotaAgl)), KK, scK, 9);

	double tmp[9] = { 0 };
	AddMat(eye, sK, 9, tmp);

	AddMat(tmp, scK, 9, Mat);


}



#ifdef _WIN32  // Windows平台

#include <Windows.h>

std::string GetDynamicLibraryPath()
{
    HMODULE hModule = GetModuleHandle(NULL);
    char path[MAX_PATH];
    GetModuleFileName(hModule, path, MAX_PATH);
    std::string fullPath(path);
    size_t pos = fullPath.find_last_of("\\/");
    return fullPath.substr(0, pos);
}

#elif __linux__  // Linux平台


#include <dlfcn.h>
#include <unistd.h>

std::string GetDynamicLibraryPath()
{
    Dl_info dlInfo;
    dladdr((void*)GetDynamicLibraryPath, &dlInfo);
    char* path = realpath(dlInfo.dli_fname, NULL);
    std::string fullPath(path);
    free(path);
    size_t pos = fullPath.find_last_of("/");
    return fullPath.substr(0, pos);
}

#else

#error Unsupported platform

#endif



cv::Mat getRGBMatFromGDFrame(GD_VIDEO_FRAME_S frame, void* pArr)
{
	// 针对拼接，目前支持Y8和NV12
	cv::Mat dst(frame.u32Height, frame.u32Width, CV_8UC3);

	//y8类型 
	if (frame.enPixelFormat == GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_GRAY_Y8)
	{
		cv::Mat mat(frame.u32Height, frame.u32Width, CV_8UC1, pArr);
		cv::cvtColor(mat, dst, cv::COLOR_GRAY2BGR);
		return dst.clone();
	}

	// nv12
	if (frame.enPixelFormat == GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_NV12)
	{
		cv::Mat mat(frame.u32Height * 1.5, frame.u32Width, CV_8UC1, pArr);
		cv::cvtColor(mat, dst, cv::COLOR_YUV2BGR_NV12);
		return dst.clone();
	}

	// rgb
	if (frame.enPixelFormat == GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_RGB_PACKED)
	{
		cv::Mat mat(frame.u32Height, frame.u32Width, CV_8UC3, pArr);
		return mat.clone();
	}


	return cv::Mat();
}

unsigned char FourPointInterpolation(unsigned char* pSrcImage, int iWidth, int iHeight, float x, float y)
{
	if (x < 0 || y < 0 || x > iWidth || y > iHeight)
		return 0;
	// 四个最临近象素的坐标(i1, j1), (i2, j1), (i1, j2), (i2, j2)
	int	i1, i2;
	int	j1, j2;

	// 四个最临近象素值
	unsigned char f1, f2, f3, f4;

	// 计算四个最临近象素的坐标
	i1 = fmax((int)x, 0);
	i2 = fmin(i1 + 1, iWidth - 1);
	j1 = fmax((int)y, 0);
	j2 = fmin(j1 + 1, iHeight - 1);


	float t, s;		//t = x - [x], s = y- [y];

	t = x - i1;
	s = y - j1;

	//四个最临近象素点的值的加权值
	float uv0, uv1, uv2, uv3;  //权重之和为1：uv0+uv1+uv2+uv3 = 1
	uv0 = (1 - s) * (1 - t);
	uv1 = (1 - s) * t;
	uv2 = (1 - t) * s;
	uv3 = s * t;


	//根据不同情况分别处理
	if (x - iWidth + 1 > 0)
	{
		// 要计算的点在图像右边缘上
		if (y - iHeight + 1 < 0)
		{
			// 要计算的点正好是图像最右/左下角那一个象素，直接返回该点象素值
			f1 = *(pSrcImage + iWidth * j1 + i1);
			return f1;
		}
		else
		{
			// 在图像右/左边缘上且不是最后一点，直接一次插值即可
			f1 = *(pSrcImage + iWidth * j1 + i1);
			f3 = *(pSrcImage + iWidth * j2 + i1);

			// 返回插值结果
			return ((unsigned char)(f1 + s * (f3 - f1)));
		}
	}
	else if (y - iHeight + 1 > 0)
	{
		// 要计算的点在图像下边缘上且不是最后一点，直接一次插值即可
		f1 = *(pSrcImage + iWidth * j1 + i1);
		f2 = *(pSrcImage + iWidth * j1 + i2);

		// 返回插值结果
		return ((unsigned char)(f1 + t * (f2 - f1)));
	}
	else
	{
		// 计算四个最临近象素值
		f1 = *(pSrcImage + iWidth * j1 + i1);
		f2 = *(pSrcImage + iWidth * j1 + i2);
		f3 = *(pSrcImage + iWidth * j2 + i1);
		f4 = *(pSrcImage + iWidth * j2 + i2);

		return (unsigned char)(uv0 * f1 + uv1 * f2 + uv2 * f3 + uv3 * f4);
	}
}