/*
 *    This file is part of MotionPlus.
 *
 *    MotionPlus is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    MotionPlus is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with MotionPlus.  If not, see <https://www.gnu.org/licenses/>.
 *
 *    Copyright 2020-2023 MotionMrDave@gmail.com
 */

#include "motionplus.hpp"
#include "logger.hpp"
#include "util.hpp"
#include "jpegutils.hpp"

typedef struct {
    int is_abs;
    int len;
    int val;
} code_table;

/**
 * sonix_decompress_init
 *   pre-calculates a locally stored table for efficient huffman-decoding.
 *
 *   Each entry at index x in the table represents the codeword
 *   present at the MSB of byte x.
 *
 */
static void vid_sonix_decompress_init(code_table *table)
{
    int i;
    int is_abs, val, len;

    for (i = 0; i < 256; i++) {
        is_abs = 0;
        val = 0;
        len = 0;
        if ((i & 0x80) == 0) {
            /* code 0 */
            val = 0;
            len = 1;
        } else if ((i & 0xE0) == 0x80) {
            /* code 100 */
            val = +4;
            len = 3;
        } else if ((i & 0xE0) == 0xA0) {
            /* code 101 */
            val = -4;
            len = 3;
        } else if ((i & 0xF0) == 0xD0) {
            /* code 1101 */
            val = +11;
            len = 4;
        } else if ((i & 0xF0) == 0xF0) {
            /* code 1111 */
            val = -11;
            len = 4;
        } else if ((i & 0xF8) == 0xC8) {
            /* code 11001 */
            val = +20;
            len = 5;
        } else if ((i & 0xFC) == 0xC0) {
            /* code 110000 */
            val = -20;
            len = 6;
        } else if ((i & 0xFC) == 0xC4) {
            /* code 110001xx: unknown */
            val = 0;
            len = 8;
        } else if ((i & 0xF0) == 0xE0) {
            /* code 1110xxxx */
            is_abs = 1;
            val = (i & 0x0F) << 4;
            len = 8;
        }
        table[i].is_abs = is_abs;
        table[i].val = val;
        table[i].len = len;
    }
}

/**
 * sonix_decompress
 *      Decompresses an image encoded by a SN9C101 camera controller chip.
 *
 *   IN    width
 *         height
 *         inp     pointer to compressed frame (with header already stripped)
 *   OUT   outp    pointer to decompressed frame
 *
 *         Returns 0 if the operation was successful.
 *         Returns <0 if operation failed.
 *
 */
int vid_sonix_decompress(unsigned char *img_dst, unsigned char *img_src, int width, int height)
{
    int row, col;
    int val;
    int bitpos;
    unsigned char code;
    unsigned char *addr;

    /* Local storage */
    static code_table table[256];
    static int init_done = 0;

    if (!init_done) {
        init_done = 1;
        vid_sonix_decompress_init(table);
    }

    bitpos = 0;
    for (row = 0; row < height; row++) {

        col = 0;

        /* First two pixels in first two rows are stored as raw 8-bit. */
        if (row < 2) {
            addr = img_src + (bitpos >> 3);
            code =(unsigned char)( (addr[0] << (bitpos & 7)) |
                    (addr[1] >> (8 - (bitpos & 7))));
            bitpos += 8;
            *img_dst++ = code;

            addr = img_src + (bitpos >> 3);
            code = (unsigned char)((addr[0] << (bitpos & 7)) |
                    (addr[1] >> (8 - (bitpos & 7))));
            bitpos += 8;
            *img_dst++ = code;

            col += 2;
        }

        while (col < width) {
            /* Get bitcode from bitstream. */
            addr = img_src + (bitpos >> 3);
            code =(unsigned char)((addr[0] << (bitpos & 7)) |
                (addr[1] >> (8 - (bitpos & 7))));

            /* Update bit position. */
            bitpos += table[code].len;

            /* Calculate pixel value. */
            val = table[code].val;
            if (!table[code].is_abs) {
                /* Value is relative to top and left pixel. */
                if (col < 2) {
                    /* Left column: relative to top pixel. */
                    val += img_dst[-2 * width];
                } else if (row < 2) {
                    /* Top row: relative to left pixel. */
                    val += img_dst[-2];
                } else {
                    /* Main area: average of left pixel and top pixel. */
                    val += (img_dst[-2] + img_dst[-2 * width]) / 2;
                }
            }

            /* Store pixel */
            if (val < 0) {
                *img_dst++ = 0;
            } else if (val > 255) {
                *img_dst++ = 255;
            } else {
                *img_dst++ =(unsigned char)val;
            }
            col++;
        }
    }

    return 0;
}

/**
 * bayer2rgb24
 * BAYER2RGB24 ROUTINE TAKEN FROM:
 *
 * Sonix SN9C10x based webcam basic I/F routines
 * Takafumi Mizuno <taka-qce@ls-a.jp>
 *
 */
void vid_bayer2rgb24(unsigned char *img_dst, unsigned char *img_src, long int width, long int height)
{
    long int i;
    unsigned char *rawpt, *scanpt;
    long int size;

    rawpt = img_src;
    scanpt = img_dst;
    size = width * height;

    for (i = 0; i < size; i++) {
        if (((i / width) & 1) == 0) {
            if ((i & 1) == 0) {
                /* B */
                if ((i > width) && ((i % width) > 0)) {
                    *scanpt++ = (unsigned char)(*rawpt);        /* B */
                    *scanpt++ = (unsigned char)((*(rawpt - 1) +
                                *(rawpt + 1) +
                                *(rawpt + width) +
                                *(rawpt - width)) / 4);         /* G */
                    *scanpt++ = (unsigned char)((*(rawpt - width - 1) +
                                *(rawpt - width + 1) +
                                *(rawpt + width - 1) +
                                *(rawpt + width + 1)) / 4);     /* R */
                } else {
                    /* First line or left column. */
                    *scanpt++ = (unsigned char)(*rawpt);        /* B */
                    *scanpt++ = (unsigned char)((*(rawpt + 1) +
                                *(rawpt + width)) / 2);         /* G */
                    *scanpt++ = (unsigned char)(*(rawpt + width + 1));       /* R */
                }
            } else {
                /* (B)G */
                if ((i > width) && ((i % width) < (width - 1))) {
                    *scanpt++ = (unsigned char)((*(rawpt - 1) +
                                *(rawpt + 1)) / 2);             /* B */
                    *scanpt++ = (unsigned char)(*rawpt);        /* G */
                    *scanpt++ = (unsigned char)((*(rawpt + width) +
                                *(rawpt - width)) / 2);  /* R */
                } else {
                    /* First line or right column. */
                    *scanpt++ = (unsigned char)(*(rawpt - 1));      /* B */
                    *scanpt++ = (unsigned char)(*rawpt);            /* G */
                    *scanpt++ = (unsigned char)(*(rawpt + width));  /* R */
                }
            }
        } else {
            if ((i & 1) == 0) {
                /* G(R) */
                if ((i < (width * (height - 1))) && ((i % width) > 0)) {
                    *scanpt++ =(unsigned char)( (*(rawpt + width) +
                                *(rawpt - width)) / 2);                 /* B */
                    *scanpt++ =(unsigned char)( *rawpt);                /* G */
                    *scanpt++ =(unsigned char)( (*(rawpt - 1) +
                                *(rawpt + 1)) / 2);                     /* R */
                } else {
                    /* Bottom line or left column. */
                    *scanpt++ = (unsigned char)(*(rawpt - width));      /* B */
                    *scanpt++ = (unsigned char)(*rawpt);                /* G */
                    *scanpt++ = (unsigned char)(*(rawpt + 1));          /* R */
                }
            } else {
                /* R */
                if (i < (width * (height - 1)) && ((i % width) < (width - 1))) {
                    *scanpt++ = (unsigned char)( (*(rawpt - width - 1) +
                                *(rawpt - width + 1) +
                                *(rawpt + width - 1) +
                                *(rawpt + width + 1)) / 4);             /* B */
                    *scanpt++ = (unsigned char)((*(rawpt - 1) +
                                *(rawpt + 1) +
                                *(rawpt - width) +
                                *(rawpt + width)) / 4);                 /* G */
                    *scanpt++ = (unsigned char)(*rawpt);                /* R */
                } else {
                    /* Bottom line or right column. */
                    *scanpt++ = (unsigned char)(*(rawpt - width - 1));  /* B */
                    *scanpt++ = (unsigned char)((*(rawpt - 1) +
                                *(rawpt - width)) / 2);                 /* G */
                    *scanpt++ = (unsigned char)(*rawpt);                /* R */
                }
            }
        }
        rawpt++;
    }

}

void vid_yuv422to420p(unsigned char *img_dst, unsigned char *img_src, int width, int height)
{
    unsigned char *src, *dest, *src2, *dest2;
    int i, j;

    /* Create the Y plane. */
    src = img_src;
    dest = img_dst;
    for (i = width * height; i > 0; i--) {
        *dest++ = *src;
        src += 2;
    }
    /* Create U and V planes. */
    src = img_src + 1;
    src2 = img_src + width * 2 + 1;
    dest = img_dst + width * height;
    dest2 = dest + (width * height) / 4;
    for (i = height / 2; i > 0; i--) {
        for (j = width / 2; j > 0; j--) {
            *dest = (unsigned char)(((int) *src + (int) *src2) / 2);
            src += 2;
            src2 += 2;
            dest++;
            *dest2 = (unsigned char)(((int) *src + (int) *src2) / 2);
            src += 2;
            src2 += 2;
            dest2++;
        }
        src += width * 2;
        src2 += width * 2;
    }
}

void vid_yuv422pto420p(unsigned char *img_dst, unsigned char *img_src, int width, int height)
{
    unsigned char *src, *dest, *dest2;
    unsigned char *src_u, *src_u2, *src_v, *src_v2;

    int i, j;
    /*Planar version of 422 */
    /* Create the Y plane. */
    src = img_src;
    dest = img_dst;
    for (i = width * height; i > 0; i--) {
        *dest++ = *src++;
    }

    /* Create U and V planes. */
    dest = img_dst + width * height;
    dest2 = dest + (width * height) / 4;
    for (i = 0; i< (height / 2); i++) {
        src_u = img_src + (width * height) + ((i*2) * (width/2));
        src_u2 = src_u  + (width/2);
        src_v = src_u + (width/2 * height);
        src_v2 = src_v  + (width/2);

        for (j = 0; j < (width / 2); j++) {
            *dest = (unsigned char)(((int) *src_u + (int) *src_u2) / 2);
            src_u ++;
            src_u2++;
            dest++;

            *dest2 = (unsigned char)(((int) *src_v + (int) *src_v2) / 2);
            src_v ++;
            src_v2++;
            dest2++;
        }
    }
}

void vid_uyvyto420p(unsigned char *img_dst, unsigned char *img_src, int width, int height)
{
    unsigned char *pY = img_dst;
    unsigned char *pU = pY + (width * height);
    unsigned char *pV = pU + (width * height) / 4;
    unsigned int uv_offset = width * 2 * sizeof(unsigned char);
    int ix, jx;

    for (ix = 0; ix < height; ix++) {
        for (jx = 0; jx < width; jx += 2) {
            unsigned short int calc;

            if ((ix&1) == 0) {
                calc = *img_src;
                calc += *(img_src + uv_offset);
                calc /= 2;
                *pU++ = (unsigned char) calc;
            }

            img_src++;
            *pY++ = *img_src++;

            if ((ix&1) == 0) {
                calc = *img_src;
                calc += *(img_src + uv_offset);
                calc /= 2;
                *pV++ = (unsigned char) calc;
            }

            img_src++;
            *pY++ = *img_src++;
        }
    }
}

static void vid_rgb_bgr(unsigned char *img_dst, unsigned char *img_src
    , int width, int height, int rgb)
{
    unsigned char *y, *u, *v;
    unsigned char *r, *g, *b;
    int i, loop;

    if (rgb == 1) {
        r = img_src;
        g = r + 1;
        b = g + 1;
    } else {
        b = img_src;
        g = b + 1;
        r = g + 1;
    }

    y = img_dst;
    u = y + width * height;
    v = u + (width * height) / 4;
    memset(u, 0, width * height / 4);
    memset(v, 0, width * height / 4);

    for (loop = 0; loop < height; loop++) {
        for (i = 0; i < width; i += 2) {
            *y++ = (unsigned char)((9796 ** r + 19235 ** g + 3736 ** b) >> 15);
            *u += (unsigned char)(((-4784 ** r - 9437 ** g + 14221 ** b) >> 17) + 32);
            *v += (unsigned char)(((20218 ** r - 16941 ** g - 3277 ** b) >> 17) + 32);
            r += 3;
            g += 3;
            b += 3;
            *y++ = (unsigned char)((9796 ** r + 19235 ** g + 3736 ** b) >> 15);
            *u += (unsigned char)(((-4784 ** r - 9437 ** g + 14221 ** b) >> 17) + 32);
            *v += (unsigned char)(((20218 ** r - 16941 ** g - 3277 ** b) >> 17) + 32);
            r += 3;
            g += 3;
            b += 3;
            u++;
            v++;
        }

        if ((loop & 1) == 0) {
            u -= width / 2;
            v -= width / 2;
        }
    }
}

void vid_rgb24toyuv420p(unsigned char *img_dst, unsigned char *img_src
    , int width, int height)
{
    vid_rgb_bgr(img_dst, img_src, width, height, 1);
}

void vid_bgr24toyuv420p(unsigned char *img_dst, unsigned char *img_src
    , int width, int height)
{
    vid_rgb_bgr(img_dst, img_src, width, height, 0);
}

/**
 * mjpegtoyuv420p
 *
 * Return values
 *  -1 on fatal error
 *  0  on success
 *  2  if jpeg lib threw a "corrupt jpeg data" warning.
 *     in this case, "a damaged output image is likely."
 */
int vid_mjpegtoyuv420p(unsigned char *img_dst, unsigned char *img_src, int width, int height, unsigned int size)
{
    unsigned char *ptr_buffer;
    size_t soi_pos = 0;
    int ret = 0;

    ptr_buffer =(unsigned char*) memmem(img_src, size, "\xff\xd8", 2);
    if (ptr_buffer == NULL) {
        MOTPLS_LOG(CRT, TYPE_VIDEO, NO_ERRNO,_("Corrupt image ... continue"));
        return 1;
    }
    /**
     Some cameras are sending multiple SOIs in the buffer.
     Move the pointer to the last SOI in the buffer and proceed.
    */
    while (ptr_buffer != NULL && ((size - soi_pos - 1) > 2) ){
        soi_pos = ptr_buffer - img_src;
        ptr_buffer =(unsigned char*) memmem(img_src + soi_pos + 1, size - soi_pos - 1, "\xff\xd8", 2);
    }

    if (soi_pos != 0) {
        MOTPLS_LOG(INF, TYPE_VIDEO, NO_ERRNO,_("SOI position adjusted by %d bytes."), soi_pos);
    }

    memmove(img_src, img_src + soi_pos, size - soi_pos);
    size -= (unsigned int)soi_pos;

    ret = jpgutl_decode_jpeg(img_src,size, width, height, img_dst);

    if (ret == -1) {
        MOTPLS_LOG(CRT, TYPE_VIDEO, NO_ERRNO,_("Corrupt image ... continue"));
        ret = 1;
    }
    return ret;
}

void vid_y10torgb24(unsigned char *img_dst, unsigned char *img_src, int width, int height, int shift)
{
    /* Source code: raw2rgbpnm project */
    /* url: http://salottisipuli.retiisi.org.uk/cgi-bin/gitweb.cgi?p=~sailus/raw2rgbpnm.git;a=summary */

    /* bpp - bits per pixel */
    /* bpp: 'Pixels are stored in 16-bit words with unused high bits padded with 0' */
    /* url: https://linuxtv.org/downloads/v4l-dvb-apis/V4L2-PIX-FMT-Y12.html */
    /* url: https://linuxtv.org/downloads/v4l-dvb-apis/V4L2-PIX-FMT-Y10.html */

    int src_size[2] = {width,height};
    int bpp = 16;
    unsigned int src_stride = (src_size[0] * bpp) / 8;
    unsigned int rgb_stride = src_size[0] * 3;
    int a = 0;
    int src_x = 0, src_y = 0;
    int dst_x = 0, dst_y = 0;

    for (src_y = 0, dst_y = 0; dst_y < src_size[1]; src_y++, dst_y++) {
        for (src_x = 0, dst_x = 0; dst_x < src_size[0]; src_x++, dst_x++) {
            a = (img_src[src_y*src_stride + src_x*2+0] |
                (img_src[src_y*src_stride + src_x*2+1] << 8)) >> shift;
            img_dst[dst_y*rgb_stride+3*dst_x+0] = (unsigned char)a;
            img_dst[dst_y*rgb_stride+3*dst_x+1] = (unsigned char)a;
            img_dst[dst_y*rgb_stride+3*dst_x+2] = (unsigned char)a;
        }
    }
}

void vid_greytoyuv420p(unsigned char *img_dst, unsigned char *img_src, int width, int height)
{

    memcpy(img_dst, img_src, (width*height));
    memset(img_dst+(width*height), 128, (width * height) / 2);

}