#ifndef ORIGINCAR_BIRDSEYE__GROUND_PROJECTION_HPP_
#define ORIGINCAR_BIRDSEYE__GROUND_PROJECTION_HPP_

#include <array>
#include <optional>

namespace origincar_birdseye
{

struct CameraIntrinsics
{
  double fx;
  double fy;
  double cx;
  double cy;
  int width;
  int height;
};

struct GroundPoint
{
  double x;
  double y;
};

struct PixelPoint
{
  double x;
  double y;
};

class RigidTransform
{
public:
  RigidTransform(std::array<double, 9> rotation, std::array<double, 3> translation)
  : rotation_(rotation), translation_(translation)
  {
  }

  std::array<double, 3> apply(const std::array<double, 3> & point) const
  {
    return {
      rotation_[0] * point[0] + rotation_[1] * point[1] + rotation_[2] * point[2] + translation_[0],
      rotation_[3] * point[0] + rotation_[4] * point[1] + rotation_[5] * point[2] + translation_[1],
      rotation_[6] * point[0] + rotation_[7] * point[1] + rotation_[8] * point[2] +
      translation_[2]};
  }

private:
  std::array<double, 9> rotation_;
  std::array<double, 3> translation_;
};

class GroundProjection
{
public:
  GroundProjection(CameraIntrinsics intrinsics, RigidTransform camera_from_base)
  : intrinsics_(intrinsics), camera_from_base_(camera_from_base)
  {
  }

  std::optional<PixelPoint> projectBaseGroundPoint(const GroundPoint & point) const
  {
    const auto camera_point = camera_from_base_.apply({point.x, point.y, 0.0});
    if (camera_point[2] <= 0.0) {
      return std::nullopt;
    }

    return PixelPoint{
      intrinsics_.fx * camera_point[0] / camera_point[2] + intrinsics_.cx,
        intrinsics_.fy * camera_point[1] / camera_point[2] + intrinsics_.cy};
  }

private:
  CameraIntrinsics intrinsics_;
  RigidTransform camera_from_base_;
};

}  // namespace origincar_birdseye

#endif  // ORIGINCAR_BIRDSEYE__GROUND_PROJECTION_HPP_
