#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;

const double PI = 3.141592653589793;
const string PATH_IMAGE = "/Users/Kenza/Desktop/Xcode_cpp_opencv/PaulBourke2/PaulBourke2/Images/img1.jpg";
const int ESC = 27;

Point2f findCorrespondingFisheyePoint(int Xe, int Ye, double He, double We, double Hf, double Wf, double FOV){
    Point2f fisheyePoint;
    double Xfn, Yfn; //Normalized Cartesian Coordinates
    double longitude, latitude, Px, Py, Pz; //Spherical Coordinates
    double r, theta; //Polar coordinates
    double Xpn, Ypn; //Normalized Polar coordinates

    //Normalize Coordinates
    Xfn = ( ( 2.0 * (double)Xe ) - We) / Wf;//Between -1 and 1
    Yfn = ( ( 2.0 * (double)Ye ) - He) / Hf;//Between -1 and 1

    //Normalize Coordinates to Spherical Coordinates
    longitude = Xfn*PI; //Between -PI and PI (2*PI interval)
    latitude = Yfn*(PI/2.0); //Between -PI/2 and PI/2 (PI interval)
    Px = cos(latitude)*cos(longitude);
    Py = cos(latitude)*sin(longitude);
    Pz = sin(latitude);

    //Spherical Coordinates to Polar Coordinates
    r =  2.0 * atan2(sqrt(pow(Px,2)+pow(Pz,2)),Py)/FOV;
    theta = atan2(Pz,-Px);
    Xpn = r * cos(theta);
    Ypn = r * sin(theta);

    //Normalize Coordinates to CartesianImage Coordinates
    fisheyePoint.x = (int)(((Xpn+1.0)*Wf)/2.0);
    fisheyePoint.y = (int)(((Ypn+1.0)*Hf)/2.0);

    return fisheyePoint;
}

int main(int argc, char** argv){

    Mat fisheyeImage, equirectangularImage;

    fisheyeImage = imread(PATH_IMAGE, CV_32FC1);
    namedWindow("Fisheye Image", WINDOW_AUTOSIZE);
    imshow("Fisheye Image", fisheyeImage);

    while(waitKey(0) != ESC) {
        //wait until the key ESC is pressed
    }

    //destroyWindow("Fisheye Image");

    int Hf, Wf; //Height, width and FOV for the input image (=fisheyeImage)
    double FOV;
    int He, We; //Height and width for the outpout image (=EquirectangularImage)

    Hf = fisheyeImage.size().height;
    Wf = fisheyeImage.size().width;
    FOV = PI; //FOV in radian

    //We keep the same ratio for the image input and the image output
    We = Wf;
    He = Hf;

    equirectangularImage.create(Hf, Wf, fisheyeImage.type()); //We create the outpout image (=EquirectangularImage)

    //For each pixels of the ouput equirectangular Image
    for (int Xe = 0; Xe <equirectangularImage.size().width; Xe++){
        for (int Ye = 0; Ye <equirectangularImage.size().height; Ye++){

            equirectangularImage.at<Vec3b>(Point(Xe,Ye)) = fisheyeImage.at<Vec3b>(findCorrespondingFisheyePoint(Xe, Ye, He, We, Hf, Wf, FOV)); //We find the corresponding point in the fisheyeImage
        }
    }

    namedWindow("Equirectangular Image", WINDOW_AUTOSIZE);
    imshow("Equirectangular Image",equirectangularImage);

    while(waitKey(0) != ESC) {
        //wait until the key ESC is pressed
    }

    destroyWindow("Fisheye Image");

    imwrite("equirectangularImage.jpg", equirectangularImage);

    return 0;

}