#include <iostream>
#include <opencv2/opencv.hpp>

namespace
{
void scaleApplyColormap(const cv::Mat &img_float, cv::Mat &img)
{
  cv::Mat img_scale = cv::Mat::zeros(img_float.size(), CV_8UC3);

  double min_val = 0, max_val = 0;
  cv::minMaxLoc(img_float, &min_val, &max_val);
  std::cout << "min_val=" << min_val << " ; max_val=" << max_val << std::endl;

  if (max_val - min_val > 1e-2) {
    float a = 255 / (max_val - min_val);
    float b = -a * min_val;

    cv::convertScaleAbs(img_float, img_scale, a, b);
    cv::applyColorMap(img_scale, img, cv::COLORMAP_TURBO);
  }
  else {
    std::cerr << "max_val - min_val <= 1e-2" << std::endl;
  }
}

cv::Mat drawEllipseDistanceMap(const cv::RotatedRect &ellipse_params)
{
  float bb_rect_w = ellipse_params.center.x + ellipse_params.size.width;
  float bb_rect_h = ellipse_params.center.y + ellipse_params.size.height;

  std::vector<cv::Point2f> points_list;
  points_list.resize(1);
  cv::Mat pointsf;
  cv::Mat closest_pts;
  cv::Mat dist_map = cv::Mat::zeros(bb_rect_h*1.5, bb_rect_w*1.5, CV_32F);
  for (int i = 0; i < dist_map.rows; i++) {
    for (int j = 0; j < dist_map.cols; j++) {
      points_list[0].x = j;
      points_list[0].y = i;
      cv::Mat(points_list).convertTo(pointsf, CV_32F);
      cv::getClosestEllipsePoints(ellipse_params, pointsf, closest_pts);
      dist_map.at<float>(i, j) = std::hypot(closest_pts.at<cv::Point2f>(0).x-j, closest_pts.at<cv::Point2f>(0).y-i);
    }
  }

  cv::Mat dist_map_8u;
  scaleApplyColormap(dist_map, dist_map_8u);
  return dist_map_8u;
}
}

int main()
{
  std::vector<cv::Point2f> points_list;

  // [1434, 308], [1434, 309], [1433, 310], [1427, 310], [1427, 312], [1426, 313], [1422, 313], [1422, 314],
  points_list.push_back(cv::Point2f(1434, 308));
  points_list.push_back(cv::Point2f(1434, 309));
  points_list.push_back(cv::Point2f(1433, 310));
  points_list.push_back(cv::Point2f(1427, 310));
  points_list.push_back(cv::Point2f(1427, 312));
  points_list.push_back(cv::Point2f(1426, 313));
  points_list.push_back(cv::Point2f(1422, 313));
  points_list.push_back(cv::Point2f(1422, 314));

  // [1421, 315], [1415, 315], [1415, 316], [1414, 317], [1408, 317], [1408, 319], [1407, 320], [1403, 320],
  points_list.push_back(cv::Point2f(1421, 315));
  points_list.push_back(cv::Point2f(1415, 315));
  points_list.push_back(cv::Point2f(1415, 316));
  points_list.push_back(cv::Point2f(1414, 317));
  points_list.push_back(cv::Point2f(1408, 317));
  points_list.push_back(cv::Point2f(1408, 319));
  points_list.push_back(cv::Point2f(1407, 320));
  points_list.push_back(cv::Point2f(1403, 320));

  // [1403, 321], [1402, 322], [1396, 322], [1396, 323], [1395, 324], [1389, 324], [1389, 326], [1388, 327],
  points_list.push_back(cv::Point2f(1403, 321));
  points_list.push_back(cv::Point2f(1402, 322));
  points_list.push_back(cv::Point2f(1396, 322));
  points_list.push_back(cv::Point2f(1396, 323));
  points_list.push_back(cv::Point2f(1395, 324));
  points_list.push_back(cv::Point2f(1389, 324));
  points_list.push_back(cv::Point2f(1389, 326));
  points_list.push_back(cv::Point2f(1388, 327));

  // [1382, 327], [1382, 328], [1381, 329], [1376, 329], [1376, 330], [1375, 331], [1369, 331], [1369, 333],
  points_list.push_back(cv::Point2f(1382, 327));
  points_list.push_back(cv::Point2f(1382, 328));
  points_list.push_back(cv::Point2f(1381, 329));
  points_list.push_back(cv::Point2f(1376, 329));
  points_list.push_back(cv::Point2f(1376, 330));
  points_list.push_back(cv::Point2f(1375, 331));
  points_list.push_back(cv::Point2f(1369, 331));
  points_list.push_back(cv::Point2f(1369, 333));

  // [1368, 334], [1362, 334], [1362, 335], [1361, 336], [1359, 336], [1359, 1016], [1365, 1016], [1366, 1017],
  points_list.push_back(cv::Point2f(1368, 334));
  points_list.push_back(cv::Point2f(1362, 334));
  points_list.push_back(cv::Point2f(1362, 335));
  points_list.push_back(cv::Point2f(1361, 336));
  points_list.push_back(cv::Point2f(1359, 336));
  points_list.push_back(cv::Point2f(1359, 1016));
  points_list.push_back(cv::Point2f(1365, 1016));
  points_list.push_back(cv::Point2f(1366, 1017));

  // [1366, 1019], [1430, 1019], [1430, 1017], [1431, 1016], [1440, 1016], [1440, 308]
  points_list.push_back(cv::Point2f(1366, 1019));
  points_list.push_back(cv::Point2f(1430, 1019));
  points_list.push_back(cv::Point2f(1430, 1017));
  points_list.push_back(cv::Point2f(1431, 1016));
  points_list.push_back(cv::Point2f(1440, 1016));
  points_list.push_back(cv::Point2f(1440, 308));

  cv::Mat pointsf;
  cv::Mat(points_list).convertTo(pointsf, CV_32F);

  cv::RotatedRect ellipse_params = cv::fitEllipseAMS(pointsf);
  std::cout << "ellipse_params, center=" << ellipse_params.center << " ; size=" << ellipse_params.size
    << " ; angle=" << ellipse_params.angle << std::endl;

  cv::TickMeter tm;
  tm.start();
  cv::Mat dist_map_8u = drawEllipseDistanceMap(ellipse_params);
  tm.stop();
  std::cout << "Elapsed time: " << tm.getAvgTimeSec() << " sec" << std::endl;

  cv::Point center(ellipse_params.center.x, ellipse_params.center.y);
  cv::Point axis(ellipse_params.size.width/2, ellipse_params.size.height/2);
  std::vector<cv::Point> ellipse_pts_list;
  cv::ellipse2Poly(center, axis, ellipse_params.angle, 0, 360, 1, ellipse_pts_list);
  cv::polylines(dist_map_8u, ellipse_pts_list, false, cv::Scalar(0, 0, 0), 3);

  // Points to be fitted
  cv::Mat closest_pts;
  cv::getClosestEllipsePoints(ellipse_params, pointsf, closest_pts);
  for (int i = 0; i < closest_pts.rows; i++) {
    cv::Point pt;
    pt.x = closest_pts.at<cv::Point2f>(i).x;
    pt.y = closest_pts.at<cv::Point2f>(i).y;
    cv::circle(dist_map_8u, pt, 8, cv::Scalar(0, 0, 255), 2);
  }

  cv::imwrite("dist_map_8u.png", dist_map_8u);

  return EXIT_SUCCESS;
}