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

// SystemC global header
#include <string>
#include <systemc>

// Include common routines
#include <sys/stat.h> // mkdir
#include <vector>
#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 "bitmap_image.hpp"

static const uint16_t IN_WIDTH = 1936;
static const uint16_t IN_HEIGHT = 1088;
static const uint32_t IN_SIZE = (IN_WIDTH * IN_HEIGHT);
static const uint16_t OUT_WIDTH = 1920;
static const uint16_t OUT_HEIGHT = 1080;
static const uint32_t OUT_SIZE = (OUT_WIDTH * OUT_HEIGHT);
static const uint32_t FLAMES = 2;

// Input image path and Output directory path
#ifndef INPUT_IMG
const char *INPUT_IMG = "./src/transform/test.bin";
#endif
#ifndef OUTPUT_DIR
const char *OUTPUT_DIR = "./logs/";
#endif

// 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 contrast = 1.2;
// static const double white_radio = 0.1;

using namespace sc_core;
using namespace sc_dt;

bool picProcess(uint32_t *image, uint16_t number);

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_en;
  sc_out<bool> out_ready;
  // sc_out<bool> out_receceive;
  sc_in<uint32_t> im_data;

  sc_out<bool> is_done;
  std::vector<uint16_t> image; // the data of image
  std::vector<uint32_t> process_image = std::vector<uint32_t>(
      OUT_SIZE, 0); // after isp process, the data of image

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

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

  void send_Data(void) {
    uint16_t pos_x = 0, pos_y = 0, cnt_flame = 0;
    bool is_finish = false;
    while (true) {
      if (in_ready.read() && !is_finish) {
        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]);

        pos_x++;
        if (pos_x >= IN_WIDTH) {
          pos_x = 0;
          pos_y++;
        }
        if (pos_y >= IN_HEIGHT - 1) {
          pos_y = 0;
          cnt_flame++;
        }
        if (cnt_flame >= FLAMES) {
          is_finish = true;
        }
      } else {
        out_en.write(0);
      }

      wait();
    }
  }

  void read_Data(void) {
    is_done.write(0);
    uint32_t pos_x = 0, pos_y = 0, cnt_flame = 0;
    uint32_t last_data = 0, cnt = 0;
    bool is_finish = false;
    while (true) {
      if (im_en.read() && !is_finish) {
        out_ready.write(false);

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

        if (pos_x >= OUT_WIDTH) {
          pos_x = 0;
          pos_y++;
        }
        if (pos_y >= OUT_HEIGHT) {
          pos_y = 0;
          saveData(
              ("output_img_" + std::to_string(cnt_flame) + ".bmp").c_str());
          cnt_flame++;
        }
        if (cnt_flame >= FLAMES) {
          is_finish = true;
        }
      } else {
        out_ready.write(true);
        // out_receceive.write(false);
      }

      // when data didn't change some time, it end
      if (last_data == im_data.read() && is_finish) {
        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();
    }
  }

  bool saveData(const char *name) {
    bool ret = true;

    // Check Image Size
    if (process_image.size() > OUT_SIZE) {
      std::cout << "Process Image Over Size!!!\n"
                << "Image Size:" << process_image.size() << "\n";
      return false;
    }

    // Transform isp image
    // std::vector<uint8_t> bmp_image(3 * OUT_SIZE);
    // for (int i = 0; i < OUT_SIZE; i++) {
    //   bmp_image[3 * i + 0] = (process_image[i] & 0x00ff0000) >> 16;
    //   bmp_image[3 * i + 1] = (process_image[i] & 0x0000ff00) >> 8;
    //   bmp_image[3 * i + 2] = (process_image[i] & 0x000000ff) >> 0;
    // }

    // Write BMP image
    bitmap_image bmp(OUT_WIDTH, OUT_HEIGHT);
    if (!bmp) {
      std::cout << "Output Image Open Failed!!!\n";
      return false;
    }
    for (int y = 0; y < OUT_HEIGHT; y++)
      for (int x = 0; x < OUT_WIDTH; x++)
        bmp.set_pixel(x, y,
                      (process_image[y * OUT_WIDTH + x] & 0x00ff0000) >> 16,
                      (process_image[y * OUT_WIDTH + x] & 0x0000ff00) >> 8,
                      (process_image[y * OUT_WIDTH + x] & 0x000000ff) >> 0);
    bmp.save_image(std::string(OUTPUT_DIR) + name);
    return ret;
  }
};

// bool picProcess(uint32_t *image, uint16_t number) {
//   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 = (image[y * OUT_WIDTH + x] & 0x00ff0000) >> 16;
//       uint8_t green = (image[y * OUT_WIDTH + x] & 0x0000ff00) >> 8;
//       uint8_t blue = (image[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));
//       //     }
//       // }

//       // Contrast enhancement
//       // red = static_cast<uchar>(contrast * (red - 128) + 128);
//       // green = static_cast<uchar>(contrast * (green - 128) + 128);
//       // blue = static_cast<uchar>(contrast * (blue - 128) + 128);

//       // save data
//       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 bmp
//   std::cout << "Ready to save raw RGB image" << std::endl;
//   char file_name[64] = {0};
//   snprintf(file_name, sizeof(file_name), "pic_%d.bmp", number);
//   write_bmp(file_name, data, OUT_WIDTH, OUT_HEIGHT);
//   delete[] data;

//   return true;
// }

int sc_main(int argc, char *argv[]) {
  std::printf("Enter into sc_main\n");

  // Open Image
  std::ifstream image;
  image.open(INPUT_IMG, std::ios::in | std::ios::binary);
  // Check image whether is open
  if (!image.is_open()) {
    std::printf("Open Image Failed!!!\n");
    exit(0);
  } else {
    std::printf("Open Image Successfully!!!\n");
  }

  // Read and Transform Image
  std::vector<uint16_t> in_image(IN_SIZE);
  uint8_t *buf = new uint8_t[2 * IN_SIZE];
  image.read((char *)buf, 2 * IN_SIZE);
  uint32_t i = 0;
  for (int y = 0; y < IN_HEIGHT; y++) {
    for (int x = 0; x < IN_WIDTH; x++) {
      in_image[y * IN_WIDTH + x] =
          (uint16_t)buf[i] + ((uint16_t)buf[i + 1] << 8);
      i += 2;
    }
  }
  // Close and delete image
  image.close();
  delete[] buf;
  std::printf("Finish Reading Image\n");

  // 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<uint32_t> white_gain[3];
  sc_signal<uint32_t> flame_rate;
  sc_signal<bool> white_enable;

  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_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->white_enable(white_enable);
  isp->flame_rate(flame_rate);
  isp->white_gain[0](white_gain[0]);
  isp->white_gain[1](white_gain[1]);
  isp->white_gain[2](white_gain[2]);

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

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

  gamma_enable = true;
  gamma_inverse = static_cast<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] =
        static_cast<uint32_t>(255 * pow(i / 255.0, 1.0 / gamma_value));
  }

  white_enable = true;
  flame_rate = 0;
  white_gain[0] = 255;
  white_gain[1] = 255;
  white_gain[2] = 255;

  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_en(out_en);
  tb_isp.im_data(out_data);
  tb_isp.is_done(flag_done);
  tb_isp.image = std::move(in_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_DIR
    // 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;
  }

  // Return good completion status
  return 0;
}
