#include "MF_VisionMeasureBase.h"

#define M_PI 3.1415923

MF_VisionMeasureBase::MF_VisionMeasureBase()
{
}

MF_VisionMeasureBase::~MF_VisionMeasureBase()
{
}

bool MF_VisionMeasureBase::measureAxis(std::vector<double>& measureRes, const MN_VisionImage::MS_ImageParam& _bufImg)
{
    Mat image; 

    bool bRet = buffer2Mat(_bufImg, image);

    // 将图片转换为灰度图像
    Mat gray_image;
    cvtColor(image, gray_image, COLOR_BGR2GRAY);

    // Canny边缘检测
    if (gray_image.empty())
    {
        std::cerr << "Failed to load the binary image." << std::endl;
    }

    cv::Point middleBlackPixelFirstRow = getMiddleBlackPixelInRow(gray_image, 0); // First row
    cv::Point middleBlackPixelLastRow = getMiddleBlackPixelInRow(gray_image, gray_image.rows / 4 * 3); // Last row

    double angleInDegrees = CalculatingAngle(middleBlackPixelFirstRow, middleBlackPixelLastRow);

    float scale = 1.0; // 不缩放，保持原比例

    // 计算旋转中心（这里假设以最后一行的黑色像素作为旋转中心）
    Size img_size = gray_image.size();
    cv::Point2f center;
    center.x = (middleBlackPixelFirstRow.x + middleBlackPixelLastRow.x) / 2;
    center.y = (middleBlackPixelFirstRow.y + middleBlackPixelLastRow.y) / 2;
    //int width = image.cols;
    //int height = image.rows;
    //center.x = width / 2;
    //center.y = height / 2;
    double angel = angleInDegrees - 90;
    std::cout << "center: " << center << std::endl;

    // 计算旋转矩阵
    Mat rot_mat = getRotationMatrix(center, angel, scale);

    // 旋转图像
    Mat rotated_image_center;
    warpAffine(gray_image, rotated_image_center, rot_mat, img_size, INTER_LINEAR, BORDER_CONSTANT, Scalar(255));

    //// 保存旋转后的图像
    ////string output_path_center = "E:\\project\\Chip detection\\240407\\short\\rotated_Image_20240407144606626.jpg";
    //string outimage_file_extension("rotated_" + file_extension + ".jpg");
    //string output_path_center(Current_image.input_path + outimage_file_extension);
    //imwrite(output_path_center, rotated_image_center);

    //std::string output_txt_path(Current_image.input_path + file_extension + ".txt"); // 生成与输入图片同名但扩展名为.txt的输出文件路径
    //ofstream outputFile(output_txt_path); // 创建输出文件

    //if (!outputFile.is_open())
    //{
    //    cerr << "Error: Could not open or create the output file!" << endl;
    //}

    for (int row = 0; row < rotated_image_center.rows; ++row)
    {
        int blackPixelCount = 0;

        for (int col = 0; col < rotated_image_center.cols; ++col)
        {
            if (rotated_image_center.at<uchar>(row, col) < 10) // 黑色像素
            {
                ++blackPixelCount;
            }
        }

       // outputFile << blackPixelCount << endl; // 输出当前行的黑色像素数量
    }

	return true;
}

bool MF_VisionMeasureBase::buffer2Mat(const MN_VisionImage::MS_ImageParam& _inImg, cv::Mat& _mat)
{
    if (_inImg.m_channels == 1)
    {
        _mat = cv::Mat::zeros(cv::Size(_inImg.m_width, _inImg.m_height), CV_8UC1);
    }
    else if (_inImg.m_channels == 3)
    {
        _mat = cv::Mat::zeros(cv::Size(_inImg.m_width, _inImg.m_height), CV_8UC3);
    }
    else
    {
        printf("图像通道格式错误！ \n");
        return false;
    }

    for (int j = 0; j < _inImg.m_height; ++j)
    {
        unsigned char* data = _mat.ptr<unsigned char>(j);
        unsigned char* pSubBuffer = _inImg.m_data.get() + (_inImg.m_height - 1 - j) * _inImg.m_width * _inImg.m_channels;
        memcpy(data, pSubBuffer, _inImg.m_width * _inImg.m_channels);
    }

    if (_inImg.m_channels == 1)
    {
        cv::cvtColor(_mat, _mat, cv::COLOR_GRAY2BGR);
    }
    else if (_inImg.m_channels == 3)
    {
        cv::cvtColor(_mat, _mat, cv::COLOR_RGB2BGR);
    }
    else
    {
        printf("图像通道格式错误！ \n");
        return false;
    }

    return true;
}

Mat MF_VisionMeasureBase::getRotationMatrix(Point2f center, float angle, float scale) {
    Mat rot_mat = getRotationMatrix2D(center, angle, scale);
    return rot_mat;
}

cv::Point MF_VisionMeasureBase::getMiddleBlackPixelInRow(const cv::Mat& binaryImage, int row)
{
    int startCol = -1;
    int endCol = -1;

    for (int col = 0; col < binaryImage.cols; ++col)
    {
        if (binaryImage.at<uchar>(row, col) < 10) // Black pixel
        {
            if (startCol == -1)
            {
                startCol = col;
            }
            endCol = col;
        }
    }

    if (startCol != -1 && endCol != -1)
    {
        return cv::Point((startCol + endCol) / 2, row);
    }
    else
    {
        // No black pixels found in this row
        return cv::Point(-1, -1);
    }
}

double MF_VisionMeasureBase::CalculatingAngle(Point middleBlackPixelFirstRow, Point middleBlackPixelLastRow)
{
    double angleInDegrees = 0;
    if (middleBlackPixelFirstRow.x != -1 && middleBlackPixelLastRow.x != -1)
    {
        double dx = middleBlackPixelLastRow.x - middleBlackPixelFirstRow.x;
        double dy = middleBlackPixelLastRow.y - middleBlackPixelFirstRow.y;

        std::cout << "middleBlackPixelFirstRow: " << middleBlackPixelFirstRow << std::endl;
        std::cout << "middleBlackPixelLastRow: " << middleBlackPixelLastRow << std::endl;

        double angleInRadians = std::atan2(dy, dx);
        angleInDegrees = angleInRadians * 180.0 / M_PI;

        std::cout << "Angle of inclination (in degrees): " << angleInDegrees << std::endl;
    }
    else
    {
        std::cout << "No black pixels found in either the first or last row." << std::endl;
    }
    return angleInDegrees;
}