#ifndef CALIBRATION_HPP
#define CALIBRATION_HPP

#include "../../include/dv-sdk/config.hpp"
#include "../../include/dv-sdk/log.hpp"
#include "../../include/dv-sdk/module_io.hpp"

#include <boost/filesystem.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>

class Calibration {
public:
	enum class CalibrationPattern {
		CHESSBOARD,
		CIRCLES_GRID,
		ASYMMETRIC_CIRCLES_GRID
	};
	static inline const std::string FILE_FORMAT       = "xml";
	static inline const std::string IMAGE_FORMAT      = "png";
	static inline const std::string STATISTICS_FORMAT = "csv";

	virtual ~Calibration() {
	}

	virtual bool isCalibrated() = 0;
	/**
	 * Fill up camera[X].input with possible images to process.
	 *
	 * @param input frame input to save for later processing.
	 * @param source camera ID (0/1).
	 */
	void addInput(const dv::InputDataWrapper<dv::Frame> &input, const size_t source);
	/**
	 * Get last possible input to process from camera[X], store converted
	 * grayscale image in camera[X].currentImage.
	 * Also updates 'currentTimestamp' with this inputs' timestamp if successful.
	 *
	 * @return wether a viable new input was found.
	 */
	virtual bool getInput()            = 0;
	virtual bool searchPattern()       = 0;
	virtual void checkImages()         = 0;
	virtual bool calibrate()           = 0;
	virtual void saveCalibrationData() = 0;

	bool enoughImagesCollected();
	void clearImages();
	void regeneratePatternPoints();

private:
	std::string saveFilePath();

protected:
	static bool cvExists(const cv::FileNode &fn);

	static void putText(const std::string &str, cv::Mat &img, const int maxWidth = 320);

	virtual void writeToFile(cv::FileStorage &fs) = 0;
	virtual std::string getDefaultFileName()      = 0;

	// utilty
	void setCameraID(const std::string &originDescription, const size_t source);
	void convertInputToGray(dv::InputDataWrapper<dv::Frame> &input, cv::Mat &dest);
	void setDefaultPatternSizes();
	CalibrationPattern getCalibrationPattern();
	cv::Size getBoardSize();
	std::string saveFileTimeSuffix(const std::string &fmt);

	// output visualization
	void updateCurrentOutput(const cv::Mat &image, const std::vector<cv::Point2f> &points, const bool found,
		const size_t source, const bool highlight, const std::string &text = "");
	void sendCurrentOutput(const size_t source);
	void highlightCoveredArea(const size_t source);
	void undistortOutput(const size_t source);

	// loading / saving
	void saveCalibration(const double totalAvgError);
	void writeToFileCamera(cv::FileStorage &fs, const size_t source);
	void loadCalibrationCamera(const std::string &filename, const size_t source);
	void savePNGImages(const size_t source);

	// calibration
	static cv::Size getSubPixelSearchWindowHalfSize(const std::vector<cv::Point2f> &points);
	bool findPattern(const cv::Mat &image, std::vector<cv::Point2f> &points);
	double calibrateCamera(const size_t source);

	// parameters
	double totalReprojectionError = -1.0; // -1.0 to signal no calibration has been run.
	int64_t currentTimestamp      = 0;
	size_t checkedImages          = 0;
	size_t consecFound            = 0;

	dv::RuntimeConfig *config;
	dv::RuntimeOutputs *outputs;
	dv::Logger log; // Logger always forwards to dv::Log().

	struct Camera {
		bool cameraCalibrated = false;
		std::string cameraID;
		cv::Size imageSize;
		std::vector<dv::InputDataWrapper<dv::Frame>> input;
		// calibration in-progress
		cv::Mat currentImage;
		cv::Mat overlay;
		cv::Mat currentOutput;
		std::vector<std::vector<cv::Point2f>> imagePoints;
		std::vector<cv::Mat> images;
		std::vector<cv::Mat> rVecs;
		std::vector<cv::Mat> tVecs;
		// calibration results
		cv::Mat cameraMatrix;
		cv::Mat distCoeffs;
		// loaded matrices
		cv::Mat loadedCameraMatrix = cv::Mat::eye(3, 3, CV_64F);
		cv::Mat loadedDistCoeffs   = cv::Mat::zeros(8, 1, CV_64F);
	};

	std::array<Camera, 2> camera;

	// Object points depend on pattern only.
	std::vector<cv::Point3f> patternPoints;
};

#endif // CALIBRATION_HPP
