#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>

using namespace cv;
using namespace std;

#define M 5

int main()
{
	Mat image = imread("Src.png", IMREAD_GRAYSCALE);

	vector<uchar> histogram;
	for (int i = 0; i < 256; ++i)
	{
		histogram.push_back(0);
	}
	for (int y = 0; y < image.rows; y++)
	{
		for (int x = 0; x < image.cols; x++)
		{
			histogram[image.at<uchar>(y, x)]++;
		}
	}

	int N = image.cols * image.rows;
	vector<uchar> thresholds;
	vector<uchar> tempThresholds;
	for (int i = 0; i < M - 1; i++)
	{
		thresholds.push_back(0);
		tempThresholds.push_back(i + 1);
	}

	float minSumSigmaW = 999999999999999.f;
	int L = 255;

	while (tempThresholds[0] < (L - (M - 1)))
	{
		float sumSigmaW = 0;
		int leftBound = 0;
		int rightBound = 255;
		for (int numberOfCluster = 0; numberOfCluster < M; numberOfCluster++)
		{
			leftBound = 0;
			rightBound = 255;
			if (numberOfCluster > 0)
			{
				leftBound = tempThresholds[numberOfCluster - 1] + 1;
			}
			if (numberOfCluster < M - 1)
			{
				rightBound = tempThresholds[numberOfCluster];
			}

			float mu = 0;
			float sumNf = 0;
			float nf = 0;
			for (int f = leftBound; f <= rightBound; f++)
			{
				nf = histogram[f];
				sumNf = sumNf + nf;
				mu = mu + f * nf;
			}
			mu = mu / sumNf;
			float q = sumNf / N;
			float sigmaW = 0;
			for (int f = leftBound; f <= rightBound; f++)
			{
				sigmaW = sigmaW + (f - mu) * (f - mu) * histogram[f];
			}
			sumSigmaW = sumSigmaW + sigmaW;
		}

		if (sumSigmaW < minSumSigmaW)
		{
			minSumSigmaW = sumSigmaW;
			for (int i = 0; i < M - 1; ++i)
			{
				thresholds[i] = tempThresholds[i];
			}
		}

		tempThresholds[M - 2]++;
		for (int i = 1; i < M - 1; i++)
		{
			if (tempThresholds[i] == 255)
			{
				tempThresholds[i - 1]++;
				tempThresholds[i] = tempThresholds[i - 1] + 1;
			}
		}
	}

	for (int i = 0; i < M - 1; i++)
	{
		if (i == 0)
		{
			cout << i + 1 << "st treshold = ";
		}
		else if (i == 1)
		{
			cout << i + 1 << "nd treshold = ";
		}
		else if (i == 2)
		{
			cout << i + 1 << "rd treshold = ";
		}
		else
		{
			cout << i + 1 << "th treshold = ";
		}
		cout << (int)thresholds[i] << endl;
	}
	imshow("Source image", image);
	int amountOfTreshholds = M - 1;
	uchar fPerThreshold = 255 / amountOfTreshholds;
	for (int y = 0; y < image.rows; y++)
	{
		for (int x = 0; x < image.cols; x++)
		{
			uchar f = image.at<uchar>(y, x);
			for (int i = 0; i < amountOfTreshholds; i++)
			{
				if (f < thresholds[i])
				{
					image.at<uchar>(y, x) = fPerThreshold * i;
					break;
				}
				else if (i == amountOfTreshholds - 1)
				{
					image.at<uchar>(y, x) = 255;
				}
			}
		}
	}
	imwrite("result.bmp", image);
	imshow("Result image", image);

	unsigned long maxF = 0;
	for (int f = 0; f < 256; f++)
	{
		if (histogram[f] > maxF)
		{
			maxF = histogram[f];
		}
	}
	Mat hist = Mat(300, 256, CV_8UC3, Scalar(0, 0, 0));
	for (int f = 0; f < 255; f++)
	{
		line(hist, Point(f, 299), Point(f, (maxF - histogram[f]) * 300 / maxF), Scalar(0, 255, 0));
	}
	for (int i = 0; i < thresholds.size(); i++)
	{
		line(hist, Point(thresholds[i], 299), Point(thresholds[i], 0), Scalar(0, 0, 255));
	}
	imwrite("histogram.bmp", hist);
	imshow("Histogram", hist);
	waitKey(0);
	return 0;
}