#ifndef DEPTHGUIDE_CAMERA_CALIBRATOR_IMPL_H
#define DEPTHGUIDE_CAMERA_CALIBRATOR_IMPL_H

#include "image_process/camera_calibrator.h"
#include "core/image_utility_v2.h"
#include "core/math_helper.hpp"
#include "render/render_utility.h"

#include <opencv2/core/mat.hpp>

#include <nanovg.h>

#include <BS_thread_pool.hpp>

#include <atomic>
#include <list>

namespace camera_calibrator_impl {

    // TODO: make configurable
    static constexpr auto sharpness_threshold = 4.f;
    static constexpr auto track_err_threshold = 0.25f;

    using corner_type = std::vector<cv::Point2f>;

    cv::Mat to_cv_mat(const glm::mat3 &mat);

    cv::Mat to_cv_mat(const glm::vec3 &vec);

    glm::mat3 rodrigues_to_mat(const cv::Mat &vec);

    cv::Mat to_cv_rodigues(const glm::mat3 &mat);

    glm::mat3 to_mat3(const cv::Mat &mat);

    glm::vec3 to_vec3(const cv::Mat &mat);

    struct hand_eye_calib {

        // fill before use

        transform_buffer track_pool;

        struct image_store_type {
            corner_type corner;
            timestamp_type sample_ts;
        };

        using image_pool_type =
                std::vector<image_store_type>;
        image_pool_type img_pool;

        cv::Size img_size;

        // fill by member functions

        using object_points_type =
                std::vector<cv::Vec3f>;
        object_points_type obj_ps;

        // result
        glm::mat3 intrinsic_mat; // intrinsic matrix
        using dist_coeffs_type = std::vector<float>;
        dist_coeffs_type dist_coeffs; // distortion coefficients
        glm::mat4 result_mat; // result matrix, camera in camera reference
        int result_ts_offset; // temporal latency between tracker and camera

        // result error
        float obj_reproj_err; // average error of spacial corners after hand-eye calibration, in mm
        float img_reproj_err; // average error of image corners after hand-eye calibration, in pixel

        void set_object_points(cv::Size size, float dis); // dis: in mm

        void calc();

        camera_intrinsic_v0 intrinsic_v0();

    private:

        // interpolate track matrix
        glm::mat4 calc_track_mat(timestamp_type t);

        // p: in the normalized plane
        glm::vec2 distort_point(glm::vec2 p);

        // p: in camera coordinate
        // result: in pixel
        // duplicate cv::projectPoints
        glm::vec2 project_point(glm::vec3 p);

        // average distance between two sets of corner points
        static float corner_distance(const corner_type &c1,
                                     const corner_type &c2);

        struct camera_calib_result {
            cv::Mat intrinsic, dist_coeffs;
        };

        using img_index_list_type = std::vector<size_t>;

        camera_calib_result camera_calib(const img_index_list_type &index);

        float reproject_error(const camera_calib_result &cam,
                              const corner_type &img_ps);

        camera_calib_result camera_calib_ransac();

        // do hand-eye calibration
        // return reference in camera
        glm::mat4 calib_hand_eye();

        // evaluate hand-eye calibration result
        cv::Scalar evaluate_hand_eye(const glm::mat4 &ret_mat);

        size_t sample_num, corner_num;
        camera_calib_result cam_cv;
        int ts_offset = 0; // timestamp offset
        std::vector<cv::Mat> cam_r_vec; // chess board in camera
        std::vector<cv::Mat> cam_t_vec;
        cv::Mat aux_r, aux_t; // tracker in chess board
    };

}

using namespace camera_calibrator_impl;

struct camera_calibrator::impl {

    create_config conf;

    using conn_type = boost::signals2::connection;
    conn_type img_conn;

    struct img_store_type {
        image_ptr img;
        cv::Mat img_mat;
        timestamp_type sample_ts;
        corner_type corners;
        float corner_sharpness;

        bool process_finished: 1 = false;
        bool corners_detected: 1 = false;
    };

    using img_pool_type = std::list<img_store_type>;
    img_pool_type img_pool;
    std::atomic_int img_ok_cnt = 0;

    struct track_store_type {
        glm::mat4 ref_mat;
        float track_error;
        timestamp_type sample_ts;
    };

    using track_pool_type = std::vector<track_store_type>;
    track_pool_type track_pool;

    std::atomic<img_store_type *> last_finish = nullptr;
    float last_track_err = std::numeric_limits<float>::quiet_NaN();
    std::unique_ptr<smart_frame_buffer> coverage_fbo;
    NVGcontext *vg = nullptr;

    std::unique_ptr<BS::thread_pool> tp; // thread pool

    cv::Size img_size;
    std::unique_ptr<hand_eye_calib> calib;

    explicit impl(const create_config &conf);

    ~impl();

    void per_image_process(img_store_type *st);

    void img_callback(obj_name_type name);

    void save_track_data();

    void load_track_data(const std::string &path);

    void process(); // do calibration

    void simulate_process(const simulate_info_type &info);

    void start();

    void stop(bool on_exit = false);

    void render_corners(const corner_type &corner, NVGcolor color);

    void render();

    void show();

};

#endif //DEPTHGUIDE_CAMERA_CALIBRATOR_IMPL_H