// SimpleMotionTracker.cpp : DLL アプリケーション用にエクスポートされる関数を定義します。
//

#include "stdafx.h"
#include "SimpleMotionTracker.h"

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/aruco.hpp>
#include <iostream>
#include <map>
#include <chrono>
#include "OpenCVDeviceEnumerator/DeviceEnumerator.h"

#include <dlib/opencv.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>

#define M_PI 3.14159265359
#define deg_to_rad(deg) (((deg)/180)*M_PI)
#define rad_to_deg(rad) (((rad)/M_PI)*180)

class SimpleMotionTracker {
public:
	void initVideoDeviceList();
	void init(const std::string& videoDeviceName, const std::string& dataPath);
	void init(int cameraId, const std::string& dataPath);
	void destroy();
	void update();

	void setUseARMarker(bool useARMarker);
	void setUseFaceTracking(bool useFaceTracking);
	void setUseEyesTracking(bool useEyesTracking);
	void setUseHandTracking(bool useHandTracking);

	void setCaptureShown(bool isShown);
	bool isCaptureShown();

	void setIrisThresh(int thresh) { _irisThresh = thresh; }

	void setARMarkerEdgeLength(float length);
	bool isARMarkerDetected(int id);
	void getARMarker6DoF(int id, float* outArray);

	void setMinHandTranslationThreshold(float thresh) { _minHandTranslationThreshold = thresh; }
	void setMaxHandTranslationThreshold(float thresh) { _maxHandTranslationThreshold = thresh; }
	void setHandUndetectedDuration(int msec) { _handUndetectedDuration = msec; }
	bool isFacePointsDetected();
	void getFacePoints(float* outArray);

	bool isLeftHandDetected();
	bool isRightHandDetected();
	bool isLeftHandDown();
	bool isRightHandDown();
	void getHandPoints(float* outArray);

	int getErrorCode();

private:
	void loadCameraParam(const std::string& fileName);
	static cv::Rect adjustRect(const cv::Rect& rect, const cv::Mat frame);
	void detectARMarker();

	static void detectMultiScaleRecursive(cv::CascadeClassifier& cascade, cv::Mat& frame, std::vector<cv::Rect>& outRects, cv::Size min, cv::Size max, int depth);
	void detectFace();
	void detectFaceDlib();
	void detectIris(int irisThresh, cv::Mat faceFrame, cv::Rect eyeRect, cv::Rect& outIris);
	void detectIrisDlib(int irisThresh, cv::Mat faceFrame, cv::Rect eyeRect, cv::Rect& outIris);

	void detectHandCircle(float* handCircle, bool& isHandDetected, bool& isHandDown,  int& handUndetectedTick,
		                  const cv::Point& point, std::vector<cv::Point>& points,
		                  float minHandRadius, float maxHandRadius,
						  std::list<float>& radiusBuf, float resizeRatio, cv::Mat frame);
	void detectHand();

	bool _useARMarker = false;
	bool _useFaceTracking = false;
	bool _useEyesTracking = false;
	bool _useHandTracking = false;
	bool _isCaptureShown = false;

	static const char* WINDOW_NAME;

	cv::VideoCapture _cap;
	cv::Mat _capFrame; // 取得フレーム.
	cv::Mat _outputFrame; // 表示用(描き込み対象)

	cv::Mat _cameraMatrix;
	cv::Mat _distCoeffs;

	std::vector<int> _markerIds;
	std::vector<std::vector<cv::Point2f>> _markerCorners, _rejectedCandidates;
	cv::Ptr<cv::aruco::DetectorParameters> _parameters;
	cv::Ptr<cv::aruco::Dictionary> _dictionary;
	float _markerEdgeLength = 0.1f; // マーカーの辺の長さ. 単位はメートル

	std::map<int, std::vector<float>> _markerIdTRvec; // マーカーIDをキーにした tvec, rvec の6つ値を保持
	std::map<int, bool> _markerIdDetected; // マーカーIDをキーにした検出済フラグ

	cv::CascadeClassifier _faceCascade; // 顏検出器.
	cv::CascadeClassifier _eyesCascade; // 目検出器.

	dlib::frontal_face_detector _faceDetector;
	dlib::shape_predictor _facePredictor;

	bool _isFacePointsDetected = false;
	float _faceCircle[3]; // X,Y,半径
	float _leftEyeCircle[3];
	float _rightEyeCircle[3];
	float _leftIrisCircle[3];
	float _rightIrisCircle[3];
	float _faceAngle[3]; // X,Y,Z

	int _irisThresh = 30;

	cv::Ptr<cv::BackgroundSubtractor> _handBackSub; // 手検出用の背景除去マスク
	float _minHandTranslationThreshold = 0.05f;
	float _maxHandTranslationThreshold = 3.0f;
	bool _isLeftHandDetected = false;
	bool _isRightHandDetected = false;
	bool _isLeftHandDown = false;
	bool _isRightHandDown = false;
	int _leftHandUndetectedTick = 0;
	int _rightHandUndetectedTick = 0;
	int _handUndetectedDuration = 5000; // ミリ秒
	float _leftHandCircle[3];
	float _rightHandCircle[3];
	const int HAND_RADIUS_BUF_SIZE = 3;
	std::list<float> _leftHandRadiusBuf; // 半径を遅延採用するために使う
	std::list<float> _rightHandRadiusBuf;

	int _errorCode = SMT_ERROR_NOEN;
};

const char* SimpleMotionTracker::WINDOW_NAME = "SimpleMotionTracker(Capture)";

void SimpleMotionTracker::initVideoDeviceList() {
	DeviceEnumerator de;
	std::map<int, Device> devices = de.getVideoDevicesMap();
}

void SimpleMotionTracker::init(const std::string& videoDeviceName, const std::string& dataPath) {
	int cameraId = -1;
	DeviceEnumerator de;
	std::map<int, Device> devices = de.getVideoDevicesMap();
	for (auto it : devices) {
		if (it.second.deviceName == videoDeviceName) {
			cameraId = it.first;
			break;
		}
	}
	init(cameraId, dataPath);
}

void SimpleMotionTracker::init(int cameraId, const std::string& dataPath) {
	// デバイスを開く.
	_cap.open(cameraId);

	if (!_cap.isOpened()) {
		_errorCode = SMT_ERROR_UNOPEND_CAMERA;
		return;
	}

	if (!_cap.read(_capFrame)) {
		_errorCode = SMT_ERROR_UNREADABLE_CAMERA;
		return;
	}

	// キャプチャ速度計測50msec以下は非対応
	auto start = std::chrono::system_clock::now();
	_cap.read(_capFrame);
	auto end = std::chrono::system_clock::now();
	auto dur = end - start;
	auto msec = std::chrono::duration_cast<std::chrono::milliseconds>(dur).count();
	if (msec > 500) {
		_errorCode = SMT_ERROR_INSUFFICIENT_CAMERA_CAPTURE_SPEED;
		return;
	}

	if (!_faceCascade.load(dataPath + "haarcascade_frontalface_alt.xml")) {
		_errorCode = SMT_ERROR_UNOPEN_FACE_CASCADE;
		return;
	}

	if (!_eyesCascade.load(dataPath + "haarcascade_eye.xml")) {
		_errorCode = SMT_ERROR_UNOPEN_EYE_CASCADE;
		return;
	}

	// カメラパラメータ読み込み.
	loadCameraParam(dataPath + "camera_param.xml");

	// Dlib用顏検出器とランドマークを読み込み
	_faceDetector = dlib::get_frontal_face_detector();
	dlib::deserialize(dataPath + "sp_human_face_68_for_mobile.dat") >> _facePredictor;

	// マーカー検出
	_markerIds.clear();
	_markerCorners.clear();
	_rejectedCandidates.clear();
	_parameters = cv::aruco::DetectorParameters::create();
	_dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
	for (int i = 0; i < 50; i++) {
		_markerIdTRvec[i].resize(6);
		_markerIdDetected[i] = false;
	}

	// 顏検出
	_isFacePointsDetected = false;
	for (int i = 0; i < 3; i++) {
		_faceCircle[i] = 0;
		_leftEyeCircle[i] = 0;
		_rightEyeCircle[i] = 0;
		_leftIrisCircle[i] = 0;
		_rightIrisCircle[i] = 0;
		_faceAngle[i] = 0;
	}

	// 手検出
	//_handBackSub = cv::createBackgroundSubtractorMOG2();
	_handBackSub = cv::createBackgroundSubtractorKNN();
	_isLeftHandDetected = false;
	_isRightHandDetected = false;
	_leftHandUndetectedTick = 0;
	_rightHandUndetectedTick = 0;
	for (int i = 0; i < 3; i++) {
		_leftHandCircle[i] = 0;
		_rightHandCircle[i] = 0;
	}
	_leftHandRadiusBuf.clear();
	_rightHandRadiusBuf.clear();
	for (int i = 0; i < HAND_RADIUS_BUF_SIZE; i++) {
		_leftHandRadiusBuf.push_back(0);
		_rightHandRadiusBuf.push_back(0);
	}
}

void SimpleMotionTracker::destroy() {
	_cap.release(); // デストラクタで呼ばれるらしいけど明示的に開放.
	cv::destroyAllWindows();
}

void SimpleMotionTracker::update() {
	if (_errorCode != SMT_ERROR_NOEN) {
		return;
	}

	if (_cap.read(_capFrame)) {
		_outputFrame = _capFrame.clone();
		if (_useARMarker) {
			detectARMarker();
		}
		if (_useFaceTracking || _useEyesTracking) {
			//detectFace();
			detectFaceDlib();
		}
		if (_useHandTracking) {
			detectHand();
		}

		if (_isCaptureShown) {
			cv::imshow(WINDOW_NAME, _outputFrame);
		}
		else {
			cv::destroyWindow(WINDOW_NAME);
		}
		//cv::waitKey(1); // これが無いとWaitが無くて画面が表示されない
	}
}

void SimpleMotionTracker::setUseARMarker(bool useARMarker) {
	_useARMarker = useARMarker;
}

void SimpleMotionTracker::setUseFaceTracking(bool useFaceTracking) {
	_useFaceTracking = useFaceTracking;
}

void SimpleMotionTracker::setUseEyesTracking(bool useEyesTracking) {
	_useEyesTracking = useEyesTracking;
}

void SimpleMotionTracker::setUseHandTracking(bool useHandTracking) {
	_useHandTracking = useHandTracking;
}

void SimpleMotionTracker::setCaptureShown(bool isShown) {
	_isCaptureShown = isShown;
}

bool SimpleMotionTracker::isCaptureShown() {
	return _isCaptureShown;
}

int SimpleMotionTracker::getErrorCode() {
	return _errorCode;
}

void SimpleMotionTracker::loadCameraParam(const std::string& fileName) {
	cv::FileStorage fs(fileName, cv::FileStorage::READ);
	if (!fs.isOpened()) {
		_errorCode = SMT_ERROR_UNOPEND_CAMERA_PARAM_FILE;
		return; // エラー
	}
	fs["camera_matrix"] >> _cameraMatrix;
	fs["distortion_coefficients"] >> _distCoeffs;
	fs.release();
}

cv::Rect SimpleMotionTracker::adjustRect(const cv::Rect& rect, const cv::Mat frame) {
	cv::Rect ret = rect;
	if (ret.x < 0) {
		ret.width += ret.x;
		ret.x = 0;
	}
	if (ret.y < 0) {
		ret.height += ret.y;
		ret.y = 0;
	}
	if (ret.x + ret.width >= frame.cols) {
		int remainder = (ret.x + ret.width) - frame.cols;
		ret.width -= remainder;
	}
	if (ret.y + ret.height >= frame.rows) {
		int remainder = (ret.y + ret.height) - frame.rows;
		ret.height -= remainder;
	}
	return ret;
}

void SimpleMotionTracker::detectARMarker() {
	for (int i = 0; i < 50; i++) {
		_markerIdDetected[i] = false; // 非検出状態へ
	}

	// マーカーを検出
	cv::aruco::detectMarkers(_capFrame, _dictionary, _markerCorners, _markerIds, _parameters, _rejectedCandidates, _cameraMatrix, _distCoeffs);

	// マーカー姿勢推定
	std::vector<cv::Vec3d> rvecs, tvecs;
	cv::aruco::estimatePoseSingleMarkers(_markerCorners, _markerEdgeLength, _cameraMatrix, _distCoeffs, rvecs, tvecs);

	for (int i = 0; i < rvecs.size(); ++i) {
		auto rvec = rvecs[i];
		auto tvec = tvecs[i];
		cv::aruco::drawAxis(_outputFrame, _cameraMatrix, _distCoeffs, rvec, tvec, 0.1);

		// オイラー角を得る
		cv::Vec3d eulerAngles;
		{
			cv::Mat rotMatrix;
			cv::Rodrigues(rvec, rotMatrix);

			double* _r = rotMatrix.ptr<double>();
			double projMatrix[12] = { _r[0],_r[1],_r[2],0,_r[3],_r[4],_r[5],0,_r[6],_r[7],_r[8],0 };
			cv::Mat projMat = cv::Mat(3, 4, CV_64FC1, projMatrix);

			cv::Mat cameraMatrix, rotation_matrix, translation_vector, rotMatrixX, rotMatrixY, rotMatrixZ;
			decomposeProjectionMatrix(projMat, cameraMatrix, rotation_matrix, translation_vector, rotMatrixX, rotMatrixY, rotMatrixZ, eulerAngles);
		}

		auto id = _markerIds[i];
		_markerIdDetected[id] = true;

		_markerIdTRvec[id][0] = tvec[0];
		_markerIdTRvec[id][1] = tvec[1];
		_markerIdTRvec[id][2] = tvec[2];
		_markerIdTRvec[id][3] = eulerAngles[0];
		_markerIdTRvec[id][4] = eulerAngles[1];
		_markerIdTRvec[id][5] = eulerAngles[2];
	}

	// マーカー情報を書き込み
	cv::aruco::drawDetectedMarkers(_outputFrame, _markerCorners, _markerIds);
}

void SimpleMotionTracker::setARMarkerEdgeLength(float length) {
	_markerEdgeLength = length;
}

bool SimpleMotionTracker::isARMarkerDetected(int id) {
	return _markerIdDetected[id];
}

void SimpleMotionTracker::getARMarker6DoF(int id, float* outArray) {
	auto& vec = _markerIdTRvec[id];
	for (int i = 0; i < 6; i++) {
		outArray[i] = vec[i];
	}
}

void SimpleMotionTracker::detectIris(int irisThresh, cv::Mat faceFrame, cv::Rect eyeRect, cv::Rect& outIris) {
	if (eyeRect.width > 0) {
		auto eyeFrame = faceFrame(eyeRect);
		cv::equalizeHist(eyeFrame, eyeFrame);

		// 虹彩検出
		cv::equalizeHist(eyeFrame, eyeFrame);
		cv::threshold(eyeFrame, eyeFrame, irisThresh, 255, cv::THRESH_BINARY_INV);

		int morphSize = 1;
		cv::Mat element = cv::getStructuringElement(0, cv::Size(2 * morphSize + 1, 2 * morphSize + 1), cv::Point(morphSize, morphSize));
		cv::morphologyEx(eyeFrame, eyeFrame, cv::MORPH_CLOSE, element);
		cv::morphologyEx(eyeFrame, eyeFrame, cv::MORPH_OPEN, element);
		cv::bitwise_not(eyeFrame, eyeFrame);

		cv::GaussianBlur(eyeFrame, eyeFrame, cv::Size(5, 5), 0);
		cv::threshold(eyeFrame, eyeFrame, 0, 255, cv::THRESH_BINARY);

		int cannyThresh = 100;
		cv::Mat cannyOutput;
		cv::Canny(eyeFrame, cannyOutput, cannyThresh, cannyThresh * 2);
		std::vector<std::vector<cv::Point>> contours;
		cv::findContours(cannyOutput, contours, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);

		std::vector<std::vector<cv::Point>> contoursPoly(contours.size());
		std::vector<cv::Rect> boundRect(contours.size());
		std::vector<cv::Point2f>centers(contours.size());
		for (size_t i = 0; i < contours.size(); i++) {
			approxPolyDP(contours[i], contoursPoly[i], 3, true);
			boundRect[i] = boundingRect(contoursPoly[i]);
		}

		cv::Rect max;
		for (auto rect : boundRect) {
			if (max.width < rect.width) {
				max = rect;
			}
		}
		outIris = max;
		//cv::rectangle(eyeFrame, max.tl(), max.br(), cv::Scalar(0, 0, 0), 2);
		//cv::imshow("test", eyeFrame);
	}
}

void SimpleMotionTracker::detectIrisDlib(int irisThresh, cv::Mat eyeFrame, cv::Rect eyeRect, cv::Rect& outIris) {
	if (eyeRect.width > 0) {
		// 虹彩検出
		cv::equalizeHist(eyeFrame, eyeFrame);
		cv::threshold(eyeFrame, eyeFrame, irisThresh, 255, cv::THRESH_BINARY_INV);

		int morphSize = 1;
		cv::Mat element = cv::getStructuringElement(0, cv::Size(2 * morphSize + 1, 2 * morphSize + 1), cv::Point(morphSize, morphSize));
		cv::morphologyEx(eyeFrame, eyeFrame, cv::MORPH_CLOSE, element);
		cv::morphologyEx(eyeFrame, eyeFrame, cv::MORPH_OPEN, element);
		cv::bitwise_not(eyeFrame, eyeFrame);

		cv::GaussianBlur(eyeFrame, eyeFrame, cv::Size(5, 5), 0);
		cv::threshold(eyeFrame, eyeFrame, 0, 255, cv::THRESH_BINARY);

		int cannyThresh = 100;
		cv::Mat cannyOutput;
		cv::Canny(eyeFrame, cannyOutput, cannyThresh, cannyThresh * 2);
		std::vector<std::vector<cv::Point>> contours;
		cv::findContours(cannyOutput, contours, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);

		std::vector<std::vector<cv::Point>> contoursPoly(contours.size());
		std::vector<cv::Rect> boundRect(contours.size());
		std::vector<cv::Point2f>centers(contours.size());
		for (size_t i = 0; i < contours.size(); i++) {
			approxPolyDP(contours[i], contoursPoly[i], 3, true);
			boundRect[i] = boundingRect(contoursPoly[i]);
		}

		cv::Rect max;
		for (auto rect : boundRect) {
			if (max.width < rect.width) {
				max = rect;
			}
		}
		outIris = max;
		//cv::rectangle(eyeFrame, max.tl(), max.br(), cv::Scalar(0, 0, 0), 2);
		//cv::imshow("test", eyeFrame);
	}
}

void SimpleMotionTracker::detectMultiScaleRecursive(cv::CascadeClassifier& cascade, cv::Mat& frame, std::vector<cv::Rect>& outRects, cv::Size min, cv::Size max, int depth) {
	if (depth == 0) return;

	std::vector<cv::Rect> rects;
	cascade.detectMultiScale(frame, rects, 1.1, 3, 0, min, max);
	if (rects.size() > 0) {
		auto rect = rects[0];
		cv::Rect scaledRect = rect;
		float scale = 1.1f;
		scaledRect.width *= scale;
		scaledRect.height *= scale;
		scaledRect.x -= (scaledRect.width - rect.width) / 2;
		scaledRect.y -= (scaledRect.height - rect.height) / 2;
		scaledRect = adjustRect(scaledRect, frame);
		cv::Mat roi = frame(scaledRect);

		min.width = rect.width * 0.9f;
		min.height = rect.height * 0.9f;
		max.width = scaledRect.width;
		max.height = scaledRect.height;
		// 再帰的に処理
		detectMultiScaleRecursive(cascade, roi, outRects, min, max, depth - 1);
		if (outRects.size() == 0) {
			outRects.push_back(rect);
		}
		else {
			outRects[0].x += scaledRect.x;
			outRects[0].y += scaledRect.y;
		}
	}
}

void SimpleMotionTracker::detectFace() {
	bool prevIsFacePointsDetected = _isFacePointsDetected;
	_isFacePointsDetected = false;

	// 不可対策で検索範囲を絞る
	int minEdgeLen = (_faceCircle[2] * 2) * 0.9f; // 前回の1割りを最小サイズとする
	cv::Size minSize(minEdgeLen, minEdgeLen);

	float targetScale = 1.2f;
	cv::Rect targetRect(_faceCircle[0] - (_faceCircle[2] * targetScale), // 前回の少し大きめを対象とする
		                _faceCircle[1] - (_faceCircle[2] * targetScale),
					    (_faceCircle[2] * targetScale) * 2,
						(_faceCircle[2] * targetScale) * 2);
	targetRect = adjustRect(targetRect, _capFrame);
	cv::Size maxSize(targetRect.width, targetRect.height);

	float resizeRate = 0.2f;

	cv::Mat frameGray;
	if (prevIsFacePointsDetected) {
		frameGray = _capFrame(targetRect);
		resizeRate *= _capFrame.rows / (_faceCircle[2] * 2); // 前回を元に縮小率を調整
		if (resizeRate > 1) { resizeRate = 1; }
		minSize.width *= resizeRate;
		minSize.height *= resizeRate;
		maxSize.width *= resizeRate;
		maxSize.height *= resizeRate;
	}
	else {
		frameGray = _capFrame.clone();
		minSize.width = 0;
		minSize.height = 0;
		maxSize.width = 0;
		maxSize.height = 0;
	}
	cv::cvtColor(frameGray, frameGray, cv::COLOR_BGR2GRAY);

	// 顏検出用に縮小画像で負荷軽減.
	cv::Mat faceDetectFrame;
	cv::resize(frameGray, faceDetectFrame, cv::Size(frameGray.cols * resizeRate, (frameGray.rows * resizeRate)));
	cv::equalizeHist(faceDetectFrame, faceDetectFrame);

	float detectedAngle = 0;
	for (auto angle : { 0, -20, 20, -40, 40 }) {
		// Z回転している顔を検出するために画像を回転させる.
		cv::Mat rotatedFaceDetectFrame;
		auto trans = cv::getRotationMatrix2D(cv::Point2f(faceDetectFrame.cols / 2, faceDetectFrame.rows / 2), angle, 1.0f);
		cv::warpAffine(faceDetectFrame, rotatedFaceDetectFrame, trans, cv::Size(faceDetectFrame.cols, faceDetectFrame.rows));

		std::vector<cv::Rect> faces; // 複数検出を考慮.
		//_faceCascade.detectMultiScale(rotatedFaceDetectFrame, faces, 1.1, 3, 0, minSize, maxSize);

		// 多段検出で精度を上げる
		int _faceDetectionLevel = 3;
		detectMultiScaleRecursive(_faceCascade, rotatedFaceDetectFrame, faces, minSize, maxSize, _faceDetectionLevel);

		cv::Rect face;
		cv::Rect leftEye;
		cv::Rect rightEye;
		cv::Rect leftIris;
		cv::Rect rightIris;

		for (auto f : faces) {
			f.x /= resizeRate;
			f.y /= resizeRate;
			f.width /= resizeRate;
			f.height /= resizeRate;
			if (f.width <= face.width) {
				continue;
			}
			face = f;

			// 目の検出.
			auto trans = cv::getRotationMatrix2D(cv::Point2f(frameGray.cols / 2, frameGray.rows / 2), angle, 1.0f);
			cv::warpAffine(frameGray, frameGray, trans, cv::Size(frameGray.cols, frameGray.rows));
			cv::Rect upperFace(f);
			int splitHeight = upperFace.height / 4;
			upperFace.height = splitHeight * 1.5f;
			upperFace.y += splitHeight;
			cv::Mat faceFrame = frameGray(upperFace).clone();
			cv::circle(faceFrame, cv::Point(faceFrame.cols / 2, faceFrame.rows), faceFrame.rows / 4, cv::Scalar(255, 255, 255), -1); // 鼻の孔隠し
			cv::equalizeHist(faceFrame, faceFrame);
			std::vector<cv::Rect> eyes;
			_eyesCascade.detectMultiScale(faceFrame, eyes);
			//cv::imshow("test", faceFrame);

			// 左右の目に振り分け.
			for (auto eye : eyes) {
				if ((eye.x + eye.width / 2) > (faceFrame.cols / 2)) {
					if (eye.width > leftEye.width) {
						leftEye = eye;
					}
				}
				else {
					if (eye.width > rightEye.width) {
						rightEye = eye;
					}
				}
			}
			// 検出できなかった時はそれっぽい場所を指定する
			auto adjustEyeRect = [faceFrame](cv::Rect& rect, int block) {
				int w = faceFrame.cols / 10;
				int h = faceFrame.rows / 10;
				rect.x = w * block;
				rect.y = h * 2;
				rect.width = w * 2;
				rect.height = h * 8;
				if (rect.height == 0) {
					rect.y = 0;
					rect.height = faceFrame.rows;
				}
			};
			if (leftEye.width == 0) {
				adjustEyeRect(leftEye, 6);
			}
			if (rightEye.width == 0) {
				adjustEyeRect(rightEye, 2);
			}
			detectedAngle = angle;

			// 虹彩検出
			if (_useEyesTracking) {
				faceFrame = frameGray(upperFace).clone();
				if (leftEye.width > 0) {
					detectIris(_irisThresh, faceFrame, leftEye, leftIris);
				}
				if (rightEye.width > 0) {
					detectIris(_irisThresh, faceFrame, rightEye, rightIris);
				}
			}

			break; // 検出は1つだけ前提で抜ける.
		}
		if (face.width > 0) {
			cv::Mat_<double> trans = cv::getRotationMatrix2D(cv::Point2f(0, 0), -detectedAngle, 1.0f);
			cv::Point2f frameCenter(frameGray.cols / 2, frameGray.rows / 2);

			auto afinTransform = [=](const cv::Rect& rect) {
				cv::Point2f center(rect.x + rect.width / 2, rect.y + rect.height / 2);
				float x = center.x - frameCenter.x;
				float y = center.y - frameCenter.y;
				float x2 = frameCenter.x + (trans(0, 0) * x + trans(0, 1) * y);
				float y2 = frameCenter.y + (trans(1, 0) * x + trans(1, 1) * y);
				if (prevIsFacePointsDetected) {
					x2 += targetRect.x;
					y2 += targetRect.y;
				}
				return cv::Point2f(x2, y2);
			};

			{
				auto p = afinTransform(face);
				cv::circle(_outputFrame, p, face.width / 2, cv::Scalar(255, 0, 255), 2);
				_faceCircle[0] = p.x;
				_faceCircle[1] = p.y;
				_faceCircle[2] = face.width / 2;
			}
			{
				cv::Rect upperFace(face);
				upperFace.height /= 4;
				upperFace.y += upperFace.height;
				if (leftEye.width > 0) {
					leftEye.x += upperFace.x;
					leftEye.y += upperFace.y;

					auto p = afinTransform(leftEye);
					cv::circle(_outputFrame, p, leftEye.width / 2, cv::Scalar(255, 255, 0), 2);
					_leftEyeCircle[0] = p.x;
					_leftEyeCircle[1] = p.y;
					_leftEyeCircle[2] = leftEye.width / 2;
				}
				if (rightEye.width > 0) {
					rightEye.x += upperFace.x;
					rightEye.y += upperFace.y;

					auto p = afinTransform(rightEye);
					cv::circle(_outputFrame, p, rightEye.width / 2, cv::Scalar(255, 255, 0), 2);
					_rightEyeCircle[0] = p.x;
					_rightEyeCircle[1] = p.y;
					_rightEyeCircle[2] = rightEye.width / 2;
				}
			}
			{
				if (leftEye.width > 0 && leftIris.width > 0) {
					leftIris.x += leftEye.x;
					leftIris.y += leftEye.y;

					auto p = afinTransform(leftIris);
					cv::circle(_outputFrame, p, leftIris.height / 2, cv::Scalar(0, 0, 255), 2);
					_leftIrisCircle[0] = p.x;
					_leftIrisCircle[1] = p.y;
					_leftIrisCircle[2] = (float)(leftIris.height) / leftIris.width;
				}
				if (rightEye.width > 0 && rightIris.width > 0) {
					rightIris.x += rightEye.x;
					rightIris.y += rightEye.y;

					auto p = afinTransform(rightIris);
					cv::circle(_outputFrame, p, rightIris.height / 2, cv::Scalar(0, 0, 255), 2);
					_rightIrisCircle[0] = p.x;
					_rightIrisCircle[1] = p.y;
					_rightIrisCircle[2] = (float)(rightIris.height) / rightIris.width;
				}
			}

			_isFacePointsDetected = true;
			//cv::imshow("test", frameGray);
			return;
		}
	}
}

void SimpleMotionTracker::detectFaceDlib() {
	_isFacePointsDetected = false;

	cv::Mat temp = _capFrame.clone();
	dlib::cv_image<dlib::bgr_pixel> cimg(temp);
	std::vector<dlib::rectangle> faces = _faceDetector(cimg);
	if (faces.size() > 0) {
		auto face = faces[0];
		dlib::full_object_detection shape = _facePredictor(cimg, face);
		auto minPoint = cv::Point(9999, 9999);
		auto maxPoint = cv::Point(0, 0);
		for (int i = 0; i < shape.num_parts(); i++)
		{
			if (shape.part(i).x() < minPoint.x) { minPoint.x = shape.part(i).x(); }
			if (shape.part(i).x() > maxPoint.x) { maxPoint.x = shape.part(i).x(); }
			if (shape.part(i).y() < minPoint.y) { minPoint.y = shape.part(i).y(); }
			if (shape.part(i).y() > maxPoint.y) { maxPoint.y = shape.part(i).y(); }
			cv::circle(_outputFrame, cv::Point2d(shape.part(i).x(), shape.part(i).y()), 3, cv::Scalar(0, 255, 0), -1);
		}

		{ // 顏の位置
			auto faceRect = cv::Rect(minPoint.x, minPoint.y, maxPoint.x - minPoint.x, maxPoint.y - minPoint.y);
			auto p = cv::Point(faceRect.x + faceRect.width / 2, faceRect.y + faceRect.height / 2);
			auto r = (faceRect.width + faceRect.height) / 2 / 2; // 平均の半分
			cv::circle(_outputFrame, p, r, cv::Scalar(255, 0, 255), 2);
			_faceCircle[0] = p.x;
			_faceCircle[1] = p.y;
			_faceCircle[2] = r;
		}

		{ // 左目の位置
			auto& p1 = shape.part(42);
			auto& p2 = shape.part(45);
			auto p = cv::Point(
				(p1.x() + p2.x()) / 2,
				(p1.y() + p2.y()) / 2);
			auto toX = p2.x() - p1.x();
			auto toY = p2.y() - p1.y();
			auto r = std::sqrtf((toX * toX) + (toY * toY)) / 2;
			cv::circle(_outputFrame, p, r, cv::Scalar(255, 255, 0), 2);
			_leftEyeCircle[0] = p.x;
			_leftEyeCircle[1] = p.y;
			_leftEyeCircle[2] = r;
		}

		{ // 右目の位置
			auto& p1 = shape.part(36);
			auto& p2 = shape.part(39);
			auto p = cv::Point(
				(p1.x() + p2.x()) / 2,
				(p1.y() + p2.y()) / 2);
			auto toX = p2.x() - p1.x();
			auto toY = p2.y() - p1.y();
			auto r = std::sqrtf((toX * toX) + (toY * toY)) / 2;
			cv::circle(_outputFrame, p, r, cv::Scalar(255, 255, 0), 2);
			_rightEyeCircle[0] = p.x;
			_rightEyeCircle[1] = p.y;
			_rightEyeCircle[2] = r;
		}

		// 虹彩検出
		if (_useEyesTracking) {
			cv::Rect leftIris;
			cv::Rect rightIris;

			cv::Mat frameGray;
			cv::cvtColor(_capFrame, frameGray, cv::COLOR_BGR2GRAY);

			{ // 左虹彩
				auto eyeRect = cv::Rect(
					_leftEyeCircle[0] - _leftEyeCircle[2],
					_leftEyeCircle[1] - _leftEyeCircle[2],
					_leftEyeCircle[2] * 2,
					_leftEyeCircle[2] * 2);
				if (eyeRect.x >= 0 &&
					eyeRect.y >= 0 &&
					eyeRect.x + eyeRect.width < frameGray.cols &&
					eyeRect.y + eyeRect.height < frameGray.rows) {
					cv::Mat eyeFrame = frameGray(eyeRect);
					detectIrisDlib(_irisThresh, eyeFrame, eyeRect, leftIris);

					if (eyeRect.width > 0 && leftIris.width > 0) {
						leftIris.x += eyeRect.x;
						leftIris.y += eyeRect.y;

						auto& p1 = shape.part(43);
						auto& p2 = shape.part(47);
						auto toX = p2.x() - p1.x();
						auto toY = p2.y() - p1.y();
						auto vertical = std::sqrtf((toX * toX) + (toY * toY));
						auto horizontal = _leftEyeCircle[2];

						auto p = cv::Point(leftIris.x + leftIris.width / 2, leftIris.y + leftIris.height / 2);
						cv::circle(_outputFrame, p, leftIris.height / 2, cv::Scalar(0, 0, 255), 2);
						_leftIrisCircle[0] = p.x;
						_leftIrisCircle[1] = p.y;
						_leftIrisCircle[2] = vertical / horizontal;
					}
				}
			}

			{ // 右虹彩
				auto eyeRect = cv::Rect(
					_rightEyeCircle[0] - _rightEyeCircle[2],
					_rightEyeCircle[1] - _rightEyeCircle[2],
					_rightEyeCircle[2] * 2,
					_rightEyeCircle[2] * 2);
				if (eyeRect.x >= 0 &&
					eyeRect.y >= 0 &&
					eyeRect.x + eyeRect.width < frameGray.cols &&
					eyeRect.y + eyeRect.height < frameGray.rows) {
					cv::Mat eyeFrame = frameGray(eyeRect);
					detectIrisDlib(_irisThresh, eyeFrame, eyeRect, rightIris);

					if (eyeRect.width > 0 && rightIris.width > 0) {
						rightIris.x += eyeRect.x;
						rightIris.y += eyeRect.y;

						auto& p1 = shape.part(38);
						auto& p2 = shape.part(40);
						auto toX = p2.x() - p1.x();
						auto toY = p2.y() - p1.y();
						auto vertical = std::sqrtf((toX * toX) + (toY * toY));
						auto horizontal = _rightEyeCircle[2];

						auto p = cv::Point(rightIris.x + rightIris.width / 2, rightIris.y + rightIris.height / 2);
						cv::circle(_outputFrame, p, rightIris.height / 2, cv::Scalar(0, 0, 255), 2);
						_rightIrisCircle[0] = p.x;
						_rightIrisCircle[1] = p.y;
						_rightIrisCircle[2] = vertical / horizontal;
					}
				}
			}
		}

		// 向き推定
		// 顏のパーツの位置関係からおおよその傾きを推定する
		auto nosePoint = cv::Point2d(shape.part(30).x(), shape.part(30).y()); // 鼻の中心
		auto leftEye = cv::Point2d(shape.part(36).x(), shape.part(36).y()); // 左目の左端
		auto rightEye = cv::Point2d(shape.part(45).x(), shape.part(45).y()); // 右目の右端

		// Z軸での傾きを両目の位置関係から計算
		auto eyesTilt = cv::Point2d(rightEye.x - leftEye.x, rightEye.y - leftEye.y);
		float angleZ = rad_to_deg(-std::atan2(eyesTilt.y, eyesTilt.x));

		auto radius = _faceCircle[2];
		auto diffNoseAndFaceCenter = cv::Point2d(_faceCircle[0] - nosePoint.x, _faceCircle[1] - nosePoint.y); // 顏の中心との差
		auto angleX = rad_to_deg(std::asin(diffNoseAndFaceCenter.y / radius));
		auto angleY = rad_to_deg(std::asin(diffNoseAndFaceCenter.x / radius));

		_faceAngle[0] = angleX;
		_faceAngle[1] = angleY;
		_faceAngle[2] = angleZ;

		// オイラー角の変換結果を表示 Debug用
		std::ostringstream outtext;
		//outtext << "X: " << std::setprecision(3) << _faceAngle[0];
		//cv::putText(_outputFrame, outtext.str(), cv::Point(50, 40), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0));
		//outtext.str("");
		//outtext << "Y: " << std::setprecision(3) << _faceAngle[1];
		//cv::putText(_outputFrame, outtext.str(), cv::Point(50, 60), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0));
		//outtext.str("");
		//outtext << "Z: " << std::setprecision(3) << _faceAngle[2];
		//cv::putText(_outputFrame, outtext.str(), cv::Point(50, 80), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0));
		//outtext.str("");

		_isFacePointsDetected = true;
		return;
	}
}

bool SimpleMotionTracker::isFacePointsDetected() {
	return _isFacePointsDetected;
}

void SimpleMotionTracker::getFacePoints(float* outArray) {
	outArray[0] = _faceCircle[0];
	outArray[1] = _faceCircle[1];
	outArray[2] = _faceCircle[2];
	outArray[3] = _leftEyeCircle[0];
	outArray[4] = _leftEyeCircle[1];
	outArray[5] = _leftEyeCircle[2];
	outArray[6] = _rightEyeCircle[0];
	outArray[7] = _rightEyeCircle[1];
	outArray[8] = _rightEyeCircle[2];
	outArray[9]  = _leftIrisCircle[0];
	outArray[10] = _leftIrisCircle[1];
	outArray[11] = _leftIrisCircle[2];
	outArray[12] = _rightIrisCircle[0];
	outArray[13] = _rightIrisCircle[1];
	outArray[14] = _rightIrisCircle[2];
	outArray[15] = _faceAngle[0];
	outArray[16] = _faceAngle[1];
	outArray[17] = _faceAngle[2];
}

void SimpleMotionTracker::detectHandCircle(float* handCircle, bool& isHandDetected, bool& isHandDown, int& handUndetectedTick, const cv::Point& point, std::vector<cv::Point>& points, float minHandRadius, float maxHandRadius, std::list<float>& radiusBuf, float resizeRatio, cv::Mat frame) {
	bool isDetected = isHandDetected;
	int currentTickCount = GetTickCount();
	int count = points.size();

	// 最小半径時の円の面積と動体検知量比較する
	// 非検知状態から検知状態への移行は大きく動く必要がある.
	// 一度検知が途切れても一定時間内であれば低い閾値で再検知する
	float thresholdRatio = _minHandTranslationThreshold;
	if (!isDetected) {
		if (currentTickCount > handUndetectedTick + _handUndetectedDuration) {
			thresholdRatio = _maxHandTranslationThreshold;
		}
	}
	thresholdRatio = thresholdRatio * resizeRatio * resizeRatio;
	int translationThrashold = (minHandRadius * minHandRadius * 3.14f) * thresholdRatio;
	if (count > translationThrashold) {
		isDetected = true;
	}
	else {
		isDetected = false;

		//半径バッファをリセットして先頭の値で埋めなおす
		float radius = radiusBuf.front();
		radiusBuf.clear();
		for (int i = 0; i < HAND_RADIUS_BUF_SIZE; i++) { radiusBuf.push_back(radius); };
	}

	// 半径の計算
	if (isDetected) {
		float radius = 0;
		int rx = 0;
		int ry = 0;
		// Z値として使うために半径を計算
		// 平均位置から各動体検知座標へのX,Y成分ごとの平均値も計算

		for (int i = 0; i < count; i++) {
			auto x = points[i].x - point.x;
			auto y = points[i].y - point.y;
			radius += std::sqrtf((x * x) + (y * y));
			rx += std::abs(x);
			ry += std::abs(y);
		}
		radius /= count;
		rx /= count;
		ry /= count;

		// X,Y成分の比率が1から離れると、円形ではなく長方形の動体が検知されたことになる.
		// 半径をそのまま使うと長い辺の影響を受けるので、短い辺に合わせて補正する.
		if (rx > 0 && ry > 0) {
			float ratio = (float)rx / ry;
			if (ratio > 1) { ratio = 1.0f / ratio; }
			radius *= ratio;
		}

		// 半径を遅延採用する
		// 動体検知では検出できなくなる直前は対象を小さく判定してしまうため.
		radiusBuf.push_back(radius);
		radius = radiusBuf.front();
		radiusBuf.pop_front();

		if (radius < minHandRadius) {
			radius = minHandRadius;
		}
		else if (radius > maxHandRadius) {
			radius = maxHandRadius;
		}

		handCircle[0] = point.x;
		handCircle[1] = point.y;
		handCircle[2] = radius;
		handUndetectedTick = currentTickCount;

		isHandDown = false;

		cv::circle(_outputFrame, point, radius, cv::Scalar(0, 255, 0), 2);

		//{
		//	static float radiusAve = 0;
		//	radiusAve = radiusAve * 0.9 + radius * 0.1;
		//	printf("%f\n", radiusAve / minHandRadius);
		//	//printf("%f\n", radius);
		//}
	}
	isHandDetected = isDetected;
}

void SimpleMotionTracker::detectHand() {
	cv::Mat handFrame = _capFrame.clone();
	cv::Mat handFrameGray;
	cv::cvtColor(handFrame, handFrameGray, cv::COLOR_BGR2GRAY);
	cv::equalizeHist(handFrameGray, handFrameGray);

	auto faceCenter = cv::Point(0, 0);
	auto faceRadius = 0;
	if (_useFaceTracking) {
		faceCenter.x = _faceCircle[0];
		faceCenter.y = _faceCircle[1];
		faceRadius = _faceCircle[2] * 1.5f;
	}
	else {
		std::vector<cv::Rect> faces; // 複数検出を考慮.
		cv::Mat resizedFrame;
		const float resizeRate = 0.2f;
		cv::resize(handFrameGray, resizedFrame, cv::Size(handFrameGray.cols * resizeRate, (handFrameGray.rows * resizeRate)));
		_faceCascade.detectMultiScale(resizedFrame, faces);
		if (faces.size() > 0) {
			auto f = faces[0];
			f.x /= resizeRate;
			f.y /= resizeRate;
			f.width /= resizeRate;
			f.height /= resizeRate;
			faceCenter = cv::Point(f.x + f.width / 2, f.y + f.height / 2);
			faceRadius = (f.width / 2) * 1.5f;
			_faceCircle[0] = faceCenter.x;
			_faceCircle[1] = faceCenter.y;
			_faceCircle[2] = faceRadius;
		}
	}
	// 例外対策
	if (faceRadius == 0) {
		faceRadius = 100;
	}

	int minHandRadius = (faceRadius * 0.25f); // 拳の大きさは顏の半分だが、動体検知平均はさらに半分になるので 1/4を設定
	int maxHandRadius = faceRadius;

	// 顔を見つけられなかった場合は古い情報を拡大して使う
	if (faceRadius == 0) {
		faceCenter.x = _faceCircle[0];
		faceCenter.y = _faceCircle[1];
		faceRadius = _faceCircle[2] * 1.5f;
	}

	int originalFaceRadius = faceRadius;
	int shoulderY = faceCenter.y + (faceRadius * 1.5f); // 肩の位置推定
	int handShutterMaskMargin = faceRadius * 1;

	float resizeRatio = 1.0f;
	cv::Mat handBSMask;
	cv::resize(handFrameGray, handFrameGray, cv::Size(handFrameGray.cols * resizeRatio, handFrameGray.rows * resizeRatio));
	_handBackSub->apply(handFrameGray, handBSMask, 0.999);

	faceCenter.y -= (faceRadius / 2);
	faceRadius *= 1.5f;

	faceCenter.x *= resizeRatio;
	faceCenter.y *= resizeRatio;
	faceRadius *= resizeRatio;

	cv::circle(handBSMask, faceCenter, faceRadius, cv::Scalar(0, 0, 0), -1);
	cv::circle(handBSMask, cv::Point(faceCenter.x, faceCenter.y + faceRadius), faceRadius * 0.5f, cv::Scalar(0, 0, 0), -1);

	// 手前の数フレーム分も塗りつぶす
	static float prevFCx[3] = { 0 };
	static float prevFCy[3] = { 0 };
	static float prevFCr[3] = { 0 };

	for (int i = 0; i < 3; i++) {
		cv::circle(handBSMask, cv::Point(prevFCx[i], prevFCy[i]), prevFCr[i], cv::Scalar(0, 0, 0), -1);
		cv::circle(handBSMask, cv::Point(prevFCx[i], prevFCy[i] + prevFCr[i]), prevFCr[i] * 0.5f, cv::Scalar(0, 0, 0), -1);
	}
	for (int i = 3 - 1; i > 0; i--) {
		prevFCx[i] = prevFCx[i-1];
		prevFCy[i] = prevFCy[i-1];
		prevFCr[i] = prevFCr[i-1];
	}
	prevFCx[0] = faceCenter.x;
	prevFCy[0] = faceCenter.y;
	prevFCr[0] = faceRadius;

	// 頭の移動量が多い場合、胸周辺を塗りつぶして検知されないようにする
	if (true) {
		float vx = prevFCx[0] - prevFCx[2];
		float vy = prevFCy[0] - prevFCy[2];
		float r = ((prevFCr[0] + prevFCr[2] + prevFCr[2]) / 3) / 1.5f; // 半径は平均値を使う
		float len = std::sqrtf((vx * vx) + (vy * vy));
		float ratio = len / r;

		if (ratio > 0.1f) {
			cv::Point center(prevFCx[0], prevFCy[0]);
			int height = handBSMask.rows - center.y;
			int halfWidth = prevFCr[0] + (len * 5);
			cv::Rect bodyRect(center.x - halfWidth, center.y, halfWidth * 2, height);
			cv::rectangle(handBSMask, bodyRect, cv::Scalar(0), -1);
		}
	}

	// 前回の手の位置＋顔の直径より下は塗りつぶす
	if (_isRightHandDetected) {
		cv::Rect rightHandUnderMask;
		rightHandUnderMask.x = 0;
		rightHandUnderMask.y = (_rightHandCircle[1] + handShutterMaskMargin) * resizeRatio;
		if (rightHandUnderMask.y >= handBSMask.rows) { rightHandUnderMask.y = handBSMask.rows - 2; }
		rightHandUnderMask.width = faceCenter.x;
		rightHandUnderMask.height = handBSMask.rows - rightHandUnderMask.y;
		cv::rectangle(handBSMask, rightHandUnderMask, cv::Scalar(0, 0, 0), -1);
		//cv::rectangle(_outputFrame, rightHandUnderMask, cv::Scalar(0, 0, 0), -1); // 位置確認用
	}
	if (_isLeftHandDetected) {
		cv::Rect leftHandUnderMask;
		leftHandUnderMask.x = faceCenter.x;
		leftHandUnderMask.y = (_leftHandCircle[1] + handShutterMaskMargin) * resizeRatio;
		if (leftHandUnderMask.y >= handBSMask.rows) { leftHandUnderMask.y = handBSMask.rows - 2; }
		leftHandUnderMask.width = handBSMask.cols - leftHandUnderMask.x;
		leftHandUnderMask.height = handBSMask.rows - leftHandUnderMask.y;
		cv::rectangle(handBSMask, leftHandUnderMask, cv::Scalar(0, 0, 0), -1);
		//cv::rectangle(_outputFrame, leftHandUnderMask, cv::Scalar(0, 0, 0), -1); // 位置確認用
	}

	// 検出された部分を一回り小さくする
	{
		int erosion_size = 1;
		int erosion_type = 0;
		erosion_type = cv::MORPH_RECT;
		//erosion_type = cv::MORPH_CROSS;
		//erosion_type = cv::MORPH_ELLIPSE;
		cv::Mat element = getStructuringElement(erosion_type,
			cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1),
			cv::Point(erosion_size, erosion_size));
		cv::erode(handBSMask, handBSMask, element);
		// 小さくした後拡大
		//cv::dilate(handBSMask, handBSMask, element);
	}

	// 孤立している小さな検出点(ノイズ)を除去
	{
		int morphSize = 1;
		cv::Mat element = cv::getStructuringElement(0, cv::Size(2 * morphSize + 1, 2 * morphSize + 1), cv::Point(morphSize, morphSize));
		cv::morphologyEx(handBSMask, handBSMask, cv::MORPH_OPEN, element);
		cv::threshold(handBSMask, handBSMask, 0, 255, cv::THRESH_BINARY);
	}

	std::vector<cv::Point> leftPoints;
	std::vector<cv::Point> rightPoints;
	int leftCount = 0;
	int rightCount = 0;
	cv::Point leftPoint, rightPoint;
	for (int y = 0; y < handBSMask.rows; y++) {
		for (int x = 0; x < handBSMask.cols; x++) {
			auto c = handBSMask.at<uchar>(y, x);
			if (c > 0) {
				if (x > faceCenter.x) { // 画面右側（左手）
					leftCount++;
					leftPoint.x += x / resizeRatio;
					leftPoint.y += y / resizeRatio;

					leftPoints.push_back(cv::Point( x / resizeRatio, y / resizeRatio));
				}
				else {
					rightCount++;
					rightPoint.x += x / resizeRatio;
					rightPoint.y += y / resizeRatio;

					rightPoints.push_back(cv::Point(x / resizeRatio, y / resizeRatio));
				}
			}
		}
	}
	// 座標の平均と半径を計算
	if (leftCount > 0) {
		if (leftPoints[0].y + _leftHandCircle[2] > shoulderY) {
			leftPoint.x /= leftCount;
			leftPoint.y /= leftCount;
		}
		else {
			leftPoint.x /= leftCount;
			leftPoint.y = leftPoints[0].y + (originalFaceRadius / 2);
			//leftPoint.y += +_leftHandCircle[2];
		}
		detectHandCircle(_leftHandCircle, _isLeftHandDetected, _isLeftHandDown, _leftHandUndetectedTick, leftPoint, leftPoints, minHandRadius, maxHandRadius, _leftHandRadiusBuf, resizeRatio, handBSMask);
	}
	else {
		_isLeftHandDetected = false;
		// 非検知状態 かつ 画面下部で見失っているときは手を下げたと判定
		if (_leftHandCircle[1] + _leftHandCircle[2] >= handFrame.rows / 15 * 14) {
			_isLeftHandDown = true;
			if (_leftHandUndetectedTick > _handUndetectedDuration) { _leftHandUndetectedTick -= _handUndetectedDuration; }
			//cv::rectangle(_outputFrame, cv::Rect(0, 0, 30, 30), cv::Scalar(0, 0, 255), -1); // 判定確認用
		}
	}
	if (rightCount > 0) {
		if (rightPoints[0].y + _rightHandCircle[2] > shoulderY) {
			rightPoint.x /= rightCount;
			rightPoint.y /= rightCount;
		}
		else {
			rightPoint.x /= rightCount;
			rightPoint.y = rightPoints[0].y + (originalFaceRadius / 2);
			//rightPoint.y += +_rightHandCircle[2];
		}
		detectHandCircle(_rightHandCircle, _isRightHandDetected, _isRightHandDown, _rightHandUndetectedTick, rightPoint, rightPoints, minHandRadius, maxHandRadius, _rightHandRadiusBuf, resizeRatio, handBSMask);
	}
	else {
		_isRightHandDetected = false;
		// 非検知状態 かつ 画面下部で見失っているときは手を下げたと判定
		if (_rightHandCircle[1] + _rightHandCircle[2] >= handFrame.rows / 15 * 14) {
			_isRightHandDown = true;
			if (_rightHandUndetectedTick > _handUndetectedDuration) { _rightHandUndetectedTick -= _handUndetectedDuration; }
		}
	}
	//cv::imshow("Live", handBSMask);
}

bool SimpleMotionTracker::isLeftHandDetected() {
	return _isLeftHandDetected;
}

bool SimpleMotionTracker::isRightHandDetected() {
	return _isRightHandDetected;
}

bool SimpleMotionTracker::isLeftHandDown() {
	return _isLeftHandDown;
}

bool SimpleMotionTracker::isRightHandDown() {
	return _isRightHandDown;
}

void SimpleMotionTracker::getHandPoints(float* outArray) {
	outArray[0] = _leftHandCircle[0];
	outArray[1] = _leftHandCircle[1];
	outArray[2] = _leftHandCircle[2];
	outArray[3] = _rightHandCircle[0];
	outArray[4] = _rightHandCircle[1];
	outArray[5] = _rightHandCircle[2];
}

/*----------------------------------------------------------
                         DLL用API
----------------------------------------------------------*/
static SimpleMotionTracker* instance = nullptr;

void SMT_initRaw(int cameraId, const char* dataPath) {
	if (instance == nullptr) {
		instance = new SimpleMotionTracker();
	}
	instance->init(cameraId, dataPath);
}

void SMT_init(const char* videoDeviceName, const char* dataPath) {
	if (instance ==nullptr) {
		instance = new SimpleMotionTracker();
	}
	std::string name = videoDeviceName;
	instance->init(name, dataPath);
}

void SMT_destroy() {
	if (instance == nullptr) return;
	instance->destroy();
	delete instance;
	instance = nullptr;
}

void SMT_update() {
	if (instance == nullptr) return;
	instance->update();
}

void SMT_setUseARMarker(bool useARMarker) {
	if (instance == nullptr) return;
	instance->setUseARMarker(useARMarker);
}

void SMT_setUseFaceTracking(bool useFaceTracking) {
	if (instance == nullptr) return;
	instance->setUseFaceTracking(useFaceTracking);
}

void SMT_setUseEyeTracking(bool useEyesTracking) {
	if (instance == nullptr) return;
	instance->setUseEyesTracking(useEyesTracking);
}

void SMT_setUseHandTracking(bool useHandTracking) {
	if (instance == nullptr) return;
	instance->setUseHandTracking(useHandTracking);
}

void SMT_setCaptureShown(bool isShown) {
	if (instance == nullptr) return;
	instance->setCaptureShown(isShown);
}

bool SMT_isCaptureShown() {
	if (instance == nullptr) return false;
	return instance->isCaptureShown();
}

void SMT_setARMarkerEdgeLength(float length) {
	if (instance == nullptr) return;
	instance->setARMarkerEdgeLength(length);
}

bool SMT_isARMarkerDetected(int id) {
	if (instance == nullptr) return false;
	return instance->isARMarkerDetected(id);
}

void SMT_getARMarker6DoF(int id, float* outArray) {
	if (instance == nullptr) return;
	instance->getARMarker6DoF(id, outArray);
}

bool SMT_isFacePointsDetected() {
	if (instance == nullptr) return false;
	return instance->isFacePointsDetected();
}

void SMT_getFacePoints(float* outArray) {
	if (instance == nullptr) return;
	instance->getFacePoints(outArray);
}

void SMT_setIrisThresh(int thresh) {
	if (instance == nullptr) return;
	instance->setIrisThresh(thresh);
}

void SMT_setMinHandTranslationThreshold(float thresh) {
	if (instance == nullptr) return;
	instance->setMinHandTranslationThreshold(thresh);
}

void SMT_setMaxHandTranslationThreshold(float thresh) {
	if (instance == nullptr) return;
	instance->setMaxHandTranslationThreshold(thresh);
}

void SMT_setHandUndetectedDuration(int msec) {
	if (instance == nullptr) return;
	instance->setHandUndetectedDuration(msec);
}

bool SMT_isLeftHandDetected() {
	if (instance == nullptr) return false;
	return instance->isLeftHandDetected();
}

bool SMT_isRightHandDetected() {
	if (instance == nullptr) return false;
	return instance->isRightHandDetected();
}

bool SMT_isLeftHandDown() {
	if (instance == nullptr) return false;
	return instance->isLeftHandDown();
}

bool SMT_isRightHandDown() {
	if (instance == nullptr) return false;
	return instance->isRightHandDown();
}

void SMT_getHandPoints(float* outArray) {
	if (instance == nullptr) return;
	instance->getHandPoints(outArray);
}

void SMT_cvWait() {
	cv::waitKey(1);
}

int SMT_getErrorCode() {
	if (instance == nullptr) return SMT_ERROR_NOEN;
	return instance->getErrorCode();
}


#include "parser_impl.hpp"
#include <boost/spirit/include/qi_uint.hpp>

#define BOOST_SPIRIT_NO_PREDEFINED_TERMINALS
// #define BOOST_SPIRIT_QI_DEBUG

// shorthand

namespace qi = boost::spirit::qi;

// integer grammar implementation

formast::Parser::Impl::integer_grammar::integer_grammar() : integer_grammar::base_type(uint_or_version)
{

    qi::uint_parser<boost::uint64_t, 16> hex_parser;
    qi::lit_type lit;
    qi::uint_type ulong_long;
    qi::_val_type _val;
    qi::_1_type _1;
    qi::_2_type _2;
    qi::_3_type _3;
    qi::_4_type _4;

    uint_or_version =
        (hex              [_val = _1])
        |   (lit("2.3")   [_val = 0x02030000])
        |   (lit("3.03")  [_val = 0x03000300]) // special case
        |   (lit("3.0")   [_val = 0x03000000])
        |   (lit("3.1")   [_val = 0x03010000])
        |   ((ulong_long                [_val = _1])
             >> -('.' >> (ulong_long    [_val = _val * 256 + _1])
                  >> '.' > (ulong_long  [_val = _val * 256 + _1])
                  > '.' > (ulong_long   [_val = _val * 256 + _1])))
        ;

    hex %= lit("0x") > hex_parser;

    // Debugging and error handling and reporting support.
    BOOST_SPIRIT_DEBUG_NODE(uint_or_version);
    BOOST_SPIRIT_DEBUG_NODE(hex);

    // Error handling
    qi::on_error<qi::fail>(uint_or_version, error_handler(_4, _3, _2));
}


#include <boost\crc.hpp>
#include <string>
#include <iostream>

int main()
{
    std::string str( "The quick brown fox jumps over the lazy dog" );
    boost::crc_32_type  crc;
    crc.process_bytes( str.data(), str.size() );

    std::cout << "Checksum: " << std::hex << crc.checksum() << std::endl;
    return 0;
}


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/list.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>

#include <rstbx/symmetry/constraints/a_g_conversion.h>

#include <vector>
#include <map>

using namespace boost::python;
namespace rstbx{
namespace boost_python { namespace {

  void
  symmetry_constraints_init_module() {
    using namespace boost::python;

    typedef return_value_policy<return_by_value> rbv;
    typedef default_call_policies dcp;

    class_<rstbx::symmetry::AG>("AGconvert", init<>())
      .def("forward", &rstbx::symmetry::AG::forward)
      .def("setAngles", &rstbx::symmetry::AG::setAngles,
           (arg_("phi"),arg_("psi"),arg_("theta"))
          )
      .def("validate_and_setG", &rstbx::symmetry::AG::validate_and_setG)
      .def("back_as_orientation", &rstbx::symmetry::AG::back_as_orientation)
      .def("back", &rstbx::symmetry::AG::back)
      .add_property("G",make_getter(&rstbx::symmetry::AG::G, rbv()),
                        make_setter(&rstbx::symmetry::AG::G, dcp())
          )
      .add_property("phi",make_getter(&rstbx::symmetry::AG::phi, rbv()))
      .add_property("psi",make_getter(&rstbx::symmetry::AG::psi, rbv()))
      .add_property("theta",make_getter(&rstbx::symmetry::AG::theta, rbv()))
    ;

  }

}
}} // namespace rstbx::boost_python::<anonymous>

BOOST_PYTHON_MODULE(rstbx_symmetry_constraints_ext)
{
  rstbx::boost_python::symmetry_constraints_init_module();

}


//Tencent is pleased to support the open source community by making FeatherCNN available.

//Copyright (C) 2019 THL A29 Limited, a Tencent company. All rights reserved.

//Licensed under the BSD 3-Clause License (the "License"); you may not use this file except
//in compliance with the License. You may obtain a copy of the License at
//
//https://opensource.org/licenses/BSD-3-Clause
//
//Unless required by applicable law or agreed to in writing, software distributed
//under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
//CONDITIONS OF ANY KIND, either express or implied. See the License for the
//specific language governing permissions and limitations under the License.

#include <booster/sgemv.h>
#include <booster/helper.h>

#include <assert.h>
#include <immintrin.h>
#include <string.h>
#if 0
void fully_connected_inference_direct(const int input_size, const int output_size, const float *x, const float *y, float *z, const int num_threads)
{
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int i = 0; i < output_size; i++)
    {
        float sum = 0.f;
        for (int j = 0; j < input_size; j++)
            sum += x[j] * y[i * input_size + j];
        z[i] = sum;
    }
}

void fully_connected_transpose_inference_sse8(const int input_size, const int output_size, const float *x, const float *y, float *z, const int num_threads)
{
    assert(input_size % 8 == 0);
    assert(output_size % 8 == 0);
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int k = 0; k < output_size / 8; k++)
    {
        const float *yPtr = y + k * 8 * input_size;
        __m128 res = {0.0, 0.0, 0.0, 0.0};
        __m128 res1 = {0.0, 0.0, 0.0, 0.0};
        __m128 va0, va1, va2, va3, vb0, vb1, vb2, vb3, vb4, vb5, vb6, vb7;
        for (int i = 0; i < input_size; i += 4)
        {
            //          __m128 v1, v2;
            //va = _mm_load_ps(x + i);

            vb0 = _mm_load_ps(yPtr);
            vb1 = _mm_load_ps(yPtr + 4);
            vb2 = _mm_load_ps(yPtr + 8);
            vb3 = _mm_load_ps(yPtr + 12);
            vb4 = _mm_load_ps(yPtr + 16);
            vb5 = _mm_load_ps(yPtr + 20);
            vb6 = _mm_load_ps(yPtr + 24);
            vb7 = _mm_load_ps(yPtr + 28);

            va0 = _mm_broadcast_ss(x + i);
            va1 = _mm_broadcast_ss(x + i + 1);
            va2 = _mm_broadcast_ss(x + i + 2);
            va3 = _mm_broadcast_ss(x + i + 3);

            res = _mm_fmadd_ps(vb0, va0, res);
            res1 = _mm_fmadd_ps(vb1, va0, res1);
            res = _mm_fmadd_ps(vb2, va1, res);
            res1 = _mm_fmadd_ps(vb3, va1, res1);
            res = _mm_fmadd_ps(vb4, va2, res);
            res1 = _mm_fmadd_ps(vb5, va2, res1);
            res = _mm_fmadd_ps(vb6, va3, res);
            res1 = _mm_fmadd_ps(vb7, va3, res1);

            yPtr += 32;
        }
        _mm_store_ps((z + 8 * k), res);
        _mm_store_ps((z + 8 * k + 4), res1);
    }
}

#include <stdio.h>
#include <stdlib.h>

//For fully connected layers, the weights matrix is transposed.
//To reduce memory allocation, a KxSTRIDE packed matrix is sufficient.
template<int STRIDE>
void packed_sgemv_transposed_init(const int N, const int K, float* matrix, float* packed_buffer)
{
    size_t N_aligned = N - N % STRIDE;
    for (int j = 0; j < N_aligned; j += STRIDE)
    {
        float* pMatrix = matrix + j * K;
        float* pPacked = packed_buffer;
        for (int k = 0; k < K; ++k)
        {
            for (int i = 0; i < STRIDE; ++i)
                pPacked[i] = pMatrix[i * K + k];
            pPacked += STRIDE;
        }
        memcpy(pMatrix, packed_buffer, STRIDE * K * sizeof(float));
    }
    int rem = N % STRIDE;
    if (rem > 0)
    {
        float* pMatrix = matrix + N_aligned * K;
        float* pPacked = packed_buffer;
        for (int k = 0; k < K; ++k)
        {
            for (int i = 0; i < rem; ++i)
                pPacked[i] = pMatrix[i * K + k];
            pPacked += rem;
        }
        memcpy(pMatrix, packed_buffer, rem * K * sizeof(float));
    }
}
template void packed_sgemv_transposed_init<8>(const int N, const int K, float* matrix, float* packed_matrix);
template void packed_sgemv_transposed_init<16>(const int N, const int K, float* matrix, float* packed_matrix);

//The packed matrix will only be scanned once. Cache miss would only occur with very large K, which is not a very common case.

template<bool fuseBias, bool fuseRelu>
void packed_sgemv(const int N, const int K, const float* A, const float* B, float* C, const float* bias_data, const int num_threads)
{
    assert(K % 4 == 0);
    int N_aligned = N - N % 16;
    __m256 vZero = _mm256_set1_ps(0.f);
    #pragma omp parallel for
    for (int j = 0; j < N_aligned; j += 16)
    {
        const float *pB = B + j * K;
        float *pC = C + j;
        __m256 acc0 = _mm256_set1_ps(0.f);
        __m256 acc1 = _mm256_set1_ps(0.f);
        if (fuseBias)
        {
            acc0 = _mm256_load_ps(bias_data + j);
            acc1 = _mm256_load_ps(bias_data + j + 8);
        }
        __m256 va0, va1, va2, va3;
        __m256 vb0, vb1, vb2, vb3, vb4, vb5, vb6, vb7;
        for (int k = 0; k < K; k += 4)
        {
            vb0 = _mm256_load_ps(pB);
            vb1 = _mm256_load_ps(pB + 8);
            vb2 = _mm256_load_ps(pB + 16);
            vb3 = _mm256_load_ps(pB + 24);
            vb4 = _mm256_load_ps(pB + 32);
            vb5 = _mm256_load_ps(pB + 40);
            vb6 = _mm256_load_ps(pB + 48);
            vb7 = _mm256_load_ps(pB + 56);

            //print_floats(pB, 8);
            va0 = _mm256_broadcast_ss(A + k);
            va1 = _mm256_broadcast_ss(A + k + 1);
            va2 = _mm256_broadcast_ss(A + k + 2);
            va3 = _mm256_broadcast_ss(A + k + 3);

            acc0 = _mm256_fmadd_ps(vb0, va0, acc0);
            acc1 = _mm256_fmadd_ps(vb1, va0, acc1);
            acc0 = _mm256_fmadd_ps(vb2, va1, acc0);
            acc1 = _mm256_fmadd_ps(vb3, va1, acc1);
            acc0 = _mm256_fmadd_ps(vb4, va2, acc0);
            acc1 = _mm256_fmadd_ps(vb5, va2, acc1);
            acc0 = _mm256_fmadd_ps(vb6, va3, acc0);
            acc1 = _mm256_fmadd_ps(vb7, va3, acc1);
            pB += 64;
        }
        if (fuseRelu)
        {
            acc0 = _mm256_max_ps(vZero, acc0);
            acc1 = _mm256_max_ps(vZero, acc1);
        }
        _mm256_store_ps(pC, acc0);
        _mm256_store_ps(pC + 8, acc1);
    }

    int rem = N % 16;
    if (rem == 8)
    {
        const float *pB = B + N_aligned * K;
        __m256 vacc = _mm256_set1_ps(0.f);
        if (fuseBias)
            vacc = _mm256_load_ps(bias_data + N_aligned);
        __m256 va, vb;
        float acc[4];
        for (int k = 0; k < K; ++k)
        {
            vb  = _mm256_loadu_ps(pB);
            va  = _mm256_broadcast_ss(A + k);
            vacc = _mm256_fmadd_ps(vb, va, vacc);
            pB += rem;
        }
        if (fuseRelu)
            vacc = _mm256_max_ps(vZero, vacc);
        _mm256_store_ps(C + N_aligned, vacc);
    }
    else if (rem > 0)
    {
        const float *pB = B + N_aligned * K;
        float *pC = C + N_aligned;
        __m256 vacc = _mm256_set1_ps(0.f);
        if (fuseBias && rem >= 8)
            vacc = _mm256_load_ps(bias_data + N_aligned);

        __m256 va, vb;
        float acc[4];
        for (int i = 0; i < rem - 8; ++i)
        {
            if (fuseBias)
                acc[i] = bias_data[N_aligned + i];
            else
                acc[i] = 0;
        }
        for (int k = 0; k < K; ++k)
        {
            if (rem >= 8)
            {
                vb  = _mm256_loadu_ps(pB);
                va  = _mm256_broadcast_ss(A + k);
                vacc = _mm256_fmadd_ps(vb, va, vacc);
            }
            if (rem - 8 > 0)
            {
                for (int i = 0; i < rem - 8; ++i)
                    acc[i] = A[k] * pB[i + 8];
            }
            pB += rem;
        }
        if (fuseRelu)
        {
            vacc = _mm256_max_ps(vZero, vacc);
        }
        _mm256_store_ps(pC, vacc);
        for (int i = 0; i < rem - 8; ++i)
        {
            if (fuseRelu)
                pC[i + 8] = ((acc[i] > 0.f) ? acc[i] : 0.f);
            else
                pC[i + 8] = acc[i];
        }
    }
}

template void packed_sgemv<false, false>(const int N, const int K, const float* A, const float* B, float* C, const float* bias_data, const int num_threads);
template void packed_sgemv<true, false>(const int N, const int K, const float* A, const float* B, float* C, const float* bias_data, const int num_threads);
template void packed_sgemv<false, true>(const int N, const int K, const float* A, const float* B, float* C, const float* bias_data, const int num_threads);
template void packed_sgemv<true, true>(const int N, const int K, const float* A, const float* B, float* C, const float* bias_data, const int num_threads);

void fully_connected_inference_direct_BiasReLU(int input_size, int output_size, float *x, float *y, float *z, float* biasArr, int num_threads)
{
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int i = 0; i < output_size; i++)
    {
        float sum = 0.f;
        for (int j = 0; j < input_size; j++)
            sum += x[j] * y[i * input_size + j];

        sum += biasArr[i];
        if (sum < 0.f) sum = 0.f;
        z[i] = sum;
    }
}

void fully_connected_transpose_inference_sse8_BiasReLU(int input_size, int output_size, float *x, float *y, float *z, float* biasArr, int num_threads)
{
    assert(input_size % 8 == 0);
    assert(output_size % 8 == 0);
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int k = 0; k < output_size / 8; k++)
    {
        float *yPtr = y + k * 8 * input_size;
        const __m128 vzero = _mm_set1_ps(0.f);

        __m128 res  = _mm_load_ps(biasArr + k * 8);
        __m128 res1 = _mm_load_ps(biasArr + k * 8 + 4);

        __m128 va0, va1, va2, va3, vb0, vb1, vb2, vb3, vb4, vb5, vb6, vb7;
        for (int i = 0; i < input_size; i += 4)
        {
            vb0 = _mm_load_ps(yPtr);
            vb1 = _mm_load_ps(yPtr + 4);
            vb2 = _mm_load_ps(yPtr + 8);
            vb3 = _mm_load_ps(yPtr + 12);
            vb4 = _mm_load_ps(yPtr + 16);
            vb5 = _mm_load_ps(yPtr + 20);
            vb6 = _mm_load_ps(yPtr + 24);
            vb7 = _mm_load_ps(yPtr + 28);

            va0 = _mm_broadcast_ss(x + i);
            va1 = _mm_broadcast_ss(x + i + 1);
            va2 = _mm_broadcast_ss(x + i + 2);
            va3 = _mm_broadcast_ss(x + i + 3);

            res = _mm_fmadd_ps(vb0, va0, res);
            res1 = _mm_fmadd_ps(vb1, va0, res1);
            res = _mm_fmadd_ps(vb2, va1, res);
            res1 = _mm_fmadd_ps(vb3, va1, res1);
            res = _mm_fmadd_ps(vb4, va2, res);
            res1 = _mm_fmadd_ps(vb5, va2, res1);
            res = _mm_fmadd_ps(vb6, va3, res);
            res1 = _mm_fmadd_ps(vb7, va3, res1);

            yPtr += 32;
        }

        res  = _mm_max_ps(res, vzero);
        res1 = _mm_max_ps(res1, vzero);

        _mm_store_ps((z + 8 * k), res);
        _mm_store_ps((z + 8 * k + 4), res1);
    }
}

#else

template <bool fuseBias, bool fuseRelu>
void fully_connected_inference_direct(const int input_size, const int output_size, const float *x, const float *y, float *z, const int num_threads, float *bias_arr)
{
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int i = 0; i < output_size; i++)
    {
        float sum = 0;
        for (int j = 0; j < input_size; j++)
            sum += x[j] * y[i * input_size + j];
        if (fuseBias)
            sum += bias_arr[i];
        if (fuseRelu)
            sum = (sum > 0.f) ? sum : 0.f;
        z[i] = sum;
    }
}

template <bool fuseBias, bool fuseRelu>
void fully_connected_transpose_inference(const int input_size, const int output_size, const float *x, const float *y, float *z, const int num_threads, float *bias_arr)
{
    assert(input_size % 8 == 0);
    assert(output_size % 8 == 0);
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int k = 0; k < output_size / 8; k++)
    {
        __m128 vBias, vBias1;
        const __m128 vZero = _mm_set1_ps(0.f);
        __m128 res = _mm_set1_ps(0.f);
        __m128 res1 = _mm_set1_ps(0.f);

        if (fuseBias)
        {
            vBias = _mm_load_ps(bias_arr + k * 8);
            vBias1 = _mm_load_ps(bias_arr + k * 8 + 4);
        }
        const float *yPtr = y + k * 8 * input_size;
        __m128 va0, va1, va2, va3, vb0, vb1, vb2, vb3, vb4, vb5, vb6, vb7;
        for (int i = 0; i < input_size; i += 4)
        {
            vb0 = _mm_load_ps(yPtr);
            vb1 = _mm_load_ps(yPtr + 4);
            vb2 = _mm_load_ps(yPtr + 8);
            vb3 = _mm_load_ps(yPtr + 12);
            vb4 = _mm_load_ps(yPtr + 16);
            vb5 = _mm_load_ps(yPtr + 20);
            vb6 = _mm_load_ps(yPtr + 24);
            vb7 = _mm_load_ps(yPtr + 28);

            va0 = _mm_broadcast_ss(x + i);
            va1 = _mm_broadcast_ss(x + i + 1);
            va2 = _mm_broadcast_ss(x + i + 2);
            va3 = _mm_broadcast_ss(x + i + 3);

            res = _mm_fmadd_ps(vb0, va0, res);
            res1 = _mm_fmadd_ps(vb1, va0, res1);
            res = _mm_fmadd_ps(vb2, va1, res);
            res1 = _mm_fmadd_ps(vb3, va1, res1);
            res = _mm_fmadd_ps(vb4, va2, res);
            res1 = _mm_fmadd_ps(vb5, va2, res1);
            res = _mm_fmadd_ps(vb6, va3, res);
            res1 = _mm_fmadd_ps(vb7, va3, res1);

            yPtr += 32;
        }

        if (fuseBias)
        {
            res  = _mm_add_ps(res, vBias);
            res1 = _mm_add_ps(res1, vBias1);
        }
        if (fuseRelu)
        {
            res = _mm_max_ps(res, vZero);
            res1 = _mm_max_ps(res1, vZero);
        }
        _mm_store_ps((z + 8 * k), res);
        _mm_store_ps((z + 8 * k + 4), res1);
    }
}

template void fully_connected_inference_direct<false, false>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_inference_direct<false, true>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_inference_direct<true, false>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_inference_direct<true, true>(const int, const int, const float *, const float *, float *, const int, float *);

template void fully_connected_transpose_inference<false, false>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_transpose_inference<false, true>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_transpose_inference<true, false>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_transpose_inference<true, true>(const int, const int, const float *, const float *, float *, const int, float *);

#endif

void matrixTranspose(float *array, size_t m, size_t n, float *buffer) //  A[m][n] -> A[n][m]
{
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            buffer[j * m + i] = array[i * n + j];
    memcpy(array, buffer, m * n * sizeof(float));
}


//
//  main.cpp
//  PhyloAcc
//
//  Created by hzr on 3/8/16.
//  Copyright © 2016 hzr. All rights reserved.
//
#include <dirent.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <algorithm>
#include <iomanip>
#include <omp.h>
#include <string>
#include <armadillo>
#include "profile.h"
#include "newick.h"
#include "bpp.hpp"
#include "bpp_c.hpp"
#include "utils.h"
#include <math.h>
#include <gsl/gsl_errno.h>

using namespace std;
using namespace arma;

// parameters, input and output files paths
string params_path;
string phytree_path;
string align_path;
string output_path="";
string output_path2="";
string segment_path;
string id_path="";
string result_prefix="test";

//string refspecies;
string outgroup;
string targetspecies;
string conservegroup; // can't be missing in more than 50%
double conserve_prop = 0.8;

int num_thread = 1;

// running parameters
int num_burn = 200;         // num of burn-in updates
int num_mcmc = 800;         // num of MCMC updates,10
int num_thin = 500;    // num of updates between two samples, adaptive frequency
int num_chain; // outer loop of updates Q matrix and hyperparameter of substitution rates

double prep_lrate = 0.3;
double prep_lrate2 = 0.0;//0.1
double prep_grate = 0.5; // initalization

double prior_lrate2_a = 0,prior_lrate2_b = 1 ; // beta prior for lrate2, 0.5
double prior_lrate_a = 1 ,prior_lrate_b = 9 ; // beta prior for lrate, 2
double prior_grate_a = 3,prior_grate_b = 1 ; // beta prior for grate

double ratio0 = 0.5; //initial conserved rate
double ratio1 = 1; // initial accelerated rate
double missing_thres = 0.8;

double nprior_a = 10, nprior_b = 0.2;  //around 1
double cprior_a = 5, cprior_b = 0.04;  //around ratio
int ropt = 1;
double cub = 1;
double nlb = 0.6;

int batch = -1 ;
int seed = 5;
double indel = 0;
double indel2;
bool sample_indel = 0;
bool sample_hyper;
char gapchar = '-';
bool verbose = 0;
double consToMis = 0.01;
bool prune=0;
double revgap=1;
int min_length = 50;

// load the program parameters
void LoadParams(int argc, char* argv[])
{
    cout << "Loading input data and running parameters......" << endl;

    if (argc > 1)
        params_path = string(argv[1]);
    else
        params_path = "/Users/hzr/Phd_2/Bird/PhyloAcc_init3-3/params2s.txt"; //params2

    cout << "Loading program configurations from " << params_path << "......" <<endl;

    const int BUFF_SIZE = 1024;
    char line_buff[BUFF_SIZE];

    ifstream in_params(params_path.c_str());
    if (!in_params)
    {
        cerr << "Cannot open the parameters file: " << params_path.c_str() << endl;
        exit(1);
    }
    while(!in_params.eof())
    {
        in_params.getline(line_buff, BUFF_SIZE);
        istringstream line_stream(line_buff);
        string tmp; line_stream >> tmp;

        // input and output file paths
        if (tmp=="PHYTREE_FILE")
            line_stream >> phytree_path;
        else if (tmp=="ALIGN_FILE")
            line_stream >> align_path;
        else if (tmp=="SEG_FILE")
            line_stream >> segment_path;
        else if (tmp=="ID_FILE")
            line_stream >> id_path;
        else if (tmp=="BATCH")
            line_stream >> batch;
        else if (tmp=="RESULT_FOLDER")
            line_stream >> output_path;
        else if (tmp=="PREFIX")
            line_stream >> result_prefix;

        //else if (tmp=="RESULT_INDIV")
        //    line_stream >> output_path2;

        else if (tmp=="SEED")
            line_stream >> seed;
        else if (tmp=="INIT_CONSERVE_RATE")
            line_stream >> ratio0;
        else if (tmp=="INIT_ACCE_RATE")
            line_stream >> ratio1;
        else if (tmp=="CONSERVE_PRIOR_A")
            line_stream >> cprior_a;
        else if (tmp=="CONSERVE_PRIOR_B")
            line_stream >> cprior_b;
        else if (tmp=="ACCE_PRIOR_A")
            line_stream >> nprior_a;
        else if (tmp=="ACCE_PRIOR_B")
            line_stream >> nprior_b;
        else if (tmp=="ROPT")
            line_stream >> ropt;
        else if (tmp=="CUB")
            line_stream >> cub;
        else if (tmp=="NLB")
            line_stream >> nlb;

        // running parameters
        else if (tmp=="BURNIN")
            line_stream >> num_burn;
        else if (tmp=="MCMC")
            line_stream >> num_mcmc;
        else if (tmp=="ADAPT_FREQ")
            line_stream >> num_thin;
        else if (tmp=="INIT_LRATE")
            line_stream >> prep_lrate;
        else if (tmp=="INIT_GRATE")
            line_stream >> prep_grate;
        else if (tmp=="HYPER_LRATE_A")
            line_stream >> prior_lrate_a;
        else if (tmp=="HYPER_LRATE_B")
            line_stream >> prior_lrate_b;
        else if (tmp=="HYPER_GRATE_A")
            line_stream >> prior_grate_a;
        else if (tmp=="HYPER_GRATE_B")
            line_stream >> prior_grate_b;
        else if (tmp=="CHAIN")
            line_stream >> num_chain;

        // constraint
        else if (tmp == "OUTGROUP")
            line_stream >> outgroup;
        else if (tmp == "TARGETSPECIES")
            line_stream >> targetspecies;
        else if (tmp == "CONSERVE")
            line_stream >> conservegroup;
        else if (tmp == "CONSERVE_PROP")
            line_stream >> conserve_prop;
        else if (tmp == "GAP_PROP")
            line_stream >> missing_thres;
        //else if (tmp == "REF")
         //   line_stream >> refspecies;
        else if (tmp == "CONSTOMIS")
            line_stream >> consToMis;

        // treat indel as additional character
        else if (tmp == "GAPCHAR")
            line_stream >> gapchar;
        else if (tmp == "PRUNE_TREE")
            line_stream >> prune;
        else if (tmp == "TRIM_GAP_PERCENT")
            line_stream >> revgap;
        else if (tmp == "MIN_LEN")
            line_stream >> min_length;
        else if (tmp == "INDEL") // not used
            line_stream >> indel;
        else if (tmp == "INDEL2") // not used
            line_stream >> indel2;
        else if(tmp == "SAMPLE_INDEL")  // not used
            line_stream >> sample_indel;
        else if(tmp == "SAMPLE_HYPER")
            line_stream >> sample_hyper;
        else if(tmp == "VERBOSE")
            line_stream >> verbose;
        else if(tmp == "NUM_THREAD")
            line_stream >> num_thread;
        else if(tmp != "")
            cout << "Unknown parameter: " << tmp <<endl;

    }

    // trimming file names
    phytree_path = strutils::trim(phytree_path, " \"\t\n");
    align_path = strutils::trim(align_path, " \"\t\n");
    output_path  = strutils::trim(output_path,  " \"\t\n");
    //output_path2  = strutils::trim(output_path2,  " \"\t\n");
    segment_path = strutils::trim(segment_path, " \"\t\n");

}

bool DirectoryExists( string pzPath )
{
    if ( pzPath == "") return false;

    DIR *pDir;
    bool bExists = false;

    pDir = opendir (pzPath.c_str());

    if (pDir != NULL)
    {
        bExists = true;
        (void) closedir (pDir);
    }

    return bExists;
}

void DispParams(PhyloProf profile, int seed)
{
    double mean_seg_size = 0;
    for(unsigned int c=0; c<profile.C; c++)
        mean_seg_size += (double)(profile.element_pos[c][1] - profile.element_pos[c][0]) / profile.C;
    cout << "  # total length = " << profile.G << " (" << profile.C << ")" << ". # Species = " << profile.S << ". # elements = " << profile.C << ". Mean gene set size = " << mean_seg_size << "." << endl;
    cout << "# Burn-ins = " << num_burn << ". # MCMC Updates = " << num_mcmc << ". # adaptive frequency = " << num_thin << ".  RND SEED = " << seed << "." << endl ; //
    cout << "# Threads = " << num_thread << endl << endl;
}

int main(int argc, char* argv[])
{
    time_t start = time(NULL);

    cout << std::fixed << setprecision(4);

    // load the program parameters
    LoadParams(argc, argv);

    // check output path
    if(! DirectoryExists(output_path))
    {
    	cout << "output path doesn't exist or empty!" << endl;
    	return 1;
    }

    // load the phylogenetic profile
    PhyloProf profile = LoadPhyloProfiles(align_path,segment_path);

    // load the phylogenetic tree
    PhyloTree phytree = LoadPhyloTree(phytree_path);

    // init and display the running parameters
    //InitParams(profile.G);
    DispParams(profile, seed);

    // create and init the BPP object
    //int pC = 500;  // only read in some elements for testing
    BPP bpp(0, profile, phytree, output_path, targetspecies, outgroup, conserve_prop, conservegroup, ratio0, ratio1, ropt, cub, nlb, nprior_a, nprior_b, cprior_a, cprior_b, seed, prep_grate, prep_lrate, prep_lrate2, prior_grate_a, prior_grate_b,prior_lrate_a, prior_lrate_b,prior_lrate2_a, prior_lrate2_b,  indel, indel2, missing_thres, sample_indel);  //c=1 test run first element

    // remove profile?
    //profile.~PhyloProf();

    //initialize the MCMC sampling
    bpp.InitMCMC(num_burn, num_mcmc, num_thin);

    output_path = output_path + "/" + result_prefix ;
    output_path2 = output_path;
    string outpath_Z0 = output_path + "_rate_postZ_M" +to_string(0) +".txt";  //null
    string outpath_Z1 = output_path + "_rate_postZ_M" +to_string(2) +".txt";  //full
    string outpath_Z2 = output_path + "_rate_postZ_M" +to_string(1) +".txt";  // M1

    string outpath_hyper = output_path+"_hyper.txt";
    ofstream out_hyper(outpath_hyper.c_str());
    out_hyper << "iter\tnprior_a\tnprior_b\tcprior_a\tcprior_b\tprior_l_a\tprior_l_b\tprior_g_a\tprior_g_b\n";
    out_hyper << 0 << "\t"<< nprior_a<< "\t"<< nprior_b <<"\t"<< cprior_a << "\t"<< cprior_b << "\t"<< prior_lrate_a << "\t"<< prior_lrate_b << "\t"<< prior_grate_a << "\t"<< prior_grate_b <<endl;

    ofstream out_lik;
    if(sample_hyper) {
        string outpath_elem = output_path+ "_elem_lik.txt";
        out_lik.open(outpath_elem.c_str());
        out_lik.precision(8);
        out_lik << "No.\tID\tloglik_all\tloglik_Max"<<endl;
    }

    ofstream out_Z0(outpath_Z0.c_str());
    ofstream out_Z1(outpath_Z1.c_str());
    ofstream out_Z2(outpath_Z2.c_str());

    // output species name
    string species_name = output_path+"_species_names.txt";
    ofstream out_species(species_name.c_str());

    out_Z0 << "No.\tn_rate\tc_rate\tg_rate\tl_rate\tl2_rate"; out_Z1 << "No.\tn_rate\tc_rate\tg_rate\tl_rate\tl2_rate"; out_Z2 << "No.\tn_rate\tc_rate\tg_rate\tl_rate\tl2_rate";
    for(int s=0; s<bpp.N;s++){
         for(int k=0;k<4;k++){
            out_Z0 <<"\t"<<bpp.nodes_names[s]<<"_"<<k;
            out_Z1 <<"\t"<<bpp.nodes_names[s]<<"_"<<k;
            out_Z2 <<"\t"<<bpp.nodes_names[s]<<"_"<<k;
         }
         out_species << bpp.nodes_names[s] << endl;
    }
	out_Z0 <<endl; out_Z1 <<endl; out_Z2 <<endl;

    out_species.close();

    double lrate_prop = 0.5, grate_prop = 0.5;

    vector<int> ids;
    if(id_path=="")
    {
        if(batch==-1)
        {
          for(int c =0;c<bpp.C;c++)
          {
            ids.push_back(c);
          }
        }else{
          int temp = ceil(bpp.C/3);
          for(int c =batch*temp ;c< (batch+1)*temp;c++)
          {
            if(c >= bpp.C) break;
            ids.push_back(c);
          }

        }

    }else{
        ifstream in_params(id_path.c_str());
        if (!in_params)
        {
            cerr << "Cannot open the id file: " << id_path.c_str() << endl;
            exit(1);
        }
        while(!in_params.eof())
        {
            const int BUFF_SIZE = 1024;
            char line_buff[BUFF_SIZE];
            in_params.getline(line_buff, BUFF_SIZE);
            istringstream line_stream(line_buff);
            string tmp; line_stream >> tmp;
            tmp = strutils::trim(tmp);
            if(tmp=="") continue;
            ids.push_back(atoi(tmp.c_str()));

        }

    }

    cout << ids.size() << " of elements to be computed" << endl;
    //gsl_set_error_handler_off();
    for(int iter =0; iter<num_chain; iter++)
    {
        cout << "Running MCMC chain " << iter +1 << " ..." << endl;
        // generate next indel parameter, nprior_a, nprior_b, etc...
        //bpp.sample_proposal(iter, lrate_prop, grate_prop,out_hyper);

        // Gibbs sampling
        #pragma omp parallel for schedule (guided) num_threads(num_thread)
        for(std::size_t i = 0; i < ids.size(); i++ )
        {
            int c = ids[i];
            bool filter = false;

            // null model
            try{
            BPP_C bppc(c, profile, bpp, gapchar, missing_thres, filter, verbose, consToMis, prune, revgap, min_length);  // for individual element
            if(filter) {
              if(verbose) cerr << "filter: "<< c <<endl;
              continue;
            }

            if(!sample_hyper)
            {
                bppc.initMCMC(iter,bpp,0);  //not constrain log_prob_back
                bppc.Gibbs(iter,bpp,out_Z0,output_path,output_path2,0,true,sample_hyper, lrate_prop, grate_prop);  // Gibbs run to get Z for each element
                //if(!bppc.failure)
                //{
                bppc.Eval2(bpp,0);
                if(bppc.verbose || bppc.failure) bppc.Output_sampling(iter, output_path2, bpp, 0);
                bppc.Output_init(output_path,output_path2,bpp,out_Z0, 0); //sort rates!!, posterior median of nrate and crate; posterior mean of Z
               // }
                // res model, crate by null model
                bppc.initMCMC(iter,bpp,2);  //not constrain log_prob_back
                bppc.Gibbs(iter,bpp,out_Z2,output_path,output_path2,2,true,sample_hyper, lrate_prop, grate_prop);  // Gibbs run to get Z for each element
                //if(!bppc.failure)
                //{
                bppc.Eval2(bpp,2);
                if(bppc.verbose || bppc.failure) bppc.Output_sampling(iter, output_path2, bpp, 1);
                bppc.Output_init(output_path,output_path2,bpp,out_Z2, 2); //sort rates!!
                //}
            }

            // full model, nrate, crate by res model
            bppc.initMCMC(iter,bpp,1);  //not constrain log_prob_back
            bppc.Gibbs(iter, bpp,out_Z1,output_path,output_path2,1, true, sample_hyper, lrate_prop, grate_prop);  // Gibbs run to get Z for each element
            //if(!bppc.failure)
            //{
            bppc.Eval2(bpp,1);
            if(bppc.verbose || bppc.failure) bppc.Output_sampling(iter, output_path2, bpp, 2);
            bppc.Output_init(output_path,output_path2,bpp,out_Z1, 1); //sort rates!!
            //}

            }catch (exception& e){
              cout << c << " Standard exception: " << e.what() << endl;
            }
        }

        // sample indel, nprior_a, nprior_b, etc...
        try{
          if(sample_hyper) {
            bpp.sample_hyperparam(iter, ids, out_hyper);
            bpp.Output_init0(profile,out_lik, ids);
          }else{
            bpp.Output_init(profile,output_path, ids);
          }
        }catch (exception& e){
              cout << " Standard exception: " << e.what() << endl;
        }
    }

        out_Z0.close();
        out_Z1.close();
        out_Z2.close();
        out_hyper.close();
        out_lik.close();

    cout << endl << endl << "time used:  " << (time(NULL)-start)/60 << " min." << endl << endl;

    return 0;
}