// #include "../util.h"

#include <csc_images.hpp.opencv.inl>

using namespace CSC ;

/*
	@info: 'VAL64'是浮点数'double'
	@info: 'LENGTH'和'INDEX'都是整形'int64'
	@info：'ARRAY2<T>是Array<T ,ARGC<2>>，既'T[2]'
	@info: 'Array'是动态定长容器
	@info: 'Queue'是动态变长容器
	@info: 'VAR_NONE'指总是无效的元素索引
*/
namespace panorama_demo {
/*
	@function: 相机在坐标原点上，返回空间点在球目图片上的投影点，和'panorama_xyz_from_txy'互逆
	@param {xyz}: 空间中的3D点，不需要先进行归一化
	@return {ret}: 纹理空间的2D点，范围为[0 ,1]
*/
inline static ARRAY2<VAL64> panorama_txy_from_xyz (const ARRAY3<VAL64> &xyz) {
	ARRAY2<VAL64> ret ;
	//@info: 这一步是为了兼容我们所用的球目相机的正方向，并且进行归一化
	const auto r1x = Vector<VAL64> {-xyz[1] ,-xyz[0] ,xyz[2] ,0}.normalize () ;
	//@info: 分别计算投影点坐标
	//@info: 反三角函数，缩放，平移
	ret[0] = _ATAN_ (r1x[1] ,r1x[0]) / VAL64 (VALX_PI * 2) + VAL64 (0.5) ;
	ret[1] = _ACOS_ (r1x[2]) / VAL64 (VALX_PI * 2) * 2 ;
	return std::move (ret) ;
}

/*
	@function: 相机在坐标原点上，返回球目图片上的投影点所在单位球上的原点，和'panorama_txy_from_xyz'互逆
	@param {txy}: 纹理空间的2D点，范围为[0 ,1]
	@return {ret}: 空间中的3D点，在单位球上
*/
inline static ARRAY3<VAL64> panorama_xyz_from_txy (const ARRAY2<VAL64> &txy) {
	ARRAY3<VAL64> ret ;
	//@info: 分别计算反投影点坐标
	//@info: 平移，缩放
	const auto r1x = txy[1] / 2 * VAL64 (VALX_PI * 2) ;
	const auto r2x = (txy[0] - VAL64 (0.5)) * VAL64 (VALX_PI * 2) ;
	//@info: 对应的三角函数
	ret[0] = -_SIN_ (r1x) * _SIN_ (r2x) ;
	ret[1] = -_SIN_ (r1x) * _COS_ (r2x) ;
	ret[2] = _COS_ (r1x) ;
	return std::move (ret) ;
}

/*
	@function: 计算栅格化所有线段后最大生成点的数量
	@param {line}: 线段的数组，line[i][0]为第一个点，line[i][1]为第二个点
	@param {gap}: 栅格化所需的步长，既每多长生成一个点
	@return {ret}: 最大生成点的数量
*/
inline static LENGTH projection_line_max_count (const Array<ARRAY2<ARRAY3<VAL64>>> &line ,VAL64 gap) {
	LENGTH ret = 0 ;
	//@info: 循环所有的线段
	for (auto &i : line) {
		//@info: 转成向量
		const auto r1x = Vector<VAL64> {i[0] ,1} ;
		const auto r2x = Vector<VAL64> {i[1] ,1} ;
		//@info: 计算每条线段需要栅格化多少个点
		//@info: 向量差，求模，除步长，向上取整
		ret += LENGTH ((r1x - r2x).magnitude () / gap + 0.5) ;
	}
	printf('ret',ret)
	return std::move (ret) ;
}

/*
	@function: 计算3D线段在球目图片上的栅格化投影
	@param {camera_pose_matrix}: 相机位置，外参矩阵
	@param {line}: 线段的数组，line[i][0]为第一个点，line[i][1]为第二个点
	@param {gap}: 栅格化所需的步长，既每多长生成一个点
	@return {ret}: 栅格化投影投影后的2D线段，点坐标属于纹理空间，范围为[0 ,1]
*/
inline static Queue<ARRAY2<ARRAY2<VAL64>>> panorama_projection_line (const Matrix<VAL64> &camera_pose_matrix ,const Array<ARRAY2<ARRAY3<VAL64>>> &line ,VAL64 gap) {
	_DEBUG_ASSERT_ (gap > 0) ;
	//@info: 计算栅格化所有线段后最大生成点的数量
	const auto r4x = projection_line_max_count (line ,gap) ;
	//@info: 创建容器
	Queue<ARRAY2<ARRAY2<VAL64>>> ret = Queue<ARRAY2<ARRAY2<VAL64>>> (r4x) ;
	//@info: 预分配元素索引，'ix'指当前线段，'iy'指上一条线段
	INDEX ix = ret.insert () ;
	INDEX iy = VAR_NONE ;
	const auto r10x = camera_pose_matrix.inverse () ;
	//@info: 循环所有的线段
	for (auto &i : line) {
		//@info: 转成向量，并应用相机外参矩阵的反变换
		const auto r1x = r10x * Vector<VAL64> {i[0] ,1} ;
		const auto r2x = r10x * Vector<VAL64> {i[1] ,1} ;
		//@info: 计算栅格化的点数量
		const auto r3x = (r2x - r1x).magnitude () / gap ;
		//@info: 计算栅格化的步长向量
		const auto r5x = (r2x - r1x).normalize () * gap ;
		for (VAL64 j = 0 ; j < r3x ; j += 1) {
			//@info: 计算循环栅格化的每一个3D点的坐标
			const auto r6x = r1x + r5x * j ;
			//@info: 计算3D点对应的2D投影
			const auto r7x = panorama_txy_from_xyz (r6x.xyz ()) ;
			//@info: 记录'ix'的第一个点和'iy'的第二个点
			ret[ix][0] = r7x ;
			if (iy != VAR_NONE)
				ret[iy][1] = r7x ;
			//@info: 预分配元素索引
			iy = ix ;
			ix = ret.insert () ;
		}
		//@info: 跳过一个点都没有的情况
		if (iy == VAR_NONE)
			continue ;
		//@info: 对栅格化向上取整，补上最后一条线段的第二个点
		const auto r8x = r1x + r5x * _CEIL_ (r3x ,VAL64 (1)) ;
		const auto r9x = panorama_txy_from_xyz (r8x.xyz ()) ;
		
		ret[iy][1] = r9x ;
		iy = VAR_NONE ;
	}
	//@info: 因为使用了预分配元素索引，最后需要弹出末尾未实际使用的元素
	ret.pop () ;
	return std::move (ret) ;
}

/*
	@function: 操作图片，需要根据平台自行实现
*/
static const PhanRef<const AbstractImage<COLOR_BGR>::Engine> &ABSTRACTIMAGE_ENGINE = PhanRef<const AbstractImage<COLOR_BGR>::Engine>::make (_CACHE_ ([] () {
	return AutoRef<AbstractImage_Engine_OPENCV<COLOR_BGR>>::make () ;
})) ;

/*
	@function: 绘制线段，需要根据平台自行实现
	@warn: 一个很重要的点，球目图片是横向循环的，既'x(width)=x(0)'，因此'[{w-2,0} ,{1 ,0}]'和'[{1 ,0} ,{w-2,0}]'不同，不能直接绘制，需要变换成'[{0 ,0} ,{1 ,0}]'和'[{w-2 ,0} ,{w-1 ,0}]'进行绘制
*/
inline static void image_draw_line (AbstractImage<COLOR_BGR> &image ,const Queue<ARRAY2<ARRAY2<VAL64>>> &line ,LENGTH thickness ,const COLOR_BGR &color) {
	const auto r4x = image.native<cv::Mat> () ;
	const auto r7x = ARRAY2<VAL64> {VAL64 (_XVALUE_<cv::Mat &> (r4x).cols) ,VAL64 (_XVALUE_<cv::Mat &> (r4x).rows)} ;
	const auto r3x = cv::Scalar {cv::Scalar::value_type (color[0]) ,cv::Scalar::value_type (color[1]) ,cv::Scalar::value_type (color[2])} ;
	for (auto &i : line) {
		const auto r10x = i[0][0] * r7x[0] ;
		const auto r11x = i[0][1] * r7x[1] ;
		const auto r12x = i[1][0] * r7x[0] ;
		const auto r13x = i[1][1] * r7x[1] ;
		const auto r14x = _SQE_ (r12x - r10x) ;
		const auto r15x = _SQE_ (_ABS_ (r12x - r10x) - r7x[0]) ;
		if (r15x >= r14x) {
			const auto r1x = cv::Point {cv::Point::value_type (r10x) ,cv::Point::value_type (r11x)} ;
			const auto r2x = cv::Point {cv::Point::value_type (r12x) ,cv::Point::value_type (r13x)} ;
			cv::line (_XVALUE_<cv::Mat &> (r4x) ,r1x ,r2x ,r3x ,VAR32 (thickness) ,cv::LineTypes::FILLED) ;
		} else {
			const auto r16x = r10x < r12x ? ARRAY2<VAL64> {r12x - r7x[0] ,r13x} : ARRAY2<VAL64> {r10x - r7x[0] ,r11x} ;
			const auto r17x = r10x < r12x ? ARRAY2<VAL64> {r10x ,r11x} : ARRAY2<VAL64> {r12x ,r13x} ;
			const auto r18x = r10x < r12x ? ARRAY2<VAL64> {r12x ,r13x} : ARRAY2<VAL64> {r10x ,r11x} ;
			const auto r19x = r10x < r12x ? ARRAY2<VAL64> {r10x + r7x[0] ,r11x} : ARRAY2<VAL64> {r12x + r7x[0] ,r13x} ;
			const auto r20x = cv::Point {cv::Point::value_type (r16x[0]) ,cv::Point::value_type (r16x[1])} ;
			const auto r21x = cv::Point {cv::Point::value_type (r17x[0]) ,cv::Point::value_type (r17x[1])} ;
			const auto r22x = cv::Point {cv::Point::value_type (r18x[0]) ,cv::Point::value_type (r18x[1])} ;
			const auto r23x = cv::Point {cv::Point::value_type (r19x[0]) ,cv::Point::value_type (r19x[1])} ;
			cv::line (_XVALUE_<cv::Mat &> (r4x) ,r20x ,r21x ,r3x ,VAR32 (thickness) ,cv::LineTypes::FILLED) ;
			cv::line (_XVALUE_<cv::Mat &> (r4x) ,r22x ,r23x ,r3x ,VAR32 (thickness) ,cv::LineTypes::FILLED) ;
		}
	}
}

/*
	@function: 加载3D线段，需要根据平台自行实现
	@info: 这里生成一个3D单位CUBE的所有线段
*/
inline static Array<ARRAY2<ARRAY3<VAL64>>> line_of_demo_cube () {
	const auto r1x = VAL64 (0.5) ;
	const auto r2x = ARRAY8<ARRAY3<VAL64>> ({
		{-r1x ,-r1x ,+r1x} ,
		{-r1x ,+r1x ,+r1x} ,
		{+r1x ,+r1x ,+r1x} ,
		{+r1x ,-r1x ,+r1x} ,
		{-r1x ,-r1x ,-r1x} ,
		{-r1x ,+r1x ,-r1x} ,
		{+r1x ,+r1x ,-r1x} ,
		{+r1x ,-r1x ,-r1x}}) ;
	const auto r3x = Array<ARRAY2<INDEX>> ({
		{0 ,1} ,
		{1 ,2} ,
		{2 ,3} ,
		{3 ,0} ,
		{4 ,5} ,
		{5 ,6} ,
		{6 ,7} ,
		{7 ,4} ,
		{0 ,4} ,
		{1 ,5} ,
		{2 ,6} ,
		{3 ,7}}) ;
	Array<ARRAY2<ARRAY3<VAL64>>> ret = Array<ARRAY2<ARRAY3<VAL64>>> (r3x.length ()) ;
	for (INDEX i = 0 ; i < r3x.length () ; i++) {
		ret[i][0] = r2x[r3x[i][0]] ;
		ret[i][1] = r2x[r3x[i][1]] ;
	}
	return std::move (ret) ;
}
} ;

int main () {
	//@info: 加载3D线段
	const auto r1x = panorama_demo::line_of_demo_cube () ;
	//@info: 计算3D线段栅格化投影后的2D线段
	const auto r2x = panorama_demo::panorama_projection_line (Matrix<VAL64>::make_identity () ,r1x ,0.1) ;
	if (TRUE) {
		auto rax = AbstractImage<COLOR_BGR> (panorama_demo::ABSTRACTIMAGE_ENGINE) ;
		//@info: 加载图片，文件'panorama_demo.input.jpg'
		rax.load_file (_PCSTR_ ("panorama_demo.input.jpg")) ;
		//@info: 绘制线段
		panorama_demo::image_draw_line (rax ,r2x ,30 ,{0 ,0 ,255}) ;
		//@info: 保存图片，文件'panorama_demo.result.jpg'
		rax.save_file (_PCSTR_ ("panorama_demo.result.jpg") ,AnyRef<std::vector<int>>::make (std::vector<int> {cv::IMWRITE_PNG_COMPRESSION ,5})) ;
	}
	return 0 ;
}