#include "modulecomm.h"
#include "xmlparam.h"
#include "ivfault.h"
#include "ivlog.h"
#include "ivexit.h"
#include "ivversion.h"
#include "rawpic.pb.cc"
#include <QCoreApplication>
#include <thread>
#include "ivversion.h"
#include <chrono>
#include "opencv2/imgcodecs/legacy/constants_c.h"
#include "qmutex.h"
#include "condition_variable"
#include "laneatt.hh"
#include <opencv2/opencv.hpp>
#include "lanearray.pb.h"
#include "imageBuffer.h"

#define INPUT_H 720
#define INPUT_W 1280
#define N_OFFSETS 72
#define N_STRIPS (N_OFFSETS - 1)
#define MAX_COL_BLOCKS 1000

using namespace std;

const std::string trt_path = "./model/LaneATT_test.trt8"; // trt paths

const bool calibrationstate = false;
const std::string calibration_yamlpath = "./yaml/camera_middle_640_360.yaml";
cv::Mat camera_matrix,dist_coe,map1,map2;

//透视变换的点
std::string PERS_FILE = "./yaml/pers_ori.yaml";


const bool cropstate = false;
cv::Range crop_height = cv::Range(240,600);
cv::Range crop_width = cv::Range(320,960);

//是否显示检测结果
bool imshow_flag = true;

//是否读取视频
bool test_video = false;
string video_path = "v4l2src device=/dev/video0 ! video/x-raw, width=(int)1280, height=(int)720 ! videoconvert ! appsink";
//std::string video_path = "/home/nvidia/code/modularization/src/detection/laneATT_trt_old/test/camera_test1.mp4";
bool stop_read = false;

//图片队列
void * gpcamera;
const std::string cameraname="image00";  //共享内存的名字
ConsumerProducerQueue<cv::Mat> * imageBuffer =  new ConsumerProducerQueue<cv::Mat>(3,true);

std::vector<cv::Point2f> warped_point, origin_point;
cv::Point2i transed_O;

float METER_PER_PIXEL_X,METER_PER_PIXEL_Y;

float BOARD_SIDE_LENGTH_X = 3.7;
float BOARD_SIDE_LENGTH_Y = 10.0;


void * gpdetect;
const std::string resultname="linearray";  //共享内存的名字


iv::Ivfault *gfault = nullptr;
iv::Ivlog *givlog = nullptr;

cv::VideoWriter outputVideo;


//读取视频数据
void ReadFunc(int n)
{
    cv::VideoCapture cap(video_path);
    if(!cap.isOpened())
    {
        cout<<"camera failed to open"<<endl;
    }
    while(!stop_read)
    {
        cv::Mat frame;
        //读视频的时候加上,读摄像头去掉
        if(imageBuffer->isFull())
        {
            continue;
        }
        if(cap.read(frame))
        {
            if(calibrationstate)
                cv::remap(frame,frame,map1,map2,cv::INTER_LINEAR,cv::BORDER_CONSTANT);
            if(cropstate)
                frame = frame(crop_height,crop_width);
            imageBuffer->add(frame);
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }
    return;
}
void Listenpic(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize<1000)return;
    iv::vision::rawpic pic;
    if(false == pic.ParseFromArray(strdata,nSize))
    {
        std::cout<<"picview Listenpic fail."<<std::endl;
        return;
    }
    cv::Mat mat(pic.height(),pic.width(),pic.mattype());
    if(pic.type() == 1)
        memcpy(mat.data,pic.picdata().data(),mat.rows*mat.cols*mat.elemSize());
    else
    {
        //     mat.release();
        std::vector<unsigned char> buff(pic.picdata().data(),pic.picdata().data() + pic.picdata().size());
        mat = cv::imdecode(buff,cv::IMREAD_COLOR);
    }
    if(calibrationstate)
        cv::remap(mat,mat,map1,map2,cv::INTER_LINEAR,cv::BORDER_CONSTANT);
    if(cropstate)
        mat = mat(crop_height,crop_width);
    imageBuffer->add(mat);
    mat.release();
}



void exitfunc()
{
//    gbstate = false;
//    gpthread->join();
//    std::cout<<"state thread closed."<<std::endl;
    return;
}

cv::Mat get_pers_mat(std::string pers_file)
{
    cv::Point2f src[4];
    cv::Point2f dst[4];
    cv::FileStorage pf(pers_file, cv::FileStorage::READ);
    pf["board_left_top"]>>src[0];
    pf["board_left_bottom"]>>src[1];
    pf["board_right_top"]>>src[2];
    pf["board_right_bottom"]>>src[3];

//    pf["board_left_top_dst"]>>dst[0];
//    pf["board_left_bottom_dst"]>>dst[1];
//    pf["board_right_top_dst"]>>dst[2];
//    pf["board_right_bottom_dst"]>>dst[3];

/* -------------------------------------zhengke
    dst[1].x = src[0].x;
    dst[1].y = src[1].y;

    dst[3].x = src[2].x;
    dst[3].y = src[3].y;

    int side_length = dst[3].x - dst[1].x;

    METER_PER_PIXEL = BOARD_SIDE_LENGTH / side_length;

    dst[0].x = src[0].x;
    dst[0].y = dst[1].y - side_length;

    dst[2].x = src[2].x;
    dst[2].y = dst[3].y - side_length;

    cv::Mat pers_mat = getPerspectiveTransform(dst, src);
    cv::Mat pers_mat_inv = pers_mat.inv();

    return pers_mat_inv;
*///-------------------------------------zhengke
    dst[1].x = 130*2;
    dst[1].y = 330*2;

    dst[3].x = 510*2;
    dst[3].y = 330*2;

    int side_length_x = dst[3].x - dst[1].x;

    METER_PER_PIXEL_X = BOARD_SIDE_LENGTH_X / float(side_length_x);

    dst[0].x = dst[1].x;
    dst[0].y = 10*2;

    dst[2].x = dst[3].x;
    dst[2].y = 10*2;

    int side_length_y = dst[1].y - dst[0].y;

    METER_PER_PIXEL_Y = BOARD_SIDE_LENGTH_Y / float(side_length_y);

    transed_O.x = (dst[1].x + dst[3].x)/2;
    transed_O.y = (dst[1].y + dst[3].y)/2;

    cv::Mat pers_mat = getPerspectiveTransform(dst, src);
    cv::Mat pers_mat_inv = pers_mat.inv();

    return pers_mat_inv;
}

void transform (std::vector<std::vector<cv::Point2f>> &lanes,cv::Mat &pers_mat_inv,
                cv::Mat &raw_image,cv::Mat &img_warp)
{
    // PerspectiveTransform
    std::vector<cv::Point2f> leftLane;
    std::vector<cv::Point2f> rightLane;
    std::vector<std::vector<cv::Point2f>> warped_lanes;

    for (int i = 0; i < lanes.size(); i++) {
        cv::perspectiveTransform(lanes[i], warped_point , pers_mat_inv); //  坐标变换(输入点，输出点，变换矩阵)
        warped_lanes.push_back(warped_point);
        double sum = 0.0;
        for (int i = 0 ; i < 5 ; i++){
            sum += warped_point[i].x;
        }
        double mean = sum / 5;
        if (mean < raw_image.size().width / 2){
            leftLane.push_back(cv::Point2f(mean, i));
        }else{
            rightLane.push_back(cv::Point2f(mean, i));
        }
    }
    float left_min = 0;
    float right_max = INPUT_W;
    int left_num = -1;
    int right_num = -1;
    int left_left = -1;
    int right_right = -1;
    for (const auto& points : leftLane){
        if(points.x > left_min){
            left_num = (int) round(points.y);
            left_min = points.x;
        }
    }
    for (const auto& points : leftLane){
        if((int)round(points.y) != left_num){
            left_left = points.y;
        }
    }
//    if (left_num == -1){
//        std::cout<<"left lane failed"<<std::endl;
//    }
    for (const auto& points : rightLane){
        if(points.x < right_max){
            right_num = (int) round(points.y);
            right_max = points.x;
        }
    }
    for (const auto& points : rightLane){
        if((int)round(points.y) != right_num){
            right_right = points.y;
        }
    }
//    if (right_num == -1){
//        std::cout<<"right lane failed"<<std::endl;
//    }

    // Visualize

    /*-----------------warp image and points ----------------------*/
    cv::Point2f pp;
    cv::Point2f ppp;
    int flag = 0;
    float DX = 0.055, DY = 0.26; // 像素和实际距离的比例
    std::vector<std::vector<cv::Point2f>> world_lanes;
    cv::warpPerspective(raw_image, img_warp, pers_mat_inv, raw_image.size());        //  图像变换(输入图像，输出图像，变换矩阵)
    for (const auto& lane_points : warped_lanes) {
        if(flag == left_num || flag == right_num){
            std::vector<cv::Point2f> world_points;
            for (const auto& point : lane_points) {
                pp.x =float(point.x);
                pp.y =float(point.y);
                cv::circle(img_warp, pp, 3, cv::Scalar(0, 255, 0), -1);
                //ppp.x = float((point.x - img_warp.size().width / 2) * METER_PER_PIXEL_X);
                //ppp.y = float((img_warp.size().height - point.y) * METER_PER_PIXEL_Y);
                ppp.x = float((point.x - transed_O.x) * METER_PER_PIXEL_X);
                ppp.y = float((transed_O.y - point.y) * METER_PER_PIXEL_Y);
                world_points.push_back(ppp);
            }
            world_lanes.push_back(world_points);
        }else{
            std::vector<cv::Point2f> world_points;
            for (const auto& point : lane_points) {
                pp.x =float(point.x);
                pp.y =float(point.y);
                //std::cout<<pp.x<<" "<<pp.y<<std::endl;
                cv::circle(img_warp, pp, 3, cv::Scalar(0, 0, 255), -1);
                ppp.x = float((point.x - transed_O.x) * METER_PER_PIXEL_X);
                ppp.y = float((transed_O.y - point.y) * METER_PER_PIXEL_Y);
                world_points.push_back(ppp);
                //std::cout<<ppp.x<<" "<<ppp.y<<std::endl;
            }
            world_lanes.push_back(world_points);
        }
        flag++;
    }
    //cv::imshow("warp",img_warp);


   /*------------------------show circle--------------------------*/
    flag = 0;
    for (const auto& lane_points : lanes) {
        if(flag == left_num || flag == right_num){
            for (int i=0; i<lane_points.size(); i++)  {
                pp.x =float(lane_points[i].x);
                pp.y =float(lane_points[i].y);
                cv::circle(raw_image, pp, 3, cv::Scalar(0, 255, 0), -1);

            }
        }else if(flag == left_left){
            for (int i=0; i<lane_points.size(); i++) {
                pp.x =float(lane_points[i].x);
                pp.y =float(lane_points[i].y);
                cv::circle(raw_image, pp, 3, cv::Scalar(0, 0, 255), -1);
            }
        }else {
            for (int i=0; i<lane_points.size(); i++) {
                pp.x =float(lane_points[i].x);
                pp.y =float(lane_points[i].y);
                cv::circle(raw_image, pp, 3, cv::Scalar(255, 0, 0), -1);
            }
        }
        flag++;
    }
/*------------------------show lanes_line------------------------------*/
//    cv::Point2f p1, p2;
//    flag = 0;
//    for (const auto& lane_points : lanes) {
//        if(flag == left_num || flag == right_num){
//            for (int i=0; i<lane_points.size()-1; i++) {
//                p1.x =float(lane_points[i].x) *  x_ratio;
//                p1.y =float(lane_points[i].y) * y_ratio;
//                p2.x =float(lane_points[i+1].x) *  x_ratio;
//                p2.y =float(lane_points[i+1].y) *  y_ratio;
//                cv::line(raw_image, p1, p2, cv::Scalar(0, 255, 0), 1);

//            }
//        }else{
//            for (int i=0; i<lane_points.size()-1; i++) {
//                p1.x =float(lane_points[i].x) *  x_ratio;
//                p1.y =float(lane_points[i].y) * y_ratio;
//                p2.x =float(lane_points[i+1].x) *  x_ratio;
//                p2.y =float(lane_points[i+1].y) *  y_ratio;
//                cv::line(raw_image, p1, p2, cv::Scalar(0, 0, 255), 1);
//            }
//        }
//        flag++;
//    }

    //cv::imshow("raw",raw_image);

    /*---------------------- protobuf ------------------------*/

    iv::vision::Linearray line_array;

    flag = 0;
    for (const auto& lane_points : world_lanes) {
        iv::vision::Line *line = line_array.add_line();

        if(flag == left_left){
            line->set_index(1);
            for (int i=0; i<lane_points.size(); i++)  {
                iv::vision::Point2f *points = line->add_linepoint();
                points->set_x(lane_points[i].x);
                points->set_y(lane_points[i].y);

            }
        }else if(flag == left_num){
            line->set_index(2);
            for (int i=0; i<lane_points.size(); i++) {
                iv::vision::Point2f *points = line->add_linepoint();
                points->set_x(lane_points[i].x);
                points->set_y(lane_points[i].y);
            }
        }else if(flag == right_num){
            line->set_index(3);
            for (int i=0; i<lane_points.size(); i++) {
                iv::vision::Point2f *points = line->add_linepoint();
                points->set_x(lane_points[i].x);
                points->set_y(lane_points[i].y);
            }
        }else{
            line->set_index(4);
            for (int i=0; i<lane_points.size(); i++) {
                iv::vision::Point2f *points = line->add_linepoint();
                points->set_x(lane_points[i].x);
                points->set_y(lane_points[i].y);
            }
        }
        flag++;
    }
    /*-----------------------shareMsg-------------------------*/

    int size = line_array.ByteSize();
    char * strdata = new char[line_array.ByteSize()];
    if(line_array.SerializeToArray(strdata, size))
    {
        iv::modulecomm::ModuleSendMsg(gpdetect, strdata, size);
        std::cout<<"lane serialize success."<<std::endl;
    }
    else
    {
        std::cout<<"lane serialize error."<<std::endl;
    }
    delete strdata;

}

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

// ==============================================================================
    showversion("laneatt");
    QCoreApplication a(argc, argv);

    gfault = new iv::Ivfault("laneATT");
    givlog = new iv::Ivlog("laneATT");

    gfault->SetFaultState(0,0,"laneATT initialize.");


    if(argc==3)
    {
        test_video = (strcmp(argv[1], "true") == 0)?true:false;
        video_path = argv[2];
    }
    if(argc==2)
    {
        test_video = (strcmp(argv[1], "true") == 0)?true:false;
    }

    if(test_video)
        std::thread * readthread = new std::thread(ReadFunc,1);
    else
        gpcamera= iv::modulecomm::RegisterRecv(&cameraname[0],Listenpic);

    gpdetect = iv::modulecomm::RegisterSend(&resultname[0],10000000,1);

    if (calibrationstate)
    {
        cv::FileStorage calib_file(calibration_yamlpath, cv::FileStorage::READ);
        calib_file["cameraMatrix"]>>camera_matrix;
        calib_file["distCoeffs"]>>dist_coe;
        cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
        cv::Size imgsize=cv::Size(1280,720);
        cv::initUndistortRectifyMap(camera_matrix, dist_coe, R, camera_matrix,imgsize,CV_16SC2,map1,map2);
    }
    cv::Mat pers_mat_inv = get_pers_mat(PERS_FILE);  // 得到透视变换关系

    //初始化模型
    LaneATT model(trt_path);

    double waittime = (double)cv::getTickCount();
    while (1)
    {
        if(imageBuffer->isEmpty())
        {
            double waittotal = (double)cv::getTickCount() - waittime;
            double totaltime = waittotal/cv::getTickFrequency();
//            //--------长时间获取不到图片则退出程序----------
//            if(totaltime>120.0)
//            {
//                cout<<"Cant't get frame and quit"<<endl;
//                cv::destroyAllWindows();
//                std::cout<<"------end program------"<<std::endl;
//                break;
//            }
            cout<<"Wait for frame "<<totaltime<<"s"<<endl;
            continue;
        }
        auto start = std::chrono::system_clock::now();  //时间函数
        cv::Mat raw_image,img_warp;
        imageBuffer->consume(raw_image);
        std::vector<std::vector<cv::Point2f>> lanes;
        lanes = model.DetectLane(raw_image);
        transform (lanes,pers_mat_inv,raw_image,img_warp);
        auto end = std::chrono::system_clock::now();  //时间函数
        std::cout <<"total time lane : "<<
                    std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() <<
                    "ms" << std::endl;
        if(imshow_flag)
        {

            cv::namedWindow("rst",cv::WINDOW_NORMAL);
            cv::namedWindow("warp",cv::WINDOW_NORMAL);
            cv::imshow("rst",raw_image);
            cv::imshow("warp",img_warp );
            if (cv::waitKey(10) == 'q')
            {
                stop_read = true;
                break;
            }
        }
        waittime = (double)cv::getTickCount();

    }

    cv::destroyAllWindows();
    //iv::ivexit::RegIVExitCall(exitfunc);
    return a.exec();
}