/* 
 * File:   StereoKitti.h
 * Author: carlos
 *
 * Created on 6 de junio de 2013, 18:13
 */

#ifndef STEREOKITTI_H
#define	STEREOKITTI_H

#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>


#include <dirent.h>
#include <sys/stat.h>
#include <vector>
#include <string>
#include <iostream>
#include <pcl-1.6/pcl/visualization/pcl_visualizer.h>
//#include <pcl/visualization/pcl_visualizer.h>
#include <pcl-1.6/pcl/io/pcd_io.h>
#include <pcl-1.6/pcl/point_types.h>
#include <opencv/highgui.h>
#include <opencv/cv.h>
#include <opencv2/calib3d/calib3d.hpp>

using namespace std;

class StereoKitti {
public:
    StereoKitti();
    StereoKitti(const StereoKitti& orig);
    virtual ~StereoKitti();
   
    int readCalibrationFile(string calibFile);
    int computeDispMap(int leftCamId, int rightCamId);
    int computePointCloud();
    int savePointCloud(string pcdFile);
    
    
    
private:
    
    
    /** 
    * \struct t_CalibParams 
    * \brief Contains the calibration parameters of one camera.
    **/
    typedef struct t_Camera
    {
        int img_size[2]; /**< Correspond with the width and height of the camera view. Two elements of the array.  */
        double matrix[9]; /**< Intrinsic parameters: [fx 0 cx;0 fy cy;0 0 1 ].*/
        double distortion[4]; /**< Distortion coefficients - two coefficients for radial distortion and another two for tangential: [ k1 k2 p1 p2 ]. */
    } t_Camera;
    
    /** 
    * \struct t_CalibParams 
    * \brief Contains the calibration parameters.
    **/
    typedef struct t_CalibParams
    {
        t_Camera camera[2]; /**< Two individual camera parameters. */
        double fund_matrix[9]; /**< Fundamental matrix. */
        double rot_matrix[9]; /**< Rotation matrix. */
        double trans_vector[3]; /**< Translation vector. */
        double yaw; /**< Yaw angle. */
        double pitch; /**< Pitch angle. */
        double roll; /**< Roll angle. */
        double height; /**< Height of cameras (mm). */
    } t_CalibParams;
    
    /** 
    * \struct t_Map 
    * \brief Contains matrices for computing the undistortion and rectification. 
    **/
    typedef struct
    {
        cv::Mat mrx1; /**< Mapping matrix. */
        cv::Mat mrx2; /**< Mapping matrix. */
        cv::Mat mry1; /**< Mapping matrix. */
        cv::Mat mry2; /**< Mapping matrix. */
        cv::Mat mq; /**< Disparity-to-depth mapping matrix. */
    } t_Map;
    
    /** 
    * \struct t_ImagesStereo 
    * \brief Contains images to run stereovision. 
    **/
    typedef struct
    {
        cv::Mat left_image; /**< Left original image. */
        cv::Mat right_image; /**< Right original image. */
        cv::Mat undist_left_image; /**< Left image of video without distortion. */
        cv::Mat undist_right_image; /**< Right image of video without distortion. */
        cv::Mat undist_left_image_gray; /**< Left image of video without distortion in gray levels. */
        cv::Mat undist_right_image_gray; /**< Right image of video without distortion in gray levels. */
        cv::Mat disp_image; /**< Disparity map image. */
        cv::Mat vdisp_image; /**< Disparity map image in level grey. */
        cv::Mat recons3D; /**< 3 channel matrix for 3D info (ch1=x, ch2=y ch3=z).*/

    } t_ImagesStereo;
    
    /** 
    * \struct t_Point 
    * \brief Contains all data of a point.
    **/
    typedef struct
    {
        double x; /**< Coordinate x. */
        double y; /**< Coordinate y. */
        double z;/**< Coordinate z. */ 
        int r; /**< Value of red channel. */
        int g; /**< Value of green channel. */
        int b; /**< Value of blue channel. */
        float rgb; /**< Value of rgb. */
        int u; /**< u coordinate in the image. */
        int v; /**< v coordinate in the image. */
    } t_Point;

    int getPoint_3dImage(int u, int v, StereoKitti::t_Point* pt);
    
    t_Map m_map_matrices;
    t_CalibParams m_calib_params;
    cv::StereoBM m_bm_state;
    t_ImagesStereo m_images_stereo;
    cv::Mat m_mask;
    int m_frameWidth;
    int m_frameHeight;
    int m_numChannels;
    string m_outDir;
    
    
    
};

#endif	/* STEREOKITTI_H */