#pragma once

#include <algorithm>
#include <cmath>
#include <optional>
#include <string>
#include <vector>

#include "origincar_birdseye/birdseye_geometry.hpp"
#include "origincar_birdseye/ground_projection.hpp"

namespace origincar_birdseye
{

inline bool allFinite(const std::vector<double> & values)
{
  return std::all_of(
    values.begin(), values.end(), [](double value) {
      return std::isfinite(value);
    });
}

inline std::optional<std::string> validateCameraConfiguration(
  const CameraIntrinsics & intrinsics, const std::vector<double> & rotation,
  const std::vector<double> & translation)
{
  if (intrinsics.width <= 0 || intrinsics.height <= 0 ||
    !std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) ||
    !std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) ||
    intrinsics.fx <= 0.0 || intrinsics.fy <= 0.0)
  {
    return "camera intrinsics and input dimensions must be finite and positive";
  }
  if (rotation.size() != 9 || translation.size() != 3 ||
    !allFinite(rotation) || !allFinite(translation))
  {
    return "T_camera_from_base must contain finite 3x3 rotation and 3D translation values";
  }
  return std::nullopt;
}

inline std::optional<std::string> validateCanvas(
  const BirdseyeCanvas & canvas, const std::string & name)
{
  if (canvas.width <= 0 || canvas.height <= 0) {
    return name + " canvas width and height must be positive";
  }
  if (!std::isfinite(canvas.meters_per_pixel) || canvas.meters_per_pixel <= 0.0 ||
    !std::isfinite(canvas.rear_meters) || !std::isfinite(canvas.left_meters))
  {
    return name + " canvas metric parameters must be finite and meters_per_pixel must be positive";
  }
  return std::nullopt;
}

inline std::optional<std::string> validateCoefficientVector(
  const std::vector<double> & values, size_t minimum_size, const std::string & name)
{
  if (values.size() < minimum_size || !allFinite(values)) {
    return name + " must contain the required number of finite values";
  }
  return std::nullopt;
}

}  // namespace origincar_birdseye
