From 52fbe25154e4fcff368ce3b746ba69920167fba8 Mon Sep 17 00:00:00 2001
From: Brandon Thomas <echelon@gmail.com>
Date: Mon, 13 Apr 2020 05:09:08 -0400
Subject: [PATCH] streaming color camera

---
 src/graphics_gl.rs     | 14 +++++++-------
 src/k4a_sys_wrapper.rs | 43 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 50 insertions(+), 7 deletions(-)

diff --git a/src/graphics_gl.rs b/src/graphics_gl.rs
index 838a2a2eba..846d677ede 100644
--- a/src/graphics_gl.rs
+++ b/src/graphics_gl.rs
@@ -57,7 +57,8 @@ void main() {
     //out_color = vec4(1.0, 1.0, 1.0, 1.0);
     //out_color = vec4(triangleColor, 1.0); // Uniform
     //out_color = vec4(Color, 1.0);
-    out_color = texture(tex, Texcoord) * vec4(Color, 1.0);
+    //out_color = texture(tex, Texcoord) * vec4(Color, 1.0);
+    out_color = texture(tex, Texcoord);
 }";
 
 // Vertex data
@@ -290,27 +291,26 @@ pub fn run(capture_provider: Arc<CaptureProvider>) {
 
     // TODO: This belongs in a worker thread with buffers on both producer and consumer.
     if let Some(capture) = capture_provider.get_capture() {
-      if let Ok(image) = capture.get_depth_image() {
+      if let Ok(image) = capture.get_color_image() {
         let width = image.get_width_pixels() as i32;
         let height = image.get_height_pixels() as i32;
         println!("Size: {}x{}", width, height);
+
+        let format = image.get_format();
+        println!("format: {:?}", format);
+
         let buffer = image.get_buffer();
 
-        //let img_ptr: *const c_void = raw_img.as_ptr() as *const c_void;
-
-
         unsafe {
           gl::BindTexture(gl::TEXTURE_2D, tex);
           gl::TexImage2D(
             gl::TEXTURE_2D,
             0,
             gl::RGBA as i32,
-            //format as i32,
             width,
             height,
             0,
             gl::RGBA,
-            //format,
             gl::UNSIGNED_BYTE,
             buffer as *const c_void,
           );
diff --git a/src/k4a_sys_wrapper.rs b/src/k4a_sys_wrapper.rs
index 4dd7073149..3785c587f2 100644
--- a/src/k4a_sys_wrapper.rs
+++ b/src/k4a_sys_wrapper.rs
@@ -6,6 +6,8 @@ use std::ffi::{CStr, CString};
 use std::ptr;
 
 use k4a_sys;
+use k4a_sys_wrapper::ImageFormat::ColorMjpg;
+use glutin::platform::unix::x11::ffi::IconMaskHint;
 
 pub fn device_get_installed_count() -> u32 {
   unsafe {
@@ -261,6 +263,47 @@ impl Image {
       k4a_sys::k4a_image_get_buffer(self.0)
     }
   }
+
+  /// Use this function to determine the format of the image buffer.
+  /// This function is not expected to fail, all k4a_image_t's are created with a
+  /// known format. If the image_handle is invalid, the function will return
+  /// K4A_IMAGE_FORMAT_CUSTOM.
+  pub fn get_format(&self) -> ImageFormat {
+    unsafe {
+      let format = k4a_sys::k4a_image_get_format(self.0);
+      match format {
+        0 => ImageFormat::ColorMjpg,
+        1 => ImageFormat::ColorNv12,
+        2 => ImageFormat::ColorYuy2,
+        3 => ImageFormat::ColorBgra32,
+        4 => ImageFormat::Depth16,
+        5 => ImageFormat::Ir16,
+        6 => ImageFormat::Custom8,
+        7 => ImageFormat::Custom16,
+        8 => ImageFormat::Custom,
+        _ => ImageFormat::UnknownFormatError,
+      }
+    }
+  }
+}
+
+#[derive(Debug,Clone,Copy)]
+pub enum ImageFormat {
+  ColorMjpg,
+  ColorNv12,
+  ColorYuy2,
+  ColorBgra32,
+  /// Depth image type DEPTH16.
+  /// Each pixel of DEPTH16 data is two bytes of little endian unsigned depth data.
+  /// The unit of the data is in millimeters from the origin of the camera.
+  /// Stride indicates the length of each line in bytes and should be used to determine
+  /// the start location of each line of the image in memory.
+  Depth16,
+  Ir16,
+  Custom8,
+  Custom16,
+  Custom,
+  UnknownFormatError, // FIXME: Just return Result<T>?
 }
 
 /// Deallocate open device handles