#include <string>
#include <iostream>
#include <vector>
#include <stdio.h>
#include <cmath>
#include <unistd.h>
#include <sys/time.h>


#include "opencv2/opencv.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"

#include "yolo_v2_class.hpp"

using namespace std;
using namespace cv;

/********************************* lane detection start *********************************/


/********************************* lane detection end *********************************/

/********************************* multi area *****************************************/
#define PI 3.141592654
const int maxn = 300;
const double eps = 1e-6;

typedef struct parallelogram {
	float width;
	float height;
	float startX;
	float startY;
	float angleLine;
	float anglePoint;
	cv::Point2f points[4];
}parallelogram;

int dcmp(double x)
{
    if(x > eps) return 1;
    return x < -eps ? -1 : 0;
}

double cross(cv::Point2f a, cv::Point2f b, cv::Point2f c) ///叉积
{
    return (a.x-c.x)*(b.y-c.y)-(b.x-c.x)*(a.y-c.y);
}

cv::Point intersection(cv::Point2f a, cv::Point2f b, cv::Point2f c, cv::Point2f d)
{
    cv::Point2f p = a;
    double t =((a.x-c.x)*(c.y-d.y)-(a.y-c.y)*(c.x-d.x))/((a.x-b.x)*(c.y-d.y)-(a.y-b.y)*(c.x-d.x));
    p.x +=(b.x-a.x)*t;
    p.y +=(b.y-a.y)*t;
    //cout << "intersection p.x=" << p.x << ", p.y=" << p.y << endl;
    return p;
}

//计算多边形面积
double PolygonArea(cv::Point2f p[], int n)
{
    if(n < 3) return 0.0;
    double s = p[0].y * (p[n - 1].x - p[1].x);
    for(int i = 1; i < n - 1; ++ i) {
        s += p[i].y * (p[i - 1].x - p[i + 1].x);
        // cout << "p[i-1].x =" << p[i-1].x << ", p[i-1].y=" << p[i-1].y << endl;
        // cout << "p[i].x =" << p[i].x << ", p[i].y=" << p[i].y << endl;
        // cout << "p[i+1].x =" << p[i+1].x << ", p[i+1].y=" << p[i+1].y << endl;
    }
    s += p[n - 1].y * (p[n - 2].x - p[0].x);
    //cout << "s =" << s << endl;
    return fabs(s * 0.5);
}

double CPIA(cv::Point2f a[], cv::Point2f b[], int na, int nb)//ConvexPolygonIntersectArea
{
    cv::Point2f p[20], tmp[20];
    int tn, sflag, eflag;
    memcpy(p,b,sizeof(cv::Point)*(nb));
    for(int i = 0; i < na && nb > 2; i++)
    {
    	if (i == na - 1) {
    		sflag = dcmp(cross(a[0], p[0],a[i]));
    	} else {
    		sflag = dcmp(cross(a[i + 1], p[0],a[i]));
    	}
        for(int j = tn = 0; j < nb; j++, sflag = eflag)
        {
            if(sflag>=0) {
            	tmp[tn++] = p[j];
            }
            if (i == na - 1) {
            	if (j == nb -1) {
            		eflag = dcmp(cross(a[0], p[0], a[i]));
				} else {
					eflag = dcmp(cross(a[0], p[j + 1], a[i]));
				}
			} else {
				if (j == nb -1) {
					eflag = dcmp(cross(a[i + 1], p[0], a[i]));
				} else {
					eflag = dcmp(cross(a[i + 1], p[j + 1], a[i]));
				}
			}
            if((sflag ^ eflag) == -2){
            	if (i == na - 1) {
            		if (j == nb -1) {
            			tmp[tn++] = intersection(a[i], a[0], p[j], p[0]); //求交点
            		} else {
            			tmp[tn++] = intersection(a[i], a[0], p[j], p[j + 1]);
            		}
				} else {
					if (j == nb -1) {
						tmp[tn++] = intersection(a[i], a[i + 1], p[j], p[0]);
					} else {
						tmp[tn++] = intersection(a[i], a[i + 1], p[j], p[j + 1]);
					}
				}
            }
        }
        memcpy(p, tmp, sizeof(cv::Point) * tn);
        nb = tn, p[nb] = p[0];
    }
    if(nb < 3) return 0.0;
    return PolygonArea(p, nb);
}

double SPIA(cv::Point2f a[], cv::Point2f b[], int na, int nb)///SimplePolygonIntersectArea 调用此函数
{
    int i, j;
    cv::Point2f t1[na], t2[nb];
    double res = 0, num1, num2;
    t1[0] = a[0], t2[0] = b[0];
    for(i = 2; i < na; i++)
    {
        t1[1] = a[i-1], t1[2] = a[i];
        num1 = dcmp(cross(t1[1], t1[2],t1[0]));
        if(num1 < 0) swap(t1[1], t1[2]);
        for(j = 2; j < nb; j++)
        {
            t2[1] = b[j - 1], t2[2] = b[j];
            num2 = dcmp(cross(t2[1], t2[2],t2[0]));
            if(num2 < 0) swap(t2[1], t2[2]);
            res += CPIA(t1, t2, 3, 3) * num1 * num2;
        }
    }
    //cout << "Sum::res=" <<res << endl;
    return res;
}

/********************************* multi area end *****************************************/


/*********************************CarDetect*****************************************/
std::string UTF8ToGB(const char* str, int flag = 1)
{
#ifdef _WIN32
	if (flag == 1)
	{
		return str;
	}
	std::string result;
	WCHAR *strSrc;
	LPSTR szRes;

	//获得临时变量的大小
	int i = MultiByteToWideChar(CP_UTF8, 0, str, -1, NULL, 0);
	strSrc = new WCHAR[i + 1];
	MultiByteToWideChar(CP_UTF8, 0, str, -1, strSrc, i);

	//获得临时变量的大小
	i = WideCharToMultiByte(CP_ACP, 0, strSrc, -1, NULL, 0, NULL, NULL);
	szRes = new CHAR[i + 1];
	WideCharToMultiByte(CP_ACP, 0, strSrc, -1, szRes, i, NULL, NULL);

	result = szRes;
	delete[]strSrc;
	delete[]szRes;

	return result;
#else
	return str;
#endif
}

static int ToWchar(char* &src, wchar_t* &dest, const char *locale = "zh_CN.utf8")
{
    if (src == NULL) {
        dest = NULL;
        return 0;
    }

    // 根据环境变量设置locale
    setlocale(LC_CTYPE, locale);

    // 得到转化为需要的宽字符大小
    int w_size = mbstowcs(NULL, src, 0) + 1;

    // w_size = 0 说明mbstowcs返回值为-1。即在运行过程中遇到了非法字符(很有可能使locale
    // 没有设置正确)
    if (w_size == 0) {
        dest = NULL;
        return -1;
    }

    //wcout << "w_size" << w_size << endl;
    dest = new wchar_t[w_size];
    if (!dest) {
        return -1;
    }

    int ret = mbstowcs(dest, src, strlen(src)+1);
    if (ret <= 0) {
        return -1;
    }
    return 0;
}

static int rotateLine(cv::Point &p0, cv::Point &p1, cv::Point &dst, float angle)
{
	angle = -angle;
	dst.x = (p1.x - p0.x) * cos(angle*PI/180) - (p1.y - p0.y) * sin(angle*PI/180) + p0.x;
	dst.y = (p1.x - p0.x) * sin(angle*PI/180) + (p1.y - p0.y) * cos(angle*PI/180) + p0.y;
	return 0;
}

static int DrawAndGetRotatedRect(cv::Mat &img, cv::Point2f *p)
{
	int width = img.cols * 2.2 / 10, height = img.rows * 2.5 / 10;
	cv::Point p0(img.cols*2/5, img.rows*7/8);
	cv::Point p1(p0.x + height, img.rows*7/8);
	cv::Point p2(p0.x + width, img.rows*7/8);

	cv::Point dst, dst1, dst2;

	rotateLine(p0, p1, dst, 25);
	cv::line(img, p0, dst, cv::Scalar(0, 255, 0));

	rotateLine(p0, p2, dst1, 150);
	cv::line(img, p0, dst1, cv::Scalar(0, 0, 255));

	cv::Point p3(dst1.x + height*0.95, dst1.y);
	rotateLine(dst1, p3, dst2, 25);
	cv::line(img, dst, dst2, cv::Scalar(255, 0, 255));

	cv::line(img, dst1, dst2, cv::Scalar(255, 0, 0));

	p[0].x = dst1.x; 
	p[0].y = dst1.y; 
	p[1].x = dst2.x; 
	p[1].y = dst2.y; 
	p[2].x = dst.x; 
	p[2].y = dst.y; 
	p[3].x = p0.x; 
	p[3].y = p0.y; 
	double result = PolygonArea(p, 4);

	std::cout << "result: " << result << std::endl; 
	
	return 0;
}

static int DrawAndGetRotatedRect(float startX, float startY, float width, float height, cv::Mat &img, parallelogram &rRect)
{
	cv::Point p0(startX, startY);
	cv::Point p1(p0.x + height, p0.y);
	cv::Point p2(p0.x + width, p0.y);
	cv::Point dst, dst1, dst2;
	
	rotateLine(p0, p1, dst, 25);
	cv::line(img, p0, dst, cv::Scalar(0, 255, 0));

	rotateLine(p0, p2, dst1, 150);
	cv::line(img, p0, dst1, cv::Scalar(0, 0, 255));

	cv::Point p3(dst1.x + height*0.95, dst1.y);
	
	rotateLine(dst1, p3, dst2, 25);
	cv::line(img, dst, dst2, cv::Scalar(255, 0, 255));

	cv::line(img, dst1, dst2, cv::Scalar(255, 0, 0));

	rRect.height = height;
	rRect.width = width;
	rRect.startX = startX;
	rRect.startY = startY;
	rRect.points[0].x = dst1.x; 
	rRect.points[0].y = dst1.y; 
	rRect.points[1].x = dst2.x; 
	rRect.points[1].y = dst2.y; 
	rRect.points[2].x = dst.x; 
	rRect.points[2].y = dst.y; 
	rRect.points[3].x = p0.x; 
	rRect.points[3].y = p0.y;

	return 0;
}

static int DrawAndGetRotatedRect(cv::Mat &img, cv::RotatedRect &rRect)
{
	rRect.center.x = img.cols * 14 / 36;
	rRect.center.y = img.rows * 25 / 36;
	rRect.size.width = img.cols * 7 / 36;
	rRect.size.height = img.rows * 2 / 36;
	rRect.angle = 78;

	cv::Point2f* vertices = new cv::Point2f[4];
	rRect.points(vertices);

	for (int i=0;i<4;i++) {
		cv::line(img, vertices[i], vertices[(i+1)%4], cv::Scalar(255, 255, 0));
	}

	return 0;
}

static bool IsRectIntersection(cv::RotatedRect &rRect1, cv::Rect &rect)
{
    bool ret = true;

    std::vector<cv::Point2f> intersectingRegion;
    cv::RotatedRect rRect2(cv::Point2f((rect.x + rect.width / 2), (rect.y / rect.height / 2)), 
    cv::Size2f(rect.width, rect.height), 0);

    cv::Point p1(rect.x, rect.y); 
    cv::Point p2(rect.x + rect.width, rect.y);
    cv::Point p3(rect.x + rect.width, rect.y + rect.height);
    cv::Point p4(rect.x, rect.y + rect.height);

    std::vector<cv::Point> selectpoint;
    selectpoint.push_back(p1);
    selectpoint.push_back(p2);
    selectpoint.push_back(p3);
    selectpoint.push_back(p4);
    
    cv::RotatedRect rotate_rect = cv::minAreaRect(selectpoint);
    
    cv::rotatedRectangleIntersection(rRect1, rotate_rect, intersectingRegion);
    
    if(intersectingRegion.empty()) {
        ret = false;
    }

    return ret; 
}


static bool IsRectIntersectionOld(cv::Mat &img, cv::RotatedRect &rRect1, cv::Rect &rect)
{
	bool ret = true;

	std::vector<cv::Point2f> intersectingRegion;
	cv::RotatedRect rRect2(cv::Point2f(rect.x, rect.y), cv::Size2f(rect.width, rect.height), 0);
	rotatedRectangleIntersection(rRect1, rRect2, intersectingRegion);

	if(intersectingRegion.empty()) {
		ret = false;
	}

    cv::Point2f ps[4];
    rRect2.points(ps);
    for(int i=0;i<4;i++) {
        cv::line(img, ps[i], ps[(i+1)%4], cv::Scalar(0, 0, 0), 10);
    }

	return ret;	
}

float computRectJoinUnion(const cv::Rect &rc1, const cv::Rect &rc2)
{
	float AJoin;
	float AUnion;
	cv::Point p1, p2;                 //p1为相交位置的左上角坐标，p2为相交位置的右下角坐标
	p1.x = std::max(rc1.x, rc2.x);
	p1.y = std::max(rc1.y, rc2.y);
 
	p2.x = std::min(rc1.x + rc1.width, rc2.x + rc2.width);
	p2.y = std::min(rc1.y + rc1.height, rc2.y + rc2.height);
 
	AJoin = 0;
	if (p2.x > p1.x && p2.y > p1.y)            //判断是否相交
	{
		AJoin = (p2.x - p1.x)*(p2.y - p1.y);    //求出相交面积
	}
	float A1 = rc1.width * rc1.height;
	float A2 = rc2.width * rc2.height;
	AUnion = (A1 + A2 - AJoin);                 //两者组合的面积
 
	if (AUnion > 0)
	{
		return (AJoin / A1);                  //相交面积与组合面积的比例
		//return (AJoin / AUnion);                  //相交面积与组合面积的比例
	}	
	else
		return 0;
}


bool bbOverlap(const cv::Rect& box1, const cv::Rect& box2)
{
	cv::Rect tmp = box1 & box2;
	return tmp.area();
}

bool plateFilter(const char *str){
	if(str[0]=='\0')return false;
	int s1 = int(str[0]);
	if(s1 >=48 && s1 <= 90)return false;
	int str_len = 0;
	int c = -1;
	for(int i = 3; i < 20; i++){
		if(str[i]=='\0'){
			str_len = i;
			break;
		}
		if(c==-1){
			s1 = int(str[i]);
			if(s1 <48 || s1 > 90) c = i;
		}
	}

	if(c == -1) return true;
	if(c < str_len-3)return false;
	return true;
}

void *darknetProcess(void *args)
{
    int count = 0;
    int plateCount = 0;
    struct timeval start, end;

    //Detector *detector = new Detector("/mnt/hgfs/cars/data_image_src/train/label/jpeglist/yolov4-tiny-3l-plate.cfg", 
    //    "/mnt/hgfs/cars/data_image_src/train/label/jpeglist/yolov4-tiny-3l-plate_last.weights");

    Detector *detector = new Detector("/mnt/hgfs/cars/data_image_src/train/label/jpeglist/yolov4-tiny-3l-car_in_park.cfg", 
        "/mnt/hgfs/cars/data_image_src/train/label/jpeglist/yolov4-tiny-3l-car_in_park_last.weights");

    std::string  root = "/mnt/hgfs/cars/data_image_src/train/toCorrect/";
    std::string  saveroot = "/mnt/hgfs/cars/data_image_src/out/";
    std::string  saverootplate = "/mnt/hgfs/cars/data_image_src/out/plate/";

    std::string image_file_dir = root;
    std::string image_file_savedir = saveroot;
    std::string image_plate = saverootplate;

    std::vector<cv::String> img_paths;
    cv::glob(root, img_paths);

    for (auto img_path : img_paths) {
        FILE *lableFile = NULL;
	    if(strstr(img_path.c_str(), "plate")) {
            continue;
        }
        cv::Mat img = cv::imread(img_path);
        if(img.empty()) {
            continue;
        }
        cv::Mat img_clone = img.clone();

        gettimeofday(&start,NULL);
        std::vector<bbox_t> boxes = detector->detect(img, 0.5f);
        gettimeofday(&end,NULL);
        printf("detect time = %ld ms\r\n", 
                (end.tv_sec - start.tv_sec) * 1000 + (end.tv_usec - start.tv_usec) / 1000);

        char lableBuf[256], *p;
        strncpy(lableBuf, img_path.c_str(), sizeof(lableBuf));
        if(p = strchr(lableBuf, '.')) {
            p++;
            strncpy(p, "txt", 3);
            lableFile = fopen(lableBuf, "w+");
            printf("fopen %s %p\r\n", lableBuf, lableFile);
        }
                
        for(auto box : boxes) {
            char buf[64];
            char buf1[64];
            cv::Rect car_rect(box.x, box.y, box.w, box.h);

            if(lableFile) {
                char writeBuf[128];
                snprintf(writeBuf, sizeof(writeBuf), "%d %f %f %f %f\n",
                        box.obj_id, 1.0f * (box.x + box.w / 2.0f) / img.cols,
                        1.0f * (box.y + box.h / 2.0f) / img.rows, 
                        1.0f * box.w / img.cols, 1.0f * box.h / img.rows);
                fwrite(writeBuf, 1, strlen(writeBuf), lableFile);
            }
            
            switch (box.obj_id) {
                case 0:
                    cv::rectangle(img_clone, car_rect, cv::Scalar(255, 0, 0), 2, 1, 0);
                    break;
                case 1:
                    cv::rectangle(img_clone, car_rect, cv::Scalar(0, 255, 0), 2, 1, 0);
                    break;
                case 2:
                    cv::rectangle(img_clone, car_rect, cv::Scalar(0, 0, 255), 2, 1, 0);
                    break;
                case 3:
                    cv::rectangle(img_clone, car_rect, cv::Scalar(0, 128, 255), 2, 1, 0);
                    break;
                case 4:
                {
                    int x,y,w,h;
                    x = box.x - 10;
                    y = box.y - 10;
                    w = box.w + 20;
                    h = box.h + 20;
                    cv::rectangle(img_clone, car_rect, cv::Scalar(0, 255, 255), 2, 1, 0);
                    if(x > 0 && y > 0 && w > 0 && h > 0 && x + w < img.cols &&
                            y + h < img.rows) {
                        cv::Rect car_rect1(x, y, w, h);
                        cv::Mat plateMat = img(car_rect1);
                        snprintf(buf1, 64, "/mnt/hgfs/cars/data_image_src/out/plate/%d.jpg", plateCount++);
                        cv::imwrite(buf1, plateMat);
                    }
                    break;
                }
            }
            
            snprintf(buf, 64, "class:%d, score:%f", box.obj_id, box.prob);
        }

        if(lableFile) {
            fclose(lableFile);
        }

        char buffer[256] = {0};
        sprintf(buffer, "%d.jpg\n", count++);
        printf("buffer:%s\n", buffer);
        std::string str;
        std::stringstream ss;
        ss << count;
        ss >> str;
        std::string save_dir = saveroot+ str +".jpg";
        //cv::imwrite(save_dir, img_clone);
        img.release();

        if(count == 10000) {
            break;
        }
    }

    delete detector;
}

int main(int argc, char *argv[]) 
{
    pthread_t darknetTask;

    pthread_create(&darknetTask, NULL, darknetProcess, NULL);

    while(1) {
        sleep(100);
    }
    
}