#include <iostream>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d.hpp>
#include <fstream>
#include <opencv2/core/core.hpp>
#define PATH "/Users/mac/ClionProjects/opencv_test/";
using namespace std;//標準庫　名稱空間
using namespace cv; //opencv庫名稱空間

vector<Point2f> points1, points2, points3;
String path = "/Users/mac/ClionProjects/opencv_test/";

// 雙目校正用到量, 影象解析度為2040*2048
double scale = 0.2;
const int imageWidth=2040, imageHeight = 2048;
Size imageSize(imageWidth, imageHeight);
Mat rectifyImgL,rectifyImgR;
Rect validRoiL, validRoiR; // 裁剪後的區域
Mat mapLx, mapLy, mapRx, mapRy; //對映
Mat Rl, Rr, Pl, Pr, Q; // 校正旋轉矩陣R 投影矩陣P 重投影矩陣Q
Mat xyz;

Point origin; // 滑鼠起始點
Rect selection; // 框選
bool selectOb = false; // 是否選擇物件

int blockSize = 0, uniqueRatio = 0, numDisparity = 0;
Ptr<StereoBM> bm = StereoBM::create(16, 9);

//使用matlab標定好的資料,使用過載後的流操作符初始化
Mat camKKL = (Mat_<double >(3, 3)<<2194.63, 0, 992.88, 0, 2196.167, 1043.53, 0, 0, 1);
Mat camKKR = camKKL;
Mat distCoeL = (Mat_<double>(1, 4)<<-0.04, 0.13485, -0.0017, -0.0008);
Mat distCoeR = distCoeL;

void stereo_match(int , void*);
static void onMouse(int event, int x, int y, int, void*);// 滑鼠操作回撥
void readCsv(string filename, vector<Point2f>& points);//初始化，從csv中讀取資料,讀取後結果存到對應的points中
void writeXML(Mat mat, string name); // 資料寫入xml

// 計算本質矩陣， 並進行雙目校正 ，產生校正變換，將校正後的影象放在rectifyImg中
void myCalib(InputArray points1, Mat camKKL, Mat distCoeL, InputArray points2, Mat camKKR, Mat distCoeR, string pic1, string pic2);

int main()
{
    string filename1 = "point1B.csv";
    string filename2 = "point2M.csv";
    readCsv(filename1, points1);
    readCsv(filename2, points2);
    myCalib(points1, camKKL, distCoeL, points2, camKKR, distCoeR, "B.jpg", "M.jpg");

    return 0;

}


void myCalib(InputArray points1, Mat camKKL, Mat distCoeL, InputArray points2, Mat camKKR, Mat distCoeR, string pic1, string pic2){
    Mat EssenMatrix, R, T;
    EssenMatrix= findEssentialMat(points1, points2, camKKL);

    recoverPose(EssenMatrix, points1, points2, camKKL, R, T);
    stereoRectify(camKKL, distCoeL, camKKR, distCoeR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY, 1, imageSize, &validRoiL, &validRoiR);
    fisheye::initUndistortRectifyMap(camKKL, distCoeL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
    fisheye::initUndistortRectifyMap(camKKR, distCoeR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
    cout<<Pr<<endl<<Q;

    Mat matPic1 = imread(path+pic1);
    Mat matPic2 = imread(path+pic2);
    remap(matPic1, rectifyImgL, mapLx, mapLy, INTER_LINEAR);
    remap(matPic2, rectifyImgR, mapRx, mapRy, INTER_LINEAR);

    //太大不能顯示則放縮
    Mat SRectifyPic1, SRectifyPic2;
    resize(rectifyImgL, SRectifyPic1, Size(), scale, scale, INTER_LINEAR);
    resize(rectifyImgR, SRectifyPic2, Size(), scale, scale, INTER_LINEAR);
    imshow("imageL", SRectifyPic1);
    imshow("imageR", SRectifyPic2);
    cvStereo
    waitKey(0);


}

void writeXML(Mat mat, string name){
    FileStorage fs(path+name, FileStorage::WRITE);
    fs<<"Mat"<<mat;
    fs.release();
}

void readCsv(string filename, vector<Point2f>& points){
    ifstream fin(path+filename);
    string line, value;
    int xflag=0, index=0;
    float xdata, ydata;
    while(fin.good()){
        getline(fin, value);
        //cout<<value<<endl;
        int delim = value.find(",");
        //cout<<value.substr(0, delim)<<"  "<<value.substr(delim+1, value.length()-delim-2)<<endl;
        xdata = stof(value.substr(0, delim));
        ydata = stof(value.substr(delim+1, value.length()-delim-2));
        points.push_back(Point2f(xdata, ydata));

    }

}

void stereo_match(int,void*)
{
    bm->setBlockSize(2*blockSize+5);     //SAD視窗大小，5~21之間為宜
    bm->setROI1(validRoiL);
    bm->setROI2(validRoiR);
    bm->setPreFilterCap(31);
    bm->setMinDisparity(0);  //最小視差，預設值為0, 可以是負值，int型
    bm->setNumDisparities(numDisparity*16+16);//視差視窗，即最大視差值與最小視差值之差,視窗大小必須是16的整數倍，int型
    bm->setTextureThreshold(10);
    bm->setUniquenessRatio(uniqueRatio);//uniquenessRatio主要可以防止誤匹配
    bm->setSpeckleWindowSize(100);
    bm->setSpeckleRange(32);
    bm->setDisp12MaxDiff(-1);
    Mat disp, disp8;
    bm->compute(rectifyImgL, rectifyImgR, disp);//輸入影象必須為灰度圖
    disp.convertTo(disp8, CV_8U, 255 / ((numDisparity * 16 + 16)*16.));//計算出的視差是CV_16S格式
    reprojectImageTo3D(disp, xyz, Q, true); //在實際求距離時，ReprojectTo3D出來的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正確的三維座標資訊。
    xyz = xyz * 16;
    imshow("disparity", disp8);
}


static void onMouse(int event, int x, int y, int, void*)
{
    if (selectOb)
    {
        selection.x = MIN(x, origin.x);
        selection.y = MIN(y, origin.y);
        selection.width = std::abs(x - origin.x);
        selection.height = std::abs(y - origin.y);
    }

    switch (event)
    {
        case EVENT_LBUTTONDOWN:   //滑鼠左按鈕按下的事件
            origin = Point(x, y);
            selection = Rect(x, y, 0, 0);
            selectOb = true;
            cout << origin <<"in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
            break;
        case EVENT_LBUTTONUP:    //滑鼠左按鈕釋放的事件
            selectOb = false;
            if (selection.width > 0 && selection.height > 0)
                break;
    }
}