
#ifndef _STEREORIG_H_
#define _STEREORIG_H_

//=============================================================================
// Includes
//=============================================================================
#include "global_types.h"

//=============================================================================
// StereoRig Class Declaration
//=============================================================================
class StereoRig
{

private:

	   // Parameters of the stereo rig
	   double Fx;	// Camera focal length in pixels
	   double Fy;	// Fx = Fy if the stereo is rectified
	   double u0;	// Horizontal pixelic central point
	   double v0;	// Vertical pixelic central point
	   double B;		// Stereo Camera baseline in (mm)
	   unsigned int width;	// Image width in pixels
	   unsigned int height;  // Image height in pixels
	   unsigned int minDisp; // Minimum disparity
	   unsigned int maxDisp; // Maximum Disparity
	   TooN::Matrix<3,3> ML; // ML Left Camera Matrix
	   TooN::Matrix<3,3> MR; // MR Right Camera Matrix	
   	   TooN::Matrix<3,4> ML_vo; // ML Left Camera Matrix
	   TooN::Matrix<3,4> MR_vo; // MR Right Camera Matrix	
   	   TooN::Matrix<3,3> CL; // CL Left Camera Matrix Undistorted+Rectified
	   TooN::Matrix<3,3> CR; // CR Right Camera Matrix Undistorted+Rectified
   	   TooN::Matrix<3,3> ML_inv; // ML Inverse Left Camera Matrix
	   TooN::Matrix<3,3> MR_inv; // MR Inverse Right Camera Matrix	
	   TooN::Matrix<3,3> R; // Rotation Matrix
	   TooN::Matrix<3,3> E; // Essential Matrix
	   TooN::Matrix<3,3> F; // Fundamental Matrix
	   TooN::Vector<3> T; // Translation vector
	   TooN::Vector<5> DL; // Left Camera Distortion Parameters
	   TooN::Vector<5> DR; // Right Camera Distortion Parameters
	   char calibration_info[MAX_NUMBER_CHARACTERS]; // StereoRig Calibration Information
   	   IplImage *img_left; // Current undistorted left image
	   IplImage *img_right; // Current undistorted right image
	   IplImage *disp, *img_disp_c; // Disparity images
	   CvMat *mxl, *myl, *mxr, *myr; // CvMat necessary for image undistortion process
	   CvStereoBMState *BMState; // Structure for computing the disparity map with block matching
	   double trectification, tdisparity; // For checking computation times
	   
public:

	   // Constructor
	   StereoRig();
	   StereoRig(tcamera_calib *stereo);

	   // Destructor
	   ~StereoRig(void);

	   // Set Methods
	   void Set_Baseline(double Bx){B = Bx;}
	   void Set_Focal_Length_F(double fl){Fx = Fy = fl;}
	   void Set_u0(double u0aux){u0 = u0aux;}
	   void Set_v0(double v0aux){v0 = v0aux;}
	   
	   void Set_Image_Width(unsigned int imwidth){width = imwidth;}
	   void Set_Image_Height(unsigned int imheight){height = imheight;}
	   void Set_minDisp(unsigned int Disp){minDisp = Disp;}
	   void Set_maxDisp(unsigned int Disp){maxDisp = Disp;}

	   void Set_Previous_Left_Image(IplImage *current);
   	   void Set_ML(TooN::Matrix<3,3> aux){ML = aux;}
	   void Set_MR(TooN::Matrix<3,3> aux){MR = aux;}
	   void Set_E(TooN::Matrix<3,3> aux){E = aux;}
	   void Set_F(TooN::Matrix<3,3> aux){F = aux;}
	   void Set_CL_Values(void);
	   void Set_CR_Values(void);
	   void Set_ML_Values(void);
	   void Set_MR_Values(void);
	   void Set_ML_VO_Values(void);
	   void Set_MR_VO_Values(void);
	   
	   // Get Methods
	   double Get_Baseline(void){return B;}
	   double Get_Focal_Length(void){return Fx;}
	   double Get_u0(void){return u0;}
	   double Get_v0(void){return v0;}
	   unsigned int Get_Image_Width(void){return width;}
	   unsigned int Get_Image_Height(void){return height;}
	   unsigned int Get_minDisp(void){return minDisp;}
	   unsigned int Get_maxDisp(void){return maxDisp;}
   	   double Get_Disparity_32(int u, int v);
	   int Get_Disparity_8u(int u, int v);
	   void Get_XYZ_hi(double &x, double &y, double &z, int u, int v);
	   void Get_XYZ_hi(double &x, double &y, double &z, double u, double v);
	   TooN::Vector<3> Get_XYZ_hi(int u, int v);
	   TooN::Vector<3> Get_XYZ_hi(double u, double v);
   	   TooN::Matrix<3,3> Get_ML(void){return ML;}
	   TooN::Matrix<3,3> Get_MR(void){return MR;}
   	   TooN::Matrix<3,3> Get_CL(void){return CL;}
	   TooN::Matrix<3,3> Get_CR(void){return CR;}
   	   TooN::Matrix<3,4> Get_ML_VO(void){return ML_vo;}
	   TooN::Matrix<3,4> Get_MR_VO(void){return MR_vo;}
	   IplImage *Get_Left_Image(void){return img_left;}
	   IplImage *Get_Right_Image(void){return img_right;}
	   IplImage *Get_Disparity_Color_Image(void){return img_disp_c;}
	   double Get_TRectification(void){return trectification;}
	   double Get_TDisparity(void){return tdisparity;}
	   
	   // Other Methods
	   void Project_Left_Camera_3D_Point(double &u, double &v, TooN::Vector<3> hi);
	   void Project_Right_Camera_3D_Point(double &u, double &v, TooN::Vector<3> hi);
	   void Project_Left_Camera_Yi3D_Point(double &u, double &v, TooN::Vector<3> Xcam, 
					       TooN::Matrix<3,3> R_CW,TooN::Vector<3> Yi);
	   void Project_Right_Camera_Yi3D_Point(double &u, double &v, TooN::Vector<3> Xcam, 
					       TooN::Matrix<3,3> R_CW,TooN::Vector<3> Yi);
	   TooN::Matrix<3,3> Compute_3D_Covariance(TooN::Vector<3> hi, double uL, double vL, double uR);
	   void Project_Camera_hi3D_Point(double &u, double &v, const TooN::Vector<3> &hi);
	   void Project_Camera_Yi3D_Point(double &u, double &v, const TooN::Vector<3> &Xcam, const TooN::Matrix<3,3> & R_CW, const TooN::Vector<3> &Yi);
	   void Project_Camera_Yi3D_Point(double &u, double &v, const TooN::Vector<3> &Xcam, const TooN::Matrix<3,3> & R_CW,const TooN::Vector<3> &Yi, TooN::Vector<3> &xs);
	   void UndistortRectify_Init(void);
	   void UndistortRectify_Images(IplImage *img_left_d, IplImage *img_right_d);
	   void Do_Stereo_Processing(IplImage *left, IplImage *right);
	   void Copy_Rectified_Images(IplImage *img_left_d, IplImage *img_right_d);
	   void Compute_Disparity_Map(void);
	   void Depth_Image_to_CV_Image(void);
	   void Show_StereoRig_Calibration_Info(void);
};	

//*************************************************************************************
//*************************************************************************************

#endif
