Merge pull request #742 from cpuwolf/autopair4

tracker/wii: add WIImote auto-pair function

10 files changed, 116 insertions, 22 deletions

diff --git a/tracker-wii/CMakeLists.txt b/tracker-wii/CMakeLists.txt
index ced3ba50..76a43a77 100644
--- a/tracker-wii/CMakeLists.txt
+++ b/tracker-wii/CMakeLists.txt
@@ -3,7 +3,7 @@ if(WIN32)
     if(OpenCV_FOUND)
         add_subdirectory(wiiyourself)
         otr_module(tracker-wii)
-        target_link_libraries(opentrack-tracker-wii opentrack-tracker-pt-base opentrack-wiiyourself)
+        target_link_libraries(opentrack-tracker-wii opentrack-tracker-pt-base opentrack-wiiyourself bthprops)
         target_include_directories(opentrack-tracker-wii PRIVATE "${CMAKE_SOURCE_DIR}/tracker-pt")
     endif()
 endif()
diff --git a/tracker-wii/Resources/on.png b/tracker-wii/Resources/on.png
new file mode 100644
index 00000000..70df81ee
--- /dev/null
+++ b/tracker-wii/Resources/on.png
diff --git a/tracker-wii/Resources/sync.png b/tracker-wii/Resources/sync.png
new file mode 100644
index 00000000..9da94c3c
--- /dev/null
+++ b/tracker-wii/Resources/sync.png
diff --git a/tracker-wii/Resources/usb.png b/tracker-wii/Resources/usb.png
new file mode 100644
index 00000000..0205aafa
--- /dev/null
+++ b/tracker-wii/Resources/usb.png
diff --git a/tracker-wii/tracker_wii.qrc b/tracker-wii/tracker_wii.qrc
index a7a50512..5acd2a4c 100644
--- a/tracker-wii/tracker_wii.qrc
+++ b/tracker-wii/tracker_wii.qrc
@@ -1,5 +1,8 @@
 <RCC>
     <qresource prefix="/">
         <file>Resources/wii.png</file>
+        <file>Resources/sync.png</file>
+        <file>Resources/on.png</file>
+        <file>Resources/usb.png</file>
     </qresource>
 </RCC>
diff --git a/tracker-wii/wii_camera.cpp b/tracker-wii/wii_camera.cpp
index 9c2d618c..1186afc1 100644
--- a/tracker-wii/wii_camera.cpp
+++ b/tracker-wii/wii_camera.cpp
@@ -18,6 +18,7 @@
 
 #include "cv/video-property-page.hpp"
 
+#include <BluetoothAPIs.h>
 
 using namespace pt_module;
 
@@ -102,12 +103,108 @@ void WIICamera::stop()
     cam_desired = pt_camera_info();
 }
 
+
+wii_camera_status WIICamera::_pair()
+{
+	wii_camera_status ret = wii_cam_wait_for_sync;
+	HBLUETOOTH_RADIO_FIND hbt;
+	BLUETOOTH_FIND_RADIO_PARAMS bt_param;
+	HANDLE hbtlist[10];
+	int ibtidx = 0;
+	bool wiifound = false;
+
+	bt_param.dwSize = sizeof(bt_param);
+	hbt = BluetoothFindFirstRadio(&bt_param, hbtlist + ibtidx);
+	if (!hbt) { ret = wii_cam_wait_for_dongle; return ret; }
+	do
+	{
+		ibtidx++;
+	} while (BluetoothFindNextRadio(&bt_param, hbtlist + ibtidx));
+	BluetoothFindRadioClose(hbt);
+
+
+	int i;
+	bool error = false;
+	for (i = 0; i < ibtidx; i++)
+	{
+		BLUETOOTH_RADIO_INFO btinfo;
+		btinfo.dwSize = sizeof(btinfo);
+
+		if (ERROR_SUCCESS != BluetoothGetRadioInfo(hbtlist[i], &btinfo)) {break;}
+
+		HBLUETOOTH_DEVICE_FIND hbtdevfd;
+		BLUETOOTH_DEVICE_SEARCH_PARAMS btdevparam;
+		BLUETOOTH_DEVICE_INFO btdevinfo;
+
+		btdevinfo.dwSize = sizeof(btdevinfo);
+		btdevparam.dwSize = sizeof(btdevparam);
+		btdevparam.fReturnAuthenticated = TRUE;
+		btdevparam.fReturnConnected = TRUE;
+		btdevparam.fReturnRemembered = TRUE;
+		btdevparam.fIssueInquiry = TRUE;
+		btdevparam.cTimeoutMultiplier = 2;
+		btdevparam.hRadio = hbtlist[i];
+		hbtdevfd=BluetoothFindFirstDevice(&btdevparam, &btdevinfo);
+		if (!hbtdevfd) {
+			int error= GetLastError();
+			qDebug() << error;
+			break;
+		}
+		do
+		{
+			if (wcscmp(btdevinfo.szName, L"Nintendo RVL-CNT-01-TR") && wcscmp(btdevinfo.szName, L"Nintendo RVL-CNT-01"))
+			{
+				continue;
+			}
+
+			if ((btdevinfo.fRemembered)&&error) {
+				BluetoothRemoveDevice(&btdevinfo.Address);
+			}
+
+			wiifound = true;
+			if (btdevinfo.fConnected) {
+				break;
+			}
+			WCHAR pwd[6];
+			pwd[0] = btinfo.address.rgBytes[0];
+			pwd[1] = btinfo.address.rgBytes[1];
+			pwd[2] = btinfo.address.rgBytes[2];
+			pwd[3] = btinfo.address.rgBytes[3];
+			pwd[4] = btinfo.address.rgBytes[4];
+			pwd[5] = btinfo.address.rgBytes[5];
+
+			if (ERROR_SUCCESS != BluetoothAuthenticateDevice(NULL, hbtlist[i], &btdevinfo, pwd, 6)) { error = true; continue; }
+			DWORD servicecount = 32;
+			GUID guids[32];
+			if (ERROR_SUCCESS != BluetoothEnumerateInstalledServices(hbtlist[i], &btdevinfo, &servicecount, guids)) { error = true; continue; }
+			if (ERROR_SUCCESS != BluetoothSetServiceState(hbtlist[i], &btdevinfo, &HumanInterfaceDeviceServiceClass_UUID, BLUETOOTH_SERVICE_ENABLE)) { error = true; continue; }
+			break;
+		} while (BluetoothFindNextDevice(hbtdevfd, &btdevinfo));
+		BluetoothFindDeviceClose(hbtdevfd);
+	}
+
+	for (i = 0; i < ibtidx; i++)
+	{
+		CloseHandle(hbtlist[i]);
+	}
+	if (wiifound) { ret = wii_cam_wait_for_connect; }
+	return ret;
+}
+
 wii_camera_status WIICamera::_get_frame(cv::Mat& frame)
 {
 	wii_camera_status ret = wii_cam_wait_for_connect;
 
 	if (!m_pDev->IsConnected()) {
 		qDebug() << "wii wait";
+		ret = _pair();
+		switch(ret){
+		case wii_cam_wait_for_sync:
+			m_pDev->Disconnect();
+			goto goodbye;
+		case wii_cam_wait_for_connect:
+			break;
+		}
 		if (!m_pDev->Connect(wiimote::FIRST_AVAILABLE)) {
 			Beep(500, 30); Sleep(1000);
 			goto goodbye;
@@ -158,7 +255,7 @@ bool WIICamera::_get_points(struct wii_info& wii)
 			wii.Points[index].bvis = dot.bVisible;
 		}
 	}
-	m_pDev->SetLEDs(3 - point_count);
+	m_pDev->SetLEDs(0);
 	return ret;
 }
 
diff --git a/tracker-wii/wii_camera.h b/tracker-wii/wii_camera.h
index 7bd4a7ae..d0d7f6dd 100644
--- a/tracker-wii/wii_camera.h
+++ b/tracker-wii/wii_camera.h
@@ -54,6 +54,7 @@ private:
 	bool onExit = false;
 	pt_frame internalframe;
 	
+	wii_camera_status _pair();
 	wii_camera_status _get_frame(cv::Mat& Frame);
 	bool _get_points(struct wii_info&);
 	void _get_status(struct wii_info&);
diff --git a/tracker-wii/wii_frame.cpp b/tracker-wii/wii_frame.cpp
index 9e367926..9332a704 100644
--- a/tracker-wii/wii_frame.cpp
+++ b/tracker-wii/wii_frame.cpp
@@ -19,9 +19,12 @@ using namespace pt_module;
 
 WIIPreview& WIIPreview::operator=(const pt_frame& frame_)
 {
+    const struct wii_info& wii = frame_.as_const<WIIFrame>()->wii;
     const cv::Mat& frame = frame_.as_const<const WIIFrame>()->mat;
     ensure_size(frame_copy, frame_out.cols, frame_out.rows, CV_8UC3);
 
+    status = wii.status;
+
     if (frame.channels() != 3)
     {
         once_only(qDebug() << "tracker/pt: camera frame depth: 3 !=" << frame.channels());
@@ -46,6 +49,14 @@ WIIPreview::WIIPreview(int w, int h)
 
 QImage WIIPreview::get_bitmap()
 {
+	switch (status) {
+	case wii_cam_wait_for_dongle:
+		return QImage(":/Resources/usb.png");
+	case wii_cam_wait_for_sync:
+		return QImage(":/Resources/sync.png");
+	case wii_cam_wait_for_connect:
+		return QImage(":/Resources/on.png");
+	}
     int stride = frame_out.step.p[0];
 
     if (stride < 64 || stride < frame_out.cols * 4)
diff --git a/tracker-wii/wii_frame.hpp b/tracker-wii/wii_frame.hpp
index 2bffa862..31967d10 100644
--- a/tracker-wii/wii_frame.hpp
+++ b/tracker-wii/wii_frame.hpp
@@ -16,7 +16,7 @@
 
 namespace pt_module {
 
-enum wii_camera_status : unsigned { wii_cam_wait_for_connect, wii_cam_data_no_change, wii_cam_data_change };
+enum wii_camera_status : unsigned { wii_cam_wait_for_dongle, wii_cam_wait_for_sync, wii_cam_wait_for_connect, wii_cam_data_no_change, wii_cam_data_change };
 
 struct wii_info_points {
 	unsigned ux;
@@ -59,6 +59,7 @@ private:
 
     bool fresh = true;
     cv::Mat frame_copy, frame_color, frame_resize, frame_out;
+	wii_camera_status status;
 };
 
 } // ns pt_module
diff --git a/tracker-wii/wii_point_extractor.cpp b/tracker-wii/wii_point_extractor.cpp
index 99d4d375..215d50b8 100644
--- a/tracker-wii/wii_point_extractor.cpp
+++ b/tracker-wii/wii_point_extractor.cpp
@@ -109,35 +109,16 @@ void WIIPointExtractor::_draw_bg(cv::Mat& preview_frame, const struct wii_info &
 		1);
 }
 
-
 void WIIPointExtractor::extract_points(const pt_frame& frame_, pt_preview& preview_frame_, std::vector<vec2>& points)
 {
-        //const cv::Mat& frame = frame_.as_const<WIIFrame>()->mat;
 	const struct wii_info& wii = frame_.as_const<WIIFrame>()->wii;
 	cv::Mat& preview_frame = *preview_frame_.as<WIIPreview>();
 
-	//create a blank frame
-	//cv::Mat blank_frame(preview_frame.cols, preview_frame.rows, CV_8UC3, cv::Scalar(0, 0, 0));
-	//cv::cvtColor(_frame, _frame2, cv::COLOR_BGR2BGRA);
-	//cv::resize(blank_frame, preview_frame, cv::Size(preview_frame.cols, preview_frame.rows), 0, 0, cv::INTER_NEAREST);
-
 	switch (wii.status) {
 	case wii_cam_data_change:
 		_draw_bg(preview_frame, wii);
 		_draw_points(preview_frame, wii, points);
 		break;
-	case wii_cam_wait_for_connect:
-		char txtbuf[64];
-		sprintf(txtbuf, "%s", "wait for WIImote");
-		//draw wait text
-		cv::putText(preview_frame,
-			txtbuf,
-			cv::Point(preview_frame.cols / 10, preview_frame.rows / 2),
-			cv::FONT_HERSHEY_SIMPLEX,
-			1,
-			cv::Scalar(255, 255, 255),
-			1);
-		break;
 	}
 }