enum e_sdk_err_code
{
    e_timeout = -7,						// USB传输超时
    e_libusb_io_err = -1,				// libusb库IO错误
    e_sdk_no_err = 0,					// 成功，没有错误
    e_load_libusb_err=1,				// 加载的libusb库错误
    e_sdk_not_inited=2,					// SDK软件还没有准备好
    e_hardware_not_ready=3,				// Guidance硬件还没有准备好
    e_disparity_not_allowed=4,			// 视差图或深度图不允许被选择
    e_image_frequency_not_allowed=5,	// 图像频率必须是枚举类型e_image_data_frequecy之一
    e_config_not_ready=6,				// 配置没有准备好
    e_online_flag_not_ready=7,			// 在线标志没有准备好 
    e_stereo_cali_not_ready = 8,    	// 摄像头标定参数没有准备好
    e_max_sdk_err = 100					// 错误最大数量
};

enum e_vbus_index
{
    e_vbus1 = 1,        // M100上为前视
    e_vbus2 = 2,        // M100上为右视
    e_vbus3 = 3,        // M100上为后视
    e_vbus4 = 4,        // M100上为左视
    e_vbus5 = 0         // M100上为下视
};

enum e_image_data_frequecy
{
    e_frequecy_5  = 0,  // frequecy of image data: 5Hz  
    e_frequecy_10 = 1,  // frequecy of image data: 10Hz  
    e_frequecy_20 = 2   // frequecy of image data: 20Hz  
};

enum e_guidance_event
{
    e_image = 0,            // called back when image comes 
    e_imu,                  // called back when imu comes 
    e_ultrasonic,           // called back when ultrasonic comes 
    e_velocity,             // called back when velocity data comes 
    e_obstacle_distance,    // called back when obstacle data comes 
    e_motion,               // called back when global position comes
    e_event_num
};

typedef struct _image_data
{
    unsigned int     frame_index;	                              // frame index 
    unsigned int     time_stamp;	                              // time stamp of image captured in ms 
    char             *m_greyscale_image_left[CAMERA_PAIR_NUM];	  // greyscale image of left camera 
    char             *m_greyscale_image_right[CAMERA_PAIR_NUM];   // greyscale image of right camera 
    har             *m_depth_image[CAMERA_PAIR_NUM];	          // depth image in *128 meters 
    char             *m_disparity_image[CAMERA_PAIR_NUM];         // disparity image in *16 pixels 
}image_data;

typedef struct _ultrasonic_data
{
    unsigned int     frame_index;    // corresponse frame index 
    unsigned int     time_stamp;     // time stamp of corresponse image captured in ms 
    short            ultrasonic[CAMERA_PAIR_NUM];	// distance in mm. -1 means invalid measurement. 
    unsigned short   reliability[CAMERA_PAIR_NUM];   // reliability of the distance data 
}ultrasonic_data;

typedef struct _velocity
{
    unsigned int     frame_index;        // corresponse frame index 
    unsigned int     time_stamp;         // time stamp of corresponse image captured in ms 
    short            vx;                 // velocity of x in mm/s
    short            vy;                 // velocity of y in mm/s 
    short            vz;                 // velocity of z in mm/s 
}velocity;

typedef struct _obstacle_distance
{
    unsigned int     frame_index;    // corresponse frame index 
    unsigned int     time_stamp;     // time stamp of corresponse image captured in ms 
    unsigned short   distance[CAMERA_PAIR_NUM];  // distance of obstacle in cm
}obstacle_distance;

typedef struct _imu
{
    unsigned int     frame_index;             // corresponse frame index 
    unsigned int     time_stamp;              // time stamp of corresponse image captured in ms 
    float            acc_x;                   // acceleration of x in unit of m/s^2
    float            acc_y;                   // acceleration of y in unit of m/s^2
    float            acc_z;                   // acceleration of z in unit of m/s^2
    float            q[4];                    // quaternion: [w,x,y,z]
}imu;

typedef struct _stereo_cali
{
    float cu;			// x position of focal center in units of pixels 
    float cv;			// y position of focal center in units of pixels 
    float focal;		// focal length in units of pixels 
    float baseline;		// baseline of stereo cameras in units of meters 
    _stereo_cali() { }
    _stereo_cali(float _cu, float _cv, float _focal, float _baseline)
    {
        cu = _cu, cv = _cv;
        focal = _focal, baseline = _baseline;
    }
}stereo_cali;

typedef struct _exposure_param
{
    float	      m_step;		// adjustment step for auto exposure control (AEC). Default is 10.
    float		  m_exposure_time;	// constant exposure time in mini-seconds. Range is 0.1~20. Default is 7.25.
    unsigned int  m_expected_brightness;// constant expected brightness for AEC. Range is 50~200. Default is 85.
    unsigned int  m_is_auto_exposure;	// 1: auto exposure; 0: constant exposure
    int           m_camera_pair_index;	// index of Guidance Sensor
    _exposure_param(){
        m_step = 10;
        m_exposure_time = 7.68;
        m_expected_brightness = 85;
        m_is_auto_exposure = 1;
        m_camera_pair_index = 1;
    }
}exposure_param;

typedef struct _motion
{
    unsigned int     frame_index;
    unsigned int     time_stamp;
    int	             corresponding_imu_index;
    float            q0;
    float            q1;
    float            q2;
    float            q3;
    int              attitude_status;  // 0:invalid; 1:valid
    float            position_in_global_x;  // position in global frame: x 
    float            position_in_global_y;  // position in global frame: y 
    float            position_in_global_z;  // position in global frame: z 
    int              position_status; // lower 3 bits are confidence. 0:invalid; 1:valid
    float            velocity_in_global_x;  // velocity in global frame: x 
    float            velocity_in_global_y;  // velocity in global frame: y 
    float            velocity_in_global_z;  // velocity in global frame: z 
    int              velocity_status; // lower 3 bits are confidence. 0:invalid; 1:valid
    float            reserve_float[8];
    int              reserve_int[4];
    float            uncertainty_location[3];// uncertainty of position
    float            uncertainty_velocity[3];// uncertainty of velocity
} motion;

typedef int (*user_call_back)( int event_type, int data_len, char *data );

SDK_API int reset_config ( void );

SDK_API int init_transfer ( void );

SDK_API void select_imu ( void );

SDK_API void select_ultrasonic ( void );

SDK_API void select_velocity ( void );

SDK_API void select_obstacle_distance ( void );

SDK_API int set_image_frequecy ( e_image_data_frequecy frequecy );

SDK_API int select_depth_image ( e_vbus_index camera_pair_index );

#include "DJI_guidance.h"
#include "DJI_utility.h"
#include "opencv2/opencv.hpp"
#include <stdio.h>
#include <string>
e_vbus_index sensor_id = e_vbus1;
Mat	    g_depth;
int my_callback(int data_type, int data_len, char *content)
{
    g_lock.enter();
    if (e_image == data_type && NULL != content)
    {
        image_data* data = (image_data* )content;
        if ( data->m_depth_image[sensor_id] ){
            g_depth = Mat::zeros(HEIGHT,WIDTH,CV_16SC1);
            memcpy( g_depth.data, data->m_depth_image[sensor_id], IMAGE_SIZE * 2 );
        }
    }
    g_lock.leave();
    g_event.set_event();
    return 0;
}
int main(int argc, const char** argv)
{
    reset_config();  // clear all data subscription
    int err_code = init_transfer(); //wait for board ready and init transfer thread
    err_code = select_depth_image( sensor_id );
    
    err_code = set_sdk_event_handler( my_callback );	
    err_code = start_transfer();	
    
    while(1)
    {
        g_event.wait_event();
        if(!g_depth.empty()){
            Mat depth8(HEIGHT,WIDTH,CV_8UC1);
            g_depth.convertTo(depth8, CV_8UC1);
            imshow(string("depth_")+char('0'+sensor_id), depth8);
            printf("Depth at point (%d,%d) is %f meters!\n", HEIGHT/2, WIDTH/2,  float(g_depth.at<short>( HEIGHT/2,WIDTH/2))/128);
        }
    }
    err_code = stop_transfer();
    //make sure the ack packet from GUIDANCE is received
    sleep( 1000000 );
    err_code = release_transfer();
}

SDK_API int select_disparity_image ( e_vbus_index camera_pair_index );

#include "DJI_guidance.h"
#include "DJI_utility.h"
#include "opencv2/opencv.hpp"
#include <stdio.h>
#include <string>
e_vbus_index sensor_id = e_vbus1;
Mat		g_disparity;
int my_callback(int data_type, int data_len, char *content)
{
    g_lock.enter();
    if (e_image == data_type && NULL != content)
    {
        image_data* data = (image_data* )content;
        if ( data->m_disparity_image[sensor_id] ){
            g_disparity = Mat::zeros(HEIGHT,WIDTH,CV_16SC1);
            memcpy( g_disparity.data, data->m_disparity_image[sensor_id], IMAGE_SIZE * 2 );
        }
    }
    g_lock.leave();
    g_event.set_event();
    return 0;
}
int main(int argc, const char** argv)
{
    reset_config();  // clear all data subscription
    int err_code = init_transfer(); //wait for board ready and init transfer thread
    err_code = select_disparity_image( sensor_id );
   
    err_code = set_sdk_event_handler( my_callback );	
    err_code = start_transfer();	
    
    while(1)
    {
        g_event.wait_event();
        if(!g_disparity.empty()){
            Mat disp8(HEIGHT,WIDTH,CV_8UC1);
            g_disparity.convertTo(disp8, CV_8UC1);
            imshow(string("disparity_")+char('0'+sensor_id), disp8);
            printf("Disparity at point (%d,%d) is %f pixels!\n", HEIGHT/2, WIDTH/2,  float(g_disparity.at<short>( HEIGHT/2,WIDTH/2))/16);
        }
    }
    err_code = stop_transfer();
    //make sure the ack packet from GUIDANCE is received
    sleep( 1000000 );
    err_code = release_transfer();
}

SDK_API int select_greyscale_image ( e_vbus_index camera_pair_index, bool is_left );

SDK_API void select_motion( void );

SDK_API int set_sdk_event_handler ( user_call_back handler );

SDK_API int start_transfer ( void );

SDK_API int stop_transfer ( void );

SDK_API int release_transfer ( void );

SDK_API int get_online_status (int online_status[CAMERA_PAIR_NUM] );

SDK_API int get_device_type(e_device_type* device_type);

SDK_API	int get_image_size(int* width, int* height);

SDK_API int wait_for_board_ready();

SDK_API int set_exposure_param( exposure_param *param );