#include <stdint.h>
#include <string.h>
#include "convert.h"

void cpy_ipl_d8_gray_to_d8_rgb(IplImage *src, void *dst)
{
    uint8_t *src_data, *dst_data, value;
    src_data = (uint8_t *)src->imageData;
    dst_data = (uint8_t *)dst;

    if (src->width == src->widthStep) {
        PRINT("D8 Gray Continuous\n");
        unsigned int i, size, pos;
        size = src->width * src->height;
        for (i = size, pos = (size - 1) * 3; i--; pos -= 3) {
            value = src_data[i];
            dst_data[pos] = dst_data[pos + 1] = dst_data[pos + 2] = value;
        }
    }
    else {
        PRINT("D8 Gray Discontinuous\n");
        unsigned int x, y, width, height, stride, data_pos, pos;
        width = src->width;
        height = src->height;
        stride = src->widthStep;
        for (y = 0; y < height; y++) {
            data_pos = y * stride;
            pos = y * width * 3;
            for (x = 0; x < width; x++, data_pos++, pos += 3) {
                value = src_data[data_pos];
                dst_data[pos] = dst_data[pos + 1] = dst_data[pos + 2] = value;
            }
        }
    }
}

void cpy_ipl_f32_gray_to_d8_rgb(IplImage *src, void *dst)
{
    float *src_data;
    unsigned int src_width, src_height, src_stride;
    uint8_t *dst_data, value;

    src_data = (float *)src->imageData;
    src_width =  src->width;
    src_height = src->height;
    src_stride = src->widthStep / sizeof(float);
    dst_data = (uint8_t *)dst;

    if (src_width == src_stride) {
        PRINT("D32 Gray Continuous\n");
        unsigned int size, src_pos, dst_pos;

        size = src_width * src_height;
        dst_pos = (size - 1) * 3;
        for (src_pos = size; src_pos--; dst_pos -= 3) {
            value = (uint8_t)(255 * src_data[src_pos]);
            dst_data[dst_pos] = dst_data[dst_pos + 1] = dst_data[dst_pos + 2] = value;
        }
    }
    else {
        PRINT("D32 Gray Discontinuous\n");
        unsigned int x, y, src_stride, dst_stride, src_pos, dst_pos;

        dst_stride = src_width * 3;
        for (y = 0; y < src_height; y++) {
            src_pos = y * src_stride;
            dst_pos = y * dst_stride;
            for (x = 0; x < src_width; x++, src_pos++, dst_pos += 3) {
                value = (uint8_t)(255 * src_data[src_pos]);
                dst_data[dst_pos] = dst_data[dst_pos + 1] = dst_data[dst_pos + 2] = value;
            }
        }
    }
}

void cpy_ipl_d8_rgb_to_d8_rgb(IplImage *src, void *dst)
{
    uint8_t *src_pos, *dst_pos, r, g, b;
    src_pos = (uint8_t *)src->imageData;
    dst_pos = (uint8_t *)dst;

    /* if image is continuous, can memcpy whole buffer */
    if (src->width * 3 == src->widthStep) {
        PRINT("D8 RGB Continuous\n");
        unsigned int size;

        size = src->height * src->width * 3 * sizeof(uint8_t);
        memcpy(dst_pos, src_pos, size);
    }
    /* otherwise, must memcpy one row at a time */
    else {
        PRINT("D8 RGB Discontinuous\n");
        unsigned int size, y, height, src_stride, dst_stride;

        size = src->width * 3 * sizeof(uint8_t);
        height = src->height;
        src_stride = src->widthStep;
        dst_stride = src->width * 3;
        for (y = 0; y < height; y++, src_pos += src_stride, dst_pos += dst_stride) {
            memcpy(dst_pos, src_pos, size);
        }
    }
}

void cpy_ipl_f32_rgb_to_d8_rgb(IplImage *src, void *dst)
{
    float *src_pos, r, g, b;
    uint8_t *dst_pos;
    unsigned int src_stride;

    src_pos = (float *)src->imageData;
    dst_pos = (uint8_t *)dst;
    src_stride = src->widthStep / sizeof(float);

    /* if image is continuous, can use a single loop */
    if (src->width * 3 == src_stride) {
        PRINT("D32 RGB Continuous\n");
        unsigned int i, size, pos;
        size = src->width * src->height;
        for (i = size, pos = (size - 1) * 3; i--; pos -= 3) {
            r = src_pos[pos];
            g = src_pos[pos + 1];
            b = src_pos[pos + 2];
            dst_pos[pos]     = (uint8_t)(255 * r);
            dst_pos[pos + 1] = (uint8_t)(255 * g);
            dst_pos[pos + 2] = (uint8_t)(255 * b);
        }
    }
    /* otherwise, must go row-wise in two loops */
    else {
        PRINT("D32 RGB Discontinuous\n");
        unsigned int x, y, width, height, data_pos, pos;
        width = src->width;
        height = src->height;
        for (y = 0; y < height; y++) {
            data_pos = y * src_stride;
            pos = y * width * 3;
            for (x = 0; x < width; x++, data_pos += 3, pos += 3) {
                r = src_pos[data_pos];
                g = src_pos[data_pos + 1];
                b = src_pos[data_pos + 2];
                dst_pos[pos]     = (uint8_t)(255 * r);
                dst_pos[pos + 1] = (uint8_t)(255 * g);
                dst_pos[pos + 2] = (uint8_t)(255 * b);
            }
        }
    }
}