//! The fixed 3-camera rig and the virtual camera steered between its
//! cameras.

use crate::camera::{Camera, CameraIntrinsics, CameraPose};

/// The physical rig: the Kinect (`center`, the depth source) flanked by
/// two webcams (`left`, `right`).
///
/// Extrinsics are known exactly from the offline calibration (see
/// PLAN.md); for the synthetic dataset they are simply declared.
#[derive(Debug, Clone, PartialEq)]
pub struct Rig {
    pub left: Camera,
    pub center: Camera,
    pub right: Camera,
}

impl Rig {
    /// Iterate the rig cameras in order `left, center, right`.
    pub fn cameras(&self) -> [Camera; 3] {
        [self.left.clone(), self.center.clone(), self.right.clone()]
    }

    /// The virtual camera pose at position `t` between the side cameras:
    /// `t = 0` is the left camera, `t = 1` the right camera.
    ///
    /// `t` is clamped to `[0, 1]`.
    pub fn virtual_pose(&self, t: f32) -> CameraPose {
        let t = t.clamp(0.0, 1.0);
        self.left.pose.blend(&self.right.pose, t)
    }

    /// The full virtual camera at `t`, using the given intrinsics and
    /// resolution (normally the webcams': the virtual camera *is* a
    /// webcam placed where no real one sits).
    pub fn virtual_camera(
        &self,
        t: f32,
        intrinsics: CameraIntrinsics,
        resolution: glam::UVec2,
    ) -> Camera {
        Camera {
            id: "virtual".into(),
            pose: self.virtual_pose(t),
            intrinsics,
            resolution,
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::camera::CameraIntrinsics;
    use glam::{uvec2, vec3};

    /// A symmetric converging rig, like the real one: side cameras
    /// `±baseline/2` apart, all looking at a subject 2 m out.
    fn symmetric_rig(baseline: f32) -> Rig {
        let intrinsics = CameraIntrinsics {
            fx: 500.0,
            fy: 500.0,
            cx: 320.0,
            cy: 240.0,
        };
        let make = |x: f32| Camera {
            id: "test".into(),
            pose: CameraPose::looking_at(vec3(x, 0.0, 0.0), vec3(0.0, 0.0, 2.0)),
            intrinsics,
            resolution: uvec2(640, 480),
        };
        Rig {
            left: make(-baseline / 2.0),
            center: make(0.0),
            right: make(baseline / 2.0),
        }
    }

    #[test]
    fn virtual_pose_hits_the_side_cameras() {
        let rig = symmetric_rig(0.6);
        assert_eq!(rig.virtual_pose(0.0), rig.left.pose);
        assert_eq!(rig.virtual_pose(1.0), rig.right.pose);
    }

    #[test]
    fn virtual_pose_midpoint_is_center_for_symmetric_rig() {
        let rig = symmetric_rig(0.6);
        let mid = rig.virtual_pose(0.5);
        assert!(mid.position.distance(rig.center.pose.position) < 1e-6);
        // Orientation within a degree of the center camera.
        let dot = mid
            .rotation
            .normalize()
            .dot(rig.center.pose.rotation.normalize());
        assert!(dot > (1.0_f32).cos().to_radians());
    }

    #[test]
    fn virtual_pose_moves_monotonically_along_baseline() {
        let rig = symmetric_rig(1.0);
        let mut last_x = f32::MIN;
        for i in 0..=10 {
            let t = i as f32 / 10.0;
            let x = rig.virtual_pose(t).position.x;
            assert!(x >= last_x, "t={t} moved backwards");
            last_x = x;
        }
    }

    #[test]
    fn virtual_pose_clamps_out_of_range() {
        let rig = symmetric_rig(0.6);
        assert_eq!(rig.virtual_pose(-2.0), rig.virtual_pose(0.0));
        assert_eq!(rig.virtual_pose(7.0), rig.virtual_pose(1.0));
    }

    #[test]
    fn virtual_camera_carries_intrinsics() {
        let rig = symmetric_rig(0.6);
        let intrinsics = CameraIntrinsics {
            fx: 600.0,
            fy: 600.0,
            cx: 320.0,
            cy: 240.0,
        };
        let cam = rig.virtual_camera(0.25, intrinsics, uvec2(640, 480));
        assert_eq!(cam.intrinsics, intrinsics);
        assert_eq!(cam.resolution, uvec2(640, 480));
        assert_eq!(cam.pose, rig.virtual_pose(0.25));
    }
}