#include <iostream>
#include <string>
#include <opencv2\opencv.hpp>

using namespace std;
using namespace cv;

const float PI = 3.1415926;

void rectifyMap(Mat &mapImg, const int inWidth, const int inHeight,const float* rot, const int outWidth, const int outHeight, const float FOV, const float radius)
{
	float cx = outWidth/2.0;
	float cy = outHeight/2.0;

    float* pMapData = (float*)mapImg.data;
	for (int j = 0; j < outHeight; j++)
	{
		for (int i = 0; i < outWidth; i++)
		{
            //求fi 范围0-2PI
            if (i==cx && j==outHeight-1)
            {
                int flg=0;
            }
            float fi2=atan2( (j-cy),(i-cx));
            if (fi2<0)
            {
                fi2 +=2*PI;
            }
            //求半径
            float radius2;
            if(abs(sin(fi2))<1e-3)
            {
                radius2=abs(i-cx);
            }
            else
            {
                radius2=abs((j-cy)/sin(fi2));
            }

            float theta2=radius2*FOV/(radius*2);
            if (theta2 <= (FOV / 2) && theta2 >= 0)
            {
                float x2,y2,z2;
                z2=cos(theta2*PI/180);
                if(abs(fi2-PI/2)<1e-3 || abs(fi2-PI/2)<1e-3)
                {
                   x2=0;
                }
                else
                {
                     x2=sqrt(1-z2*z2)/(1+tan(fi2)*tan(fi2));
                }
                if(fi2<(3*PI/2) && fi2>(PI/2))   x2 *=-1;
                y2=sqrt(1-x2*x2-z2*z2);
                if(fi2<(2*PI) && fi2>(PI))   y2 *=-1;

                float norm = sqrt(x2*x2 + y2*y2 + z2*z2);
			    x2 *= norm;
			    y2 *= norm;
			    z2 *= norm;

                float x1 = rot[0] * x2 + rot[1] * y2 + rot[2] * z2;
			    float y1 = rot[3] * x2 + rot[4] * y2 + rot[5] * z2;
			    float z1 = rot[6] * x2 + rot[7] * y2 + rot[8] * z2;

                float theta1=acos(z1);
                float fi1;
                if (abs(sin(theta1))<1e-3)
                {
                   fi1=PI/2;
                }
                else
                {
                   fi1=acos(x1/sin(theta1))+PI/2;
                }

                float u=(2*PI-fi1)*inWidth/(2*PI);
                float v=theta1*inHeight/PI;

               if (u >= 0 && u < inWidth - 1 && v >= 0 && v < inHeight - 1)
				{
					pMapData[j*outWidth * 2 + 2 * i + 0] = u;
					pMapData[j*outWidth * 2 + 2 * i + 1] = v;
				}
            }
            else
            {
                pMapData[j*outWidth * 2 + 2 * i + 0] = 0;
				pMapData[j*outWidth * 2 + 2 * i + 1] = 0;
            }
        }
    }
}

void remap(const cv::Mat& srcImg, cv::Mat& dstImg, const cv::Mat& mapImg, int inHeight, int inWidth, int outHeight, int outWidth)
{
	uchar* pSrcData = (uchar*)srcImg.data;
	uchar* pDstData = (uchar*)dstImg.data;
	float* pMapData = (float*)mapImg.data;

	for (int j = 0; j < outHeight; j++)
	{
		for (int i = 0; i < outWidth; i++)
		{
			int idx = j*outWidth * 2 + i * 2;
			float u = pMapData[idx + 0];
			float v = pMapData[idx + 1];

			int u0 = floor(u);
			int v0 = floor(v);
			float dx = u - u0;
			float dy = v - v0;
			float weight1 = (1 - dx)*(1 - dy);
			float weight2 = dx*(1 - dy);
			float weight3 = (1 - dx)*dy;
			float weight4 = dx*dy;

			if (u0 >= 0 && v0 >= 0 && (u0 + 1) < inWidth && (v0 + 1) < inHeight)
			{
				float B = weight1*pSrcData[v0*inWidth * 3 + u0 * 3 + 0] + weight2*pSrcData[v0*inWidth * 3 + (u0 + 1) * 3 + 0] +
					weight3*pSrcData[(v0 + 1)*inWidth * 3 + u0 * 3 + 0] + weight4*pSrcData[(v0 + 1)*inWidth * 3 + (u0 + 1) * 3 + 0];

				float G = weight1*pSrcData[v0*inWidth * 3 + u0 * 3 + 1] + weight2*pSrcData[v0*inWidth * 3 + (u0 + 1) * 3 + 1] +
					weight3*pSrcData[(v0 + 1)*inWidth * 3 + u0 * 3 + 1] + weight4*pSrcData[(v0 + 1)*inWidth * 3 + (u0 + 1) * 3 + 1];

				float R = weight1*pSrcData[v0*inWidth * 3 + u0 * 3 + 2] + weight2*pSrcData[v0*inWidth * 3 + (u0 + 1) * 3 + 2] +
					weight3*pSrcData[(v0 + 1)*inWidth * 3 + u0 * 3 + 2] + weight4*pSrcData[(v0 + 1)*inWidth * 3 + (u0 + 1) * 3 + 2];

				int idxResult = j*outWidth * 3 + i * 3;
				pDstData[idxResult + 0] = uchar(B);
				pDstData[idxResult + 1] = uchar(G);
				pDstData[idxResult + 2] = uchar(R);
			}
		}
	}
}

void main()
{
	Mat srcImg = imread("animal.jpg");
    //输入经纬度图像尺寸
	int inHeight = srcImg.rows;
	int inWidth = srcImg.cols;
    //输出鱼眼图像尺寸
	int outHeight = 1000;
	int outWidth = 1000;
    //视场角
	float FOV = 180;
    //鱼眼半径
	float radius = outWidth / 2.0;

    //以图像中心为北极
    float rot[9] = { 1,0,0,0,1,0,0,0,1 };
	float angle = PI/2;
	rot[4] = cos(angle);
	rot[5] = sin(angle);
	rot[7] = -sin(angle);
	rot[8] = cos(angle);

    //求映射Map
	cv::Mat mapImg = cv::Mat::zeros(outHeight, outWidth, CV_32FC2);
	rectifyMap(mapImg, inWidth, inHeight,rot, outWidth, outHeight, FOV, radius);
    //remap得到经纬度图像
	Mat dstImg = Mat::zeros(outHeight, outWidth, CV_8UC3);
	remap(srcImg, dstImg, mapImg, inHeight, inWidth, outHeight, outWidth);

	imwrite("dstImg.jpg", dstImg);
}