#include "FeedLevelDetector.h"
#include <opencv2/imgproc.hpp>
#include <iostream>

cv::Mat FeedLevelDetector::preprocessImage(const cv::Mat& input) {
    cv::Mat gray, blurred;
    
    // 转换为灰度图像
    cv::cvtColor(input, gray, cv::COLOR_BGR2GRAY);
    
    // 高斯模糊去噪
    cv::GaussianBlur(gray, blurred, cv::Size(5, 5), 1.5);
    
    return blurred;
}

float FeedLevelDetector::findFeedSurface(const cv::Mat& image) {
    cv::Mat edges;
    
    // Canny边缘检测
    //cv::Canny(image, edges, 100, 150);
    cv::Canny(image, edges, 100, 200);
    //cv::Canny(image, edges, 50, 100);
    // 查找轮廓
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(edges, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
    
    float minY = image.rows; // 初始设置为图像高度
    
    // 查找所有轮廓中的最高点（距离顶部最近的点）
    for (const auto& contour : contours) {
        for (const auto& point : contour) {
            if (point.y < minY) {
                minY = point.y;
            }
        }
    }
    
    // 如果没有找到轮廓，返回图像高度
    if (minY == image.rows) {
        return image.rows;
    }
    
    return minY;
}

void FeedLevelDetector::displayResult(cv::Mat& image, float distance) {
    // 绘制饲料表面线
    cv::line(image, cv::Point(0, distance), 
             cv::Point(image.cols-1, distance), 
             cv::Scalar(0, 0, 255), 2);
    
    // 显示距离信息（图像坐标系原点在左上角）
    std::string distanceText = "Distance: " + std::to_string(distance) + " pixels";
    cv::putText(image, distanceText, cv::Point(20, 30), 
                cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(0, 255, 0), 2);
    
    // 显示图像
    cv::imshow("Feed Level Detection", image);
    cv::waitKey(0);
}

float FeedLevelDetector::detectFeedLevel(const std::string& imagePath) {
    // 读取图像
    cv::Mat image = cv::imread(imagePath);
    if (image.empty()) {
        std::cerr << "Could not open or find the image: " << imagePath << std::endl;
        return -1;
    }
    
    // 预处理图像
    cv::Mat processed = preprocessImage(image);
    
    // 检测饲料表面
    float distance = findFeedSurface(processed);
    
    // 显示结果
    displayResult(image, distance);
    
    return distance;
}


