#ifndef STEREOCALIBRATION_HPP
#define STEREOCALIBRATION_HPP

#include "calibration.hpp"

class StereoCalibration : public Calibration {
public:
	StereoCalibration(dv::RuntimeConfig *config_, dv::RuntimeOutputs *outputs_, dv::RuntimeInput<dv::Frame> &input1,
		dv::RuntimeInput<dv::Frame> &input2);
	bool isCalibrated() override;
	bool getInput() override;
	bool searchPattern() override;
	void checkImages() override;
	bool calibrate() override;
	void saveCalibrationData() override;

private:
	static size_t otherCameraID(const size_t source) {
		const size_t other = (source == 0) ? (1) : (0);
		return (other);
	}

	void writeToFile(cv::FileStorage &fs) override;
	std::string getDefaultFileName() override;

	double errorFunction(const size_t i);
	double epipolarLinesError();

	void extraOutputVisualization(const std::vector<cv::Point2f> &points, const bool found, const size_t source);

	std::vector<cv::Vec3f> epipolarLines(const std::vector<cv::Point2f> &sigleImagePoints, const size_t source);
	void drawEpipolarLines(cv::Mat &output, const std::vector<cv::Point2f> &points, const size_t source);

	void checkPointsOrder(std::vector<std::vector<cv::Point2f>> &points);
	double calibrateStereo();
	void loadCalibrationStereo(const std::string &filename);

	bool stereoCalibrated     = false;
	double totalEpipolarError = 0.0;
	cv::Mat R;
	cv::Mat T;
	cv::Mat E;
	cv::Mat F;
};

#endif // STEREOCALIBRATION_HPP
