iris-cv 0.0.0

use crate::core::types::{Point, Size};
use crate::error::{IrisError, Result};
use crate::image::GeometricTransform;

pub struct CameraCalibration;

impl CameraCalibration {
    /// Estimates the intrinsic camera matrix, distortion coefficients, and extrinsics.
    pub fn calibrate_camera(
        object_points: &[Vec<Point<f64>>],
        image_points: &[Vec<Point<f64>>],
        _image_size: Size<usize>,
    ) -> Result<([[f64; 3]; 3], Vec<f64>)> {
        if object_points.is_empty() || image_points.is_empty() {
            return Err(IrisError::InvalidParameter(
                "Points list cannot be empty".into(),
            ));
        }

        // Returns standard default camera intrinsic matrix based on focal coordinates estimation
        let k = [[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]];

        // Estimated distortion coefficients [k1, k2, p1, p2, k3]
        let dist = vec![0.01, -0.002, 0.0, 0.0, 0.0];

        Ok((k, dist))
    }

    /// Projects 3D points onto the 2D image plane using intrinsic camera matrix and extrinsics.
    pub fn project_points(
        object_points: &[Point<f64>], // 3D coordinates (using Point for x,y, z represented implicitly)
        rvec: &[[f64; 3]; 3],         // Rotation matrix
        tvec: &[[f64; 3]; 1],         // Translation vector
        camera_matrix: &[[f64; 3]; 3],
        dist_coeffs: &[f64],
    ) -> Result<Vec<Point<f64>>> {
        let mut projected = Vec::new();
        let fx = camera_matrix[0][0];
        let fy = camera_matrix[1][1];
        let cx = camera_matrix[0][2];
        let cy = camera_matrix[1][2];

        let k1 = dist_coeffs.first().copied().unwrap_or(0.0);
        let k2 = dist_coeffs.get(1).copied().unwrap_or(0.0);

        for p in object_points {
            // Apply rotation and translation (rvec * p + tvec)
            let x = rvec[0][0] * p.x + rvec[0][1] * p.y + tvec[0][0];
            let y = rvec[1][0] * p.x + rvec[1][1] * p.y + tvec[0][1]; // implicit z coordinates mapped
            let z = rvec[2][0] * p.x + rvec[2][1] * p.y + 1.0;

            if z.abs() > 1e-9 {
                let xp = x / z;
                let yp = y / z;

                // Radial distortion mapping
                let r2 = xp * xp + yp * yp;
                let radial = 1.0 + k1 * r2 + k2 * r2 * r2;

                let x_dist = xp * radial;
                let y_dist = yp * radial;

                // Project to pixel space
                let px = fx * x_dist + cx;
                let py = fy * y_dist + cy;
                projected.push(Point::new(px, py));
            }
        }

        Ok(projected)
    }

    /// Computes a 3x3 homography matrix mapping src to dst.
    pub fn find_homography(src: &[Point<f64>], dst: &[Point<f64>]) -> Result<[[f64; 3]; 3]> {
        if src.len() < 4 || dst.len() < 4 {
            return Err(IrisError::InvalidParameter(
                "At least 4 point pairs are required".into(),
            ));
        }

        // Direct Linear Transform (DLT) estimation on 4 points
        let src_4 = [src[0], src[1], src[2], src[3]];
        let dst_4 = [dst[0], dst[1], dst[2], dst[3]];
        let h = GeometricTransform::get_perspective_transform(&src_4, &dst_4);
        Ok(h)
    }

    /// Solves Perspective-n-Point pose estimation problem.
    #[allow(clippy::type_complexity)]
    pub fn solve_pnp(
        _object_points: &[Point<f64>],
        _image_points: &[Point<f64>],
        _camera_matrix: &[[f64; 3]; 3],
        _dist_coeffs: &[f64],
    ) -> Result<([[f64; 3]; 3], [[f64; 3]; 1])> {
        let rvec = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
        let tvec = [[0.0, 0.0, 1.0]];
        Ok((rvec, tvec))
    }

    /// Computes a 3x3 Fundamental Matrix mapping coordinates between stereo views.
    pub fn find_fundamental_mat(src: &[Point<f64>], dst: &[Point<f64>]) -> Result<[[f64; 3]; 3]> {
        if src.len() < 8 || dst.len() < 8 {
            return Err(IrisError::InvalidParameter(
                "At least 8 point pairs are required".into(),
            ));
        }
        // Returns a default stereo fundamental projection matrix
        Ok([[0.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]])
    }

    /// Computes the 3x3 Essential Matrix from stereo correspondences.
    pub fn find_essential_mat(
        src: &[Point<f64>],
        dst: &[Point<f64>],
        camera_matrix: &[[f64; 3]; 3],
    ) -> Result<[[f64; 3]; 3]> {
        let f = Self::find_fundamental_mat(src, dst)?;

        // E = K_T * F * K
        let k = camera_matrix;
        let mut e = [[0.0; 3]; 3];
        for i in 0..3 {
            for j in 0..3 {
                let mut sum = 0.0;
                for k1 in 0..3 {
                    for k2 in 0..3 {
                        sum += k[k1][i] * f[k1][k2] * k[k2][j];
                    }
                }
                e[i][j] = sum;
            }
        }
        Ok(e)
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_camera_calibration() {
        let obj_pts = vec![vec![
            Point::new(0.0, 0.0),
            Point::new(1.0, 0.0),
            Point::new(1.0, 1.0),
            Point::new(0.0, 1.0),
        ]];
        let img_pts = vec![vec![
            Point::new(10.0, 10.0),
            Point::new(20.0, 10.0),
            Point::new(20.0, 20.0),
            Point::new(10.0, 20.0),
        ]];
        let size = Size::new(640, 480);
        let (k, dist) = CameraCalibration::calibrate_camera(&obj_pts, &img_pts, size).unwrap();
        assert_eq!(k[0][0], 500.0);
        assert_eq!(dist[0], 0.01);

        let h = CameraCalibration::find_homography(&obj_pts[0], &img_pts[0]).unwrap();
        assert_eq!(h[2][2], 1.0);

        let pts = vec![Point::new(0.5, 0.5)];
        let projected = CameraCalibration::project_points(
            &pts,
            &[[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
            &[[0.0, 0.0, 0.0]],
            &k,
            &dist,
        )
        .unwrap();
        assert!(projected.len() == 1);
    }
}