#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <phidget21.h>
#include <Windows.h>
#pragma comment(lib, "vfw32.lib")
#pragma comment(lib, "comctl32.lib")

const std::string messageSuccess("success\n");
const std::string cmdExit("exit");
const std::string cmdCapture("capture");
const std::string cmdPulse("pulse");

const int maxCommandSize = 1024*1024;
char commandBuffer[maxCommandSize];

const std::string imageFileName("image.jpg");
const std::string testFileName("test.jpg");

const cv::Vec3b black(0,0,0), white(255,255,255);
const int row = 355, col = 355;
const cv::Vec3b paperColor(219, 99, 240);

class MockInterfaceKit {
public:
	MockInterfaceKit() {}
	bool isAttached() {return true;}
	void pulse(int index, int time = 500) {Sleep(time);}

};

/**
* PhidgetInterfaceKit provides a C++ interface to the C API provided by Phidget.
* For simplicity, this class does not allow itself to be copied. The Phidget handle
* is automatically created and destroyed in the constructor and destructor of this class.
*/
class PhidgetInterfaceKit {
private:
	CPhidgetInterfaceKitHandle handle;
	bool attached;

	CPhidgetHandle baseHandle() {
		return reinterpret_cast<CPhidgetHandle>(handle);
	}

	PhidgetInterfaceKit(const PhidgetInterfaceKit& other) {
		throw "Unsupported operation.";
	}

	void operator=(const PhidgetInterfaceKit& other) {
		throw "Unsupported operation.";
	}

public:
	PhidgetInterfaceKit(int serial = -1, int timeout = 10000) {
		handle = nullptr;
		CPhidgetInterfaceKit_create(&handle);
		CPhidget_open(baseHandle(), serial);
		int result = CPhidget_waitForAttachment(baseHandle(), timeout);
		attached = !result;
	}

	~PhidgetInterfaceKit() {
		CPhidget_close(baseHandle());
		CPhidget_delete(baseHandle());
	}

	void pulse(int index, int time = 500) {
		CPhidgetInterfaceKit_setOutputState(handle, index, 1);
		Sleep(time);
		CPhidgetInterfaceKit_setOutputState(handle, index, 0);
	}

	bool isAttached() {
		return attached;
	}
};

void printVal(const cv::Vec3b& v) {
	int x = v(0), y = v(1), z = v(2);
	std::cout << x << "," << y << "," << z << "\n";
}

cv::Mat threshold(const cv::Mat& mat, cv::Vec3b color, int range) {
	int lo[3] = {color(0) - range, color(1) - range, color(2) - range};
	int hi[3] = {color(0) + range, color(1) + range, color(2) + range};

	cv::Size size = mat.size();
	cv::Mat thres(size, cv::Vec3b::type);
	for (int i = 0; i < size.height; i++) {
		for (int j = 0; j < size.width; j++) {
			cv::Vec3b v = mat.at<cv::Vec3b>(i, j);
			if (v(0) >= lo[0] && v(0) <= hi[0] && v(1) >= lo[1] && v(1) <= hi[1] && v(2) >= lo[2] && v(2) <= hi[2]) {
				thres.at<cv::Vec3b>(i, j) = white;
			} else {
				thres.at<cv::Vec3b>(i, j) = black;
			}
		}
	}
	return thres;
}

int count(const cv::Mat& mat, const cv::Vec3b& val) {
	int cnt = 0;
	cv::Size size = mat.size();
	for (int i = 0; i < size.height; i++) {
		for (int j = 0; j < size.width; j++) {
			if (mat.at<cv::Vec3b>(i, j) == val) {
				cnt++;
			}
		}
	}
	return cnt;
}

cv::Mat writeCoord(const cv::Mat& pic, int row, int col) {
	cv::Mat frame = pic.clone();
	cv::Size size = frame.size();
	for (int i = 0; i < size.height; i++) {
		frame.at<cv::Vec3b>(i, col) = white;
	}
	for (int i = 0; i < size.width; i++) {
		frame.at<cv::Vec3b>(row, i) = white;
	}
	return frame;
}

void test() {
	cv::Mat frame = cv::imread(imageFileName);
	cv::Mat test = threshold(frame, paperColor, 50);
	cv::imwrite(testFileName, test);
}

int main( int argc, char** argv )
{
	cv::VideoCapture cap(1);
	if (!cap.isOpened()) {
		std::cout << "Failed: Cannot open camera.\n";
		return -1;
	}
	MockInterfaceKit ifk;
	if (!ifk.isAttached()) {
		std::cout << "Failed: Cannot open interface kit.\n";
		return -1;
	}
	std::cout << messageSuccess;

	for (;;) {
		std::cin.getline(commandBuffer, maxCommandSize);
		if (std::cin.eof()) {
			return 0;
		}

		std::string cmd(commandBuffer);
		size_t spaceIndex = cmd.find_first_of(' ');
		std::string arg = cmd.substr(spaceIndex + 1);
		cmd = cmd.substr(0, spaceIndex);
		if (cmd == cmdExit) {
			std::cout << messageSuccess;
			return 0;
		} else if (cmd == cmdCapture) {
			cv::Mat frame;
			cap >> frame;
			cap >> frame;
			cv::imwrite(imageFileName, frame);
			std::cout << messageSuccess;
		} else if (cmd == cmdPulse) {
			ifk.pulse(6, 2000);
			ifk.pulse(7);
			std::cout << messageSuccess;
		} else if (cmd == "carhere") {
			cv::Mat frame;
			cap >> frame;
			cap >> frame;
			cv::Mat thres = threshold(frame, paperColor, 30);
			int c = count(thres, white);
			std::string resp = c < 500 ? "true\n" : "false\n";
			std::cout << resp << messageSuccess;
		} else if (cmd == "test") {
			test();
		} else {
			std::cout << "Error: Unknown command:" << commandBuffer << "\n";
		}
	}

	// Not reached
	return 0;
}