// PrintCallback.h
#include <OpenNI.h>
#include <thread>
#define RESOULTION_X 640           // 标定时的分辨率
#define RESOULTION_Y 480           // 标定时的分辨率  
#define FRAMES_PER_SECOND 30       // 每秒传输帧数       
#define TIME_OUT 100               // 彩色传感器与深度传感器切等待的时长,等待一帧数据能及时完成转换
using namespace openni;

class CDepthColorVideoDlg;

struct SParam
{
    CDepthColorVideoDlg* pDlg = NULL;
    void* depthData = NULL;
    RGB888Pixel* colorData = NULL;
    BOOL m_run = false;
};

// Callback object that show images
class PrintCallback : public VideoStream::NewFrameListener
{
public:
    static void ShowError(const char* pDescribe, const char* pError, const PrintCallback& printer);
    void collectColorStream()
    {
        std::thread t(onColorRenderProc, m_param);
        t.detach();
    }
    void collectDepthStream()
    {
        std::thread t(onDepthRenderProc, m_param);
        t.detach();
    }
    SParam m_param;
private:
    static void AnalyzeFrame(PrintCallback* pPrintCallBack, const VideoFrameRef& frame, SParam& param);
    static void onColorRenderProc(SParam& param);
    static void onDepthRenderProc(SParam& param);
    void onNewFrame(VideoStream& stream);
private:
    VideoFrameRef m_frame;
};

// PrintCallback.cpp
#include "PrintCallback.h"
#include "DepthColorVideo.h"
#include "DepthColorVideoDlg.h"
#include <opencv.hpp>
using namespace cv;

void PrintCallback::AnalyzeFrame(PrintCallback* pPrintCallBack, const VideoFrameRef& frame, SParam& param)
{   
    PixelFormat format = frame.getVideoMode().getPixelFormat();
    switch (format)
    {
    case PIXEL_FORMAT_RGB888:
        param.colorData = (RGB888Pixel*)frame.getData();
        pPrintCallBack->collectColorStream();
        TRACE("解析彩色帧\n");
        break;
    case PIXEL_FORMAT_DEPTH_1_MM:
        if (param.m_run)
        {
            param.depthData = (void*)frame.getData();
            pPrintCallBack->collectDepthStream();
            TRACE("解析深度帧\n");
        }
        break;
    default:
        ::MessageBox(param.pDlg->GetSafeHwnd(), L"不支持的未知格式图像\n", _T("错误提示"), MB_OK);
        break;
    }
}

void PrintCallback::onColorRenderProc(SParam& param)
{
    if (!param.colorData)
        return;

    RGB888Pixel* pColor = (RGB888Pixel*)param.colorData;
    cv::Mat& mat = theApp.GetColorMatrix();
    cv::Mat img(RESOULTION_Y, RESOULTION_X, CV_8UC3);

    int h = param.pDlg->GetColorWidth();
    int w = param.pDlg->GetColorHeight();

    int nRows = img.rows;
    int nClos = img.cols;   // 获取帧以像素为单位的宽度
    auto p = img.data;
    for (int row = 0; row < nRows; row++)
    {
        for (int col = 0; col < nClos; col++)
        {
            COLORREF& d = (COLORREF&)pColor[row * nClos + col];  // 获取像素的颜色 RGB
            *p++ = GetBValue(d), * p++ = GetGValue(d), * p++ = GetRValue(d);
        }
    }
    param.colorData = NULL;

    if (!theApp.m_pMainWnd)
        return;

    cv::Mat tmp;
    resize(img, tmp, Size{ w, h }, (0, 0), (0, 0), INTER_LINEAR);
    mat = tmp.clone();

    theApp.m_pMainWnd->PostMessage(UM_COLOR);
}

void PrintCallback::onDepthRenderProc(SParam& param)
{
    if (!param.depthData)
        return;

    cv::Mat& mat = theApp.GetDepthMatrix();
    cv::Mat img(RESOULTION_X, RESOULTION_Y, CV_8UC1);

    int h = param.pDlg->GetColorWidth();
    int w = param.pDlg->GetColorHeight();

    int nRows = img.rows;
    int nClos = img.cols;   // 获取帧以像素为单位的宽度
    auto p = img.data;
    for (int row = 0; row < nRows; row++)
    {
        for (int col = 0; col < nClos; col++)
        {
            uchar& d = (uchar&)((uchar*)param.depthData)[row * nClos + col];  // 获取像素的颜色 RGB
            *p++ = d;
        }
    }
    param.depthData = NULL;

    if (!theApp.m_pMainWnd)
        return;

    cv::Mat tmp;
    resize(img, tmp, Size { w, h }, (0, 0), (0, 0), INTER_LINEAR);
    mat = tmp.clone();

    theApp.m_pMainWnd->PostMessage(UM_DEPTH);
}

void PrintCallback::onNewFrame(VideoStream& stream)
{
    stream.readFrame(&m_frame);
    AnalyzeFrame(this, m_frame, m_param);
}

void PrintCallback::ShowError(const char* pDescribe, const char* pError, const PrintCallback& printer)
{
    if (!pDescribe || !pError)
        return;

    CString str;
    str = pDescribe;
    str += pError;

    ::MessageBox(printer.m_param.pDlg->GetSafeHwnd(), str, _T("错误提示"), MB_OK);
}

// ColorStream.h
#include "PrintCallback.h"
class CDepthColorVideoDlg;
class ColorStream : public VideoStream
{
public:
    ColorStream();
    BOOL SetDlgPointer(CDepthColorVideoDlg* pDlg);
    BOOL Create(Device& device);
    BOOL Start();
    void Stop();
    BOOL FrameListener();

private:
    void RemoveFrame();
private:
    void ShowError(const char* pDescribe);
    Status m_retCode;
    PrintCallback m_printer;
};

// ColorStream.cpp
#include "ColorStream.h"
#include "DepthColorVideoDlg.h"

ColorStream::ColorStream()
    : VideoStream(), m_retCode(STATUS_OK)
{

}

BOOL ColorStream::SetDlgPointer(CDepthColorVideoDlg* pDlg)
{
    if (!pDlg || !pDlg->GetSafeHwnd())
        return FALSE;

    m_printer.m_param.pDlg = pDlg;

    return TRUE;
}

void ColorStream::ShowError(const char* pDescribe)
{
    PrintCallback::ShowError(pDescribe, OpenNI::getExtendedError(), m_printer);
}

BOOL ColorStream::Create(Device& device)
{
    Status status = create(device, SENSOR_COLOR);
    if (status == STATUS_OK)
    {
        // 设置彩色视频流的模式
        VideoMode mMode;
        mMode.setResolution(RESOULTION_X, RESOULTION_Y);
        mMode.setFps(FRAMES_PER_SECOND);
        mMode.setPixelFormat(PIXEL_FORMAT_RGB888);

        if (setVideoMode(mMode) != STATUS_OK)
        {
            ShowError("设置彩色视频的工作模式失败\n");
            return FALSE;
        }
        // 开启彩色流
        if (!Start())  
        {
            ShowError("开启彩色流失败\n");
            return FALSE;
        }
    }
    else
    {
        ShowError("在设备上创建彩色流失败\n");
        return FALSE;
    }
    return TRUE;
}

BOOL ColorStream::Start()
{
    m_retCode = start();
    if (m_retCode != STATUS_OK)
    {
        ShowError("采集彩色流失败\n");
        return FALSE;
    }
    return FrameListener();
}

void ColorStream::Stop()
{
    // if status ok, should remove frames from Frame listener
    if (m_retCode == STATUS_OK)
    {
        RemoveFrame();
    }
    // related operations when closing
    stop();
    destroy();
}

BOOL ColorStream::FrameListener()
{
    m_retCode = addNewFrameListener(&m_printer);
    if (m_retCode != STATUS_OK)  // if status is not ok then close camera
    {
        RemoveFrame();
        return FALSE;
    }

    return TRUE;
}

void ColorStream::RemoveFrame()
{
    removeNewFrameListener(&m_printer);
    // Wait while we're getting frames through the printer
    Sleep(TIME_OUT);
}

// DepthStream.h
#include "PrintCallback.h"
class CDepthColorVideoDlg;
class DepthStream : public VideoStream
{
public:
    DepthStream();
    BOOL SetDlgPointer(CDepthColorVideoDlg* pDlg);
    inline void SetRun(BOOL bRun) { m_printer.m_param.m_run = bRun; }
    inline BOOL IsRun() const { return m_printer.m_param.m_run; }
    BOOL Create(Device& device);
    BOOL Start();
    void Stop();
    BOOL FrameListener();

private:
    void RemoveFrame();
    void ShowError(const char* pDescribe);

private:
    Status m_retCode;
    PrintCallback m_printer;
};

// DepthStream.cpp
#include "DepthStream.h"
#include "DepthColorVideoDlg.h"

DepthStream::DepthStream()
    : VideoStream(), m_retCode(STATUS_OK)
{

}

void DepthStream::ShowError(const char* pDescribe)
{
    PrintCallback::ShowError(pDescribe, OpenNI::getExtendedError(), m_printer);
}

BOOL DepthStream::SetDlgPointer(CDepthColorVideoDlg* pDlg)
{
    if (!pDlg || !pDlg->GetSafeHwnd())
        return FALSE;

    m_printer.m_param.pDlg = pDlg;

    return TRUE;
}

BOOL DepthStream::Create(Device& device)
{
    if (device.hasSensor(SENSOR_DEPTH))
    {
        if (create(device, SENSOR_DEPTH) == STATUS_OK)
        {
            // 设置深度视频流的模式
            openni::VideoMode mMode;
            mMode.setResolution(RESOULTION_X, RESOULTION_Y);
            mMode.setFps(FRAMES_PER_SECOND);
            mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);

            if (setVideoMode(mMode) != openni::STATUS_OK)
            {
                ShowError("设置深度视频的工作模式失败\n");
                return FALSE;
            }
        }
        else
        {
            ShowError("开启深度流失败\n");
            return FALSE;
        }
        // 开启深度流
        if (!Start())  // 失败
        {
            std::endl;
            ShowError("在设备上创建彩色流失败\n");
            return FALSE;
        }
        else          // 成功
        {
            // 深度流与彩色流对齐
            if (device.isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
            {
                device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
            }
            else
            {
                ShowError("设备不支持深度流对齐到彩色流的功能\n");
                return FALSE;
            }
        }
    }
    else
    {
        ShowError("设备是不带深度传感器的普通摄像头\n");
        return FALSE;
    }
    return TRUE;
}

BOOL DepthStream::Start()
{
    m_retCode = start();
    if (m_retCode != STATUS_OK)
    {   
        ShowError("采集深度流失败\n");
        return FALSE;
    }
    return FrameListener();
}

void DepthStream::Stop()
{
    if (m_retCode == STATUS_OK)  // if status ok, should remove frames from Frame listener
    {
        RemoveFrame();
    }
    // related operations when closing
    stop();
    destroy();
}

BOOL DepthStream::FrameListener()
{
    m_retCode = addNewFrameListener(&m_printer);
    if (m_retCode != STATUS_OK)  // if status is not ok then close camera
    {
        RemoveFrame();
        return FALSE;
    }

    return TRUE;
}

void DepthStream::RemoveFrame()
{
    removeNewFrameListener(&m_printer);
    //Sleep(WAITTING_TIME);
    // Wait while we're getting frames through the printer
    Sleep(TIME_OUT);    
}

// StreamPool.h
#include "ColorStream.h"
#include "DepthStream.h"

class CDepthColorVideoDlg;

class StreamPool
{
public:
    BOOL SetDlgPointer(CDepthColorVideoDlg* pDlg);
    BOOL OpenDevice();
    void CloseDevice();
    inline void SetRun(BOOL bRun) { m_depth.SetRun(bRun); }
    inline BOOL IsRun() const { return m_depth.IsRun(); }
private:
    BOOL Start();

private:
    Status m_retCode;
    Device m_device;
    ColorStream m_color;
    DepthStream m_depth;
};

// StreamPool.cpp
#include "StreamPool.h"
#include "DepthColorVideoDlg.h"

BOOL StreamPool::SetDlgPointer(CDepthColorVideoDlg* pDlg)
{
    if (!pDlg)
        return FALSE;
    if (!m_color.SetDlgPointer(pDlg))
        return FALSE;
    if (!m_depth.SetDlgPointer(pDlg))
        return FALSE;
    return TRUE;
}

BOOL StreamPool::OpenDevice()
{
    // Initial OpenNI
    if (OpenNI::initialize() != STATUS_OK)
    {
        return FALSE;
    }
    // Open Device
    if (m_device.open(ANY_DEVICE) != STATUS_OK)
    {
        return FALSE;
    }
    if (!Start())
    {
        return FALSE;
    }
    return TRUE;
}

BOOL StreamPool::Start()
{
    if (!m_color.Create(m_device))
        return FALSE;
    if (!m_depth.Create(m_device))
        return FALSE;
    return TRUE;
}

void StreamPool::CloseDevice()
{
    m_color.Stop();
    m_depth.Stop();
    OpenNI::shutdown();
}

// DepthColorVideo.h
#include <opencv.hpp>
using namespace cv;

enum { UM_COLOR = WM_USER + 1000, UM_DEPTH = WM_USER + 1001 };

class CDepthColorVideoApp : public CWinApp
{
public:
    CDepthColorVideoApp();

    static Mat& GetColorMatrix()
    {
        static Mat mat;
        return mat;
    }

    static Mat& GetDepthMatrix()
    {
        static Mat mat;
        return mat;
    }
    ...... 
};

// CDepthColorVideoDlg.h

#include"StreamPool.h"
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui_c.h>
using namespace cv;

class StreamWrapper
{
public:
    inline CRect* ColorWrapper() { return &m_rcColor; }
    inline CRect* DepthWrapper() { return &m_rcDepth; }

    inline const int GetColorWrapperWidth() const { return m_rcColor.Width(); }
    inline const int GetColorWrapperHeight() const { return m_rcColor.Height(); }
    inline const int GetDepthWapperWidth() const { return m_rcColor.Width(); }
    inline const int GetDepthWapperHeight() const { return m_rcColor.Height(); }

private:
    CRect m_rcColor;
    CRect m_rcDepth;
};


// CDepthColorVideoDlg.h

class CDepthColorVideoDlg : public CDialogEx
{
public:
    inline const int GetColorWidth() const { return m_wrapper.GetColorWrapperWidth(); }
    inline const int GetColorHeight() const { return m_wrapper.GetColorWrapperHeight(); }
    inline const int GetDepthWidth() const { return m_wrapper.GetDepthWapperWidth(); }
    inline const int GetDepthHeight() const { return m_wrapper.GetDepthWapperHeight(); }
    CDepthColorVideoDlg(CWnd* pParent = nullptr);   // standard constructor   

    ......

public:
    afx_msg void OnDestroy();
    afx_msg void OnBnClickedDepthStart();
    afx_msg void OnBnClickedDepthStop();
protected:
    afx_msg LRESULT OnUmColorStream(WPARAM wParam, LPARAM lParam);
    afx_msg LRESULT OnUmDepthStream(WPARAM wParam, LPARAM lParam);

private:
    StreamPool m_pool;
    StreamWrapper m_wrapper;
    void SetWrapper();
    void  CreateHighGui(const char* pStrWndName, UINT nCtrlID);
};

// CDepthColorVideoDlg.cpp

void  CDepthColorVideoDlg::CreateHighGui(const char* pStrWndName, UINT nCtrlID)
{
    namedWindow(pStrWndName, WINDOW_AUTOSIZE); // WINDOW_AUTOSIZE
    cv::moveWindow(pStrWndName, -260, 0);

    HWND hPicWnd = (HWND)cvGetWindowHandle(pStrWndName);
    if (!hPicWnd)
        return;

    HWND hGuiWnd = ::GetParent(hPicWnd);
    ::ShowWindow(hGuiWnd, SW_HIDE);

    // 将视频流窗口的父窗口指定为MFC控件窗口
    ::SetParent(hPicWnd, GetDlgItem(nCtrlID)->GetSafeHwnd());
    ::ShowWindow(GetDlgItem(nCtrlID)->GetSafeHwnd(), SW_SHOW);
}

BOOL CDepthColorVideoDlg::OnInitDialog()
{
    ......

    SetWrapper();                 // 初始化包装帧的边框的长宽
    CreateHighGui("Color", IDC_COLOR);   // 创建OpenCV窗口  
    CreateHighGui("Depth", IDC_DEPTH);
    m_pool.SetDlgPointer(this);   // 向回调对象传递主对话框的指针
    if (!m_pool.OpenDevice())     // 打开摄像头的彩色传感器和深度传感器
        return FALSE;

    ......
}

void CDepthColorVideoDlg::OnDestroy()
{
    CDialogEx::OnDestroy();

    // TODO: Add your message handler code here
    m_pool.CloseDevice();
}


afx_msg LRESULT CDepthColorVideoDlg::OnUmColorStream(WPARAM wParam, LPARAM lParam)
{
    imshow("Color", theApp.GetColorMatrix());
    return 0;
}


afx_msg LRESULT CDepthColorVideoDlg::OnUmDepthStream(WPARAM wParam, LPARAM lParam)
{
    // 深度视频还在采集中，这个标记将来受到称台的状态和称台视口是否有杂物(手的遮挡，物体的一部分不在称台区    域内)的控制
    if(m_pool.IsRun())  
    {
        imshow("Depth", theApp.GetDepthMatrix());
    }

    return 0;
}

// 模拟放置货物稳定后，深度摄像头开始采集货物的深度视频流
void CDepthColorVideoDlg::OnBnClickedDepthStart()  
{
    // TODO: Add your control notification handler code here
    if (!m_pool.IsRun())
        m_pool.SetRun(TRUE);
}

//模拟物体被拿开或者物体放置不规范或者有人体出现在规定的范围内
void CDepthColorVideoDlg::OnBnClickedDepthStop() 
{
    // TODO: Add your control notification handler code here
    if (m_pool.IsRun())
        m_pool.SetRun(FALSE);
}