// MsgHandleDlg.cpp : 实现文件
//

#include "stdafx.h"
#include "Fast.h"
#include "HikRgdbCamera.h"
#include "afxdialogex.h"
#include "FastView.h"
#include "RenderImage.hpp"



#define IMAGE_W 1280
#define IMAGE_H 720
char rgb24[IMAGE_W * IMAGE_H * 3];


#define TIMER_LOGIN             10


// CMsgHandleDlg 对话框

IMPLEMENT_DYNAMIC(CHikRgdbCamera, CBaseCamera)

CHikRgdbCamera::CHikRgdbCamera(CFastView * pParentWnd, int nIdx, CWnd* pParent /*=NULL*/)
	: CBaseCamera(IDD_DLG_CAM, pParentWnd)
{
	m_nCameraIdx    = nIdx;
	m_pParentWnd    = pParentWnd;
	m_hCameraHandle = NULL;
}

CHikRgdbCamera::~CHikRgdbCamera()
{

}


void CHikRgdbCamera::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
}


BEGIN_MESSAGE_MAP(CHikRgdbCamera, CDialog)
	ON_WM_DESTROY()
	ON_WM_TIMER()
	ON_MESSAGE(WM_CAMERA_SNAP, &CHikRgdbCamera::OnSnap)
END_MESSAGE_MAP()



short CHikRgdbCamera::GetPointDepth(unsigned char * pSrcData, int x, int y)
{
	if (nullptr == pSrcData)
	{
		return -1;
	}

	int nDepth = 0;
	int nMax = INT_MIN;
	int nMin = INT_MAX;

	short* pValue = (short*)pSrcData;

	if (C16_INVALID_VALUE == nDepth)
	{
		return -1;
	}

	return pValue[y * IMAGE_W + x];
}

short CHikRgdbCamera::GeRectDepth(unsigned char * pSrcData, cv::Point2f lt, cv::Point2f rb)
{
	int nMin = 999999;
	for (int i = lt.x; i < rb.x; i++)
	{
		for (int j = lt.y; j < rb.y; j++)
		{
			int depth = GetPointDepth(pSrcData, i, j);
			if (depth < nMin && depth > 0)
			{
				nMin = depth;
			}
		}
	}
	return nMin;
}


// CMsgHandleDlg 消息处理程序


BOOL CHikRgdbCamera::OnInitDialog()
{
	CDialog::OnInitDialog();
	InitCameraSDK();
	return TRUE;  // return TRUE unless you set the focus to a control
}


BOOL CHikRgdbCamera::InitCameraSDK()
{

	MV3D_RGBD_VERSION_INFO stVersion;
	MV3D_RGBD_GetSDKVersion(&stVersion);
	MV3D_RGBD_Initialize();
	int ret = MV3D_RGBD_Initialize();

	unsigned int nDevNum = 0;
	ret = MV3D_RGBD_GetDeviceNumber(DeviceType_USB, &nDevNum);

	if (0 == nDevNum)
	{
		//AfxMessageBox("未检测到相机，请检查相机连接是否正常.");
		return FALSE;
	}

	std::vector<MV3D_RGBD_DEVICE_INFO> devs(nDevNum);
	ret = MV3D_RGBD_GetDeviceList(DeviceType_USB, &devs[0], nDevNum, &nDevNum);

	//打开设备
	MV3D_RGBD_OpenDevice(&m_hCameraHandle, &devs[0]);
	//开始工作流程
	MV3D_RGBD_Start(m_hCameraHandle);

	return TRUE;  // return TRUE unless you set the focus to a control
}



LRESULT  CHikRgdbCamera::OnSnap(WPARAM wParam, LPARAM lParam)
{
	AfxMessageBox("采样");
	MV3D_RGBD_FRAME_DATA stFrameData = { 0 };
	int nRet = MV3D_RGBD_FetchFrame(m_hCameraHandle, &stFrameData, 5000);
	CTime currentTime = CTime::GetCurrentTime(); // 获取当前时间
	CString dateStr = currentTime.Format(_T("%Y_%m_%d")); // 格式化为字符串
	CString tiemStr = currentTime.Format(_T("%H_%M_%S")); // 格式化为字符串
	CString strPath = theApp.m_strDataPath + "\\" + dateStr;
	
	if (MV3D_RGBD_OK == nRet)
	{
		if (!stFrameData.nValidInfo)
		{
			RIFrameInfo depth = { 0 };
			RIFrameInfo rgb = { 0 };
			RIFrameInfo rgbd = { 0 };
			parseFrame(&stFrameData, &depth, &rgb, &rgbd);

			/*for (int i = 0; i < 20; i++)
			{
				for (int j = 0; j < 10; j++)
				{
					int nDepth = GeRectDepth(depth.pData, cv::Point2f(i * 64, j * 72), cv::Point2f((i + 1) * 64, (j + 1) * 72));
					char acDepth[32];
					sprintf(acDepth, "%d", nDepth);
					cv::rectangle(image, cv::Point2f(i * 64, j * 72), cv::Point2f((i + 1) * 64, (j + 1) * 72), cv::Scalar(5, 5, 2555), 1, 8, 0);
					cv::putText(image, acDepth, cv::Point2f(i * 64, j * 72 + 20), cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(5, 5, 255), 1);
				}
			}*/

			if (!PathIsDirectory(strPath))//判断日期路径是否存在     
			{
				CreateDirectory(strPath, NULL);//新建日期文件夹
			}

			char acFileName[256] = "";
			sprintf(acFileName, "%s\\%s-%d.jpg", strPath, tiemStr, m_nCameraIdx);

			vector<CRect>  targetRects;
			cv::Mat image(IMAGE_H, IMAGE_W, CV_8UC3, rgb.pData); //buffer为转完的rgb数据
			//识别目标区域位置
			TargetDetection(m_nCameraIdx, image, targetRects);

			//模拟检测区域
			targetRects.push_back(CRect(500, 400, 900, 600));

			//计算目标区域深度
			cv::Point3f  coordinate = cv::Point3f(0, 0, 99999);
			CRect        rect;
			TargetPosition(depth.pData, targetRects, coordinate, rect);
			if (coordinate.z != 99999)
			{
				char acCoordinate[32];
				sprintf(acCoordinate, "x:%.0f, y:%.0f, z:%.0f", coordinate.x, coordinate.y, coordinate.z);
				cv::rectangle(image, cv::Point2f(rect.left, rect.top), cv::Point2f(rect.right, rect.bottom), cv::Scalar(5, 5, 2555), 1, 8, 0);
				cv::putText(image, acCoordinate, cv::Point2f(coordinate.x, coordinate.y), cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(5, 5, 255), 1);

				
				//m_pParentWnd->CameraSnapPicRet(acFileName, m_nCameraIdx, coordinate);
			}

			//生成解析文件
			imwrite(acFileName, image);
			//返回最优目标坐标

			m_pParentWnd->StartPlay(acFileName);

		}
	}

	return 0;
}


void CHikRgdbCamera::OnTimer(UINT_PTR nIDEvent)
{
	// TODO: 在此添加消息处理程序代码和/或调用默认值
	if (nIDEvent == TIMER_LOGIN)
	{
		//设备断开，重新打开
	}

	CDialog::OnTimer(nIDEvent);
}


void CHikRgdbCamera::OnDestroy()
{
	CDialog::OnDestroy();
}



void CHikRgdbCamera::TargetDetection(int nCameraIdx, cv::Mat &mat_img, vector<CRect> &findRects)
{
	CRect  findRect;
	try {
		cv::Mat dst = mat_img;
		//cv::Size newSize(mat_img.size().width / 4, mat_img.size().height / 4); // 设置新的尺寸
		//cv::resize(mat_img, dst, newSize); // 调整图像尺寸

		auto det_image = theApp.m_yoloV4.m_pDetector->mat_to_image_resize(dst);

		auto start = std::chrono::steady_clock::now();
		std::vector<bbox_t> result_vec = theApp.m_yoloV4.m_pDetector->detect_resized(*det_image, dst.size().width, dst.size().height);
		auto end = std::chrono::steady_clock::now();
		std::chrono::duration<double> spent = end - start;
		std::cout << " Time: " << spent.count() << " sec \n";

		theApp.m_yoloV4.DrawObjectBoxes(dst, result_vec, theApp.m_yoloV4.m_vObjects_Names);

		for (int i = 0; i < result_vec.size(); i++)
		{
			bbox_t  result_area = result_vec.at(i);
			std::string obj_name = theApp.m_yoloV4.m_vObjects_Names[result_area.obj_id];
			//if (result_area.prob >= 0.9 && (0 == obj_name.compare("up") ))
			{
				CRect  rect;
				rect.left = result_area.x;
				rect.top = result_area.y;
				rect.right = result_area.x + result_area.w;
				rect.bottom = result_area.y + result_area.h;
				findRects.push_back(rect);
			}

		}
	}
	catch (std::exception &e) { std::cerr << "exception: " << e.what() << "\n"; getchar(); }
	catch (...) { std::cerr << "unknown exception \n"; getchar(); }

}


void  CHikRgdbCamera::TargetPosition(unsigned char* pDepthData, vector<CRect> &findRects, cv::Point3f& coordinate, CRect& rect)
{
	for (int i = 0; i < findRects.size(); i++)
	{
		CRect  rt = findRects.at(i);

		if (rt.Width() > rt.Height())
		{
			int x = (rt.left + rt.right) / 2;
			int y = (rt.top  + rt.bottom) / 2;
			int z = GetPointDepth(pDepthData, x, y);

			if (z < coordinate.z && z > 0)
			{
				coordinate = cv::Point3f(x, y, z);
				rect       = rt;
			}
		}
		else
		{
			//旋转90度
		}
		
	}
}