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

using namespace cv;
using namespace std;

int applyMask(unsigned short* input, unsigned char* pmask, int Width, int Height, unsigned short* output)
{
    if (input == nullptr)
    {
        return 0;
    }
    if (pmask == nullptr)
    {
        return 0;
    }

    Mat src(Size(Width, Height), CV_16U, input);
    Mat mask(Size(Width, Height), CV_8U, pmask);
    Mat result;
    bitwise_and(src, src, result, mask);
  
    if (result.empty() || result.rows != Height || result.cols != Width )
    {
        return 0;
    }
    memcpy(output, result.data, Width * Height * sizeof(unsigned short));

    return 1;
}

int getMaskRect(unsigned char* pmask, int Width, int Height, int &x,int &y,int &RectWidth,int &RectHeight)
{
    if (pmask == nullptr)
    {
        return 0;
    }

    Mat mask(Size(Width, Height), CV_8U, pmask);

    if (!sum(mask)[0])
    {
        return 0;
    }

    cv::Mat nonZeroPoints;
    cv::findNonZero(mask, nonZeroPoints);
    cv::Rect rect = cv::boundingRect(nonZeroPoints);

    x = rect.x;
    y = rect.y;
    RectWidth = rect.width;
    RectHeight = rect.height;
    
    return 1;
}

int cropImg(unsigned short *input, unsigned short *output, int Width, int Height, int x, int y, int RectWidth, int RectHeight)
{
    if (input == nullptr)
    {
        return 0;
    }
    if (output == nullptr)
    {
        return 0;
    }

    Mat src(Size(Width, Height), CV_16U, input);
    memcpy(output, src(Rect(x, y, RectWidth, RectHeight)).clone().data, RectWidth * RectHeight * sizeof(unsigned short));

    return 1;
}

int applyInvertMask(unsigned short* input, unsigned char* pmask, int Width, int Height, unsigned short* output,int fillvalue = 4095)
{
	if (input == nullptr)
	{
		return 0;
	}
	if (pmask == nullptr)
	{
		return 0;
	}

#pragma omp parallel for
	for (int i = 0; i < Width * Height; i++)
	{
		if (!pmask[i])
		{
			output[i] = fillvalue;
		}	
		else
		{
			output[i] = input[i];
		}
	}

	return 1;
}


//input:rawdata
//pmask:collimatorMask
//output:ForegroundMask  后续计算窗宽窗位传入ForegroundMask
int getForegroundMask(unsigned short* input, unsigned char* pmask, int Width, int Height, unsigned char* output)
{
	if (!input || !pmask || !output) {
		std::cerr << "Error: Null pointer input!" << std::endl;
		return 0;
	}
	if (Width <= 0 || Height <= 0) {
		std::cerr << "Error: Invalid image size!" << std::endl;
		return 0;
	}

	int max_grayv = 65535;
	int* h = new int[max_grayv + 1]();
	int i, j = { 0 };
	double masksum = 0;
	double mu = 0;
#pragma omp parallel for
	for (i = 0; i < Width* Height; i++)
	{
		if (pmask[i])
		{
			h[input[i]]++;
			masksum++;
			mu += input[i];
		}	
	}
	
	mu /= masksum;

	if (masksum < 1e-6) {
		std::cerr << "Warning: Mask region is empty." << std::endl;
		delete[] h;
		std::fill(output, output + Width * Height, 0);
		return 0;
	}

	double mu1 = 0, q1 = 0;
	double max_sigma = 0, max_val = 0;
	double scale = 1. / masksum;
	for (i = 0; i < max_grayv + 1; i++)
	{
		double p_i, q2, mu2, sigma;

		p_i = h[i] * scale;
		mu1 *= q1;
		q1 += p_i;
		q2 = 1. - q1;

		if (std::min(q1, q2) < FLT_EPSILON || std::max(q1, q2) > 1. - FLT_EPSILON)
			continue;

		mu1 = (mu1 + i * p_i) / q1;
		mu2 = (mu - q1 * mu1) / q2;
		sigma = q1 * q2 * (mu1 - mu2) * (mu1 - mu2);
		if (sigma > max_sigma)
		{
			max_sigma = sigma;
			max_val = i;
		}
	}

	delete[]h;

	for (i = 0; i < Width * Height; i++)
	{
		if (pmask[i])
		{
			if (input[i] < max_val)
			{
				output[i] = 1;
			}
			else
			{
				output[i] = 0;
			}
		}
		else
		{
			output[i] = 0;
		}
	}

	return 1;
}