#include <iostream>

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

using namespace std;

using namespace cv;
using namespace cv::fisheye;

static Mat_<Point2f> GetDistortionMapping(const Size2i &image_size, const array<float, 4> &distortion_vector)
{
  Mat_<float> empty_camera_matrix = Mat::eye(3, 3, CV_32F);
  empty_camera_matrix(2, 0) = image_size.width / 2.f;
  empty_camera_matrix(2, 1) = image_size.height / 2.f;
  Mat_<Point2f> image_points(image_size);
  for (int y = 0; y < image_size.height; y++)
    for (int x = 0; x < image_size.width; x++)
      image_points(x, y) = Point2f(y, x);
  Mat_<Point2f> distorted_points(image_size);
  distortPoints(image_points, distorted_points, empty_camera_matrix, distortion_vector);
  return distorted_points;
}

static void ShowImage(const Mat &image)
{
  namedWindow("Distorted");
  const array<float, 4> distortion_vector { 1.f, 0.f, 0.f, 0.f }; //Even an all-zero vector yields an extremely distorted image
  const Mat distortion_mapping = GetDistortionMapping(image.size(), distortion_vector);
  Mat distorted_image;
  remap(image, distorted_image, distortion_mapping, noArray(), INTER_LANCZOS4);
  imshow("Distorted", distorted_image);
}

int main(const int argc, const char * const argv[])
{
  if (argc != 2)
  {
    cout << "Illustrates the effect of the distortion vector on a camera." << endl;
    cout << "Usage: " << argv[0] << " <camera image>" << endl;
    return 1;
  }
  const auto filename = argv[1];
  const Mat image = imread(filename);
  if (image.empty())
  {
    cerr << "Could not read input image '" << filename << "'" << endl;
    return 2;
  }
  ShowImage(image);
  waitKey(0);
  return 0;
}