// My local folder : ~/RUST/rover_vo, run as carge run
// --- Project: rover_vo (Image Capture and Display) ---
use opencv::prelude::*;
use opencv::{
    Result,
    Error, 
    core::{Mat, Scalar, Point, Vector, Code, Rect}, 
    videoio::{self, VideoCapture, CAP_ANY},
    imgcodecs,
    imgproc,
    highgui,
};
use std::time::Instant;

const CAPTURE_INTERVAL_SECONDS: u64 = 4; // Capture every 4 seconds
const OUTPUT_DIR: &str = "."; 
const FRAME_WIDTH: f64 = 640.0;
const FRAME_HEIGHT: f64 = 480.0;

fn run_capture_app() -> Result<()> {
    
    // --- 1. Setup Video Capture: Single Combined Stereo Camera on Index 0 ---
    println!("-> Attempting to open single integrated stereo camera on index 0...");
    let mut cap_combined = VideoCapture::new(0, CAP_ANY)?;
    
    // Set properties expecting a combined frame (e.g., side-by-side)
    let expected_width = FRAME_WIDTH * 2.0;
    let expected_height = FRAME_HEIGHT;

    cap_combined.set(videoio::CAP_PROP_FRAME_WIDTH, expected_width)?;
    cap_combined.set(videoio::CAP_PROP_FRAME_HEIGHT, expected_height)?;
    
    // Check if the camera opened and confirm the actual properties
    if !cap_combined.is_opened()? {
         return Err(Error::new(
            Code::StsError.into(), 
            "Failed to open single stereo camera at index 0. Check device connection.",
        ));
    }

    // Read the actual frame dimensions for robust splitting
    let actual_width = cap_combined.get(videoio::CAP_PROP_FRAME_WIDTH)?;
    let actual_height = cap_combined.get(videoio::CAP_PROP_FRAME_HEIGHT)?;
    
    if actual_width < FRAME_WIDTH * 2.0 {
        return Err(Error::new(
            Code::StsError.into(), 
            format!("Camera at index 0 did not provide expected wide frame (expected width >= {}, got {}).", FRAME_WIDTH * 2.0, actual_width),
        ));
    }
    
    println!("-> Successfully opened single camera at index 0. Actual frame size: {}x{}. Proceeding to split frame.", 
        actual_width, actual_height);

    // --- 2. Setup Windows and Loop Variables ---
    highgui::named_window("Combined Stereo Feed (GUIDE)", highgui::WINDOW_AUTOSIZE)?; // NEW WINDOW
    highgui::named_window("Left Camera Feed", highgui::WINDOW_AUTOSIZE)?;
    highgui::named_window("Right Camera Feed", highgui::WINDOW_AUTOSIZE)?;

    let mut frame_count = 0;
    let mut last_capture_time = Instant::now();

    println!("\nStereo Capture Started. Press ESC to exit.");
    println!("--- Capturing image pair every {} seconds automatically. ---", CAPTURE_INTERVAL_SECONDS);
    println!("Files will be saved in {} as left_frame_XXXXX.jpg and right_frame_XXXXX.jpg", OUTPUT_DIR);
    
    // --- 3. Main Capture Loop ---
    loop {
        let mut frame_combined = Mat::default();
        let mut frame_l_final = Mat::default();
        let mut frame_r_final = Mat::default();
        
        // Read combined frame
        if !cap_combined.read(&mut frame_combined)? {
            println!("End of combined stream reached or camera disconnected.");
            break;
        }
        if frame_combined.empty() { continue; }

        // Split the combined frame (assuming side-by-side)
        let combined_width = frame_combined.cols();
        let frame_height = frame_combined.rows();
        let half_width = combined_width / 2;
        
        // Split the frame using ROI (Region of Interest)
        frame_l_final = Mat::roi(&frame_combined, Rect::new(0, 0, half_width, frame_height))?.try_clone()?;
        frame_r_final = Mat::roi(&frame_combined, Rect::new(half_width, 0, half_width, frame_height))?.try_clone()?;
        

        let now = Instant::now();
        let elapsed = now.duration_since(last_capture_time).as_secs();
        let is_capture_ready = elapsed >= CAPTURE_INTERVAL_SECONDS;

        // --- 4. Capture & Save Logic ---
        if is_capture_ready {
            // Save in .jpg format
            let left_path = format!("{}/left_frame_{:05}.jpg", OUTPUT_DIR, frame_count);
            let right_path = format!("{}/right_frame_{:05}.jpg", OUTPUT_DIR, frame_count);

            // Save the color frame
            if imgcodecs::imwrite(&left_path, &frame_l_final, &Vector::new())? {
                println!("Saved Left Frame: {}", left_path);
            }
            if imgcodecs::imwrite(&right_path, &frame_r_final, &Vector::new())? {
                println!("Saved Right Frame: {}", right_path);
                frame_count += 1;
                last_capture_time = now; // Reset timer
            }
        }
        
        // --- 5. Visual Feedback ---
        
        let mut display_l = frame_l_final.clone();
        let mut display_r = frame_r_final.clone();
        
        // Calculate remaining time for next capture
        let remaining = CAPTURE_INTERVAL_SECONDS.saturating_sub(elapsed);
        let (status_text, text_color) = if is_capture_ready {
            (format!("!!! CAPTURING VIEW #{} !!!", frame_count), 
             Scalar::new(0.0, 0.0, 255.0, 0.0)) // Red when capturing (BGR format)
        } else {
            (format!("NEXT CAPTURE IN: {}s", remaining), 
             Scalar::new(0.0, 255.0, 0.0, 0.0)) // Green countdown (BGR format)
        };
        
        // Draw status text on both frames
        imgproc::put_text(
            &mut display_l,
            &status_text,
            Point::new(10, 30),
            imgproc::FONT_HERSHEY_SIMPLEX,
            0.8,
            text_color.clone(),
            2,
            imgproc::LINE_8,
            false,
        )?;
        
        imgproc::put_text(
            &mut display_r,
            &status_text,
            Point::new(10, 30),
            imgproc::FONT_HERSHEY_SIMPLEX,
            0.8,
            text_color,
            2,
            imgproc::LINE_8,
            false,
        )?;

        // Display frames
        highgui::imshow("Combined Stereo Feed (GUIDE)", &frame_combined)?; // DISPLAY NEW WINDOW
        highgui::imshow("Left Camera Feed", &display_l)?;
        highgui::imshow("Right Camera Feed", &display_r)?;

        // Handle Exit Key (ESC key = 27)
        let key = highgui::wait_key(1)?;
        if key == 27 { 
            break; 
        }
    }

    Ok(())
}

fn main() {
    if let Err(e) = run_capture_app() {
        eprintln!("Application Error in rover_vo: {:?}", e);
    }
}
 

//=== RUN RESULT ============