/*	Code borrowed from DOSBox and adapted by OBattler.
        Original author: reenigne. */

#include <stdlib.h>
#include <stdint.h>
#include <math.h>

#include "ibm.h"
#include "device.h"
#include "mem.h"
#include "vid_cga.h"
#include "dosbox/vid_cga_comp.h"

int CGA_Composite_Table[1024];

static double brightness = 0;
static double contrast = 100;
static double saturation = 100;
static double sharpness = 0;
static double hue_offset = 0;

// New algorithm by reenigne
// Works in all CGA modes/color settings and can simulate older and newer CGA revisions

static const double tau = 6.28318531; // == 2*pi

static unsigned char chroma_multiplexer[256] = {
        2,   2,   2,   2,   114, 174, 4,   3,   2,   1,   133, 135, 2,   113, 150, 4,   133, 2,  1,   99,  151, 152, 2,   1,
        3,   2,   96,  136, 151, 152, 151, 152, 2,   56,  62,  4,   111, 250, 118, 4,   0,   51, 207, 137, 1,   171, 209, 5,
        140, 50,  54,  100, 133, 202, 57,  4,   2,   50,  153, 149, 128, 198, 198, 135, 32,  1,  36,  81,  147, 158, 1,   42,
        33,  1,   210, 254, 34,  109, 169, 77,  177, 2,   0,   165, 189, 154, 3,   44,  33,  0,  91,  197, 178, 142, 144, 192,
        4,   2,   61,  67,  117, 151, 112, 83,  4,   0,   249, 255, 3,   107, 249, 117, 147, 1,  50,  162, 143, 141, 52,  54,
        3,   0,   145, 206, 124, 123, 192, 193, 72,  78,  2,   0,   159, 208, 4,   0,   53,  58, 164, 159, 37,  159, 171, 1,
        248, 117, 4,   98,  212, 218, 5,   2,   54,  59,  93,  121, 176, 181, 134, 130, 1,   61, 31,  0,   160, 255, 34,  1,
        1,   58,  197, 166, 0,   177, 194, 2,   162, 111, 34,  96,  205, 253, 32,  1,   1,   57, 123, 125, 119, 188, 150, 112,
        78,  4,   0,   75,  166, 180, 20,  38,  78,  1,   143, 246, 42,  113, 156, 37,  252, 4,  1,   188, 175, 129, 1,   37,
        118, 4,   88,  249, 202, 150, 145, 200, 61,  59,  60,  60,  228, 252, 117, 77,  60,  58, 248, 251, 81,  212, 254, 107,
        198, 59,  58,  169, 250, 251, 81,  80,  100, 58,  154, 250, 251, 252, 252, 252};

static double intensity[4] = {77.175381, 88.654656, 166.564623, 174.228438};

#define NEW_CGA(c, i, r, g, b)                                                                                                   \
        (((c) / 0.72) * 0.29 + ((i) / 0.28) * 0.32 + ((r) / 0.28) * 0.1 + ((g) / 0.28) * 0.22 + ((b) / 0.28) * 0.07)

double mode_brightness;
double mode_contrast;
double mode_hue;
double min_v;
double max_v;

double video_ri, video_rq, video_gi, video_gq, video_bi, video_bq;
int video_sharpness;
int tandy_mode_control = 0;

static bool new_cga = 0;

FILE *df;

void update_cga16_color(uint8_t cgamode) {
        int x;

        if (!new_cga) {
                min_v = chroma_multiplexer[0] + intensity[0];
                max_v = chroma_multiplexer[255] + intensity[3];
        } else {
                double i0 = intensity[0];
                double i3 = intensity[3];
                min_v = NEW_CGA(chroma_multiplexer[0], i0, i0, i0, i0);
                max_v = NEW_CGA(chroma_multiplexer[255], i3, i3, i3, i3);
        }
        mode_contrast = 256 / (max_v - min_v);
        mode_brightness = -min_v * mode_contrast;
        if ((cgamode & 3) == 1)
                mode_hue = 14;
        else
                mode_hue = 4;

        mode_contrast *= contrast * (new_cga ? 1.2 : 1) / 100;              // new CGA: 120%
        mode_brightness += (new_cga ? brightness - 10 : brightness) * 5;    // new CGA: -10
        double mode_saturation = (new_cga ? 4.35 : 2.9) * saturation / 100; // new CGA: 150%

        for (x = 0; x < 1024; ++x) {
                int phase = x & 3;
                int right = (x >> 2) & 15;
                int left = (x >> 6) & 15;
                int rc = right;
                int lc = left;
                if ((cgamode & 4) != 0) {
                        rc = (right & 8) | ((right & 7) != 0 ? 7 : 0);
                        lc = (left & 8) | ((left & 7) != 0 ? 7 : 0);
                }
                double c = chroma_multiplexer[((lc & 7) << 5) | ((rc & 7) << 2) | phase];
                double i = intensity[(left >> 3) | ((right >> 2) & 2)];
                double v;
                if (!new_cga)
                        v = c + i;
                else {
                        double r = intensity[((left >> 2) & 1) | ((right >> 1) & 2)];
                        double g = intensity[((left >> 1) & 1) | (right & 2)];
                        double b = intensity[(left & 1) | ((right << 1) & 2)];
                        v = NEW_CGA(c, i, r, g, b);
                }
                CGA_Composite_Table[x] = (int)(v * mode_contrast + mode_brightness);
        }

        double i = CGA_Composite_Table[6 * 68] - CGA_Composite_Table[6 * 68 + 2];
        double q = CGA_Composite_Table[6 * 68 + 1] - CGA_Composite_Table[6 * 68 + 3];

        double a = tau * (33 + 90 + hue_offset + mode_hue) / 360.0;
        double c = cos(a);
        double s = sin(a);
        double r = 256 * mode_saturation / sqrt(i * i + q * q);

        double iq_adjust_i = -(i * c + q * s) * r;
        double iq_adjust_q = (q * c - i * s) * r;

        static const double ri = 0.9563;
        static const double rq = 0.6210;
        static const double gi = -0.2721;
        static const double gq = -0.6474;
        static const double bi = -1.1069;
        static const double bq = 1.7046;

        video_ri = (int)(ri * iq_adjust_i + rq * iq_adjust_q);
        video_rq = (int)(-ri * iq_adjust_q + rq * iq_adjust_i);
        video_gi = (int)(gi * iq_adjust_i + gq * iq_adjust_q);
        video_gq = (int)(-gi * iq_adjust_q + gq * iq_adjust_i);
        video_bi = (int)(bi * iq_adjust_i + bq * iq_adjust_q);
        video_bq = (int)(-bi * iq_adjust_q + bq * iq_adjust_i);
        video_sharpness = (int)(sharpness * 256 / 100);
}

static Bit8u byte_clamp(int v) {
        v >>= 13;
        return v < 0 ? 0 : (v > 255 ? 255 : v);
}

/* 2048x1536 is the maximum we can possibly support. */
#define SCALER_MAXWIDTH 2048

static int temp[SCALER_MAXWIDTH + 10] = {0};
static int atemp[SCALER_MAXWIDTH + 2] = {0};
static int btemp[SCALER_MAXWIDTH + 2] = {0};

Bit8u *Composite_Process(uint8_t cgamode, Bit8u border, Bit32u blocks /*, bool doublewidth*/, Bit8u *TempLine) {
        int x;
        Bit32u x2;

        int w = blocks * 4;

#define COMPOSITE_CONVERT(I, Q)                                                                                                  \
        do {                                                                                                                     \
                i[1] = (i[1] << 3) - ap[1];                                                                                      \
                a = ap[0];                                                                                                       \
                b = bp[0];                                                                                                       \
                c = i[0] + i[0];                                                                                                 \
                d = i[-1] + i[1];                                                                                                \
                y = ((c + d) << 8) + video_sharpness * (c - d);                                                                  \
                rr = y + video_ri * (I) + video_rq * (Q);                                                                        \
                gg = y + video_gi * (I) + video_gq * (Q);                                                                        \
                bb = y + video_bi * (I) + video_bq * (Q);                                                                        \
                ++i;                                                                                                             \
                ++ap;                                                                                                            \
                ++bp;                                                                                                            \
                *srgb = (byte_clamp(rr) << 16) | (byte_clamp(gg) << 8) | byte_clamp(bb);                                         \
                ++srgb;                                                                                                          \
        } while (0)

#define OUT(v)                                                                                                                   \
        do {                                                                                                                     \
                *o = (v);                                                                                                        \
                ++o;                                                                                                             \
        } while (0)

        // Simulate CGA composite output
        int *o = temp;
        Bit8u *rgbi = TempLine;
        int *b = &CGA_Composite_Table[border * 68];
        for (x = 0; x < 4; ++x)
                OUT(b[(x + 3) & 3]);
        OUT(CGA_Composite_Table[(border << 6) | ((*rgbi) << 2) | 3]);
        for (x = 0; x < w - 1; ++x) {
                OUT(CGA_Composite_Table[(rgbi[0] << 6) | (rgbi[1] << 2) | (x & 3)]);
                ++rgbi;
        }
        OUT(CGA_Composite_Table[((*rgbi) << 6) | (border << 2) | 3]);
        for (x = 0; x < 5; ++x)
                OUT(b[x & 3]);

        if ((cgamode & 4) != 0) {
                // Decode
                int *i = temp + 5;
                Bit32u *srgb = (Bit32u *)TempLine;
                for (x2 = 0; x2 < blocks * 4; ++x2) {
                        int c = (i[0] + i[0]) << 3;
                        int d = (i[-1] + i[1]) << 3;
                        int y = ((c + d) << 8) + video_sharpness * (c - d);
                        ++i;
                        *srgb = byte_clamp(y) * 0x10101;
                        ++srgb;
                }
        } else {
                // Store chroma
                int *i = temp + 4;
                int *ap = atemp + 1;
                int *bp = btemp + 1;
                for (x = -1; x < w + 1; ++x) {
                        ap[x] = i[-4] - ((i[-2] - i[0] + i[2]) << 1) + i[4];
                        bp[x] = (i[-3] - i[-1] + i[1] - i[3]) << 1;
                        ++i;
                }

                // Decode
                i = temp + 5;
                i[-1] = (i[-1] << 3) - ap[-1];
                i[0] = (i[0] << 3) - ap[0];
                Bit32u *srgb = (Bit32u *)TempLine;
                for (x2 = 0; x2 < blocks; ++x2) {
                        int y, a, b, c, d, rr, gg, bb;
                        COMPOSITE_CONVERT(a, b);
                        COMPOSITE_CONVERT(-b, a);
                        COMPOSITE_CONVERT(-a, -b);
                        COMPOSITE_CONVERT(b, -a);
                }
        }
#undef COMPOSITE_CONVERT
#undef OUT

        return TempLine;
}

void IncreaseHue(uint8_t cgamode) {
        hue_offset += 5.0;

        update_cga16_color(cgamode);
}

void DecreaseHue(uint8_t cgamode) {
        hue_offset -= 5.0;

        update_cga16_color(cgamode);
}

void IncreaseSaturation(uint8_t cgamode) {
        saturation += 5;

        update_cga16_color(cgamode);
}

void DecreaseSaturation(uint8_t cgamode) {
        saturation -= 5;

        update_cga16_color(cgamode);
}

void IncreaseContrast(uint8_t cgamode) {
        contrast += 5;

        update_cga16_color(cgamode);
}

void DecreaseContrast(uint8_t cgamode) {
        contrast -= 5;

        update_cga16_color(cgamode);
}

void IncreaseBrightness(uint8_t cgamode) {
        brightness += 5;

        update_cga16_color(cgamode);
}

void DecreaseBrightness(uint8_t cgamode) {
        brightness -= 5;

        update_cga16_color(cgamode);
}

void IncreaseSharpness(uint8_t cgamode) {
        sharpness += 10;

        update_cga16_color(cgamode);
}

void DecreaseSharpness(uint8_t cgamode) {
        sharpness -= 10;

        update_cga16_color(cgamode);
}

void cga_comp_init(int revision) {
        new_cga = revision;

        /* Making sure this gets reset after reset. */
        brightness = 0;
        contrast = 100;
        saturation = 100;
        sharpness = 0;
        hue_offset = 0;

        update_cga16_color(0);
}
