Internal colorspace conversion routine

3 files changed, 155 insertions, 37 deletions

diff --git a/src/libsstv.template.h b/src/libsstv.template.h
index 76c50ee..ce4cf26 100644
--- a/src/libsstv.template.h
+++ b/src/libsstv.template.h
@@ -43,6 +43,7 @@ typedef enum {
     SSTV_BAD_FORMAT             = 105,
     SSTV_BAD_RESOLUTION         = 106,
     SSTV_BAD_SAMPLE_TYPE        = 107,
+    SSTV_UNSUPPORTED_CONVERSION = 108,
 
     SSTV_ALLOC_FAIL             = 200,
 
@@ -178,6 +179,28 @@ extern sstv_error_t sstv_create_image_from_mode(sstv_image_t *out_img, sstv_mode
 extern sstv_error_t sstv_create_image_from_props(sstv_image_t *out_img, size_t w, size_t h, sstv_image_format_t format);
 
 /*
+ * Deletes an image.
+ *   img(in): pointer to an image structure to delete
+ *   returns: error code
+ *
+ * NOTE: This function deallocates the pixel buffer, so it requires a valid
+ * call to sstv_init().
+ */
+extern sstv_error_t sstv_delete_image(sstv_image_t *img);
+
+/*
+ * Converts an image.
+ *   img(in): pointer to an image structure
+ *   format(in): format to convert image to
+ *   returns: error code
+ *
+ * NOTE: Conversions _from_ SSTV_FORMAT_Y to any format are NOT supported,
+ * since the conversion is performed in-place and extra memory would be
+ * required.
+ */
+extern sstv_error_t sstv_convert_image(sstv_image_t *img, sstv_image_format_t format);
+
+/*
  * Pack an image into an image structure, given properties and buffer.
  *   out_img(out): pointer to an image structure to initialize
  *   width(in): width
@@ -192,16 +215,6 @@ extern sstv_error_t sstv_create_image_from_props(sstv_image_t *out_img, size_t w
 extern sstv_error_t sstv_pack_image(sstv_image_t *out_img, size_t width, size_t height, sstv_image_format_t format, uint8_t *buffer);
 
 /*
- * Deletes an image.
- *   img(img): pointer to an image structure to delete
- *   returns: error code
- *
- * NOTE: This function deallocates the pixel buffer, so it requires a valid
- * call to sstv_init().
- */
-extern sstv_error_t sstv_delete_image(sstv_image_t *img);
-
-/*
  * Pack a signal buffer into a signal structure.
  *   sig(in): signal structure to initialize
  *   type(in): sample type
diff --git a/src/sstv.c b/src/sstv.c
index 07c7123..3365bee 100644
--- a/src/sstv.c
+++ b/src/sstv.c
@@ -6,6 +6,20 @@
  */
 
 #include "libsstv.h"
+#include "sstv.h"
+
+/*
+ * Colorspace conversion macros
+ * Kudos to Leszek Szary
+ * https://stackoverflow.com/questions/1737726/how-to-perform-rgb-yuv-conversion-in-c-c
+ */
+#define CLIP(X) ( (X) > 255 ? 255 : (X) < 0 ? 0 : X)
+#define CRGB2Y(R, G, B) CLIP((19595 * R + 38470 * G + 7471 * B ) >> 16)
+#define CRGB2Cb(R, G, B) CLIP((36962 * (B - CLIP((19595 * R + 38470 * G + 7471 * B ) >> 16) ) >> 16) + 128)
+#define CRGB2Cr(R, G, B) CLIP((46727 * (R - CLIP((19595 * R + 38470 * G + 7471 * B ) >> 16) ) >> 16) + 128)
+#define CYCbCr2R(Y, Cb, Cr) CLIP( Y + ( 91881 * Cr >> 16 ) - 179 )
+#define CYCbCr2G(Y, Cb, Cr) CLIP( Y - (( 22544 * Cb + 46793 * Cr ) >> 16) + 135)
+#define CYCbCr2B(Y, Cb, Cr) CLIP( Y + (116129 * Cb >> 16 ) - 226 )
 
 sstv_malloc_t sstv_malloc_user = NULL;
 sstv_free_t sstv_free_user = NULL;
@@ -25,6 +39,114 @@ sstv_init(sstv_malloc_t alloc_func, sstv_free_t dealloc_func)
 }
 
 sstv_error_t
+sstv_convert_image(sstv_image_t *img, sstv_image_format_t format)
+{
+    if (!img) {
+        return SSTV_BAD_PARAMETER;
+    }
+
+    if (img->format == format) {
+        /* no conversion necessary */
+        return SSTV_OK;
+    }
+
+    if (img->format == SSTV_FORMAT_Y) {
+        /* can't convert from grayscale to anything */
+        return SSTV_UNSUPPORTED_CONVERSION;
+    }
+
+    /* perform conversion */
+    uint32_t num_px = img->width * img->height;
+    uint32_t i;
+    switch (format) {
+        case SSTV_FORMAT_Y:
+            {
+                switch (img->format) {
+                    case SSTV_FORMAT_Y:
+                        /* identity, do nothing */
+                        break;
+                    
+                    case SSTV_FORMAT_YCBCR:
+                        /* condense first channel */
+                        for (i = 0; i < num_px; i ++) {
+                            img->buffer[i] = img->buffer[i * 3];
+                        }
+                        break;
+                    
+                    case SSTV_FORMAT_RGB:
+                        /* extract Y from RGB and set as first channel */
+                        for (i = 0; i < num_px; i ++) {
+                            int32_t r = img->buffer[i * 3 + 0];
+                            int32_t g = img->buffer[i * 3 + 1];
+                            int32_t b = img->buffer[i * 3 + 2];
+                            img->buffer[i] = CRGB2Y(r, g, b);
+                        }
+                        break;
+
+                    default:
+                        return SSTV_UNSUPPORTED_CONVERSION;
+                }
+            }
+            break;
+        
+        case SSTV_FORMAT_YCBCR:
+            {
+                switch (img->format) {
+                    case SSTV_FORMAT_YCBCR:
+                        /* identity, do nothing */
+                        break;
+                    
+                    case SSTV_FORMAT_RGB:
+                        for (i = 0; i < num_px; i ++) {
+                            int32_t r = img->buffer[i * 3 + 0];
+                            int32_t g = img->buffer[i * 3 + 1];
+                            int32_t b = img->buffer[i * 3 + 2];
+                            img->buffer[i * 3 + 0] = CRGB2Y(r, g, b);
+                            img->buffer[i * 3 + 1] = CRGB2Cb(r, g, b);
+                            img->buffer[i * 3 + 2] = CRGB2Cr(r, g, b);
+                        }
+                        break;
+
+                    default:
+                        return SSTV_UNSUPPORTED_CONVERSION;
+                }
+            }
+            break;
+        
+        case SSTV_FORMAT_RGB:
+            {
+                switch (img->format) {
+                    case SSTV_FORMAT_YCBCR:
+                        for (i = 0; i < num_px; i ++) {
+                            int32_t y = img->buffer[i * 3 + 0];
+                            int32_t b = img->buffer[i * 3 + 1];
+                            int32_t r = img->buffer[i * 3 + 2];
+                            img->buffer[i * 3 + 0] = CYCbCr2R(y, b ,r);
+                            img->buffer[i * 3 + 1] = CYCbCr2G(y, b ,r);
+                            img->buffer[i * 3 + 2] = CYCbCr2B(y, b ,r);
+                        }
+                        break;
+                    
+                    case SSTV_FORMAT_RGB:
+                        /* identity, do nothing */
+                        break;
+
+                    default:
+                        return SSTV_UNSUPPORTED_CONVERSION;
+                }
+            }
+            break;
+
+        default:
+            return SSTV_BAD_FORMAT;
+    }
+
+    /* all ok */
+    img->format = format;
+    return SSTV_OK;
+}
+
+sstv_error_t
 sstv_get_mode_image_props(sstv_mode_t mode, size_t *width, size_t *height, sstv_image_format_t *format)
 {
     switch (mode) {
diff --git a/src/tools/sstv-encode.cpp b/src/tools/sstv-encode.cpp
index 51b5a33..48c6623 100644
--- a/src/tools/sstv-encode.cpp
+++ b/src/tools/sstv-encode.cpp
@@ -61,7 +61,6 @@ int main(int argc, char **argv)
     LOG(INFO) << "Loading image from " << FLAGS_input;
 
     Magick::Image image;
-    std::string map = "";
     uint8_t *image_buffer = NULL;
 
     try {
@@ -73,42 +72,26 @@ int main(int argc, char **argv)
         Magick::Geometry nsize(width, height);
         nsize.aspect(true);
         image.scale(nsize);
-
-        /* format */
-        //format = SSTV_FORMAT_Y;
-        switch (format) {
-            case SSTV_FORMAT_Y:
-                map = "R";
-                image.magick("Y");
-                break;
-
-            case SSTV_FORMAT_YCBCR:
-                map = "RGB";
-                image.colorSpace(Magick::YCbCrColorspace);
-                break;
-
-            case SSTV_FORMAT_RGB:
-                map = "RGB";
-                image.colorSpace(Magick::RGBColorspace);
-                break;
-
-            default:
-                LOG(FATAL) << "Unknown pixel format";
-                break;
-        }
     } catch (int e) {
         LOG(FATAL) << "Magick++ failed";
     }
 
-    /* get raw */
-    Magick::PixelData blob(image, map, Magick::CharPixel);
+    /* get raw RGB (and convert it if necessary) */
+    image.colorSpace(Magick::RGBColorspace);
+    Magick::PixelData blob(image, "RGB", Magick::CharPixel);
     image_buffer = (uint8_t *)blob.data();
 
     sstv_image_t sstv_image;
-    if (sstv_pack_image(&sstv_image, width, height, format, image_buffer) != SSTV_OK) {
+    if (sstv_pack_image(&sstv_image, width, height, SSTV_FORMAT_RGB, image_buffer) != SSTV_OK) {
+        LOG(INFO) << image_buffer;
         LOG(FATAL) << "sstv_pack_image() failed";
     }
 
+    /* convert to mode's colorspace */
+    if (sstv_convert_image(&sstv_image, format) != SSTV_OK) {
+        LOG(FATAL) << "sstv_convert_image() failed";
+    }
+
     /* create a sample buffer for output */
     int16_t samp_buffer[128 * 1024];
     sstv_signal_t signal;