#include <iostream>
#include <fstream>
#include <opencv2/calib3d.hpp>

#define CAM_WIDTH        (1280)

#define CAM_HEIGHT        (720)

int main()
{

      /* Input camera matrix A=[fx, 0, cx; 0, fy, cy; 0, 0, 1];
         In ADAS, fx = focal_u, fy = focal_v, cx = center_u, cy = center_v;
       */

      cv::Mat cameraMatrix = cv::Mat(3, 3, cv::DataType<double>::type);

      cameraMatrix.at<double>(0, 0) = 308.18804931640625;
      cameraMatrix.at<double>(0, 1) = 0;
      cameraMatrix.at<double>(0, 2) = 659.6455688476563;
      cameraMatrix.at<double>(1, 0) = 0;
      cameraMatrix.at<double>(1, 1) = 307.761962890625;
      cameraMatrix.at<double>(1, 2) = 371.9734802246094;
      cameraMatrix.at<double>(2, 0) = 0;
      cameraMatrix.at<double>(2, 1) = 0;
      cameraMatrix.at<double>(2, 2) = 1;

      /* Input vector of distortion coefficients (k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy]]]])
         of 4, 5, 8, 12 or 14 elements;
         In ADAS, this vector is 'distort';
       */

      cv::Mat distCoeffs = cv::Mat(4, 1, cv::DataType<double>::type);

      distCoeffs.at<double>(0, 0) = 0.0833858847618103;
      distCoeffs.at<double>(1, 0) = 0.010020568035542965;
      distCoeffs.at<double>(2, 0) = -0.0019307926995679736;
      distCoeffs.at<double>(3, 0) = -0.0009038179414346814;

      /* Optional rectification transformation in the object space (3x3 matrix). */
      cv::Mat R;

      /* New camera matrix A′=[fx′, 0, cx′; 0, fy′, cy′; 0, 0, 1] */
      cv::Mat newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, cv::Size(CAM_WIDTH,CAM_HEIGHT),0, cv::Size(CAM_WIDTH,CAM_HEIGHT));

      /* Undistorted image size. */
      cv::Size size(CAM_WIDTH,CAM_HEIGHT);

      /* Type of the first output map */
      int m1type = CV_32FC1;

      /* The first output map and the second output map */
      cv::Mat map1, map2;      

      /* call initUndistortRectifyMap */
      cv::initUndistortRectifyMap(cameraMatrix, distCoeffs, R, newCameraMatrix, size, m1type, map1, map2);

      /* write map file. */
      std::ofstream ofs;
      ofs.open("gdc_map.txt");

      if (ofs.is_open()) {
              int r, c;
              float x, y;

              ofs << 1 << std::endl;
              ofs << 50 << " " << 50 << std::endl;
              ofs << CAM_HEIGHT << " " << CAM_WIDTH << std::endl;
              ofs << (CAM_HEIGHT / 2 - 1) << " " << (CAM_WIDTH / 2 - 1) << std::endl;

              for (r = 0; r < CAM_HEIGHT; r++) {

                      for (c = 0; c < CAM_WIDTH; c++) {
                              x = map1.at<float>(r,c);
                              y = map2.at<float>(r,c);
                              ofs << y << ":" << x << " ";

                      }
                      ofs << std::endl;
              }
              ofs.flush();
              ofs.close();
              std::cout << "gdc_map.txt generated." << std::endl;

      }
      else {
              std::cout << "Failed to open gdc_map.txt" << std::endl;
      }
}