1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118

use std::{ffi::CString, slice::from_raw_parts};

use anyhow::bail;
use webots_bindings::{
    wb_camera_disable, wb_camera_enable, wb_camera_get_exposure, wb_camera_get_focal_distance,
    wb_camera_get_focal_length, wb_camera_get_fov, wb_camera_get_height, wb_camera_get_image,
    wb_camera_get_max_focal_distance, wb_camera_get_max_fov, wb_camera_get_min_focal_distance,
    wb_camera_get_min_fov, wb_camera_get_near, wb_camera_get_sampling_period, wb_camera_get_width,
    wb_camera_has_recognition, wb_camera_save_image, wb_camera_set_exposure,
    wb_camera_set_focal_distance, wb_camera_set_fov, wb_device_get_node_type, WbDeviceTag,
    WbNodeType_WB_NODE_CAMERA,
};

use crate::Recognition;

pub struct Camera(WbDeviceTag);

impl Camera {
    pub(crate) fn new(device: WbDeviceTag) -> Self {
        assert_eq!(WbNodeType_WB_NODE_CAMERA, unsafe {
            wb_device_get_node_type(device)
        });
        Self(device)
    }

    pub fn enable(&self, sampling_period: i32) {
        unsafe { wb_camera_enable(self.0, sampling_period) }
    }

    pub fn disable(&self) {
        unsafe { wb_camera_disable(self.0) }
    }

    pub fn get_sampling_period(&self) -> i32 {
        unsafe { wb_camera_get_sampling_period(self.0) }
    }

    pub fn get_image(&self) -> anyhow::Result<&[u8]> {
        let width = self.get_width();
        let height = self.get_height();
        unsafe {
            let image = wb_camera_get_image(self.0);
            if image.is_null() {
                bail!("Failed to get image: image data is NULL");
            }
            Ok(from_raw_parts(image, (width * height * 4) as usize))
        }
    }

    pub fn get_width(&self) -> i32 {
        unsafe { wb_camera_get_width(self.0) }
    }

    pub fn get_height(&self) -> i32 {
        unsafe { wb_camera_get_height(self.0) }
    }

    pub fn get_fov(&self) -> f64 {
        unsafe { wb_camera_get_fov(self.0) }
    }

    pub fn get_max_fov(&self) -> f64 {
        unsafe { wb_camera_get_max_fov(self.0) }
    }

    pub fn get_min_fov(&self) -> f64 {
        unsafe { wb_camera_get_min_fov(self.0) }
    }

    pub fn set_fov(&self, fov: f64) {
        unsafe { wb_camera_set_fov(self.0, fov) }
    }

    pub fn get_exposure(&self) -> f64 {
        unsafe { wb_camera_get_exposure(self.0) }
    }

    pub fn set_exposure(&self, exposure: f64) {
        unsafe { wb_camera_set_exposure(self.0, exposure) }
    }

    pub fn get_focal_length(&self) -> f64 {
        unsafe { wb_camera_get_focal_length(self.0) }
    }

    pub fn get_focal_distance(&self) -> f64 {
        unsafe { wb_camera_get_focal_distance(self.0) }
    }

    pub fn get_max_focal_distance(&self) -> f64 {
        unsafe { wb_camera_get_max_focal_distance(self.0) }
    }

    pub fn get_min_focal_distance(&self) -> f64 {
        unsafe { wb_camera_get_min_focal_distance(self.0) }
    }

    pub fn set_focal_distance(&self, focal_distance: f64) {
        unsafe { wb_camera_set_focal_distance(self.0, focal_distance) }
    }

    pub fn get_near(&self) -> f64 {
        unsafe { wb_camera_get_near(self.0) }
    }

    pub fn save_image(&self, filename: &str, quality: i32) -> i32 {
        let filename = CString::new(filename).expect("CString::new failed");
        unsafe { wb_camera_save_image(self.0, filename.as_ptr(), quality) }
    }

    pub fn has_recognition(&self) -> bool {
        unsafe { wb_camera_has_recognition(self.0) != 0 }
    }

    pub fn get_recognition(&self) -> Recognition {
        Recognition::new(self.0)
    }
}