#include <iostream>
#include <memory>
#include "cvimagex.h"
#include "opencv2/opencv.hpp"
#include <FreeImagePlus.h>
#include "StopWatch.h"
#include <iostream>
#include "iimageencode.h"
#include "imageencode.h"
#include "ImageApply.h"
#include "ImageApplyHeaders.h"
#include "ThreadPool.h"


std::vector<double> caculate(const std::vector<double>& points_x, const std::vector<double>& points_y)
{
    int MaxElement = points_x.size() - 1;
    //计算常数f
    double f = points_y[0];
    //求解
    int n, m;
    //double a[MaxElement][MaxElement+1];
    std::vector<std::vector<double>> a;
    //a.resize(MaxElement);
    for (int i = 0; i < MaxElement; i++)
    {
        std::vector<double> b;
        b.resize(MaxElement + 1);
        a.push_back(b);
    }

    for (int i = 0; i < MaxElement; i++)
    {
        for (int j = 0; j < MaxElement; j++)
            a[i][j] = cv::pow(points_x[i + 1], MaxElement - j);
        a[i][MaxElement] = points_y[i + 1] - f;
    }

    int i, j;
    n = MaxElement;

    for (j = 0; j < n; j++)
    {
        double max = 0;
        double imax = 0;
        for (i = j; i < n; i++)
            if (imax < cv::abs(a[i][j]))
            {
                imax = cv::abs(a[i][j]);
                max = a[i][j];//得到各行中所在列最大元素
                m = i;
            }

        if (cv::abs(a[j][j]) != max)
        {
            double b = 0;
            for (int k = j; k < n + 1; k++)
            {
                b = a[j][k];
                a[j][k] = a[m][k];
                a[m][k] = b;
            }
        }

        for (int r = j; r < n + 1; r++)
            a[j][r] = a[j][r] / max;//让该行的所在列除以所在列的第一个元素，目的是让首元素为1

        for (i = j + 1; i < n; i++)
        {
            double c = a[i][j];
            if (c == 0.0) continue;
            for (int s = j; s < n + 1; s++)
                a[i][s] = a[i][s] - a[j][s] * c;//前后行数相减，使下一行或者上一行的首元素为0
        }
    }

    for (i = n - 2; i >= 0; i--)
        for (j = i + 1; j < n; j++)
            a[i][n] = a[i][n] - a[j][n] * a[i][j];

    std::vector<double> result;
    for (int k = 0; k < n; k++)
        result.push_back(a[k][n]);
    result.push_back(f);
    return result;
}

void hsvCorrect_init(uint* table)
{
    cv::Mat mat_rgbTable(256 * 256, 256, CV_8UC3);
    uchar* ptr_mat_rgbTable = mat_rgbTable.data;
    for (size_t r = 0; r < 256; r++)
        for (size_t g = 0; g < 256; g++)
            for (size_t b = 0; b < 256; b++, ptr_mat_rgbTable += 3)
            {
                ptr_mat_rgbTable[0] = b;
                ptr_mat_rgbTable[1] = g;
                ptr_mat_rgbTable[2] = r;
            }

    cv::cvtColor(mat_rgbTable, mat_rgbTable, cv::COLOR_BGR2HSV_FULL);

    uchar table_data[256];
    cv::Mat tableLUT(256, 1, CV_8UC1, table_data);
    std::vector<double> points_x, points_y, coefficient;
    points_x = { 0, 80, 175, 255 };
    points_y = { 12, 85, 175, 270 };
    coefficient = caculate(points_x, points_y);
    for (int j = 0; j < 256; j++)
        table_data[j] = static_cast<int>(cv::max(0.0, coefficient[0] * j * j * j + coefficient[1] * j * j + coefficient[2] * j + coefficient[3]));

    cv::Mat hsv_ms[3];
    cv::split(mat_rgbTable, hsv_ms);
    cv::LUT(hsv_ms[0], tableLUT, hsv_ms[0]);
    cv::merge(hsv_ms, 3, mat_rgbTable);

    cv::cvtColor(mat_rgbTable, mat_rgbTable, cv::COLOR_HSV2BGR_FULL);

    cv::Mat mat_bgr32(256, 256 * 256, CV_8UC4);
    cv::cvtColor(mat_rgbTable, mat_bgr32, cv::COLOR_BGR2BGRA);

    memcpy(table, mat_bgr32.data, mat_bgr32.total() * sizeof(uint));
}

void hsvCorrect(cv::Mat& image, uint* table)
{
    if (image.channels() != 3)
        return;

    cv::Mat bgra;
    cv::cvtColor(image, bgra, cv::COLOR_BGR2BGRA);
    uint* ptr = bgra.ptr<uint>();
    int rows = bgra.rows, cols = bgra.cols;
    for (int i = 0; i < rows; i++)
    {
        ptr = reinterpret_cast<uint*>(bgra.ptr(i));
        for (int j = 0; j < cols; j++)
            ptr[j] = table[ptr[j] & 0x00ffffff];
    }
    cv::cvtColor(bgra, image, cv::COLOR_BGRA2BGR);
}

static std::string read_all_from(std::string path)
{
	int t;
	FILE* file = fopen(path.c_str(), "rb");
	fseek(file, 0, SEEK_END);
	t = ftell(file);
	std::string buf(t, 0);
	fseek(file, 0, SEEK_SET);
	fread((void*)buf.c_str(), t, 1, file);
	fclose(file);
	return buf;
}

cv::Mat interpolation_600dpi(cv::Mat src,bool side)
{
	cv::Mat dst(src.rows, src.cols + (side + 1) * 16, CV_8UC(src.channels()));
	int channel_w = side ? (src.cols / 17) : (src.cols / 17);

	int copy_lenght = 0;
	int dst_copy_lenght = 0;
	std::vector<std::pair<int, int>> v;
	for (int i = 0; i < ((side + 1) * 17); i++)
	{
		src(cv::Rect(copy_lenght, 0, channel_w, src.rows)).copyTo(dst(cv::Rect(dst_copy_lenght, 0, channel_w, src.rows)));
		copy_lenght += channel_w;
		dst_copy_lenght += channel_w + (i == 16 ? 0 : 1);
		if (i == (side ? 33 : 16)) continue;
		v.push_back({ copy_lenght ,dst_copy_lenght });

	}
	for (auto node : v)
	{
		cv::Vec3b f, b;
		for (int row = 0; row < src.rows; row++)
		{
			f = src.at<cv::Vec3b>(row, node.first - 1);
			b = src.at<cv::Vec3b>(row, node.first);
			dst.at<cv::Vec3b>(row, node.second - 1)[0] = (f[0] + b[0]) / 2;
			dst.at<cv::Vec3b>(row, node.second - 1)[1] = (f[1] + b[1]) / 2;
			dst.at<cv::Vec3b>(row, node.second - 1)[2] = (f[2] + b[2]) / 2;
		}
	}
	return dst;
}

#include <numeric>
#include <thread>
#include <RockchipRga.h>
#include <RockchipRgaMacro.h>



cv::Mat HG_RGA_Resize(cv::Mat srcMat, cv::Size dsize, double fx = 0, double fy = 0,
                          int interpolation = cv::INTER_LINEAR)
{
    if(srcMat.channels() == 3)
    {
        int ret = 0;
        int srcWidth, srcHeight, srcFormat;
        int dstWidth, dstHeight, dstFormat;
        bo_t bo_src, bo_dst;

        srcWidth = srcMat.cols;
        srcHeight = srcMat.rows;
        srcFormat = RK_FORMAT_RGB_888;
        if(dsize.empty())
        {
            dstWidth = srcMat.cols*fx;
            dstHeight = srcMat.rows*fy;
        }
        else
        {
            dstWidth = dsize.width;
            dstHeight = dsize.height;
        }
        printf("!!! RGA dst size: [%d,%d] src size: [%d,%d]\n",dstWidth,dstHeight,srcMat.cols,srcMat.rows);
        if(srcMat.cols > 8192 || srcMat.rows > 8192 || dstHeight > 4096 || dstWidth > 4096)
        {
            cv::resize(srcMat,srcMat,dsize,fx,fy,interpolation);
            return srcMat;
        }
        dstFormat = RK_FORMAT_RGB_888;

        RockchipRga rkRga;
        rkRga.RkRgaInit();
        rga_info_t src;
        rga_info_t dst;
        memset(&src, 0, sizeof(rga_info_t));
        src.fd = -1;
        src.mmuFlag = -1;
        src.virAddr = srcMat.data;
        cv::Mat dstmat(dstHeight, dstWidth, CV_8UC3);
        memset(&dst, 0, sizeof(rga_info_t));
        dst.fd = -1;
        dst.mmuFlag = -1;
        dst.virAddr = dstmat.data;
        rga_set_rect(&src.rect, 0, 0, srcWidth, srcHeight, srcWidth /*stride*/, srcHeight, srcFormat);
        rga_set_rect(&dst.rect, 0, 0, dstWidth, dstHeight, dstWidth /*stride*/, dstHeight, dstFormat);
        src.rotation = HAL_TRANSFORM_ROT_180;
        src.scale_mode = bilinear;
        for(int i  = 0;i <1;i++)
        {
            ret = rkRga.RkRgaBlit(&src, &dst, NULL);
            if (ret)
            {
                printf("rgaFillColor error : %s\n", strerror(errno));
            }
        }
        return dstmat;
    }
    else
    {
        cv::resize(srcMat,srcMat,dsize,fx,fy,interpolation);
        return srcMat;
    }
}

int main()
{
    // cpu_set_t cpuset;
    // CPU_ZERO(&cpuset);
    // CPU_SET(1, &cpuset);
    // pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset); 
    // std::this_thread::sleep_for(std::chrono::milliseconds(200));
    
    // auto f = cv::imread("/root/1.jpg",cv::IMREAD_ANYCOLOR);
    // std::vector<double> v;
    // for(int i =0;i<100;i++){
    //     auto clone = f.clone();
    //     StopWatch sw;
    //     interpolation_600dpi(clone,false);
    //     v.push_back(sw.elapsed_us());
    //     //printf("interpolation_600dpi time = %f \n",sw.elapsed_ms());
    // }
    // double mean = std::accumulate(v.begin(),v.end(),0.0) / v.size() /1000.0;
    // printf("interpolation_600dpi mean time = %f \n",mean*2);
    // v.clear();
    // CPU_ZERO(&cpuset);
    // CPU_SET(5, &cpuset);
    // pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset); 
    // std::this_thread::sleep_for(std::chrono::milliseconds(200));
    
    // auto b = cv::imread("/root/1.jpg",cv::IMREAD_ANYCOLOR);

    // for(int i =0;i<100;i++){
    //     auto clone = b.clone();
    //     StopWatch sw;
    //     interpolation_600dpi(clone,false);
    //     v.push_back(sw.elapsed_us());
    //     //printf("interpolation_600dpi time = %f \n",sw.elapsed_ms());
    // }
    // mean = std::accumulate(v.begin(),v.end(),0.0) / v.size() /1000.0;
    // printf("interpolation_600dpi mean time = %f \n",mean*2);
    
    ThreadPool pool(5);
    cv::Mat mat = cv::imread("/root/img/rga.png",cv::IMREAD_ANYCOLOR);
    std::queue<std::future<void>> fus;
    for(int i=0;i<5000;i++)
    {
        while(fus.size()>30)
        {
            fus.front().get();
            fus.pop();
        }
       fus.push(pool.enqueue([i](cv::Mat mat){
            StopWatch sw;
            HG_RGA_Resize(mat,cv::Size(0,0),1.0,0.517);
            std::cout<< "rga "<<i<< "times : "<< sw.elapsed_ms() <<std::endl;
            },
        mat.clone()));
    }
    while (fus.size())
    {
        fus.front().get();
        fus.pop();
    }
    
    return 0;

}