//SplinesCalibration.h

//jose miguel sanchiz marti (www3.uji.es/~sanchiz) (sanchiz@uji.es)

#ifndef SPLINES_CALIBRATION_H
#define SPLINES_CALIBRATION_H

#include <vector>

using namespace std;


class SplinesCalibration
{
public:

	struct TEMPLATE_MEASUREMENTS
	{
		float upperSlope,lowerSlope,leftSlope,rightSlope;
		float diagonalsDifference;
		float upperMean,upperDeviation;
		float lowerMean,lowerDeviation;
		float leftMean,leftDeviation;
		float rightMean,rightDeviation;
	};

	struct REGION
	{
		int x1,y1,x2,y2;
	};

	struct SPLINE_CONTROL_POINT
	{
		bool valid; //indicates that the control point is defined
		float uimg,wimg; //point coordinates in the image
		float x,y; //real point coordinates in mm
		float xu,xw,xuw; //partial derivatives in x
		float yu,yw,yuw; //partial derivatives in y
	};

	//regions are specified in an array of 4 ints, containing {x1,y1,x2,y2}, where
	//x1,y1 is the upper-left point of the region (x:column, y:row, and
	//x2,y2 is the lower-right point of the region
	SplinesCalibration(int numberMarksHor, //number of horizontal marks
				int numberMarksVer, //number of vertical marks
				float spaceHor, //space between horizontal marks in mm (upper and lower)
				float spaceVer, //space between horizontal marks in mm (left and right)
				REGION upperRegion,REGION lowerRegion,REGION leftRegion,REGION rightRegion); //regions where the calibration marks are

	~SplinesCalibration();

	//Calling to this routine is needed before calibrating calling PixelToMilimeters
	//organizes the centres of the calibration marks in the upper, lower. left and right regions of the image
	//builds the calibration matrix
	bool OrganizeCalibrationMatrix(float centresX[],float centresY[],int n);

	//computes a pixel coordinates in mm interpolating the splines defined in the calibration matrix
	//(xPix,yPix): column and row of pixel, (xmm,ymm): returns real pixel coordinates in mm
	//returns true if success
	//returns false if the point cannot be calibrated, in that case makes xmm=-1000000.0, ymm=-1000000.0,
	//this happens when the point is outside the regions defined by the calibration marks
	bool PixelToMilimeters(int xPix,int yPix,float & xmm,float & ymm);

	//computes a pixel coordinates in mm interpolating the splines defined in the calibration matrix
	//uses bilinear interpolation and computes the result in subpixel precision
	//(xPix,yPix): column and row of pixel, (xmm,ymm): returns real pixel coordinates in mm
	//returns true if success
	//returns false if the point cannot be calibrated, in that case makes xmm=-1000000.0, ymm=-1000000.0,
	//this happens when the point is outside the regions defined by the calibration marks
	bool PixelToMilimeters(float xPix,float yPix,float & xmm,float & ymm);

	//computes measures about marks alignmnent that can be used to position the camera centred and vertical to the calibration template
	TEMPLATE_MEASUREMENTS TemplateMeasurements();

	//returns vectors with the calibration marks pixel coordinates
	//useful only for monitoring or drawing the marks positions
	void CalibrationMarksCoordinates(vector<int> & upperMarksX,vector<int> & upperMarksY,
		vector<int> & lowerMarksX,vector<int> & lowerMarksY,
		vector<int> & leftMarksX, vector<int> & leftMarksY,
		vector<int> & rightMarksX,vector<int> & rightMarksY);

	//returns the calibration matrix pixel coordinates x
	//useful only for monitoring or drawing the calibrated control points of the splines
	int **CalibrationMatrixCoordX();

	//returns the calibration matrix pixel coordinates y
	//useful only for monitoring or drawing the calibrated control points of the splines
	int **CalibrationMatrixCoordY();
	
	//returns the number of columns of the calibration matrix
	int CalibrationMatrixCols() const;
 
	//returns the number of rows of the calibration matrix
	int CalibrationMatrixRows() const;
 

private:

	struct POINT_PIXEL_MM
	{
		float xPix,yPix;
		float xmm,ymm;
		float sortWeight;
		bool operator<(const POINT_PIXEL_MM &p) const { return (sortWeight<p.sortWeight); }
	};

	//configuration parametres
	int _matrixColsCfg; //number of marks horizontaly
	int _matrixRowsCfg; //number de marks verticaly
	float _horSpace; //space between horizontal marks, in mm. (for upper and lower marks)
	float _verSpace; //space between verticalal marks, in mm. (for left and right marks)
	REGION _upperRegion,_lowerRegion,_leftRegion,_rightRegion; //regions where marks are in the image

	//calibration marks
	vector<POINT_PIXEL_MM> _upperVector,_lowerVector,_leftVector,_rightVector;
	SPLINE_CONTROL_POINT **_matrix;
	int **_matrixCoordX;
	int **_matrixCoordY;
	int _matrixCols,_matrixRows;

	//organizes marks centres in the marks vectors of upper, lower, left and right regions
	void OrganizeMarksCentres(float centresX[],float centresY[],int n);

	//constructs the calibration matrix that defines the splines
	//returns true if success (there are enough calibration points to construct the matrix), false if not
    bool OrganizeCalibratedPointsMatrix();
};

#endif