#ifndef CALIBRATION__BOARD_PATTERN_HPP
#define CALIBRATION__BOARD_PATTERN_HPP

#include <algorithm>
#include <cctype>
#include <stdexcept>
#include <string>
#include <vector>

#include <opencv2/opencv.hpp>
#include <yaml-cpp/yaml.h>

namespace calibration
{
enum class PatternType
{
  circles_grid,
  chessboard
};

struct BoardPattern
{
  cv::Size pattern_size;
  double center_distance_mm;
  PatternType pattern_type;
};

inline std::string normalize(std::string text)
{
  std::transform(
    text.begin(), text.end(), text.begin(),
    [](unsigned char c) { return static_cast<char>(std::tolower(c)); });
  return text;
}

inline PatternType parse_pattern_type(const YAML::Node & yaml)
{
  if (!yaml["pattern_type"]) return PatternType::circles_grid;

  auto raw = normalize(yaml["pattern_type"].as<std::string>());
  if (raw == "circles_grid" || raw == "circle_grid" || raw == "circles") {
    return PatternType::circles_grid;
  }
  if (raw == "chessboard" || raw == "checkerboard") {
    return PatternType::chessboard;
  }

  throw std::runtime_error(
    "Unsupported pattern_type: " + raw +
    ". Supported values: circles_grid, chessboard.");
}

inline std::string pattern_name(PatternType pattern_type)
{
  return pattern_type == PatternType::chessboard ? "chessboard" : "circles_grid";
}

inline BoardPattern load_board_pattern(const YAML::Node & yaml)
{
  BoardPattern board_pattern;
  board_pattern.pattern_size = cv::Size(yaml["pattern_cols"].as<int>(), yaml["pattern_rows"].as<int>());
  board_pattern.center_distance_mm = yaml["center_distance_mm"].as<double>();
  board_pattern.pattern_type = parse_pattern_type(yaml);
  return board_pattern;
}

inline bool find_pattern_points(
  const cv::Mat & img, const BoardPattern & board_pattern, std::vector<cv::Point2f> & points)
{
  if (board_pattern.pattern_type == PatternType::circles_grid) {
    return cv::findCirclesGrid(
      img, board_pattern.pattern_size, points, cv::CALIB_CB_SYMMETRIC_GRID);
  }

  cv::Mat gray;
  if (img.channels() == 1)
    gray = img;
  else
    cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);

  auto flags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_FAST_CHECK;
  auto success = cv::findChessboardCorners(gray, board_pattern.pattern_size, points, flags);
  if (!success) return false;

  cv::cornerSubPix(
    gray, points, cv::Size(11, 11), cv::Size(-1, -1),
    cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 1e-3));
  return true;
}

inline std::vector<cv::Point3f> planar_points(
  const cv::Size & pattern_size, const double center_distance_mm)
{
  std::vector<cv::Point3f> points;
  points.reserve(static_cast<size_t>(pattern_size.width * pattern_size.height));

  for (int i = 0; i < pattern_size.height; i++) {
    for (int j = 0; j < pattern_size.width; j++) {
      points.push_back({static_cast<float>(j * center_distance_mm), static_cast<float>(i * center_distance_mm), 0.0f});
    }
  }

  return points;
}

}  // namespace calibration

#endif  // CALIBRATION__BOARD_PATTERN_HPP