// For std::unique_ptr
#include <memory>

// SystemC global header
#include <systemc>

// Include common routines
#include <sys/stat.h>  // mkdir
#include <verilated.h>
#include <verilated_vcd_sc.h>

// Include model header, generated from Verilating "isp.v"
#include "Visp.h"

// Handle file
#include <fstream>
#include <iostream>

// math
#include <cmath>

#include "bmp.hpp"

static const uint16_t IN_WIDTH = 1936;
static const uint16_t IN_HEIGHT = 1088;
#define IN_SIZE (IN_WIDTH * IN_HEIGHT)
static const uint16_t OUT_WIDTH = 1920;
static const uint16_t OUT_HEIGHT = 1080;
#define OUT_SIZE (OUT_WIDTH * OUT_HEIGHT)

// color gain for correcting color
struct color_gain {
    double red;
    double green;
    double blue;
} color_gain{1.1, 0.7, 1.3}, white_gain;

static const double gamma_value = 2.2;
static const double saturation_inc = 0.5;
static const double white_radio = 0.1;

using namespace sc_core;
using namespace sc_dt;

SC_MODULE(TB_ISP) {
    sc_in_clk clk;
    sc_in<bool> reset;

    sc_in<bool> in_ready;
    sc_in<bool> in_receive;
    sc_out<bool> out_en;
    sc_out<uint32_t> out_data[3];

    sc_in<bool> im_clk;
    sc_in<bool> im_en;
    sc_out<bool> out_ready;
    sc_out<bool> out_receceive;
    sc_in<uint32_t> im_data;

    sc_out<bool> is_done;
    std::unique_ptr<uint16_t[]> image = std::make_unique<uint16_t[]>(IN_SIZE);
    std::unique_ptr<uint32_t[]> out = std::make_unique<uint32_t[]>(OUT_SIZE);

    SC_CTOR(TB_ISP) {
        SC_CTHREAD(send_Data, clk.pos());
        reset_signal_is(reset, true);

        SC_CTHREAD(read_Data, im_clk.pos());
    }

    void send_Data(void) {
        uint16_t pos_x = 0, pos_y = 0;
        while (true) {
            if (in_ready.read() && pos_y < IN_HEIGHT - 2) {
                out_en.write(1);

                printf("x=%4d, y=%4d, data=0x%04x\t", pos_x, pos_y,
                       image[(pos_y + 0) * IN_WIDTH + pos_x]);
                printf("x=%4d, y=%4d, data=0x%04x\t", pos_x, pos_y,
                       image[(pos_y + 1) * IN_WIDTH + pos_x]);
                printf("x=%4d, y=%4d, data=0x%04x\n", pos_x, pos_y,
                       image[(pos_y + 2) * IN_WIDTH + pos_x]);

                out_data[0].write(image[(pos_y + 0) * IN_WIDTH + pos_x]);
                out_data[1].write(image[(pos_y + 1) * IN_WIDTH + pos_x]);
                out_data[2].write(image[(pos_y + 2) * IN_WIDTH + pos_x]);

                // wait(1);
                // out_en.write(0);

                if (++pos_x >= IN_WIDTH) {
                    pos_x = 0;
                    pos_y++;
                }
            } else {
                out_en.write(0);
            }

            wait();
        }
    }

    void read_Data(void) {
        is_done.write(0);
        uint16_t pos_x = 0, pos_y = 0;
        uint32_t last_data = 0;
        uint32_t cnt = 0;
        while (true) {
            if (im_en.read()) {
                out_ready.write(false);
                out_receceive.write(true);

                out[pos_y * OUT_WIDTH + pos_x] = im_data.read();

                if (pos_x++ >= OUT_WIDTH) {
                    pos_x = 0;
                    pos_y++;
                }
            } else {
                out_ready.write(true);
                out_receceive.write(false);
            }

            // when data didn't change some time, it end
            if (last_data == im_data.read()) {
                cnt++;
                if (cnt >= 100000L) {
                    is_done.write(1);
                    printf("x=%d, y=%d\n", pos_x, pos_y);
                }
            } else {
                cnt = 0;
            }
            last_data = im_data.read();

            wait();
        }
    }
};

int sc_main(int argc, char* argv[]) {
    std::cout << "Get into sc_main" << std::endl;
    // Open image
    std::ifstream in_image;
    std::ofstream out_image;
    in_image.open("./transform/test.bin", std::ios::in | std::ios::binary);
    // out_image.open("./transform/out.bin", std::ios::out | std::ios::binary);
    if (!in_image.is_open()) {
        std::cout << "Open image fail" << std::endl;
        exit(0);
    } else {
        std::cout << "Ready to sim" << std::endl;
    }

    // Read image
    auto buf = std::make_unique<uint8_t[]>(2 * IN_SIZE);
    in_image.read((char*)buf.get(), IN_SIZE * 2);
    in_image.close();
    // Reshape data
    auto image = std::make_unique<uint16_t[]>(IN_SIZE);
    uint32_t i = 0;
    for (int y = 0; y < IN_HEIGHT; y++) {
        for (int x = 0; x < IN_WIDTH; x++) {
            image[y * IN_WIDTH + x] =
                (uint16_t)buf[i] + ((uint16_t)buf[i + 1] << 8);
            i += 2;
        }
    }
    std::cout << "Finish Reading data" << std::endl;

    // This is a more complicated example, please also see the simpler
    // examples/make_hello_c.

    // Create logs/ directory in case we have traces to put under it
    Verilated::mkdir("logs");

    // Set debug level, 0 is off, 9 is highest presently used
    // May be overridden by commandArgs argument parsing
    Verilated::debug(0);

    // Randomization reset policy
    // May be overridden by commandArgs argument parsing
    Verilated::randReset(2);

    // Before any evaluation, need to know to calculate those signals only used
    // for tracing
    Verilated::traceEverOn(true);

    // Pass arguments so Verilated code can see them, e.g. $value$plusargs
    // This needs to be called before you create any model
    Verilated::commandArgs(argc, argv);

    // General logfile
    std::ios::sync_with_stdio();

    // Define clocks
    sc_clock clk{"clk", 10, SC_NS, 0.5, 3, SC_NS, true};
    // Define interconnect
    sc_signal<bool> reset;

    sc_signal<bool> in_en;
    sc_signal<bool> in_ready;
    sc_signal<bool> in_receive;
    sc_signal<uint32_t> in_data[3];

    sc_signal<bool> out_clk;
    sc_signal<bool> out_en;
    sc_signal<bool> out_ready;
    sc_signal<bool> out_receive;
    sc_signal<uint32_t> out_data;

    sc_signal<bool> blender_enable;
    sc_signal<uint32_t> gain_red;
    sc_signal<uint32_t> gain_green;
    sc_signal<uint32_t> gain_blue;

    sc_signal<bool> gamma_enable;
    sc_signal<uint32_t> gamma_inverse;
    sc_signal<uint32_t> gamma_table[256];

    sc_signal<bool> saturation_enable;
    sc_signal<uint32_t> saturation_increase;

    sc_signal<bool> flag_done;

    // Construct the Verilated model, from inside Visp.h
    // Using unique_ptr is similar to "Visp* isp = new Visp" then deleting at
    // end
    const std::unique_ptr<Visp> isp{new Visp{"isp"}};
    // Attach Visp's signals to this upper model
    isp->clk(clk);
    isp->reset(reset);
    isp->in_en(in_en);
    isp->in_ready(in_ready);
    isp->in_receive(in_receive);
    isp->in_data[0](in_data[0]);
    isp->in_data[1](in_data[1]);
    isp->in_data[2](in_data[2]);
    isp->out_clk(out_clk);
    isp->out_en(out_en);
    isp->out_ready(out_ready);
    isp->out_receive(out_receive);
    isp->out_data(out_data);

    isp->gain_red(gain_red);
    isp->gain_green(gain_green);
    isp->gain_blue(gain_blue);
    isp->blender_enable(blender_enable);

    isp->gamma_enable(gamma_enable);
    // isp->gamma_inverse(gamma_inverse);

    isp->saturation_enable(saturation_enable);
    isp->saturation_inc(saturation_increase);

    blender_enable = true;  // enable color correction
    gain_red = (uint32_t)(color_gain.red * std::pow(2, 8));
    gain_green = (uint32_t)(color_gain.green * std::pow(2, 8));
    gain_blue = (uint32_t)(color_gain.blue * std::pow(2, 8));

    gamma_enable = true;
    gamma_inverse = (uint32_t)((1.0 / gamma_value) * std::pow(2, 8));
    for (int i = 0; i < 256; i++) {
        // calculate gamma table
        isp->gamma_table[i](gamma_table[i]);
        gamma_table[i] = (uint32_t)(255 * pow(i / 255.0, 1.0 / gamma_value));
    }

    saturation_enable = true;
    saturation_increase =
        (int32_t)((saturation_inc >= 0) ? (saturation_inc * std::pow(2, 8))
                                        : (saturation_inc * std::pow(2, 8)));

    // Construct testbench module
    TB_ISP tb_isp("tb_isp");
    tb_isp.clk(clk);
    tb_isp.reset(reset);
    tb_isp.in_ready(out_ready);
    tb_isp.in_receive(out_receive);
    tb_isp.out_en(in_en);
    tb_isp.out_ready(in_ready);
    tb_isp.out_receceive(in_receive);
    tb_isp.out_data[0](in_data[0]);
    tb_isp.out_data[1](in_data[1]);
    tb_isp.out_data[2](in_data[2]);
    tb_isp.im_clk(out_clk);
    tb_isp.im_en(out_en);
    tb_isp.im_data(out_data);
    tb_isp.is_done(flag_done);
    tb_isp.image = move(image);

    // You must do one evaluation before enabling waves, in order to allow
    // SystemC to interconnect everything for testing.
    sc_start(SC_ZERO_TIME);

    // If verilator was invoked with --trace argument,
    // and if at run time passed the +trace argument, turn on tracing
    VerilatedVcdSc* tfp = nullptr;
    const char* flag = Verilated::commandArgsPlusMatch("trace");
    if (flag && 0 == std::strcmp(flag, "+trace")) {
        std::cout << "Enabling waves into logs/vlt_dump.vcd...\n";
        tfp = new VerilatedVcdSc;
        isp->trace(tfp, 99);  // Trace 99 levels of hierarchy
        Verilated::mkdir("logs");
        tfp->open("logs/vlt_dump.vcd");
    }

    // Simulate until $finish
    while (!Verilated::gotFinish()) {
        // Flush the wave files each cycle so we can immediately see the output
        // Don't do this in "real" programs, do it in an abort() handler instead
        if (tfp) tfp->flush();

        // Apply inputs
        if (sc_time_stamp() < sc_time(10, SC_NS)) {
            reset.write(1);  // Assert reset
        } else {
            reset.write(0);  // Deassert reset
        }

        if (flag_done.read()) break;

        // Simulate 1ns
        sc_start(1, SC_NS);
    }

    // Final model cleanup
    isp->final();

    // Close trace if opened
    if (tfp) {
        tfp->close();
        tfp = nullptr;
    }

    uint8_t* data =
        new uint8_t[OUT_WIDTH * OUT_HEIGHT * 3];  // RGB24格式像素数据

    // software algorthms analyze
    uint32_t red_total = 0, green_total = 0, blue_total = 0;
    uint8_t red_max = 0, green_max = 0, blue_max = 0;
    for (int32_t y = 0; y < OUT_HEIGHT; ++y) {
        for (int32_t x = 0; x < OUT_WIDTH; ++x) {
            int32_t index = (y * OUT_WIDTH + x) * 3;

            uint8_t red = (tb_isp.out[y * OUT_WIDTH + x] & 0x00ff0000) >> 16;
            uint8_t green = (tb_isp.out[y * OUT_WIDTH + x] & 0x0000ff00) >> 8;
            uint8_t blue = (tb_isp.out[y * OUT_WIDTH + x] & 0x000000ff);

            // Adjust gamma line
            // red = 255 * std::pow(red / 255.0, 1 / gamma_value);
            // green = 255 * std::pow(green / 255.0, 1 / gamma_value);
            // blue = 255 * std::pow(blue / 255.0, 1 / gamma_value);

            // Calculate white balance data
            red_max = std::max(red_max, red);
            green_max = std::max(green_max, green);
            blue_max = std::max(blue_max, blue);
            red_total += red;
            green_total += green;
            blue_total += blue;

            // Adjust vibrance
            // uint8_t max = std::max({red, green, blue});
            // uint8_t min = std::min({red, green, blue});
            // double delta = (max - min) / 255.0;
            // double value = (max + min) / 255.0;
            // if (delta != 0) {
            //     double L = value / 2.0;
            //     // double S = (L <= 0.5) ? delta / value : delta / (2 -
            //     value); double S = delta / max; double alpha = 0.0; if
            //     (saturation_inc >= 0) {
            //         if ((saturation_inc + S) >= 1)
            //             alpha = S;
            //         else
            //             alpha = 1 - saturation_inc;
            //         alpha = 1 / alpha - 1;
            //         red = static_cast<uchar>(red + (red - L * 255) * alpha);
            //         green =
            //             static_cast<uchar>(green + (green - L * 255) *
            //             alpha);
            //         blue = static_cast<uchar>(blue + (blue - L * 255) *
            //         alpha);
            //     } else {
            //         alpha = saturation_inc;
            //         red = static_cast<uchar>(L * 255 +
            //                                  (red - L * 255) * (1 + alpha));
            //         green = static_cast<uchar>(L * 255 +
            //                                    (green - L * 255) * (1 +
            //                                    alpha));
            // blue = static_cast<uchar>(L * 255 +
            //                                   (blue - L * 255) * (1 +
            //                                   alpha));
            //     }
            // }

            data[index + 0] = red;    // R
            data[index + 1] = green;  // G
            data[index + 2] = blue;   // B
        }
    }

    // Adjust White Balance : Grey World Color Correction
    double K = static_cast<double>(red_total + green_total + blue_total) / (3 * OUT_SIZE);
    white_gain.red = static_cast<double>(K * OUT_SIZE) / red_total;
    white_gain.green = static_cast<double>(K * OUT_SIZE) / green_total;
    white_gain.blue = static_cast<double>(K * OUT_SIZE) / blue_total;
    printf("Gain: red = %f, green = %f, blue = %f", white_gain.red,
           white_gain.green, white_gain.blue);
    for (int32_t y = 0; y < OUT_HEIGHT; ++y) {
        for (int32_t x = 0; x < OUT_WIDTH; ++x) {
            int32_t index = (y * OUT_WIDTH + x) * 3;

            data[index + 0] =
                static_cast<uint8_t>(white_gain.red * data[index + 0]);
            data[index + 1] =
                static_cast<uint8_t>(white_gain.green * data[index + 1]);
            data[index + 2] =
                static_cast<uint8_t>(white_gain.blue * data[index + 2]);
        }
    }

    // save to bin
    std::cout << "Ready to save raw RGB image" << std::endl;
    // for (int32_t y = 0; y < OUT_HEIGHT; ++y) {
    //     for (int32_t x = 0; x < OUT_WIDTH; ++x) {
    //         int32_t index = (y * OUT_WIDTH + x) * 3;

    //         uint8_t red = data[index + 0];
    //         uint8_t green = data[index + 1];
    //         uint8_t blue = data[index + 2];

    //         out_image.write((const char*)&red, sizeof(red));
    //         out_image.write((const char*)&green, sizeof(green));
    //         out_image.write((const char*)&blue, sizeof(blue));

    //         printf("x=%4d, y=%4d, red=0x%02x, green=0x%02x, blue=0x%02x\n",
    //         x,
    //                y, red, green, blue);
    //     }
    // }

    // save to bmp
    write_bmp("test.bmp", data, OUT_WIDTH, OUT_HEIGHT);
    delete[] data;

    // Return good completion status
    return 0;
}