#include "slice_predict.h"

//MIP画像の生成
cv::Mat Slice_predict::createMIP(Raw_image<short>& images)
{
	cv::Mat MIP(images.data.Depth, images.data.Width, CV_16S, cv::Scalar::all(0));

	short* pt = MIP.ptr<short>(0);
	for (int i = 0; i < images.data.Width * images.data.Depth; i++) {
		short max = 0;
		for (int j = 0; j < images.data.Height; j++) {
			int point = (i / images.data.Width) * images.data.IMAGESIZE + j * images.data.Width + (i % images.data.Width);
			if (max < images.image[point])
				max = images.image[point];
		}
		pt[i] = max;
	}
	double grad = 255.0 / 1700.0;
	cv::Mat dispMIP;
	MIP.convertTo(dispMIP, CV_8U, grad, 127.5 - (1000.0 * grad));
	cvtColor(dispMIP.clone(), dispMIP, cv::COLOR_GRAY2BGR);
	return dispMIP;
}

//相関係数の計算
std::vector<float> Slice_predict::Correlation(std::vector<float>& input1, std::vector<float>& input2)
{
	int range = input2.size() / 2;
	std::vector<float> result;
	float ave2 = 0;
	//printf("range:%d\n", range);
	for (int i = range; i < input1.size() - range; i++) {
		//input1 Average;
		float ave1 = 0;
		for (int j = i - range; j <= i + range; j++) {
			ave1 += input1[j];
		}
		ave1 = ave1 / input2.size();
		float Sxy = 0;
		float Sx = 0, Sy = 0;
		for (int j = i - range, k = 0; j < i + range; j++, k++) {
			Sxy += (input1[j] - ave1) * (input2[k] - ave2);
			Sx += (input1[j] - ave1) * (input1[j] - ave1);
			Sy += (input2[k] - ave2) * (input2[k] - ave2);
		}
		Sxy = Sxy / input2.size();
		Sx = sqrt(Sx / input2.size());
		Sy = sqrt(Sy / input2.size());
		result.push_back(Sxy / (Sx * Sy));
	}
	return result;
}

//学習したモデルを読み込んでスライス位置を推定
void Slice_predict::predict(Raw_image<short>& image, int& shoulder, int& low, cv::dnn::Net& class_model, cv::dnn::Net& shoulde_model, cv::dnn::Net& lower_model)
{

	cv::Mat im = createMIP(image);
	cv::Mat imr;
	resize(im, imr, cv::Size(224, 224));
	cv::Mat blob;
	class_model.setInput(cv::dnn::blobFromImage(imr, 1.0 / 255.0, cv::Size(224, 224), cv::Scalar(), true));
	cv::Mat prob = class_model.forward();
	auto p = prob.ptr<float>(0);
	float max = 0;
	int arg = 0;
	for (int j = 0; j < 4; j++)
		if (max < p[j]) {
			max = p[j];
			arg = j;
		}

	if (arg == 0) {
		cv::Mat ims(im.rows + 512, im.cols, CV_8UC3, cv::Scalar::all(0));
		cv::Mat q(ims, cv::Rect(0, 256, im.cols, im.rows));
		im.copyTo(q);

		std::vector<float> liner = { -60, -30, 0 , 30 , 60 };
		int range = liner.size() / 2;
		std::vector<cv::Mat> imgs;
		for (int j = 0; j < im.rows - 1; j += 30) {
			cv::Mat input(ims, cv::Rect(0, j, im.cols, im.cols));
			imgs.push_back(input);
		}

		std::vector<float> predS(imgs.size());

		shoulde_model.setInput(cv::dnn::blobFromImages(imgs, 1.0 / 255.0, cv::Size(224, 224), cv::Scalar(), true));
		cv::Mat pred = shoulde_model.forward();
		for (int j = 0; j < pred.rows; j++) {
			auto p = pred.ptr<float>(0);
			predS[j] = p[j];
		}

		auto correration = Correlation(predS, liner);
		float min = 9999;
		int place = 0;

		for (int j = 0; j < correration.size(); j++) {
			auto lengthabs = abs(predS[j + range]);
			if (correration[j] > 0.98 && lengthabs < 15) {
				if (min > lengthabs) {
					min = lengthabs;
					place = j + range;
				}
			}
		}
		printf("%d,%d,%d\n", correration.size(), predS.size(), place);
		int splace;
		if (place != 0) {
			splace = (place * 30) - predS[place];
			shoulder = splace;
		}
		else {
			splace = shoulder = 0;
		}

		imgs = std::vector<cv::Mat>();
		std::vector<float> predL;
		for (int j = splace; j < im.rows - 1; j += 30) {
			cv::Mat input(ims, cv::Rect(0, j, im.cols, im.cols));
			imgs.push_back(input);
		}
		lower_model.setInput(cv::dnn::blobFromImages(imgs, 1.0 / 255.0, cv::Size(224, 224), cv::Scalar(), true));
		cv::Mat predLs = lower_model.forward();
		auto p = predLs.ptr<float>(0);
		for (int j = 0; j < predLs.rows; j++) {
			predL.push_back(p[j]);
		}

		auto cL = Correlation(predL, liner);
		min = 9999;
		place = im.rows;
		for (int j = 0; j < cL.size(); j++) {
			auto lengthabs = abs(predL[j + range]);
			if (cL[j] > 0.98 && lengthabs < 15) {
				if (min > lengthabs) {
					min = lengthabs;
					place = j + range;
				}
			}
		}

		int lplace;
		if (place != im.rows) {
			lplace = (place * 30) - predL[place] + splace;
			low = lplace;
		}
		else {
			lplace = im.rows;
			low = im.rows;
		}
		printf("Soulder:%d Lower:%d\n", splace, lplace);
		return;
	}
	else {
		switch (arg)
		{
		case 1:
			shoulder = im.rows;
			low = im.rows;
			break;
		case 2:
			shoulder = 0;
			low = 0;
			break;
		case 3:
			shoulder = 0;
			low = im.rows;
		default:
			break;
		}
	}

}