#pragma once

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

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

struct Camera
{
    std::string name;
    std::array<std::array<float, 3>, 3> K;
    std::vector<float> DC;
    std::array<std::array<float, 3>, 3> R;
    std::array<std::array<float, 1>, 3> T;
    int width;
    int height;
    std::string type;

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

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

class CameraInternal
{
public:
    CameraInternal(const Camera &cam);

    Camera cam;

    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;

    static std::array<std::array<float, 3>, 3> transpose3x3(
        const std::array<std::array<float, 3>, 3> &M);

    static std::array<std::array<float, 3>, 3> invert3x3(
        const std::array<std::array<float, 3>, 3> &M);

    static void undistort_point_pinhole(std::array<float, 3> &p, const std::vector<float> &k);
    static void undistort_point_fisheye(std::array<float, 3> &p, const std::vector<float> &k);

    std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_fisheye(
        float balance, std::pair<int, int> new_size);
    std::array<std::array<float, 3>, 3> calc_optimal_camera_matrix_pinhole(
        float alpha, std::pair<int, int> new_size);
};