#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;
}