// ECTrainer1Dlg.cpp : 実装ファイル
//

#include "pch.h"
#include "framework.h"
#include "ECTrainer1.h"
#include "ECTrainer1Dlg.h"
#include "afxdialogex.h"
#include <iostream>
#include <fstream>
#include "RingBuffer.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

const char* FILE_LIST[] = { 
	//"man2", "manL1", "manL2", "manL1", "manL3", "manL1" , "manL4", "manL1",
	//"manS1", "manS2", "manS1", "manS3", "manS1" , "manS4", "manS1" 
	"instruction",
	"circle1", "circle2", "circle3",
	"break",
	"face_I_L_E","face_I_L_M","face_F_L_E","face_F_L_M","face_M_L_E","face_M_L_M",
	"break",
	"face_I_M_E","face_I_M_M","face_F_M_E","face_F_M_M",
	"break",
	"face_F_S_E","face_F_S_M","face_F_SS_E","face_F_SS_M",
};
const int NUM_FILES = sizeof(FILE_LIST) / sizeof(char*);


// アプリケーションのバージョン情報に使われる CAboutDlg ダイアログ

class CAboutDlg : public CDialogEx
{
public:
	CAboutDlg();

// ダイアログ データ
#ifdef AFX_DESIGN_TIME
	enum { IDD = IDD_ABOUTBOX };
#endif

	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV サポート

// 実装
protected:
	DECLARE_MESSAGE_MAP()
public:
	afx_msg void OnBnClickedButton1();
};

CAboutDlg::CAboutDlg() : CDialogEx(IDD_ABOUTBOX)
{
}

void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialogEx::DoDataExchange(pDX);
}

BEGIN_MESSAGE_MAP(CAboutDlg, CDialogEx)
END_MESSAGE_MAP()


// CECTrainer1Dlg ダイアログ

// コンストラクタ
CECTrainer1Dlg::CECTrainer1Dlg(CWnd* pParent /*=nullptr*/)
	: CDialogEx(IDD_ECTRAINER1_DIALOG, pParent)
	, _bRunning(false)
	, _pCameraThread(NULL)
	, _message(_T("")) {
	_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

// DDX処理
void CECTrainer1Dlg::DoDataExchange(CDataExchange* pDX)
{
	CDialogEx::DoDataExchange(pDX);
	DDX_Text(pDX, IDC_EDT_MSG, _message);
}

// メッセージマップ
BEGIN_MESSAGE_MAP(CECTrainer1Dlg, CDialogEx)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_WM_CLOSE()
	ON_BN_CLICKED(IDC_BTN_START, &CECTrainer1Dlg::OnBnClickedBtnStart)
END_MESSAGE_MAP()


// CECTrainer1Dlg メッセージ ハンドラー

// ダイアログ初期化
BOOL CECTrainer1Dlg::OnInitDialog()
{
	CDialogEx::OnInitDialog();

	// "バージョン情報..." メニューをシステム メニューに追加します。

	// IDM_ABOUTBOX は、システム コマンドの範囲内になければなりません。
	ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
	ASSERT(IDM_ABOUTBOX < 0xF000);

	CMenu* pSysMenu = GetSystemMenu(FALSE);
	if (pSysMenu != nullptr)
	{
		BOOL bNameValid;
		CString strAboutMenu;
		bNameValid = strAboutMenu.LoadString(IDS_ABOUTBOX);
		ASSERT(bNameValid);
		if (!strAboutMenu.IsEmpty())
		{
			pSysMenu->AppendMenu(MF_SEPARATOR);
			pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
		}
	}

	// このダイアログのアイコンを設定します。アプリケーションのメイン ウィンドウがダイアログでない場合、
	//  Framework は、この設定を自動的に行います。
	SetIcon(_hIcon, TRUE);			// 大きいアイコンの設定
	SetIcon(_hIcon, FALSE);		// 小さいアイコンの設定

	// ワーカースレッド起動
	_bRunning = true;
	StartImageThreads();
	StartDataThreads();

	_message = _T("待機中");
	UpdateData(FALSE);

	return TRUE;  // フォーカスをコントロールに設定した場合を除き、TRUE を返します。
}

// システムコマンド
void CECTrainer1Dlg::OnSysCommand(UINT nID, LPARAM lParam)
{
	if ((nID & 0xFFF0) == IDM_ABOUTBOX)
	{
		CAboutDlg dlgAbout;
		dlgAbout.DoModal();
	}
	else
	{
		CDialogEx::OnSysCommand(nID, lParam);
	}
}

// 描画イベント
void CECTrainer1Dlg::OnPaint()
{
	if (IsIconic())
	{
		// ダイアログに最小化時のアイコンを描画
		CPaintDC dc(this); // 描画のデバイス コンテキスト
		SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);
		// クライアントの四角形領域内の中央
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;
		// アイコンの描画
		dc.DrawIcon(x, y, _hIcon);
	}
	else
	{
		CDialogEx::OnPaint();

		if (_mode > 0) {
			GetDlgItem(IDC_BTN_START)->SetWindowTextW(_T("中断"));
			_message = _T("試験中");
			UpdateData(FALSE);
		} else {
			GetDlgItem(IDC_BTN_START)->SetWindowTextW(_T("開始"));
			_message = _T("待機中");
			UpdateData(FALSE);
		}
	}
}

// 最小化ウィンドウドラッグ時のカーソル取得関数
HCURSOR CECTrainer1Dlg::OnQueryDragIcon()
{
	return static_cast<HCURSOR>(_hIcon);
}

// Mat画像をピクチャボックスに表示
void CECTrainer1Dlg::ShowImage(int id, Mat& img)
{
	CDC* dc = GetDlgItem(id)->GetDC();
	HDC dstDC = dc->GetSafeHdc();

	CRect rc;
	GetDlgItem(id)->GetClientRect(&rc);
	cv::Size size(rc.Width() & ~0x03, rc.Height() & ~0x03);
	resize(img, img, size);

	BITMAPINFO info = {};
	info.bmiHeader.biBitCount = 24;
	info.bmiHeader.biWidth = size.width;
	info.bmiHeader.biHeight = -size.height;
	info.bmiHeader.biPlanes = 1;
	info.bmiHeader.biSize = sizeof(BITMAPINFOHEADER);
	info.bmiHeader.biCompression = BI_RGB;

	StretchDIBits(dstDC, 0, 0, size.width, size.height,
		0, 0, size.width, size.height,
		img.data, &info, DIB_RGB_COLORS, SRCCOPY);

	ReleaseDC(dc);
}

// 閉じる時のイベント
void CECTrainer1Dlg::OnClose()
{
	//if (IDOK == MessageBox(_T("本当に終了しますか？"), _T("確認"), MB_OKCANCEL)) {
	//	
	//}
	_bRunning = false;
	HANDLE handles[] = { _pCameraThread->m_hThread, _pImageThread->m_hThread, _pTargetThread->m_hThread,
		_pKeepAliveThread->m_hThread, _pDataThread->m_hThread };
	WaitForMultipleObjects(sizeof(handles)/sizeof(HANDLE), handles, TRUE, 2000);

	CDialogEx::OnClose();
}

// カメラスレッド開始
bool CECTrainer1Dlg::StartImageThreads()
{
	// カメラスレッド生成
	_pCameraThread = AfxBeginThread(CallCameraThreadProc, 
		(LPVOID)this, THREAD_PRIORITY_NORMAL, 0, CREATE_SUSPENDED, NULL);
	if (!_pCameraThread)	return false;
	_pCameraThread->m_pMainWnd = this;	// メインウィンドウに自分自身を設定
	_pCameraThread->m_bAutoDelete = TRUE;	// スレッド終了時にオブジェクトを自動的に破棄
	_pCameraThread->ResumeThread();	// スレッドを開始

	// 画像処理スレッド生成
	_pImageThread = AfxBeginThread(CallImageThreadProc,
		(LPVOID)this, THREAD_PRIORITY_NORMAL, 0, CREATE_SUSPENDED, NULL);
	if (!_pImageThread)	return false;
	_pImageThread->m_pMainWnd = this;	// メインウィンドウに自分自身を設定
	_pImageThread->m_bAutoDelete = TRUE;	// スレッド終了時にオブジェクトを自動的に破棄
	_pImageThread->ResumeThread();	// スレッドを開始

	// 刺激スレッド生成
	_pTargetThread = AfxBeginThread(CallTargetThreadProc,
		(LPVOID)this, THREAD_PRIORITY_NORMAL, 0, CREATE_SUSPENDED, NULL);
	if (!_pTargetThread)	return false;
	_pTargetThread->m_pMainWnd = this;	// メインウィンドウに自分自身を設定
	_pTargetThread->m_bAutoDelete = TRUE;	// スレッド終了時にオブジェクトを自動的に破棄
	_pTargetThread->ResumeThread();	// スレッドを開始

	return true;
}

// カメラスレッドを呼び出す static関数
UINT CECTrainer1Dlg::CallCameraThreadProc(LPVOID pParam)
{
	CECTrainer1Dlg* pDlg = dynamic_cast<CECTrainer1Dlg*>(reinterpret_cast<CWnd*>(pParam));
	if (pDlg) pDlg->CameraThreadProc();
	return 0;
}

// 画像処理スレッドを呼び出す static関数
UINT CECTrainer1Dlg::CallImageThreadProc(LPVOID pParam) {
	CECTrainer1Dlg* pDlg = dynamic_cast<CECTrainer1Dlg*>(reinterpret_cast<CWnd*>(pParam));
	if (pDlg) pDlg->ImageThreadProc();
	return 0;
}

// 刺激スレッドを呼び出す static関数
UINT CECTrainer1Dlg::CallTargetThreadProc(LPVOID pParam) {
	CECTrainer1Dlg* pDlg = dynamic_cast<CECTrainer1Dlg*>(reinterpret_cast<CWnd*>(pParam));
	if (pDlg) pDlg->TargetThreadProc();
	return 0;
}

// カメラスレッド本体
void CECTrainer1Dlg::CameraThreadProc()
{
	OutputDebugString(_T("CameraThreadProc start\n"));

	VideoCapture cam("rtsp://" ADDR ":8554/live/scene");
	if (!cam.isOpened()) {
		MessageBox(_T("cannot open camera " ADDR));
		return;
	}
	Mat frame;
	cam >> frame;
	_frameSize = frame.size();

	_write = 0, _read = 0;
	do {
		cam >> _frames[_write];
		_read = _write;
		_write = (++_write) % FRAMES;
	} while (_bRunning);

	OutputDebugString(_T("CameraThreadProc end\n"));
}

// 画像処理スレッド本体
void CECTrainer1Dlg::ImageThreadProc() 	{
	OutputDebugString(_T("ImageThreadProc start\n"));

	cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
	Point2f cornerPosW[] = { {5, 155}, {215, 155}, {5, 5}, {215, 5}, {5, 60},
		{5, 105}, {215, 60}, {215, 105}, {54, 5}, {54, 60} };

	int lastRead = 0;
	do {
		while (_read == lastRead && _bRunning) Sleep(1);
		Mat img = _frames[_read].clone();

		// マーカーの検出
		std::vector<int> mids;
		std::vector<std::vector<cv::Point2f>> corners;
		cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
		cv::aruco::detectMarkers(img, dictionary, corners, mids, parameters);
		cv::aruco::drawDetectedMarkers(img, corners, mids);

		std::vector<cv::Point2f> cornerW, cornerC;
		for (int i = 0; i < corners.size(); i++) {
			cornerC.push_back(corners[i][0]);
			cornerW.push_back(cornerPosW[mids[i] - 1]);

			//circle(img, corners[i][0], 10, CV_RGB(255, 0, 0), 2);
			//char msg[256];
			//sprintf_s(msg, 256, "%d", mids[i]);
			////putText(img, cv::format("%d", mids[i]), corners[i][0], FONT_HERSHEY_COMPLEX, 1.0, CV_RGB(255, 0, 0));
			//putText(img, msg, corners[i][0], FONT_HERSHEY_COMPLEX, 1.0, CV_RGB(255, 0, 0));
		}
		if (cornerC.size() >= 4) {
			_homography = findHomography(cornerC, cornerW);
			if (!_homography.empty()) {
				putText(img, "H", Point(5, 20), FONT_HERSHEY_COMPLEX, 1.0, CV_RGB(255, 0, 0), 1);
			}
		}

		if (_gpC.x > 0 && _gpC.y > 0) {
			circle(img, _gpC, 10, CV_RGB(255, 0, 0), 2);
		}

		ShowImage(IDC_IMAGE, img);
	} while (_bRunning);

	OutputDebugString(_T("ImageThreadProc end\n"));
}

// 刺激スレッド本体
void CECTrainer1Dlg::TargetThreadProc() {
	OutputDebugString(_T("TargetThreadProc start\n"));

	Size targetW = Size(240, 180);
	Mat target = imread(GetTargetFilename());
	Mat logimg = target.clone();
	int logimgCount = 0;
	int curMode = _mode;
	namedWindow("target", WINDOW_AUTOSIZE);

	//Mat target;
	//cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
	//aruco::drawMarker(dictionary, 1, 100, target, 1);

	ULONGLONG start = GetTickCount64();
	do {
		Mat tdisp = target.clone();

		Point2f gpW = Point2f(_gpW.x * target.size().width / targetW.width,
			_gpW.y * target.size().height / targetW.height);
		if (_gpW.x > 0 && _gpW.x < targetW.width && _gpW.y > 0 && _gpW.y < targetW.height) {
			if (GetTickCount64() > start + NOLOG) circle(logimg, gpW, 6, CV_RGB(0, 0, 255), 2);
			if (_mode < 1) circle(tdisp, gpW, 10, CV_RGB(0, 0, 255), 2);
		}

		imshow("target", tdisp);
		waitKey(1);

		if (_mode != curMode) {
			if (_mode == 1) logimgCount = 1;
			else {
				char writefn[256];
				sprintf_s(writefn, 256, "_img%02d", logimgCount++);
				imwrite(_logfile + writefn + ".png", logimg);
			}

			target = imread(GetTargetFilename());
			logimg = target.clone();
			curMode = _mode;
			start = GetTickCount64();
		}
		if (_mode > 0) {
			if (GetTickCount64() > start + DURATION) {
				if (++_mode >= NUM_FILES) {
					_mode = 0;
					Invalidate();
				}
			}
		}
	} while (_bRunning);

	OutputDebugString(_T("TargetThreadProc end\n"));
}

// 刺激画像ファイル名取得
String CECTrainer1Dlg::GetTargetFilename() {
	int i = _mode < NUM_FILES ? _mode : 0;
	String fname = "../images/";
	fname.append(FILE_LIST[i]);
	fname.append(".png");
	return fname;
}

// データスレッド開始
bool CECTrainer1Dlg::StartDataThreads()
{
	//Initialize winsock
	WSADATA wsa;
	if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0)
	{
		TRACE("Initialising Winsock Failed with error Code : %d\n", WSAGetLastError());
		return false;
	}
	if ((_socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == SOCKET_ERROR)
	{
		TRACE("Creating socket failed with error code : %d", WSAGetLastError());
		return false;
	}

	//setup address structure
	memset((char*)&_socketAddr, 0, sizeof(_socketAddr));
	_socketAddr.sin_family = AF_INET;
	_socketAddr.sin_port = htons(PORT);
	InetPton(_socketAddr.sin_family, _T(ADDR), &_socketAddr.sin_addr.S_un.S_addr);

	// Keep Alive スレッド生成
	_pKeepAliveThread = AfxBeginThread(CallKeepAliveThreadProc,
		(LPVOID)this, THREAD_PRIORITY_NORMAL, 0, CREATE_SUSPENDED, NULL);
	if (!_pKeepAliveThread) return false;
	_pKeepAliveThread->m_pMainWnd = this;	// メインウィンドウに自分自身を設定
	_pKeepAliveThread->m_bAutoDelete = TRUE;	// スレッド終了時にオブジェクトを自動的に破棄
	_pKeepAliveThread->ResumeThread();	// スレッドを開始

	// データスレッド生成
	_pDataThread = AfxBeginThread(CallDataThreadProc,
		(LPVOID)this, THREAD_PRIORITY_NORMAL, 0, CREATE_SUSPENDED, NULL);
	if (!_pDataThread) return false;
	_pDataThread->m_pMainWnd = this;	// メインウィンドウに自分自身を設定
	_pDataThread->m_bAutoDelete = TRUE;	// スレッド終了時にオブジェクトを自動的に破棄
	_pDataThread->ResumeThread();	// スレッドを開始

	return true;
}

// Keep Aliveスレッドを呼び出す static関数
UINT CECTrainer1Dlg::CallKeepAliveThreadProc(LPVOID pParam)
{
	CECTrainer1Dlg* pDlg = dynamic_cast<CECTrainer1Dlg*>(reinterpret_cast<CWnd*>(pParam));
	if (pDlg) pDlg->KeepAliveThreadProc();
	return 0;
}

// データスレッドを呼び出す static関数
UINT CECTrainer1Dlg::CallDataThreadProc(LPVOID pParam)
{
	CECTrainer1Dlg* pDlg = dynamic_cast<CECTrainer1Dlg*>(reinterpret_cast<CWnd*>(pParam));
	if (pDlg) pDlg->DataThreadProc();
	return 0;
}

// Keep Aliveスレッド本体
void CECTrainer1Dlg::KeepAliveThreadProc()
{
	OutputDebugString(_T("KeepAliveThreadProc start\n"));

	const int KEEP_ALIVE_WAIT_COUNT = 20; // x 100ms
	int slen = sizeof(_socketAddr);
	char message[BUFLEN];
	strcpy_s(message, BUFLEN, KA_DATA_MSG.c_str());

	do {
		//TRACE(_T("Keep Alive Data Sent \n"));
		if (sendto(_socket, message, (int)strlen(message), 0, (struct sockaddr*) & _socketAddr, slen) == SOCKET_ERROR)
		{
			TRACE(_T("Keep Alive Data Sent Error\n"));
		}
		for (int i = 0; i < KEEP_ALIVE_WAIT_COUNT && _bRunning; i ++) Sleep(100);
	} while (_bRunning);

	closesocket(_socket);
	WSACleanup();

	OutputDebugString(_T("KeepAliveThreadProc end\n"));
}

// データスレッド本体
void CECTrainer1Dlg::DataThreadProc()
{
	OutputDebugString(_T("DataThreadProc start\n"));

	int slen = sizeof(_socketAddr);
	char buf[BUFLEN];
	int lastGidx = 0;
	Point2f gp;
	std::ofstream ofs;
	int64 start = 0;
	MeanBuffer gpCx(DATA_MEAN_SIZE), gpCy(DATA_MEAN_SIZE);

	Sleep(500);	// 最初のKeepAlive送信待ち

	do {
		memset(buf, '\0', BUFLEN);
		if (recvfrom(_socket, buf, BUFLEN, 0,
			(struct sockaddr*) & _socketAddr, &slen) == SOCKET_ERROR)
		{
			TRACE(_T("Data Receive Error\n"));
			Sleep(10);
			continue;
		}
		//TRACE("%s\n", buf);

		GazeData gd(buf);
		if (gd.gidx > lastGidx) {
			if (gp.x > 0 && gp.y > 0 && !_homography.empty()) {
				// 座標変換
				gpCx.Push(gp.x * _frameSize.width);
				gpCy.Push(gp.y * _frameSize.height);
				_gpC = Point2f(gpCx.Mean(), gpCy.Mean());
				Mat gpCe = (cv::Mat_<double>(3, 1) << _gpC.x, _gpC.y, 1);
				Mat gpWe = _homography * gpCe;
				double* pgpWe = gpWe.ptr<double>(0);
				_gpW = Point2f((float)(*pgpWe / *(pgpWe + 2)), (float)(*(pgpWe + 1) / *(pgpWe + 2)));

				//TRACE("gpC %d,%d  gpWe %.1f,%.1f,%.1f  gpW %.1f,%.1f \n", 
				//	(int)gpC.x, (int)gpC.y, *pgpWe, *(pgpWe + 1), *(pgpWe + 2), 
				//	*pgpWe / *(pgpWe + 2), *(pgpWe + 1) / *(pgpWe + 2));

				// ログ出力
				if (_mode > 0) {
					if (!ofs) {
						SYSTEMTIME st;
						GetLocalTime(&st);
						char filename[256];
						sprintf_s(filename, 256, "../log/log%04d%02d%02d_%02d%02d%02d",
							st.wYear, st.wMonth, st.wDay, st.wHour, st.wMinute, st.wSecond);
						_logfile = filename;
						ofs.open(_logfile + ".csv");
						ofs << "time(ms),img,x,y" << std::endl;
						start = getTickCount();
					}
					ofs << (getTickCount() - start) * 1000. / getTickFrequency() << "," << _mode << ","
						<< _gpW.x << "," << _gpW.y << std::endl;
				} else {
					if (ofs) ofs.close();
				}
			}

			gp = Point(0, 0);
			lastGidx = gd.gidx;
		}
		if (gd.isGP && gd.s == 0) {
			gp = gd.gp;
			//_gp = gp;
			//TRACE("%d, %f, %f\n", gd.gidx, gd.gpx, gd.gpy);
		}
		
	} while (_bRunning);

	closesocket(_socket);
	WSACleanup();

	OutputDebugString(_T("DataThreadProc end\n"));
}

// 開始ボタン
void CECTrainer1Dlg::OnBnClickedBtnStart()
{
	if (_mode == 0) _mode = 1;
	else _mode = 0;
	Invalidate();
}