/* 
 * File:   Stereo.h
 * Author: carlos
 *
 * Created on 5 de diciembre de 2012, 10:34
 */

#ifndef STEREO_H
#define	STEREO_H

//#include "structs.h"

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

using namespace std;

class Stereo {
public:

    /** 
     * \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[5]; /**< Distortion coefficients - two coefficients for radial distortion and another two for tangential: [ k1 k2 p1 p2 k3]. */
    } 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;

    typedef struct {
        t_Point center;
        vector<t_Point> pts;
        double height;
        t_Point centerH;
    } t_Cell;
    
    typedef struct {        
        vector<int> vect;        
    } t_VecInt;

    Stereo();
    Stereo(const Stereo& orig);
    virtual ~Stereo();


    int openStereoFile(string leftPath, string rightPath, string calibPath, string mask, int width, int height, int numChannels);
    int setMaskFile(string maskFile);
    int getVideoLength();
    int readFrame(int n);
    int showMask();
    int showLeft();
    int showRight();
    int showUndistLeft();
    int showUndistRight();
    int processDisparity();
    int showDisparity();
    cv::Mat getLeft();
    cv::Mat getRight();
    cv::Mat getUndistLeft();
    cv::Mat getUndistRight();
    cv::Mat getImg3D();
    cv::Mat getDisparity();
    void setLeft(cv::Mat m);
    void setRight(cv::Mat m);
    void setUndistLeft(cv::Mat m);
    void setUndistRight(cv::Mat m);
    void setDisparity(cv::Mat m);
    void setImg3D(cv::Mat m);

    void showCurvImg(pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr curv, 
                         pcl::PointCloud<pcl::PointXYZRGB>::Ptr xyz, 
                         cv::Mat img, float threshold, int r, int g, int b);
    
    int get3dPoint(int u, int v, Stereo::t_Point *pt);
    int closeStereoFile();
    int getPoint_3dImage(int u, int v, Stereo::t_Point* pt);

    int initKittiStereo(string calibFile, string leftDir, string rightDir, string pcdDir);
    int exampleOpencvBaseKitti(string imgL, string imgR, cv::Mat *imgDisp,
            int nDisp, int sadWind, int preFiltSize, int preFiltCap, int minDisp, int textureTh,
            int uinqRatio, int speckleWind, int speckleRange, int disp12MaxDiff);

    int sgbmKitti(string imgL, string imgR, cv::Mat *imgDisp,
            int minDisp, int nDisp, int sadWind, int p1, int p2,
            int disp12MaxDiff, int preFiltCap,
            int uinqRatio, int speckleWind, int speckleRange);

    int compute3dKitti(string pcdName, string img25Name, bool save, double pitch, double roll, double yaw, double transX, double transY, double transZ, bool manual);

    int getCurrFrame() const {
        return m_currFrame;
    }


    int exampleOpencvBase(string imgL, string imgR, cv::Mat *imgDisp, int nDisp, int sadWind);

    PclPointCloud getCloud() {
        return m_cloud;
    }

    int initLlorcaCam(double pitch, double roll, double yaw, double camHeight);

    int updateDEM(cv::Mat demZ, cv::Mat *demFull);
    int showDEM(cv::Mat demFull, cv::Mat imgIn, string windName);
    int computeDEM_maxHist(cv::Mat imgIn, cv::Mat img3d, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
            int cellWidth, int cellHeight, double beansSize, double beanStart, cv::Mat *imgOut, cv::Mat *demFull);

    int computeDEM_maxHistEfficient(cv::Mat imgIn, cv::Mat img3d, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
            int cellWidth, int cellHeight, double beansSize, double beanStart, cv::Mat *imgOut, cv::Mat *demFull);
    
    int computeDEM_maxHistEfficientBinary(cv::Mat imgIn, cv::Mat img3d, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
            int cellWidth, int cellHeight, double beansSize, double beanStart, cv::Mat *imgOut, cv::Mat *demFull);
    
    int saveMatToXmlFile(cv::Mat mat, string xmlFile);
    int readMatFromXmlFile(string xmlFile, cv::Mat *mat);
    int saveDemToBinFile(cv::Mat mat, string binFile);
    int readDemFromBinFile(string binFile, cv::Mat *mat);

private:
    int readKittiStereoCalibFiles(string fileName, t_CalibParams * calib_params);

    int LoadCalibParams(const char* file_name, t_CalibParams* calib_params);

    void ComputeStereo(t_CalibParams* calib_params, t_Map *map_matrices, cv::StereoBM *bm_state);

    int InitStereo(t_Map *map_matrices, t_CalibParams* calib_params, cv::StereoBM *bm_state, string calibFile);

    //    void GetPointCloudStereo(t_Map *map_matrices, t_ImagesStereo *images_stereo, t_PointCloud *point_cloud);

    void GetDisparityMapStereo(t_Map *map_matrices, cv::StereoBM *bm_state, t_ImagesStereo *images_stereo);

    int AllocImagesStereo(t_CalibParams* calib_params, t_ImagesStereo *images_stereo);

    void ShowImagesStereo(t_ImagesStereo *images_stereo);

    void SaveImagesStereo(char dir_str[], int frame, t_ImagesStereo *images_stereo);

    void LoadImagesStereo(char dir_str[], int frame, t_ImagesStereo *images_stereo);

    int readNachoStereoFile(string pathFile, t_CalibParams* calib_params);

    int getNearestCell(vector<t_Cell> cells, float x, float y, float z);

    int fromCam3D_to_cloud(float xCam, float yCam, float zCam, float *xCloud, float *yCloud, float *zCloud);

    int fromCloud_to_cam3D(float xCloud, float yCloud, float zCloud, float *xCam, float *yCam, float *zCam);

    int fromCam2D_to_cloud(cv::Mat img3d, int u, int v, float *xCloud, float *yCloud, float *zCloud);

    double distPtPlaneSigned(pcl::PointXYZRGB pt, pcl::ModelCoefficients coeff);

    void getPlaneDegreeInclination(pcl::ModelCoefficients plane, double* x, double* y, double* z);

    void getPlaneRadiansInclination(pcl::ModelCoefficients plane, double* x, double* y, double* z);

    int calcHistMaxValue(vector<t_Point> pts, pcl::ModelCoefficients coeff, double *beanMin, double *beanMax, int *beanCnt, const double beanSize, const double beanStart);

    int exampleOpencv(t_ImagesStereo *images_stereo, t_Map *map_matrices, cv::StereoBM bm_state, cv::Mat imgMask);



    t_Map m_map_matrices;
    t_CalibParams m_calib_params;
    cv::StereoBM m_bm_state;
    t_ImagesStereo m_images_stereo;
    FILE* m_leftFile;
    FILE* m_rightFile;
    int m_currFrame;
    cv::Mat m_mask;
    int m_frameWidth;
    int m_frameHeight;
    int m_numChannels;
    string m_outDir;
    int m_videoLength;

    string m_kittiLeft;
    string m_kittiRight;
    string m_kittiPCD;
    cv::Mat m_k00, m_d00, m_k01, m_d01, m_r01, m_t01;
    cv::Mat m_rRect00, m_rRect01, m_pRect00, m_pRect01, m_q;
    PclPointCloud m_cloud;
    t_CameraLlorca m_camLlorca;
};

#endif	/* STEREO_H */