#include "flash_detector/flash_detector.h"
#include <cv_bridge/cv_bridge.h>

namespace flash_detector
{

FlashDetector::FlashDetector()
    : nh_private_("~")
    , it_(nh_)
    , fx_(0.0), fy_(0.0), cx_(0.0), cy_(0.0)
    , camera_info_received_(false)
    , frame_count_(0), detection_count_(0)
    , kernel_size_(5), min_area_(50)
{
    // Get parameters
    nh_private_.param<std::string>("camera_topic", camera_topic_, "/camera/color/image_raw");
    nh_private_.param<std::string>("camera_info_topic", camera_info_topic_, "/camera/color/camera_info");
    nh_private_.param<bool>("debug_mode", debug_mode_, false);

    // HSV thresholds
    int hsv_h_min, hsv_h_max, hsv_s_min, hsv_s_max, hsv_v_min, hsv_v_max;
    nh_private_.param("hsv_h_min", hsv_h_min, 0);
    nh_private_.param("hsv_h_max", hsv_h_max, 30);
    nh_private_.param("hsv_s_min", hsv_s_min, 100);
    nh_private_.param("hsv_s_max", hsv_s_max, 255);
    nh_private_.param("hsv_v_min", hsv_v_min, 200);
    nh_private_.param("hsv_v_max", hsv_v_max, 255);
    hsv_lower_ = cv::Scalar(hsv_h_min, hsv_s_min, hsv_v_min);
    hsv_upper_ = cv::Scalar(hsv_h_max, hsv_s_max, hsv_v_max);

    // Update morphological kernel
    updateKernel();

    // Subscribe to camera image
    image_sub_ = it_.subscribe(camera_topic_, 1, &FlashDetector::imageCallback, this);

    // Subscribe to camera info
    if (!camera_info_topic_.empty())
    {
        camera_info_sub_ = nh_.subscribe(camera_info_topic_, 1, &FlashDetector::cameraInfoCallback, this);
    }

    // Publishers
    flash_pub_ = nh_.advertise<std_msgs::Bool>("/flash_detector/flash_detected", 1);
    detection_pub_ = nh_.advertise<std_msgs::Float32MultiArray>("/flash_detector/detection", 1);
    image_pub_ = it_.advertise("/flash_detector/bounding_box_image", 1);
    mask_pub_ = it_.advertise("/flash_detector/mask_image", 1);

    // Dynamic reconfigure - create server with NodeHandle
    server_ = boost::make_shared<dynamic_reconfigure::Server<flash_detector::FlashDetectorConfig>>(nh_private_);
    server_->setCallback(boost::bind(&FlashDetector::dynamicReconfigureCallback, this, _1, _2));
    
    ROS_INFO("[Flash Detector] Initialization complete");
    ROS_INFO("[Flash Detector] Subscribed to: %s", camera_topic_.c_str());
    ROS_INFO("[Flash Detector] Camera info topic: %s", camera_info_topic_.c_str());
    ROS_INFO("[Flash Detector] HSV thresholds: H[%d,%d] S[%d,%d] V[%d,%d]",
             hsv_h_min, hsv_h_max, hsv_s_min, hsv_s_max, hsv_v_min, hsv_v_max);
}

FlashDetector::~FlashDetector()
{
}

void FlashDetector::updateKernel()
{
    // Ensure kernel_size is odd
    if (kernel_size_ % 2 == 0)
    {
        kernel_size_ += 1;
    }
    kernel_ = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(kernel_size_, kernel_size_));
}

void FlashDetector::cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
{
    if (!camera_info_received_)
    {
        fx_ = msg->K[0];
        fy_ = msg->K[4];
        cx_ = msg->K[2];
        cy_ = msg->K[5];
        camera_info_received_ = true;
        ROS_INFO("[Flash Detector] Camera intrinsics: fx=%.2f, fy=%.2f, cx=%.2f, cy=%.2f",
                 fx_, fy_, cx_, cy_);
    }
}

void FlashDetector::dynamicReconfigureCallback(flash_detector::FlashDetectorConfig &config, uint32_t level)
{
    ROS_INFO("[Reconfigure] Updating parameters: H[%d,%d] S[%d,%d] V[%d,%d] kernel=%d area=%.1f",
             config.hsv_h_min, config.hsv_h_max,
             config.hsv_s_min, config.hsv_s_max,
             config.hsv_v_min, config.hsv_v_max,
             config.kernel_size, config.min_area);

    // Update HSV thresholds
    hsv_lower_ = cv::Scalar(config.hsv_h_min, config.hsv_s_min, config.hsv_v_min);
    hsv_upper_ = cv::Scalar(config.hsv_h_max, config.hsv_s_max, config.hsv_v_max);

    // Update morphological parameters
    kernel_size_ = config.kernel_size;
    updateKernel();

    // Update minimum area
    min_area_ = config.min_area;
}

void FlashDetector::imageCallback(const sensor_msgs::Image::ConstPtr& msg)
{
    try
    {
        // Convert ROS image to OpenCV format
        cv::Mat cv_image = cv_bridge::toCvShare(msg, "bgr8")->image;
        
        frame_count_++;
        
        // Execute flash detection
        DetectionResult result = detectFlash(cv_image);
        
        // Publish results
        publishResults(cv_image, result, msg->header.frame_id);
        
        // Debug mode - display images
        if (debug_mode_)
        {
            cv::imshow("Flash Detector - Original", cv_image);
            cv::imshow("Flash Detector - Mask", result.mask);
            cv::imshow("Flash Detector - Result", result.result_image);
            cv::waitKey(1);
        }
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("[Flash Detector] Image conversion failed: %s", e.what());
    }
}

DetectionResult FlashDetector::detectFlash(const cv::Mat& image)
{
    DetectionResult result;
    result.detected = false;
    result.cx = 0;
    result.cy = 0;
    result.area = 0.0;
    result.sight_x = 0.0;
    result.sight_y = 0.0;

    // 1. Convert to HSV color space
    cv::Mat hsv_image;
    cv::cvtColor(image, hsv_image, cv::COLOR_BGR2HSV);

    // 2. Create mask (threshold segmentation)
    cv::Mat mask;
    cv::inRange(hsv_image, hsv_lower_, hsv_upper_, mask);

    // 3. Morphological operations (denoising, connecting nearby regions)
    cv::morphologyEx(mask, mask, cv::MORPH_CLOSE, kernel_);  // Closing
    cv::morphologyEx(mask, mask, cv::MORPH_OPEN, kernel_);   // Opening

    // 4. Find contours
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

    // 5. Filter and mark
    cv::Mat result_image = image.clone();
    result.centers.clear();

    for (size_t i = 0; i < contours.size(); i++)
    {
        // Calculate contour area
        double area = cv::contourArea(contours[i]);

        // Filter small noise
        if (area > min_area_)
        {
            // Calculate bounding box
            cv::Rect bbox = cv::boundingRect(contours[i]);

            // Draw detection box (green)
            cv::rectangle(result_image, bbox.tl(), bbox.br(), cv::Scalar(0, 255, 0), 2);

            // Calculate center point
            int cx = bbox.x + bbox.width / 2;
            int cy = bbox.y + bbox.height / 2;

            // Mark center point (red)
            cv::circle(result_image, cv::Point(cx, cy), 5, cv::Scalar(0, 0, 255), -1);

            result.centers.push_back(std::make_pair(cx, cy));
            detection_count_++;

            // Log first detection
            if (!result.detected)
            {
                result.detected = true;
                result.cx = cx;
                result.cy = cy;
                result.area = area;

                // Calculate sight angles (requires camera intrinsics)
                if (camera_info_received_ && fx_ > 0)
                {
                    result.sight_x = std::atan2(cx - cx_, fx_);
                    result.sight_y = std::atan2(cy - cy_, fy_);
                }

                ROS_INFO_THROTTLE(5, "[Flash Detector] Flash detected! Position: (%d, %d), Area: %.1f",
                                 cx, cy, area);
            }
        }
    }

    result.mask = mask;
    result.result_image = result_image;

    return result;
}

void FlashDetector::publishResults(const cv::Mat& original_image, const DetectionResult& result, const std::string& frame_id)
{
    // Publish flash detection flag
    std_msgs::Bool flash_msg;
    flash_msg.data = result.detected;
    flash_pub_.publish(flash_msg);

    // Publish detection data [cx, cy, area, sight_x, sight_y]
    std_msgs::Float32MultiArray detection_msg;
    
    if (result.detected)
    {
        detection_msg.data.push_back(static_cast<float>(result.cx));
        detection_msg.data.push_back(static_cast<float>(result.cy));
        detection_msg.data.push_back(static_cast<float>(result.area));
        detection_msg.data.push_back(static_cast<float>(result.sight_x));
        detection_msg.data.push_back(static_cast<float>(result.sight_y));
    }
    else
    {
        detection_msg.data.push_back(0.0f);
        detection_msg.data.push_back(0.0f);
        detection_msg.data.push_back(0.0f);
        detection_msg.data.push_back(0.0f);
        detection_msg.data.push_back(0.0f);
    }
    
    detection_pub_.publish(detection_msg);

    // Publish image with detection box
    sensor_msgs::ImagePtr bbox_image_msg = cv_bridge::CvImage(
        std_msgs::Header(),
        "bgr8",
        result.result_image).toImageMsg();
    bbox_image_msg->header.frame_id = frame_id;
    image_pub_.publish(bbox_image_msg);

    // Publish binary mask image (convert to BGR for better visualization)
    cv::Mat mask_colored;
    cv::cvtColor(result.mask, mask_colored, cv::COLOR_GRAY2BGR);
    sensor_msgs::ImagePtr mask_image_msg = cv_bridge::CvImage(
        std_msgs::Header(),
        "bgr8",
        mask_colored).toImageMsg();
    mask_image_msg->header.frame_id = frame_id;
    mask_pub_.publish(mask_image_msg);
}

void FlashDetector::run()
{
    ROS_INFO("[Flash Detector] Node started, waiting for images...");
    ros::spin();
}

} // namespace flash_detector
