#pragma once

#include <array>
#include <iostream>
#include <string>
#include <vector>

// =================================================================================================

enum class CameraModel
{
    Pinhole,
    Fisheye,
};

const char *camera_model_name(CameraModel model);
CameraModel parse_camera_model(const std::string &value);

// =================================================================================================

struct Camera
{
    std::string name;
    std::array<std::array<float, 3>, 3> K;
    std::array<float, 5> DC = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
    std::array<std::array<float, 3>, 3> R;
    std::array<std::array<float, 1>, 3> T;
    int width = 0;
    int height = 0;
    CameraModel model = CameraModel::Pinhole;
    std::array<std::array<float, 3>, 3> invR {};
    std::array<float, 3> center {};
    std::array<std::array<float, 3>, 3> newK {};
    std::array<std::array<float, 3>, 3> invK {};

    friend std::ostream &operator<<(std::ostream &out, Camera const &camera);
    std::string to_string() const;
};

Camera make_camera(
    std::string name,
    std::array<std::array<float, 3>, 3> K,
    std::array<float, 5> DC,
    std::array<std::array<float, 3>, 3> R,
    std::array<std::array<float, 1>, 3> T,
    int width,
    int height,
    CameraModel model);

void undistort_point_pinhole(std::array<float, 3> &point, const std::array<float, 5> &distortion);
void undistort_point_fisheye(std::array<float, 3> &point, const std::array<float, 5> &distortion);